When writing my own controller, always face the error: libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_acceleration_discontinuity"]

I want to write my own controller based on the cartesian_example_controller. I have tried two ways to send_command in update() function. 1 send the the command directly without using the RobotState.O_T_EE_D the cartesian_move_controller.cpp code is listed as follow:

// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_example_controllers/cartesian_move_controller.h>

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

#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 franka_example_controllers {

bool CartesianMoveController::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(
        "CartesianPoseExampleController: Could not get Cartesian Pose "
        "interface from hardware");
    return false;
  }

  std::string arm_id;
  if (!node_handle.getParam("arm_id", arm_id)) {
    ROS_ERROR("CartesianPoseExampleController: 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(
        "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
    return false;
  }

  auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
  if (state_interface == nullptr) {
    ROS_ERROR_STREAM(
        "CartesianImpedanceExampleController: Error getting state interface from hardware");
    return false;
  }
  try {
    auto state_handle = state_interface->getHandle(arm_id + "_robot");
  } catch (hardware_interface::HardwareInterfaceException& ex) {
    ROS_ERROR_STREAM(
        "CartesianImpedanceExampleController: Exception getting state handle from interface: "
        << ex.what());
    return false;
  }

  return true;
}

void CartesianMoveController::starting(const ros::Time& /* time */) {
  initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
  new_pose_=initial_pose_;
  new_pose_filter_=initial_pose_;
  elapsed_time_ = ros::Duration(0.0);
  cycleCount = 0;
  const float samplingrate = 1000; // Hz
  const float cutoff_frequency = 5; // Hz
  ROS_INFO("The initial z postion is : %f",initial_pose_[14]);

  // calc the coefficients
  f.setup(samplingrate, cutoff_frequency);

}

void CartesianMoveController::update(const ros::Time& /* time */,
                                            const ros::Duration& period) {
  elapsed_time_ += period;
  double delta_z = 0.00001;
  if ( cycleCount < 20000)
  {
    if (cycleCount < 10000)
    {
        if ((cycleCount < 1000) || (cycleCount > 9000))
        {
            new_pose_[14] -= delta_z/2;
        }
        else
        {
        new_pose_[14] -= delta_z;
        }
    }
    // else
    // {
    //     if ((cycleCount < 101000) || (cycleCount > 199000))
    //     {
    //         new_pose_[14] = cartesian_pose_handle_->getRobotState().O_T_EE_d[14]-delta_z/2;
    //     }
    //     else
    //     {
    //     new_pose_[14] -= delta_z;
    //     }
    // }
    // ROS_INFO("The desired z position in controller is : %f",cartesian_pose_handle_->getRobotState().O_T_EE_d[14]);
  }
  else if (cycleCount == 20000)
  {
    ROS_INFO("The movement has ended! The end z postion is : %f",cartesian_pose_handle_->getRobotState().O_T_EE[14]);
  }

//   else
//   {
//     new_pose_[14] -= delta_z;
//     cycleCount = cycleCount - 200000;
//   }
  new_pose_filter_[14]= f.filter(new_pose_[14]-initial_pose_[14])+initial_pose_[14];
  cartesian_pose_handle_->setCommand(new_pose_filter_);
  cycleCount = cycleCount + 1;
//   ROS_INFO("The command z position is : %f",new_pose_filter_[14]);
}

}  // namespace franka_example_controllers

PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianMoveController,
                       controller_interface::ControllerBase)

Anyone who would like to run the code just need to add the iir for low-pass filer. And add the

find_package(iir)
target_link_libraries(... iir::iir)

into the CMakeList.txt Result: libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_acceleration_discontinuity"] control_command_success_rate: 0.97 Analysis: 1、 The cartesian_path don't violate the limits in cartesian space but in joint space (jerk limit violation). However, there is no way for us to get the desired joint command in internal controller (through internal IK solver), because we could get the IK solver of q_d only the next control loop when using the FrankaCartesianPoseInterface.send_command(idealpose) while the joint jerk limits might be violated in this loop which turns out to be an error and aborts. 2、 Since the move speed is not so fast 0.01mm/s which is slower than most of the example_controller could provide, It's quite confusing.

2 send the the command directly without using the RobotState.O_T_EE_D Just like the previous issue #71, I have tried to use the current RobotState.O_T_EE_D to send command.

void CartesianMoveController::update(const ros::Time& /* time */,
                                            const ros::Duration& period) {
  elapsed_time_ += period;
  double delta_z = 0.00001;
  if ( cycleCount < 20000)
  {
    if (cycleCount < 10000)
    {
        if ((cycleCount < 1000) || (cycleCount > 9000))
        {
            new_pose_[14] = cartesian_pose_handle_->getRobotState().O_T_EE_d[14]-delta_z/2;
        }
        else
        {
        new_pose_[14] = cartesian_pose_handle_->getRobotState().O_T_EE_d[14]-delta_z;
        }
    }
    // else
    // {
    //     if ((cycleCount < 101000) || (cycleCount > 199000))
    //     {
    //         new_pose_[14] = cartesian_pose_handle_->getRobotState().O_T_EE_d[14]-delta_z/2;
    //     }
    //     else
    //     {
    //     new_pose_[14] -= delta_z;
    //     }
    // }
    // ROS_INFO("The desired z position in controller is : %f",cartesian_pose_handle_->getRobotState().O_T_EE_d[14]);
  }
  else if (cycleCount == 20000)
  {
    ROS_INFO("The movement has ended! The end z postion is : %f",cartesian_pose_handle_->getRobotState().O_T_EE[14]);
  }

//   else
//   {
//     new_pose_[14] -= delta_z;
//     cycleCount = cycleCount - 200000;
//   }
  new_pose_filter_[14]= f.filter(new_pose_[14]-initial_pose_[14])+initial_pose_[14];
  cartesian_pose_handle_->setCommand(new_pose_filter_);
  cycleCount = cycleCount + 1;
//   ROS_INFO("The command z position is : %f",new_pose_filter_[14]);
}

Result: The command sent by every loop to substract a fixed delta_z from current O_T_EE_d, It should move 0.9 m ideally, but from the ROS_INFO, the real movement is only 0.019 m which differs a lot. Analysis: Maybe the cartesian command of current loop sent to the controller is not regarded as the O_T_EE_d of the next loop. I am also confused of the inner trajectory tracking controller, how it works?

I appreciate it if someone could solve my problem, thanks for your help.

frankaemika / franka_ros

When writing my own controller, always face the error: libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_acceleration_discontinuity"] #391