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))