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)
