def __init__(self,h): GLPluginInterface.__init__(self) self.hand = h h.load_frames() self.world = WorldModel() #load robot robot = moving_base.make_moving_base_robot(h.robot_file,self.world,floating=True) #load object id = self.world.loadElement(object_file) if id < 0: raise IOError("Unable to load "+object_file) assert self.world.numRigidObjects() > 0 object = self.world.rigidObject(0) object.setTransform(so3.rotation([1,0,0],math.pi/2),[0,0,0.2]) #set up unit frames coordinates.setWorldModel(self.world) coordinates.listItems() hand_coordinates = coordinates.manager().subgroups[robot.getName()] hand_coordinates.listItems() self.unit_frames = [] for i,(key,unit) in enumerate(h.all_units): self.unit_frames.append(hand_coordinates.addFrame("unit"+str(i),parent=hand_coordinates.frame(key),relativeCoordinates=unit.localTransform)) #set up visualization vis.add("robot",robot) vis.edit("robot") vis.add("rigidObject",object) vis.edit("rigidObject") print "COM",object.getMass().getCom() vis.add("COM",se3.apply(object.getTransform(),object.getMass().getCom())) vis.add("origin",se3.identity()) #vis.add("coordinates",coordinates.manager()) self.config_name = None #set up friction solver folder="FrictionCones/" self.friction_factories = {} for key,unit in h.all_units: if unit.type not in self.friction_factories: self.friction_factories[unit.type] = stability.GeneralizedFrictionConeFactory() self.friction_factories[unit.type].load(folder + unit.type) self.contact_units = [] self.wrenches = None self.torques = None self.feasible = None self.angle = 0 self.wrenchSpace = None self.wrenchSpace6D = None self.frame_gl_objects = [] self.wrench_space_gl_object = None
def coordinates_template(world): """Tests integration with the coordinates module.""" #add the world to the visualizer vis.add("world", world) coordinates.setWorldModel(world) #add the coordinate Manager to the visualizer vis.add("coordinates", coordinates.manager()) vis.setWindowTitle("Coordinates visualiation test") if MANUAL_EDITING: #manually adds a poser, and adds a callback whenever the widget changes widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) #update the coordinates every time the widget changes widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld()) vis.pushPlugin(widgets) if not MULTITHREADED: vis.loop(callback=None, setup=vis.show) else: vis.show() while vis.shown(): time.sleep(0.01) else: vis.edit(("world", world.robot(0).getName())) if not MULTITHREADED: def callback(coordinates=coordinates): coordinates.updateFromWorld() vis.loop(callback=callback, setup=vis.show) else: vis.show() while vis.shown(): vis.lock() #reads the coordinates from the world coordinates.updateFromWorld() vis.unlock() time.sleep(0.01) #quit the visualization thread nicely vis.kill()
def visualize_pc_in_klampt_vis(self, pcloud_fname): title = pcloud_fname + " klampt world" vis_window_id = vis.createWindow(title) vis.setWindowTitle(title) world = WorldModel() vis.add("world", world) pcd = o3d.io.read_point_cloud(pcloud_fname) print(pcd) pc_xyzs = np.asarray(pcd.points) pc_xyzs_as_list = pc_xyzs.flatten().astype("float") # pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0]) pcloud = PointCloud() pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list) print(pcloud.numPoints()) vis.add("pcloud", pcloud) # vis.setColor("pcloud", 0, 0, 1, a=1) # vis.setAttribute("p1", "size", 5.0) box = klampt.GeometricPrimitive() box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2]) g = klampt.Geometry3D(box) vis.add("box", g) vis.setColor("box", 0, 0, 1, 0.5) coordinates.setWorldModel(world) vis.add("coordinates", coordinates.manager()) vis.show() while vis.shown(): vis.lock() vis.unlock() time.sleep(0.01) vis.kill()
return True if __name__ == "__main__": if len(sys.argv) <= 1: print "USAGE: kinematicSim.py [world_file]" exit() ## Creates a world and loads all the items on the command line world = WorldModel() for fn in sys.argv[1:]: res = world.readFile(fn) if not res: raise RuntimeError("Unable to load model " + fn) coordinates.setWorldModel(world) ## A rooms separated by a wall with a door bW.getRoom(world, 8, 8, 1) ## Add the world to the visualizer vis.add("world", world) vp = vis.getViewport() vp.w, vp.h = 1200, 800 vis.setViewport(vp) ## Create robot object. Change the class to the desired robot. ## Also, make sure the robot class corresponds to the robot in simpleWorld.xml file #robot = kobuki(world.robot(0), vis) #robot.setAltitude(0.01)
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 __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)