Merge pull request #60 from Simple-Robotics/topic/terminal_constraint

Change terminal constraint to produce better jumping motions

Author: edantec

src/fulldynamics.cpp

@@ -3,7 +3,6 @@
 
 #include "aligator/modelling/dynamics/multibody-constraint-fwd.hpp"
 #include <aligator/modelling/dynamics/integrator-semi-euler.hpp>
-#include <aligator/modelling/multibody/center-of-mass-translation.hpp>
 #include <aligator/modelling/multibody/centroidal-momentum.hpp>
 #include <aligator/modelling/multibody/contact-force.hpp>
 #include <aligator/modelling/multibody/dcm-position.hpp>
@@ -26,7 +25,6 @@ namespace simple_mpc
   using FrameTranslationResidual = FrameTranslationResidualTpl<double>;
   using FrameVelocityResidual = FrameVelocityResidualTpl<double>;
   using DCMPositionResidual = DCMPositionResidualTpl<double>;
-  using CenterOfMassTranslationResidual = CenterOfMassTranslationResidualTpl<double>;
   using IntegratorSemiImplEuler = dynamics::IntegratorSemiImplEulerTpl<double>;
 
   FullDynamicsOCP::FullDynamicsOCP(const FullDynamicsSettings & settings, const RobotModelHandler & model_handler)
@@ -423,9 +421,7 @@ namespace simple_mpc
 
     term_cost.addCost(
       "state_cost", QuadraticStateCost(ter_space, nu_, model_handler_.getReferenceState(), settings_.w_x));
-    /* term_cost.addCost(
-        "centroidal_cost",
-        QuadraticResidualCost(ter_space, cent_mom, settings_.w_cent)); */
+    term_cost.addCost("centroidal_cost", QuadraticResidualCost(ter_space, cent_mom, settings_.w_cent * 10));
 
     return term_cost;
   }
@@ -436,30 +432,12 @@ namespace simple_mpc
     {
       throw std::runtime_error("Create problem first!");
     }
-    CenterOfMassTranslationResidual com_cstr =
-      CenterOfMassTranslationResidual(ndx_, nu_, model_handler_.getModel(), com_ref);
 
     double tau = sqrt(com_ref[2] / 9.81);
     DCMPositionResidual dcm_cstr = DCMPositionResidual(ndx_, nu_, model_handler_.getModel(), com_ref, tau);
 
     problem_->addTerminalConstraint(dcm_cstr, EqualityConstraint());
 
-    Motion v_ref = Motion::Zero();
-    for (auto const & name : model_handler_.getFeetNames())
-    {
-      FrameVelocityResidual frame_vel = FrameVelocityResidual(
-        ndx_, nu_, model_handler_.getModel(), v_ref, model_handler_.getFootId(name), pinocchio::LOCAL_WORLD_ALIGNED);
-      if (settings_.force_size == 6)
-        problem_->addTerminalConstraint(frame_vel, EqualityConstraint());
-      else
-      {
-        std::vector<int> vel_id = {0, 1, 2};
-
-        FunctionSliceXpr vel_slice = FunctionSliceXpr(frame_vel, vel_id);
-        problem_->addTerminalConstraint(vel_slice, EqualityConstraint());
-      }
-    }
-
     terminal_constraint_ = true;
   }
 

src/kinodynamics.cpp

@@ -4,9 +4,9 @@
 #include <aligator/modelling/centroidal/centroidal-wrench-cone.hpp>
 #include <aligator/modelling/dynamics/integrator-semi-euler.hpp>
 #include <aligator/modelling/dynamics/kinodynamics-fwd.hpp>
-#include <aligator/modelling/multibody/center-of-mass-translation.hpp>
 #include <aligator/modelling/multibody/centroidal-momentum-derivative.hpp>
 #include <aligator/modelling/multibody/centroidal-momentum.hpp>
+#include <aligator/modelling/multibody/dcm-position.hpp>
 #include <aligator/modelling/multibody/frame-placement.hpp>
 #include <aligator/modelling/multibody/frame-translation.hpp>
 #include <aligator/modelling/multibody/frame-velocity.hpp>
@@ -23,8 +23,8 @@ namespace simple_mpc
   using FramePlacementResidual = FramePlacementResidualTpl<double>;
   using FrameTranslationResidual = FrameTranslationResidualTpl<double>;
   using FrameVelocityResidual = FrameVelocityResidualTpl<double>;
-  using CenterOfMassTranslationResidual = CenterOfMassTranslationResidualTpl<double>;
   using IntegratorSemiImplEuler = dynamics::IntegratorSemiImplEulerTpl<double>;
+  using DCMPositionResidual = DCMPositionResidualTpl<double>;
 
   KinodynamicsOCP::KinodynamicsOCP(const KinodynamicsSettings & settings, const RobotModelHandler & model_handler)
   : Base(model_handler)
@@ -367,21 +367,20 @@ namespace simple_mpc
     {
       throw std::runtime_error("Create problem first!");
     }
-    CenterOfMassTranslationResidual com_cstr =
-      CenterOfMassTranslationResidual(ndx_, nu_, model_handler_.getModel(), com_ref);
+    double tau = sqrt(com_ref[2] / 9.81);
+    DCMPositionResidual dcm_cstr = DCMPositionResidual(ndx_, nu_, model_handler_.getModel(), com_ref, tau);
 
-    problem_->addTerminalConstraint(com_cstr, EqualityConstraint());
+    problem_->addTerminalConstraint(dcm_cstr, EqualityConstraint());
     terminal_constraint_ = true;
   }
 
   void KinodynamicsOCP::updateTerminalConstraint(const Eigen::Vector3d & com_ref)
   {
     if (terminal_constraint_)
     {
-      CenterOfMassTranslationResidual * CoMres =
-        problem_->term_cstrs_.getConstraint<CenterOfMassTranslationResidual>(0);
+      DCMPositionResidual * DCMres = problem_->term_cstrs_.getConstraint<DCMPositionResidual>(0);
 
-      CoMres->setReference(com_ref);
+      DCMres->setReference(com_ref);
     }
   }