From 08578ca012580525b63f49ee52d117852d3f4335 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Fri, 30 Aug 2024 11:11:17 +0000 Subject: [PATCH 1/5] Make the package self contained by removing dependencies on * cartesian_controllers * my fork of ros_control_boilerplate which included a protoype for CombinableHw support --- .devcontainer/devcontainer.json | 2 +- .rosinstall | 9 +-------- README.md | 7 +++---- lbrmed_bringup/launch/hardware.launch | 2 +- lbrmed_bringup/launch/simulation.launch | 6 +++--- lbrmed_bringup/package.xml | 1 - lbrmed_control/.rosinstall | 4 ++++ lbrmed_control/CMakeLists.txt | 6 +++--- lbrmed_control/README.md | 13 ++++++++++++- lbrmed_control/launch/lbrmed_control.launch | 2 +- lbrmed_control/package.xml | 2 -- lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml | 7 ------- lbrmed_hw_fri/src/lbrmed_hw_fri.cpp | 5 +---- 13 files changed, 30 insertions(+), 36 deletions(-) create mode 100644 lbrmed_control/.rosinstall delete mode 100644 lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 71051ee..ef74cef 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,7 +7,7 @@ "workspaceFolder": "/workspace/src/lbrmed-ros", // Source the ROS workspace, the ${containerWorkspaceFolder} variable only gets replaced in this .json // Install dependencies for workspace except Gazebo packages as the OSRF version is installed - "postCreateCommand": "echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> ${HOME}/.bashrc && echo 'source ${containerWorkspaceFolder}/devel/setup.bash' >> ${HOME}/.bashrc && /bin/sh .devcontainer/postCreate.sh", + "postCreateCommand": "echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> ${HOME}/.bashrc && echo 'source /workspace/devel/setup.bash' >> ${HOME}/.bashrc && /bin/sh .devcontainer/postCreate.sh", "customizations": { "vscode": { "extensions": [ diff --git a/.rosinstall b/.rosinstall index 05a877c..68bb4ae 100644 --- a/.rosinstall +++ b/.rosinstall @@ -1,8 +1 @@ -- git: - local-name: cartesian_controllers - uri: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git - version: ros1 -- git: - local-name: ros_control_boilerplate - uri: https://github.com/Tuebel/ros_control_boilerplate.git - version: combined_robot_hw_draft +# No source code dependencies, only lbrmed_control contains an optional dependency diff --git a/README.md b/README.md index a285d98..c5d24a4 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Please look at the READMEs in the specific hardware packages. * No need to patch a kernel as we do not use kernel interrupts only the standard `ros_control` loop. * You might observe connectivity errors if you launch additional loads, e.g. RViz, or use a network switch. * We provide a **development container**, so you can start without manually installing any dependencies. -* For custom setups, **only ROS tools** like `wstool` / `vcs` and `rosdep` are required to install dependencies. +* Otherwise, this package is simple to set up without manual steps using `wstool` / `vcs` and `rosdep`. * We built collision and visualization meshes for the LBR Med with internal electric connectors. # Install @@ -53,12 +53,11 @@ For the Sunrise Project you have two ways to proceed: ### Installing Dependencies In the catkin workspace, run the following commands to install all workspace and system dependencies. ```bash -wstool init src -wstool merge -t src src/lbrmed-ros/.rosinstall -wstool update -t src rosdep install --from-paths src --ignore-src -r -y ``` +If you want to use the [FZI Cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers), you can find the installation steps in the `lbrmed_control` [README](./lbrmed_control/README.md). + ### Building the workspace To build the packages in parallel, we use the [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/installing.html). After installing them, run: ```bash diff --git a/lbrmed_bringup/launch/hardware.launch b/lbrmed_bringup/launch/hardware.launch index 0362467..f0405db 100644 --- a/lbrmed_bringup/launch/hardware.launch +++ b/lbrmed_bringup/launch/hardware.launch @@ -40,7 +40,7 @@ - + diff --git a/lbrmed_bringup/launch/simulation.launch b/lbrmed_bringup/launch/simulation.launch index ec54e02..336e23c 100644 --- a/lbrmed_bringup/launch/simulation.launch +++ b/lbrmed_bringup/launch/simulation.launch @@ -23,7 +23,7 @@ EffortJointInterface for grasping tasks PositionJointInterface for FZI cartesian controllers --> - + @@ -52,7 +52,7 @@ - + @@ -65,7 +65,7 @@ - + diff --git a/lbrmed_bringup/package.xml b/lbrmed_bringup/package.xml index 5951a84..9a5291c 100644 --- a/lbrmed_bringup/package.xml +++ b/lbrmed_bringup/package.xml @@ -8,7 +8,6 @@ BSD-3 catkin - cartesian_controllers joint_state_publisher lbrmed_control lbrmed_description diff --git a/lbrmed_control/.rosinstall b/lbrmed_control/.rosinstall new file mode 100644 index 0000000..7fdd3b6 --- /dev/null +++ b/lbrmed_control/.rosinstall @@ -0,0 +1,4 @@ +- git: + local-name: cartesian_controllers + uri: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git + version: ros1 \ No newline at end of file diff --git a/lbrmed_control/CMakeLists.txt b/lbrmed_control/CMakeLists.txt index 2abf30b..f890eed 100644 --- a/lbrmed_control/CMakeLists.txt +++ b/lbrmed_control/CMakeLists.txt @@ -11,9 +11,9 @@ find_package(catkin REQUIRED COMPONENTS roscpp) catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS actionlib controller_interface lbrmed_msgs diff --git a/lbrmed_control/README.md b/lbrmed_control/README.md index ee1019f..0683587 100644 --- a/lbrmed_control/README.md +++ b/lbrmed_control/README.md @@ -18,10 +18,21 @@ Provides launch files to load and start a set of default controllers. - `joint_state_controller`: Implements a controller which re-publishes the joint states of the robot. - `force_torque_sensor_controller`: Implements a controller which re-publishes the force and torque sensors of the robot. +## FZI Cartesian controllers +The config contains entries for the [FZI Cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers). +However, this package is intended to be a self-contained build and source dependencies are reduced to a minimum. +To install the Cartesian controllers, run the following commands in the workspace root: +```shell +wstool init src +wstool merge -t src src/lbrmed-ros/lbrmed_control/.rosinstall +wstool update -t src +``` +Then re-build and -source the whole workspace + ## Custom controllers - `PositionJointInterface_abs_admittance_controller`: Implements an absolute force-based admittance controller. Compared to a normal admittance controller, the robot approaches an object until the total force exceeds a limit and stops. -I tried implementing a similar behavior using the [FZI cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers), but it deviated from the commanded position, no matter how I tuned the parameters. +I tried implementing a similar behavior using the [FZI Cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers), but it deviated from the commanded position, no matter how I tuned the parameters. This controller uses good old KDL inverse kinematics and monitors the forces in the ROS control loop. Compared to the FZI version, this implementation is **less robust near singularities**. - Offers `abs_admittance_control` action: diff --git a/lbrmed_control/launch/lbrmed_control.launch b/lbrmed_control/launch/lbrmed_control.launch index 961d7f1..ff1b7bb 100644 --- a/lbrmed_control/launch/lbrmed_control.launch +++ b/lbrmed_control/launch/lbrmed_control.launch @@ -16,7 +16,7 @@ --> - + diff --git a/lbrmed_control/package.xml b/lbrmed_control/package.xml index 3169cd8..764d5e2 100644 --- a/lbrmed_control/package.xml +++ b/lbrmed_control/package.xml @@ -8,8 +8,6 @@ BSD-3 catkin - cartesian_compliance_controller - cartesian_motion_controller controller_manager effort_controllers joint_state_controller diff --git a/lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml b/lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml deleted file mode 100644 index f4bd193..0000000 --- a/lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Interface for an KUKA LBR lbrmed arm. - - - diff --git a/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp b/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp index 92ec4b6..fbe7b32 100644 --- a/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp +++ b/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp @@ -18,7 +18,6 @@ #include // ROS #include -#include #include #include // KUKA lib @@ -212,6 +211,4 @@ double HardwareInterface::low_pass_filter(double y_k, double u_k, double T, doub } } -} // namespace lbrmed_hw_fri -PLUGINLIB_EXPORT_CLASS(ros_control_boilerplate::CombinableGenericHW, - hardware_interface::RobotHW) \ No newline at end of file +} // namespace lbrmed_hw_fri \ No newline at end of file From ec7d2c57de828e15fd53c143c3a81adcc9c8065f Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Fri, 30 Aug 2024 11:13:58 +0000 Subject: [PATCH 2/5] remove .rosinstall --- .github/workflows/ci.yml | 1 - .rosinstall | 1 - 2 files changed, 2 deletions(-) delete mode 100644 .rosinstall diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d616374..e90ee68 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -23,4 +23,3 @@ jobs: ROS_DISTRO: noetic CATKIN_LINT: true CATKIN_LINT_ARGS: -W2 - UPSTREAM_WORKSPACE: .rosinstall diff --git a/.rosinstall b/.rosinstall deleted file mode 100644 index 68bb4ae..0000000 --- a/.rosinstall +++ /dev/null @@ -1 +0,0 @@ -# No source code dependencies, only lbrmed_control contains an optional dependency From 8255d0aa8ea1b525026d82bfaca8b95537274124 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Fri, 30 Aug 2024 11:15:29 +0000 Subject: [PATCH 3/5] add .vscode (tasks) --- .gitignore | 1 - .vscode/copyright_template.txt | 3 ++ .vscode/tasks.json | 70 ++++++++++++++++++++++++++++++++++ 3 files changed, 73 insertions(+), 1 deletion(-) create mode 100644 .vscode/copyright_template.txt create mode 100644 .vscode/tasks.json diff --git a/.gitignore b/.gitignore index a659ccf..27324f3 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1 @@ .catkin_tools -.vscode diff --git a/.vscode/copyright_template.txt b/.vscode/copyright_template.txt new file mode 100644 index 0000000..7c8e624 --- /dev/null +++ b/.vscode/copyright_template.txt @@ -0,0 +1,3 @@ +@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) $CURRENT_YEAR, Institute of Automatic Control - RWTH Aachen University +All rights reserved. \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..066fa4b --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,70 @@ +/* Copyright (c) 2020, Institute of Automatic Control - RWTH Aachen University + All rights reserved. */ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "merge compile_commands.json", + "type": "shell", + "command": "cd /workspace && jq -s 'map(.[])' /workspace/build/**/compile_commands.json > /workspace/build/compile_commands.json", + "group": "none", + "problemMatcher": "$catkin-gcc" + }, + { + "label": "catkin build workspace", + "type": "shell", + "command": "cd /workspace && catkin build && jq -s 'map(.[])' /workspace/build/**/compile_commands.json > /workspace/build/compile_commands.json", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [ + "$catkin-gcc" + ] + }, + { + "label": "catkin build package", + "type": "shell", + "command": "cd ${fileDirname} && catkin build --this --workspace /workspace && jq -s 'map(.[])' /workspace/build/**/compile_commands.json > /workspace/build/compile_commands.json", + "problemMatcher": [ + "$catkin-gcc" + ] + }, + { + "label": "catkin test current", + "type": "shell", + "command": "cd ${fileDirname} && catkin build --this --workspace /workspace --cmake-args -DCMAKE_BUILD_TYPE=Debug && catkin test --no-deps --this --verbose", + "group": { + "kind": "test", + "isDefault": true + }, + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "catkin config debug", + "type": "shell", + "command": "cd /workspace && catkin config -j $(($(nproc)-1)) --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=1", + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "catkin config release", + "type": "shell", + "command": "cd /workspace && catkin config -j $(($(nproc)-1)) --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=1", + "problemMatcher": [ + "$msCompile" + ] + }, + { + "label": "catkin clean", + "type": "shell", + "command": "cd /workspace && catkin clean --yes", + "problemMatcher": [ + "$msCompile" + ] + } + ] +} \ No newline at end of file From e9f04cb3b4637cd6f67b2076686a5e88183659b1 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Fri, 30 Aug 2024 11:22:09 +0000 Subject: [PATCH 4/5] Remove export of CombinableHw plugin --- lbrmed_hw_fri/package.xml | 4 ---- lbrmed_moveit_config/launch/moveit.rviz | 23 ++++++++++++----------- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/lbrmed_hw_fri/package.xml b/lbrmed_hw_fri/package.xml index 48132f9..55033fa 100644 --- a/lbrmed_hw_fri/package.xml +++ b/lbrmed_hw_fri/package.xml @@ -20,8 +20,4 @@ hardware_interface lbrmed_control - - - - diff --git a/lbrmed_moveit_config/launch/moveit.rviz b/lbrmed_moveit_config/launch/moveit.rviz index cee13e5..d70da18 100644 --- a/lbrmed_moveit_config/launch/moveit.rviz +++ b/lbrmed_moveit_config/launch/moveit.rviz @@ -5,9 +5,10 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /MotionPlanning1 - /MotionPlanning1/Planned Path1 Splitter Ratio: 0.5415094494819641 - Tree Height: 390 + Tree Height: 285 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -120,7 +121,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Loop Animation: false + Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 Show Robot Collision: false @@ -351,7 +352,7 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 3.281726837158203 + Distance: 2.0772390365600586 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -359,17 +360,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.13066630065441132 + Y: -0.12184453010559082 + Z: -5.960464477539063e-08 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.260398268699646 + Pitch: 0.3603981137275696 Target Frame: lbrmed14_link0 - Yaw: 0.5303983688354492 + Yaw: 1.4153984785079956 Saved: ~ Window Geometry: Displays: @@ -383,9 +384,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd00000003000000000000023100000391fc020000000cfb000000100044006900730070006c006100790073010000003b00000217000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000258000001740000016900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001f1000002070000000000000000fb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006702000000000000001b000002b60000020dfb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0000000010000012a00000391fc0200000001fb0000000a00560069006500770073010000003b00000391000000a000ffffff000000030000059e00000174fc0100000001fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000021a0000059e00000000000000000000040d0000039100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000300000000000001ca00000391fc020000000cfb000000100044006900730070006c006100790073010000003b000001ae000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ef000001dd0000016900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001f1000002070000000000000000fb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006702000000000000001b000002b60000020dfb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0000000010000012a00000391fc0200000001fb0000000a00560069006500770073010000003b00000391000000a000ffffff000000030000059e00000174fc0100000001fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000021a0000059e0000000000000000000004740000039100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1908 - X: -34 - Y: 17 + X: 0 + Y: 4 From 6e77c83d43e36e73ca14da41bc199964fc6efdf3 Mon Sep 17 00:00:00 2001 From: Tim Redick Date: Fri, 30 Aug 2024 11:29:39 +0000 Subject: [PATCH 5/5] remove plugin installation from CMakeLists --- lbrmed_hw_fri/CMakeLists.txt | 4 ---- 1 file changed, 4 deletions(-) diff --git a/lbrmed_hw_fri/CMakeLists.txt b/lbrmed_hw_fri/CMakeLists.txt index cc2169c..141cbb4 100644 --- a/lbrmed_hw_fri/CMakeLists.txt +++ b/lbrmed_hw_fri/CMakeLists.txt @@ -62,10 +62,6 @@ install(TARGETS ${PROJECT_NAME}_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -# Install Plugin -install(FILES ${PROJECT_NAME}_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - - ############ # Unit Tests ############