diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 784d5d3362b7b1678ce6b45b96e20e8d72da04fa..e702cee34fc5f7a232a48b65c5c8f5cf164e8f9f 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -10,11 +10,11 @@ from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoPathFollowingMP
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik import getClikArgs
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner
import pinocchio as pin
import crocoddyl
import importlib.util
-from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs
+from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
import yaml
@@ -44,14 +44,37 @@ if __name__ == "__main__":
if importlib.util.find_spec('mim_solvers'):
import mim_solvers
robot = RobotManager(args)
+ robot.q[0] = 9.0
+ robot.q[1] = 4.0
#time.sleep(5)
x0 = np.concatenate([robot.getQ(), robot.getQd()])
robot._step()
goal = np.array([0.5, 5.5])
+
+ ###########################
+ # visualizing obstacles #
+ ###########################
+ _, map_as_list = createMap()
+ # we're assuming rectangles here
+ # be my guest and implement other options
+ if args.visualize_manipulator:
+ for obstacle in map_as_list:
+ length = obstacle[1][0] - obstacle[0][0]
+ width = obstacle[3][1] - obstacle[0][1]
+ height = 0.4 # doesn't matter because plan because planning is 2D
+ pose = pin.SE3(np.eye(3), np.array([
+ obstacle[0][0] + (obstacle[1][0] - obstacle[0][0]) / 2,
+ obstacle[0][1] + (obstacle[3][1] - obstacle[0][1]) / 2,
+ 0.0]))
+ dims = [length, width, height]
+ command = {"obstacle" : [pose, dims]}
+ robot.visualizer_manager.sendCommand(command)
+
planning_function = partial(starPlanner, goal)
path_planner = ProcessManager(args, planning_function, None, 2, None)
- CrocoPathFollowingMPC(args, robot, x0, path_planner)
+ cartesianPathFollowingWithPlanner(args, robot, path_planner)
+ #CrocoPathFollowingMPC(args, robot, x0, path_planner)
print("final position:")
print(robot.getT_w_e())
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 40511b816a6b68fd49a7add19b82cf4e14ae4b1e..3255f1818e5bdf7182b33c8f7779adf790273f80 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -3,10 +3,11 @@ import numpy as np
import copy
import argparse
from functools import partial
-from ur_simple_control.managers import ControlLoopManager, RobotManager
+from ur_simple_control.managers import ControlLoopManager, RobotManager, ProcessManager
import time
from qpsolvers import solve_qp
import argparse
+from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
def getClikArgs(parser):
"""
@@ -85,7 +86,7 @@ def jacobianTranspose(J, err_vector):
qd = J.T @ err_vector
return qd
-def invKinmQP(J, err_vector):
+def invKinmQP(J, err_vector, lb=None, ub=None):
# maybe a lower precision dtype is equally good, but faster?
P = np.eye(J.shape[1], dtype="double")
# TODO: why is q all 0?
@@ -95,8 +96,8 @@ def invKinmQP(J, err_vector):
b = err_vector#[:3]
A = J#[:3]
# TODO: you probably want limits here
- lb = None
- ub = None
+ #lb = None
+ #ub = None
h = None
#qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
@@ -258,9 +259,72 @@ def moveLFollowingLine(args, robot, goal_point):
"""
pass
-# TODO: implement
-def cartesianPathFollowing(args, robot, path):
- pass
+def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, path_planner : ProcessManager, i, past_data):
+ """
+ cartesianPathFollowingWithPlanner
+ -----------------------------
+ end-effector(s) follow their path(s) according to what a 2D path-planner spits out
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ p = q[:2]
+ path_planner.sendFreshestCommand({'p' : p})
+
+ # NOTE: it's pointless to recalculate the path every time
+ # if it's the same 2D path
+ data = path_planner.getData()
+ if data == None:
+ if args.debug_prints:
+ print("got no path so no doing anything")
+ robot.sendQd(np.zeros(robot.model.nv))
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+ if data == "done":
+ breakFlag = True
+
+ path_pol, path2D_untimed = data
+ path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ # should be precomputed somewhere but this is nowhere near the biggest problem
+ max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+
+ # 1) make 6D path out of [[x0,y0],...]
+ # that represents the center of the cart
+ pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
+ #print(path2D_untimed)
+ #for pp in pathSE3:
+ # print(pp.translation)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualize_manipulator:
+ robot.visualizer_manager.sendCommand({"path": pathSE3})
+
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ # NOTE: obviously not path following but i want to see things working here
+ SEerror = T_w_e.actInv(pathSE3[1])
+ err_vector = pin.log6(SEerror).vector
+ #vel_cmd = invKinmQP(J, err_vector, lb=-1*robot.model.velocityLimit,
+ # ub=robot.model.velocityLimit)
+ #vel_cmd = invKinmQP(J, err_vector)
+ vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
+ robot.sendQd(vel_cmd)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+def cartesianPathFollowingWithPlanner(args, robot : RobotManager, path_planner : ProcessManager):
+ controlLoop = partial(cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner)
+ log_item = {
+ 'qs' : np.zeros(robot.model.nq),
+ 'dqs' : np.zeros(robot.model.nv)
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager.run()
def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
"""
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index b012566e7276a33ef8ca8c4df6d79e35d96a2583..c85e7daee11dca7ada368d09c067dbf6afe890ee 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,5 +1,6 @@
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoPathFollowingOCP1
+from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
import pinocchio as pin
import crocoddyl
import numpy as np
@@ -93,118 +94,6 @@ def CrocoIKMPC(args, robot, x0, goal):
loop_manager.run()
-def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
- return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
-
-def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
- """
- path2D_to_timed_SE3
- ---------------------
- we have a 2D path of (N,2) shape as reference
- the OCP accepts SE3 (it could just rotations or translation too),
- so this function constructs it from the 2D path.
- it also times it as this is what the current ocp formulation needs:
- there should be a timing associated with the path,
- because defining the cost function as the fit of the rollout to the path
- is complicated to do software-wise.
- as it is now, we're pre-selecting points of the path, and associating those
- with rollout states at a set time step between the states in the rollout.
- this isn't a problem if we have an idea of how fast the robot can go,
- which gives us a heuristic of how to select the points (if you went at maximum
- speed, this is how far you'd go in this time, so this is your point).
- thankfully this does not need to be exact because we just care about the distance
- between the current point and the point on the path, so if it's further out
- that just means the error is larger, and that doesn't necessarily correspond
- to a different action.
- NOTE: we are constructing a possibly bullshit
- trajectory here, it's a man-made heuristic,
- and we should leave that to the MPC,
- but that requires too much coding and there is no time rn.
- """
-
- ####################################################
- # getting a timed 2D trajectory from a heuristic #
- ####################################################
- # the strategy is somewhat reasonable at least:
- # assume we're moving at 90% max velocity in the base,
- # and use that.
- perc_of_max_v = 0.9
- base_v = perc_of_max_v * max_base_v
-
- # 1) the length of the path divided by 0.9 * max_vel
- # gives us the total time of the trajectory,
- # so we first compute that
- # easiest possible way to get approximate path length
- # (yes it should be from the polynomial approximation but that takes time to write)
- x_i = path2D_untimed[:,0][:-1] # no last element
- y_i = path2D_untimed[:,1][:-1] # no last element
- x_i_plus_1 = path2D_untimed[:,0][1:] # no first element
- y_i_plus_1 = path2D_untimed[:,1][1:] # no first element
- x_diff = x_i_plus_1 - x_i
- x_diff = x_diff.reshape((-1,1))
- y_diff = y_i_plus_1 - y_i
- y_diff = y_diff.reshape((-1,1))
- path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
- total_time = path_length / base_v
- # 2) we find the correspondence between s and time
- # TODO: read from where it should be, but seba checked that it's 5 for some reason
- max_s = 5
- t_to_s = max_s / total_time
- # 3) construct the ocp-timed 2D path
- path2D = []
- # time is i * args.ocp_dt
- for i in range(args.n_knots + 1):
- s = (i * args.ocp_dt) * t_to_s
- path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
- path2D = np.array(path2D)
-
- ####################################################
- # constructing the SE3 reference from [x,y] path #
- ####################################################
- # 1) we need to add points so that there are args.n_knots of them
- # the -1 is here because we're finite differencing to get theta,
- # so we need one extra
- #path2D = list(path2D)
- ## in case there's too few
- #while len(path2D) - 1 < args.n_knots:
- # path2D.append(path2D[-1])
- ## in case there's too much
- #while len(path2D) - 1> args.n_knots:
- # path2D.pop()
- #path2D = np.array(path2D)
-
-
- #########################
- # path2D into pathSE2 #
- #########################
- # construct theta
- # since it's a pairwise operation it can't be done on the last point
- x_i = path2D[:,0][:-1] # no last element
- y_i = path2D[:,1][:-1] # no last element
- x_i_plus_1 = path2D[:,0][1:] # no first element
- y_i_plus_1 = path2D[:,1][1:] # no first element
- x_diff = x_i_plus_1 - x_i
- y_diff = y_i_plus_1 - y_i
- # elementwise arctan2
- thetas = np.arctan2(x_diff, y_diff)
-
- #thetas = thetas.reshape((-1, 1))
- #path_SE2 = np.hstack((path2D, thetas))
-
- ######################################
- # construct SE3 from SE2 reference #
- ######################################
- # the plan is parallel to the ground because it's a mobile
- # manipulation task
- # TODO: enforce timing, interpolate the path accordingly
- path = []
- for i in range(len(path2D) - 1):
- rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
- translation = np.array([path2D[i][0], path2D[i][1],
- args.handlebar_height])
- path.append(pin.SE3(rotation, translation))
-
- return path
def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
"""
@@ -230,6 +119,8 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
+ if data == "done":
+ breakFlag = True
path_pol, path2D_untimed = data
path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
@@ -239,6 +130,9 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
# 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualize_manipulator:
+ robot.visualizer_manager.sendCommand({"path": pathSE3})
# 2) do T_mobile_base_ee_pos to get
# end-effector reference frame(s)
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 655d8157a31d2ea0776d066861e16ade4b92d8ee..9596d3ccaffb4bf36cd31d9f7c9db5eae4ae4c92 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -9,6 +9,7 @@ from ur_simple_control.path_generation.star_navigation.robot.unicycle import Uni
from ur_simple_control.path_generation.starworlds.obstacles import StarshapedPolygon
import shapely
import yaml
+import pinocchio as pin
import matplotlib.pyplot as plt
import matplotlib.collections as plt_col
@@ -396,6 +397,7 @@ def createMap():
---------
return obstacles that define the 2D map
"""
+ # [lower_left, lower_right, top_right, top_left]
map_as_list = [
[[2, 2], [8, 2], [8, 3], [2, 3]] ,
[[2, 3], [3, 3], [3, 4.25], [2, 4.25]],
@@ -457,6 +459,119 @@ def pathVisualizer(x0, goal, map_as_list, positions):
# return path_SE2
+def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
+ return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
+
+def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
+ """
+ path2D_to_timed_SE3
+ ---------------------
+ we have a 2D path of (N,2) shape as reference
+ the OCP accepts SE3 (it could just rotations or translation too),
+ so this function constructs it from the 2D path.
+ it also times it as this is what the current ocp formulation needs:
+ there should be a timing associated with the path,
+ because defining the cost function as the fit of the rollout to the path
+ is complicated to do software-wise.
+ as it is now, we're pre-selecting points of the path, and associating those
+ with rollout states at a set time step between the states in the rollout.
+ this isn't a problem if we have an idea of how fast the robot can go,
+ which gives us a heuristic of how to select the points (if you went at maximum
+ speed, this is how far you'd go in this time, so this is your point).
+ thankfully this does not need to be exact because we just care about the distance
+ between the current point and the point on the path, so if it's further out
+ that just means the error is larger, and that doesn't necessarily correspond
+ to a different action.
+ NOTE: we are constructing a possibly bullshit
+ trajectory here, it's a man-made heuristic,
+ and we should leave that to the MPC,
+ but that requires too much coding and there is no time rn.
+ """
+
+ ####################################################
+ # getting a timed 2D trajectory from a heuristic #
+ ####################################################
+ # the strategy is somewhat reasonable at least:
+ # assume we're moving at 90% max velocity in the base,
+ # and use that.
+ perc_of_max_v = 0.9
+ base_v = perc_of_max_v * max_base_v
+
+ # 1) the length of the path divided by 0.9 * max_vel
+ # gives us the total time of the trajectory,
+ # so we first compute that
+ # easiest possible way to get approximate path length
+ # (yes it should be from the polynomial approximation but that takes time to write)
+ x_i = path2D_untimed[:,0][:-1] # no last element
+ y_i = path2D_untimed[:,1][:-1] # no last element
+ x_i_plus_1 = path2D_untimed[:,0][1:] # no first element
+ y_i_plus_1 = path2D_untimed[:,1][1:] # no first element
+ x_diff = x_i_plus_1 - x_i
+ x_diff = x_diff.reshape((-1,1))
+ y_diff = y_i_plus_1 - y_i
+ y_diff = y_diff.reshape((-1,1))
+ path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
+ total_time = path_length / base_v
+ # 2) we find the correspondence between s and time
+ # TODO: read from where it should be, but seba checked that it's 5 for some reason
+ max_s = 5
+ t_to_s = max_s / total_time
+ # 3) construct the ocp-timed 2D path
+ path2D = []
+ # time is i * args.ocp_dt
+ for i in range(args.n_knots + 1):
+ s = (i * args.ocp_dt) * t_to_s
+ path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ path2D = np.array(path2D)
+
+ ####################################################
+ # constructing the SE3 reference from [x,y] path #
+ ####################################################
+ # 1) we need to add points so that there are args.n_knots of them
+ # the -1 is here because we're finite differencing to get theta,
+ # so we need one extra
+ #path2D = list(path2D)
+ ## in case there's too few
+ #while len(path2D) - 1 < args.n_knots:
+ # path2D.append(path2D[-1])
+ ## in case there's too much
+ #while len(path2D) - 1> args.n_knots:
+ # path2D.pop()
+ #path2D = np.array(path2D)
+
+
+ #########################
+ # path2D into pathSE2 #
+ #########################
+ # construct theta
+ # since it's a pairwise operation it can't be done on the last point
+ x_i = path2D[:,0][:-1] # no last element
+ y_i = path2D[:,1][:-1] # no last element
+ x_i_plus_1 = path2D[:,0][1:] # no first element
+ y_i_plus_1 = path2D[:,1][1:] # no first element
+ x_diff = x_i_plus_1 - x_i
+ y_diff = y_i_plus_1 - y_i
+ # elementwise arctan2
+ thetas = np.arctan2(x_diff, y_diff)
+
+ #thetas = thetas.reshape((-1, 1))
+ #path_SE2 = np.hstack((path2D, thetas))
+
+ ######################################
+ # construct SE3 from SE2 reference #
+ ######################################
+ # the plan is parallel to the ground because it's a mobile
+ # manipulation task
+ # TODO: enforce timing, interpolate the path accordingly
+ path = []
+ for i in range(len(path2D) - 1):
+ rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
+ translation = np.array([path2D[i][0], path2D[i][1],
+ args.handlebar_height])
+ path.append(pin.SE3(rotation, translation))
+
+ return path
+
# function to be put into ProcessManager,
# spitting out path points
def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
@@ -501,17 +616,11 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
break
# Update the scene
+ # r0 is the current position, rg is the goal, rho is a constant
+ # --> why do we need to output this?
r0, rg, rho, obstacles_star = scene_updater.update(p, goal, obstacles)
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
- # delete comment once new thing works
- ################################################
- # create (N,2) path for list [x0,y0,x1,y1,...]
- # of course it shouldn't be like that to begin with but
- # i have no time for that
- #path2D = np.array(path_gen.target_path)
- #path2D = path2D.reshape((-1, 2))
- ################################################
data_queue.put((path_pol, path_gen.target_path))
if args.debug_prints:
print("put in new plan")
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 1def7147bf3b5c4ed1bfb7ca66479b3a5142b94a..0c5b24f2f30666787d02956795b99414a79b25c3 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
index 781dfc3de0c97693b0ce006dba3b64c24f11ab2d..e241d050fc4713e1e7aabee98d621a3cdb4e603b 100644
--- a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -41,6 +41,8 @@ class MeshcatVisualizer(PMV):
# there will be times when i just want to drop in points
# which will never be changed
self.n_points = 0
+ self.n_path_points = 0
+ self.n_obstacles = 0
if robot is not None:
super().__init__(robot.model, robot.collision_model, robot.visual_model)
elif model is not None:
@@ -75,6 +77,14 @@ class MeshcatVisualizer(PMV):
def addBox(self, name, dims, color):
material = materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Box(dims), material)
+
+ def addObstacle(self, pose, dims):
+ color = [0.5, 0.5, 0.5, 0.8]
+ obstacle_name = f"world/obstacle_{self.n_obstacles}"
+ self.addBox(obstacle_name, dims, color)
+ self.applyConfiguration(obstacle_name, pose)
+ self.n_obstacles += 1
+
def addEllipsoid(self, name, dims, color):
material = materialFromColor(color)
@@ -87,9 +97,14 @@ class MeshcatVisualizer(PMV):
self.n_points += 1
def addPath(self, name, path : list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]):
+ # who cares about the name
+ name = "path"
for i, point in enumerate(path):
- self.addSphere(f"world/path_{name}_point_{i}", radius, color)
+ if i < self.n_path_points:
+ self.addSphere(f"world/path_{name}_point_{i}", radius, color)
self.applyConfiguration(f"world/path_{name}_point_{i}", point)
+ self.n_path_points = max(i, self.n_path_points)
+
def applyConfiguration(self, name, placement):
if isinstance(placement, list) or isinstance(placement, tuple):
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index fc25e69be117a284890e7a94ae474e6890d56f33..3c039a0449183cb6c4384a5fa6c869325003d8ba 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -191,6 +191,12 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.display(cmd["q"])
if key == "point":
viz.addPoint(cmd["point"])
+ if key == "obstacle":
+ # stupid and evil but there is no time
+ viz.addObstacle(cmd["obstacle"][0], cmd["obstacle"][1])
+ if key == "path":
+ # stupid and evil but there is no time
+ viz.addPath("", cmd["path"])
except KeyboardInterrupt:
if args.debug_prints: