def setParabolic(): traj = trajectory.path_to_trajectory(traj0, velocities='parabolic', timing='Linf', speed=1.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "Parabolic velocity profile")
def setStartStop(): traj = trajectory.path_to_trajectory(traj0, velocities='trapezoidal', timing='Linf', speed=1.0, zerotol=0.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "Start/stop trapezoidal velocity profile")
def setMinJerk(): traj = trajectory.path_to_trajectory(traj0, velocities='minimum-jerk', timing='Linf', speed=1.0, stoptol=0.0) print("*** Resulting duration", traj.endTime(), "***") vis.animate("robot", traj) vis.addText("label", "Start/stop minimum-jerk velocity profile")
def setCosine(): traj = trajectory.path_to_trajectory(traj0, velocities='cosine', timing='Linf', speed=1.0, stoptol=0.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "Start/stop cosine velocity profile")
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 executePlan(): global solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time if solved_trajectory is None: return executing_plan = True if PHYSICS_SIMULATION: execute_start_time = 0 solved_trajectory = path_to_trajectory(solved_trajectory, timing='robot', smoothing=None) solved_trajectory.times = [10 * t for t in solved_trajectory.times] else: execute_start_time = time.time()
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 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")
def setDoubleSpeed(): traj = trajectory.path_to_trajectory(traj0, speed=2.0) print "*** Resulting duration", traj.endTime(), "***" vis.animate("robot", traj) vis.addText("label", "2x speed")
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.animate("robot", traj) vis.addText("label", "0.5x speed")
def setHalfSpeed(): traj = trajectory.path_to_trajectory(traj0, speed=0.5) print("*** Resulting duration", traj.endTime(), "***") vis.animate("robot", traj) vis.addText("label", "0.5x speed")
print 6,":",traj.eval(6) #print some interpolated points print 0.5,":",traj.eval(0.5) print 2.5,":",traj.eval(2.5) #print some stuff after the end of trajectory print 7,":",traj.eval(6) print 100.3,":",traj.eval(100.3) print -2,":",traj.eval(-2) from klampt import vis vis.add("point",[0,0,0]) vis.animate("point",traj) vis.add("traj",traj) #vis.spin(float('inf')) #show the window until you close it traj2 = trajectory.HermiteTrajectory() traj2.makeSpline(traj) vis.animate("point",traj2) vis.hide("traj") vis.add("traj2",traj2.configTrajectory().discretize(0.1)) #vis.spin(float('inf')) traj_timed = trajectory.path_to_trajectory(traj,vmax=2,amax=4) #next, try this line instead #traj_timed = trajectory.path_to_trajectory(traj,timing='sqrt-L2',speed='limited',vmax=2,amax=4) #or this line #traj_timed = trajectory.path_to_trajectory(traj2.configTrajectory().discretize(0.1),timing='sqrt-L2',speed=0.3) vis.animate("point",traj_timed) vis.spin(float('inf'))