[dart] Compute Inverse Dynamics for all skeletons to populate joint forces

[diegoferigo]

From @azeey's #124 (comment):

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.

I'm not sure I got what you meant in the alternative option. Contrarily to what I initially proposed in #124 (comment), this PR computes ID for all simulated models in the SimulationFeatures::WorldForwardStep function. This prevents computing ID every time JointFeatures::GetJointForce is called.

Fixes #124.

[mxgrey]

If the goal of this PR is just to make sure that the joint forces aren't being cleared between simulation steps, then we should simply change line 54 to world->step(false);. Computing inverse dynamics here will be very very computationally expensive in comparison to just turning off the behavior that clears out the joint forces.

[mxgrey]

One thing that ignition-gazebo developers need to decide is what is the desire behavior for default joint forces? If a user doesn't specify a joint force, should the joint force be zero, or should it retain the last joint force that it was set to?

If the default joint force should always be zero, then we'll want to do what @azeey suggested and use world->step(false);, then save the joint forces in some separate data structure, and then clear out the joint forces in the skeleton.

[diegoferigo]

Thanks @mxgrey for the explanation, now I got what @azeey meant. It's not clear to me the rationale behind clearing the joint forces when stepping the world. Can any of the developers shed some light on this need?

If the physics would provide joint forces, ign-gazebo's Physics system could read them and fill the JointForce component as discussed in #124.

[mxgrey]

It's not clear to me the rationale behind clearing the joint forces when stepping the world.

The question is this: If the user does not explicitly set a joint force command between the last simulation step and the next one, then what should its fallback value be? The most obvious choices are:

