# lock hands in closed position
lockedjointDict = robot.leftHandClosed()
lockedJoints = list()
for name, value in lockedjointDict.items():
ljName = "locked_" + name
ps.createLockedJoint(ljName, name, value)
lockedJoints.append(ljName)
lockedjointDict = robot.rightHandClosed()
for name, value in lockedjointDict.items():
ljName = "locked_" + name
ps.createLockedJoint(ljName, name, value)
lockedJoints.append(ljName)
ps.addLockedJointConstraints("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.")
