Get joint torques

[diegoferigo]

I am trying to extract joint torques in something comparable to the UpdateSim call of the Physics system (that is called in the Update phase, not the PostUpdate). I will store the torques either in the JointForce component or a custom one, it's not clear if the existing component is supposed to store the Cmd or the net torque after removing the loss due to e.g. friction.

Beyond these ign-gazebo preliminaries, it seems that ign-physics already has the following APIs:

https://github.com/ignitionrobotics/ign-physics/blob/8750521a331290918b0b7e3c3a59dbdfc69d0496/include/ignition/physics/Joint.hh#L62-L68

https://github.com/ignitionrobotics/ign-physics/blob/8750521a331290918b0b7e3c3a59dbdfc69d0496/include/ignition/physics/detail/Joint.hh#L55-L61

Implemented for DART here:

https://github.com/ignitionrobotics/ign-physics/blob/8750521a331290918b0b7e3c3a59dbdfc69d0496/dartsim/src/JointFeatures.cc#L53-L57

That method, however, called in something similar to the UpdateSim phase, always returns 0. I delved into the stack and I found out that in order to trigger DART to populate the data that can be gathered by Joint::getForce, the method Skeleton::computeInverseDynamics should be called in the parent's model dartsim/dart#569.

I modified DART's call as follows:

double JointFeatures::GetJointForce(
    const Identity &_id, const std::size_t _dof) const
{
  this->ReferenceInterface<JointInfo>(_id)->joint->getSkeleton()->computeInverseDynamics(true, true, true);
  return this->ReferenceInterface<JointInfo>(_id)->joint->getForce(_dof);
}

And the joint forces are finally different than 0. I enabled the damping / spring / external forces by default so that they are considered in the ID and the returned values are more accurate and take into consideration modelled actuation losses.

With this being said, there are a few comments I'd like to ask feedback:

• If the JointForce component suitable to hold this value?
• ID computation can be called just once per step, but in this naive fix it is called every time a joint torque is gathered. I don't know how to improve the behaviour and if it's worth. If consecutive ID calls are no-op, maybe it's ok leaving it there.

---

Related resources:

• Proper joint torque feedback gazebo-classic#1321
• Implement properly simulated joint torque control mode robotology/gazebo-yarp-plugins#471
• changed the calculation of force/torque on parent link for ODEJoint::GetForceTorque() gazebo-classic#1975

[chapulina]

always returns 0.

That sounds like a bug

it's not clear if the existing component is supposed to store the Cmd or the net torque after removing the loss due to e.g. friction.

The commands are meant to go through JointForceCmd, so I think JointForce is supposed to store the resulting force.

If the JointForce component suitable to hold this value?

I'd assume yes, but I'd like to hear @azeey 's opinion. In any case, I think we should update that component's documentation to make it clearer if that's storing the resulting force or the commanded force.

[azeey]

hmm, I didn't know that we needed Skeleton::computeInverseDynamics, but it makes sense since step clears all forces. Another option might be to call step(false) first, collect all the joint forces into ignition::physics::ForwardStep::Output and then clear the forces.

If the JointForce component suitable to hold this value?

Yes, I think the convention now is *Cmd for commands and the corresponding component without Cmd for current state.

[traversaro]

I just realized that the comment in #143 (comment) may be more appropriate here, hence I am cross-linking it here.

[azeey]

Responding here to #143 (comment)

Thanks for the clarification @traversaro. In Gazebo-classic, Joint::GetForce returned \tau_sim by simply recording the last commanded force. I think what you're suggesting is that we do the same for Joint::GetForce and create another function that returns \tau_control. That sounds reasonable to me.

jointMechanicalForce = joint->getRelativeJacobianStatic().transpose() * joint->getChildBodyNode->getBodyForce();

Yes, I've been testing that same expression for the last few days and it seems to work. I was planning to create a PR with that as the value returned by Joint::GetForce, but as you suggested, I'm now thinking it should be a separate feature/function.

When it comes to ign-gazebo and its components though, there's still a question about what components::JointForce should hold. I think we should keep it consistent with other similar components like components::LinearVelocity, which hold the current state of the entity, and have component::JointForce hold \tau_control.

[diegoferigo]

When it comes to ign-gazebo and its components though, there's still a question about what components::JointForce should hold. I think we should keep it consistent with other similar components like components::LinearVelocity, which hold the current state of the entity, and have component::JointForce hold \tau_control.

I agree fully on this, and it does not look a change of behavior for the downstream users since the JointForce component isn't populated in the Physics system at the time of writing. The former command, as was described in #143 (comment), could maybe be held by a new component, if needed.

Something to keep in mind is that, AFAIK with my small experience with its API, bullet does not provide \tau_control and only exposes the last applied command. Therefore, the new feature providing \tau_control has to be implemented in gazebo as optional.

[traversaro]

Something to keep in mind is that, AFAIK with my small experience with its API, bullet does not provide \tau_control and only exposes the last applied command. Therefore, the new feature providing \tau_control has to be implemented in gazebo as optional.

If Bullet exposes the 6D force/torque transmitted over a joint, \tau_control can be easily implemented with the same logic used for DART.

[diegoferigo]

Crosslinking a relevant comment to close this issue: #143 (comment)

[graiola]

Hi, is there any news on this issue? I am trying to figure out how to populate the joint efforts in the ign_ros_control plugin.
Should I populate these efforts with the command values?

https://github.com/ros-controls/ign_ros_control/blob/master/ign_ros_control/src/ign_system.cpp#L248-L271

[azeey]

Closing now that #283 has been implemented. Please feel free to reopen if there are any unresolved issues.