Change all instances of taking argument "const Eigen::VectorXd &" to "const ConstVectorRef&"

bindings/expose-mpc.cpp

@@ -11,6 +11,7 @@
 
 #include <eigenpy/deprecation-policy.hpp>
 #include <eigenpy/eigenpy.hpp>
+#include <eigenpy/std-unique-ptr.hpp>
 #include <eigenpy/std-vector.hpp>
 
 #include "simple-mpc/mpc.hpp"
@@ -24,7 +25,7 @@ namespace simple_mpc
     namespace bp = boost::python;
     using eigenpy::StdVectorPythonVisitor;
 
-    void initialize(MPC & self, const bp::dict & settings, std::shared_ptr<OCPHandler> problem)
+    MPC * createMPC(const bp::dict & settings, std::shared_ptr<OCPHandler> problem)
     {
       MPCSettings conf;
 
@@ -42,7 +43,7 @@ namespace simple_mpc
       conf.T_contact = bp::extract<int>(settings["T_contact"]);
       conf.timestep = bp::extract<double>(settings["timestep"]);
 
-      self.initialize(conf, problem);
+      return new MPC{conf, problem};
     }
 
     bp::dict getSettings(MPC & self)
@@ -72,9 +73,8 @@ namespace simple_mpc
 
       StdVectorPythonVisitor<std::vector<MapBool>, true>::expose("StdVec_MapBool");
 
-      bp::class_<MPC>("MPC", bp::no_init)
-        .def(bp::init<>(bp::args("self")))
-        .def("initialize", &initialize)
+      bp::class_<MPC, boost::noncopyable>("MPC", bp::no_init)
+        .def("__init__", bp::make_constructor(&createMPC, bp::default_call_policies()))
         .def("getSettings", &getSettings)
         .def("generateCycleHorizon", &MPC::generateCycleHorizon, bp::args("self", "contact_states"))
         .def("iterate", &MPC::iterate, bp::args("self", "x"))
@@ -103,11 +103,7 @@ namespace simple_mpc
         .def(
           "getCycleHorizon", &MPC::getCycleHorizon, "self"_a, bp::return_internal_reference<>(),
           "Get the cycle horizon.")
-        .def(
-          "getSolver", &MPC::getSolver, "self"_a,
-          eigenpy::deprecated_member<eigenpy::DeprecationType::DEPRECATION, bp::return_internal_reference<>>(),
-          "Get the SolverProxDDP object")
-        .def_readonly("solver", &MPC::solver_)
+        .add_property("solver", bp::make_getter(&MPC::solver_, eigenpy::ReturnInternalStdUniquePtr{}))
         .add_property("xs", &MPC::xs_)
         .add_property("us", &MPC::us_)
         .add_property("Ks", &MPC::Ks_);

bindings/expose-robot-handler.cpp

@@ -28,7 +28,7 @@ namespace simple_mpc
         .def("difference", &RobotModelHandler::difference)
         .def("shapeState", &RobotModelHandler::shapeState)
         .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
-        .def("getReferenceState", &RobotModelHandler::getReferenceState, bp::return_internal_reference<>())
+        .def("getReferenceState", &RobotModelHandler::getReferenceState)
         .def("getFootNb", &RobotModelHandler::getFootNb)
         .def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>())
         .def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>())

examples/go2_fulldynamics.py

@@ -83,8 +83,7 @@
     timestep=dt,
 )
 
-mpc = MPC()
-mpc.initialize(mpc_conf, dynproblem)
+mpc = MPC(mpc_conf, dynproblem)
 
 """ Define contact sequence throughout horizon"""
 contact_phase_quadru = {
@@ -223,17 +222,17 @@
     solve_time.append(end - start)
 
     a0 = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[0]
         .dynamics_data.continuous_data.xdot[nv:]
     )
     a1 = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[1]
         .dynamics_data.continuous_data.xdot[nv:]
     )
 
