def test_sum(self): mass = 3.14 position = iDynTree.Position(1, 2, 3) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[1, 1] = 1 inertia1 = iDynTree.SpatialInertia(mass, position, rotational_inertia) mass = 6.28 position = iDynTree.Position(4, 5, 6) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[2, 2] = 1 inertia2 = iDynTree.SpatialInertia(mass, position, rotational_inertia) # Both inertias are expressed w.r.t. the same frame. We can sum them. inertia_sum = inertia1 + inertia2 # Mass is the sum. self.assertAlmostEqual(inertia_sum.get_mass(), inertia1.get_mass() + inertia2.get_mass()) # The first moment of mass is the sum. isum_first = self._get_first_moment(inertia_sum) isum_first_computed = (self._get_first_moment(inertia1) + self._get_first_moment(inertia2)) for i in range(3): self.assertAlmostEqual(isum_first[i], isum_first_computed[i]) # The rotational inertia is the sum. isum_rot = np.array( inertia_sum.get_rotational_inertia_wrt_frame_origin()) isum_rot_computed = ( np.array(inertia1.get_rotational_inertia_wrt_frame_origin()) + np.array(inertia2.get_rotational_inertia_wrt_frame_origin())) for r, c in itertools.product(range(3), range(3)): self.assertAlmostEqual(isum_rot[r, c], isum_rot_computed[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_subtract_operation(self): pos1 = iDynTree.Position(1, 2, 3) pos2 = iDynTree.Position(4, 5, 6) sum_pos = pos1 - pos2 self.assertEqual(sum_pos[0], -3) self.assertEqual(sum_pos[1], -3) self.assertEqual(sum_pos[2], -3) # Original objects are unmodified. self.assertEqual(tuple(pos1), (1, 2, 3)) self.assertEqual(tuple(pos2), (4, 5, 6))
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_creation(self): mass = 3.14 position = iDynTree.Position(1, 2, 3) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[1, 1] = 1 inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia) # Get the matrix (6x6) representation of the inertia. matrix = np.array(inertia.as_matrix(), copy=False) # The top left 3x3 matrix is a diagonal matrix with the mass on the # diagonal. for r, c in itertools.product(range(3), range(3)): if r == c: self.assertEqual(matrix[r, c], mass) else: self.assertEqual(matrix[r, c], 0) # The bottom right 3x3 matrix is the rotational inertia. for r, c in itertools.product(range(3), range(3)): self.assertEqual(matrix[3 + r, 3 + c], rotational_inertia[r, c]) # The off diagonal 3x3 matrix blocks are m r^ and symmetric. def r_vector_item(vector, row, col): matrix = np.zeros((3, 3)) matrix[0, 1] = -vector[2] matrix[0, 2] = vector[1] matrix[1, 2] = -vector[0] matrix[1, 0] = -matrix[0, 1] matrix[2, 0] = -matrix[0, 2] matrix[2, 1] = -matrix[1, 2] return matrix[row, col] for r, c in itertools.product(range(3), range(3)): r_vector = r_vector_item(position, r, c) self.assertEqual(matrix[3 + r, c], r_vector * mass) self.assertEqual(matrix[r, 3 + c], -r_vector * mass)
def test_read_origin_and_direction_properties(self): n_factor = np.linalg.norm([1, 2, 3]) raw_dir = [1 / (n_factor), 2 / n_factor, 3 / n_factor] direction = iDynTree.Direction(raw_dir[0], raw_dir[1], raw_dir[2]) origin = iDynTree.Position(3, 4, 5) axis = iDynTree.Axis(direction, origin) self.assertEqual(list(axis.direction), raw_dir) self.assertEqual(list(axis.origin), [3, 4, 5])
def test_negate_operator(self): pos = iDynTree.Position(1, 2, 3) neg_pos = -pos self.assertEqual(neg_pos[0], -1) self.assertEqual(neg_pos[1], -2) self.assertEqual(neg_pos[2], -3) # Original object is unmodified. self.assertEqual(tuple(pos), (1, 2, 3))
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_inertia(self): mass = 3.14 position = iDynTree.Position(1, 2, 3) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[1, 1] = 1 inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia) link = iDynTree.Link() link.inertia = inertia self.assertEqual(link.inertia.get_mass(), mass)
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_sum_assignment(self): # We do not explicitly define this operation. # Check if Python does the correct thing by using the elementary + operator. mass = 3.14 position = iDynTree.Position(1, 2, 3) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[1, 1] = 1 inertia1 = iDynTree.SpatialInertia(mass, position, rotational_inertia) mass = 6.28 position = iDynTree.Position(4, 5, 6) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[2, 2] = 1 inertia_sum = iDynTree.SpatialInertia(mass, position, rotational_inertia) com = self._get_first_moment(inertia_sum) rot = inertia_sum.get_rotational_inertia_wrt_frame_origin() # Both inertias are expressed w.r.t. the same frame. We can sum them. inertia_sum += inertia1 # Mass is the sum. self.assertAlmostEqual(inertia_sum.get_mass(), inertia1.get_mass() + mass) # The first moment of mass is the sum. isum_first = self._get_first_moment(inertia_sum) isum_first_computed = self._get_first_moment(inertia1) + com for i in range(3): self.assertAlmostEqual(isum_first[i], isum_first_computed[i]) # The rotational inertia is the sum. isum_rot = np.array( inertia_sum.get_rotational_inertia_wrt_frame_origin()) isum_rot_computed = ( np.array(inertia1.get_rotational_inertia_wrt_frame_origin()) + rot) for r, c in itertools.product(range(3), range(3)): self.assertAlmostEqual(isum_rot[r, c], isum_rot_computed[r, c])
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])
def test_non_zero_creation(self): pos = iDynTree.Position(1, 2, 3) self.assertEqual(pos[0], 1) self.assertEqual(pos[1], 2) self.assertEqual(pos[2], 3)