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 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)
def test_calcForces (self):
self.q.fill(0.)
self.qd.fill(0.)
self.qdd.fill(0.)
self.tau.fill(0.)
self.q[7] = -self.l1
self.tau[3] = -self.l2*0.5*self.m2*self.model.gravity[1]
#print("Q")
#print(self.q)
#print("QDot")
#print(self.qd)
#print("Tau")
#print(self.tau)
rbdl.ForwardDynamicsConstraintsDirect(self.model,self.q,self.qd,self.tau,self.cs,self.qdd)
#print("QDDot")
#print(self.qdd)
for i in range(0,self.model.qdot_size):
assert_almost_equal(self.qdd[i],0.)
gId0 = 0
gId1 = 1
csListBodyIds = np.ndarray([2], dtype=np.uintc )
csListX = np.ndarray([6,6,2],dtype=float)
csListF = np.ndarray([6,2],dtype=float)
csListXTest = np.ndarray([6,6,2],dtype=float)
csListFTest = np.ndarray([6,2],dtype=float)
csListXTest.fill(0.)
csListFTest.fill(0.)
csListFTest[4,0] = (self.m1+self.m2)*self.model.gravity[1]
csListFTest[4,1] = -(self.m1+self.m2)*self.model.gravity[1]
for i in range(0,6):
csListXTest[i,i,0]=1.
csListXTest[i,i,1]=1.
self.cs.calcForces( gId0,self.model,self.q,self.qd,
csListBodyIds,csListX,csListF,False,False)
#print("bodyId, Transform, Forces")
#for i in range(0,csListBodyIds.shape[0]):
# print(csListBodyIds[i])
# print(csListX[:,:,i])
# print(csListF[:,i])
assert_equal(csListBodyIds[0],0)
assert_equal(csListBodyIds[1],self.iLink1)
assert_almost_equal(csListX, csListXTest)
assert_almost_equal(csListF, csListFTest)
csListBodyIds = np.ndarray([2], dtype=np.uintc )
csListX = np.ndarray([6,6,2],dtype=float)
csListF = np.ndarray([6,2],dtype=float)
self.cs.calcForces( gId1,self.model,self.q,self.qd,
csListBodyIds,csListX,csListF,False,False)
#print("bodyId, Transform, Forces")
#for i in range(0,csListBodyIds.shape[0]):
# print(csListBodyIds[i])
# print(csListX[:,:,i])
# print(csListF[:,i])
assert_equal(csListBodyIds[0],self.iLink1)
assert_equal(csListBodyIds[1],self.iLink2)
csListXTest.fill(0.)
csListFTest.fill(0.)
csListFTest[2,0] = (self.m2*self.l2*0.5)*self.model.gravity[1]
csListFTest[2,1] = -csListFTest[2,0]
csListFTest[4,0] = (self.m2)*self.model.gravity[1]
csListFTest[4,1] = -csListFTest[4,0]
for i in range(0,6):
csListXTest[i,i,0]=1.
csListXTest[i,i,1]=1.
csListXTest[3,2,0] = self.l1 #Spatial transform of r=(0,-l1,0), E=eye
csListXTest[5,0,0] = -self.l1
assert_almost_equal(csListX, csListXTest)
assert_almost_equal(csListF, csListFTest)
