generator.py
Go to the documentation of this file.
1 from math import pi
2 
3 import hppfcl as fcl
4 import numpy as np
5 import pinocchio as pin
6 
7 
8 def create_7dof_arm(revoluteOnly=False):
9  """Create a 7 DoF robot arm (with spherical joint for shoulder and wrist and revolute
10  joint for elbow)
11 
12  Optionnal parameters:
13  revoluteOnly (default=False): if True, the arm is created with only revolute
14  joints. (Spherical joints are replaced by 3 revolute joints)
15 
16  Return:
17  ------
18  model: pinocchio model of the robot
19  geom_model: pinocchio geometric model of the robot
20 
21  """
22  # Some useful values
23  geom_radius = 0.1 # radius of the arm geometries
24  geom_length = 0.66 # length of the lower or upper arm geometries
25 
26  def set_limit(model, joint_id, pos_absmax, vel_absmax, eff_absmax):
27  idx_q = model.joints[joint_id].idx_q
28  nq = model.joints[joint_id].nq
29  for idx in range(idx_q, idx_q + nq):
30  model.upperPositionLimit[idx] = pos_absmax
31  model.lowerPositionLimit[idx] = -pos_absmax
32 
33  idx_v = model.joints[joint_id].idx_v
34  nv = model.joints[joint_id].nv
35  for idx in range(idx_v, idx_v + nv):
36  model.velocityLimit[idx] = vel_absmax
37  model.effortLimit[idx] = eff_absmax
38 
39  # Create models
40  model = pin.Model()
41  geom_model = pin.GeometryModel()
42 
43  # Base
44  id_base = 0
45 
46  color_base = np.array([1.0, 0.1, 0.1, 1.0])
47 
48  geom_model.addGeometryObject(
49  pin.GeometryObject(
50  "base",
51  id_base, # parent
52  fcl.Box(0.5, 0.5, 0.02),
53  pin.SE3(np.identity(3), np.array([0, 0, -0.01 - geom_radius])),
54  "",
55  np.ones(3),
56  False, # Default values
57  color_base,
58  )
59  )
60 
61  geom_model.addGeometryObject(
62  pin.GeometryObject(
63  "shoulderGeom",
64  id_base,
65  fcl.Sphere(geom_radius),
66  pin.SE3.Identity(),
67  "",
68  np.ones(3),
69  False, # Default values
70  color_base,
71  )
72  )
73 
74  # Upper arm
75  placement_upper = pin.SE3(np.identity(3), np.array([0, 0, geom_length / 2.0]))
76  if not revoluteOnly:
77  id_upper = model.addJoint(
78  id_base, pin.JointModelSpherical(), pin.SE3.Identity(), "shoulder"
79  )
80  set_limit(model, id_upper, pi, 3.0, 100.0)
81  else:
82  id_upper = model.addJoint(
83  id_base, pin.JointModelRX(), pin.SE3.Identity(), "shoulderX"
84  )
85  set_limit(model, id_upper, pi, 3.0, 100.0)
86  id_upper = model.addJoint(
87  id_upper, pin.JointModelRY(), pin.SE3.Identity(), "shoulderY"
88  )
89  set_limit(model, id_upper, pi, 3.0, 100.0)
90  id_upper = model.addJoint(
91  id_upper, pin.JointModelRZ(), pin.SE3.Identity(), "shoulderZ"
92  )
93  set_limit(model, id_upper, pi, 3.0, 100.0)
94  model.appendBodyToJoint(
95  id_upper,
96  pin.Inertia.FromCylinder(1.0, geom_radius, geom_length),
97  placement_upper,
98  )
99 
100  color_upper = np.array([0.1, 0.8, 0.1, 0.8])
101 
102  geom_model.addGeometryObject(
103  pin.GeometryObject(
104  "elbowGeom",
105  id_upper,
106  fcl.Sphere(geom_radius),
107  pin.SE3(np.identity(3), np.array([0, 0, geom_length])),
108  "",
109  np.ones(3),
110  False, # Default values
111  color_upper,
112  )
113  )
114 
115  geom_model.addGeometryObject(
116  pin.GeometryObject(
117  "upperGeom",
118  id_upper,
119  fcl.Cylinder(geom_radius, geom_length),
120  placement_upper,
121  "",
122  np.ones(3),
123  False, # Default values
124  color_upper,
125  )
126  )
127 
128  # Lower arm
129  placement_lower = pin.SE3(np.identity(3), np.array([0, 0, geom_length / 2.0]))
130  id_lower = model.addJoint(
131  id_upper,
132  pin.JointModelRX(),
133  pin.SE3(np.identity(3), np.array([0, 0, geom_length])),
134  "elbow",
135  )
136  model.appendBodyToJoint(
137  id_lower,
138  pin.Inertia.FromCylinder(1.0, geom_radius, geom_length),
139  placement_lower,
140  )
141  set_limit(model, id_lower, pi, 3.0, 100.0)
142 
143  color_lower = np.array([0.1, 0.1, 0.8, 0.8])
144 
145  geom_model.addGeometryObject(
146  pin.GeometryObject(
147  "wristGeom",
148  id_lower,
149  fcl.Sphere(geom_radius),
150  pin.SE3(np.identity(3), np.array([0, 0, geom_length])),
151  "",
152  np.ones(3),
153  False, # Default values
154  color_lower,
155  )
156  )
157 
158  geom_model.addGeometryObject(
159  pin.GeometryObject(
160  "lowerGeom",
161  id_lower,
162  fcl.Cylinder(geom_radius, geom_length),
163  placement_lower,
164  "",
165  np.ones(3),
166  False, # Default values
167  color_lower,
168  )
169  )
170 
171  # Hand
172  placement_hand = pin.SE3(np.identity(3), np.array([0, 0, geom_length]))
173  if not revoluteOnly:
174  id_hand = model.addJoint(
175  id_lower,
176  pin.JointModelSpherical(),
177  pin.SE3(np.identity(3), np.array([0, 0, geom_length])),
178  "wrist",
179  )
180  set_limit(model, id_hand, pi, 3.0, 100.0)
181  else:
182  id_hand = model.addJoint(
183  id_lower,
184  pin.JointModelRX(),
185  pin.SE3(np.identity(3), np.array([0, 0, geom_length])),
186  "wristX",
187  )
188  set_limit(model, id_hand, pi, 3.0, 100.0)
189  id_hand = model.addJoint(
190  id_hand, pin.JointModelRY(), pin.SE3.Identity(), "wristY"
191  )
192  set_limit(model, id_hand, pi, 3.0, 100.0)
193  id_hand = model.addJoint(
194  id_hand, pin.JointModelRZ(), pin.SE3.Identity(), "wristZ"
195  )
196  set_limit(model, id_hand, pi, 3.0, 100.0)
197  model.appendBodyToJoint(
198  id_hand, pin.Inertia.FromSphere(1.0, geom_radius), placement_hand
199  )
200 
201  color_hand = np.array([0.7, 0.7, 0.7, 0.8])
202 
203  geom_model.addGeometryObject(
204  pin.GeometryObject(
205  "palmGeom",
206  id_hand,
207  fcl.Sphere(geom_radius),
208  pin.SE3(np.identity(3), np.array([0, 0, 2 * geom_radius])),
209  "",
210  np.ones(3),
211  False, # Default values
212  color_hand,
213  )
214  )
215 
216  geom_model.addGeometryObject(
217  pin.GeometryObject(
218  "finger1Geom",
219  id_hand,
220  fcl.Cylinder(0.02, 0.1),
221  pin.SE3(np.identity(3), np.array([0.05, 0, 3 * geom_radius])),
222  "",
223  np.ones(3),
224  False, # Default values
225  color_hand,
226  )
227  )
228 
229  geom_model.addGeometryObject(
230  pin.GeometryObject(
231  "finger2Geom",
232  id_hand,
233  fcl.Cylinder(0.02, 0.1),
234  pin.SE3(np.identity(3), np.array([0, 0, 3 * geom_radius])),
235  "",
236  np.ones(3),
237  False, # Default values
238  color_hand,
239  )
240  )
241 
242  geom_model.addGeometryObject(
243  pin.GeometryObject(
244  "finger3Geom",
245  id_hand,
246  fcl.Cylinder(0.02, 0.1),
247  pin.SE3(np.identity(3), np.array([-0.05, 0, 3 * geom_radius])),
248  "",
249  np.ones(3),
250  False, # Default values
251  color_hand,
252  )
253  )
254 
255  geom_model.addGeometryObject(
256  pin.GeometryObject(
257  "thumbGeom",
258  id_hand,
259  fcl.Cylinder(0.02, 0.1),
260  pin.XYZQUATToSE3(
261  np.array([geom_radius, 0, 2 * geom_radius, 0, 0.707, 0, 0.707])
262  ),
263  "",
264  np.ones(3),
265  False, # Default values
266  color_hand,
267  )
268  )
269 
270  # Add end-effector frame
271  model.addFrame(
272  pin.Frame(
273  "eeFrame",
274  id_hand,
275  0,
276  pin.SE3(np.identity(3), np.array([0, 0, 2 * geom_radius])),
277  pin.FrameType.OP_FRAME,
278  )
279  )
280 
281  return model, geom_model
generator.create_7dof_arm
def create_7dof_arm(revoluteOnly=False)
Definition: generator.py:8

tsid
Author(s): Andrea Del Prete, Justin Carpentier
autogenerated on Thu Apr 3 2025 02:47:15