2

I am trying to calculate the result of the inverse dynamic for a plant, however, I have noticed that the returned vector doesn't consider the gravity of the links.

For example, for a simple 1 link robot with a mass of 2 Kg that is positioned at the pi/4 angle, the following inverse dynamics calculation returns 0 without considering the weight of the link.

auto multibody_forces = MultibodyForces<double>(plant);
auto tau = plant.CalcInverseDynamics(plant_context, VectorXd::Zero(1), multibody_forces);

// tau will be 0

Is this a bug or a mis-documentation for CalcInverseDynamics?

Amin Ya
  • 1,515
  • 1
  • 19
  • 30

2 Answers2

2

MultibodyPlant::CalcInverseDynamics() does not include any forces. It only performs the documented operation. That is, given a set of external forces, it performs the computation:

tau = M(q)v̇ + C(q, v)v - tau_app - ∑ J_WBᵀ(q) Fapp_Bo_W

where the applied external are supplied with external_forces.

If you would like to only include gravity in that computation, you'd need to add it to external_forces. You can do that with:

// Generalized forces due to gravity depend on the configuration and 
// therefore we must set positions "before" we make any computation.
VectorXd postions = ...  // Code to set vector of positions.
plant.SetPositions(&context, positions);

// With the configuration set, we can now compute generalized forces.
MultibodyForces<double> external_forces(plant);
external_forces.generalized_forces() = plant.CalcGravityGeneralizedForces(context);

// Finally followed by the inverse dynamics computation.
const VectorXd tau_id = plant.CalcInverseDynamics(context, known_vdot, external_forces);
Alejandro
  • 1,064
  • 1
  • 8
  • 22
  • Adding this to the generalized_forces doesn't fix the issue. See https://github.com/RobotLocomotion/drake/pull/17153 – Amin Ya May 10 '22 at 23:26
  • We found out in the review of this code in github that the problem was that the user was making the call to `CalcGravityGeneralizedForces()` before setting the state. Generalized forces do depend on the configuration and therefore state must be set (say calling `MultibodyPlant::SetPositions()`) **before** generalized forces are computed. The discussion is recorded [in the PR review here](https://reviewable.io/reviews/RobotLocomotion/drake/17153#-N1nQKQh52pGKJ4bDxJO). – Alejandro Sep 20 '22 at 13:44
1

Here is a test that confirms that MultibodyPlant::CalcInverseDynamics does include the gravity torques.

Is it possible that you are creating the plant from a urdf/sdf that might not be putting the mass where you expect? For instance, if the gravity of the link was at the joint instead of along the arm, you might see this?

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • 1
    The reason that test is correct, is because it calls `CalcForceElementsContribution()` to add gravity into `forces`. `CalcInverseDynamics()` is just the bare bones O(n) inverse dynamics computation. External forces are supplied as an argument. See detailed response. – Alejandro May 10 '22 at 13:28
  • I added a test to prove this. See https://github.com/RobotLocomotion/drake/pull/17153 – Amin Ya May 10 '22 at 23:27