Even if the controller torque is saturated i get the controller_torque_discontinuity error

[nicolapiccinelli]

Current setup:

Firmware version: 4.2.2 libfranka version: 0.9.2 franka_ros version: 0.9.1

I'm directly controlling a Franka Emika Panda robot using the EffortJointInterface. How can I get the controller_torque_discontinuity error if I set the maximum torque difference to less than 1.0, for instancekDeltaTauMax = 0.5?

tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d);
for (size_t i = 0; i < 7; ++i)
{
  joint_handles_[i].setCommand(tau_cmd(i));
  msg.data[i] = tau_cmd[i];
}

the saturation function is the following:

Eigen::Matrix<double, 7, 1> FeedbackLinearizationCtrl::saturateTorqueRate(
      const Eigen::Matrix<double, 7, 1> &tau_d_calculated,
      const Eigen::Matrix<double, 7, 1> &tau_J_d)
  {
    Eigen::Matrix<double, 7, 1> tau_d_saturated{};
    for (size_t i = 0; i < 7; i++)
    {
      double difference = tau_d_calculated[i] - tau_J_d[i];
      tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
    }
    return tau_d_saturated;
  }