# 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"])
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"))
q1 [robot.rankInConfiguration ['l_gripper_l_finger_joint']] = 0.12
q1 [robot.rankInConfiguration ['r_gripper_r_finger_joint']] = 0.13
q1 [robot.rankInConfiguration ['r_gripper_l_finger_joint']] = 0.095
r(q1)
q2 = q1 [::]
q2 [0:2] = [-1.5, -4] # x, y
#q2 [2:4] = [-0.707, 0.707] # theta
q2 [2:4] = [-1, 0] # theta
q2 [robot.rankInConfiguration ['torso_lift_joint']] = 0.02
r (q2)
## CONSTRAINTS ##
# Relative position constraint between PR2's right hand and the set's right handle.
ps.createPositionConstraint ("posConstraint1", "r_gripper_r_finger_joint", "j_marker_set", [0,0,0], [0.8455, -0.089, 0.01], [1,1,1])
#ps.createPositionConstraint ("posConstraint2", "r_gripper_l_finger_joint", "j_marker_set", [0,0,0], [0.841, 0.001, -0.0248], [1,1,1]) #WRONG!
# Glogal orientation constraint of the set that has to stay horizontal.
ps.createOrientationConstraint ("orConstraint", "j_marker_set", "", [0.707106781,0,0,-0.707106781], [1,1,0])
ps.setNumericalConstraints ("constraints", ["posConstraint1","orConstraint"])
res = ps.applyConstraints (q1)
if res [0]:
q1proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
if not(robot.isConfigValid(q1proj)):
ps.setInitialConfig (q_init)
# ps.addGoalConfig (q_goal)
# ps.addPathOptimizer ("RandomShortcut")
# ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, r)
# pp (0)
# pp (1)
# test
jp = [-4.016784835909555, -4.671737012514295, 0.9493205942691932, -0.19838422293030356, 0.17672477884262752, -0.35476407311266434, 0.8964120174695772]
ps.createPositionConstraint ("l_gripper", "l_gripper_tool_joint", "", [0,0,0], jp[:3], [1,1,1])
ps.client.problem.setGoalNumericalConstraints ("test",["l_gripper",], [0,])
# qq = [-3.2736323380281704, -4.919012557067956, -0.5645503720485425, 0.8253986172873397, 0.31, 2.4756472106318212, -0.10231121294734401, 0.5832544388135313, 0.9877368674476137, 0.351352194158052, -1.1240948972856866, 0.9832501749355624, -0.1822610586197337, -0.8650504112999698, 0.6602954608097523, -0.7510059283614456, 0.28134369878719734, 0.5218218893957427, 0.5020748972660279, 0.34837002692202573, 0.39307871732911037, 0.012744229982022303, 0.33261894418886817, -0.7484151440244099, -1.5567378515100605, -0.2601290957285226, -0.1203692551331449, -1.9576008301144934, 0.8124905337042794, -0.5829743842064892, -1.8222188044396315, -0.7752624281039475, -0.63163927013001, 0.5474106358659504, 0.11960478411037699, 0.2810869521336104, 0.45983350461154876, 0.3357266282624224, 0.026642845592760873, 0.34937864270125457]
# ps.addGoalConfig (qq)
time = list()
nbNode = list()
nbIter = 1
for i in range(nbIter):
ps.clearRoadmap ()
time.append (ps.solve ())
nbNode.append(len(ps.nodes()))
print i, time[-1], nbNode[-1]
i = -1
