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_ForwardDynamicsConstraintsDirectMoving (self):
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
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
contact_body_id = self.body_1
contact_point = np.array( [0., -1., 0.]);
constraint_set = rbdl.ConstraintSet()
constraint_set.AddContactConstraint (contact_body_id, contact_point, np.array ([1., 0., 0.]), "ground_x");
constraint_set.AddContactConstraint (contact_body_id, contact_point, np.array ([0., 1., 0.]), "ground_y");
constraint_set.AddContactConstraint (contact_body_id, contact_point, np.array ([0., 0., 1.]), "ground_z");
constraint_set.Bind (self.model);
rbdl.ForwardDynamicsConstraintsDirect (self.model, self.q, self.qdot, self.tau, constraint_set, self.qddot);
point_acceleration = rbdl.CalcPointAcceleration (self.model, self.q, self.qdot, self.qddot, contact_body_id, contact_point);
assert_almost_equal( np.array([0., 0., 0.]), point_acceleration)
def test_ForwardDynamicsConstraintsDirectSimple(self):
self.q[1] = 1.
self.qdot[0] = 1.
self.qdot[3] = -1.
contact_body_id = self.body_1
contact_point = np.array([0., -1., 0.])
constraint_set = rbdl.ConstraintSet()
#Since each of these constraints have different user-defined-id values
#the do not get grouped together.
i0 = constraint_set.AddContactConstraint(contact_body_id,
contact_point,
np.array([1., 0.,
0.]), "ground_x", 3)
i1 = constraint_set.AddContactConstraint(contact_body_id,
contact_point,
np.array([0., 1.,
0.]), "ground_y", 4)
i2 = constraint_set.AddContactConstraint(contact_body_id,
contact_point,
np.array([0., 0.,
1.]), "ground_z", 5)
constraint_set.Bind(self.model)
rbdl.ForwardDynamicsConstraintsDirect(self.model, self.q, self.qdot,
self.tau, constraint_set,
self.qddot)
point_acceleration = rbdl.CalcPointAcceleration(
self.model, self.q, self.qdot, self.qddot, contact_body_id,
contact_point)
assert_almost_equal(np.array([0., 0., 0.]), point_acceleration)
#Test the functions to access the group index
gId = constraint_set.getGroupIndexByName("ground_x")
assert_equal(0, gId)
gId = constraint_set.getGroupIndexByName("ground_y")
assert_equal(1, gId)
gId = constraint_set.getGroupIndexByName("ground_z")
assert_equal(2, gId)
gId = constraint_set.getGroupIndexById(3)
assert_equal(0, gId)
gId = constraint_set.getGroupIndexById(4)
assert_equal(1, gId)
gId = constraint_set.getGroupIndexById(5)
assert_equal(2, gId)
gId = constraint_set.getGroupIndexByAssignedId(i0)
assert_equal(0, gId)
gId = constraint_set.getGroupIndexByAssignedId(i1)
assert_equal(1, gId)
gId = constraint_set.getGroupIndexByAssignedId(i2)
assert_equal(2, gId)
def get_point_acceleration(self, link, point=(0., 0., 0.)):
"""
Computes the linear acceleration of a point on a link.
Args:
link (int, str): unique link id, or name.
point (np.array[float[3]]): position of the point in link's local frame
Returns:
np.array[float[3]]: The cartesian acceleration of the point in global frame
"""
link = self.get_link_id(link)
point = np.asarray(point)
return rbdl.CalcPointAcceleration(self.model, self._q, self._dq, self._ddq, link, point, update_kinematics=True)
def test_ForwardDynamicsConstraintsDirectSimple (self):
self.q[1] = 1.
self.qdot[0] = 1.
self.qdot[3] = -1.
contact_body_id = self.body_1
contact_point = np.array( [0., -1., 0.]);
constraint_set = rbdl.ConstraintSet()
i0 = constraint_set.AddContactConstraint (contact_body_id, contact_point, np.array ([1., 0., 0.]), "ground_x",3);
i1 = constraint_set.AddContactConstraint (contact_body_id, contact_point, np.array ([0., 1., 0.]), "ground_y",4);
i2 = constraint_set.AddContactConstraint (contact_body_id, contact_point, np.array ([0., 0., 1.]), "ground_z",5);
constraint_set.Bind (self.model);
rbdl.ForwardDynamicsConstraintsDirect (self.model, self.q, self.qdot, self.tau, constraint_set, self.qddot);
point_acceleration = rbdl.CalcPointAcceleration (self.model, self.q, self.qdot, self.qddot, contact_body_id, contact_point);
assert_almost_equal( np.array([0., 0., 0.]), point_acceleration)
#Test the functions to access the group index
gId = constraint_set.getGroupIndexByName("ground_x")
assert_equal(0,gId)
gId = constraint_set.getGroupIndexByName("ground_y")
assert_equal(0,gId)
gId = constraint_set.getGroupIndexByName("ground_z")
assert_equal(0,gId)
gId = constraint_set.getGroupIndexById(3)
assert_equal(0,gId)
gId = constraint_set.getGroupIndexById(4)
assert_equal(0,gId)
gId = constraint_set.getGroupIndexById(5)
assert_equal(0,gId)
gId = constraint_set.getGroupIndexByAssignedId(i0)
assert_equal(0,gId)
gId = constraint_set.getGroupIndexByAssignedId(i1)
assert_equal(0,gId)
gId = constraint_set.getGroupIndexByAssignedId(i2)
assert_equal(0,gId)