-    FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.getSolver().workspace, 0)
+    FL_f, FR_f, RL_f, RR_f, contact_states = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0)
     total_forces = np.concatenate((FL_f, FR_f, RL_f, RR_f))
     force_FL.append(FL_f)
     force_FR.append(FR_f)

examples/go2_kinodynamics.py

@@ -93,8 +93,7 @@
     timestep=0.01,
 )
 
-mpc = MPC()
-mpc.initialize(mpc_conf, dynproblem)
+mpc = MPC(mpc_conf, dynproblem)
 
 """ Define contact sequence throughout horizon"""
 contact_phase_quadru = {
@@ -227,12 +226,12 @@
     L_measured.append(mpc.getDataHandler().getData().hg.angular.copy())
 
     a0 = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[0]
         .dynamics_data.continuous_data.xdot[nv:]
     )
     a1 = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[1]
         .dynamics_data.continuous_data.xdot[nv:]
     )

examples/talos_centroidal.py

@@ -87,8 +87,7 @@
     timestep=problem_conf["timestep"],
 )
 
-mpc = MPC()
-mpc.initialize(mpc_conf, problem)
+mpc = MPC(mpc_conf, problem)
 
 """ Define contact sequence throughout horizon"""
 contact_phase_double = {
@@ -210,7 +209,7 @@
     foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetNames()]
     foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetNames()]
     dH = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[0]
         .dynamics_data.continuous_data.xdot[3:9]
     )
@@ -229,7 +228,7 @@
 
         forces = (
             mpc.us[0][: nk * force_size]
-            - 1 * mpc.getSolver().results.controlFeedbacks()[0] @ state_diff
+            - 1 * mpc.solver.results.controlFeedbacks()[0] @ state_diff
         )
 
 

examples/talos_fulldynamics.py

@@ -112,8 +112,7 @@
     timestep=problem_conf["timestep"],
 )
 
-mpc = MPC()
-mpc.initialize(mpc_conf, dynproblem)
+mpc = MPC(mpc_conf, dynproblem)
 
 """ Define contact sequence throughout horizon"""
 contact_phase_double = {

examples/talos_kinodynamics.py

@@ -123,8 +123,7 @@
     timestep=problem_conf["timestep"],
 )
 
-mpc = MPC()
-mpc.initialize(mpc_conf, problem)
+mpc = MPC(mpc_conf, problem)
 
 """ Define contact sequence throughout horizon"""
 contact_phase_double = {
@@ -212,12 +211,12 @@
     end = time.time()
     print("MPC iterate = " + str(end - start))
     a0 = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[0]
         .dynamics_data.continuous_data.xdot[nv:]
     )
     a1 = (
-        mpc.getSolver()
+        mpc.solver
         .workspace.problem_data.stage_data[1]
         .dynamics_data.continuous_data.xdot[nv:]
     )

include/simple-mpc/centroidal-dynamics.hpp

@@ -87,14 +87,13 @@ namespace simple_mpc
 
     // Getters and setters for contact forces
     void setReferenceForces(const std::size_t t, const std::map<std::string, Eigen::VectorXd> & force_refs) override;
-    void
-    setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override;
+    void setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override;
     const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) override;
     void computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs);
     const Eigen::VectorXd getVelocityBase(const std::size_t t) override;
-    void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
+    void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override;
     const Eigen::VectorXd getPoseBase(const std::size_t t) override;
-    void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
+    void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override;
     const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
 

include/simple-mpc/fulldynamics.hpp

@@ -92,14 +92,13 @@ namespace simple_mpc
     void setReferencePoses(const std::size_t t, const std::map<std::string, pinocchio::SE3> & pose_refs) override;
     void setTerminalReferencePose(const std::string & ee_name, const pinocchio::SE3 & pose_ref) override;
     void setReferenceForces(const std::size_t t, const std::map<std::string, Eigen::VectorXd> & force_refs) override;
-    void
-    setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override;
+    void setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override;
     const pinocchio::SE3 getReferencePose(const std::size_t t, const std::string & cost_name) override;
     const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & cost_name) override;
     const Eigen::VectorXd getVelocityBase(const std::size_t t) override;
