def test_trajectory_editing(): traj = SE3Trajectory([0, 1], [se3.identity(), se3.identity()]) saved, result = resource.edit("se3 traj", traj, 'SE3Trajectory', 'Some spatial trajectory', world=w, referenceObject=r.link(7)) traj = Trajectory([0, 1], [[0, 0, 0], [0, 0, 1]]) saved, result = resource.edit("point traj", traj, 'auto', 'Some point trajectory', world=w, referenceObject=r.link(7)) traj = RobotTrajectory(r, [0, 1, 2], [q0, qrand, qrand2]) saved, result = resource.edit("robot traj", traj, 'auto', 'Random robot trajectory', world=w, referenceObject=r) vis.kill()
def xform_edit_template(world): """Testing the transform editor.""" #The first call doesn't attach any geometry to the frame. The second call attaches geometry to it. #Both return the edited transform *relative* to the link. link = world.robot(0).link(5) xform = resource.edit(name="Base link, floating", value=None, type='RigidTransform', frame=link.getTransform(), world=world) xform = resource.edit(name="Base link, should be relative to robot", value=None, type='RigidTransform', frame=link.getTransform(), referenceObject=link, world=world) link = world.robot(0).link(8) xform = resource.edit(name="Leg link", value=None, type='RigidTransform', frame=link.getTransform(), world=world) xform = resource.edit(name="Leg link, should be relative to robot", value=None, type='RigidTransform', frame=link.getTransform(), referenceObject=link, world=world)
def test_geometry_editing(): g = GeometricPrimitive() g.setPoint([-0.5, 0, 1]) saved, result = resource.edit("Point", g, world=w) print(result) g.setSphere([-0.5, 0, 1], 0.3) saved, result = resource.edit("Sphere", g, world=w) print(result) g.setAABB([-0.5, 0, 1], [-0.2, 0.1, 1.3]) saved, result = resource.edit("AABB", g, world=w) print(result) g.setBox([-0.5, 0, 1], so3.identity(), [0.4, 0.4, 0.4]) saved, result = resource.edit("Box", g, world=w) print(result) vis.kill()
def onCreateIndexChanged(self, item): if item == 0: return type = create_types[item - 1] robot = None if self.world.numRobots() > 0: robot = self.world.robot(0) try: (save, obj) = resource.edit("untitled", types.make(type, robot), type=type, world=self.world) except Exception as e: print("klampt_browser: Exception raised during resource.edit():", e) QtWidgets.QMessageBox.warning( self.splitter, "Creation not available", "Unable to create item of type " + type + ", did you remember to add items to the reference world?") return if obj is not None and save: fn = resource.save(obj, type, directory='') if fn is not None: self.loadedItem(fn, obj) #TODO: should we add to selection in tree view? self.createComboBox.setCurrentIndex(0)
def doedit(): print("klampt_browser: Launching resource.edit",fn,"...") try: (save,obj) = resource.edit(name=fn,value=self.active[fn].obj,world=self.world) except Exception as e: print("klampt_browser: Exception raised during resource.edit:",e) QtWidgets.QMessageBox.warning(self.splitter,"Editing not available","Unable to edit item of type "+self.active[fn].obj.__class__.__name__) return if save and obj is not None: self.active[fn].obj = obj #mark it as modified and re-add it to the visualization basename = os.path.basename(fn) self.active[fn].plugin.add(basename,obj) self.modified.add(fn) self.saveButton.setEnabled(True)
def config_edit_template(world): """Shows how to edit Config, Configs, and Trajectory resources""" config1 = resource.get("resourcetest1.config", description="First config, always edited", doedit=True, editor='visual', world=world) print("Config 1:", config1) config2 = resource.get("resourcetest1.config", description="Trying this again...", editor='visual', world=world) print("Config 2:", config2) config3 = resource.get("resourcetest2.config", description="Another configuration", editor='visual', world=world) print("Config 3:", config3) if config3 != None: config3hi = config3[:] config3hi[3] += 1.0 resource.set("resourcetest3_high.config", config3hi) world.robot(0).setConfig(config3hi) configs = [] if config1 != None: configs.append(config1) if config2 != None: configs.append(config2) if config3 != None: configs.append(config3) print("Configs resource:", configs) configs = resource.get("resourcetest.configs", default=configs, description="Editing config sequence", doedit=True, editor='visual', world=world) if configs is None: print("To edit the trajectory, press OK next time") else: traj = RobotTrajectory(world.robot(0), list(range(len(configs))), configs) traj = resource.edit(name="Timed trajectory", value=traj, description="Editing trajectory", world=world)
def user_set_config(q, type_str="<..>"): """ :param q: :param type_str: (Default value = "<..>") """ save = None # it's worthwhile to make sure that it's feasible while q is None or not self.cc.feasible(q): print("=" * 20) print("=" * 20) print(type_str, q) print(type_str + " configuration isn't feasible") save, q = resource.edit( type_str + " config", q, "Config", world=self.cc.world ) q = self.cc.translate_from_klampt(q) return save, q
qmin,qmax = robot.getJointLimits() q0 = robot.getConfig() for i in range(robot.numLinks()): if math.isinf(qmin[i]) or math.isinf(qmax[i]): #don't animate floating base continue traj.times.append(len(traj.times)*0.5) q = q0[:] q[i] = qmin[i] traj.milestones.append(q) traj.times.append(len(traj.times)*0.5) q[i] = qmax[i] traj.milestones.append(q) save,traj.milestones = resource.edit("trajectory",traj.milestones,world=world) vis.animate("robot",traj) #pop up the window to show the trajectory vis.spin(float('inf')) if len(sys.argv)>1: vis.animate("robot",None) sim = Simulator(world) 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)
def launchdialog(): resource.edit("object transform launched from window", world.rigidObject(0).getTransform(), world=world)
from klampt import * from klampt import vis from klampt.io import resource import time print("Tests threading after dialog") world = WorldModel() world.readFile('../../data/objects/block.obj') resource.edit("object transform", world.rigidObject(0).getTransform(), world=world) def launchdialog(): resource.edit("object transform launched from window", world.rigidObject(0).getTransform(), world=world) def launchwindow(): origwindow = vis.getWindow() vis.createWindow("Pop up window") vis.add("world2", world) vis.show() vis.setWindow(origwindow) print("Now running show() (only works on multithreaded systems, not mac)") vis.add("world", world) vis.addAction(launchdialog, "Launch a dialog", "d")
def load_save_template(world): """Shows the load/save functions. First loads a Config resource, then ask you to save the edited config""" fn, q = resource.load('Config') resource.edit('loaded config', q, world=world) resource.save(q)
from klampt.plan import cspace, robotplanning from klampt.io import resource import time world = klampt.WorldModel() world.readFile("Klampt-examples/data/tx90soft.xml") robot = world.robot(0) # this is the CSpace that will be used. Standard collision and joint limit constraints # will be checked space = robotplanning.makeSpace(world, robot, edgeCheckResolution=0.05) # fire up a visual editor to get some start and goal configurations qstart = robot.getConfig() qgoal = robot.getConfig() save, qstart = resource.edit("Start config", qstart, "Config", world=world) # it's worthwile to make sure that it's feasible while save and not space.feasible(qstart): print( "Start configuration isn't feasible, please pick one that is collision-free" ) save, qstart = resource.edit("Start config", qstart, "Config", world=world) save, qgoal = resource.edit("Goal config", qgoal, "Config", world=world) while save and not space.feasible(qgoal): print( "Goal configuration isn't feasible, please pick one that is collision-free" ) save, qgoal = resource.edit("Goal config", qgoal, "Config", world=world) settings = {
configs = [] if config1 != None: configs.append(config1) if config2 != None: configs.append(config2) if config3 != None: configs.append(config3) print "Configs resource:", configs configs = resource.get("resourcetest.configs", default=configs, description="Editing config sequence", doedit=True, editor='visual', world=world) traj = RobotTrajectory(world.robot(0), range(len(configs)), configs) traj = resource.edit(name="Timed trajectory", value=traj, description="Editing trajectory", world=world) #testing transform editor. The first call doesn't attach any geometry to the frame. The second call attaches geometry to it. #Both return the edited transform *relative* to the base link. xform = resource.edit(name="Base link, floating", value=None, type='RigidTransform', frame=world.robot(0).link(5).getTransform(), world=world) xform = resource.edit(name="Base link, should be attached to robot", value=None, type='RigidTransform', frame=world.robot(0).link(5), world=world)
from klampt.plan import robotplanning,cspace from klampt.io import resource import time world = WorldModel() world.readFile("/home/motion/Klampt-examples/data/tx90cuptable.xml") robot = world.robot(0) #this is the CSpace that will be used. Standard collision and joint limit constraints #will be checked space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05) #fire up a visual editor to get some start and goal configurations qstart = robot.getConfig() qgoal = robot.getConfig() save,qstart = resource.edit("Start config",qstart,"Config",world=world) #it's worthwile to make sure that it's feasible while save and not space.feasible(qstart): print "Start configuration isn't feasible, please pick one that is collision-free" save,qstart = resource.edit("Start config",qstart,"Config",world=world) save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world) while save and not space.feasible(qgoal): print "Goal configuration isn't feasible, please pick one that is collision-free" save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world) settings = {'type':'rrt', 'perturbationRadius':0.25, 'bidirectional':True, 'shortcut':True, 'restart':True,
trajcache.qstart[1] = 1.0 trajcache.qstart[2] = 1.8 trajcache.qend = q0[:] trajcache.qend[1] = -1.0 trajcache.qend[2] = 1.8 trajcache.qstart = [ 0.0, 0.2399999999999998, 0.03999999999999922, -0.8000000000000002, -0.7400000000000002, -1.6000000000000005, 0.0 ] trajcache.qend = [ 0.0, -1.3000000000000003, 1.6199999999999997, 0.6999999999999997, 2.5200000000000014, -1.6000000000000008, 0.0 ] if EDIT_ENDPOINTS: ok, res = resource.edit("Trajectory endpoints", [trajcache.qstart, trajcache.qend], world=world) if ok: trajcache.qstart, trajcache.qend = res print(trajcache.qstart) print(trajcache.qend) xtraj = trajcache.straightLineState() trajinit = trajcache.stateToTrajectory(xtraj) else: trajinit = RobotTrajectory(robot, trajinit.times, trajinit.milestones) if MORE_SUBDIVISION > 1: times = [] for i in range(len(trajinit.times) - 1): t0 = trajinit.times[i] t1 = trajinit.times[i + 1] for k in range(MORE_SUBDIVISION):
print("#########################################") print("Editing the waypoints") print("#########################################") configs = resource.get("planningtest.configs", "Configs", default=[], world=world, doedit=False) cindex = 0 while True: if cindex < len(configs): robot.setConfig(configs[cindex]) (save, q) = resource.edit("plan config " + str(cindex + 1), robot.getConfig(), "Config", world=world, description="Press OK to add waypoint, cancel to stop") if save: if False and not space.feasible(q): print("Configuration is infeasible. Failures:") print(" ", space.cspace.feasibilityFailures(q)) print("Please pick another") else: if cindex >= len(configs): configs.append(q) else: configs[cindex] = q cindex += 1 else: break
exit(1) if world.numRobots() == 0: print("Must specify a robot") exit(1) robot = world.robot(0) print("%d robots, %d rigid objects, %d terrains" % (world.numRobots(), world.numRigidObjects(), world.numTerrains())) #raw_input("Press enter to continue... ") vis.add("world", world) q0 = robot.getConfig() qtarget = robot.getConfig() configs = [q0, qtarget] save, result = resource.edit('start and goal', configs, 'Configs', world=world) configs = result vis.add("start", configs[0]) vis.add("target", configs[1]) vis.setColor("start", 0, 1, 0, 0.5) vis.setColor("target", 1, 0, 0, 0.5) print("Initializing geometries for semi-infinite optimization") kinematicscache = geometryopt.RobotKinematicsCache(robot, gridres, pcres) if DUMP_SDF: for i in xrange(robot.numLinks()): fn = 'output/' + robot.link(i).getName() + '.mat' print("Saving SDF to", fn) geometryopt.dump_grid_mat(kinematicscache.geometry[i].grid, fn)