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)