Merge pull request #74 from Simple-Robotics/debugging

Fix base velocity in Inverse Dynamics

Author: EtienneAr

bindings/module.cpp

@@ -15,8 +15,6 @@ namespace simple_mpc::python
   void exposeCentroidalOcp();
   void exposeKinodynamicsOcp();
   void exposeMPC();
-  void exposeIDSolver();
-  void exposeIKIDSolver();
   void exposeInterpolator();
   void exposeFrictionCompensation();
   void exposeInverseDynamics();

examples/go2_kinodynamics.py

@@ -289,8 +289,6 @@
         q_meas, v_meas = device.measureState()
         x_measured = np.concatenate([q_meas, v_meas])
 
-        mpc.getDataHandler().updateInternalData(x_measured, True)
-
         kino_ID.setTarget(q_interp, v_interp, acc_interp, contact_states, force_interp)
         tau_cmd = kino_ID.solve(t, q_meas, v_meas)
 

include/simple-mpc/fwd.hpp

@@ -36,8 +36,6 @@ namespace simple_mpc
   class KinodynamicsOCP;
   class CentroidalOCP;
   class OCPHandler;
-  class IDSolver;
-  class IKIDSolver;
   class FrictionCompensation;
 
   /// EIGEN TYPEDEFS

include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp

@@ -85,6 +85,8 @@ namespace simple_mpc
     tsid::solvers::HQPOutput last_solution_;
     tsid::trajectories::TrajectorySample samplePosture_;
     tsid::trajectories::TrajectorySample sampleBase_;
+    pinocchio::Motion targetVelBase_;
+    pinocchio::Motion targetAccBase_;
   };
 
 } // namespace simple_mpc

src/inverse-dynamics/kinodynamics-id.cpp

@@ -134,9 +134,11 @@ void KinodynamicsID::setTarget(
 
   // 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_);
+  /* Velocity and acceleration not set here since the actual position of the robot is needed to transform from local to
+   * world-aligned frame. Will be done in solve()
+   */
+  targetVelBase_ = v_target.head<6>();
+  targetAccBase_ = a_target.head<6>();
 
   // Foot contacts
   for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++)
@@ -214,6 +216,15 @@ void KinodynamicsID::solve(
     }
   }
 
+  // Convert robot base vel/acc from local to world-aligned frame using actual robot pose
+  const pinocchio::SE3 oMb_rotation(data_handler_.getBaseFramePose().rotation(), Eigen::Vector3d::Zero());
+  const pinocchio::Motion v_world_aligned{oMb_rotation.act(pinocchio::Motion(targetVelBase_))};
+  const pinocchio::Motion a_world_aligned{oMb_rotation.act(pinocchio::Motion(targetAccBase_))};
+  sampleBase_.setDerivative(v_world_aligned.toVector());
+  sampleBase_.setDerivative(a_world_aligned.toVector());
+  baseTask_->setReference(sampleBase_);
+
+  // Solve QP
   const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(t, q_meas, v_meas);
   last_solution_ = solver_.solve(solver_data);
   assert(last_solution_.status == tsid::solvers::HQPStatus::HQP_STATUS_OPTIMAL);