diff --git a/COLCON_IGNORE b/COLCON_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index 3d78e8df1587d127d2826f2d742f82af6f67e755..d29008e8b02e99e69ae4171ede21d462252ef8c4 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -64,7 +64,7 @@ if __name__ == "__main__":
# and because it is sampled at a much lower frequency
#followKinematicJointTrajP(args, robot, reference)
- CrocoIKMPC(args, robot, x0, Mgoal)
+ CrocoIKMPC(args, robot, Mgoal)
print("final position:")
print(robot.getT_w_e())
diff --git a/python/examples/ros2_clik.py b/python/examples/ros2_clik.py
new file mode 100644
index 0000000000000000000000000000000000000000..be65bc01e0deb01b0186eb9f0ad67c13c69c99cb
--- /dev/null
+++ b/python/examples/ros2_clik.py
@@ -0,0 +1,38 @@
+# PYTHON_ARGCOMPLETE_OK
+import numpy as np
+import time
+import pinocchio as pin
+import argcomplete, argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!'
+ parser = getClikArgs(parser)
+ argcomplete.autocomplete(parser)
+ args = parser.parse_args()
+ return args
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+ print(robot.getT_w_e())
+ Mgoal = robot.defineGoalPointCLI()
+ compliantMoveL(args, robot, Mgoal)
+ #moveL(args, robot, Mgoal)
+ robot.closeGripper()
+ robot.openGripper()
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ #loop_manager.stopHandler(None, None)
+
diff --git a/python/examples/smc_node.py b/python/examples/smc_node.py
new file mode 100644
index 0000000000000000000000000000000000000000..7fc29a86c4eb2704004a979e7cd7ef544bdb6404
--- /dev/null
+++ b/python/examples/smc_node.py
@@ -0,0 +1,102 @@
+# PYTHON_ARGCOMPLETE_OK
+import rclpy
+from rclpy.node import Node
+from geometry_msgs import msg
+from rclpy import wait_for_message
+import pinocchio as pin
+import argcomplete, argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP
+import threading
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!'
+ parser = getClikArgs(parser)
+ argcomplete.autocomplete(parser)
+ args = parser.parse_args()
+ return args
+
+class ROSCommHandler(Node):
+
+ def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
+ super().__init__('ROSCommHandler')
+
+ self.publisher_vel_base = self.create_publisher(msg.Twist, '/cmd_vel', 5)
+ print("created publisher")
+ robot_manager.set_publisher_vel_base(self.publisher_vel_base)
+
+ qos_prof = rclpy.qos.QoSProfile(
+ reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
+ durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
+ history = rclpy.qos.HistoryPolicy.KEEP_LAST,
+ depth = 1,
+ )
+ self.subscription = self.create_subscription(msg.PoseWithCovarianceStamped,
+ '/amcl_pose', self.pose_callback, qos_prof)
+ #wait_for_message.wait_for_message(msg.PoseWithCovarianceStamped, self, '/amcl_pose')
+ #print("subscribed to /amcl_pose")
+ self.subscription # prevent unused variable warning
+ self.robot_manager = robot_manager
+ self.loop_manager = loop_manager
+ self.args = args
+
+ def pose_callback(self, mesg):
+ self.robot_manager.q[0] = mesg.pose.pose.position.x
+ self.robot_manager.q[1] = mesg.pose.pose.position.y
+ # TODO: check that z can be used as cos(theta) and w as sin(theta)
+ # (they could be defined some other way or theta could be offset of something)
+ self.robot_manager.q[2] = mesg.pose.pose.orientation.z
+ self.robot_manager.q[3] = mesg.pose.pose.orientation.w
+
+ #vel_cmd = msg.Twist()
+ print("received new amcl")
+ #self.publisher_vel_base.publish(vel_cmd)
+
+
+
+
+if __name__ == '__main__':
+ rclpy.init()
+ args = get_args()
+ robot = RobotManager(args)
+ goal = robot.defineGoalPointCLI()
+ #goal = pin.SE3.Identity()
+ loop_manager = compliantMoveL(args, robot, goal, run=False)
+ ros_comm_handler = ROSCommHandler(args, robot, loop_manager)
+
+ thread = threading.Thread(target=rclpy.spin, args=(ros_comm_handler,), daemon=True)
+ thread.start()
+
+ rate = ros_comm_handler.create_rate(250)
+ while rclpy.ok():
+ #msgg = msg.Twist()
+ #print("publisihng")
+ #ros_comm_handler.publisher_vel_base.publish(msgg)
+ breakFlag = ros_comm_handler.loop_manager.run_one_iter(0)
+ rate.sleep()
+
+
+
+# if not args.pinocchio_only:
+# robot.stopRobot()
+#
+# if args.visualize_manipulator:
+# robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ robot.log_manager.plotAllControlLoops()
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ ros_comm_handler.destroy_node()
+ rclpy.shutdown()
+
+
+
+
diff --git a/python/ur_simple_control.egg-info/PKG-INFO b/python/ur_simple_control.egg-info/PKG-INFO
index 84b865bdb559c9c6924c033eb1baba2982422ea7..80d9185ea780b3c481dad3d7bacf8213c104c6a0 100644
--- a/python/ur_simple_control.egg-info/PKG-INFO
+++ b/python/ur_simple_control.egg-info/PKG-INFO
@@ -1,7 +1,12 @@
Metadata-Version: 2.1
-Name: ur_simple_control
+Name: ur-simple-control
Version: 0.1
Summary: Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.
Home-page: https://gitlab.control.lth.se/marko-g/ur_simple_control
Author: Marko Guberina
+License: UNKNOWN
+Platform: UNKNOWN
License-File: LICENSE.txt
+
+UNKNOWN
+
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
index 6ac766121c788ec1d445bbbec43e5015235151f4..bf3ed08977ae3f5773625f56e8e8db1824a507a8 100644
--- a/python/ur_simple_control.egg-info/SOURCES.txt
+++ b/python/ur_simple_control.egg-info/SOURCES.txt
@@ -33,14 +33,11 @@ examples/test_crocoddyl_opt_ctrl.py
examples/__pycache__/robotiq_gripper.cpython-310.pyc
examples/data/clik_comparison_0.pickle
examples/data/clik_comparison_1.pickle
-examples/data/clik_run_001.pickle
-examples/data/clik_run_001_args.pickle
examples/data/fts.png
examples/data/joint_trajectory.csv
examples/data/latest_run
examples/data/latest_run_0
examples/data/path_in_pixels.csv
-examples/data/test2_0.pickle
examples/old_or_experimental/clik_old.py
examples/old_or_experimental/force_mode_api.py
examples/old_or_experimental/forcemode_example.py
@@ -228,8 +225,6 @@ ur_simple_control/util/grippers/abstract_gripper.py
ur_simple_control/util/grippers/on_robot/twofg.py
ur_simple_control/util/grippers/robotiq/robotiq_gripper.py
ur_simple_control/visualize/__init__.py
-ur_simple_control/visualize/main.py
-ur_simple_control/visualize/make_run.py
ur_simple_control/visualize/manipulator_comparison_visualizer.py
ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
ur_simple_control/visualize/visualize.py
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index b60a7a8093ce7ef6223789fbf696d7cf59504c35..013bf4c3c931ca6dca9274fbad730bf76f14dc35 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index e9b96271a7b765d8cd998b4bf188ba6f4beb244c..9450f01b9ff3639106a03cef0cc1522b1ebe61c2 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc
index 7959a39494ca6a0aeb658248bb5a3a205372822a..8edaa2f577ddfd74d815dee53f8265d38c0f2fca 100644
Binary files a/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc
index ff88e439ab199e7fe995968252caea7174bd1d85..ecf89b920431584195c255ccc99637ca09fd2338 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
index 22ff463a3a86e96cac507291831172989a59a7cc..c14ed132923a1fd4fe37a2eea6179541d994dc02 100644
Binary files a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index c26e75c9ffe7dc58398453c5816f08b85e6f88ef..25ed089ba69942c8f005f0625a87707c0c728b75 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -376,7 +376,7 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
return breakFlag, save_past_dict, log_item
# add a threshold for the wrench
-def compliantMoveL(args, robot, goal_point):
+def compliantMoveL(args, robot, goal_point, run=True):
"""
compliantMoveL
-----
@@ -393,14 +393,17 @@ def compliantMoveL(args, robot, goal_point):
log_item = {
'qs' : np.zeros(robot.model.nq),
'wrench' : np.zeros(6),
- 'tau' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nq),
+ 'tau' : np.zeros(robot.model.nv),
+ 'dqs' : np.zeros(robot.model.nv),
}
save_past_dict = {
'wrench': np.zeros(6),
}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- loop_manager.run()
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
def clikCartesianPathIntoJointPath(args, robot, path, \
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
index 1935145d6fced2a08057f336744c1f3174952a6e..8f623ffcc78243df2a5ee3e73e7c0e561efecc48 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
index 0f8c0ab2560a9677153f22c766fe22fa96a859c8..e81144824ae642b3e14a08a19d1224855ba643bb 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc and b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 70e8783c25401084bb70c7741593b4353053d1bb..df733acfd49f789012812efcd66287fff25aeee7 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -27,6 +27,12 @@ from os import getpid
from functools import partial
import pickle
+# import ros stuff if you're rosing
+# TODO: add more as needed
+import importlib.util
+if importlib.util.find_spec('rclpy'):
+ from geometry_msgs import msg
+
"""
general notes
---------------
@@ -80,7 +86,7 @@ def getMinimalArgParser():
#################################################
parser.add_argument('--robot', type=str, \
help="which robot you're running or simulating", default="ur5e", \
- choices=['ur5e', 'heron', 'herondd', 'gripperlessur5e'])
+ choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -208,6 +214,7 @@ class ControlLoopManager:
self.controlLoop = controlLoop
self.final_iteration = -1 # because we didn't even start yet
self.args = args
+ self.iter_n = 0
self.past_data = {}
# save_past_dict has to have the key and 1 example of what you're saving
# so that it's type can be inferred (but we're in python so types don't really work).
@@ -230,10 +237,9 @@ class ControlLoopManager:
if self.args.real_time_plotting:
self.plotter_manager = ProcessManager(args, realTimePlotter, log_item, 0)
- # give real-time plotter some time to set itself up
- #self.plotter_manager.sendCommand(log_item)
- def run(self):
+
+ def run_one_iter(self, i):
"""
run
---
@@ -242,56 +248,57 @@ class ControlLoopManager:
it's the controlLoop's responsibility to break if it achieved it's goals.
this is done via the breakFlag
"""
+ # NOTE: all required pre-computations are handled here
+ self.robot_manager._step()
+ # TODO make the arguments to controlLoop kwargs or whatever
+ # so that you don't have to declare them on client side if you're not using them
+ breakFlag, latest_to_save_dict, log_item = self.controlLoop(i, self.past_data)
+ self.final_iteration = i
+
+ # update past rolling window
+ # TODO: write an assert assuring the keys are what's been promised
+ # (ideally this is done only once, not every time, so think whether/how that can be avoided)
+ for key in latest_to_save_dict:
+ # remove oldest entry
+ self.past_data[key].popleft()
+ # add new entry
+ self.past_data[key].append(latest_to_save_dict[key])
+
+ # log the data
+ # check that you can
+ # TODO only need to check this once, pls enforce better
+ #if len(self.log_dict) > 0:
+ for key in log_item:
+ #if key not in self.log_dict.keys():
+ # raise KeyError("you need to provide log items you promised!")
+ # break
+ self.log_dict[key].append(log_item[key])
+
+ # TODO: do it this way if running on the real robot.
+ # but if not, we want to control the speed of the simulation,
+ # and how much we're plotting.
+ # so this should be an argument that is use ONLY if we're in simulation
+ if i % 20 == 0:
+ # don't send what wasn't ready
+ if self.args.visualize_manipulator:
+ self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
+ "T_w_e" : self.robot_manager.getT_w_e()})
+ if self.robot_manager.robot_name == "heron":
+ T_base = self.robot_manager.data.oMi[1]
+ self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
+
+ if self.args.real_time_plotting:
+ # don't put new stuff in if it didn't handle the previous stuff.
+ # it's a plotter, who cares if it's late.
+ # the number 5 is arbitrary
+ self.plotter_manager.sendCommand(log_item)
+ return breakFlag
+
+ def run(self):
self.final_iteration = 0
for i in range(self.max_iterations):
start = time.time()
- # NOTE: all required pre-computations are handled here
- self.robot_manager._step()
- # TODO make the arguments to controlLoop kwargs or whatever
- # so that you don't have to declare them on client side if you're not using them
- breakFlag, latest_to_save_dict, log_item = self.controlLoop(i, self.past_data)
- self.final_iteration = i
-
- # update past rolling window
- # TODO: write an assert assuring the keys are what's been promised
- # (ideally this is done only once, not every time, so think whether/how that can be avoided)
- for key in latest_to_save_dict:
- # remove oldest entry
- self.past_data[key].popleft()
- # add new entry
- self.past_data[key].append(latest_to_save_dict[key])
-
- # log the data
- # check that you can
- # TODO only need to check this once, pls enforce better
- #if len(self.log_dict) > 0:
- for key in log_item:
- #if key not in self.log_dict.keys():
- # raise KeyError("you need to provide log items you promised!")
- # break
- self.log_dict[key].append(log_item[key])
-
- # TODO: do it this way if running on the real robot.
- # but if not, we want to control the speed of the simulation,
- # and how much we're plotting.
- # so this should be an argument that is use ONLY if we're in simulation
- if i % 20 == 0:
- # don't send what wasn't ready
- if self.args.visualize_manipulator:
- self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
- "T_w_e" : self.robot_manager.getT_w_e()})
- if self.robot_manager.robot_name == "heron":
- T_base = self.robot_manager.data.oMi[1]
- self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
-
- #if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
- # self.robot_manager.updateViz({"q" : self.robot_manager.q,
- # "T_w_e" : self.robot_manager.getT_w_e()})
- if self.args.real_time_plotting:
- # don't put new stuff in if it didn't handle the previous stuff.
- # it's a plotter, who cares if it's late.
- # the number 5 is arbitrary
- self.plotter_manager.sendCommand(log_item)
+ breakFlag = self.run_one_iter(i)
# break if done
if breakFlag:
@@ -311,6 +318,7 @@ class ControlLoopManager:
# (because i did know about it, just didn't even think of changing it)
if not (self.args.pinocchio_only and self.args.fast_simulation):
time.sleep(self.robot_manager.dt - diff)
+
######################################################################
# for over
######################################################################
@@ -413,9 +421,11 @@ class RobotManager:
if self.robot_name == "heron":
self.model, self.collision_model, self.visual_model, self.data = \
heron_approximation()
- if self.robot_name == "herondd":
+ if self.robot_name == "heronros":
self.model, self.collision_model, self.visual_model, self.data = \
- heron_approximationDD()
+ heron_approximation()
+ #self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
+ #self.publisher_vel_base = publisher_vel_base
if self.robot_name == "gripperlessur5e":
self.model, self.collision_model, self.visual_model, self.data = \
getGripperlessUR5e()
@@ -828,11 +838,20 @@ class RobotManager:
# TODO: implement real thing
if self.robot_name == "heron":
- self.v_q = qd
- self.q = pin.integrate(self.model, self.q, qd * self.dt)
- if self.robot_name == "herondd":
- self.v_q = qd
- self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ # y-direction is not possible on heron
+ qd[1] = 0
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
+ if self.robot_name == "heronros":
+ # y-direction is not possible on heron
+ qd[1] = 0
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = qd[0]
+ cmd_msg.angular.z = qd[2]
+ #print("about to publish", cmd_msg)
+ self.publisher_vel_base.publish(cmd_msg)
+ # good to keep because updating is slow otherwise
+ # it's not correct, but it's more correct than not updating
+ #self.q = pin.integrate(self.model, self.q, qd * self.dt)
if self.robot_name == "gripperlessur5e":
qd_cmd = qd[:6]
@@ -975,6 +994,10 @@ class RobotManager:
if self.args.debug_prints:
print("you didn't select viz")
+ def set_publisher_vel_base(self, publisher_vel_base):
+ self.publisher_vel_base = publisher_vel_base
+ print("set publisher into robotmanager")
+
class ProcessManager:
"""
ProcessManager
diff --git a/python/ur_simple_control/optimal_control/__init__.py b/python/ur_simple_control/optimal_control/__init__.py
index 569adb6a237941a26870d3ebd68f85937dacedf2..def857098a0e15e46693d661a0b1744ace405aac 100644
--- a/python/ur_simple_control/optimal_control/__init__.py
+++ b/python/ur_simple_control/optimal_control/__init__.py
@@ -1,6 +1,6 @@
import importlib.util
-if importlib.util.find_spec('casadi'):
- from .create_pinocchio_casadi_ocp import *
+#if importlib.util.find_spec('casadi'):
+# from .create_pinocchio_casadi_ocp import *
from .crocoddyl_mpc import *
from .crocoddyl_optimal_control import *
from .get_ocp_args import *
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index f48dca2fca2573f98152a4dfae84776c88ba14a6..b3caf99830613dface09cb63378d4e37e9ed58ea 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -18,10 +18,12 @@ from multiprocessing import Queue, Lock, shared_memory
def getPlanningArgs(parser):
parser.add_argument('--planning-robot-params-file', type=str,
- default='/home/gospodar/lund/praxis/projects/ur_simple_control/python/ur_simple_control/path_generation/robot_params.yaml',
+ #default='/home/gospodar/lund/praxis/projects/ur_simple_control/python/ur_simple_control/path_generation/robot_params.yaml',
+ default='/home/gospodar/colcon_venv/ur_simple_control/python/ur_simple_control/path_generation/robot_params.yaml',
help="path to robot params file, required for path planning because it takes kinematic constraints into account")
parser.add_argument('--tunnel-mpc-params-file', type=str,
- default='/home/gospodar/lund/praxis/projects/ur_simple_control/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml',
+ #default='/home/gospodar/lund/praxis/projects/ur_simple_control/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml',
+ default='/home/gospodar/colcon_venv/ur_simple_control/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml',
help="path to mpc (in original tunnel) params file, required for path planning because it takes kinematic constraints into account")
parser.add_argument('--n-pol', type=int,
default='0',
diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py b/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py
index 01774898e12eb7e3aba235b9e662276522e720ca..772056d31d8fee4925631d0a826df6bfbb310afd 100644
--- a/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py
+++ b/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py
@@ -5,11 +5,11 @@ import numpy as np
class MobileRobot(ABC):
- def __init__(self, nu, nx, radius, name, u_min=None, u_max=None, x_min=None, x_max=None,
+ def __init__(self, nu, nx, width, name, u_min=None, u_max=None, x_min=None, x_max=None,
integration_method='euler'):
self.nx = nx
self.nu = nu
- self.radius = radius
+ self.width = width
self.name = name
def valid_u_bound(bound): return bound is not None and len(bound) == self.nu
def valid_q_bound(bound): return bound is not None and len(bound) == self.nx
@@ -55,15 +55,15 @@ class MobileRobot(ABC):
# def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10, **kwargs):
# if ax is None:
# _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
-# if self.radius == 0:
+# if self.width == 0:
# handles = plt.plot(0, 0, '+', color=color, alpha=alpha, markersize=markersize, label=self.name, **kwargs)
# else:
-# handles = [patches.Circle((0, 0), self.radius, color=color, alpha=alpha, label=self.name, **kwargs)]
+# handles = [patches.Circle((0, 0), self.width, color=color, alpha=alpha, label=self.name, **kwargs)]
# ax.add_patch(handles[0])
# return handles, ax
#
# def update_plot(self, x, handles):
-# if self.radius == 0:
+# if self.width == 0:
# handles[0].set_data(*self.h(x))
# else:
# handles[0].set(center=self.h(x))
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
index f45c70919684dc135f35391ab63f8d9e19989cbb..21a305dc4d6e70e9b3fda722cf1e3cd64d5275d1 100644
--- a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
@@ -1,3 +1,10 @@
Metadata-Version: 2.1
-Name: star_navigation
+Name: star-navigation
Version: 1.0
+Summary: UNKNOWN
+Home-page: UNKNOWN
+License: UNKNOWN
+Platform: UNKNOWN
+
+UNKNOWN
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
index a108d483e9240fcd5282f5f14888656a3c3e862d..bf0f73b58d66077ae4694538ff3f9339bc797597 100644
--- a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
@@ -1,5 +1,3 @@
-.gitignore
-.gitmodules
README.md
setup.py
config/__init__.py
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..c335b9889a6cd24920756fa4ba62ccd215516f12
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
@@ -0,0 +1,10 @@
+Metadata-Version: 2.1
+Name: starworld-tunnel-mpc
+Version: 1.0
+Summary: UNKNOWN
+Home-page: UNKNOWN
+License: UNKNOWN
+Platform: UNKNOWN
+
+UNKNOWN
+
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0be315e9b16ce2ac41854e6f504645a804a3ed61
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
@@ -0,0 +1,30 @@
+README.md
+setup.py
+config/__init__.py
+config/load_config.py
+config/scene.py
+motion_control/__init__.py
+motion_control/soads/__init__.py
+motion_control/soads/soads.py
+motion_control/tunnel_mpc/__init__.py
+motion_control/tunnel_mpc/path_generator.py
+motion_control/tunnel_mpc/tunnel_mpc.py
+motion_control/tunnel_mpc/tunnel_mpc_controller.py
+motion_control/tunnel_mpc/workspace_modification.py
+motion_control/tunnel_mpc_convergence/__init__.py
+motion_control/tunnel_mpc_convergence/path_generator.py
+motion_control/tunnel_mpc_convergence/tunnel_mpc.py
+motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py
+motion_control/tunnel_mpc_convergence/workspace_modification.py
+robot/__init__.py
+robot/bicycle.py
+robot/mobile_robot.py
+robot/omnidirectional.py
+robot/unicycle.py
+starworld_tunnel_mpc.egg-info/PKG-INFO
+starworld_tunnel_mpc.egg-info/SOURCES.txt
+starworld_tunnel_mpc.egg-info/dependency_links.txt
+starworld_tunnel_mpc.egg-info/requires.txt
+starworld_tunnel_mpc.egg-info/top_level.txt
+visualization/__init__.py
+visualization/scene_gui.py
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c75a0c3a268cd969c655a6052e60cad8589bab36
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
@@ -0,0 +1,7 @@
+casadi
+matplotlib
+numpy
+opengen
+pyyaml
+scipy
+shapely
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/top_level.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3242cb585c2d47f3c9b545aaa444cc3bf839cc74
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/top_level.txt
@@ -0,0 +1,4 @@
+config
+motion_control
+robot
+visualization
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
index 4dee13fc6c070e01a3bca7b804984f6d9645b9f5..70433406afc348c8d243871e3bac6a7fc3c41903 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
@@ -1,9 +1,11 @@
Metadata-Version: 2.1
Name: starworlds
Version: 1.0
+Summary: UNKNOWN
+Home-page: UNKNOWN
+License: UNKNOWN
+Platform: UNKNOWN
License-File: LICENSE
-Requires-Dist: pyyaml
-Requires-Dist: numpy
-Requires-Dist: scipy
-Requires-Dist: matplotlib
-Requires-Dist: shapely
+
+UNKNOWN
+
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt
index 853123cb2e00b78f079beab8e29ceb9251e378d7..bad89832078a424410d19018171475813882ed9a 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/SOURCES.txt
@@ -1,4 +1,3 @@
-.gitignore
LICENSE
README.md
requirements.txt
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
index f849890421afe2baa719e8822ed041b4c0ea68da..16fb7174f640ff4a00107b606bbf4abef70ae6e2 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
@@ -1,5 +1,5 @@
-pyyaml
+matplotlib
numpy
+pyyaml
scipy
-matplotlib
shapely
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
index 3c8fc8dc0aef0b7824804dd26c62c026082113cd..654427aae8dcbedf493f3a356f4a6e5732f76014 100644
Binary files a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc
index 604494c47ace559fed14c143479ef69e2f44abc8..b21ae860d8993fbb7c2b56d3cb77e67b296b9dff 100644
Binary files a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
index 41c97b7686410f2feed21cab462d3e8c926d6c9c..49efa1685621d359f4209efe2cdfd6fac8a86bfa 100644
Binary files a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
index 2850cb14d4726a62def37e709cf6bb52d3b5cf17..f3163af30f2c40fddc884a5ec95aabd5f6cbf222 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc
index a9d82f13280e4f3e31a0b9ed7c941fa52e5621e6..f3e4e64d35092a39fd51bda1a2b81e6f0f0f1341 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
index 88efef940c25032d0c34f0ec64c58af7190861fb..66a8209bfd54f39391e1003fc8c1a3e7a66d382b 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
index f4f3e5fc87b29baec3f22e19efb26294ddcf7091..86c35e0c7f317e7d72788853f7280a7d2570a6c3 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc differ