def test_CalcCenterOfMass(self):
""" Tests calculation of center of mass
TODO: add checks for angular momentum
"""
com = np.array([-1., -1., -1.])
com_vel = np.array([-2., -2., -2.])
ang_mom = np.array([-3., -3., -3.])
self.qdot[0] = 1.
mass = rbdl.CalcCenterOfMass(self.model, self.q, self.qdot, com, None,
None)
self.assertEqual(3, mass)
assert_almost_equal(np.array([0., 0., 1.5]), com)
assert_almost_equal(np.array([0., 0., 1.5]), com)
mass = rbdl.CalcCenterOfMass(self.model, self.q, self.qdot, com,
com_vel, None)
self.assertEqual(3, mass)
assert_almost_equal(np.array([0., 0., 1.5]), com)
assert_almost_equal(np.array([1.5, 0., 0.0]), com_vel)
mass = rbdl.CalcCenterOfMass(self.model, self.q, self.qdot, com,
com_vel, ang_mom)
self.assertEqual(3, mass)
assert_almost_equal(np.array([0., 0., 1.5]), com)
def get_com_velocity(self):
"""
Get the linear CoM velocity.
Returns:
np.array[float[3]]: CoM velocity.
"""
rbdl.CalcCenterOfMass(self.model, self._q, self._dq, self._ddq, self.com, com_velocity=self.com_vel)
return self.com_vel
def get_com_acceleration(self):
"""
Get the linear CoM acceleration.
Returns:
np.array[float[3]]: CoM acceleration.
"""
rbdl.CalcCenterOfMass(self.model, self._q, self._dq, self._ddq, self.com, com_acceleration=self.com_acc)
return self.com_acc
def get_com_position(self):
"""
Get the position of the center of mass (CoM).
Returns:
np.array[float[3]]: position of the center of mass
"""
# return rbdl.CalcCenterOfMass(self.model, self._q, self._dq, self._ddq, self.com, self.com_vel, self.com_acc,
# self.angular_momentum_com, self.change_angular_momentum_com,
# update_kinematics=True)
rbdl.CalcCenterOfMass(self.model, self._q, self._dq, self._ddq, self.com) # TODO: update library
return self.com
