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