Unable to retrieve Feed Forward Torque

[Wang-Zhicheng]

Hello there,

I'm learning about PD+Feed Forward Controllers. I wrote the code below to check the torques generated by PD+FF Controller.

quadruped->setGeneralizedForce(Eigen::VectorXd::Zero(quadruped->getDOF()));
quadruped->setControlMode(raisim::ControlMode::PD_PLUS_FEEDFORWARD_TORQUE);

...

jointTorque = quadruped->getGeneralizedForce().e().tail(12);
jointSpeed = quadruped->getGeneralizedVelocity().e().tail(12);
quadrued->getState(gc_, gv_);

Eigen::VectorXd jointNominalConfig(19), jointVelocityTarget(18);
jointVelocityTarget.setZero();
jointNominalConfig << 0, 0, 0, 0, 0, 0, 0, 0.03,-0.65,1.35, -0.03,-0.65,1.35,0.03,-0.65,1.35, -0.03,-0.65,1.35;
for (size_t k = 0; k < quadrued->getGeneralizedCoordinateDim(); k++)
    jointNominalConfig(k) += distribution(generator);
quadrued->setPdTarget(jointNominalConfig,jointVelocityTarget);

Eigen::VectorXd PDTerm(12),FFTerm(12);
PDTerm.setZero(); FFTerm.setConstant(8.3);
PDTerm = jointPgain.tail(12).cwiseProduct(jointNominalConfig.tail(12)-gc_.tail(12)) \
                +jointDgain.tail(12).cwiseProduct(jointVelocityTarget.tail(12)-jointSpeed.tail(12));
FFTerm = quadruped->getFeedForwardGeneralizedForce().e().tail(12);

cout<<"\nGeneralizedForce  PD+FF  PDTerms  FeedForward  SimTime="<<world.getWorldTime()<<endl;
for(int g=0;g<PDTerm.size();g++)
    std::cout<<jointTorque[g]<<", "<<PDTerm[g]+FFTerm[g]<<", "<<PDTerm[g]<<", "<<FFTerm[g]<<std::endl;

The code is based on the anymal.cpp example, and the code about retrieving and printing torques are located in a colreol callback function. The compiling process showed no warning or error, and the robot in simulation can move normally(and of course randomly). However, the vector FFTerm which stands for the last 12 elements of function getFeedForwardGeneralizedForce() is always an all-zero-vector.

I have tried to change the sequence of getGeneralizedForce() , setPdTarget() and getFeedForwardGeneralizedForce() but nothing changed.

A typical terminal output is as follows:

GeneralizedForce PD+FF PDTerms FeedForward SimTime=2.355 1.59563, 0.955289, 0.955289, 0 5.81237, 2.1668, 2.1668, 0 3.20339, 3.07939, 3.07939, 0 -1.65067, -0.653838, -0.653838, 0 6.39475, -3.97876, -3.97876, 0 -13.4957, -7.89707, -7.89707, 0 -1.50792, 0.0852972, 0.0852972, 0 3.06845, 2.19863, 2.19863, 0 -4.43382, 1.72279, 1.72279, 0 -3.18077, -4.48276, -4.48276, 0 3.4871, -4.09694, -4.09694, 0 -14.7907, -9.91607, -9.91607, 0

GeneralizedForce PD+FF PDTerms FeedForward SimTime=2.36 1.38818, 1.17031, 1.17031, 0 6.19792, 3.18782, 3.18782, 0 5.46882, -0.453176, -0.453176, 0 -1.14092, -1.41715, -1.41715, 0 4.14613, -4.96256, -4.96256, 0 -13.6882, -6.61298, -6.61298, 0 -0.830944, 0.76547, 0.76547, 0 4.47162, 2.74053, 2.74053, 0 -2.2879, 4.21932, 4.21932, 0 -4.00571, -4.53645, -4.53645, 0 3.56062, -4.56443, -4.56443, 0 -15.6382, -9.28111, -9.28111, 0

GeneralizedForce PD+FF PDTerms FeedForward SimTime=2.365 1.11403, 2.4202, 2.4202, 0 7.57367, 2.25886, 2.25886, 0 3.79053, -1.49852, -1.49852, 0 -1.32275, -1.95383, -1.95383, 0 3.22742, -7.94604, -7.94604, 0 -12.459, -2.05887, -2.05887, 0 0.247587, -0.183707, -0.183707, 0 5.44681, 3.65927, 3.65927, 0 2.39168, -0.645711, -0.645711, 0 -4.63927, -3.15894, -3.15894, 0 3.1721, -6.94252, -6.94252, 0 -14.8704, -5.41287, -5.41287, 0

It is kind of confusing. Am I using the function of getFFGF() in a wrong way? Or there should be other prerequisite of calling this function?

Hope to get answered soon.

Thanks a lot, sincerely.

[jhwangbo]

yes. feedforward torque is what you set by "quadruped->setGeneralizedForce(Eigen::VectorXd::Zero(quadruped->getDOF()));"

[Wang-Zhicheng]

Thank you for answering. So when under the mode "PD_PLUS_FEEDFORWARD_TORQUE", if I do not use setGeneralizedForce() to set the torque and use setPdTarget() only, the torques given to the joints are decided by the PD terms, which is calculated through the equation tauPD = PGain * (gcTarget - gc)+DGain * (velTarget - gv_) (as I had written in the code). Is that correct?

[Wang-Zhicheng]

The PD torques I manually calculated in the code seems to vary from the torques retrieved with getGeneralizedForce(). So I'm thinking whether there are other torque terms in the _PD_PLUS_FEEDFORWARDTORQUE mode. Or maybe I have made some kind of mistakes in the self-written code?

It would be appreciated if you could help me to check it out. Thanks.

[jhwangbo]

not exactly. Because RaiSim will account for the fact that gc_ is chaining in the next time step. You can get the exact force by getGeneralizedForce()

[jhwangbo]

Here is a full description

http://raisim.com/sections/ArticulatedSystem.html#pd-controller

[Wang-Zhicheng]

Oops. I get that. The calculation process in the RaiSim PD Controller is way more complex than the equation I described above... I noticed the OverLeaf document named "RaiSim Equations" linked in the RaiSim Doc. I guess that is the final version of algorithms in RaiSim built-in controllers. If true, the issue can be closed, and I will go take a deep look into it.

Again, thanks a lot for your patience and instructions.

[jhwangbo]

yes you are right. I guess simple controllers can be implemented by users. I made one that is more stable