bindings.py
Go to the documentation of this file.
1 import unittest
2 import pinocchio as pin
3 from pinocchio.utils import np, npl, rand, zero
4 
5 from test_case import PinocchioTestCase as TestCase
6 
7 # This whole file seems to be outdated and superseded by more recent tests
8 # Probably it should be removed and its contents moved or split somewhere else
9 
10 
11 class TestSE3(TestCase):
12  def setUp(self):
13  self.R = rand([3, 3])
14  self.R, _, _ = npl.svd(self.R)
15  self.p = rand(3)
16  self.m = pin.SE3(self.R, self.p)
17 
18  def test_se3(self):
19  R, p, m = self.R, self.p, self.m
20  X = np.vstack(
21  [np.hstack([R, pin.skew(p).dot(R)]), np.hstack([zero([3, 3]), R])]
22  )
23  self.assertApprox(m.action, X)
24  M = np.vstack(
25  [np.hstack([R, np.expand_dims(p, 1)]), np.array([[0.0, 0.0, 0.0, 1.0]])]
26  )
27  self.assertApprox(m.homogeneous, M)
28  m2 = pin.SE3.Random()
29  self.assertApprox((m * m2).homogeneous, m.homogeneous.dot(m2.homogeneous))
30  self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
31 
32  p = rand(3)
33  self.assertApprox(m * p, m.rotation.dot(p) + m.translation)
34  self.assertApprox(
35  m.actInv(p), m.rotation.T.dot(p) - m.rotation.T.dot(m.translation)
36  )
37 
38  # Currently, the different cases do not throw the same exception type.
39  # To have a more robust test, only Exception is checked.
40  # In the comments, the most specific actual exception class at the time of writing
41  p = rand(5)
42  with self.assertRaises(Exception): # RuntimeError
43  m * p
44  with self.assertRaises(Exception): # RuntimeError
45  m.actInv(p)
46  with self.assertRaises(
47  Exception
48  ): # Boost.Python.ArgumentError (subclass of TypeError)
49  m.actInv("42")
50 
51  def test_motion(self):
52  m = self.m
53  self.assertApprox(pin.Motion.Zero().vector, zero(6))
54  v = pin.Motion.Random()
55  self.assertApprox((m * v).vector, m.action.dot(v.vector))
56  self.assertApprox((m.actInv(v)).vector, npl.inv(m.action).dot(v.vector))
57  vv = v.linear
58  vw = v.angular
59  self.assertApprox(v.vector, np.concatenate([vv, vw]))
60  self.assertApprox((v ^ v).vector, zero(6))
61 
62  def test_force(self):
63  m = self.m
64  self.assertApprox(pin.Force.Zero().vector, zero(6))
65  f = pin.Force.Random()
66  ff = f.linear
67  ft = f.angular
68  self.assertApprox(f.vector, np.concatenate([ff, ft]))
69 
70  self.assertApprox((m * f).vector, npl.inv(m.action.T).dot(f.vector))
71  self.assertApprox((m.actInv(f)).vector, m.action.T.dot(f.vector))
72  v = pin.Motion.Random()
73  f = pin.Force(np.concatenate([v.vector[3:], v.vector[:3]]))
74  self.assertApprox((v ^ f).vector, zero(6))
75 
76  def test_inertia(self):
77  m = self.m
78  Y1 = pin.Inertia.Random()
79  Y2 = pin.Inertia.Random()
80  Y = Y1 + Y2
81  self.assertApprox(Y1.matrix() + Y2.matrix(), Y.matrix())
82  v = pin.Motion.Random()
83  self.assertApprox((Y * v).vector, Y.matrix().dot(v.vector))
84  self.assertApprox(
85  (m * Y).matrix(),
86  m.inverse().action.T.dot(Y.matrix()).dot(m.inverse().action),
87  )
88  self.assertApprox(
89  (m.actInv(Y)).matrix(), m.action.T.dot(Y.matrix()).dot(m.action)
90  )
91 
92  def test_cross(self):
93  m = pin.Motion.Random()
94  f = pin.Force.Random()
95  self.assertApprox(m ^ m, m.cross(m))
96  self.assertApprox(m ^ f, m.cross(f))
97  with self.assertRaises(TypeError):
98  m ^ 2
99 
100 
101 if __name__ == "__main__":
102  unittest.main()
bindings.TestSE3.test_inertia
def test_inertia(self)
Definition: bindings.py:76
bindings.TestSE3.setUp
def setUp(self)
Definition: bindings.py:12
bindings.TestSE3.test_force
def test_force(self)
Definition: bindings.py:62
pinocchio.utils
Definition: bindings/python/pinocchio/utils.py:1
bindings.TestSE3.test_motion
def test_motion(self)
Definition: bindings.py:51
bindings.TestSE3.p
p
Definition: bindings.py:15
bindings.TestSE3.test_cross
def test_cross(self)
Definition: bindings.py:92
bindings.TestSE3.R
R
Definition: bindings.py:13
bindings.TestSE3
Definition: bindings.py:11
bindings.TestSE3.m
m
Definition: bindings.py:16
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:42
bindings.TestSE3.test_se3
def test_se3(self)
Definition: bindings.py:18
pinocchio.utils.zero
def zero(n)
Definition: bindings/python/pinocchio/utils.py:38


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:06