diff --git a/Dockerfile b/Dockerfile index b15f9b0b147f79247dfa621e0a3ac408191155ba..a77c10710cb62dc3e8bc35bcf5107f5e893a2f9c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -40,48 +40,47 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # make the environment more usable # create user -######### TEST ########### -# uncomment later -#RUN useradd -m -s /bin/zsh -G sudo -u 1000 student -# -#WORKDIR /home/student/ -#RUN passwd -d student -#USER student -## copy repo to workdir -#RUN mkdir 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/ -# -# -## 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 useradd -m -s /bin/zsh -G sudo -u 1000 student + +WORKDIR /home/student/ +RUN passwd -d student +USER student +# copy repo to workdir +RUN mkdir 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/ + + +# 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 opencv -#RUN pip install matplotlib meshcat ur_rtde argcomplete \ -# qpsolvers ecos example_robot_data meshcat_shapes \ -# pyqt6 opencv-python -# -#RUN vam install python-jedi && vam install youcompleteme +RUN conda install -y pinocchio crocoddyl -c conda-forge +#RUN conda install -y opencv +RUN pip install matplotlib meshcat ur_rtde argcomplete \ + qpsolvers ecos example_robot_data meshcat_shapes \ + pyqt6 opencv-python + +RUN vam install python-jedi && vam install youcompleteme diff --git a/python/examples/camera_no_lag.py b/python/examples/camera_no_lag.py index 9b84d9a187ca91a7e0c9b9e9b09518b8f24f5cd8..5f3b37d7e84d44efdeef49032fc7f41632f31ed2 100644 --- a/python/examples/camera_no_lag.py +++ b/python/examples/camera_no_lag.py @@ -59,17 +59,15 @@ if __name__ == "__main__": loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item) loop_manager.run() - + camera_manager.terminateProcess() # get expected behaviour here (library can't know what the end is - you have to do this here) if not args.pinocchio_only: robot.stopRobot() - if args.save_log: - robot.log_manager.plotAllControlLoops() - if args.visualize_manipulator: robot.killManipulatorVisualizer() if args.save_log: robot.log_manager.saveLog() + robot.log_manager.plotAllControlLoops() diff --git a/python/examples/challenge_main.py b/python/examples/challenge_main.py deleted file mode 100644 index 0d81483f79b2d6f9f2a451c24205989abfe1893d..0000000000000000000000000000000000000000 --- a/python/examples/challenge_main.py +++ /dev/null @@ -1 +0,0 @@ -# TODO GET IT diff --git a/python/examples/clik.py b/python/examples/clik.py index be65bc01e0deb01b0186eb9f0ad67c13c69c99cb..332ab5635b51dd8a0be82cc707ea1937f94dde90 100644 --- a/python/examples/clik.py +++ b/python/examples/clik.py @@ -6,6 +6,7 @@ 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 +from ur_simple_control.util.define_random_goal import getRandomlyGeneratedGoal def get_args(): @@ -13,6 +14,8 @@ def get_args(): parser.description = 'Run closed loop inverse kinematics \ of various kinds. Make sure you know what the goal is before you run!' parser = getClikArgs(parser) + parser.add_argument('--randomly-generate-goal', action=argparse.BooleanOptionalAction, + default=False, help="if true, rand generate a goal, if false you type it in via text input") argcomplete.autocomplete(parser) args = parser.parse_args() return args @@ -20,10 +23,15 @@ def get_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) + if args.randomly_generate_goal: + Mgoal = getRandomlyGeneratedGoal(args) + if args.visualize_manipulator: + robot.visualizer_manager.sendCommand( + {"Mgoal" : Mgoal}) + else: + Mgoal = robot.defineGoalPointCLI() + #compliantMoveL(args, robot, Mgoal) + moveL(args, robot, Mgoal) robot.closeGripper() robot.openGripper() if not args.pinocchio_only: diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run index 4680ab807b8ec0484caccccb3060d4052566c3bb..2098a797629da5c383b1c86c8b08c573978ba440 100644 Binary files a/python/examples/data/latest_run and b/python/examples/data/latest_run differ diff --git a/python/examples/test_by_running.sh b/python/examples/test_by_running.sh new file mode 100755 index 0000000000000000000000000000000000000000..6afcb16df55a6b90f59d9a5c4b53702fc9a45a86 --- /dev/null +++ b/python/examples/test_by_running.sh @@ -0,0 +1,71 @@ +#!/bin/bash +# the idea here is to run all the runnable things +# and test for syntax errors + +############ +# camera # +############ +# the only one where you check plotter +runnable="camera_no_lag.py --max-iterations=1500 --no-visualize-manipulator" +echo $runnable +python $runnable +echo "==========================================================" + +################### +# classic cliks # +################### +# the only one where you check visualizer +# damped pseudoinverse arm +runnable="clik.py --randomly-generate-goal" +echo $runnable +python $runnable + +# damped pseudoinverse whole body mobile +runnable="clik.py --robot=heron --randomly-generate-goal --fast-simulation --no-visualize-manipulator --no-real-time-plotting --max-iterations=2000" +echo $runnable +python $runnable + +# QP arm +runnable="clik.py --randomly-generate-goal --clik-controller=invKinmQP --fast-simulation --no-visualize-manipulator --no-real-time-plotting --max-iterations=2000" +echo $runnable +python $runnable + +# QP whole body mobile +runnable="clik.py --robot=heron --randomly-generate-goal --clik-controller=invKinmQP --fast-simulation --no-visualize-manipulator --no-real-time-plotting --max-iterations=2000" +echo $runnable +python $runnable + + +runnable="" +echo $runnable +python $runnable +#python cart_pulling.py +#python casadi_ocp_collision_avoidance.py +#python challenge_main.py +#python comparing_logs_example.py +#python crocoddyl_mpc.py +#python crocoddyl_ocp_clik.py +#python data +#python drawing_from_input_drawing.py +#python force_control_test.py +#python graz +#python heron_pls.py +#python joint_trajectory.csv +#python log_analysis_example.py +#python old_or_experimental +#python path_following_mpc.py +#python path_in_pixels.csv +#python pin_contact3d.py +#python plane_pose.pickle +#python plane_pose.pickle_save +#python plane_pose_sim.pickle +#python point_force_control.py +#python point_impedance_control.py +#python pushing_via_friction_cones.py +#python __pycache__ +#python ros2_clik.py +#python smc_node.py +#python test_by_running.sh +#python test_crocoddyl_opt_ctrl.py +#python wiping_path.csv_save +# diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc index 46d05cb7e76111fa4e419d1d05311a21bc5b3b3f..11b255d84e82ed0bbd0e7def6f743ea11d3e8f1e 100644 Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py index 10ce06747f1a323ae88632dfd1761c7f6253b7f1..5fc227209086cfad64e7d419604fa8ab43afd51a 100644 --- a/python/ur_simple_control/clik/clik.py +++ b/python/ur_simple_control/clik/clik.py @@ -88,10 +88,29 @@ def jacobianTranspose(J, err_vector): qd = J.T @ err_vector return qd +# TODO: put something into q of the QP +# also, put in lb and ub def invKinmQP(J, err_vector, lb=None, ub=None): - # maybe a lower precision dtype is equally good, but faster? + """ + invKinmQP + --------- + generic QP: + minimize 1/2 x^T P x + q^T x + subject to + G x \leq h + A x = b + lb <= x <= ub + inverse kinematics QP: + minimize 1/2 qd^T P qd + + q^T qd (optional secondary objective) + subject to + G qd \leq h (optional) + J qd = b (mandatory) + lb <= qd <= ub (optional) + """ P = np.eye(J.shape[1], dtype="double") - # TODO: why is q all 0? + # secondary objective is given via q + # we set it to 0 here, but we should give a sane default here q = np.array([0] * J.shape[1], dtype="double") G = None b = err_vector @@ -163,6 +182,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data): log_item['qs'] = q.reshape((robot.model.nq,)) log_item['dqs'] = robot.getQd().reshape((robot.model.nv,)) + log_item['dqs_cmd'] = qd.reshape((robot.model.nv,)) # we're not saving here, but need to respect the API, # hence the empty dict return breakFlag, {}, log_item @@ -241,6 +261,7 @@ def moveL(args, robot : RobotManager, goal_point): log_item = { 'qs' : np.zeros(robot.model.nq), 'dqs' : np.zeros(robot.model.nv), + 'dqs_cmd' : np.zeros(robot.model.nv), } loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item) loop_manager.run() diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py index 9a943bc19503f8e94bbba1ce9b3d060f2e0a2498..d0c7ba6e8adedc11b61614570187eef81ac60a54 100644 --- a/python/ur_simple_control/managers.py +++ b/python/ur_simple_control/managers.py @@ -331,7 +331,7 @@ class ControlLoopManager: print("success in", i, "iterations!") else: print("FAIL: did not succed in", self.max_iterations, "iterations") - self.stopHandler(None, None) + #self.stopHandler(None, None) def stopHandler(self, signum, frame): """ @@ -374,6 +374,8 @@ class ControlLoopManager: if self.args.visualize_manipulator: 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() @@ -488,19 +490,14 @@ class RobotManager: if self.args.gripper == "onrobot": self.gripper = TWOFG() - # also TODO: figure out how to best solve the gripper_velocity problem - # NOTE: you need to initialize differently for other types of joints - # TODO: add the option for this being a shared variable, - # and then write ifs with locks to make this correct. - self.q = np.zeros(self.model.nq) - # planar joint is [x, y, cos(theta), sin(theta)], - # so it can't be all zeros - if self.robot_name == "heron": - self.q[2] = 1.0 - if self.robot_name == "heronros": - self.q[2] = 1.0 - if self.robot_name == "mirros": - self.q[2] = 1.0 + # TODO: specialize for each robot, + # add reasonable home positions + self.q = pin.randomConfiguration(self.model, + -1 * np.ones(self.model.nq), + np.ones(self.model.nq)) + if self.robot_name == "ur5e": + self.q[-1] = 0.0 + self.q[-2] = 0.0 # v_q is the generalization of qd for every type of joint. # for revolute joints it's qd, but for ex. the planar joint it's the body velocity. self.v_q = np.zeros(self.model.nv) @@ -845,16 +842,13 @@ class RobotManager: if self.robot_name == "heron": # y-direction is not possible on heron - qd[1] = 0 - self.q = pin.integrate(self.model, self.q, qd * self.dt) + qd_cmd = np.clip(qd, -1 * self.model.velocityLimit, self.model.velocityLimit) + #qd[1] = 0 + self.v_q = qd_cmd + self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt) if self.robot_name == "heronros": # y-direction is not possible on heron - # TODO: REMOVE OBVIOUSLY -#############################################33 -# qd[:] = 0.0 -#############################################33 - qd[1] = 0 cmd_msg = msg.Twist() cmd_msg.linear.x = qd[0] @@ -867,11 +861,6 @@ class RobotManager: if self.robot_name == "mirros": # y-direction is not possible on heron - # TODO: REMOVE OBVIOUSLY -#############################################33 - #qd[:] = 0.0 -#############################################33 - qd[1] = 0 cmd_msg = msg.Twist() cmd_msg.linear.x = qd[0] @@ -883,12 +872,12 @@ class RobotManager: #self.q = pin.integrate(self.model, self.q, qd * self.dt) if self.robot_name == "gripperlessur5e": - qd_cmd = qd[:6] + qd_cmd = np.clip(qd, -1 * self.max_qd, self.max_qd) if not self.pinocchio_only: self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt) else: - self.v_q = qd - self.q = pin.integrate(self.model, self.q, qd * self.dt) + self.v_q = qd_cmd + self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt) @@ -1188,9 +1177,9 @@ class ProcessManager: if self.comm_direction == 3: self.shm_cmd.close() self.shm_cmd.unlink() - if self.args.debug_prints: - print(f"i am putting befree in {self.side_process.name}'s command queue to stop it") - if self.comm_direction != 3: + 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() diff --git a/python/ur_simple_control/util/define_random_goal.py b/python/ur_simple_control/util/define_random_goal.py new file mode 100644 index 0000000000000000000000000000000000000000..40ee89eff5d10429d6aaaa5290232b1b5c552ff2 --- /dev/null +++ b/python/ur_simple_control/util/define_random_goal.py @@ -0,0 +1,14 @@ +# PYTHON_ARGCOMPLETE_OK +import numpy as np +import pinocchio as pin + +def getRandomlyGeneratedGoal(args): + Mgoal = pin.SE3.Random() + # has to be close + translation = np.random.random(3) * 0.4 + translation[2] = np.abs(translation[2]) + translation = translation + np.ones(3) * 0.1 + Mgoal.translation = translation + if args.debug_prints: + print(Mgoal) + return Mgoal