def mass_matrix_at(self, s_OP):
""" Body mass matrix at a ginve point"""
J = self.inertia_at(s_OP)
s_PG = -np.asarray(s_OP) + self._s_OG
return rigidBodyMassMatrix(self.mass, J, s_PG) # TODO change interface
def mass_matrix(self):
""" Body mass matrix at origin"""
return rigidBodyMassMatrix(self.mass, self.inertia,
self._s_OG) # TODO change interface