add reduceModel to RobotWrapper and update python example

bindings/python/algorithm/expose-model.cpp

@@ -125,7 +125,7 @@ namespace pinocchio
               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 "
+          "Build a reduced model and a reduced geometry model from a given "
           "input model,"
           "a given input geometry model and a list of joint to lock.\n\n"
           "Parameters:\n"
@@ -143,7 +143,7 @@ namespace pinocchio
               buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
           bp::args("model", "list_of_geom_models", "list_of_joints_to_lock",
                    "reference_configuration"),
-          "Build a reduced model and a rededuced geometry model  from a given "
+          "Build a reduced model and a reduced geometry model from a given "
           "input model,"
           "a given input geometry model and a list of joint to lock.\n\n"
           "Parameters:\n"

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 reduceModel(self,
+                    list_of_joints_to_lock,
+                    reference_configuration=None):
+        """
+          Reduced the robot model given a list of joints to lock.
+          Parameters:\n
+          \tlist_of_joints_to_lock: list of joint indexes/names to lock\n
+          \treference_configuration: reference configuration to compute the
+          placement of the lock joints.\n
+        If not provided, reference_configuration defaults to the robot's neutral configuration.
+
+        NOTE: This operation is not reversible, the original model is lost.
+        """
+
+        # 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)
+        self.model, self.visual_model, self.collision_model = model, geom_models[
+            0], geom_models[1]
+        self.rebuildData()
+
     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

@@ -12,12 +12,14 @@ model_path = pinocchio_model_dir + '/example-robot-data/robots'
 mesh_dir = pinocchio_model_dir
 # You should change here to set up your own URDF file
 urdf_filename = model_path + '/ur_description/urdf/ur5_robot.urdf'
-model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
+model, collision_model, visual_model = pin.buildModelsFromUrdf(
+    urdf_filename, mesh_dir)
 
 # Check dimensions of the original model
 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']
@@ -31,16 +33,49 @@ for jn in jointsToLock:
         print('Warning: joint ' + str(jn) + ' does not belong to the model!')
 
 # Set initial position of both fixed and revoulte joints
-initialJointConfig = np.array([0,0,0,   # shoulder and elbow
-                                1,1,1]) # gripper)
+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:
+
+# Option 4: Build a RobotWrapper and reduce the model using the wrapper
+# 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)
+robot.reduceModel(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(robot.model.joints)))
+for jn in robot.model.joints:
     print(jn)