Remove joint ids and joint names

Author: edantec

bindings/expose-robot-handler.cpp

@@ -31,7 +31,6 @@ namespace simple_mpc
         .def("getFeetIds", &RobotModelHandler::getFeetIds, bp::return_internal_reference<>())
         .def("getFootName", &RobotModelHandler::getFootName, bp::return_internal_reference<>())
         .def("getFeetNames", &RobotModelHandler::getFeetNames, bp::return_internal_reference<>())
-        .def("getControlledJointNames", &RobotModelHandler::getControlledJointNames)
         .def("getFootId", &RobotModelHandler::getFootId)
         .def("getRefFootId", &RobotModelHandler::getRefFootId)
         .def("getMass", &RobotModelHandler::getMass)

examples/utils.py

@@ -10,8 +10,13 @@ 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]
+    locked_joints_names = ["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"]
+    locked_joints = [robotComplete.model.getJointId(el) for el in locked_joints_names]
     robot = robotComplete.buildReducedRobot(locked_joints, qComplete)
     rmodel: pin.Model = robot.model
     q0 = rmodel.referenceConfigurations["half_sitting"]

include/simple-mpc/robot-handler.hpp

@@ -38,11 +38,6 @@ namespace simple_mpc
      */
     double mass_;
 
-    /**
-     * @brief Joint id to be controlled in full model
-     */
-    std::vector<unsigned long> controlled_joints_ids_;
-
     /**
      * @brief Reference configuration and velocity (most probably null velocity)
      * to use
@@ -128,8 +123,6 @@ namespace simple_mpc
       return feet_names_;
     }
 
-    std::vector<std::string> getControlledJointNames() const;
-
     FrameIndex getBaseFrameId() const
     {
       return base_id_;

src/robot-handler.cpp

@@ -1,6 +1,5 @@
 #include "simple-mpc/robot-handler.hpp"
 
-#include <iostream>
 #include <pinocchio/algorithm/centroidal.hpp>
 #include <pinocchio/algorithm/compute-all-terms.hpp>
 #include <pinocchio/algorithm/crba.hpp>
@@ -14,10 +13,6 @@ namespace simple_mpc
     const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name)
   : model_(model)
   {
-    // Controlled joints index
-    for (auto joint_name : model_.names)
-      controlled_joints_ids_.push_back(model.getJointId(joint_name));
-
     // Root frame id
     base_id_ = model_.getFrameId(base_frame_name);
 
@@ -65,16 +60,6 @@ namespace simple_mpc
     return dx;
   }
 
-  std::vector<std::string> RobotModelHandler::getControlledJointNames() const
-  {
-    std::vector<std::string> joint_names;
-    for (JointIndex id : controlled_joints_ids_)
-    {
-      joint_names.push_back(model_.names.at(id));
-    }
-    return joint_names;
-  }
-
   RobotDataHandler::RobotDataHandler(const RobotModelHandler & model_handler)
   : model_handler_(model_handler)
   , data_(model_handler.getModel())