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