From 204fe5784925a3d179dd5cfcd835229fa430b27d Mon Sep 17 00:00:00 2001 From: m-guberina <gubi.guberina@gmail.com> Date: Tue, 26 Nov 2024 14:13:52 +0100 Subject: [PATCH] made some imports optional for easier install for the wara manipulation challenge --- Dockerfile_challenge | 82 ++++ TODOS | 1 + .../casadi_ocp_collision_avoidance.py | 3 +- python/examples/comparing_logs_example.py | 18 +- python/examples/crocoddyl_mpc.py | 8 +- python/examples/crocoddyl_ocp_clik.py | 3 +- python/examples/drawing_from_input_drawing.py | 2 +- python/examples/joint_trajectory.csv | 356 +++++++++--------- python/examples/path_following_mpc.py | 9 +- python/examples/path_in_pixels.csv | 356 +++++++++--------- .../__pycache__/managers.cpython-312.pyc | Bin 49235 -> 49250 bytes python/ur_simple_control/clik/clik.py | 4 +- python/ur_simple_control/managers.py | 2 +- .../optimal_control/__init__.py | 4 +- .../create_pinocchio_casadi_ocp.py | 3 + .../optimal_control/crocoddyl_mpc.py | 4 +- .../crocoddyl_optimal_control.py | 4 +- .../__pycache__/visualize.cpython-312.pyc | Bin 16540 -> 16632 bytes .../manipulator_comparison_visualizer.py | 46 +-- .../ur_simple_control/visualize/visualize.py | 13 +- 20 files changed, 515 insertions(+), 403 deletions(-) create mode 100644 Dockerfile_challenge diff --git a/Dockerfile_challenge b/Dockerfile_challenge new file mode 100644 index 0000000..a90a1ce --- /dev/null +++ b/Dockerfile_challenge @@ -0,0 +1,82 @@ +FROM docker.io/ros:humble + +LABEL org.opencontainers.image.authors="marko.guberina@control.lth.se" + +# install python3-tk without questions +ARG DEBIAN_FRONTEND=noninteractive +ENV TZ=Europe/Stockholm + +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3 \ + python3-pip \ + python3-tk \ + ipython3 \ + git \ + sudo \ + man-db \ + manpages-posix \ + iputils-ping \ + arp-scan \ + # nice to have +# python3-opencv \ + vim \ + vim-addon-manager \ + vim-youcompleteme \ + vim-python-jedi \ + zsh \ + zsh-syntax-highlighting \ + libarchive-tools \ + libxcb-cursor0\ + python3-python-qt-binding + +# qt-binding is a really unnecessary 300MB, but i don't want +# to do more matplotlib hacks + +#RUN sed -i 's:^path-exclude=/usr/share/man:#path-exclude=/usr/share/man:' \ +# /etc/dpkg/dpkg.cfg.d/excludes +#RUN yes | unminimize + +# make the environment more usable +# create user +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/ + +RUN vam install python-jedi && vam install youcompleteme + +# 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 -y casadi opencv +#RUN conda install --solver=classic -y pinocchio crocoddyl -c conda-forge +RUN pip install matplotlib meshcat ur_rtde argcomplete \ + qpsolvers ecos example_robot_data meshcat_shapes \ + pyqt6 pin crocoddyl diff --git a/TODOS b/TODOS index 419550a..b7ef755 100644 --- a/TODOS +++ b/TODOS @@ -164,6 +164,7 @@ goal 5: more convenience - set gripper payload etc automatically # ALSO NOTE: to use this you need to change the version inclusions in # ur_rtde due to a bug there in the current ur_rtde + robot firmware version # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't) +2. debug comparison log goal 6: panda/yumi ---------------- diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/casadi_ocp_collision_avoidance.py index d2a3c10..dce56f7 100644 --- a/python/examples/casadi_ocp_collision_avoidance.py +++ b/python/examples/casadi_ocp_collision_avoidance.py @@ -27,8 +27,7 @@ if __name__ == "__main__": T_goal = robot.defineGoalPointCLI() T_goal.rotation = robot.getT_w_e().rotation if args.visualize_manipulator: - robot.manipulator_visualizer_queue.put( - {"Mgoal" : T_goal}) + robot.updateViz({"Mgoal" : T_goal}) reference, opti = createCasadiIKObstacleAvoidanceOCP(args, robot, T_goal) followKinematicJointTrajP(args, robot, reference) diff --git a/python/examples/comparing_logs_example.py b/python/examples/comparing_logs_example.py index c590978..bc92dd6 100644 --- a/python/examples/comparing_logs_example.py +++ b/python/examples/comparing_logs_example.py @@ -1,9 +1,6 @@ from ur_simple_control.visualize.manipulator_comparison_visualizer import getLogComparisonArgs, ManipulatorComparisonManager import time -args = getLogComparisonArgs() -cmp_manager = ManipulatorComparisonManager(args) - print(""" this code animates 2 manipulator and can run an animated plot along side it with the same timing. @@ -13,15 +10,22 @@ you have nor what's logged in them, apart from the fact that everything has to have qs to visualize the manipulators. here we're assuming you have 1 control loop per log, and that the same things are logged. +also, you need to provide the two log files +with --log-file1=/path/to/file1 and +--log-file1=/path/to/file2 """) +args = getLogComparisonArgs() +cmp_manager = ManipulatorComparisonManager(args) + key = list(cmp_manager.logm1.loop_logs.keys())[0] -cmp_manager.createRunningPlot(cmp_manager.logm1.loop_logs[key], 0, len(cmp_manager.logm1.loop_logs[key]['qs'])) -cmp_manager.createRunningPlot(cmp_manager.logm2.loop_logs[key], 0, len(cmp_manager.logm2.loop_logs[key]['qs'])) +#cmp_manager.createRunningPlot(cmp_manager.logm1.loop_logs[key], 0, len(cmp_manager.logm1.loop_logs[key]['qs'])) +#cmp_manager.createRunningPlot(cmp_manager.logm2.loop_logs[key], 0, len(cmp_manager.logm2.loop_logs[key]['qs'])) cmp_manager.visualizeWholeRuns() -cmp_manager.manipulator_visualizer_cmd_queue.put("befree") +cmp_manager.visualizer_manager.sendCommand("befree") +time.sleep(100) print("main done") time.sleep(0.1) -cmp_manager.manipulator_visualizer_process.terminate() +cmp_manager.visualizer_manager.terminateProcess() if args.debug_prints: print("terminated manipulator_visualizer_process") diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py index 8b95290..3d78e8d 100644 --- a/python/examples/crocoddyl_mpc.py +++ b/python/examples/crocoddyl_mpc.py @@ -13,7 +13,7 @@ from ur_simple_control.visualize.visualize import plotFromDict from ur_simple_control.clik.clik import getClikArgs import pinocchio as pin import crocoddyl -import mim_solvers +import importlib.util import argcomplete @@ -27,6 +27,9 @@ def get_args(): if __name__ == "__main__": + if importlib.util.find_spec('mim_solvers'): + import mim_solvers + args = get_args() robot = RobotManager(args) # TODO: put this back for nicer demos @@ -36,8 +39,7 @@ if __name__ == "__main__": robot.Mgoal = Mgoal.copy() if args.visualize_manipulator: # TODO document this somewhere - robot.manipulator_visualizer_queue.put( - {"Mgoal" : Mgoal}) + robot.visualizer_manager.sendCommand({"Mgoal" : Mgoal}) # create and solve the optimal control problem of # getting from current to goal end-effector position. # reference is position and velocity reference (as a dictionary), diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py index 71cc5bf..fc3ba00 100644 --- a/python/examples/crocoddyl_ocp_clik.py +++ b/python/examples/crocoddyl_ocp_clik.py @@ -31,8 +31,7 @@ if __name__ == "__main__": if args.visualize_manipulator: # TODO document this somewhere - robot.manipulator_visualizer_queue.put( - {"Mgoal" : Mgoal}) + robot.visualizer_manager.sendCommand({"Mgoal" : Mgoal}) # create and solve the optimal control problem of # getting from current to goal end-effector position. diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py index 0b12e57..e3213a2 100644 --- a/python/examples/drawing_from_input_drawing.py +++ b/python/examples/drawing_from_input_drawing.py @@ -46,7 +46,7 @@ def getArgs(): parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \ help=""" whether you want to do find marker offset (recalculate TCP - based on the marker""", default=False) + based on the marker""", default=True) parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \ help="are you wiping the board (default is no because you're writing)", \ default=False) diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv index 1bbd90f..5b8f74a 100644 --- a/python/examples/joint_trajectory.csv +++ b/python/examples/joint_trajectory.csv @@ -1,173 +1,183 @@ -0.00000,0.85946,-1.46493,-1.40353,-1.08818,2.11438,-0.49363 -0.11628,0.85941,-1.46494,-1.40354,-1.08817,2.11440,-0.49366 -0.23256,0.85937,-1.46495,-1.40354,-1.08817,2.11443,-0.49370 -0.34884,0.85935,-1.46496,-1.40353,-1.08818,2.11444,-0.49372 -0.46512,0.85934,-1.46496,-1.40351,-1.08818,2.11444,-0.49372 -0.58140,0.85935,-1.46497,-1.40349,-1.08820,2.11444,-0.49371 -0.69767,0.85937,-1.46496,-1.40347,-1.08821,2.11442,-0.49369 -0.81395,0.86007,-1.46479,-1.40289,-1.08853,2.11388,-0.49305 -0.93023,0.86253,-1.46416,-1.40106,-1.08955,2.11200,-0.49084 -1.04651,0.86472,-1.46353,-1.39968,-1.09034,2.11036,-0.48889 -1.16279,0.86665,-1.46287,-1.39874,-1.09091,2.10893,-0.48719 -1.27907,0.86954,-1.46191,-1.39740,-1.09172,2.10680,-0.48467 -1.39535,0.87300,-1.46058,-1.39627,-1.09247,2.10429,-0.48169 -1.51163,0.87706,-1.45897,-1.39518,-1.09322,2.10135,-0.47821 -1.62791,0.88047,-1.45741,-1.39473,-1.09365,2.09890,-0.47532 -1.74419,0.88480,-1.45544,-1.39425,-1.09414,2.09580,-0.47167 -1.86047,0.89139,-1.45216,-1.39420,-1.09459,2.09111,-0.46616 -1.97674,0.89821,-1.44860,-1.39466,-1.09482,2.08627,-0.46051 -2.09302,0.90640,-1.44403,-1.39588,-1.09481,2.08046,-0.45379 -2.20930,0.91320,-1.44005,-1.39735,-1.09459,2.07564,-0.44825 -2.32558,0.92006,-1.43588,-1.39919,-1.09423,2.07077,-0.44269 -2.44186,0.92420,-1.43332,-1.40042,-1.09396,2.06782,-0.43935 -2.55814,0.92964,-1.42982,-1.40231,-1.09349,2.06395,-0.43498 -2.67442,0.93516,-1.42618,-1.40443,-1.09294,2.06002,-0.43056 -2.79070,0.93779,-1.42440,-1.40555,-1.09263,2.05814,-0.42846 -2.90698,0.94185,-1.42154,-1.40747,-1.09207,2.05525,-0.42524 -3.02326,0.94589,-1.41860,-1.40956,-1.09144,2.05236,-0.42203 -3.13953,0.94845,-1.41664,-1.41107,-1.09097,2.05053,-0.42001 -3.25581,0.95241,-1.41343,-1.41372,-1.09011,2.04769,-0.41688 -3.37209,0.95629,-1.41004,-1.41675,-1.08908,2.04491,-0.41383 -3.48837,0.96149,-1.40507,-1.42157,-1.08740,2.04118,-0.40974 -3.60465,0.96791,-1.39878,-1.42782,-1.08520,2.03656,-0.40473 -3.72093,0.97188,-1.39461,-1.43219,-1.08364,2.03370,-0.40163 -3.83721,0.97691,-1.38927,-1.43783,-1.08163,2.03007,-0.39772 -3.95349,0.98075,-1.38511,-1.44229,-1.08003,2.02729,-0.39474 -4.06977,0.98456,-1.38091,-1.44684,-1.07840,2.02453,-0.39179 -4.18605,0.98591,-1.37941,-1.44849,-1.07781,2.02356,-0.39075 -4.30233,0.98852,-1.37640,-1.45184,-1.07660,2.02167,-0.38874 -4.41860,0.99098,-1.37359,-1.45495,-1.07548,2.01988,-0.38685 -4.53488,0.99341,-1.37084,-1.45800,-1.07439,2.01812,-0.38498 -4.65116,0.99606,-1.36776,-1.46145,-1.07315,2.01619,-0.38295 -4.76744,0.99898,-1.36404,-1.46582,-1.07155,2.01407,-0.38071 -4.88372,1.00043,-1.36211,-1.46813,-1.07070,2.01302,-0.37960 -5.00000,1.00069,-1.36175,-1.46858,-1.07054,2.01282,-0.37940 -5.11628,1.00376,-1.35680,-1.47493,-1.06815,2.01059,-0.37705 -5.23256,1.00622,-1.35157,-1.48216,-1.06536,2.00879,-0.37516 -5.34884,1.00743,-1.34805,-1.48731,-1.06334,2.00790,-0.37422 -5.46512,1.00808,-1.34556,-1.49109,-1.06184,2.00741,-0.37371 -5.58140,1.00869,-1.34168,-1.49724,-1.05937,2.00696,-0.37323 -5.69767,1.00885,-1.34037,-1.49935,-1.05852,2.00684,-0.37310 -5.81395,1.00878,-1.33765,-1.50398,-1.05662,2.00688,-0.37313 -5.93023,1.00781,-1.33405,-1.51069,-1.05381,2.00758,-0.37383 -6.04651,1.00531,-1.32942,-1.52011,-1.04978,2.00937,-0.37568 -6.16279,1.00268,-1.32677,-1.52630,-1.04706,2.01127,-0.37764 -6.27907,0.99803,-1.32472,-1.53284,-1.04404,2.01463,-0.38114 -6.39535,0.99165,-1.32358,-1.53902,-1.04104,2.01923,-0.38597 -6.51163,0.97010,-1.32759,-1.54706,-1.03614,2.03474,-0.40249 -6.62791,0.96078,-1.33028,-1.54906,-1.03465,2.04142,-0.40972 -6.74419,0.94887,-1.33520,-1.54929,-1.03370,2.04993,-0.41904 -6.86047,0.93945,-1.33963,-1.54863,-1.03332,2.05663,-0.42647 -6.97674,0.92389,-1.34795,-1.54608,-1.03331,2.06767,-0.43887 -7.09302,0.91465,-1.35323,-1.54410,-1.03352,2.07419,-0.44631 -7.20930,0.90691,-1.35776,-1.54231,-1.03377,2.07963,-0.45257 -7.32558,0.90221,-1.36054,-1.54121,-1.03394,2.08293,-0.45640 -7.44186,0.89759,-1.36329,-1.54012,-1.03411,2.08617,-0.46017 -7.55814,0.89265,-1.36614,-1.53913,-1.03422,2.08962,-0.46423 -7.67442,0.87846,-1.37316,-1.53837,-1.03374,2.09950,-0.47594 -7.79070,0.87118,-1.37652,-1.53846,-1.03331,2.10454,-0.48200 -7.90698,0.86391,-1.37969,-1.53893,-1.03275,2.10957,-0.48809 -8.02326,0.85690,-1.38217,-1.54040,-1.03180,2.11439,-0.49399 -8.13953,0.85078,-1.38417,-1.54200,-1.03086,2.11859,-0.49916 -8.25581,0.84478,-1.38602,-1.54380,-1.02984,2.12270,-0.50427 -8.37209,0.84073,-1.38723,-1.54510,-1.02912,2.12546,-0.50772 -8.48837,0.83876,-1.38781,-1.54575,-1.02876,2.12681,-0.50941 -8.60465,0.83767,-1.38808,-1.54620,-1.02853,2.12754,-0.51034 -8.72093,0.83716,-1.38820,-1.54642,-1.02842,2.12789,-0.51078 -8.83721,0.83661,-1.38831,-1.54669,-1.02829,2.12827,-0.51125 -8.95349,0.83602,-1.38842,-1.54699,-1.02814,2.12867,-0.51175 -9.06977,0.83548,-1.38851,-1.54729,-1.02799,2.12903,-0.51221 -9.18605,0.83491,-1.38859,-1.54763,-1.02783,2.12942,-0.51271 -9.30233,0.83434,-1.38866,-1.54799,-1.02766,2.12981,-0.51320 -9.41860,0.83430,-1.38867,-1.54801,-1.02765,2.12984,-0.51323 -9.53488,0.83426,-1.38867,-1.54804,-1.02763,2.12987,-0.51327 -9.65116,0.83413,-1.38869,-1.54810,-1.02760,2.12995,-0.51337 -9.76744,0.83118,-1.39037,-1.54776,-1.02760,2.13196,-0.51592 -9.88372,0.82780,-1.39313,-1.54599,-1.02815,2.13425,-0.51884 -10.00000,0.82245,-1.39880,-1.54102,-1.02991,2.13788,-0.52350 -10.11628,0.81409,-1.41035,-1.52877,-1.03451,2.14355,-0.53085 -10.23256,0.81003,-1.41701,-1.52105,-1.03750,2.14629,-0.53443 -10.34884,0.80468,-1.42703,-1.50877,-1.04232,2.14990,-0.53920 -10.46512,0.80130,-1.43428,-1.49945,-1.04603,2.15218,-0.54223 -10.58140,0.79687,-1.44412,-1.48663,-1.05118,2.15516,-0.54621 -10.69767,0.79257,-1.45403,-1.47355,-1.05647,2.15805,-0.55010 -10.81395,0.78840,-1.46355,-1.46099,-1.06158,2.16084,-0.55388 -10.93023,0.78493,-1.47143,-1.45059,-1.06583,2.16316,-0.55704 -11.04651,0.78217,-1.47764,-1.44242,-1.06919,2.16500,-0.55956 -11.16279,0.78003,-1.48260,-1.43582,-1.07191,2.16642,-0.56152 -11.27907,0.77591,-1.49297,-1.42170,-1.07777,2.16916,-0.56530 -11.39535,0.77267,-1.50210,-1.40886,-1.08315,2.17132,-0.56830 -11.51163,0.76914,-1.51297,-1.39326,-1.08972,2.17367,-0.57159 -11.62791,0.76691,-1.52007,-1.38296,-1.09409,2.17514,-0.57366 -11.74419,0.76310,-1.53136,-1.36682,-1.10095,2.17767,-0.57723 -11.86047,0.75804,-1.54696,-1.34424,-1.11060,2.18101,-0.58198 -11.97674,0.75586,-1.55373,-1.33441,-1.11484,2.18244,-0.58403 -12.09302,0.75312,-1.56265,-1.32129,-1.12051,2.18425,-0.58663 -12.20930,0.75097,-1.57037,-1.30969,-1.12555,2.18566,-0.58867 -12.32558,0.74898,-1.57865,-1.29694,-1.13111,2.18697,-0.59058 -12.44186,0.74722,-1.58643,-1.28481,-1.13643,2.18813,-0.59227 -12.55814,0.74625,-1.59247,-1.27496,-1.14078,2.18877,-0.59322 -12.67442,0.74539,-1.60077,-1.26095,-1.14699,2.18935,-0.59410 -12.79070,0.74515,-1.60415,-1.25512,-1.14958,2.18952,-0.59435 -12.90698,0.74511,-1.60480,-1.25400,-1.15009,2.18955,-0.59439 -13.02326,0.74508,-1.60539,-1.25296,-1.15055,2.18956,-0.59442 -13.13953,0.74507,-1.60603,-1.25183,-1.15106,2.18957,-0.59444 -13.25581,0.74506,-1.60671,-1.25062,-1.15160,2.18958,-0.59446 -13.37209,0.74509,-1.60796,-1.24833,-1.15262,2.18957,-0.59444 -13.48837,0.74514,-1.60860,-1.24712,-1.15317,2.18953,-0.59440 -13.60465,0.74520,-1.60887,-1.24658,-1.15341,2.18949,-0.59434 -13.72093,0.74522,-1.60890,-1.24651,-1.15344,2.18949,-0.59433 -13.83721,0.74609,-1.60978,-1.24402,-1.15459,2.18893,-0.59353 -13.95349,0.75016,-1.61035,-1.23885,-1.15706,2.18630,-0.58980 -14.06977,0.75560,-1.60905,-1.23567,-1.15869,2.18277,-0.58479 -14.18605,0.76401,-1.60453,-1.23538,-1.15914,2.17728,-0.57708 -14.30233,0.76798,-1.60198,-1.23605,-1.15899,2.17468,-0.57345 -14.41860,0.77275,-1.59821,-1.23814,-1.15823,2.17155,-0.56911 -14.53488,0.77970,-1.59170,-1.24310,-1.15628,2.16696,-0.56280 -14.65116,0.78657,-1.58434,-1.24974,-1.15358,2.16240,-0.55659 -14.76744,0.79160,-1.57872,-1.25503,-1.15141,2.15905,-0.55207 -14.88372,0.79665,-1.57294,-1.26066,-1.14909,2.15568,-0.54754 -15.00000,0.80069,-1.56824,-1.26531,-1.14717,2.15298,-0.54393 -15.11628,0.80373,-1.56468,-1.26887,-1.14571,2.15093,-0.54122 -15.23256,0.80470,-1.56350,-1.27009,-1.14520,2.15029,-0.54036 -15.34884,0.80669,-1.56107,-1.27260,-1.14416,2.14895,-0.53859 -15.46512,0.80865,-1.55858,-1.27525,-1.14306,2.14763,-0.53685 -15.58140,0.81062,-1.55600,-1.27806,-1.14189,2.14630,-0.53511 -15.69767,0.81160,-1.55473,-1.27944,-1.14132,2.14564,-0.53425 -15.81395,0.81355,-1.55221,-1.28216,-1.14019,2.14433,-0.53252 -15.93023,0.81455,-1.55088,-1.28364,-1.13958,2.14365,-0.53164 -16.04651,0.81647,-1.54823,-1.28661,-1.13834,2.14235,-0.52994 -16.16279,0.81841,-1.54552,-1.28971,-1.13705,2.14104,-0.52823 -16.27907,0.82128,-1.54145,-1.29437,-1.13511,2.13909,-0.52571 -16.39535,0.82320,-1.53870,-1.29755,-1.13378,2.13779,-0.52403 -16.51163,0.82605,-1.53443,-1.30260,-1.13168,2.13586,-0.52153 -16.62791,0.82881,-1.53032,-1.30745,-1.12966,2.13397,-0.51911 -16.74419,0.83072,-1.52747,-1.31084,-1.12825,2.13268,-0.51745 -16.86047,0.83258,-1.52466,-1.31419,-1.12686,2.13141,-0.51582 -16.97674,0.83261,-1.52462,-1.31424,-1.12684,2.13139,-0.51580 -17.09302,0.83350,-1.52331,-1.31578,-1.12620,2.13078,-0.51503 -17.20930,0.83439,-1.52200,-1.31731,-1.12557,2.13017,-0.51425 -17.32558,0.83533,-1.52066,-1.31887,-1.12492,2.12953,-0.51343 -17.44186,0.83635,-1.51917,-1.32063,-1.12419,2.12884,-0.51255 -17.55814,0.83732,-1.51770,-1.32238,-1.12347,2.12817,-0.51171 -17.67442,0.83743,-1.51752,-1.32259,-1.12338,2.12809,-0.51161 -17.79070,0.83874,-1.51514,-1.32567,-1.12209,2.12720,-0.51048 -17.90698,0.83905,-1.51453,-1.32650,-1.12175,2.12699,-0.51021 -18.02326,0.84002,-1.51246,-1.32929,-1.12057,2.12632,-0.50936 -18.13953,0.84052,-1.51125,-1.33101,-1.11984,2.12598,-0.50892 -18.25581,0.84101,-1.50983,-1.33309,-1.11895,2.12564,-0.50850 -18.37209,0.84145,-1.50827,-1.33546,-1.11794,2.12533,-0.50810 -18.48837,0.84173,-1.50717,-1.33717,-1.11721,2.12514,-0.50786 -18.60465,0.84198,-1.50598,-1.33905,-1.11640,2.12497,-0.50764 -18.72093,0.84218,-1.50481,-1.34094,-1.11559,2.12483,-0.50745 -18.83721,0.84241,-1.50294,-1.34404,-1.11425,2.12466,-0.50724 -18.95349,0.84253,-1.50166,-1.34619,-1.11332,2.12458,-0.50713 -19.06977,0.84257,-1.50104,-1.34725,-1.11286,2.12455,-0.50709 -19.18605,0.84261,-1.50038,-1.34838,-1.11237,2.12452,-0.50705 -19.30233,0.84264,-1.49972,-1.34952,-1.11188,2.12450,-0.50702 -19.41860,0.84265,-1.49907,-1.35065,-1.11139,2.12449,-0.50700 -19.53488,0.84266,-1.49842,-1.35178,-1.11090,2.12448,-0.50700 -19.65116,0.84265,-1.49778,-1.35291,-1.11040,2.12448,-0.50699 -19.76744,0.84263,-1.49715,-1.35404,-1.10991,2.12449,-0.50700 -19.88372,0.84261,-1.49648,-1.35525,-1.10939,2.12451,-0.50702 -20.00000,0.84261,-1.49644,-1.35532,-1.10936,2.12451,-0.50702 +0.00000,0.93845,-1.46316,-1.40613,-1.05605,2.05935,-0.42868 +0.05495,0.93841,-1.46317,-1.40614,-1.05604,2.05938,-0.42872 +0.10989,0.93837,-1.46317,-1.40615,-1.05604,2.05940,-0.42875 +0.16484,0.93834,-1.46318,-1.40616,-1.05603,2.05942,-0.42877 +0.21978,0.93831,-1.46319,-1.40616,-1.05602,2.05944,-0.42879 +0.27473,0.93828,-1.46319,-1.40616,-1.05602,2.05946,-0.42881 +0.32967,0.93827,-1.46319,-1.40617,-1.05601,2.05946,-0.42882 +0.38462,0.93827,-1.46318,-1.40617,-1.05601,2.05946,-0.42882 +0.43956,0.93829,-1.46317,-1.40616,-1.05602,2.05943,-0.42879 +0.49451,0.93833,-1.46316,-1.40615,-1.05602,2.05941,-0.42877 +0.54945,0.94162,-1.46188,-1.40514,-1.05663,2.05679,-0.42599 +0.60440,0.94633,-1.46034,-1.40345,-1.05762,2.05314,-0.42210 +0.65934,0.96110,-1.45514,-1.40004,-1.06010,2.04208,-0.41023 +0.71429,0.96696,-1.45285,-1.39930,-1.06083,2.03771,-0.40558 +0.76923,0.97236,-1.45106,-1.39815,-1.06173,2.03372,-0.40134 +0.82418,0.99662,-1.44066,-1.39826,-1.06358,2.01597,-0.38260 +0.87912,1.00312,-1.43783,-1.39852,-1.06399,2.01120,-0.37764 +0.93407,1.01069,-1.43436,-1.39923,-1.06431,2.00565,-0.37190 +0.98901,1.03109,-1.42384,-1.40357,-1.06416,1.99070,-0.35659 +1.04396,1.03416,-1.42220,-1.40436,-1.06408,1.98844,-0.35430 +1.09890,1.03624,-1.42106,-1.40496,-1.06400,1.98691,-0.35275 +1.15385,1.03726,-1.42050,-1.40527,-1.06395,1.98616,-0.35200 +1.20879,1.04878,-1.41339,-1.41007,-1.06289,1.97767,-0.34347 +1.26374,1.04883,-1.41336,-1.41009,-1.06289,1.97764,-0.34343 +1.31868,1.05067,-1.41208,-1.41112,-1.06260,1.97628,-0.34208 +1.37363,1.05375,-1.40981,-1.41308,-1.06203,1.97400,-0.33981 +1.42857,1.05685,-1.40730,-1.41547,-1.06128,1.97171,-0.33753 +1.48352,1.05723,-1.40694,-1.41586,-1.06114,1.97143,-0.33725 +1.53846,1.05836,-1.40581,-1.41709,-1.06071,1.97059,-0.33641 +1.59341,1.05948,-1.40467,-1.41837,-1.06026,1.96977,-0.33559 +1.64835,1.06029,-1.40369,-1.41956,-1.05982,1.96916,-0.33499 +1.70330,1.07022,-1.38772,-1.44116,-1.05144,1.96178,-0.32768 +1.75824,1.07330,-1.38128,-1.45046,-1.04773,1.95949,-0.32541 +1.81319,1.07521,-1.37752,-1.45581,-1.04561,1.95806,-0.32400 +1.86813,1.07766,-1.37149,-1.46478,-1.04201,1.95622,-0.32219 +1.92308,1.08496,-1.34982,-1.49779,-1.02870,1.95075,-0.31680 +1.97802,1.08687,-1.34034,-1.51297,-1.02248,1.94931,-0.31536 +2.03297,1.08765,-1.33677,-1.51863,-1.02018,1.94871,-0.31478 +2.08791,1.08782,-1.33590,-1.52001,-1.01961,1.94859,-0.31465 +2.14286,1.08816,-1.33331,-1.52427,-1.01785,1.94832,-0.31439 +2.19780,1.08826,-1.33242,-1.52573,-1.01725,1.94825,-0.31431 +2.25275,1.08839,-1.32982,-1.53012,-1.01542,1.94815,-0.31420 +2.30769,1.08815,-1.32783,-1.53370,-1.01390,1.94831,-0.31435 +2.36264,1.08743,-1.32478,-1.53937,-1.01145,1.94884,-0.31485 +2.41758,1.07796,-1.31761,-1.55757,-1.00287,1.95581,-0.32158 +2.47253,1.07419,-1.31575,-1.56313,-1.00017,1.95859,-0.32429 +2.52747,1.07179,-1.31457,-1.56667,-0.99845,1.96036,-0.32601 +2.58242,1.05474,-1.31213,-1.58178,-0.99044,1.97291,-0.33836 +2.63736,1.04854,-1.31152,-1.58687,-0.98771,1.97746,-0.34289 +2.69231,1.04435,-1.31127,-1.59003,-0.98599,1.98053,-0.34596 +2.74725,1.03792,-1.31135,-1.59415,-0.98365,1.98523,-0.35068 +2.80220,1.02013,-1.31400,-1.60160,-0.97883,1.99823,-0.36389 +2.85714,1.01458,-1.31499,-1.60372,-0.97743,2.00227,-0.36804 +2.91209,1.00724,-1.31672,-1.60583,-0.97585,2.00761,-0.37356 +2.96703,0.99540,-1.32115,-1.60660,-0.97442,2.01619,-0.38253 +3.02198,0.99124,-1.32270,-1.60690,-0.97391,2.01920,-0.38569 +3.07692,0.98823,-1.32397,-1.60688,-0.97364,2.02137,-0.38799 +3.13187,0.98100,-1.32724,-1.60650,-0.97314,2.02660,-0.39353 +3.18681,0.97790,-1.32877,-1.60613,-0.97301,2.02883,-0.39592 +3.24176,0.97470,-1.33047,-1.60554,-0.97297,2.03113,-0.39838 +3.29670,0.97003,-1.33304,-1.60458,-0.97295,2.03449,-0.40200 +3.35165,0.96503,-1.33572,-1.60368,-0.97288,2.03808,-0.40588 +3.40659,0.96402,-1.33627,-1.60348,-0.97287,2.03881,-0.40667 +3.46154,0.95349,-1.34301,-1.59982,-0.97347,2.04635,-0.41490 +3.51648,0.95241,-1.34372,-1.59942,-0.97355,2.04712,-0.41574 +3.57143,0.94928,-1.34585,-1.59813,-0.97381,2.04936,-0.41821 +3.62637,0.94405,-1.34960,-1.59569,-0.97439,2.05309,-0.42233 +3.68132,0.93994,-1.35266,-1.59359,-0.97492,2.05602,-0.42559 +3.73626,0.93789,-1.35422,-1.59249,-0.97521,2.05748,-0.42722 +3.79121,0.93697,-1.35495,-1.59193,-0.97537,2.05813,-0.42795 +3.84615,0.92745,-1.36381,-1.58412,-0.97784,2.06489,-0.43555 +3.90110,0.92454,-1.36693,-1.58103,-0.97890,2.06696,-0.43789 +3.95604,0.91913,-1.37356,-1.57390,-0.98146,2.07079,-0.44227 +4.01099,0.90474,-1.39758,-1.54395,-0.99298,2.08097,-0.45402 +4.06593,0.89803,-1.40912,-1.52935,-0.99871,2.08570,-0.45954 +4.12088,0.87738,-1.45433,-1.46737,-1.02396,2.10019,-0.47682 +4.17582,0.87143,-1.46740,-1.44931,-1.03148,2.10435,-0.48185 +4.23077,0.86544,-1.47993,-1.43221,-1.03861,2.10852,-0.48694 +4.28571,0.84436,-1.52459,-1.37096,-1.06446,2.12310,-0.50505 +4.34066,0.84005,-1.53462,-1.35674,-1.07060,2.12607,-0.50880 +4.39560,0.83705,-1.54126,-1.34746,-1.07461,2.12812,-0.51142 +4.45055,0.83540,-1.54508,-1.34206,-1.07696,2.12926,-0.51287 +4.50549,0.83178,-1.55368,-1.32977,-1.08230,2.13174,-0.51604 +4.56044,0.82471,-1.56996,-1.30674,-1.09233,2.13657,-0.52227 +4.61538,0.82109,-1.57923,-1.29322,-1.09829,2.13904,-0.52548 +4.67033,0.81921,-1.58407,-1.28615,-1.10142,2.14033,-0.52715 +4.72527,0.81762,-1.58858,-1.27940,-1.10442,2.14141,-0.52858 +4.78022,0.81728,-1.58950,-1.27803,-1.10503,2.14164,-0.52888 +4.83516,0.81623,-1.59254,-1.27345,-1.10706,2.14236,-0.52982 +4.89011,0.81621,-1.59259,-1.27338,-1.10710,2.14237,-0.52983 +4.94505,0.81604,-1.59312,-1.27257,-1.10746,2.14248,-0.52999 +5.00000,0.81603,-1.59317,-1.27249,-1.10749,2.14249,-0.53000 +5.05495,0.81585,-1.59380,-1.27152,-1.10793,2.14262,-0.53017 +5.10989,0.81583,-1.59384,-1.27144,-1.10796,2.14263,-0.53018 +5.16484,0.81582,-1.59389,-1.27137,-1.10800,2.14263,-0.53019 +5.21978,0.81582,-1.59393,-1.27130,-1.10803,2.14264,-0.53019 +5.27473,0.81735,-1.59459,-1.26858,-1.10937,2.14161,-0.52887 +5.32967,0.81875,-1.59493,-1.26660,-1.11037,2.14067,-0.52766 +5.38462,0.83542,-1.58930,-1.26107,-1.11395,2.12940,-0.51327 +5.43956,0.84030,-1.58724,-1.26031,-1.11460,2.12608,-0.50910 +5.49451,0.84382,-1.58530,-1.26059,-1.11469,2.12368,-0.50609 +5.54945,0.84977,-1.58167,-1.26178,-1.11452,2.11961,-0.50102 +5.60440,0.86461,-1.57253,-1.26512,-1.11391,2.10941,-0.48848 +5.65934,0.87012,-1.56857,-1.26748,-1.11318,2.10560,-0.48386 +5.71429,0.87556,-1.56470,-1.26980,-1.11246,2.10184,-0.47931 +5.76923,0.88942,-1.55405,-1.27724,-1.10996,2.09219,-0.46780 +5.82418,0.89581,-1.54928,-1.28052,-1.10888,2.08772,-0.46255 +5.87912,0.90221,-1.54459,-1.28369,-1.10786,2.08324,-0.45730 +5.93407,0.91325,-1.53539,-1.29127,-1.10516,2.07548,-0.44831 +5.98901,0.91669,-1.53246,-1.29377,-1.10426,2.07305,-0.44553 +6.04396,0.91965,-1.53016,-1.29553,-1.10367,2.07095,-0.44313 +6.09890,0.92487,-1.52590,-1.29904,-1.10243,2.06726,-0.43892 +6.15385,0.92751,-1.52371,-1.30090,-1.10178,2.06539,-0.43681 +6.20879,0.93014,-1.52149,-1.30283,-1.10109,2.06353,-0.43470 +6.26374,0.93093,-1.52078,-1.30348,-1.10085,2.06297,-0.43407 +6.31868,0.93670,-1.51474,-1.30983,-1.09840,2.05886,-0.42944 +6.37363,0.93759,-1.51377,-1.31088,-1.09799,2.05823,-0.42873 +6.42857,0.93846,-1.51277,-1.31200,-1.09755,2.05761,-0.42804 +6.48352,0.94343,-1.50626,-1.31987,-1.09437,2.05406,-0.42407 +6.53846,0.94797,-1.49828,-1.33073,-1.08984,2.05080,-0.42044 +6.59341,0.94800,-1.49823,-1.33079,-1.08981,2.05079,-0.42042 +6.64835,0.94995,-1.49424,-1.33648,-1.08741,2.04938,-0.41886 +6.70330,0.95449,-1.47425,-1.36889,-1.07332,2.04609,-0.41516 +6.75824,0.95498,-1.47198,-1.37258,-1.07172,2.04573,-0.41475 +6.81319,0.95592,-1.46576,-1.38300,-1.06717,2.04504,-0.41397 +6.86813,0.95629,-1.44199,-1.42504,-1.04869,2.04469,-0.41350 +6.92308,0.95622,-1.43615,-1.43544,-1.04413,2.04472,-0.41350 +6.97802,0.95462,-1.41562,-1.47284,-1.02775,2.04580,-0.41459 +7.03297,0.95413,-1.41007,-1.48298,-1.02332,2.04614,-0.41494 +7.08791,0.95343,-1.40608,-1.49049,-1.02002,2.04662,-0.41545 +7.14286,0.95139,-1.39595,-1.50973,-1.01159,2.04804,-0.41696 +7.19780,0.95019,-1.39000,-1.52101,-1.00668,2.04888,-0.41786 +7.25275,0.94968,-1.38744,-1.52585,-1.00457,2.04923,-0.41824 +7.30769,0.94911,-1.38511,-1.53033,-1.00261,2.04963,-0.41867 +7.36264,0.94845,-1.38288,-1.53470,-1.00070,2.05009,-0.41917 +7.41758,0.94822,-1.38214,-1.53615,-1.00006,2.05026,-0.41934 +7.47253,0.94737,-1.38014,-1.54026,-0.99824,2.05086,-0.41999 +7.52747,0.94705,-1.37942,-1.54175,-0.99759,2.05108,-0.42024 +7.58242,0.94673,-1.37892,-1.54286,-0.99709,2.05131,-0.42048 +7.63736,0.94630,-1.37835,-1.54419,-0.99649,2.05161,-0.42082 +7.69231,0.94374,-1.37639,-1.54954,-0.99400,2.05342,-0.42282 +7.74725,0.94302,-1.37590,-1.55093,-0.99335,2.05394,-0.42338 +7.80220,0.94115,-1.37508,-1.55379,-0.99199,2.05526,-0.42484 +7.85714,0.94092,-1.37500,-1.55411,-0.99183,2.05542,-0.42502 +7.91209,0.93810,-1.37500,-1.55629,-0.99067,2.05742,-0.42724 +7.96703,0.93756,-1.37503,-1.55666,-0.99046,2.05780,-0.42767 +8.02198,0.93577,-1.37530,-1.55760,-0.98992,2.05907,-0.42908 +8.07692,0.92872,-1.37787,-1.55868,-0.98886,2.06406,-0.43468 +8.13187,0.92613,-1.37898,-1.55880,-0.98859,2.06589,-0.43675 +8.18681,0.92339,-1.38030,-1.55868,-0.98841,2.06783,-0.43894 +8.24176,0.91389,-1.38615,-1.55616,-0.98869,2.07453,-0.44658 +8.29670,0.90999,-1.38875,-1.55479,-0.98895,2.07727,-0.44974 +8.35165,0.90710,-1.39077,-1.55362,-0.98922,2.07930,-0.45208 +8.40659,0.89358,-1.40279,-1.54386,-0.99231,2.08878,-0.46314 +8.46154,0.88810,-1.40785,-1.53961,-0.99371,2.09261,-0.46766 +8.51648,0.88376,-1.41230,-1.53548,-0.99515,2.09564,-0.47125 +8.57143,0.88119,-1.41495,-1.53303,-0.99601,2.09743,-0.47339 +8.62637,0.87199,-1.42516,-1.52302,-0.99964,2.10383,-0.48108 +8.68132,0.87114,-1.42612,-1.52208,-0.99998,2.10442,-0.48179 +8.73626,0.86863,-1.42917,-1.51889,-1.00118,2.10616,-0.48390 +8.79121,0.86626,-1.43215,-1.51570,-1.00239,2.10780,-0.48590 +8.84615,0.86526,-1.43354,-1.51413,-1.00300,2.10850,-0.48675 +8.90110,0.86346,-1.43639,-1.51070,-1.00435,2.10975,-0.48827 +8.95604,0.86343,-1.43643,-1.51065,-1.00437,2.10977,-0.48829 +9.01099,0.86113,-1.44156,-1.50364,-1.00724,2.11136,-0.49026 +9.06593,0.86046,-1.44344,-1.50094,-1.00836,2.11183,-0.49083 +9.12088,0.85960,-1.44764,-1.49431,-1.01117,2.11244,-0.49159 +9.17582,0.85930,-1.44994,-1.49053,-1.01279,2.11265,-0.49186 +9.23077,0.85915,-1.45229,-1.48650,-1.01453,2.11276,-0.49200 +9.28571,0.85928,-1.45381,-1.48371,-1.01576,2.11268,-0.49190 +9.34066,0.86640,-1.46518,-1.45740,-1.02771,2.10783,-0.48604 +9.39560,0.86798,-1.46768,-1.45160,-1.03037,2.10675,-0.48475 +9.45055,0.87141,-1.47135,-1.44211,-1.03476,2.10440,-0.48193 +9.50549,0.88524,-1.48141,-1.41237,-1.04880,2.09486,-0.47059 +9.56044,0.89056,-1.48635,-1.39904,-1.05507,2.09118,-0.46626 +9.61538,0.89406,-1.48922,-1.39096,-1.05889,2.08875,-0.46343 +9.67033,0.90127,-1.49475,-1.37501,-1.06647,2.08373,-0.45761 +9.72527,0.90334,-1.49610,-1.37087,-1.06846,2.08229,-0.45594 +9.78022,0.90393,-1.49651,-1.36964,-1.06905,2.08188,-0.45546 +9.83516,0.90469,-1.49697,-1.36818,-1.06976,2.08135,-0.45485 +9.89011,0.90472,-1.49699,-1.36813,-1.06978,2.08133,-0.45483 +9.94505,0.90486,-1.49706,-1.36787,-1.06990,2.08123,-0.45472 +10.00000,0.90489,-1.49708,-1.36782,-1.06993,2.08121,-0.45469 diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py index 9651e99..19dd803 100644 --- a/python/examples/path_following_mpc.py +++ b/python/examples/path_following_mpc.py @@ -1,10 +1,11 @@ # PYTHON_ARGCOMPLETE_OK import numpy as np import time -import argparse +import argparse, argcomplete from functools import partial from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager -from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, createCrocoPathFollowingOCP1, solveCrocoOCP +from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args +from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoPathFollowingOCP1, solveCrocoOCP from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoPathFollowingMPCControlLoop, CrocoPathFollowingMPC from ur_simple_control.basics.basics import followKinematicJointTrajP from ur_simple_control.util.logging_utils import LogManager @@ -12,7 +13,7 @@ from ur_simple_control.visualize.visualize import plotFromDict from ur_simple_control.clik.clik import getClikArgs import pinocchio as pin import crocoddyl -import mim_solvers +import importlib.util def get_args(): @@ -26,6 +27,8 @@ def get_args(): if __name__ == "__main__": args = get_args() + if importlib.util.find_spec('mim_solvers'): + import mim_solvers robot = RobotManager(args) # TODO: put this back for nicer demos #Mgoal = robot.defineGoalPointCLI() diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv index 8fb43e1..c1306d7 100644 --- a/python/examples/path_in_pixels.csv +++ b/python/examples/path_in_pixels.csv @@ -1,173 +1,183 @@ -0.55466,0.55325 -0.55466,0.55325 -0.55466,0.55440 -0.54454,0.56710 -0.53645,0.57518 -0.52634,0.58442 -0.51623,0.59134 -0.51016,0.59711 -0.50612,0.60058 -0.50207,0.60289 -0.49803,0.60404 -0.49398,0.60750 -0.48792,0.60981 -0.48185,0.61328 -0.47578,0.61443 -0.46971,0.61789 -0.45960,0.62136 -0.44949,0.62482 -0.43735,0.62828 -0.42724,0.63059 -0.41713,0.63290 -0.41106,0.63405 -0.40297,0.63521 -0.39488,0.63636 -0.39084,0.63636 -0.38477,0.63636 -0.37870,0.63636 -0.37466,0.63521 -0.36859,0.63405 -0.36252,0.63175 -0.35443,0.62828 -0.34432,0.62597 -0.33825,0.62251 -0.33016,0.62020 -0.32410,0.61789 -0.31803,0.61558 -0.31601,0.61443 -0.31196,0.61212 -0.30792,0.61097 -0.30387,0.60981 -0.29983,0.60750 -0.29578,0.60289 -0.29376,0.60058 -0.29376,0.59942 -0.28972,0.59250 -0.28769,0.58326 -0.28769,0.57633 -0.28769,0.57172 -0.28769,0.56479 -0.28769,0.56248 -0.28972,0.55671 -0.29376,0.54863 -0.29781,0.53824 -0.30387,0.53131 -0.31399,0.52439 -0.32410,0.51746 -0.35443,0.50476 -0.36657,0.50014 -0.38275,0.49668 -0.39488,0.49322 -0.41511,0.48745 -0.42724,0.48398 -0.43735,0.48052 -0.44342,0.47821 -0.44949,0.47590 -0.45556,0.47244 -0.47174,0.45974 -0.47983,0.45281 -0.48792,0.44589 -0.49398,0.43665 -0.50005,0.42973 -0.50612,0.42280 -0.51016,0.41818 -0.51219,0.41587 -0.51219,0.41356 -0.51219,0.41241 -0.51219,0.41126 -0.51219,0.41010 -0.51219,0.40895 -0.51219,0.40779 -0.51219,0.40664 -0.51219,0.40664 -0.51219,0.40664 -0.51421,0.40779 -0.52634,0.41587 -0.53443,0.42280 -0.54454,0.42973 -0.56072,0.44012 -0.56881,0.44704 -0.58095,0.45512 -0.58904,0.46205 -0.60117,0.46898 -0.61331,0.47590 -0.62544,0.48167 -0.63555,0.48629 -0.64364,0.48975 -0.64971,0.49322 -0.66185,0.50130 -0.67196,0.50938 -0.68409,0.51861 -0.69218,0.52439 -0.70634,0.53131 -0.72454,0.54286 -0.73263,0.54747 -0.74274,0.55440 -0.75083,0.56133 -0.75892,0.56941 -0.76701,0.57633 -0.77106,0.58442 -0.77713,0.59481 -0.77915,0.59942 -0.77915,0.60058 -0.77915,0.60173 -0.77915,0.60289 -0.77915,0.60404 -0.77915,0.60635 -0.77713,0.60866 -0.77308,0.61097 -0.76904,0.61212 -0.75892,0.61558 -0.74274,0.62020 -0.72859,0.62367 -0.71039,0.62713 -0.70230,0.62828 -0.69218,0.62828 -0.67803,0.62828 -0.66387,0.62713 -0.65376,0.62713 -0.64364,0.62713 -0.63555,0.62713 -0.62949,0.62713 -0.62746,0.62597 -0.62342,0.62597 -0.61938,0.62482 -0.61533,0.62367 -0.61331,0.62367 -0.60926,0.62367 -0.60724,0.62251 -0.60320,0.62136 -0.59915,0.62020 -0.59308,0.61905 -0.58904,0.61789 -0.58297,0.61558 -0.57690,0.61443 -0.57286,0.61328 -0.56881,0.61212 -0.56881,0.61212 -0.56679,0.61212 -0.56477,0.61212 -0.56275,0.61212 -0.56072,0.61097 -0.55870,0.60981 -0.55870,0.60866 -0.55668,0.60404 -0.55668,0.60173 -0.55466,0.59942 -0.55466,0.59596 -0.55466,0.59250 -0.55466,0.58903 -0.55466,0.58672 -0.55466,0.58442 -0.55466,0.58211 -0.55466,0.57864 -0.55466,0.57633 -0.55466,0.57518 -0.55466,0.57403 -0.55466,0.57287 -0.55466,0.57172 -0.55466,0.57056 -0.55466,0.56941 -0.55466,0.56825 -0.55466,0.56710 -0.55466,0.56710 +0.48757,0.61206 +0.48757,0.61206 +0.48609,0.61358 +0.48461,0.61510 +0.48017,0.61662 +0.47869,0.62117 +0.46832,0.62573 +0.46239,0.63029 +0.44461,0.64548 +0.44017,0.64700 +0.43424,0.65307 +0.42832,0.65915 +0.40758,0.67130 +0.39869,0.67434 +0.39276,0.68041 +0.35869,0.69256 +0.34980,0.69560 +0.33943,0.69864 +0.31128,0.70472 +0.30684,0.70472 +0.30388,0.70472 +0.30240,0.70472 +0.28610,0.70472 +0.28610,0.70472 +0.28314,0.70168 +0.27869,0.70016 +0.27425,0.69712 +0.27425,0.69256 +0.27277,0.69105 +0.27129,0.68953 +0.27129,0.68497 +0.25647,0.66522 +0.25351,0.65459 +0.24906,0.65155 +0.24610,0.64244 +0.22981,0.61510 +0.22684,0.59991 +0.22388,0.59535 +0.22388,0.59383 +0.22388,0.58928 +0.22388,0.58776 +0.22388,0.58320 +0.22684,0.57864 +0.22832,0.57257 +0.24610,0.55130 +0.25203,0.54523 +0.25351,0.54067 +0.27425,0.52092 +0.28017,0.51333 +0.28462,0.50877 +0.29203,0.50270 +0.31277,0.48903 +0.31869,0.48447 +0.32758,0.47991 +0.34388,0.47687 +0.34832,0.47384 +0.35276,0.47384 +0.36165,0.47080 +0.36610,0.47080 +0.37054,0.47080 +0.37647,0.46928 +0.38239,0.46624 +0.38387,0.46624 +0.39869,0.46624 +0.40017,0.46624 +0.40461,0.46624 +0.41202,0.46624 +0.41795,0.46624 +0.42091,0.46624 +0.42239,0.46776 +0.43721,0.47232 +0.44165,0.47687 +0.45054,0.48295 +0.47869,0.50573 +0.49350,0.51333 +0.54535,0.55282 +0.56165,0.56194 +0.57794,0.56953 +0.63424,0.59839 +0.64609,0.60598 +0.65498,0.60902 +0.65942,0.61206 +0.66979,0.61814 +0.69053,0.62725 +0.70090,0.63484 +0.70683,0.63788 +0.71127,0.64244 +0.71275,0.64244 +0.71571,0.64548 +0.71571,0.64548 +0.71571,0.64700 +0.71571,0.64700 +0.71571,0.64852 +0.71423,0.64852 +0.71275,0.65003 +0.71127,0.65155 +0.67720,0.66370 +0.67275,0.66522 +0.63868,0.67738 +0.62979,0.68041 +0.62238,0.68041 +0.61201,0.68345 +0.58831,0.69408 +0.57794,0.69408 +0.56905,0.69712 +0.54535,0.70168 +0.53498,0.70472 +0.52461,0.70775 +0.50535,0.70775 +0.49943,0.70775 +0.49498,0.71079 +0.48609,0.71079 +0.48165,0.71079 +0.47720,0.71079 +0.47572,0.70927 +0.46535,0.70472 +0.46387,0.70320 +0.46239,0.70168 +0.45350,0.69560 +0.44609,0.68345 +0.44609,0.68345 +0.44313,0.67738 +0.43276,0.64700 +0.43128,0.64396 +0.42832,0.63484 +0.41647,0.59839 +0.41350,0.58928 +0.40313,0.55586 +0.40017,0.54675 +0.40017,0.53915 +0.39573,0.52092 +0.39276,0.51029 +0.39128,0.50573 +0.39128,0.50118 +0.39128,0.49662 +0.39128,0.49510 +0.39276,0.49054 +0.39276,0.48903 +0.39573,0.48751 +0.39721,0.48599 +0.40313,0.47991 +0.40461,0.47839 +0.40906,0.47536 +0.41054,0.47536 +0.42091,0.47536 +0.42239,0.47536 +0.42684,0.47536 +0.44017,0.47536 +0.44461,0.47536 +0.44906,0.47536 +0.46387,0.47536 +0.46980,0.47536 +0.47424,0.47536 +0.49646,0.47991 +0.50535,0.47991 +0.51276,0.48295 +0.51720,0.48295 +0.53350,0.48599 +0.53498,0.48599 +0.53942,0.48903 +0.54387,0.49054 +0.54535,0.49358 +0.54831,0.49814 +0.54831,0.49814 +0.55128,0.50877 +0.55128,0.51333 +0.55128,0.52244 +0.55128,0.52700 +0.55128,0.53156 +0.54831,0.53611 +0.53942,0.56649 +0.53942,0.57257 +0.53498,0.58320 +0.52461,0.61814 +0.52461,0.63333 +0.52165,0.64244 +0.51720,0.66067 +0.51424,0.66522 +0.51424,0.66674 +0.51276,0.66826 +0.51276,0.66826 +0.51128,0.66826 +0.51128,0.66826 diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc index f74d281c0b1c18985912c21b5766180e6ed6e926..8aac4dcf219d27bcd32fee4d77ef29b50de1926c 100644 GIT binary patch delta 115 zcmccI!2GCznfEj=FBby?1A~#BOZvTyye2y)-_GD<+%$PZjr8PwVKS4Ob~rGVa85qJ zEHn9ajo9Q1>KvTG3`MIM7#K9!CjZ@`%;+{*W@n1%9e&~d+RoY;A=hQ~F3Rd%=GVW# Xp+EU|l=S8eJ4IL+4K^R$+s6a|U7RZ9 delta 107 zcmaFVz<jxZnfEj=FBby?14GwN$MkC(c};dqwhQHCTEQ^+;7;ku`@^IsH|=nk%(z&5 z@~%bFoWTr5s~H#=G+8IJ?^K?wzcWGf2EXuieuazt3fK8nFY>Ei=2ySKp+5O{l=SBP PJ4IL+H8x+_+s6a|`idsB diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py index 5ec984d..d402bf8 100644 --- a/python/ur_simple_control/clik/clik.py +++ b/python/ur_simple_control/clik/clik.py @@ -301,9 +301,9 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data): # get actual velocity, not the commanded one. # although that would be fun to compare i guess # TODO: have both, but call the compute control signal like it should be - log_item['dqs'] = robot.getQd().reshape((robot.model.nq,)) + log_item['dqs'] = robot.getQd().reshape((robot.model.nv,)) log_item['wrench'] = wrench.reshape((6,)) - log_item['tau'] = tau.reshape((robot.model.nq,)) + log_item['tau'] = tau.reshape((robot.model.nv,)) # we're not saving here, but need to respect the API, # hence the empty dict return breakFlag, save_past_dict, log_item diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py index 28f10d3..df84f3b 100644 --- a/python/ur_simple_control/managers.py +++ b/python/ur_simple_control/managers.py @@ -1035,7 +1035,7 @@ class ProcessManager: def getData(self): if not self.data_queue.empty(): self.lastest_data = self.data_queue.get_nowait() - return self.lastest_data.copy() + return copy.deepcopy(self.lastest_data) def terminateProcess(self): if self.args.debug_prints: diff --git a/python/ur_simple_control/optimal_control/__init__.py b/python/ur_simple_control/optimal_control/__init__.py index 255cf7b..569adb6 100644 --- a/python/ur_simple_control/optimal_control/__init__.py +++ b/python/ur_simple_control/optimal_control/__init__.py @@ -1,4 +1,6 @@ -from .create_pinocchio_casadi_ocp import * +import importlib.util +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/optimal_control/create_pinocchio_casadi_ocp.py b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py index a11cf65..be9364a 100644 --- a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py +++ b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py @@ -1,5 +1,8 @@ import casadi import pinocchio as pin +if int(pin.__version__[0]) < 3: + print("you need to have pinocchio version 3.0.0 or greater to use pinocchio.casadi!") + exit() from pinocchio import casadi as cpin import numpy as np from ur_simple_control.managers import RobotManager, ControlLoopManager diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py index abad479..2d36c72 100644 --- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py +++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py @@ -2,9 +2,11 @@ from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoPathFollowingOCP1 import pinocchio as pin import crocoddyl -import mim_solvers import numpy as np from functools import partial +import importlib.util +if importlib.util.find_spec('mim_solvers'): + import mim_solvers diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py index 0b9a45d..f1856a4 100644 --- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py +++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py @@ -1,13 +1,15 @@ import numpy as np import pinocchio as pin import crocoddyl -import mim_solvers from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager import argparse from ur_simple_control.basics.basics import followKinematicJointTrajP from ur_simple_control.visualize.visualize import plotFromDict import example_robot_data import time +import importlib.util +if importlib.util.find_spec('mim_solvers'): + import mim_solvers def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3): # create torque bounds which correspond to percentage diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc index da6b4e16226e0ccf7592148fce5af4781ca85e75..d892f4bb1b092ff28f489a8358275af98997021d 100644 GIT binary patch delta 2160 zcmbQ!$oQj?k?%AwFBby?1B0o9OZpV&iF^`_braP+%$aj|a=D{;85vR-QW$giqWDr+ zS{S1EQ@B!CTNtARQrKD;q6AaeTNt8*QaD-|qJ&d8TNt85HqLTqViel^h)IK$QDZYV z$7?=Dm(AbB-g7Xn-27aAG9!030|P@*4v5H|+^Xot+Qh)XuyXSS#q*4e?2{KLi*QV1 zWnh@jP{TBNkFqo?dkSL>`{X;y?z-&pEFf2caF0F<LkTyC1;RB9HH<Y(YnWFvgLn)K zwH!4ZlPAiE2&AyAVO`A#5oBbj;h20tRYWYEA%$%b;~I8ki7dg%69hyizm(xt=ExF; zXh0@YSddjuUZ5(%mBJ~>z`#(;S;IM5K~<EWD}{SC6GRIm15}JBg%@4Sm@kDloiT;K zij{!@Ay)!23alB!D_k{PAg?gha7<pHD$mF@`GBe@zrY$nB=@jRexNQg`2i=#BsErc z?pmH2jx5>9AEkx)xseocLBu3^ki^)s<iVDLiOB_OA~Ge4U^W8-Ly9m$AtwVTLk-Lp zPIulCWtgfOUYM#96_{AI1_MJ;3zA79Fg1(}HM}XJVEK|7=E(-?B2q=%ND9S}6oR}f zGI@cz=wx>u;mHb`a*z;!3o&KsPiAD35vXLS;f-NpV5p_Y<6=Gcl^IG5U|vvRC@N!Q zz~)48(w#WjPLtP8!tWMKMq*xyCd>0#GeO{_eNihY3m}Lr1_p)@O~zX+`30$Ynk>lD zlUp<c*z&>3H{a6aVPuq?{9Mb9v3RnYwt9UT0|Ub?p5oM!_=2MRg4Cjt%3A`VdErHg z1qG=ozNy6-$%!RJVIa-5AeGDor6olkAg&LH0HuH;cMz)_L{xx?N)S;6B4QXA7&Q5c zka8?YR3AjtgAC(L&YiqfTavM1@_B6uu0jR|hGISj1_p)6AGA%l<}ffYoB;{BPu9~h z7g?ZsL&y4trs)?}CT{Zv=MQWQtQ?bzb=pN`7O39PwYi~X_Jx&+$D+XnB*VFxQ#X@Q zTGL6*lat|ufTbrV!%0p@8*dKg)9j4i9L&kAlLI(K#01zG7}`bK#ilcqK(jdmLk+{^ z?Rw?RVx3}>4fRDeMN4>L(VD`N!V1fDwXB&8wQME)Fu4-O$;X-Hli6!IN(5nIHSDn1 zMiS$IiGc#Bma_z2TGTMWWLy{~Fvf;3Fo4q~S1osmDBMJb8t%y-^*fnCw7#MI<T(Zc z%sic9lh+%_^Kv5D!ZrD}fig314ew+|Ls3bjj9j9LVwS$4E;C;Z@8ozxTd<k)4MqJK zYPdn=NeXicb1PE~Um6pLSHn}o2d24G*g-BwvWpMqZjksIj@8gomXV<+goUAocQUh) zsxm(lLk(*UZw)^z{A$>0xNR617(g`0+$@91h6ciu<BT+zMYBvnS(BwxG|L>skm?l8 zvV^%BL3fH}SxshS6Q2Cegk7LkphL7nER89JuZ5#l5FB+PlVy#?c?Dq>)(X|IOm;L@ zWM<5Am|P$vKiN`)dvd=qtDrC^Lyd3>TNN(@L#=QPbC&aDMm8ydT9F!&6d`a~BT)m> z^3Ir#QFO9`fT*MZ%+HJrH4H8cu|A*@1mu^=+9tZ<BI%4s@@$asg!-__M1&J2;s$j; zr$Ub(3j-%Zr)ZWh!h<jtl1I6ip<%)V_Kd=0OH*Y=vB|Nf@-T00o@Xk|!k9LBlVu!Z z_hvyWQAUZypzIb3%5L1b1v&A_`FVM%$t6WoCfiv%iHqN2&dE<Nf@jI1sUX$UCQq{V zXIwh@k+lY^I5<P{*=VqCWME*>-E3{+z|1H*xyF97%q`)9%)I>M<c!RGy|T>W(!`w1 zs??&%AQMU^3pxmMgR15tP(Ci&I9cC8N^3U*0|O|d6}vDnFnnNUWMsU}Aas{O{yu}| pZ3gb!4BTJY7#L$1?=rYfE^v?$2xVl{{p`e?#K<UIB*wtN0029Rwx<99 delta 1976 zcmey-$T+8wk?%AwFBby?1B2Tp$MjC8iF^`_MHAIMv>9`FqIgnRS{R~uQ@B!CTNtDG zQrKD;qWDwTTNt7QQaD-|q6AYoTNt8*HZF5#Vier`j!A=+QGK&G$7?=D=grLG?>QJX zH-DC&%*dU^z`#(H4I*+TPgHbcZDe3zSh4wz;(10}Hjvd&%*w#Pz|6qF@Ocs|1H*KN z8fK6_s2rSfVTjdbV5ntFVXR@B{6g8Ck!`Ypili`O4O0#CYLIRQ28LSp8ul#S$s1XO z`I*<StY(C0W@M;gpWL7#F3h@!aSa=?XqF&IEiXH=*yM>UB77;#C_)=lMJBHkU}LN0 zsNu*G2k8;yOyOG11UDZh%ALZ4DQe7{!jsOJ!dJ!0z<`h~;fCmDV5mX$17{6q7VqSR ztip`!lQ*c!GjdM8peoAGzeWJbNi35e%8E=@5aO6DpvKD1Rm)w&o+S;^!_S4JiW4F> zSznEZk#+J1brHT4L4<UP{A33W5jIW+PKFwYc^`~9)p<%3;qKyrDK1fhiDhdrFch_v z$WLxi6XC1jNf7}Hl+-X}sZ3^+7ZsO>DNGTCX$A*BC<GOFL?_Qx7h%+%Y%ePd_MFIM zsMzF8by<N*h8mt2CI*IDqWz<S?4O?d$_yoXF#A;)ipm%nu((T%RCi7G)8w@j_q)ZC zk(igF$?|;GOb|F}U(^Ch*l;35lkpZyenD!UCJVC6<e8cQY<Xano8N2lFfvL^{-b5b zSTxyMTV1l0fq~%`PjPBVd_hruL26M+Wl<PNY0czbZCULy5VIUaRDg&|5D~+`z@W)n zqz__)atSPV-O^T&tp}OLl9!*7dW)qjv&!feQ=#E4rb45l0tN<#$#2*tCL8D|a?N63 zU^vadz));GIa<e@Z-(mv`wc#qxf~muZ?LdWUZ&H|FFPZ2f&Ye>%iK;4E;m>>HaqEN zGD@mDsd=(9oZz$cWM??Z?r7u9#(bKU(VLBVawD^tm;gHiL%T@3=yZk>Xr==t>&YMV z$|XfRMYF)=3@m0KbeG8F27PfsmQ03P))Ib*7z0BI<KzWg^2uzq>?MLQu^M=mMG}K& z*(y-B=O__|t7e47uM5Kj#@G-B25^$%tmP^ZMX0Ianyh8e$;nv5CCR|Rkfi|1(v!~{ zh%s|_icWrEAkWW%WEW=&+vIaRimW^}JXz|XOvcDHIlxd9LpR&xc0)c^-Wr}P?a2*- zGLyF%%CmrMnC!qRVhCa|FfgPrr!coN)$pb<fh?}!uHgmKTq*1zk04pfJ6X<1mXT+& zfu_jh03$(JJ|>15mKvTKK3EA-!&<{-!@$4*q9@lGIWme&K42uzC^Gp0h@50B$}E~? zHTl1+lt3+ihe(HL8dC~i3rDR$r|4uSV{Lu`m^rnAHOyJ|-~i}1=3!>YasXxI$@TnP zllK}62numB)Ci@pRq-+~)C$!wWjRltFC)z_TqB$!1kU;rHOM+T)wtR67Icbaxq`w; zo*xz$j0`o5E)20gpv(<&Fv!zcZj%E9)dhvq;W5St^Ni%=-6kTOtkBqToBUr^Mxn=# zg@KcyQzXk57L5qH6XI1FrcRLe<yeq>#67vlRGCq9@-$O<WWQ~`ZYs>em^k^hWgKJI zW-lvIM#hDc6RgD;CrvK5b`lZ;=dfGMIr-^DlR<fH%H#{y{)~$!E7)i-icLN*CO$dQ zMuc@e0|SG`<|-QpW=7%3yX_~t6cvLMgA!ViIf#`3B0#lX5h&Xgfz+>LC=vsSihyz} thfQvNN@-52UC|B(1_n^pDR!K^#zA)S83!4GKt@L0&s9uGjEuryEdcc<qsIUM diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py index 064d192..11f018e 100644 --- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py +++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py @@ -4,9 +4,10 @@ import os import time from itertools import zip_longest from multiprocessing import Process, Queue -from ur_simple_control.managers import getMinimalArgParser, RobotManager +from ur_simple_control.managers import getMinimalArgParser, RobotManager, ProcessManager from ur_simple_control.util.logging_utils import LogManager from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer, logPlotter +from functools import partial def getLogComparisonArgs(): parser = getMinimalArgParser() @@ -24,6 +25,7 @@ def getLogComparisonArgs(): args.simulation = False return args +# TODO: use ProcessManager for this class ManipulatorComparisonManager: def __init__(self, args): self.args = args @@ -53,27 +55,16 @@ class ManipulatorComparisonManager: print("you did not give me a valid path for log2, exiting") exit() - self.manipulator_visualizer_cmd_queue = Queue() - self.manipulator_visualizer_ack_queue = Queue() - - # we are assuming both robots are the same, - # but this does not necessarily have to be true - self.manipulator_visualizer_process = Process(target=manipulatorComparisonVisualizer, - args=(args, self.robot1.model, self.robot1.collision_model, - self.robot2.visual_model, self.manipulator_visualizer_cmd_queue, - self.manipulator_visualizer_ack_queue)) - if self.args.debug_prints: - print("MANIPULATOR_COMPARISON_VISUALIZER: manipulator_comparison_visualizer_process started") - self.manipulator_visualizer_process.start() - # wait for meshcat to start - self.manipulator_visualizer_cmd_queue.put((np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq))) - if self.args.debug_prints: - print("COMPARE_LOGS_MAIN: i managed to put initializing (q1, q2) to manipulator_comparison_visualizer_queue") + side_function = partial(manipulatorComparisonVisualizer, self.robot1.model, + self.robot1.visual_model, self.robot1.collision_model) + cmd = (np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq)) + self.visualizer_manager = ProcessManager(args, side_function, + cmd, 2) # wait until it's ready (otherwise you miss half the sim potentially) # 5 seconds should be more than enough, # and i don't want to complicate this code by complicating IPC time.sleep(5) - self.manipulator_visualizer_ack_queue.get() + self.visualizer_manager.getData() ########################################### # in case you will want log plotters too # @@ -89,24 +80,20 @@ class ManipulatorComparisonManager: def __init__(self, args, log, log_plotter_time_start, log_plotter_time_stop): self.time_start = log_plotter_time_start self.time_stop = log_plotter_time_stop - self.cmd_queue = Queue() - self.ack_queue = Queue() - self.process = Process(target=logPlotter, - args=(args, log, self.cmd_queue, - self.ack_queue)) - self.process.start() - self.ack_queue.get() + side_function = partial(logPlotter, log) + self.plotter_manager = ProcessManager(args, side_function, None, 2) + self.plotter_manager.getData() def createRunningPlot(self, log, log_plotter_time_start, log_plotter_time_stop): self.log_plotters.append(self.RunningPlotter(self.args, log, log_plotter_time_start, log_plotter_time_stop)) def updateViz(self, q1, q2, time_index): - self.manipulator_visualizer_cmd_queue.put((q1, q2)) + self.visualizer_manager.sendCommand((q1, q2)) for log_plotter in self.log_plotters: if (time_index >= log_plotter.time_start) and (time_index < log_plotter.time_stop): - log_plotter.cmd_queue.put(time_index - log_plotter.time_start) - log_plotter.ack_queue.get() - self.manipulator_visualizer_ack_queue.get() + log_plotter.plotter_manager.sendCommand(time_index - log_plotter.time_start) + log_plotter.plotter_manager.getData() + self.visualizer_manager.getData() # NOTE: this uses slightly fancy python to make it bareable to code # NOTE: dict keys are GUARANTEED to be in insert order from python 3.7 onward @@ -142,6 +129,7 @@ class ManipulatorComparisonManager: +# TODO: update if __name__ == "__main__": args = getLogComparisonArgs() cmp_manager = ManipulatorComparisonManager(args) diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py index f39cf81..fc25e69 100644 --- a/python/ur_simple_control/visualize/visualize.py +++ b/python/ur_simple_control/visualize/visualize.py @@ -6,6 +6,7 @@ import copy #from pinocchio.visualize import MeshcatVisualizer # TODO: use wrapped meshcat visualizer to have access to more nifty plotting from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer +from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat import meshcat_shapes # tkinter stuff for later reference @@ -200,11 +201,13 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue # could be merged with the above function. # but they're different enough in usage to warrent a different function, # instead of polluting the above one with ifs -def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, cmd_queue, ack_queue): +def manipulatorComparisonVisualizer(model, collision_model, visual_model, args, cmd, cmd_queue, ack_queue): for geom in visual_model.geometryObjects: if "hand" in geom.name: geom.meshColor = np.array([0.2,0.2,0.2,0.2]) - viz = MeshcatVisualizer(model, collision_model, visual_model) + viz = MeshcatVisualizer(model=model, + collision_model=collision_model, + visual_model=visual_model) viz.initViewer(open=True) # load the first one viz.loadViewerModel(collision_color=[0.2,0.2,0.2,0.6]) @@ -245,7 +248,9 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, # other robot display - viz2 = MeshcatVisualizer(model, collision_model, visual_model) + viz2 = UnWrappedMeshcat(model=model, + collision_model=collision_model, + visual_model=visual_model) viz2.initViewer(viz.viewer) # i don't know if rootNodeName does anything apart from being different #viz2.loadViewerModel(rootNodeName="pinocchio2", visual_color=(1.0,1.0,1.0,0.1)) @@ -293,7 +298,7 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, # so no cigar from that. # NOTE this is being tried out in realtimelogplotter branch # there is no successful re-drawing here. -def logPlotter(args, log, cmd_queue, ack_queue): +def logPlotter(log, args, cmd, cmd_queue, ack_queue): """ logPlotter --------------- -- GitLab