Merge pull request #1427 from ntorresalberto/RobotWrapper.reduceModel

add Robot wrapper.reduceModel function and extend buildReducedModel for multiple GeometricModel

bindings/python/algorithm/expose-model.cpp

@@ -4,6 +4,7 @@
 
 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
 #include "pinocchio/bindings/python/utils/list.hpp"
+#include "pinocchio/bindings/python/utils/std-vector.hpp"
 #include "pinocchio/algorithm/model.hpp"
 
 namespace pinocchio
@@ -45,11 +46,31 @@ namespace pinocchio
       
       return bp::make_tuple(reduced_model,reduced_geom_model);
     }
-  
+
+    template <typename Scalar, int Options,
+              template <typename, int> class JointCollectionTpl,
+              typename ConfigVectorType>
+    bp::tuple buildReducedModel(const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
+                                const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &list_of_geom_models,
+                                const std::vector<JointIndex> &list_of_joints_to_lock,
+                                const Eigen::MatrixBase<ConfigVectorType> &reference_configuration)
+  {
+      typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
+      std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > reduced_geom_models;
+      Model reduced_model;
+      buildReducedModel(model, list_of_geom_models, list_of_joints_to_lock,
+                        reference_configuration, reduced_model,
+                        reduced_geom_models);
+      return bp::make_tuple(reduced_model, reduced_geom_models);
+    }
+
     void exposeModelAlgo()
     {
       using namespace Eigen;
-      
+
+      typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector;
+      StdVectorPythonVisitor<GeometryModel,GeometryModelVector::allocator_type>::expose("StdVec_GeometryModel");
+
       bp::def("appendModel",
               (Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,
               bp::args("modelA","modelB","frame_in_modelA","aMb"),
@@ -85,20 +106,40 @@ namespace pinocchio
               "\treference_configuration: reference configuration to compute the placement of the lock joints\n");
 
       bp::def("buildReducedModel",
-              (bp::tuple (*)(const Model &, const GeometryModel &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))
+              (bp::tuple (*)(const Model &,
+                             const GeometryModel &,
+                             const std::vector<JointIndex> &,
+                             const Eigen::MatrixBase<VectorXd> &))
               &buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
               bp::args("model",
                        "geom_model",
                        "list_of_joints_to_lock",
                        "reference_configuration"),
-              "Build a reduced model and a rededuced geometry model  from a given input model,"
-              "a given input geometry model and a list of joint to lock.\n\n"
+              "Build a reduced model and a reduced geometry model from a given input model,"
+              "an input geometry model and a list of joints to lock.\n\n"
               "Parameters:\n"
-              "\tmodel: input kinematic modell to reduce\n"
+              "\tmodel: input kinematic model to reduce\n"
               "\tgeom_model: input geometry model to reduce\n"
               "\tlist_of_joints_to_lock: list of joint indexes to lock\n"
-              "\treference_configuration: reference configuration to compute the placement of the lock joints\n");
-      
+              "\treference_configuration: reference configuration to compute the placement of the locked joints\n");
+
+      bp::def("buildReducedModel",
+              (bp::tuple(*)(const Model &,
+                            const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &,
+                            const std::vector<JointIndex> &,
+                            const Eigen::MatrixBase<VectorXd> &))
+              buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
+              bp::args("model", "list_of_geom_models", "list_of_joints_to_lock",
+                       "reference_configuration"),
+              "Build a reduced model and the related reduced geometry models from a given "
+              "input model,"
+              "a list of input geometry models and a list of joints to lock.\n\n"
+              "Parameters:\n"
+              "\tmodel: input kinematic model to reduce\n"
+              "\tlist_of_geom_models: input geometry models to reduce\n"
+              "\tlist_of_joints_to_lock: list of joint indexes to lock\n"
+              "\treference_configuration: reference configuration to compute the "
+              "placement of the locked joints\n");
     }
     
   } // namespace python

