Merge branch 'master' into joss_paper

.github/workflows/build_code.yml

@@ -1,6 +1,6 @@
 name: CI
 
-on: [push]
+on: [push, pull_request]
 
 jobs:
   build-code:
@@ -29,4 +29,10 @@ jobs:
         catkin init
         catkin config --extend /opt/ros/$ROS_DISTRO
         catkin build
-        echo "Compile complete."
+        echo "Compile complete."
+    - name: Run tests
+      shell: bash
+      run: |
+        source /opt/ros/$ROS_DISTRO/setup.bash
+        cd catkin_ws
+        catkin test

CMakeLists.txt

@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
+cmake_policy(SET CMP0048 NEW) # Version not in project() command
 project(cartesian_impedance_controller)
 
 find_package(Boost 1.49 REQUIRED)
@@ -98,3 +99,21 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE)
+
+## Testing
+if (CATKIN_ENABLE_TESTING)
+  # Base library tests
+	find_package(rostest REQUIRED)
+  add_rostest_gtest(base_tests test/base.test test/base_tests.cpp)
+  target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES})
+
+  # ROS basic tests
+  add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp)
+  target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES})
+
+  # ROS functionality tests
+  add_rostest_gtest(ros_func_tests test/ros_func.test test/ros_func_tests.cpp)
+  target_link_libraries(ros_func_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES})
+endif()
+
+

README.md

@@ -3,7 +3,8 @@
 ## Description
 This project is an implementation of Cartesian impedance control for robotic manipulators. It is a type of control strategy that sets a dynamic relationship between contact forces and the position of a robot arm, making it suitable for collaborative robots. It is particularily useful when the interesting dimensions in the workspace are in the Cartesian space.
 
-The controller is developed using the seven degree-of-freedom (DoF) robot arm `LBR iiwa` by `KUKA AG` and has also been tested `Franka Emika Robot (Panda)` both in reality and simulation.
+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
@@ -26,21 +27,20 @@ http://www.youtube.com/watch?v=Q4aPm4O_9fY
 - 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 null-space of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector (`tau_ns`).
+- 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 commanded a wrench)
+- No built-in gravity compensation for tools or workpieces (can be achieved by commanding a wrench)
 
 ## Prerequisites
 ### Required
