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 update(self):
"""
Update RBDL model with current joint positions, velocities and
accelerations.
:return: None
"""
# rbdl.UpdateKinematicsCustom(_rbdl_model, q_ptr, qdot_ptr, qddot_ptr)
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
def update(self, q=None, dq=None, ddq=None):
"""Update: move to the next step."""
if q is not None:
self._q = q
if dq is not None:
self._dq = dq
if ddq is not None:
self._ddq = ddq
rbdl.UpdateKinematics(self.model, self._q, self._dq, self._ddq)
def cb_joint_state(self, data):
self.joint_names = sort_data(data.name)
self.q = np.array(sort_data(data.position))
rbdl.CalcBodyToBaseCoordinates(self.model, self.q, 0, self.X, False)
rbdl.CalcBodySpatialJacobian(self.model, self.q, 0, self.X, self.J,
False)
rbdl.UpdateKinematics(self.model, self.q, self.qd, self.qdd)
self.calculate_torque()
self.calculate_acceleration()
def test_AccessToModelParameters(self):
"""
Checks whether vital model parameters can be accessed that are
stored in the "Model" class.
"""
rbdl.UpdateKinematics(self.model, self.q, self.qdot, self.qddot)
assert_equal(self.model.mBodies[2].mMass, self.body.mMass)
assert_equal(self.model.mBodies[2].mCenterOfMass,
self.body.mCenterOfMass)
assert_equal(self.model.mBodies[2].mInertia, self.body.mInertia)
assert_equal(self.model.X_T[1].E, rbdl.SpatialTransform().E)
assert_equal(self.model.X_T[1].r, rbdl.SpatialTransform().r)
assert_equal(self.model.X_T[2].E, self.xtrans.E)
assert_equal(self.model.X_T[2].r, self.xtrans.r)
assert_almost_equal(self.model.X_base[0].E, np.identity(3))
assert_almost_equal(self.model.X_base[3].r,
self.xtrans.r + self.xtrans.r)
assert_equal(self.model.mJoints[2].mJointType, "JointTypeRevoluteY")
