gergondet
commented
3 years ago Hi @anastasiabolotnikova
Thanks for the very clear description and providing a sample. In tau = J^t f, f is the force applied at the contact point and J is the jacobian of the contact body at the contact location (i.e. bodyJacobian in RBDyn). However, MultiBodyConfig::force are given in the world/inertia frame.
If you apply the following changes, you get the result you're expecting:
Eigen::MatrixXd jacMat = jac.bodyJacobian(robot_module->mb, robot_module->mbc);Eigen::MatrixXd extTauFromJac = fullJacMat.transpose() * (robot_module->mbc.bodyPosW[linkIndex].dualMul(robot_module->mbc.force[linkIndex])).vector();mc_rtc::log::info("{}", robot_module->mbc.bodyVelB[linkIndex]);(not strictly needed but bodyJac * dq gives bodyVelB not bodyVelW)
Below the full modified code if you just want to copy/paste it:
#include <RBDyn/ID.h>
#include <RBDyn/FV.h>
#include <RBDyn/FK.h>
#include <RBDyn/Jacobian.h>
#include <mc_rbdyn/RobotLoader.h>
int main()
{
// Load robot
std::shared_ptr<mc_rbdyn::RobotModule> robot_module;
robot_module = mc_rbdyn::RobotLoader::get_robot_module("env", "../data/3LinkRobot", "3LinkRobot");
mc_rtc::log::info("Loaded robot {}\n", robot_module->name);
// Link for applying external force
std::string linkName = "link3";
unsigned int linkIndex = robot_module->mb.bodyIndexByName(linkName);
robot_module->mbc.gravity = Eigen::Vector3d(0,0,9.81);
robot_module->mbc.zero(robot_module->mb);
// Robot configuration
Eigen::Vector3d jointPos(0.1, 0.2, 0.3);
Eigen::Vector3d jointVel(0.0, 0.1, 0.0);
rbd::vectorToParam(jointPos, robot_module->mbc.q);
rbd::vectorToParam(jointVel, robot_module->mbc.alpha);
// Update robot configuration
rbd::forwardKinematics(robot_module->mb, robot_module->mbc);
rbd::forwardVelocity(robot_module->mb, robot_module->mbc);
// Compute joint torque
rbd::InverseDynamics id(robot_module->mb);
id.inverseDynamics(robot_module->mb, robot_module->mbc);
// Save results
Eigen::Vector3d tauNoExtForce;
rbd::paramToVector(robot_module->mbc.jointTorque, tauNoExtForce);
mc_rtc::log::info("Joint torque with no external force: {}", tauNoExtForce.transpose());
// Reset robot configuration
robot_module->mbc.zero(robot_module->mb);
rbd::vectorToParam(jointPos, robot_module->mbc.q);
rbd::vectorToParam(jointVel, robot_module->mbc.alpha);
rbd::forwardKinematics(robot_module->mb, robot_module->mbc);
rbd::forwardVelocity(robot_module->mb, robot_module->mbc);
// Add external force to a body
robot_module->mbc.force[linkIndex] = sva::ForceVecd(Eigen::Vector3d(0.,0.,0.), Eigen::Vector3d(0.0,0.0,6.0));
// Compute joint torques
id.inverseDynamics(robot_module->mb, robot_module->mbc);
// Save results
Eigen::Vector3d tauWithExtForce;
rbd::paramToVector(robot_module->mbc.jointTorque, tauWithExtForce);
mc_rtc::log::info("Joint torque with external force: {}\n", tauWithExtForce.transpose());
// Compute joint torque caused by external force via difference of two previously computed ID
Eigen::Vector3d tauDiff = tauWithExtForce - tauNoExtForce;
mc_rtc::log::info("Joint torque due to external force (from InvDyn difference):");
mc_rtc::log::info("{}", tauDiff.transpose());
// Compute Jacobian of the link where external force was applied
rbd::Jacobian jac(robot_module->mb, linkName);
Eigen::MatrixXd jacMat = jac.bodyJacobian(robot_module->mb, robot_module->mbc);
Eigen::MatrixXd fullJacMat(6, robot_module->mb.nrDof());
jac.fullJacobian(robot_module->mb, jacMat, fullJacMat);
// Compute joint torques generated by external force via Jacobian (tau = J^T f)
mc_rtc::log::info("Joint torque due to external force (from Jacobian):");
Eigen::MatrixXd extTauFromJac = fullJacMat.transpose() * (robot_module->mbc.bodyPosW[linkIndex].dualMul(robot_module->mbc.force[linkIndex])).vector();
mc_rtc::log::info("{}\n", extTauFromJac.transpose());
// Check that computed Jacobian J satisfies v = J * dq
mc_rtc::log::info("Body velocity from Jacobian");
mc_rtc::log::info("{}", (fullJacMat * jointVel).transpose());
mc_rtc::log::info("Body velocity from forward velocity");
mc_rtc::log::info("{}", robot_module->mbc.bodyVelB[linkIndex]);
return 0;
}
anastasiabolotnikova
Hello,
I am trying to use the Jacobian transpose to calculate the part of the robot joint torque that is caused by an external force applied on a robot body:
tau = J^T f_{ext}. However, I am facing the problem that the computation results do not match with analogous computations via Inverse Dynamics (ID).First, I compute robot torque via ID with no external force acting on any of the robot links and save the results:
Then, I reset the robot state and add an external force to one of the links, compute the joint torques via ID and save the result:
From these two ID results, I get that the portion of the joint torque caused by the external force is:
Now, I try to compute the joint torques by multiplying the external force with Jacobian transpose of the link where force was applied
However, when I compare
tauDiffandextTauFromJac, the results are very different (normally, they should be only differ in sign). Here is a sample output:Could you please tell me what is the reason of this mismatch? Perhaps, I misunderstand something or maybe something is missing in my code? I attach here the zip of a small project containing all computations to reproduce these results: rbdyn_force.zip
Thank you in advance for helping to clarify this!