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, 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 ] res = ps.applyConstraints(q1) if res[0]: q1proj = res[1] else: raise RuntimeError("Failed to apply constraint.") q2 = [ 0.0, 0.0, 0.705, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -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 ] res = ps.applyConstraints(q2) if res[0]:
ps.lockOneDofJoint("NeckYaw", robot.getJointDofValue("NeckYaw")) ps.lockOneDofJoint("NeckPitch", robot.getJointDofValue("NeckPitch")) ps.lockOneDofJoint("HeadPitch", robot.getJointDofValue("HeadPitch")) ps.lockOneDofJoint("HeadRoll", robot.getJointDofValue("HeadRoll")) ps.lockOneDofJoint("LToePitch", robot.getJointDofValue("LToePitch")) ps.lockOneDofJoint("RToePitch", robot.getJointDofValue("RToePitch")) ps.lockOneDofJoint("LEyeYaw", robot.getJointDofValue("LEyeYaw")) ps.lockOneDofJoint("LEyePitch", robot.getJointDofValue("LEyePitch")) ps.lockOneDofJoint("REyeYaw", robot.getJointDofValue("REyeYaw")) ps.lockOneDofJoint("REyePitch", robot.getJointDofValue("REyePitch")) q_init = robot.getCurrentConfig() #q_start = robot.getCurrentConfig() # Get one config wrt constraints res, q, error, time = ps.applyConstraints (q_init) full_time = time while res == False: full_time += time res, q, error, time = ps.applyConstraints (q) #robot.shootRandomConfig() #robot.setCurrentConfig(q) #q = robot.getRightHandOpenConfig() full_time r(q) ################################################### # Test de timing pour trouver une solution IK
"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] res = ps.applyConstraints (q1) if res [0]: q1proj = res [1] else: raise RuntimeError ("Failed to apply constraint.") q2 = [0.0, 0.0, 0.705, 0, 0, 0, 1, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -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] res = ps.applyConstraints (q2) if res [0]: q2proj = res [1] else: raise RuntimeError ("Failed to apply constraint.") ps.selectPathValidation ("Progressive", 0.025)
q_goal = robot.halfSitting [::] for j, v in dict_goal.iteritems (): i = robot.rankInConfiguration [j] q_goal [i] = v for j, v in robot.openHand (None, 0.2, 'left').iteritems (): ps.lockJoint (j, [v,]) for j, v in robot.openHand (None, 0.2, 'right').iteritems (): ps.lockJoint (j, [v,]) ps.lockJoint ('base_joint_xy', [0,0]) ps.lockJoint ('base_joint_rz', [1,0]) res = ps.applyConstraints (q_init) if res [0]: q_init = res [1] else: raise RuntimeError ('Failed to apply constraints on init config.') res = ps.applyConstraints (q_goal) if res [0]: q_goal = res [1] else: raise RuntimeError ('Failed to apply constraints on goal config.') robot.setCurrentConfig (q_init) r = vf.createViewer (collisionURDF = True) r (q_init)