diff --git a/docker/Dockerfile b/docker/Dockerfile
index 5e76c53bee717fbf83271a42a0a255fc51e2650d..2d2956b9073de3549538ae765e43ad3fca4fe31b 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -7,11 +7,6 @@ LABEL org.opencontainers.image.authors="marko.guberina@control.lth.se"
ARG DEBIAN_FRONTEND=noninteractive
ENV TZ=Europe/Stockholm
-# pinocchio install
-RUN apt install -qqy lsb-release curl
-RUN mkdir -p /etc/apt/keyrings curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc
-RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list
-
RUN apt-get update && apt-get install -y --no-install-recommends \
python3 \
python3-pip \
@@ -32,10 +27,20 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
zsh \
zsh-syntax-highlighting \
libarchive-tools \
- libxcb-cursor0\
- python3-python-qt-binding \
- robotpkg-py3*-pinocchio
+ libxcb-cursor0 \
+# python3-pyqt6 \
+# opencv dependencies
+ ffmpeg libsm6 libxext6
+
+# python3-python-qt-binding \
+# robotpkg-py3*-pinocchio
+
+# pinocchio install as debian package - compiled with numpy 1.26 so incompatible with pip which has numpy 2.2
+# necessary for ubuntu 22
+#RUN apt install -qqy lsb-release curl
+#RUN mkdir -p /etc/apt/keyrings curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc
+#RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list
# qt-binding is a really unnecessary 300MB, but i don't want
@@ -48,26 +53,26 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
#RUN yes | unminimize
# make the environment more usable
-# create user
-RUN useradd -m -s /bin/zsh -G sudo -u 1000 student
+# create user - jammy comes with user ubuntu with UID 1000, so now we do 1001
+RUN useradd -m -s /bin/zsh -G sudo -u 1001 student
WORKDIR /home/student/
RUN passwd -d student
USER student
# copy repo to workdir
RUN mkdir SimpleManipulatorControl
-COPY --chown=student .. ./SimpleManipulatorControl
+COPY --chown=student . ./SimpleManipulatorControl
RUN mkdir -p .cache/zsh/
-COPY --chown=student /dot_files_for_docker/.vimrc /home/student/
-COPY --chown=student /dot_files_for_docker/.zshrc /home/student/
-COPY --chown=student /dot_files_for_docker/global_extra_conf.py /home/student/
+COPY --chown=student /docker/dot_files_for_docker/.vimrc /home/student/
+COPY --chown=student /docker/dot_files_for_docker/.zshrc /home/student/
+COPY --chown=student /docker/dot_files_for_docker/global_extra_conf.py /home/student/
# this is enough to run clik
WORKDIR /home/student/
USER student
-RUN pip install -e ./SimpleManipulatorControl/python/
-RUN pip install crocoddyl matplotlib meshcat ur_rtde \
+RUN pip install --break-system-packages -e ./SimpleManipulatorControl/python/
+RUN pip install --break-system-packages pin crocoddyl matplotlib meshcat ur_rtde \
qpsolvers ecos example_robot_data meshcat_shapes \
pyqt6 opencv-python qpsolvers quadprog \
proxsuite casadi pin-pink
diff --git a/docs/installation.md b/docs/installation.md
index 3d92bd06e427f80dc7f045f046613a46fa4b0106..dce2c4d20cec307bb6fa3752fc5a85ecc2d46816 100644
--- a/docs/installation.md
+++ b/docs/installation.md
@@ -35,7 +35,7 @@ To proceed you need to have the Docker desktop GUI app running (open).
--------------------------------
5. [WINDOWS] open wsl (type wsl in powershell) and navigate to git folder (now in wsl) with "cd /mnt/Users/YOURUSERNAME/PATH_TO_GIT_FOLDER"
5. [MAC] open a terminal and navigate to git folder with "cd /mnt/Users/YOURUSERNAME/PATH_TO_GIT_FOLDER"
-6. build the image with "docker build -t ur_simple_control ." NOTE: if you are on mac and have an apple silicone processor,
+6. build the image with "docker build -t smc -f docker/Dockerfile ." NOTE: if you are on mac and have an apple silicone processor,
you might need to change the ubuntu version to one that works on the apple silicone processor.
#### pulling the image
@@ -45,7 +45,7 @@ To proceed you need to have the Docker desktop GUI app running (open).
### using the image
----------------------
-7. to run the image run "docker run --rm -it --net=host -e DISPLAY=$DISPLAY -v /tmp:/tmp ur_simple_control /bin/zsh".
+7. to run the image run "docker run --rm -it --net=host -e DISPLAY=$DISPLAY -v /tmp:/tmp smc /bin/zsh".
[Linux and MAC]: NOTE: FIRST RUN "xhost +" EVERY TIME before you run the image. otherwise docker can't open new windows,
so real-time-plotting won't work. [MAC] you need to have xquartz installed to have the xhost + command (look at installation for more).
NOTE! if you don't get real-time-plotting with xhost +, do xhost + 127.0.0.1 and replace -e DISPLAY=$DISPLAY with
diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
index 8414bc09ac264f10dbb95dcb77af4a0daa80ba0e..1a3f9dbc44f4365da06c867b03c9bd9e6f0fa1a0 100644
--- a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
+++ b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
@@ -14,7 +14,7 @@ from smc.control.cartesian_space.pink_p2p import (
from smc.control.cartesian_space.cartesian_space_point_to_point import moveL
from utils import get_args, constructInitialT_w_abs
-from dual_arm_cart_pulling_control_loop import DualArmCartPulling
+from dual_arm_cart_pulling_control_loop import DualArmCartPulling, DualArmCartPullingMPC
from fixed_path_planner import contructPath, fixedPathPlanner
import time
@@ -35,11 +35,11 @@ if __name__ == "__main__":
# TODO: before running on the real robot change this to just reading the current position!!
if args.planner:
robot._step()
- robot._q[0] = 9.0
- robot._q[1] = 4.0
+ robot._q[0] = 1.0
+ robot._q[1] = 0.6
robot._step()
x0 = np.concatenate([robot.q, robot.v])
- goal = np.array([0.5, 5.5])
+ goal = np.array([5.5, 5.0])
######################################################
# 1. initialize path planner, or instantiate pre-made path
@@ -74,7 +74,7 @@ if __name__ == "__main__":
################################################
pathSE3 = path2D_to_SE3(path_base, args.handlebar_height, np.pi)
init_rot = pathSE3[0].rotation @ pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0)
- init_rot = init_rot @ pin.rpy.rpyToMatrix(0.0, 0.0, np.pi)
+ init_rot = init_rot @ pin.rpy.rpyToMatrix(0.0, 0.0, np.pi / 2)
# T_w_absgoal = constructInitialT_w_abs(args, path_base, init_rot)
T_w_absgoal = constructInitialT_w_abs(args, path_base, pathSE3[0].rotation)
@@ -138,6 +138,7 @@ if __name__ == "__main__":
# TODO: for final demo replace this with disjoint controller
robot._mode = robot.control_mode.whole_body
DualArmCartPulling(args, robot, path_planner, T_absgoal_l, T_absgoal_r)
+ # DualArmCartPullingMPC(args, robot, path_planner, T_absgoal_l, T_absgoal_r)
if args.real:
robot.stopRobot()
diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
index 3c0b05cd1047b010d6e258d516331e76ac7fc4a9..6d784574a212a5741e756bb716c02c39ad5e82a9 100644
--- a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
+++ b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
@@ -16,6 +16,10 @@ from smc.path_generation.path_math.path_to_trajectory import (
from smc.control.controller_templates.path_following_template import (
PathFollowingFromPlannerCtrllLoopTemplate,
)
+from smc.path_generation.path_math.path2d_to_6d import (
+ path2D_to_SE3,
+)
+from smc.control.cartesian_space.ik_solvers import *
import numpy as np
from functools import partial
@@ -24,9 +28,186 @@ from argparse import Namespace
from pinocchio import SE3, log6
from collections import deque
from utils import initializePastData
+import pinocchio as pin
def DualArmCartPullingControlLoop(
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ ik_solver,
+ path2D_base: np.ndarray,
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ BaseAndDualEEPathFollowingMPCControlLoop
+ -----------------------------
+ cart pulling dual arm control loop
+ """
+ v_cmd = np.zeros(robot.model.nv)
+ p = robot.T_w_b.translation[:2]
+
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ # NOTE: can't initialize a priori now because we're not running immediately,
+ # hence nothing is initialized
+ if t == 0:
+ path2D_handlebar = initializePastData(
+ args, robot.T_w_abs, robot.T_w_b.translation[:2], float(max_base_v)
+ )
+ past_data["path2D"].clear()
+ past_data["path2D"].extend(
+ path2D_handlebar[i] for i in range(args.past_window_size)
+ )
+ trajectory_base = path2D_to_trajectory2D(args, path2D_base, max_base_v)
+ trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
+
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
+ trajectorySE3_l = []
+ trajectorySE3_r = []
+ for traj_pose in trajectorySE3_handlebar:
+ # trajectorySE3_l.append(T_absgoal_l.act(traj_pose))
+ # trajectorySE3_r.append(T_absgoal_r.act(traj_pose))
+ trajectorySE3_l.append(traj_pose.act(T_absgoal_l))
+ trajectorySE3_r.append(traj_pose.act(T_absgoal_r))
+
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"path": trajectory_base})
+ robot.visualizer_manager.sendCommand(
+ {"frame_path": trajectorySE3_handlebar}
+ )
+
+ # CONTROL BASE
+ robot.mode = robot.control_mode.base_only
+ trajectory_base_SE3 = path2D_to_SE3(trajectory_base[:, :2], 0.0, 0.0)
+ rotm_z = pin.rpy.rpyToMatrix(0.0, 0.0, np.pi)
+ for z in range(len(trajectory_base_SE3)):
+ trajectory_base_SE3[z].rotation = rotm_z @ trajectory_base_SE3[z].rotation
+
+ SEerror = robot.T_w_e.actInv(trajectory_base_SE3[-1])
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < 0.2:
+ V_path = (
+ 5 * pin.log6(trajectory_base_SE3[1].actInv(trajectory_base_SE3[2])).vector
+ )
+ err_vector += V_path
+ err_vector[3:] = err_vector[3:] * 2
+ J = robot.getJacobian()
+ v_cmd_base = ik_solver(J, err_vector)
+ if v_cmd_base is None:
+ v_cmd_base = dampedPseudoinverse(1e-2, J, err_vector)
+ v_cmd[:3] = v_cmd_base
+
+ # CONTROL ARMS
+ robot._mode = robot.control_mode.upper_body
+ SEerror_left = robot.T_w_l.actInv(trajectorySE3_l[0])
+ SEerror_right = robot.T_w_r.actInv(trajectorySE3_r[0])
+ err_vector_left = pin.log6(SEerror_left).vector
+ err_vector_right = pin.log6(SEerror_right).vector
+
+ err_vector = np.concatenate((err_vector_left, err_vector_right))
+ J = robot.getJacobian()
+
+ if args.ik_solver == "QPManipMax":
+ v_cmd_arms = QPManipMax(
+ J,
+ err_vector,
+ robot.computeManipulabilityIndexQDerivative(),
+ lb=-1 * robot.max_v,
+ ub=robot.max_v,
+ )
+ else:
+ v_cmd_arms = ik_solver(J, err_vector)
+ if v_cmd_arms is None:
+ v_cmd_arms = dampedPseudoinverse(1e-2, J, err_vector)
+ v_cmd[3:] = v_cmd_arms
+
+ robot._mode = robot.control_mode.whole_body
+ err_vector_ee_l = log6(robot.T_w_l.actInv(trajectorySE3_l[0]))
+ err_norm_ee_l = np.linalg.norm(err_vector_ee_l)
+ err_vector_ee_r = log6(robot.T_w_r.actInv(trajectorySE3_r[0]))
+ err_norm_ee_r = np.linalg.norm(err_vector_ee_r)
+ err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
+ log_item = {}
+ log_item["err_vec_ee_l"] = err_vector_ee_l.vector
+ log_item["err_norm_ee_l"] = err_norm_ee_l.reshape((1,))
+ log_item["err_vec_ee_r"] = err_vector_ee_r.vector
+ log_item["err_norm_ee_r"] = err_norm_ee_r.reshape((1,))
+ log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
+ save_past_item = {"path2D": p}
+ return v_cmd, save_past_item, log_item
+
+
+def DualArmCartPulling(
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ path_planner: ProcessManager | types.FunctionType,
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ CrocoEndEffectorPathFollowingMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq).
+ """
+ robot._mode = robot.control_mode.whole_body
+ # NOTE: i'm shoving these into the class here - bad style,
+ # but i don't
+ T_w_abs = robot.T_w_abs
+ ik_solver = getIKSolver(args, robot)
+
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+
+ path2D_handlebar = initializePastData(
+ args, T_w_abs, robot.T_w_b.translation[:2], float(max_base_v)
+ )
+
+ get_position = lambda robot: robot.q[:2]
+ DualArmCartPullingControlLoop_with_l_r = partial(
+ DualArmCartPullingControlLoop, T_absgoal_l, T_absgoal_r
+ )
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ik_solver,
+ DualArmCartPullingControlLoop_with_l_r,
+ args,
+ robot,
+ )
+
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee_l": np.zeros((6,)),
+ "err_norm_ee_l": np.zeros((1,)),
+ "err_vec_ee_r": np.zeros((6,)),
+ "err_norm_ee_r": np.zeros((1,)),
+ "err_norm_base": np.zeros((1,)),
+ }
+ save_past_dict = {"path2D": T_w_abs.translation[:2]}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ # actually put past data into the past window
+ loop_manager.past_data["path2D"].clear()
+ loop_manager.past_data["path2D"].extend(
+ path2D_handlebar[i] for i in range(args.past_window_size)
+ )
+
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
+
+
+def DualArmCartPullingMPCControlLoop(
T_absgoal_l: SE3,
T_absgoal_r: SE3,
ocp: CrocoOCP,
@@ -97,7 +278,7 @@ def DualArmCartPullingControlLoop(
return v_cmd, save_past_item, log_item
-def DualArmCartPulling(
+def DualArmCartPullingMPC(
args: Namespace,
robot: DualArmWholeBodyInterface,
path_planner: ProcessManager | types.FunctionType,
@@ -127,7 +308,7 @@ def DualArmCartPulling(
get_position = lambda robot: robot.q[:2]
DualArmCartPullingControlLoop_with_l_r = partial(
- DualArmCartPullingControlLoop, T_absgoal_l, T_absgoal_r
+ DualArmCartPullingMPCControlLoop, T_absgoal_l, T_absgoal_r
)
controlLoop = partial(
PathFollowingFromPlannerCtrllLoopTemplate,
diff --git a/examples/cart_pulling/control_sub_problems/fixed_path_planner.py b/examples/cart_pulling/control_sub_problems/fixed_path_planner.py
index 0116bc4edd42787726ab00630d8fa55ea765dbda..d13fbab073e5877bec2211f3b9bb06f1996525b8 100644
--- a/examples/cart_pulling/control_sub_problems/fixed_path_planner.py
+++ b/examples/cart_pulling/control_sub_problems/fixed_path_planner.py
@@ -26,6 +26,8 @@ def fixedPathPlanner(
path_base: np.ndarray,
p_base: np.ndarray,
) -> np.ndarray:
+ if p_base.shape[0] == 2:
+ p_base = np.append(p_base, 0.0)
distances = np.linalg.norm(p_base - path_base, axis=1)
index = np.argmin(distances)
# NOTE: bullshit heuristic to deal with the path intersecting with itself.
diff --git a/examples/cart_pulling/control_sub_problems/ros_dual_arm_cart_pulling.py b/examples/cart_pulling/control_sub_problems/ros_dual_arm_cart_pulling.py
new file mode 100644
index 0000000000000000000000000000000000000000..0107f8e325a26020257b24c3da47bcbe14d3053f
--- /dev/null
+++ b/examples/cart_pulling/control_sub_problems/ros_dual_arm_cart_pulling.py
@@ -0,0 +1,181 @@
+#!/usr/bin/env python3
+
+
+from smc.multiprocessing.smc_yumi_node import get_args, SMCMobileYuMiNode
+from smc.path_generation.maps.premade_maps import createSampleStaticMap
+from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3
+from smc.path_generation.planner import starPlanner
+from smc.multiprocessing import ProcessManager
+from smc.robots.abstract_robotmanager import AbstractRobotManager
+
+import numpy as np
+from smc.robots.implementations.mobile_yumi import RealMobileYumiRobotManager
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.control.cartesian_space import getClikArgs, getIKSolver
+from smc.control.joint_space.joint_space_point_to_point import moveJP
+from smc.control.cartesian_space.cartesian_space_point_to_point import (
+ controlLoopClik,
+ moveL,
+ moveLDualArm,
+ moveL,
+)
+from smc.control.optimal_control.croco_point_to_point.mpc.base_reference_mpc import (
+ CrocoBaseP2PMPC,
+)
+from smc.control.optimal_control.croco_path_following.mpc.base_reference_mpc import (
+ CrocoBasePathFollowingMPC,
+)
+from smc.control.cartesian_space.cartesian_space_trajectory_following import (
+ cartesianPathFollowingWithPlanner,
+)
+from smc.control.cartesian_space.pink_p2p import (
+ DualArmIKSelfAvoidanceViaEndEffectorSpheres,
+)
+
+from fixed_path_planner import contructPath, fixedPathPlanner
+from utils import constructInitialT_w_abs
+from dual_arm_cart_pulling_control_loop import DualArmCartPulling
+
+import pinocchio as pin
+import matplotlib
+from functools import partial
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+import numpy as np
+import time
+from copy import deepcopy
+
+
+# def main(args=None):
+def main(args=None):
+ # evil and makes args unusable but what can you do
+ args_smc = get_args()
+ assert args_smc.robot == "myumi"
+ robot = RealMobileYumiRobotManager(args_smc)
+ robot._step()
+ modes_and_loops = []
+ # you can't do terminal input with ros
+ # goal = robot.defineGoalPointCLI()
+ # T_w_absgoal = pin.SE3.Identity()
+ # T_w_absgoal.translation = np.array([0.5, 0.0, 0.5])
+ T_absgoal_l = pin.SE3.Identity()
+ T_absgoal_l.translation = np.array([0.0, -0.2, 0.0])
+ T_absgoal_r = pin.SE3.Identity()
+ T_absgoal_r.translation = np.array([0.0, 0.2, 0.0])
+
+ goal = np.array([2.5, 0.0])
+
+ ######################################################
+ # 1. initialize path planner, or instantiate pre-made path
+ # and make that a path planner, for the base
+ #################################################
+ if args_smc.planner:
+ planning_function = partial(starPlanner, goal)
+ # here we're following T_w_e reference so that's what we send
+ path_planner = ProcessManager(
+ args_smc, planning_function, robot.T_w_b.translation[:2], 3, None
+ )
+ _, map_as_list = createSampleStaticMap()
+ # TODO: put in real lab map
+ if args_smc.visualizer:
+ robot.sendRectangular2DMapToVisualizer(map_as_list)
+
+ data = None
+ while data is None:
+ path_planner.sendCommand(robot.T_w_b.translation[:2])
+ data = path_planner.getData()
+ time.sleep(1)
+
+ _, path_base = data
+ path_base = np.array(path_base).reshape((-1, 2))
+ path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
+ else:
+ path_base = contructPath(args_smc, robot)
+ path_planner = partial(fixedPathPlanner, [0], path_base)
+
+ ################################################
+ # 2. construct initial T_w_abs (starting middle of handlebar pose)
+ ################################################
+ pathSE3 = path2D_to_SE3(path_base, args_smc.handlebar_height, np.pi)
+ init_rot = pathSE3[0].rotation @ pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0)
+ init_rot = init_rot @ pin.rpy.rpyToMatrix(0.0, 0.0, np.pi)
+ # T_w_absgoal = constructInitialT_w_abs(args, path_base, init_rot)
+ T_w_absgoal = constructInitialT_w_abs(args_smc, path_base, pathSE3[0].rotation)
+
+ if args_smc.visualizer:
+ robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
+
+ ##################################################
+ # 3. get arms into comfy position
+ ####################################################
+ # print("putting arms to a comfy configuration")
+ mode_1 = AbstractRobotManager.control_mode.upper_body
+ loop_manager_jp = moveJP(robot._comfy_configuration, args_smc, robot, run=False)
+ modes_and_loops.append((mode_1, loop_manager_jp))
+
+ ###################################################
+ # 4. get base to starting pose
+ ###################################################
+ # print("getting base to start of path")
+ p_basegoal = path_base[0]
+ # NOTE: requires passing a reduced model to work because crocoddyl needs the model to formulate itself
+ # robot.mode = AbstractRobotManager.control_mode.base_only
+
+ ##########################################
+ # orient base to look at the goal (yes this should've be handled with basep2p)
+ ##########################################
+ # NOTE: gets stuck on real for some unknown reason
+ mode_2 = robot.control_mode.base_only
+ T_w_bgoal = pin.SE3(init_rot, p_basegoal)
+ args_smc_1 = deepcopy(args_smc)
+ args_smc_1.goal_error = 10
+ loop_manager_moveL_1 = moveL(args_smc_1, robot, T_w_bgoal.copy(), run=False)
+ modes_and_loops.append((mode_2, loop_manager_moveL_1))
+
+ # NOTE: alternative hack didn't work
+ # q_desired = np.zeros(robot.model.nq)
+ # q_desired[:2] = p_basegoal[:2]
+ # rp = pin.rpy.matrixToRpy(init_rot)
+ # q_desired[2] = np.cos(rp[2] + np.pi)
+ # q_desired[3] = np.sin(rp[2] + np.pi)
+ # q_desired[4:] = robot._comfy_configuration[4:]
+ # loop_manager_jp2 = moveJP(q_desired, args_smc, robot, run=False)
+ # modes_and_loops.append((mode_1, loop_manager_jp2))
+
+ ##########################################
+ # go down and grasp
+ #######################################
+ mode_3 = AbstractRobotManager.control_mode.upper_body
+ T_w_absgoal_1 = T_w_absgoal.copy()
+ T_w_absgoal_2 = T_w_absgoal.copy()
+ T_w_absgoal_1.translation[2] += 0.1
+ # print("going above handlebar")
+ loop_manager_ik_1 = DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+ T_w_absgoal, T_absgoal_l, T_absgoal_r, args_smc, robot, run=False
+ )
+ modes_and_loops.append((mode_3, loop_manager_ik_1))
+
+ # DualArmMoveL down to handlebar
+ # print("going down to handlebar")
+ # T_w_absgoal.translation[2] -= 0.1
+ loop_manager_ik_2 = DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+ T_w_absgoal_2, T_absgoal_l, T_absgoal_r, args_smc, robot, run=False
+ )
+ modes_and_loops.append((mode_3, loop_manager_ik_2))
+
+ mode_4 = robot.control_mode.whole_body
+ loop_manager_pulling = DualArmCartPulling(
+ args_smc, robot, path_planner, T_absgoal_l, T_absgoal_r, run=False
+ )
+ modes_and_loops.append((mode_4, loop_manager_pulling))
+
+ rclpy.init(args=args)
+
+ executor = MultiThreadedExecutor()
+ node = SMCMobileYuMiNode(args_smc, robot, modes_and_loops)
+ executor.add_node(node)
+ executor.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/examples/cart_pulling/technical_report/cart_pulling.log b/examples/cart_pulling/technical_report/cart_pulling.log
index 15cc14d968f6762a583cd13d62591af21d5b2bf1..aee3289971fdd93f7e561da6c05de72787bad83d 100644
--- a/examples/cart_pulling/technical_report/cart_pulling.log
+++ b/examples/cart_pulling/technical_report/cart_pulling.log
@@ -1,6 +1,6 @@
-This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 26 FEB 2025 11:47
+This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 13 MAR 2025 16:38
entering extended mode
- \write18 enabled.
+ restricted \write18 enabled.
%&-line parsing enabled.
**cart_pulling.tex
(./cart_pulling.tex
@@ -282,7 +282,7 @@ m-super/sfrm0500.pfb></usr/share/texmf-dist/fonts/type1/public/cm-super/sfrm070
0.pfb></usr/share/texmf-dist/fonts/type1/public/cm-super/sfrm1000.pfb></usr/sha
re/texmf-dist/fonts/type1/public/cm-super/sfrm1200.pfb></usr/share/texmf-dist/f
onts/type1/public/cm-super/sfrm1728.pfb>
-Output written on cart_pulling.pdf (8 pages, 288786 bytes).
+Output written on cart_pulling.pdf (8 pages, 288863 bytes).
PDF statistics:
131 PDF objects out of 1000 (max. 8388607)
81 compressed objects within 1 object stream
diff --git a/examples/cart_pulling/technical_report/cart_pulling.pdf b/examples/cart_pulling/technical_report/cart_pulling.pdf
index 9c4ffafbdc6ff237ebdc0e4f72fd896447bdadc1..eb4a4e10ca10d8d9554aeadd889292a37fd55baa 100644
Binary files a/examples/cart_pulling/technical_report/cart_pulling.pdf and b/examples/cart_pulling/technical_report/cart_pulling.pdf differ
diff --git a/examples/cart_pulling/technical_report/cart_pulling.synctex.gz b/examples/cart_pulling/technical_report/cart_pulling.synctex.gz
deleted file mode 100644
index fafbe7f80a80ebfacefe75076a23a4d1886c3977..0000000000000000000000000000000000000000
Binary files a/examples/cart_pulling/technical_report/cart_pulling.synctex.gz and /dev/null differ
diff --git a/examples/ros/myumi_cart_pulling.py b/examples/ros/myumi_cart_pulling.py
deleted file mode 100644
index 073bc7b61f16c13d0ad52602c5efe2655459aa00..0000000000000000000000000000000000000000
--- a/examples/ros/myumi_cart_pulling.py
+++ /dev/null
@@ -1,119 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.executors import MultiThreadedExecutor
-from smc_yumi_node import get_args, SMCMobileYuMiNode
-
-import numpy as np
-import argparse
-from smc.robots.implementations.mobile_yumi import RealMobileYumiRobotManager
-from smc import getMinimalArgParser
-from smc.control.control_loop_manager import ControlLoopManager
-from smc.control.cartesian_space import getClikArgs, getIKSolver
-from smc.control.joint_space.joint_space_point_to_point import moveJP
-from smc.control.cartesian_space.cartesian_space_point_to_point import (
- controlLoopClik,
- moveL,
- moveLDualArm,
- moveL,
-)
-from smc.control.optimal_control.croco_point_to_point.mpc.base_reference_mpc import (
- CrocoBaseP2PMPC,
-)
-from smc.control.optimal_control.croco_path_following.mpc.base_reference_mpc import (
- CrocoBasePathFollowingMPC,
-)
-from smc.util.draw_path import drawPath
-from smc.control.cartesian_space.cartesian_space_trajectory_following import (
- cartesianPathFollowingWithPlanner,
-)
-
-import pinocchio as pin
-import matplotlib
-from functools import partial
-
-import numpy as np
-
-
-def fixedPathPlanner(
- path_parameter: list[int], path2D: np.ndarray, p_base: np.ndarray
-) -> np.ndarray:
- """
- fixedPathPlanner
- ----------------
- we can assume robot is following this path and that it started the following controller at the first point.
- to make the controller make sense (without a sense of timing), we need to find the closest point on the path,
- and delete the already traversed portion of the path.
- the assumption is that the controller constructs a trajectory from the path, starting with the first point.
-
- the cleanest way to get this done is associate robot's current position with the path parameter s,
- but i'll just implement it as quickly as possible, not in the most correct or the most efficient manner.
-
- NOTE: path is (N,3) in shape, last column is 0, because subotimal code structure
- """
-
- p_base_3 = np.array([p_base[0], p_base[1], 0.0])
- distances = np.linalg.norm(p_base_3 - path2D, axis=1)
- index = np.argmin(distances)
- if (index - path_parameter[0]) > 0 and (index - path_parameter[0]) < 10:
- path_parameter[0] += 1
- path2D = path2D[path_parameter[0] :]
- return path2D
-
-
-# def main(args=None):
-def main(args=None):
- # evil and makes args unusable but what can you do
- args_smc = get_args()
- assert args_smc.robot == "myumi"
- robot = RealMobileYumiRobotManager(args_smc)
- # you can't do terminal input with ros
- # goal = robot.defineGoalPointCLI()
- T_w_absgoal = pin.SE3.Identity()
- T_w_absgoal.translation = np.array([0.5, 0.0, 0.5])
- T_absgoal_l = pin.SE3.Identity()
- T_absgoal_l.translation = np.array([0.0, 0.2, 0.0])
- T_absgoal_r = pin.SE3.Identity()
- T_absgoal_r.translation = np.array([0.0, -0.2, 0.0])
- # loop_manager = CrocoIKMPC(args, robot, goal, run=False)
- # robot._mode = robot.control_mode.upper_body
- # loop_manager = moveJP(robot._comfy_configuration, args_smc, robot, run=False)
- # loop_manager = moveLDualArm(
- # args_smc, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r, run=False
- # )
- # loop_manager = moveL(args_smc, robot, T_w_absgoal, run=False)
- robot._mode = robot.control_mode.whole_body
- robot.setInitialPose()
- robot._step()
- if not args_smc.draw_new:
- pixel_path_file_path = "./parameters/path_in_pixels.csv"
- path2D = np.genfromtxt(pixel_path_file_path, delimiter=",")
- else:
- matplotlib.use("tkagg")
- path2D = drawPath(args_smc)
- matplotlib.use("qtagg")
- path2D[:, 0] = path2D[:, 0] * args_smc.map_width
- path2D[:, 1] = path2D[:, 1] * args_smc.map_height
- path2D = np.hstack((path2D, np.zeros((len(path2D), 1))))
- robot.updateViz({"fixed_path": path2D})
- path_planner = partial(fixedPathPlanner, [0], path2D)
-
- # x0 = np.concatenate([robot.q, robot.v])
- # loop_manager = CrocoBasePathFollowingMPC(
- # args_smc, robot, x0, path_planner, run=False
- # )
- robot._mode = robot.control_mode.base_only
- loop_manager = cartesianPathFollowingWithPlanner(
- args_smc, robot, path_planner, 0.0, run=False
- )
-
- rclpy.init(args=args)
-
- executor = MultiThreadedExecutor()
- node = SMCMobileYuMiNode(args_smc, robot, loop_manager)
- executor.add_node(node)
- executor.spin()
-
-
-if __name__ == "__main__":
- main()
diff --git a/examples/ros/navigation_example.py b/examples/ros/navigation_example.py
index c7439db84b0c56ff9ddf0d8e2574f7ccf496b66e..60ea6c2b5ef06e8f37285dd15050f18b69a5a146 100644
--- a/examples/ros/navigation_example.py
+++ b/examples/ros/navigation_example.py
@@ -3,7 +3,7 @@
import rclpy
from rclpy.executors import MultiThreadedExecutor
-from smc_yumi_node import get_args, SMCMobileYuMiNode
+from smc.multiprocessing.smc_yumi_node import get_args, SMCMobileYuMiNode
import numpy as np
import argparse
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index 6a26983fa6352abb1ec5c7bdda1a2e5e7e679e89..1a923c9ae8ec294ac431687214b370b3489eb662 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -15,6 +15,7 @@ import numpy as np
from argparse import Namespace
from collections import deque
from typing import Callable
+from IPython import embed
from smc.control.cartesian_space.ik_solvers import QPManipMax
@@ -38,6 +39,8 @@ def controlLoopClik(
T_w_e = robot.T_w_e
SEerror = T_w_e.actInv(T_w_goal)
err_vector = pin.log6(SEerror).vector
+ if (robot.robot_name == "myumi") and (robot.mode == robot.control_mode.base_only):
+ err_vector[5] = err_vector[5] * 10
J = robot.getJacobian()
# compute the joint velocities based on controller you passed
# qd = ik_solver(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index d4e3880b47b0c8657d5d932185048017d1914df2..c07ff735c3bb67dcd2988d1639fbfaa8b1486ee6 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -137,8 +137,8 @@ def QPquadprog(
b = (V_e_e / V_e_e_norm) * max_V_e_e_norm
A = J
# TODO: you probably want limits here
- # lb = None
- # ub = None
+ lb = None
+ ub = None
# lb *= 20
# ub *= 20
h = None
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index 1cc63d15d3778382a1feb48f98606210d6d4f9d1..b6140ac4b2a9c9c73bd7314d1d6a6356c562d3cc 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -14,6 +14,7 @@ from smc.robots.interfaces.whole_body_dual_arm_interface import (
from smc.robots.interfaces.whole_body_single_arm_interface import (
SingleArmWholeBodyInterface,
)
+
global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
@@ -103,9 +104,9 @@ def EEP2PCtrlLoopTemplate(
if err_vector_norm < robot.args.goal_error:
breakFlag = True
log_item["qs"] = robot.q
- log_item["err_norm"] = err_vector_norm.reshape((1,))
log_item["dqs"] = robot.v
- log_item["dqs_cmd"] = v_cmd.reshape((robot.nv,))
+ log_item["dqs_cmd"] = v_cmd
+ log_item["err_norm"] = err_vector_norm.reshape((1,))
return breakFlag, save_past_item, log_item
diff --git a/examples/ros/smc_yumi_node.py b/python/smc/multiprocessing/smc_yumi_node.py
similarity index 83%
rename from examples/ros/smc_yumi_node.py
rename to python/smc/multiprocessing/smc_yumi_node.py
index dfa1958ae9d9bd146f4e7bc40b320e94bf080205..b56957891cf70b8ff9cccd08475117fb5ff44392 100644
--- a/examples/ros/smc_yumi_node.py
+++ b/python/smc/multiprocessing/smc_yumi_node.py
@@ -1,6 +1,7 @@
from smc.robots.implementations.mobile_yumi import RealMobileYumiRobotManager
from smc.control.control_loop_manager import ControlLoopManager
from smc import getMinimalArgParser
+from smc.robots.abstract_robotmanager import AbstractRobotManager
import rclpy
from rclpy.time import Time
@@ -13,6 +14,7 @@ from abb_python_utilities.names import get_rosified_name
from rclpy.callback_groups import ReentrantCallbackGroup
from smc.control.cartesian_space import getClikArgs
from smc.control.optimal_control.util import get_OCP_args
+from smc.path_generation.planner import getPlanningArgs
import numpy as np
import argparse
@@ -24,6 +26,7 @@ def get_args():
of various kinds. Make sure you know what the goal is before you run!"
parser = getClikArgs(parser)
parser = get_OCP_args(parser)
+ parser = getPlanningArgs(parser)
parser.add_argument(
"--ros-namespace",
type=str,
@@ -60,6 +63,25 @@ def get_args():
help="if in sim you need to set sim time, otherwise not",
default=True,
)
+ # TODO: move elsewhere
+ parser.add_argument(
+ "--handlebar-height",
+ type=float,
+ default=0.5,
+ help="heigh of handlebar of the cart to be pulled",
+ )
+ parser.add_argument(
+ "--base-to-handlebar-preferred-distance",
+ type=float,
+ default=0.5,
+ help="prefered path arclength from mobile base position to handlebar",
+ )
+ parser.add_argument(
+ "--planner",
+ action=argparse.BooleanOptionalAction,
+ help="if on, you're in a pre-set map and a planner produce a plan to navigate. if off, you draw the path to be followed",
+ default=True,
+ )
# parser.add_argument('--ros-args', action='extend', nargs="+")
# parser.add_argument('-r', action='extend', nargs="+")
parser.add_argument("--ros-args", action="append", nargs="*")
@@ -80,7 +102,12 @@ def get_args():
class SMCMobileYuMiNode(Node):
def __init__(
- self, args, robot: RealMobileYumiRobotManager, loop_manager: ControlLoopManager
+ self,
+ args,
+ robot: RealMobileYumiRobotManager,
+ modes_and_loops: list[
+ tuple[AbstractRobotManager.control_mode, ControlLoopManager]
+ ],
):
super().__init__("SMCMobileYuMiNode")
if args.sim:
@@ -93,7 +120,9 @@ class SMCMobileYuMiNode(Node):
)
self.wait_for_sim_time()
self.robot = robot
- self.loop_manager = loop_manager
+ mode, self.loop_manager = modes_and_loops.pop(0)
+ self.robot.mode = mode
+ self.modes_and_loops = modes_and_loops
self.args = args
# give me the latest thing even if there wasn't an update
# qos_prof = rclpy.qos.QoSProfile(
@@ -165,6 +194,15 @@ class SMCMobileYuMiNode(Node):
# self.get_logger().info("TIMER_CMD")
self.current_iteration += 1
breakFlag = self.loop_manager.run_one_iter(self.loop_manager.current_iteration)
+ if breakFlag:
+ if len(self.modes_and_loops) > 0:
+ mode, self.loop_manager = self.modes_and_loops.pop(0)
+ self.robot.mode = mode
+ self.get_logger().info(
+ "about to run: " + str(self.loop_manager.controlLoop.func.__name__)
+ )
+ else:
+ self.robot._v_cmd[:] = 0.0
if not self.odom_initialized:
self.get_logger().info(
@@ -172,7 +210,7 @@ class SMCMobileYuMiNode(Node):
)
# self.get_logger().info("current iteration: " + str(self.current_iteration))
- self.get_logger().info(str(self.robot._v_cmd))
+ # self.get_logger().info(str(self.robot._v_cmd))
if self.args.unreal:
msg = self.empty_msg
msg.header.stamp = Time().to_msg()
@@ -196,6 +234,9 @@ class SMCMobileYuMiNode(Node):
# REAL
for i in range(3):
msg.velocity[i] = self.robot._v_cmd[i]
+ if self.robot._v_cmd[2] < 0.01:
+ self.robot._v_cmd[2] = 0.0
+ # self.get_logger().info("this velocity is too small for anything!!")
for i in range(15, 29):
msg.velocity[i] = self.robot._v_cmd[i - 12]
# self.get_logger().info(str(self.robot._q))
@@ -223,7 +264,7 @@ class SMCMobileYuMiNode(Node):
self.init_odom[2] = th
self.odom_initialized = True
self.get_logger().info(str(self.init_odom))
- if self.args.unreal and self.odom_initialized:
+ if (self.args.unreal and self.odom_initialized) or self.current_iteration < 50:
T_odom = np.zeros((3, 3))
T_odom[0, 0] = np.cos(self.init_odom[2])
T_odom[0, 1] = -1 * np.sin(self.init_odom[2])
@@ -249,6 +290,9 @@ class SMCMobileYuMiNode(Node):
self.robot._q[1] = p_ctrl[1]
self.robot._q[2] = np.cos(self.init_odom[2] - th)
self.robot._q[3] = np.sin(self.init_odom[2] - th)
+ # self.get_logger().info(str(self.robot._q))
+ # self.get_logger().info(str(th))
+ # self.get_logger().info(str(self.init_odom))
# self.get_logger().info("CALLBACK_ODOM")
# self.get_logger().info(str(self.robot._q[:4]))
# self.get_logger().info(str(self.init_odom))
@@ -257,7 +301,6 @@ class SMCMobileYuMiNode(Node):
if self.current_iteration < 200 or self.args.unreal:
self.robot._q[-14:-7] = msg.position[-14:-7]
self.robot._q[-7:] = msg.position[-7:]
- # self.get_logger().info("CALLBACK_JOINT_STATE")
def wait_for_sim_time(self):
"""Wait for the /clock topic to start publishing."""
diff --git a/python/smc/path_generation/maps/premade_maps.py b/python/smc/path_generation/maps/premade_maps.py
index 7037857efa21ad64937c6343838febd99021d2c8..511984b200b826bd4530e127d8a8db25bfdb00fc 100644
--- a/python/smc/path_generation/maps/premade_maps.py
+++ b/python/smc/path_generation/maps/premade_maps.py
@@ -8,11 +8,18 @@ def createSampleStaticMap():
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]],
+ # [[2, 5], [8, 5], [8, 6], [2, 6]],
+ # [[2, 8], [8, 8], [8, 9], [2, 9]],
+ # ]
map_as_list = [
- [[2, 2], [8, 2], [8, 3], [2, 3]],
- [[2, 3], [3, 3], [3, 4.25], [2, 4.25]],
- [[2, 5], [8, 5], [8, 6], [2, 6]],
- [[2, 8], [8, 8], [8, 9], [2, 9]],
+ [[0.5, -1.0], [3.5, -1.0], [3.5, -0.5], [0.5, -0.5]],
+ [[3.5, -0.5], [4.0, -0.5], [4.0, 3.5], [3.5, 3.5]],
+ [[-1.0, 1.0], [2.5, 1.0], [2.5, 2.0], [-1.0, 2.0]],
+ [[0.0, 3.5], [4.0, 3.5], [4.0, 4.0], [0.0, 4.0]],
+ # [[-1.5, -1.0], [-1.0, -1.0], [-1.0, 5.0], [-1.5, 5.0]],
]
obstacles = []
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index a409401f54292ccd83c760969696c748cf06ef92..21537035eb93e4c1bf36e592814eef6e6be81b15 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -6,7 +6,9 @@ from smc.robots.interfaces.force_torque_sensor_interface import (
from smc.robots.interfaces.mobile_base_interface import (
get_mobile_base_model,
)
-from smc.robots.interfaces.whole_body_single_arm_interface import SingleArmWholeBodyInterface
+from smc.robots.interfaces.whole_body_single_arm_interface import (
+ SingleArmWholeBodyInterface,
+)
from smc.robots.implementations.ur5e import get_model
from smc.robots.grippers.robotiq.robotiq_gripper import RobotiqGripper
from smc.robots.grippers.rs485_robotiq.rs485_robotiq import RobotiqHand
@@ -38,6 +40,20 @@ class AbstractHeronRobotManager(
# TODO: CHANGE THIS TO REAL VALUES
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
+ self._comfy_configuration = np.array(
+ [
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 1.54027569e00,
+ -1.95702042e00,
+ 1.46127540e00,
+ -1.07315435e00,
+ -1.61189968e00,
+ -1.65158907e-03,
+ ]
+ )
super().__init__(args)
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index 25d3ccf6a1983f93e60d29fa8fa852bd930b1055..4cd10706e423d07edbb64e2eff38927d0ed4c7dc 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -30,6 +30,16 @@ class AbstractUR5eRobotManager(ForceTorqueOnSingleArmWrist):
self._ee_frame_id = self.model.getFrameId("tool0")
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
+ self._comfy_configuration = np.array(
+ [
+ 1.54027569e00,
+ -1.95702042e00,
+ 1.46127540e00,
+ -1.07315435e00,
+ -1.61189968e00,
+ -1.65158907e-03,
+ ]
+ )
super().__init__(args)
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index 4d539873ff433e9273545929448d88be2db02e4c..488c05f3c995b979a8d3377687b866f02260b29d 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -85,7 +85,7 @@ def getMinimalArgParser():
type=str,
help="gripper you're using (no gripper is the default)",
default="none",
- choices=["none", "robotiq", "onrobot"],
+ choices=["none", "robotiq", "onrobot", "rs485"],
)
# 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