def edit_template(world): """Shows how to pop up a visualization window with a world in which the robot configuration and a transform can be edited""" #add the world to the visualizer vis.add("world",world) xform = se3.identity() vis.add("transform",xform) robotPath = ("world",world.robot(0).getName()) #compound item reference: refers to robot 0 in the world vis.edit(robotPath) vis.edit("transform") #This prints how to get references to items in the visualizer print("Visualization items:") vis.listItems(indent=2) vis.setWindowTitle("Visualization editing test") if not MULTITHREADED: vis.loop(setup=vis.show) else: vis.show() while vis.shown(): vis.lock() #TODO: you may modify the world here. vis.unlock() time.sleep(0.01) print("Resulting configuration",vis.getItemConfig(robotPath)) print("Resulting transform (config)",vis.getItemConfig("transform")) # this is a vector describing the item parameters xform = list(xform) #convert se3 element from tuple to list config.setConfig(xform,vis.getItemConfig("transform")) print("Resulting transform (se3)",xform) #quit the visualization thread nicely vis.kill()
def play_with_trajectory(traj, configs=[3]): vis.add("trajectory", traj) names = [] for i, x in enumerate(traj.milestones): if i in configs: print("Editing", i) names.append("milestone " + str(i)) vis.add(names[-1], x[:]) vis.edit(names[-1]) #vis.addPlot("distance") vis.show() while vis.shown(): vis.lock() t0 = time.time() updated = False for name in names: index = int(name.split()[1]) qi = vis.getItemConfig(name) if qi != traj.milestones[index]: traj.milestones[index] = qi updated = True if updated: vis.add("trajectory", traj) xnew = trajcache.trajectoryToState(traj) ctest2.setx(xnew) res = ctest2.minvalue(xtraj) print(res) ctest2.clearx() vis.unlock() t1 = time.time() #vis.logPlot("timing","opt",t1-t0) time.sleep(max(0.001, 0.025 - (t1 - t0)))
def edit_camera_xform(world_fn, xform=None, title=None): """Visual editor of the camera position """ world = WorldModel() world.readFile(world_fn) world.readFile("camera.rob") robot = world.robot(0) sensor = robot.sensor(0) if xform is not None: sensing.set_sensor_xform(sensor, xform) vis.createWindow() if title is not None: vis.setWindowTitle(title) vis.resizeWindow(1024, 768) vis.add("world", world) vis.add("sensor", sensor) vis.add("sensor_xform", sensing.get_sensor_xform(sensor, robot)) vis.edit("sensor_xform") def update_sensor_xform(): sensor_xform = vis.getItemConfig("sensor_xform") sensor_xform = sensor_xform[:9], sensor_xform[9:] sensing.set_sensor_xform(sensor, sensor_xform) vis.loop(callback=update_sensor_xform) sensor_xform = vis.getItemConfig("sensor_xform") return sensor_xform[:9], sensor_xform[9:]
def reset(): vis.lock() robot.setConfig(q0) base_link.setParentTransform( *se3.mul(se3.inv(parent_xform), base_xform0)) vis.unlock() vis.add("base_xform", base_xform0) vis.edit("base_xform") vis.setItemConfig("gripper", grob.getConfig())
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 edit_config(): global world, robot resource.setDirectory("resources/") qhands = [r.getConfig() for r in hand_subrobots.values()] q = resource.get("robosimian_body_stability.config", default=robot.getConfig(), doedit=False) robot.setConfig(q) for r, qhand in zip(hand_subrobots.values(), qhands): r.setConfig(qhand) vis.add("world", world) vis.edit(("world", robot.getName()), True) vis.dialog() vis.edit(("world", robot.getName()), False) resource.set("robosimian_body_stability.config", robot.getConfig())
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 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 keyboardfunc(self,c,x,y): if c == 'h': print "Help: " print "- s: save grasp configuration to disk" print "- l: load grasp configuration from disk" print "- <: rotate left" print "- >: rotate right" print "- [space]: solve for stability" print "- f: solve for the feasible force volume" print "- w: solve for the feasible wrench space" print "- f: solve for the robust force volume" print "- m: save the current volume to a mesh" print "- M: load the current volume from a mesh" print "- 6: calculates the 6D wrench space" print "- 7: loads the 6D wrench space" print "- 0: zeros the robot's base transform" print "- 9: hides the widgets" if c == 's': name = raw_input("Enter a name for this configuration > ") if name.strip() == '': print "Not saving..." else: self.save_config(name) self.config_name = name elif c == 'l': vis.animate("rigidObject",None) name = raw_input("Enter a name for the configuration to load > ") if name.strip() == '': print "Not loading..." else: self.load_config(name) elif c == ',' or c == '<': vis.animate("rigidObject",None) self.rotate(math.radians(1)) print "Now at angle",math.degrees(self.angle) elif c == '.' or c == '>': vis.animate("rigidObject",None) self.rotate(-math.radians(1)) print "Now at angle",math.degrees(self.angle) elif c == ' ': self.solve(self.contact_units) elif c == 'f': if self.wrenchSpace6D != None: cm = se3.apply(self.world.rigidObject(0).getTransform(),self.world.rigidObject(0).getMass().getCom()) self.wrenchSpace = self.wrenchSpace6D.getSlice(cm).convex_decomposition[0] self.wrench_space_gl_object = None else: self.wrenchSpace = self.calculate_force_volume((0,0,0)) self.wrenchSpace6D = None self.wrench_space_gl_object = None elif c == 'w': self.wrenchSpace = self.calculate_3D_wrench_space() self.wrenchSpace6D = None self.wrench_space_gl_object = None elif c == 'r': dropout = raw_input("How much to drop out? (default 0) > ") if dropout.strip() == '': dropout = 0 else: dropout = float(dropout.strip()) perturb = raw_input("How much to perturb? (default 0.1) > ") if perturb.strip() == '': perturb = 0.1 else: perturb = float(perturb.strip()) N = raw_input("How many samples? (default 100) > ") if N.strip() == '': N = 100 else: N = int(N.strip()) self.wrenchSpace = self.calculate_robust_force_volume((0,0,0),dropout=dropout,configurationPerturbation=perturb,maxVolumes=N) self.wrenchSpace6D = None self.wrench_space_gl_object = None elif c == 'm': assert self.wrenchSpace != None from PyQt4.QtGui import QFileDialog savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name name = QFileDialog.getSaveFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)") g = polytope.hull_to_klampt_geom(self.wrenchSpace) g.saveFile(str(name)) elif c == 'M': from PyQt4.QtGui import QFileDialog savedir = "data/"+self.world.robot(0).getName()+'/'+self.config_name name = QFileDialog.getOpenFileName(caption="Mesh file (*.obj, *.off, etc)",directory=savedir,filter="Wavefront OBJ (*.obj);;Object File Format (*.off);;All files (*.*)") g = Geometry3D() g.loadFile(str(name)) self.wrenchSpace = polytope.klampt_geom_to_hull(g) self.wrench_space_gl_object = None #decomp = stability.approximate_convex_decomposition(g) #print "Convex decomposition into",len(decomp),"pieces" print len(polytope.pockets(g)),"pocket faces" elif c == '6': N = raw_input("How many samples? (default 100) > ") if N.strip() == '': N = 100 else: N = int(N.strip()) self.wrenchSpace6D = self.calculate_6D_wrench_space(N) self.wrenchSpace = None self.wrench_space_gl_object = None elif c == '7': self.wrenchSpace6D = self.load_6D_wrench_space() self.wrenchSpace = None self.wrench_space_gl_object = None elif c == '0': robot = self.world.robot(0) q = robot.getConfig() q[0:6] = [0]*6 robot.setConfig(q) vis.setItemConfig("robot",robot.getConfig()) coordinates.updateFromWorld() elif c == '9': vis.edit("robot",False) vis.edit("rigidObject",False) #vis.hide("COM") #object = self.world.rigidObject(0) #vis.edit("COM") else: return False return True
robot = world.robot(0) link = robot.link(7) #coordinates of the constrained IK point on the robot, in the link's local frame localpos = [0.17,0,0] #coordinates of the IK goal in world coordinates position = [0,0,0] t0 = time.time() #draw the world, local point in yellow, and target point in white vis.add("world",world) vis.add("local_point",position) vis.setColor("local_point",1,1,0) vis.add("target",position) vis.setColor("target",1,1,1) vis.edit("target") vis.show() while vis.shown(): vis.lock() #move the target and solve t = time.time()-t0 r = 0.4 position = vis.getItemConfig("target") position[1] = r*math.cos(t) position[2] = 0.7+r*math.sin(t) q = solve_ik(link,localpos,position) robot.setConfig(q) vis.setItemConfig("local_point",link.getWorldPosition(localpos)) vis.setItemConfig("target",position) vis.unlock() time.sleep(0.01)
#if res: # print "Return value",val # endeffectors = val vis.add("world",world) vis.listItems() vis.setColor(("world",world.robot(0).getName()),1,1,0,1) coordinates.setRobotModel(robot) eenames = [robot.link(e).getName() for e in endeffectors] eeobjectives = [] for e in endeffectors: f = coordinates.frame(robot.link(e).getName()) fw = coordinates.addFrame(robot.link(e).getName()+"_tgt",robot.link(e).getTransform()) assert f != None vis.add("ee_"+robot.link(e).getName(),fw) vis.edit("ee_"+robot.link(e).getName()) obj = coordinates.ik_fixed_objective(f) eeobjectives.append(obj) #this tests the cartesian interpolation stuff print "***** BEGINNING CARTESIAN INTERPOLATION TEST *****" vis.setWindowTitle("Klamp't Cartesian interpolation test") vis.pushPlugin(InterpKeyCapture(endeffectors,eeobjectives)) vis.show() while vis.shown(): coordinates.updateFromWorld() time.sleep(0.1) vis.popPlugin() vis.hide("ghost1") vis.hide("ghost2") vis.animate(("world",world.robot(0).getName()),None)
raise RuntimeError("Couldn't read the world file") shelf = make_shelf(w,*shelf_dims) shelf.geometry().translate((shelf_offset_x,shelf_offset_y,shelf_height)) obj = w.makeRigidObject("point_cloud_object") #Making Box obj.geometry().loadFile(KLAMPT_Demo+"/data/objects/apc/genuine_joe_stir_sticks.pcd") #set up a "reasonable" inertial parameter estimate for a 200g object m = obj.getMass() m.estimate(obj.geometry(),0.200) obj.setMass(m) #we'll move the box slightly forward so the robot can reach it obj.setTransform(so3.identity(),[shelf_offset_x-0.05,shelf_offset_y-0.3,shelf_height+0.01]) vis.add("world",w) vis.add("ghost",w.robot(0).getConfig(),color=(0,1,0,0.5)) vis.edit("ghost") from klampt import Simulator sim = Simulator(w) def setup(): vis.show() def callback(): sim.controller(0).setPIDCommand(vis.getItemConfig("ghost"),[0]*w.robot(0).numLinks()) sim.simulate(0.01) sim.updateWorld() vis.loop(setup=setup,callback=callback)
topic = sys.argv[1] #rospy.init_node("Klampt_point_cloud_visualizer") world = klampt.WorldModel() world.makeRigidObject("point_cloud") g = world.rigidObject(0).geometry() g.loadFile("ros:/" + topic) if len(sys.argv) > 2 and sys.argv[2] == 'save': dosave = True else: dosave = False point_cloud_count = 0 vis.add("world", world) vis.edit(("world", "point_cloud")) def updatePointCloud(): #in klampt / robotio.h -- this needs to be done to update ROS processed = io.ProcessStreams() if processed: #don't strictly need the prior if statement, this is just a slight optimization #if the sender is slower than the visualization if g.empty(): print("empty") else: pc = g.getPointCloud() print(pc.numPoints(), "points and", pc.numProperties(), "properties")
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)
def grasp_edit_ui(gripper, object, grasp=None): assert gripper.klampt_model is not None world = WorldModel() res = world.readFile(gripper.klampt_model) if not res: raise ValueError("Unable to load klampt model") robot = world.robot(0) base_link = robot.link(gripper.base_link) base_xform = base_link.getTransform() base_xform0 = base_link.getTransform() parent_xform = se3.identity() if base_link.getParent() >= 0: parent_xform = robot.link(base_link.getParent()).getTransform() if grasp is not None: base_xform = grasp.ik_constraint.closestMatch(*base_xform) base_link.setParentTransform( *se3.mul(se3.inv(parent_xform), base_xform)) robot.setConfig( gripper.set_finger_config(robot.getConfig(), grasp.finger_config)) q0 = robot.getConfig() grob = gripper.get_subrobot(robot) grob._links = [l for l in grob._links if l != gripper.base_link] #set up visualizer oldWindow = vis.getWindow() if oldWindow is None: oldWindow = vis.createWindow() vis.createWindow() vis.add("gripper", grob) vis.edit("gripper") vis.add("object", object) vis.add("base_xform", base_xform) vis.edit("base_xform") def make_grasp(): return Grasp(ik.objective(base_link, R=base_xform[0], t=base_xform[1]), gripper.finger_links, gripper.get_finger_config(robot.getConfig())) #add hooks robot_appearances = [ robot.link(i).appearance().clone() for i in range(robot.numLinks()) ] robot_shown = [True] def toggle_robot(arg=0, data=robot_shown): vis.lock() if data[0]: for i in range(robot.numLinks()): if i not in grob._links and i != gripper.base_link: robot.link(i).appearance().setDraw(False) data[0] = False else: for i in range(robot.numLinks()): if i not in grob._links and i != gripper.base_link: robot.link(i).appearance().set(robot_appearances[i]) data[0] = True vis.unlock() def randomize(): print("TODO") def reset(): vis.lock() robot.setConfig(q0) base_link.setParentTransform( *se3.mul(se3.inv(parent_xform), base_xform0)) vis.unlock() vis.add("base_xform", base_xform0) vis.edit("base_xform") vis.setItemConfig("gripper", grob.getConfig()) def save(): fmt = gripper.name + "_" + object.getName() + '_grasp_%d.json' ind = 0 while os.path.exists(fmt % (ind, )): ind += 1 fn = fmt % (ind, ) g = make_grasp() print("Saving grasp to", fn) with open(fn, 'w') as f: json.dump(g.toJson(), f) vis.addAction(toggle_robot, 'Toggle show robot', 'v') vis.addAction(randomize, 'Randomize', 'r') vis.addAction(reset, 'Reset', '0') vis.addAction(save, 'Save to disk', 's') def loop_setup(): vis.show() def loop_callback(): global base_xform xform = vis.getItemConfig("base_xform") base_xform = (xform[:9], xform[9:]) vis.lock() base_link.setParentTransform( *se3.mul(se3.inv(parent_xform), base_xform)) vis.unlock() def loop_cleanup(): vis.show(False) vis.loop(setup=loop_setup, callback=loop_callback, cleanup=loop_cleanup) # this works with Linux/Windows, but not Mac # loop_setup() # while vis.shown(): # loop_callback() # time.sleep(0.02) # loop_cleanup() g = make_grasp() #restore RobotModel base_link.setParentTransform(*se3.mul(se3.inv(parent_xform), base_xform0)) vis.setWindow(oldWindow) return g
#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]]) #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) vis.edit("some point") #vis.edit("some blinking transform") #vis.edit("coordinates:ATHLETE:ankle roll 3") #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)) vis.addPlot('plot') vis.addPlotItem('plot', 'some point') vis.setPlotDuration('plot', 10.0)
Ra = so3.rotation((0, 1, 0), math.pi * -0.9) Rb = so3.rotation((0, 1, 0), math.pi * 0.9) print("Angle between") print(so3.matrix(Ra)) print("and") print(so3.matrix(Rb)) print("is", so3.distance(Ra, Rb)) Ta = [Ra, [-1, 0, 1]] Tb = [Rb, [1, 0, 1]] if __name__ == "__main__": vis.add("world", se3.identity()) vis.add("start", Ta) vis.add("end", Tb) vis.add("interpolated", Ta) vis.edit("start") vis.edit("end") vis.setAttribute("world", "length", 0.25) vis.setAttribute("interpolated", "fancy", True) vis.setAttribute("interpolated", "width", 0.03) vis.setAttribute("interpolated", "length", 1.0) vis.addText("angle_display", "") vis.show() t0 = time.time() while vis.shown(): #interpolate with a period of 3 seconds period = 3.0 u = ((time.time() - t0) % period) / period T = interpolate_transform(Ta, Tb, u) #uncomment for question 3
print "Input object has type", obj.type(), "with", obj.numElements( ), "elements" geom1 = PenetrationDepthGeometry(obj, gridres, pcres) geom2 = PenetrationDepthGeometry(obj2, gridres, pcres) geom2.setTransform((so3.identity(), [1.2, 0, 0])) vis.add("obj1", geom1.grid) vis.add("obj2", geom2.pc) vis.setColor("obj1", 1, 1, 0, 0.5) vis.setColor("obj2", 0, 1, 1, 0.5) vis.setAttribute("obj2", "size", 3) vis.addPlot("timing") vis.add("transform", se3.identity()) vis.edit("transform") vis.show() oldcps = [] while vis.shown(): vis.lock() t0 = time.time() q = vis.getItemConfig("transform") T = (q[:9], q[9:]) #debug optimization geom1.setTransform(T) Tcollfree, trace, tracetimes, cps = optimizeCollFree(geom1, geom2, T, verbose=0, want_trace=True,
for i in xrange(1,world.numRobots()): for j in xrange(world.robot(i).numLinks()): obstacles.append(world.robot(i).link(j)) for i in xrange(world.numRigidObjects()): obstacles.append(world.rigidObject(i)) #for i in xrange(world.numTerrains()): # obstacles.append(world.terrain(i)) print("%d robots, %d rigid objects, %d terrains"%(world.numRobots(),world.numRigidObjects(),world.numTerrains())) assert len(obstacles) > 0 constraints,pairs = geometryopt.makeCollisionConstraints(robot,obstacles,gridres,pcres) print("Created",len(constraints),"constraints") vis.add("world",world) movableObjects = [] for i in xrange(world.numRigidObjects()): vis.edit(("world",world.rigidObject(i).getName())) movableObjects.append(("world",world.rigidObject(i).getName())) #extract geometries from constraints linkgeoms = [None]*robot.numLinks() obstaclegeoms = [None]*len(obstacles) for c,(link,obj) in zip(constraints,pairs): for i,obs in enumerate(obstacles): if obj is obs: obstaclegeoms[i] = c.env linkgeoms[link.index] = c.robot.geometry[link.index] #assert all(o is not None for o in linkgeoms),"Hm... couldn't find link geometries?" assert all(o is not None for o in obstaclegeoms),"Hm... couldn't find obstacle geometries?" #this recreates the geometries too much #obstaclegeoms = [PenetrationDepthGeometry(obs.geometry(),gridres,pcres) for obs in obstacles]
from klampt import * from klampt import vis import sys if len(sys.argv) <= 1: fn = "../../data/baxter_apc.xml" else: fn = sys.argv[1] w = WorldModel() w.readFile(fn) w.makeRobot("reduced") dofmap = w.robot(1).reduce(w.robot(0)) vis.add("robot", w.robot(0)) vis.add("reduced", w.robot(1), color=(1, 0, 0)) vis.edit("reduced") vis.loop()
qmin, qmax = robot.getJointLimits() for i in range(len(qmin)): if qmax[i] - qmin[i] > math.pi * 2: qmin[i] = -float('inf') qmax[i] = float('inf') robot.setJointLimits(qmin, qmax) qstart = resource.get("start.config", world=world) #add the world elements individually to the visualization vis.add("world", world) vis.add("start", qstart, color=(0, 1, 0, 0.5)) # qgoal = resource.get("goal.config",world=world) qgoal = resource.get("goal_easy.config", world=world) robot.setConfig(qgoal) vis.edit(vis.getItemName(robot)) def planTriggered(): global world, robot qtgt = vis.getItemConfig(vis.getItemName(robot)) qstart = vis.getItemConfig("start") robot.setConfig(qstart) if PROBLEM == '1a': path = planning.feasible_plan(world, robot, qtgt) else: path = planning.optimizing_plan(world, robot, qtgt) if path is not None: ptraj = trajectory.RobotTrajectory(robot, milestones=path) ptraj.times = [t / len(ptraj.times) * 5.0 for t in ptraj.times] #this function should be used for creating a C1 path to send to a robot controller
m.estimate(g, 1.0, 0.0) H = m.getInertia() print(" COM volume-only %.3f %.3f %.3f" % tuple(m.getCom())) print(" Inertia volume-only %.3f %.3f %.3f" % (H[0], H[4], H[8])) a = atypes['GeometricPrimitive'] b = btypes['GeometricPrimitive'] vis.add("A", a) vis.add("B", b) vis.setColor("A", 1, 0, 0, 0.5) vis.setColor("B", 0, 1, 0, 0.5) Ta = se3.identity() Tb = [so3.identity(), [1, 0, 0]] vis.add("Ta", Ta) vis.add("Tb", Tb) vis.edit("Ta") vis.edit("Tb") ray = ([-3, 0, 0], [1, 0, 0]) vis.add("ray", Trajectory([0, 1], [ray[0], vectorops.madd(ray[0], ray[1], 20)]), color=[1, 0.5, 0, 1]) vis.add("hitpt", [0, 0, 0], color=[1, 0, 1, 1]) def convert(geom, type, label): global a, b, atypes, btypes, Ta, Tb if label == 'A': vis.add(label, atypes[type]) vis.setColor(label, 1, 0, 0, 0.5) a = atypes[type]