@@ -54,22 +54,23 @@ We use `RBDyn` to calculate forward kinematics and the Jacobian.
 - [mc_rbdyn_urdf](https://github.com/jrl-umi3218/mc_rbdyn_urdf)
 - [SpaceVecAlg](https://github.com/jrl-umi3218/SpaceVecAlg)
 
-The installation steps are automated in `scripts/install_dependencies.sh`:
+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.
 
-After cloning this repository in your catkin workspace, execute these commands:
+Here are the steps:
 
 ```bash
 cd catkin_ws
-src/cartesian_impedance_controller/scripts/install_dependencies.sh
+git clone https://github.com/matthias-mayr/Cartesian-Impedance-Controller src/Cartesian-Impedance-Controller
+src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh
 rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
-catkin build # or catkin_make
+catkin_make # or 'catkin build'
 source devel/setup.bash
 ```
 
-This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController`.
+This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController` in your `ros_control` configuration.
 
 ### Configuration file
 When using the controller it is a good practice to describe the parameters in a `YAML` file and load it. Usually this is already done by your robot setup - e.g. for [iiwa_ros](https://github.com/epfl-lasa/iiwa_ros/) that is [here](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_control/config/iiwa_control.yaml).
@@ -180,7 +181,7 @@ wrench:
 
 #### 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,1].
+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`:
@@ -189,11 +190,11 @@ When also setting the nullspace stiffnes, a custom messages of the type `cartesi
 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:
-  force: {x: 0.0, y: 0.0, z: 0.0}
-  torque: {x: 0.0, y: 0.0, z: 0.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: 0.1
+nullspace_damping_factor: 0.1
 q_d_nullspace: [0, 0, 0, 0, 0, 0, 0]" 
 ```
 
@@ -269,7 +270,23 @@ 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 and pull requests are welcome. Feel free to contact the main author at `firstname.lastname@cs.lth.se` if you are using this implementation.
+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 under submission at [JOSS](https://joss.theoj.org/). For now there is a [preprint on arXiv](https://arxiv.org/abs/2212.11215).<br>
+If you are using it or interacting with it, we would be happy if you could cite it.
+```bibtex
+@misc{https://doi.org/10.48550/arxiv.2212.11215,
+  doi = {10.48550/ARXIV.2212.11215},
+  url = {https://arxiv.org/abs/2212.11215},
+  author = {Mayr, Matthias and Salt-Ducaju, Julian M.},
+  keywords = {Robotics (cs.RO), FOS: Computer and information sciences, FOS: Computer and information sciences},
+  title = {A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators},
+  publisher = {arXiv},
+  year = {2022}, 
+  copyright = {Creative Commons Attribution Share Alike 4.0 International}
+}
+```
 
 ## Troubleshooting
 ### Compilation - A required package was not found
@@ -289,3 +306,16 @@ 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.

include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -73,7 +73,7 @@ namespace cartesian_impedance_controller
     * \param[in] r_z Rotational damping z
     * \param[in] n   Nullspace damping
     */
-    void setDamping(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n);
+    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
     *
@@ -206,12 +206,17 @@ namespace cartesian_impedance_controller
     double nullspace_damping_{0.0};           //!< Current nullspace damping
     double nullspace_damping_target_{0.0};    //!< Nullspace damping target
 
+    Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_{Eigen::Matrix<double, 6, 6>::Identity()}; //!< Cartesian stiffness target
+    Eigen::Matrix<double, 6, 6> cartesian_damping_target_{Eigen::Matrix<double, 6, 6>::Identity()};   //!< Cartesian damping target
+    Eigen::Matrix<double, 7, 1> damping_factors_{Eigen::Matrix<double, 7, 1>::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<double, 6, 1> 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
@@ -220,6 +225,10 @@ namespace cartesian_impedance_controller
     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<double, 6, 1> cartesian_wrench_{Eigen::Matrix<double, 6, 1>::Zero()};         //!< Current Cartesian wrench
+    Eigen::Matrix<double, 6, 1> cartesian_wrench_target_{Eigen::Matrix<double, 6, 1>::Zero()};  //!< Cartesian wrench target
+
     Eigen::VectorXd tau_c_; //!< Last commanded torques
 
     double update_frequency_{1000};               //!< Update frequency in Hz
@@ -273,16 +282,6 @@ namespace cartesian_impedance_controller
     /*! \brief Adds a percental filtering effect to the applied Cartesian wrench
     */
     void updateFilteredWrench();
-
-    Eigen::Matrix<double, 6, 1> error_; //!< Calculate pose error
-
-    Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_{Eigen::Matrix<double, 6, 6>::Identity()}; //!< Cartesian stiffness target
-    Eigen::Matrix<double, 6, 6> cartesian_damping_target_{Eigen::Matrix<double, 6, 6>::Identity()};   //!< Cartesian damping target
-    Eigen::Matrix<double, 7, 1> damping_factors_{Eigen::Matrix<double, 7, 1>::Ones()};                //!< Damping factors
-
-    //  External applied forces
-    Eigen::Matrix<double, 6, 1> cartesian_wrench_{Eigen::Matrix<double, 6, 1>::Zero()};         //!< Current Cartesian wrench
-    Eigen::Matrix<double, 6, 1> cartesian_wrench_target_{Eigen::Matrix<double, 6, 1>::Zero()};  //!< Cartesian wrench target
   };
 
-} // namespace cartesian_impedance_controller
+} // namespace cartesian_impedance_controller

include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h

@@ -151,7 +151,7 @@ namespace cartesian_impedance_controller
     * \param[in] cart_damping   Cartesian damping [0,1]
     * \param[in] nullspace      Nullspace damping [0,1]
     */
-    void setDamping(const geometry_msgs::Wrench &cart_damping, double nullspace);
+    void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace);
 
     /*! \brief Sets Cartesian and nullspace stiffness
     *
@@ -164,11 +164,11 @@ namespace cartesian_impedance_controller
 
     /*! \brief Message callback for Cartesian damping.
     *
-    * Calls setDamping function.
-    * @sa setDamping.
+    * Calls setDampingFactors function.
+    * @sa setDampingFactors.
     * \param[in] msg Received message
     */
-    void cartesianDampingCb(const geometry_msgs::WrenchConstPtr &msg);
+    void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg);
 
     /*! \brief Message callback for Cartesian stiffness.
     *
@@ -181,7 +181,7 @@ namespace cartesian_impedance_controller
     /*! \brief Message callback for the whole controller configuration.
     *
     * Sets stiffness, damping and nullspace.
-    * @sa setDamping, setStiffness
+    * @sa setDampingFactors, setStiffness
     * \param[in] msg Received message
     */
     void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg);
@@ -289,8 +289,8 @@ namespace cartesian_impedance_controller
 
     ros::Subscriber sub_cart_stiffness_;    //!< Cartesian stiffness subscriber
     ros::Subscriber sub_cart_wrench_;       //!< Cartesian wrench subscriber
-    ros::Subscriber sub_damping_;           //!< Damping subscriber
-    ros::Subscriber sub_impedance_config_;  //!< Controller configuration 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
@@ -303,8 +303,8 @@ namespace cartesian_impedance_controller
     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_min_{0.001};       //!< Minimum damping
-    const double dmp_max_{1.0};         //!< Maximum damping
+    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_ =

msg/ControllerConfig.msg

@@ -2,17 +2,18 @@
 # Values: Translation: [0.0, 2000.0] in [N/m]; Rotation [0.0; 500.0] in [Nm/rad]
 geometry_msgs/Wrench cartesian_stiffness
 
-# Damping values for translation and rotation of the axes x,y,z
-# Values: [0.0, 1.0]
-geometry_msgs/Wrench cartesian_damping
+# Damping factor for translation and rotation of the axes x,y,z
+# The rule is always 2*sqrt(stiffness)
+# Values: [0.001, 2.0]
+geometry_msgs/Wrench cartesian_damping_factors
 
 # Stiffness value for nullspace
 # Values: >= 0.0 in [Nm/rad]
 float64 nullspace_stiffness
 
 # Damping parameter for nullspace [Nm*s/rad]. 
-# Value: [0.3, 1.0]; A good damping value is 0.7. 
-float64 nullspace_damping
+# Value: [0.001, 2.0]
+float64 nullspace_damping_factor
 
 # The desired nullspace configuration
 # Values: According to the limits of the robot

package.xml

@@ -1,14 +1,15 @@
-<package>
+<package format="3">
   <name>cartesian_impedance_controller</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>
     A Cartesian Impedance controller implementation
   </description>
   <maintainer email="matthias.mayr@cs.lth.se">Matthias Mayr</maintainer>
-  <license>BSD</license>
+  <license>BSD-3-Clause</license>
 
   <author>Matthias Mayr</author>
   <author>Oussama Chouman</author>
+  <author>Julian Salt Ducaju</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
@@ -32,27 +33,37 @@
   <build_depend>tf_conversions</build_depend>
   <build_depend>trajectory_msgs</build_depend>
 
-  <run_depend>actionlib</run_depend>
-  <run_depend>actionlib_msgs</run_depend>
-  <run_depend>control_msgs</run_depend>
-  <run_depend>controller_interface</run_depend>
-  <run_depend>controller_manager</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>eigen_conversions</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>hardware_interface</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>pluginlib</run_depend>
-  <run_depend>realtime_tools</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>tf_conversions</run_depend>
-  <run_depend>trajectory_msgs</run_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>controller_interface</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>eigen_conversions</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>hardware_interface</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>realtime_tools</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>tf_conversions</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>yaml-cpp</exec_depend>
 
-   <export>
+  <export>
     <!-- Other tools can request additional information be placed here -->
     <controller_interface plugin="${prefix}/controller_plugins.xml"/>
   </export>
+
+  <test_depend>gtest</test_depend>
+  <test_depend>eigen</test_depend>
+  <test_depend>roscpp</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>geometry_msgs</test_depend>
+  <test_depend>std_msgs</test_depend>
+  <test_depend>dynamic_reconfigure</test_depend>
+  <test_depend>yaml-cpp</test_depend>
 </package>