Reduced model is now built outside robot_handler

Author: edantec

benchmark/talos.cpp

@@ -2,6 +2,7 @@
 #include "simple-mpc/mpc.hpp"
 #include "simple-mpc/ocp-handler.hpp"
 #include "simple-mpc/robot-handler.hpp"
+#include <pinocchio/algorithm/model.hpp>
 #include <pinocchio/fwd.hpp>
 #include <pinocchio/parsers/srdf.hpp>
 #include <pinocchio/parsers/urdf.hpp>
@@ -19,13 +20,13 @@ using simple_mpc::OCPHandler;
 int main()
 {
   // Load pinocchio model from example robot data
-  pinocchio::Model model;
+  pinocchio::Model modelFull;
   std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf";
   std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf";
 
-  pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model);
-  pinocchio::srdf::loadReferenceConfigurations(model, srdf_path, false);
-  pinocchio::srdf::loadRotorParameters(model, srdf_path, false);
+  pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), modelFull);
+  pinocchio::srdf::loadReferenceConfigurations(modelFull, srdf_path, false);
+  pinocchio::srdf::loadRotorParameters(modelFull, srdf_path, false);
 
   // Lock joint list
   const std::vector<std::string> controlled_joints_names{
@@ -36,7 +37,8 @@ int main()
     "arm_right_1_joint", "arm_right_2_joint", "arm_right_3_joint", "arm_right_4_joint",
   };
 
-  std::vector<std::string> locked_joints_names{model.names};
+  std::vector<std::string> locked_joints_names{modelFull.names};
+  std::vector<pinocchio::JointIndex> controlled_joints_ids;
   locked_joints_names.erase(
     std::remove_if(
       locked_joints_names.begin(), locked_joints_names.end(),
@@ -46,9 +48,28 @@ int main()
       }),
     locked_joints_names.end());
 
+  // Construct controlled and locked joints ids list
+  pinocchio::Model model;
+  std::vector<unsigned long> locked_joint_ids;
+  for (size_t i = 1; i < modelFull.names.size(); i++)
+  {
+    const std::string joint_name = modelFull.names.at(i);
+    if (count(locked_joints_names.begin(), locked_joints_names.end(), joint_name) == 0)
+    {
+      controlled_joints_ids.push_back(modelFull.getJointId(joint_name));
+    }
+    else
+    {
+      locked_joint_ids.push_back(modelFull.getJointId(joint_name));
+    }
+  }
+
+  // Build reduced model with locked joints
+  pinocchio::buildReducedModel(modelFull, locked_joint_ids, modelFull.referenceConfigurations["half_sitting"], model);
+
   // Actually create handler
   std::string base_joint = "root_joint";
-  RobotModelHandler model_handler(model, "half_sitting", base_joint, locked_joints_names);
+  RobotModelHandler model_handler(model, "half_sitting", base_joint);
 
   // Add feet
   model_handler.addFoot(

bindings/expose-robot-handler.cpp

@@ -21,12 +21,10 @@ namespace simple_mpc
     void exposeHandler()
     {
       bp::class_<RobotModelHandler>(
-        "RobotModelHandler",
-        bp::init<const pinocchio::Model &, const std::string &, const std::string &, const std::vector<std::string> &>(
-          bp::args("self", "model", "reference_configuration_name", "base_frame_name", "locked_joint_names")))
+        "RobotModelHandler", bp::init<const pinocchio::Model &, const std::string &, const std::string &>(
+                               bp::args("self", "model", "reference_configuration_name", "base_frame_name")))
         .def("addFoot", &RobotModelHandler::addFoot)
         .def("difference", &RobotModelHandler::difference)
-        .def("shapeState", &RobotModelHandler::shapeState)
         .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
         .def("getReferenceState", &RobotModelHandler::getReferenceState)
         .def("getFootNb", &RobotModelHandler::getFootNb)
@@ -37,8 +35,7 @@ namespace simple_mpc
         .def("getFootId", &RobotModelHandler::getFootId)
         .def("getRefFootId", &RobotModelHandler::getRefFootId)
         .def("getMass", &RobotModelHandler::getMass)
-        .def("getModel", &RobotModelHandler::getModel, bp::return_internal_reference<>())
-        .def("getCompleteModel", &RobotModelHandler::getCompleteModel, bp::return_internal_reference<>());
+        .def("getModel", &RobotModelHandler::getModel, bp::return_internal_reference<>());
 
       ENABLE_SPECIFIC_MATRIX_TYPE(RobotDataHandler::CentroidalStateVector);
 

examples/go2_fulldynamics.py

@@ -14,7 +14,7 @@
 robot_wrapper = erd.load('go2')
 
 # Create Model and Data handler
-model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name, [])
+model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
 model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
 model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
 model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))

