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 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 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 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 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 traj = trajectory.path_to_trajectory(ptraj,timing='robot',smoothing=None) #show the path in the visualizer, repeating for 60 seconds vis.animate("start",traj) vis.add("traj",traj,endeffectors=[9])
def rotate(self,radians): robot = self.world.robot(0) object = self.world.rigidObject(0) robot.setConfig(vis.getItemConfig("robot")) T = vis.getItemConfig("rigidObject") object.setTransform(T[:9],T[9:]) self.angle += radians T = (so3.rotation((0,1,0),radians),[0.0]*3) Tbase = moving_base.get_moving_base_xform(robot) moving_base.set_moving_base_xform(robot,*se3.mul(T,Tbase)) Tobj = object.getTransform() object.setTransform(*se3.mul(T,Tobj)) #update visualization vis.setItemConfig("robot",robot.getConfig()) vis.setItemConfig("rigidObject",sum(object.getTransform(),[])) vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom())) coordinates.updateFromWorld() self.refresh()
def goToWidget(): #first, set all constraints so they are fit at the robot's last solved configuration, and get the #current workspace coordinates vis.setItemConfig("ghost1", self.goalConfig) self.robot.setConfig(self.goalConfig) for e, d in zip(self.endeffectors, self.constraints): d.matchDestination(*self.robot.link(e).getTransform()) wcur = config.getConfig(self.constraints) wdest = [] for e in self.endeffectors: xform = vis.getItemConfig("ee_" + robot.link(e).getName()) wdest += xform print "Current workspace coords", wcur print "Dest workspace coords", wdest #Now interpolate self.robot.setConfig(self.goalConfig) traj1 = cartesian_trajectory.cartesian_interpolate_linear( robot, wcur, wdest, self.constraints, delta=1e-2, maximize=False) self.robot.setConfig(self.goalConfig) traj2 = cartesian_trajectory.cartesian_interpolate_bisect( robot, wcur, wdest, self.constraints, delta=1e-2) self.robot.setConfig(self.goalConfig) #traj3 = cartesian_trajectory.cartesian_path_interpolate(robot,[wcur,wdest],self.constraints,delta=1e-2,method='any',maximize=False) traj3 = None print "Method1 Method2 Method3:" print " ", (traj1 != None), (traj2 != None), (traj3 != None) if traj1: traj = traj1 elif traj2: traj = traj2 elif traj3: traj = traj3 else: traj = cartesian_trajectory.cartesian_interpolate_linear( robot, wcur, wdest, self.constraints, delta=1e-2, maximize=True) if traj: print "Result has", len(traj.milestones), "milestones" self.goalConfig = traj.milestones[-1] vis.setItemConfig(("world", world.robot(0).getName()), self.goalConfig) vis.animate(("world", world.robot(0).getName()), traj, speed=0.2, endBehavior='loop') vis.setItemConfig("ghost2", traj.milestones[-1]) vis.add("ee_trajectory", traj) self.refresh()
def bumpTrajectory(): relative_xforms = [] robot.setConfig(self.refConfig) for e in self.endeffectors: xform = vis.getItemConfig("ee_"+robot.link(e).getName()) T = (xform[:9],xform[9:]) T0 = robot.link(e).getTransform() Trel = se3.mul(se3.inv(T0),T) print "Relative transform of",e,"is",Trel relative_xforms.append(Trel) bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,self.traj,self.constraints,relative_xforms,ee_relative=True,closest=True) assert bumpTraj != None vis.animate(("world",world.robot(0).getName()),bumpTraj) self.refresh()
def load_config(self,name): object = self.world.rigidObject(0) self.config_name = name print "Loading from",name+".config,",name+".xform","and",name+".array" qrob = resource.get(name+".config",type="Config",doedit=False) Tobj = resource.get(name+".xform",type="RigidTransform",doedit=False) self.contact_units = resource.get(name+"_units.array",type="IntArray",default=[],doedit=False) if len(self.contact_units) == 0: print "COULDNT LOAD "+name+"_units.array, trying "+name+".array" contact_links = resource.get(name+".array",type="IntArray",default=[],doedit=False) if len(contact_links) > 0: robot = self.world.robot(0) contact_links = [robot.link(l).getName() for l in contact_links] self.contact_units = [] for i,(link,unit) in enumerate(self.hand.all_units): if link in contact_links: self.contact_units.append(i) print "UNITS",self.contact_units object.setTransform(*Tobj) qobj = vis.getItemConfig("rigidObject") vis.setItemConfig("robot",qrob) vis.setItemConfig("rigidObject",qobj) vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom())) coordinates.updateFromWorld()
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) vis.kill()
settings = None settings = SemiInfiniteOptimizationSettings() #if you use qinit = random-collision-free, you'll want to set this higher settings.max_iters = 5 settings.minimum_constraint_value = -0.02 vis.addPlot("timing") vis.show() oldcps = [] while vis.shown(): vis.lock() t0 = time.time() for path in movableObjects: q = vis.getItemConfig(path) T = (q[:9],q[9:]) for c,p in zip(constraints,pairs): if p[1].getName() == path[1]: c.env.setTransform(T) q0 = robot.getConfig() robot.setConfig(qinit) #qcollfree,trace,cps = geometryopt.optimizeCollFreeRobot(robot,obstacles,constraints=constraints,qinit='random-collision-free',qdes=q0,verbose=VERBOSE,settings=settings, # want_trace=True,want_times=False,want_constraints=True) qcollfree,trace,cps = geometryopt.optimizeCollFreeRobot(robot,obstacles,constraints=constraints,qinit=None,qdes=q0,verbose=VERBOSE,settings=settings, want_trace=True,want_times=False,want_constraints=True) #vis.add("transformTrace",trace) assert len(qcollfree) == robot.numLinks() #robot.setConfig(qcollfree) vis.setItemConfig("qsoln",qcollfree)
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)
prim2 = GeometricPrimitive() prim2.setPolygon([0,0,1.2]+[0.5,0,1.2]+[0.5,0.5,1.2]+[0,0.5,1.2]) vis.add("prim2",prim2,color=[0.5,1,0.5]) prim3 = GeometricPrimitive() prim3.setBox([0,0,1.5],so3.rotation([0,1,0],math.radians(10)),[0.5,0.5,0.5]) vis.add("prim3",prim3,color=[1,0.5,0.5]) """ counter = 0 Ntimes = 30 last_cycle_times = deque() last_query_times = deque() t0 = time.time() vis.show() while vis.shown(): qa = vis.getItemConfig("Ta") qb = vis.getItemConfig("Tb") config.setConfig(Ta, qa) config.setConfig(Tb, qb) vis.lock() a.setCurrentTransform(*Ta) b.setCurrentTransform(*Tb) vis.unlock() tq0 = time.time() if mode == 'collision': coll = a.collides(b) tq1 = time.time() if coll: vis.setColor('B', 1, 1, 0, 0.5) else: vis.setColor('B', 0, 1, 0, 0.5)
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, want_times=True, want_constraints=True) traceTraj = SE3Trajectory(range(len(trace)), trace) vis.add("transformTrace", traceTraj) for i in oldcps: vis.hide(i)
def callback(): sim.controller(0).setPIDCommand(vis.getItemConfig("ghost"),[0]*w.robot(0).numLinks()) sim.simulate(0.01) sim.updateWorld()
def save_config(self,name): self.config_name = name print "Saving to",name+".config,",name+".xform","and",name+".array" resource.set(name+".config",type="Config",value=vis.getItemConfig("robot")) resource.set(name+".xform",type="RigidTransform",value=self.world.rigidObject(0).getTransform()) resource.set(name+"_units.array",type="IntArray",value=self.contact_units)