Apply Torque Constraints from URDF in Controller

[Ahmadreza-Shahrokhshahi]

Dear @gergondet and @arntanguy,

I am currently working on applying torque constraints to the robot joints using the effort limits specified in the URDF file. Initially, I expected that the effort limits would be automatically applied by the system, similar to position and velocity limits. However, it appears that a dynamics constraint needs to be explicitly added to the controller to enforce the torque constraints.

To address this, I declared an object of the DynamicsConstraint class in the controller header as follows:
mc_solver::DynamicsConstraint robotTorqueConstraint = mc_solver::DynamicsConstraint(this->robots(), 0, this->timeStep, {0.01, 0.001, 0.05}, 1.0, false);

In declaration, I used the one with the damper and velocityPercent since the other one would cause a QP failure once it is called.

Then, I added this constraint to the solver in the controller:
robotTorqueConstraint.addToSolver(solver());

Additionally, I adjusted the default minimum and maximum torque values in Robot.cpp from zero to infinity to allow the robot to walk:
fill_bound(mb, "lower torque", bounds.at(4), &rbd::Joint::dof, -INFINITY, -INFINITY),
fill_bound(mb, "upper torque", bounds.at(5), &rbd::Joint::dof, INFINITY, INFINITY)
Without these adjustments, the robot exhibits sudden, erratic movements and is unable to walk.

At this point, the robot walks without any issues, but it seems that the torque constraints specified in the URDF file are not being applied to the system.

The above image shows that the robot walked a few steps with the effort limit set to 100 for all joints. The joint torques and limits are shown only for L_HIP_R and R_HIP_R for clarity, but the same issue is observed for the other joints. Despite this, it is evident that the joint torques exceed these limits. I have double-checked the torque limits in the controller, and they match the URDF.

Could you please advise if there are any additional modifications or steps required to ensure that the torque constraints are correctly enforced?

Thank you for your assistance.

[Ahmadreza-Shahrokhshahi]

Hi @gergondet, @arntanguy, @mmurooka, @antodld,

I am following up on the issue I opened earlier and was hoping someone from your team could respond.

As an update, in the meantime, I have tried to apply this tutorial, which suggests that torque constraints can be applied by simply changing kinematicsConstraint to dynamicsConstraint. I made this change and added the following lines to establish contacts between the robot's feet and the ground:

addContact({robot().name(), "ground", "LeftFoot", "AllGround"});
addContact({robot().name(), "ground", "RightFoot", "AllGround"});

However, the output is unsatisfactory, as it shows a strange motion of the robot attempting to walk without the contacts being properly updated.

Could someone provide guidance on resolving this issue?
Thank you.

[antodld]

Hello, @Ahmadreza-Shahrokhshahi,
Can you specify the robot you are using and if you are making your test with a custom controller ?

Regarding the torque limits, they are linked to the "effort" limit in the URDF. After running a few tests on my end, I didn't notice any limit violations. However, it's possible that the logger isn't displaying the proper limit. To verify this, could you please add the following lines to the controller constructor function:

  for (int i = 0 ; i < robot().mb().nrJoints() ; i++)
  {
      for (int j = 0 ; j < robot().mb().joint(i).dof() ; j++)
      {
        const std::string name = robot().mb().joints()[i].name();
        logger().addLogEntry("tau_qp_" + name + "_" + std::to_string(j),[this,i,j] () -> const double {return robot().mbc().jointTorque[i][j];});
        logger().addLogEntry("tau_lim_up_"+name + "_" + std::to_string(j),[this,i,j] () -> const double {return robot().tu()[i][j];});
        logger().addLogEntry("tau_lim_low_"+name + "_" + std::to_string(j),[this,i,j] () -> const double {return robot().tl()[i][j];});
      }
  }

It should log the current value used in the controller (including the floating base where all values should be at 0).

Additionally, I adjusted the default minimum and maximum torque values in Robot.cpp from zero to infinity to allow the robot to walk:

By modifying this line, it is possible that you have set an infinite forces limit to the floating base free-joint which discard the real dynamic of the robot.

Concerning the contact declared, in case of walking, it is necessary to remove them and add periodically depending on the support phase

[Ahmadreza-Shahrokhshahi]

@antodld
Thanks for your response.

Can you specify the robot you are using and if you are making your test with a custom controller ?

I am working on a custom biped robot, and the controller is similar to this one.

To verify this, could you please add the following lines to the controller constructor function:

I already have that part of the code you provided. It’s actually available in mc_rtc as tauOut. I also have access to tauIn, which comes from the Choreonoid simulation.

I have analyzed the difference between them based on this issue. I can confirm that tauOut considers the URDF effort limits in the QP solver, but I'm seeing a noticeable difference between tauOut (computed by the QP solver), and tauIn (measured from sensor/Choreonoid). I’ve attached a comparison for knee and ankle pitch below:

It seems that tauIn is more accurate since the joint torque values make more sense.
I have a few clear questions based on my findings:

1. What is the main reason for the discrepancies between tauIn and tauOut? Do you also see this discrepancy in your robot?

2. Are we only able to set the limits on tauOut? Is there a way to apply limits on tauIn? This seems to be the primary goal of having a torque constraint. Otherwise, setting torque limits in the QP solver, which produces torques significantly different from what we actually measure, does not seem to make sense.

I would appreciate any insights from other contributors as well@arntanguy @gergondet @mehdi-benallegue

[antodld]

What is the main reason for the discrepancies between tauIn and tauOut? Do you also see this discrepancy in your robot?

The QP is formulated having the joint accelerations as the decision variables. Those variables are then integrated twice and fed to the simulation environment. Choreonoid then simulate a PD behavior on each joints. This is why you have no guarantee to obtain similar torque as choreonoid does not directly use the joints torques information.

Moreover, if your controller is similar to lipm_walking controller, it means that you are using a stabilizer. The stabilizer computes a wrench distribution on the contacts that is not linked to the one computed by the QP. This discrepancy can then be seen in the torque measurements.

Keep in mind that in a position control loop, the QP state is open loop which means that any discrepancies in the robot state such as oscillation are not considered. The stabilizer is an exception but the desired forces being applied through admittance tasks, the discrepancies remains.

Are we only able to set the limits on tauOut? Is there a way to apply limits on tauIn?

tauIn is a measurement and therefore cannot be bound direclty. Ensuring a bounded tauIn imply that the real robot has a torque control loop, that way the tauOut will be directly fed as reference with their set boundaries
Otherwise, to bound the joints torques on the real robot, the PD control outputted desired torque should be bounded but this can result in poor tracking of the joint values