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 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)
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
if cindex == 0:
vis.kill()
exit(0)
configs = configs[:cindex]
resource.set("planningtest.configs", configs)
if CLOSED_LOOP_TEST:
#need to project those onto the manifold
for i, q in enumerate(configs):
configs[i] = space.solveConstraints(q)
resource.edit(
"IK solved configs",
configs,
"Configs",
description="These configurations try to solve the IK constraint",
world=world)
#set up a settings dictionary here. This is a random-restart + shortcutting
#SBL planner.
settings = {
configs = resource.get("pathtest.configs",
"Configs",
default=[],
world=world,
doedit=False)
if len(configs) < 2:
print "Invalid # of milestones, need 2 or more"
configs = resource.get("pathtest.configs",
"Configs",
default=[],
world=world,
doedit=True)
if len(configs) < 2:
print "Didn't add 2 or more milestones, quitting"
exit(-1)
resource.set("pathtest.configs", configs)
traj0 = trajectory.RobotTrajectory(robot,
times=range(len(configs)),
milestones=configs)
#show the path in the visualizer, repeating for 60 seconds
traj = trajectory.path_to_trajectory(traj0, speed=1.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "Default values")
vis.addPlot("plot")
vis.addPlotItem("plot", "robot")
#action callbacks
trajinit = trajinit.remesh(times)[0]
print("Trajectory has", len(trajinit.milestones), "milestones")
trajcache.qstart = trajinit.milestones[0][:]
trajcache.qend = trajinit.milestones[-1][:]
print("Time range", trajinit.times[0], trajinit.times[-1])
trajcache.tstart = trajinit.times[0]
trajcache.tend = trajinit.times[-1]
xtraj = trajcache.trajectoryToState(trajinit)
if EDIT_INITIAL_GUESS:
ok, res = resource.edit("Initial trajectory", trajinit, world=world)
if ok:
trajinit = res
trajinit.times[-1] = trajcache.tend
print("Saving to robottrajopt_initial.path")
resource.set('robottrajopt_initial.path', res)
obstaclegeoms = [
geometryopt.PenetrationDepthGeometry(obs.geometry(), None, pcres)
for obs in obstacles
]
ctest2 = geometryopt.RobotTrajectoryCollisionConstraint(
obstaclegeoms, trajcache)
if DEBUG_CONSTRAINTS:
print("Testing link 6 trajectory collision constraint")
ctest = geometryopt.RobotLinkTrajectoryCollisionConstraint(
robot.link(6), obstaclegeoms[0], trajcache)
ctest.setx(xtraj)
res = ctest.minvalue(xtraj)
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)
traj = RobotTrajectory(world.robot(0), range(len(configs)), configs)
print "CSpace stats:"
spacestats = space.getStats()
for k in sorted(spacestats.keys()):
print " ",k,":",spacestats[k]
print "Planner stats:"
planstats = planner.getStats()
for k in sorted(planstats.keys()):
print " ",k,":",planstats[k]
if path:
#save planned milestone path to disk
print "Saving to my_plan.configs"
resource.set("my_plan.configs",path,"Configs")
#visualize path as a Trajectory resource
from klampt.model.trajectory import RobotTrajectory
traj = RobotTrajectory(robot,range(len(path)),path)
#resource.edit("Planned trajectory",traj,world=world)
#visualize path in the vis module
from klampt import vis
vis.add("world",world)
vis.animate(("world",robot.getName()),path)
vis.add("trajectory",traj)
vis.spin(float('inf'))
#play nice with garbage collection
planner.space.close()
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)
