Change to Eigen::Vector

Author: edantec

bindings/expose-mpc.cpp

@@ -64,12 +64,9 @@ namespace simple_mpc
     void exposeMPC()
     {
       using StageVec = std::vector<std::shared_ptr<StageModel>>;
-      using ForceVec = std::vector<Eigen::Vector3d>;
       using MapBool = std::map<std::string, bool>;
       StdVectorPythonVisitor<StageVec, true>::expose(
         "StdVec_StageModel", eigenpy::details::overload_base_get_item_for_std_vector<StageVec>());
-      StdVectorPythonVisitor<ForceVec, true>::expose(
-        "StdVec_Force", eigenpy::details::overload_base_get_item_for_std_vector<ForceVec>());
 
       StdVectorPythonVisitor<std::vector<MapBool>, true>::expose("StdVec_MapBool");
 

examples/go2_fulldynamics.py

@@ -241,16 +241,16 @@
     a0 = mpc.getStateDerivative(0)[nv:]
     a1 = mpc.getStateDerivative(1)[nv:]
 
-    forces_vec = mpc.getContactForces(0)
+    forces_vec0 = mpc.getContactForces(0)
+    forces_vec1 = mpc.getContactForces(1)
     contact_states = mpc.ocp_handler.getContactState(0)
-    total_forces = np.concatenate((forces_vec[0], forces_vec[1], forces_vec[2], forces_vec[3]))
 
-    force_FL.append(forces_vec[0])
-    force_FR.append(forces_vec[1])
-    force_RL.append(forces_vec[2])
-    force_RR.append(forces_vec[3])
+    force_FL.append(forces_vec0[:3])
+    force_FR.append(forces_vec0[3:6])
+    force_RL.append(forces_vec0[6:9])
+    force_RR.append(forces_vec0[9:12])
 
-    forces = [total_forces, total_forces]
+    forces = [forces_vec0, forces_vec1]
     ddqs = [a0, a1]
     xss = [mpc.xs[0], mpc.xs[1]]
     uss = [mpc.us[0], mpc.us[1]]

include/simple-mpc/mpc.hpp

@@ -186,9 +186,11 @@ namespace simple_mpc
       return int_data->continuous_data->xdot_;
     }
 
-    const std::vector<Eigen::Vector3d> getContactForces(const std::size_t t)
+    const Eigen::VectorXd getContactForces(const std::size_t t)
     {
-      std::vector<Eigen::Vector3d> contact_forces;
+      Eigen::VectorXd contact_forces;
+      contact_forces.resize(3 * (long)ee_names_.size());
+
       ExplicitIntegratorData * int_data =
         dynamic_cast<ExplicitIntegratorData *>(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data);
       assert(int_data != nullptr);
@@ -202,12 +204,12 @@ namespace simple_mpc
       {
         if (contact_state[i])
         {
-          contact_forces.push_back(mc_data->constraint_datas_[force_id].contact_force.linear());
+          contact_forces.segment((long)i * 3, 3) = mc_data->constraint_datas_[force_id].contact_force.linear();
           force_id += 1;
         }
         else
         {
-          contact_forces.push_back(Eigen::Vector3d::Zero());
+          contact_forces.segment((long)i * 3, 3).setZero();
         }
       }
       return contact_forces;

tests/mpc.cpp

@@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics)
 
   Eigen::VectorXd xdot = mpc.getStateDerivative(0);
 
-  std::vector<Eigen::Vector3d> forces = mpc.getContactForces(0);
+  Eigen::VectorXd forces = mpc.getContactForces(0);
 }
 
 BOOST_AUTO_TEST_CASE(mpc_kinodynamics)