test-op-point-modifier.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
4 
5 import unittest
6 
7 import numpy as np
8 from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
9 
10 gaze = np.array(
11  (
12  (
13  (1.0, 0.0, 0.0, 0.025),
14  (0.0, 1.0, 0.0, 0.0),
15  (0.0, 0.0, 1.0, 0.648),
16  (0.0, 0.0, 0.0, 1.0),
17  )
18  )
19 )
20 
21 Jgaze = np.array(
22  (
23  (
24  (1.0, 0.0, 0.0, 0.0, 0.648, 0.0),
25  (0.0, 1.0, 0.0, -0.648, 0.0, 0.025),
26  (0.0, 0.0, 1.0, 0.0, -0.025, 0.0),
27  (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
28  (0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
29  (0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
30  )
31  )
32 )
33 
34 I4 = np.array(
35  (
36  (1.0, 0.0, 0.0, 0.0),
37  (0.0, 1.0, 0.0, 0.0),
38  (0.0, 0.0, 1.0, 0.0),
39  (0.0, 0.0, 0.0, 1.0),
40  )
41 )
42 
43 I6 = np.array(
44  (
45  (1.0, 0.0, 0.0, 0.0, 0.0, 0.0),
46  (0.0, 1.0, 0.0, 0.0, 0.0, 0.0),
47  (0.0, 0.0, 1.0, 0.0, 0.0, 0.0),
48  (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
49  (0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
50  (0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
51  )
52 )
53 
54 
55 class OpPointModifierTest(unittest.TestCase):
56  def test_simple(self):
57  op = OpPointModifier("op")
58  op.setTransformation(I4)
59  op.positionIN.value = I4
60  op.jacobianIN.value = I6
61  op.position.recompute(0)
62  op.jacobian.recompute(0)
63 
64  self.assertTrue((op.getTransformation() == I4).all())
65  self.assertTrue((op.position.value == I4).all())
66  self.assertTrue((op.jacobian.value == I6).all())
67 
68  def test_translation(self):
69  tx = 11.0
70  ty = 22.0
71  tz = 33.0
72 
73  T = np.array(
74  (
75  (1.0, 0.0, 0.0, tx),
76  (0.0, 1.0, 0.0, ty),
77  (0.0, 0.0, 1.0, tz),
78  (0.0, 0.0, 0.0, 1.0),
79  )
80  )
81 
82  op = OpPointModifier("op2")
83  op.setTransformation(T)
84  op.positionIN.value = gaze
85  op.jacobianIN.value = Jgaze
86  op.position.recompute(1)
87  op.jacobian.recompute(1)
88 
89  self.assertTrue((op.getTransformation() == T).all())
90 
91  # w_M_s = w_M_g * g_M_s
92  w_M_g = gaze
93  g_M_s = T
94  w_M_s_ref = w_M_g.dot(g_M_s)
95  w_M_s = op.position.value
96 
97  # Check w_M_s == w_M_s_ref
98  self.assertTrue((w_M_s == w_M_s_ref).all())
99 
100  twist = np.array(
101  [
102  [1.0, 0.0, 0.0, 0.0, tz, -ty],
103  [0.0, 1.0, 0.0, -tz, 0.0, tx],
104  [0.0, 0.0, 1.0, ty, -tx, 0.0],
105  [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
106  [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
107  [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
108  ]
109  )
110 
111  J = op.jacobian.value
112  J_ref = twist.dot(Jgaze)
113 
114  # Check w_M_s == w_M_s_ref
115  self.assertTrue((J == J_ref).all())
116 
117  def test_rotation(self):
118  T = np.array(
119  (
120  (0.0, 0.0, 1.0, 0.0),
121  (0.0, -1.0, 0.0, 0.0),
122  (1.0, 0.0, 0.0, 0.0),
123  (0.0, 0.0, 0.0, 1.0),
124  )
125  )
126 
127  op = OpPointModifier("op3")
128  op.setTransformation(T)
129  op.positionIN.value = gaze
130  op.jacobianIN.value = Jgaze
131  op.position.recompute(1)
132  op.jacobian.recompute(1)
133 
134  self.assertTrue((op.getTransformation() == T).all())
135 
136  # w_M_s = w_M_g * g_M_s
137  w_M_g = gaze
138  g_M_s = T
139  w_M_s_ref = w_M_g.dot(g_M_s)
140  w_M_s = op.position.value
141 
142  # Check w_M_s == w_M_s_ref
143  self.assertTrue((w_M_s == w_M_s_ref).all())
144 
145  twist = np.array(
146  [
147  [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
148  [0.0, -1.0, 0.0, 0.0, 0.0, 0.0],
149  [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
150  [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
151  [0.0, 0.0, 0.0, 0.0, -1.0, 0.0],
152  [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
153  ]
154  )
155 
156  J = op.jacobian.value
157  J_ref = twist.dot(Jgaze)
158 
159  # Check w_M_s == w_M_s_ref
160  self.assertTrue((J == J_ref).all())
161 
162  def test_rotation_translation(self):
163  tx = 11.0
164  ty = 22.0
165  tz = 33.0
166 
167  T = np.array(
168  (
169  (0.0, 0.0, 1.0, tx),
170  (0.0, -1.0, 0.0, ty),
171  (1.0, 0.0, 0.0, tz),
172  (0.0, 0.0, 0.0, 1.0),
173  )
174  )
175 
176  op = OpPointModifier("op4")
177  op.setTransformation(T)
178  op.positionIN.value = gaze
179  op.jacobianIN.value = Jgaze
180  op.position.recompute(1)
181  op.jacobian.recompute(1)
182 
183  self.assertTrue((op.getTransformation() == T).all())
184 
185  # w_M_s = w_M_g * g_M_s
186  w_M_g = gaze
187  g_M_s = T
188  w_M_s_ref = w_M_g.dot(g_M_s)
189  w_M_s = op.position.value
190 
191  # Check w_M_s == w_M_s_ref
192  self.assertTrue((w_M_s == w_M_s_ref).all())
193 
194  twist = np.array(
195  [
196  [0.0, 0.0, 1.0, ty, -tx, 0.0],
197  [0.0, -1.0, 0.0, tz, 0.0, -tx],
198  [1.0, 0.0, 0.0, 0.0, tz, -ty],
199  [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
200  [0.0, 0.0, 0.0, 0.0, -1.0, 0.0],
201  [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
202  ]
203  )
204 
205  J = op.jacobian.value
206  J_ref = twist.dot(Jgaze)
207 
208  # Check w_M_s == w_M_s_ref
209  self.assertTrue((J == J_ref).all())
210 
211 
212 if __name__ == "__main__":
213  unittest.main()
test-op-point-modifier.OpPointModifierTest
Definition: test-op-point-modifier.py:55
test-op-point-modifier.OpPointModifierTest.test_rotation
def test_rotation(self)
Definition: test-op-point-modifier.py:117
test-op-point-modifier.OpPointModifierTest.test_rotation_translation
def test_rotation_translation(self)
Definition: test-op-point-modifier.py:162
test-op-point-modifier.OpPointModifierTest.test_simple
def test_simple(self)
Definition: test-op-point-modifier.py:56
test-op-point-modifier.OpPointModifierTest.test_translation
def test_translation(self)
Definition: test-op-point-modifier.py:68

sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:32