robot = Robot('hrp2_14') robot.setJointBounds("root_joint", [-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1]) cl = robot.client q0 = robot.getInitialConfig() # Add constraints ps = ProblemSolver(robot) ps.addPartialCom('hrp2_14', ['root_joint']) robot.createSlidingStabilityConstraint("balance/", 'hrp2_14', robot.leftAnkle, robot.rightAnkle, q0) ps.setNumericalConstraints("balance", [ "balance/relative-com", "balance/relative-pose", "balance/pose-left-foot", ]) # lock hands in closed position lockedjointDict = robot.leftHandClosed() lockedJoints = list() for name, value in lockedjointDict.iteritems(): ljName = "locked_" + name ps.createLockedJoint(ljName, name, value) lockedJoints.append(ljName) lockedjointDict = robot.rightHandClosed() for name, value in lockedjointDict.iteritems(): ljName = "locked_" + name ps.createLockedJoint(ljName, name, value)
robot.setTranslationBounds(-3, 3, -3, 3, 0, 1) cl = robot.client r = ScenePublisher(robot) q0 = robot.getInitialConfig() r(q0) # Add constraints wcl = WsClient() wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle, robot.rightAnkle) ps = ProblemSolver(robot) ps.setNumericalConstraints("balance", [ "balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot" ]) # lock hands in closed position lockedDofs = robot.leftHandClosed() for name, value in lockedDofs.iteritems(): ps.lockDof(name, value, 0, 0) lockedDofs = robot.rightHandClosed() for name, value in lockedDofs.iteritems(): ps.lockDof(name, value, 0, 0) q1 = [ 0.0, 0.0, 0.705, 1.0, 0., 0., 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532,
q_goal [0:2] = [1.05,0] # 3}}} # Generate constraints {{{3 constraintName = "balance" wcl.problem.addStaticStabilityConstraints (constraintName, q_init, robot.leftAnkle, robot.rightAnkle, "", Problem.SLIDING) balanceConstraints = [constraintName + "/relative-com", constraintName + "/relative-orientation", constraintName + "/relative-position", constraintName + "/orientation-left-foot", constraintName + "/position-left-foot"] # 3}}} ps.setNumericalConstraints ("balance", balanceConstraints); ps.setInitialConfig (q_init) r = Viewer (ps) r(q_init) ### Walk forward if walk_forward: ps.addGoalConfig (q_goal) ps.solve () print "Solved forward" ### Walk sideway q_goal_side = q_init[:] q_goal_side [0:2] = [0, 0.95] if walk_sideway:
robot = Robot ('hrp2_14') robot.setJointBounds ("base_joint_xyz", (-3, 3, -3, 3, 0, 1)) cl = robot.client q0 = robot.getInitialConfig () # Add constraints wcl = WsClient () wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle, robot.rightAnkle, "", Problem.SLIDING) ps = ProblemSolver (robot) ps.setNumericalConstraints ("balance", ["balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot"]) # lock hands in closed position lockedjoints = robot.leftHandClosed () for name, value in lockedjoints.iteritems (): ps.lockJoint (name, value) lockedjoints = robot.rightHandClosed () for name, value in lockedjoints.iteritems (): ps.lockJoint (name, value) q1 = [0.0, 0.0, 0.705, 0., 0., 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
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]: q2proj = res [1] else:
# 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 num_log = 32234
import time robot = Robot('squel_robot') robot.setJointBounds('base_joint_xyz', [-3, 3, -3, 3, 0, 1]) ps = ProblemSolver(robot) cl = robot.client Viewer.withFloor = True r = Viewer(ps) #r.loadObstacleModel ("room_description","room","room") #r.loadObstacleModel ("room_description","walls","walls") #pp = PathPlayer (cl, r) #r.loadObstacleModel ("room_description","squel","squel") # Add constraints """ wcl = WsClient () wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle, robot.rightAnkle) ps.setNumericalConstraints ("balance", ["balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot"]) """ q1 = [ 0.0, 0.0, 1.2, 0, 0, 1.57, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0.0, 0.0, 0, 0, 0, -0.174532, 0, 0, 0, 0, 0.0, 0, 0.0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0, 0, 0, 0.0, 0.0, 0.0, 0, 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
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"]) ps.lockOneDofJoint("LFinger11", robot.getJointDofValue("LFinger11")) ps.lockOneDofJoint("LFinger12", robot.getJointDofValue("LFinger12")) ps.lockOneDofJoint("LFinger13", robot.getJointDofValue("LFinger13")) ps.lockOneDofJoint("LFinger21", robot.getJointDofValue("LFinger21"))
import time robot = Robot ('squel_robot') robot.setJointBounds ('base_joint_xyz', [-3, 3, -3, 3, 0, 1]) ps = ProblemSolver (robot) cl = robot.client Viewer.withFloor = True r = Viewer (ps) #r.loadObstacleModel ("room_description","room","room") #r.loadObstacleModel ("room_description","walls","walls") #pp = PathPlayer (cl, r) #r.loadObstacleModel ("room_description","squel","squel") # Add constraints """ wcl = WsClient () wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle, robot.rightAnkle) ps.setNumericalConstraints ("balance", ["balance/relative-com", "balance/relative-orientation", "balance/relative-position", "balance/orientation-left-foot", "balance/position-left-foot"]) """ q1 = [0.0, 0.0, 1.2, 0, 0, 1.57, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0.0, 0.0, 0, 0, 0, -0.174532, 0, 0, 0, 0, 0.0, 0, 0.0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0, 0, 0, 0.0, 0.0, 0.0, 0, 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] r(q1) """