examples/go2_kinodynamics.py

@@ -14,7 +14,7 @@
 robot_wrapper = erd.load('go2')
 
 # Create Model and Data handler
-model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name, [])
+model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
 model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
 model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
 model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
@@ -225,8 +225,8 @@
     com_measured.append(mpc.getDataHandler().getData().com[0].copy())
     L_measured.append(mpc.getDataHandler().getData().hg.angular.copy())
 
-    a0 = mpc.getStateDerivative(0)[nv:]
-    a1 = mpc.getStateDerivative(1)[nv:]
+    a0 = mpc.getStateDerivative(0)[nv:].copy()
+    a1 = mpc.getStateDerivative(1)[nv:].copy()
 
     a0[6:] = mpc.us[0][nk * force_size :]
     a1[6:] = mpc.us[1][nk * force_size :]

examples/talos_centroidal.py

@@ -3,29 +3,18 @@
 import example_robot_data as erd
 from bullet_robot import BulletRobot
 from simple_mpc import RobotModelHandler, RobotDataHandler, CentroidalOCP, MPC, IKIDSolver
+from utils import loadTalos
 import time
 
 # RobotWrapper
 URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf"
 base_joint_name ="root_joint"
-robot_wrapper = erd.load('talos')
-
 reference_configuration_name = "half_sitting"
-locked_joints = [
-    'arm_left_5_joint',
-    'arm_left_6_joint',
-    'arm_left_7_joint',
-    'gripper_left_joint',
-    'arm_right_5_joint',
-    'arm_right_6_joint',
-    'arm_right_7_joint',
-    'gripper_right_joint',
-    'head_1_joint',
-    'head_2_joint'
-]
+
+rmodelComplete, rmodel, qComplete, q0 = loadTalos()
 
 # Create Model and Data handler
-model_handler = RobotModelHandler(robot_wrapper.model, reference_configuration_name, base_joint_name, locked_joints)
+model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
 model_handler.addFoot("left_sole_link",  base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)

examples/talos_fulldynamics.py

@@ -7,36 +7,28 @@
 import numpy as np
 import time
 from bullet_robot import BulletRobot
-import example_robot_data as erd
 import pinocchio as pin
 from simple_mpc import MPC, FullDynamicsOCP, RobotModelHandler, RobotDataHandler
+from utils import loadTalos
+import example_robot_data as erd
 
 # ####### CONFIGURATION  ############
 # RobotWrapper
 URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf"
 base_joint_name ="root_joint"
-robot_wrapper = erd.load('talos')
-
 reference_configuration_name = "half_sitting"
-locked_joints = [
-    'arm_left_5_joint',
-    'arm_left_6_joint',
-    'arm_left_7_joint',
-    'gripper_left_joint',
-    'arm_right_5_joint',
-    'arm_right_6_joint',
-    'arm_right_7_joint',
-    'gripper_right_joint',
-    'head_1_joint',
-    'head_2_joint'
-]
+
+rmodelComplete, rmodel, qComplete, q0 = loadTalos()
 
 # Create Model and Data handler
-model_handler = RobotModelHandler(robot_wrapper.model, reference_configuration_name, base_joint_name, locked_joints)
+model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
 model_handler.addFoot("left_sole_link",  base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
 
+controlled_joints = rmodel.names[1:].tolist()
+controlled_ids = [rmodelComplete.getJointId(name_joint) for name_joint in controlled_joints[1:]]
+
 nq = model_handler.getModel().nq
 nv = model_handler.getModel().nv
 nu = nv - 6
@@ -150,7 +142,7 @@
 device.changeCamera(1.0, 90, -5, [1.5, 0, 1])
 
 q_meas, v_meas = device.measureState()
-x_measured  = np.concatenate([q_meas, v_meas])
+x_measured = np.concatenate((q_meas, v_meas))
 
 land_LF = -1
 land_RF = -1
@@ -181,6 +173,7 @@
         str(land_LF),
     )
     start = time.time()
+
     mpc.iterate(x_measured)
     end = time.time()
     print("MPC iterate = " + str(end - start))
@@ -191,7 +184,7 @@
 
     for j in range(10):
         q_meas, v_meas = device.measureState()
