ps = ProblemSolver (robot)
# create static stability constraint
wcl = WholebodyStepClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q, robot.leftAnkle,
robot.rightAnkle)
# Define quatenion for righthand orientation constraints
#quat = Quaternion()
#quat.fromRPY(-1.55, -1.55, 0.0) # Right hand horizontal, grasping to -z
#quat.fromRPY(-1.55, 0.0, 0.0) # Right hand vertical, pointing down
#quat.fromRPY(0.0, 0.0, -1.55) # Right hand vertical, pointing forward
#quat.normalize()
ps.resetConstraints()
# 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"])