-    void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
+    void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override;
     const Eigen::VectorXd getPoseBase(const std::size_t t) override;
-    void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
+    void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override;
     const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
     FullDynamicsSettings getSettings()

include/simple-mpc/kinodynamics.hpp

@@ -80,14 +80,13 @@ namespace simple_mpc
     void setReferencePoses(const std::size_t i, const std::map<std::string, pinocchio::SE3> & pose_refs) override;
     void setTerminalReferencePose(const std::string & ee_name, const pinocchio::SE3 & pose_ref) override;
     void setReferenceForces(const std::size_t i, const std::map<std::string, Eigen::VectorXd> & force_refs) override;
-    void
-    setReferenceForce(const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref) override;
+    void setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref) override;
     const Eigen::VectorXd getReferenceForce(const std::size_t i, const std::string & cost_name) override;
     const pinocchio::SE3 getReferencePose(const std::size_t i, const std::string & cost_name) override;
     const Eigen::VectorXd getVelocityBase(const std::size_t t) override;
     const Eigen::VectorXd getPoseBase(const std::size_t t) override;
-    void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override;
-    void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override;
+    void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override;
+    void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override;
     const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) override;
     size_t getContactSupport(const std::size_t t) override;
 

include/simple-mpc/mpc.hpp

@@ -79,24 +79,22 @@ namespace simple_mpc
     std::shared_ptr<RobotDataHandler> data_handler_;
 
   public:
-    std::shared_ptr<SolverProxDDP> solver_;
+    std::unique_ptr<SolverProxDDP> solver_;
     Vector6d velocity_base_;
     Vector7d pose_base_;
     Eigen::Vector3d next_pose_;
     Eigen::Vector2d twist_vect_;
     MPCSettings settings_;
     std::shared_ptr<OCPHandler> ocp_handler_;
 
-    explicit MPC();
     explicit MPC(const MPCSettings & settings, std::shared_ptr<OCPHandler> problem);
-    void initialize(const MPCSettings & settings, std::shared_ptr<OCPHandler> problem);
 
     // Generate the cycle walking problem along which we will iterate
     // the receding horizon
     void generateCycleHorizon(const std::vector<std::map<std::string, bool>> & contact_states);
 
     // Perform one iteration of MPC
-    void iterate(const Eigen::VectorXd & x);
+    void iterate(const ConstVectorRef & x);
 
     void updateCycleTiming(const bool updateOnlyHorizon);
 
@@ -131,11 +129,6 @@ namespace simple_mpc
     // getters and setters
     TrajOptProblem & getTrajOptProblem();
 
-    SIMPLE_MPC_DEPRECATED_MESSAGE("The MPC::solver_ member is now public.")
-    SolverProxDDP & getSolver()
-    {
-      return *solver_;
-    }
     const RobotDataHandler & getDataHandler() const
     {
       return *data_handler_;
@@ -186,15 +179,14 @@ namespace simple_mpc
     std::map<std::string, std::vector<int>> foot_takeoff_times_, foot_land_times_;
 
     // Solution vectors for state and control
-    std::vector<Eigen::VectorXd> xs_;
-    std::vector<Eigen::VectorXd> us_;
-
+    std::vector<VectorXd> xs_;
+    std::vector<VectorXd> us_;
     // Riccati gains
-    std::vector<Eigen::MatrixXd> Ks_;
+    std::vector<MatrixXd> Ks_;
 
     // Initial quantities
-    Eigen::VectorXd x0_;
-    Eigen::VectorXd u0_;
+    VectorXd x0_;
+    VectorXd u0_;
   };
 
 } // namespace simple_mpc

include/simple-mpc/ocp-handler.hpp

@@ -82,16 +82,16 @@ namespace simple_mpc
 
     // Setter and getter for base velocity
     virtual const Eigen::VectorXd getVelocityBase(const std::size_t t) = 0;
-    virtual void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) = 0;
+    virtual void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) = 0;
 
     // Setter and getter for base pose
     virtual const Eigen::VectorXd getPoseBase(const std::size_t t) = 0;
-    virtual void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) = 0;
+    virtual void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) = 0;
 
     // Setter and getter for forces reference
     virtual void setReferenceForces(const std::size_t t, const std::map<std::string, Eigen::VectorXd> & force_refs) = 0;
     virtual void
-    setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) = 0;
+    setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) = 0;
     virtual const Eigen::VectorXd getReferenceForce(const std::size_t t, const std::string & ee_name) = 0;
     virtual const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler) = 0;
     virtual size_t getContactSupport(const std::size_t t) = 0;
@@ -100,14 +100,14 @@ namespace simple_mpc
 
     // Create one TrajOptProblem from contact sequence
     void createProblem(
-      const Eigen::VectorXd & x0,
+      const ConstVectorRef & x0,
       const size_t horizon,
       const int force_size,
       const double gravity,
       const bool terminal_constraint);
 
     // Setter and getter for control reference
-    void setReferenceControl(const std::size_t t, const Eigen::VectorXd & u_ref);
+    void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref);
     ConstVectorRef getReferenceControl(const std::size_t t);
 
     // Getter for various objects and quantities

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

@@ -125,7 +125,7 @@ namespace simple_mpc
       }
 
       void
-      setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref) override
+      setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref) override
       {
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setReferenceForce", t, ee_name, force_ref);
       }
@@ -135,7 +135,7 @@ namespace simple_mpc
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getReferenceForce", t, ee_name);
       }
 
-      void setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base) override
+      void setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base) override
       {
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setVelocityBase", t, velocity_base);
       }
@@ -145,7 +145,7 @@ namespace simple_mpc
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(Eigen::VectorXd, "getVelocityBase", t);
       }
 
-      void setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base) override
+      void setPoseBase(const std::size_t t, const ConstVectorRef & pose_base) override
       {
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(void, "setPoseBase", t, pose_base);
       }
@@ -165,7 +165,7 @@ namespace simple_mpc
         SIMPLE_MPC_PYTHON_OVERRIDE_PURE(std::size_t, "getContactSupport", t);
       }
 
-      void setReferenceControl(const std::size_t t, const Eigen::VectorXd & u_ref)
+      void setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref)
       {
         SIMPLE_MPC_PYTHON_OVERRIDE(void, OCPHandler, setReferenceControl, t, u_ref);
       }

include/simple-mpc/robot-handler.hpp

@@ -111,7 +111,7 @@ namespace simple_mpc
      * @return Eigen::VectorXd The vector that must be integrated during a unit of
      * time to go from x1 to x2.
      */
-    Eigen::VectorXd difference(const Eigen::VectorXd & x1, const Eigen::VectorXd & x2) const;
+    Eigen::VectorXd difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const;
 
     /**
      * @brief Compute reduced state from measures by concatenating q,v of the
@@ -121,10 +121,10 @@ namespace simple_mpc
      * @param v Velocity vector of the full model
      * @return const Eigen::VectorXd State vector of the reduced model.
      */
-    Eigen::VectorXd shapeState(const Eigen::VectorXd & q, const Eigen::VectorXd & v) const;
+    Eigen::VectorXd shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const;
 
     // Const getters
-    const Eigen::VectorXd & getReferenceState() const
+    ConstVectorRef getReferenceState() const
     {
       return reference_state_;
     }
@@ -195,8 +195,8 @@ namespace simple_mpc
     RobotDataHandler(const RobotModelHandler & model_handler);
 
     // Set new robot state
-    void updateInternalData(const Eigen::VectorXd & x, const bool updateJacobians);
-    void updateJacobiansMassMatrix(const Eigen::VectorXd & x);
+    void updateInternalData(const ConstVectorRef & x, const bool updateJacobians);
+    void updateJacobiansMassMatrix(const ConstVectorRef & x);
 
     // Const getters
     const SE3 & getRefFootPose(const std::string & foot_name) const