Exemplo n.º 1
0
 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])
Exemplo n.º 2
0
 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])
Exemplo n.º 3
0
    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])
Exemplo n.º 4
0
    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])
Exemplo n.º 5
0
    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])
Exemplo n.º 6
0
  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])
Exemplo n.º 7
0
 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])