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)
#time.sleep(0.01)
#animationTime = visualization.animationTime()
iteration += 1
if len(sys.argv) > 1:
visualization.animate("robot", None)
sim = Simulator(world)
sim.simulate(0)
trajectory.execute_path(traj.milestones, sim.controller(0))
#for some tricky Qt reason, need to sleep before showing a window again
#Perhaps the event loop must complete some extra cycles?
time.sleep(0.01)
visualization.show()
t0 = time.time()
while visualization.shown():
#print "Time",sim.getTime()
sim.simulate(0.01)
if sim.controller(0).remainingTime() <= 0:
print "Executing timed trajectory"
trajectory.execute_trajectory(traj,
sim.controller(0),
smoothing='pause')
visualization.setItemConfig("config",
sim.controller(0).getCommandedConfig())
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.0))
t0 = t1
print "Ending visualization."
visualization.kill()
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.")