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.")