def callback(): if time.time() >= data['next_sim_time']: sim.simulate(0.01) if sim.controller(0).remainingTime() <= 0: print("Executing timed trajectory") trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause') vis.setItemConfig("robot",sim.controller(0).getCommandedConfig()) data['next_sim_time'] += 0.01
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 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 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 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()
Tb = [euler_angle_to_rotation(eb),[1,0,1]] def update_interpolation(u): #linear interpolation with euler angles e = interpolate_euler_angles(ea,eb,u) t = interpolate_linear(Ta[1],Tb[1],u) return (euler_angle_to_rotation(e),t) if __name__ == "__main__": #draw the world reference frame, the start and the goal, and the interpolated frame vis.add("world",se3.identity()) vis.add("start",Ta) vis.add("end",Tb) vis.add("interpolated",Ta) vis.setAttribute("world","length",0.25) vis.setAttribute("interpolated","fancy",True) vis.setAttribute("interpolated","width",0.03) vis.setAttribute("interpolated","length",1.0) period = 3.0 t0 = time.time() vis.show() while vis.shown(): #repeat the interpolation every 3 seconds u = ((time.time()-t0)%period)/period T = update_interpolation(u) vis.setItemConfig("interpolated",T) time.sleep(0.01) vis.kill()
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()
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) if geometryopt.TEST_PYCCD: gx = [c(qcollfree) for c in constraints] else: gx = [c.eval_minimum(qcollfree) for c in constraints] feasible = all([v >= 0 for v in gx]) if feasible: vis.setColor("qsoln",0,1,0,0.5) else: vis.setColor("qsoln",1,0,0,0.5) #initialize the next step from the last solved configuration qinit = qcollfree """ #debug printing
sim.simulate(0) trajectory.execute_path(traj.milestones,sim.controller(0)) vis.setWindowTitle("Simulating path using trajectory.execute_trajectory") if vis.multithreaded(): #for some tricky Qt reason, need to sleep before showing a window again #Perhaps the event loop must complete some extra cycles? time.sleep(0.01) vis.show() t0 = time.time() while vis.shown(): #print "Time",sim.getTime() sim.simulate(0.01) if sim.controller(0).remainingTime() <= 0: print("Executing timed trajectory") trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause') vis.setItemConfig("robot",sim.controller(0).getCommandedConfig()) t1 = time.time() time.sleep(max(0.01-(t1-t0),0.0)) t0 = t1 else: data = {'next_sim_time':time.time()} def callback(): if time.time() >= data['next_sim_time']: sim.simulate(0.01) if sim.controller(0).remainingTime() <= 0: print("Executing timed trajectory") trajectory.execute_trajectory(traj,sim.controller(0),smoothing='pause') vis.setItemConfig("robot",sim.controller(0).getCommandedConfig()) data['next_sim_time'] += 0.01 vis.loop(callback=callback,setup=vis.show) print("Ending vis.")
else: dmin = qmin[driver.getAffectedLink()] dmax = qmax[driver.getAffectedLink()] if dmax - dmin > math.pi * 2: if d0[index] > math.pi: dmin = 0 dmax = math.pi * 2 else: dmin = -math.pi dmax = math.pi uswitch1 = (dmax - d0[index]) / (dmax - dmin) * 0.5 uswitch2 = uswitch1 + 0.5 vis.lock() if u < uswitch1: driver.setValue(d0[index] + u / uswitch1 * (dmax - d0[index])) elif u < uswitch2: driver.setValue(dmax + (u - uswitch1) / 0.5 * (dmin - dmax)) elif u < 1: driver.setValue(dmin + (u - uswitch1 - 0.5) / (1 - uswitch1 - 0.5) * (d0[index] - dmin)) else: driver.setValue(d0[index]) vis.setItemConfig("robot", r.getConfig()) vis.unlock() if u > 1: index += 1 index = index % r.numDrivers() period_start += period time.sleep(0.01) 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
def motionfunc(self,x,y,dx,dy): res = GLPluginInterface.motionfunc(self,x,y,dx,dy) coordinates.updateFromWorld() object = self.world.rigidObject(0) vis.setItemConfig("COM",se3.apply(object.getTransform(),object.getMass().getCom())) return res