Setter for state reference

Author: edantec

bindings/expose-mpc.cpp

@@ -89,6 +89,7 @@ namespace simple_mpc
         .def("getFootLandCycle", &MPC::getFootLandCycle, ("self"_a, "ee_name"))
         .def("getStateDerivative", &MPC::getStateDerivative, ("self"_a, "t"))
         .def("getContactForces", &MPC::getContactForces, ("self"_a, "t"))
+        .def("setReferenceState", &MPC::setReferenceState, ("self"_a, "state_ref"))
         .def("getCyclingContactState", &MPC::getCyclingContactState, ("self"_a, "t", "ee_name"))
         .def(
           "getModelHandler", &MPC::getModelHandler, "self"_a, bp::return_internal_reference<>(),

bindings/expose-problem.cpp

@@ -77,6 +77,8 @@ namespace simple_mpc
           ("self"_a, "x0", "horizon", "force_size", "gravity", "terminal_constraint"))
         .def("setReferenceControl", &OCPHandler::setReferenceControl, ("self"_a, "t", "u_ref"))
         .def("getReferenceControl", &OCPHandler::getReferenceControl, ("self"_a, "t"))
+        .def("setReferenceState", &OCPHandler::setReferenceState, ("self"_a, "x_ref"))
+        .def("getReferenceState", &OCPHandler::getReferenceState, ("self"_a))
         .def("getProblem", +[](OCPHandler & ocp) { return boost::ref(ocp.getProblem()); }, "self"_a);
 
       exposeContainers();

include/simple-mpc/fulldynamics.hpp

@@ -112,9 +112,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

@@ -100,7 +100,6 @@ namespace simple_mpc
 
   protected:
     KinodynamicsSettings settings_;
-    Eigen::VectorXd x0_;
   };
 
 } // namespace simple_mpc

include/simple-mpc/mpc.hpp

@@ -131,6 +131,11 @@ namespace simple_mpc
       pose_base_ = pose_ref;
     }
 
+    void setReferenceState(const VectorXd & state_ref)
+    {
+      ocp_handler_->setReferenceState(state_ref);
+    }
+
     ConstVectorRef getPoseBase(const std::size_t t) const;
 
     // getters and setters

include/simple-mpc/ocp-handler.hpp

@@ -107,10 +107,13 @@ 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);
 
+    void setReferenceState(const ConstVectorRef & x_ref);
+    ConstVectorRef getReferenceState();
+
     // Getter for various objects and quantities
     CostStack * getCostStack(std::size_t t);
     CostStack * getTerminalCostStack();
@@ -150,6 +153,9 @@ namespace simple_mpc
     bool problem_initialized_ = false;
     bool terminal_constraint_ = false;
 
+    /// State reference
+    Eigen::VectorXd x0_;
+
     /// The robot model
     RobotModelHandler model_handler_;
 

src/mpc.cpp

@@ -303,7 +303,6 @@ namespace simple_mpc
     }
 
     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;

src/ocp-handler.cpp

@@ -69,6 +69,17 @@ namespace simple_mpc
     return qc->getTarget();
   }
 
+  void OCPHandler::setReferenceState(const ConstVectorRef & x_ref)
+  {
+    assert(x_ref.size() == x0_.size() && "x_ref not of the right size");
+    x0_ = x_ref;
+  }
+
+  ConstVectorRef OCPHandler::getReferenceState()
+  {
+    return x0_;
+  }
+
   CostStack * OCPHandler::getCostStack(std::size_t t)
   {
     if (t >= getSize())