Error when using Cartesian Velocity contol (`libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_acceleration_discontinuity"]`)

[jstmn]

Hi there,

I'm getting the following error: libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_acceleration_discontinuity"] when sending franka::CartesianVelocities commands which are too aggressive. I've found that sending a constant command like franka::CartesianVelocities output = {{0.0, 0.0, 0.05, 0.0, 0.0, 0.0}};, but slowly increasing the value like:

double v_z = 0.03*time_elapsed/time_max;
franka::CartesianVelocities output = {{0.0, 0.0, v_z, 0.0, 0.0, 0.0}};

works fine. Ideally, i'd just be able to command: 'go 5cm/s in the +z axis', and the controller would reach that as quickly as possible while respecting joint/torque limits. Is there a way to do that with libfranka? Do I need to determine how to slowly increase the velocity commands on my own? How do people normally do this?

Here's the code i'm using to debug:

robot.control([=, &time](const franka::RobotState& robot_state, franka::Duration period) -> franka::CartesianVelocities {
  time += period.toSec();
  double v_z = 0.03*time/time_max;
 // double v_z = 0.025; // failse with 'cartesian_motion_generator_joint_acceleration_discontinuity'
  franka::CartesianVelocities output = {{0.0, 0.0, v_z, 0.0, 0.0, 0.0}};
  if (time >= 2 * time_max) {
    return franka::MotionFinished(output);
  }
  return output;
});

I plotted q, qdot, qddot, torque, torque_dot but didn't see any values that were close to limits when running without the error, or when it runs which is weird. The plots are here: https://imgur.com/gallery/panda-cartesianvelocity-debugging-jmvpf2c

Thanks! This is my first time working with the franka so I'm not sure if i'm missing something obvious here.

Setup:

• Panda robot (not fr3)
• libfranka 0.8.0
• ubuntu 20

[AndreasKuhner]

Hi @jstmn ,
you are absolutely right with your observation -> the tricky part about our control is that we allow maximum controllability for the user. But that comes at a cost that the user has to execute a 'smooth trajectory'. For the allowed kind of control, we don't interfere much with the input and just execute it on the robot. Meaning, if you send a constant velocity and start from stand-still, you create a huge discontinuity -> we throw an error.

What people usually do is e.g. using moveit or any of many libfranka wrapper providing motion generation.

Cheers,
Andreas