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)
