def make(robot, file="mypath.path"): if robot == None: l = trajectory.Trajectory() else: l = trajectory.RobotTrajectory(robot) l.load(file) return TrajectoryController(l)
def go_to_random(self): """Moves to a random destination using the trajectory module and tracking the trajectory using PID commands""" c = self.sim.controller(0) robot = self.world.robot(0) q0 = c.getCommandedConfig() robot.randomizeConfig() q1 = robot.getConfig() times = [0,5.0] milestones = [q0,q1] self.trajectory = trajectory.path_to_trajectory(trajectory.RobotTrajectory(robot,times,milestones),velocities='trapezoidal') self.trajectoryStart = self.sim.getTime()
def make(robot, file="mypath.path", ff_torque_file=None): if robot == None: l = trajectory.Trajectory() else: l = trajectory.RobotTrajectory(robot) l.load(file) if ff_torque_file is not None: tcmd = trajectory.Trajectory() tcmd.load(ff_torque_file) return TrajectoryWithFeedforwardTorqueController(l, ff_torque_file) return TrajectoryPositionController(l)
def animation_template(world): """Shows how to animate a robot.""" #first, build a trajectory with 10 random configurations robot = world.robot(0) times = range(10) milestones = [] for t in times: robot.randomizeConfig() milestones.append(robot.getConfig()) traj = trajectory.RobotTrajectory(robot, times, milestones) vis.add("world", world) robotPath = ("world", world.robot(0).getName() ) #compound item reference: refers to robot 0 in the world #we're also going to visualize the end effector trajectory #eetraj = traj.getLinkTrajectory(robot.numLinks()-1,0.05) #vis.add("end effector trajectory",eetraj) #uncomment this to automatically visualize the end effector trajectory vis.add("robot trajectory", traj) vis.setAttribute("robot trajectory", "endeffectors", [13, 20]) vis.setWindowTitle("Animation test") MANUAL_ANIMATION = False if not MANUAL_ANIMATION: #automatic animation, just call vis.animate vis.animate(robotPath, traj) if not MULTITHREADED: #need to set up references to function-local variables manually, and the easiest way is to use a default argument def callback(robot=robot): if MANUAL_ANIMATION: #with manual animation, you just set the robot's configuration based on the current time. t = vis.animationTime() q = traj.eval(t, endBehavior='loop') robot.setConfig(q) pass vis.loop(callback=callback, setup=vis.show) else: vis.show() while vis.shown(): vis.lock() if MANUAL_ANIMATION: #with manual animation, you just set the robot's configuration based on the current time. t = vis.animationTime() q = traj.eval(t, endBehavior='loop') robot.setConfig(q) vis.unlock() time.sleep(0.01) #quit the visualization thread nicely vis.kill()
def debug_plan_results(plan, robot): """Potentially useful for debugging planning results...""" assert isinstance(plan, MotionPlan) #this code just gives some debugging information. it may get expensive V, E = plan.getRoadmap() print(len(V), "feasible milestones sampled,", len(E), "edges connected") print("Plan stats:") pstats = plan.getStats() for k in sorted(pstats.keys()): print(" ", k, ":", pstats[k]) print("CSpace stats:") sstats = plan.space.getStats() for k in sorted(sstats.keys()): print(" ", k, ":", sstats[k]) """ print(" Joint limit failures:") for i in range(robot.numLinks()): print(" ",robot.link(i).getName(),":",plan.space.ambientspace.joint_limit_failures[i]) """ path = plan.getPath() if path is None or len(path) == 0: print("Failed to plan path between configuration") #debug some sampled configurations numconfigs = min(10, len(V)) vis.debug("some milestones", V[2:numconfigs], world=world) pts = [] for i, q in enumerate(V): robot.setConfig(q) pt = robot.link(9).getTransform()[1] pts.append(pt) for i, q in enumerate(V): vis.add("pt" + str(i), pts[i], hide_label=True, color=(1, 1, 0, 0.75)) for (a, b) in E: vis.add("edge_{}_{}".format(a, b), trajectory.Trajectory(milestones=[pts[a], pts[b]]), color=(1, 0.5, 0, 0.5), width=1, pointSize=0, hide_label=True) return None print("Planned path with length", trajectory.RobotTrajectory(robot, milestones=path).length())
def planTriggered(): global world,robot qtgt = vis.getItemConfig(vis.getItemName(robot)) qstart = vis.getItemConfig("start") robot.setConfig(qstart) if PROBLEM == '1a': path = planning.feasible_plan(world,robot,qtgt) else: path = planning.optimizing_plan(world,robot,qtgt) if path is not None: ptraj = trajectory.RobotTrajectory(robot,milestones=path) ptraj.times = [t / len(ptraj.times) * 5.0 for t in ptraj.times] #this function should be used for creating a C1 path to send to a robot controller traj = trajectory.path_to_trajectory(ptraj,timing='robot',smoothing=None) #show the path in the visualizer, repeating for 60 seconds vis.animate("start",traj) vis.add("traj",traj,endeffectors=[9])
def visualize(world, robot, path=None, start=None, goal=None): """Visualize a path""" # visualize start/goal as spheres if exist r = 0.04 if start != None: sphere = create.primitives.sphere(r, start, world=world, name='start') # vis.add('start', sphere) if goal != None: create.primitives.sphere(r, goal, world=world, name='goal') vis.add("world", world) # animate path if exist if path != None: traj = trajectory.RobotTrajectory(robot, range(len(path)), path) vis.animate(("world", robot.getName()), path, speed=0.5) vis.add("trajectory", traj) vis.spin(float('inf'))
def setHermite(): smoothInput = trajectory.HermiteTrajectory() smoothInput.makeSpline(traj0) dtraj = smoothInput.discretize(0.1) dtraj = trajectory.RobotTrajectory(robot, dtraj.times, dtraj.milestones) traj = trajectory.path_to_trajectory( dtraj, velocities='constant', timing='limited', smoothing=None, stoptol=10.0, vmax=robot.getVelocityLimits(), amax=robot.getAccelerationLimits(), speed=1.0, ) print("*** Resulting duration", traj.endTime(), "***") #vis.animate("robot",ltraj) #vis.animate("robot",dtraj) vis.animate("robot", traj) vis.addText("label", "Hermite trajectory")
if len(sys.argv) == 1: world.readFile("../../data/athlete_plane.xml") q = world.robot(0).getConfig() q[2] = 2 world.robot(0).setConfig(q) sim = Simulator(world) share = HTMLSharePath(name="Klamp't simulation path") share.start(sim) for i in range(100): sim.simulate(0.02) share.animate() share.end() else: assert len(sys.argv) == 3,"Usage: sharepath.py world.xml robot_path" world.readFile(sys.argv[1]) traj = trajectory.RobotTrajectory(world.robot(0)) traj.load(sys.argv[2]) world.robot(0).setConfig(traj.milestones[0]) dt = 0.02 excess = 1.0 share = HTMLSharePath(name="Klamp't path "+os.path.split(sys.argv[2])[1]) share.start(world) share.dt = dt t = traj.times[0] while t < traj.times[-1] + excess: world.robot(0).setConfig(traj.eval(t)) share.animate(t) t += dt share.end()
def keyboardfunc(self, c, x, y): if c == 'h': print "Keyboard help:" print "- x: verifies edge checks for existing resolution" print "- i: toggles between interpolation mode and graph inspection mode" print "- g: toggles drawing the workspace graph" print "- w: performs a walk to a random workspace node" print "- m: saves a real-time movie" print "- M: saves a 360 spin movie" elif c == 'x': self.resolution.verify() self.disconnectionDisplayList.markChanged() elif c == 'i': if self.mode == 'interpolate': self.mode = 'inspect' else: self.mode = 'interpolate' print "Toggled visualization mode to", self.mode elif c == 'g': self.drawWorkspaceRoadmap = not self.drawWorkspaceRoadmap elif c == 'b': if self.useboundary: self.useboundary = False else: self.useboundary = True self.disconnectionDisplayList.markChanged() elif c == 'm': if self.movie_frame is None: self.movie_frame = 0 self.movie_rotate = False else: self.movie_frame = None elif c == 'M': if self.movie_frame is None: self.movie_frame = 0 self.movie_rotate = True else: self.movie_frame = None elif c == 'w': import random resolved = [] for iw, d in self.resolution.Gw.nodes(data=True): if d.get('config', None) is not None: resolved.append(iw) if self.walk_workspace_path == None: #draw boundaries transparent now self.disconnectionDisplayList.markChanged() for iters in range(10): wtgt = random.choice(resolved) #TEMP: place widgets far away for capturing video far = [20] * 3 self.pointWidget.set(far) R, t = self.xformWidget.get() self.xformWidget.set(R, far[:3]) #get current config if self.configs != None: self.robot.setConfig(self.configs[0]) try: x, p = self.resolution.walkPath(wtgt) except Exception as e: import traceback print "Exception", e traceback.print_exc() print "WalkPath failed, trying with a new random point" continue self.walk_workspace_path = None if p != None: t = 0 times = [] for i, q in enumerate(p): times.append(t) if i + 1 < len(p): t += linf_distance(q, p[i + 1], self.resolution.spinJoints) #t += self.robot.distance(q,p[i+1]) #t += 0.1 self.walk_workspace_path = trajectory.Trajectory(times, x) self.walk_path = trajectory.RobotTrajectory( self.robot, times, p) self.walk_progress = 0 if self.temp_config == None: self.temp_config = p[0] break self.refresh()
print(plan.space.cspace.feasibilityQueryOrder()) print("Plan stats:") print(plan.getStats()) print("CSpace stats:") print(plan.space.getStats()) #to be nice to the C++ module, do this to free up memory plan.space.close() plan.close() if len(wholepath) > 1: #print "Path:" #for q in wholepath: # print " ",q #if you want to save the path to disk, uncomment the following line #wholepath.save("test.path") #draw the path as a RobotTrajectory (you could just animate wholepath, but for robots with non-standard joints #the results will often look odd). Animate with 5-second duration times = [i * 5.0 / (len(wholepath) - 1) for i in range(len(wholepath))] traj = trajectory.RobotTrajectory(robot, times=times, milestones=wholepath) #show the path in the visualizer, repeating for 60 seconds vis.animate("robot", traj) vis.spin(60) else: print("Failed to generate a plan") vis.kill()
world=world, doedit=False) if len(configs) < 2: print "Invalid # of milestones, need 2 or more" configs = resource.get("pathtest.configs", "Configs", default=[], world=world, doedit=True) if len(configs) < 2: print "Didn't add 2 or more milestones, quitting" exit(-1) resource.set("pathtest.configs", configs) traj0 = trajectory.RobotTrajectory(robot, times=range(len(configs)), milestones=configs) #show the path in the visualizer, repeating for 60 seconds traj = trajectory.path_to_trajectory(traj0, speed=1.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "Default values") vis.addPlot("plot") vis.addPlotItem("plot", "robot") #action callbacks def setHalfSpeed(): traj = trajectory.path_to_trajectory(traj0, speed=0.5) print "*** Resulting duration", traj.endTime(), "***"
vis.setAttribute("xform_milestones","width",1.0) vis.setAttribute("xform spline","width",1.0) vis.setAttribute("xform bezier","width",1.0) else: #creates a world and loads all the items on the command line world = WorldModel() for fn in sys.argv[1:]: res = world.readFile(fn) if not res: raise RuntimeError("Unable to load model "+fn) #add the world to the visualizer robot = world.robot(0) vis.add("robot",robot) traj = trajectory.RobotTrajectory(robot) 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)
vis.show() while vis.shown(): coordinates.updateFromWorld() time.sleep(0.1) vis.popPlugin() vis.hide("ghost1") vis.hide("ghost2") vis.animate(("world",world.robot(0).getName()),None) print print #this tests the "bump" function stuff print "***** BEGINNING BUMP FUNCTION TEST *****" configs = resource.get("cartesian_test"+world.robot(0).getName()+".configs",description="Make a reference trajectory for bump test",world=world) if configs is None: print "To test the bump function, you need to create a reference trajectory" vis.kill() exit(0) print "Found trajectory with",len(configs),"configurations" traj = trajectory.RobotTrajectory(robot,range(len(configs)),configs) vis.setWindowTitle("Klamp't Trajectory bump test") vis.pushPlugin(BumpKeyCapture(endeffectors,eeobjectives,traj)) vis.show() while vis.shown(): coordinates.updateFromWorld() time.sleep(0.1) vis.popPlugin() print "Ending vis." vis.kill()