# AddBody(parent id, joint_frame, joint, body, body name)
# Parameters:
# parent_id: id of the parent body
# joint_frame: the transformation from the parent frame to the origin of the joint frame (represents X_T in RBDA)
# joint: specification for the joint that describes the connection
# body: specification of the body itself
# body_name: human readable name for the body (can be used to retrieve its id with GetBodyId())
idB1 = model.AddBody(0., Xs, joint_rot_z, link1)
idB2 = model.AddBody(idB1, Xp, joint_rot_z, link2)
idB3 = model.AddBody(0., Xs, joint_rot_z, link1)
idB4 = model.AddBody(idB3, Xp, joint_rot_z, link2)
idB5 = model.AddBody(idB4, Xp, joint_rot_z, virtual_body)
# AddLoopConstraint (id_predecessor, id_successor,SpatialTransform & X_predecessor,SpatialTransform & X_successor,SpatialVector&axis,enable_stabilization = false,stabilization_param = 0.1,constraint_name = NULL)
cs = rbdl.ConstraintSet()
cs.AddLoopConstraint(idB2, idB5, Xp, Xs,
rbdl.SpatialVector(0, [0, 0, 0, 1, 0, 0]), False, 0.1)
cs.AddLoopConstraint(idB2, idB5, Xp, Xs,
rbdl.SpatialVector(0, [0, 0, 0, 0, 1, 0]), False, 0.1)
cs.AddLoopConstraint(idB2, idB5, Xp, Xs,
rbdl.SpatialVector(0, [0, 0, 1, 0, 0, 0]), False, 0.1)
cs.Bind(model)
q = np.zeros(model.q_size)
qdot = np.zeros(model.qdot_size)
qddot = np.zeros(model.qdot_size)
tau = np.zeros(model.qdot_size)
print("shape of q:", np.shape(q))
print("Size of q:", np.size(q))
# Testing the 4-bar linkage in the 0 degree angle for all the joints
q[0] = 0.
def setUp(self):
# Make a triple-perpendicular-pendulum in absolute coordinates using
# 5 loop constraints to implement the first 2 pin joints
#
# The perpendicular pendulum pictured with joint angles of 0,0.
# The first joint rotates about the x axis, while the second
# joint rotates about the local y axis of link 1
#
# y
# |
# |___ x
# z / |
# | |
# / | | link1
# | |
# / | |
# axis1:z0| |__________
# (_____________) link 2
# | |
# |
#
# |
#
# | axis2:y1
#
self.model = rbdl.Model()
self.model.gravity = np.array([0.,-9.81,0.])
l1=1.
l2=1.
m1=1.
m2=1.
self.l1=l1
self.l2=l2
self.m1=m1
self.m2=m2
c1 = np.array([0.,-l1*0.5,0.])
J1 = np.array([[m1*l1*l1/3., 0., 0.],
[ 0., m1*l1*l1/30., 0.],
[ 0., 0., m1*l1*l1/3.]])
c2 = np.array([l2*0.5,0.,0.])
J2 = np.array([ [m2*l2*l2/30., 0., 0.],
[ 0., m2*l2*l2/3., 0.],
[ 0., 0., m2*l2*l2/3.]])
Xp1 = rbdl.SpatialTransform()
Xp1.r = np.array([0.,0.,0.])
Xp1.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
Xs1 = rbdl.SpatialTransform()
Xs1.r = np.array([0.,0.,0.])
Xs1.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
Xp2 = rbdl.SpatialTransform()
Xp2.r = np.array([0.,-l1,0.])
Xp2.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
Xs2 = rbdl.SpatialTransform()
Xs2.r = np.array([0.,0.,0.])
Xs2.E = np.array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])
axis = np.asarray([
[0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 1.],
[0., 0., 1., 0., 0., 0.],
[0., 1., 0., 0., 0., 0.],
[1., 0., 0., 0., 0., 0.],
])
joint6Dof = rbdl.Joint.fromJointAxes (axis)
self.link1 = rbdl.Body.fromMassComInertia(m1,c1,J1)
self.link2 = rbdl.Body.fromMassComInertia(m2,c2,J2)
self.iLink1= self.model.AppendBody(rbdl.SpatialTransform(),joint6Dof,
self.link1)
self.iLink2= self.model.AppendBody(rbdl.SpatialTransform(),joint6Dof,
self.link2)
#point_coords = np.array ([0., 0., 1.])
self.q = np.zeros (self.model.q_size)
self.qd = np.zeros (self.model.qdot_size)
self.qdd = np.zeros (self.model.qdot_size)
self.tau = np.zeros (self.model.qdot_size)
assert(self.iLink1==6)
assert(self.iLink2==12)
self.cs = rbdl.ConstraintSet()
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,1,0,0]),False,0.1,"LoopGroundLink1",7)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,0,1,0]),False)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,0,0,1]),False)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[1,0,0,0,0,0]),False)
self.cs.AddLoopConstraint(0,self.iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,1,0,0,0,0]),False)
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,1,0,0]),False,0.1,"LoopLink1Link2",11)
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,0,1,0]))
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,0,0,1]))
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[1,0,0,0,0,0]))
self.cs.AddLoopConstraint( self.iLink1, self.iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,1,0,0,0]))
self.cs.Bind(self.model)
link2 = rbdl.Body.fromMassComInertia(m2,c2,J2) iLink1= model.AppendBody(rbdl.SpatialTransform(),joint6Dof,link1) iLink2= model.AppendBody(rbdl.SpatialTransform(),joint6Dof,link2) #point_coords = np.array ([0., 0., 1.]) q = np.zeros (model.q_size) qd = np.zeros (model.qdot_size) qdd = np.zeros (model.qdot_size) tau = np.zeros (model.qdot_size) assert(iLink1==6) assert(iLink2==12) cs = rbdl.ConstraintSet() cs.AddLoopConstraint(0,iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,1,0,0]),False,0.1,"LoopGroundLink1",7) cs.AddLoopConstraint(0,iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,0,1,0]),False) cs.AddLoopConstraint(0,iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,0,0,0,0,1]),False) cs.AddLoopConstraint(0,iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[1,0,0,0,0,0]),False) cs.AddLoopConstraint(0,iLink1,Xp1,Xs1,rbdl.SpatialVector(0,[0,1,0,0,0,0]),False) cs.AddLoopConstraint( iLink1, iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,1,0,0]),False,0.1,"LoopLink1Link2",11) cs.AddLoopConstraint( iLink1, iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,0,1,0])) cs.AddLoopConstraint( iLink1, iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,0,0,0,1])) cs.AddLoopConstraint( iLink1, iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[1,0,0,0,0,0])) cs.AddLoopConstraint( iLink1, iLink2, Xp2, Xs2, rbdl.SpatialVector(0,[0,0,1,0,0,0])) cs.Bind(model) # self.qdot[5] = -1.6 contact_body_id = body_1