q0 = robot.getConfig() for i in range(robot.numLinks()): if math.isinf(qmin[i]) or math.isinf(qmax[i]): #don't animate floating base continue traj.times.append(len(traj.times) * 0.5) q = q0[:] q[i] = qmin[i] traj.milestones.append(q) traj.times.append(len(traj.times) * 0.5) q[i] = qmax[i] traj.milestones.append(q) save, traj.milestones = resource.edit("trajectory", traj.milestones, world=world) visualization.animate("robot", traj) visualization.show() iteration = 0 while visualization.shown(): #visualization.lock() #can modify the visualization here #visualization.unlock() #time.sleep(0.01) #animationTime = visualization.animationTime() iteration += 1 if len(sys.argv) > 1:
obj = ik.objective(robot.link(robot.numLinks() - 1), local=[0, 0, 0], world=[0.5, 0, 0.5]) visualization.add("IK goal", obj) visualization.dialog() space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider) space.eps = 1e-2 space.setup() #Generate some waypoint configurations using the resource editor configs = [] while True: (save, q) = resource.edit("plan config " + str(len(configs) + 1), robot.getConfig(), "Config", world=world, description="Press OK to add waypoint, cancel to stop") if save: if False and not space.feasible(q): print "Configuration is infeasible. Failures:" print " ", space.cspace.feasibilityFailures(q) print "Please pick another" else: configs.append(q) robot.setConfig(q) else: break if len(configs) == 0: exit(0)
space.eps = 1e-2 space.setup() else: #TESTING: closed loop robot cspace collider = WorldCollider(world) obj = ik.objective(robot.link(robot.numLinks()-1),local=[0,0,0],world=[0.5,0,0.5]) visualization.add("IK goal",obj) visualization.dialog() space = robotcspace.ClosedLoopRobotCSpace(robot,obj,collider) space.eps = 1e-2 space.setup() #Generate some waypoint configurations using the resource editor configs = [] while True: (save,q) = resource.edit("plan config "+str(len(configs)+1),robot.getConfig(),"Config",world=world,description="Press OK to add waypoint, cancel to stop") if save: if False and not space.feasible(q): print "Configuration is infeasible. Failures:" print " ",space.cspace.feasibilityFailures(q) print "Please pick another" else: configs.append(q) robot.setConfig(q) else: break if len(configs)==0: exit(0) if CLOSED_LOOP_TEST: #need to project those onto the manifold
def show(world,desc=""): resource.edit(None,value=world.robot(0).getConfig(),type='Config',description=desc,world=world)
qmin,qmax = robot.getJointLimits() q0 = robot.getConfig() for i in range(robot.numLinks()): if math.isinf(qmin[i]) or math.isinf(qmax[i]): #don't animate floating base continue traj.times.append(len(traj.times)*0.5) q = q0[:] q[i] = qmin[i] traj.milestones.append(q) traj.times.append(len(traj.times)*0.5) q[i] = qmax[i] traj.milestones.append(q) save,traj.milestones = resource.edit("trajectory",traj.milestones,world=world) visualization.animate("robot",traj) visualization.show() iteration = 0 while visualization.shown(): #visualization.lock() #can modify the visualization here #visualization.unlock() #time.sleep(0.01) #animationTime = visualization.animationTime() iteration += 1 if len(sys.argv)>1: visualization.animate("robot",None)