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)):
raise RuntimeError ("Projected config non valid.")
res = ps.applyConstraints (q2)
if res [0]:
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"))
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"))
r = Viewer (ps)
pp = PathPlayer (cl, r)
# q = [x, y, z, theta] # (z not considered since planar)
q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1
#q1 = [2.4, -4.6, 1.0, 0.0]; q2 = [-0.4, 4.6, 1.0, 0.0] # obstS 2
ps.setInitialConfig (q1)
ps.addGoalConfig (q2)
# Load box obstacle in HPP for collision avoidance #
#cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')
cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','')
r.loadObstacleModel ("potential_description","obstacles_concaves","obstacles_concaves") # in viewer !
ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [0.7071067812
,0,0,0.7071067812], [0,0,1]) # OK
T = [[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,1]]
ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.solve ()
begin=time.time()
cl.problem.optimizePath(0)
end=time.time()
print "Solving time: "+str(end-begin)
len(cl.problem.nodes ())
cl.problem.pathLength(0)
cl.problem.pathLength(1)
## Debug Optimization Tools ##############
import matplotlib.pyplot as plt
sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
kRange = 5
robot = Robot ('potential')
robot.setJointBounds('base_joint_xy', [-kRange, kRange, -kRange, kRange])
ps = ProblemSolver (robot)
cl = robot.client
# q = [x, y, theta] # (z not considered since planar)
q1 = [-2, 0, 1, 0]; q2 = [2, 0, 1, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')
ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1]) # OK
ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.solve ()
ps.configAtParam(0,2)
ps.configAtParam(0,6)
ps.configAtParam(0,13)
ps.getWaypoints (0)[1]
ps.optimizePath(0)
ps.pathLength(0)
ps.pathLength(1)
ps.configAtParam(1,2)
ps.configAtParam(1,6)
