test_Tasks.py
Go to the documentation of this file.
1 from pathlib import Path
2 
3 import numpy as np
4 import pinocchio as pin
5 import tsid
6 from numpy.linalg import norm
7 
8 # Get robot model generator module
9 from generator import create_7dof_arm
10 
11 print("")
12 print("Test Task COM")
13 print("")
14 
15 
16 tol = 1e-5
17 filename = str(Path(__file__).resolve().parent)
18 path = filename + "/../../models/romeo"
19 urdf = path + "/urdf/romeo.urdf"
20 vector = pin.StdVec_StdString()
21 vector.extend(item for item in path)
22 robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
23 model = robot.model()
24 data = robot.data()
25 
26 srdf = path + "/srdf/romeo_collision.srdf"
27 pin.loadReferenceConfigurations(model, srdf, False)
28 q = model.referenceConfigurations["half_sitting"]
29 
30 q[2] += 0.84
31 print("q:", q.transpose())
32 
33 pin.centerOfMass(model, data, q)
34 
35 taskCOM = tsid.TaskComEquality("task-com", robot)
36 
37 Kp = 100 * np.ones(3)
38 Kd = 20.0 * np.ones(3)
39 taskCOM.setKp(Kp)
40 taskCOM.setKd(Kd)
41 
42 assert np.linalg.norm(Kp - taskCOM.Kp, 2) < tol
43 assert np.linalg.norm(Kd - taskCOM.Kd, 2) < tol
44 
45 com_ref = data.com[0] + np.ones(3) * 0.02
46 traj = tsid.TrajectoryEuclidianConstant("traj_se3", com_ref)
47 sample = tsid.TrajectorySample(0)
48 
49 t = 0.0
50 dt = 0.001
51 max_it = 1000
52 Jpinv = np.zeros((robot.nv, 3))
53 error_past = 1e100
54 v = np.zeros(robot.nv)
55 
56 for i in range(0, max_it):
57  robot.computeAllTerms(data, q, v)
58  sample = traj.computeNext()
59  taskCOM.setReference(sample)
60  const = taskCOM.compute(t, q, v, data)
61 
62  Jpinv = np.linalg.pinv(const.matrix, 1e-5)
63  dv = Jpinv.dot(const.vector)
64 
65  assert np.linalg.norm(Jpinv.dot(const.matrix), 2) - 1.0 < tol
66  v += dt * dv
67  q = pin.integrate(model, q, dt * v)
68  t += dt
69 
70  error = np.linalg.norm(taskCOM.position_error, 2)
71  assert error - error_past < 1e-4
72  error_past = error
73  if error < 1e-8:
74  print("Success Convergence")
75  break
76  if i % 100 == 0:
77  print(
78  "Time :",
79  t,
80  "COM pos error :",
81  error,
82  "COM vel error :",
83  np.linalg.norm(taskCOM.velocity_error, 2),
84  )
85 
86 print("")
87 print("Test Task Joint Posture")
88 print("")
89 
90 q = model.referenceConfigurations["half_sitting"]
91 q[2] += 0.84
92 
93 task_joint = tsid.TaskJointPosture("task-posture", robot)
94 
95 na = robot.nv - 6
96 Kp = 100 * np.ones(na)
97 Kd = 20.0 * np.ones(na)
98 task_joint.setKp(Kp)
99 task_joint.setKd(Kd)
100 
101 assert np.linalg.norm(Kp - task_joint.Kp, 2) < tol
102 assert np.linalg.norm(Kd - task_joint.Kd, 2) < tol
103 
104 rng = np.random.default_rng()
105 q_ref = rng.standard_normal(na)
106 traj = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
107 sample = tsid.TrajectorySample(0)
108 
109 error_past = 1e100
110 t = 0.0
111 max_it = 1000
112 
113 for i in range(0, max_it):
114  robot.computeAllTerms(data, q, v)
115  sample = traj.computeNext()
116  task_joint.setReference(sample)
117  const = task_joint.compute(t, q, v, data)
118 
119  Jpinv = np.linalg.pinv(const.matrix, 1e-5)
120  dv = Jpinv.dot(const.vector)
121 
122  assert np.linalg.norm(Jpinv.dot(const.matrix), 2) - 1.0 < tol
123  v += dt * dv
124  q = pin.integrate(model, q, dt * v)
125  t += dt
126 
127  error = np.linalg.norm(task_joint.position_error, 2)
128  assert error - error_past < 1e-4
129  error_past = error
130  if error < 1e-8:
131  print("Success Convergence")
132  break
133  if i % 100 == 0:
134  print(
135  "Time :",
136  t,
137  "Joint pos error :",
138  error,
139  "Joint vel error :",
140  np.linalg.norm(task_joint.velocity_error, 2),
141  )
142 
143 print("")
144 print("Test Task SE3")
145 print("")
146 
147 q = model.referenceConfigurations["half_sitting"]
148 q[2] += 0.84
149 
150 task_se3 = tsid.TaskSE3Equality("task-se3", robot, "RWristPitch")
151 
152 na = 6
153 Kp = 100 * np.ones(na)
154 Kd = 20.0 * np.ones(na)
155 task_se3.setKp(Kp)
156 task_se3.setKd(Kd)
157 
158 assert np.linalg.norm(Kp - task_se3.Kp, 2) < tol
159 assert np.linalg.norm(Kd - task_se3.Kd, 2) < tol
160 
161 M_ref = pin.SE3.Random()
162 
163 traj = tsid.TrajectorySE3Constant("traj_se3", M_ref)
164 sample = tsid.TrajectorySample(0)
165 
166 t = 0.0
167 max_it = 1000
168 error_past = 1e100
169 
170 for i in range(0, max_it):
171  robot.computeAllTerms(data, q, v)
172  sample = traj.computeNext()
173  task_se3.setReference(sample)
174 
175  const = task_se3.compute(t, q, v, data)
176 
177  Jpinv = np.linalg.pinv(const.matrix, 1e-5)
178  dv = Jpinv.dot(const.vector)
179 
180  assert np.linalg.norm(Jpinv.dot(const.matrix), 2) - 1.0 < tol
181 
182  v += dt * dv
183  q = pin.integrate(model, q, dt * v)
184  t += dt
185 
186  error = np.linalg.norm(task_se3.position_error, 2)
187  assert error - error_past < 1e-4
188  error_past = error
189  if error < 1e-8:
190  print("Success Convergence")
191  break
192  if i % 100 == 0:
193  print(
194  "Time :",
195  t,
196  "EE pos error :",
197  error,
198  "EE vel error :",
199  np.linalg.norm(task_se3.velocity_error, 2),
200  )
201 
202 print("")
203 print("Test Task Angular Momentum")
204 print("")
205 
206 tol = 1e-5
207 filename = str(Path(__file__).resolve().parent)
208 path = filename + "/../../models/romeo"
209 urdf = path + "/urdf/romeo.urdf"
210 vector = pin.StdVec_StdString()
211 vector.extend(item for item in path)
212 robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
213 model = robot.model()
214 data = robot.data()
215 
216 srdf = path + "/srdf/romeo_collision.srdf"
217 pin.loadReferenceConfigurations(model, srdf, False)
218 q = model.referenceConfigurations["half_sitting"]
219 
220 q[2] += 0.84
221 print("q:", q.transpose())
222 
223 taskAM = tsid.TaskAMEquality("task-AM", robot)
224 
225 Kp = 100 * np.ones(3)
226 Kd = 20.0 * np.ones(3)
227 taskAM.setKp(Kp)
228 taskAM.setKd(Kd)
229 
230 assert np.linalg.norm(Kp - taskAM.Kp, 2) < tol
231 assert np.linalg.norm(Kd - taskAM.Kd, 2) < tol
232 
233 am_ref = np.zeros(3)
234 traj = tsid.TrajectoryEuclidianConstant("traj_se3", am_ref)
235 sample = tsid.TrajectorySample(0)
236 
237 t = 0.0
238 dt = 0.001
239 max_it = 1000
240 Jpinv = np.zeros((robot.nv, 3))
241 error_past = 1e100
242 v = rng.standard_normal(robot.nv)
243 
244 for i in range(0, max_it):
245  robot.computeAllTerms(data, q, v)
246  sample = traj.computeNext()
247  taskAM.setReference(sample)
248  const = taskAM.compute(t, q, v, data)
249 
250  # compare the pinocchio method to
251  # Del Prete's quick and dirty way to compute drift
252  # compute momentum Jacobian at next time step assuming zero acc
253  dt = 1e-3
254  q_next = pin.integrate(model, q, dt * v)
255  data_next = robot.data().copy()
256  robot.computeAllTerms(data_next, q_next, v)
257  J_am = data.Ag
258  J_am_next = data_next.Ag
259  drift = (J_am_next[-3:, :] - J_am[-3:, :]).dot(v) / dt
260  drift_pin = pin.computeCentroidalMomentumTimeVariation(model, data).angular
261  diff_drift = norm(drift_pin - drift)
262  print("Difference between drift computations: ", diff_drift)
263 
264  Jpinv = np.linalg.pinv(const.matrix, 1e-5)
265  dv = Jpinv.dot(const.vector)
266 
267  assert np.linalg.norm(Jpinv.dot(const.matrix), 2) - 1.0 < tol
268  v += dt * dv
269  q = pin.integrate(model, q, dt * v)
270  t += dt
271 
272  error = np.linalg.norm(taskAM.momentum_error, 2)
273  assert error - error_past < 1e-4
274  error_past = error
275  if error < 1e-8:
276  print("Success Convergence")
277  break
278  if i % 100 == 0:
279  print("Time :", t, "Momentum error :", error)
280 
281 print("")
282 print("Test Task Joint Posture (Uncommon joints)")
283 print("")
284 
285 
286 # Get robot model
287 (
288  model,
289  geom_model,
290 ) = create_7dof_arm() # A robot containing sperical joints where nq != nv
291 
292 # Initialize problem
293 robot = tsid.RobotWrapper(model, tsid.FIXED_BASE_SYSTEM, False)
294 data = robot.data()
295 
296 q = pin.neutral(model)
297 v = np.zeros(robot.nv)
298 
299 task_joint = tsid.TaskJointPosture("task-posture-uncommon", robot)
300 
301 Kp = 100 * np.ones(robot.nv)
302 Kd = 20.0 * np.ones(robot.na)
303 task_joint.setKp(Kp)
304 task_joint.setKd(Kd)
305 
306 assert np.linalg.norm(Kp - task_joint.Kp, 2) < tol
307 assert np.linalg.norm(Kd - task_joint.Kd, 2) < tol
308 
309 q_ref = pin.randomConfiguration(model)
310 
311 sample = tsid.TrajectorySample(robot.nq, robot.nv)
312 sample.value(q_ref)
313 sample.derivative(np.zeros(robot.nv))
314 sample.second_derivative(np.zeros(robot.nv))
315 
316 error_past = 1e100
317 t = 0.0
318 max_it = 1000
319 
320 for i in range(0, max_it):
321  robot.computeAllTerms(data, q, v)
322  task_joint.setReference(sample)
323  const = task_joint.compute(t, q, v, data)
324 
325  Jpinv = np.linalg.pinv(const.matrix, 1e-5)
326  dv = Jpinv.dot(const.vector)
327 
328  assert np.linalg.norm(Jpinv.dot(const.matrix), 2) - 1.0 < tol
329  v += dt * dv
330  q = pin.integrate(model, q, dt * v)
331  t += dt
332 
333  error = np.linalg.norm(task_joint.position_error, 2)
334  assert error - error_past < 1e-4
335  error_past = error
336  if error < 1e-8:
337  print("Success Convergence")
338  break
339  if i % 100 == 0:
340  print(
341  "Time :",
342  t,
343  "Joint pos error :",
344  error,
345  "Joint vel error :",
346  np.linalg.norm(task_joint.velocity_error, 2),
347  )
348 
349 print("All test is done")
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:16