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)