def multiwindow_template(world): """Tests multiple windows and views.""" vis.add("world",world) vp = vis.getViewport() vp.w,vp.h = 800,800 vis.setViewport(vp) vis.setWindowTitle("vis.spin test: will close in 5 seconds...") vis.spin(5.0) #Now testing ability to re-launch windows vis.setWindowTitle("Shown again. Close me to proceed.") vis.spin(float('inf')) vis.setWindowTitle("Dialog test. Close me to proceed.") vp = vis.getViewport() vp.w,vp.h = 400,600 vis.setViewport(vp) vis.dialog() vp.w,vp.h = 640,480 vis.setViewport(vp) for i in range(3): widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) vis.addPlugin(widgets) vis.setWindowTitle("Split screen test") vis.spin(float('inf')) vis.setPlugin(None) vis.setWindowTitle("Back to normal. Close me to quit.") vis.dialog() vis.kill()
def multiwindow_template(world): """Tests multiple windows and views.""" vis.add("world", world) vp = vis.getViewport() vp.w, vp.h = 400, 600 vis.setViewport(vp) vis.addText("label1", "This is Window 1", (20, 20)) vis.setWindowTitle("Window 1") vis.show() id1 = vis.getWindow() print("First window ID:", id1) id2 = vis.createWindow("Window 2") vis.add("Lone point", [0, 0, 0]) vis.setViewport(vp) vis.addText("label1", "This is Window 2", (20, 20)) print("Second window ID:", vis.getWindow()) vis.setWindow(id2) vis.spin(float('inf')) #restore back to 1 window, clear the text vis.setWindow(id1) vis.clearText() vp = vis.getViewport() vp.w, vp.h = 800, 800 vis.setViewport(vp) vis.setWindowTitle("vis.spin test: will close in 5 seconds...") vis.spin(5.0) #Now testing ability to re-launch windows vis.setWindowTitle("Shown again. Close me to proceed.") vis.spin(float('inf')) vis.setWindowTitle("Dialog test. Close me to proceed.") vp = vis.getViewport() vp.w, vp.h = 400, 600 vis.setViewport(vp) vis.dialog() vp.w, vp.h = 640, 480 vis.setViewport(vp) for i in range(3): widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) vis.addPlugin(widgets) vis.setWindowTitle("Split screen test") vis.spin(float('inf')) vis.setPlugin(None) vis.setWindowTitle("Back to normal. Close me to quit.") vis.dialog() vis.kill()
def viewport_template(world): """Changes the parameters of the viewport and main window""" #add the world to the visualizer vis.add("world",world) vp = vis.getViewport() vp.w,vp.h = 800,800 vis.setViewport(vp) #this auto-sizes the camera vis.autoFitCamera() vis.setWindowTitle("Viewport modification test") vis.spin(float('inf')) vis.kill()
def run(self): """Standardized interface for running program""" vp = vis.getViewport() #Square screen #vp.w,vp.h = 800,800 #For saving HD quality movies vp.w, vp.h = 1024, 768 vp.clippingplanes = self.clippingplanes vis.setViewport(vp) #vis.run(program) vis.setPlugin(self) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill()
def visualize(self): 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.unlock() while vis.shown(): time.sleep(0.01) vis.kill()
def run(self): """Standardized interface for running program""" if KLAMPT_VERSION >= 0.7: vp = vis.getViewport() #Square screen #vp.w,vp.h = 800,800 #For saving HD quality movies vp.w, vp.h = 1024, 768 vp.clippingplanes = self.clippingplanes vis.setViewport(vp) #vis.run(program) vis.setPlugin(self) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill() else: #Square screen #self.width,self.height = 800,800 #For saving HD quality movies self.width, self.height = 1024, 768 GLBaseClass.run(self)
world = WorldModel() world_file = "../data/simulation_test_worlds/drc_rough_terrain_world.xml" if not world.readFile(world_file): raise RuntimeError("Unable to load terrain model") robot_file = "../data/robot_model/robosimian_caesar_new.rob" if not world.readFile(robot_file): raise RuntimeError("Unable to load robot model") vis_window_id = vis.createWindow(world_file) vis.setWindow(vis_window_id) vis.add("world", world) vp = vis.getViewport() vis.setViewport(vp) vis.autoFitCamera() world.robot(0).setConfig([ 12.621508747630084, 1.3060978650033888, 0.7271994997360561, -0.18389666460947365, -0.2336561986984183, 0.23915345995072382, 0.0, 0.12877367095232392, -0.24152711808937907, -1.352324085841938, -1.3962990011600755,
def oncamerainfo(ci): vp = vis.getViewport() kros.from_CameraInfo(ci, vp) vis.setViewport(vp)
def determine_reachable_points_robot(self, sampling_places, world, robot, lamp, collider, show_vis=False, neighborhood=0.4, float_height=0.08, base_linknum=2): self.set_robot_link_collision_margins(robot, 0.15, collider, []) show_vis = True if (show_vis): vis.add('world', world) # eliminating draw distance vis.lock() # time.sleep(0.5) vp = vis.getViewport() # vp.h = 640 # vp.w = 640 vp.clippingplanes = [0.1, 10000] tform = pickle.load(open('tform.p', 'rb')) vp.setTransform(tform) scale = 1 vp.w = 1853 // scale vp.h = 1123 // scale # vis.setViewport(vp) vis.scene().setViewport(vp) vis.unlock() vis.show() reachable = [] configs = [] # world.terrain(0).geometry().setCollisionMargin(0.05) for place in tqdm(sampling_places): robot.setConfig(place) reachable.append( self.solve_ik_near_sample(robot, lamp, collider, world, place, restarts=10, tol=1e-2, neighborhood=neighborhood, float_height=float_height)) # print('reachable? = {}'.format(reachable[-1])) cfig = robot.getConfig() # print(cfig[2]) configs.append(cfig + place.tolist()) # after checking for those margins, we reset the robot to its original configs for general motion planning # self.set_robot_link_collision_margins(robot,0,collider,self.angular_dofs) self.set_robot_link_collision_margins(robot, 0, collider, []) # we also reset the base and environment collision to simplify path planning: world.terrain(0).geometry().setCollisionMargin(0) return reachable, configs
if not w.loadFile("../../data/simulation_test_worlds/sensortest.xml"): print("Error loading test world?") exit(1) sim = Simulator(w) robot = w.robot(0) cam = sim.controller(0).sensor("rgbd_camera") link = robot.link(robot.numLinks() - 1) amplitudes = [random.uniform(0, 2) for i in range(robot.numLinks())] phases = [random.uniform(0, math.pi * 2) for i in range(robot.numLinks())] periods = [random.uniform(0.5, 2) for i in range(robot.numLinks())] qmin, qmax = robot.getJointLimits() vis.add("world", w) vis.add("cam", cam) vp = vis.getViewport() vp.camera.rot[1] -= 0.5 vis.setViewport(vp) default = vis.getViewport().getTransform() print('x:', so3.apply(default[0], [1, 0, 0])) print('y:', so3.apply(default[0], [0, 1, 0])) print('z:', so3.apply(default[0], [0, 0, 1])) print('offset:', default[1]) circle_points = [] npts = 50 times = [] for i in range(npts + 1): angle = math.radians(360 * i / npts) circle_points.append( se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default)) times.append(i * 20 / npts)
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 vis_world(self): world = WorldModel() self.rposer = None res = world.readFile(self.world_file) if not res: raise RuntimeError("Unable to load terrain model") res = world.readFile(self.robot_file) if not res: raise RuntimeError("Unable to load robot model") vis.createWindow(self.worldname) vis.setWindowTitle(self.worldname + " visualization") vis.add("world", world) vp = vis.getViewport() # vp.w, vp.h = 800, 800 vis.setViewport(vp) vis.autoFitCamera() vis.show() q = [ 0, 0.0, 0.5, # torso x y z 0, 0, 0, # torso roll pitch yaw 0, 0, 0, -0.785398, -1.5707, -1.5707, 0.785398, 0, # fr 0, 0, 0, 0.785398, 1.5707, 1.5707, -0.785398, 0, # br 0, 0, 0, -0.785398, -1.5707, -1.5707, 0.785398, 0, # bl 0, 0, 0, 0.785398, 1.5707, 1.5707, -0.785398, 0, # fl ] world.robot(0).setConfig(q)
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")
mesh = w.terrain(0).geometry().getTriangleMesh() terrain_pub = kros.object_publisher("terrain", mesh, latch=True, queue_size=10) terrain_pub.publish(mesh) pc = w.terrain(0).geometry().convert('PointCloud').getPointCloud() pc_pub = kros.object_publisher("terrain_point_cloud", pc, latch=True, queue_size=10) pc_pub.publish(pc) vis.add("world", w) vis.show() camera_pub = kros.object_publisher('camera_info', vis.getViewport(), latch=True, queue_size=10) camera_pub.publish(vis.getViewport()) sim = Simulator(w) js_pub = kros.object_publisher("js", sim.controller(0), convert_kwargs={ 'q': 'actual', 'dq': 'actual', 'effort': 'actual' }, queue_size=10) print("Simulating and publishing...")
def __init__(self, fn): ## Creates a world and loads all the items on the command line self.world = WorldModel() self.robotSystemName = 'O' for f in fn: print(f) res = self.world.readFile(f) if not res: raise RuntimeError("Unable to load model " + fn) self.showVis = False coordinates.setWorldModel(self.world) vis.lock() bW.getDoubleRoomWindow(self.world, 8, 8, 1) vis.unlock() ## Add the world to the visualizer vis.add("world", self.world) vp = vis.getViewport() vp.w, vp.h = 1800, 800 vis.setViewport(vp) self.robots = [] self.n = self.world.numRobots() for i in range(self.n): self.robots.append( sphero6DoF(self.world.robot(i), self.world.robot(i).getName())) self.eps = 0.000001 self.sj = [[0, 0, 0], [0.2, 0, 0]] self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0], [0.070534, -0.097082, 0], [0.057063, -0.018541, 0], [0.11413, 0.037082, 0], [0.035267, 0.048541, 0], [0, 0.12, 0], [-0.035267, 0.048541, 0], [-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]] self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0], [0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0], [0.6, 0, 0], [0.8, 0, 0]] self.sj = self.sjL self.xB = [-4, 4] self.yB = [-4, 4] self.zB = [0.02, 1] self.rad = 0.04 self.currConfig = [0, 0, 1, 1, 0] self.scMin = 1 self.scXMin = 1 self.scYMin = 2 self.sumDist = 0 if self.n > 1: minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1])) minSijX = math.fabs(self.sj[0][0] - self.sj[1][0]) minSijY = math.fabs(self.sj[0][1] - self.sj[1][1]) for i in range(self.n): for j in range(self.n): if i != j: dist = vectorops.norm( vectorops.sub(self.sj[i], self.sj[j])) if dist < minSij: minSij = dist dist = math.fabs(self.sj[i][0] - self.sj[j][0]) if dist < minSijX: minSijX = dist dist = math.fabs(self.sj[i][1] - self.sj[j][1]) if dist < minSijY: minSijY = dist for i in range(self.n): self.sumDist += vectorops.norm(self.sj[i]) if minSij > self.eps: self.scMin = 2 * math.sqrt(2) * self.rad / minSij if minSijX > self.eps: self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX if minSijY > self.eps: self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY self.scMax = max(2, self.scMin) self.scB = [self.scMin, 2 * self.scMin] print('Minimum scale') print(self.scMin) vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]]) vis.setAttribute(self.robotSystemName, "size", 12) vis.edit(self.robotSystemName) vis.add("WCS", [so3.identity(), [0, 0, 0]]) vis.setAttribute("WCS", "size", 32) vis.edit("WCS") self.collisionChecker = collide.WorldCollider(self.world) if self.showVis: ## Display the world coordinate system vis.addText("textCol", "No collision") vis.setAttribute("textCol", "size", 24) ## On-screen text display vis.addText("textConfig", "Robot configuration: ") vis.setAttribute("textConfig", "size", 24) vis.addText("textbottom", "WCS: X-axis Red, Y-axis Green, Z-axis Blue", (20, -30)) print "Starting visualization window#..." ## Run the visualizer, which runs in a separate thread vis.setWindowTitle("Visualization for kinematic simulation") vis.show() simTime = 60 startTime = time.time() oldTime = startTime self.setConfig(0, 0, 1, 1, 0) q = self.robots[0].getConfig() if self.showVis: q2f = ['{0:.2f}'.format(elem) for elem in q] strng = "Robot configuration: " + str(q2f) vis.addText("textConfig", strng) colFlag = self.checkCollision() print(colFlag) if self.showVis: time.sleep(10)