def test_inverse(self):
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
exp_rot = iDynTree.Rotation(0, 1, 0, 0, 0, 1, 1, 0, 0)
rot_inv = rotation.inverse()
for r in range(3):
for c in range(3):
self.assertAlmostEqual(rot_inv[r, c], exp_rot[r, c])
def test_composition(self):
r1 = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
r2 = iDynTree.Rotation(0, -1, 0, 0, 0, -1, 1, 0, 0)
r_final = r1 * r2
r_expected = iDynTree.Rotation(1, 0, 0, 0, -1, 0, 0, 0, -1)
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(r_final[r, c], r_expected[r, c])
def test_transform_composition(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
t1 = iDynTree.Transform(rotation, position)
position = iDynTree.Position(3, 4, 5)
rotation = iDynTree.Rotation(0, -1, 0, 0, 0, -1, 1, 0, 0)
t2 = iDynTree.Transform(rotation, position)
t_final = t1 * t2
exp_position = t1.position + t1.rotation * t2.position
exp_rot = t1.rotation * t2.rotation
for i in range(3):
self.assertEqual(t_final.position[i], exp_position[i])
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(t_final.rotation[r, c], exp_rot[r, c])
def test_position_rotation(self):
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
pos = iDynTree.Position(0, 0, 1)
pos_rotated = rotation * pos
expected_pos = iDynTree.Position(1, 0, 0)
for i in range(3):
self.assertAlmostEqual(pos_rotated[i], expected_pos[i])
def test_position_transform(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
pos = iDynTree.Position(0, 0, 1)
pos_transformed = transform * pos
expected_pos = iDynTree.Position(2, 2, 3)
for i in range(3):
self.assertAlmostEqual(pos_transformed[i], expected_pos[i])
def test_creation(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
# Assert the content is the same.
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(transform.rotation[r, c], rotation[r, c])
for i in range(3):
self.assertEqual(transform.position[i], position[i])
def test_explicit_constructor(self):
# Note: the rotation class does not check if the matrix is a real rotation.
rot_raw = np.arange(9).reshape((3, 3))
rotation = iDynTree.Rotation(rot_raw[0, 0], rot_raw[0, 1], rot_raw[0,
2],
rot_raw[1, 0], rot_raw[1, 1], rot_raw[1,
2],
rot_raw[2, 0], rot_raw[2, 1], rot_raw[2,
2])
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(rotation[r, c], rot_raw[r, c])
def test_transform(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
mesh = iDynTree.ExternalMesh()
mesh.link_H_geometry = transform
self.assertEqual(list(mesh.link_H_geometry.position), list(position))
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(mesh.link_H_geometry.rotation[r, c], rotation[r,
c])
def test_rest_transform(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
joint = iDynTree.RevoluteJoint()
joint.set_attached_links(1, 2)
joint.set_rest_transform(transform)
joint_transform = joint.get_rest_transform(1, 2)
self.assertEqual(list(joint_transform.position), list(position))
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(joint_transform.rotation[r, c], rotation[r, c])
def test_link_sensor_transform(self):
sensor = iDynTree.AccelerometerSensor()
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1,
1, 0, 0,
0, 1, 0)
sensor.link_sensor_transform = iDynTree.Transform(rotation, position)
# Assert the content is the same.
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(
sensor.link_sensor_transform.rotation[r, c], rotation[r, c])
for i in range(3):
self.assertEqual(sensor.link_sensor_transform.position[i], position[i])
def test_model_frame(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
model = iDynTree.Model()
link1 = iDynTree.Link()
model.add_link("link1", link1)
num_of_frames = model.get_nr_of_frames()
self.assertTrue(
model.add_additional_frame_to_link("link1", "frame1", transform))
self.assertEqual(model.get_nr_of_frames(), num_of_frames + 1)
frame_index = model.get_frame_index("frame1")
self.assertGreaterEqual(frame_index, 0)
self.assertEqual(model.get_frame_name(frame_index), "frame1")
self.assertEqual(model.get_frame_link(frame_index), 0)
frame_transform = model.get_frame_transform(frame_index)
self.assertEqual(list(frame_transform.position), list(position))
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(frame_transform.rotation[r, c], rotation[r, c])