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! 😊