description="Press OK to add waypoint, cancel to stop") if save: if False and not space.feasible(q): print("Configuration is infeasible. Failures:") print(" ", space.cspace.feasibilityFailures(q)) print("Please pick another") else: if cindex >= len(configs): configs.append(q) else: configs[cindex] = q cindex += 1 else: break if cindex == 0: vis.kill() exit(0) configs = configs[:cindex] resource.set("planningtest.configs", configs) if CLOSED_LOOP_TEST: #need to project those onto the manifold for i, q in enumerate(configs): configs[i] = space.solveConstraints(q) resource.edit( "IK solved configs", configs, "Configs", description="These configurations try to solve the IK constraint", world=world)
def main(): print( "================================================================================" ) print(sys.argv[0] + ": Simulates a robot file and Python controller") if len(sys.argv) <= 1: print( "USAGE: klampt_sim [world_file] [trajectory (.traj) or controller (.py)]" ) print( "================================================================================" ) if len(sys.argv) <= 1: exit() world = WorldModel() #load up any world items, control modules, or paths control_modules = [] for fn in sys.argv[1:]: path, base = os.path.split(fn) mod_name, file_ext = os.path.splitext(base) if file_ext == '.py' or file_ext == '.pyc': sys.path.append(os.path.abspath(path)) mod = importlib.import_module(mod_name, base) control_modules.append(mod) elif file_ext == '.path': control_modules.append(fn) else: res = world.readFile(fn) if not res: print("Unable to load model " + fn) print("Quitting...") sys.exit(1) viewer = MyGLViewer(world) for i, c in enumerate(control_modules): if isinstance(c, str): sys.path.append(os.path.abspath("../control")) import trajectory_controller #it's a path file, try to load it controller = trajectory_controller.make(world.robot(i), c) else: try: maker = c.make except AttributeError: print("Module", c.__name__, "must have a make() method") print("Quitting...") sys.exit(1) controller = maker(world.robot(i)) viewer.sim.setController(world.robot(i), controller) vis.setWindowTitle("Klamp't Simulation Tester") if SPLIT_SCREEN_TEST: viewer2 = MyGLViewer(world) vis.setPlugin(viewer) vis.addPlugin(viewer2) viewer2.window.broadcast = True vis.show() while vis.shown(): time.sleep(0.01) vis.kill() else: vis.run(viewer)
def test_grasp(robotname, object_set, objectname, xyzrpy): world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname, world) m_object = make_object(object_set, objectname, world) #this sets the initial condition for the simulation xform = xyzrpy_to_xform(xyzrpy) set_moving_base_xform(robot, xform[0], xform[1]) #now the simulation is launched program = GLSimulationPlugin(world) sim = program.sim #setup some simulation parameters visPreshrink = True #turn this to true if you want to see the "shrunken" models used for collision detection for l in range(robot.numLinks()): sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink) for l in range(world.numRigidObjects()): sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink) #create a hand emulator from the given robot name module = importlib.import_module('plugins.' + robotname) #emulator takes the robot index (0), start link index (6), and start driver index (6) hand = module.HandEmulator(sim, 0, 6, 6) sim.addEmulator(0, hand) #the result of simple_controller.make() is now attached to control the robot import simple_controller sim.setController(robot, simple_controller.make(sim, hand, delta)) #the next line latches the current configuration in the PID controller... sim.controller(0).setPIDCommand(robot.getConfig(), robot.getVelocity()) #this code manually updates the visualization vis.add("world", world) vis.show() t0 = time.time() count = 0 initial_z = m_object.getTransform()[1][2] while vis.shown(): vis.lock() sim.simulate(0.01) sim.updateWorld() vis.unlock() t1 = time.time() time.sleep(max(0.01 - (t1 - t0), 0.001)) t0 = t1 count += 1 if count == 800: break if m_object.getTransform()[1][2] > initial_z + 0.05: print "grasp successful" vis.kill() return True print "grasp failed" vis.kill() return False
def simulation_template(world): """Runs a custom simulation plugin""" viewer = MyGLSimulationViewer(world) vis.run(viewer) vis.kill()
def main(): print(""" =============================================================================== A program to quickly browse Klamp't objects. USAGE: %s [item1 item2 ...] where the given items are world, robot, terrain, object, or geometry files. Run it without arguments %s for an empty reference world. You may add items to the reference world using the `Add to World` button. If you know what items to use in the reference world, run it with %s world.xml or %s item1 item2 ... where the items are world, robot, terrain, object, or geometry files. =============================================================================== """ % (sys.argv[0], sys.argv[0], sys.argv[0], sys.argv[0])) #must be explicitly deleted for some reason in PyQt5... g_browser = None def makefunc(gl_backend): global g_browser browser = ResourceBrowser(gl_backend) g_browser = browser dw = QtWidgets.QDesktopWidget() x = dw.width() * 0.8 y = dw.height() * 0.8 browser.setFixedSize(x, y) for fn in sys.argv[1:]: res = browser.world.readFile(fn) if not res: print("Unable to load model", fn) print("Quitting...") sys.exit(1) print("Added", fn, "to world") if len(sys.argv) > 1: browser.emptyVisPlugin.add("world", browser.world) return browser vis.customUI(makefunc) vis.show() vis.setWindowTitle("Klamp't Resource Browser") vis.spin(float('inf')) vis.kill() del g_browser return #this code below is incorrect... app = QtWidgets.QApplication(sys.argv) browser = ResourceBrowser() for fn in sys.argv[1:]: res = browser.world.readFile(fn) if not res: print("Unable to load model", fn) print("Quitting...") sys.exit(1) print("Added", fn, "to world") if len(sys.argv) > 1: browser.emptyVisPlugin.add("world", browser.world) dw = QtWidgets.QDesktopWidget() x = dw.width() * 0.8 y = dw.height() * 0.8 browser.setFixedSize(x, y) #browser.splitter.setWindowState(QtCore.Qt.WindowMaximized) browser.setWindowTitle("Klamp't Resource Browser") browser.show() # Start the main loop. res = app.exec_() return res
class Simulation: def __init__(self, world): self.world = world self.robot = None self.collision_checker = None self.step_size = 0.03 self.actions = np.radians(np.arange(180, -180, -45)) # self.actions = np.array([np.pi, np.pi/2, 0, -np.pi/2]) self.actions = np.array([-np.pi, 0, np.pi / 2, -np.pi / 2]) self.reward_system = { "collision": -1, "boundary": -100, "free": -0.1, "goal": 200 } self.distance_factor = 0.05 self.start_pc = None self.goal_pc = None self.goal_range = 0.1 # size of the goal self.terrain_limit = [1, 1, 0] self.vis = None def reset(self, spc): # This function resets the robot to the start position again self.robot.setConfig(spc) def check_collision(self): for iT in range(self.world.numTerrains()): collRT0 = self.collision_checker.robotTerrainCollisions( self.world.robot(0), iT) collision_flag = False for i, j in collRT0: collision_flag = True strng = "Robot collides with " + j.getName() vis.addText("textCol", strng) vis.setColor("textCol", 1, 0, 0) break for iR in range(self.world.numRigidObjects()): collRT2 = self.collision_checker.robotObjectCollisions( self.world.robot(0), iR) for i, j in collRT2: if j.getName() != "goal": collision_flag = True strng = self.world.robot( 0).getName() + " collides with " + j.getName() vis.addText("textCol", strng) vis.setColor("textCol", 1, 0, 0) if collision_flag: vis.addText("textCol", "Collision") vis.setColor("textCol", 1, 0.0, 0.0) if not collision_flag: vis.addText("textCol", "No collision") vis.setColor("textCol", 0.4660, 0.6740, 0.1880) return collision_flag def check_goal_distance(self, cpc): """ This method checks the distance of the gaol from the current position of thr robot :param cpc: current position coordinate of the robot :return: """ if type(cpc) is np.ndarray: distance = np.sqrt( np.square(cpc[0, 0] - self.goal_pc[0]) + np.square(cpc[0, 1] - self.goal_pc[1])) if distance < self.goal_range: return 0 else: # return the distance from the goal return distance else: distance = np.sqrt( np.square(cpc[0] - self.goal_pc[0]) + np.square(cpc[1] - self.goal_pc[1])) if distance < self.goal_range: return 0 else: # return the distance from the goal return distance def create(self, start_pc, goal_pc): """ This method cretes the simulation :param start_pc: robot's initial position coordinate :param goal_pc: goal position coordinate :return: """ print "Creating the Simulator" object_dir = "/home/jeet/PycharmProjects/DeepQMotionPlanning/" self.start_pc = start_pc self.goal_pc = goal_pc coordinates.setWorldModel(self.world) getDoubleRoomDoor(self.world, 8, 8, 1) builder = Builder(object_dir) # Create a goal cube n_objects = 1 width = 0.1 depth = 0.1 height = 0.1 x = goal_pc[0] y = goal_pc[1] z = goal_pc[2] / 2 thickness = 0.005 color = (0.2, 0.6, 0.3, 1.0) builder.make_objects(self.world, n_objects, "goal", width, depth, height, thickness, self.terrain_limit, color, self.goal_pc) # Create a obstacle cube n_objects = 4 width = 0.2 depth = 0.2 height = 0.2 x = 2.3 y = 0.8 z = goal_pc[2] / 2 thickness = 0.001 color = (0.8, 0.2, 0.2, 1.0) builder.make_objects(self.world, n_objects, "rigid", width, depth, height, thickness, self.terrain_limit, color) self.vis = vis vis.add("world", self.world) # Create the view port vp = vis.getViewport() vp.w, vp.h = 1200, 900 vp.x, vp.y = 0, 0 vis.setViewport(vp) # Create the robot self.robot = sphero6DoF(self.world.robot(0), "", None) self.robot.setConfig(start_pc) # Create the axis representation # vis.add("WCS", [so3.identity(), [0, 0, 0]]) # vis.setAttribute("WCS", "size", 24) # Add text messages component for collision check and robot position vis.addText("textCol", "No collision") vis.setAttribute("textCol", "size", 24) vis.addText("textStep", "Steps: ") vis.setAttribute("textStep", "size", 24) vis.addText("textGoalDistance", "Goal Distance: ") vis.setAttribute("textGoalDistance", "size", 24) vis.addText("textConfig", "Robot configuration: ") vis.setAttribute("textConfig", "size", 24) self.collision_checker = collide.WorldCollider(self.world) vis.setWindowTitle("Simulator") vis.show() return vis, self.robot, self.world def get_current_state(self): """ This method returns teh current configuration of the robot :return: """ config = self.robot.getConfig() state = np.array([[config[0], config[1]]]) return state def get_next_state(self, action, step_count, query=False): """ This method implements the take action functionality by asking the robot to take the action, and return the next robot configuration :param action: action to take :param step_count: Number of steps taken by the robot so far :param query: A boolean variable denotes weather it slearning phase or query phase, if it is a query phase add a delay for better visualization :return: """ pc = self.robot.getConfig() old_pc = copy.deepcopy(pc) # Compute the new positions pc[0] = pc[0] + self.step_size * np.cos(self.actions[action]) pc[1] = pc[1] + self.step_size * np.sin(self.actions[action]) pc[2] = 0.1 pc[3] = 0 pc[4] = 0 pc[5] = 0 self.robot.setConfig(pc) boundary_reached = False if np.abs(pc[0]) > self.terrain_limit[0] or np.abs( pc[1]) > self.terrain_limit[1]: boundary_reached = True q2f = ['{0:.2f}'.format(elem) for elem in pc] label = "Steps: " + str(step_count) vis.addText("textStep", label) cpc = self.robot.getConfig() # compute the distance distance = self.check_goal_distance(cpc) label_goal = "Goal Distance: " + str(distance) vis.addText("textGoalDistance", label_goal) label = "Robot configuration: " + str(q2f) vis.addText("textConfig", label) collision = self.check_collision() goal_reached = False # Update the robot positions if boundary_reached: self.robot.setConfig(old_pc) reward = self.reward_system['boundary'] print("Hit Boundary", reward) pc = old_pc elif collision: self.robot.setConfig(old_pc) reward = self.reward_system['collision'] print("Collision", reward) # Don't let it cross the boundary pc = old_pc elif distance == 0: reward = self.reward_system['goal'] goal_reached = True else: reward = self.reward_system['free'] * distance # send only the x and y position state = np.array([[pc[0], pc[1]]]) if query: time.sleep(0.002) return reward, state, goal_reached def run(self): while vis.shown(): vis.lock() vis.unlock() # changes to the visualization must be done outside the lock vis.clearText() print "Ending klampt.vis visualization." vis.kill()
def modification_template(world): """Tests a variety of miscellaneous vis functions""" vis.add("world",world) robot = world.robot(0) vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque import random for i in range(10): #set some links to random colors randlink = random.randint(0,robot.numLinks()-1) color = (random.random(),random.random(),random.random()) vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color) #test the on-screen text display vis.addText("text2","Here's some red text") vis.setColor("text2",1,0,0) vis.addText("text3","Here's bigger text") vis.setAttribute("text3","size",24) vis.addText("text4","Transform status") vis.addText("textbottom","Text anchored to bottom of screen",(20,-30)) #test a point pt = [2,5,1] vis.add("some point",pt) #test a rigid transform vis.add("some blinking transform",[so3.identity(),[1,3,0.5]]) vis.edit("some point") #vis.edit("some blinking transform") #vis.edit("coordinates:ATHLETE:ankle roll 3") #test an IKObjective link = world.robot(0).link(world.robot(0).numLinks()-1) #point constraint obj = ik.objective(link,local=[[0,0,0]],world=[pt]) #hinge constraint #obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]]) #transform constraint #obj = ik.objective(link,R=link.getTransform()[0],t=pt) vis.add("ik objective",obj) #enable plotting vis.addPlot('plot') vis.addPlotItem('plot','some point') vis.setPlotDuration('plot',10.0) #run the visualizer, which runs in a separate thread vis.setWindowTitle("Manual animation visualization test") class MyCallback: def __init__(self): self.iteration = 0 def __call__(self): vis.lock() #TODO: you may modify the world here. This line tests a sin wave. pt[2] = 1 + math.sin(self.iteration*0.03) vis.unlock() #changes to the visualization with vis.X functions can done outside the lock if (self.iteration % 100) == 0: if (self.iteration / 100)%2 == 0: vis.hide("some blinking transform") vis.addText("text4","The transform was hidden") vis.logPlotEvent('plot','hide') else: vis.hide("some blinking transform",False) vis.addText("text4","The transform was shown") vis.logPlotEvent('plot','show') #this is another way of changing the point's data without needing a lock/unlock #vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True) #or #vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)]) if self.iteration == 200: vis.addText("text2","Going to hide the text for a second...") if self.iteration == 400: #use this to remove text vis.clearText() if self.iteration == 500: vis.addText("text2","Text added back again") vis.setColor("text2",1,0,0) self.iteration += 1 callback = MyCallback() if not MULTITHREADED: vis.loop(callback=callback,setup=vis.show) else: vis.show() while vis.shown(): callback() time.sleep(0.01) #use this to remove a plot vis.remove("plot") vis.kill()
def load_pcloud(self, save_file): """ Converts a geometry to another type, if a conversion is available. The interpretation of param depends on the type of conversion, with 0 being a reasonable default. Available conversions are: PointCloud -> TriangleMesh, if the point cloud is structured. param is the threshold for splitting triangles by depth discontinuity, by default infinity. """ long_np_cloud = np.fromfile(save_file) print(long_np_cloud.shape, ": shape of long numpy cloud") num_points = long_np_cloud.shape[0] / 3 np_cloud = np.zeros(shape=(num_points, 3)) pcloud = klampt.PointCloud() scaling_factor = 0.1 points = [] xs = [] ys = [] zs = [] for x in range(num_points): i = x * 3 x_val = long_np_cloud[i] * scaling_factor y_val = long_np_cloud[i + 1] * scaling_factor z_val = long_np_cloud[i + 2] * scaling_factor np_cloud[x][0] = x_val np_cloud[x][1] = y_val np_cloud[x][2] = z_val xs.append(x_val) ys.append(y_val) zs.append(z_val) points.append(np_cloud[x]) points.sort(key=lambda tup: tup[2]) x_sorted = sorted(xs) # sorted y_sorted = sorted(ys) # sorted z_sorted = sorted(zs) # sorted xfit = stats.norm.pdf(x_sorted, np.mean(x_sorted), np.std(x_sorted)) yfit = stats.norm.pdf(y_sorted, np.mean(y_sorted), np.std(y_sorted)) zfit = stats.norm.pdf(z_sorted, np.mean(z_sorted), np.std(z_sorted)) # plot with various axes scales plt.figure(1) # linear plt.subplot(221) plt.plot(x_sorted, xfit) plt.hist(x_sorted, normed=True) plt.title("X values") plt.grid(True) plt.subplot(222) plt.plot(y_sorted, yfit) plt.hist(y_sorted, normed=True) plt.title("Y values") plt.grid(True) plt.subplot(223) plt.plot(z_sorted, zfit) plt.hist(z_sorted, normed=True) plt.title("Z values") plt.grid(True) # Format the minor tick labels of the y-axis into empty strings with # `NullFormatter`, to avoid cumbering the axis with too many labels. plt.gca().yaxis.set_minor_formatter(NullFormatter()) # Adjust the subplot layout, because the logit one may take more space # than usual, due to y-tick labels like "1 - 10^{-3}" plt.subplots_adjust(top=0.92, bottom=0.08, left=0.10, right=0.95, hspace=0.25, wspace=0.35) # plt.show() median_z = np.median(zs) threshold = 0.25 * scaling_factor for point in points: if np.fabs(point[2] - median_z) < threshold: pcloud.addPoint(point) print(pcloud.numPoints(), ": num points") # Visualize pcloud.setSetting("width", "3") pcloud.setSetting("height", str(len(points) / 3)) g3d_pcloud = Geometry3D(pcloud) mesh = g3d_pcloud.convert("TriangleMesh", 0) world = robotsim.WorldModel() vis.add("world", world) vis.add("coordinates", coordinates.manager()) vis.setWindowTitle("PointCloud World") vp = vis.getViewport() vp.w, vp.h = 800, 800 vis.setViewport(vp) vis.autoFitCamera() vis.show() vis.lock() vis.add("mesh", mesh) vis.unlock() while vis.shown(): time.sleep(0.01) vis.kill() print("done")
def endEnv(self): vis.clearText() print "Ending klampt.vis visualization." vis.kill()
def showTwoStep(plugin, world, robot, ground1, ground2, ground3, ground4, link_foot, link_foot_other): """Read the data file and select trajectories to show, they are composed of two steps""" data = np.load('data/twoStepBunch.npz')['output'] ndata = len(data) N = 20 force_len = 0.5 vis.show() for j in range(ndata): i = j print('Entering traj %d' % i) l0, l1, h0, h1, vel, phase0, phase1 = data[i] # set those grounds ground1.setConfig([0, 0, 0, 0, 0, 0]) ground2.setConfig([l0, 0, h0, 0, 0, 0]) ground3.setConfig([l0 + l1, 0, h0 + h1, 0, 0, 0]) ground4.setConfig([2 * l0 + l1, 0, 2 * h0 + h1, 0, 0, 0]) while vis.shown() and not plugin.quit: if plugin.nextone: # check if we want next one plugin.nextone = False break # show phase0 t, q, force = getTraj(phase0) h_ = t[1] - t[0] if h_ < 0: break nPoint = len(t) for k in range(nPoint): vis.lock() useq = copy_copy(q[k], False) robot.setConfig(useq) footpos = link_foot.getWorldPosition([0, 0, 0.5]) support = np.array([force[k, 0], 0, force[k, 1]]) use_support = support / np.linalg.norm(support) * force_len force_end = vectorops.add(footpos, use_support.tolist()) vis.add("force", Trajectory([0, 1], [footpos, force_end])) vis.unlock() time.sleep(h_ * 5.0) vis.remove('force') # phase 1 t, q, force = getTraj(phase1) h_ = t[1] - t[0] if h_ < 0: break print('h_ = %f' % h_) for k in range(nPoint): vis.lock() useq = copy_copy(q[k], True) robot.setConfig(useq) footpos = link_foot_other.getWorldPosition([0, 0, 0.5]) support = np.array([force[k, 0], 0, force[k, 1]]) use_support = support / np.linalg.norm(support) * force_len force_end = vectorops.add(footpos, use_support.tolist()) vis.add("force", Trajectory([0, 1], [footpos, force_end])) vis.unlock() time.sleep(h_ * 5.0) vis.remove('force') while vis.shown() and not plugin.quit: vis.lock() vis.unlock() time.sleep(0.05) print("Ending visualization.") vis.kill()
dataset,objname = o.split('/',2) try: index = int(objname) objname = objects[dataset][index] except: pass shelved.append((dataset,objname)) else: #format: dataset/object for o in sys.argv[2:]: dataset,objname = o.split('/',2) try: index = int(objname) objname = objects[dataset][index] except: pass shelved.append((dataset,objname)) launch_shelf(robot,shelved) else: #just plan grasping try: index = int(sys.argv[2]) objname = objects[dataset][index] except IndexError: index = random.randint(0,len(objects[dataset])-1) objname = objects[dataset][index] except ValueError: objname = sys.argv[2] launch_simple(robot,dataset,objname) vis.kill()
def main(): print( "================================================================================" ) print(sys.argv[0] + ": Simulates a robot file and Python controller") if len(sys.argv) <= 1: print( "USAGE: klampt_sim [world_file] [trajectory (.traj/.path) or controller (.py)]" ) print() print( " Try: klampt_sim athlete_plane.xml motions/athlete_flex_opt.path " ) print(" [run from Klampt-examples/data/]") print( "================================================================================" ) if len(sys.argv) <= 1: exit() world = WorldModel() #load up any world items, control modules, or paths control_modules = [] for fn in sys.argv[1:]: path, base = os.path.split(fn) mod_name, file_ext = os.path.splitext(base) if file_ext == '.py' or file_ext == '.pyc': sys.path.append(os.path.abspath(path)) mod = importlib.import_module(mod_name, base) control_modules.append(mod) elif file_ext == '.path': control_modules.append(fn) else: res = world.readFile(fn) if not res: print("Unable to load model " + fn) print("Quitting...") sys.exit(1) viewer = MyGLViewer(world) for i, c in enumerate(control_modules): if isinstance(c, str): from klampt.control.blocks import trajectory_tracking from klampt.model.trajectory import RobotTrajectory from klampt.io import loader traj = loader.load('Trajectory', c) rtraj = RobotTrajectory(world.robot(i), traj.times, traj.milestones) #it's a path file, try to load it controller = trajectory_tracking.TrajectoryPositionController( rtraj) else: try: maker = c.make except AttributeError: print("Module", c.__name__, "must have a make() method") print("Quitting...") sys.exit(1) controller = maker(world.robot(i)) viewer.sim.setController(world.robot(i), controller) vis.setWindowTitle("Klamp't Simulation Tester") if SPLIT_SCREEN_TEST: viewer2 = MyGLViewer(world) vis.setPlugin(viewer) vis.addPlugin(viewer2) viewer2.window.broadcast = True vis.show() while vis.shown(): time.sleep(0.01) vis.kill() else: vis.run(viewer)