def testCartesianDrive():
w = WorldModel()
#w.readFile("../../data/tx90scenario0.xml")
w.readFile("../../data/robots/jaco.rob")
r = w.robot(0)
solver = CartesianDriveSolver(r)
#set a non-singular configuration
q = r.getConfig()
q[3] = 0.5
r.setConfig(q)
solver.start(q, 6)
vis.add("world", w)
vis.addPlot("timing")
vis.addPlot("info")
vis.show()
time.sleep(0.1)
dt = 0.01
t = 0
while t < 20 and vis.shown():
vis.lock()
if t < 2:
v = [0, 0, 0.25]
elif t < 3:
v = [0, 0, -0.1]
elif t < 3.2:
v = [0, 0, -1]
elif t < 8:
v = [0, 0, 0]
elif t < 10:
v = [-1, 0, 0]
else:
v = [1, 0, 0]
if t < 4:
w = [0, 0, 0]
elif t < 10:
w = [0, -0.25, 0]
else:
w = None
t0 = time.time()
progress, qnext = solver.drive(q, w, v, dt)
t1 = time.time()
vis.addText("debug", "Vel %s" % (str(v), ))
vis.logPlot("timing", "t", t1 - t0)
vis.logPlot("info", "progress", progress)
vis.logPlot("info", "adj", solver.driveSpeedAdjustment)
r.setConfig(qnext)
q = qnext
vis.unlock()
vis.add("tgt", solver.driveTransforms[0])
t += dt
time.sleep(max(0.005 - (t1 - t0), 0))
vis.show(False)
vis.clear()
vis.setColor("grid "+str(i),0,1,0,0.5)
vis.hideLabel("grid "+str(i))
#vis.add("pc "+str(i),g.pc)
#vis.setColor("pc "+str(i),1,1,0,0.5)
for i,obs in enumerate(obstaclegeoms):
vis.add("envpc "+str(i),obs.pc)
vis.setColor("envpc "+str(i),1,1,0,0.5)
vis.hideLabel("envpc "+str(i))
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,
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
def setHalfSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=0.5)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
vis.addText("label", "0.5x speed")
def setDoubleSpeed():
traj = trajectory.path_to_trajectory(traj0, speed=2.0)
print "*** Resulting duration", traj.endTime(), "***"
vis.animate("robot", traj)
def modification_template(world):
"""Tests a variety of miscellaneous vis functions"""
vis.add("world",world)
robot = world.robot(0)
vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque
import random
for i in range(10):
#set some links to random colors
randlink = random.randint(0,robot.numLinks()-1)
color = (random.random(),random.random(),random.random())
vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color)
#test the on-screen text display
vis.addText("text2","Here's some red text")
vis.setColor("text2",1,0,0)
vis.addText("text3","Here's bigger text")
vis.setAttribute("text3","size",24)
vis.addText("text4","Transform status")
vis.addText("textbottom","Text anchored to bottom of screen",(20,-30))
#test a point
pt = [2,5,1]
vis.add("some point",pt)
#test a rigid transform
vis.add("some blinking transform",[so3.identity(),[1,3,0.5]])
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test an IKObjective
link = world.robot(0).link(world.robot(0).numLinks()-1)
#point constraint
obj = ik.objective(link,local=[[0,0,0]],world=[pt])
#hinge constraint
#obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]])
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
vis.add("ik objective",obj)
#enable plotting
vis.addPlot('plot')
vis.addPlotItem('plot','some point')
vis.setPlotDuration('plot',10.0)
#run the visualizer, which runs in a separate thread
vis.setWindowTitle("Manual animation visualization test")
class MyCallback:
def __init__(self):
self.iteration = 0
def __call__(self):
vis.lock()
#TODO: you may modify the world here. This line tests a sin wave.
pt[2] = 1 + math.sin(self.iteration*0.03)
vis.unlock()
#changes to the visualization with vis.X functions can done outside the lock
if (self.iteration % 100) == 0:
if (self.iteration / 100)%2 == 0:
vis.hide("some blinking transform")
vis.addText("text4","The transform was hidden")
vis.logPlotEvent('plot','hide')
else:
vis.hide("some blinking transform",False)
vis.addText("text4","The transform was shown")
vis.logPlotEvent('plot','show')
#this is another way of changing the point's data without needing a lock/unlock
#vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True)
#or
#vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)])
if self.iteration == 200:
vis.addText("text2","Going to hide the text for a second...")
if self.iteration == 400:
#use this to remove text
vis.clearText()
if self.iteration == 500:
vis.addText("text2","Text added back again")
vis.setColor("text2",1,0,0)
self.iteration += 1
callback = MyCallback()
if not MULTITHREADED:
vis.loop(callback=callback,setup=vis.show)
else:
vis.show()
while vis.shown():
callback()
time.sleep(0.01)
#use this to remove a plot
vis.remove("plot")
vis.kill()
r.randomizeConfig()
elif config is not None:
if config.endswith('.config'):
q = io.load(config, 'Config')
else:
q = io.read('Config', config)
r.setConfig(q)
vis.add("robot", r)
for i in range(1, w.numRobots()):
vis.add(w.robot(i).getName(), w.robot(i))
for i in range(0, w.numRigidObjects()):
vis.add(w.rigidObject(i).getName(), w.rigidObject(i))
for i in range(0, w.numTerrains()):
vis.add(w.terrain(i).getName(), w.terrain(i))
vis.addPlot("joints")
vis.addPlotItem("joints", "robot")
vis.show()
d0 = [r.driver(i).getValue() for i in range(r.numDrivers())]
qmin, qmax = r.getJointLimits()
index = 0
period = 5
period_start = time.time()
while vis.shown():
t = time.time()
u = (t - period_start) / period
driver = r.driver(index)
if driver.getType() == 'affine':
dmin = float('inf')
#transform constraint
#obj = ik.objective(link,R=link.getTransform()[0],t=pt)
vis.add("ik objective", obj)
vis.edit("some point")
#vis.edit("some blinking transform")
#vis.edit("coordinates:ATHLETE:ankle roll 3")
#test the on-screen text display
vis.addText("text2", "Here's some red text")
vis.setColor("text2", 1, 0, 0)
vis.addText("text3", "Here's bigger text")
vis.setAttribute("text3", "size", 24)
vis.addText("text4", "Transform status")
vis.addText("textbottom", "Text anchored to bottom of screen", (20, -30))
vis.addPlot('plot')
vis.addPlotItem('plot', 'some point')
vis.setPlotDuration('plot', 10.0)
print "Visualization items:"
vis.listItems(indent=2)
vis.autoFitCamera()
print "Starting visualization window..."
#run the visualizer, which runs in a separate thread
vis.setWindowTitle("Basic visualization test")
vis.show()
iteration = 0
while vis.shown():
vis.lock()
