1 import pinocchio
as pin
10 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
11 model_path = pinocchio_model_dir +
"/example-robot-data/robots"
12 mesh_dir = pinocchio_model_dir
14 urdf_filename = model_path +
"/ur_description/urdf/ur5_robot.urdf"
15 model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
18 print(
"standard model: dim=" + str(len(model.joints)))
19 for jn
in model.joints:
24 jointsToLock = [
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"]
28 for jn
in jointsToLock:
29 if model.existJointName(jn):
30 jointsToLockIDs.append(model.getJointId(jn))
32 print(
"Warning: joint " + str(jn) +
" does not belong to the model!")
35 initialJointConfig = np.array(
47 model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
50 model_reduced, visual_model_reduced = pin.buildReducedModel(
51 model, visual_model, jointsToLockIDs, initialJointConfig
55 geom_models = [visual_model, collision_model]
56 model_reduced, geometric_models_reduced = pin.buildReducedModel(
58 list_of_geom_models=geom_models,
59 list_of_joints_to_lock=jointsToLockIDs,
60 reference_configuration=initialJointConfig,
63 visual_model_reduced, collision_model_reduced = (
64 geometric_models_reduced[0],
65 geometric_models_reduced[1],
70 print(
"joints to lock (only ids):", jointsToLockIDs)
71 print(
"reduced model: dim=" + str(len(model_reduced.joints)))
77 mixed_jointsToLockIDs = [jointsToLockIDs[0],
"wrist_2_joint",
"wrist_3_joint"]
78 robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir)
79 reduced_robot = robot.buildReducedRobot(
80 list_of_joints_to_lock=mixed_jointsToLockIDs,
81 reference_configuration=initialJointConfig,
85 print(
"mixed joints to lock (names and ids):", mixed_jointsToLockIDs)
86 print(
"RobotWrapper reduced model: dim=" + str(len(reduced_robot.model.joints)))
87 for jn
in robot.model.joints: