def make(robot, file="mypath.path", ff_torque_file=None):
if robot == None:
l = trajectory.Trajectory()
else:
l = trajectory.RobotTrajectory(robot)
l.load(file)
if ff_torque_file is not None:
tcmd = trajectory.Trajectory()
tcmd.load(ff_torque_file)
return TrajectoryWithFeedforwardTorqueController(l, ff_torque_file)
return TrajectoryPositionController(l)
def get_trajectory(self):
"""
Calculates a trajectory between two waypoints on the torso. The waypoints are extracted from a grid on the torso.
The first waypoint is given at time t=0, and the second waypoint is given at t=1.
If self.deterministic_trajectory is true, a deterministic trajectory along the x-axis of the torso is calculated.
Args:
Returns:
(klampt.model.trajectory Object): trajectory
"""
grid = self._get_torso_grid()
if self.deterministic_trajectory:
start_point = [0.062, -0.020, 0.896]
end_point = [-0.032, -0.075, 0.896]
#start_point = [grid[0, 0], grid[1, 4], self._torso_xpos[-1] + self.top_torso_offset]
#end_point = [grid[0, int(self.grid_pts / 2) - 1], grid[1, 5], self._torso_xpos[-1] + self.top_torso_offset]
else:
start_point = self._get_waypoint(grid)
end_point = self._get_waypoint(grid)
milestones = np.array([start_point, end_point])
self.num_waypoints = np.size(milestones, 0)
return trajectory.Trajectory(milestones=milestones)
def visualize_cost(x, y, cost, name, with_z=0, hm=None, color=None, hide_label=False):
traj = trajectory.Trajectory(milestones=[[x, y, with_z], [x, y, with_z + cost]])
vis.add(name, traj)
if color:
VisUtils.set_color(name, color)
if hide_label:
vis.hideLabel(name)
def make(robot, file="mypath.path"):
if robot == None:
l = trajectory.Trajectory()
else:
l = trajectory.RobotTrajectory(robot)
l.load(file)
return TrajectoryController(l)
def sphere_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = sphere_staggered_grid(N)
else:
G = sphere_grid(N)
for n in G.nodes():
x = G.node[n]['params']
vis.add(str(n), x)
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
xi = G.node[i]['params']
xj = G.node[j]['params']
vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj]))
vis.hideLabel(str(i) + '-' + str(j))
dist = vectorops.distance(xi, xj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
def visualize_line(p1, p2, name="default_line", add_z=0, color=None, hm=None):
if len(p1) < 3:
p1 = [p1[0], p1[1], 0]
if len(p2) < 3:
p2 = [p2[0], p2[1], 0]
p1 = [p1[0], p1[1], p1[2] + add_z]
p2 = [p2[0], p2[1], p2[2] + add_z]
traj = trajectory.Trajectory(milestones=[p1[0:3], p2[0:3]])
vis.add(name, traj)
if color:
VisUtils.set_color(name, color)
def visualize_in_klampt(self):
from klampt import vis
height = 0.25
for xyz in self.scatter:
name = f"{xyz[0]}{xyz[1]}{xyz[2]}"
traj = trajectory.Trajectory(
milestones=[[xyz[0], xyz[1], xyz[2] +
0.001], [xyz[0], xyz[1], xyz[2] + height]])
vis.add(name, traj)
vis.hideLabel(name)
for i in range(len(self.scatter) - 4, len(self.scatter)):
xyz = self.scatter[i]
name = f"{xyz[0]}{xyz[1]}{xyz[2]}_2"
traj = trajectory.Trajectory(
milestones=[[xyz[0], xyz[1], xyz[2] +
0.001], [xyz[0], xyz[1], xyz[2] + 1]])
vis.add(name, traj)
vis.setColor(name, 1, 0, 0, 1)
def visualize(self, arc_step=15):
milestones = []
for i in range(0, 361, arc_step):
x = self.x + self.R * np.cos(np.deg2rad(i))
y = self.y + self.R * np.sin(np.deg2rad(i))
try:
z = self.height_map.height_at_xy(x, y)
except ValueError:
z = 0
milestones.append([x, y, z + 0.01])
circle = trajectory.Trajectory(milestones=milestones)
if not self.name:
self.name = "circle " + str(random.randint(1, 1000))
vis.add(self.name, circle)
def basic_template(world):
"""Shows how to pop up a visualization window with a world"""
#add the world to the visualizer
vis.add("world", world)
#adding a point
vis.add("point", [1, 1, 1])
vis.setColor("point", 0, 1, 0)
vis.setAttribute("point", "size", 5.0)
#adding lines is currently not super convenient because a list of lists is treated as
#a Configs object... this is a workaround to force the vis module to treat it as a polyline.
vis.add("line", trajectory.Trajectory([0, 1], [[0, 0, 0], [1, 1, 1]]))
vis.setAttribute("line", "width", 1.0)
sphere = klampt.GeometricPrimitive()
sphere.setSphere([1.5, 1, 1], 0.2)
vis.add("sphere", sphere)
vis.setColor("sphere", 0, 0, 1, 0.5)
box = klampt.GeometricPrimitive()
box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2])
g = klampt.Geometry3D(box)
vis.add("box", g)
vis.setColor("box", 0, 0, 1, 0.5)
vis.setWindowTitle("Basic visualization test")
if not MULTITHREADED:
print("Running vis loop in single-threaded mode with vis.loop()")
#single-threaded code
def callback():
#TODO: you may modify the world here.
pass
vis.loop(setup=vis.show, callback=callback)
else:
print("Running vis loop in multithreaded mode")
#multithreaded code
vis.show()
while vis.shown():
vis.lock()
#TODO: you may modify the world here. Do not modify the internal state of any
#visualization items outside of the lock
vis.unlock()
#outside of the lock you can use any vis.X functions, including vis.setItemConfig()
#to modify the state of objects
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def visualize_circle(x0, y0, r, name="default_circle", hm=None, color=None, arc_step=15):
milestones = []
for i in range(0, 361, arc_step):
x = x0 + r * np.cos(np.deg2rad(i))
y = y0 + r * np.sin(np.deg2rad(i))
if hm:
z = hm.height_at_xy(x, y)
else:
z = 0
milestones.append([x, y, z + 0.01])
circle = trajectory.Trajectory(milestones=milestones)
vis.add(name, circle)
if color:
VisUtils.set_color(name, color)
def debug_plan_results(plan, robot):
"""Potentially useful for debugging planning results..."""
assert isinstance(plan, MotionPlan)
#this code just gives some debugging information. it may get expensive
V, E = plan.getRoadmap()
print(len(V), "feasible milestones sampled,", len(E), "edges connected")
print("Plan stats:")
pstats = plan.getStats()
for k in sorted(pstats.keys()):
print(" ", k, ":", pstats[k])
print("CSpace stats:")
sstats = plan.space.getStats()
for k in sorted(sstats.keys()):
print(" ", k, ":", sstats[k])
"""
print(" Joint limit failures:")
for i in range(robot.numLinks()):
print(" ",robot.link(i).getName(),":",plan.space.ambientspace.joint_limit_failures[i])
"""
path = plan.getPath()
if path is None or len(path) == 0:
print("Failed to plan path between configuration")
#debug some sampled configurations
numconfigs = min(10, len(V))
vis.debug("some milestones", V[2:numconfigs], world=world)
pts = []
for i, q in enumerate(V):
robot.setConfig(q)
pt = robot.link(9).getTransform()[1]
pts.append(pt)
for i, q in enumerate(V):
vis.add("pt" + str(i),
pts[i],
hide_label=True,
color=(1, 1, 0, 0.75))
for (a, b) in E:
vis.add("edge_{}_{}".format(a, b),
trajectory.Trajectory(milestones=[pts[a], pts[b]]),
color=(1, 0.5, 0, 0.5),
width=1,
pointSize=0,
hide_label=True)
return None
print("Planned path with length",
trajectory.RobotTrajectory(robot, milestones=path).length())
def visualize_in_klampt(self, height_map, step=10):
for x_inx in range(0, self.x_indices - 1, step):
for y_inx in range(0, self.y_indices - 1, step):
x_world = self.get_x_from_xindex(x_inx)
y_world = self.get_y_from_yindex(y_inx)
cost = self.np_cmap_arr[x_inx][y_inx]
name = str(x_world) + "," + str(y_world)
if cost > 0.001:
base_z = height_map.height_at_xy(x_world, y_world)
traj = trajectory.Trajectory(
milestones=[[x_world, y_world, base_z +
0.001], [x_world, y_world, base_z +
cost]])
vis.add(name, traj)
vis.hideLabel(name)
def visualize_dashed_line(self, xyz1, xyz2, name, dashes):
delta_x = xyz2[0] - xyz1[0]
delta_y = xyz2[1] - xyz1[1]
delta_z = xyz2[2] - xyz1[2]
dx = delta_x / (2.0 * float(dashes))
dy = delta_y / (2.0 * float(dashes))
dz = delta_z / (2.0 * float(dashes))
for i in range(int(dashes)):
h = i * 2
p1 = [xyz1[0] + h * dx, xyz1[1] + h * dy, xyz1[2] + h * dz]
p2 = [xyz1[0] + (h + 1) * dx, xyz1[1] + (h + 1) * dy, xyz1[2] + (h + 1) * dz]
traj = trajectory.Trajectory(milestones=[p1, p2])
vis.add((name + str(h)), traj)
def visualize_in_klampt(self, step=5, xmin=4, xmax=12, ymin=8, ymax=12):
"""Visualize the height map using klampt"""
for x_inx in range(self.get_xindex_from_x(xmin),
self.get_xindex_from_x(xmax), step):
for y_inx in range(self.get_yindex_from_y(ymin),
self.get_yindex_from_y(ymax), step):
x_world = self.get_x_from_xindex(x_inx)
y_world = self.get_y_from_yindex(y_inx)
z = self.np_arr[x_inx][y_inx]
if z > 0:
name = str(x_world) + ", " + str(y_world)
traj = trajectory.Trajectory(
milestones=[[x_world, y_world, 0],
[x_world, y_world, z]])
vis.add(name, traj)
vis.hideLabel(name)
def get_trajectory(self):
"""
Calculates a trajectory between two waypoints on the torso. The waypoints are extracted from a grid on the torso.
The first waypoint is given at time t=0, and the second waypoint is given at t=1.
Args:
Returns:
(klampt.model.trajectory Object): trajectory
"""
start_point = [0.3, 0, 0.295]
end_point = [0.5, -0.2, 0.295]
milestones = np.array([start_point, end_point])
self.num_waypoints = np.size(milestones, 0)
return trajectory.Trajectory(milestones=milestones)
def visualize_dashed_line(xyz1, xyz2, name, dashes, color=None):
delta_x = xyz2[0] - xyz1[0]
delta_y = xyz2[1] - xyz1[1]
delta_z = xyz2[2] - xyz1[2]
dx = delta_x / (2.0 * dashes)
dy = delta_y / (2.0 * dashes)
dz = delta_z / (2.0 * dashes)
for i in range(dashes):
h = i * 2
p1 = [xyz1[0] + h * dx, xyz1[1] + h * dy, xyz1[2] + h * dz]
p2 = [xyz1[0] + (h + 1) * dx, xyz1[1] + (h + 1) * dy, xyz1[2] + (h + 1) * dz]
traj = trajectory.Trajectory(milestones=[p1, p2])
vis.add((name + str(h)), traj)
if color:
VisUtils.set_color(name + str(h), color)
else:
VisUtils.set_color(name + str(h), VisUtils.WHITE)
def visRoadMap(self):
vis.show()
edgeList = self.G.edges()
cnt = 0
for e in edgeList:
n1 = e[0]
n2 = e[1]
tr = trajectory.Trajectory()
tr.milestones.append([n1.x, n1.y, n1.z])
tr.milestones.append([n2.x, n2.y, n2.z])
fName = "a" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setAttribute(fName, "width", 0.5)
vis.setColor(fName, 0.4940, 0.1840, 0.5560)
cnt = cnt + 1
self.rmCnt = cnt
def visualize_xyz_list(xyz, name="defaultname", height_map=None, loop=False, color=None):
milestones = []
for xy_y in xyz:
x = xy_y[0]
y = xy_y[1]
if height_map:
milestones.append([x, y, height_map.height_at_xy(x, y) + 0.05])
else:
milestones.append([x, y, 0])
if loop:
if height_map:
milestones.append(
[xyz[0][0] + 0.0001, xyz[0][1] + 0.0001, height_map.height_at_xy(xyz[0][0], xyz[0][1]) + 0.05]
)
else:
milestones.append([xyz[0][0] + 0.0001, xyz[0][1] + 0.0001, 0.001])
path = trajectory.Trajectory(milestones=milestones)
vis.add(name, path)
if color:
VisUtils.set_color(name, color)
def so3_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = so3_staggered_grid(N)
else:
G = so3_grid(N)
vispt = [1, 0, 0]
for n in G.nodes():
R = G.node[n]['params']
trans = so3.apply(R, vispt)
#trans = so3.axis_angle(R)[0]
#trans = vectorops.unit(so3.quaternion(R)[:3])
vis.add(str(n), [R, trans])
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
Ri = G.node[i]['params']
Rj = G.node[j]['params']
tmax = 9
times = range(tmax + 1)
milestones = []
for t in times:
u = float(t) / tmax
trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt)
milestones.append(trans)
vis.add(
str(i) + '-' + str(j), trajectory.Trajectory(times, milestones))
vis.hideLabel(str(i) + '-' + str(j))
dist = so3.distance(Ri, Rj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
def visualize(self, style="none", hide_label=True):
if style == "none":
milestones = []
for p in list(self.shapely_poly.exterior.coords):
z = self.height_map.height_at_xy(p[0], p[1]) + 0.01
milestones.append([p[0], p[1], z])
path = trajectory.Trajectory(milestones=milestones)
if not self.name:
self.name = "Support Triangle " + str(random.randint(1, 1000))
vis.add(self.name, path)
if hide_label:
vis.hideLabel(self.name)
elif style == "dashed":
dashes = 10.0
xyz_points = list(self.shapely_poly.exterior.coords)
for i in range(len(xyz_points)):
x, y = xyz_points[i][0], xyz_points[i][1]
xyz_points[i] = [x, y, self.height_map.height_at_xy(x, y) + 0.01]
self.visualize_dashed_line(xyz_points[0], xyz_points[1], "1-2", dashes)
self.visualize_dashed_line(xyz_points[0], xyz_points[2], "1-3", dashes)
self.visualize_dashed_line(xyz_points[1], xyz_points[2], "2-3", dashes)
def visualize_cost(self, x, y, cost, name, with_z=0):
traj = trajectory.Trajectory(
milestones=[[x, y, with_z + 0.005], [x, y, with_z + cost]])
vis.add(name, traj)
def keyboardfunc(self, c, x, y):
if c == 'h':
print "Keyboard help:"
print "- x: verifies edge checks for existing resolution"
print "- i: toggles between interpolation mode and graph inspection mode"
print "- g: toggles drawing the workspace graph"
print "- w: performs a walk to a random workspace node"
print "- m: saves a real-time movie"
print "- M: saves a 360 spin movie"
elif c == 'x':
self.resolution.verify()
self.disconnectionDisplayList.markChanged()
elif c == 'i':
if self.mode == 'interpolate':
self.mode = 'inspect'
else:
self.mode = 'interpolate'
print "Toggled visualization mode to", self.mode
elif c == 'g':
self.drawWorkspaceRoadmap = not self.drawWorkspaceRoadmap
elif c == 'b':
if self.useboundary: self.useboundary = False
else: self.useboundary = True
self.disconnectionDisplayList.markChanged()
elif c == 'm':
if self.movie_frame is None:
self.movie_frame = 0
self.movie_rotate = False
else:
self.movie_frame = None
elif c == 'M':
if self.movie_frame is None:
self.movie_frame = 0
self.movie_rotate = True
else:
self.movie_frame = None
elif c == 'w':
import random
resolved = []
for iw, d in self.resolution.Gw.nodes(data=True):
if d.get('config', None) is not None:
resolved.append(iw)
if self.walk_workspace_path == None:
#draw boundaries transparent now
self.disconnectionDisplayList.markChanged()
for iters in range(10):
wtgt = random.choice(resolved)
#TEMP: place widgets far away for capturing video
far = [20] * 3
self.pointWidget.set(far)
R, t = self.xformWidget.get()
self.xformWidget.set(R, far[:3])
#get current config
if self.configs != None:
self.robot.setConfig(self.configs[0])
try:
x, p = self.resolution.walkPath(wtgt)
except Exception as e:
import traceback
print "Exception", e
traceback.print_exc()
print "WalkPath failed, trying with a new random point"
continue
self.walk_workspace_path = None
if p != None:
t = 0
times = []
for i, q in enumerate(p):
times.append(t)
if i + 1 < len(p):
t += linf_distance(q, p[i + 1],
self.resolution.spinJoints)
#t += self.robot.distance(q,p[i+1])
#t += 0.1
self.walk_workspace_path = trajectory.Trajectory(times, x)
self.walk_path = trajectory.RobotTrajectory(
self.robot, times, p)
self.walk_progress = 0
if self.temp_config == None:
self.temp_config = p[0]
break
self.refresh()
#test_stability_running_time()
#break
#print test_stability()
#print test_stability_traditional()
links = hand_types.keys()
indices = WrenchSpaceIndices(links, [0] * len(links), 'inner')
for i in xrange(innermax):
indices.indices = [i] * len(links)
sp = compute_support_polygon(indices, (0, 0, -9.8))
if sp is not None:
points = [(p[0], p[1], 0) for p in sp]
points.append(points[0])
name = "inner " + str(inner_sizes[i])
if len(inner_sizes) == 1:
name = "inner"
vis.add(name, trajectory.Trajectory(range(len(points)), points))
vis.setColor(name, 0, 1, float(i) / float(innermax))
vis.hideLabel(name)
indices.mode = 'outer'
if len(outer_sizes) > 1:
for i in xrange(outermax):
indices.indices = [i] * len(links)
sp = compute_support_polygon(indices, (0, 0, -9.8))
if sp is not None:
points = [(p[0], p[1], 0) for p in sp]
points.append(points[0])
if i == 0:
name = 'outer BB'
elif i == 1:
name = 'outer f+m'
else:
def add_line_to_vis(self, name, p1, p2):
traj = trajectory.Trajectory(milestones=[p2, p1])
vis.add(name, traj)
from klampt.model import trajectory
#milestones = [[0,0,0],[0,0,0],[1,0,0],[2,0,1],[2.2,0,1.5],[3,0,1],[4,0,-0.3]]
milestones = [[0,0,0],[0.02,0,0],[1,0,0],[2,0,1],[2.2,0,1.5],[3,0,1],[4,0,-0.3]]
traj = trajectory.Trajectory(milestones=milestones)
#prints milestones 0-5
print 0,":",traj.eval(0)
print 1,":",traj.eval(1)
print 2,":",traj.eval(2)
print 3,":",traj.eval(3)
print 4,":",traj.eval(4)
print 5,":",traj.eval(5)
print 6,":",traj.eval(6)
#print some interpolated points
print 0.5,":",traj.eval(0.5)
print 2.5,":",traj.eval(2.5)
#print some stuff after the end of trajectory
print 7,":",traj.eval(6)
print 100.3,":",traj.eval(100.3)
print -2,":",traj.eval(-2)
from klampt import vis
vis.add("point",[0,0,0])
vis.animate("point",traj)
vis.add("traj",traj)
#vis.spin(float('inf')) #show the window until you close it
traj2 = trajectory.HermiteTrajectory()
def queryPRM(self, qI, qG):
vis.show()
vis.lock()
vis.add("Initial", [qI.x, qI.y, qI.z])
vis.setAttribute("Initial", "size", 14)
vis.setColor("Initial", 0, 0.4470, 0.7410)
vis.add("Goal", [qG.x, qG.y, qG.z])
vis.setAttribute("Goal", "size", 14)
vis.setColor("Goal", 0.8500, 0.3250, 0.0980)
vis.unlock()
vis.show(False)
if (self.localPlanner(qI, qG)):
print("Direct path found")
cnt = 0
tr = trajectory.Trajectory()
tr.milestones.append([qI.x, qI.y, qI.z])
tr.milestones.append([qG.x, qG.y, qG.z])
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setColor(fName, 0, 0.4470, 0.7410)
cnt = cnt + 1
self.pathCnt = cnt
flagIn = True
self.env.setConfig(qI.x, qI.y, qI.z, qI.sc, qI.tht)
vis.show()
while (flagIn):
inText = ''
while not (inText == 'y' or inText == 'n'):
inText = raw_input("Run Path (y/n) ?")
if inText == 'n':
flagIn = False
break
if inText == 'y':
self.localPlanner(qI, qG, True)
return True
self.pathCnt = 0
#vis.show(False)
nodeList = list(self.G)
minNodeI = None
minDist = 100000
for nodeG in nodeList:
dist = self.distMetric(qI, nodeG)
if dist < self.maxDist:
if (self.localPlanner(qI, nodeG)):
if (minDist > dist):
minNodeI = nodeG
minDist = dist
if minNodeI == None:
return False
minNodeG = None
minDist = 100000
for nodeG in nodeList:
dist = self.distMetric(qG, nodeG)
if dist < self.maxDist:
if (self.localPlanner(qG, nodeG)):
if (minDist > dist):
minNodeG = nodeG
minDist = dist
if minNodeG == None:
return False
try:
shortest_path = nx.dijkstra_path(self.G,
source=minNodeI,
target=minNodeG)
except:
return False
shortest_path = self.smoothPath(shortest_path)
vis.show()
cnt = 0
tr = trajectory.Trajectory()
tr.milestones.append([qI.x, qI.y, qI.z])
tr.milestones.append([minNodeI.x, minNodeI.y, minNodeI.z])
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setColor(fName, 0, 0.4470, 0.7410)
cnt = cnt + 1
prevNode = None
for nodeP in shortest_path:
if prevNode == None:
prevNode = nodeP
continue
tr = trajectory.Trajectory()
tr.milestones.append([prevNode.x, prevNode.y, prevNode.z])
tr.milestones.append([nodeP.x, nodeP.y, nodeP.z])
prevNode = nodeP
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setColor(fName, 0.4660, 0.6740, 0.1880)
cnt = cnt + 1
tr = trajectory.Trajectory()
tr.milestones.append([qG.x, qG.y, qG.z])
tr.milestones.append([minNodeG.x, minNodeG.y, minNodeG.z])
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
#vis.setAttribute(fName,"width",1.0)
vis.setColor(fName, 0.8500, 0.3250, 0.0980)
cnt = cnt + 1
self.pathCnt = cnt
flagIn = True
self.env.setConfig(qI.x, qI.y, qI.z, qI.sc, qI.tht)
vis.show()
while (flagIn):
inText = ''
while not (inText == 'y' or inText == 'n'):
inText = raw_input("Run Path (y/n) ?")
if inText == 'n':
flagIn = False
break
if inText == 'y':
self.runPath(qI, minNodeI, minNodeG, qG, shortest_path)
return True
def make(robot, q_file="q_cmd.txt", ff_torque_file="ff_torque_cmd.txt"):
qcmd = trajectory.Trajectory()
tcmd = trajectory.Trajectory()
qcmd.load(q_file)
tcmd.load(ff_torque_file)
return TrajectoryWithFeedforwardController(qcmd, tcmd)
def __init__(self, world, path_file):
GLSimulationPlugin.__init__(self, world)
self.world = world
#The trajectory is loaded here
self.trajectory = trajectory.Trajectory()
self.trajectory.load(path_file)
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:
m[1] = 0.25
vis.add("point mintime, nonlinear",traj3.discretize(0.05),color=(0.2,0.2,1,1))
