Cartesian impedance: compliant but does not move toward /panda/target_pose

[AlexD15216]

Hi,
Joint impedance behaves as expected. However, when I run the Cartesian impedance controller and publish a desired hand pose to /panda_2/target_pose (the topic subscribed by the crisp controller), the Franka does not move toward the commanded pose. I’m using the config from
https://github.com/utiasDSL/crisp_controllers_demos/blob/main/crisp_controllers_robot_demos/config/fr3/controllers.yaml,
with the ros__parameters adjusted to match my setup.

Environment
OS: Ubuntu 22.04
ROS 2: Humble
crisp_controllers: v1.1.0
Pinocchio: 3.8.0
Robot: Franka Panda

[danielsanjosepro]

Hey @AlexD15216 ! Could you send also the logs that you get from the controller when you try that.
docker compose up xxxxxxxxx > output.log

[AlexD15216]

Hey @danielsanjosepro, thanks for your quick response!
Unfortunately, we are running our own docker container with ros2 humble and other components and testing crisp_controller in that environment.
When we configure and activate crisp_controller via our launch file, we see the following output, which might be helpful:

[ros2_control_node-2] [INFO] [1760457876.041700562] [FrankaMultiHardwareInterface]: Preparing command mode switch
[ros2_control_node-2] [INFO] [1760457876.042083439] [FrankaMultiHardwareInterface]: Performing command mode switch
[ros2_control_node-2] [INFO] [1760457876.042099127] [FrankaMultiHardwareInterface]: Arm panda_2 current mode: <none>
[ros2_control_node-2] Stopping
[ros2_control_node-2] Initializing continuous reading
[ros2_control_node-2] Default Parameterssss
[ros2_control_node-2] [INFO] [1760457878.825460705] [panda_2.controller_manager]: Loading controller 'cart_imp_cntrl'
[ros2_control_node-2] [INFO] [1760457878.855177000] [panda_2.controller_manager]: Configuring controller 'cart_imp_cntrl'
[ros2_control_node-2] [INFO] [1760457878.858894292] [panda_2.cart_imp_cntrl]: Checking available joints in model:
[ros2_control_node-2] [INFO] [1760457878.858946621] [panda_2.cart_imp_cntrl]: Joint 0 with name universe is of type JointModelRX
[ros2_control_node-2] [INFO] [1760457878.858957713] [panda_2.cart_imp_cntrl]: Joint 1 with name panda_2_joint1 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.858967547] [panda_2.cart_imp_cntrl]: Joint 2 with name panda_2_joint2 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.858978299] [panda_2.cart_imp_cntrl]: Joint 3 with name panda_2_joint3 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.858989700] [panda_2.cart_imp_cntrl]: Joint 4 with name panda_2_joint4 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.858996937] [panda_2.cart_imp_cntrl]: Joint 5 with name panda_2_joint5 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.859003443] [panda_2.cart_imp_cntrl]: Joint 6 with name panda_2_joint6 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.859010241] [panda_2.cart_imp_cntrl]: Joint 7 with name panda_2_joint7 is of type JointModelRZ
[ros2_control_node-2] [INFO] [1760457878.859017193] [panda_2.cart_imp_cntrl]: Joint 8 with name panda_2_finger_joint1 is of type JointModelPY
[ros2_control_node-2] [INFO] [1760457878.859049134] [panda_2.cart_imp_cntrl]: Joint 9 with name panda_2_finger_joint2 is of type JointModelPrismaticUnaligned
[ros2_control_node-2] [INFO] [1760457878.859058487] [panda_2.cart_imp_cntrl]: All joints passed in the parameters exist in the kinematic tree of the URDF.
[ros2_control_node-2] [INFO] [1760457878.859066691] [panda_2.cart_imp_cntrl]: Removing the rest of the joints that are not used: 
[ros2_control_node-2] [INFO] [1760457878.859075881] [panda_2.cart_imp_cntrl]: Joint panda_2_finger_joint1 is not used, removing it from the model.
[ros2_control_node-2] [INFO] [1760457878.859085729] [panda_2.cart_imp_cntrl]: Joint panda_2_finger_joint2 is not used, removing it from the model.
[ros2_control_node-2] [INFO] [1760457878.859970464] [panda_2.cart_imp_cntrl]: State interfaces and control vectors initialized.
[ros2_control_node-2] [INFO] [1760457878.872386392] [FrankaMultiHardwareInterface]: Preparing command mode switch
[ros2_control_node-2] [INFO] [1760457878.873080896] [FrankaMultiHardwareInterface]: Performing command mode switch
[ros2_control_node-2] [INFO] [1760457878.873098782] [FrankaMultiHardwareInterface]: Arm panda_2 current mode: joint_torque
[ros2_control_node-2] Stopping
[ros2_control_node-2] Initializing joint torque control
[ros2_control_node-2] [INFO] [1760457878.875729914] [panda_2.cart_imp_cntrl]: Controller activated.

