CAUTION: Please be aware that this project was deployed on the real robot, but is still in progress!
This project is a migration of ROS 1 implementation. You can find the original repo here.
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
- 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_controlinterface - ROS 2 control integration on top of it.
http://www.youtube.com/watch?v=Q4aPm4O_9fY
- 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
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).
- 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)
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.
Assuming that there is an initialized colcon workspace you can clone this repository, install the dependencies and compile the controller.
Here are the steps:
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
colcon build
source install/setup.bashTo build and run the Docker container for the Cartesian Impedance Controller, follow these steps:
- Navigate to the directory containing the
Dockerfile. - Build the Docker image:
docker build -t cartesian_impedance_controller:latest .- Start a container from the built image:
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=$DISPLAYand-v /tmp/.X11-unix:/tmp/.X11-unix: Enable GUI applications (e.g.,rviz2) to run inside the container.
If you want to include Franka ROS 2 installation inside docker, build the docker image with the BUILD_FRANKA_ROS argument set to true:
docker build --build-arg BUILD_FRANKA_ROS=true -t cartesian_impedance_controller:franka .Once inside the container, you can access the ROS 2 workspace at /ros2_ws. To build the workspace or source it:
source /opt/ros/humble/setup.bash
cd /ros2_ws
colcon build
source install/setup.bashThis allows you to add a controller configuration for the controller type cartesian_impedance_controller/CartesianImpedanceController in your ros2_control configuration.
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:
cartesian_impedance_controller:
type: cartesian_impedance_controller/CartesianImpedanceController
joints:
- fr3_joint1
- fr3_joint2
- fr3_joint3
- fr3_joint4
- fr3_joint5
- fr3_joint6
- fr3_joint7
end_effector: fr3_hand_tcp
update_frequency: 500
handle_trajectories: true
robot_description: "robot_description"
wrench_ee_frame: fr3_hand_tcp
delta_tau_max: 1.0
damping:
translation:
x: 1.0
y: 1.0
z: 1.0
rotation:
x: 1.0
y: 1.0
z: 1.0
nullspace_damping: 1.0
update_damping_factors: false
stiffness:
translation:
x: 200.0
y: 200.0
z: 200.0
rotation:
x: 20.0
y: 20.0
z: 20.0
nullspace_stiffness: 0.0
update_stiffness: false
wrench:
apply_wrench: false
force_x: 0.0
force_y: 0.0
force_z: 0.0
torque_x: 0.0
torque_y: 0.0
torque_z: 0.0
filtering:
nullspace_config: 0.1
pose: 0.1
stiffness: 0.1
wrench: 0.1
verbosity:
verbose_print: false
state_msgs: true
tf_frames: falseThe 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:
ros2 run rqt_reconfigure rqt_reconfigureFor 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.
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.
In order to set only the Cartesian stiffnesses, once can send a geometry_msgs/msgs/WrenchStamped to set_cartesian_stiffness:
ros2 topic pub --once /set_cartesian_stiffness geometry_msgs/msg/WrenchStamped "header:
stamp:
sec: 0
nanosec: 0
frame_id: ''
wrench:
force:
x: 300.0
y: 300.0
z: 300.0
torque:
x: 30.0
y: 30.0
z: 30.0"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).
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.
ros2 topic pub --once /set_cartesian_wrench geometry_msgs/msg/WrenchStamped "header:
stamp:
sec: 0
nanosec: 0
frame_id: ''
wrench:
force:
x: 0.0
y: 0.0
z: 5.0
torque:
x: 0.0
y: 0.0
z: 0.0"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) needs to look somewhat like this:
controller_list:
- name: cartesian_impedance_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- fr3_joint1
- fr3_joint2
- fr3_joint3
- fr3_joint4
- fr3_joint5
- fr3_joint6
- fr3_joint7Note: A nullspace stiffness needs to be specified so that the arm also follows the joint configuration and not just the end-effector pose.
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.
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:
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
@article{mayr2024cartesian,
doi = {10.21105/joss.05194},
url = {https://doi.org/10.21105/joss.05194},
year = {2024},
publisher = {The Open Journal},
volume = {9},
number = {93},
pages = {5194},
author = {Matthias Mayr and Julian M. Salt-Ducaju},
title = {A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators}, journal = {Journal of Open Source Software}
}