Ports the ROS1 Cartesian Impedance Controller [draft]

[kamiradi]

Introduction

The aim of this pull request is to get some feedback on an ongoing Cartesian Impedance Controller port of the corresponding ROS1 implementation. The hope is to learn from someone more experienced at writing ROS2 controllers and ROS2 nodes (I have experience with ROS1 but am relatively new to the ROS2 paradigm), while porting what is a very useful controller in the franka_ros stack.

The draft comes with a basic set of commits that reproduce the skeleton of a CIC ros2_controller, with the update functionality pretty much completely ported. This has been emulated from the other controller implementations.

The following is a short summary of where the code is doubtful

1. Acquiring command and state interfaces.
2. Reading gain parameters from a server.
3. Subscribing to the equilibrium pose

Acquiring command and state interfaces

The CIC has to directly command joint torques. For now, I have fashioned this piece similar to how the Joint_impedance_controller commands are sent. Similarly, the state interfaces have been acquired from a franka_semantic_components::FrankaRobotState object. Since there is a requirement for the coriolis and jacobian matrix the corresponding interfaces from the franka_semantic_components::FrankaRobotModel were also acquired. However, I don't know what is required here. Would like to hear your comments on this. For now I have assigned loaned interfaces for the robot model, robot state and a cartesian pose semantic component.

CallbackReturn CartesianImpedanceExampleController::on_activate(
  const rclcpp_lifecycle::State& /*previous_state*/) {
  /*
   * Assign loaned command and state interfaces
  */
  // state interfaces
  franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_);
  franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_);
  equilibrium_pose_d_->assign_loaned_state_interfaces(state_interfaces_);

  // command interfaces
  // franka_robot_state_->assign_loaned_command_interfaces(command_interfaces_);

  return CallbackReturn::SUCCESS;
}

Reading gain parameters

franka_ros subscribes to a topic to dynamically reconfigure the cartesian stiffness and damping gains. Could you point me towards a resource somewhere within ROS2 docs that I can use to emulate?

Subscribing to the equilibrium pose

The CIC needs a target pose that acts as the equilibrium. Is it as simple as creating a subscriber and a corresponding callback?

Looking forward to your feedback

[BarisYazici]

Do you have an FR3 robot to test on?

[BarisYazici]

We can simplify this a bit. After we get the simplified version read we can go on to iterate to get more advanced ones. I think one good reference point would be the libfranka example of cartesian_impedance control-> https://github.com/frankaemika/libfranka/blob/fr3-develop/examples/cartesian_impedance_control.cpp. You could try to implement the libfranka version instead of the franka_ros 1 version.

To sum up:

• You can have the initial pose as the equilibrium pose and assume that it will not change. So you don't need to subscribe to a topic to read the pose.
• You can have the stiffness and damping parameters fixed.

[kamiradi]

Hi,
Thanks for your feedback. Firstly, I do not have an FR3 to test this on. We have a Panda running ROS2 and libfranka and assumed that I can test it on that, but unfortunately it seems that our local ROS2 humble is an older commit.

However, I'd like to still develop it and test it on sim if that is possible. Would like to hear your thoughts on how I could get such a workflow.

Your idea on the libfranka CIC implementation as an inspiration is something to look into. Let me change the current implementation to reflect that and ping you again.

In the meantime It would be helpful if I can get up and running with a local sim setup that can test the controller.

Looking forward to hear from you.