2 from test_case
import PinocchioTestCase
as TestCase
4 import pinocchio
as pin
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(
53 model, -np.ones((model.nq)), np.ones((model.nq))
56 self.assertTrue(pin.isSameConfiguration(model, self.
q, self.
q, 1e-8))
58 self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2, 1e-8))
66 J0, J1 = pin.dIntegrate(model, q, v)
67 res_0 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG0)
68 res_1 = pin.dIntegrate(model, q, v, pin.ArgumentPosition.ARG1)
70 self.assertApprox(J0, res_0)
71 self.assertApprox(J1, res_1)
73 q_next = pin.integrate(model, q, v)
75 J0, J1 = pin.dDifference(model, q, q_next)
76 res_0 = pin.dDifference(model, q, q_next, pin.ARG0)
77 res_1 = pin.dDifference(model, q, q_next, pin.ARG1)
79 self.assertApprox(J0, res_0)
80 self.assertApprox(J1, res_1)
84 Jq, Jv = pin.dIntegrate(model, self.
q, self.
v)
85 mat = np.random.randn(model.nv, 2)
87 mat_transported_q = pin.dIntegrateTransport(
88 model, self.
q, self.
v, mat, pin.ARG0
90 mat_transported_v = pin.dIntegrateTransport(
91 model, self.
q, self.
v, mat, pin.ARG1
94 self.assertApprox(mat_transported_q, np.dot(Jq, mat))
95 self.assertApprox(mat_transported_v, np.dot(Jv, mat))
98 if __name__ ==
"__main__":