Unexpected discontinuity errors in cartesian pose controller with external trajectory publisher

[bdlim99]

Summary

I have implemented a Cartesian pose controller for the Franka Emika Panda robot, based on the franka_example_controllers/cartesian_pose_example_controller.cpp. However, when running my implementation, I encounter the following errors:

• cartesian_motion_generator_joint_velocity_discontinuity
• cartesian_motion_generator_joint_acceleration_discontinuity

These errors occur even though my controller closely follows the example, with the primary difference being that my desired Cartesian poses are published externally via a Python script, rather than being generated within the C++ controller itself.

System Setup

• Robot: Franka Emika Panda
• ROS Version: ROS melodic
• Operating System: ubuntu 18.04

Code Details

I have implemented:

1. A Python-based trajectory generator that publishes the desired pose to the desired_pose topic at 100 Hz.
2. A C++ Cartesian pose controller, subscribing to desired_pose and setting the robot's Cartesian pose accordingly.

Python Trajectory Generator (Publishes to desired_pose)

#!/usr/bin/python
import rospy
import tf.transformations
from geometry_msgs.msg import PoseStamped
from franka_msgs.msg import FrankaState


def main():
    rospy.init_node('cartesian_pose_publisher')

    pub = rospy.Publisher('desired_pose', PoseStamped, queue_size=10)

    rate = rospy.Rate(100)

    state_msg = rospy.wait_for_message("/franka_state_controller/franka_states", FrankaState)

    O_T_EE = state_msg.O_T_EE

    initial_x = O_T_EE[12]
    initial_y = O_T_EE[13]
    initial_z = O_T_EE[14]

    trans_matrix = [
        [O_T_EE[0], O_T_EE[4], O_T_EE[8],  O_T_EE[12]],
        [O_T_EE[1], O_T_EE[5], O_T_EE[9],  O_T_EE[13]],
        [O_T_EE[2], O_T_EE[6], O_T_EE[10], O_T_EE[14]],
        [O_T_EE[3], O_T_EE[7], O_T_EE[11], O_T_EE[15]]
    ]

    q = tf.transformations.quaternion_from_matrix(trans_matrix)

    initial_qx, initial_qy, initial_qz, initial_qw = q[0], q[1], q[2], q[3]

    while not rospy.is_shutdown():
        pose_msg = PoseStamped()

        pose_msg.header.stamp = rospy.Time.now()
        pose_msg.header.frame_id = ""
        pose_msg.pose.position.x = initial_x
        pose_msg.pose.position.y = initial_y
        pose_msg.pose.position.z = initial_z
        pose_msg.pose.orientation.x = initial_qx
        pose_msg.pose.orientation.y = initial_qy
        pose_msg.pose.orientation.z = initial_qz
        pose_msg.pose.orientation.w = initial_qw

        pub.publish(pose_msg)

        rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

C++ Cartesian Pose Controller (Subscribes to desired_pose)

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_controllers/cartesian_pose_controller.h>

#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>

#include <Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <controller_interface/controller_base.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>

namespace test {

bool CartesianPoseController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) {
  cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
  if (cartesian_pose_interface_ == nullptr) {
    ROS_ERROR("CartesianPoseController: Could not get Cartesian Pose interface from hardware");
    return false;
  }

  std::string arm_id;
  if (!node_handle.getParam("arm_id", arm_id)) {
    ROS_ERROR("CartesianPoseController: Could not get parameter arm_id");
    return false;
  }

  try {
    cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(cartesian_pose_interface_->getHandle(arm_id + "_robot"));
  } catch (const hardware_interface::HardwareInterfaceException& e) {
    ROS_ERROR_STREAM("CartesianPoseController: Exception getting Cartesian handle: " << e.what());
    return false;
  }

  // Subscribe to the "desired_pose" topic.
  sub_desired_pose_ = node_handle.subscribe("desired_pose", 20,
                           &CartesianPoseController::desiredPoseCallback, this,
                           ros::TransportHints().reliable().tcpNoDelay());

  auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
  if (state_interface == nullptr) {
    ROS_ERROR("CartesianPoseController: Could not get state interface from hardware");
    return false;
  }

  try {
    auto state_handle = state_interface->getHandle(arm_id + "_robot");

    std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    for (size_t i = 0; i < q_start.size(); i++) {
      if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
        ROS_ERROR_STREAM(
            "CartesianPoseController: Robot is not in the expected starting position for running this example. "
            "Run `roslaunch test move_to_start.launch robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
        return false;
      }
    }
  } catch (const hardware_interface::HardwareInterfaceException& e) {
    ROS_ERROR_STREAM("CartesianPoseController: Exception getting state handle: " << e.what());
    return false;
  }

  // Initially, no desired pose has been received.
  received_desired_pose_ = false;

  return true;
}

