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