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

Author: ManifoldFR

include/simple-mpc/centroidal-dynamics.hpp

@@ -90,14 +90,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() override;
     size_t getContactSupport(const std::size_t t) override;
 

include/simple-mpc/fulldynamics.hpp

@@ -95,14 +95,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() override;
     size_t getContactSupport(const std::size_t t) override;
     FullDynamicsSettings getSettings()

include/simple-mpc/kinodynamics.hpp

@@ -83,14 +83,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() override;
     size_t getContactSupport(const std::size_t t) override;
 

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() = 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
+    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

src/centroidal-dynamics.cpp

@@ -190,7 +190,7 @@ namespace simple_mpc
   }
 
   void
-  CentroidalOCP::setReferenceForce(const std::size_t t, const std::string & ee_name, const Eigen::VectorXd & force_ref)
+  CentroidalOCP::setReferenceForce(const std::size_t t, const std::string & ee_name, const ConstVectorRef & force_ref)
   {
     std::vector<std::string> hname = model_handler_.getFeetNames();
     std::vector<std::string>::iterator it = std::find(hname.begin(), hname.end(), ee_name);
@@ -224,7 +224,7 @@ namespace simple_mpc
     return v;
   }
 
-  void CentroidalOCP::setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base)
+  void CentroidalOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base)
   {
     CostStack * cs = getCostStack(t);
     QuadraticResidualCost * qcm = cs->getComponent<QuadraticResidualCost>("linear_mom_cost");
@@ -245,7 +245,7 @@ namespace simple_mpc
     return cfr->getReference();
   }
 
-  void CentroidalOCP::setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base)
+  void CentroidalOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base)
   {
     if (pose_base.size() != 7)
     {

src/fulldynamics.cpp

@@ -290,8 +290,8 @@ namespace simple_mpc
     }
   }
 
-  void FullDynamicsOCP::setReferenceForce(
-    const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref)
+  void
+  FullDynamicsOCP::setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref)
   {
     CostStack * cs = getCostStack(i);
     QuadraticResidualCost * qrc = cs->getComponent<QuadraticResidualCost>(ee_name + "_force_cost");
@@ -332,7 +332,7 @@ namespace simple_mpc
     return qc->getTarget().segment(nq_, 6);
   }
 
-  void FullDynamicsOCP::setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base)
+  void FullDynamicsOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base)
   {
     if (velocity_base.size() != 6)
     {
@@ -351,7 +351,7 @@ namespace simple_mpc
     return qc->getTarget().head(7);
   };
 
-  void FullDynamicsOCP::setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base)
+  void FullDynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base)
   {
     if (pose_base.size() != 7)
     {

src/kinodynamics.cpp

@@ -247,8 +247,8 @@ namespace simple_mpc
     setReferenceControl(i, control_ref_);
   }
 
-  void KinodynamicsOCP::setReferenceForce(
-    const std::size_t i, const std::string & ee_name, const Eigen::VectorXd & force_ref)
+  void
+  KinodynamicsOCP::setReferenceForce(const std::size_t i, const std::string & ee_name, const ConstVectorRef & force_ref)
   {
     std::vector<std::string> hname = model_handler_.getFeetNames();
     std::vector<std::string>::iterator it = std::find(hname.begin(), hname.end(), ee_name);
@@ -273,7 +273,7 @@ namespace simple_mpc
     return qc->getTarget().segment(nq_, 6);
   }
 
-  void KinodynamicsOCP::setVelocityBase(const std::size_t t, const Eigen::VectorXd & velocity_base)
+  void KinodynamicsOCP::setVelocityBase(const std::size_t t, const ConstVectorRef & velocity_base)
   {
     if (velocity_base.size() != 6)
     {
@@ -292,7 +292,7 @@ namespace simple_mpc
     return qc->getTarget().head(7);
   };
 
-  void KinodynamicsOCP::setPoseBase(const std::size_t t, const Eigen::VectorXd & pose_base)
+  void KinodynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base)
   {
     if (pose_base.size() != 7)
     {

src/ocp-handler.cpp

@@ -56,7 +56,7 @@ namespace simple_mpc
     return stage_models;
   }
 
-  void OCPHandler::setReferenceControl(const std::size_t t, const Eigen::VectorXd & u_ref)
+  void OCPHandler::setReferenceControl(const std::size_t t, const ConstVectorRef & u_ref)
   {
     CostStack * cs = getCostStack(t);
     QuadraticControlCost * qc = cs->getComponent<QuadraticControlCost>("control_cost");
@@ -95,7 +95,7 @@ namespace simple_mpc
   }
 
   void OCPHandler::createProblem(
-    const Eigen::VectorXd & x0,
+    const ConstVectorRef & x0,
     const size_t horizon,
     const int force_size,
     const double gravity,