# lock hands in closed position
lockedjointDict = robot.leftHandClosed ()
lockedJoints = list ()
for name, value in lockedjointDict.iteritems ():
ljName = "locked_" + name
ps.createLockedJoint (ljName, name, value)
lockedJoints.append (ljName)
lockedjointDict = robot.rightHandClosed ()
for name, value in lockedjointDict.iteritems ():
ljName = "locked_" + name
ps.createLockedJoint (ljName, name, value)
lockedJoints.append (ljName)
ps.setLockedJointConstraints ("locked-hands", lockedJoints)
q1 = [0.0, 0.0, 0.705, 0., 0., 0., 1.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q1)
if res [0]:
q1proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
q2 = [0.0, 0.0, 0.705, 0., 0., 0., 1., 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q2)
if res[0]:
q2proj = res[1]
