`plant.CalcInverseDynamics` doesn't consider the gravity of the links

user_7910299 0
Viewed 57

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?

drake
2 Answers

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);
user_18899750

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?

user_95100200
Related