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