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_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_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_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])