bindings/python/pinocchio/robot_wrapper.py

@@ -175,6 +175,40 @@ class RobotWrapper(object):
     def framesForwardKinematics(self, q):
         pin.framesForwardKinematics(self.model, self.data, q)
 
+    def buildReducedRobot(self,
+                          list_of_joints_to_lock,
+                          reference_configuration=None):
+        """
+          Build a reduced robot model given a list of joints to lock.
+          Parameters:
+          \tlist_of_joints_to_lock: list of joint indexes/names to lock.
+          \treference_configuration: reference configuration to compute the
+          placement of the lock joints. If not provided, reference_configuration
+          defaults to the robot's neutral configuration.
+
+          Returns: a new robot model.
+        """
+
+        # if joint to lock is a string, try to find its index
+        lockjoints_idx = []
+        for jnt in list_of_joints_to_lock:
+            idx = jnt
+            if isinstance(jnt, str):
+                idx = self.model.getJointId(jnt)
+            lockjoints_idx.append(idx)
+
+        if reference_configuration is None:
+            reference_configuration = pin.neutral(self.model)
+
+        model, geom_models = pin.buildReducedModel(
+            model=self.model,
+            list_of_geom_models=[self.visual_model, self.collision_model],
+            list_of_joints_to_lock=lockjoints_idx,
+            reference_configuration=reference_configuration)
+
+        return RobotWrapper(model=model, visual_model=geom_models[0],
+                            collision_model=geom_models[1])
+
     def getFrameJacobian(self, frame_id, rf_frame=pin.ReferenceFrame.LOCAL):
         """
             It computes the Jacobian of frame given by its id (frame_id) either expressed in the

examples/build-reduced-model.py

@@ -18,6 +18,7 @@ model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, me
 print('standard model: dim=' + str(len(model.joints)))
 for jn in model.joints:
     print(jn)
+print('-' * 30)
 
 # Create a list of joints to lock
 jointsToLock = ['wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
@@ -34,13 +35,40 @@ for jn in jointsToLock:
 initialJointConfig = np.array([0,0,0,   # shoulder and elbow
                                 1,1,1]) # gripper)
 
-# Option 1: Build the reduced model including the geometric model for proper displaying of the robot
-model_reduced, visual_model_reduced = pin.buildReducedModel(model, visual_model, jointsToLockIDs, initialJointConfig)
+# Option 1: Only build the reduced model in case no display needed:
+model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
 
-# Option 2: Only build the reduced model in case no display needed:
-# model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
+# Option 2: Build the reduced model including the geometric model for proper displaying of the robot
+model_reduced, visual_model_reduced = pin.buildReducedModel(
+    model, visual_model, jointsToLockIDs, initialJointConfig)
+
+# Option 3: Build the reduced model including multiple geometric models (for example: visuals, collision)
+geom_models = [visual_model, collision_model]
+model_reduced, geometric_models_reduced = pin.buildReducedModel(
+    model,
+    list_of_geom_models=geom_models,
+    list_of_joints_to_lock=jointsToLockIDs,
+    reference_configuration=initialJointConfig)
+# geometric_models_reduced is a list, ordered as the passed variable "geom_models" so:
+visual_model_reduced, collision_model_reduced = geometric_models_reduced[
+    0], geometric_models_reduced[1]
 
 # Check dimensions of the reduced model
+# options 1-3 only take joint ids
+print('joints to lock (only ids):', jointsToLockIDs)
 print('reduced model: dim=' + str(len(model_reduced.joints)))
-for jn in model_reduced.joints:
+print('-' * 30)
+
+# Option 4: Build a reduced model of a robot using RobotWrapper
+# reference_configuration is optional: if not provided, neutral configuration used
+# you can even mix joint names and joint ids
+mixed_jointsToLockIDs = [jointsToLockIDs[0], 'wrist_2_joint', 'wrist_3_joint']
+robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir)
+reduced_robot = robot.buildReducedRobot(list_of_joints_to_lock=mixed_jointsToLockIDs,
+                                        reference_configuration=initialJointConfig)
+
+# Check dimensions of the reduced model and joint info
+print('mixed joints to lock (names and ids):', mixed_jointsToLockIDs)
+print('RobotWrapper reduced model: dim=' + str(len(reduced_robot.model.joints)))
+for jn in robot.model.joints:
     print(jn)

src/algorithm/model.hpp

@@ -144,6 +144,32 @@ namespace pinocchio
                     ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
                     GeometryModel & reduced_geom_model);
 
+  /**
+   *
+   *  \brief Build a reduced model and a rededuced geometry model  from a given input model, a given input geometry model and a list of joint to lock.
+   *
+   *  \param[in] model the input model to reduce.
+   *  \param[in] list_of_geom_models the input geometry model to reduce (example: visual_model, collision_model).
+   *  \param[in] list_of_joints_to_lock list of joints to lock in the input model.
+   *  \param[in] reference_configuration reference configuration.
+   *  \param[out] reduced_model the reduced model.
+   *  \param[out] list_of_reduced_geom_model the list of reduced geometry models.
+   *
+   *  \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.  
+   *
+   */
+  template <typename Scalar, int Options,
+            template <typename, int> class JointCollectionTpl,
+            typename GeometryModelAllocator,
+            typename ConfigVectorType>
+  void buildReducedModel(
+      const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
+      const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
+      const std::vector<JointIndex> &list_of_joints_to_lock,
+      const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
+      ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
+      std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models);
+
 } // namespace pinocchio
 
 #include "pinocchio/algorithm/model.hxx"

src/algorithm/model.hxx

@@ -477,56 +477,85 @@ namespace pinocchio
                     ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
                     GeometryModel & reduced_geom_model)
   {
-    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
-    buildReducedModel(input_model,list_of_joints_to_lock,reference_configuration,reduced_model);
-    
-    // Add all the geometries
-    typedef GeometryModel::GeometryObject GeometryObject;
-    typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
-    for(GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
-        it != input_geom_model.geometryObjects.end(); ++it)
-    {
-      const GeometryModel::GeometryObject & geom = *it;
-      
-      const JointIndex joint_id_in_input_model = geom.parentJoint;
-      _PINOCCHIO_CHECK_INPUT_ARGUMENT_2((joint_id_in_input_model < (JointIndex)input_model.njoints),
-                                        "Invalid joint parent index for the geometry with name " + geom.name);
-      const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
-      
-      JointIndex reduced_joint_id = (JointIndex)-1;
-      typedef typename Model::SE3 SE3;
-      SE3 relative_placement = SE3::Identity();
-      if(reduced_model.existJointName(parent_joint_name))
+
+    const std::vector<GeometryModel> temp_input_geoms { input_geom_model };
+    std::vector<GeometryModel> temp_reduced_geom_models;
+
+    buildReducedModel(input_model, temp_input_geoms, list_of_joints_to_lock,
+                      reference_configuration, reduced_model,
+                      temp_reduced_geom_models);
+    reduced_geom_model = temp_reduced_geom_models.front();
+  }
+
+  template <typename Scalar, int Options,
+            template <typename, int> class JointCollectionTpl,
+            typename GeometryModelAllocator,
+            typename ConfigVectorType>
+  void buildReducedModel(
+      const ModelTpl<Scalar, Options, JointCollectionTpl> &input_model,
+      const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
+      const std::vector<JointIndex> &list_of_joints_to_lock,
+      const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
+      ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
+      std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models) {
+
+    typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
+    buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);
+
+    // for all GeometryModels
+    for (size_t gmi = 0; gmi < list_of_geom_models.size(); ++gmi) {
+      const GeometryModel &input_geom_model = list_of_geom_models[gmi];
+      GeometryModel reduced_geom_model;
+
+      // Add all the geometries
+      typedef GeometryModel::GeometryObject GeometryObject;
+      typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
+      for(GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
+          it != input_geom_model.geometryObjects.end(); ++it)
       {
-        reduced_joint_id = reduced_model.getJointId(parent_joint_name);
+        const GeometryModel::GeometryObject & geom = *it;
+
+        const JointIndex joint_id_in_input_model = geom.parentJoint;
+        _PINOCCHIO_CHECK_INPUT_ARGUMENT_2((joint_id_in_input_model < (JointIndex)input_model.njoints),
+                                          "Invalid joint parent index for the geometry with name " + geom.name);
+        const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
+
+        JointIndex reduced_joint_id = (JointIndex)-1;
+        typedef typename Model::SE3 SE3;
+        SE3 relative_placement = SE3::Identity();
+        if(reduced_model.existJointName(parent_joint_name))
+        {
+          reduced_joint_id = reduced_model.getJointId(parent_joint_name);
+        }
+        else // The joint is now a frame
+        {
+          const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
+          reduced_joint_id = reduced_model.frames[reduced_frame_id].parent;
+          relative_placement = reduced_model.frames[reduced_frame_id].placement;
+        }
+
+        GeometryObject reduced_geom(geom);
+        reduced_geom.parentJoint = reduced_joint_id;
+        reduced_geom.parentFrame = reduced_model.getBodyId(
+            input_model.frames[geom.parentFrame].name);
+        reduced_geom.placement = relative_placement * geom.placement;
+        reduced_geom_model.addGeometryObject(reduced_geom);
       }
-      else // The joint is now a frame
+
+  #ifdef PINOCCHIO_WITH_HPP_FCL
+      // Add all the collision pairs - the index of the geometry objects should have not changed
+
+      typedef GeometryModel::CollisionPairVector CollisionPairVector;
+      for(CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
+          it != input_geom_model.collisionPairs.end(); ++it)
       {
-        const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
-        reduced_joint_id = reduced_model.frames[reduced_frame_id].parent;
-        relative_placement = reduced_model.frames[reduced_frame_id].placement;
+        const CollisionPair & cp = *it;
+        reduced_geom_model.addCollisionPair(cp);
       }
-      
-      GeometryObject reduced_geom(geom);
-      reduced_geom.parentJoint = reduced_joint_id;
-      reduced_geom.parentFrame = reduced_model.getBodyId(
-          input_model.frames[geom.parentFrame].name);
-      reduced_geom.placement = relative_placement * geom.placement;
-      reduced_geom_model.addGeometryObject(reduced_geom);
-    }
-    
-#ifdef PINOCCHIO_WITH_HPP_FCL
-    // Add all the collision pairs - the index of the geometry objects should have not changed
-    
-    typedef GeometryModel::CollisionPairVector CollisionPairVector;
-    for(CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
-        it != input_geom_model.collisionPairs.end(); ++it)
-    {
-      const CollisionPair & cp = *it;
-      reduced_geom_model.addCollisionPair(cp);
+  #endif
+
+    list_of_reduced_geom_models.push_back(reduced_geom_model);
     }
-#endif
-    
   }
 
 } // namespace pinocchio

unittest/model.cpp

@@ -494,6 +494,22 @@ BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom)
   {
     BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i]));
   }
+  
+  // Test other signature
+  std::vector<GeometryModel> full_geometry_models;
+  full_geometry_models.push_back(humanoid_geometry);
+  full_geometry_models.push_back(humanoid_geometry);
+  full_geometry_models.push_back(humanoid_geometry);
+  
+  std::vector<GeometryModel> reduced_geometry_models;
+  
+  Model reduced_humanoid_model_other_sig;
+  buildReducedModel(humanoid_model,full_geometry_models,joints_to_lock,
+                    reference_config_humanoid,reduced_humanoid_model_other_sig,reduced_geometry_models);
+  
+  BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry);
+  BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry);
+  BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry);
 }
 #endif // PINOCCHIO_WITH_HPP_FCL