CODEWITHSUNDEEP
drake
Robert Szasz
Robert Szasz

Reputation: 35

How to use autoDiffToGradientMatrix to solve for Coriolis Matrix in drake?

I am trying to get the Coriolis matrix for my robot (need the matrix explicitly for the controller) based on the following approach which I have found online:

plant_.CalcBiasTerm(*context, &Cv_);
auto jac = autoDiffToGradientMatrix(Cv_);
C = 0.5*jac.rightCols(n_v_);

where Cv_, plant_, context are AutoDiffXd and n_v_ is the number of generalized velocities. So basically I have a 62-joint robot loaded from URDF into drake which is a free body (floating base system). After finalizing the robot I am using the DiagramBuilder.Build() method and then the CreateDefaultContext() in order to get the context. Next, I am trying to set up the AutoDiff environment like this:

plant_autodiff = drake::systems::System<double>::ToAutoDiffXd(*multibody_plant);
context_autodiff = plant_autodiff->CreateDefaultContext();
context_autodiff->SetTimeStateAndParametersFrom(*diagram_context);

The code above is contained in an initialization setup code. In another method, which is called on update events, the following lines of code are written:

drake::AutoDiffVecXd c_auto_diff_ = drake::AutoDiffVecXd::Zero(62);
plant_autodiff->CalcBiasTerm(*context_autodiff, &c_auto_diff_);
MatrixXd jac = drake::math::autoDiffToGradientMatrix(c_auto_diff_);
auto C = 0.5*jac.rightCols(jac.size());

This setup compiles and runs, however the size of the jac matrix is 0, whereas I would expect 62x62. I am also extracting and then exposing the Coriolis vector, which is 62x1 and seems to be more or less correct. The c_auto_diff_ variable is 62x1 as well, but all the elements are 0. I am clearly making a mistake, but I do not know where exactly.

Any help is appreciated,

Thank you all,

Robert

Upvotes: 1

Views: 173

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

You are close. You need to tell the autodiff pipeline what you want to take the derivative with respect to. In this case, I believe you want

auto v = drake::math::initializeAutoDiff(Eigen::VectorXd::Zero(62))
plant_autodiff->SetVelocities(context_autodiff.get(), v);

By calling initializeAutoDiff, you are initializing the autodiff terms to the identity matrix, which is saying that you want to take the derivative with respect to v. Then you should get non-zero derivatives.

Btw - I normally would use

plant_autodiff = multibody_plant->ToAutoDiffXd();

but I guess what you have must work, too!

Upvotes: 1

Related Questions