def callback(robot=robot):
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t,endBehavior='loop')
robot.setConfig(q)
pass
def animation_template(world):
"""Shows how to animate a robot."""
#first, build a trajectory with 10 random configurations
robot = world.robot(0)
times = range(10)
milestones = []
for t in times:
robot.randomizeConfig()
milestones.append(robot.getConfig())
traj = trajectory.RobotTrajectory(robot, times, milestones)
vis.add("world", world)
robotPath = ("world", world.robot(0).getName()
) #compound item reference: refers to robot 0 in the world
#we're also going to visualize the end effector trajectory
#eetraj = traj.getLinkTrajectory(robot.numLinks()-1,0.05)
#vis.add("end effector trajectory",eetraj)
#uncomment this to automatically visualize the end effector trajectory
vis.add("robot trajectory", traj)
vis.setAttribute("robot trajectory", "endeffectors", [13, 20])
vis.setWindowTitle("Animation test")
MANUAL_ANIMATION = False
if not MANUAL_ANIMATION:
#automatic animation, just call vis.animate
vis.animate(robotPath, traj)
if not MULTITHREADED:
#need to set up references to function-local variables manually, and the easiest way is to use a default argument
def callback(robot=robot):
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
pass
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
if MANUAL_ANIMATION:
#with manual animation, you just set the robot's configuration based on the current time.
t = vis.animationTime()
q = traj.eval(t, endBehavior='loop')
robot.setConfig(q)
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
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()
vis.add("robot", robot)
vis.add("coordinates", coordinates.manager())
vis.show()
iteration = 0
while vis.shown():
vis.lock()
#set the desired position goalpoint to move in a circle
r = 0.4
t = vis.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()
vis.unlock()
time.sleep(0.05)
iteration += 1
#terminate smoothly
vis.kill()
