build-reduced-model.py
Go to the documentation of this file.
1 import pinocchio as pin
2 import numpy as np
3 
4 from os.path import *
5 
6 # Goal: Build a reduced model from an existing URDF model by fixing the desired joints at a specified position.
7 
8 # Load UR robot arm
9 # This path refers to Pinocchio source code but you can define your own directory here.
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
13 # You should change here to set up your own URDF file
14 urdf_filename = model_path + '/ur_description/urdf/ur5_robot.urdf'
15 model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
16 
17 # Check dimensions of the original model
18 print('standard model: dim=' + str(len(model.joints)))
19 for jn in model.joints:
20  print(jn)
21 print('-' * 30)
22 
23 # Create a list of joints to lock
24 jointsToLock = ['wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
25 
26 # Get the ID of all existing joints
27 jointsToLockIDs = []
28 for jn in jointsToLock:
29  if model.existJointName(jn):
30  jointsToLockIDs.append(model.getJointId(jn))
31  else:
32  print('Warning: joint ' + str(jn) + ' does not belong to the model!')
33 
34 # Set initial position of both fixed and revoulte joints
35 initialJointConfig = np.array([0,0,0, # shoulder and elbow
36  1,1,1]) # gripper)
37 
38 # Option 1: Only build the reduced model in case no display needed:
39 model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
40 
41 # Option 2: Build the reduced model including the geometric model for proper displaying of the robot
42 model_reduced, visual_model_reduced = pin.buildReducedModel(
43  model, visual_model, jointsToLockIDs, initialJointConfig)
44 
45 # Option 3: Build the reduced model including multiple geometric models (for example: visuals, collision)
46 geom_models = [visual_model, collision_model]
47 model_reduced, geometric_models_reduced = pin.buildReducedModel(
48  model,
49  list_of_geom_models=geom_models,
50  list_of_joints_to_lock=jointsToLockIDs,
51  reference_configuration=initialJointConfig)
52 # geometric_models_reduced is a list, ordered as the passed variable "geom_models" so:
53 visual_model_reduced, collision_model_reduced = geometric_models_reduced[
54  0], geometric_models_reduced[1]
55 
56 # Check dimensions of the reduced model
57 # options 1-3 only take joint ids
58 print('joints to lock (only ids):', jointsToLockIDs)
59 print('reduced model: dim=' + str(len(model_reduced.joints)))
60 print('-' * 30)
61 
62 # Option 4: Build a reduced model of a robot using RobotWrapper
63 # reference_configuration is optional: if not provided, neutral configuration used
64 # you can even mix joint names and joint ids
65 mixed_jointsToLockIDs = [jointsToLockIDs[0], 'wrist_2_joint', 'wrist_3_joint']
66 robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir)
67 reduced_robot = robot.buildReducedRobot(list_of_joints_to_lock=mixed_jointsToLockIDs,
68  reference_configuration=initialJointConfig)
69 
70 # Check dimensions of the reduced model and joint info
71 print('mixed joints to lock (names and ids):', mixed_jointsToLockIDs)
72 print('RobotWrapper reduced model: dim=' + str(len(reduced_robot.model.joints)))
73 for jn in robot.model.joints:
74  print(jn)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28