Setter for state reference

bindings/expose-mpc.cpp

@@ -79,10 +79,8 @@ namespace simple_mpc
         .def("getReferencePose", &MPC::getReferencePose, bp::args("self", "t", "ee_name"))
         .def("setTerminalReferencePose", &MPC::setTerminalReferencePose, bp::args("self", "ee_name", "pose_ref"))
         .def_readwrite("velocity_base", &MPC::velocity_base_)
-        .def_readwrite("pose_base", &MPC::pose_base_)
+        .def_readwrite("x_reference", &MPC::x_reference_)
         .def_readonly("ocp_handler", &MPC::ocp_handler_)
-        .def("setPoseBase", &MPC::setPoseBase, ("self"_a, "pose_base"))
-        .def("getPoseBase", &MPC::getPoseBase, ("self"_a, "t"))
         .def("switchToWalk", &MPC::switchToWalk, ("self"_a, "velocity_base"))
         .def("switchToStand", &MPC::switchToStand, "self"_a)
         .def("getFootTakeoffCycle", &MPC::getFootTakeoffCycle, ("self"_a, "ee_name"))

bindings/expose-problem.cpp

@@ -69,6 +69,8 @@ namespace simple_mpc
         .def("getVelocityBase", bp::pure_virtual(&OCPHandler::getVelocityBase), bp::args("self", "t"))
         .def("setPoseBase", bp::pure_virtual(&OCPHandler::setPoseBase), bp::args("self", "t", "pose_base"))
         .def("getPoseBase", bp::pure_virtual(&OCPHandler::getPoseBase), bp::args("self", "t"))
+        .def("setReferenceState", bp::pure_virtual(&OCPHandler::setReferenceState), bp::args("self", "t", "x_ref"))
+        .def("getReferenceState", bp::pure_virtual(&OCPHandler::getReferenceState), bp::args("self", "t"))
         .def("getProblemState", bp::pure_virtual(&OCPHandler::getProblemState), bp::args("self", "data_handler"))
         .def("getContactSupport", bp::pure_virtual(&OCPHandler::getContactSupport), bp::args("self", "t"))
         .def("getContactState", bp::pure_virtual(&OCPHandler::getContactState), bp::args("self", "t"))

include/simple-mpc/centroidal-dynamics.hpp

@@ -97,6 +97,8 @@ namespace simple_mpc
     const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
     std::vector<bool> getContactState(const std::size_t t) override;
+    void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override;
+    const ConstVectorRef getReferenceState(const std::size_t t) override;
 
     CentroidalSettings getSettings()
     {

include/simple-mpc/fulldynamics.hpp

@@ -102,6 +102,8 @@ namespace simple_mpc
     const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
     std::vector<bool> getContactState(const std::size_t t) override;
+    void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override;
+    const ConstVectorRef getReferenceState(const std::size_t t) override;
     FullDynamicsSettings getSettings()
     {
       return settings_;
@@ -112,9 +114,6 @@ namespace simple_mpc
     FullDynamicsSettings settings_;
     ProximalSettings prox_settings_;
 
-    // State reference
-    Eigen::VectorXd x0_;
-
     // Actuation matrix
     Eigen::MatrixXd actuation_matrix_;
 

include/simple-mpc/kinodynamics.hpp

@@ -90,6 +90,8 @@ namespace simple_mpc
     const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
     std::vector<bool> getContactState(const std::size_t t) override;
+    void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override;
+    const ConstVectorRef getReferenceState(const std::size_t t) override;
 
     void computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs);
 
@@ -100,7 +102,6 @@ namespace simple_mpc
 
   protected:
     KinodynamicsSettings settings_;
-    Eigen::VectorXd x0_;
   };
 
 } // namespace simple_mpc

include/simple-mpc/mpc.hpp

@@ -88,7 +88,6 @@ namespace simple_mpc
   public:
     std::unique_ptr<SolverProxDDP> solver_;
     Vector6d velocity_base_;
-    Vector7d pose_base_;
     Eigen::Vector3d next_pose_;
     Eigen::Vector2d twist_vect_;
     MPCSettings settings_;
@@ -120,18 +119,10 @@ namespace simple_mpc
       velocity_base_ = v;
     }
 
-    void setPoseBaseFromSE3(const pin::SE3 & pose_ref)
+    void setReferenceState(const VectorXd & state_ref)
     {
-      Eigen::Map<pin::SE3::Quaternion> q{pose_base_.tail<4>().data()};
-      pose_base_.head<3>() = pose_ref.translation();
-      q = pose_ref.rotation();
+      x_reference_ = state_ref;
     }
-    SIMPLE_MPC_DEPRECATED void setPoseBase(const Vector7d & pose_ref)
-    {
-      pose_base_ = pose_ref;
-    }
-
-    ConstVectorRef getPoseBase(const std::size_t t) const;
 
     // getters and setters
     TrajOptProblem & getTrajOptProblem();
@@ -198,6 +189,7 @@ namespace simple_mpc
     std::vector<VectorXd> us_;
     // Riccati gains
     std::vector<MatrixXd> Ks_;
+    VectorXd x_reference_;
 
     // Initial quantities
     VectorXd x0_;

include/simple-mpc/ocp-handler.hpp

@@ -96,6 +96,8 @@ namespace simple_mpc
     virtual const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) = 0;
     virtual size_t getContactSupport(const std::size_t t) = 0;
     virtual std::vector<bool> getContactState(const std::size_t t) = 0;
+    virtual void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) = 0;
+    virtual const ConstVectorRef getReferenceState(const std::size_t t) = 0;
 
     /// Common functions for all problems
 
@@ -107,7 +109,7 @@ namespace simple_mpc
       const double gravity,
       const bool terminal_constraint);
 
-    // Setter and getter for control reference
+    // Setter and getter for state and control reference
     void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref);
     ConstVectorRef getReferenceControl(const std::size_t t);
 
@@ -150,6 +152,9 @@ namespace simple_mpc
     bool problem_initialized_ = false;
     bool terminal_constraint_ = false;
 
+    /// State reference
+    Eigen::VectorXd x0_;
+
     /// The robot model
     RobotModelHandler model_handler_;
 

include/simple-mpc/python/py-ocp-handler.hpp

@@ -170,6 +170,16 @@ namespace simple_mpc
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(std::vector<bool>, "getContactState", t);
       }
 
+      void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref) override
+      {
+        SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setReferenceState", t, x_ref);
+      }
+
+      const ConstVectorRef getReferenceState(const std::size_t t) override
+      {
+        SIMPLE_MPC_PYTHON_OVERRIDE_PURE(ConstVectorRef, "getReferenceState", t);
+      }
+
       void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref)
       {
         SIMPLE_MPC_PYTHON_OVERRIDE(void, OCPHandler, setReferenceControl, t, u_ref);

src/centroidal-dynamics.cpp

@@ -33,6 +33,7 @@ namespace simple_mpc
     control_ref_.resize(nu_);
     control_ref_.setZero();
     com_ref_.setZero();
+    x0_.resize(9);
   }
 
   StageModel CentroidalOCP::createStage(
@@ -225,6 +226,7 @@ namespace simple_mpc
 
   void CentroidalOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base)
   {
+    assert(velocity_base.size() == 6 && "velocity_base not of the right size");
     CostStack * cs = getCostStack(t);
     QuadraticResidualCost * qcm = cs->getComponent<QuadraticResidualCost>("linear_mom_cost");
     LinearMomentumResidual * cfm = qcm->getResidual<LinearMomentumResidual>();
@@ -246,10 +248,7 @@ namespace simple_mpc
 
   void CentroidalOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base)
   {
-    if (pose_base.size() != 7)
-    {
-      throw std::runtime_error("pose_base size should be 7");
-    }
+    assert(pose_base.size() == 3 && "pose_base not of the right size");
     CostStack * cs = getCostStack(t);
     QuadraticResidualCost * qrc = cs->getComponent<QuadraticResidualCost>("com_cost");
     CentroidalCoMResidual * cfr = qrc->getResidual<CentroidalCoMResidual>();
@@ -290,6 +289,20 @@ namespace simple_mpc
     return contact_state;
   }
 
+  void CentroidalOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref)
+  {
+    assert(x_ref.size() == 9 && "x_ref not of the right size");
+    setPoseBase(t, x_ref.head(3));
+    setVelocityBase(t, x_ref.tail(6));
+  }
+
+  const ConstVectorRef CentroidalOCP::getReferenceState(const std::size_t t)
+  {
+    x0_.head(3) = getPoseBase(t);
+    x0_.tail(6) = getVelocityBase(t);
+    return x0_;
+  }
+
   CostStack CentroidalOCP::createTerminalCost()
   {
     auto ter_space = VectorSpace(nx_);

src/fulldynamics.cpp

@@ -88,7 +88,7 @@ namespace simple_mpc
     auto space = MultibodyPhaseSpace(model_handler_.getModel());
     auto rcost = CostStack(space, nu_);
 
-    rcost.addCost("state_cost", QuadraticStateCost(space, nu_, x0_, settings_.w_x));
+    rcost.addCost("state_cost", QuadraticStateCost(space, nu_, model_handler_.getReferenceState(), settings_.w_x));
     rcost.addCost("control_cost", QuadraticControlCost(space, Eigen::VectorXd::Zero(nu_), settings_.w_u));
 
     auto cent_mom = CentroidalMomentumResidual(space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6));
@@ -337,6 +337,7 @@ namespace simple_mpc
     }
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    x0_ = getReferenceState(t);
     x0_.segment(nq_, 6) = velocity_base;
     qc->setTarget(x0_);
   }
@@ -356,6 +357,7 @@ namespace simple_mpc
     }
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    x0_ = getReferenceState(t);
     x0_.head(7) = pose_base;
     qc->setTarget(x0_);
   }
@@ -398,14 +400,30 @@ namespace simple_mpc
     return contact_state;
   }
 
+  void FullDynamicsOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref)
+  {
+    assert(x_ref.size() == nq_ + nv_ && "x_ref not of the right size");
+    CostStack * cs = getCostStack(t);
+    QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    qc->setTarget(x_ref);
+  }
+
+  const ConstVectorRef FullDynamicsOCP::getReferenceState(const std::size_t t)
+  {
+    CostStack * cs = getCostStack(t);
+    QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    return qc->getTarget();
+  }
+
   CostStack FullDynamicsOCP::createTerminalCost()
   {
     auto ter_space = MultibodyPhaseSpace(model_handler_.getModel());
     auto term_cost = CostStack(ter_space, nu_);
     auto cent_mom =
       CentroidalMomentumResidual(ter_space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6));
 
-    term_cost.addCost("state_cost", QuadraticStateCost(ter_space, nu_, x0_, settings_.w_x));
+    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)); */

src/kinodynamics.cpp

@@ -57,7 +57,7 @@ namespace simple_mpc
     auto centder_mom = CentroidalMomentumDerivativeResidual(
       space.ndx(), model_handler_.getModel(), settings_.gravity, contact_states, model_handler_.getFeetIds(),
       settings_.force_size);
-    rcost.addCost("state_cost", QuadraticStateCost(space, nu_, x0_, settings_.w_x));
+    rcost.addCost("state_cost", QuadraticStateCost(space, nu_, model_handler_.getReferenceState(), settings_.w_x));
     rcost.addCost("control_cost", QuadraticControlCost(space, control_ref_, settings_.w_u));
     rcost.addCost("centroidal_cost", QuadraticResidualCost(space, cent_mom, settings_.w_cent));
     rcost.addCost("centroidal_derivative_cost", QuadraticResidualCost(space, centder_mom, settings_.w_centder));
@@ -278,6 +278,7 @@ namespace simple_mpc
     }
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    x0_ = getReferenceState(t);
     x0_.segment(nq_, 6) = velocity_base;
     qc->setTarget(x0_);
   }
@@ -297,6 +298,7 @@ namespace simple_mpc
     }
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    x0_ = getReferenceState(t);
     x0_.head(7) = pose_base;
     qc->setTarget(x0_);
   }
@@ -306,6 +308,21 @@ namespace simple_mpc
     return data_handler.getState();
   }
 
+  void KinodynamicsOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref)
+  {
+    assert(x_ref.size() == nq_ + nv_ && "x_ref not of the right size");
+    CostStack * cs = getCostStack(t);
+    QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    qc->setTarget(x_ref);
+  }
+
+  const ConstVectorRef KinodynamicsOCP::getReferenceState(const std::size_t t)
+  {
+    CostStack * cs = getCostStack(t);
+    QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
+    return qc->getTarget();
+  }
+
   size_t KinodynamicsOCP::getContactSupport(const std::size_t t)
   {
     KinodynamicsFwdDynamics * ode =
@@ -337,7 +354,8 @@ namespace simple_mpc
     auto cent_mom =
       CentroidalMomentumResidual(ter_space.ndx(), nu_, model_handler_.getModel(), Eigen::VectorXd::Zero(6));
 
-    term_cost.addCost("state_cost", QuadraticStateCost(ter_space, nu_, x0_, settings_.w_x));
+    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 * 10));
 
     return term_cost;

src/mpc.cpp

@@ -36,6 +36,7 @@ namespace simple_mpc
 
     foot_trajectories_.updateApex(settings.swing_apex);
     x0_ = ocp_handler_->getProblemState(*data_handler_);
