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_Simple(self):
# only tests whether the API seems to work. No functional
# tests yet.
cs = rbdl.ConstraintSet()
idx = cs.AddContactConstraint(1, [1., 2., 3.], [4., 5., 6.])
assert_equal(0, idx)
X = rbdl.SpatialTransform()
sv = rbdl.SpatialVector.fromPythonArray([1., 2., 3., 4., 5., 6.])
idx2 = cs.AddLoopConstraint(1, 2, X, X, sv, 1.)
assert_equal(1, idx2)
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 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)
planar_float_type = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
joint_planar_x = rbdl.Joint.fromJointAxes(planar_float_type)
# 创建关节，x轴转动约束(j1, j2)
joint_rot_x = rbdl.Joint.fromJointType("JointTypeRevoluteX")
# 3. 向模型中添加body
trans = rbdl.SpatialTransform()
trans.r = np.array([0.0, 1.0, 1.0]) # floating base位置
b_base = model.AppendBody(trans, joint_planar_x, b1)
trans.r = np.array([0.0, -l2 / 2, 0.0]) # 关节1相对fb坐标系位置
b_link1 = model.AddBody(b_base, trans, joint_rot_x, b1)
trans.r = np.array([0.0, l2 / 2, 0.0]) # 关节2相对fb坐标系位置
b_link3 = model.AddBody(b_base, trans, joint_rot_x, b3)
# 添加约束
constrain_set_l1 = rbdl.ConstraintSet()
c_point = np.array([0.0, l1, 0.0])
# 话说这个名字参数居然不是可以不要的，虽然没什么用
constrain_set_l1.AddConstraint(b_link1, c_point, np.array([0., 1., 0.]),
'ground_y'.encode('utf-8'))
constrain_set_l1.AddConstraint(b_link1, c_point, np.array([0., 0., 1.]),
'ground_z'.encode('utf-8'))
constrain_set_l1.Bind(model)
# 基于该模型仿真测试一下
q0 = np.zeros(model.q_size)
qd0 = np.zeros(model.qdot_size)
qdd0 = np.zeros(model.qdot_size)
tau = np.zeros(model.qdot_size)
# 思考：基于约束的仿真出来约束点是否会变化，最佳的仿真效果还是从当前出发
# 不过：我们不用这个来仿真，用来只做当前的控制，是没有问题的
joint_rot_z = rbdl.Joint.fromJointType("JointTypeRevoluteZ")
# 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))
link1 = rbdl.Body.fromMassComInertia(m1,c1,J1) 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
def biped_model_create():
model = rbdl.Model()
model.gravity = np.array([0.0, 0.0, -9.81])
thigh_len = 0.5
shank_len = 0.5
# 1. 创建body,全部写下来是为了方便调试
# 左右腿一般是对称的
# 非精准的建模也是允许的
mass, com = 20.0, np.array([0., 0., 0.]) # 躯干
inertia = np.diag([0.37, 0.29, 0.128])
b_torso = rbdl.Body.fromMassComInertia(mass, com, inertia)
mass, com = 0.5, np.array([0., 0., 0.]) # 连接件1
inertia = np.diag([0.0003, 0.0003, 0.0003])
b_link1 = rbdl.Body.fromMassComInertia(mass, com, inertia)
mass, com = 2.0, np.array([0., 0., -thigh_len / 2]) # 大腿
inertia = np.diag([0.04208, 0.04208, 0.00083])
b_thigh = rbdl.Body.fromMassComInertia(mass, com, inertia)
mass, com = 2.0, np.array([0., 0., -shank_len / 2]) # 小腿
inertia = np.diag([0.04208, 0.04208, 0.00083])
b_shank = rbdl.Body.fromMassComInertia(mass, com, inertia)
# 允许存在误差，不对足端进行建模
# mass, com = 0.5, np.array([0., 0., 0.]) # 足--看能不能固定关节
# inertia = np.diag([0.0001, 0.0001, 0.0001])
# b_foot = rbdl.Body.fromMassComInertia(mass, com, inertia)
# 2. 创建运动副
space_free_type = np.diag([1, 1, 1, 1, 1, 1])
joint_float_space = rbdl.Joint.fromJointAxes(space_free_type)
joint_rot_x = rbdl.Joint.fromJointType("JointTypeRevoluteX")
joint_rot_y = rbdl.Joint.fromJointType("JointTypeRevoluteY")
# 3. 向模型中添加body
trans = rbdl.SpatialTransform() # 躯干
trans.r = np.array([0.0, 0.0, 0.0])
id_torso = model.AppendBody(trans, joint_float_space, b_torso)
trans.r = np.array([0.0, 0.12, -0.2]) # 左连接件
id_left_link1 = model.AddBody(id_torso, trans, joint_rot_x, b_link1)
trans.r = np.array([0.0, 0.0, 0.0]) # 左大腿
id_left_thigh = model.AddBody(id_left_link1, trans, joint_rot_y, b_thigh)
trans.r = np.array([0.0, 0.0, -thigh_len]) # 左小腿
id_left_shank = model.AddBody(id_left_thigh, trans, joint_rot_y, b_shank)
trans.r = np.array([0.0, -0.12, -0.2]) # 右连接件
id_right_link1 = model.AddBody(id_torso, trans, joint_rot_x, b_link1)
trans.r = np.array([0.0, 0.0, 0.0]) # 右大腿
id_right_thigh = model.AddBody(id_right_link1, trans, joint_rot_y, b_thigh)
trans.r = np.array([0.0, 0.0, -thigh_len]) # 右小腿
id_right_shank = model.AddBody(id_right_thigh, trans, joint_rot_y, b_shank)
# 4. 设计约束
cs_left = rbdl.ConstraintSet() # 左腿约束
c_point = np.array([0.0, 0.0, -0.55])
cs_left.AddConstraint(id_left_shank, c_point, np.array([1., 0., 0.]), 'ground_x'.encode('utf-8'))
cs_left.AddConstraint(id_left_shank, c_point, np.array([0., 1., 0.]), 'ground_y'.encode('utf-8'))
cs_left.AddConstraint(id_left_shank, c_point, np.array([0., 0., 1.]), 'ground_z'.encode('utf-8'))
cs_right = rbdl.ConstraintSet() # 右腿约束
c_point = np.array([0.0, 0.0, -0.55])
cs_right.AddConstraint(id_right_shank, c_point, np.array([1., 0., 0.]), 'ground_x'.encode('utf-8'))
cs_right.AddConstraint(id_right_shank, c_point, np.array([0., 1., 0.]), 'ground_y'.encode('utf-8'))
cs_right.AddConstraint(id_right_shank, c_point, np.array([0., 0., 1.]), 'ground_z'.encode('utf-8'))
lid = [id_torso,
id_left_link1,id_left_thigh, id_left_shank,
id_right_link1, id_right_thigh, id_right_shank]
# 添加部件的id_list
return model, cs_left, cs_right, lid