void CartesianPoseController::starting(const ros::Time& /* time */) {
  if (!received_desired_pose_) {
    std::array<double, 16> current_pose = cartesian_pose_handle_->getRobotState().O_T_EE_d;
    desired_pose_msg_.header.stamp = ros::Time::now();
    desired_pose_msg_.header.frame_id = "base_link";
    desired_pose_msg_.pose.position.x = current_pose[12];
    desired_pose_msg_.pose.position.y = current_pose[13];
    desired_pose_msg_.pose.position.z = current_pose[14];
    Eigen::Matrix3d R;
    R << current_pose[0],  current_pose[1],  current_pose[2],
         current_pose[4],  current_pose[5],  current_pose[6],
         current_pose[8],  current_pose[9],  current_pose[10];
    Eigen::Quaterniond q(R);
    desired_pose_msg_.pose.orientation.x = q.x();
    desired_pose_msg_.pose.orientation.y = q.y();
    desired_pose_msg_.pose.orientation.z = q.z();
    desired_pose_msg_.pose.orientation.w = q.w();
    received_desired_pose_ = true;
  }
}


void CartesianPoseController::update(const ros::Time& /* time */,
                                       const ros::Duration& /* period */) {
  if (received_desired_pose_) {
    // Convert the received desired_pose_msg_ into a 4×4 homogeneous transformation matrix.
    Eigen::Quaterniond q(desired_pose_msg_.pose.orientation.w,
                         desired_pose_msg_.pose.orientation.x,
                         desired_pose_msg_.pose.orientation.y,
                         desired_pose_msg_.pose.orientation.z);
    Eigen::Vector3d pos(desired_pose_msg_.pose.position.x,
                        desired_pose_msg_.pose.position.y,
                        desired_pose_msg_.pose.position.z);
    Eigen::Matrix3d R = q.toRotationMatrix();

    std::array<double, 16> new_pose;
    // Fill in rotation part (row-major order).
    new_pose[0]  = R(0, 0); new_pose[1]  = R(0, 1); new_pose[2]  = R(0, 2); new_pose[3]  = 0.0;
    new_pose[4]  = R(1, 0); new_pose[5]  = R(1, 1); new_pose[6]  = R(1, 2); new_pose[7]  = 0.0;
    new_pose[8]  = R(2, 0); new_pose[9]  = R(2, 1); new_pose[10] = R(2, 2); new_pose[11] = 0.0;
    // Fill in translation part.
    new_pose[12] = pos(0);   new_pose[13] = pos(1);   new_pose[14] = pos(2);   new_pose[15] = 1.0;

    // Set the command for the robot using the new pose.
    cartesian_pose_handle_->setCommand(new_pose);
  } else {
    // If no desired pose has been received, warn (throttled so as not to spam).
    ROS_WARN_THROTTLE(1.0, "CartesianPoseController: No desired_pose received yet.");
  }
}

void CartesianPoseController::desiredPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
  desired_pose_msg_ = *msg;
  received_desired_pose_ = true;
}

}  // namespace test

PLUGINLIB_EXPORT_CLASS(test::CartesianPoseController,
                       controller_interface::ControllerBase)

Issue

Despite my Python script only publishing the initial pose (which remains constant), I encounter discontinuity errors related to velocity and acceleration when running my controller.

The key differences between my implementation and the example are:

1. In franka_example_controllers, the desired pose is generated internally in C++.
2. In my implementation, the desired pose is published externally via Python.

Since the pose remains constant, I wouldn't expect any velocity or acceleration discontinuities. However, they still occur.

Possible Causes

• Timing Issues?

  • Could the timing between the ROS message publication and the controller’s update loop be causing jumps in the commanded pose?
  • Would using rospy.Timer(rospy.Duration(0.005), callback) (like in interactive_marker.py) help ensure smooth updates?

• Filtering Needed?

  • Should I implement a low-pass filter to smooth out small fluctuations in received pose messages?

• Synchronization Issues?

  • Does the control loop expect more precisely synchronized updates than my Python script provides?

Questions

1. Why does externally publishing a constant pose cause discontinuity errors, while internally generating the pose does not?
2. Would a low-pass filter or a different ROS timing mechanism (rospy.Timer) help mitigate the issue?
3. Is there a recommended best practice for externally publishing desired Cartesian poses to ensure smooth execution?

Any insights or recommendations would be greatly appreciated!

Additional Information

• The robot is properly initialized and started in the correct pose.
• Running cartesian_pose_example_controller.cpp without modifications works as expected.
• The issue occurs consistently when my external publisher is used.

Thanks in advance for your help! 😊

[Longyouyuan]

Hi sir, did you solve it? I met very similar problem. /(ㄒoㄒ)/~~

[higifnr]

@Longyouyuan, I started working on franka robots this summer and have been encountering these kinds of issues every single day. I asked people who've been using them for years and they've confirmed that it's the typical franka experience. Sadly I don't think we can do anything about it.

[JasperTan97]

Hi, from my understanding, you cannot command positions like this on the franka. You have to write your position controller solely in C++ and run in there at 1000Hz. By the time your rclpy node has finished computing your next desired pose, passed it to your rclcpp node, and then sent it to libfranka to move, the duration elapsed (and possible small jitters) will trigger velocity violations.

The way to do it is the write a controller that converts to joint torques directly, and then command the joint torques (stiffly if you need high precision). You can use your python code to provide a moving reference position for the C++ controller to chase.

If someone has another way do let me know.