+    x_reference_ = ocp_handler_->getReferenceState(0);
 
     solver_ = std::make_unique<SolverProxDDP>(settings_.TOL, settings_.mu_init, maxiters, aligator::QUIET);
     solver_->rollout_type_ = aligator::RolloutType::LINEAR;
@@ -89,7 +90,7 @@ namespace simple_mpc
 
     com0_ = data_handler_->getData().com[0];
     now_ = WALKING;
-    pose_base_ = x0_.head<7>();
+
     velocity_base_.setZero();
     next_pose_.setZero();
     twist_vect_.setZero();
@@ -302,8 +303,8 @@ namespace simple_mpc
       }
     }
 
+    ocp_handler_->setReferenceState(ocp_handler_->getSize() - 1, x_reference_);
     ocp_handler_->setVelocityBase(ocp_handler_->getSize() - 1, velocity_base_);
-    ocp_handler_->setPoseBase(ocp_handler_->getSize() - 1, pose_base_);
 
     Eigen::Vector3d com_ref;
     com_ref << 0, 0, 0;
@@ -332,11 +333,6 @@ namespace simple_mpc
     return ocp_handler_->getReferencePose(t, ee_name);
   }
 
-  ConstVectorRef MPC::getPoseBase(const std::size_t t) const
-  {
-    return ocp_handler_->getPoseBase(t);
-  }
-
   TrajOptProblem & MPC::getTrajOptProblem()
   {
     return ocp_handler_->getProblem();

tests/problem.cpp

@@ -94,6 +94,13 @@ BOOST_AUTO_TEST_CASE(fulldynamics)
   pose_base << 0, 0, 2, 0, 0, 0, 1;
   fdproblem.setPoseBase(2, pose_base);
   BOOST_CHECK_EQUAL(fdproblem.getPoseBase(2), pose_base);
+
+  Eigen::VectorXd new_q = pinocchio::randomConfiguration(model_handler.getModel());
+  Eigen::VectorXd new_x(model_handler.getModel().nq + model_handler.getModel().nv);
+  new_x.head(model_handler.getModel().nq) = new_q;
+  new_x.tail(model_handler.getModel().nv).setRandom();
+  fdproblem.setReferenceState(2, new_x);
+  BOOST_CHECK_EQUAL(fdproblem.getReferenceState(2), new_x);
 }
 
 BOOST_AUTO_TEST_CASE(kinodynamics)
@@ -179,6 +186,13 @@ BOOST_AUTO_TEST_CASE(kinodynamics)
   pose_base << 0, 0, 2, 0, 0, 0, 1;
   knproblem.setPoseBase(2, pose_base);
   BOOST_CHECK_EQUAL(knproblem.getPoseBase(2), pose_base);
+
+  Eigen::VectorXd new_q = pinocchio::randomConfiguration(model_handler.getModel());
+  Eigen::VectorXd new_x(model_handler.getModel().nq + model_handler.getModel().nv);
+  new_x.head(model_handler.getModel().nq) = new_q;
+  new_x.tail(model_handler.getModel().nv).setRandom();
+  knproblem.setReferenceState(2, new_x);
+  BOOST_CHECK_EQUAL(knproblem.getReferenceState(2), new_x);
 }
 
 BOOST_AUTO_TEST_CASE(centroidal)
@@ -260,10 +274,15 @@ BOOST_AUTO_TEST_CASE(centroidal)
   BOOST_CHECK_EQUAL(cproblem.getReferencePose(3, "left_sole_link"), new_poses.at("left_sole_link"));
   BOOST_CHECK_EQUAL(cproblem.getReferencePose(3, "right_sole_link"), new_poses.at("right_sole_link"));
 
-  Eigen::VectorXd pose_base(7);
-  pose_base << 0, 0, 2, 0, 0, 0, 1;
+  Eigen::VectorXd pose_base(3);
+  pose_base << 0, 0, 2;
   cproblem.setPoseBase(2, pose_base);
-  BOOST_CHECK_EQUAL(cproblem.getPoseBase(2), pose_base.head(3));
+  BOOST_CHECK_EQUAL(cproblem.getPoseBase(2), pose_base);
+
+  Eigen::VectorXd new_x(9);
+  new_x << 0, 0, 1, 0, 0.1, 0.1, 0.2, 0.2, 0.2;
+  cproblem.setReferenceState(2, new_x);
+  BOOST_CHECK_EQUAL(cproblem.getReferenceState(2), new_x);
 }
 
 BOOST_AUTO_TEST_CASE(centroidal_solo)