Even if the controller torque is saturated i get the controller_torque_discontinuity error
Opened this issue · 0 comments
nicolapiccinelli commented
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;
}