from klampt.io import resource
from klampt.model import coordinates
from klampt.math import vectorops,so3,se3
from klampt.model import trajectory
import random
import time
import math
if __name__ == "__main__":
print("trajectorytest.py: This example demonstrates several types of trajectory")
if len(sys.argv)<=1:
print("USAGE: trajectorytest.py [robot or world file]")
print("With no arguments, shows some 3D trajectories")
#add a point to the visualizer and animate it
point = coordinates.addPoint("point")
vis.add("point",point)
traj = trajectory.Trajectory()
x = -2
for i in range(10):
traj.times.append(i)
traj.milestones.append([x + random.uniform(0.1,0.3),0,random.uniform(-1,1)])
x = traj.milestones[-1][0]
traj2 = trajectory.HermiteTrajectory()
traj2.makeMinTimeSpline(traj.milestones,vmax=[2,2,2],amax=[4,4,4])
vis.add("point minTime",traj2.discretize(0.05),color=(0,0,1,1))
traj3 = trajectory.HermiteTrajectory()
traj3.makeMinTimeSpline(traj.milestones,[[0,0,0]]*len(traj.milestones),vmax=[2,2,2],amax=[8,8,8])
for m in traj3.milestones:
return q
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()
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)