4 import pinocchio
as pin
5 from test_case
import PinocchioTestCase
as TestCase
10 self.
model = pin.buildSampleModelHumanoidRandom()
12 qmax = np.full((self.
model.nq, 1), np.pi)
13 self.
q = pin.randomConfiguration(self.
model, -qmax, qmax)
14 self.
v = np.random.rand(self.
model.nv)
19 q_ones = np.ones(model.nq)
20 self.assertFalse(pin.isNormalized(model, q_ones))
21 self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
22 self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
24 q_rand = np.random.rand(model.nq)
25 q_rand = pin.normalize(model, q_rand)
26 self.assertTrue(pin.isNormalized(model, q_rand))
27 self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
29 self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.0) <= 1e-8)
31 q_next = pin.integrate(model, self.
q, np.zeros(model.nv))
32 self.assertApprox(q_next, self.
q)
34 v_diff = pin.difference(model, self.
q, q_next)
35 self.assertApprox(v_diff, np.zeros(model.nv))
37 q_next = pin.integrate(model, self.
q, self.
v)
38 q_int = pin.interpolate(model, self.
q, q_next, 0.5)
40 self.assertApprox(q_int, q_int)
42 value = pin.squaredDistance(model, self.
q, self.
q)
43 self.assertTrue((value <= 1e-8).all())
45 dist = pin.distance(model, self.
q, self.
q)
46 self.assertTrue(dist <= 1e-8)
48 q_neutral = pin.neutral(model)
49 self.assertApprox(q_neutral, q_neutral)
51 q_rand1 = pin.randomConfiguration(model)
52 q_rand2 = pin.randomConfiguration(model, -np.ones(model.nq), np.ones(model.nq))
54 self.assertTrue(pin.isSameConfiguration(model, self.
q, self.
q, 1e-8))
56 self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 1e-8))
64 J0, J1 = pin.dIntegrate(model, q, v)
65 res_0 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG0)
66 res_1 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG1)
68 self.assertApprox(J0, res_0)
69 self.assertApprox(J1, res_1)
71 q_next = pin.integrate(model, q, v)
73 J0, J1 = pin.dDifference(model, q, q_next)
74 res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
75 res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
77 self.assertApprox(J0, res_0)
78 self.assertApprox(J1, res_1)
82 Jq, Jv = pin.dIntegrate(model, self.
q, self.
v)
83 mat = np.random.randn(model.nv, 2)
85 mat_transported_q = pin.dIntegrateTransport(
86 model, self.
q, self.
v, mat, pin.ARG0
88 mat_transported_v = pin.dIntegrateTransport(
89 model, self.
q, self.
v, mat, pin.ARG1
92 self.assertApprox(mat_transported_q, np.dot(Jq, mat))
93 self.assertApprox(mat_transported_v, np.dot(Jv, mat))
96 if __name__ ==
"__main__":