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 + diff --git a/CMakeLists.txt b/CMakeLists.txt index 47ce961..311bfa6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,119 +1,103 @@ -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 controller_interface - controller_manager - dynamic_reconfigure - eigen_conversions geometry_msgs hardware_interface - message_generation - pluginlib + rclcpp_lifecycle + rclcpp_action realtime_tools - roscpp - sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros std_msgs - tf - tf_conversions trajectory_msgs + generate_parameter_library + urdf + RBDyn + mc_rbdyn_urdf + Eigen3 + pluginlib ) -add_message_files(FILES - ControllerConfig.msg - ControllerState.msg +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(rosidl_default_generators REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ControllerConfig.msg" + "msg/ControllerState.msg" + DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) -generate_messages( -DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs -) +set(library_name cartesian_impedance_controller_lib) -generate_dynamic_reconfigure_options( - cfg/damping.cfg - cfg/stiffness.cfg - cfg/wrench.cfg +generate_parameter_library(cartesian_impedance_controller_parameters + src/cartesian_impedance_controller_parameters.yaml ) -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 +add_library(${library_name} SHARED + src/cartesian_impedance_controller_ros.cpp + src/cartesian_impedance_controller.cpp ) -########### -## Build ## -########### - -# 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) - -# Core library -add_library(${PROJECT_NAME} src/cartesian_impedance_controller.cpp) -add_dependencies( - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) +target_compile_features(${library_name} PUBLIC cxx_std_17) -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 +target_include_directories(${library_name} PUBLIC + $ + $ + $ +) +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + +target_link_libraries(${library_name} PUBLIC + "${cpp_typesupport_target}" + cartesian_impedance_controller_parameters + Eigen3::Eigen + RBDyn + mc_rbdyn_urdf::mc_rbdyn_urdf ) -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) +pluginlib_export_plugin_description_file(controller_interface cartesian_impedance_controller_plugin.xml) -## 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}) +ament_target_dependencies(${library_name} PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) - # 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}) +install( + DIRECTORY include/ + DESTINATION include/cartesian_impedance_controller +) - # 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() +install( + TARGETS ${library_name} cartesian_impedance_controller_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() 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/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 index 9f2beb0..32a5cb0 100644 --- a/README.md +++ b/README.md @@ -1,26 +1,31 @@ # 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/) +[![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 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. +### This project is a migration of ROS 1 implementation. You can find the original repo [here.](https://github.com/matthias-mayr/Cartesian-Impedance-Controller) -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. +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 @@ -30,26 +35,26 @@ http://www.youtube.com/watch?v=Q4aPm4O_9fY - 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. @@ -59,118 +64,151 @@ We use `RBDyn` to calculate forward kinematics and the Jacobian. - [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. - + +## 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: +### Local Machine Installation ```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 +mkdir -p ~/ros2_ws/src +cd ~/ros2_ws +git clone https://github.com/mrceki/Cartesian-Impedance-Controller src/Cartesian-Impedance-Controller +bash 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 +colcon build +source install/setup.bash +``` +### Docker Installation + +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 . ``` -This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController` in your `ros_control` configuration. +### 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 -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). +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 -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 +cartesian_impedance_controller: + 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 ``` - -### 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: +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 -rosrun rqt_reconfigure rqt_reconfigure +ros2 run 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`: - + +In order to set only the Cartesian stiffnesses, once can send a `geometry_msgs/msgs/WrenchStamped` to `set_cartesian_stiffness`: + ```bash -rostopic pub --once /${controller_ns}/set_cartesian_stiffness geometry_msgs/WrenchStamped "header: - seq: 0 +ros2 topic pub --once /set_cartesian_stiffness geometry_msgs/msg/WrenchStamped "header: stamp: - secs: 0 - nsecs: 0 + sec: 0 + nanosec: 0 frame_id: '' wrench: force: @@ -182,41 +220,23 @@ wrench: 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. - + +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/WrenchStamped` to the topic `${controller_ns}/set_cartesian_wrench`. + +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 -rostopic pub --once /${controller_ns}/set_cartesian_wrench geometry_msgs/WrenchStamped "header: - seq: 0 +ros2 topic pub --once /set_cartesian_wrench geometry_msgs/msg/WrenchStamped "header: stamp: - secs: 0 - nsecs: 0 + sec: 0 + nanosec: 0 frame_id: '' wrench: force: @@ -228,61 +248,45 @@ wrench: 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`. - + +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: ${controller_ns} + - name: cartesian_impedance_controller 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 + - 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 + +## 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. - -## 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). +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}, @@ -296,35 +300,3 @@ Mayr et al., (2024). A C++ Implementation of a Cartesian Impedance Controller fo 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..8cb96ab --- /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.h deleted file mode 100644 index 969333b..0000000 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.h +++ /dev/null @@ -1,287 +0,0 @@ -#pragma once - -#include -#include - -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 diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp new file mode 100644 index 0000000..9fce59e --- /dev/null +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp @@ -0,0 +1,293 @@ +#pragma once + +#include +#include + +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 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..9328b4c --- /dev/null +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.hpp @@ -0,0 +1,152 @@ +#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 +#include +#include +#include +#include + +#include +#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: + 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/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/ControllerState.msg b/msg/ControllerState.msg index b81e2df..7440262 100644 --- a/msg/ControllerState.msg +++ b/msg/ControllerState.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header geometry_msgs/Pose current_pose geometry_msgs/Pose reference_pose diff --git a/package.xml b/package.xml index ceff515..d73b7df 100644 --- a/package.xml +++ b/package.xml @@ -1,69 +1,52 @@ + 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 - gtest - eigen - roscpp - rostest + action_msgs + control_msgs + controller_interface + eigen3_cmake_module + geometry_msgs + hardware_interface + pluginlib + realtime_tools + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_ros + trajectory_msgs + rclcpp_lifecycle + generate_parameter_library + rosidl_default_runtime + rosidl_typesupport_cpp + + + rosidl_interface_packages + + + ament_cmake + + diff --git a/scripts/install_dependencies.sh b/scripts/install_dependencies.sh index 87bc9cd..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 @@ -11,6 +11,7 @@ then mkdir depends fi cd depends +touch COLCON_IGNORE # SpaceVecAlg git clone --recursive https://github.com/jrl-umi3218/SpaceVecAlg diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index 14615b0..aa9c124 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -1,4 +1,4 @@ -#include +#include #include "pseudo_inversion.h" #include @@ -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_parameters.yaml b/src/cartesian_impedance_controller_parameters.yaml new file mode 100644 index 0000000..686ca74 --- /dev/null +++ b/src/cartesian_impedance_controller_parameters.yaml @@ -0,0 +1,227 @@ +cartesian_impedance_controller: + delta_tau_max: + type: double + default_value: 1.0 + description: "Maximum allowed change in joint torques." + validation: + bounds<>: [0.0, 1.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: "" + description: "Name of the end effector link." + + robot_description: + type: string + default_value: "robot_description" + description: "Name of the robot description parameter." + + damping: + translation: + x: + type: double + default_value: 1.0 + description: "Translational damping factor for the x-axis." + validation: + bounds<>: [0.0, 1.0] + y: + type: double + default_value: 1.0 + description: "Translational damping factor for the y-axis." + validation: + bounds<>: [0.0, 1.0] + z: + type: double + default_value: 1.0 + description: "Translational damping factor for the z-axis." + validation: + bounds<>: [0.0, 1.0] + + rotation: + x: + type: double + default_value: 1.0 + description: "Rotational damping factor around the x-axis." + validation: + bounds<>: [0.0, 1.0] + y: + type: double + default_value: 1.0 + description: "Rotational damping factor around the y-axis." + validation: + bounds<>: [0.0, 1.0] + z: + type: double + default_value: 1.0 + description: "Rotational damping factor around the z-axis." + validation: + bounds<>: [0.0, 1.0] + + nullspace_damping: + type: double + default_value: 1.0 + description: "Damping factor for the nullspace." + validation: + bounds<>: [0.0, 1.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: 200.0 + description: "Translational stiffness for the x-axis." + validation: + bounds<>: [0.0, 2000.0] + y: + type: double + default_value: 200.0 + description: "Translational stiffness for the y-axis." + validation: + bounds<>: [0.0, 2000.0] + z: + type: double + default_value: 200.0 + description: "Translational stiffness for the z-axis." + validation: + bounds<>: [0.0, 2000.0] + + rotation: + x: + type: double + default_value: 20.0 + description: "Rotational stiffness around the x-axis." + validation: + bounds<>: [0.0, 300.0] + y: + type: double + default_value: 20.0 + description: "Rotational stiffness around the y-axis." + validation: + bounds<>: [0.0, 300.0] + z: + type: double + default_value: 20.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, 50.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_config: + type: double + default_value: 0.1 + description: "Filtering factor for nullspace." + validation: + bounds<>: [0.0, 1.0] + stiffness: + type: double + default_value: 0.1 + description: "Filtering factor for stiffness." + validation: + bounds<>: [0.0, 1.0] + pose: + type: double + default_value: 0.1 + description: "Filtering factor for pose." + validation: + bounds<>: [0.0, 1.0] + wrench: + type: double + default_value: 0.1 + 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 + + 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 6354ff0..59311b5 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -1,272 +1,552 @@ -#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 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) { - return std::min(std::max(x, x_min), x_max); + // The Eigen vectors will be resized in on_configure. + q_.resize(0); + dq_.resize(0); + tau_m_.resize(0); + tau_c_.resize(0); } - /*! \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) + CartesianImpedanceControllerRos::~CartesianImpedanceControllerRos() { - 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); } - bool CartesianImpedanceControllerRos::initDynamicReconfigure(const ros::NodeHandle &nh) + CallbackReturn CartesianImpedanceControllerRos::on_init() { - 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)); - - 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)); + auto node = get_node(); + auto logger = node->get_logger(); - 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; + 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; } - bool CartesianImpedanceControllerRos::initJointHandles(hardware_interface::EffortJointInterface *hw, const ros::NodeHandle &nh) + controller_interface::InterfaceConfiguration CartesianImpedanceControllerRos::command_interface_configuration() const { - std::vector joint_names; - if (!nh.getParam("joints", joint_names)) + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &jn : params_.joints) { - ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); - return false; + conf.names.push_back(jn + "/" + hardware_interface::HW_IF_EFFORT); } - for (size_t i = 0; i < joint_names.size(); ++i) + 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) { - 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; - } + 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); } - ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); - this->setNumberOfJoints(joint_names.size()); - return true; + return conf; } - bool CartesianImpedanceControllerRos::initMessaging(ros::NodeHandle *nh) + CallbackReturn CartesianImpedanceControllerRos::on_configure(const rclcpp_lifecycle::State &) { - // 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; + auto node = get_node(); + auto logger = node->get_logger(); + + if (!parameter_handler_) + { + 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(); + + 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; + } + + 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; } - bool CartesianImpedanceControllerRos::initRBDyn(const ros::NodeHandle &nh) + CallbackReturn CartesianImpedanceControllerRos::on_activate(const rclcpp_lifecycle::State &) { - // 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)) + 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_) { - ROS_INFO_ONCE("Waiting for robot description in parameter %s on the ROS param server.", - robot_description_.c_str()); - usleep(100000); + RCLCPP_ERROR(logger, "Size mismatch in internal vectors (q_, dq_, tau_m_, tau_c_) vs. dof_"); + return CallbackReturn::ERROR; } - try + + for (const auto &interface : state_interfaces) { - this->rbdyn_wrapper_.init_rbdyn(urdf_string, end_effector_); + 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; } - catch (std::runtime_error e) + + for (auto &interface : command_interfaces) { - ROS_ERROR("Error when intializing RBDyn: %s", e.what()); - return false; + 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; } - ROS_INFO_STREAM("Number of joints found in urdf: " << this->rbdyn_wrapper_.n_joints()); - if (this->rbdyn_wrapper_.n_joints() < this->n_joints_) + + update(rclcpp::Time(0), rclcpp::Duration(0, 0)); + initDesiredPose(position_, orientation_); + initNullspaceConfig(q_); + for (size_t i = 0; i < dof_; i++) { - 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; + if (!joint_command_handles_[i]) + { + RCLCPP_ERROR(logger, "Command handle for joint %zu is null!", i); + return CallbackReturn::ERROR; + } } - else if (this->rbdyn_wrapper_.n_joints() > this->n_joints_) + 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_)) { - 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_); + RCLCPP_INFO(get_node()->get_logger(), "Parameters are outdated. Refreshing..."); + parameter_handler_->refresh_dynamic_parameters(); + params_ = parameter_handler_->get_params(); + applyRuntimeParameters(); } - return true; + + if (traj_running_) + trajUpdate(); + + read_state_from_hardware(); + calculateCommandedTorques(); // Populates tau_c_ + write_command_to_hardware(); + publishMsgsAndTf(); + + return controller_interface::return_type::OK; } - bool CartesianImpedanceControllerRos::initTrajectories(ros::NodeHandle *nh) + CallbackReturn CartesianImpedanceControllerRos::on_cleanup(const rclcpp_lifecycle::State &) { - 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; + return CallbackReturn::SUCCESS; } - bool CartesianImpedanceControllerRos::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &node_handle) + CallbackReturn CartesianImpedanceControllerRos::on_error(const rclcpp_lifecycle::State &) { - 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 CallbackReturn::SUCCESS; + } + + 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; + 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()); + } + } + + bool CartesianImpedanceControllerRos::initRBDyn() + { + auto node = get_node(); + auto logger = node->get_logger(); + + if (robot_description_.empty()) + { + 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 + { + 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) + { + RCLCPP_ERROR(logger, "Exception while retrieving robot_description: %s", e.what()); return false; } - if (enable_trajectories && !this->initTrajectories(&node_handle)) + + if (urdf_string.empty()) { + RCLCPP_ERROR(logger, "Retrieved empty 'robot_description'"); return false; } - this->root_frame_ = this->rbdyn_wrapper_.root_link(); - node_handle.setParam("root_frame", this->root_frame_); - // Initialize base_tools and member variables - this->setNumberOfJoints(this->joint_handles_.size()); - if (this->n_joints_ < 6) + // Initialize RBDyn with the URDF + try { - ROS_WARN("Number of joints is below 6. Functions might be limited."); + 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()); } - if (this->n_joints_ < 7) + catch (const std::exception &e) { - ROS_WARN("Number of joints is below 7. No redundant joint for nullspace."); + RCLCPP_ERROR(logger, "Error initializing RBDyn: %s", e.what()); + return false; } - this->tau_m_ = Eigen::VectorXd(this->n_joints_); - // Needs to be after base_tools init since the wrench callback calls it - if (dynamic_reconfigure && !this->initDynamicReconfigure(node_handle)) + 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; } - ROS_INFO("Finished initialization."); return true; } - void CartesianImpedanceControllerRos::starting(const ros::Time & /*time*/) + void CartesianImpedanceControllerRos::read_state_from_hardware() { - this->updateState(); - - // 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"); - } - - void CartesianImpedanceControllerRos::update(const ros::Time & /*time*/, const ros::Duration &period /*period*/) - { - if (this->traj_running_) + if (q_.size() != dof_ || dq_.size() != dof_ || tau_m_.size() != dof_) { - trajUpdate(); + RCLCPP_ERROR(get_node()->get_logger(), "Mismatched Eigen vector sizes in read_state_from_hardware."); + return; } - this->updateState(); - - // Apply control law in base library - this->calculateCommandedTorques(); - - // Write commands - for (size_t i = 0; i < this->n_joints_; ++i) + for (size_t i = 0; i < dof_; i++) { - this->joint_handles_[i].setCommand(this->tau_c_(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(); } - publishMsgsAndTf(); + 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) const + 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 the URDF contains more joints than there are controlled, only the state of the controlled ones are known + + if (q.size() != this->n_joints_) + { + 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; - ee_state = this->rbdyn_wrapper_.perform_fk(q_rb); + 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 { - ee_state = this->rbdyn_wrapper_.perform_fk(q); + 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; + } } + *position = ee_state.translation; *orientation = ee_state.orientation; + RCLCPP_DEBUG(logger, "position: %f, %f, %f", position->x(), position->y(), position->z()); return true; } bool CartesianImpedanceControllerRos::getJacobian(const Eigen::VectorXd &q, const Eigen::VectorXd &dq, Eigen::MatrixXd *jacobian) { - // If the URDF contains more joints than there are controlled, only the state of the controlled ones are known + 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()); @@ -283,316 +563,389 @@ namespace cartesian_impedance_controller return true; } - void CartesianImpedanceControllerRos::updateState() + void CartesianImpedanceControllerRos::controllerConfigCb( + const cartesian_impedance_controller::msg::ControllerConfig::SharedPtr msg) { - for (size_t i = 0; i < this->n_joints_; ++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 { - 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(); + RCLCPP_WARN(get_node()->get_logger(), "Nullspace configuration has wrong dimension: got %zu, expected %zu", + msg->q_d_nullspace.size(), dof_); } - getJacobian(this->q_, this->dq_, &this->jacobian_); - getFk(this->q_, &this->position_, &this->orientation_); } - void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg) + void CartesianImpedanceControllerRos::publishMsgsAndTf() { - this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); - this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); - - if (msg->q_d_nullspace.size() == this->n_joints_) + if (rt_pub_torques_ && rt_pub_torques_->trylock()) { - Eigen::VectorXd q_d_nullspace(this->n_joints_); - for (size_t i = 0; i < this->n_joints_; i++) + rt_pub_torques_->msg_.data.resize(tau_c_.size()); + for (size_t i = 0; i < tau_c_.size(); ++i) { - q_d_nullspace(i) = msg->q_d_nullspace.at(i); + rt_pub_torques_->msg_.data[i] = tau_c_(i); } - this->setNullspaceConfig(q_d_nullspace); + rt_pub_torques_->unlockAndPublish(); } - else + + Eigen::Matrix error = getPoseError(); + + if (params_.verbosity.verbose_print) { - ROS_WARN_STREAM("Nullspace configuration does not have the correct amount of entries. Got " << msg->q_d_nullspace.size() << " expected " << this->n_joints_ << ". Ignoring."); - } - } + 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)); - void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg) - { - this->setDampingFactors(*msg, this->damping_factors_[6]); - } + 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)); - void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg) - { - if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) + 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) { - ROS_WARN_STREAM("Reference poses need to be in the root frame '" << this->root_frame_ << "'. Ignoring."); - return; + 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; + } } - 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 (params_.verbosity.state_msgs) { - this->orientation_d_.coeffs() << -this->orientation_d_.coeffs(); + 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(); + } } - this->setReferencePose(position_d, orientation_d); } - void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::WrenchStampedConstPtr &msg) + void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::msg::Wrench::SharedPtr msg) { - this->setStiffness(msg->wrench, this->nullspace_stiffness_target_); + 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::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace) + void CartesianImpedanceControllerRos::cartesianStiffnessCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - 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_)); + 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::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping) + void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - 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); + 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; + } + + 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; + } + + 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::wrenchCommandCb(const geometry_msgs::WrenchStampedConstPtr &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; - - if (!msg->header.frame_id.empty() && msg->header.frame_id != this->root_frame_) - { - if (!transformWrench(&F, msg->header.frame_id, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } - } - else if (msg->header.frame_id.empty()) + std::string from_frame = msg->header.frame_id.empty() ? wrench_ee_frame_ : msg->header.frame_id; + if (!transformWrench(&F, from_frame, root_frame_)) { - if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) - { - ROS_ERROR("Could not transform wrench. Not applying it."); - return; - } + RCLCPP_ERROR(get_node()->get_logger(), "Could not transform wrench. Not applying it."); + return; } - this->applyWrench(F); + applyWrench(F); } - bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix *cartesian_wrench, + bool CartesianImpedanceControllerRos::transformWrench(Eigen::Matrix *wrench, const std::string &from_frame, const std::string &to_frame) const { + geometry_msgs::msg::TransformStamped transform; 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; + transform = tf_buffer_->lookupTransform(to_frame, from_frame, tf2::TimePointZero); } - catch (const tf::TransformException &ex) + catch (const tf2::TransformException &ex) { - ROS_ERROR_THROTTLE(1, "%s", ex.what()); + 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::publishMsgsAndTf() + void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) { - // publish commanded torques - if (this->pub_torques_.trylock()) + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory from topic"); + if (traj_as_active_) { - 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()}; - - 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++; + RCLCPP_INFO(get_node()->get_logger(), "Preempting running action server goal due to new trajectory"); + traj_running_ = false; } + trajStart(*msg); + rt_trajectory_->writeFromNonRT(*msg); } - // Dynamic reconfigure - // -------------------------------------------------------------------------------------------------------------------------------------- - void CartesianImpedanceControllerRos::dynamicStiffnessCb( - cartesian_impedance_controller::stiffnessConfig &config, uint32_t level) + rclcpp_action::GoalResponse CartesianImpedanceControllerRos::trajGoalCb( + const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { - if (config.update_stiffness) + RCLCPP_INFO(get_node()->get_logger(), "Received FollowJointTrajectory action goal request"); + if (goal->trajectory.points.empty() || goal->trajectory.joint_names.size() != dof_) { - 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); + 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; } - void CartesianImpedanceControllerRos::dynamicDampingCb( - cartesian_impedance_controller::dampingConfig &config, uint32_t level) + rclcpp_action::CancelResponse CartesianImpedanceControllerRos::trajCancelCb( + const std::shared_ptr>) { - 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); - } + RCLCPP_INFO(get_node()->get_logger(), "Canceling trajectory action"); + traj_running_ = false; + return rclcpp_action::CancelResponse::ACCEPT; } - void CartesianImpedanceControllerRos::dynamicWrenchCb(cartesian_impedance_controller::wrenchConfig &config, - uint32_t level) + void CartesianImpedanceControllerRos::trajAcceptCb( + std::shared_ptr> goal_handle) { - 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); - } + auto goal = goal_handle->get_goal(); + auto logger = get_node()->get_logger(); + RCLCPP_INFO(logger, "Accepted FollowJointTrajectory action goal"); - void CartesianImpedanceControllerRos::trajCb(const trajectory_msgs::JointTrajectoryConstPtr &msg) - { - 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); - } + traj_as_active_ = true; + traj_as_goal_ = goal_handle; - void CartesianImpedanceControllerRos::trajGoalCb() - { - this->traj_as_goal_ = this->traj_as_->acceptNewGoal(); - ROS_INFO("Accepted new goal from action server."); - trajStart(this->traj_as_goal_->trajectory); + trajStart(goal->trajectory); + rt_trajectory_->writeFromNonRT(goal->trajectory); } - void CartesianImpedanceControllerRos::trajPreemptCb() + void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::msg::JointTrajectory &trajectory) { - ROS_INFO("Actionserver got preempted."); - this->traj_as_->setPreempted(); - } + trajectory_ = trajectory; + traj_index_ = 0; + traj_running_ = true; + traj_start_time_ = get_node()->now(); - void CartesianImpedanceControllerRos::trajStart(const trajectory_msgs::JointTrajectory &trajectory) - { - 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.) + 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 { - ROS_WARN("Nullspace stiffness is low. The joints might not follow the planned path."); + RCLCPP_WARN(get_node()->get_logger(), "Empty trajectory. Not running."); + traj_running_ = false; } } void CartesianImpedanceControllerRos::trajUpdate() { - if (ros::Time::now() > (this->traj_start_ + trajectory_.points.at(this->traj_index_).time_from_start)) + auto now = get_node()->now(); + double t_since_start = (now - traj_start_time_).seconds(); + + if (t_since_start > traj_duration_.seconds()) { - // 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_) + RCLCPP_INFO(get_node()->get_logger(), "Finished executing trajectory."); + traj_running_ = false; + if (traj_as_active_ && traj_as_goal_) { - ROS_INFO_STREAM("Index " << this->traj_index_ << " q_nullspace: " << q.transpose()); + auto result = std::make_shared(); + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + traj_as_goal_->succeed(result); + traj_as_active_ = false; } - // Update end-effector pose and nullspace - getFk(q, &this->position_d_target_, &this->orientation_d_target_); - this->setNullspaceConfig(q); - this->traj_index_++; + return; } - if (ros::Time::now() > (this->traj_start_ + this->traj_duration_)) + if (now > (traj_start_time_ + rclcpp::Duration(trajectory_.points.at(traj_index_).time_from_start))) { - ROS_INFO_STREAM("Finished executing trajectory."); - if (this->traj_as_->isActive()) - { - this->traj_as_->setSucceeded(); - } - this->traj_running_ = false; + 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