A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators

Cartesian impedance control is a type of motion control strategy for robots that improves safety in partially unknown environments by achieving a compliant behavior of the robot with respect to its external forces. This compliant robot behavior has the added benefit of allowing physical human guidance of the robot. In this paper, we propose a C++ implementation of compliance control valid for any torque-commanded robotic manipulator. The proposed controller implements Cartesian impedance control to track a desired end-effector pose. Additionally, joint impedance is projected in the nullspace of the Cartesian robot motion to track a desired robot joint configuration without perturbing the Cartesian motion of the robot. The proposed implementation also allows the robot to apply desired forces and torques to its environment. Several safety features such as filtering, rate limiting, and saturation are included in the proposed implementation. The core functionalities are in a re-usable base library and a Robot Operating System (ROS) ros_control integration is provided on top of that. The implementation was tested with the KUKA LBR iiwa robot and the Franka Emika Robot (Panda) both in simulation and with the physical robots.


Statement of Need
Modern robotics is moving more and more past the traditional robot systems that have hard-coded paths and stiff manipulators.Many use-cases require the robots to work in semi-structured environments.These environments impose uncertainties that could cause collisions.Furthermore, many advanced assembly, manufacturing and household scenarios such as insertions or wiping motions require the robot to excert a controlled force on the environment.Finally, the robot workspace is becoming increasingly shared with human workers in order to leverage both agents and allow them to complement each other.
An implementation of compliant control for robotic manipulators is an attractive solution for robots in contact-rich environments.To cover the wide variety of tasks and scenarios, we think that it needs to fulfill the following criteria: 1. Dynamically adapt the end-effector reference point.2. Dynamically adjust the robot's impedance (i.e., its ability to resist or comply with external forces).3. Apply commanded forces and torques (i.e., a wrench) with the end-effector of the robot.4. Command a joint configuration and apply it in the nullspace of the Cartesian robotic task. 5. Execute joint-space trajectories.
A complete implementation with respect to items 1-5 above of compliance for torque-commanded robotic manipulators is not available, and the existing solutions franka_ros (Franka Emika, 2017a) and libfranka (Franka Emika, 2017b) as well as the KUKA FRI Cartesian impedance controller can only be used for a single type of robotic manipulator: KUKA FRI controller franka_ros libfranka

Reference Pose Update
1 An FRI connection can send either joint position updates or wrench updates 2 Reaching a joint limit triggers a safety stop 3 Can be implemented by setting the Cartesian stiffness to zero This implementation offers a base library that can easily be integrated into other software and also implements a ros_control controller on top of the base library for the popular ROS middleware.The base library can be used with simulation software such as DART (Lee et al., 2018).It is utilized in several research papers such as Mayr, Ahmad, et al. (2022), Mayr, Hvarfner, et al. (2022) and Ahmad et al. (2022) that explore reinforcement learning as a strategy to accomplish contact-rich industrial robot tasks.
The Robot Operating System (ROS) is an open-source middleware that is widely used in the robotics community for the development of robotic software systems (Quigley et al., 2009).Within ROS, an implementation of compliant control is available for position-commanded and velocity-commanded robotic manipulators with the cartesian_controllers package (Scherzinger et al., 2017).However, if a robotic manipulator supports direct control of the joint torques, e.g., the KUKA LBR iiwa or the Franka Emika Robot (Panda), torque-commanded Cartesian impedance control is often the preferred control strategy, since a stable compliant behavior might not be achieved for position-commanded and velocity-commanded robotic manipulators (Lawrence, 1988).

Control Implementation
The gravity-compensated rigid-body dynamics of the controlled robot can be described, in the joint space of the robot q ∈ R n , as (Siciliano & Khatib, 2016): where M (q) ∈ R n×n is the generalized inertia matrix, C(q, q) ∈ R n×n captures the effects of Coriolis and centripetal forces, τ c ∈ R n represents the input torques, and τ ext ∈ R n represents the external torques, with n being the number of joints of the robot.Since the proposed controller was evaluated using robots that are automatically gravity-compensated (KUKA LBR iiwa and Franka Emika Robot (Panda)), the gravity-induced torques have not been included in (Equation 1).However, the proposed controller can be used in robots that are not automatically gravity-compensated by adding a gravity-compensation term to the commanded torque signal, τ c .
Moreover, the torque signal commanded by the proposed controller to the robot, τ c in (Equation 1), is composed by the superposition of three joint-torque signals: where • τ ca c is the torque commanded to achieve a Cartesian impedance behavior (Hogan, 1985) with respect to a Cartesian pose reference in the mdimensional task space, ξ D ∈ R m , in the frame of the end-effector of the robot: with J(q) ∈ R m×n being the Jacobian relative to the end-effector (task) frame of the robot, and K ca ∈ R m×m and D ca ∈ R m×m being the virtual Cartesian stiffness and damping matrices, respectively.Also, the Cartesian pose error, ∆ξ in (Equation 3) is defined as ∆ξ tr = ξ tr − ξ D tr for the translational degrees of freedom of the Cartesian pose and as ∆ξ ro = ξ ro ξ D ro −1 for the rotational degrees of freedom.
• τ ns c is the torque commanded to achieve a joint impedance behavior with respect to a desired configuration and projected in the null-space of the robot's Jacobian, to not affect the Cartesian motion of the robot's endeffector (Ott, 2008): with the superscript † denoting the Moore-Penrose pseudoinverse matrix (Khatib, 1995) 4 given by J † = (J T J) −1 J T (Ben-Israel & Greville, 2003), and τ 0 being an arbitrary joint torque formulated to achieve joint compliance, where K ns ∈ R n×n and D ns ∈ R n×n are the virtual joint stiffness and damping matrices, respectively.
• τ ext c is the torque commanded to achieve the desired external force command in the frame of the end-effector of the robot, F ext c :

Safety Measures
As described in Figure 1, there are several safety measures that have been implemented in the controller to achieve a smooth behavior of the robot:

Filtering
The proposed controller allows the online modification of relevant variables: ξ D , K ca and D ca in (Equation 3), K ns and D ns in (Equation 5), and F ext c in (Equation 6).However, for a smoother behavior of the controller, the values of these variables are low-pass filtered.The update law at each time-step k is: where α D is the desired new variable value and a ∈ (0, 1] is defined in such a way that a user-defined percentage of the difference between the desired value α D and the variable value at the time of the online modification instruction, α 0 , is applied after a user-defined amount of time.