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)