def test_UpdateKinematicsConsistency(self):
contact_body_id = self.body_1
contact_point = np.array([0., -1., 0.])
self.q[0] = 0.1
self.q[1] = 0.2
self.q[2] = 0.3
self.q[3] = 0.4
self.q[4] = 0.5
self.q[5] = 0.6
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
point1 = rbdl.CalcBodyToBaseCoordinates(self.model, self.q,
contact_body_id, contact_point)
rbdl.UpdateKinematicsCustom(self.model, self.q)
point2 = rbdl.CalcBodyToBaseCoordinates(self.model, self.q,
contact_body_id, contact_point)
self.qdot[0] = 1.1
self.qdot[1] = 1.2
self.qdot[2] = 1.3
self.qdot[3] = -1.4
self.qdot[4] = -1.5
self.qdot[5] = -1.6
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
point_velocity1 = rbdl.CalcPointVelocity(self.model, self.q, self.qdot,
contact_body_id,
contact_point)
rbdl.UpdateKinematicsCustom(self.model, self.q, self.qdot)
point_velocity2 = rbdl.CalcPointVelocity(self.model, self.q, self.qdot,
contact_body_id,
contact_point)
self.qdot[0] = 10.1
self.qdot[1] = 10.2
self.qdot[2] = 10.3
self.qdot[3] = -10.4
self.qdot[4] = -10.5
self.qdot[5] = -10.6
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
point_acceleration1 = rbdl.CalcPointAcceleration(
self.model, self.q, self.qdot, self.qddot, contact_body_id,
contact_point)
rbdl.UpdateKinematicsCustom(self.model, self.q, self.qdot, self.qddot)
point_acceleration2 = rbdl.CalcPointAcceleration(
self.model, self.q, self.qdot, self.qddot, contact_body_id,
contact_point)
assert_almost_equal(point1, point2)
assert_almost_equal(point_velocity1, point_velocity2)
assert_almost_equal(point_acceleration1, point_acceleration2)
def test_CalcPointJacobianNonSquare(self):
""" Computes point Jacobian and checks whether G * qdot is consistent
with CalcPointVelocity. """
self.model = rbdl.Model()
joint_trans_xyz = rbdl.Joint.fromJointType("JointTypeTranslationXYZ")
self.body_1 = self.model.AppendBody(rbdl.SpatialTransform(),
joint_trans_xyz, self.body)
self.body_4 = self.model.AppendBody(rbdl.SpatialTransform(),
joint_trans_xyz, self.body)
point_coords = np.array([0., 0., 1.])
q = np.zeros(self.model.q_size)
G = np.zeros([3, self.model.q_size])
rbdl.CalcPointJacobian(self.model, q, self.body_4, point_coords, G)
qdot = np.ones(self.model.qdot_size)
jac_point_vel = np.dot(G, qdot)
point_vel = rbdl.CalcPointVelocity(self.model, q, qdot, self.body_4,
point_coords)
assert_almost_equal(jac_point_vel, point_vel)
def test_CalcPointJacobian (self):
""" Computes point Jacobian and checks whether G * qdot is consistent
with CalcPointVelocity. """
q = np.zeros (self.model.q_size)
G = np.zeros ([3, self.model.q_size])
point_coords = np.array ([0., 0., 1.])
rbdl.CalcPointJacobian (
self.model,
q,
self.body_3,
point_coords,
G
)
qdot = np.ones(self.model.qdot_size)
point_vel = rbdl.CalcPointVelocity (
self.model,
q,
qdot,
self.body_3,
point_coords
)
jac_point_vel = np.dot (G, qdot)
assert_almost_equal (jac_point_vel, point_vel)
def test_CalcPointVelocity(self):
"""
Checks whether CalcBodyToBaseCoordinates and CalcBaseToBodyCoordinates
give the right results.
"""
q = np.zeros(self.model.q_size)
qdot = np.zeros(self.model.q_size)
qdot[0] = 1.
point_local = np.array([0., 0., 0.])
point_vel = rbdl.CalcPointVelocity(self.model, q, qdot, self.body_3,
point_local)
assert_almost_equal(np.array([2., 0., 0.]), point_vel)