From 4b8672565620a417f8e3d0d1635ec994d955ea84 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 7 Feb 2025 17:42:45 +0000 Subject: [PATCH 01/25] Ported Cartesian Impedance Controller from ROS 1 to ROS 2 --- CMakeLists.txt | 162 +-- LICENSE | 11 - README.md | 330 ----- cartesian_impedance_controller_plugin.xml | 7 + cfg/damping.cfg | 21 - cfg/stiffness.cfg | 20 - cfg/wrench.cfg | 17 - controller_plugins.xml | 5 - ...r.h => cartesian_impedance_controller.hpp} | 47 +- .../cartesian_impedance_controller_ros.h | 350 ----- .../cartesian_impedance_controller_ros.hpp | 142 ++ .../rbdyn_wrapper.h | 61 +- msg/ControllerConfig.msg | 20 - msg/ControllerState.msg | 20 - package.xml | 92 +- src/cartesian_impedance_controller.cpp | 2 +- ...esian_impedance_controller_parameters.yaml | 211 +++ src/cartesian_impedance_controller_ros.cpp | 1270 ++++++++++------- 18 files changed, 1299 insertions(+), 1489 deletions(-) delete mode 100644 LICENSE delete mode 100644 README.md create mode 100644 cartesian_impedance_controller_plugin.xml delete mode 100644 cfg/damping.cfg delete mode 100644 cfg/stiffness.cfg delete mode 100644 cfg/wrench.cfg delete mode 100644 controller_plugins.xml rename include/cartesian_impedance_controller/{cartesian_impedance_controller.h => cartesian_impedance_controller.hpp} (93%) delete mode 100644 include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h create mode 100644 include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp delete mode 100644 msg/ControllerConfig.msg delete mode 100644 msg/ControllerState.msg create mode 100644 src/cartesian_impedance_controller_parameters.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 47ce961..6bd6b2e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,119 +1,97 @@ -cmake_minimum_required(VERSION 2.8.3) -cmake_policy(SET CMP0048 NEW) # Version not in project() command +cmake_minimum_required(VERSION 3.16) project(cartesian_impedance_controller) -find_package(Boost 1.49 REQUIRED) -find_package(PkgConfig) +# Compiler warnings (for GNU and Clang) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options( + -Wall -Wextra -Wpedantic + -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format + -Werror=range-loop-construct -Werror=missing-braces + ) +endif() -# Find RBDyn library and dependencies -pkg_search_module(Eigen3 REQUIRED eigen3) -pkg_check_modules(mc_rbdyn_urdf REQUIRED mc_rbdyn_urdf) -pkg_check_modules(RBDyn REQUIRED RBDyn) -pkg_check_modules(SpaceVecAlg REQUIRED SpaceVecAlg) -pkg_check_modules(tinyxml2 REQUIRED tinyxml2) +# Export all symbols on Windows +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp control_msgs + sensor_msgs + control_toolbox controller_interface - controller_manager - dynamic_reconfigure - eigen_conversions geometry_msgs + joint_trajectory_controller hardware_interface - message_generation - pluginlib + kinematics_interface + rclcpp_lifecycle + rclcpp_action realtime_tools - roscpp - sensor_msgs - std_msgs - tf - tf_conversions + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros trajectory_msgs + generate_parameter_library + urdf + controller_manager + ros2_control ) -add_message_files(FILES - ControllerConfig.msg - ControllerState.msg +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(RBDyn REQUIRED) +find_package(mc_rbdyn_urdf REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(pluginlib REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(cartesian_impedance_controller_parameters + src/cartesian_impedance_controller_parameters.yaml ) -generate_messages( -DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs +add_library(cartesian_impedance_controller SHARED + src/cartesian_impedance_controller_ros.cpp + src/cartesian_impedance_controller.cpp ) -generate_dynamic_reconfigure_options( - cfg/damping.cfg - cfg/stiffness.cfg - cfg/wrench.cfg +target_compile_features(cartesian_impedance_controller PUBLIC cxx_std_17) + +target_include_directories(cartesian_impedance_controller PUBLIC + $ + $ ) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS actionlib actionlib_msgs control_msgs controller_interface controller_manager dynamic_reconfigure eigen_conversions geometry_msgs hardware_interface message_runtime pluginlib realtime_tools roscpp sensor_msgs std_msgs tf tf_conversions trajectory_msgs - DEPENDS Boost Eigen3 mc_rbdyn_urdf RBDyn SpaceVecAlg tinyxml2 - LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_ros +target_link_libraries(cartesian_impedance_controller PUBLIC + cartesian_impedance_controller_parameters + Eigen3::Eigen + RBDyn + mc_rbdyn_urdf::mc_rbdyn_urdf ) -########### -## Build ## -########### +pluginlib_export_plugin_description_file(controller_interface cartesian_impedance_controller_plugin.xml) -# Sets build type to "Release" in case no build type has not been set before. This is necessary to run this controller at 1 kHz. -# On our RT system with an i5 11th Gen processor this reduces update evaluations from about 800 to 300 microseconds -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif(NOT CMAKE_BUILD_TYPE) +ament_target_dependencies(cartesian_impedance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Core library -add_library(${PROJECT_NAME} src/cartesian_impedance_controller.cpp) -add_dependencies( - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +install( + DIRECTORY include/ + DESTINATION include/cartesian_impedance_controller ) -target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${SpaceVecAlg_INCLUDE_DIRS} ${RBDyn_INCLUDE_DIRS} ${mc_rbdyn_urdf_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${Eigen3_LIBRARIES}) - -# ROS Integration -add_library(${PROJECT_NAME}_ros src/cartesian_impedance_controller_ros.cpp) -add_dependencies( - ${PROJECT_NAME}_ros - ${${PROJECT_NAME}_ros_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg +install( + TARGETS cartesian_impedance_controller cartesian_impedance_controller_parameters + EXPORT export_cartesian_impedance_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) -target_compile_options(${PROJECT_NAME}_ros PUBLIC -std=c++14) -target_include_directories(${PROJECT_NAME}_ros PUBLIC include ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${tinyxml2_INCLUDE_DIRS} ${SpaceVecAlg_INCLUDE_DIRS} ${RBDyn_INCLUDE_DIRS} ${mc_rbdyn_urdf_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME}_ros PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${tinyxml2_LIBRARIES} RBDyn mc_rbdyn_urdf ${Eigen3_LIBRARIES}) - - -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE) - -## Testing -if (CATKIN_ENABLE_TESTING) - # Base library tests - find_package(rostest REQUIRED) - add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) - target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) - - # ROS basic tests - add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) - target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) - - # ROS functionality tests - add_rostest_gtest(ros_func_tests test/ros_func.test test/ros_func_tests.cpp) - target_link_libraries(ros_func_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) -endif() - +ament_export_targets(export_cartesian_impedance_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 21e5f40..0000000 --- a/LICENSE +++ /dev/null @@ -1,11 +0,0 @@ -Copyright 2022 Matthias Mayr - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/README.md b/README.md deleted file mode 100644 index 9f2beb0..0000000 --- a/README.md +++ /dev/null @@ -1,330 +0,0 @@ -# Cartesian Impedance Controller -[![CI](https://github.com/matthias-mayr/Cartesian-Impedance-Controller/actions/workflows/build_code.yml/badge.svg?branch=master)](https://github.com/matthias-mayr/Cartesian-Impedance-Controller/actions/workflows/build_code.yml) -[![rosdoc](https://github.com/matthias-mayr/Cartesian-Impedance-Controller/actions/workflows/build_docs.yml/badge.svg)](https://matthias-mayr.github.io/Cartesian-Impedance-Controller/) -[![DOI](https://joss.theoj.org/papers/10.21105/joss.05194/status.svg)](https://doi.org/10.21105/joss.05194) -[![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) - -## Description -This project is an implementation of Cartesian impedance control for robotic manipulators. It is a type of control strategy that sets a dynamic relationship between contact forces and the position of a robot arm, making it suitable for collaborative robots. It is particularily useful when the interesting dimensions in the workspace are in the Cartesian space. - -The controller is developed using the seven degree-of-freedom (DoF) robot arm `LBR iiwa` by `KUKA AG` and has also been tested with the `Franka Emika Robot (Panda)` both in reality and simulation. -This controller is used and tested with ROS 1 `melodic` and `noetic`. - -The implementation consists of a -1. base library that has few dependencies and can e.g. be directly integrated into software such as the DART simulator and a -2. ROS control integration on top of it. - -### Short Pitch at ROSCon: -[![IMAGE ALT TEXT](http://img.youtube.com/vi/Q4aPm4O_9fY/0.jpg)](http://www.youtube.com/watch?v=Q4aPm4O_9fY "Cartesian Impedance Controller ROSCon 2022 Lightning Talk") - -http://www.youtube.com/watch?v=Q4aPm4O_9fY - -## Features - -- Configurable stiffness values along all Cartesian dimensions at runtime -- Configurable damping factors along all Cartesian dimensions at runtime -- Change reference pose at runtime -- Apply Cartesian forces and torques at runtime -- Optional filtering of stiffnesses, pose and wrenches for smoother operation -- Handling of joint trajectories with nullspace configurations, e.g. from MoveIt -- Jerk limitation -- Separate base library that can be integrated in non-ROS environments -- Interface to ROS messages and dynamic_reconfigure for easy runtime configuration - -![](./res/flowchart.png) - -## Torques - -The torque signal commanded to the joints of the robot is composed by the superposition of three joint-torque signals: -- The torque calculated for Cartesian impedance control with respect to a Cartesian pose reference in the frame of the EE of the robot (`tau_task`). -- The torque calculated for joint impedance control with respect to a desired configuration and projected in the nullspace of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector (`tau_ns`). -- The torque necessary to achieve the desired external force command (`cartesian_wrench`), in the frame of the EE of the robot (`tau_ext`). - -## Limitations - -- Joint friction is not accounted for -- Stiffness and damping values along the Cartesian dimensions are uncoupled -- No built-in gravity compensation for tools or workpieces (can be achieved by commanding a wrench) - -## Prerequisites -### Required -- [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) - -### ROS Controller -We use `RBDyn` to calculate forward kinematics and the Jacobian. - -- [ROS](https://www.ros.org/) -- [RBDyn](https://github.com/jrl-umi3218/RBDyn) -- [mc_rbdyn_urdf](https://github.com/jrl-umi3218/mc_rbdyn_urdf) -- [SpaceVecAlg](https://github.com/jrl-umi3218/SpaceVecAlg) - -The installation steps for the installation of the non-ROS dependencies are automated in `scripts/install_dependencies.sh`. - -## Controller Usage in ROS -Assuming that there is an [initialized catkin workspace](https://catkin-tools.readthedocs.io/en/latest/quick_start.html#initializing-a-new-workspace) you can clone this repository, install the dependencies and compile the controller. - -Here are the steps: - -```bash -cd catkin_ws -git clone https://github.com/matthias-mayr/Cartesian-Impedance-Controller src/Cartesian-Impedance-Controller -src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh -rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y -catkin_make # or 'catkin build' -source devel/setup.bash -``` - -This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController` in your `ros_control` configuration. - -### Configuration file -When using the controller it is a good practice to describe the parameters in a `YAML` file and load it. Usually this is already done by your robot setup - e.g. for [iiwa_ros](https://github.com/epfl-lasa/iiwa_ros/) that is [here](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_control/config/iiwa_control.yaml). -Here is a template of what needs to be in that YAML file that can be adapted: -```YAML -CartesianImpedance_trajectory_controller: - type: cartesian_impedance_controller/CartesianImpedanceController - joints: # Joints to control - - iiwa_joint_1 - - iiwa_joint_2 - - iiwa_joint_3 - - iiwa_joint_4 - - iiwa_joint_5 - - iiwa_joint_6 - - iiwa_joint_7 - end_effector: iiwa_link_ee # Link to control arm in - update_frequency: 500 # Controller update frequency in Hz - # Optional parameters - the mentioned values are the defaults - dynamic_reconfigure: true # Starts dynamic reconfigure server - handle_trajectories: true # Accept traj., e.g. from MoveIt - robot_description: /robot_description # In case of a varying name - wrench_ee_frame: iiwa_link_ee # Default frame for wrench commands - delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm - filtering: # Update existing values (0.0 1.0] per s - nullspace_config: 0.1 # Nullspace configuration filtering - pose: 0.1 # Reference pose filtering - stiffness: 0.1 # Cartesian and nullspace stiffness - wrench: 0.1 # Commanded torque - verbosity: - verbose_print: false # Enables additional prints - state_msgs: false # Messages of controller state - tf_frames: false # Extra tf frames -``` - -### Startup - -To start up with this controller, eventually the controller spawner needs to load the controller. Typically this is baked into the robot driver. For example if using the YAML example above, with [iiwa_ros](https://github.com/epfl-lasa/iiwa_ros/), this can be achieved with command: - -```bash -roslaunch iiwa_gazebo iiwa_gazebo.launch controller:=CartesianImpedance_trajectory_controller -``` - -### Changing parameters with Dynamic Reconfigure -If it is not deactivated, the controller can be configured with [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure) both with [command line tools](http://wiki.ros.org/dynamic_reconfigure#dynamic_reconfigure.2Fgroovy.dynparam_command-line_tool) as well as the graphical user interface [rqt_reconfigure](http://wiki.ros.org/rqt_reconfigure). To start the latter you can run: -```bash -rosrun rqt_reconfigure rqt_reconfigure -``` - -There are several entries: -- `cartesian_wrench_reconfigure` -- `damping_factors_reconfigure` -- `stiffness_reconfigure` - -For applying wrench, the `apply` checkbox needs to be ticked for the values to be used. Damping and stiffness changes are only updated when the `update` checkbox is ticked, allowing to configure changes before applying them. Note that the end-effector reference pose can not be set since it usually should follow a smooth trajectory. - -### Changing parameters with ROS messages -In addition to the configuration with `dynamic_reconfigure`, the controller configuration can always be adapted by sending ROS messages. Outside prototyping this is the main way to parameterize it. - -The instructions below use `${controller_ns}` for the namespace of your controller. This can for example be `/cartesian_impedance_controller`. If you want to copy the example commands 1:1, you can set an environment variable with `export controller_ns=/`. - -#### End-effector reference pose -New reference poses can be sent to the topic `${controller_ns}/reference_pose`. They are expected to be in the root frame of the robot description which is often `world`. The root frame can be obtained from the parameter server with `rosparam get ${controller_ns}/root_frame`. - -To send a new reference pose 0.6m above the root frame pointing into the z-direction of it, execute this: - -```bash -rostopic pub --once /${controller_ns}/reference_pose geometry_msgs/PoseStamped "header: - seq: 0 - stamp: - secs: 0 - nsecs: 0 - frame_id: '' -pose: - position: - x: 0.0 - y: 0.0 - z: 0.6 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 1.0" -``` - -Most often one wants to have a controlled way to set reference poses. Once can for example use a [trajectory generator for Cartesian trajectories](https://git.cs.lth.se/robotlab/cartesian_trajectory_generator). - -#### Cartesian Stiffness - -In order to set only the Cartesian stiffnesses, once can send a `geometry_msgs/WrenchStamped` to `set_cartesian_stiffness`: - -```bash -rostopic pub --once /${controller_ns}/set_cartesian_stiffness geometry_msgs/WrenchStamped "header: - seq: 0 - stamp: - secs: 0 - nsecs: 0 - frame_id: '' -wrench: - force: - x: 300.0 - y: 300.0 - z: 300.0 - torque: - x: 30.0 - y: 30.0 - z: 30.0" -``` - -#### Cartesian Damping factors - -The damping factors can be configured with a `geometry_msgs/WrenchStamped` msg similar to the stiffnesses to the topic `${controller_ns}/set_damping_factors`. Damping factors are in the interval [0.01,2]. These damping factors are additionally applied to the damping rule which is `2*sqrt(stiffness)`. - -#### Stiffnesses, damping and nullspace at once -When also setting the nullspace stiffnes, a custom messages of the type `cartesian_impedance_controller/ControllerConfig` is to be sent to `set_config`: - -```bash -rostopic pub --once /${controller_ns}/set_config cartesian_impedance_controller/ControllerConfig "cartesian_stiffness: - force: {x: 300.0, y: 300.0, z: 300.0} - torque: {x: 30.0, y: 30.0, z: 30.0} -cartesian_damping_factors: - force: {x: 1.0, y: 1.0, z: 1.0} - torque: {x: 1.0, y: 1.0, z: 1.0} -nullspace_stiffness: 10.0 -nullspace_damping_factor: 0.1 -q_d_nullspace: [0, 0, 0, 0, 0, 0, 0]" -``` - -`q_d_nullspace` is the nullspace joint configuration, so the joint configuration that should be achieved if the nullspace allows it. Note that the end-effector pose would deviate if the forward kinematics of this joint configuration do not overlap with it. - -#### Cartesian Wrenches - -A Cartesian wrench can be commanded by sending a `geometry_msgs/WrenchStamped` to the topic `${controller_ns}/set_cartesian_wrench`. -Internally the wrenches are applied in the root frame. Therefore wrench messages are transformed into the root frame using `tf`.
-**Note:** An empty field `frame_id` is interpreted as end-effector frame since this is the most applicable one when working with a manipulator.
-**Note:** The wrenches are transformed into the root frame when they arrive, but not after that. E.g. end-effector wrenches might need to get updated. - -```bash -rostopic pub --once /${controller_ns}/set_cartesian_wrench geometry_msgs/WrenchStamped "header: - seq: 0 - stamp: - secs: 0 - nsecs: 0 - frame_id: '' -wrench: - force: - x: 0.0 - y: 0.0 - z: 5.0 - torque: - x: 0.0 - y: 0.0 - z: 0.0" -``` - -### Trajectories and MoveIt - -If `handle_trajectories` is not disabled, the controller can also execute trajectories. An action server is spun up at `${controller_ns}/follow_joint_trajectory` and a fire-and-forget topic for the message type `trajectory_msgs/JointTrajectory` is at `${controller_ns}/joint_trajectory`. - -In order to use it with `MoveIt` its controller configuration ([example in iiwa_ros](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_moveit/config/EffortJointInterface_controllers.yaml)) needs to look somewhat like this: -```yaml -controller_list: - - name: ${controller_ns} - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - iiwa_joint_1 - - iiwa_joint_2 - - iiwa_joint_3 - - iiwa_joint_4 - - iiwa_joint_5 - - iiwa_joint_6 - - iiwa_joint_7 -``` - -**Note:** A nullspace stiffness needs to be specified so that the arm also follows the joint configuration and not just the end-effector pose. - -## Safety -We have used the controller with Cartesian translational stiffnesses of up to 1000 N/m and experienced it as very stable. It is also stable in singularities. - -One additional measure can be to limit the maximum joint torques that can be applied by the robot arm in the URDF. On our KUKA iiwas we limit the maximum torque of each joint to 20 Nm, which allows a human operator to easily interfere at any time just by grabbing the arm and moving it. - -When using `iiwa_ros`, these limits can be applied [here](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_description/urdf/iiwa7.xacro#L53-L59). For the Panda they are applied [here](https://github.com/frankaemika/franka_ros/blob/develop/franka_description/robots/panda/joint_limits.yaml#L6). Both arms automatically apply gravity compensation, the limits are only used for the task-level torques on top of that. - -## Documentation -The source code comes with Doxygen documentation. In a `catkin` workspace it can be built with: -```bash -sudo apt-get install ros-$ROS_DISTRO-rosdoc-lite -roscd cartesian_impedance_controller -rosdoc_lite . -``` -It can then be found in the `doc` folder with `doc/html/index.html` being the entry point. - -The documentation for the public Github repository is automatically built and is deployed at:
-https://matthias-mayr.github.io/Cartesian-Impedance-Controller/ - -## Repository and Contributions -The main public code repository is at: https://github.com/matthias-mayr/Cartesian-Impedance-Controller - -Issues, questions and pull requests are welcome. Feel free to contact the main author at `firstname.lastname@cs.lth.se` if you are using this implementation. - -## Citing this Work -A brief paper about the features and the control theory is accepted at [JOSS](https://joss.theoj.org/papers/10.21105/joss.05194). -If you are using it or interacting with it, we would appreciate a citation: -``` -Mayr et al., (2024). A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators. Journal of Open Source Software, 9(93), 5194, https://doi.org/10.21105/joss.05194 -``` - -```bibtex -@article{mayr2024cartesian, - doi = {10.21105/joss.05194}, - url = {https://doi.org/10.21105/joss.05194}, - year = {2024}, - publisher = {The Open Journal}, - volume = {9}, - number = {93}, - pages = {5194}, - author = {Matthias Mayr and Julian M. Salt-Ducaju}, - title = {A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators}, journal = {Journal of Open Source Software} -} -``` - -## Troubleshooting -### Compilation - A required package was not found - -catkin build shows this CMake Error: -``` -CMake Error at /usr/share/cmake-3.16/Modules/FindPkgConfig.cmake:463 (message): - A required package was not found -Call Stack (most recent call first): - /usr/share/cmake-3.16/Modules/FindPkgConfig.cmake:643 (_pkg_check_modules_internal) - CMakeLists.txt:12 (pkg_check_modules) -``` - -There are missing dependencies. When replacing `catkin_ws` with your workspace, they can be resolved like this : -``` -cd catkin_ws -src/cartesian_impedance_controller/scripts/install_dependencies.sh -rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y -``` - -### RBDyn Library not found - -When starting the controller, this message appears: -``` - [ControllerManager::loadController]: Could not load class 'cartesian_impedance_controller/CartesianImpedanceController': Failed to load library /home/matthias/catkin_ws/devel/lib//libcartesian_impedance_controller_ros.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Pocoexception = libRBDyn.so.1: cannot open shared object file: No such file or directory) /gazebo: [ControllerManager::loadController]: Could not load controller 'CartesianImpedance_trajectory_controller' because controller type 'cartesian_impedance_controller/CartesianImpedanceController' does exist. - ``` - - This happens when a shared library can not be found. They are installed with `scripts/install_dependencies.sh`. The dynamic linker has a cache and we now manually update it by calling `ldconfig` after the installation. - - ### Controller crashes - - Most likely this happens because some parts of the stack like `iiwa_ros` or `RBDyn` were built with `SIMD`/`march=native` being turned on and other parts are not. Everything needs to be built with or without this option in order to have working alignment. This package builds without, because it is otherwise cumbersome for people to ensure that this happens across the whole stack. diff --git a/cartesian_impedance_controller_plugin.xml b/cartesian_impedance_controller_plugin.xml new file mode 100644 index 0000000..47d2454 --- /dev/null +++ b/cartesian_impedance_controller_plugin.xml @@ -0,0 +1,7 @@ + + + Cartesian impedance controller implementation + + \ No newline at end of file diff --git a/cfg/damping.cfg b/cfg/damping.cfg deleted file mode 100644 index ac88630..0000000 --- a/cfg/damping.cfg +++ /dev/null @@ -1,21 +0,0 @@ -from dynamic_reconfigure.parameter_generator_catkin import * -PACKAGE = "cartesian_impedance_controller" - - -gen = ParameterGenerator() -group_translation = gen.add_group("Set translational damping factors", type="hidden") -group_rotation = gen.add_group("Set rotational damping factors", type="hidden") -group_nullspace_ = gen.add_group("Set nullspace damping factor", type="hidden") -group_apply_ = gen.add_group("Update the damping factors", type="hidden") -group_translation.add("translation_x", double_t, 0, "translational damping", 1, 0, 1) -group_translation.add("translation_y", double_t, 0, "translational damping", 1, 0, 1) -group_translation.add("translation_z", double_t, 0, "translational damping", 1, 0, 1) - -group_rotation.add("rotation_x", double_t, 0, "rotational stiffness", 1, 0, 1) -group_rotation.add("rotation_y", double_t, 0, "rotational stiffness", 1, 0, 1) -group_rotation.add("rotation_z", double_t, 0, "rotational stiffness", 1, 0, 1) - -group_nullspace_.add("nullspace_damping", double_t, 0, "nullspace damping", 1, 0, 1) -group_apply_.add("update_damping_factors", bool_t, 0, "Update damping factors", False) - -exit(gen.generate(PACKAGE, "cartesian_impedance_controller", "damping")) diff --git a/cfg/stiffness.cfg b/cfg/stiffness.cfg deleted file mode 100644 index cbf26e4..0000000 --- a/cfg/stiffness.cfg +++ /dev/null @@ -1,20 +0,0 @@ -from dynamic_reconfigure.parameter_generator_catkin import * -PACKAGE = "cartesian_impedance_controller" - -gen = ParameterGenerator() -group_translation = gen.add_group("Set translational stiffness", type="hidden") -group_rotation = gen.add_group("Set rotational stiffness", type="hidden") -group_nullspace_ = gen.add_group("Set nullspace stiffness", type="hidden") -group_apply_ = gen.add_group("Update the stiffness", type="hidden") -group_translation.add("translation_x", double_t, 0, "translational stiffness", 200, 0, 2000) -group_translation.add("translation_y", double_t, 0, "translational stiffness", 200, 0, 2000) -group_translation.add("translation_z", double_t, 0, "translational stiffness", 200, 0, 2000) - -group_rotation.add("rotation_x", double_t, 0, "rotational stiffness", 20, 0, 300) -group_rotation.add("rotation_y", double_t, 0, "rotational stiffness", 20, 0, 300) -group_rotation.add("rotation_z", double_t, 0, "rotational stiffness", 20, 0, 300) - -group_nullspace_.add("nullspace_stiffness", double_t, 0, "nullspace stiffness", 0, 0, 50) -group_apply_.add("update_stiffness", bool_t, 0, "Update stiffness", False) - -exit(gen.generate(PACKAGE, "cartesian_impedance_controller", "stiffness")) diff --git a/cfg/wrench.cfg b/cfg/wrench.cfg deleted file mode 100644 index 4d02038..0000000 --- a/cfg/wrench.cfg +++ /dev/null @@ -1,17 +0,0 @@ -from dynamic_reconfigure.parameter_generator_catkin import * -PACKAGE = "cartesian_impedance_controller" - -gen = ParameterGenerator() - -group_apply_ = gen.add_group("Apply the Cartesian wrench", type="hidden") -group_wrench_ = gen.add_group("Cartesian Wrench in End effector Frame", type="hidden") -group_wrench_.add("f_x", double_t, 0, "Apply a force in the x-direction", 0, -30, 30) -group_wrench_.add("f_y", double_t, 0, "Apply a force in the y-direction", 0, -30, 30) -group_wrench_.add("f_z", double_t, 0, "Apply a force in the z-direction", 0, -30, 30) -group_wrench_.add("tau_x", double_t, 0, "Apply a torque around the x direction", 0, -10, 10) -group_wrench_.add("tau_y", double_t, 0, "Apply a torque around the y direction", 0, -10, 10) -group_wrench_.add("tau_z", double_t, 0, "Apply a torque around the z direction", 0, -10, 10) - -group_apply_.add("apply_wrench", bool_t, 0, "Apply Cartesian Wrench", False) - -exit(gen.generate(PACKAGE, "cartesian_impedance_controller", "wrench")) diff --git a/controller_plugins.xml b/controller_plugins.xml deleted file mode 100644 index 6a66bee..0000000 --- a/controller_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - \ No newline at end of file diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.h b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp similarity index 93% rename from include/cartesian_impedance_controller/cartesian_impedance_controller.h rename to include/cartesian_impedance_controller/cartesian_impedance_controller.hpp index 969333b..3330ebb 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.h +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp @@ -26,20 +26,20 @@ namespace cartesian_impedance_controller void initNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target); /*! \brief Sets the number of joints - * + * * \param[in] n_joints Number of joints */ void setNumberOfJoints(size_t n_joints); /*! \brief Set the desired diagonal stiffnessess + nullspace stiffness - * + * * \param[in] stiffness Stiffnesses: position, orientation, nullspace * \param[in] auto_damping Apply automatic damping */ void setStiffness(const Eigen::Matrix &stiffness, bool auto_damping = true); /*! \brief Sets the Cartesian and nullspace stiffnesses - * + * * \param[in] t_x Translational stiffness x * \param[in] t_y Translational stiffness y * \param[in] t_z Translational stiffness z @@ -52,7 +52,7 @@ namespace cartesian_impedance_controller void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, double n, bool auto_damping = true); /*! \brief Sets the Cartesian and nullspace stiffnesses - * + * * \param[in] t_x Translational stiffness x * \param[in] t_y Translational stiffness y * \param[in] t_z Translational stiffness z @@ -64,7 +64,7 @@ namespace cartesian_impedance_controller void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, bool auto_damping = true); /*! \brief Set the desired damping factors - * + * * \param[in] t_x Translational damping x * \param[in] t_y Translational damping y * \param[in] t_z Translational damping z @@ -103,14 +103,14 @@ namespace cartesian_impedance_controller double filter_params_wrench); /*! \brief Maximum commanded torque change per time step - * + * * Prevents too large changes in the commanded torques by using saturation. * \param[in] d Torque change per timestep */ void setMaxTorqueDelta(double d); /*! \brief Sets maximum commanded torque change per time step and the update frequency - * + * * Prevents too large changes in the commanded torques by using saturation. * \param[in] d Torque change per timestep * \param[in] update_frequency Update frequency @@ -118,21 +118,21 @@ namespace cartesian_impedance_controller void setMaxTorqueDelta(double d, double update_frequency); /*! \brief Apply a virtual Cartesian wrench in the root frame (often "world") - * + * * Prevents too large changes in the commanded torques by using saturation. * \param[in] cartesian_wrench Wrench to apply */ void applyWrench(const Eigen::Matrix &cartesian_wrench); /*! \brief Returns the commanded torques. Performs a filtering step. - * + * * This function assumes that the internal states have already been updates. The it utilizes the control rules to calculate commands. * \return Eigen Vector of the commanded torques */ Eigen::VectorXd calculateCommandedTorques(); /*! \brief Returns the commanded torques. Performs a filtering step and updates internal state. - * + * * This function utilizes the control rules. * \param[in] q Joint positions * \param[in] dq Joint velocities @@ -145,7 +145,7 @@ namespace cartesian_impedance_controller const Eigen::MatrixXd &jacobian); /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called - * + * * \param[out] q Joint positions * \param[out] dq Joint velocities * \param[out] position End-effector position @@ -163,7 +163,7 @@ namespace cartesian_impedance_controller Eigen::VectorXd *q_d_nullspace, Eigen::Matrix *cartesian_damping) const; /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called - * + * * \param[out] position_d End-effector reference position * \param[out] orientation_d End-effector reference orientation * \param[out] cartesian_stiffness Cartesian stiffness @@ -176,19 +176,19 @@ namespace cartesian_impedance_controller Eigen::VectorXd *q_d_nullspace, Eigen::Matrix *cartesian_damping) const; /*! \brief Get the currently applied commands - * + * * \return Eigen Vector with commands */ Eigen::VectorXd getLastCommands() const; /*! \brief Get the currently applied Cartesian wrench - * + * * \return Eigen Vector with the applied wrench */ Eigen::Matrix getAppliedWrench() const; /*! \brief Get the current pose error - * + * * \return Eigen Vector with the pose error for translation and rotation */ Eigen::Matrix getPoseError() const; @@ -196,15 +196,16 @@ namespace cartesian_impedance_controller protected: size_t n_joints_{7}; //!< Number of joints to control - Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness matrix - Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; //!< Cartesian damping matrix - + // Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness matrix + // Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; //!< Cartesian damping matrix +Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; +Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; Eigen::VectorXd q_d_nullspace_; //!< Current nullspace reference pose Eigen::VectorXd q_d_nullspace_target_; //!< Nullspace reference target pose - double nullspace_stiffness_{0.0}; //!< Current nullspace stiffness - double nullspace_stiffness_target_{0.0}; //!< Nullspace stiffness target - double nullspace_damping_{0.0}; //!< Current nullspace damping - double nullspace_damping_target_{0.0}; //!< Nullspace damping target + double nullspace_stiffness_{1.0}; //!< Current nullspace stiffness + double nullspace_stiffness_target_{1.0}; //!< Nullspace stiffness target + double nullspace_damping_{1.0}; //!< Current nullspace damping + double nullspace_damping_target_{1.0}; //!< Nullspace damping target Eigen::Matrix cartesian_stiffness_target_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness target Eigen::Matrix cartesian_damping_target_{Eigen::Matrix::Identity()}; //!< Cartesian damping target @@ -237,7 +238,7 @@ namespace cartesian_impedance_controller double filter_params_pose_{1.0}; //!< Reference pose filtering double filter_params_wrench_{1.0}; //!< Commanded wrench filtering - double delta_tau_max_{1.0}; //!< Maximum allowed torque change per time step + double delta_tau_max_{100.0}; //!< Maximum allowed torque change per time step private: /*! \brief Implements the damping based on a stiffness diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h deleted file mode 100644 index 99b119b..0000000 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h +++ /dev/null @@ -1,350 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -namespace cartesian_impedance_controller -{ - /*! \brief The ROS control implementation of the Cartesian impedance controller - * - * It utilizes a list of joint names and the URDF description to control these joints. - */ - class CartesianImpedanceControllerRos - : public controller_interface::Controller, public CartesianImpedanceController - { - - public: - /*! \brief Initializes the controller - * - * - Reads ROS parameters - * - Initializes - * - joint handles - * - ROS messaging - * - RBDyn - * - rqt_reconfigure - * - Trajectory handling - * \param[in] hw Hardware interface - * \param[in] node_handle Node Handle - * \return True on success, false on failure - */ - bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) override; - - /*! \brief Starts the controller - * - * Updates the states and sets the desired pose and nullspace configuration to the current state. - * \param[in] time Not used - */ - void starting(const ros::Time &) override; - - /*! \brief Periodically called update function - * - * Updates the state and the trajectory. Calculated new commands and sets them. - * Finally publishes ROS messages and tf transformations. - * \param[in] time Not used - * \param[in] period Control period - */ - void update(const ros::Time &, const ros::Duration &period) override; - - private: - /*! \brief Initializes dynamic reconfigure - * - * Initiliazes dynamic reconfigure for stiffness, damping and wrench. - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initDynamicReconfigure(const ros::NodeHandle &nh); - - /*! \brief Initializes the joint handles - * - * Fetches the joint names from the parameter server and initializes the joint handles. - * \param[in] hw Hardware interface to obtain handles - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh); - - /*! \brief Initializes messaging - * - * Initializes realtime publishers and the subscribers. - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initMessaging(ros::NodeHandle *nh); - - /*! \brief Initializes RBDyn - * - * Reads the robot URDF and initializes RBDyn. - * \param[in] nh Nodehandle - * \return True on success, false on failure. - */ - bool initRBDyn(const ros::NodeHandle &nh); - - /*! \brief Initializes trajectory handling - * - * Subscribes to joint trajectory topic and starts the trajectory action server. - * \param[in] nh Nodehandle - * \return Always true. - */ - bool initTrajectories(ros::NodeHandle *nh); - - /*! \brief Get forward kinematics solution. - * - * Calls RBDyn to get the forward kinematics solution. - * \param[in] q Joint position vector - * \param[out] position End-effector position - * \param[out] orientation End-effector orientation - * \return Always true. - */ - bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *rotation) const; - - /*! \brief Get Jacobian from RBDyn - * - * Gets the Jacobian for given joint positions and joint velocities. - * \param[in] q Joint position vector - * \param[in] dq Joint velocity vector - * \param[out] jacobian Calculated Jacobian - * \return True on success, false on failure. - */ - bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); - - /*! \brief Updates the state based on the joint handles. - * - * Gets latest joint positions, velocities and efforts and updates the forward kinematics as well as the Jacobian. - */ - void updateState(); - - /*! \brief Sets damping for Cartesian space and nullspace. - * - * Long - * \param[in] cart_damping Cartesian damping [0,1] - * \param[in] nullspace Nullspace damping [0,1] - */ - void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace); - - /*! \brief Sets Cartesian and nullspace stiffness - * - * Sets Cartesian and nullspace stiffness. Allows to set if automatic damping should be applied. - * \param[in] cart_stiffness Cartesian stiffness - * \param[in] nullspace Nullspace stiffness - * \param[in] auto_damping Apply automatic damping - */ - void setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping = true); - - /*! \brief Message callback for Cartesian damping. - * - * Calls setDampingFactors function. - * @sa setDampingFactors. - * \param[in] msg Received message - */ - void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg); - - /*! \brief Message callback for Cartesian stiffness. - * - * Calls setStiffness function. - * @sa setStiffness - * \param[in] msg Received message - */ - void cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg); - - /*! \brief Message callback for the whole controller configuration. - * - * Sets stiffness, damping and nullspace. - * @sa setDampingFactors, setStiffness - * \param[in] msg Received message - */ - void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg); - - /*! \brief Message callback for a Cartesian reference pose. - * - * Accepts new reference poses in the root frame - ignores them otherwise. - * Sets the reference target pose. - * @sa setReferencePose. - * \param[in] msg Received message - */ - void referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg); - - /*! \brief Message callback for Cartesian wrench messages. - * - * If the wrench is not given in end-effector frame, it will be transformed in the root frame. Once when a new wrench message arrives. - * Sets the wrench using the base library. - * @sa applyWrench. - * \param[in] msg Received message - */ - void wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg); - - /*! \brief Transforms the wrench in a target frame. - * - * Takes a vector with the wrench and transforms it to a given coordinate frame. E.g. from_frame= "world" , to_frame = "bh_link_ee" - - * @sa wrenchCommandCb - * \param[in] cartesian_wrench Vector with the Cartesian wrench - * \param[in] from_frame Source frame - * \param[in] to_frame Target frame - * \return True on success, false on failure. - */ - bool transformWrench(Eigen::Matrix *cartesian_wrench, const std::string &from_frame, const std::string &to_frame) const; - - /*! \brief Verbose printing; publishes ROS messages and tf frames. - * - * Always publishes commanded torques. - * Optional: request publishes tf frames for end-effector forward kinematics and the reference pose. - * Optional: verbose printing - * Optional: publishes state messages - */ - void publishMsgsAndTf(); - - /*! \brief Callback for stiffness dynamic reconfigure. - * - * Takes the dynamic reconfigure stiffness configuration, applies the limits and sets it. - * \param[in] config - */ - void dynamicStiffnessCb(cartesian_impedance_controller::stiffnessConfig &config, uint32_t level); - - /*! \brief Callback for damping dynamic reconfigure. - * - * Takes the dynamic reconfigure configuration, applies limits and sets it. - * \param[in] config - */ - void dynamicDampingCb(cartesian_impedance_controller::dampingConfig &config, uint32_t level); - - /*! \brief Callback for wrench dynamic reconfigure. - * - * Takes the dynamic reconfigure configuration, applies limits and sets it. - * \param[in] config - */ - void dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, uint32_t level); - - /*! \brief Callback for a joint trajectory message. - * - * Preempts the action server if that one has a running goal. - * \param[in] msg Joint Trajectory Message - */ - void trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg); - - /*! \brief Callback for a trajectory action goal. - * - * Accepts the new goal and starts the trajectory. - */ - void trajGoalCb(); - - /*! \brief Preempt function of the action server. - * - * Sets the goal as preempted. - */ - void trajPreemptCb(); - - /*! \brief Starts the trajectory. - * - * Resets the trajectory member variables. - */ - void trajStart(const trajectory_msgs::JointTrajectory &trajectory); - - /*! \brief Updates the trajectory. - * - * Called periodically from the update function if a trajectory is running. - * A trajectory is run by going through it point by point, calculating forward kinematics and applying - * the joint configuration to the nullspace control. - */ - void trajUpdate(); - - std::vector joint_handles_; //!< Joint handles for states and commands - rbdyn_wrapper rbdyn_wrapper_; //!< Wrapper for RBDyn library for kinematics - std::string end_effector_; //!< End-effector link name - std::string robot_description_; //!< URDF of the robot - std::string root_frame_; //!< Base frame obtained from URDF - - Eigen::VectorXd tau_m_; //!< Measured joint torques - - ros::Subscriber sub_cart_stiffness_; //!< Cartesian stiffness subscriber - ros::Subscriber sub_cart_wrench_; //!< Cartesian wrench subscriber - ros::Subscriber sub_damping_factors_; //!< Damping subscriber - ros::Subscriber sub_controller_config_; //!< Controller configuration subscriber - ros::Subscriber sub_reference_pose_; //!< Cartesian reference pose subscriber - - tf::TransformListener tf_listener_; //!< tf transformation listener - std::string wrench_ee_frame_; //!< Frame for the application of the commanded wrench - - // Hard limits. They are enforced on input. - const double trans_stf_min_{0}; //!< Minimum translational stiffness - const double trans_stf_max_{1500}; //!< Maximum translational stiffness - const double rot_stf_min_{0}; //!< Minimum rotational stiffness - const double rot_stf_max_{100}; //!< Maximum rotational stiffness - const double ns_min_{0}; //!< Minimum nullspace stiffness - const double ns_max_{100}; //!< Maximum nullspace stiffness - const double dmp_factor_min_{0.001}; //!< Minimum damping factor - const double dmp_factor_max_{2.0}; //!< Maximum damping factor - - // The Jacobian of RBDyn comes with orientation in the first three lines. Needs to be interchanged. - const Eigen::VectorXi perm_indices_ = - (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); //!< Permutation indices to switch position and orientation - const Eigen::PermutationMatrix jacobian_perm_ = - Eigen::PermutationMatrix(perm_indices_); //!< Permutation matrix to switch position and orientation entries - - // Dynamic reconfigure - std::unique_ptr> - dynamic_server_compliance_param_; //!< Dybanic reconfigure server for stiffness - std::unique_ptr> - dynamic_server_damping_param_; //!< Dynamic reconfigure server for damping - std::unique_ptr> - dynamic_server_wrench_param_; //!< Dynamic reconfigure server for commanded wrench - - // Trajectory handling - ros::Subscriber sub_trajectory_; //!< Subscriber for a single trajectory - std::unique_ptr> traj_as_; //!< Trajectory action server - boost::shared_ptr traj_as_goal_; //!< Trajectory action server goal - trajectory_msgs::JointTrajectory trajectory_; //!< Currently played trajectory - ros::Time traj_start_; //!< Time the current trajectory is started - ros::Duration traj_duration_; //!< Duration of the current trajectory - unsigned int traj_index_{0}; //!< Index of the current trajectory point - bool traj_running_{false}; //!< True when running a trajectory - - // Extra output - bool verbose_print_{false}; //!< Verbose printing enabled - bool verbose_state_{false}; //!< Verbose state messages enabled - bool verbose_tf_{false}; //!< Verbose tf pubishing enabled - tf::TransformBroadcaster tf_br_; //!< tf transform broadcaster for verbose tf - realtime_tools::RealtimePublisher pub_torques_; //!< Realtime publisher for commanded torques - realtime_tools::RealtimePublisher pub_state_; //!< Realtime publisher for controller state - tf::Transform tf_br_transform_; //!< tf transform for publishing - tf::Vector3 tf_pos_; //!< tf position for publishing - tf::Quaternion tf_rot_; //!< tf orientation for publishing - ros::Time tf_last_time_ = ros::Time::now(); //!< Last published tf message - }; - - // Declares this controller - PLUGINLIB_EXPORT_CLASS(cartesian_impedance_controller::CartesianImpedanceControllerRos, - controller_interface::ControllerBase); - -} // namespace cartesian_impedance_controller diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp new file mode 100644 index 0000000..f3a4d59 --- /dev/null +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -0,0 +1,142 @@ +#ifndef CARTESIAN_IMPEDANCE_CONTROLLER__CARTESIAN_IMPEDANCE_CONTROLLER_ROS_HPP_ +#define CARTESIAN_IMPEDANCE_CONTROLLER__CARTESIAN_IMPEDANCE_CONTROLLER_ROS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "cartesian_impedance_controller/cartesian_impedance_controller.hpp" +#include "cartesian_impedance_controller/rbdyn_wrapper.h" +#include + +namespace cartesian_impedance_controller { + +class ParamListener; + +class CartesianImpedanceControllerRos : public controller_interface::ControllerInterface, + public CartesianImpedanceController +{ +public: + CartesianImpedanceControllerRos(); + ~CartesianImpedanceControllerRos() override; + + controller_interface::CallbackReturn on_init() override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::string urdf_; + + const Eigen::VectorXi perm_indices_ = + (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); + const Eigen::PermutationMatrix jacobian_perm_ = + Eigen::PermutationMatrix(perm_indices_); + + std::vector joint_names_; + std::vector command_joint_names_; + size_t dof_{0}; + std::shared_ptr> rt_trajectory_; + + std::vector joint_command_handles_; + std::vector joint_position_state_; + std::vector joint_velocity_state_; + std::vector joint_effort_state_; + + std::shared_ptr parameter_handler_; + Params params_; + + std::string end_effector_; + std::string wrench_ee_frame_; + std::string root_frame_; + std::vector joints_angle_wraparound_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rbdyn_wrapper rbdyn_wrapper_; + + rclcpp::Publisher::SharedPtr pub_torques_; + + rclcpp::Subscription::SharedPtr sub_cart_stiffness_; + rclcpp::Subscription::SharedPtr sub_cart_wrench_; + rclcpp::Subscription::SharedPtr sub_damping_factors_; + rclcpp::Subscription::SharedPtr sub_reference_pose_; + rclcpp::Subscription::SharedPtr trajectory_sub_; + + rclcpp_action::Server::SharedPtr traj_as_; + bool traj_running_ = false; + bool traj_as_active_ = false; + std::shared_ptr> traj_as_goal_; + + trajectory_msgs::msg::JointTrajectory trajectory_; + size_t traj_index_ = 0; + rclcpp::Time traj_start_time_; + rclcpp::Duration traj_duration_; + Eigen::Vector3d position_d_target_; + Eigen::Quaterniond orientation_d_target_; + + Eigen::VectorXd tau_m_; + Eigen::VectorXd damping_factors_; + double nullspace_stiffness_target_; + + void read_state_from_hardware(); + void write_command_to_hardware(); + void write_zero_commands_to_hardware(); + bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *orientation); + bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); + + void cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg); + void cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg); + rclcpp_action::GoalResponse trajGoalCb(const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse trajCancelCb(const std::shared_ptr> goal_handle); + void trajAcceptCb(std::shared_ptr> goal_handle); + void trajStart(const trajectory_msgs::msg::JointTrajectory & trajectory); + void trajUpdate(); + + bool transformWrench(Eigen::Matrix * wrench, + const std::string & from_frame, + const std::string & to_frame) const; + void publishMsgs(); + void applyRuntimeParameters(); + bool initRBDyn(); +}; + +} // namespace cartesian_impedance_controller + +#endif // CARTESIAN_IMPEDANCE_CONTROLLER__CARTESIAN_IMPEDANCE_CONTROLLER_ROS_HPP_ \ No newline at end of file diff --git a/include/cartesian_impedance_controller/rbdyn_wrapper.h b/include/cartesian_impedance_controller/rbdyn_wrapper.h index 1476634..cc6bf3c 100644 --- a/include/cartesian_impedance_controller/rbdyn_wrapper.h +++ b/include/cartesian_impedance_controller/rbdyn_wrapper.h @@ -1,6 +1,8 @@ #pragma once #include +#include +#include #include #include @@ -18,31 +20,33 @@ class rbdyn_wrapper Eigen::Quaterniond orientation; }; - void init_rbdyn(const std::string &urdf_string, const std::string &end_effector) + void init_rbdyn(const std::string& urdf_string, const std::string& end_effector) { // Convert URDF to RBDyn _rbdyn_urdf = mc_rbdyn_urdf::rbdyn_from_urdf(urdf_string); _rbd_indices.clear(); - for (size_t i = 0; i < _rbdyn_urdf.mb.nrJoints(); i++) + int nrJoints = _rbdyn_urdf.mb.nrJoints(); + for (int i = 0; i < nrJoints; ++i) { if (_rbdyn_urdf.mb.joint(i).type() != rbd::Joint::Fixed) - _rbd_indices.push_back(i); + _rbd_indices.push_back(static_cast(i)); } - for (size_t i = 0; i < _rbdyn_urdf.mb.nrBodies(); i++) + int nrBodies = _rbdyn_urdf.mb.nrBodies(); + for (int i = 0; i < nrBodies; ++i) { if (_rbdyn_urdf.mb.body(i).name() == end_effector) { - _ef_index = i; + _ef_index = static_cast(i); return; } } throw std::runtime_error("Index for end effector link " + end_effector + " not found in URDF. Aborting."); } - Eigen::MatrixXd jacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq) + Eigen::MatrixXd jacobian(const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { mc_rbdyn_urdf::URDFParserResult rbdyn_urdf = _rbdyn_urdf; @@ -51,7 +55,7 @@ class rbdyn_wrapper _update_urdf_state(rbdyn_urdf, q, dq); // Compute jacobian - rbd::Jacobian jac(rbdyn_urdf.mb, rbdyn_urdf.mb.body(_ef_index).name()); + rbd::Jacobian jac(rbdyn_urdf.mb, rbdyn_urdf.mb.body(static_cast(_ef_index)).name()); // // TO-DO: Check if we need this rbd::forwardKinematics(rbdyn_urdf.mb, rbdyn_urdf.mbc); @@ -60,25 +64,26 @@ class rbdyn_wrapper return jac.jacobian(rbdyn_urdf.mb, rbdyn_urdf.mbc); } - EefState perform_fk(const Eigen::VectorXd &q) const + EefState perform_fk(const Eigen::VectorXd& q) const { mc_rbdyn_urdf::URDFParserResult rbdyn_urdf = _rbdyn_urdf; Eigen::VectorXd q_low = Eigen::VectorXd::Ones(_rbd_indices.size()); Eigen::VectorXd q_high = q_low; - for (size_t i = 0; i < _rbd_indices.size(); i++) + size_t n_ctrl = _rbd_indices.size(); + for (size_t i = 0; i < n_ctrl; ++i) { - size_t index = _rbd_indices[i]; + int index = static_cast(_rbd_indices[i]); q_low(i) = rbdyn_urdf.limits.lower[rbdyn_urdf.mb.joint(index).name()][0]; q_high(i) = rbdyn_urdf.limits.upper[rbdyn_urdf.mb.joint(index).name()][0]; } rbdyn_urdf.mbc.zero(rbdyn_urdf.mb); - for (size_t i = 0; i < _rbd_indices.size(); i++) + for (size_t i = 0; i < n_ctrl; ++i) { - size_t rbd_index = _rbd_indices[i]; + int rbd_index = static_cast(_rbd_indices[i]); double jt = q[i]; // wrap in [-pi,pi] jt = _wrap_angle(jt); @@ -93,17 +98,17 @@ class rbdyn_wrapper rbd::forwardKinematics(rbdyn_urdf.mb, rbdyn_urdf.mbc); - sva::PTransformd tf = rbdyn_urdf.mbc.bodyPosW[_ef_index]; + sva::PTransformd tf = rbdyn_urdf.mbc.bodyPosW[static_cast(_ef_index)]; Eigen::Matrix4d eig_tf = sva::conversions::toHomogeneous(tf); Eigen::Vector3d trans = eig_tf.col(3).head(3); Eigen::Matrix3d rot_mat = eig_tf.block(0, 0, 3, 3); Eigen::Quaterniond quat = Eigen::Quaterniond(rot_mat).normalized(); - return {trans, quat}; + return { trans, quat }; } - int n_joints() const + size_t n_joints() const { return _rbd_indices.size(); } @@ -114,12 +119,13 @@ class rbdyn_wrapper } private: - void _update_urdf_state(mc_rbdyn_urdf::URDFParserResult &rbdyn_urdf, const Eigen::VectorXd &q, - const Eigen::VectorXd &dq) + void _update_urdf_state(mc_rbdyn_urdf::URDFParserResult& rbdyn_urdf, const Eigen::VectorXd& q, + const Eigen::VectorXd& dq) { - for (size_t i = 0; i < _rbd_indices.size(); i++) + size_t n_ctrl = _rbd_indices.size(); + for (size_t i = 0; i < n_ctrl; ++i) { - size_t rbd_index = _rbd_indices[i]; + int rbd_index = static_cast(_rbd_indices[i]); if (q.size() > i) rbdyn_urdf.mbc.q[rbd_index][0] = q[i]; @@ -128,22 +134,9 @@ class rbdyn_wrapper } } - double _wrap_angle(const double &angle) const + double _wrap_angle(const double& angle) const { - double wrapped; - if ((angle <= M_PI) && (angle >= -M_PI)) - { - wrapped = angle; - } - else if (angle < 0.0) - { - wrapped = std::fmod(angle - M_PI, 2.0 * M_PI) + M_PI; - } - else - { - wrapped = std::fmod(angle + M_PI, 2.0 * M_PI) - M_PI; - } - return wrapped; + return std::atan2(std::sin(angle), std::cos(angle)); } mc_rbdyn_urdf::URDFParserResult _rbdyn_urdf; diff --git a/msg/ControllerConfig.msg b/msg/ControllerConfig.msg deleted file mode 100644 index 5f81f2b..0000000 --- a/msg/ControllerConfig.msg +++ /dev/null @@ -1,20 +0,0 @@ -# Cartesian Stiffness values for translation and rotation of the axes x,y,z -# Values: Translation: [0.0, 2000.0] in [N/m]; Rotation [0.0; 500.0] in [Nm/rad] -geometry_msgs/Wrench cartesian_stiffness - -# Damping factor for translation and rotation of the axes x,y,z -# The rule is always 2*sqrt(stiffness) -# Values: [0.001, 2.0] -geometry_msgs/Wrench cartesian_damping_factors - -# Stiffness value for nullspace -# Values: >= 0.0 in [Nm/rad] -float64 nullspace_stiffness - -# Damping parameter for nullspace [Nm*s/rad]. -# Value: [0.001, 2.0] -float64 nullspace_damping_factor - -# The desired nullspace configuration -# Values: According to the limits of the robot -float64[] q_d_nullspace \ No newline at end of file diff --git a/msg/ControllerState.msg b/msg/ControllerState.msg deleted file mode 100644 index b81e2df..0000000 --- a/msg/ControllerState.msg +++ /dev/null @@ -1,20 +0,0 @@ -Header header - -geometry_msgs/Pose current_pose -geometry_msgs/Pose reference_pose -geometry_msgs/Pose pose_error - -sensor_msgs/JointState joint_state -float64[] commanded_torques - -# Stiffness values for the cartesian impedance, x, y, z in [N/m], x, y, z in [Nm/rad]. -geometry_msgs/Wrench cartesian_stiffness -geometry_msgs/Wrench cartesian_damping - -float64 nullspace_stiffness -float64 nullspace_damping -float64[] nullspace_config - -geometry_msgs/Wrench commanded_wrench - -float64 cartesian_velocity # Calculated Cartesian velocity [m/s] \ No newline at end of file diff --git a/package.xml b/package.xml index ceff515..6c4d729 100644 --- a/package.xml +++ b/package.xml @@ -1,69 +1,53 @@ + cartesian_impedance_controller - 1.1.1 + 0.1.0 A Cartesian Impedance controller implementation Matthias Mayr BSD-3-Clause - + Matthias Mayr Oussama Chouman Julian Salt Ducaju + Cenk Cetin - catkin - - actionlib - actionlib_msgs - control_msgs - controller_interface - controller_manager - dynamic_reconfigure - eigen - eigen_conversions - geometry_msgs - hardware_interface - message_generation - pluginlib - realtime_tools - roscpp - sensor_msgs - std_msgs - tf - tf_conversions - trajectory_msgs - - actionlib - actionlib_msgs - control_msgs - controller_interface - controller_manager - dynamic_reconfigure - eigen_conversions - geometry_msgs - hardware_interface - message_runtime - pluginlib - realtime_tools - roscpp - sensor_msgs - std_msgs - tf - tf_conversions - trajectory_msgs - yaml-cpp - - - - - + ament_cmake + rosidl_default_generators + + action_msgs + control_msgs + controller_interface + controller_manager + eigen3_cmake_module + geometry_msgs + hardware_interface + pluginlib + realtime_tools + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_ros + trajectory_msgs + ros2_control + rclcpp_lifecycle + generate_parameter_library + rosidl_default_runtime - gtest - eigen - roscpp - rostest + ament_cmake_gtest + eigen3_cmake_module + rclcpp geometry_msgs std_msgs - dynamic_reconfigure - yaml-cpp + yaml_cpp_vendor + ament_add_gmock + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index 14615b0..c8fc3b9 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -1,4 +1,4 @@ -#include +#include #include "pseudo_inversion.h" #include diff --git a/src/cartesian_impedance_controller_parameters.yaml b/src/cartesian_impedance_controller_parameters.yaml new file mode 100644 index 0000000..154daba --- /dev/null +++ b/src/cartesian_impedance_controller_parameters.yaml @@ -0,0 +1,211 @@ +cartesian_impedance_controller: + delta_tau_max: + type: double + default_value: 100.0 + description: "Maximum allowed change in joint torques." + validation: + bounds<>: [0.0, 100.0] + wrench_ee_frame: + type: string + default_value: "" + description: "Name of the end effector frame for wrench application" + + update_frequency: + type: double + default_value: 500.0 + description: "Controller update frequency in Hz." + validation: + bounds<>: [200.0, 1000.0] + + end_effector: + type: string + default_value: "panda_hand" + description: "Name of the end effector link." + + + damping: + translation: + x: + type: double + default_value: 20.0 + description: "Translational damping factor for the x-axis." + validation: + bounds<>: [0.0, 20.0] + y: + type: double + default_value: 20.0 + description: "Translational damping factor for the y-axis." + validation: + bounds<>: [0.0, 20.0] + z: + type: double + default_value: 20.0 + description: "Translational damping factor for the z-axis." + validation: + bounds<>: [0.0, 20.0] + + rotation: + x: + type: double + default_value: 20.0 + description: "Rotational damping factor around the x-axis." + validation: + bounds<>: [0.0, 20.0] + y: + type: double + default_value: 20.0 + description: "Rotational damping factor around the y-axis." + validation: + bounds<>: [0.0, 20.0] + z: + type: double + default_value: 20.0 + description: "Rotational damping factor around the z-axis." + validation: + bounds<>: [0.0, 20.0] + + nullspace_damping: + type: double + default_value: 10.0 + description: "Damping factor for the nullspace." + validation: + bounds<>: [0.0, 100.0] + + update_damping_factors: + type: bool + default_value: false + description: "Flag to update damping factors dynamically at runtime." + + stiffness: + translation: + x: + type: double + default_value: 600.0 + description: "Translational stiffness for the x-axis." + validation: + bounds<>: [0.0, 2000.0] + y: + type: double + default_value: 600.0 + description: "Translational stiffness for the y-axis." + validation: + bounds<>: [0.0, 2000.0] + z: + type: double + default_value: 600.0 + description: "Translational stiffness for the z-axis." + validation: + bounds<>: [0.0, 2000.0] + + rotation: + x: + type: double + default_value: 100.0 + description: "Rotational stiffness around the x-axis." + validation: + bounds<>: [0.0, 300.0] + y: + type: double + default_value: 100.0 + description: "Rotational stiffness around the y-axis." + validation: + bounds<>: [0.0, 300.0] + z: + type: double + default_value: 100.0 + description: "Rotational stiffness around the z-axis." + validation: + bounds<>: [0.0, 300.0] + + nullspace_stiffness: + type: double + default_value: 10.0 + description: "Stiffness factor for the nullspace." + validation: + bounds<>: [0.0, 100.0] + + update_stiffness: + type: bool + default_value: false + description: "Flag to update stiffness factors dynamically at runtime." + + wrench: + apply_wrench: + type: bool + default_value: false + description: "Flag to apply a Cartesian wrench to the end effector." + + force_x: + type: double + default_value: 0.0 + description: "Force in the x-axis direction (End Effector Frame)." + validation: + bounds<>: [-30.0, 30.0] + force_y: + type: double + default_value: 0.0 + description: "Force in the y-axis direction (End Effector Frame)." + validation: + bounds<>: [-30.0, 30.0] + force_z: + type: double + default_value: 0.0 + description: "Force in the z-axis direction (End Effector Frame)." + validation: + bounds<>: [-30.0, 30.0] + + torque_x: + type: double + default_value: 0.0 + description: "Torque around the x-axis (End Effector Frame)." + validation: + bounds<>: [-10.0, 10.0] + torque_y: + type: double + default_value: 0.0 + description: "Torque around the y-axis (End Effector Frame)." + validation: + bounds<>: [-10.0, 10.0] + torque_z: + type: double + default_value: 0.0 + description: "Torque around the z-axis (End Effector Frame)." + validation: + bounds<>: [-10.0, 10.0] + + + filtering: + nullspace: + type: double + default_value: 1.0 + description: "Filtering factor for nullspace." + validation: + bounds<>: [0.0, 1.0] + stiffness: + type: double + default_value: 1.0 + description: "Filtering factor for stiffness." + validation: + bounds<>: [0.0, 1.0] + pose: + type: double + default_value: 1.0 + description: "Filtering factor for pose." + validation: + bounds<>: [0.0, 1.0] + wrench: + type: double + default_value: 1.0 + description: "Filtering factor for wrench." + validation: + bounds<>: [0.0, 1.0] + + joints: { + type: string_array, + default_value: [], + description: "Joint names to control and listen to", + read_only: true, + validation: { + unique<>: null, + } + } \ No newline at end of file diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 6354ff0..2beb91f 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -1,598 +1,886 @@ -#include - -#include -#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/version.h" +#if RCLCPP_VERSION_GTE(29, 0, 0) +#include "urdf/model.hpp" +#else +#include "urdf/model.h" +#endif + +#include namespace cartesian_impedance_controller { - /*! \brief Saturate a variable x with the limits x_min and x_max - * - * \param[in] x Value - * \param[in] x_min Minimal value - * \param[in] x_max Maximum value - * \return Saturated value - */ - double saturateValue(double x, double x_min, double x_max) + +using CallbackReturn = controller_interface::CallbackReturn; +using namespace std::chrono_literals; + +CartesianImpedanceControllerRos::CartesianImpedanceControllerRos() + : controller_interface::ControllerInterface(), traj_duration_(0, 0) +{ + // The Eigen vectors will be resized in on_configure. + q_.resize(0); + dq_.resize(0); + tau_m_.resize(0); + tau_c_.resize(0); +} + +CartesianImpedanceControllerRos::~CartesianImpedanceControllerRos() +{ +} + +CallbackReturn CartesianImpedanceControllerRos::on_init() +{ + auto node = get_node(); + auto logger = node->get_logger(); + + try + { + parameter_handler_ = std::make_shared(node); + params_ = parameter_handler_->get_params(); + } + catch (const std::exception& e) { - return std::min(std::max(x, x_min), x_max); + RCLCPP_ERROR(logger, "Exception during init: %s", e.what()); + return CallbackReturn::ERROR; } - /*! \brief Populates a wrench msg with value from Eigen vector - * - * It is assumed that the vector has the form transl_x, transl_y, transl_z, rot_x, rot_y, rot_z - * \param[in] v Input vector - * \param[out] wrench Wrench message - */ - void EigenVectorToWrench(const Eigen::Matrix &v, geometry_msgs::Wrench *wrench) + std::string urdf_string; + if (!node->get_parameter("robot_description", urdf_string)) { - wrench->force.x = v(0); - wrench->force.y = v(1); - wrench->force.z = v(2); - wrench->torque.x = v(3); - wrench->torque.y = v(4); - wrench->torque.z = v(5); + RCLCPP_ERROR(logger, "Failed to get robot_description parameter"); + return CallbackReturn::ERROR; } + return CallbackReturn::SUCCESS; +} - bool CartesianImpedanceControllerRos::initDynamicReconfigure(const ros::NodeHandle &nh) +controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto& jn : params_.joints) { - this->dynamic_server_compliance_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/stiffness_reconfigure"))); - this->dynamic_server_compliance_param_->setCallback( - boost::bind(&CartesianImpedanceControllerRos::dynamicStiffnessCb, this, _1, _2)); + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); + } + return conf; +} - this->dynamic_server_damping_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/damping_factors_reconfigure"))); - dynamic_server_damping_param_->setCallback( - boost::bind(&CartesianImpedanceControllerRos::dynamicDampingCb, this, _1, _2)); +controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto& jn : params_.joints) + { + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_POSITION); + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_VELOCITY); + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); + } + return conf; +} - this->dynamic_server_wrench_param_ = std::make_unique>(ros::NodeHandle(std::string(nh.getNamespace() + "/cartesian_wrench_reconfigure"))); - dynamic_server_wrench_param_->setCallback( - boost::bind(&CartesianImpedanceControllerRos::dynamicWrenchCb, this, _1, _2)); - return true; +CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecycle::State&) +{ + auto node = get_node(); + auto logger = node->get_logger(); + + if (!parameter_handler_) + { + RCLCPP_ERROR(logger, "Parameter handler not initialized."); + return CallbackReturn::ERROR; } - bool CartesianImpedanceControllerRos::initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh) + rt_trajectory_ = std::make_shared>(); + parameter_handler_->refresh_dynamic_parameters(); + params_ = parameter_handler_->get_params(); + + if (params_.joints.empty()) { - std::vector joint_names; - if (!nh.getParam("joints", joint_names)) - { - ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); - return false; - } - for (size_t i = 0; i < joint_names.size(); ++i) - { - try - { - this->joint_handles_.push_back(hw->getHandle(joint_names[i])); - } - catch (const hardware_interface::HardwareInterfaceException &ex) - { - ROS_ERROR_STREAM("Exception getting joint handles: " << ex.what()); - return false; - } - } - ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); - this->setNumberOfJoints(joint_names.size()); - return true; - } - - bool CartesianImpedanceControllerRos::initMessaging(ros::NodeHandle *nh) - { - // Queue size of 1 since we are only interested in the last message - this->sub_cart_stiffness_ = nh->subscribe("set_cartesian_stiffness", 1, - &CartesianImpedanceControllerRos::cartesianStiffnessCb, this); - this->sub_cart_wrench_ = nh->subscribe("set_cartesian_wrench", 1, - &CartesianImpedanceControllerRos::wrenchCommandCb, this); - this->sub_damping_factors_ = nh->subscribe("set_damping_factors", 1, - &CartesianImpedanceControllerRos::cartesianDampingFactorCb, this); - this->sub_controller_config_ = - nh->subscribe("set_config", 1, &CartesianImpedanceControllerRos::controllerConfigCb, this); - this->sub_reference_pose_ = nh->subscribe("reference_pose", 1, &CartesianImpedanceControllerRos::referencePoseCb, this); - - // Initializing the realtime publisher and the message - this->pub_torques_.init(*nh, "commanded_torques", 20); - this->pub_torques_.msg_.layout.dim.resize(1); - this->pub_torques_.msg_.layout.data_offset = 0; - this->pub_torques_.msg_.layout.dim[0].size = this->n_joints_; - this->pub_torques_.msg_.layout.dim[0].stride = 0; - this->pub_torques_.msg_.data.resize(this->n_joints_); - - std::vector joint_names; - nh->getParam("joints", joint_names); - this->pub_state_.init(*nh, "controller_state", 10); - this->pub_state_.msg_.header.seq = 0; - for (size_t i = 0; i < this->n_joints_; i++) - { - this->pub_state_.msg_.joint_state.name.push_back(joint_names.at(i)); - } - this->pub_state_.msg_.joint_state.position = std::vector(this->n_joints_); - this->pub_state_.msg_.joint_state.velocity = std::vector(this->n_joints_); - this->pub_state_.msg_.joint_state.effort = std::vector(this->n_joints_); - this->pub_state_.msg_.commanded_torques = std::vector(this->n_joints_); - this->pub_state_.msg_.nullspace_config = std::vector(this->n_joints_); - return true; + RCLCPP_ERROR(logger, "No 'joints' parameter specified. Aborting configure."); + return CallbackReturn::ERROR; + } + dof_ = params_.joints.size(); + RCLCPP_INFO(logger, "Found %zu joints.", dof_); + + q_ = Eigen::VectorXd::Zero(dof_); + dq_ = Eigen::VectorXd::Zero(dof_); + tau_m_ = Eigen::VectorXd::Zero(dof_); + tau_c_ = Eigen::VectorXd::Zero(dof_); + command_joint_names_ = params_.joints; + + end_effector_ = params_.end_effector; + wrench_ee_frame_ = params_.wrench_ee_frame; + delta_tau_max_ = params_.delta_tau_max; + update_frequency_ = params_.update_frequency; + + tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + std::stringstream joints_ss; + joints_ss << "Joints: "; + for (const auto& joint : params_.joints) + { + joints_ss << joint << " "; + } + RCLCPP_INFO(logger, "%s", joints_ss.str().c_str()); + RCLCPP_INFO(logger, "End Effector: %s", params_.end_effector.c_str()); + RCLCPP_INFO(logger, "Wrench EE Frame: %s", params_.wrench_ee_frame.c_str()); + RCLCPP_INFO(logger, "Update Frequency: %.2f", params_.update_frequency); + RCLCPP_INFO(logger, "Filtering parameters - Nullspace: %.2f, Stiffness: %.2f, Pose: %.2f, Wrench: %.2f", + params_.filtering.nullspace, params_.filtering.stiffness, params_.filtering.pose, + params_.filtering.wrench); + RCLCPP_INFO(logger, "Stiffness - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f", + params_.stiffness.translation.x, params_.stiffness.translation.y, params_.stiffness.translation.z, + params_.stiffness.rotation.x, params_.stiffness.rotation.y, params_.stiffness.rotation.z, + params_.stiffness.nullspace_stiffness); + RCLCPP_INFO(logger, + "Damping - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f, Update: %s", + params_.damping.translation.x, params_.damping.translation.y, params_.damping.translation.z, + params_.damping.rotation.x, params_.damping.rotation.y, params_.damping.rotation.z, + params_.damping.nullspace_damping, params_.damping.update_damping_factors ? "true" : "false"); + + // pub_state_ = get_node()->create_publisher("controller_state", + // 10); + + // sub_controller_config_ = get_node()->create_subscription( + // "set_config", + // rclcpp::SystemDefaultsQoS(), + // + + trajectory_sub_ = node->create_subscription( + "joint_trajectory", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::trajCb, this, std::placeholders::_1)); + + traj_as_ = rclcpp_action::create_server( + node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), + node->get_node_waitables_interface(), std::string(node->get_name()) + "/follow_joint_trajectory", + std::bind(&CartesianImpedanceControllerRos::trajGoalCb, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&CartesianImpedanceControllerRos::trajCancelCb, this, std::placeholders::_1), + std::bind(&CartesianImpedanceControllerRos::trajAcceptCb, this, std::placeholders::_1)); + + pub_torques_ = node->create_publisher("commanded_torques", 10); + + sub_cart_stiffness_ = node->create_subscription( + "set_cartesian_stiffness", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::cartesianStiffnessCb, this, std::placeholders::_1)); + + sub_cart_wrench_ = node->create_subscription( + "set_cartesian_wrench", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::wrenchCommandCb, this, std::placeholders::_1)); + + sub_damping_factors_ = node->create_subscription( + "set_damping_factors", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::cartesianDampingFactorCb, this, std::placeholders::_1)); + + sub_reference_pose_ = node->create_subscription( + "reference_pose", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::referencePoseCb, this, std::placeholders::_1)); + + setNumberOfJoints(dof_); + + if (!initRBDyn()) + { + RCLCPP_ERROR(logger, "Failed to initialize RBDyn. Check robot_description param!"); + return CallbackReturn::ERROR; } - bool CartesianImpedanceControllerRos::initRBDyn(const ros::NodeHandle &nh) + double filter_ns = params_.filtering.nullspace; + double filter_stiff = params_.filtering.stiffness; + double filter_pose = params_.filtering.pose; + double filter_wrench = params_.filtering.wrench; + setFiltering(update_frequency_, filter_ns, filter_stiff, filter_pose, filter_wrench); + + double stiff_tx = params_.stiffness.translation.x; + double stiff_ty = params_.stiffness.translation.y; + double stiff_tz = params_.stiffness.translation.z; + double stiff_rx = params_.stiffness.rotation.x; + double stiff_ry = params_.stiffness.rotation.y; + double stiff_rz = params_.stiffness.rotation.z; + double stiff_ns = params_.stiffness.nullspace_stiffness; + + double damp_tx = params_.damping.translation.x; + double damp_ty = params_.damping.translation.y; + double damp_tz = params_.damping.translation.z; + double damp_rx = params_.damping.rotation.x; + double damp_ry = params_.damping.rotation.y; + double damp_rz = params_.damping.rotation.z; + double damp_ns = params_.damping.nullspace_damping; + + if (params_.damping.update_damping_factors) + setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); + + damping_factors_.resize(7); + damping_factors_ << damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns; + nullspace_stiffness_target_ = stiff_ns; + + RCLCPP_INFO(logger, "on_configure() successful."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn CartesianImpedanceControllerRos::on_activate(const rclcpp_lifecycle::State&) +{ + auto node = get_node(); + auto logger = node->get_logger(); + RCLCPP_INFO(logger, "Activating Cartesian Impedance Controller..."); + + auto& command_interfaces = this->command_interfaces_; + const auto& state_interfaces = this->state_interfaces_; + + joint_command_handles_.clear(); + joint_position_state_.clear(); + joint_velocity_state_.clear(); + joint_effort_state_.clear(); + + joint_command_handles_.resize(dof_, nullptr); + joint_position_state_.resize(dof_, nullptr); + joint_velocity_state_.resize(dof_, nullptr); + joint_effort_state_.resize(dof_, nullptr); + + if (q_.size() != dof_) + q_.resize(dof_); + if (dq_.size() != dof_) + dq_.resize(dof_); + if (tau_m_.size() != dof_) + tau_m_.resize(dof_); + if (tau_c_.size() != dof_) + tau_c_.resize(dof_); + + for (const auto& interface : state_interfaces) { - // Get the URDF XML from the parameter server. Wait if needed. - std::string urdf_string; - nh.param("robot_description", robot_description_, "/robot_description"); - while (!nh.getParam(robot_description_, urdf_string)) - { - ROS_INFO_ONCE("Waiting for robot description in parameter %s on the ROS param server.", - robot_description_.c_str()); - usleep(100000); - } - try - { - this->rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); - } - catch (std::runtime_error e) - { - ROS_ERROR("Error when intializing RBDyn: %s", e.what()); - return false; - } - ROS_INFO_STREAM("Number of joints found in urdf: " << this->rbdyn_wrapper_.n_joints()); - if (this->rbdyn_wrapper_.n_joints() < this->n_joints_) - { - ROS_ERROR("Number of joints in the URDF is smaller than supplied number of joints. %i < %zu", this->rbdyn_wrapper_.n_joints(), this->n_joints_); - return false; - } - else if (this->rbdyn_wrapper_.n_joints() > this->n_joints_) - { - ROS_WARN("Number of joints in the URDF is greater than supplied number of joints: %i > %zu. Assuming that the actuated joints come first.", this->rbdyn_wrapper_.n_joints(), this->n_joints_); - } - return true; - } - - bool CartesianImpedanceControllerRos::initTrajectories(ros::NodeHandle *nh) - { - this->sub_trajectory_ = nh->subscribe("joint_trajectory", 1, &CartesianImpedanceControllerRos::trajCb, this); - this->traj_as_ = std::unique_ptr>( - new actionlib::SimpleActionServer( - *nh, std::string("follow_joint_trajectory"), false)); - this->traj_as_->registerGoalCallback(boost::bind(&CartesianImpedanceControllerRos::trajGoalCb, this)); - this->traj_as_->registerPreemptCallback(boost::bind(&CartesianImpedanceControllerRos::trajPreemptCb, this)); - this->traj_as_->start(); - return true; - } - - bool CartesianImpedanceControllerRos::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) - { - ROS_INFO("Initializing Cartesian impedance controller in namespace: %s", node_handle.getNamespace().c_str()); - - // Fetch parameters - node_handle.param("end_effector", this->end_effector_, "iiwa_link_ee"); - ROS_INFO_STREAM("End effektor link is: " << this->end_effector_); - // Frame for applying commanded Cartesian wrenches - node_handle.param("wrench_ee_frame", this->wrench_ee_frame_, this->end_effector_); - bool dynamic_reconfigure{true}; - node_handle.param("dynamic_reconfigure", dynamic_reconfigure, true); - bool enable_trajectories{true}; - node_handle.param("handle_trajectories", enable_trajectories, true); - node_handle.param("delta_tau_max", this->delta_tau_max_, 1.); - node_handle.param("update_frequency", this->update_frequency_, 500.); - node_handle.param("filtering/nullspace_config", this->filter_params_nullspace_config_, 0.1); - node_handle.param("filtering/stiffness", this->filter_params_stiffness_, 0.1); - node_handle.param("filtering/pose", this->filter_params_pose_, 0.1); - node_handle.param("filtering/wrench", this->filter_params_wrench_, 0.1); - node_handle.param("verbosity/verbose_print", this->verbose_print_, false); - node_handle.param("verbosity/state_msgs", this->verbose_state_, false); - node_handle.param("verbosity/tf_frames", this->verbose_tf_, false); - - if (!this->initJointHandles(hw, node_handle) || !this->initMessaging(&node_handle) || !this->initRBDyn(node_handle)) - { - return false; - } - if (enable_trajectories && !this->initTrajectories(&node_handle)) - { - return false; - } - this->root_frame_ = this->rbdyn_wrapper_.root_link(); - node_handle.setParam("root_frame", this->root_frame_); + const std::string& joint_name = interface.get_prefix_name(); + const std::string& interface_type = interface.get_interface_name(); + + auto it = std::find(params_.joints.begin(), params_.joints.end(), joint_name); + if (it == params_.joints.end()) + continue; + const size_t index = std::distance(params_.joints.begin(), it); + + if (interface_type == hardware_interface::HW_IF_POSITION) + joint_position_state_[index] = &interface; + else if (interface_type == hardware_interface::HW_IF_VELOCITY) + joint_velocity_state_[index] = &interface; + else if (interface_type == hardware_interface::HW_IF_EFFORT) + joint_effort_state_[index] = &interface; + } - // Initialize base_tools and member variables - this->setNumberOfJoints(this->joint_handles_.size()); - if (this->n_joints_ < 6) - { - ROS_WARN("Number of joints is below 6. Functions might be limited."); - } - if (this->n_joints_ < 7) - { - ROS_WARN("Number of joints is below 7. No redundant joint for nullspace."); - } - this->tau_m_ = Eigen::VectorXd(this->n_joints_); + for (auto& interface : command_interfaces) + { + if (interface.get_interface_name() != hardware_interface::HW_IF_EFFORT) + continue; + const std::string& joint_name = interface.get_prefix_name(); + auto it = std::find(params_.joints.begin(), params_.joints.end(), joint_name); + if (it == params_.joints.end()) + continue; + const size_t index = std::distance(params_.joints.begin(), it); + joint_command_handles_[index] = &interface; + } - // Needs to be after base_tools init since the wrench callback calls it - if (dynamic_reconfigure && !this->initDynamicReconfigure(node_handle)) + update(rclcpp::Time(0), rclcpp::Duration(0, 0)); + initDesiredPose(position_, orientation_); + initNullspaceConfig(q_d_nullspace_); + for (size_t i = 0; i < dof_; i++) + { + if (!joint_command_handles_[i]) { - return false; + RCLCPP_ERROR(logger, "Command handle for joint %zu is null!", i); + return CallbackReturn::ERROR; } - - ROS_INFO("Finished initialization."); - return true; } + RCLCPP_INFO(logger, "Controller activated successfully."); + return CallbackReturn::SUCCESS; +} - void CartesianImpedanceControllerRos::starting(const ros::Time & /*time*/) - { - this->updateState(); +CallbackReturn CartesianImpedanceControllerRos::on_deactivate(const rclcpp_lifecycle::State&) +{ + write_zero_commands_to_hardware(); + RCLCPP_INFO(get_node()->get_logger(), "on_deactivate() done."); + return CallbackReturn::SUCCESS; +} - // Set reference pose to current pose and q_d_nullspace - this->initDesiredPose(this->position_, this->orientation_); - this->initNullspaceConfig(this->q_); - ROS_INFO("Started Cartesian Impedance Controller"); +controller_interface::return_type CartesianImpedanceControllerRos::update(const rclcpp::Time&, const rclcpp::Duration&) +{ + if (parameter_handler_->is_old(params_)) + { + RCLCPP_INFO(get_node()->get_logger(), "Parameters are outdated. Refreshing..."); + parameter_handler_->refresh_dynamic_parameters(); + params_ = parameter_handler_->get_params(); + applyRuntimeParameters(); } - void CartesianImpedanceControllerRos::update(const ros::Time & /*time*/, const ros::Duration &period /*period*/) + if (traj_running_) + trajUpdate(); + + read_state_from_hardware(); + calculateCommandedTorques(); // Populates tau_c_ + + std::vector torque_vector(tau_c_.data(), tau_c_.data() + tau_c_.size()); + std_msgs::msg::Float64MultiArray torque_msg; + torque_msg.data = torque_vector; + pub_torques_->publish(torque_msg); + + write_command_to_hardware(); + + if (auto current_traj = rt_trajectory_->readFromRT()) { - if (this->traj_running_) - { - trajUpdate(); - } + } - this->updateState(); + return controller_interface::return_type::OK; +} - // Apply control law in base library - this->calculateCommandedTorques(); +CallbackReturn CartesianImpedanceControllerRos::on_cleanup(const rclcpp_lifecycle::State&) +{ + return CallbackReturn::SUCCESS; +} - // Write commands - for (size_t i = 0; i < this->n_joints_; ++i) - { - this->joint_handles_[i].setCommand(this->tau_c_(i)); - } +CallbackReturn CartesianImpedanceControllerRos::on_error(const rclcpp_lifecycle::State&) +{ + return CallbackReturn::SUCCESS; +} - publishMsgsAndTf(); +void CartesianImpedanceControllerRos::applyRuntimeParameters() +{ + if (params_.stiffness.update_stiffness) + { + double sx = params_.stiffness.translation.x; + double sy = params_.stiffness.translation.y; + double sz = params_.stiffness.translation.z; + double rx = params_.stiffness.rotation.x; + double ry = params_.stiffness.rotation.y; + double rz = params_.stiffness.rotation.z; + double ns = params_.stiffness.nullspace_stiffness; + RCLCPP_INFO(get_node()->get_logger(), + "Updating stiffness: trans=(%.2f, %.2f, %.2f), rot=(%.2f, %.2f, %.2f), nullspace=%.2f", sx, sy, sz, rx, + ry, rz, ns); + setStiffness(sx, sy, sz, rx, ry, rz, ns, true); } - bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, - Eigen::Quaterniond *orientation) const + if (params_.damping.update_damping_factors) { - rbdyn_wrapper::EefState ee_state; - // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known - if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) - { - Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - q_rb.head(q.size()) = q; - ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); - } - else - { - ee_state = this->rbdyn_wrapper_.perform_fk(q); - } - *position = ee_state.translation; - *orientation = ee_state.orientation; - return true; + double tx = params_.damping.translation.x; + double ty = params_.damping.translation.y; + double tz = params_.damping.translation.z; + double rx = params_.damping.rotation.x; + double ry = params_.damping.rotation.y; + double rz = params_.damping.rotation.z; + double ns = params_.damping.nullspace_damping; + setDampingFactors(tx, ty, tz, rx, ry, rz, ns); } - bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, - Eigen::MatrixXd *jacobian) + if (params_.wrench.apply_wrench) { - // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known - if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) - { - Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - q_rb.head(q.size()) = q; - Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - dq_rb.head(dq.size()) = dq; - *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb); - } + double fx = params_.wrench.force_x; + double fy = params_.wrench.force_y; + double fz = params_.wrench.force_z; + double tx = params_.wrench.torque_x; + double ty = params_.wrench.torque_y; + double tz = params_.wrench.torque_z; + + Eigen::Matrix F; + F << fx, fy, fz, tx, ty, tz; + + if (!transformWrench(&F, wrench_ee_frame_, root_frame_)) + RCLCPP_WARN(get_node()->get_logger(), "Could not transform param-based wrench. Not applying it."); else - { - *jacobian = this->rbdyn_wrapper_.jacobian(q, dq); - } - *jacobian = jacobian_perm_ * *jacobian; - return true; + applyWrench(F); } +} - void CartesianImpedanceControllerRos::updateState() +bool CartesianImpedanceControllerRos::initRBDyn() +{ + auto node = get_node(); + auto logger = node->get_logger(); + + std::string urdf_string; + if (!node->get_parameter("robot_description", urdf_string)) { - for (size_t i = 0; i < this->n_joints_; ++i) - { - this->q_[i] = this->joint_handles_[i].getPosition(); - this->dq_[i] = this->joint_handles_[i].getVelocity(); - this->tau_m_[i] = this->joint_handles_[i].getEffort(); - } - getJacobian(this->q_, this->dq_, &this->jacobian_); - getFk(this->q_, &this->position_, &this->orientation_); + RCLCPP_ERROR(logger, "No robot URDF found in 'robot_description' parameter!"); + return false; } - void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg) + try + { + rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); + } + catch (const std::runtime_error& e) { - this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); - this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); + RCLCPP_ERROR(logger, "Error initializing RBDyn: %s", e.what()); + return false; + } - if (msg->q_d_nullspace.size() == this->n_joints_) - { - Eigen::VectorXd q_d_nullspace(this->n_joints_); - for (size_t i = 0; i < this->n_joints_; i++) - { - q_d_nullspace(i) = msg->q_d_nullspace.at(i); - } - this->setNullspaceConfig(q_d_nullspace); - } - else - { - ROS_WARN_STREAM("Nullspace configuration does not have the correct amount of entries. Got " << msg->q_d_nullspace.size() << " expected " << this->n_joints_ << ". Ignoring."); - } + root_frame_ = rbdyn_wrapper_.root_link(); + RCLCPP_INFO(logger, "Root frame is '%s'.", root_frame_.c_str()); + + if (rbdyn_wrapper_.n_joints() < static_cast(dof_)) + { + RCLCPP_ERROR(logger, "URDF has fewer joints (%zu) than the number to be controlled (%zu)", + rbdyn_wrapper_.n_joints(), dof_); + return false; } - void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg) + return true; +} + +void CartesianImpedanceControllerRos::read_state_from_hardware() +{ + if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_) { - this->setDampingFactors(*msg, this->damping_factors_[6]); + RCLCPP_ERROR(get_node()->get_logger(), "Mismatched Eigen vector sizes in read_state_from_hardware."); + return; } - void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg) + for (size_t i = 0; i < dof_; i++) { - if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) - { - ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring."); - return; - } - Eigen::Vector3d position_d; - position_d << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; - const Eigen::Quaterniond last_orientation_d_target(this->orientation_d_); - Eigen::Quaterniond orientation_d; - orientation_d.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, - msg->pose.orientation.w; - if (last_orientation_d_target.coeffs().dot(this->orientation_d_.coeffs()) < 0.0) + if (!joint_position_state_[i] || !joint_velocity_state_[i] || !joint_effort_state_[i]) { - this->orientation_d_.coeffs() << -this->orientation_d_.coeffs(); + RCLCPP_ERROR(get_node()->get_logger(), "Joint state handle at index %zu is nullptr.", i); + q_(i) = dq_(i) = tau_m_(i) = 0.0; + continue; } - this->setReferencePose(position_d, orientation_d); + q_(i) = joint_position_state_[i]->get_value(); + dq_(i) = joint_velocity_state_[i]->get_value(); + tau_m_(i) = joint_effort_state_[i]->get_value(); } - void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg) - { - this->setStiffness(msg->wrench, this->nullspace_stiffness_target_); - } + getJacobian(q_, dq_, &jacobian_); + getFk(q_, &position_, &orientation_); +} + +void CartesianImpedanceControllerRos::write_command_to_hardware() +{ + for (size_t i = 0; i < dof_; i++) + joint_command_handles_[i]->set_value(tau_c_(i)); +} + +void CartesianImpedanceControllerRos::write_zero_commands_to_hardware() +{ + for (size_t i = 0; i < dof_; i++) + joint_command_handles_[i]->set_value(0.0); +} + +bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd& q, Eigen::Vector3d* position, + Eigen::Quaterniond* orientation) +{ + auto logger = get_node()->get_logger(); - void CartesianImpedanceControllerRos::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace) + if (!position || !orientation) { - CartesianImpedanceController::setDampingFactors(saturateValue(cart_damping.force.x, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.force.y, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.force.z, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.torque.x, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.torque.y, dmp_factor_min_, dmp_factor_max_), - saturateValue(cart_damping.torque.z, dmp_factor_min_, dmp_factor_max_), - saturateValue(nullspace, dmp_factor_min_, dmp_factor_max_)); + RCLCPP_ERROR(logger, "getFk: Null pointer provided for position or orientation."); + return false; } - void CartesianImpedanceControllerRos::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping) + rbdyn_wrapper::EefState ee_state; + + if (q.size() != this->n_joints_) { - CartesianImpedanceController::setStiffness(saturateValue(cart_stiffness.force.x, trans_stf_min_, trans_stf_max_), - saturateValue(cart_stiffness.force.y, trans_stf_min_, trans_stf_max_), - saturateValue(cart_stiffness.force.z, trans_stf_min_, trans_stf_max_), - saturateValue(cart_stiffness.torque.x, rot_stf_min_, rot_stf_max_), - saturateValue(cart_stiffness.torque.y, rot_stf_min_, rot_stf_max_), - saturateValue(cart_stiffness.torque.z, rot_stf_min_, rot_stf_max_), - saturateValue(nullspace, ns_min_, ns_max_), auto_damping); + RCLCPP_ERROR(logger, "getFk: Provided joint vector size (%ld) does not match the controller's joint count (%zu).", + q.size(), this->n_joints_); + return false; } - void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &msg) + if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) { - Eigen::Matrix F; - F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y, - msg->wrench.torque.z; - - if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) + Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + q_rb.head(q.size()) = q; + try { - if (!transformWrench(&F, msg->header.frame_id, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } + ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); } - else if (msg->header.frame_id.empty()) + catch (const std::exception& e) { - if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } + RCLCPP_ERROR(logger, "getFk: Exception during perform_fk with expanded joint vector: %s", e.what()); + return false; } - this->applyWrench(F); } - - bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix *cartesian_wrench, - const std::string &from_frame, const std::string &to_frame) const + else { try { - tf::StampedTransform transform; - tf_listener_.lookupTransform(to_frame, from_frame, ros::Time(0), transform); - tf::Vector3 v_f(cartesian_wrench->operator()(0), cartesian_wrench->operator()(1), cartesian_wrench->operator()(2)); - tf::Vector3 v_t(cartesian_wrench->operator()(3), cartesian_wrench->operator()(4), cartesian_wrench->operator()(5)); - tf::Vector3 v_f_rot = tf::quatRotate(transform.getRotation(), v_f); - tf::Vector3 v_t_rot = tf::quatRotate(transform.getRotation(), v_t); - *cartesian_wrench << v_f_rot[0], v_f_rot[1], v_f_rot[2], v_t_rot[0], v_t_rot[1], v_t_rot[2]; - return true; + ee_state = this->rbdyn_wrapper_.perform_fk(q); } - catch (const tf::TransformException &ex) + catch (const std::exception& e) { - ROS_ERROR_THROTTLE(1, "%s", ex.what()); + RCLCPP_ERROR(logger, "getFk: Exception during perform_fk: %s", e.what()); return false; } } - void CartesianImpedanceControllerRos::publishMsgsAndTf() - { - // publish commanded torques - if (this->pub_torques_.trylock()) - { - for (size_t i = 0; i < this->n_joints_; i++) - { - this->pub_torques_.msg_.data[i] = this->tau_c_[i]; - } - this->pub_torques_.unlockAndPublish(); - } - - const Eigen::Matrix error{this->getPoseError()}; + *position = ee_state.translation; + *orientation = ee_state.orientation; + RCLCPP_DEBUG(logger, "position: %f, %f, %f", position->x(), position->y(), position->z()); + return true; +} - if (this->verbose_print_) - { - ROS_INFO_STREAM_THROTTLE(0.1, "\nCartesian Position:\n" - << this->position_ << "\nError:\n" - << error << "\nCartesian Stiffness:\n" - << this->cartesian_stiffness_ << "\nCartesian damping:\n" - << this->cartesian_damping_ << "\nNullspace stiffness:\n" - << this->nullspace_stiffness_ << "\nq_d_nullspace:\n" - << this->q_d_nullspace_ << "\ntau_d:\n" - << this->tau_c_); - } - if (this->verbose_tf_ && ros::Time::now() > this->tf_last_time_) - { - // Publish result of forward kinematics - tf::vectorEigenToTF(this->position_, this->tf_pos_); - this->tf_br_transform_.setOrigin(this->tf_pos_); - tf::quaternionEigenToTF(this->orientation_, this->tf_rot_); - this->tf_br_transform_.setRotation(this->tf_rot_); - tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_fk")); - // Publish tf to the reference pose - tf::vectorEigenToTF(this->position_d_, this->tf_pos_); - this->tf_br_transform_.setOrigin(this->tf_pos_); - tf::quaternionEigenToTF(this->orientation_d_, this->tf_rot_); - this->tf_br_transform_.setRotation(this->tf_rot_); - tf_br_.sendTransform(tf::StampedTransform(this->tf_br_transform_, ros::Time::now(), this->root_frame_, this->end_effector_ + "_ee_ref_pose")); - this->tf_last_time_ = ros::Time::now(); - } - if (this->verbose_state_ && this->pub_state_.trylock()) - { - this->pub_state_.msg_.header.stamp = ros::Time::now(); - tf::pointEigenToMsg(this->position_, this->pub_state_.msg_.current_pose.position); - tf::quaternionEigenToMsg(this->orientation_, this->pub_state_.msg_.current_pose.orientation); - tf::pointEigenToMsg(this->position_d_, this->pub_state_.msg_.reference_pose.position); - tf::quaternionEigenToMsg(this->orientation_d_, this->pub_state_.msg_.reference_pose.orientation); - tf::pointEigenToMsg(error.head(3), this->pub_state_.msg_.pose_error.position); - Eigen::Quaterniond q = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); - tf::quaternionEigenToMsg(q, this->pub_state_.msg_.pose_error.orientation); - - EigenVectorToWrench(this->cartesian_stiffness_.diagonal(), &this->pub_state_.msg_.cartesian_stiffness); - EigenVectorToWrench(this->cartesian_damping_.diagonal(), &this->pub_state_.msg_.cartesian_damping); - EigenVectorToWrench(this->getAppliedWrench(), &this->pub_state_.msg_.commanded_wrench); - - for (size_t i = 0; i < this->n_joints_; i++) - { - this->pub_state_.msg_.joint_state.position.at(i) = this->q_(i); - this->pub_state_.msg_.joint_state.velocity.at(i) = this->dq_(i); - this->pub_state_.msg_.joint_state.effort.at(i) = this->tau_m_(i); - this->pub_state_.msg_.nullspace_config.at(i) = this->q_d_nullspace_(i); - this->pub_state_.msg_.commanded_torques.at(i) = this->tau_c_(i); - } - this->pub_state_.msg_.nullspace_stiffness = this->nullspace_stiffness_; - this->pub_state_.msg_.nullspace_damping = this->nullspace_damping_; - const Eigen::Matrix dx = this->jacobian_ * this->dq_; - this->pub_state_.msg_.cartesian_velocity = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); - - this->pub_state_.unlockAndPublish(); - this->pub_state_.msg_.header.seq++; - } +bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, + Eigen::MatrixXd* jacobian) +{ + if (!jacobian) + { + RCLCPP_ERROR(get_node()->get_logger(), "Jacobian pointer is null."); + return false; } - // Dynamic reconfigure - // -------------------------------------------------------------------------------------------------------------------------------------- - void CartesianImpedanceControllerRos::dynamicStiffnessCb( - cartesian_impedance_controller::stiffnessConfig &config, uint32_t level) + if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) { - if (config.update_stiffness) - { - CartesianImpedanceController::setStiffness(saturateValue(config.translation_x, trans_stf_min_, trans_stf_max_), - saturateValue(config.translation_y, trans_stf_min_, trans_stf_max_), - saturateValue(config.translation_z, trans_stf_min_, trans_stf_max_), - saturateValue(config.rotation_x, trans_stf_min_, trans_stf_max_), - saturateValue(config.rotation_y, trans_stf_min_, trans_stf_max_), - saturateValue(config.rotation_z, trans_stf_min_, trans_stf_max_), config.nullspace_stiffness); - } + Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + q_rb.head(q.size()) = q; + Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + dq_rb.head(dq.size()) = dq; + *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb); + } + else + { + *jacobian = this->rbdyn_wrapper_.jacobian(q, dq); } + *jacobian = jacobian_perm_ * *jacobian; + return true; +} + +// void CartesianImpedanceControllerRos::controllerConfigCb( +// const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) +// { +// //// This merges all config: +// setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); +// setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); + +// //// Nullspace configuration +// if (msg->q_d_nullspace.size() == dof_) +// { +// Eigen::VectorXd qd(dof_); +// for (size_t i = 0; i < dof_; i++) +// { +// qd(i) = msg->q_d_nullspace[i]; +// } +// setNullspaceConfig(qd); +// } +// else +// { +// RCLCPP_WARN( +// get_node()->get_logger(), +// "Nullspace configuration has wrong dimension: got %zu, expected %zu", +// msg->q_d_nullspace.size(), dof_); +// } +// } +// void CartesianImpedanceControllerRos::publishMsgs() +// { +// //// Publish ControllerState +// cartesian_impedance_controller::msg::ControllerState st; +// st.header.stamp = get_node()->now(); + +// // Fill joint states +// st.joint_state.name = params_.joints; +// st.joint_state.position.resize(dof_); +// st.joint_state.velocity.resize(dof_); +// st.joint_state.effort.resize(dof_); +// st.commanded_torques.resize(dof_); +// st.nullspace_config.resize(dof_); + +// for (size_t i = 0; i < dof_; i++) +// { +// st.joint_state.position[i] = q_(i); +// st.joint_state.velocity[i] = dq_(i); +// st.joint_state.effort[i] = tau_m_(i); +// st.commanded_torques[i] = tau_c_(i); +// st.nullspace_config[i] = q_d_nullspace_(i); +// } + +// // Fill current and reference poses +// st.current_pose.position.x = position_(0); +// st.current_pose.position.y = position_(1); +// st.current_pose.position.z = position_(2); +// st.current_pose.orientation.x = orientation_.x(); +// st.current_pose.orientation.y = orientation_.y(); +// st.current_pose.orientation.z = orientation_.z(); +// st.current_pose.orientation.w = orientation_.w(); + +// st.reference_pose.position.x = position_d_target_(0); +// st.reference_pose.position.y = position_d_target_(1); +// st.reference_pose.position.z = position_d_target_(2); +// st.reference_pose.orientation.x = orientation_d_target_.x(); +// st.reference_pose.orientation.y = orientation_d_target_.y(); +// st.reference_pose.orientation.z = orientation_d_target_.z(); +// st.reference_pose.orientation.w = orientation_d_target_.w(); + +// // Compute pose error +// Eigen::Matrix pose_err = getPoseError(); +// st.pose_error.position.x = pose_err(0); +// st.pose_error.position.y = pose_err(1); +// st.pose_error.position.z = pose_err(2); +// // Convert small rotation error to a quaternion (axis-angle) +// Eigen::Quaterniond eq; +// double angle = pose_err.tail<3>().norm(); +// if (angle > 1e-6) +// { +// Eigen::Vector3d axis = pose_err.tail<3>() / angle; +// eq = Eigen::AngleAxisd(angle, axis); +// } +// else +// { +// eq = Eigen::Quaterniond::Identity(); +// } +// st.pose_error.orientation.x = eq.x(); +// st.pose_error.orientation.y = eq.y(); +// st.pose_error.orientation.z = eq.z(); +// st.pose_error.orientation.w = eq.w(); + +// // Fill Cartesian stiffness/damping +// Eigen::Matrix diag_stiff = cartesian_stiffness_.diagonal(); +// st.cartesian_stiffness.force.x = diag_stiff(0); +// st.cartesian_stiffness.force.y = diag_stiff(1); +// st.cartesian_stiffness.force.z = diag_stiff(2); +// st.cartesian_stiffness.torque.x = diag_stiff(3); +// st.cartesian_stiffness.torque.y = diag_stiff(4); +// st.cartesian_stiffness.torque.z = diag_stiff(5); + +// Eigen::Matrix diag_damp = cartesian_damping_.diagonal(); +// st.cartesian_damping.force.x = diag_damp(0); +// st.cartesian_damping.force.y = diag_damp(1); +// st.cartesian_damping.force.z = diag_damp(2); +// st.cartesian_damping.torque.x = diag_damp(3); +// st.cartesian_damping.torque.y = diag_damp(4); +// st.cartesian_damping.torque.z = diag_damp(5); + +// // Fill commanded wrench +// Eigen::Matrix w_applied = getAppliedWrench(); +// st.commanded_wrench.force.x = w_applied(0); +// st.commanded_wrench.force.y = w_applied(1); +// st.commanded_wrench.force.z = w_applied(2); +// st.commanded_wrench.torque.x = w_applied(3); +// st.commanded_wrench.torque.y = w_applied(4); +// st.commanded_wrench.torque.z = w_applied(5); + +// // Nullspace stiffness/damping +// st.nullspace_stiffness = nullspace_stiffness_; +// st.nullspace_damping = nullspace_damping_; + +// // Cartesian velocity (optional) +// st.cartesian_velocity = getCartesianVelocity(); + +// //// Publish the message +// pub_state_->publish(st); + +// // Also, publish commanded torques +// std_msgs::msg::Float64MultiArray torques_msg; +// torques_msg.data = std::vector(tau_c_.data(), tau_c_.data() + tau_c_.size()); +// pub_torques_->publish(torques_msg); +// } + +void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg) +{ + damping_factors_(0) = msg->force.x; + damping_factors_(1) = msg->force.y; + damping_factors_(2) = msg->force.z; + damping_factors_(3) = msg->torque.x; + damping_factors_(4) = msg->torque.y; + damping_factors_(5) = msg->torque.z; + setDampingFactors(damping_factors_(0), damping_factors_(1), damping_factors_(2), damping_factors_(3), + damping_factors_(4), damping_factors_(5), damping_factors_(6)); +} + +void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) +{ + Eigen::Matrix stiffness_vector; + stiffness_vector << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, + msg->wrench.torque.y, msg->wrench.torque.z, nullspace_stiffness_target_; + setStiffness(stiffness_vector, true); +} - void CartesianImpedanceControllerRos::dynamicDampingCb( - cartesian_impedance_controller::dampingConfig &config, uint32_t level) +void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + if (msg->header.frame_id == root_frame_) { - if (config.update_damping_factors) - { - CartesianImpedanceController::setDampingFactors( - config.translation_x, config.translation_y, config.translation_z, config.rotation_x, config.rotation_y, config.rotation_z, config.nullspace_damping); - } + Eigen::Vector3d pos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + Eigen::Quaterniond ori(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z); + setReferencePose(pos, ori); + return; } - void CartesianImpedanceControllerRos::dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, - uint32_t level) + geometry_msgs::msg::PoseStamped transformed_pose; + try { - Eigen::Vector6d F{Eigen::Vector6d::Zero()}; - if (config.apply_wrench) - { - F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z; - if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } - } - this->applyWrench(F); + transformed_pose = tf_buffer_->transform(*msg, root_frame_, tf2::durationFromSec(0.1)); } + catch (const tf2::TransformException& ex) + { + RCLCPP_WARN(get_node()->get_logger(), "Transform failed: %s", ex.what()); + return; + } + + Eigen::Vector3d pos(transformed_pose.pose.position.x, transformed_pose.pose.position.y, + transformed_pose.pose.position.z); + Eigen::Quaterniond ori(transformed_pose.pose.orientation.w, transformed_pose.pose.orientation.x, + transformed_pose.pose.orientation.y, transformed_pose.pose.orientation.z); + setReferencePose(pos, ori); +} - void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg) +void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) +{ + Eigen::Matrix F; + F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y, + msg->wrench.torque.z; + std::string from_frame = msg->header.frame_id.empty() ? wrench_ee_frame_ : msg->header.frame_id; + if (!transformWrench(&F, from_frame, root_frame_)) { - ROS_INFO("Got trajectory msg from trajectory topic."); - if (this->traj_as_->isActive()) - { - this->traj_as_->setPreempted(); - ROS_INFO("Preempted running action server goal."); - } - trajStart(*msg); + RCLCPP_ERROR(get_node()->get_logger(), "Could not transform wrench. Not applying it."); + return; } + applyWrench(F); +} - void CartesianImpedanceControllerRos::trajGoalCb() +bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix* wrench, + const std::string& from_frame, const std::string& to_frame) const +{ + geometry_msgs::msg::TransformStamped transform; + try + { + transform = tf_buffer_->lookupTransform(to_frame, from_frame, tf2::TimePointZero); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(get_node()->get_logger(), "transformWrench() exception: %s", ex.what()); + return false; + } + tf2::Vector3 force_in((*wrench)(0), (*wrench)(1), (*wrench)(2)); + tf2::Vector3 torque_in((*wrench)(3), (*wrench)(4), (*wrench)(5)); + tf2::Transform tf; + tf2::fromMsg(transform.transform, tf); + tf2::Vector3 force_out = tf * force_in; + tf2::Vector3 torque_out = tf.getBasis() * torque_in; + (*wrench)(0) = force_out.x(); + (*wrench)(1) = force_out.y(); + (*wrench)(2) = force_out.z(); + (*wrench)(3) = torque_out.x(); + (*wrench)(4) = torque_out.y(); + (*wrench)(5) = torque_out.z(); + return true; +} + +void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory from topic"); + if (traj_as_active_) { - this->traj_as_goal_ = this->traj_as_->acceptNewGoal(); - ROS_INFO("Accepted new goal from action server."); - trajStart(this->traj_as_goal_->trajectory); + RCLCPP_INFO(get_node()->get_logger(), "Preempting running action server goal due to new trajectory"); + traj_running_ = false; } + trajStart(*msg); + rt_trajectory_->writeFromNonRT(*msg); +} - void CartesianImpedanceControllerRos::trajPreemptCb() +rclcpp_action::GoalResponse CartesianImpedanceControllerRos::trajGoalCb( + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received FollowJointTrajectory action goal request"); + if (goal->trajectory.points.empty() || goal->trajectory.joint_names.size() != dof_) { - ROS_INFO("Actionserver got preempted."); - this->traj_as_->setPreempted(); + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory OR mismatched joints in action. Rejecting."); + return rclcpp_action::GoalResponse::REJECT; } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse CartesianImpedanceControllerRos::trajCancelCb( + const std::shared_ptr>) +{ + RCLCPP_INFO(get_node()->get_logger(), "Canceling trajectory action"); + traj_running_ = false; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void CartesianImpedanceControllerRos::trajAcceptCb( + std::shared_ptr> goal_handle) +{ + auto goal = goal_handle->get_goal(); + auto logger = get_node()->get_logger(); + RCLCPP_INFO(logger, "Accepted FollowJointTrajectory action goal"); + + traj_as_active_ = true; + traj_as_goal_ = goal_handle; - void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::JointTrajectory &trajectory) + trajStart(goal->trajectory); + rt_trajectory_->writeFromNonRT(goal->trajectory); +} + +void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::msg::JointTrajectory& trajectory) +{ + trajectory_ = trajectory; + traj_index_ = 0; + traj_running_ = true; + traj_start_time_ = get_node()->now(); + + if (!trajectory.points.empty()) { - this->traj_duration_ = trajectory.points[trajectory.points.size() - 1].time_from_start; - ROS_INFO_STREAM("Starting a new trajectory with " << trajectory.points.size() << " points that takes " << this->traj_duration_ << "s."); - this->trajectory_ = trajectory; - this->traj_running_ = true; - this->traj_start_ = ros::Time::now(); - this->traj_index_ = 0; - trajUpdate(); - if (this->nullspace_stiffness_ < 5.) - { - ROS_WARN("Nullspace stiffness is low. The joints might not follow the planned path."); - } + traj_duration_ = rclcpp::Duration(trajectory.points.back().time_from_start.sec, + trajectory.points.back().time_from_start.nanosec); + RCLCPP_INFO(get_node()->get_logger(), "Started a trajectory with %zu points lasting %.2f s.", + trajectory.points.size(), traj_duration_.seconds()); } + else + { + RCLCPP_WARN(get_node()->get_logger(), "Empty trajectory. Not running."); + traj_running_ = false; + } +} + +void CartesianImpedanceControllerRos::trajUpdate() +{ + auto now = get_node()->now(); + double t_since_start = (now - traj_start_time_).seconds(); - void CartesianImpedanceControllerRos::trajUpdate() + if (t_since_start > traj_duration_.seconds()) { - if (ros::Time::now() > (this->traj_start_ + trajectory_.points.at(this->traj_index_).time_from_start)) + RCLCPP_INFO(get_node()->get_logger(), "Finished executing trajectory."); + traj_running_ = false; + if (traj_as_active_ && traj_as_goal_) { - // Get end effector pose - Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(this->traj_index_).positions.data(), - trajectory_.points.at(this->traj_index_).positions.size()); - if (this->verbose_print_) - { - ROS_INFO_STREAM("Index " << this->traj_index_ << " q_nullspace: " << q.transpose()); - } - // Update end-effector pose and nullspace - getFk(q, &this->position_d_target_, &this->orientation_d_target_); - this->setNullspaceConfig(q); - this->traj_index_++; + auto result = std::make_shared(); + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + traj_as_goal_->succeed(result); + traj_as_active_ = false; } + return; + } - if (ros::Time::now() > (this->traj_start_ + this->traj_duration_)) - { - ROS_INFO_STREAM("Finished executing trajectory."); - if (this->traj_as_->isActive()) - { - this->traj_as_->setSucceeded(); - } - this->traj_running_ = false; - } + if (now > (traj_start_time_ + rclcpp::Duration(trajectory_.points.at(traj_index_).time_from_start))) + { + Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(traj_index_).positions.data(), + trajectory_.points.at(traj_index_).positions.size()); + getFk(q, &position_d_target_, &orientation_d_target_); + setReferencePose(position_d_target_, orientation_d_target_); + setNullspaceConfig(q); + traj_index_++; } -} // namespace cartesian_impedance_controller +} + +} // end namespace cartesian_impedance_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(cartesian_impedance_controller::CartesianImpedanceControllerRos, + controller_interface::ControllerInterface) \ No newline at end of file From 055b9137adc68012846cd93fc14e351bfcf45e39 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Mon, 10 Feb 2025 12:25:39 +0000 Subject: [PATCH 02/25] Add custom messages ControllerConfig and ControllerState --- CMakeLists.txt | 13 ++++++++++++- msg/ControllerConfig.msg | 20 ++++++++++++++++++++ msg/ControllerState.msg | 20 ++++++++++++++++++++ 3 files changed, 52 insertions(+), 1 deletion(-) create mode 100644 msg/ControllerConfig.msg create mode 100644 msg/ControllerState.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 6bd6b2e..37a985f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,12 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +rosidl_generate_interfaces(${PROJECT_NAME}_msgs + "msg/ControllerConfig.msg" + "msg/ControllerState.msg" + DEPENDENCIES std_msgs geometry_msgs sensor_msgs +) + generate_parameter_library(cartesian_impedance_controller_parameters src/cartesian_impedance_controller_parameters.yaml ) @@ -60,10 +66,13 @@ add_library(cartesian_impedance_controller SHARED src/cartesian_impedance_controller.cpp ) +add_dependencies(cartesian_impedance_controller ${PROJECT_NAME}_msgs) + target_compile_features(cartesian_impedance_controller PUBLIC cxx_std_17) target_include_directories(cartesian_impedance_controller PUBLIC $ + $ $ ) @@ -76,7 +85,9 @@ target_link_libraries(cartesian_impedance_controller PUBLIC pluginlib_export_plugin_description_file(controller_interface cartesian_impedance_controller_plugin.xml) -ament_target_dependencies(cartesian_impedance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(cartesian_impedance_controller PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) install( DIRECTORY include/ diff --git a/msg/ControllerConfig.msg b/msg/ControllerConfig.msg new file mode 100644 index 0000000..5f81f2b --- /dev/null +++ b/msg/ControllerConfig.msg @@ -0,0 +1,20 @@ +# Cartesian Stiffness values for translation and rotation of the axes x,y,z +# Values: Translation: [0.0, 2000.0] in [N/m]; Rotation [0.0; 500.0] in [Nm/rad] +geometry_msgs/Wrench cartesian_stiffness + +# Damping factor for translation and rotation of the axes x,y,z +# The rule is always 2*sqrt(stiffness) +# Values: [0.001, 2.0] +geometry_msgs/Wrench cartesian_damping_factors + +# Stiffness value for nullspace +# Values: >= 0.0 in [Nm/rad] +float64 nullspace_stiffness + +# Damping parameter for nullspace [Nm*s/rad]. +# Value: [0.001, 2.0] +float64 nullspace_damping_factor + +# The desired nullspace configuration +# Values: According to the limits of the robot +float64[] q_d_nullspace \ No newline at end of file diff --git a/msg/ControllerState.msg b/msg/ControllerState.msg new file mode 100644 index 0000000..7440262 --- /dev/null +++ b/msg/ControllerState.msg @@ -0,0 +1,20 @@ +std_msgs/Header header + +geometry_msgs/Pose current_pose +geometry_msgs/Pose reference_pose +geometry_msgs/Pose pose_error + +sensor_msgs/JointState joint_state +float64[] commanded_torques + +# Stiffness values for the cartesian impedance, x, y, z in [N/m], x, y, z in [Nm/rad]. +geometry_msgs/Wrench cartesian_stiffness +geometry_msgs/Wrench cartesian_damping + +float64 nullspace_stiffness +float64 nullspace_damping +float64[] nullspace_config + +geometry_msgs/Wrench commanded_wrench + +float64 cartesian_velocity # Calculated Cartesian velocity [m/s] \ No newline at end of file From c35c7310c95a4a97e5b3f7ab4412b49ddb7550ce Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Mon, 10 Feb 2025 16:43:32 +0000 Subject: [PATCH 03/25] Fix custom msg generation and enable config subscriber --- CMakeLists.txt | 3 + .../cartesian_impedance_controller_ros.hpp | 3 + src/cartesian_impedance_controller_ros.cpp | 62 ++++++++++--------- 3 files changed, 39 insertions(+), 29 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 37a985f..cafd7e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs tf2_kdl tf2_ros + std_msgs trajectory_msgs generate_parameter_library urdf @@ -75,8 +76,10 @@ target_include_directories(cartesian_impedance_controller PUBLIC $ $ ) +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}_msgs "rosidl_typesupport_cpp") target_link_libraries(cartesian_impedance_controller PUBLIC + "${cpp_typesupport_target}" cartesian_impedance_controller_parameters Eigen3::Eigen RBDyn diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index f3a4d59..2866c68 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -36,6 +36,7 @@ #include "cartesian_impedance_controller/cartesian_impedance_controller.hpp" #include "cartesian_impedance_controller/rbdyn_wrapper.h" #include +#include "cartesian_impedance_controller/msg/controller_config.hpp" namespace cartesian_impedance_controller { @@ -94,6 +95,7 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI rclcpp::Subscription::SharedPtr sub_damping_factors_; rclcpp::Subscription::SharedPtr sub_reference_pose_; rclcpp::Subscription::SharedPtr trajectory_sub_; + rclcpp::Subscription::SharedPtr sub_controller_config_; rclcpp_action::Server::SharedPtr traj_as_; bool traj_running_ = false; @@ -117,6 +119,7 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *orientation); bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); + void controllerConfigCb(const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg); void cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg); void cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); void referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 2beb91f..a1d85c7 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -12,6 +12,7 @@ #include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" +// #include "cartesian_impedance_controller/msg/controller_state.hpp" #include #include @@ -168,11 +169,6 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy // pub_state_ = get_node()->create_publisher("controller_state", // 10); - // sub_controller_config_ = get_node()->create_subscription( - // "set_config", - // rclcpp::SystemDefaultsQoS(), - // - trajectory_sub_ = node->create_subscription( "joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&CartesianImpedanceControllerRos::trajCb, this, std::placeholders::_1)); @@ -185,6 +181,11 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy std::bind(&CartesianImpedanceControllerRos::trajAcceptCb, this, std::placeholders::_1)); pub_torques_ = node->create_publisher("commanded_torques", 10); + + sub_controller_config_ = get_node()->create_subscription( + "set_config", + rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::controllerConfigCb, this, std::placeholders::_1)); sub_cart_stiffness_ = node->create_subscription( "set_cartesian_stiffness", rclcpp::SystemDefaultsQoS(), @@ -561,31 +562,34 @@ bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd& q, cons return true; } -// void CartesianImpedanceControllerRos::controllerConfigCb( -// const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) -// { -// //// This merges all config: -// setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); -// setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); +void CartesianImpedanceControllerRos::controllerConfigCb( + const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) +{ + setStiffness(msg->cartesian_stiffness.force.x, msg->cartesian_stiffness.force.y, msg->cartesian_stiffness.force.z, + msg->cartesian_stiffness.torque.x, msg->cartesian_stiffness.torque.y, msg->cartesian_stiffness.torque.z, + msg->nullspace_stiffness, false); + setDampingFactors(msg->cartesian_damping_factors.force.x, msg->cartesian_damping_factors.force.y, msg->cartesian_damping_factors.force.z, + msg->cartesian_damping_factors.torque.x, msg->cartesian_damping_factors.torque.y, msg->cartesian_damping_factors.torque.z, + msg->nullspace_damping_factor); + + if (msg->q_d_nullspace.size() == dof_) + { + Eigen::VectorXd qd(dof_); + for (size_t i = 0; i < dof_; i++) + { + qd(i) = msg->q_d_nullspace[i]; + } + setNullspaceConfig(qd); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Nullspace configuration has wrong dimension: got %zu, expected %zu", + msg->q_d_nullspace.size(), dof_); + } +} -// //// Nullspace configuration -// if (msg->q_d_nullspace.size() == dof_) -// { -// Eigen::VectorXd qd(dof_); -// for (size_t i = 0; i < dof_; i++) -// { -// qd(i) = msg->q_d_nullspace[i]; -// } -// setNullspaceConfig(qd); -// } -// else -// { -// RCLCPP_WARN( -// get_node()->get_logger(), -// "Nullspace configuration has wrong dimension: got %zu, expected %zu", -// msg->q_d_nullspace.size(), dof_); -// } -// } // void CartesianImpedanceControllerRos::publishMsgs() // { // //// Publish ControllerState From 34c8025f015c3469569f760bb5eae2b8c64fa6d6 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Tue, 11 Feb 2025 14:55:11 +0000 Subject: [PATCH 04/25] New: Adds controller state publisher --- .../cartesian_impedance_controller_ros.hpp | 13 +- ...esian_impedance_controller_parameters.yaml | 32 +- src/cartesian_impedance_controller_ros.cpp | 311 +++++++++++------- 3 files changed, 225 insertions(+), 131 deletions(-) diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index 2866c68..e37610f 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -37,6 +38,10 @@ #include "cartesian_impedance_controller/rbdyn_wrapper.h" #include #include "cartesian_impedance_controller/msg/controller_config.hpp" +#include "cartesian_impedance_controller/msg/controller_state.hpp" + +#include +#include namespace cartesian_impedance_controller { @@ -86,9 +91,13 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI std::vector joints_angle_wraparound_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; + rclcpp::Time tf_last_time_; + std::shared_ptr tf_br_; + rbdyn_wrapper rbdyn_wrapper_; - rclcpp::Publisher::SharedPtr pub_torques_; + std::shared_ptr> rt_pub_state_; + std::shared_ptr> rt_pub_torques_; rclcpp::Subscription::SharedPtr sub_cart_stiffness_; rclcpp::Subscription::SharedPtr sub_cart_wrench_; @@ -135,7 +144,7 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI bool transformWrench(Eigen::Matrix * wrench, const std::string & from_frame, const std::string & to_frame) const; - void publishMsgs(); + void publishMsgsAndTf(); void applyRuntimeParameters(); bool initRBDyn(); }; diff --git a/src/cartesian_impedance_controller_parameters.yaml b/src/cartesian_impedance_controller_parameters.yaml index 154daba..c852aca 100644 --- a/src/cartesian_impedance_controller_parameters.yaml +++ b/src/cartesian_impedance_controller_parameters.yaml @@ -199,13 +199,25 @@ cartesian_impedance_controller: description: "Filtering factor for wrench." validation: bounds<>: [0.0, 1.0] - - joints: { - type: string_array, - default_value: [], - description: "Joint names to control and listen to", - read_only: true, - validation: { - unique<>: null, - } - } \ No newline at end of file + + joints: + type: string_array + default_value: [] + description: "Joint names to control and listen to" + read_only: true + validation: + unique<>: null + + verbosity: + verbose_print: + type: bool + default_value: false + description: "Enable verbose printing of internal states." + tf_frames: + type: bool + default_value: false + description: "Enable broadcasting of TF transforms for the end-effector." + state_msgs: + type: bool + default_value: false + description: "Enable publishing of the controller state message." diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index a1d85c7..d0ff890 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -12,7 +12,6 @@ #include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -// #include "cartesian_impedance_controller/msg/controller_state.hpp" #include #include @@ -35,8 +34,6 @@ #include "urdf/model.h" #endif -#include - namespace cartesian_impedance_controller { @@ -79,6 +76,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_init() RCLCPP_ERROR(logger, "Failed to get robot_description parameter"); return CallbackReturn::ERROR; } + tf_br_ = std::make_shared(node); return CallbackReturn::SUCCESS; } @@ -142,6 +140,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy tf_buffer_ = std::make_shared(node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + tf_last_time_ = get_node()->now(); std::stringstream joints_ss; joints_ss << "Joints: "; @@ -166,9 +165,6 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy params_.damping.rotation.x, params_.damping.rotation.y, params_.damping.rotation.z, params_.damping.nullspace_damping, params_.damping.update_damping_factors ? "true" : "false"); - // pub_state_ = get_node()->create_publisher("controller_state", - // 10); - trajectory_sub_ = node->create_subscription( "joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&CartesianImpedanceControllerRos::trajCb, this, std::placeholders::_1)); @@ -179,9 +175,16 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy std::bind(&CartesianImpedanceControllerRos::trajGoalCb, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CartesianImpedanceControllerRos::trajCancelCb, this, std::placeholders::_1), std::bind(&CartesianImpedanceControllerRos::trajAcceptCb, this, std::placeholders::_1)); - - pub_torques_ = node->create_publisher("commanded_torques", 10); + auto state_pub = + get_node()->create_publisher("controller_state", 10); + auto torques_pub = get_node()->create_publisher("commanded_torques", 10); + + rt_pub_state_ = + std::make_shared>( + state_pub); + rt_pub_torques_ = std::make_shared>(torques_pub); + sub_controller_config_ = get_node()->create_subscription( "set_config", rclcpp::SystemDefaultsQoS(), @@ -339,13 +342,8 @@ controller_interface::return_type CartesianImpedanceControllerRos::update(const read_state_from_hardware(); calculateCommandedTorques(); // Populates tau_c_ - - std::vector torque_vector(tau_c_.data(), tau_c_.data() + tau_c_.size()); - std_msgs::msg::Float64MultiArray torque_msg; - torque_msg.data = torque_vector; - pub_torques_->publish(torque_msg); - write_command_to_hardware(); + publishMsgsAndTf(); if (auto current_traj = rt_trajectory_->readFromRT()) { @@ -563,7 +561,7 @@ bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd& q, cons } void CartesianImpedanceControllerRos::controllerConfigCb( - const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) + const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) { setStiffness(msg->cartesian_stiffness.force.x, msg->cartesian_stiffness.force.y, msg->cartesian_stiffness.force.z, msg->cartesian_stiffness.torque.x, msg->cartesian_stiffness.torque.y, msg->cartesian_stiffness.torque.z, @@ -586,113 +584,188 @@ void CartesianImpedanceControllerRos::controllerConfigCb( RCLCPP_WARN( get_node()->get_logger(), "Nullspace configuration has wrong dimension: got %zu, expected %zu", - msg->q_d_nullspace.size(), dof_); + msg->q_d_nullspace.size(), dof_); } } -// void CartesianImpedanceControllerRos::publishMsgs() -// { -// //// Publish ControllerState -// cartesian_impedance_controller::msg::ControllerState st; -// st.header.stamp = get_node()->now(); - -// // Fill joint states -// st.joint_state.name = params_.joints; -// st.joint_state.position.resize(dof_); -// st.joint_state.velocity.resize(dof_); -// st.joint_state.effort.resize(dof_); -// st.commanded_torques.resize(dof_); -// st.nullspace_config.resize(dof_); - -// for (size_t i = 0; i < dof_; i++) -// { -// st.joint_state.position[i] = q_(i); -// st.joint_state.velocity[i] = dq_(i); -// st.joint_state.effort[i] = tau_m_(i); -// st.commanded_torques[i] = tau_c_(i); -// st.nullspace_config[i] = q_d_nullspace_(i); -// } - -// // Fill current and reference poses -// st.current_pose.position.x = position_(0); -// st.current_pose.position.y = position_(1); -// st.current_pose.position.z = position_(2); -// st.current_pose.orientation.x = orientation_.x(); -// st.current_pose.orientation.y = orientation_.y(); -// st.current_pose.orientation.z = orientation_.z(); -// st.current_pose.orientation.w = orientation_.w(); - -// st.reference_pose.position.x = position_d_target_(0); -// st.reference_pose.position.y = position_d_target_(1); -// st.reference_pose.position.z = position_d_target_(2); -// st.reference_pose.orientation.x = orientation_d_target_.x(); -// st.reference_pose.orientation.y = orientation_d_target_.y(); -// st.reference_pose.orientation.z = orientation_d_target_.z(); -// st.reference_pose.orientation.w = orientation_d_target_.w(); - -// // Compute pose error -// Eigen::Matrix pose_err = getPoseError(); -// st.pose_error.position.x = pose_err(0); -// st.pose_error.position.y = pose_err(1); -// st.pose_error.position.z = pose_err(2); -// // Convert small rotation error to a quaternion (axis-angle) -// Eigen::Quaterniond eq; -// double angle = pose_err.tail<3>().norm(); -// if (angle > 1e-6) -// { -// Eigen::Vector3d axis = pose_err.tail<3>() / angle; -// eq = Eigen::AngleAxisd(angle, axis); -// } -// else -// { -// eq = Eigen::Quaterniond::Identity(); -// } -// st.pose_error.orientation.x = eq.x(); -// st.pose_error.orientation.y = eq.y(); -// st.pose_error.orientation.z = eq.z(); -// st.pose_error.orientation.w = eq.w(); - -// // Fill Cartesian stiffness/damping -// Eigen::Matrix diag_stiff = cartesian_stiffness_.diagonal(); -// st.cartesian_stiffness.force.x = diag_stiff(0); -// st.cartesian_stiffness.force.y = diag_stiff(1); -// st.cartesian_stiffness.force.z = diag_stiff(2); -// st.cartesian_stiffness.torque.x = diag_stiff(3); -// st.cartesian_stiffness.torque.y = diag_stiff(4); -// st.cartesian_stiffness.torque.z = diag_stiff(5); - -// Eigen::Matrix diag_damp = cartesian_damping_.diagonal(); -// st.cartesian_damping.force.x = diag_damp(0); -// st.cartesian_damping.force.y = diag_damp(1); -// st.cartesian_damping.force.z = diag_damp(2); -// st.cartesian_damping.torque.x = diag_damp(3); -// st.cartesian_damping.torque.y = diag_damp(4); -// st.cartesian_damping.torque.z = diag_damp(5); - -// // Fill commanded wrench -// Eigen::Matrix w_applied = getAppliedWrench(); -// st.commanded_wrench.force.x = w_applied(0); -// st.commanded_wrench.force.y = w_applied(1); -// st.commanded_wrench.force.z = w_applied(2); -// st.commanded_wrench.torque.x = w_applied(3); -// st.commanded_wrench.torque.y = w_applied(4); -// st.commanded_wrench.torque.z = w_applied(5); - -// // Nullspace stiffness/damping -// st.nullspace_stiffness = nullspace_stiffness_; -// st.nullspace_damping = nullspace_damping_; - -// // Cartesian velocity (optional) -// st.cartesian_velocity = getCartesianVelocity(); - -// //// Publish the message -// pub_state_->publish(st); - -// // Also, publish commanded torques -// std_msgs::msg::Float64MultiArray torques_msg; -// torques_msg.data = std::vector(tau_c_.data(), tau_c_.data() + tau_c_.size()); -// pub_torques_->publish(torques_msg); -// } +void CartesianImpedanceControllerRos::publishMsgsAndTf() +{ + if (rt_pub_torques_ && rt_pub_torques_->trylock()) + { + rt_pub_torques_->msg_.data.resize(tau_c_.size()); + for (size_t i = 0; i < tau_c_.size(); ++i) + { + rt_pub_torques_->msg_.data[i] = tau_c_(i); + } + rt_pub_torques_->unlockAndPublish(); + } + + Eigen::Matrix error = getPoseError(); + + if (params_.verbosity.verbose_print) + { + RCLCPP_INFO(get_node()->get_logger(), "Cartesian Position: [%f, %f, %f]", position_(0), position_(1), position_(2)); + + RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", error(0), error(1), error(2), + error(3), error(4), error(5)); + + RCLCPP_INFO(get_node()->get_logger(), "Cartesian Stiffness (diag): [%f, %f, %f, %f, %f, %f]", + cartesian_stiffness_.diagonal()(0), cartesian_stiffness_.diagonal()(1), + cartesian_stiffness_.diagonal()(2), cartesian_stiffness_.diagonal()(3), + cartesian_stiffness_.diagonal()(4), cartesian_stiffness_.diagonal()(5)); + + RCLCPP_INFO(get_node()->get_logger(), "Cartesian Damping (diag): [%f, %f, %f, %f, %f, %f]", + cartesian_damping_.diagonal()(0), cartesian_damping_.diagonal()(1), cartesian_damping_.diagonal()(2), + cartesian_damping_.diagonal()(3), cartesian_damping_.diagonal()(4), cartesian_damping_.diagonal()(5)); + + RCLCPP_INFO(get_node()->get_logger(), "Nullspace Stiffness: %f", nullspace_stiffness_); + + { + std::stringstream ss; + ss << "q_d_nullspace: ["; + for (int i = 0; i < q_d_nullspace_.size(); ++i) + { + ss << q_d_nullspace_(i); + if (i != q_d_nullspace_.size() - 1) + { + ss << ", "; + } + } + ss << "]"; + RCLCPP_INFO(get_node()->get_logger(), "%s", ss.str().c_str()); + } + { + std::stringstream ss; + ss << "tau_c: ["; + for (int i = 0; i < tau_c_.size(); ++i) + { + ss << tau_c_(i); + if (i != tau_c_.size() - 1) + { + ss << ", "; + } + } + ss << "]"; + RCLCPP_INFO(get_node()->get_logger(), "%s", ss.str().c_str()); + } + } + + if (params_.verbosity.tf_frames) + { + rclcpp::Time now = get_node()->now(); + if (now > tf_last_time_) + { + if (!tf_br_) + { + tf_br_ = std::make_shared(get_node()); + } + + geometry_msgs::msg::TransformStamped fk_tf; + fk_tf.header.stamp = now; + fk_tf.header.frame_id = root_frame_; + fk_tf.child_frame_id = end_effector_ + "_ee_fk"; + fk_tf.transform.translation.x = position_(0); + fk_tf.transform.translation.y = position_(1); + fk_tf.transform.translation.z = position_(2); + fk_tf.transform.rotation = + tf2::toMsg(tf2::Quaternion(orientation_.x(), orientation_.y(), orientation_.z(), orientation_.w())); + tf_br_->sendTransform(fk_tf); + + geometry_msgs::msg::TransformStamped ref_tf; + ref_tf.header.stamp = now; + ref_tf.header.frame_id = root_frame_; + ref_tf.child_frame_id = end_effector_ + "_ee_ref_pose"; + ref_tf.transform.translation.x = position_d_(0); + ref_tf.transform.translation.y = position_d_(1); + ref_tf.transform.translation.z = position_d_(2); + ref_tf.transform.rotation = + tf2::toMsg(tf2::Quaternion(orientation_d_.x(), orientation_d_.y(), orientation_d_.z(), orientation_d_.w())); + tf_br_->sendTransform(ref_tf); + + tf_last_time_ = now; + } + } + + if (params_.verbosity.state_msgs) + { + if (rt_pub_state_ && rt_pub_state_->trylock()) + { + auto& state_msg = rt_pub_state_->msg_; + state_msg.header.stamp = get_node()->now(); + + state_msg.joint_state.name = params_.joints; + state_msg.joint_state.position.assign(q_.data(), q_.data() + q_.size()); + state_msg.joint_state.velocity.assign(dq_.data(), dq_.data() + dq_.size()); + state_msg.joint_state.effort.assign(tau_m_.data(), tau_m_.data() + tau_m_.size()); + state_msg.commanded_torques.assign(tau_c_.data(), tau_c_.data() + tau_c_.size()); + state_msg.nullspace_config.assign(q_d_nullspace_.data(), q_d_nullspace_.data() + q_d_nullspace_.size()); + + state_msg.current_pose.position.x = position_(0); + state_msg.current_pose.position.y = position_(1); + state_msg.current_pose.position.z = position_(2); + state_msg.current_pose.orientation = + tf2::toMsg(tf2::Quaternion(orientation_.x(), orientation_.y(), orientation_.z(), orientation_.w())); + state_msg.reference_pose.position.x = position_d_target_(0); + state_msg.reference_pose.position.y = position_d_target_(1); + state_msg.reference_pose.position.z = position_d_target_(2); + state_msg.reference_pose.orientation.w = orientation_d_target_.w(); + state_msg.reference_pose.orientation.x = orientation_d_target_.x(); + state_msg.reference_pose.orientation.y = orientation_d_target_.y(); + state_msg.reference_pose.orientation.z = orientation_d_target_.z(); + + state_msg.pose_error.position.x = error(0); + state_msg.pose_error.position.y = error(1); + state_msg.pose_error.position.z = error(2); + double angle = error.tail<3>().norm(); + if (angle > 1e-6) + { + Eigen::Vector3d axis = error.tail<3>() / angle; + Eigen::AngleAxisd aa(angle, axis); + Eigen::Quaterniond q_err(aa); + state_msg.reference_pose.orientation = + tf2::toMsg(tf2::Quaternion(orientation_d_target_.x(), orientation_d_target_.y(), orientation_d_target_.z(), + orientation_d_target_.w())); + } + else + { + state_msg.pose_error.orientation.w = 1.0; + state_msg.pose_error.orientation.x = 0.0; + state_msg.pose_error.orientation.y = 0.0; + state_msg.pose_error.orientation.z = 0.0; + } + + state_msg.cartesian_stiffness.force.x = cartesian_stiffness_.diagonal()(0); + state_msg.cartesian_stiffness.force.y = cartesian_stiffness_.diagonal()(1); + state_msg.cartesian_stiffness.force.z = cartesian_stiffness_.diagonal()(2); + state_msg.cartesian_stiffness.torque.x = cartesian_stiffness_.diagonal()(3); + state_msg.cartesian_stiffness.torque.y = cartesian_stiffness_.diagonal()(4); + state_msg.cartesian_stiffness.torque.z = cartesian_stiffness_.diagonal()(5); + + state_msg.cartesian_damping.force.x = cartesian_damping_.diagonal()(0); + state_msg.cartesian_damping.force.y = cartesian_damping_.diagonal()(1); + state_msg.cartesian_damping.force.z = cartesian_damping_.diagonal()(2); + state_msg.cartesian_damping.torque.x = cartesian_damping_.diagonal()(3); + state_msg.cartesian_damping.torque.y = cartesian_damping_.diagonal()(4); + state_msg.cartesian_damping.torque.z = cartesian_damping_.diagonal()(5); + + Eigen::Matrix applied_wrench = getAppliedWrench(); + state_msg.commanded_wrench.force.x = applied_wrench(0); + state_msg.commanded_wrench.force.y = applied_wrench(1); + state_msg.commanded_wrench.force.z = applied_wrench(2); + state_msg.commanded_wrench.torque.x = applied_wrench(3); + state_msg.commanded_wrench.torque.y = applied_wrench(4); + state_msg.commanded_wrench.torque.z = applied_wrench(5); + + state_msg.nullspace_stiffness = nullspace_stiffness_; + state_msg.nullspace_damping = nullspace_damping_; + + Eigen::Matrix dx = jacobian_ * dq_; + state_msg.cartesian_velocity = std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + + rt_pub_state_->unlockAndPublish(); + } + } +} void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg) { From 5b9841b19014e526520b3c68d7ee9563288c7ab5 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Wed, 12 Feb 2025 13:18:50 +0000 Subject: [PATCH 05/25] Adjust default values in Cartesian Impedance Controller --- .../cartesian_impedance_controller.hpp | 49 +++++++++---------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp index 3330ebb..8e100b3 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp @@ -26,20 +26,20 @@ namespace cartesian_impedance_controller void initNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target); /*! \brief Sets the number of joints - * + * * \param[in] n_joints Number of joints */ void setNumberOfJoints(size_t n_joints); /*! \brief Set the desired diagonal stiffnessess + nullspace stiffness - * + * * \param[in] stiffness Stiffnesses: position, orientation, nullspace * \param[in] auto_damping Apply automatic damping */ void setStiffness(const Eigen::Matrix &stiffness, bool auto_damping = true); /*! \brief Sets the Cartesian and nullspace stiffnesses - * + * * \param[in] t_x Translational stiffness x * \param[in] t_y Translational stiffness y * \param[in] t_z Translational stiffness z @@ -52,7 +52,7 @@ namespace cartesian_impedance_controller void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, double n, bool auto_damping = true); /*! \brief Sets the Cartesian and nullspace stiffnesses - * + * * \param[in] t_x Translational stiffness x * \param[in] t_y Translational stiffness y * \param[in] t_z Translational stiffness z @@ -64,7 +64,7 @@ namespace cartesian_impedance_controller void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, bool auto_damping = true); /*! \brief Set the desired damping factors - * + * * \param[in] t_x Translational damping x * \param[in] t_y Translational damping y * \param[in] t_z Translational damping z @@ -103,14 +103,14 @@ namespace cartesian_impedance_controller double filter_params_wrench); /*! \brief Maximum commanded torque change per time step - * + * * Prevents too large changes in the commanded torques by using saturation. * \param[in] d Torque change per timestep */ void setMaxTorqueDelta(double d); /*! \brief Sets maximum commanded torque change per time step and the update frequency - * + * * Prevents too large changes in the commanded torques by using saturation. * \param[in] d Torque change per timestep * \param[in] update_frequency Update frequency @@ -118,21 +118,21 @@ namespace cartesian_impedance_controller void setMaxTorqueDelta(double d, double update_frequency); /*! \brief Apply a virtual Cartesian wrench in the root frame (often "world") - * + * * Prevents too large changes in the commanded torques by using saturation. * \param[in] cartesian_wrench Wrench to apply */ void applyWrench(const Eigen::Matrix &cartesian_wrench); /*! \brief Returns the commanded torques. Performs a filtering step. - * + * * This function assumes that the internal states have already been updates. The it utilizes the control rules to calculate commands. * \return Eigen Vector of the commanded torques */ Eigen::VectorXd calculateCommandedTorques(); /*! \brief Returns the commanded torques. Performs a filtering step and updates internal state. - * + * * This function utilizes the control rules. * \param[in] q Joint positions * \param[in] dq Joint velocities @@ -145,7 +145,7 @@ namespace cartesian_impedance_controller const Eigen::MatrixXd &jacobian); /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called - * + * * \param[out] q Joint positions * \param[out] dq Joint velocities * \param[out] position End-effector position @@ -163,7 +163,7 @@ namespace cartesian_impedance_controller Eigen::VectorXd *q_d_nullspace, Eigen::Matrix *cartesian_damping) const; /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called - * + * * \param[out] position_d End-effector reference position * \param[out] orientation_d End-effector reference orientation * \param[out] cartesian_stiffness Cartesian stiffness @@ -176,19 +176,19 @@ namespace cartesian_impedance_controller Eigen::VectorXd *q_d_nullspace, Eigen::Matrix *cartesian_damping) const; /*! \brief Get the currently applied commands - * + * * \return Eigen Vector with commands */ Eigen::VectorXd getLastCommands() const; /*! \brief Get the currently applied Cartesian wrench - * + * * \return Eigen Vector with the applied wrench */ Eigen::Matrix getAppliedWrench() const; /*! \brief Get the current pose error - * + * * \return Eigen Vector with the pose error for translation and rotation */ Eigen::Matrix getPoseError() const; @@ -196,16 +196,15 @@ namespace cartesian_impedance_controller protected: size_t n_joints_{7}; //!< Number of joints to control - // Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness matrix - // Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; //!< Cartesian damping matrix -Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; -Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; + Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness matrix + Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; //!< Cartesian damping matrix + Eigen::VectorXd q_d_nullspace_; //!< Current nullspace reference pose Eigen::VectorXd q_d_nullspace_target_; //!< Nullspace reference target pose - double nullspace_stiffness_{1.0}; //!< Current nullspace stiffness - double nullspace_stiffness_target_{1.0}; //!< Nullspace stiffness target - double nullspace_damping_{1.0}; //!< Current nullspace damping - double nullspace_damping_target_{1.0}; //!< Nullspace damping target + double nullspace_stiffness_{0.0}; //!< Current nullspace stiffness + double nullspace_stiffness_target_{0.0}; //!< Nullspace stiffness target + double nullspace_damping_{0.0}; //!< Current nullspace damping + double nullspace_damping_target_{0.0}; //!< Nullspace damping target Eigen::Matrix cartesian_stiffness_target_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness target Eigen::Matrix cartesian_damping_target_{Eigen::Matrix::Identity()}; //!< Cartesian damping target @@ -238,7 +237,7 @@ Eigen::Matrix cartesian_damping_{Eigen::Matrix::Iden double filter_params_pose_{1.0}; //!< Reference pose filtering double filter_params_wrench_{1.0}; //!< Commanded wrench filtering - double delta_tau_max_{100.0}; //!< Maximum allowed torque change per time step + double delta_tau_max_{1.0}; //!< Maximum allowed torque change per time step private: /*! \brief Implements the damping based on a stiffness @@ -285,4 +284,4 @@ Eigen::Matrix cartesian_damping_{Eigen::Matrix::Iden void updateFilteredWrench(); }; -} // namespace cartesian_impedance_controller +} // namespace cartesian_impedance_controller \ No newline at end of file From c955b7d97dc2b71f4a2203f2f7fb61b4dc1935a9 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Wed, 12 Feb 2025 16:39:27 +0000 Subject: [PATCH 06/25] Fix: Custom msg type error --- CMakeLists.txt | 26 +++++++++++------------ cartesian_impedance_controller_plugin.xml | 2 +- package.xml | 3 ++- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cafd7e4..e39d463 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,33 +52,33 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -rosidl_generate_interfaces(${PROJECT_NAME}_msgs +rosidl_generate_interfaces(${PROJECT_NAME} "msg/ControllerConfig.msg" "msg/ControllerState.msg" DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) +set(library_name cartesian_impedance_controller_lib) + generate_parameter_library(cartesian_impedance_controller_parameters src/cartesian_impedance_controller_parameters.yaml ) -add_library(cartesian_impedance_controller SHARED +add_library(${library_name} SHARED src/cartesian_impedance_controller_ros.cpp src/cartesian_impedance_controller.cpp ) -add_dependencies(cartesian_impedance_controller ${PROJECT_NAME}_msgs) - -target_compile_features(cartesian_impedance_controller PUBLIC cxx_std_17) +target_compile_features(${library_name} PUBLIC cxx_std_17) -target_include_directories(cartesian_impedance_controller PUBLIC +target_include_directories(${library_name} PUBLIC $ $ - $ + $ ) -rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}_msgs "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") -target_link_libraries(cartesian_impedance_controller PUBLIC +target_link_libraries(${library_name} PUBLIC "${cpp_typesupport_target}" cartesian_impedance_controller_parameters Eigen3::Eigen @@ -88,7 +88,7 @@ target_link_libraries(cartesian_impedance_controller PUBLIC pluginlib_export_plugin_description_file(controller_interface cartesian_impedance_controller_plugin.xml) -ament_target_dependencies(cartesian_impedance_controller PUBLIC +ament_target_dependencies(${library_name} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} ) @@ -98,14 +98,14 @@ install( ) install( - TARGETS cartesian_impedance_controller cartesian_impedance_controller_parameters - EXPORT export_cartesian_impedance_controller + TARGETS ${library_name} cartesian_impedance_controller_parameters + EXPORT export_${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) -ament_export_targets(export_cartesian_impedance_controller HAS_LIBRARY_TARGET) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/cartesian_impedance_controller_plugin.xml b/cartesian_impedance_controller_plugin.xml index 47d2454..f3e0f1f 100644 --- a/cartesian_impedance_controller_plugin.xml +++ b/cartesian_impedance_controller_plugin.xml @@ -1,4 +1,4 @@ - + diff --git a/package.xml b/package.xml index 6c4d729..f211c40 100644 --- a/package.xml +++ b/package.xml @@ -36,7 +36,8 @@ rclcpp_lifecycle generate_parameter_library rosidl_default_runtime - + rosidl_typesupport_cpp + ament_cmake_gtest eigen3_cmake_module rclcpp From 8d0fb75086f2fefb29351f1ea31018c6c3909a1a Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Wed, 12 Feb 2025 16:40:02 +0000 Subject: [PATCH 07/25] Adjust default values --- ...esian_impedance_controller_parameters.yaml | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/src/cartesian_impedance_controller_parameters.yaml b/src/cartesian_impedance_controller_parameters.yaml index c852aca..80a2379 100644 --- a/src/cartesian_impedance_controller_parameters.yaml +++ b/src/cartesian_impedance_controller_parameters.yaml @@ -1,10 +1,10 @@ cartesian_impedance_controller: delta_tau_max: type: double - default_value: 100.0 + default_value: 1.0 description: "Maximum allowed change in joint torques." validation: - bounds<>: [0.0, 100.0] + bounds<>: [0.0, 1.0] wrench_ee_frame: type: string default_value: "" @@ -19,7 +19,7 @@ cartesian_impedance_controller: end_effector: type: string - default_value: "panda_hand" + default_value: "" description: "Name of the end effector link." @@ -27,49 +27,49 @@ cartesian_impedance_controller: translation: x: type: double - default_value: 20.0 + default_value: 1.0 description: "Translational damping factor for the x-axis." validation: - bounds<>: [0.0, 20.0] + bounds<>: [0.0, 1.0] y: type: double - default_value: 20.0 + default_value: 1.0 description: "Translational damping factor for the y-axis." validation: - bounds<>: [0.0, 20.0] + bounds<>: [0.0, 1.0] z: type: double - default_value: 20.0 + default_value: 1.0 description: "Translational damping factor for the z-axis." validation: - bounds<>: [0.0, 20.0] + bounds<>: [0.0, 1.0] rotation: x: type: double - default_value: 20.0 + default_value: 1.0 description: "Rotational damping factor around the x-axis." validation: - bounds<>: [0.0, 20.0] + bounds<>: [0.0, 1.0] y: type: double - default_value: 20.0 + default_value: 1.0 description: "Rotational damping factor around the y-axis." validation: - bounds<>: [0.0, 20.0] + bounds<>: [0.0, 1.0] z: type: double - default_value: 20.0 + default_value: 1.0 description: "Rotational damping factor around the z-axis." validation: - bounds<>: [0.0, 20.0] + bounds<>: [0.0, 1.0] nullspace_damping: type: double - default_value: 10.0 + default_value: 1.0 description: "Damping factor for the nullspace." validation: - bounds<>: [0.0, 100.0] + bounds<>: [0.0, 1.0] update_damping_factors: type: bool @@ -80,19 +80,19 @@ cartesian_impedance_controller: translation: x: type: double - default_value: 600.0 + default_value: 200.0 description: "Translational stiffness for the x-axis." validation: bounds<>: [0.0, 2000.0] y: type: double - default_value: 600.0 + default_value: 200.0 description: "Translational stiffness for the y-axis." validation: bounds<>: [0.0, 2000.0] z: type: double - default_value: 600.0 + default_value: 200.0 description: "Translational stiffness for the z-axis." validation: bounds<>: [0.0, 2000.0] @@ -100,19 +100,19 @@ cartesian_impedance_controller: rotation: x: type: double - default_value: 100.0 + default_value: 20.0 description: "Rotational stiffness around the x-axis." validation: bounds<>: [0.0, 300.0] y: type: double - default_value: 100.0 + default_value: 20.0 description: "Rotational stiffness around the y-axis." validation: bounds<>: [0.0, 300.0] z: type: double - default_value: 100.0 + default_value: 20.0 description: "Rotational stiffness around the z-axis." validation: bounds<>: [0.0, 300.0] @@ -122,7 +122,7 @@ cartesian_impedance_controller: default_value: 10.0 description: "Stiffness factor for the nullspace." validation: - bounds<>: [0.0, 100.0] + bounds<>: [0.0, 50.0] update_stiffness: type: bool @@ -177,25 +177,25 @@ cartesian_impedance_controller: filtering: nullspace: type: double - default_value: 1.0 + default_value: 0.1 description: "Filtering factor for nullspace." validation: bounds<>: [0.0, 1.0] stiffness: type: double - default_value: 1.0 + default_value: 0.1 description: "Filtering factor for stiffness." validation: bounds<>: [0.0, 1.0] pose: type: double - default_value: 1.0 + default_value: 0.1 description: "Filtering factor for pose." validation: bounds<>: [0.0, 1.0] wrench: type: double - default_value: 1.0 + default_value: 0.1 description: "Filtering factor for wrench." validation: bounds<>: [0.0, 1.0] From c7d33404811191e644dce4ae51f4082ecf2520d3 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Thu, 13 Feb 2025 10:36:29 +0000 Subject: [PATCH 08/25] Fix: init nullspace !! --- src/cartesian_impedance_controller_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index d0ff890..170bd23 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -307,7 +307,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_activate(const rclcpp_lifecyc update(rclcpp::Time(0), rclcpp::Duration(0, 0)); initDesiredPose(position_, orientation_); - initNullspaceConfig(q_d_nullspace_); + initNullspaceConfig(q_); for (size_t i = 0; i < dof_; i++) { if (!joint_command_handles_[i]) From 3b38f046e55a4fa906837391d3504f5404f8455e Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Thu, 13 Feb 2025 15:24:30 +0000 Subject: [PATCH 09/25] Fix: Parameters assignments in on_configure --- src/cartesian_impedance_controller_ros.cpp | 44 +++++++++++----------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 170bd23..11f3686 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -175,7 +175,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy std::bind(&CartesianImpedanceControllerRos::trajGoalCb, this, std::placeholders::_1, std::placeholders::_2), std::bind(&CartesianImpedanceControllerRos::trajCancelCb, this, std::placeholders::_1), std::bind(&CartesianImpedanceControllerRos::trajAcceptCb, this, std::placeholders::_1)); - + auto state_pub = get_node()->create_publisher("controller_state", 10); auto torques_pub = get_node()->create_publisher("commanded_torques", 10); @@ -227,6 +227,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy double stiff_ry = params_.stiffness.rotation.y; double stiff_rz = params_.stiffness.rotation.z; double stiff_ns = params_.stiffness.nullspace_stiffness; + setStiffness(stiff_tx, stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns, true); double damp_tx = params_.damping.translation.x; double damp_ty = params_.damping.translation.y; @@ -235,9 +236,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy double damp_ry = params_.damping.rotation.y; double damp_rz = params_.damping.rotation.z; double damp_ns = params_.damping.nullspace_damping; - - if (params_.damping.update_damping_factors) - setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); + setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); damping_factors_.resize(7); damping_factors_ << damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns; @@ -366,29 +365,30 @@ void CartesianImpedanceControllerRos::applyRuntimeParameters() { if (params_.stiffness.update_stiffness) { - double sx = params_.stiffness.translation.x; - double sy = params_.stiffness.translation.y; - double sz = params_.stiffness.translation.z; - double rx = params_.stiffness.rotation.x; - double ry = params_.stiffness.rotation.y; - double rz = params_.stiffness.rotation.z; - double ns = params_.stiffness.nullspace_stiffness; + double stiff_tx = params_.stiffness.translation.x; + double stiff_ty = params_.stiffness.translation.y; + double stiff_tz = params_.stiffness.translation.z; + double stiff_rx = params_.stiffness.rotation.x; + double stiff_ry = params_.stiffness.rotation.y; + double stiff_rz = params_.stiffness.rotation.z; + double stiff_ns = params_.stiffness.nullspace_stiffness; + nullspace_stiffness_target_ = stiff_ns; RCLCPP_INFO(get_node()->get_logger(), - "Updating stiffness: trans=(%.2f, %.2f, %.2f), rot=(%.2f, %.2f, %.2f), nullspace=%.2f", sx, sy, sz, rx, - ry, rz, ns); - setStiffness(sx, sy, sz, rx, ry, rz, ns, true); + "Updating stiffness: trans=(%.2f, %.2f, %.2f), rot=(%.2f, %.2f, %.2f), nullspace=%.2f", stiff_tx, + stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns); + setStiffness(stiff_tx, stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns, true); } if (params_.damping.update_damping_factors) { - double tx = params_.damping.translation.x; - double ty = params_.damping.translation.y; - double tz = params_.damping.translation.z; - double rx = params_.damping.rotation.x; - double ry = params_.damping.rotation.y; - double rz = params_.damping.rotation.z; - double ns = params_.damping.nullspace_damping; - setDampingFactors(tx, ty, tz, rx, ry, rz, ns); + double damp_tx = params_.damping.translation.x; + double damp_ty = params_.damping.translation.y; + double damp_tz = params_.damping.translation.z; + double damp_rx = params_.damping.rotation.x; + double damp_ry = params_.damping.rotation.y; + double damp_rz = params_.damping.rotation.z; + double damp_ns = params_.damping.nullspace_damping; + setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); } if (params_.wrench.apply_wrench) From 64b5a9b63115bedbd10bdcc2e24b149efc67e66a Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Thu, 13 Feb 2025 15:42:21 +0000 Subject: [PATCH 10/25] Fix: Comment out test dependencies in package.xml --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index f211c40..867b8ff 100644 --- a/package.xml +++ b/package.xml @@ -38,13 +38,13 @@ rosidl_default_runtime rosidl_typesupport_cpp - ament_cmake_gtest + rosidl_interface_packages From 605a227744bce459edf72dcc16dfcd822afa5bdb Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Thu, 13 Feb 2025 16:31:43 +0000 Subject: [PATCH 11/25] Fix: Remove unused dependencies from CMakeLists.txt and package.xml - Adds colcon ignore to external depends --- CMakeLists.txt | 8 -------- package.xml | 2 -- scripts/install_dependencies.sh | 1 + 3 files changed, 1 insertion(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e39d463..549aa2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,31 +18,23 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp control_msgs sensor_msgs - control_toolbox controller_interface geometry_msgs - joint_trajectory_controller hardware_interface - kinematics_interface rclcpp_lifecycle rclcpp_action realtime_tools tf2 - tf2_eigen tf2_geometry_msgs - tf2_kdl tf2_ros std_msgs trajectory_msgs generate_parameter_library urdf - controller_manager - ros2_control ) find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) -find_package(controller_manager REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(RBDyn REQUIRED) find_package(mc_rbdyn_urdf REQUIRED) diff --git a/package.xml b/package.xml index 867b8ff..d73b7df 100644 --- a/package.xml +++ b/package.xml @@ -19,7 +19,6 @@ action_msgs control_msgs controller_interface - controller_manager eigen3_cmake_module geometry_msgs hardware_interface @@ -32,7 +31,6 @@ tf2_eigen tf2_ros trajectory_msgs - ros2_control rclcpp_lifecycle generate_parameter_library rosidl_default_runtime diff --git a/scripts/install_dependencies.sh b/scripts/install_dependencies.sh index 87bc9cd..ce25697 100755 --- a/scripts/install_dependencies.sh +++ b/scripts/install_dependencies.sh @@ -11,6 +11,7 @@ then mkdir depends fi cd depends +touch COLCON_IGNORE # SpaceVecAlg git clone --recursive https://github.com/jrl-umi3218/SpaceVecAlg From a136014d43e94256c8d5dac1d3edeef998786f81 Mon Sep 17 00:00:00 2001 From: Cenk Cetin <105711013+mrceki@users.noreply.github.com> Date: Fri, 14 Feb 2025 14:43:42 +0000 Subject: [PATCH 12/25] Update build_code.yml --- .github/workflows/build_code.yml | 62 +++++++++++++++++--------------- 1 file changed, 34 insertions(+), 28 deletions(-) diff --git a/.github/workflows/build_code.yml b/.github/workflows/build_code.yml index ac55877..f5ad029 100644 --- a/.github/workflows/build_code.yml +++ b/.github/workflows/build_code.yml @@ -6,33 +6,39 @@ jobs: build-code: runs-on: ubuntu-latest container: - image: osrf/ros:noetic-desktop-full + image: osrf/ros:humble-desktop-full steps: - - name: Create catkin workspace - shell: bash - run: | - mkdir -p ~/catkin_ws/src - - uses: actions/checkout@v3 - with: - path: 'catkin_ws/src/Cartesian-Impedance-Controller' - - name: Build in catkin_ws - shell: bash - run: | - apt-get update - apt-get install -y python3-catkin-tools python3-rosdep git - cd catkin_ws - bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh - rosdep update - source /opt/ros/$ROS_DISTRO/setup.bash - rosdep install --from-paths src --ignore-src --rosdistro=$ROS_DISTRO -y - catkin init - catkin config --extend /opt/ros/$ROS_DISTRO - catkin build - echo "Compile complete." - - name: Run tests - shell: bash - run: | - source /opt/ros/$ROS_DISTRO/setup.bash - cd catkin_ws - catkin test \ No newline at end of file + - name: Install system dependencies + shell: bash + run: | + apt-get update && apt-get install -y \ + python3-colcon-common-extensions \ + python3-rosdep \ + git \ + nodejs npm # for actions/checkout@v3 + + - name: Create ROS2 workspace directories + shell: bash + run: | + mkdir -p $GITHUB_WORKSPACE/ros2_ws/src + + - name: Checkout repository into workspace + uses: actions/checkout@v3 + with: + ref: ros2 + path: ros2_ws/src/Cartesian-Impedance-Controller + + - name: Install package dependencies & Build workspace + shell: bash + run: | + cd $GITHUB_WORKSPACE/ros2_ws + + bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh + + rosdep update + rosdep install --from-paths src --ignore-src --rosdistro=humble -y + + source /opt/ros/humble/setup.bash + colcon build --base-paths $GITHUB_WORKSPACE/ros2_ws + From 99e6aeb210f7f0d85dc1e2f7b6a97de01f197019 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Tue, 18 Feb 2025 11:20:19 +0000 Subject: [PATCH 13/25] Rename nullspace filtering parameter to keep consistency with ROS 1 version --- src/cartesian_impedance_controller_parameters.yaml | 2 +- src/cartesian_impedance_controller_ros.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/cartesian_impedance_controller_parameters.yaml b/src/cartesian_impedance_controller_parameters.yaml index 80a2379..4bf8b8b 100644 --- a/src/cartesian_impedance_controller_parameters.yaml +++ b/src/cartesian_impedance_controller_parameters.yaml @@ -175,7 +175,7 @@ cartesian_impedance_controller: filtering: - nullspace: + nullspace_config: type: double default_value: 0.1 description: "Filtering factor for nullspace." diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 11f3686..7a8eb07 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -153,7 +153,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy RCLCPP_INFO(logger, "Wrench EE Frame: %s", params_.wrench_ee_frame.c_str()); RCLCPP_INFO(logger, "Update Frequency: %.2f", params_.update_frequency); RCLCPP_INFO(logger, "Filtering parameters - Nullspace: %.2f, Stiffness: %.2f, Pose: %.2f, Wrench: %.2f", - params_.filtering.nullspace, params_.filtering.stiffness, params_.filtering.pose, + params_.filtering.nullspace_config, params_.filtering.stiffness, params_.filtering.pose, params_.filtering.wrench); RCLCPP_INFO(logger, "Stiffness - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f", params_.stiffness.translation.x, params_.stiffness.translation.y, params_.stiffness.translation.z, @@ -214,7 +214,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy return CallbackReturn::ERROR; } - double filter_ns = params_.filtering.nullspace; + double filter_ns = params_.filtering.nullspace_config; double filter_stiff = params_.filtering.stiffness; double filter_pose = params_.filtering.pose; double filter_wrench = params_.filtering.wrench; From a6160963990634df3fe79f687ebb01a22497435b Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Tue, 18 Feb 2025 12:20:14 +0000 Subject: [PATCH 14/25] Cleanups --- .../cartesian_impedance_controller_ros.hpp | 10 +-- src/cartesian_impedance_controller_ros.cpp | 73 ++++--------------- 2 files changed, 19 insertions(+), 64 deletions(-) diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index e37610f..12d2c48 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -8,7 +8,7 @@ #include #include #include -#include + #include #include #include @@ -25,12 +25,14 @@ #include #include #include +#include +#include #include +#include #include #include #include -#include #include @@ -65,14 +67,11 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - std::string urdf_; - const Eigen::VectorXi perm_indices_ = (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); const Eigen::PermutationMatrix jacobian_perm_ = Eigen::PermutationMatrix(perm_indices_); - std::vector joint_names_; std::vector command_joint_names_; size_t dof_{0}; std::shared_ptr> rt_trajectory_; @@ -88,7 +87,6 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI std::string end_effector_; std::string wrench_ee_frame_; std::string root_frame_; - std::vector joints_angle_wraparound_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; rclcpp::Time tf_last_time_; diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 7a8eb07..816c1e4 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -1,30 +1,8 @@ #include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -#include -#include -#include -#include -#include - #include #include #include -#include -#include -#include #include #include "rclcpp/version.h" @@ -69,13 +47,6 @@ CallbackReturn CartesianImpedanceControllerRos::on_init() RCLCPP_ERROR(logger, "Exception during init: %s", e.what()); return CallbackReturn::ERROR; } - - std::string urdf_string; - if (!node->get_parameter("robot_description", urdf_string)) - { - RCLCPP_ERROR(logger, "Failed to get robot_description parameter"); - return CallbackReturn::ERROR; - } tf_br_ = std::make_shared(node); return CallbackReturn::SUCCESS; } @@ -265,14 +236,11 @@ CallbackReturn CartesianImpedanceControllerRos::on_activate(const rclcpp_lifecyc joint_velocity_state_.resize(dof_, nullptr); joint_effort_state_.resize(dof_, nullptr); - if (q_.size() != dof_) - q_.resize(dof_); - if (dq_.size() != dof_) - dq_.resize(dof_); - if (tau_m_.size() != dof_) - tau_m_.resize(dof_); - if (tau_c_.size() != dof_) - tau_c_.resize(dof_); + if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_ || tau_c_.size() != dof_) + { + RCLCPP_ERROR(logger, "Size mismatch in internal vectors (q_, dq_, tau_m_, tau_c_) vs. dof_"); + return CallbackReturn::ERROR; + } for (const auto& interface : state_interfaces) { @@ -605,9 +573,8 @@ void CartesianImpedanceControllerRos::publishMsgsAndTf() if (params_.verbosity.verbose_print) { RCLCPP_INFO(get_node()->get_logger(), "Cartesian Position: [%f, %f, %f]", position_(0), position_(1), position_(2)); - - RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", error(0), error(1), error(2), - error(3), error(4), error(5)); + RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", + error(0), error(1), error(2), error(3), error(4), error(5)); RCLCPP_INFO(get_node()->get_logger(), "Cartesian Stiffness (diag): [%f, %f, %f, %f, %f, %f]", cartesian_stiffness_.diagonal()(0), cartesian_stiffness_.diagonal()(1), @@ -716,23 +683,12 @@ void CartesianImpedanceControllerRos::publishMsgsAndTf() state_msg.pose_error.position.x = error(0); state_msg.pose_error.position.y = error(1); state_msg.pose_error.position.z = error(2); - double angle = error.tail<3>().norm(); - if (angle > 1e-6) - { - Eigen::Vector3d axis = error.tail<3>() / angle; - Eigen::AngleAxisd aa(angle, axis); - Eigen::Quaterniond q_err(aa); - state_msg.reference_pose.orientation = - tf2::toMsg(tf2::Quaternion(orientation_d_target_.x(), orientation_d_target_.y(), orientation_d_target_.z(), - orientation_d_target_.w())); - } - else - { - state_msg.pose_error.orientation.w = 1.0; - state_msg.pose_error.orientation.x = 0.0; - state_msg.pose_error.orientation.y = 0.0; - state_msg.pose_error.orientation.z = 0.0; - } + Eigen::Quaterniond q_err = + Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); + state_msg.pose_error.orientation = + tf2::toMsg(tf2::Quaternion(q_err.x(), q_err.y(), q_err.z(), q_err.w())); state_msg.cartesian_stiffness.force.x = cartesian_stiffness_.diagonal()(0); state_msg.cartesian_stiffness.force.y = cartesian_stiffness_.diagonal()(1); @@ -760,7 +716,8 @@ void CartesianImpedanceControllerRos::publishMsgsAndTf() state_msg.nullspace_damping = nullspace_damping_; Eigen::Matrix dx = jacobian_ * dq_; - state_msg.cartesian_velocity = std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + state_msg.cartesian_velocity = + std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); rt_pub_state_->unlockAndPublish(); } From 740b7fb71ea7fa8c1244a68a27754735357f2af6 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Tue, 18 Feb 2025 12:25:30 +0000 Subject: [PATCH 15/25] Code formatting --- .../cartesian_impedance_controller.hpp | 566 +++++++-------- .../cartesian_impedance_controller_ros.hpp | 48 +- src/cartesian_impedance_controller.cpp | 672 +++++++++--------- src/cartesian_impedance_controller_ros.cpp | 29 +- 4 files changed, 663 insertions(+), 652 deletions(-) diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp index 8e100b3..9fce59e 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp @@ -5,283 +5,289 @@ namespace cartesian_impedance_controller { - class CartesianImpedanceController - { - public: - CartesianImpedanceController(); - ~CartesianImpedanceController() = default; - - /*! \brief Sets pose without using filtering - * - * \param[in] position_d_target Reference positions - * \param[in] orientation_d_target Reference orientation - */ - void initDesiredPose(const Eigen::Vector3d &position_d_target, - const Eigen::Quaterniond &orientation_d_target); - - /*! \brief Sets the nullspace configuration without using filtering - * - * \param[in] q_d_nullspace_target Nullspace joint positions - */ - void initNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target); - - /*! \brief Sets the number of joints - * - * \param[in] n_joints Number of joints - */ - void setNumberOfJoints(size_t n_joints); - - /*! \brief Set the desired diagonal stiffnessess + nullspace stiffness - * - * \param[in] stiffness Stiffnesses: position, orientation, nullspace - * \param[in] auto_damping Apply automatic damping - */ - void setStiffness(const Eigen::Matrix &stiffness, bool auto_damping = true); - - /*! \brief Sets the Cartesian and nullspace stiffnesses - * - * \param[in] t_x Translational stiffness x - * \param[in] t_y Translational stiffness y - * \param[in] t_z Translational stiffness z - * \param[in] r_x Rotational stiffness x - * \param[in] r_y Rotational stiffness y - * \param[in] r_z Rotational stiffness z - * \param[in] n Nullspace stiffness - * \param[in] auto_damping Apply automatic damping - */ - void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, double n, bool auto_damping = true); - - /*! \brief Sets the Cartesian and nullspace stiffnesses - * - * \param[in] t_x Translational stiffness x - * \param[in] t_y Translational stiffness y - * \param[in] t_z Translational stiffness z - * \param[in] r_x Rotational stiffness x - * \param[in] r_y Rotational stiffness y - * \param[in] r_z Rotational stiffness z - * \param[in] auto_damping Apply automatic damping - */ - void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, bool auto_damping = true); - - /*! \brief Set the desired damping factors - * - * \param[in] t_x Translational damping x - * \param[in] t_y Translational damping y - * \param[in] t_z Translational damping z - * \param[in] r_x Rotational damping x - * \param[in] r_y Rotational damping y - * \param[in] r_z Rotational damping z - * \param[in] n Nullspace damping - */ - void setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n); - - /*! \brief Sets the desired end-effector pose - * - * Sets them as a new target, so filtering can be applied on them. - * \param[in] position_d New reference position - * \param[in] orientation_d New reference orientation - */ - void setReferencePose(const Eigen::Vector3d &position_d, const Eigen::Quaterniond &orientation_d); - - /*! \brief Sets a new nullspace joint configuration - * - * Sets them as a new target, so filtering can be applied on them. - * \param[in] q_d_nullspace_target New joint configuration - */ - void setNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target); - - /*! \brief Sets filtering on stiffness + end-effector pose. - * - * Default inactive && depends on update_frequency - * \param[in] update_frequency The expected controller update frequency - * \param[in] filter_params_nullspace_config Filter setting for nullspace config - * \param[in] filter_params_nullspace_config Filter setting for the stiffness - * \param[in] filter_params_nullspace_config Filter setting for the pose - * \param[in] filter_params_nullspace_config Filter setting for the commanded wrenc - */ - void setFiltering(double update_frequency, double filter_params_nullspace_config, double filter_params_stiffness, double filter_params_pose, - double filter_params_wrench); - - /*! \brief Maximum commanded torque change per time step - * - * Prevents too large changes in the commanded torques by using saturation. - * \param[in] d Torque change per timestep - */ - void setMaxTorqueDelta(double d); - - /*! \brief Sets maximum commanded torque change per time step and the update frequency - * - * Prevents too large changes in the commanded torques by using saturation. - * \param[in] d Torque change per timestep - * \param[in] update_frequency Update frequency - */ - void setMaxTorqueDelta(double d, double update_frequency); - - /*! \brief Apply a virtual Cartesian wrench in the root frame (often "world") - * - * Prevents too large changes in the commanded torques by using saturation. - * \param[in] cartesian_wrench Wrench to apply - */ - void applyWrench(const Eigen::Matrix &cartesian_wrench); - - /*! \brief Returns the commanded torques. Performs a filtering step. - * - * This function assumes that the internal states have already been updates. The it utilizes the control rules to calculate commands. - * \return Eigen Vector of the commanded torques - */ - Eigen::VectorXd calculateCommandedTorques(); - - /*! \brief Returns the commanded torques. Performs a filtering step and updates internal state. - * - * This function utilizes the control rules. - * \param[in] q Joint positions - * \param[in] dq Joint velocities - * \param[in] position End-effector position - * \param[in] orientation End-effector orientation - * \param[in] jacobian Jacobian - */ - Eigen::VectorXd calculateCommandedTorques(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, - const Eigen::Vector3d &position, Eigen::Quaterniond orientation, - const Eigen::MatrixXd &jacobian); - - /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called - * - * \param[out] q Joint positions - * \param[out] dq Joint velocities - * \param[out] position End-effector position - * \param[out] orientation End-effector orientation - * \param[out] position_d End-effector reference position - * \param[out] orientation_d End-effector reference orientation - * \param[out] cartesian_stiffness Cartesian stiffness - * \param[out] nullspace_stiffness Nullspace stiffness - * \param[out] q_d_nullspace Nullspace reference position - * \param[out] cartesian_damping Cartesian damping - */ - void getState(Eigen::VectorXd *q, Eigen::VectorXd *dq, Eigen::Vector3d *position, Eigen::Quaterniond *orientation, - Eigen::Vector3d *position_d, Eigen::Quaterniond *orientation_d, - Eigen::Matrix *cartesian_stiffness, double *nullspace_stiffness, - Eigen::VectorXd *q_d_nullspace, Eigen::Matrix *cartesian_damping) const; - - /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called - * - * \param[out] position_d End-effector reference position - * \param[out] orientation_d End-effector reference orientation - * \param[out] cartesian_stiffness Cartesian stiffness - * \param[out] nullspace_stiffness Nullspace stiffness - * \param[out] q_d_nullspace Nullspace reference position - * \param[out] cartesian_damping Cartesian damping - */ - void getState(Eigen::Vector3d *position_d, Eigen::Quaterniond *orientation_d, - Eigen::Matrix *cartesian_stiffness, double *nullspace_stiffness, - Eigen::VectorXd *q_d_nullspace, Eigen::Matrix *cartesian_damping) const; - - /*! \brief Get the currently applied commands - * - * \return Eigen Vector with commands - */ - Eigen::VectorXd getLastCommands() const; - - /*! \brief Get the currently applied Cartesian wrench - * - * \return Eigen Vector with the applied wrench - */ - Eigen::Matrix getAppliedWrench() const; - - /*! \brief Get the current pose error - * - * \return Eigen Vector with the pose error for translation and rotation - */ - Eigen::Matrix getPoseError() const; - - protected: - size_t n_joints_{7}; //!< Number of joints to control - - Eigen::Matrix cartesian_stiffness_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness matrix - Eigen::Matrix cartesian_damping_{Eigen::Matrix::Identity()}; //!< Cartesian damping matrix - - Eigen::VectorXd q_d_nullspace_; //!< Current nullspace reference pose - Eigen::VectorXd q_d_nullspace_target_; //!< Nullspace reference target pose - double nullspace_stiffness_{0.0}; //!< Current nullspace stiffness - double nullspace_stiffness_target_{0.0}; //!< Nullspace stiffness target - double nullspace_damping_{0.0}; //!< Current nullspace damping - double nullspace_damping_target_{0.0}; //!< Nullspace damping target - - Eigen::Matrix cartesian_stiffness_target_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness target - Eigen::Matrix cartesian_damping_target_{Eigen::Matrix::Identity()}; //!< Cartesian damping target - Eigen::Matrix damping_factors_{Eigen::Matrix::Ones()}; //!< Damping factors - - Eigen::VectorXd q_; //!< Joint positions - Eigen::VectorXd dq_; //!< Joint velocities - - Eigen::MatrixXd jacobian_; //!< Jacobian. Row format: 3 translations, 3 rotation - - // End Effector - Eigen::Matrix error_; //!< Calculate pose error - Eigen::Vector3d position_{Eigen::Vector3d::Zero()}; //!< Current end-effector position - Eigen::Vector3d position_d_{Eigen::Vector3d::Zero()}; //!< Current end-effector reference position - Eigen::Vector3d position_d_target_{Eigen::Vector3d::Zero()}; //!< End-effector target position - - Eigen::Quaterniond orientation_{Eigen::Quaterniond::Identity()}; //!< Current end-effector orientation - Eigen::Quaterniond orientation_d_{Eigen::Quaterniond::Identity()}; //!< Current end-effector target orientation - Eigen::Quaterniond orientation_d_target_{Eigen::Quaterniond::Identity()}; //!< End-effector orientation target - - // External applied forces - Eigen::Matrix cartesian_wrench_{Eigen::Matrix::Zero()}; //!< Current Cartesian wrench - Eigen::Matrix cartesian_wrench_target_{Eigen::Matrix::Zero()}; //!< Cartesian wrench target - - Eigen::VectorXd tau_c_; //!< Last commanded torques - - double update_frequency_{1000}; //!< Update frequency in Hz - double filter_params_nullspace_config_{1.0}; //!< Nullspace filtering - double filter_params_stiffness_{1.0}; //!< Cartesian stiffness filtering - double filter_params_pose_{1.0}; //!< Reference pose filtering - double filter_params_wrench_{1.0}; //!< Commanded wrench filtering - - double delta_tau_max_{1.0}; //!< Maximum allowed torque change per time step - - private: - /*! \brief Implements the damping based on a stiffness - * - * Damping rule is 2*sqrt(stiffness) - * \param[in] stiffness Stiffness value - * \return Damping value - */ - double dampingRule(double stiffness) const; - - /*! \brief Applies the damping rule with all stiffness values - */ - void applyDamping(); - - /*! Sets the update frequency - * - * \param[in] freq Update frequency - */ - void setUpdateFrequency(double freq); - - /*! \brief Sets the filter value and asserts bounds - * - * \param[in] val New value - * \param[out] saved_val Pointer to the value to be set - */ - void setFilterValue(double val, double *saved_val); - - /*! \brief Adds a percental filtering effect to the nullspace configuration - * - * Gradually moves the nullspace configuration to the target configuration. - */ - void updateFilteredNullspaceConfig(); - - /*! \brief Adds a percental filtering effect to stiffness - */ - void updateFilteredStiffness(); - - /*! \brief Adds a percental filtering effect to the end-effector pose - */ - void updateFilteredPose(); - - /*! \brief Adds a percental filtering effect to the applied Cartesian wrench - */ - void updateFilteredWrench(); - }; - -} // namespace cartesian_impedance_controller \ No newline at end of file +class CartesianImpedanceController +{ +public: + CartesianImpedanceController(); + ~CartesianImpedanceController() = default; + + /*! \brief Sets pose without using filtering + * + * \param[in] position_d_target Reference positions + * \param[in] orientation_d_target Reference orientation + */ + void initDesiredPose(const Eigen::Vector3d& position_d_target, const Eigen::Quaterniond& orientation_d_target); + + /*! \brief Sets the nullspace configuration without using filtering + * + * \param[in] q_d_nullspace_target Nullspace joint positions + */ + void initNullspaceConfig(const Eigen::VectorXd& q_d_nullspace_target); + + /*! \brief Sets the number of joints + * + * \param[in] n_joints Number of joints + */ + void setNumberOfJoints(size_t n_joints); + + /*! \brief Set the desired diagonal stiffnessess + nullspace stiffness + * + * \param[in] stiffness Stiffnesses: position, orientation, nullspace + * \param[in] auto_damping Apply automatic damping + */ + void setStiffness(const Eigen::Matrix& stiffness, bool auto_damping = true); + + /*! \brief Sets the Cartesian and nullspace stiffnesses + * + * \param[in] t_x Translational stiffness x + * \param[in] t_y Translational stiffness y + * \param[in] t_z Translational stiffness z + * \param[in] r_x Rotational stiffness x + * \param[in] r_y Rotational stiffness y + * \param[in] r_z Rotational stiffness z + * \param[in] n Nullspace stiffness + * \param[in] auto_damping Apply automatic damping + */ + void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, double n, + bool auto_damping = true); + + /*! \brief Sets the Cartesian and nullspace stiffnesses + * + * \param[in] t_x Translational stiffness x + * \param[in] t_y Translational stiffness y + * \param[in] t_z Translational stiffness z + * \param[in] r_x Rotational stiffness x + * \param[in] r_y Rotational stiffness y + * \param[in] r_z Rotational stiffness z + * \param[in] auto_damping Apply automatic damping + */ + void setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, bool auto_damping = true); + + /*! \brief Set the desired damping factors + * + * \param[in] t_x Translational damping x + * \param[in] t_y Translational damping y + * \param[in] t_z Translational damping z + * \param[in] r_x Rotational damping x + * \param[in] r_y Rotational damping y + * \param[in] r_z Rotational damping z + * \param[in] n Nullspace damping + */ + void setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n); + + /*! \brief Sets the desired end-effector pose + * + * Sets them as a new target, so filtering can be applied on them. + * \param[in] position_d New reference position + * \param[in] orientation_d New reference orientation + */ + void setReferencePose(const Eigen::Vector3d& position_d, const Eigen::Quaterniond& orientation_d); + + /*! \brief Sets a new nullspace joint configuration + * + * Sets them as a new target, so filtering can be applied on them. + * \param[in] q_d_nullspace_target New joint configuration + */ + void setNullspaceConfig(const Eigen::VectorXd& q_d_nullspace_target); + + /*! \brief Sets filtering on stiffness + end-effector pose. + * + * Default inactive && depends on update_frequency + * \param[in] update_frequency The expected controller update frequency + * \param[in] filter_params_nullspace_config Filter setting for nullspace config + * \param[in] filter_params_nullspace_config Filter setting for the stiffness + * \param[in] filter_params_nullspace_config Filter setting for the pose + * \param[in] filter_params_nullspace_config Filter setting for the commanded wrenc + */ + void setFiltering(double update_frequency, double filter_params_nullspace_config, double filter_params_stiffness, + double filter_params_pose, double filter_params_wrench); + + /*! \brief Maximum commanded torque change per time step + * + * Prevents too large changes in the commanded torques by using saturation. + * \param[in] d Torque change per timestep + */ + void setMaxTorqueDelta(double d); + + /*! \brief Sets maximum commanded torque change per time step and the update frequency + * + * Prevents too large changes in the commanded torques by using saturation. + * \param[in] d Torque change per timestep + * \param[in] update_frequency Update frequency + */ + void setMaxTorqueDelta(double d, double update_frequency); + + /*! \brief Apply a virtual Cartesian wrench in the root frame (often "world") + * + * Prevents too large changes in the commanded torques by using saturation. + * \param[in] cartesian_wrench Wrench to apply + */ + void applyWrench(const Eigen::Matrix& cartesian_wrench); + + /*! \brief Returns the commanded torques. Performs a filtering step. + * + * This function assumes that the internal states have already been updates. The it utilizes the control rules to + * calculate commands. \return Eigen Vector of the commanded torques + */ + Eigen::VectorXd calculateCommandedTorques(); + + /*! \brief Returns the commanded torques. Performs a filtering step and updates internal state. + * + * This function utilizes the control rules. + * \param[in] q Joint positions + * \param[in] dq Joint velocities + * \param[in] position End-effector position + * \param[in] orientation End-effector orientation + * \param[in] jacobian Jacobian + */ + Eigen::VectorXd calculateCommandedTorques(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, + const Eigen::Vector3d& position, Eigen::Quaterniond orientation, + const Eigen::MatrixXd& jacobian); + + /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called + * + * \param[out] q Joint positions + * \param[out] dq Joint velocities + * \param[out] position End-effector position + * \param[out] orientation End-effector orientation + * \param[out] position_d End-effector reference position + * \param[out] orientation_d End-effector reference orientation + * \param[out] cartesian_stiffness Cartesian stiffness + * \param[out] nullspace_stiffness Nullspace stiffness + * \param[out] q_d_nullspace Nullspace reference position + * \param[out] cartesian_damping Cartesian damping + */ + void getState(Eigen::VectorXd* q, Eigen::VectorXd* dq, Eigen::Vector3d* position, Eigen::Quaterniond* orientation, + Eigen::Vector3d* position_d, Eigen::Quaterniond* orientation_d, + Eigen::Matrix* cartesian_stiffness, double* nullspace_stiffness, + Eigen::VectorXd* q_d_nullspace, Eigen::Matrix* cartesian_damping) const; + + /*! \brief Get the state of the controller. Updates when "calculateCommandedTorques" is called + * + * \param[out] position_d End-effector reference position + * \param[out] orientation_d End-effector reference orientation + * \param[out] cartesian_stiffness Cartesian stiffness + * \param[out] nullspace_stiffness Nullspace stiffness + * \param[out] q_d_nullspace Nullspace reference position + * \param[out] cartesian_damping Cartesian damping + */ + void getState(Eigen::Vector3d* position_d, Eigen::Quaterniond* orientation_d, + Eigen::Matrix* cartesian_stiffness, double* nullspace_stiffness, + Eigen::VectorXd* q_d_nullspace, Eigen::Matrix* cartesian_damping) const; + + /*! \brief Get the currently applied commands + * + * \return Eigen Vector with commands + */ + Eigen::VectorXd getLastCommands() const; + + /*! \brief Get the currently applied Cartesian wrench + * + * \return Eigen Vector with the applied wrench + */ + Eigen::Matrix getAppliedWrench() const; + + /*! \brief Get the current pose error + * + * \return Eigen Vector with the pose error for translation and rotation + */ + Eigen::Matrix getPoseError() const; + +protected: + size_t n_joints_{ 7 }; //!< Number of joints to control + + Eigen::Matrix cartesian_stiffness_{ Eigen::Matrix::Identity() }; //!< Cartesian stiffness + //!< matrix + Eigen::Matrix cartesian_damping_{ Eigen::Matrix::Identity() }; //!< Cartesian damping + //!< matrix + + Eigen::VectorXd q_d_nullspace_; //!< Current nullspace reference pose + Eigen::VectorXd q_d_nullspace_target_; //!< Nullspace reference target pose + double nullspace_stiffness_{ 0.0 }; //!< Current nullspace stiffness + double nullspace_stiffness_target_{ 0.0 }; //!< Nullspace stiffness target + double nullspace_damping_{ 0.0 }; //!< Current nullspace damping + double nullspace_damping_target_{ 0.0 }; //!< Nullspace damping target + + Eigen::Matrix cartesian_stiffness_target_{ + Eigen::Matrix::Identity() + }; //!< Cartesian stiffness target + Eigen::Matrix cartesian_damping_target_{ Eigen::Matrix::Identity() }; //!< Cartesian + //!< damping target + Eigen::Matrix damping_factors_{ Eigen::Matrix::Ones() }; //!< Damping factors + + Eigen::VectorXd q_; //!< Joint positions + Eigen::VectorXd dq_; //!< Joint velocities + + Eigen::MatrixXd jacobian_; //!< Jacobian. Row format: 3 translations, 3 rotation + + // End Effector + Eigen::Matrix error_; //!< Calculate pose error + Eigen::Vector3d position_{ Eigen::Vector3d::Zero() }; //!< Current end-effector position + Eigen::Vector3d position_d_{ Eigen::Vector3d::Zero() }; //!< Current end-effector reference position + Eigen::Vector3d position_d_target_{ Eigen::Vector3d::Zero() }; //!< End-effector target position + + Eigen::Quaterniond orientation_{ Eigen::Quaterniond::Identity() }; //!< Current end-effector orientation + Eigen::Quaterniond orientation_d_{ Eigen::Quaterniond::Identity() }; //!< Current end-effector target orientation + Eigen::Quaterniond orientation_d_target_{ Eigen::Quaterniond::Identity() }; //!< End-effector orientation target + + // External applied forces + Eigen::Matrix cartesian_wrench_{ Eigen::Matrix::Zero() }; //!< Current Cartesian wrench + Eigen::Matrix cartesian_wrench_target_{ Eigen::Matrix::Zero() }; //!< Cartesian wrench + //!< target + + Eigen::VectorXd tau_c_; //!< Last commanded torques + + double update_frequency_{ 1000 }; //!< Update frequency in Hz + double filter_params_nullspace_config_{ 1.0 }; //!< Nullspace filtering + double filter_params_stiffness_{ 1.0 }; //!< Cartesian stiffness filtering + double filter_params_pose_{ 1.0 }; //!< Reference pose filtering + double filter_params_wrench_{ 1.0 }; //!< Commanded wrench filtering + + double delta_tau_max_{ 1.0 }; //!< Maximum allowed torque change per time step + +private: + /*! \brief Implements the damping based on a stiffness + * + * Damping rule is 2*sqrt(stiffness) + * \param[in] stiffness Stiffness value + * \return Damping value + */ + double dampingRule(double stiffness) const; + + /*! \brief Applies the damping rule with all stiffness values + */ + void applyDamping(); + + /*! Sets the update frequency + * + * \param[in] freq Update frequency + */ + void setUpdateFrequency(double freq); + + /*! \brief Sets the filter value and asserts bounds + * + * \param[in] val New value + * \param[out] saved_val Pointer to the value to be set + */ + void setFilterValue(double val, double* saved_val); + + /*! \brief Adds a percental filtering effect to the nullspace configuration + * + * Gradually moves the nullspace configuration to the target configuration. + */ + void updateFilteredNullspaceConfig(); + + /*! \brief Adds a percental filtering effect to stiffness + */ + void updateFilteredStiffness(); + + /*! \brief Adds a percental filtering effect to the end-effector pose + */ + void updateFilteredPose(); + + /*! \brief Adds a percental filtering effect to the applied Cartesian wrench + */ + void updateFilteredWrench(); +}; + +} // namespace cartesian_impedance_controller \ No newline at end of file diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index 12d2c48..670233e 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -45,7 +45,8 @@ #include #include -namespace cartesian_impedance_controller { +namespace cartesian_impedance_controller +{ class ParamListener; @@ -59,21 +60,20 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI controller_interface::CallbackReturn on_init() override; controller_interface::InterfaceConfiguration command_interface_configuration() const override; controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: - const Eigen::VectorXi perm_indices_ = - (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); + const Eigen::VectorXi perm_indices_ = (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); const Eigen::PermutationMatrix jacobian_perm_ = Eigen::PermutationMatrix(perm_indices_); std::vector command_joint_names_; - size_t dof_{0}; + size_t dof_{ 0 }; std::shared_ptr> rt_trajectory_; std::vector joint_command_handles_; @@ -90,11 +90,12 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; rclcpp::Time tf_last_time_; - std::shared_ptr tf_br_; + std::shared_ptr tf_br_; rbdyn_wrapper rbdyn_wrapper_; - std::shared_ptr> rt_pub_state_; + std::shared_ptr> + rt_pub_state_; std::shared_ptr> rt_pub_torques_; rclcpp::Subscription::SharedPtr sub_cart_stiffness_; @@ -117,14 +118,14 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI Eigen::Quaterniond orientation_d_target_; Eigen::VectorXd tau_m_; - Eigen::VectorXd damping_factors_; + Eigen::VectorXd damping_factors_; double nullspace_stiffness_target_; void read_state_from_hardware(); void write_command_to_hardware(); void write_zero_commands_to_hardware(); - bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *orientation); - bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); + bool getFk(const Eigen::VectorXd& q, Eigen::Vector3d* position, Eigen::Quaterniond* orientation); + bool getJacobian(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, Eigen::MatrixXd* jacobian); void controllerConfigCb(const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg); void cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg); @@ -132,16 +133,17 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI void referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); void wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); void trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg); - rclcpp_action::GoalResponse trajGoalCb(const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - rclcpp_action::CancelResponse trajCancelCb(const std::shared_ptr> goal_handle); - void trajAcceptCb(std::shared_ptr> goal_handle); - void trajStart(const trajectory_msgs::msg::JointTrajectory & trajectory); + rclcpp_action::GoalResponse trajGoalCb(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse trajCancelCb( + const std::shared_ptr> goal_handle); + void trajAcceptCb( + std::shared_ptr> goal_handle); + void trajStart(const trajectory_msgs::msg::JointTrajectory& trajectory); void trajUpdate(); - bool transformWrench(Eigen::Matrix * wrench, - const std::string & from_frame, - const std::string & to_frame) const; + bool transformWrench(Eigen::Matrix* wrench, const std::string& from_frame, + const std::string& to_frame) const; void publishMsgsAndTf(); void applyRuntimeParameters(); bool initRBDyn(); diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index c8fc3b9..aa9c124 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -7,398 +7,406 @@ namespace cartesian_impedance_controller { - /*! \brief Calculates the orientation error between two quaternions - * - * \param[in] orientation_d Reference orientation - * \param[in] orientation Current orientation - * \return Eigen Vector with the error - */ - Eigen::Vector3d calculateOrientationError(const Eigen::Quaterniond &orientation_d, Eigen::Quaterniond orientation) - { - // Orientation error - if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) - { - orientation.coeffs() << -orientation.coeffs(); - } - // "difference" quaternion - const Eigen::Quaterniond error_quaternion(orientation * orientation_d.inverse()); - // convert to axis angle - Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion); - return error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle(); - } - - /*! \brief Calculates a filtered percental update - * - * \param[in] target Target value - * \param[in] current Current value - * \param[in] filter Percentage of the target value - * \return Calculated value - */ - template - inline T filteredUpdate(T target, T current, double filter) - { - return (1.0 - filter) * current + filter * target; - } - - /*! \brief Calculates the filter step - * - * \param[in] update_frequency Update frequency in Hz - * \param[in] filter_percentage Filter percentage - * \return Filter step - */ - inline double filterStep(const double &update_frequency, const double &filter_percentage) - { - const double kappa = -1 / (std::log(1 - std::min(filter_percentage, 0.999999))); - return 1.0 / (kappa * update_frequency + 1.0); - } - - /*! \brief Saturate a variable x with the limits x_min and x_max - * - * \param[in] x Value - * \param[in] x_min Minimal value - * \param[in] x_max Maximum value - * \return Saturated value - */ - inline double saturateValue(double x, double x_min, double x_max) - { - return std::min(std::max(x, x_min), x_max); - } - - /*! Saturate the torque rate to not stress the motors - * - * \param[in] tau_d_calculated Calculated input torques - * \param[out] tau_d_saturated Saturated torque values - * \param[in] delta_tau_max - */ - inline void saturateTorqueRate(const Eigen::VectorXd &tau_d_calculated, Eigen::VectorXd *tau_d_saturated, double delta_tau_max) - { - for (size_t i = 0; i < tau_d_calculated.size(); i++) - { - const double difference = tau_d_calculated[i] - tau_d_saturated->operator()(i); - tau_d_saturated->operator()(i) += saturateValue(difference, -delta_tau_max, delta_tau_max); - } - } - - CartesianImpedanceController::CartesianImpedanceController() +/*! \brief Calculates the orientation error between two quaternions + * + * \param[in] orientation_d Reference orientation + * \param[in] orientation Current orientation + * \return Eigen Vector with the error + */ +Eigen::Vector3d calculateOrientationError(const Eigen::Quaterniond& orientation_d, Eigen::Quaterniond orientation) +{ + // Orientation error + if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) + { + orientation.coeffs() << -orientation.coeffs(); + } + // "difference" quaternion + const Eigen::Quaterniond error_quaternion(orientation * orientation_d.inverse()); + // convert to axis angle + Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion); + return error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle(); +} + +/*! \brief Calculates a filtered percental update + * + * \param[in] target Target value + * \param[in] current Current value + * \param[in] filter Percentage of the target value + * \return Calculated value + */ +template +inline T filteredUpdate(T target, T current, double filter) +{ + return (1.0 - filter) * current + filter * target; +} + +/*! \brief Calculates the filter step + * + * \param[in] update_frequency Update frequency in Hz + * \param[in] filter_percentage Filter percentage + * \return Filter step + */ +inline double filterStep(const double& update_frequency, const double& filter_percentage) +{ + const double kappa = -1 / (std::log(1 - std::min(filter_percentage, 0.999999))); + return 1.0 / (kappa * update_frequency + 1.0); +} + +/*! \brief Saturate a variable x with the limits x_min and x_max + * + * \param[in] x Value + * \param[in] x_min Minimal value + * \param[in] x_max Maximum value + * \return Saturated value + */ +inline double saturateValue(double x, double x_min, double x_max) +{ + return std::min(std::max(x, x_min), x_max); +} + +/*! Saturate the torque rate to not stress the motors + * + * \param[in] tau_d_calculated Calculated input torques + * \param[out] tau_d_saturated Saturated torque values + * \param[in] delta_tau_max + */ +inline void saturateTorqueRate(const Eigen::VectorXd& tau_d_calculated, Eigen::VectorXd* tau_d_saturated, + double delta_tau_max) +{ + for (size_t i = 0; i < tau_d_calculated.size(); i++) { - // Default stiffness values - this->setStiffness(200., 200., 200., 20., 20., 20., 0.); - this->cartesian_stiffness_ = this->cartesian_stiffness_target_; - this->cartesian_damping_ = this->cartesian_damping_target_; + const double difference = tau_d_calculated[i] - tau_d_saturated->operator()(i); + tau_d_saturated->operator()(i) += saturateValue(difference, -delta_tau_max, delta_tau_max); } +} - void CartesianImpedanceController::initDesiredPose(const Eigen::Vector3d &position_d_target, - const Eigen::Quaterniond &orientation_d_target) - { - this->setReferencePose(position_d_target, orientation_d_target); - this->position_d_ = position_d_target; - this->orientation_d_ = orientation_d_target; - } +CartesianImpedanceController::CartesianImpedanceController() +{ + // Default stiffness values + this->setStiffness(200., 200., 200., 20., 20., 20., 0.); + this->cartesian_stiffness_ = this->cartesian_stiffness_target_; + this->cartesian_damping_ = this->cartesian_damping_target_; +} + +void CartesianImpedanceController::initDesiredPose(const Eigen::Vector3d& position_d_target, + const Eigen::Quaterniond& orientation_d_target) +{ + this->setReferencePose(position_d_target, orientation_d_target); + this->position_d_ = position_d_target; + this->orientation_d_ = orientation_d_target; +} - void CartesianImpedanceController::initNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target) - { - this->setNullspaceConfig(q_d_nullspace_target); - this->q_d_nullspace_ = this->q_d_nullspace_target_; - } +void CartesianImpedanceController::initNullspaceConfig(const Eigen::VectorXd& q_d_nullspace_target) +{ + this->setNullspaceConfig(q_d_nullspace_target); + this->q_d_nullspace_ = this->q_d_nullspace_target_; +} - void CartesianImpedanceController::setNumberOfJoints(size_t n_joints) +void CartesianImpedanceController::setNumberOfJoints(size_t n_joints) +{ + if (n_joints < 0) { - if (n_joints < 0) - { - throw std::invalid_argument("Number of joints must be positive"); - } - this->n_joints_ = n_joints; - this->q_ = Eigen::VectorXd::Zero(this->n_joints_); - this->dq_ = Eigen::VectorXd::Zero(this->n_joints_); - this->jacobian_ = Eigen::MatrixXd::Zero(6, this->n_joints_); - this->q_d_nullspace_ = Eigen::VectorXd::Zero(this->n_joints_); - this->q_d_nullspace_target_ = this->q_d_nullspace_; - this->tau_c_ = Eigen::VectorXd::Zero(this->n_joints_); + throw std::invalid_argument("Number of joints must be positive"); } + this->n_joints_ = n_joints; + this->q_ = Eigen::VectorXd::Zero(this->n_joints_); + this->dq_ = Eigen::VectorXd::Zero(this->n_joints_); + this->jacobian_ = Eigen::MatrixXd::Zero(6, this->n_joints_); + this->q_d_nullspace_ = Eigen::VectorXd::Zero(this->n_joints_); + this->q_d_nullspace_target_ = this->q_d_nullspace_; + this->tau_c_ = Eigen::VectorXd::Zero(this->n_joints_); +} - void CartesianImpedanceController::setStiffness(const Eigen::Matrix &stiffness, bool auto_damping) +void CartesianImpedanceController::setStiffness(const Eigen::Matrix& stiffness, bool auto_damping) +{ + for (int i = 0; i < 6; i++) { - for (int i = 0; i < 6; i++) + // Set diagonal values of stiffness matrix + if (stiffness(i) < 0.0) { - // Set diagonal values of stiffness matrix - if (stiffness(i) < 0.0) - { - assert(stiffness(i) >= 0 && "Stiffness values need to be positive."); - this->cartesian_stiffness_target_(i, i) = 0.0; - } - else - { - this->cartesian_stiffness_target_(i, i) = stiffness(i); - } - } - if (stiffness(6) < 0.0) { - assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); - this->nullspace_stiffness_target_ = 0.0; + assert(stiffness(i) >= 0 && "Stiffness values need to be positive."); + this->cartesian_stiffness_target_(i, i) = 0.0; } else { - this->nullspace_stiffness_target_ = stiffness(6); - } - if (auto_damping) - { - this->applyDamping(); + this->cartesian_stiffness_target_(i, i) = stiffness(i); } } - - void CartesianImpedanceController::setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, - double n, bool auto_damping) + if (stiffness(6) < 0.0) { - Eigen::Matrix stiffness_vector(7); - stiffness_vector << t_x, t_y, t_z, r_x, r_y, r_z, n; - this->setStiffness(stiffness_vector, auto_damping); + assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); + this->nullspace_stiffness_target_ = 0.0; } - - void CartesianImpedanceController::setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, bool auto_damping) + else { - Eigen::Matrix stiffness_vector(7); - stiffness_vector << t_x, t_y, t_z, r_x, r_y, r_z, this->nullspace_stiffness_target_; - this->setStiffness(stiffness_vector, auto_damping); + this->nullspace_stiffness_target_ = stiffness(6); } - - void CartesianImpedanceController::setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, - double d_n) + if (auto_damping) { - Eigen::Matrix damping_new; - damping_new << d_x, d_y, d_z, d_a, d_b, d_c, d_n; - for (size_t i = 0; i < damping_new.size(); i++) - { - if (damping_new(i) < 0) - { - assert(damping_new(i) >= 0 && "Damping factor must not be negative."); - damping_new(i) = this->damping_factors_(i); - } - } - this->damping_factors_ = damping_new; this->applyDamping(); } +} + +void CartesianImpedanceController::setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, + double n, bool auto_damping) +{ + Eigen::Matrix stiffness_vector(7); + stiffness_vector << t_x, t_y, t_z, r_x, r_y, r_z, n; + this->setStiffness(stiffness_vector, auto_damping); +} - void CartesianImpedanceController::applyDamping() +void CartesianImpedanceController::setStiffness(double t_x, double t_y, double t_z, double r_x, double r_y, double r_z, + bool auto_damping) +{ + Eigen::Matrix stiffness_vector(7); + stiffness_vector << t_x, t_y, t_z, r_x, r_y, r_z, this->nullspace_stiffness_target_; + this->setStiffness(stiffness_vector, auto_damping); +} + +void CartesianImpedanceController::setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, + double d_c, double d_n) +{ + Eigen::Matrix damping_new; + damping_new << d_x, d_y, d_z, d_a, d_b, d_c, d_n; + for (size_t i = 0; i < damping_new.size(); i++) { - for (int i = 0; i < 6; i++) + if (damping_new(i) < 0) { - assert(this->damping_factors_(i) >= 0.0 && "Damping values need to be positive."); - this->cartesian_damping_target_(i, i) = - this->damping_factors_(i) * this->dampingRule(this->cartesian_stiffness_target_(i, i)); + assert(damping_new(i) >= 0 && "Damping factor must not be negative."); + damping_new(i) = this->damping_factors_(i); } - assert(this->damping_factors_(6) >= 0.0 && "Damping values need to be positive."); - this->nullspace_damping_target_ = this->damping_factors_(6) * this->dampingRule(this->nullspace_stiffness_target_); } + this->damping_factors_ = damping_new; + this->applyDamping(); +} - void CartesianImpedanceController::setReferencePose(const Eigen::Vector3d &position_d_target, - const Eigen::Quaterniond &orientation_d_target) +void CartesianImpedanceController::applyDamping() +{ + for (int i = 0; i < 6; i++) { - this->position_d_target_ << position_d_target; - this->orientation_d_target_.coeffs() << orientation_d_target.coeffs(); - this->orientation_d_target_.normalize(); + assert(this->damping_factors_(i) >= 0.0 && "Damping values need to be positive."); + this->cartesian_damping_target_(i, i) = + this->damping_factors_(i) * this->dampingRule(this->cartesian_stiffness_target_(i, i)); } + assert(this->damping_factors_(6) >= 0.0 && "Damping values need to be positive."); + this->nullspace_damping_target_ = this->damping_factors_(6) * this->dampingRule(this->nullspace_stiffness_target_); +} - void CartesianImpedanceController::setNullspaceConfig(const Eigen::VectorXd &q_d_nullspace_target) - { - assert(q_d_nullspace_target.size() == this->n_joints_ && "Nullspace target needs to same size as n_joints_"); - this->q_d_nullspace_target_ << q_d_nullspace_target; - } +void CartesianImpedanceController::setReferencePose(const Eigen::Vector3d& position_d_target, + const Eigen::Quaterniond& orientation_d_target) +{ + this->position_d_target_ << position_d_target; + this->orientation_d_target_.coeffs() << orientation_d_target.coeffs(); + this->orientation_d_target_.normalize(); +} - void CartesianImpedanceController::setFiltering(double update_frequency, double filter_params_nullspace_config, double filter_params_stiffness, - double filter_params_pose, double filter_params_wrench) - { - this->setUpdateFrequency(update_frequency); - this->setFilterValue(filter_params_nullspace_config, &this->filter_params_nullspace_config_); - this->setFilterValue(filter_params_stiffness, &this->filter_params_stiffness_); - this->setFilterValue(filter_params_pose, &this->filter_params_pose_); - this->setFilterValue(filter_params_wrench, &this->filter_params_wrench_); - } +void CartesianImpedanceController::setNullspaceConfig(const Eigen::VectorXd& q_d_nullspace_target) +{ + assert(q_d_nullspace_target.size() == this->n_joints_ && "Nullspace target needs to same size as n_joints_"); + this->q_d_nullspace_target_ << q_d_nullspace_target; +} - void CartesianImpedanceController::setMaxTorqueDelta(double d) - { - assert(d >= 0.0 && "Allowed torque change must be positive"); - this->delta_tau_max_ = d; - } +void CartesianImpedanceController::setFiltering(double update_frequency, double filter_params_nullspace_config, + double filter_params_stiffness, double filter_params_pose, + double filter_params_wrench) +{ + this->setUpdateFrequency(update_frequency); + this->setFilterValue(filter_params_nullspace_config, &this->filter_params_nullspace_config_); + this->setFilterValue(filter_params_stiffness, &this->filter_params_stiffness_); + this->setFilterValue(filter_params_pose, &this->filter_params_pose_); + this->setFilterValue(filter_params_wrench, &this->filter_params_wrench_); +} + +void CartesianImpedanceController::setMaxTorqueDelta(double d) +{ + assert(d >= 0.0 && "Allowed torque change must be positive"); + this->delta_tau_max_ = d; +} - void CartesianImpedanceController::setMaxTorqueDelta(double d, double update_frequency) - { - this->setMaxTorqueDelta(d); - this->setUpdateFrequency(update_frequency); - } +void CartesianImpedanceController::setMaxTorqueDelta(double d, double update_frequency) +{ + this->setMaxTorqueDelta(d); + this->setUpdateFrequency(update_frequency); +} - void CartesianImpedanceController::applyWrench(const Eigen::Matrix &cartesian_wrench_target) - { - this->cartesian_wrench_target_ = cartesian_wrench_target; - } +void CartesianImpedanceController::applyWrench(const Eigen::Matrix& cartesian_wrench_target) +{ + this->cartesian_wrench_target_ = cartesian_wrench_target; +} + +Eigen::VectorXd CartesianImpedanceController::calculateCommandedTorques(const Eigen::VectorXd& q, + const Eigen::VectorXd& dq, + const Eigen::Vector3d& position, + Eigen::Quaterniond orientation, + const Eigen::MatrixXd& jacobian) +{ + // Update controller to the current robot state + this->q_ = q; + this->dq_ = dq; + this->position_ << position; + this->orientation_.coeffs() << orientation.coeffs(); + this->jacobian_ << jacobian; - Eigen::VectorXd CartesianImpedanceController::calculateCommandedTorques(const Eigen::VectorXd &q, - const Eigen::VectorXd &dq, - const Eigen::Vector3d &position, - Eigen::Quaterniond orientation, - const Eigen::MatrixXd &jacobian) - { - // Update controller to the current robot state - this->q_ = q; - this->dq_ = dq; - this->position_ << position; - this->orientation_.coeffs() << orientation.coeffs(); - this->jacobian_ << jacobian; - - return this->calculateCommandedTorques(); - } + return this->calculateCommandedTorques(); +} - Eigen::VectorXd CartesianImpedanceController::calculateCommandedTorques() - { - // Perform a filtering step - updateFilteredNullspaceConfig(); - updateFilteredStiffness(); - updateFilteredPose(); - updateFilteredWrench(); - - // Compute error term - this->error_.head(3) << this->position_ - this->position_d_; - this->error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); - - // Kinematic pseuoinverse - Eigen::MatrixXd jacobian_transpose_pinv; - pseudoInverse(this->jacobian_.transpose(), &jacobian_transpose_pinv); - - Eigen::VectorXd tau_task(this->n_joints_), tau_nullspace(this->n_joints_), tau_ext(this->n_joints_); - - // Torque calculated for Cartesian impedance control with respect to a Cartesian pose reference in the end, in the frame of the EE of the robot. - tau_task << this->jacobian_.transpose() * (-this->cartesian_stiffness_ * this->error_ - this->cartesian_damping_ * (this->jacobian_ * this->dq_)); - // Torque for joint impedance control with respect to a desired configuration and projected in the null-space of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector. - tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - this->jacobian_.transpose() * jacobian_transpose_pinv) * - (this->nullspace_stiffness_ * (this->q_d_nullspace_ - this->q_) - this->nullspace_damping_ * this->dq_); - // Torque to achieve the desired external force command in the frame of the EE of the robot. - tau_ext = this->jacobian_.transpose() * this->cartesian_wrench_; - - // Torque commanded to the joints of the robot is composed by the superposition of these three joint-torque signals: - Eigen::VectorXd tau_d = tau_task + tau_nullspace + tau_ext; - saturateTorqueRate(tau_d, &this->tau_c_, this->delta_tau_max_); - return this->tau_c_; - } +Eigen::VectorXd CartesianImpedanceController::calculateCommandedTorques() +{ + // Perform a filtering step + updateFilteredNullspaceConfig(); + updateFilteredStiffness(); + updateFilteredPose(); + updateFilteredWrench(); + + // Compute error term + this->error_.head(3) << this->position_ - this->position_d_; + this->error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); + + // Kinematic pseuoinverse + Eigen::MatrixXd jacobian_transpose_pinv; + pseudoInverse(this->jacobian_.transpose(), &jacobian_transpose_pinv); + + Eigen::VectorXd tau_task(this->n_joints_), tau_nullspace(this->n_joints_), tau_ext(this->n_joints_); + + // Torque calculated for Cartesian impedance control with respect to a Cartesian pose reference in the end, in the + // frame of the EE of the robot. + tau_task << this->jacobian_.transpose() * (-this->cartesian_stiffness_ * this->error_ - + this->cartesian_damping_ * (this->jacobian_ * this->dq_)); + // Torque for joint impedance control with respect to a desired configuration and projected in the null-space of the + // robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector. + tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - this->jacobian_.transpose() * jacobian_transpose_pinv) * + (this->nullspace_stiffness_ * (this->q_d_nullspace_ - this->q_) - + this->nullspace_damping_ * this->dq_); + // Torque to achieve the desired external force command in the frame of the EE of the robot. + tau_ext = this->jacobian_.transpose() * this->cartesian_wrench_; + + // Torque commanded to the joints of the robot is composed by the superposition of these three joint-torque signals: + Eigen::VectorXd tau_d = tau_task + tau_nullspace + tau_ext; + saturateTorqueRate(tau_d, &this->tau_c_, this->delta_tau_max_); + return this->tau_c_; +} + +// Get the state of the robot.Updates when "calculateCommandedTorques" is called +void CartesianImpedanceController::getState(Eigen::VectorXd* q, Eigen::VectorXd* dq, Eigen::Vector3d* position, + Eigen::Quaterniond* orientation, Eigen::Vector3d* position_d, + Eigen::Quaterniond* orientation_d, + Eigen::Matrix* cartesian_stiffness, + double* nullspace_stiffness, Eigen::VectorXd* q_d_nullspace, + Eigen::Matrix* cartesian_damping) const +{ + *q << this->q_; + *dq << this->dq_; + *position << this->position_; + orientation->coeffs() << this->orientation_.coeffs(); + this->getState(position_d, orientation_d, cartesian_stiffness, nullspace_stiffness, q_d_nullspace, cartesian_damping); +} + +void CartesianImpedanceController::getState(Eigen::Vector3d* position_d, Eigen::Quaterniond* orientation_d, + Eigen::Matrix* cartesian_stiffness, + double* nullspace_stiffness, Eigen::VectorXd* q_d_nullspace, + Eigen::Matrix* cartesian_damping) const +{ + *position_d = this->position_d_; + orientation_d->coeffs() << this->orientation_d_.coeffs(); + *cartesian_stiffness = this->cartesian_stiffness_; + *nullspace_stiffness = this->nullspace_stiffness_; + *q_d_nullspace = this->q_d_nullspace_; + *cartesian_damping << this->cartesian_damping_; +} + +Eigen::VectorXd CartesianImpedanceController::getLastCommands() const +{ + return this->tau_c_; +} - // Get the state of the robot.Updates when "calculateCommandedTorques" is called - void CartesianImpedanceController::getState(Eigen::VectorXd *q, Eigen::VectorXd *dq, Eigen::Vector3d *position, - Eigen::Quaterniond *orientation, Eigen::Vector3d *position_d, - Eigen::Quaterniond *orientation_d, - Eigen::Matrix *cartesian_stiffness, - double *nullspace_stiffness, Eigen::VectorXd *q_d_nullspace, - Eigen::Matrix *cartesian_damping) const - { - *q << this->q_; - *dq << this->dq_; - *position << this->position_; - orientation->coeffs() << this->orientation_.coeffs(); - this->getState(position_d, orientation_d, cartesian_stiffness, nullspace_stiffness, q_d_nullspace, cartesian_damping); - } +Eigen::Matrix CartesianImpedanceController::getAppliedWrench() const +{ + return this->cartesian_wrench_; +} - void CartesianImpedanceController::getState(Eigen::Vector3d *position_d, Eigen::Quaterniond *orientation_d, - Eigen::Matrix *cartesian_stiffness, - double *nullspace_stiffness, Eigen::VectorXd *q_d_nullspace, - Eigen::Matrix *cartesian_damping) const - { - *position_d = this->position_d_; - orientation_d->coeffs() << this->orientation_d_.coeffs(); - *cartesian_stiffness = this->cartesian_stiffness_; - *nullspace_stiffness = this->nullspace_stiffness_; - *q_d_nullspace = this->q_d_nullspace_; - *cartesian_damping << this->cartesian_damping_; - } +Eigen::Matrix CartesianImpedanceController::getPoseError() const +{ + return this->error_; +} - Eigen::VectorXd CartesianImpedanceController::getLastCommands() const - { - return this->tau_c_; - } +double CartesianImpedanceController::dampingRule(double stiffness) const +{ + return 2 * sqrt(stiffness); +} - Eigen::Matrix CartesianImpedanceController::getAppliedWrench() const - { - return this->cartesian_wrench_; - } +void CartesianImpedanceController::setUpdateFrequency(double freq) +{ + assert(freq >= 0.0 && "Update frequency needs to be greater or equal to zero"); + this->update_frequency_ = std::max(freq, 0.0); +} + +void CartesianImpedanceController::setFilterValue(double val, double* saved_val) +{ + assert(val > 0 && val <= 1.0 && "Filter params need to be between 0 and 1."); + *saved_val = saturateValue(val, 0.0000001, 1.0); +} - Eigen::Matrix CartesianImpedanceController::getPoseError() const +void CartesianImpedanceController::updateFilteredNullspaceConfig() +{ + if (this->filter_params_nullspace_config_ == 1.0) { - return this->error_; + this->q_d_nullspace_ = this->q_d_nullspace_target_; } - - double CartesianImpedanceController::dampingRule(double stiffness) const + else { - return 2 * sqrt(stiffness); + const double step = filterStep(this->update_frequency_, this->filter_params_nullspace_config_); + this->q_d_nullspace_ = filteredUpdate(this->q_d_nullspace_target_, this->q_d_nullspace_, step); } +} - void CartesianImpedanceController::setUpdateFrequency(double freq) +void CartesianImpedanceController::updateFilteredStiffness() +{ + if (this->filter_params_stiffness_ == 1.0) { - assert(freq >= 0.0 && "Update frequency needs to be greater or equal to zero"); - this->update_frequency_ = std::max(freq, 0.0); + this->cartesian_stiffness_ = this->cartesian_stiffness_target_; + this->cartesian_damping_ = this->cartesian_damping_target_; + this->nullspace_stiffness_ = this->nullspace_stiffness_target_; + this->q_d_nullspace_ = this->q_d_nullspace_target_; + this->nullspace_damping_ = this->nullspace_damping_target_; } - - void CartesianImpedanceController::setFilterValue(double val, double *saved_val) + else { - assert(val > 0 && val <= 1.0 && "Filter params need to be between 0 and 1."); - *saved_val = saturateValue(val, 0.0000001, 1.0); + const double step = filterStep(this->update_frequency_, this->filter_params_stiffness_); + + this->cartesian_stiffness_ = filteredUpdate(this->cartesian_stiffness_target_, this->cartesian_stiffness_, step); + this->cartesian_damping_ = filteredUpdate(this->cartesian_damping_target_, this->cartesian_damping_, step); + this->nullspace_stiffness_ = filteredUpdate(this->nullspace_stiffness_target_, this->nullspace_stiffness_, step); + this->nullspace_damping_ = filteredUpdate(this->nullspace_damping_target_, this->nullspace_damping_, step); } +} - void CartesianImpedanceController::updateFilteredNullspaceConfig() +void CartesianImpedanceController::updateFilteredPose() +{ + if (this->filter_params_pose_ == 1.0) { - if (this->filter_params_nullspace_config_ == 1.0) - { - this->q_d_nullspace_ = this->q_d_nullspace_target_; - } - else - { - const double step = filterStep(this->update_frequency_, this->filter_params_nullspace_config_); - this->q_d_nullspace_ = filteredUpdate(this->q_d_nullspace_target_, this->q_d_nullspace_, step); - } + position_d_ << position_d_target_; + orientation_d_.coeffs() << orientation_d_target_.coeffs(); } - - void CartesianImpedanceController::updateFilteredStiffness() + else { - if (this->filter_params_stiffness_ == 1.0) - { - this->cartesian_stiffness_ = this->cartesian_stiffness_target_; - this->cartesian_damping_ = this->cartesian_damping_target_; - this->nullspace_stiffness_ = this->nullspace_stiffness_target_; - this->q_d_nullspace_ = this->q_d_nullspace_target_; - this->nullspace_damping_ = this->nullspace_damping_target_; - } - else - { - const double step = filterStep(this->update_frequency_, this->filter_params_stiffness_); + const double step = filterStep(this->update_frequency_, this->filter_params_pose_); - this->cartesian_stiffness_ = filteredUpdate(this->cartesian_stiffness_target_, this->cartesian_stiffness_, step); - this->cartesian_damping_ = filteredUpdate(this->cartesian_damping_target_, this->cartesian_damping_, step); - this->nullspace_stiffness_ = filteredUpdate(this->nullspace_stiffness_target_, this->nullspace_stiffness_, step); - this->nullspace_damping_ = filteredUpdate(this->nullspace_damping_target_, this->nullspace_damping_, step); - } + this->position_d_ = filteredUpdate(this->position_d_target_, this->position_d_, step); + this->orientation_d_ = this->orientation_d_.slerp(step, this->orientation_d_target_); } +} - void CartesianImpedanceController::updateFilteredPose() +void CartesianImpedanceController::updateFilteredWrench() +{ + if (this->filter_params_wrench_ == 1.0) { - if (this->filter_params_pose_ == 1.0) - { - position_d_ << position_d_target_; - orientation_d_.coeffs() << orientation_d_target_.coeffs(); - } - else - { - const double step = filterStep(this->update_frequency_, this->filter_params_pose_); - - this->position_d_ = filteredUpdate(this->position_d_target_, this->position_d_, step); - this->orientation_d_ = this->orientation_d_.slerp(step, this->orientation_d_target_); - } + this->cartesian_wrench_ = this->cartesian_wrench_target_; } - - void CartesianImpedanceController::updateFilteredWrench() + else { - if (this->filter_params_wrench_ == 1.0) - { - this->cartesian_wrench_ = this->cartesian_wrench_target_; - } - else - { - const double step = filterStep(this->update_frequency_, this->filter_params_wrench_); - this->cartesian_wrench_ = filteredUpdate(this->cartesian_wrench_target_, this->cartesian_wrench_, step); - } + const double step = filterStep(this->update_frequency_, this->filter_params_wrench_); + this->cartesian_wrench_ = filteredUpdate(this->cartesian_wrench_target_, this->cartesian_wrench_, step); } +} -} // namespace cartesian_impedance_controller +} // namespace cartesian_impedance_controller diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 816c1e4..6bdb086 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -157,8 +157,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy rt_pub_torques_ = std::make_shared>(torques_pub); sub_controller_config_ = get_node()->create_subscription( - "set_config", - rclcpp::SystemDefaultsQoS(), + "set_config", rclcpp::SystemDefaultsQoS(), std::bind(&CartesianImpedanceControllerRos::controllerConfigCb, this, std::placeholders::_1)); sub_cart_stiffness_ = node->create_subscription( @@ -534,8 +533,9 @@ void CartesianImpedanceControllerRos::controllerConfigCb( setStiffness(msg->cartesian_stiffness.force.x, msg->cartesian_stiffness.force.y, msg->cartesian_stiffness.force.z, msg->cartesian_stiffness.torque.x, msg->cartesian_stiffness.torque.y, msg->cartesian_stiffness.torque.z, msg->nullspace_stiffness, false); - setDampingFactors(msg->cartesian_damping_factors.force.x, msg->cartesian_damping_factors.force.y, msg->cartesian_damping_factors.force.z, - msg->cartesian_damping_factors.torque.x, msg->cartesian_damping_factors.torque.y, msg->cartesian_damping_factors.torque.z, + setDampingFactors(msg->cartesian_damping_factors.force.x, msg->cartesian_damping_factors.force.y, + msg->cartesian_damping_factors.force.z, msg->cartesian_damping_factors.torque.x, + msg->cartesian_damping_factors.torque.y, msg->cartesian_damping_factors.torque.z, msg->nullspace_damping_factor); if (msg->q_d_nullspace.size() == dof_) @@ -549,9 +549,7 @@ void CartesianImpedanceControllerRos::controllerConfigCb( } else { - RCLCPP_WARN( - get_node()->get_logger(), - "Nullspace configuration has wrong dimension: got %zu, expected %zu", + RCLCPP_WARN(get_node()->get_logger(), "Nullspace configuration has wrong dimension: got %zu, expected %zu", msg->q_d_nullspace.size(), dof_); } } @@ -573,8 +571,8 @@ void CartesianImpedanceControllerRos::publishMsgsAndTf() if (params_.verbosity.verbose_print) { RCLCPP_INFO(get_node()->get_logger(), "Cartesian Position: [%f, %f, %f]", position_(0), position_(1), position_(2)); - RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", - error(0), error(1), error(2), error(3), error(4), error(5)); + RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", error(0), error(1), error(2), + error(3), error(4), error(5)); RCLCPP_INFO(get_node()->get_logger(), "Cartesian Stiffness (diag): [%f, %f, %f, %f, %f, %f]", cartesian_stiffness_.diagonal()(0), cartesian_stiffness_.diagonal()(1), @@ -683,12 +681,10 @@ void CartesianImpedanceControllerRos::publishMsgsAndTf() state_msg.pose_error.position.x = error(0); state_msg.pose_error.position.y = error(1); state_msg.pose_error.position.z = error(2); - Eigen::Quaterniond q_err = - Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); - state_msg.pose_error.orientation = - tf2::toMsg(tf2::Quaternion(q_err.x(), q_err.y(), q_err.z(), q_err.w())); + Eigen::Quaterniond q_err = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); + state_msg.pose_error.orientation = tf2::toMsg(tf2::Quaternion(q_err.x(), q_err.y(), q_err.z(), q_err.w())); state_msg.cartesian_stiffness.force.x = cartesian_stiffness_.diagonal()(0); state_msg.cartesian_stiffness.force.y = cartesian_stiffness_.diagonal()(1); @@ -716,8 +712,7 @@ void CartesianImpedanceControllerRos::publishMsgsAndTf() state_msg.nullspace_damping = nullspace_damping_; Eigen::Matrix dx = jacobian_ * dq_; - state_msg.cartesian_velocity = - std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + state_msg.cartesian_velocity = std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); rt_pub_state_->unlockAndPublish(); } From d4ca930acd53604f2cb70c46206da7c811698f57 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Tue, 18 Feb 2025 12:54:05 +0000 Subject: [PATCH 16/25] Update include statements for clarity --- .../cartesian_impedance_controller_ros.hpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index 670233e..837e704 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -16,9 +16,6 @@ #include #include -#include -#include - #include #include #include @@ -26,7 +23,7 @@ #include #include #include -#include +#include #include #include @@ -36,14 +33,14 @@ #include -#include "cartesian_impedance_controller/cartesian_impedance_controller.hpp" -#include "cartesian_impedance_controller/rbdyn_wrapper.h" +#include +#include #include -#include "cartesian_impedance_controller/msg/controller_config.hpp" -#include "cartesian_impedance_controller/msg/controller_state.hpp" +#include +#include -#include -#include +#include +#include namespace cartesian_impedance_controller { From 77feb2262ce02d9f7a5755e047145c6d3b5e15e8 Mon Sep 17 00:00:00 2001 From: Cenk Cetin <105711013+mrceki@users.noreply.github.com> Date: Tue, 8 Apr 2025 15:59:49 +0100 Subject: [PATCH 17/25] Create README.md --- README.md | 261 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 261 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..dda040f --- /dev/null +++ b/README.md @@ -0,0 +1,261 @@ +# Cartesian Impedance Controller +[![CI](https://github.com/mrceki/Cartesian-Impedance-Controller/actions/workflows/build_code.yml/badge.svg?branch=ros2)](https://github.com/mrceki/Cartesian-Impedance-Controller/actions/workflows/build_code.yml) +[![DOI](https://joss.theoj.org/papers/10.21105/joss.05194/status.svg)](https://doi.org/10.21105/joss.05194) +[![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) + +## CAUTION: Please be aware that this project was deployed on the real robot, but is still in progress! + +## Description +### This project is a migration of ROS 1 implementation. You can find the original repo [here.](https://github.com/matthias-mayr/Cartesian-Impedance-Controller) + + +This project is an implementation of Cartesian impedance control for robotic manipulators. It is a type of control strategy that sets a dynamic relationship between contact forces and the position of a robot arm, making it suitable for collaborative robots. It is particularly useful when the interesting dimensions in the workspace are in the Cartesian space. + +This controller was deployed on `Franka Emika Research 3` both in reality and simulation. + +ROS 1 version of the controller is developed using the seven degree-of-freedom (DoF) robot arm `LBR iiwa` by `KUKA AG` and has also been tested with the `Franka Emika Robot (Panda)` both in reality and simulation. + +The implementation consists of a +1. base library that has few dependencies and can e.g. be directly integrated into software such as the DART simulator or any simulator which has `ros2_control` interface +2. ROS 2 control integration on top of it. + +### Short Pitch at ROSCon: +[![IMAGE ALT TEXT](http://img.youtube.com/vi/Q4aPm4O_9fY/0.jpg)](http://www.youtube.com/watch?v=Q4aPm4O_9fY "Cartesian Impedance Controller ROSCon 2022 Lightning Talk") + +http://www.youtube.com/watch?v=Q4aPm4O_9fY + +## Features + +- Configurable stiffness values along all Cartesian dimensions at runtime +- Configurable damping factors along all Cartesian dimensions at runtime +- Change reference pose at runtime +- Apply Cartesian forces and torques at runtime +- Optional filtering of stiffnesses, pose and wrenches for smoother operation +- Handling of joint trajectories with nullspace configurations, e.g. from MoveIt +- Jerk limitation +- Separate base library that can be integrated in non-ROS environments +- Interface to ROS messages and dynamic_reconfigure for easy runtime configuration + +![](./res/flowchart.png) + +## Torques + +The torque signal commanded to the joints of the robot is composed by the superposition of three joint-torque signals: +- The torque calculated for Cartesian impedance control with respect to a Cartesian pose reference in the frame of the EE of the robot (`tau_task`). +- The torque calculated for joint impedance control with respect to a desired configuration and projected in the nullspace of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector (`tau_ns`). +- The torque necessary to achieve the desired external force command (`cartesian_wrench`), in the frame of the EE of the robot (`tau_ext`). + +## Limitations + +- Joint friction is not accounted for +- Stiffness and damping values along the Cartesian dimensions are uncoupled +- No built-in gravity compensation for tools or workpieces (can be achieved by commanding a wrench) + +## Prerequisites +### Required +- [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) + +### ROS Controller +We use `RBDyn` to calculate forward kinematics and the Jacobian. + +- [ROS](https://www.ros.org/) +- [RBDyn](https://github.com/jrl-umi3218/RBDyn) +- [mc_rbdyn_urdf](https://github.com/jrl-umi3218/mc_rbdyn_urdf) +- [SpaceVecAlg](https://github.com/jrl-umi3218/SpaceVecAlg) + +The installation steps for the installation of the non-ROS dependencies are automated in `scripts/install_dependencies.sh`. + +## Controller Usage in ROS +Assuming that there is an initialized colcon workspace you can clone this repository, install the dependencies and compile the controller. + +Here are the steps: + +```bash +cd ros2_ws +git clone https://github.com/mrceki/Cartesian-Impedance-Controller src/Cartesian-Impedance-Controller +src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh +touch depends/COLCON_IGNORE +rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y +colcon build +source install/setup.bash +``` + +This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceControllerRos` in your `ros2_control` configuration. + +### Configuration file +When using the controller it is a good practice to describe the parameters in a `YAML` file and load it. Usually this is already done by your robot setup - e.g. for iiwa_ros that is here. +Here is a template of what needs to be in that YAML file that can be adapted: +```YAML +cartesian_impedance_controller: + type: cartesian_impedance_controller/CartesianImpedanceControllerRos + joints: + - fr3_joint1 + - fr3_joint2 + - fr3_joint3 + - fr3_joint4 + - fr3_joint5 + - fr3_joint6 + - fr3_joint7 + end_effector: fr3_hand_tcp + update_frequency: 500 + handle_trajectories: true + robot_description: /robot_description + wrench_ee_frame: fr3_hand_tcp + delta_tau_max: 1.0 + + damping: + translation: + x: 1.0 + y: 1.0 + z: 1.0 + rotation: + x: 1.0 + y: 1.0 + z: 1.0 + nullspace_damping: 1.0 + update_damping_factors: false + + stiffness: + translation: + x: 200.0 + y: 200.0 + z: 200.0 + rotation: + x: 20.0 + y: 20.0 + z: 20.0 + nullspace_stiffness: 0.0 + update_stiffness: false + + wrench: + apply_wrench: false + force_x: 0.0 + force_y: 0.0 + force_z: 0.0 + torque_x: 0.0 + torque_y: 0.0 + torque_z: 0.0 + + filtering: + nullspace_config: 0.1 + pose: 0.1 + stiffness: 0.1 + wrench: 0.1 + + verbosity: + verbose_print: false + state_msgs: true + tf_frames: false +``` + +### Changing parameters with Dynamic Reconfigure +The controller can be configured with dynamic_reconfigure both with command line tools as well as the graphical user interface rqt_reconfigure. To start the latter you can run: +```bash +ros2 run rqt_reconfigure rqt_reconfigure +``` +For applying wrench, the `apply` checkbox needs to be ticked for the values to be used. Damping and stiffness changes are only updated when the `update` checkbox is ticked, allowing to configure changes before applying them. Note that the end-effector reference pose can not be set since it usually should follow a smooth trajectory. + +### Changing parameters with ROS messages +In addition to the configuration with `dynamic_reconfigure`, the controller configuration can always be adapted by sending ROS messages. Outside prototyping this is the main way to parameterize it. + + +#### Cartesian Stiffness + +In order to set only the Cartesian stiffnesses, once can send a `geometry_msgs/msgs/WrenchStamped` to `set_cartesian_stiffness`: + +```bash +ros2 topic pub --once /set_cartesian_stiffness geometry_msgs/msg/WrenchStamped "header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' +wrench: + force: + x: 300.0 + y: 300.0 + z: 300.0 + torque: + x: 30.0 + y: 30.0 + z: 30.0" +``` + +#### Cartesian Damping factors + +The damping factors can be configured with a `geometry_msgs/msgs/WrenchStamped` msg similar to the stiffnesses to the topic `/set_damping_factors`. Damping factors are in the interval [0.01,2]. These damping factors are additionally applied to the damping rule which is `2*sqrt(stiffness)`. + +#### Cartesian Wrenches + +A Cartesian wrench can be commanded by sending a `geometry_msgs/msg/WrenchStamped` to the topic `/set_cartesian_wrench`. +Internally the wrenches are applied in the root frame. Therefore wrench messages are transformed into the root frame using `tf`.
+**Note:** An empty field `frame_id` is interpreted as end-effector frame since this is the most applicable one when working with a manipulator.
+**Note:** The wrenches are transformed into the root frame when they arrive, but not after that. E.g. end-effector wrenches might need to get updated. + +```bash +ros2 topic pub --once /set_cartesian_wrench geometry_msgs/msg/WrenchStamped "header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' +wrench: + force: + x: 0.0 + y: 0.0 + z: 5.0 + torque: + x: 0.0 + y: 0.0 + z: 0.0" +``` + +### Trajectories and MoveIt + +The controller can also execute trajectories. An action server is spun up at `/follow_joint_trajectory` and a fire-and-forget topic for the message type `trajectory_msgs/msg/JointTrajectory` is at `/joint_trajectory`. + +In order to use it with `MoveIt` its controller configuration ([example in iiwa_ros](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_moveit/config/EffortJointInterface_controllers.yaml)) needs to look somewhat like this: +```yaml +controller_list: + - name: cartesian_impedance_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - fr3_joint1 + - fr3_joint2 + - fr3_joint3 + - fr3_joint4 + - fr3_joint5 + - fr3_joint6 + - fr3_joint7 +``` + +**Note:** A nullspace stiffness needs to be specified so that the arm also follows the joint configuration and not just the end-effector pose. + +## Safety (ROS 1 Version) +We have used the controller with Cartesian translational stiffnesses of up to 1000 N/m and experienced it as very stable. It is also stable in singularities. + +One additional measure can be to limit the maximum joint torques that can be applied by the robot arm in the URDF. On our KUKA iiwas we limit the maximum torque of each joint to 20 Nm, which allows a human operator to easily interfere at any time just by grabbing the arm and moving it. + +When using `iiwa_ros`, these limits can be applied [here](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_description/urdf/iiwa7.xacro#L53-L59). For the Panda they are applied [here](https://github.com/frankaemika/franka_ros/blob/develop/franka_description/robots/panda/joint_limits.yaml#L6). Both arms automatically apply gravity compensation, the limits are only used for the task-level torques on top of that. + + +## Citing this Work +A brief paper about the features and the control theory is accepted at [JOSS](https://joss.theoj.org/papers/10.21105/joss.05194). +If you are using it or interacting with it, we would appreciate a citation: +``` +Mayr et al., (2024). A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators. Journal of Open Source Software, 9(93), 5194, https://doi.org/10.21105/joss.05194 +``` + +```bibtex +@article{mayr2024cartesian, + doi = {10.21105/joss.05194}, + url = {https://doi.org/10.21105/joss.05194}, + year = {2024}, + publisher = {The Open Journal}, + volume = {9}, + number = {93}, + pages = {5194}, + author = {Matthias Mayr and Julian M. Salt-Ducaju}, + title = {A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators}, journal = {Journal of Open Source Software} +} +``` From 642ac39808649c311573475e576d52c928c1ea34 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 11 Apr 2025 11:38:53 +0000 Subject: [PATCH 18/25] Reset wrench if apply_wrench parameter false --- src/cartesian_impedance_controller_ros.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 6bdb086..8bd802d 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -375,6 +375,10 @@ void CartesianImpedanceControllerRos::applyRuntimeParameters() else applyWrench(F); } + else + { + applyWrench(Eigen::Matrix::Zero()); + } } bool CartesianImpedanceControllerRos::initRBDyn() From dc6782b7c8008ae06c225e79b667370ff812fdb1 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 11 Apr 2025 13:06:40 +0000 Subject: [PATCH 19/25] Update controller type references to CartesianImpedanceController and Fix cmakelist dependencies --- CMakeLists.txt | 8 ++++---- README.md | 4 ++-- cartesian_impedance_controller_plugin.xml | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 549aa2f..311bfa6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,15 +31,15 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS trajectory_msgs generate_parameter_library urdf + RBDyn + mc_rbdyn_urdf + Eigen3 + pluginlib ) find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(RBDyn REQUIRED) -find_package(mc_rbdyn_urdf REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(pluginlib REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/README.md b/README.md index dda040f..26e6ca1 100644 --- a/README.md +++ b/README.md @@ -80,14 +80,14 @@ colcon build source install/setup.bash ``` -This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceControllerRos` in your `ros2_control` configuration. +This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController` in your `ros2_control` configuration. ### Configuration file When using the controller it is a good practice to describe the parameters in a `YAML` file and load it. Usually this is already done by your robot setup - e.g. for iiwa_ros that is here. Here is a template of what needs to be in that YAML file that can be adapted: ```YAML cartesian_impedance_controller: - type: cartesian_impedance_controller/CartesianImpedanceControllerRos + type: cartesian_impedance_controller/CartesianImpedanceController joints: - fr3_joint1 - fr3_joint2 diff --git a/cartesian_impedance_controller_plugin.xml b/cartesian_impedance_controller_plugin.xml index f3e0f1f..8cb96ab 100644 --- a/cartesian_impedance_controller_plugin.xml +++ b/cartesian_impedance_controller_plugin.xml @@ -1,5 +1,5 @@ - Cartesian impedance controller implementation From 08b4a2bc68f186632aca1982fc4b1e39f28afab9 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 11 Apr 2025 14:01:30 +0000 Subject: [PATCH 20/25] Add Dockerfile and update installation instructions --- Dockerfile | 97 +++++++++++++++++++++++++++++++++ README.md | 50 +++++++++++++++-- scripts/install_dependencies.sh | 2 +- 3 files changed, 143 insertions(+), 6 deletions(-) create mode 100644 Dockerfile diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..eb0b21f --- /dev/null +++ b/Dockerfile @@ -0,0 +1,97 @@ +# If you would like to use lightweight image, you can use the following base image: +# FROM ros:humble-ros-base + +FROM osrf/ros:humble-desktop-full + +ENV DEBIAN_FRONTEND=noninteractive \ + LANG=C.UTF-8 \ + LC_ALL=C.UTF-8 \ + ROS_DISTRO=humble + +ARG BUILD_FRANKA_ROS=false + +ARG USER_UID=1001 +ARG USER_GID=1001 +ARG USERNAME=user + +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + bash-completion \ + curl \ + gdb \ + git \ + nano \ + openssh-client \ + python3-colcon-argcomplete \ + python3-colcon-common-extensions \ + sudo \ + libeigen3-dev \ + libboost-all-dev \ + vim && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +RUN groupadd --gid $USER_GID $USERNAME && \ + useradd --uid $USER_UID --gid $USER_GID -m $USERNAME && \ + echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers && \ + echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> /home/$USERNAME/.bashrc && \ + echo "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" >> /home/$USERNAME/.bashrc + +USER $USERNAME + +WORKDIR /ros2_ws +COPY . /ros2_ws/src/Cartesian-Impedance-Controller + +RUN sudo chown -R $USERNAME:$USERNAME /ros2_ws + +RUN sudo apt update + +RUN rosdep update && \ + rosdep install --from-paths src/Cartesian-Impedance-Controller --ignore-src --rosdistro=$ROS_DISTRO -y + +RUN if [ "$BUILD_FRANKA_ROS" = "true" ]; then \ + echo "Installing franka_ros2 dependencies and cloning repository..."; \ + sudo apt-get install -y --no-install-recommends \ + ros-humble-franka-description \ + ros-humble-ros-gz \ + ros-humble-sdformat-urdf \ + ros-humble-joint-state-publisher-gui \ + ros-humble-ros2controlcli \ + ros-humble-controller-interface \ + ros-humble-hardware-interface-testing \ + ros-humble-ament-cmake-clang-format \ + ros-humble-ament-cmake-clang-tidy \ + ros-humble-controller-manager \ + ros-humble-ros2-control-test-assets \ + libignition-gazebo6-dev \ + libignition-plugin-dev \ + ros-humble-hardware-interface \ + ros-humble-control-msgs \ + ros-humble-backward-ros \ + ros-humble-generate-parameter-library \ + ros-humble-realtime-tools \ + ros-humble-joint-state-publisher \ + ros-humble-joint-state-broadcaster \ + ros-humble-moveit-ros-move-group \ + ros-humble-moveit-kinematics \ + ros-humble-moveit-planners-ompl \ + ros-humble-moveit-ros-visualization \ + ros-humble-joint-trajectory-controller \ + ros-humble-moveit-simple-controller-manager \ + ros-humble-rviz2 \ + ros-humble-xacro && \ + sudo apt-get clean && \ + git clone https://github.com/frankaemika/franka_ros2.git /ros2_ws/src/franka_ros2 && \ + vcs import /ros2_ws/src < /ros2_ws/src/franka_ros2/franka.repos --recursive --skip-existing && \ + rosdep install --from-paths src/franka_ros2 --ignore-src --rosdistro=$ROS_DISTRO -y; \ + else \ + echo "Skipping franka_ros2 integration - building only the Cartesian Impedance Controller."; \ + fi + +RUN /bin/bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh + +RUN echo 'export CMAKE_PREFIX_PATH=/usr/local:$CMAKE_PREFIX_PATH' >> /home/$USERNAME/.bashrc + +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release" + +CMD ["/bin/bash"] diff --git a/README.md b/README.md index 26e6ca1..0ae600b 100644 --- a/README.md +++ b/README.md @@ -69,17 +69,57 @@ The installation steps for the installation of the non-ROS dependencies are auto Assuming that there is an initialized colcon workspace you can clone this repository, install the dependencies and compile the controller. Here are the steps: - + +### Local Machine Installation ```bash -cd ros2_ws +mkdir -p ~/ros2_ws/src +cd ~/ros2_ws git clone https://github.com/mrceki/Cartesian-Impedance-Controller src/Cartesian-Impedance-Controller -src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh -touch depends/COLCON_IGNORE +bash src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y colcon build source install/setup.bash ``` - +### Docker Installation +```markdown +To build and run the Docker container for the Cartesian Impedance Controller, follow these steps: + +#### Build the Docker Image +1. Navigate to the directory containing the `Dockerfile`. +2. Build the Docker image: + ```bash + docker build -t cartesian_impedance_controller:latest . + ``` + +#### Run the Docker Container +1. Start a container from the built image: + ```bash + docker run -it --rm \ + --name cartesian_impedance_controller \ + --net=host \ + -e DISPLAY=$DISPLAY \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + cartesian_impedance_controller:latest + ``` + + - `--net=host`: Allows the container to use the host network. + - `-e DISPLAY=$DISPLAY` and `-v /tmp/.X11-unix:/tmp/.X11-unix`: Enable GUI applications (e.g., `rviz2`) to run inside the container. + +#### Optional: Enable Franka ROS 2 Integration +If you want to include Franka ROS 2 installation inside docker, build the docker image with the `BUILD_FRANKA_ROS` argument set to `true`: +```bash +docker build --build-arg BUILD_FRANKA_ROS=true -t cartesian_impedance_controller:franka . +``` + +### Access the Workspace +Once inside the container, you can access the ROS 2 workspace at `/ros2_ws`. To build the workspace or source it: +```bash +source /opt/ros/humble/setup.bash +cd /ros2_ws +colcon build +source install/setup.bash +``` + This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController` in your `ros2_control` configuration. ### Configuration file diff --git a/scripts/install_dependencies.sh b/scripts/install_dependencies.sh index ce25697..154618a 100755 --- a/scripts/install_dependencies.sh +++ b/scripts/install_dependencies.sh @@ -2,7 +2,7 @@ sudo apt install python3-rosdep || sudo apt install python-rosdep if [ ! -d "src" ] then - print "This script should be run in a catkin workspace. Exiting." + echo "This script should be run in a colcon workspace. Exiting." exit -1 fi From 4b9a9bd2fd31971b3706922707dc4e989da931ea Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 11 Apr 2025 14:04:46 +0000 Subject: [PATCH 21/25] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 0ae600b..4cd69f5 100644 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ We use `RBDyn` to calculate forward kinematics and the Jacobian. The installation steps for the installation of the non-ROS dependencies are automated in `scripts/install_dependencies.sh`. -## Controller Usage in ROS +## Controller Usage in ROS 2 Assuming that there is an initialized colcon workspace you can clone this repository, install the dependencies and compile the controller. Here are the steps: @@ -81,7 +81,7 @@ colcon build source install/setup.bash ``` ### Docker Installation -```markdown + To build and run the Docker container for the Cartesian Impedance Controller, follow these steps: #### Build the Docker Image From 03ff7cc7b3fe37f500d426672c7bb80711742ba7 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 11 Apr 2025 15:44:17 +0000 Subject: [PATCH 22/25] Fix robot_description handling and add to parameter config --- README.md | 2 +- .../cartesian_impedance_controller_ros.hpp | 1 + ...esian_impedance_controller_parameters.yaml | 4 ++ src/cartesian_impedance_controller_ros.cpp | 38 +++++++++++++------ 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 4cd69f5..58dc0b5 100644 --- a/README.md +++ b/README.md @@ -139,7 +139,7 @@ cartesian_impedance_controller: end_effector: fr3_hand_tcp update_frequency: 500 handle_trajectories: true - robot_description: /robot_description + robot_description: "robot_description" wrench_ee_frame: fr3_hand_tcp delta_tau_max: 1.0 diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index 837e704..550406d 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -84,6 +84,7 @@ class CartesianImpedanceControllerRos : public controller_interface::ControllerI std::string end_effector_; std::string wrench_ee_frame_; std::string root_frame_; + std::string robot_description_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; rclcpp::Time tf_last_time_; diff --git a/src/cartesian_impedance_controller_parameters.yaml b/src/cartesian_impedance_controller_parameters.yaml index 4bf8b8b..686ca74 100644 --- a/src/cartesian_impedance_controller_parameters.yaml +++ b/src/cartesian_impedance_controller_parameters.yaml @@ -22,6 +22,10 @@ cartesian_impedance_controller: default_value: "" description: "Name of the end effector link." + robot_description: + type: string + default_value: "robot_description" + description: "Name of the robot description parameter." damping: translation: diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 8bd802d..1b5d01f 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -108,7 +108,7 @@ CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecy wrench_ee_frame_ = params_.wrench_ee_frame; delta_tau_max_ = params_.delta_tau_max; update_frequency_ = params_.update_frequency; - + robot_description_ = params_.robot_description; tf_buffer_ = std::make_shared(node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); tf_last_time_ = get_node()->now(); @@ -385,27 +385,41 @@ bool CartesianImpedanceControllerRos::initRBDyn() { auto node = get_node(); auto logger = node->get_logger(); + + auto param_client = std::make_shared(node, "/robot_state_publisher"); + if (!param_client->wait_for_service(std::chrono::seconds(5))) { + RCLCPP_ERROR(logger, "Parameter service not available on /robot_state_publisher"); + return false; + } std::string urdf_string; - if (!node->get_parameter("robot_description", urdf_string)) - { - RCLCPP_ERROR(logger, "No robot URDF found in 'robot_description' parameter!"); + try { + auto response = param_client->get_parameters({robot_description_}).get(); + if (response.empty() || response[0].get_type() != rclcpp::PARAMETER_STRING) { + RCLCPP_ERROR(logger, "Failed to retrieve 'robot_description' parameter"); + return false; + } + urdf_string = response[0].as_string(); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger, "Exception while retrieving robot_description: %s", e.what()); return false; } - try - { - rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); + if (urdf_string.empty()) { + RCLCPP_ERROR(logger, "Retrieved empty 'robot_description'"); + return false; } - catch (const std::runtime_error& e) - { + + // Initialize RBDyn with the URDF + try { + rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); + root_frame_ = rbdyn_wrapper_.root_link(); + RCLCPP_INFO(logger, "Root frame is '%s'", root_frame_.c_str()); + } catch (const std::exception& e) { RCLCPP_ERROR(logger, "Error initializing RBDyn: %s", e.what()); return false; } - root_frame_ = rbdyn_wrapper_.root_link(); - RCLCPP_INFO(logger, "Root frame is '%s'.", root_frame_.c_str()); - if (rbdyn_wrapper_.n_joints() < static_cast(dof_)) { RCLCPP_ERROR(logger, "URDF has fewer joints (%zu) than the number to be controlled (%zu)", From 592c3a9a639fc2b2c8ca2212e6671739c9270143 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Fri, 11 Apr 2025 15:46:34 +0000 Subject: [PATCH 23/25] Formating --- .../cartesian_impedance_controller_ros.hpp | 210 +-- src/cartesian_impedance_controller_ros.cpp | 1495 +++++++++-------- 2 files changed, 855 insertions(+), 850 deletions(-) diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp index 550406d..9328b4c 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -45,108 +45,108 @@ namespace cartesian_impedance_controller { -class ParamListener; - -class CartesianImpedanceControllerRos : public controller_interface::ControllerInterface, - public CartesianImpedanceController -{ -public: - CartesianImpedanceControllerRos(); - ~CartesianImpedanceControllerRos() override; - - controller_interface::CallbackReturn on_init() override; - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; - controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override; - controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - -private: - const Eigen::VectorXi perm_indices_ = (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); - const Eigen::PermutationMatrix jacobian_perm_ = - Eigen::PermutationMatrix(perm_indices_); - - std::vector command_joint_names_; - size_t dof_{ 0 }; - std::shared_ptr> rt_trajectory_; - - std::vector joint_command_handles_; - std::vector joint_position_state_; - std::vector joint_velocity_state_; - std::vector joint_effort_state_; - - std::shared_ptr parameter_handler_; - Params params_; - - std::string end_effector_; - std::string wrench_ee_frame_; - std::string root_frame_; - std::string robot_description_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - rclcpp::Time tf_last_time_; - std::shared_ptr tf_br_; - - rbdyn_wrapper rbdyn_wrapper_; - - std::shared_ptr> - rt_pub_state_; - std::shared_ptr> rt_pub_torques_; - - rclcpp::Subscription::SharedPtr sub_cart_stiffness_; - rclcpp::Subscription::SharedPtr sub_cart_wrench_; - rclcpp::Subscription::SharedPtr sub_damping_factors_; - rclcpp::Subscription::SharedPtr sub_reference_pose_; - rclcpp::Subscription::SharedPtr trajectory_sub_; - rclcpp::Subscription::SharedPtr sub_controller_config_; - - rclcpp_action::Server::SharedPtr traj_as_; - bool traj_running_ = false; - bool traj_as_active_ = false; - std::shared_ptr> traj_as_goal_; - - trajectory_msgs::msg::JointTrajectory trajectory_; - size_t traj_index_ = 0; - rclcpp::Time traj_start_time_; - rclcpp::Duration traj_duration_; - Eigen::Vector3d position_d_target_; - Eigen::Quaterniond orientation_d_target_; - - Eigen::VectorXd tau_m_; - Eigen::VectorXd damping_factors_; - double nullspace_stiffness_target_; - - void read_state_from_hardware(); - void write_command_to_hardware(); - void write_zero_commands_to_hardware(); - bool getFk(const Eigen::VectorXd& q, Eigen::Vector3d* position, Eigen::Quaterniond* orientation); - bool getJacobian(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, Eigen::MatrixXd* jacobian); - - void controllerConfigCb(const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg); - void cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg); - void cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); - void referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); - void trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg); - rclcpp_action::GoalResponse trajGoalCb(const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); - rclcpp_action::CancelResponse trajCancelCb( - const std::shared_ptr> goal_handle); - void trajAcceptCb( - std::shared_ptr> goal_handle); - void trajStart(const trajectory_msgs::msg::JointTrajectory& trajectory); - void trajUpdate(); - - bool transformWrench(Eigen::Matrix* wrench, const std::string& from_frame, - const std::string& to_frame) const; - void publishMsgsAndTf(); - void applyRuntimeParameters(); - bool initRBDyn(); -}; - -} // namespace cartesian_impedance_controller - -#endif // CARTESIAN_IMPEDANCE_CONTROLLER__CARTESIAN_IMPEDANCE_CONTROLLER_ROS_HPP_ \ No newline at end of file + class ParamListener; + + class CartesianImpedanceControllerRos : public controller_interface::ControllerInterface, + public CartesianImpedanceController + { + public: + CartesianImpedanceControllerRos(); + ~CartesianImpedanceControllerRos() override; + + controller_interface::CallbackReturn on_init() override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + private: + const Eigen::VectorXi perm_indices_ = (Eigen::Matrix() << 3, 4, 5, 0, 1, 2).finished(); + const Eigen::PermutationMatrix jacobian_perm_ = + Eigen::PermutationMatrix(perm_indices_); + + std::vector command_joint_names_; + size_t dof_{0}; + std::shared_ptr> rt_trajectory_; + + std::vector joint_command_handles_; + std::vector joint_position_state_; + std::vector joint_velocity_state_; + std::vector joint_effort_state_; + + std::shared_ptr parameter_handler_; + Params params_; + + std::string end_effector_; + std::string wrench_ee_frame_; + std::string root_frame_; + std::string robot_description_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rclcpp::Time tf_last_time_; + std::shared_ptr tf_br_; + + rbdyn_wrapper rbdyn_wrapper_; + + std::shared_ptr> + rt_pub_state_; + std::shared_ptr> rt_pub_torques_; + + rclcpp::Subscription::SharedPtr sub_cart_stiffness_; + rclcpp::Subscription::SharedPtr sub_cart_wrench_; + rclcpp::Subscription::SharedPtr sub_damping_factors_; + rclcpp::Subscription::SharedPtr sub_reference_pose_; + rclcpp::Subscription::SharedPtr trajectory_sub_; + rclcpp::Subscription::SharedPtr sub_controller_config_; + + rclcpp_action::Server::SharedPtr traj_as_; + bool traj_running_ = false; + bool traj_as_active_ = false; + std::shared_ptr> traj_as_goal_; + + trajectory_msgs::msg::JointTrajectory trajectory_; + size_t traj_index_ = 0; + rclcpp::Time traj_start_time_; + rclcpp::Duration traj_duration_; + Eigen::Vector3d position_d_target_; + Eigen::Quaterniond orientation_d_target_; + + Eigen::VectorXd tau_m_; + Eigen::VectorXd damping_factors_; + double nullspace_stiffness_target_; + + void read_state_from_hardware(); + void write_command_to_hardware(); + void write_zero_commands_to_hardware(); + bool getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, Eigen::Quaterniond *orientation); + bool getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian); + + void controllerConfigCb(const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg); + void cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg); + void cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg); + rclcpp_action::GoalResponse trajGoalCb(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse trajCancelCb( + const std::shared_ptr> goal_handle); + void trajAcceptCb( + std::shared_ptr> goal_handle); + void trajStart(const trajectory_msgs::msg::JointTrajectory &trajectory); + void trajUpdate(); + + bool transformWrench(Eigen::Matrix *wrench, const std::string &from_frame, + const std::string &to_frame) const; + void publishMsgsAndTf(); + void applyRuntimeParameters(); + bool initRBDyn(); + }; + +} // namespace cartesian_impedance_controller + +#endif // CARTESIAN_IMPEDANCE_CONTROLLER__CARTESIAN_IMPEDANCE_CONTROLLER_ROS_HPP_ \ No newline at end of file diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index 1b5d01f..edc3d5e 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -15,323 +15,181 @@ namespace cartesian_impedance_controller { -using CallbackReturn = controller_interface::CallbackReturn; -using namespace std::chrono_literals; + using CallbackReturn = controller_interface::CallbackReturn; + using namespace std::chrono_literals; -CartesianImpedanceControllerRos::CartesianImpedanceControllerRos() - : controller_interface::ControllerInterface(), traj_duration_(0, 0) -{ - // The Eigen vectors will be resized in on_configure. - q_.resize(0); - dq_.resize(0); - tau_m_.resize(0); - tau_c_.resize(0); -} - -CartesianImpedanceControllerRos::~CartesianImpedanceControllerRos() -{ -} - -CallbackReturn CartesianImpedanceControllerRos::on_init() -{ - auto node = get_node(); - auto logger = node->get_logger(); - - try + CartesianImpedanceControllerRos::CartesianImpedanceControllerRos() + : controller_interface::ControllerInterface(), traj_duration_(0, 0) { - parameter_handler_ = std::make_shared(node); - params_ = parameter_handler_->get_params(); - } - catch (const std::exception& e) - { - RCLCPP_ERROR(logger, "Exception during init: %s", e.what()); - return CallbackReturn::ERROR; + // The Eigen vectors will be resized in on_configure. + q_.resize(0); + dq_.resize(0); + tau_m_.resize(0); + tau_c_.resize(0); } - tf_br_ = std::make_shared(node); - return CallbackReturn::SUCCESS; -} -controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto& jn : params_.joints) + CartesianImpedanceControllerRos::~CartesianImpedanceControllerRos() { - conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); } - return conf; -} -controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto& jn : params_.joints) - { - conf.names.push_back(jn + "/" + hardware_interface::HW_IF_POSITION); - conf.names.push_back(jn + "/" + hardware_interface::HW_IF_VELOCITY); - conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); - } - return conf; -} - -CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecycle::State&) -{ - auto node = get_node(); - auto logger = node->get_logger(); - - if (!parameter_handler_) + CallbackReturn CartesianImpedanceControllerRos::on_init() { - RCLCPP_ERROR(logger, "Parameter handler not initialized."); - return CallbackReturn::ERROR; - } - - rt_trajectory_ = std::make_shared>(); - parameter_handler_->refresh_dynamic_parameters(); - params_ = parameter_handler_->get_params(); + auto node = get_node(); + auto logger = node->get_logger(); - if (params_.joints.empty()) - { - RCLCPP_ERROR(logger, "No 'joints' parameter specified. Aborting configure."); - return CallbackReturn::ERROR; - } - dof_ = params_.joints.size(); - RCLCPP_INFO(logger, "Found %zu joints.", dof_); - - q_ = Eigen::VectorXd::Zero(dof_); - dq_ = Eigen::VectorXd::Zero(dof_); - tau_m_ = Eigen::VectorXd::Zero(dof_); - tau_c_ = Eigen::VectorXd::Zero(dof_); - command_joint_names_ = params_.joints; - - end_effector_ = params_.end_effector; - wrench_ee_frame_ = params_.wrench_ee_frame; - delta_tau_max_ = params_.delta_tau_max; - update_frequency_ = params_.update_frequency; - robot_description_ = params_.robot_description; - tf_buffer_ = std::make_shared(node->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - tf_last_time_ = get_node()->now(); - - std::stringstream joints_ss; - joints_ss << "Joints: "; - for (const auto& joint : params_.joints) - { - joints_ss << joint << " "; - } - RCLCPP_INFO(logger, "%s", joints_ss.str().c_str()); - RCLCPP_INFO(logger, "End Effector: %s", params_.end_effector.c_str()); - RCLCPP_INFO(logger, "Wrench EE Frame: %s", params_.wrench_ee_frame.c_str()); - RCLCPP_INFO(logger, "Update Frequency: %.2f", params_.update_frequency); - RCLCPP_INFO(logger, "Filtering parameters - Nullspace: %.2f, Stiffness: %.2f, Pose: %.2f, Wrench: %.2f", - params_.filtering.nullspace_config, params_.filtering.stiffness, params_.filtering.pose, - params_.filtering.wrench); - RCLCPP_INFO(logger, "Stiffness - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f", - params_.stiffness.translation.x, params_.stiffness.translation.y, params_.stiffness.translation.z, - params_.stiffness.rotation.x, params_.stiffness.rotation.y, params_.stiffness.rotation.z, - params_.stiffness.nullspace_stiffness); - RCLCPP_INFO(logger, - "Damping - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f, Update: %s", - params_.damping.translation.x, params_.damping.translation.y, params_.damping.translation.z, - params_.damping.rotation.x, params_.damping.rotation.y, params_.damping.rotation.z, - params_.damping.nullspace_damping, params_.damping.update_damping_factors ? "true" : "false"); - - trajectory_sub_ = node->create_subscription( - "joint_trajectory", rclcpp::SystemDefaultsQoS(), - std::bind(&CartesianImpedanceControllerRos::trajCb, this, std::placeholders::_1)); - - traj_as_ = rclcpp_action::create_server( - node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), - node->get_node_waitables_interface(), std::string(node->get_name()) + "/follow_joint_trajectory", - std::bind(&CartesianImpedanceControllerRos::trajGoalCb, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&CartesianImpedanceControllerRos::trajCancelCb, this, std::placeholders::_1), - std::bind(&CartesianImpedanceControllerRos::trajAcceptCb, this, std::placeholders::_1)); - - auto state_pub = - get_node()->create_publisher("controller_state", 10); - auto torques_pub = get_node()->create_publisher("commanded_torques", 10); - - rt_pub_state_ = - std::make_shared>( - state_pub); - rt_pub_torques_ = std::make_shared>(torques_pub); - - sub_controller_config_ = get_node()->create_subscription( - "set_config", rclcpp::SystemDefaultsQoS(), - std::bind(&CartesianImpedanceControllerRos::controllerConfigCb, this, std::placeholders::_1)); - - sub_cart_stiffness_ = node->create_subscription( - "set_cartesian_stiffness", rclcpp::SystemDefaultsQoS(), - std::bind(&CartesianImpedanceControllerRos::cartesianStiffnessCb, this, std::placeholders::_1)); - - sub_cart_wrench_ = node->create_subscription( - "set_cartesian_wrench", rclcpp::SystemDefaultsQoS(), - std::bind(&CartesianImpedanceControllerRos::wrenchCommandCb, this, std::placeholders::_1)); - - sub_damping_factors_ = node->create_subscription( - "set_damping_factors", rclcpp::SystemDefaultsQoS(), - std::bind(&CartesianImpedanceControllerRos::cartesianDampingFactorCb, this, std::placeholders::_1)); - - sub_reference_pose_ = node->create_subscription( - "reference_pose", rclcpp::SystemDefaultsQoS(), - std::bind(&CartesianImpedanceControllerRos::referencePoseCb, this, std::placeholders::_1)); - - setNumberOfJoints(dof_); - - if (!initRBDyn()) - { - RCLCPP_ERROR(logger, "Failed to initialize RBDyn. Check robot_description param!"); - return CallbackReturn::ERROR; + try + { + parameter_handler_ = std::make_shared(node); + params_ = parameter_handler_->get_params(); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(logger, "Exception during init: %s", e.what()); + return CallbackReturn::ERROR; + } + tf_br_ = std::make_shared(node); + return CallbackReturn::SUCCESS; } - double filter_ns = params_.filtering.nullspace_config; - double filter_stiff = params_.filtering.stiffness; - double filter_pose = params_.filtering.pose; - double filter_wrench = params_.filtering.wrench; - setFiltering(update_frequency_, filter_ns, filter_stiff, filter_pose, filter_wrench); - - double stiff_tx = params_.stiffness.translation.x; - double stiff_ty = params_.stiffness.translation.y; - double stiff_tz = params_.stiffness.translation.z; - double stiff_rx = params_.stiffness.rotation.x; - double stiff_ry = params_.stiffness.rotation.y; - double stiff_rz = params_.stiffness.rotation.z; - double stiff_ns = params_.stiffness.nullspace_stiffness; - setStiffness(stiff_tx, stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns, true); - - double damp_tx = params_.damping.translation.x; - double damp_ty = params_.damping.translation.y; - double damp_tz = params_.damping.translation.z; - double damp_rx = params_.damping.rotation.x; - double damp_ry = params_.damping.rotation.y; - double damp_rz = params_.damping.rotation.z; - double damp_ns = params_.damping.nullspace_damping; - setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); - - damping_factors_.resize(7); - damping_factors_ << damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns; - nullspace_stiffness_target_ = stiff_ns; - - RCLCPP_INFO(logger, "on_configure() successful."); - return CallbackReturn::SUCCESS; -} - -CallbackReturn CartesianImpedanceControllerRos::on_activate(const rclcpp_lifecycle::State&) -{ - auto node = get_node(); - auto logger = node->get_logger(); - RCLCPP_INFO(logger, "Activating Cartesian Impedance Controller..."); - - auto& command_interfaces = this->command_interfaces_; - const auto& state_interfaces = this->state_interfaces_; - - joint_command_handles_.clear(); - joint_position_state_.clear(); - joint_velocity_state_.clear(); - joint_effort_state_.clear(); - - joint_command_handles_.resize(dof_, nullptr); - joint_position_state_.resize(dof_, nullptr); - joint_velocity_state_.resize(dof_, nullptr); - joint_effort_state_.resize(dof_, nullptr); - - if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_ || tau_c_.size() != dof_) + controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::command_interface_configuration() const { - RCLCPP_ERROR(logger, "Size mismatch in internal vectors (q_, dq_, tau_m_, tau_c_) vs. dof_"); - return CallbackReturn::ERROR; + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &jn : params_.joints) + { + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); + } + return conf; } - for (const auto& interface : state_interfaces) + controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::state_interface_configuration() const { - const std::string& joint_name = interface.get_prefix_name(); - const std::string& interface_type = interface.get_interface_name(); - - auto it = std::find(params_.joints.begin(), params_.joints.end(), joint_name); - if (it == params_.joints.end()) - continue; - const size_t index = std::distance(params_.joints.begin(), it); - - if (interface_type == hardware_interface::HW_IF_POSITION) - joint_position_state_[index] = &interface; - else if (interface_type == hardware_interface::HW_IF_VELOCITY) - joint_velocity_state_[index] = &interface; - else if (interface_type == hardware_interface::HW_IF_EFFORT) - joint_effort_state_[index] = &interface; + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &jn : params_.joints) + { + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_POSITION); + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_VELOCITY); + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); + } + return conf; } - for (auto& interface : command_interfaces) + CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecycle::State &) { - if (interface.get_interface_name() != hardware_interface::HW_IF_EFFORT) - continue; - const std::string& joint_name = interface.get_prefix_name(); - auto it = std::find(params_.joints.begin(), params_.joints.end(), joint_name); - if (it == params_.joints.end()) - continue; - const size_t index = std::distance(params_.joints.begin(), it); - joint_command_handles_[index] = &interface; - } + auto node = get_node(); + auto logger = node->get_logger(); - update(rclcpp::Time(0), rclcpp::Duration(0, 0)); - initDesiredPose(position_, orientation_); - initNullspaceConfig(q_); - for (size_t i = 0; i < dof_; i++) - { - if (!joint_command_handles_[i]) + if (!parameter_handler_) { - RCLCPP_ERROR(logger, "Command handle for joint %zu is null!", i); + RCLCPP_ERROR(logger, "Parameter handler not initialized."); return CallbackReturn::ERROR; } - } - RCLCPP_INFO(logger, "Controller activated successfully."); - return CallbackReturn::SUCCESS; -} - -CallbackReturn CartesianImpedanceControllerRos::on_deactivate(const rclcpp_lifecycle::State&) -{ - write_zero_commands_to_hardware(); - RCLCPP_INFO(get_node()->get_logger(), "on_deactivate() done."); - return CallbackReturn::SUCCESS; -} -controller_interface::return_type CartesianImpedanceControllerRos::update(const rclcpp::Time&, const rclcpp::Duration&) -{ - if (parameter_handler_->is_old(params_)) - { - RCLCPP_INFO(get_node()->get_logger(), "Parameters are outdated. Refreshing..."); + rt_trajectory_ = std::make_shared>(); parameter_handler_->refresh_dynamic_parameters(); params_ = parameter_handler_->get_params(); - applyRuntimeParameters(); - } - - if (traj_running_) - trajUpdate(); - - read_state_from_hardware(); - calculateCommandedTorques(); // Populates tau_c_ - write_command_to_hardware(); - publishMsgsAndTf(); - - if (auto current_traj = rt_trajectory_->readFromRT()) - { - } - - return controller_interface::return_type::OK; -} -CallbackReturn CartesianImpedanceControllerRos::on_cleanup(const rclcpp_lifecycle::State&) -{ - return CallbackReturn::SUCCESS; -} + if (params_.joints.empty()) + { + RCLCPP_ERROR(logger, "No 'joints' parameter specified. Aborting configure."); + return CallbackReturn::ERROR; + } + dof_ = params_.joints.size(); + RCLCPP_INFO(logger, "Found %zu joints.", dof_); + + q_ = Eigen::VectorXd::Zero(dof_); + dq_ = Eigen::VectorXd::Zero(dof_); + tau_m_ = Eigen::VectorXd::Zero(dof_); + tau_c_ = Eigen::VectorXd::Zero(dof_); + command_joint_names_ = params_.joints; + + end_effector_ = params_.end_effector; + wrench_ee_frame_ = params_.wrench_ee_frame; + delta_tau_max_ = params_.delta_tau_max; + update_frequency_ = params_.update_frequency; + robot_description_ = params_.robot_description; + tf_buffer_ = std::make_shared(node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_last_time_ = get_node()->now(); + + std::stringstream joints_ss; + joints_ss << "Joints: "; + for (const auto &joint : params_.joints) + { + joints_ss << joint << " "; + } + RCLCPP_INFO(logger, "%s", joints_ss.str().c_str()); + RCLCPP_INFO(logger, "End Effector: %s", params_.end_effector.c_str()); + RCLCPP_INFO(logger, "Wrench EE Frame: %s", params_.wrench_ee_frame.c_str()); + RCLCPP_INFO(logger, "Update Frequency: %.2f", params_.update_frequency); + RCLCPP_INFO(logger, "Filtering parameters - Nullspace: %.2f, Stiffness: %.2f, Pose: %.2f, Wrench: %.2f", + params_.filtering.nullspace_config, params_.filtering.stiffness, params_.filtering.pose, + params_.filtering.wrench); + RCLCPP_INFO(logger, "Stiffness - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f", + params_.stiffness.translation.x, params_.stiffness.translation.y, params_.stiffness.translation.z, + params_.stiffness.rotation.x, params_.stiffness.rotation.y, params_.stiffness.rotation.z, + params_.stiffness.nullspace_stiffness); + RCLCPP_INFO(logger, + "Damping - Translation: (%.2f, %.2f, %.2f), Rotation: (%.2f, %.2f, %.2f), Nullspace: %.2f, Update: %s", + params_.damping.translation.x, params_.damping.translation.y, params_.damping.translation.z, + params_.damping.rotation.x, params_.damping.rotation.y, params_.damping.rotation.z, + params_.damping.nullspace_damping, params_.damping.update_damping_factors ? "true" : "false"); + + trajectory_sub_ = node->create_subscription( + "joint_trajectory", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::trajCb, this, std::placeholders::_1)); + + traj_as_ = rclcpp_action::create_server( + node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), + node->get_node_waitables_interface(), std::string(node->get_name()) + "/follow_joint_trajectory", + std::bind(&CartesianImpedanceControllerRos::trajGoalCb, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&CartesianImpedanceControllerRos::trajCancelCb, this, std::placeholders::_1), + std::bind(&CartesianImpedanceControllerRos::trajAcceptCb, this, std::placeholders::_1)); + + auto state_pub = + get_node()->create_publisher("controller_state", 10); + auto torques_pub = get_node()->create_publisher("commanded_torques", 10); + + rt_pub_state_ = + std::make_shared>( + state_pub); + rt_pub_torques_ = std::make_shared>(torques_pub); + + sub_controller_config_ = get_node()->create_subscription( + "set_config", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::controllerConfigCb, this, std::placeholders::_1)); + + sub_cart_stiffness_ = node->create_subscription( + "set_cartesian_stiffness", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::cartesianStiffnessCb, this, std::placeholders::_1)); + + sub_cart_wrench_ = node->create_subscription( + "set_cartesian_wrench", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::wrenchCommandCb, this, std::placeholders::_1)); + + sub_damping_factors_ = node->create_subscription( + "set_damping_factors", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::cartesianDampingFactorCb, this, std::placeholders::_1)); + + sub_reference_pose_ = node->create_subscription( + "reference_pose", rclcpp::SystemDefaultsQoS(), + std::bind(&CartesianImpedanceControllerRos::referencePoseCb, this, std::placeholders::_1)); + + setNumberOfJoints(dof_); + + if (!initRBDyn()) + { + RCLCPP_ERROR(logger, "Failed to initialize RBDyn. Check robot_description param!"); + return CallbackReturn::ERROR; + } -CallbackReturn CartesianImpedanceControllerRos::on_error(const rclcpp_lifecycle::State&) -{ - return CallbackReturn::SUCCESS; -} + double filter_ns = params_.filtering.nullspace_config; + double filter_stiff = params_.filtering.stiffness; + double filter_pose = params_.filtering.pose; + double filter_wrench = params_.filtering.wrench; + setFiltering(update_frequency_, filter_ns, filter_stiff, filter_pose, filter_wrench); -void CartesianImpedanceControllerRos::applyRuntimeParameters() -{ - if (params_.stiffness.update_stiffness) - { double stiff_tx = params_.stiffness.translation.x; double stiff_ty = params_.stiffness.translation.y; double stiff_tz = params_.stiffness.translation.z; @@ -339,15 +197,8 @@ void CartesianImpedanceControllerRos::applyRuntimeParameters() double stiff_ry = params_.stiffness.rotation.y; double stiff_rz = params_.stiffness.rotation.z; double stiff_ns = params_.stiffness.nullspace_stiffness; - nullspace_stiffness_target_ = stiff_ns; - RCLCPP_INFO(get_node()->get_logger(), - "Updating stiffness: trans=(%.2f, %.2f, %.2f), rot=(%.2f, %.2f, %.2f), nullspace=%.2f", stiff_tx, - stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns); setStiffness(stiff_tx, stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns, true); - } - if (params_.damping.update_damping_factors) - { double damp_tx = params_.damping.translation.x; double damp_ty = params_.damping.translation.y; double damp_tz = params_.damping.translation.z; @@ -356,576 +207,730 @@ void CartesianImpedanceControllerRos::applyRuntimeParameters() double damp_rz = params_.damping.rotation.z; double damp_ns = params_.damping.nullspace_damping; setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); + + damping_factors_.resize(7); + damping_factors_ << damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns; + nullspace_stiffness_target_ = stiff_ns; + + RCLCPP_INFO(logger, "on_configure() successful."); + return CallbackReturn::SUCCESS; } - if (params_.wrench.apply_wrench) + CallbackReturn CartesianImpedanceControllerRos::on_activate(const rclcpp_lifecycle::State &) { - double fx = params_.wrench.force_x; - double fy = params_.wrench.force_y; - double fz = params_.wrench.force_z; - double tx = params_.wrench.torque_x; - double ty = params_.wrench.torque_y; - double tz = params_.wrench.torque_z; + auto node = get_node(); + auto logger = node->get_logger(); + RCLCPP_INFO(logger, "Activating Cartesian Impedance Controller..."); - Eigen::Matrix F; - F << fx, fy, fz, tx, ty, tz; + auto &command_interfaces = this->command_interfaces_; + const auto &state_interfaces = this->state_interfaces_; - if (!transformWrench(&F, wrench_ee_frame_, root_frame_)) - RCLCPP_WARN(get_node()->get_logger(), "Could not transform param-based wrench. Not applying it."); - else - applyWrench(F); - } - else - { - applyWrench(Eigen::Matrix::Zero()); - } -} + joint_command_handles_.clear(); + joint_position_state_.clear(); + joint_velocity_state_.clear(); + joint_effort_state_.clear(); -bool CartesianImpedanceControllerRos::initRBDyn() -{ - auto node = get_node(); - auto logger = node->get_logger(); - - auto param_client = std::make_shared(node, "/robot_state_publisher"); - if (!param_client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(logger, "Parameter service not available on /robot_state_publisher"); - return false; - } + joint_command_handles_.resize(dof_, nullptr); + joint_position_state_.resize(dof_, nullptr); + joint_velocity_state_.resize(dof_, nullptr); + joint_effort_state_.resize(dof_, nullptr); - std::string urdf_string; - try { - auto response = param_client->get_parameters({robot_description_}).get(); - if (response.empty() || response[0].get_type() != rclcpp::PARAMETER_STRING) { - RCLCPP_ERROR(logger, "Failed to retrieve 'robot_description' parameter"); - return false; + if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_ || tau_c_.size() != dof_) + { + RCLCPP_ERROR(logger, "Size mismatch in internal vectors (q_, dq_, tau_m_, tau_c_) vs. dof_"); + return CallbackReturn::ERROR; } - urdf_string = response[0].as_string(); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger, "Exception while retrieving robot_description: %s", e.what()); - return false; - } - if (urdf_string.empty()) { - RCLCPP_ERROR(logger, "Retrieved empty 'robot_description'"); - return false; - } + for (const auto &interface : state_interfaces) + { + const std::string &joint_name = interface.get_prefix_name(); + const std::string &interface_type = interface.get_interface_name(); + + auto it = std::find(params_.joints.begin(), params_.joints.end(), joint_name); + if (it == params_.joints.end()) + continue; + const size_t index = std::distance(params_.joints.begin(), it); + + if (interface_type == hardware_interface::HW_IF_POSITION) + joint_position_state_[index] = &interface; + else if (interface_type == hardware_interface::HW_IF_VELOCITY) + joint_velocity_state_[index] = &interface; + else if (interface_type == hardware_interface::HW_IF_EFFORT) + joint_effort_state_[index] = &interface; + } - // Initialize RBDyn with the URDF - try { - rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); - root_frame_ = rbdyn_wrapper_.root_link(); - RCLCPP_INFO(logger, "Root frame is '%s'", root_frame_.c_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger, "Error initializing RBDyn: %s", e.what()); - return false; - } + for (auto &interface : command_interfaces) + { + if (interface.get_interface_name() != hardware_interface::HW_IF_EFFORT) + continue; + const std::string &joint_name = interface.get_prefix_name(); + auto it = std::find(params_.joints.begin(), params_.joints.end(), joint_name); + if (it == params_.joints.end()) + continue; + const size_t index = std::distance(params_.joints.begin(), it); + joint_command_handles_[index] = &interface; + } - if (rbdyn_wrapper_.n_joints() < static_cast(dof_)) - { - RCLCPP_ERROR(logger, "URDF has fewer joints (%zu) than the number to be controlled (%zu)", - rbdyn_wrapper_.n_joints(), dof_); - return false; + update(rclcpp::Time(0), rclcpp::Duration(0, 0)); + initDesiredPose(position_, orientation_); + initNullspaceConfig(q_); + for (size_t i = 0; i < dof_; i++) + { + if (!joint_command_handles_[i]) + { + RCLCPP_ERROR(logger, "Command handle for joint %zu is null!", i); + return CallbackReturn::ERROR; + } + } + RCLCPP_INFO(logger, "Controller activated successfully."); + return CallbackReturn::SUCCESS; } - return true; -} - -void CartesianImpedanceControllerRos::read_state_from_hardware() -{ - if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_) + CallbackReturn CartesianImpedanceControllerRos::on_deactivate(const rclcpp_lifecycle::State &) { - RCLCPP_ERROR(get_node()->get_logger(), "Mismatched Eigen vector sizes in read_state_from_hardware."); - return; + write_zero_commands_to_hardware(); + RCLCPP_INFO(get_node()->get_logger(), "on_deactivate() done."); + return CallbackReturn::SUCCESS; } - for (size_t i = 0; i < dof_; i++) + controller_interface::return_type CartesianImpedanceControllerRos::update(const rclcpp::Time &, const rclcpp::Duration &) { - if (!joint_position_state_[i] || !joint_velocity_state_[i] || !joint_effort_state_[i]) + if (parameter_handler_->is_old(params_)) { - RCLCPP_ERROR(get_node()->get_logger(), "Joint state handle at index %zu is nullptr.", i); - q_(i) = dq_(i) = tau_m_(i) = 0.0; - continue; + RCLCPP_INFO(get_node()->get_logger(), "Parameters are outdated. Refreshing..."); + parameter_handler_->refresh_dynamic_parameters(); + params_ = parameter_handler_->get_params(); + applyRuntimeParameters(); } - q_(i) = joint_position_state_[i]->get_value(); - dq_(i) = joint_velocity_state_[i]->get_value(); - tau_m_(i) = joint_effort_state_[i]->get_value(); - } - - getJacobian(q_, dq_, &jacobian_); - getFk(q_, &position_, &orientation_); -} -void CartesianImpedanceControllerRos::write_command_to_hardware() -{ - for (size_t i = 0; i < dof_; i++) - joint_command_handles_[i]->set_value(tau_c_(i)); -} + if (traj_running_) + trajUpdate(); -void CartesianImpedanceControllerRos::write_zero_commands_to_hardware() -{ - for (size_t i = 0; i < dof_; i++) - joint_command_handles_[i]->set_value(0.0); -} + read_state_from_hardware(); + calculateCommandedTorques(); // Populates tau_c_ + write_command_to_hardware(); + publishMsgsAndTf(); -bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd& q, Eigen::Vector3d* position, - Eigen::Quaterniond* orientation) -{ - auto logger = get_node()->get_logger(); + return controller_interface::return_type::OK; + } - if (!position || !orientation) + CallbackReturn CartesianImpedanceControllerRos::on_cleanup(const rclcpp_lifecycle::State &) { - RCLCPP_ERROR(logger, "getFk: Null pointer provided for position or orientation."); - return false; + return CallbackReturn::SUCCESS; } - rbdyn_wrapper::EefState ee_state; + CallbackReturn CartesianImpedanceControllerRos::on_error(const rclcpp_lifecycle::State &) + { + return CallbackReturn::SUCCESS; + } - if (q.size() != this->n_joints_) + void CartesianImpedanceControllerRos::applyRuntimeParameters() { - RCLCPP_ERROR(logger, "getFk: Provided joint vector size (%ld) does not match the controller's joint count (%zu).", - q.size(), this->n_joints_); - return false; + if (params_.stiffness.update_stiffness) + { + double stiff_tx = params_.stiffness.translation.x; + double stiff_ty = params_.stiffness.translation.y; + double stiff_tz = params_.stiffness.translation.z; + double stiff_rx = params_.stiffness.rotation.x; + double stiff_ry = params_.stiffness.rotation.y; + double stiff_rz = params_.stiffness.rotation.z; + double stiff_ns = params_.stiffness.nullspace_stiffness; + nullspace_stiffness_target_ = stiff_ns; + RCLCPP_INFO(get_node()->get_logger(), + "Updating stiffness: trans=(%.2f, %.2f, %.2f), rot=(%.2f, %.2f, %.2f), nullspace=%.2f", stiff_tx, + stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns); + setStiffness(stiff_tx, stiff_ty, stiff_tz, stiff_rx, stiff_ry, stiff_rz, stiff_ns, true); + } + + if (params_.damping.update_damping_factors) + { + double damp_tx = params_.damping.translation.x; + double damp_ty = params_.damping.translation.y; + double damp_tz = params_.damping.translation.z; + double damp_rx = params_.damping.rotation.x; + double damp_ry = params_.damping.rotation.y; + double damp_rz = params_.damping.rotation.z; + double damp_ns = params_.damping.nullspace_damping; + setDampingFactors(damp_tx, damp_ty, damp_tz, damp_rx, damp_ry, damp_rz, damp_ns); + } + + if (params_.wrench.apply_wrench) + { + double fx = params_.wrench.force_x; + double fy = params_.wrench.force_y; + double fz = params_.wrench.force_z; + double tx = params_.wrench.torque_x; + double ty = params_.wrench.torque_y; + double tz = params_.wrench.torque_z; + + Eigen::Matrix F; + F << fx, fy, fz, tx, ty, tz; + + if (!transformWrench(&F, wrench_ee_frame_, root_frame_)) + RCLCPP_WARN(get_node()->get_logger(), "Could not transform param-based wrench. Not applying it."); + else + applyWrench(F); + } + else + { + applyWrench(Eigen::Matrix::Zero()); + } } - if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) + bool CartesianImpedanceControllerRos::initRBDyn() { - Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - q_rb.head(q.size()) = q; + auto node = get_node(); + auto logger = node->get_logger(); + + auto param_client = std::make_shared(node, "/robot_state_publisher"); + if (!param_client->wait_for_service(std::chrono::seconds(5))) + { + RCLCPP_ERROR(logger, "Parameter service not available on /robot_state_publisher"); + return false; + } + + std::string urdf_string; try { - ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); + auto response = param_client->get_parameters({robot_description_}).get(); + if (response.empty() || response[0].get_type() != rclcpp::PARAMETER_STRING) + { + RCLCPP_ERROR(logger, "Failed to retrieve 'robot_description' parameter"); + return false; + } + urdf_string = response[0].as_string(); } - catch (const std::exception& e) + catch (const std::exception &e) { - RCLCPP_ERROR(logger, "getFk: Exception during perform_fk with expanded joint vector: %s", e.what()); + RCLCPP_ERROR(logger, "Exception while retrieving robot_description: %s", e.what()); return false; } - } - else - { + + if (urdf_string.empty()) + { + RCLCPP_ERROR(logger, "Retrieved empty 'robot_description'"); + return false; + } + + // Initialize RBDyn with the URDF try { - ee_state = this->rbdyn_wrapper_.perform_fk(q); + rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); + root_frame_ = rbdyn_wrapper_.root_link(); + RCLCPP_INFO(logger, "Root frame is '%s'", root_frame_.c_str()); } - catch (const std::exception& e) + catch (const std::exception &e) { - RCLCPP_ERROR(logger, "getFk: Exception during perform_fk: %s", e.what()); + RCLCPP_ERROR(logger, "Error initializing RBDyn: %s", e.what()); return false; } - } - *position = ee_state.translation; - *orientation = ee_state.orientation; - RCLCPP_DEBUG(logger, "position: %f, %f, %f", position->x(), position->y(), position->z()); - return true; -} + if (rbdyn_wrapper_.n_joints() < static_cast(dof_)) + { + RCLCPP_ERROR(logger, "URDF has fewer joints (%zu) than the number to be controlled (%zu)", + rbdyn_wrapper_.n_joints(), dof_); + return false; + } -bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd& q, const Eigen::VectorXd& dq, - Eigen::MatrixXd* jacobian) -{ - if (!jacobian) - { - RCLCPP_ERROR(get_node()->get_logger(), "Jacobian pointer is null."); - return false; + return true; } - if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) + void CartesianImpedanceControllerRos::read_state_from_hardware() { - Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - q_rb.head(q.size()) = q; - Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); - dq_rb.head(dq.size()) = dq; - *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb); + if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Mismatched Eigen vector sizes in read_state_from_hardware."); + return; + } + + for (size_t i = 0; i < dof_; i++) + { + if (!joint_position_state_[i] || !joint_velocity_state_[i] || !joint_effort_state_[i]) + { + RCLCPP_ERROR(get_node()->get_logger(), "Joint state handle at index %zu is nullptr.", i); + q_(i) = dq_(i) = tau_m_(i) = 0.0; + continue; + } + q_(i) = joint_position_state_[i]->get_value(); + dq_(i) = joint_velocity_state_[i]->get_value(); + tau_m_(i) = joint_effort_state_[i]->get_value(); + } + + getJacobian(q_, dq_, &jacobian_); + getFk(q_, &position_, &orientation_); } - else + + void CartesianImpedanceControllerRos::write_command_to_hardware() { - *jacobian = this->rbdyn_wrapper_.jacobian(q, dq); + for (size_t i = 0; i < dof_; i++) + joint_command_handles_[i]->set_value(tau_c_(i)); } - *jacobian = jacobian_perm_ * *jacobian; - return true; -} -void CartesianImpedanceControllerRos::controllerConfigCb( - const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) -{ - setStiffness(msg->cartesian_stiffness.force.x, msg->cartesian_stiffness.force.y, msg->cartesian_stiffness.force.z, - msg->cartesian_stiffness.torque.x, msg->cartesian_stiffness.torque.y, msg->cartesian_stiffness.torque.z, - msg->nullspace_stiffness, false); - setDampingFactors(msg->cartesian_damping_factors.force.x, msg->cartesian_damping_factors.force.y, - msg->cartesian_damping_factors.force.z, msg->cartesian_damping_factors.torque.x, - msg->cartesian_damping_factors.torque.y, msg->cartesian_damping_factors.torque.z, - msg->nullspace_damping_factor); - - if (msg->q_d_nullspace.size() == dof_) + void CartesianImpedanceControllerRos::write_zero_commands_to_hardware() { - Eigen::VectorXd qd(dof_); for (size_t i = 0; i < dof_; i++) + joint_command_handles_[i]->set_value(0.0); + } + + bool CartesianImpedanceControllerRos::getFk(const Eigen::VectorXd &q, Eigen::Vector3d *position, + Eigen::Quaterniond *orientation) + { + auto logger = get_node()->get_logger(); + + if (!position || !orientation) + { + RCLCPP_ERROR(logger, "getFk: Null pointer provided for position or orientation."); + return false; + } + + rbdyn_wrapper::EefState ee_state; + + if (q.size() != this->n_joints_) { - qd(i) = msg->q_d_nullspace[i]; + RCLCPP_ERROR(logger, "getFk: Provided joint vector size (%ld) does not match the controller's joint count (%zu).", + q.size(), this->n_joints_); + return false; + } + + if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) + { + Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + q_rb.head(q.size()) = q; + try + { + ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(logger, "getFk: Exception during perform_fk with expanded joint vector: %s", e.what()); + return false; + } + } + else + { + try + { + ee_state = this->rbdyn_wrapper_.perform_fk(q); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(logger, "getFk: Exception during perform_fk: %s", e.what()); + return false; + } } - setNullspaceConfig(qd); + + *position = ee_state.translation; + *orientation = ee_state.orientation; + RCLCPP_DEBUG(logger, "position: %f, %f, %f", position->x(), position->y(), position->z()); + return true; } - else + + bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, + Eigen::MatrixXd *jacobian) { - RCLCPP_WARN(get_node()->get_logger(), "Nullspace configuration has wrong dimension: got %zu, expected %zu", - msg->q_d_nullspace.size(), dof_); + if (!jacobian) + { + RCLCPP_ERROR(get_node()->get_logger(), "Jacobian pointer is null."); + return false; + } + + if (this->rbdyn_wrapper_.n_joints() != this->n_joints_) + { + Eigen::VectorXd q_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + q_rb.head(q.size()) = q; + Eigen::VectorXd dq_rb = Eigen::VectorXd::Zero(this->rbdyn_wrapper_.n_joints()); + dq_rb.head(dq.size()) = dq; + *jacobian = this->rbdyn_wrapper_.jacobian(q_rb, dq_rb); + } + else + { + *jacobian = this->rbdyn_wrapper_.jacobian(q, dq); + } + *jacobian = jacobian_perm_ * *jacobian; + return true; } -} -void CartesianImpedanceControllerRos::publishMsgsAndTf() -{ - if (rt_pub_torques_ && rt_pub_torques_->trylock()) + void CartesianImpedanceControllerRos::controllerConfigCb( + const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) { - rt_pub_torques_->msg_.data.resize(tau_c_.size()); - for (size_t i = 0; i < tau_c_.size(); ++i) + setStiffness(msg->cartesian_stiffness.force.x, msg->cartesian_stiffness.force.y, msg->cartesian_stiffness.force.z, + msg->cartesian_stiffness.torque.x, msg->cartesian_stiffness.torque.y, msg->cartesian_stiffness.torque.z, + msg->nullspace_stiffness, false); + setDampingFactors(msg->cartesian_damping_factors.force.x, msg->cartesian_damping_factors.force.y, + msg->cartesian_damping_factors.force.z, msg->cartesian_damping_factors.torque.x, + msg->cartesian_damping_factors.torque.y, msg->cartesian_damping_factors.torque.z, + msg->nullspace_damping_factor); + + if (msg->q_d_nullspace.size() == dof_) + { + Eigen::VectorXd qd(dof_); + for (size_t i = 0; i < dof_; i++) + { + qd(i) = msg->q_d_nullspace[i]; + } + setNullspaceConfig(qd); + } + else { - rt_pub_torques_->msg_.data[i] = tau_c_(i); + RCLCPP_WARN(get_node()->get_logger(), "Nullspace configuration has wrong dimension: got %zu, expected %zu", + msg->q_d_nullspace.size(), dof_); } - rt_pub_torques_->unlockAndPublish(); } - Eigen::Matrix error = getPoseError(); - - if (params_.verbosity.verbose_print) + void CartesianImpedanceControllerRos::publishMsgsAndTf() { - RCLCPP_INFO(get_node()->get_logger(), "Cartesian Position: [%f, %f, %f]", position_(0), position_(1), position_(2)); - RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", error(0), error(1), error(2), - error(3), error(4), error(5)); + if (rt_pub_torques_ && rt_pub_torques_->trylock()) + { + rt_pub_torques_->msg_.data.resize(tau_c_.size()); + for (size_t i = 0; i < tau_c_.size(); ++i) + { + rt_pub_torques_->msg_.data[i] = tau_c_(i); + } + rt_pub_torques_->unlockAndPublish(); + } + + Eigen::Matrix error = getPoseError(); - RCLCPP_INFO(get_node()->get_logger(), "Cartesian Stiffness (diag): [%f, %f, %f, %f, %f, %f]", - cartesian_stiffness_.diagonal()(0), cartesian_stiffness_.diagonal()(1), - cartesian_stiffness_.diagonal()(2), cartesian_stiffness_.diagonal()(3), - cartesian_stiffness_.diagonal()(4), cartesian_stiffness_.diagonal()(5)); + if (params_.verbosity.verbose_print) + { + RCLCPP_INFO(get_node()->get_logger(), "Cartesian Position: [%f, %f, %f]", position_(0), position_(1), position_(2)); + RCLCPP_INFO(get_node()->get_logger(), "Pose Error: [%f, %f, %f, %f, %f, %f]", error(0), error(1), error(2), + error(3), error(4), error(5)); - RCLCPP_INFO(get_node()->get_logger(), "Cartesian Damping (diag): [%f, %f, %f, %f, %f, %f]", - cartesian_damping_.diagonal()(0), cartesian_damping_.diagonal()(1), cartesian_damping_.diagonal()(2), - cartesian_damping_.diagonal()(3), cartesian_damping_.diagonal()(4), cartesian_damping_.diagonal()(5)); + RCLCPP_INFO(get_node()->get_logger(), "Cartesian Stiffness (diag): [%f, %f, %f, %f, %f, %f]", + cartesian_stiffness_.diagonal()(0), cartesian_stiffness_.diagonal()(1), + cartesian_stiffness_.diagonal()(2), cartesian_stiffness_.diagonal()(3), + cartesian_stiffness_.diagonal()(4), cartesian_stiffness_.diagonal()(5)); - RCLCPP_INFO(get_node()->get_logger(), "Nullspace Stiffness: %f", nullspace_stiffness_); + RCLCPP_INFO(get_node()->get_logger(), "Cartesian Damping (diag): [%f, %f, %f, %f, %f, %f]", + cartesian_damping_.diagonal()(0), cartesian_damping_.diagonal()(1), cartesian_damping_.diagonal()(2), + cartesian_damping_.diagonal()(3), cartesian_damping_.diagonal()(4), cartesian_damping_.diagonal()(5)); - { - std::stringstream ss; - ss << "q_d_nullspace: ["; - for (int i = 0; i < q_d_nullspace_.size(); ++i) + RCLCPP_INFO(get_node()->get_logger(), "Nullspace Stiffness: %f", nullspace_stiffness_); + + { + std::stringstream ss; + ss << "q_d_nullspace: ["; + for (int i = 0; i < q_d_nullspace_.size(); ++i) + { + ss << q_d_nullspace_(i); + if (i != q_d_nullspace_.size() - 1) + { + ss << ", "; + } + } + ss << "]"; + RCLCPP_INFO(get_node()->get_logger(), "%s", ss.str().c_str()); + } { - ss << q_d_nullspace_(i); - if (i != q_d_nullspace_.size() - 1) + std::stringstream ss; + ss << "tau_c: ["; + for (int i = 0; i < tau_c_.size(); ++i) { - ss << ", "; + ss << tau_c_(i); + if (i != tau_c_.size() - 1) + { + ss << ", "; + } } + ss << "]"; + RCLCPP_INFO(get_node()->get_logger(), "%s", ss.str().c_str()); } - ss << "]"; - RCLCPP_INFO(get_node()->get_logger(), "%s", ss.str().c_str()); } + + if (params_.verbosity.tf_frames) { - std::stringstream ss; - ss << "tau_c: ["; - for (int i = 0; i < tau_c_.size(); ++i) + rclcpp::Time now = get_node()->now(); + if (now > tf_last_time_) { - ss << tau_c_(i); - if (i != tau_c_.size() - 1) + if (!tf_br_) { - ss << ", "; + tf_br_ = std::make_shared(get_node()); } + + geometry_msgs::msg::TransformStamped fk_tf; + fk_tf.header.stamp = now; + fk_tf.header.frame_id = root_frame_; + fk_tf.child_frame_id = end_effector_ + "_ee_fk"; + fk_tf.transform.translation.x = position_(0); + fk_tf.transform.translation.y = position_(1); + fk_tf.transform.translation.z = position_(2); + fk_tf.transform.rotation = + tf2::toMsg(tf2::Quaternion(orientation_.x(), orientation_.y(), orientation_.z(), orientation_.w())); + tf_br_->sendTransform(fk_tf); + + geometry_msgs::msg::TransformStamped ref_tf; + ref_tf.header.stamp = now; + ref_tf.header.frame_id = root_frame_; + ref_tf.child_frame_id = end_effector_ + "_ee_ref_pose"; + ref_tf.transform.translation.x = position_d_(0); + ref_tf.transform.translation.y = position_d_(1); + ref_tf.transform.translation.z = position_d_(2); + ref_tf.transform.rotation = + tf2::toMsg(tf2::Quaternion(orientation_d_.x(), orientation_d_.y(), orientation_d_.z(), orientation_d_.w())); + tf_br_->sendTransform(ref_tf); + + tf_last_time_ = now; } - ss << "]"; - RCLCPP_INFO(get_node()->get_logger(), "%s", ss.str().c_str()); } - } - if (params_.verbosity.tf_frames) - { - rclcpp::Time now = get_node()->now(); - if (now > tf_last_time_) + if (params_.verbosity.state_msgs) { - if (!tf_br_) + if (rt_pub_state_ && rt_pub_state_->trylock()) { - tf_br_ = std::make_shared(get_node()); + auto &state_msg = rt_pub_state_->msg_; + state_msg.header.stamp = get_node()->now(); + + state_msg.joint_state.name = params_.joints; + state_msg.joint_state.position.assign(q_.data(), q_.data() + q_.size()); + state_msg.joint_state.velocity.assign(dq_.data(), dq_.data() + dq_.size()); + state_msg.joint_state.effort.assign(tau_m_.data(), tau_m_.data() + tau_m_.size()); + state_msg.commanded_torques.assign(tau_c_.data(), tau_c_.data() + tau_c_.size()); + state_msg.nullspace_config.assign(q_d_nullspace_.data(), q_d_nullspace_.data() + q_d_nullspace_.size()); + + state_msg.current_pose.position.x = position_(0); + state_msg.current_pose.position.y = position_(1); + state_msg.current_pose.position.z = position_(2); + state_msg.current_pose.orientation = + tf2::toMsg(tf2::Quaternion(orientation_.x(), orientation_.y(), orientation_.z(), orientation_.w())); + state_msg.reference_pose.position.x = position_d_target_(0); + state_msg.reference_pose.position.y = position_d_target_(1); + state_msg.reference_pose.position.z = position_d_target_(2); + state_msg.reference_pose.orientation.w = orientation_d_target_.w(); + state_msg.reference_pose.orientation.x = orientation_d_target_.x(); + state_msg.reference_pose.orientation.y = orientation_d_target_.y(); + state_msg.reference_pose.orientation.z = orientation_d_target_.z(); + + state_msg.pose_error.position.x = error(0); + state_msg.pose_error.position.y = error(1); + state_msg.pose_error.position.z = error(2); + Eigen::Quaterniond q_err = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); + state_msg.pose_error.orientation = tf2::toMsg(tf2::Quaternion(q_err.x(), q_err.y(), q_err.z(), q_err.w())); + + state_msg.cartesian_stiffness.force.x = cartesian_stiffness_.diagonal()(0); + state_msg.cartesian_stiffness.force.y = cartesian_stiffness_.diagonal()(1); + state_msg.cartesian_stiffness.force.z = cartesian_stiffness_.diagonal()(2); + state_msg.cartesian_stiffness.torque.x = cartesian_stiffness_.diagonal()(3); + state_msg.cartesian_stiffness.torque.y = cartesian_stiffness_.diagonal()(4); + state_msg.cartesian_stiffness.torque.z = cartesian_stiffness_.diagonal()(5); + + state_msg.cartesian_damping.force.x = cartesian_damping_.diagonal()(0); + state_msg.cartesian_damping.force.y = cartesian_damping_.diagonal()(1); + state_msg.cartesian_damping.force.z = cartesian_damping_.diagonal()(2); + state_msg.cartesian_damping.torque.x = cartesian_damping_.diagonal()(3); + state_msg.cartesian_damping.torque.y = cartesian_damping_.diagonal()(4); + state_msg.cartesian_damping.torque.z = cartesian_damping_.diagonal()(5); + + Eigen::Matrix applied_wrench = getAppliedWrench(); + state_msg.commanded_wrench.force.x = applied_wrench(0); + state_msg.commanded_wrench.force.y = applied_wrench(1); + state_msg.commanded_wrench.force.z = applied_wrench(2); + state_msg.commanded_wrench.torque.x = applied_wrench(3); + state_msg.commanded_wrench.torque.y = applied_wrench(4); + state_msg.commanded_wrench.torque.z = applied_wrench(5); + + state_msg.nullspace_stiffness = nullspace_stiffness_; + state_msg.nullspace_damping = nullspace_damping_; + + Eigen::Matrix dx = jacobian_ * dq_; + state_msg.cartesian_velocity = std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + + rt_pub_state_->unlockAndPublish(); } - - geometry_msgs::msg::TransformStamped fk_tf; - fk_tf.header.stamp = now; - fk_tf.header.frame_id = root_frame_; - fk_tf.child_frame_id = end_effector_ + "_ee_fk"; - fk_tf.transform.translation.x = position_(0); - fk_tf.transform.translation.y = position_(1); - fk_tf.transform.translation.z = position_(2); - fk_tf.transform.rotation = - tf2::toMsg(tf2::Quaternion(orientation_.x(), orientation_.y(), orientation_.z(), orientation_.w())); - tf_br_->sendTransform(fk_tf); - - geometry_msgs::msg::TransformStamped ref_tf; - ref_tf.header.stamp = now; - ref_tf.header.frame_id = root_frame_; - ref_tf.child_frame_id = end_effector_ + "_ee_ref_pose"; - ref_tf.transform.translation.x = position_d_(0); - ref_tf.transform.translation.y = position_d_(1); - ref_tf.transform.translation.z = position_d_(2); - ref_tf.transform.rotation = - tf2::toMsg(tf2::Quaternion(orientation_d_.x(), orientation_d_.y(), orientation_d_.z(), orientation_d_.w())); - tf_br_->sendTransform(ref_tf); - - tf_last_time_ = now; } } - if (params_.verbosity.state_msgs) + void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg) { - if (rt_pub_state_ && rt_pub_state_->trylock()) - { - auto& state_msg = rt_pub_state_->msg_; - state_msg.header.stamp = get_node()->now(); - - state_msg.joint_state.name = params_.joints; - state_msg.joint_state.position.assign(q_.data(), q_.data() + q_.size()); - state_msg.joint_state.velocity.assign(dq_.data(), dq_.data() + dq_.size()); - state_msg.joint_state.effort.assign(tau_m_.data(), tau_m_.data() + tau_m_.size()); - state_msg.commanded_torques.assign(tau_c_.data(), tau_c_.data() + tau_c_.size()); - state_msg.nullspace_config.assign(q_d_nullspace_.data(), q_d_nullspace_.data() + q_d_nullspace_.size()); - - state_msg.current_pose.position.x = position_(0); - state_msg.current_pose.position.y = position_(1); - state_msg.current_pose.position.z = position_(2); - state_msg.current_pose.orientation = - tf2::toMsg(tf2::Quaternion(orientation_.x(), orientation_.y(), orientation_.z(), orientation_.w())); - state_msg.reference_pose.position.x = position_d_target_(0); - state_msg.reference_pose.position.y = position_d_target_(1); - state_msg.reference_pose.position.z = position_d_target_(2); - state_msg.reference_pose.orientation.w = orientation_d_target_.w(); - state_msg.reference_pose.orientation.x = orientation_d_target_.x(); - state_msg.reference_pose.orientation.y = orientation_d_target_.y(); - state_msg.reference_pose.orientation.z = orientation_d_target_.z(); - - state_msg.pose_error.position.x = error(0); - state_msg.pose_error.position.y = error(1); - state_msg.pose_error.position.z = error(2); - Eigen::Quaterniond q_err = Eigen::AngleAxisd(error(3), Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(error(4), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(error(5), Eigen::Vector3d::UnitZ()); - state_msg.pose_error.orientation = tf2::toMsg(tf2::Quaternion(q_err.x(), q_err.y(), q_err.z(), q_err.w())); - - state_msg.cartesian_stiffness.force.x = cartesian_stiffness_.diagonal()(0); - state_msg.cartesian_stiffness.force.y = cartesian_stiffness_.diagonal()(1); - state_msg.cartesian_stiffness.force.z = cartesian_stiffness_.diagonal()(2); - state_msg.cartesian_stiffness.torque.x = cartesian_stiffness_.diagonal()(3); - state_msg.cartesian_stiffness.torque.y = cartesian_stiffness_.diagonal()(4); - state_msg.cartesian_stiffness.torque.z = cartesian_stiffness_.diagonal()(5); - - state_msg.cartesian_damping.force.x = cartesian_damping_.diagonal()(0); - state_msg.cartesian_damping.force.y = cartesian_damping_.diagonal()(1); - state_msg.cartesian_damping.force.z = cartesian_damping_.diagonal()(2); - state_msg.cartesian_damping.torque.x = cartesian_damping_.diagonal()(3); - state_msg.cartesian_damping.torque.y = cartesian_damping_.diagonal()(4); - state_msg.cartesian_damping.torque.z = cartesian_damping_.diagonal()(5); - - Eigen::Matrix applied_wrench = getAppliedWrench(); - state_msg.commanded_wrench.force.x = applied_wrench(0); - state_msg.commanded_wrench.force.y = applied_wrench(1); - state_msg.commanded_wrench.force.z = applied_wrench(2); - state_msg.commanded_wrench.torque.x = applied_wrench(3); - state_msg.commanded_wrench.torque.y = applied_wrench(4); - state_msg.commanded_wrench.torque.z = applied_wrench(5); - - state_msg.nullspace_stiffness = nullspace_stiffness_; - state_msg.nullspace_damping = nullspace_damping_; - - Eigen::Matrix dx = jacobian_ * dq_; - state_msg.cartesian_velocity = std::sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); - - rt_pub_state_->unlockAndPublish(); - } + damping_factors_(0) = msg->force.x; + damping_factors_(1) = msg->force.y; + damping_factors_(2) = msg->force.z; + damping_factors_(3) = msg->torque.x; + damping_factors_(4) = msg->torque.y; + damping_factors_(5) = msg->torque.z; + setDampingFactors(damping_factors_(0), damping_factors_(1), damping_factors_(2), damping_factors_(3), + damping_factors_(4), damping_factors_(5), damping_factors_(6)); } -} -void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg) -{ - damping_factors_(0) = msg->force.x; - damping_factors_(1) = msg->force.y; - damping_factors_(2) = msg->force.z; - damping_factors_(3) = msg->torque.x; - damping_factors_(4) = msg->torque.y; - damping_factors_(5) = msg->torque.z; - setDampingFactors(damping_factors_(0), damping_factors_(1), damping_factors_(2), damping_factors_(3), - damping_factors_(4), damping_factors_(5), damping_factors_(6)); -} - -void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) -{ - Eigen::Matrix stiffness_vector; - stiffness_vector << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, - msg->wrench.torque.y, msg->wrench.torque.z, nullspace_stiffness_target_; - setStiffness(stiffness_vector, true); -} - -void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ - if (msg->header.frame_id == root_frame_) + void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - Eigen::Vector3d pos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); - Eigen::Quaterniond ori(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, - msg->pose.orientation.z); - setReferencePose(pos, ori); - return; + Eigen::Matrix stiffness_vector; + stiffness_vector << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, + msg->wrench.torque.y, msg->wrench.torque.z, nullspace_stiffness_target_; + setStiffness(stiffness_vector, true); } - geometry_msgs::msg::PoseStamped transformed_pose; - try + void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - transformed_pose = tf_buffer_->transform(*msg, root_frame_, tf2::durationFromSec(0.1)); - } - catch (const tf2::TransformException& ex) - { - RCLCPP_WARN(get_node()->get_logger(), "Transform failed: %s", ex.what()); - return; - } + if (msg->header.frame_id == root_frame_) + { + Eigen::Vector3d pos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + Eigen::Quaterniond ori(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z); + setReferencePose(pos, ori); + return; + } - Eigen::Vector3d pos(transformed_pose.pose.position.x, transformed_pose.pose.position.y, - transformed_pose.pose.position.z); - Eigen::Quaterniond ori(transformed_pose.pose.orientation.w, transformed_pose.pose.orientation.x, - transformed_pose.pose.orientation.y, transformed_pose.pose.orientation.z); - setReferencePose(pos, ori); -} + geometry_msgs::msg::PoseStamped transformed_pose; + try + { + transformed_pose = tf_buffer_->transform(*msg, root_frame_, tf2::durationFromSec(0.1)); + } + catch (const tf2::TransformException &ex) + { + RCLCPP_WARN(get_node()->get_logger(), "Transform failed: %s", ex.what()); + return; + } -void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) -{ - Eigen::Matrix F; - F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y, - msg->wrench.torque.z; - std::string from_frame = msg->header.frame_id.empty() ? wrench_ee_frame_ : msg->header.frame_id; - if (!transformWrench(&F, from_frame, root_frame_)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Could not transform wrench. Not applying it."); - return; + Eigen::Vector3d pos(transformed_pose.pose.position.x, transformed_pose.pose.position.y, + transformed_pose.pose.position.z); + Eigen::Quaterniond ori(transformed_pose.pose.orientation.w, transformed_pose.pose.orientation.x, + transformed_pose.pose.orientation.y, transformed_pose.pose.orientation.z); + setReferencePose(pos, ori); } - applyWrench(F); -} -bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix* wrench, - const std::string& from_frame, const std::string& to_frame) const -{ - geometry_msgs::msg::TransformStamped transform; - try + void CartesianImpedanceControllerRos::wrenchCommandCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - transform = tf_buffer_->lookupTransform(to_frame, from_frame, tf2::TimePointZero); + Eigen::Matrix F; + F << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, msg->wrench.torque.x, msg->wrench.torque.y, + msg->wrench.torque.z; + std::string from_frame = msg->header.frame_id.empty() ? wrench_ee_frame_ : msg->header.frame_id; + if (!transformWrench(&F, from_frame, root_frame_)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not transform wrench. Not applying it."); + return; + } + applyWrench(F); } - catch (const tf2::TransformException& ex) + + bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix *wrench, + const std::string &from_frame, const std::string &to_frame) const { - RCLCPP_ERROR(get_node()->get_logger(), "transformWrench() exception: %s", ex.what()); - return false; + geometry_msgs::msg::TransformStamped transform; + try + { + transform = tf_buffer_->lookupTransform(to_frame, from_frame, tf2::TimePointZero); + } + catch (const tf2::TransformException &ex) + { + RCLCPP_ERROR(get_node()->get_logger(), "transformWrench() exception: %s", ex.what()); + return false; + } + tf2::Vector3 force_in((*wrench)(0), (*wrench)(1), (*wrench)(2)); + tf2::Vector3 torque_in((*wrench)(3), (*wrench)(4), (*wrench)(5)); + tf2::Transform tf; + tf2::fromMsg(transform.transform, tf); + tf2::Vector3 force_out = tf * force_in; + tf2::Vector3 torque_out = tf.getBasis() * torque_in; + (*wrench)(0) = force_out.x(); + (*wrench)(1) = force_out.y(); + (*wrench)(2) = force_out.z(); + (*wrench)(3) = torque_out.x(); + (*wrench)(4) = torque_out.y(); + (*wrench)(5) = torque_out.z(); + return true; + } + + void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) + { + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory from topic"); + if (traj_as_active_) + { + RCLCPP_INFO(get_node()->get_logger(), "Preempting running action server goal due to new trajectory"); + traj_running_ = false; + } + trajStart(*msg); + rt_trajectory_->writeFromNonRT(*msg); } - tf2::Vector3 force_in((*wrench)(0), (*wrench)(1), (*wrench)(2)); - tf2::Vector3 torque_in((*wrench)(3), (*wrench)(4), (*wrench)(5)); - tf2::Transform tf; - tf2::fromMsg(transform.transform, tf); - tf2::Vector3 force_out = tf * force_in; - tf2::Vector3 torque_out = tf.getBasis() * torque_in; - (*wrench)(0) = force_out.x(); - (*wrench)(1) = force_out.y(); - (*wrench)(2) = force_out.z(); - (*wrench)(3) = torque_out.x(); - (*wrench)(4) = torque_out.y(); - (*wrench)(5) = torque_out.z(); - return true; -} - -void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) -{ - RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory from topic"); - if (traj_as_active_) + + rclcpp_action::GoalResponse CartesianImpedanceControllerRos::trajGoalCb( + const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { - RCLCPP_INFO(get_node()->get_logger(), "Preempting running action server goal due to new trajectory"); - traj_running_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Received FollowJointTrajectory action goal request"); + if (goal->trajectory.points.empty() || goal->trajectory.joint_names.size() != dof_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory OR mismatched joints in action. Rejecting."); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - trajStart(*msg); - rt_trajectory_->writeFromNonRT(*msg); -} -rclcpp_action::GoalResponse CartesianImpedanceControllerRos::trajGoalCb( - const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) -{ - RCLCPP_INFO(get_node()->get_logger(), "Received FollowJointTrajectory action goal request"); - if (goal->trajectory.points.empty() || goal->trajectory.joint_names.size() != dof_) + rclcpp_action::CancelResponse CartesianImpedanceControllerRos::trajCancelCb( + const std::shared_ptr>) { - RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory OR mismatched joints in action. Rejecting."); - return rclcpp_action::GoalResponse::REJECT; + RCLCPP_INFO(get_node()->get_logger(), "Canceling trajectory action"); + traj_running_ = false; + return rclcpp_action::CancelResponse::ACCEPT; } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse CartesianImpedanceControllerRos::trajCancelCb( - const std::shared_ptr>) -{ - RCLCPP_INFO(get_node()->get_logger(), "Canceling trajectory action"); - traj_running_ = false; - return rclcpp_action::CancelResponse::ACCEPT; -} -void CartesianImpedanceControllerRos::trajAcceptCb( - std::shared_ptr> goal_handle) -{ - auto goal = goal_handle->get_goal(); - auto logger = get_node()->get_logger(); - RCLCPP_INFO(logger, "Accepted FollowJointTrajectory action goal"); - - traj_as_active_ = true; - traj_as_goal_ = goal_handle; - - trajStart(goal->trajectory); - rt_trajectory_->writeFromNonRT(goal->trajectory); -} + void CartesianImpedanceControllerRos::trajAcceptCb( + std::shared_ptr> goal_handle) + { + auto goal = goal_handle->get_goal(); + auto logger = get_node()->get_logger(); + RCLCPP_INFO(logger, "Accepted FollowJointTrajectory action goal"); -void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::msg::JointTrajectory& trajectory) -{ - trajectory_ = trajectory; - traj_index_ = 0; - traj_running_ = true; - traj_start_time_ = get_node()->now(); + traj_as_active_ = true; + traj_as_goal_ = goal_handle; - if (!trajectory.points.empty()) - { - traj_duration_ = rclcpp::Duration(trajectory.points.back().time_from_start.sec, - trajectory.points.back().time_from_start.nanosec); - RCLCPP_INFO(get_node()->get_logger(), "Started a trajectory with %zu points lasting %.2f s.", - trajectory.points.size(), traj_duration_.seconds()); + trajStart(goal->trajectory); + rt_trajectory_->writeFromNonRT(goal->trajectory); } - else - { - RCLCPP_WARN(get_node()->get_logger(), "Empty trajectory. Not running."); - traj_running_ = false; - } -} - -void CartesianImpedanceControllerRos::trajUpdate() -{ - auto now = get_node()->now(); - double t_since_start = (now - traj_start_time_).seconds(); - if (t_since_start > traj_duration_.seconds()) + void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::msg::JointTrajectory &trajectory) { - RCLCPP_INFO(get_node()->get_logger(), "Finished executing trajectory."); - traj_running_ = false; - if (traj_as_active_ && traj_as_goal_) + trajectory_ = trajectory; + traj_index_ = 0; + traj_running_ = true; + traj_start_time_ = get_node()->now(); + + if (!trajectory.points.empty()) + { + traj_duration_ = rclcpp::Duration(trajectory.points.back().time_from_start.sec, + trajectory.points.back().time_from_start.nanosec); + RCLCPP_INFO(get_node()->get_logger(), "Started a trajectory with %zu points lasting %.2f s.", + trajectory.points.size(), traj_duration_.seconds()); + } + else { - auto result = std::make_shared(); - result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; - traj_as_goal_->succeed(result); - traj_as_active_ = false; + RCLCPP_WARN(get_node()->get_logger(), "Empty trajectory. Not running."); + traj_running_ = false; } - return; } - if (now > (traj_start_time_ + rclcpp::Duration(trajectory_.points.at(traj_index_).time_from_start))) + void CartesianImpedanceControllerRos::trajUpdate() { - Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(traj_index_).positions.data(), - trajectory_.points.at(traj_index_).positions.size()); - getFk(q, &position_d_target_, &orientation_d_target_); - setReferencePose(position_d_target_, orientation_d_target_); - setNullspaceConfig(q); - traj_index_++; + auto now = get_node()->now(); + double t_since_start = (now - traj_start_time_).seconds(); + + if (t_since_start > traj_duration_.seconds()) + { + RCLCPP_INFO(get_node()->get_logger(), "Finished executing trajectory."); + traj_running_ = false; + if (traj_as_active_ && traj_as_goal_) + { + auto result = std::make_shared(); + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + traj_as_goal_->succeed(result); + traj_as_active_ = false; + } + return; + } + + if (now > (traj_start_time_ + rclcpp::Duration(trajectory_.points.at(traj_index_).time_from_start))) + { + Eigen::VectorXd q = Eigen::VectorXd::Map(trajectory_.points.at(traj_index_).positions.data(), + trajectory_.points.at(traj_index_).positions.size()); + getFk(q, &position_d_target_, &orientation_d_target_); + setReferencePose(position_d_target_, orientation_d_target_); + setNullspaceConfig(q); + traj_index_++; + } } -} -} // end namespace cartesian_impedance_controller +} // end namespace cartesian_impedance_controller #include "pluginlib/class_list_macros.hpp" From fa6e8f1360afe7923902fed886ee6280f6b78137 Mon Sep 17 00:00:00 2001 From: Cenk Cetin Date: Mon, 14 Apr 2025 11:44:48 +0000 Subject: [PATCH 24/25] Fix initRBDyn parameter fetch using temporary synchronous client node. --- src/cartesian_impedance_controller_ros.cpp | 33 +++++++++++++++------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index edc3d5e..59311b5 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -381,24 +381,37 @@ namespace cartesian_impedance_controller { auto node = get_node(); auto logger = node->get_logger(); - - auto param_client = std::make_shared(node, "/robot_state_publisher"); - if (!param_client->wait_for_service(std::chrono::seconds(5))) + + if (robot_description_.empty()) { - RCLCPP_ERROR(logger, "Parameter service not available on /robot_state_publisher"); - return false; + RCLCPP_ERROR(logger, "'robot_description' parameter name is empty in controller config. Cannot initialize RBDyn."); + return false; } + RCLCPP_DEBUG(logger, "Attempting to get robot_description parameter named: '%s'", robot_description_.c_str()); std::string urdf_string; try { - auto response = param_client->get_parameters({robot_description_}).get(); - if (response.empty() || response[0].get_type() != rclcpp::PARAMETER_STRING) - { - RCLCPP_ERROR(logger, "Failed to retrieve 'robot_description' parameter"); - return false; + std::string temp_node_name = std::string(node->get_name()) + "_param_fetcher"; + auto temp_node = rclcpp::Node::make_shared(temp_node_name); + + auto param_client = std::make_shared(temp_node, "/robot_state_publisher"); + + RCLCPP_INFO(logger, "Waiting for parameter service '/robot_state_publisher'..."); + if (!param_client->wait_for_service(std::chrono::seconds(5))) { + RCLCPP_ERROR(logger, "Parameter service not available on /robot_state_publisher."); + return false; + } + + auto response = param_client->get_parameters({robot_description_}); + + if (response.empty() || response[0].get_type() == rclcpp::PARAMETER_NOT_SET) { + RCLCPP_ERROR(logger, "Parameter '%s' not set or not retrieved from '%s'.", + robot_description_.c_str(), "/robot_state_publisher"); + return false; } urdf_string = response[0].as_string(); + RCLCPP_DEBUG(logger, "Successfully retrieved URDF string (length: %zu)", urdf_string.length()); } catch (const std::exception &e) { From 29a248903309ad5060193998952cce60bceecddc Mon Sep 17 00:00:00 2001 From: Cenk Cetin <105711013+mrceki@users.noreply.github.com> Date: Mon, 7 Jul 2025 13:01:03 +0100 Subject: [PATCH 25/25] Update README.md --- README.md | 119 +++++++++++++++++++++++++++--------------------------- 1 file changed, 60 insertions(+), 59 deletions(-) diff --git a/README.md b/README.md index 58dc0b5..32a5cb0 100644 --- a/README.md +++ b/README.md @@ -127,65 +127,66 @@ When using the controller it is a good practice to describe the parameters in a Here is a template of what needs to be in that YAML file that can be adapted: ```YAML cartesian_impedance_controller: - type: cartesian_impedance_controller/CartesianImpedanceController - joints: - - fr3_joint1 - - fr3_joint2 - - fr3_joint3 - - fr3_joint4 - - fr3_joint5 - - fr3_joint6 - - fr3_joint7 - end_effector: fr3_hand_tcp - update_frequency: 500 - handle_trajectories: true - robot_description: "robot_description" - wrench_ee_frame: fr3_hand_tcp - delta_tau_max: 1.0 - - damping: - translation: - x: 1.0 - y: 1.0 - z: 1.0 - rotation: - x: 1.0 - y: 1.0 - z: 1.0 - nullspace_damping: 1.0 - update_damping_factors: false - - stiffness: - translation: - x: 200.0 - y: 200.0 - z: 200.0 - rotation: - x: 20.0 - y: 20.0 - z: 20.0 - nullspace_stiffness: 0.0 - update_stiffness: false - - wrench: - apply_wrench: false - force_x: 0.0 - force_y: 0.0 - force_z: 0.0 - torque_x: 0.0 - torque_y: 0.0 - torque_z: 0.0 - - filtering: - nullspace_config: 0.1 - pose: 0.1 - stiffness: 0.1 - wrench: 0.1 - - verbosity: - verbose_print: false - state_msgs: true - tf_frames: false + ros__parameters: + type: cartesian_impedance_controller/CartesianImpedanceController + joints: + - fr3_joint1 + - fr3_joint2 + - fr3_joint3 + - fr3_joint4 + - fr3_joint5 + - fr3_joint6 + - fr3_joint7 + end_effector: fr3_hand_tcp + update_frequency: 500 + handle_trajectories: true + robot_description: "robot_description" + wrench_ee_frame: fr3_hand_tcp + delta_tau_max: 1.0 + + damping: + translation: + x: 1.0 + y: 1.0 + z: 1.0 + rotation: + x: 1.0 + y: 1.0 + z: 1.0 + nullspace_damping: 1.0 + update_damping_factors: false + + stiffness: + translation: + x: 200.0 + y: 200.0 + z: 200.0 + rotation: + x: 20.0 + y: 20.0 + z: 20.0 + nullspace_stiffness: 0.0 + update_stiffness: false + + wrench: + apply_wrench: false + force_x: 0.0 + force_y: 0.0 + force_z: 0.0 + torque_x: 0.0 + torque_y: 0.0 + torque_z: 0.0 + + filtering: + nullspace_config: 0.1 + pose: 0.1 + stiffness: 0.1 + wrench: 0.1 + + verbosity: + verbose_print: false + state_msgs: true + tf_frames: false ``` ### Changing parameters with Dynamic Reconfigure