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)