Fix velocity in world aligned frame

Author: EtienneAr

src/inverse-dynamics/kinodynamics-id.cpp

@@ -133,10 +133,11 @@ void KinodynamicsID::setTarget(
   postureTask_->setReference(samplePosture_);
 
   // Base task
-  tsid::math::SE3ToVector(data_handler_.getBaseFramePose(), sampleBase_.pos);
-  sampleBase_.setDerivative(v_target.head<6>());
-  sampleBase_.setSecondDerivative(a_target.head<6>());
-  baseTask_->setReference(sampleBase_);
+  const pinocchio::SE3 oMb{data_handler_.getBaseFramePose()};
+  const pinocchio::SE3 oMb_rotation(oMb.rotation(), Eigen::Vector3d::Zero());
+  const pinocchio::Motion v_world_aligned{oMb_rotation.act(pinocchio::Motion(v_target.head<6>()))};
+  const pinocchio::Motion a_world_aligned{oMb_rotation.act(pinocchio::Motion(a_target.head<6>()))};
+  baseTask_->setReference(oMb, v_world_aligned, a_world_aligned);
 
   // Foot contacts
   for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++)