while isRunning:
# move to nominal joint positions
if isMoving:
baxterHand.open()
configStart = array(arm.joint_values)
configEnd = array(configNominal)
goalDist = norm(configStart - configEnd)
if goalDist > 2 * (pi / 180):
trajLen = int((1.0 / maxCSpaceJump) * goalDist) + 1
traj, cost, inCollision = planner.planSimpleTrajectory(\
configStart, configEnd, arm, endEffector, trajLen)
planner.drawTrajectory(traj, arm, endEffector, [0, 0, 0.5])
arm.followTrajectory(traj,
startSpeed=2.0,
endSpeed=0.5,
startThresh=0.15,
endThresh=0.07)
arm.update()
# Target: ========================================================================================
# request laser point
#userInput('Select your target with the laser and hit Enter or click the trigger: ')
raw_input("Select the item with the laser and hit Enter...")
centroid = node.detectStableLaserPoint(laserPointWorkspace,
laserPointQueueSize,
maxLaserPointSigma)
sphereMarker = plot.createSphereWithCenterMarker("laserPoint", [1, 0, 1],
centroid,
