Description
This project is an implementation of Cartesian impedance control for robotic manipulators. It is a type of control strategy that sets a dynamic relationship between contact forces and the position of a robot arm, making it suitable for collaborative robots. It is particularily useful when the interesting dimensions in the workspace are in the Cartesian space.
The controller is developed using the seven degree-of-freedom (DoF) robot arm LBR iiwa by KUKA AG and has also been tested with the Franka Emika Robot (Panda) both in reality and simulation. This controller is used and tested with ROS 1 melodic and noetic.
The implementation consists of a
- base library that has few dependencies and can e.g. be directly integrated into software such as the DART simulator and a
- ROS control integration on top of it.
Short Pitch at ROSCon:
http://www.youtube.com/watch?v=Q4aPm4O_9fY
Features
- Configurable stiffness values along all Cartesian dimensions at runtime
- Configurable damping factors along all Cartesian dimensions at runtime
- Change reference pose at runtime
- Apply Cartesian forces and torques at runtime
- Optional filtering of stiffnesses, pose and wrenches for smoother operation
- Handling of joint trajectories with nullspace configurations, e.g. from MoveIt
- Jerk limitation
- Separate base library that can be integrated in non-ROS environments
- Interface to ROS messages and dynamic_reconfigure for easy runtime configuration
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
ROS Controller
We use RBDyn to calculate forward kinematics and the Jacobian.
The installation steps for the installation of the non-ROS dependencies are automated in scripts/install_dependencies.sh.
Controller Usage in ROS
Assuming that there is an initialized catkin workspace you can clone this repository, install the dependencies and compile the controller.
Here are the steps:
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 that is here. Here is a template of what needs to be in that YAML file that can be adapted:
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, this can be achieved with command:
Changing parameters with Dynamic Reconfigure
If it is not deactivated, 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:
There are several entries:
cartesian_wrench_reconfiguredamping_factors_reconfigurestiffness_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=/<your_namespace>.
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:
Most often one wants to have a controlled way to set reference poses. Once can for example use a trajectory generator for Cartesian trajectories.
Cartesian Stiffness
In order to set only the Cartesian stiffnesses, once can send a geometry_msgs/WrenchStamped to set_cartesian_stiffness:
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:
q_d_nullspace is the nullspace joint configuration, so the joint configuration that should be achieved if the nullspace allows it. Note that the end-effector pose would deviate if the forward kinematics of this joint configuration do not overlap with it.
Cartesian Wrenches
A Cartesian wrench can be commanded by sending a geometry_msgs/WrenchStamped to the topic ${controller_ns}/set_cartesian_wrench. Internally the wrenches are applied in the root frame. Therefore wrench messages are transformed into the root frame using tf.
Note: An empty field frame_id is interpreted as end-effector frame since this is the most applicable one when working with a manipulator.
Note: The wrenches are transformed into the root frame when they arrive, but not after that. E.g. end-effector wrenches might need to get updated.
Trajectories and MoveIt
If handle_trajectories is not disabled, the controller can also execute trajectories. An action server is spun up at ${controller_ns}/follow_joint_trajectory and a fire-and-forget topic for the message type trajectory_msgs/JointTrajectory is at ${controller_ns}/joint_trajectory.
In order to use it with MoveIt its controller configuration (example in iiwa_ros) needs to look somewhat like this:
Note: A nullspace stiffness needs to be specified so that the arm also follows the joint configuration and not just the end-effector pose.
Safety
We have used the controller with Cartesian translational stiffnesses of up to 1000 N/m and experienced it as very stable. It is also stable in singularities.
One additional measure can be to limit the maximum joint torques that can be applied by the robot arm in the URDF. On our KUKA iiwas we limit the maximum torque of each joint to 20 Nm, which allows a human operator to easily interfere at any time just by grabbing the arm and moving it.
When using iiwa_ros, these limits can be applied here. For the Panda they are applied here. 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:
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. If you are using it or interacting with it, we would appreciate a citation:
Troubleshooting
Compilation - A required package was not found
catkin build shows this CMake Error:
There are missing dependencies. When replacing catkin_ws with your workspace, they can be resolved like this :
RBDyn Library not found
When starting the controller, this message appears:
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.