From 7aeb6719eb5a92fd041d5f15e4bd2a5053ff7b23 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 6 Feb 2025 12:24:03 +0100
Subject: [PATCH] first commit to refactoring branch. everything will be broken
until it isn't, and then we merge back to main
---
Dockerfile_no_conda | 23 +-
development_plan.toml | 19 ++
python/examples/graz/clik_client.py | 2 +-
.../basics/__pycache__/basics.cpython-311.pyc | Bin 13750 -> 13750 bytes
.../control/control_loop_manager.py | 239 ++++++++++++++++++
.../multiprocessing/multiprocess_manager.py | 6 +
.../multiprocessing/process_manager.py | 222 ++++++++++++++++
.../robots/robotmanager_abstract.py | 2 +
python/ur_simple_control/robots/utils.py | 189 +++++++++++++-
9 files changed, 678 insertions(+), 24 deletions(-)
create mode 100644 python/ur_simple_control/control/control_loop_manager.py
create mode 100644 python/ur_simple_control/multiprocessing/multiprocess_manager.py
create mode 100644 python/ur_simple_control/multiprocessing/process_manager.py
diff --git a/Dockerfile_no_conda b/Dockerfile_no_conda
index d9735f9..dbaec25 100644
--- a/Dockerfile_no_conda
+++ b/Dockerfile_no_conda
@@ -27,6 +27,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
zsh-syntax-highlighting \
libarchive-tools \
libxcb-cursor0\
+ libgomp1 \
python3-python-qt-binding
# qt-binding is a really unnecessary 300MB, but i don't want
@@ -54,31 +55,9 @@ COPY --chown=student /dot_files_for_docker/.zshrc /home/student/
COPY --chown=student /dot_files_for_docker/global_extra_conf.py /home/student/
-# sh does not have sourcing
-# and some packages (conda) want shell environment variables
-# (which i can say a lot about, but can't do anything about)
-# ((the only reason to even use conda is to not have to compile pinocchio))
-SHELL ["/bin/bash", "--login", "-c"]
-#SHELL ["/bin/bash"]
-
-# this is enough to run clik
WORKDIR /home/student/
USER student
-# TODO: install casadi and pinochio 3.0+
-# TODO: verify this stuff below works
-# --> this can be done with conda
-#RUN mkdir -p ~/miniconda3
-#RUN wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O /home/student/miniconda3/miniconda.sh
-#RUN bash /home/student/miniconda3/miniconda.sh -b -u -p ~/miniconda3
-#RUN rm /home/student/miniconda3/miniconda.sh
-#ENV PATH=/home/student/miniconda3/bin:$PATH
-#RUN source /home/student/miniconda3/bin/activate
RUN pip install -e ./SimpleManipulatorControl/python/
-#RUN conda config --add channels conda-forge
-#RUN conda install --solver=classic conda-forge::conda-libmamba-solver conda-forge::libmamba conda-forge::libmambapy conda-forge::libarchive
-#RUN conda install --solver=classic -y pinocchio crocoddyl -c conda-forge
-#RUN conda install -y pinocchio crocoddyl -c conda-forge
-#RUN conda install -y opencv
RUN pip install pinocchio crocoddyl matplotlib meshcat ur_rtde argcomplete \
qpsolvers ecos example_robot_data meshcat_shapes \
pyqt6 opencv-python
diff --git a/development_plan.toml b/development_plan.toml
index e68eb50..7461417 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -169,6 +169,25 @@ hours_required = 4
is_done = false
+[[project_plan.action_items.action_items]]
+name = "actual process management"
+description = """
+plotter process being owned by controlloopmanager, and visualizer process being owned by robotmanager works fine.
+
+however, we now have more processes to manage and it's not obvious who should take ownership of them: camera (computer vision) process, networking processes, and future ones which will be added (doing more stuff in more processes seems to be a trend). forcing the user to manually terminate them or to keep adding try-except KeyboardInterrupt blocks dramatically decreases the value of the library because all this is annoying as fuck and makes it seems broken.
+
+so the bottom line is we need an actual process management class - not for individual processes, but for all of them. this class should, upon termination, do what stophandler in controlloopmanager is doing now. it should be extensible in the sense that you can register however many processes, and it takes ownership of all of them.
+
+one possibly problematic thing is that control might have to be moved from the main thread, which immediately makes everything annoying - the transition between different controllers has to be on the same thread. writing examples must not be polluted with writing one massive function that's then passed ran as a side process. idk, think of what to do here as a part of the todo.
+"""
+priority = 2
+deadline = 2025-01-31
+dependencies = ["robotmanager abstract class"]
+purpose = """actual process management beyond ducktaping plotter and visualizer to loopmanager and robotmanager"""
+hours_required = 5
+is_done = false
+
+
[[project_plan.action_items.action_items]]
name = "velocity vs torque control"
description = """
diff --git a/python/examples/graz/clik_client.py b/python/examples/graz/clik_client.py
index 59d71a8..e4473e5 100644
--- a/python/examples/graz/clik_client.py
+++ b/python/examples/graz/clik_client.py
@@ -50,7 +50,7 @@ def controlLoopClikExternalGoal(
T_w_e = robot.getT_w_e()
data = receiver.getData()
T_goal = data["T_goal"]
- print(T_goal)
+ # print(T_goal)
# print("translation:", T_goal.translation)
# print("rotation:", pin.Quaternion(T_goal.rotation))
SEerror = T_w_e.actInv(T_goal)
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-311.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-311.pyc
index 3100ec7ba0ff30ee9bd30b615348d3ea9b94afcf..a5cc509832e4e365bc4c25ee5cdedce833cce2f9 100644
GIT binary patch
delta 20
ccmdm%y)B!2IWI340|Ns?<og91xmTM407eZ4od5s;
delta 20
bcmdm%y)B!2IWI340|NuY`_6!k+^bCiMl%L|
diff --git a/python/ur_simple_control/control/control_loop_manager.py b/python/ur_simple_control/control/control_loop_manager.py
new file mode 100644
index 0000000..087926d
--- /dev/null
+++ b/python/ur_simple_control/control/control_loop_manager.py
@@ -0,0 +1,239 @@
+class ControlLoopManager:
+ """
+ ControlLoopManager
+ -------------------
+ Slightly fancier programming (passing a function as argument and using functools.partial)
+ to get a wrapper around the control loop.
+ In other words, it's the book-keeping around the actual control loop.
+ It's a class because it keeps non-directly-control-loop-related parameters
+ like max_iterations, what data to save etc.
+ NOTE: you give this the ready-made control loop.
+ if it has arguments, bake them in with functools.partial.
+ Handles short-term data saving and logging.
+ Details on this are given below.
+
+ Short term data saving:
+ - it's a dictionaries of deques (initialized here), because deque is the most convenient class
+ for removing the first element and appending a last (it is just a linked list under the hood of course).
+ - it's a dictionary for modularity's sake, because this way you can save whatever you want
+ - and it will just work based on dictionary keys.
+ - it is the user's resposibility to make sure they're providing correct data.
+ - --> TODO but make an assert for the keys at least
+ - in the c++ imlementation, make the user write their own struct or something.
+ - since this is python, you need to give me initial values to infer types.
+ - you need to provide initial values to populate the deque to start anyway.
+
+ Logging data (for analysis and plotting):
+ - it can only be handled here because the control loop itself only cares about the present/
+ a small time-window around it.
+ - saves it all in a dictionary of ndarrays (initialized here), returns that after a run
+ TODO: it's provided by the user now, make it actually initialize here!!!
+ - you need to specify which keys you're using to do the initialization
+ - later, the controlLoop needs to return what's to be save in a small temporary dict.
+ - NOTE: this is of course a somewhat redundant solution, but it's the simplest
+ and most flexible way of doing it.
+ it probably will be done some other way down the line, but it works and is not
+ a priority right now
+
+ Other info:
+ - relies on RobotManager to handle all the magic numbers
+ that are not truly only control loop related
+
+ """
+
+ def __init__(self, robot_manager, controlLoop, args, save_past_item, log_item):
+ signal.signal(signal.SIGINT, self.stopHandler)
+ self.pid = getpid()
+ self.max_iterations = args.max_iterations
+ self.robot_manager = robot_manager
+ 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).
+ # the good thing is this way you also immediatelly put in the initial values
+ for key in save_past_item:
+ self.past_data[key] = deque()
+ # immediatelly populate every deque with initial values
+ for i in range(self.args.past_window_size):
+ # deepcopy just in case, better safe than sorry plus it's during initialization,
+ # not real time
+ self.past_data[key].append(copy.deepcopy(save_past_item[key]))
+
+ # similar story for log_dict as for past_data,
+ # except this is not used in the control loop,
+ # we don't predeclare sizes, but instead
+ # just shove items into linked lists (python lists) in dictionaries (hash-maps)
+ self.log_dict = {}
+ for key in log_item:
+ self.log_dict[key] = []
+
+ if self.args.plotter:
+ self.plotter_manager = ProcessManager(args, realTimePlotter, log_item, 0)
+
+ def run_one_iter(self, i):
+ """
+ run
+ ---
+ do timing to run at 500Hz.
+ also handle the number of iterations.
+ 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.visualizer:
+ if self.robot_manager.robot_name != "yumi":
+ self.robot_manager.visualizer_manager.sendCommand(
+ {
+ "q": self.robot_manager.q,
+ "T_w_e": self.robot_manager.getT_w_e(),
+ }
+ )
+ else:
+ T_w_e_left, T_w_e_right = self.robot_manager.getT_w_e()
+ self.robot_manager.visualizer_manager.sendCommand(
+ {"q": self.robot_manager.q, "T_w_e": T_w_e_left}
+ )
+ 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.robot_name == "yumi":
+ # T_base = self.robot_manager.data.oMi[1]
+ # self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
+
+ if self.args.visualize_collision_approximation:
+ pass
+ # TODO: here call robot manager's update ellipses function
+
+ if self.args.plotter:
+ # 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()
+ breakFlag = self.run_one_iter(i)
+
+ # break if done
+ if breakFlag:
+ break
+
+ # sleep for the rest of the frequency cycle
+ end = time.time()
+ diff = end - start
+ if self.robot_manager.dt < diff:
+ if self.args.debug_prints:
+ print("missed deadline by", diff - self.robot_manager.dt)
+ continue
+ else:
+ # there's no point in sleeping if we're in simulation
+ # NOTE: it literally took me a year to put this if here
+ # and i have no idea why i didn't think of it before lmao
+ # (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
+ ######################################################################
+ if self.args.plotter:
+ self.plotter_manager.terminateProcess()
+ if self.args.save_log:
+ self.robot_manager.log_manager.storeControlLoopRun(
+ self.log_dict, self.controlLoop.func.__name__, self.final_iteration
+ )
+ if i < self.max_iterations - 1:
+ if self.args.debug_prints:
+ print("success in", i, "iterations!")
+ else:
+ print("FAIL: did not succed in", self.max_iterations, "iterations")
+ # self.stopHandler(None, None)
+
+ def stopHandler(self, signum, frame):
+ """
+ stopHandler
+ -----------
+ upon receiving SIGINT it sends zeros for speed commands to
+ stop the robot.
+ NOTE: apparently this isn't enough,
+ nor does stopJ do anything, so it goes to freedriveMode
+ and then exits it, which actually stops ur robots at least.
+ """
+ # when we make a new process, it inherits signal handling.
+ # which means we call this more than once.
+ # and that could lead to race conditions.
+ # but if we exit immediatealy it's fine
+ if getpid() != self.pid:
+ return
+ print("sending 300 speedjs full of zeros and exiting")
+ for i in range(300):
+ vel_cmd = np.zeros(self.robot_manager.model.nv)
+ self.robot_manager.sendQd(vel_cmd)
+
+ # hopefully this actually stops it
+ if not self.args.pinocchio_only:
+ self.robot_manager.rtde_control.speedStop(1)
+ print("sending a stopj as well")
+ self.robot_manager.rtde_control.stopJ(1)
+ print("putting it to freedrive for good measure too")
+ self.robot_manager.rtde_control.freedriveMode()
+
+ if self.args.save_log:
+ print("saving log")
+ # this does not get run if you exited with ctrl-c
+ self.robot_manager.log_manager.storeControlLoopRun(
+ self.log_dict, self.controlLoop.func.__name__, self.final_iteration
+ )
+ self.robot_manager.log_manager.saveLog()
+
+ if self.args.plotter:
+ self.plotter_manager.terminateProcess()
+
+ if self.args.visualizer:
+ self.robot_manager.visualizer_manager.terminateProcess()
+
+ # TODO: this obviously only makes sense if you're on ur robots
+ # so this functionality should be wrapped in robotmanager
+ if not self.args.pinocchio_only:
+ self.robot_manager.rtde_control.endFreedriveMode()
+
+ exit()
diff --git a/python/ur_simple_control/multiprocessing/multiprocess_manager.py b/python/ur_simple_control/multiprocessing/multiprocess_manager.py
new file mode 100644
index 0000000..0575f53
--- /dev/null
+++ b/python/ur_simple_control/multiprocessing/multiprocess_manager.py
@@ -0,0 +1,6 @@
+# TODO:
+# eventually have a single thing that takes ownership of all processes
+# in a uniform manner.
+# the primary job this has is to cleanly close all side_processes.
+# the secondary job this has is to do all the clean exiting once the last control loop is over
+# that should be it - no more and no less.
diff --git a/python/ur_simple_control/multiprocessing/process_manager.py b/python/ur_simple_control/multiprocessing/process_manager.py
new file mode 100644
index 0000000..55605e2
--- /dev/null
+++ b/python/ur_simple_control/multiprocessing/process_manager.py
@@ -0,0 +1,222 @@
+from multiprocessing import Process, Queue, Lock, shared_memory
+
+
+class ProcessManager:
+ """
+ ProcessManager
+ --------------
+ A way to do processing in a thread (process because GIL) different
+ from the main one which is reserved
+ for ControlLoopManager.
+ The primary goal is to process visual input
+ from the camera without slowing down control.
+ TODO: once finished, map real-time-plotting and
+ visualization with this (it already exists, just not in a manager).
+ What this should do is hide the code for starting a process,
+ and to enforce the communication defined by the user.
+ To make life simpler, all communication is done with Queues.
+ There are two Queues - one for commands,
+ and the other for receiving the process' output.
+ Do note that this is obviously not the silver bullet for every
+ possible inter-process communication scenario,
+ but the aim of this library is to be as simple as possible,
+ not as most performant as possible.
+ NOTE: the maximum number of things in the command queue is arbitrarily
+ set to 5. if you need to get the result immediately,
+ calculate whatever you need in main, not in a side process.
+ this is meant to be used for operations that take a long time
+ and aren't critical, like reading for a camera.
+ """
+
+ # NOTE: theoretically we could pass existing queues so that we don't
+ # need to create new ones, but i won't optimize in advance
+ def __init__(
+ self, args, side_function, init_command, comm_direction, init_value=None
+ ):
+ self.args = args
+ self.comm_direction = comm_direction
+
+ # send command to slave process
+ if comm_direction == 0:
+ self.command_queue = Queue()
+ self.side_process = Process(
+ target=side_function,
+ args=(
+ args,
+ init_command,
+ self.command_queue,
+ ),
+ )
+ # get data from slave process
+ if comm_direction == 1:
+ self.data_queue = Queue()
+ self.side_process = Process(
+ target=side_function,
+ args=(
+ args,
+ init_command,
+ self.data_queue,
+ ),
+ )
+ # share data in both directions via shared memory with 2 buffers
+ # - one buffer for master to slave
+ # - one buffer for slave to master
+ if comm_direction == 2:
+ self.command_queue = Queue()
+ self.data_queue = Queue()
+ self.side_process = Process(
+ target=side_function,
+ args=(
+ args,
+ init_command,
+ self.command_queue,
+ self.data_queue,
+ ),
+ )
+ # shared memory both ways
+ # one lock because i'm lazy
+ # but also, it's just copy-pasting
+ # we're in python, and NOW do we get picky with copy-pasting???????
+ if comm_direction == 3:
+ # "sending" the command via shared memory
+ # TODO: the name should be random and send over as function argument
+ shm_name = "command"
+ self.shm_cmd = shared_memory.SharedMemory(
+ shm_name, create=True, size=init_command.nbytes
+ )
+ self.shared_command = np.ndarray(
+ init_command.shape, dtype=init_command.dtype, buffer=self.shm_cmd.buf
+ )
+ self.shared_command[:] = init_command[:]
+ # same lock for both
+ self.shared_command_lock = Lock()
+ # getting data via different shared memory
+ shm_data_name = "data"
+ # size is chosen arbitrarily but 10k should be more than enough for anything really
+ self.shm_data = shared_memory.SharedMemory(
+ shm_data_name, create=True, size=10000
+ )
+ # initialize empty
+ p = pickle.dumps(None)
+ self.shm_data.buf[: len(p)] = p
+ # the process has to create its shared memory
+ self.side_process = Process(
+ target=side_function,
+ args=(
+ args,
+ init_command,
+ shm_name,
+ self.shared_command_lock,
+ self.shm_data,
+ ),
+ )
+ # networking client (server can use comm_direction 0)
+ if comm_direction == 4:
+ from ur_simple_control.networking.util import DictPb2EncoderDecoder
+
+ self.encoder_decoder = DictPb2EncoderDecoder()
+ self.msg_code = self.encoder_decoder.dictToMsgCode(init_command)
+ # TODO: the name should be random and send over as function argument
+ shm_name = "client_socket" + str(np.random.randint(0, 1000))
+ # NOTE: size is max size of the recv buffer too,
+ # and the everything blows up if you manage to fill it atm
+ self.shm_msg = shared_memory.SharedMemory(shm_name, create=True, size=1024)
+ # need to initialize shared memory with init value
+ # NOTE: EVIL STUFF SO PICKLING ,READ NOTES IN networking/client.py
+ # init_val_as_msg = self.encoder_decoder.dictToSerializedPb2Msg(init_value)
+ # self.shm_msg.buf[:len(init_val_as_msg)] = init_val_as_msg
+ pickled_init_value = pickle.dumps(init_value)
+ self.shm_msg.buf[: len(pickled_init_value)] = pickled_init_value
+ self.lock = Lock()
+ self.side_process = Process(
+ target=side_function, args=(args, init_command, shm_name, self.lock)
+ )
+ if type(side_function) == partial:
+ self.side_process.name = side_function.func.__name__
+ else:
+ self.side_process.name = side_function.__name__ + "_process"
+ self.latest_data = init_value
+
+ self.side_process.start()
+ if self.args.debug_prints:
+ print(f"PROCESS_MANAGER: i am starting {self.side_process.name}")
+
+ # TODO: enforce that
+ # the key should be a string containing the command,
+ # and the value should be the data associated with the command,
+ # just to have some consistency
+ def sendCommand(self, command: typing.Union[dict, np.ndarray]):
+ """
+ sendCommand
+ ------------
+ assumes you're calling from controlLoop and that
+ you want a non-blocking call.
+ the maximum number of things in the command queue is arbitrarily
+ set to 5. if you need to get the result immediately,
+ calculate whatever you need in main, not in a side process.
+
+ if comm_direction == 3:
+ sendCommandViaSharedMemory
+ ---------------------------
+ instead of having a queue for the commands, have a shared memory variable.
+ this makes sense if you want to send the latest command only,
+ instead of stacking up old commands in a queue.
+ the locking and unlocking of the shared memory happens here
+ and you don't have to think about it in the control loop nor
+ do you need to pollute half of robotmanager or whatever else
+ to deal with this.
+ """
+ if self.comm_direction != 3:
+ if self.command_queue.qsize() < 5:
+ self.command_queue.put_nowait(command)
+
+ if self.comm_direction == 3:
+ assert type(command) == np.ndarray
+ assert command.shape == self.shared_command.shape
+ self.shared_command_lock.acquire()
+ self.shared_command[:] = command[:]
+ self.shared_command_lock.release()
+
+ def getData(self):
+ if self.comm_direction < 3:
+ if not self.data_queue.empty():
+ self.latest_data = self.data_queue.get_nowait()
+ if self.comm_direction == 3:
+ self.shared_command_lock.acquire()
+ # here we should only copy, release the lock, then deserialize
+ self.latest_data = pickle.loads(self.shm_data.buf)
+ self.shared_command_lock.release()
+ if self.comm_direction == 4:
+ self.lock.acquire()
+ # data_copy = copy.deepcopy(self.shm_msg.buf)
+ # REFUSES TO WORK IF YOU DON'T PRE-CROP HERE!!!
+ # MAKES ABSOLUTELY NO SENSE!!! READ MORE IN ur_simple_control/networking/client.py
+ # so we're decoding there, pickling, and now unpickling.
+ # yes, it's incredibly stupid
+ # new_data = self.encoder_decoder.serializedPb2MsgToDict(self.shm_msg.buf, self.msg_code)
+ new_data = pickle.loads(self.shm_msg.buf)
+ self.lock.release()
+ if len(new_data) > 0:
+ self.latest_data = new_data
+ # print("new_data", new_data)
+ # print("self.latest_data", self.latest_data)
+ # self.latest_data = self.encoder_decoder.serializedPb2MsgToDict(data_copy, self.msg_code)
+ return copy.deepcopy(self.latest_data)
+
+ def terminateProcess(self):
+ if self.comm_direction == 3:
+ self.shm_cmd.close()
+ self.shm_cmd.unlink()
+ if (self.comm_direction != 3) and (self.comm_direction != 1):
+ if self.args.debug_prints:
+ print(
+ f"i am putting befree in {self.side_process.name}'s command queue to stop it"
+ )
+ self.command_queue.put_nowait("befree")
+ try:
+ self.side_process.terminate()
+ if self.args.debug_prints:
+ print(f"terminated {self.side_process.name}")
+ except AttributeError:
+ if self.args.debug_prints:
+ print(f"{self.side_process.name} is dead already")
diff --git a/python/ur_simple_control/robots/robotmanager_abstract.py b/python/ur_simple_control/robots/robotmanager_abstract.py
index d097fad..5d1a3e8 100644
--- a/python/ur_simple_control/robots/robotmanager_abstract.py
+++ b/python/ur_simple_control/robots/robotmanager_abstract.py
@@ -32,6 +32,7 @@ class RobotManagerAbstract(abc.ABC):
self.visual_model: pin.pinocchio_pywrap_default.GeometryModel
self.collision_model: pin.pinocchio_pywrap_default.GeometryModel
+ # TODO: make a default
if args.save_log:
self._log_manager = LogManager(args)
@@ -67,6 +68,7 @@ class RobotManagerAbstract(abc.ABC):
# since there is only one robot and one visualizer, this is robotmanager's job
self.visualizer_manager: None | ProcessManager
self.visualizer_manager = None
+ # TODO: this should be transferred to an actual process manager
if self.args.visualizer:
side_function = partial(
manipulatorVisualizer,
diff --git a/python/ur_simple_control/robots/utils.py b/python/ur_simple_control/robots/utils.py
index de3be24..06a808d 100644
--- a/python/ur_simple_control/robots/utils.py
+++ b/python/ur_simple_control/robots/utils.py
@@ -1,8 +1,195 @@
import numpy as np
import pinocchio as pin
+import argparse
+
+
+def getMinimalArgParser():
+ """
+ getDefaultEssentialArgs
+ ------------------------
+ returns a parser containing:
+ - essential arguments (run in real or in sim)
+ - parameters for (compliant)moveJ
+ - parameters for (compliant)moveL
+ """
+ parser = argparse.ArgumentParser(
+ description="Run something with \
+ Simple Manipulator Control",
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter,
+ )
+ #################################################
+ # general arguments: connection, plotting etc #
+ #################################################
+ parser.add_argument(
+ "--robot",
+ type=str,
+ help="which robot you're running or simulating",
+ default="ur5e",
+ choices=["ur5e", "heron", "heronros", "gripperlessur5e", "mirros", "yumi"],
+ )
+ parser.add_argument(
+ "--simulation",
+ action=argparse.BooleanOptionalAction,
+ help="whether you are running the UR simulator",
+ default=False,
+ )
+ parser.add_argument(
+ "--robot-ip",
+ type=str,
+ help="robot's ip address (only needed if running on the real robot)",
+ default="192.168.1.102",
+ )
+ parser.add_argument(
+ "--pinocchio-only",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to just integrate with pinocchio",
+ default=True,
+ )
+ parser.add_argument(
+ "--ctrl-freq", type=int, help="frequency of the control loop", default=500
+ )
+ parser.add_argument(
+ "--fast-simulation",
+ action=argparse.BooleanOptionalAction,
+ help="do you want simulation to as fast as possible? (real-time viz relies on 500Hz)",
+ default=False,
+ )
+ parser.add_argument(
+ "--visualizer",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize the manipulator and workspace with meshcat",
+ default=True,
+ )
+ parser.add_argument(
+ "--plotter",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)",
+ default=True,
+ )
+ parser.add_argument(
+ "--gripper",
+ type=str,
+ help="gripper you're using (no gripper is the default)",
+ default="none",
+ choices=["none", "robotiq", "onrobot"],
+ )
+ # TODO: make controlloop manager run in a while True loop and remove this
+ # ==> max-iterations actually needs to be an option. sometimes you want to simulate and exit
+ # if the convergence does not happen in a reasonable amount of time,
+ # ex. goal outside of workspace has been passed or something
+ # =======> if it's set to 0 then the loops run infinitely long
+ parser.add_argument(
+ "--max-iterations",
+ type=int,
+ help="maximum allowable iteration number (it runs at 500Hz)",
+ default=100000,
+ )
+ parser.add_argument(
+ "--speed-slider",
+ type=float,
+ help="cap robot's speed with the speed slider \
+ to something between 0 and 1, 0.5 by default \
+ BE CAREFUL WITH THIS.",
+ default=1.0,
+ )
+ parser.add_argument(
+ "--start-from-current-pose",
+ action=argparse.BooleanOptionalAction,
+ help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
+ very useful and convenient when running simulation before running on real",
+ default=False,
+ )
+ parser.add_argument(
+ "--acceleration",
+ type=float,
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.3. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means",
+ default=0.3,
+ )
+ parser.add_argument(
+ "--max-qd",
+ type=float,
+ help="robot's joint velocities [rad/s]. scalar positive constant, max 3.14, and default 0.5. \
+ BE CAREFUL WITH THIS. also note that wrist joints can go to 6.28 rad, but here \
+ everything is clipped to this one number.",
+ default=0.5,
+ )
+ parser.add_argument(
+ "--debug-prints",
+ action=argparse.BooleanOptionalAction,
+ help="print some debug info",
+ default=False,
+ )
+ parser.add_argument(
+ "--save-log",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to save the log of the run. it saves \
+ what you pass to ControlLoopManager. check other parameters for saving directory and log name.",
+ default=False,
+ )
+ parser.add_argument(
+ "--save-dir",
+ type=str,
+ help="path to where you store your logs. default is ./data, but if that directory doesn't exist, then /tmp/data is created and used.",
+ default="./data",
+ )
+ parser.add_argument(
+ "--run-name",
+ type=str,
+ help="name the whole run/experiment (name of log file). note that indexing of runs is automatic and under a different argument.",
+ default="latest_run",
+ )
+ parser.add_argument(
+ "--index-runs",
+ action=argparse.BooleanOptionalAction,
+ help="if you want more runs of the same name, this option will automatically assign an index to a new run (useful for data collection).",
+ default=False,
+ )
+ parser.add_argument(
+ "--past-window-size",
+ type=int,
+ help="how many timesteps of past data you want to save",
+ default=5,
+ )
+ # maybe you want to scale the control signal (TODO: NOT HERE)
+ parser.add_argument(
+ "--controller-speed-scaling",
+ type=float,
+ default="1.0",
+ help="not actually_used atm",
+ )
+ ########################################
+ # environment interaction parameters #
+ ########################################
+ parser.add_argument(
+ "--contact-detecting-force",
+ type=float, # default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+ default=2.8,
+ help="the force used to detect contact (collision) in the moveUntilContact function",
+ )
+ parser.add_argument(
+ "--minimum-detectable-force-norm",
+ type=float,
+ help="we need to disregard noise to converge despite filtering. \
+ a quick fix is to zero all forces of norm below this argument threshold.",
+ default=3.0,
+ )
+ # TODO make this work without parsing (or make it possible to parse two times)
+ # if (args.gripper != "none") and args.simulation:
+ # raise NotImplementedError('Did not figure out how to put the gripper in \
+ # the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ parser.add_argument(
+ "--visualize-collision-approximation",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize the collision approximation used in controllers with obstacle avoidance",
+ default=True,
+ )
+ return parser
+
# TODO: make robot-independent
-def defineGoalPointCLI(robot. robot):
+def defineGoalPointCLI(robot):
"""
defineGoalPointCLI
------------------
--
GitLab