[danielsanjosepro]

Interesting... it does look good at first glance. You also feel the compliance right?
Could you show me your config?

Things that you could try:

1. Enable logs in the controller config. Here are the options: https://github.com/utiasDSL/crisp_controllers/blob/4e67d51df2f20b2435f181674b54cea4ae8be006/src/cartesian_impedance_controller.yaml#L190-L283
   Then you can check fast if everything is working fine.

2. You could add a log to this callback:
   https://github.com/utiasDSL/crisp_controllers/blob/4e67d51df2f20b2435f181674b54cea4ae8be006/src/cartesian_controller.cpp#L335-L346
   just to check if it is receiving the messages. Maybe you have multiple publishers an d that is why it is not working.

I hope this helps, let me know about the config!

[AlexD15216]

Hi @danielsanjosepro , thanks a lot for your help. I think I have found out the core issue.
The tcp position is (0.306, 0.005, 0.458) (m) in my robot frame (and previously I sent msg to /panda_2/target_pose based on this tcp position). However, in the crisp controller logs, the tcp position shows up as (0.030, 0.395, 1.248) (m). If I command targets based on the logged tcp pose, the robot moves correctly. I'm still confused by the crisp controller logged tcp position, the value doesn't make sense.

The following is my controller config:

 ${arm_id}/cart_imp_cntrl:
  ros__parameters:
    # Typical interfaces for a cartesian impedance controller
    joints: *joints
    # Frames (adjust to your URDF frames if the controller uses them)
    base_frame: ${arm_id}_link0
    end_effector_frame: ${arm_id}_hand_tcp

    limit_error: true
    limit_torques: true
    max_delta_tau: 0.5

    nullspace:
      stiffness: 2.0
      # projector_type: "none"

    stop_commands: false

    filter:
      q: 0.5
      dq: 0.5
      output_torque: 0.5

    task:

      k_pos_x: 100.0
      k_pos_y: 100.0
      k_pos_z: 100.0
      k_rot_x: 5.0
      k_rot_y: 5.0
      k_rot_z: 5.0

      error_clip.rx: 0.5
      error_clip.ry: 0.5
      error_clip.rz: 0.5
      error_clip.x: 0.1
      error_clip.y: 0.1
      error_clip.z: 0.1

    use_coriolis_compensation: true
    use_friction: false
    use_local_jacobian: true
    use_operational_space: false
    use_sim_time: false

The tf2 tree/frames are as follows:

frames_2025-10-15_11.06.11.pdf

[danielsanjosepro]

Hey! good to hear that you made some progress there. I think I know what the problem is: your base_frame has an offset w.r.t. to your world frame (probably (0.030, 0.395, 1.248) m - (0.306, 0.005, 0.458) m probably) pinocchio parses the urdf but it does not use the base frame of your robot as a reference but the world frame.

[danielsanjosepro]

I might try to find a fix for this this week, for now you can control it with this offset

[AlexD15216]

Hi @danielsanjosepro, the fix has been merged in PR #10 and included in release v1.2.0.
Thank you for your advice, review, and help!