def show(problem):
global robot, blocks
from klampt import visualization
visualization.clear()
robot.setConfig(problem.start)
visualization.add("robot", robot)
visualization.add("target", [problem.goal[0], 0.0, problem.goal[1]])
for i in xrange(W):
for j in xrange(H):
if problem.env[i, j]:
visualization.add("block %d,%d" % (i, j), blocks[i, j])
if problem.solution != None:
visualization.animate(
"robot",
RobotTrajectory(robot,
times=range(len(problem.solution)),
milestones=problem.solution))
visualization.dialog()
#add a point to the visualizer and animate it
point = coordinates.addPoint("point")
visualization.add("point", point)
traj = trajectory.Trajectory()
for i in range(10):
traj.times.append(i)
traj.milestones.append([
random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)
])
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
visualization.animate("point", traj2)
#add a transform to the visualizer and animate it
xform = visualization.add("xform", se3.identity())
traj3 = trajectory.SE3Trajectory()
for i in range(10):
traj3.times.append(i)
rrot = random_rotation()
rpoint = [
random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)
]
traj3.milestones.append(rrot + rpoint)
visualization.animate("xform", traj3)
space = robotcspace.RobotSubsetCSpace(world.robot(0),subset,collider)
planner = cspace.MotionPlan(space, "rrt*")
#extract out cspace configurations
start = [q0[i] for i in subset]
goal = [q1[i] for i in subset]
print "Goal config",goal
planner.setEndpoints(start,goal)
for iters in xrange(10000):
planner.planMore(1)
if planner.getPath() != None:
print "Planning succeeded"
cspacepath = planner.getPath()
#convert back to robot joint space
path = []
for qcspace in cspacepath:
print qcspace
q = q0[:]
for i,v in zip(subset,qcspace):
q[i] = v
path.append(q)
break
visualization.animate("world",path)
visualization.dialog()
visualization.kill()
#problems with code
# only works for left arm currently
# seems to have difficulty solving for locations inside the bin
# seems to discount additional pincer mesh (only pays attention to initial robot mesh)
#debug some sampled configurations
print V[0:max(10, len(V))]
break
if CLOSED_LOOP_TEST:
#the path is currently a set of milestones: discretize it so that it stays near the contact surface
path = space.discretizePath(path)
wholepath += path[1:]
#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
visualization.animate("robot", traj)
visualization.spin(60)
else:
print "Failed to generate a plan"
visualization.kill()
#debug some sampled configurations
print V[0:max(10,len(V))]
break
if CLOSED_LOOP_TEST:
#the path is currently a set of milestones: discretize it so that it stays near the contact surface
path = space.discretizePath(path)
wholepath += path[1:]
#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
visualization.animate("robot",traj)
visualization.spin(60)
else:
print "Failed to generate a plan"
visualization.kill()
print "trajectorytest.py: This example demonstrates several types of trajectory"
if len(sys.argv)<=1:
print "USAGE: trajectorytest.py [robot or world file]"
print "With no arguments, shows some 3D trajectories"
#add a point to the visualizer and animate it
point = coordinates.addPoint("point")
visualization.add("point",point)
traj = trajectory.Trajectory()
for i in range(10):
traj.times.append(i)
traj.milestones.append([random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)])
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
visualization.animate("point",traj2)
#add a transform to the visualizer and animate it
xform = visualization.add("xform",se3.identity())
traj3 = trajectory.SE3Trajectory()
for i in range(10):
traj3.times.append(i)
rrot = random_rotation()
rpoint = [random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)]
traj3.milestones.append(rrot+rpoint)
visualization.animate("xform",traj3)
else:
#creates a world and loads all the items on the command line
world = WorldModel()
for fn in sys.argv[1:]:
def main():
#add the world elements individually to the visualization
visualization.add("robot", ROBOT)
print "Added robot to visualization"
for i in range(1, WORLD.numRobots()):
visualization.add("robot" + str(i), WORLD.robot(i))
print "Added robot ", str(i)
for i in range(WORLD.numRigidObjects()):
visualization.add("rigidObject" + str(i), WORLD.rigidObject(i))
print "Added rigidObject ", str(i)
for i in range(WORLD.numTerrains()):
visualization.add("terrain" + str(i), WORLD.terrain(i))
print "Added terrain ", str(i)
# SETUP CONFIGS
print "-------------------- Path Configs --------------------"
print 'Parsing', JSON_PATHNAME, 'path from JSON'
CONFIGS = parseJson(JSON_PATHNAME)
print 'CONFIGS:'
for i in range(len(CONFIGS)):
print ' ', i, ':', CONFIGS[i]
# MOTION PLANNER
planTypeDict = {}
planTypeDict["sbl"] = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000"
}
print "-------------------- Motion Planning --------------------"
settings = planTypeDict[PLANNER_TYPE]
print "Planner type", PLANNER_TYPE
print "Planner settings", settings
wholepath = [CONFIGS[0]]
for i in range(len(CONFIGS) - 1):
path = myPlanner(CONFIGS[i], CONFIGS[i + 1], settings)
if path is None or len(path) == 0:
print 'Failed to find path between configation %d and %d' % (i,
i + 1)
exit(0)
wholepath += path[1:]
# CONTROLLER
if len(wholepath) > 1:
print 'Path:', len(wholepath)
# for q in wholepath:
# print ' ', q
for q in wholepath:
#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)
# print traj.interpolate(wholepath[0], wholepath[1], 10, 1)
#show the path in the visualizer, repeating for 60 seconds
visualization.animate("robot", traj)
visualization.spin(60)
visualization.kill()
print("Done.")
