print "USAGE: vistemplate.py [world_file]"
exit()
#creates a world and loads all the items on the command line
world = WorldModel()
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model " + fn)
coordinates.setWorldModel(world)
#add the world to the visualizer
visualization.add("world", world)
#add the coordinate Manager to the visualizer
visualization.add("coordinates", coordinates.manager())
#test a point
pt = [2, 5, 1]
visualization.add("some point", pt)
#test a rigid transform
visualization.add("some blinking transform", [so3.identity(), [1, 3, 0.5]])
#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)
print "USAGE: vistemplate.py [world_file]"
exit()
#creates a world and loads all the items on the command line
world = WorldModel()
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model "+fn)
coordinates.setWorldModel(world)
#add the world to the visualizer
visualization.add("world",world)
#add the coordinate Manager to the visualizer
visualization.add("coordinates",coordinates.manager())
#test a point
pt = [2,5,1]
visualization.add("some point",pt)
#test a rigid transform
visualization.add("some blinking transform",[so3.identity(),[1,3,0.5]])
#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)
visualization.add("ik objective",obj)
if __name__ == "__main__":
world = WorldModel()
res = world.readFile("ex2_file.xml")
if not res: raise RuntimeError("Unable to load world file")
linkindex = 7
localpos = (0.17,0,0)
robot = world.robot(0)
link = robot.link(linkindex)
coordinates.setWorldModel(world)
goalpoint = [0,0,0]
ptlocal = coordinates.addPoint("ik-constraint-local",localpos,robot.getName()+":"+link.getName())
ptworld = coordinates.addPoint("ik-constraint-world",goalpoint,"world")
print coordinates.manager().frames.keys()
visualization.add("robot",robot)
visualization.add("coordinates",coordinates.manager())
visualization.show()
iteration = 0
while visualization.shown():
visualization.lock()
#set the desired position goalpoint to move in a circle
r = 0.4
t = visualization.animationTime()
goalpoint[0],goalpoint[1],goalpoint[2] = 0.8,r*math.cos(t),0.7+r*math.sin(t)
q = solve_ik(link,localpos,goalpoint)
robot.setConfig(q)
#this updates the coordinates module
coordinates.updateFromWorld()
