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'))
