q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return so3.from_moment(m)
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")
visualization.add("point",point)
traj = trajectory.Trajectory()
for i in range(10):
traj.times.append(i)
traj.milestones.append([random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)])
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
visualization.animate("point",traj2)
#add a transform to the visualizer and animate it
xform = visualization.add("xform",se3.identity())
traj3 = trajectory.SE3Trajectory()
for i in range(10):
traj3.times.append(i)
theta = math.acos(q[3]) * 2.0
if abs(theta) < 1e-8:
m = [0, 0, 0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]), theta)
return so3.from_moment(m)
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")
visualization.add("point", point)
traj = trajectory.Trajectory()
for i in range(10):
traj.times.append(i)
traj.milestones.append([
random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)
])
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
visualization.animate("point", traj2)
#add a transform to the visualizer and animate it
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()
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)