-        x_measured  = np.concatenate([q_meas, v_meas])
+        x_measured = np.concatenate((q_meas, v_meas))
 
         current_torque = mpc.us[0] - mpc.Ks[0] @ model_handler.difference(x_measured, mpc.xs[0])
         device.execute(current_torque)

examples/talos_kinodynamics.py

@@ -3,29 +3,18 @@
 import pinocchio as pin
 from bullet_robot import BulletRobot
 import time
+from utils import loadTalos
 from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver
 
 # RobotWrapper
 URDF_SUBPATH = "/talos_data/robots/talos_reduced.urdf"
 base_joint_name ="root_joint"
-robot_wrapper = erd.load('talos')
-
 reference_configuration_name = "half_sitting"
-locked_joints = [
-    'arm_left_5_joint',
-    'arm_left_6_joint',
-    'arm_left_7_joint',
-    'gripper_left_joint',
-    'arm_right_5_joint',
-    'arm_right_6_joint',
-    'arm_right_7_joint',
-    'gripper_right_joint',
-    'head_1_joint',
-    'head_2_joint'
-]
+
+rmodelComplete, rmodel, qComplete, q0 = loadTalos()
 
 # Create Model and Data handler
-model_handler = RobotModelHandler(robot_wrapper.model, reference_configuration_name, base_joint_name, locked_joints)
+model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
 model_handler.addFoot("left_sole_link",  base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
@@ -211,8 +200,8 @@
     end = time.time()
     print("MPC iterate = " + str(end - start))
 
-    a0 = mpc.getStateDerivative(0)[nv:]
-    a1 = mpc.getStateDerivative(1)[nv:]
+    a0 = mpc.getStateDerivative(0)[nv:].copy()
+    a1 = mpc.getStateDerivative(1)[nv:].copy()
     a0[6:] = mpc.us[0][nk * force_size :]
     a1[6:] = mpc.us[1][nk * force_size :]
     forces0 = mpc.us[0][: nk * force_size]

examples/utils.py

@@ -1,9 +1,22 @@
 import numpy as np
+import pinocchio as pin
+import example_robot_data
 import os
 
 CURRENT_DIRECTORY = os.getcwd()
 DEFAULT_SAVE_DIR = CURRENT_DIRECTORY + '/tmp'
 
+def loadTalos():
+    robotComplete = example_robot_data.load("talos")
+    qComplete = robotComplete.model.referenceConfigurations["half_sitting"]
+
+    locked_joints = [20,21,22,23,28,29,30,31]
+    locked_joints += [32, 33]
+    robot = robotComplete.buildReducedRobot(locked_joints, qComplete)
+    rmodel: pin.Model = robot.model
+    q0 = rmodel.referenceConfigurations["half_sitting"]
+
+    return robotComplete.model, rmodel, qComplete, q0
 
 def save_trajectory(
     xs,

include/simple-mpc/robot-handler.hpp

@@ -29,12 +29,7 @@ namespace simple_mpc
   {
   private:
     /**
-     * @brief Robot model with all joints unlocked
-     */
-    Model model_full_;
-
-    /**
-     * @brief Reduced model to be used by ocp
+     * @brief Model to be used by ocp
      */
     Model model_;
 
@@ -82,14 +77,9 @@ namespace simple_mpc
      * @param feet_names Name of the frames corresponding to the feet (e.g. can be
      * used for contact with the ground)
      * @param reference_configuration_name Reference configuration to use
-     * @param locked_joint_names List of joints to lock (values will be fixed at
-     * the reference configuration)
      */
     RobotModelHandler(
-      const Model & model,
-      const std::string & reference_configuration_name,
-      const std::string & base_frame_name,
-      const std::vector<std::string> & locked_joint_names = {});
+      const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name);
 
     /**
      * @brief
@@ -113,16 +103,6 @@ namespace simple_mpc
      */
     Eigen::VectorXd difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const;
 
-    /**
-     * @brief Compute reduced state from measures by concatenating q,v of the
-     * reduced model.
-     *
-     * @param q Configuration vector of the full model
-     * @param v Velocity vector of the full model
-     * @return const Eigen::VectorXd State vector of the reduced model.
-     */
-    Eigen::VectorXd shapeState(const ConstVectorRef & q, const ConstVectorRef & v) const;
-
     // Const getters
     ConstVectorRef getReferenceState() const
     {
@@ -174,11 +154,6 @@ namespace simple_mpc
     {
       return model_;
     }
-
-    const Model & getCompleteModel() const
-    {
-      return model_full_;
-    }
   };
 
   class RobotDataHandler