Verified Commit e9e6023f authored by Nicolas Torres's avatar Nicolas Torres Committed by Justin Carpentier
Browse files

add reduceModel to RobotWrapper and update python example

parent f306ba60
......@@ -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"
......
......@@ -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
......
......@@ -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)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment