bindings_SE3.py
Go to the documentation of this file.
1 import unittest
2 import pinocchio as pin
3 import numpy as np
4 from pinocchio.utils import eye, zero, rand
5 from copy import deepcopy, copy
6 
7 ones = lambda n: np.ones([n, 1] if isinstance(n, int) else n)
8 
9 
10 class TestSE3Bindings(unittest.TestCase):
11  def test_identity(self):
12  transform = pin.SE3.Identity()
13  self.assertTrue(np.allclose(zero(3), transform.translation))
14  self.assertTrue(np.allclose(eye(3), transform.rotation))
15  self.assertTrue(np.allclose(eye(4), transform.homogeneous))
16  self.assertTrue(np.allclose(eye(6), transform.action))
17  transform.setRandom()
18  transform.setIdentity()
19  self.assertTrue(np.allclose(eye(4), transform.homogeneous))
20 
21  def test_constructor(self):
22  M = pin.SE3.Random()
23  quat = pin.Quaternion(M.rotation)
24  M_from_quat = pin.SE3(quat, M.translation)
25  self.assertTrue(M_from_quat.isApprox(M))
26 
28  transform = pin.SE3.Identity()
29  self.assertTrue(np.allclose(transform.translation, zero(3)))
30 
31  def test_get_rotation(self):
32  transform = pin.SE3.Identity()
33  self.assertTrue(np.allclose(transform.rotation, eye(3)))
34 
36  transform = pin.SE3.Identity()
37  transform.translation = ones(3)
38  self.assertFalse(np.allclose(transform.translation, zero(3)))
39  self.assertTrue(np.allclose(transform.translation, ones(3)))
40 
41  def test_set_rotation(self):
42  transform = pin.SE3.Identity()
43  transform.rotation = zero([3, 3])
44  self.assertFalse(np.allclose(transform.rotation, eye(3)))
45  self.assertTrue(np.allclose(transform.rotation, zero([3, 3])))
46 
47  def test_homogeneous(self):
48  amb = pin.SE3.Random()
49  H = amb.homogeneous
50  self.assertTrue(
51  np.allclose(H[0:3, 0:3], amb.rotation)
52  ) # top left 33 corner = rotation of amb
53  self.assertTrue(
54  np.allclose(H[0:3, 3], amb.translation)
55  ) # top 3 of last column = translation of amb
56  self.assertTrue(
57  np.allclose(H[3, :], [0.0, 0.0, 0.0, 1.0])
58  ) # last row = 0 0 0 1
59 
60  amb_from_H = pin.SE3(H)
61  self.assertTrue(amb_from_H.isApprox(amb))
62 
63  def test_action_matrix(self):
64  amb = pin.SE3.Random()
65  aXb = amb.action
66  self.assertTrue(
67  np.allclose(aXb[:3, :3], amb.rotation)
68  ) # top left 33 corner = rotation of amb
69  self.assertTrue(
70  np.allclose(aXb[3:, 3:], amb.rotation)
71  ) # bottom right 33 corner = rotation of amb
72  tblock = pin.skew(amb.translation).dot(amb.rotation)
73  self.assertTrue(
74  np.allclose(aXb[:3, 3:], tblock)
75  ) # top right 33 corner = translation + rotation
76  self.assertTrue(
77  np.allclose(aXb[3:, :3], zero([3, 3]))
78  ) # bottom left 33 corner = 0
79 
80  def test_inverse(self):
81  amb = pin.SE3.Random()
82  aMb = amb.homogeneous
83  bma = amb.inverse()
84  self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous))
85 
87  amb = pin.SE3.Random()
88  bmc = pin.SE3.Random()
89  amc = amb * bmc
90  cma = amc.inverse()
91 
92  aMb = amb.homogeneous
93  bMc = bmc.homogeneous
94  aMc = amc.homogeneous
95  cMa = np.linalg.inv(aMc)
96 
97  self.assertTrue(np.allclose(aMb.dot(bMc), aMc))
98  self.assertTrue(np.allclose(cma.homogeneous, cMa))
99 
101  amb = pin.SE3.Random()
102  bmc = pin.SE3.Random()
103  amc = amb * bmc
104  cma = amc.inverse()
105 
106  aXb = amb.action
107  bXc = bmc.action
108  aXc = amc.action
109  cXa = np.linalg.inv(aXc)
110 
111  self.assertTrue(np.allclose(aXb.dot(bXc), aXc))
112  self.assertTrue(np.allclose(cma.action, cXa))
113 
114  def test_point_action(self):
115  amb = pin.SE3.Random()
116  aMb = amb.homogeneous
117  p_homogeneous = rand(4)
118  p_homogeneous[3] = 1
119  p = p_homogeneous[0:3].copy()
120 
121  # act
122  self.assertTrue(np.allclose(amb.act(p), (aMb.dot(p_homogeneous))[0:3]))
123 
124  # actinv
125  bMa = np.linalg.inv(aMb)
126  bma = amb.inverse()
127  self.assertTrue(np.allclose(bma.act(p), (bMa.dot(p_homogeneous))[0:3]))
128 
129  def test_member(self):
130  M = pin.SE3.Random()
131  trans = M.translation
132  M.translation[2] = 1.0
133 
134  self.assertTrue(trans[2] == M.translation[2])
135 
136  def test_conversions(self):
137  def compute(m):
138  tq_vec = pin.SE3ToXYZQUAT(m)
139  tq_tup = pin.SE3ToXYZQUATtuple(m)
140  mm_vec = pin.XYZQUATToSE3(tq_vec)
141  mm_tup = pin.XYZQUATToSE3(tq_tup)
142  mm_lis = pin.XYZQUATToSE3(list(tq_tup))
143  return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
144 
145  m = pin.SE3.Identity()
146  tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute(m)
147  self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
148  self.assertTrue(np.allclose(m.homogeneous, mm_vec.homogeneous))
149  self.assertTrue(np.allclose(m.homogeneous, mm_lis.homogeneous))
150  for i in range(6):
151  self.assertTrue(tq_vec[i] == 0 and tq_tup[i] == 0)
152  self.assertTrue(tq_vec[6] == 1 and tq_tup[6] == 1)
153 
154  m = pin.SE3.Random()
155  tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute(m)
156  self.assertTrue(len(tq_vec) == 7)
157  self.assertTrue(len(tq_tup) == 7)
158  for a, b in zip(tq_vec, tq_tup):
159  self.assertTrue(a == b)
160  self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
161  self.assertTrue(mm_vec == mm_tup)
162  self.assertTrue(mm_vec == mm_lis)
163 
164  M = pin.SE3.Random()
165  h = np.array(M)
166 
167  M_from_h = pin.SE3(h)
168  self.assertTrue(M == M_from_h)
169 
170  def test_several_init(self):
171  for _ in range(100000):
172  r = pin.SE3.Random() * pin.SE3.Random()
173  s = r.__str__()
174  self.assertTrue(s != "")
175 
176  def test_copy(self):
177  M = pin.SE3.Random()
178  Mc = copy(M)
179  Mdc = deepcopy(M)
180  self.assertTrue(M == Mc)
181  self.assertTrue(M == Mdc)
182 
183  Mc.setRandom()
184  self.assertFalse(M == Mc)
185  Mdc.setRandom()
186  self.assertFalse(M == Mc)
187 
188 
189 if __name__ == "__main__":
190  unittest.main()
bindings_SE3.TestSE3Bindings
Definition: bindings_SE3.py:10
bindings_SE3.TestSE3Bindings.test_homogeneous
def test_homogeneous(self)
Definition: bindings_SE3.py:47
bindings_SE3.TestSE3Bindings.test_identity
def test_identity(self)
Definition: bindings_SE3.py:11
bindings_SE3.TestSE3Bindings.test_several_init
def test_several_init(self)
Definition: bindings_SE3.py:170
bindings_SE3.TestSE3Bindings.test_action_matrix
def test_action_matrix(self)
Definition: bindings_SE3.py:63
bindings_SE3.TestSE3Bindings.test_get_rotation
def test_get_rotation(self)
Definition: bindings_SE3.py:31
bindings_SE3.TestSE3Bindings.test_conversions
def test_conversions(self)
Definition: bindings_SE3.py:136
bindings_SE3.TestSE3Bindings.test_copy
def test_copy(self)
Definition: bindings_SE3.py:176
bindings_SE3.TestSE3Bindings.test_point_action
def test_point_action(self)
Definition: bindings_SE3.py:114
bindings_SE3.ones
ones
Definition: bindings_SE3.py:7
bindings_SE3.TestSE3Bindings.test_constructor
def test_constructor(self)
Definition: bindings_SE3.py:21
pinocchio.utils
Definition: bindings/python/pinocchio/utils.py:1
bindings_SE3.TestSE3Bindings.test_internal_product_vs_homogeneous
def test_internal_product_vs_homogeneous(self)
Definition: bindings_SE3.py:86
bindings_SE3.TestSE3Bindings.test_member
def test_member(self)
Definition: bindings_SE3.py:129
bindings_SE3.TestSE3Bindings.test_inverse
def test_inverse(self)
Definition: bindings_SE3.py:80
pinocchio.utils.eye
def eye(n)
Definition: bindings/python/pinocchio/utils.py:33
bindings_SE3.TestSE3Bindings.test_get_translation
def test_get_translation(self)
Definition: bindings_SE3.py:27
bindings_SE3.TestSE3Bindings.test_internal_product_vs_action
def test_internal_product_vs_action(self)
Definition: bindings_SE3.py:100
bindings_SE3.TestSE3Bindings.test_set_translation
def test_set_translation(self)
Definition: bindings_SE3.py:35
bindings_SE3.TestSE3Bindings.test_set_rotation
def test_set_rotation(self)
Definition: bindings_SE3.py:41
pinocchio::copy
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
Definition: copy.hpp:42
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:42
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