# Add constraints to problem solver
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot",])
ps.createPositionConstraint("RightHandPos", "RWristPitch", "", [0.0, 0.0, 0.0], [-2.6, -4, 0.75], True, True, True)
#ps.createOrientationConstraint("RightHandOr", "", "RWristPitch", [quat.array[0], quat.array[1], quat.array[2], quat.array[3]], True, True, True)
#ps.setNumericalConstraints ("eef", ["RightHandPos", "RightHandOr"])
ps.setNumericalConstraints ("eef", ["RightHandPos"])
ps.createPositionConstraint("LeftAnklePos", robot.leftAnkle, "", [0.0, 0.0, 0.0], [robot.getJointPosition(robot.leftAnkle)[0], robot.getJointPosition(robot.leftAnkle)[1], robot.getJointPosition(robot.leftAnkle)[2]], True, True, False)
ps.createOrientationConstraint("LeftAnkleOr", "", robot.leftAnkle, [robot.getJointPosition(robot.leftAnkle)[3], robot.getJointPosition(robot.leftAnkle)[4], robot.getJointPosition(robot.leftAnkle)[5], robot.getJointPosition(robot.leftAnkle)[6]], False, False, True)
ps.setNumericalConstraints ("lock", ["LeftAnklePos", "LeftAnkleOr"])
ps.lockOneDofJoint("LFinger11", robot.getJointDofValue("LFinger11"))
ps.lockOneDofJoint("LFinger12", robot.getJointDofValue("LFinger12"))
ps.lockOneDofJoint("LFinger13", robot.getJointDofValue("LFinger13"))
ps.lockOneDofJoint("LFinger21", robot.getJointDofValue("LFinger21"))
ps.lockOneDofJoint("LFinger22", robot.getJointDofValue("LFinger22"))
ps.lockOneDofJoint("LFinger23", robot.getJointDofValue("LFinger23"))
ps.lockOneDofJoint("LFinger31", robot.getJointDofValue("LFinger31"))
ps.lockOneDofJoint("LFinger32", robot.getJointDofValue("LFinger32"))
ps.lockOneDofJoint("LFinger33", robot.getJointDofValue("LFinger33"))
ps.lockOneDofJoint("LThumb1", robot.getJointDofValue("LThumb1"))
ps.lockOneDofJoint("LThumb2", robot.getJointDofValue("LThumb2"))
ps.lockOneDofJoint("LThumb3", robot.getJointDofValue("LThumb3"))
ps.lockOneDofJoint("RFinger11", robot.getJointDofValue("RFinger11"))
ps.lockOneDofJoint("RFinger12", robot.getJointDofValue("RFinger12"))
ps.lockOneDofJoint("RFinger13", robot.getJointDofValue("RFinger13"))
ps.lockOneDofJoint("RFinger21", robot.getJointDofValue("RFinger21"))