1. The last joint force that was commanded (this would be the behavior if you don't clear the joint forces)
2. No joint force at all, i.e. passive dynamics (this would be the behavior if you do clear the joint forces)

If every joint force is guaranteed to get set between every simulation step, then the question is irrelevant in terms of the simulation outcome (although option 1 might be marginally more efficient for performance).

But in my experience it's pretty common to have simulations where some joints simply stop receiving force commands, with the expectation that the joint will start exhibiting its passive dynamics of swinging freely. For that reason, option 2 is likely to be the most intuitive for users. But then if the user needs to know the most recent joint force commands, then option 2 would interfere with that.

So I'd suggest not having DART clear the joint forces, and instead have the ignition-physics save the forces in a separate container and then clear the forces in DART at each step. Unless there's some desire for a behavior where the simulation should use the last joint force command that was given, even if no new command have come in. That's ultimately up to the ignition-gazebo architects to decide what the appropriate default behavior is.

[diegoferigo]

I dag a bit more in the APIs, to be more clear, "clearing joint forces" with world->step(); means calling DART's:

  /// Calculate the dynamics and integrate the world for one step
  /// \param[in] _resetCommand True if you want to reset to zero the joint
  /// command after simulation step.
  void step(bool _resetCommand = true);

If I got it, using step(true) prevents a previous command to be applied also in subsequent steps in absence of new commands. Since you mentioned it, this parameter would not be enough to expose the passive dynamics in any case, right? I didn't try myself, but I suspect that the only way to get the passive dynamics is computing ID on the skeletons.

But in my experience it's pretty common to have simulations where some joints simply stop receiving force commands, with the expectation that the joint will start exhibiting its passive dynamics of swinging freely. For that reason, option 2 is likely to be the most intuitive for users. But then if the user needs to know the most recent joint force commands, then option 2 would interfere with that.

I think what you described here is what I would expect as well. Although, I fear that using the same APIs to handle both the "measurement" (let's assume we consider the joint forces as the output of perfect joint torque sensors) and the command could not be the optimal solution. Why not separating the two things? Would it make sense?

Staying in the ign-physics layer, I would expect that the exposed joint force is always the current joint force of the model. If there was a joint force command and this command bypasses friction (some more detail in gazebosim/gz-sim#407), the returned force would match the command. If, instead, there was no command, I'd expect the joint force to be the passive one. This way, who reads the joint forces always has the right value, regardless of what was done before the step.

Then, if the last command has to be retrieved, both ign-physics and ign-gazebo could handle it. In the ign-physics case, maybe one way to go would be adding a new API that gathers the last command. In the ign-gazebo side, instead, the content of the last JointForceCmd could be copied to a new component before being cleared out so that users can read it if they need.

So I'd suggest not having DART clear the joint forces, and instead have the ignition-physics save the forces in a separate container and then clear the forces in DART at each step. Unless there's some desire for a behavior where the simulation should use the last joint force command that was given, even if no new command have come in. That's ultimately up to the ignition-gazebo architects to decide what the appropriate default behavior is.

To repeat, if I understood, this approach would allow to clear out the forces and exposing the commands. It's not yet clear to me how to deal with the passive dynamics.

[mxgrey]

Okay, I think I understand where the disconnect was. I wasn't accounting for joint axis friction when talking about the passive dynamics.

I believe joint axis friction is accounted for in the constraint solver, so it's kept separate from the joint force command. You're right that it wouldn't show up in the joint forces that get reported by copying the joint force command fields the way that I suggested. And I believe you're also right that, without making some changes to the internals of DART, computing the inverse dynamics after the simulation step would be the most straightforward way to obtain the joint forces that account for friction.

However, computing the inverse dynamics is not a free lunch. Since DART uses Featherstone's ABA, it's not outrageously costly, but it's still not free. Since many users will care more about performance than they do about getting this information, I don't think we should always compute the inverse dynamics.

I think I would propose adding a feature to ignition-physics for retrieving the inverse dynamics calculations. This would be an "optional" feature, so physics engines that don't support computing inverse dynamics can just skip it. Then ignition-gazebo would need to add one or two component(s) for requesting/retrieving the inverse dynamics calculations. If the inverse dynamics aren't requested, then ignition-gazebo simply skips trying to use that feature.

[diegoferigo]

Okay, I think I understand where the disconnect was. I wasn't accounting for joint axis friction when talking about the passive dynamics.

In general, I find confusing if a physics engine for some operation takes into account friction and for some other doesn't. There are commonly used engines out there that work like that, unfortunately. I'm noticing some recent activity in the implementation of the bullet engine. Standing on the Python API that I'm more familiar with, it seems that it just provides the last command:

getJointState
[...]
appliedJointMotorTorque: This is the motor torque applied during the last stepSimulation. Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque is exactly what you provide, so there is no need to report it separately.

And also calling ID would not take friction into account:

Also note that calculateInverseDynamics ignores the joint/link damping, while forward dynamics (in stepSimulation) includes those damping terms. So if you want to compare the inverse dynamics and forward dynamics, make sure to set those damping terms to zero using changeDynamics with jointDamping and link damping through linearDamping and angularDamping.

I'm not sure if the C / C++ APIs would provide more information.

However, computing the inverse dynamics is not a free lunch. Since DART uses Featherstone's ABA, it's not outrageously costly, but it's still not free. Since many users will care more about performance than they do about getting this information, I don't think we should always compute the inverse dynamics.

I totally agree with you, especially for large words, calling ID on all models would not be worth if the data is not necessary.

I think I would propose adding a feature to ignition-physics for retrieving the inverse dynamics calculations. This would be an "optional" feature, so physics engines that don't support computing inverse dynamics can just skip it. Then ignition-gazebo would need to add one or two component(s) for requesting/retrieving the inverse dynamics calculations. If the inverse dynamics aren't requested, then ignition-gazebo simply skips trying to use that feature.

Awesome, thanks! Just to put another option on the table, instead of creating new APIs, what about enabling the ID computation lazily? And I mean, the first call to getForce could enable ID computation for the selected model. At first glance the problem I see is that getForce is marked as const, therefore some ugly const-workarounds are necessary. I tried to prototype this approach in e5d6f57.

I think that from the ign-gazebo point of view, it is not completely clear what the JointForce component should store. When I think about reading joint force data, my personal definition is what I described in #143 (comment).

[diegoferigo]

I just realized that the JointFeatures::GetJointForce method will likely be the one that will be used to implement F/T sensors (still missing), similarly to what is done in Gazebo Classic (ForceTorqueSensor.cc). The solution of this PR should also take into account this use-case.

Edit: I was wrong. Af first sight I thought that ODEJoint::GetForceTorque was using that method to compute the wrench, instead it just corrects the internal torques for specific joint types. DartJoint::GetForceTorque has a different implementation.

[azeey]

I haven't had a chance to look at this in a while. I noticed you mentioned DartJoint::GetForceTorque from Gazebo classic. Would it be possible to do something similar here instead of computing the inverse dynamics for the whole skeleton?

[diegoferigo]

I still haven't wrapped my head around this. I think what I wrote in #143 (comment) could be useful to take two birds with one stone:

• Expose joint torques
• Implement F/T sensors

It seems to me that these two cases could be mapped exactly to what described in Modeling a Real Force/Torque Sensor and could share the implementation (and maybe tested against ID?). Gazebo Classic's DartJoint::GetForceTorque still performs some operations that are not 100% clear to me, but I see that a lot of people already checked it (cc @traversaro).

I don't have anything against a new PR superseding this one that ports Gazebo Classic's approach to the DART plugin. My only doubt is how the optional presence of joint friction would affect the resulting value. In this moment, reading again gazebosim/gz-sim#407 and the linked resources still does not create a clear picture in my mind, further discussion might be necessary.

xref gazebosim/gazebo-classic#1321 robotology/gazebo-yarp-plugins#471

[azeey]

I started looking into this yesterday and I think there's a piece missing from this needed to implement Force/Torque sensors, namely, the constraint forces on the joint. For example, to implement a 6 axis Force/Torque sensor, we would use a fixed joint between two links and get the constraint forces on the joint. But the approach in this PR gives the forces only in the non-constrained DOF of the joint and calling getForce on a fixed joint is ill-defined.

I'm going to see if we can use dart::dynamics::Joint::getBodyConstraintWrench to get just constraint forces in a separate ign-physics feature.

[azeey]

I've created #283, which exposes joint constraint forces.

@diegoferigo If we just want the applied force minus joint friction, spring stiffness, etc, I'm not sure inverse dynamics is what we need. I tested this on a pendulum without any applied force and no joint friction, etc. and I get non-zero values. This is because the inverse dynamics algorithm gives you the torque necessary to create joint acceleration, so if the pendulum is swinging due to gravity, we get non-zero values because the joint is accelerating. Is that what you expect from calling GetForce?

[diegoferigo]

Thanks @azeey for having a look at this! I think that a practical example could help in this case.

Let's assume we want to design a joint torque controller, i.e. something that sends joint torque references and closes the loop on joint torque measurements (something similar to what discussed in robotology/gazebo-yarp-plugins#471). Practically, the joint torque references could be stored by the user in the JointForceCmd component, and the joint torque measurements could be gathered from the JointForce component.

In the current status of Ignition, there's no way to populate JointForce with the measured joint torque. There are two cases to consider: when a JointForceCmd is present (i.e. the user has a torque reference to actuate), and when there's no JointForceCmd:

1. In the first case, I'd expect that the commanded torque is forwarded to the physics engine and, in absence of lossy behaviors, actuates exactly the command and populates JointForce=JointForceCmd. In presence of lossy behaviors, instead, I'd expect that part of the torque is lost and the resulting JointForce≠JointForceCmd.
2. In the second case, that could happen in the steps subsequent of the application of JointForceCmd (since it is cleared after the first application), the joint torque corresponding to the passive dynamics would populate JointForce. Also in this case, the presence of lossy components like friction would affect the measurement.

This should match what discussed above in in #143 (comment) (please correct me if I'm either wrong or misunderstood), particularly in option 2:

If the user does not explicitly set a joint force command between the last simulation step and the next one, then what should its fallback value be?
[...]
2. No joint force at all, i.e. passive dynamics

Then, if we want to also provide the user the last applied command, a new component could be created (LastJointForceCmd?). I'm not sure how helpful it would be, in most cases users already know the command since they sent it in the first place. Do you see any flaw, limitation, or edge cases? cc-ing also @traversaro for his experience on similar applications, feel free to either correct me if I was wrong or add your two cents.

[traversaro]

Hi all,
first of all, sorry for joining the discussion so late, I never found the time to look into the details of this.

Recap

TL;DR: I feel that joint input force and joint mechanical force (see later for strict definitions) are different concepts and for this reason it make sense to have separate methods to read them. If GetForce already returns joint input force, we should have a separate method to return joint mechanical force. To compute in DART the joint mechanical force, there is no need for computing a complete ID pass.

Details

Naming clarification

As a first step, I feel that the first thing that we may need to do is to clarify naming w.r.t. to this variables we are discussing about, as I feel that "Joint Force" is hopefully overloaded with different meanings across different discussion, source code and documentation to be useful. : )

Quoting from the related discussion robotology/gazebo-yarp-plugins#471 :

The rigid body simulators usually, i.e. in formulas, permit to set the quantity \tau_{sim} :

On the other hand, the torque control mode on real robots used by impedance controllers and whole-body-controllers typically aims to control/regulated (using feed-forward terms and feedback control laws, if some kind of torque feedback is available) the quantity \tau_{control}:

To avoid confusion, I propose at least in the rest of the discussion to refer to this two quantities as:

• \tau_sim : joint input force, as it is the one that it can be set as input
• \tau_control : joint mechanical force, as it is the one that is transmitted to the mechanical structure

I would be happy to change or bikeshed this names, but I feel that is fundamental to use separate names for these two concepts to avoid confusion.

Additional required features

I think that now that if we agree that joint input force and joint mechanical force are both interesting quantities, I think we should clarify how this two quantities are already exposed or how we want them to be exposed.

From what understand (but please correct me if this is not the case), ignition::physics::GetBasicJointState::GetForce is meant to return the joint input force (see https://github.com/ignitionrobotics/ign-physics/blob/ignition-physics4_4.2.0/include/ignition/physics/Joint.hh#L160, probably this can be clarified in the docs), so if we also want to expose the joint mechanical force we may need to add a new dedicated feature/method only for it, a bit like is done in https://github.com/ignitionrobotics/ign-physics/pull/283/files .

How to compute joint mechanical force in DART

Once we added a new method for exposing the joint mechanical force as discussed in the previous section, we need a way to compute it with the support physics engines, in particular (as this is the topic for this PR). For this, I don't think it is necessary to call any InverseDynamics call.

In particular, if you check how a simulation step in DART works, you can see that in the Skeleton::ComputeForwardDynamics method, the compute joint force torque transmitted by a body to its parent are stored in https://github.com/dartsim/dart/blob/v6.11.1/dart/dynamics/Skeleton.cpp#L3743, and can then be accessed via the BodyNode::getBodyForce() method. Furthermore, in there you can see that also the joint input force is stored, in particular by the bodyNode->updateJointForceFD(mAspectProperties.mTimeStep, true, true) method. If you follow the call stack, you see that for non-fixed joints that method eventually calls the GenericJoint::updateForceID. There we can see that the joint mechanical force can be easily computed as, given a pointer to a joint:

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

[traversaro]

@azeey if you think that a quick chat on this over a virtual whiteboard can be useful for you, feel free to propose a time, thanks!

[azeey]

@azeey if you think that a quick chat on this over a virtual whiteboard can be useful for you, feel free to propose a time, thanks!

That sounds great. I'd like @scpeters to joint if he can, so would next Thursday, September 2, at 4pm or 5 pm UTC work?

[traversaro]

@azeey if you think that a quick chat on this over a virtual whiteboard can be useful for you, feel free to propose a time, thanks!

That sounds great. I'd like @scpeters to joint if he can, so would next Thursday, September 2, at 4pm or 5 pm UTC work?

The time slot is fine for me! I think on our side @diegoferigo may be interested as well on joining.

[diegoferigo]

@azeey if you think that a quick chat on this over a virtual whiteboard can be useful for you, feel free to propose a time, thanks!

That sounds great. I'd like @scpeters to joint if he can, so would next Thursday, September 2, at 4pm or 5 pm UTC work?

The time slot is fine for me! I think on our side @diegoferigo may be interested as well on joining.

Interested! Though, I might have to leave around 4:30pm / 4:45pm UTC.

[scpeters]

@azeey if you think that a quick chat on this over a virtual whiteboard can be useful for you, feel free to propose a time, thanks!

That sounds great. I'd like @scpeters to joint if he can, so would next Thursday, September 2, at 4pm or 5 pm UTC work?

The time slot is fine for me! I think on our side @diegoferigo may be interested as well on joining.

Interested! Though, I might have to leave around 4:30pm / 4:45pm UTC.

I can join at 4pm UTC

[vatanaksoytezer]

So just to follow up on the topic: did the chat happened or is there any consensus on how joint forces should be calculated yet? I see most of Force Torque sensor and related PRs merged, but if I am not mistaken they should not work without this and gazebosim/gz-sim#952.

[azeey]

Yeah, we had a consensus and the feature is implemented in #283. That and gazebosim/gz-sim#989 was used to implement Force/Torque sensors. We also discussed that the Joint::GetForce function in ign-physics should just provide the last commanded force on the joint. This is what's done in Gazebo-classic. And for convenience, we plan to create another feature that provides the measured force in the DOF of the joint. I say it's a convenience function because the value can be obtained from Joint::GetTransmittedWrench by projecting the force or the torque in the axis of the DOF of the joint. So, I'll go ahead and close this PR.