Different ways to compute the end-effector linear velocity in base frame

[MMarcus95]

Hello,

I'm playing with Pinocchio (in C++) to understand a bit the library and I'm trying to compute the linear velocity of the end-effector of a robot, expressed in base frame. I have a frame attached to my end-effector.

I tried three methods and before each of them I call forwardKinematics(model,data,q,qd) and updateFramePlacements(model,data):

- first method: use getFrameVelocity --> got expected results

pinocchio::SE3 bMo = data.oMf[base_frame_id].inverse();
v_ee = bMo.rotation()*pinocchio::getFrameVelocity(model, data, ee_frame_id, pinocchio::LOCAL_WORLD_ALIGNED).linear()

- second method: use computeJointJacobian --> got expected results, same as first method

pinocchio::SE3 bMo = data.oMf[base_frame_id].inverse();
pinocchio::context::Data::Matrix6x J(6, model.nv);
J.setZero();
pinocchio::computeJointJacobian(model, data, q, parent_joint_id, J); //parent_joint_id is the id of the parent joint of the ee frame
auto local_vel = J*qd;
auto local_ee_transl = model.frames[ee_frame_id].placement.translation();
auto oMi = data.oMi[parent_joint_id];
v_ee = baseMo.rotation()*oMi.rotation()*(local_vel.block<3,1>(0,0) + local_vel.block<3,1>(3,0).cross(local_ee_transl));

• third method: use computeFrameJacobian --> wrong results

pinocchio::SE3 bMo = data.oMf[base_frame_id].inverse();
pinocchio::context::Data::Matrix6x J(6, model.nv);
J.setZero();
pinocchio::computeFrameJacobian(model, data, q, ee_frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J);
v_ee = bMo.rotation()*(J.block<3,1>(0,0)*qd);

Could someone explain me why the third method is not working? I expected to have the same results of the first and second methods.

Thanks!

[jorisv]

Hello @MMarcus95,

The following line look wrong to me

v_ee = bMo.rotation()*(J.block<3,1>(0,0)*qd);

You should replace it by

v_ee = bMo.rotation()*(J.topRows<3>()*qd);