From d6ddbfc8524518e732a44bbf4d6da0f66a528b7e Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Tue, 13 May 2025 13:48:46 +0200 Subject: [PATCH] AS: updated readme --- .DS_Store | Bin 8196 -> 6148 bytes README.md | 181 +++++++++++++----- .../__pycache__/osc_ros2.cpython-310.pyc | Bin 25324 -> 25044 bytes workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 45 ++--- workspace/src/osc_ros2/setup.py | 11 +- 5 files changed, 157 insertions(+), 80 deletions(-) diff --git a/.DS_Store b/.DS_Store index 3a66700b0026deb5c56a22da5c05821585e60ae2..9fd8c92fd825cd302ac2fe9f1436871dbe9c658f 100644 GIT binary patch delta 140 zcmZp1XfcprU|?W$DortDU=RQ@Ie-{MGjUEV6q~50D9R3!2aEYKxH9N5on z&dA6D)Bpqm+(5z=WYEUK@640=WgI~^fsA4S Q(M%Alz+#)@dFC(!0E)C0!vFvP literal 8196 zcmeHM-D(p-6h4!t?Z%=MwGh2p@HW&WHvK__G^Q7VB6O=>sKjh`+lKD$g#0u}DCAmw z3@?RxsV^Y-0)mg?h173mwqbXh)C)_kI1^^hWafNlX1;T>yJtg0tkQD|L|Gy-P=%bI zLo=p`c~MW4R5-E#$$%$Xr^*A5H*AmFC$#l|W+ z`?z1XpElioZT|Xw9(3!?z->!F({Z8l;#o6rc)!AXfzy##OHCLVBU78dF&Gq=3OTc| zT3kKInS-@rK4&i7Sw1+(7z?*<-`%Y3d49m(sx!0jxPhV%>Y6l(d(nUB;oOvuy+k>*0Fn*MN5c9@FA~TBKzSKG zV|^t({vpDlAu**%zNHG*J5>5dN9a$}6ZpzPbodF@Z(`PtSk6!%QPZY9ct}K5Bk4~W z&C$J#LHuoxt(2B?W~sFNhaMYqvV`wIo-Ue!vteLLnaPU$e`E6d|Fc<4ThRSE!;0y`hp4f<~s}kTM;I9Q|R4z6n) +``` + +On another terminal run + +``` +docker run -it --rm --network ros_ursim ros2_humble:latest +``` +Also note the IP address, using `hostname` + +``` +hostname -i +``` + +With both containers running we can now open the URSim interface on any browser by going to this address: -Open the simulator in your browser: ``` http://localhost:6080/vnc.html ``` ---- +It should now open the `VNC` interface with a big `connect` button. Press the connect button and you should be able to see the UR10e teachpendant interface. Add a payload of 0.5 kg and start the robot. Now we must enable ROS2 to control this. Go to `New...`>`Program` to create a new program for the robot. Within this program, click on `URCaps`>`External Control` to allow control from outside the simulation. Under `Installation`>`URCaps`>`External Control`, input the IP address of the ROS2 container and give it a name (ROS perhaps). +Save this program so you don't have to repeat these steps, but don't run it yet as we have not initiated ROS2 communication. -## Connecting to Simulation - -Launch the robot driver (replace `x` with correct IP): -```bash -ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=172.18.0.x +In the ROS2 container, source the ROS2 environment. +``` +source /opt/ros/humble/setup.bash ``` -Use `hostname -i` inside each container to retrieve IPs. - -On the teach pendant or URSim, set the ROS 2 container’s IP in the External Control URCap. - ---- - -## Check Connection - -You can test if data is flowing correctly with: -```bash -ros2 topic echo /joint_states +Next, launch the driver for communication with the UR simulation, with the IP address of the UR container +``` +ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:= ``` +Now, from the VNC interface, we can start the external control program by clicking on the play button in the bottom right of the screen. + +To run other ROS2 programs to send commands to the UR10e, we must use a third terminal and access the ROS2 container again. First we must find the container ID of our ROS2 container. + +``` +docker ps +``` + +Use the appropriate container ID to launch bash within that container. We need the container ID of the ros2_humble container. + +``` +docker exec -it /bin/bash +``` + +Once inside, we again must source the ROS2 environment +``` +source /opt/ros/humble/setup.bash +``` + +Finally, we can run ROS2 scripts to control the robot. For example +``` +ros2 launch ur_robot_driver test_scaled_joint_trajectory_controller.launch.py +``` + +If everything worked, this should either move the robot or complain about it being in a strange position. --- ## Installation Instructions for the Interface @@ -97,7 +136,19 @@ ros2 topic echo /joint_states 2. **Install Required Python Libraries** ```bash - pip install numpy==1.23.5 scipy==1.10.1 matplotlib==3.6.3 spatialmath-python==1.0.0 roboticstoolbox-python==1.0.1 + pip install numpy==1.22.4 scipy==1.7.3 spatialmath-python==1.1.14 roboticstoolbox-python==1.1.1 osc4py3 + ``` + if an error occurs during installation run: + ```bash + python3 -m pip install --upgrade pip setuptools wheel + ``` + and try again. + + If you don't have pip installed run + + ```bash + sudo apt update + sudo apt install python3-pip ``` 3. **Clone and Build the Workspace** @@ -108,6 +159,16 @@ ros2 topic echo /joint_states source install/setup.bash ``` +Now you should find three new folders in your directory: log, build and install. You can check with `ls` +```bash +workspace/ +├── src +├── build +├── install +└── log +``` + + 4. **Run the Interface** ```bash ros2 run osc_ros2 interface @@ -119,12 +180,45 @@ ros2 topic echo /joint_states The script will guide you through the setup: -- **URDF File**: Choose whether to load a URDF to enable kinematics, joint limits, etc. +- **URDF File**: Choose whether to load a URDF to enable kinematics. +- **Cost Mask**: Let the interface know which coordinates you want to control. +- **Topic Name**: On which topic is your robot listening for JointTrajectories? - **Log/State IP & Ports**: Enter where logs and joint state OSC messages should be sent. - **Command Port**: Set the port to listen for incoming OSC commands. - **Limits**: Define workspace limits (x/y/z) and joint limits if desired. - **Refresh Rate**: Choose how often the interface updates (Hz). +### URDF File + +You will be asked whether you want to use an URDF. Most commertial robots come with an URDF but if you don't have a URDF you can enter `n`. If you have a URDF enter the path to it. This way he interface can find and use it. If you are using the simulation of the ur10e you can use the URDF file for the ur10e in this repository. + +The robot strucute will now be printent out in the terminl. You can check if this is the correct robot. +Some of the following options will only be asked if you did enter a URDF. You can skip them if you did't. + +### Cost Mask (only with URDF) + +Here you can bestimmen which coordinates of your commands will be used to move the robot. Since a robot with two joints can only follow two coordinates you can use a maximum of two coordinates. +For each coordinate you wan tto use enter a `1`, for the coordinates you dont want to use enter a `0`. + +## Topic Name for JointTrajectories + +Enter to which topic name to which the interface should send the trajectories. On this topic your robot sould be listening for JointTrajectory messages. Otherwise the trajectories will go nowhere. The default is `/scaled_joint_trajectory_controller/joint_trajectory` which works for the ur10e. + +## Log/State IP & Ports + +Enter the IP addresses and port on which you want to recieve the logs and joint states as OSC messages. The default IP adress it `127.0.0.1` which is the localhost, so the system you're running this interface on. + +## Command Port + +Next enter the port on which the interface should listen for your OSC messages. + +## Limits (x,y,z only with URDF) + +The interace asks for workspace and end-effector limits only if a URDF was provided. For each coordinate you can set lower and upper limits. If you don't want to set a limit eter a `x`. The robot will not be constrained in this coordinates direction. A coommon limit is to set the lower z limit to 0 such that the manipulator does not move into the floor. + +Now you can also add joint limits. If you have a URDF provided the current joint limits will be shown. You can enter new joint limits if you want. For the ur10e at the Ligeti Center it is helpfull to set the joint limits of + + --- ## Supported OSC Commands @@ -149,5 +243,4 @@ An example patch in puredata is provided that allows the user to control a 6-DOF [^1]: [What is Docker?](https://docs.docker.com/get-started/) [^2]: [WSL2 & Docker Networking](https://stackoverflow.com/questions/65426891) [^3]: [USB Device in Docker](https://stackoverflow.com/questions/46467295) -[^4]: [Cisco VPN and WSL2](https://github.com/Microsoft/WSL/issues/4277) -[^5]: macOS is not tested—use Linux or Windows for reliability. +[^4]: [Cisco VPN and WSL2](https://github.com/Microsoft/WSL/issues/4277) \ No newline at end of file diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc index 84b75c0f7a5b1c3a788a49d67bea6025708fcc76..ee95261463ba83cfd132ae875c376744a254222c 100644 GIT binary patch delta 5033 zcmaJFZE#f8_1*X0?!NsDNl3_dHrafwB!mPcK*Ef^$y;*wK}hDU z+Zr69iUu#OwgOFHs#2XQ-D*ehkF_7Rqt?<&JF#`FQd_N}t^R07Dm~}!M-pY4ojrTL z&$;KGckX@p;OFG`FA`7K?RH4;7p(H_-Ff!7r-)oRKKESXZ4$Lp$6<*&g7T=cqZnu> zbph=XbP07+&tYjtY0yhE`ee30SQ__`&j`up4&@C}%->axldSk5bv+@I@fR&N;9x@r`eHjvr2NxEB=jN;*Eh=d=o zFGTz8(%@Y~(w96I36Gko36C$)IZ+KP@yyW1RtSr@LMv$HVF^l71v;e@(yh`cF{Lgqtxoc5VlxC~)SndGG=pDm_)KY^ zca+e%eaaY#bPF2Nnn6P%37Ul0oSE6|qKQC+LJ3S|#X&RT%|-+Nyk-)eB%%j?!fC1f8$~ek@_eUVx0}O=S#Jz1UqV zosUU9BAt?xbOWX_jc&vMDVoYrm+6A?D^T8&r;di787L&8Da=Z$9X%=`}!Vfy~vn#QlW=6jB zh(ucw8vKl-HTJlf1zA0ARGXQj1R(f>JwT3?u1XeD9^7PR!lR)8|9nDICsc}NnpsKi z^9f7A%>rEsbQNYZGtF74xP1~>Ul%L7#ms~(R-=r!II}ac>$A=5WY=Fcs`+kb{+fS7 z25T^bX*H1tU(vNh1}V=i5@eBxx7o}BF7yemvrmN#*76HZ-wgi>YXbjQo%!2Wf&aB% z@UIN!3ja`rlqc{%3+vT|18N;$S;;CkC#slQ-?X~!c9pFu9#e)7r=R}y7-zerlOd?W z;O+2j)m^Zc*b@pHo3X+seHf#RsgYUy7p?_`P@UwSU)0;p**GJFoyr`1w!5he={tm< zE4stX0e>HuWW2`AnZPbm1~2h`cWEr;MP;gpwgoc+>5H(c_9T81Itsc8ded**G>Rj2 zuBpLL+&ZcV+-lnBHi6gDubB?IUEp0+=A0)F@<47q|5uLAf0{EtK9;*y(d^!u;dY!A z&Ec!+BF-_Cf=g+{D|nad?piMvnvH@Y1nDS~_Mm=XAjkqnkhP;Eh+r#%9SAleco2Z! znp`YDGq3iB)fwRL%p2f4YFsfV=x4z9Huy$7&?L|{68n7Q&j!J#j1sum6j7Nfc)BA8 zPxcOLRE{c9HEM}kqc)hanqHICSNkk%A+^lZ-({PwAJyRE0@DhQtk}3ptqD`;YuhOW zyL}rRIc=DTg=&f8l(iYw6m80*aukjxkR}gX)@|5QbXuyVZ3l&|piKpA70}g^wlTuK zE{p{uEw-nP9}vcZHdQcI!PuS-d|KEFIw^%3p>3ZMdD>I}$J+wpD&P*p*=vcF!@`}W z9D`N6Oa-9ZQ~`Pt=7KAXg+!PFmT}6B^`Wy&8Jw%8#e`vJYP}lF7YEG*=uFfOE_SZr z{}L(cAkql%#qGdNv!-UvHW>7qc;+ z&UU6c`@V(#iW@!mgJ3xXxkT`hu*x!jV__@t#H(+1sm{mJIkt!;Y+9bf_bi!LcMhXX z&5s`-`9zv0X2en;&k6JR3rn&Id5*unw3$4`Gn=cd7m)unU)J21WL!)zPBt$PjJf1# zeyw?k?Bqs^pPb~UTRMyPg1<>?aL=AFGuU@AmUw!_v;H8@TlQ@J)Z#=cB8$zSIL`mJ z>@lktR1a{yat?oDd2Yt9A>t=0NM#tZBh|3FJpT9P?~x9EX~jkIYW)6{zq66kynF4- z~?jsJ1Jir%KxV8^C8&Tqi*Vy}Tyjyql^c{>4}lv!}n))OAsALi#EOwuhgbn(Kjt59sU^?zg7*33x4gC8w;_sb0|7&ThZKt- z7(jr}1G6B&=3!!yZzJ}Fm163wj$de>>nj5^QVT$n;2f8wm=x3UHCgj$3VbxJ#N+4M z)`|?bP?01B^`De^^Hv{um3M9ZboNon$T@;4eS|$?7w2~ zmHmTyq82?9sIn$0>N1MPi)lG(PT8P7yJE)RCu%XBcsnKV@(qSo_^Q-%RMIUF%7>u_ zEk>yrttzZRjXFmfM{MBAn+TtLZz_ZLX4n<{fL#UJ!Ob~i2oA55f3PXXIED{zsnpXZ zE&6}L8=DPD{>u0#+kZ$%JwLOvl3eC50c_$|cNRK-0)=NQ0gR0E*&Qp0gKz1m&siN{ zMlccz=-!@in9-09_XRKiXJY|>q9f)`Tt~phPgn;ZtSz#Bh|j~re0%3@spt3d!Rwnf7lQ>jPMQ<@dD_D zQ>D|@AxBA6J|dX}4|TX#$fkT;9+cZ8_E%666WFh?jf;_3f&l(80aKT4NlBS_%5WBc zib*#E0(YH-C~iP-7QqJy-ba8#ju-9PSLm;pw6LE3{lk-LNDmoo9ZJ_D*u_8G)xM%0 z;!LW0LOM-cFqfg|j|jwp)rpuG()fR-Nqb~}Nbd^kJwYaJX>o3LH5F))y$1{$9?~b> z;Yd#x+|l4PgVOum{G+bMwM`I$9YaTpF_`CIus_@rGKRbQLjxgWQq{v$%!&g#SSAjm zC1`>xo>T_{A^m5t=hk zSyz|8j4kDTfg?ocp9bnXSEA4YH_83O>=Jr+M)n7cP@sPxVC-e@A}51|yTeAPCt`%d y{oUb1Y&E80$L!+>T9B+pa1cqX3M)sjmEYYxmn`DvyM6p}cZRJ->u?bD`u_l_-t3A1 delta 5203 zcmb6dYj9NM^?vu>eZTS`fjmjF3G7Np2=6yULEaPy4+(^2$+y6go{7qI%`|osdj9k;t%Ugi;tf3-Q6XKPN&(~v*&%z zcfNDa_uaht61n_K;*EJcP6_^urs}@ot;Z1O1i7stCmUBL7hh=>Ws)k zN_#cXF6su_E$H#oL%m0&_L@j7^>xWikJO~CR z8%gew3@KFMkOmucD=ir!MA9pDnU)TbA(F5fR;q%ZlXPst3i5G6F4HoQ*$kVIJxa?H zHci#53>$K{NSa5L^y-0UiG&}psz9r)Qr{i@(hYHZl_HScunT9u#^8CIC#kYJg0LnNh) zNU8B7Qo?1psJ}}dmRUKZ`97_u4M$LC0_fC?NK2$40;Q^@6SMq4j-M2QIvUIhZpsM+ z->=sj(mn`JCv_>qBpwzt1ip^;=V-PUD%Oa}ZjpvbpC4`Mnw=!#nM@`xzi=ED4 zb$Om&!^c3x0lQMsL$ha(%GGOlMXm~q=?!A1i?G3$K$woYgwFxLgKk)L$-W7xiF~iK zws-;--0+o2rzJYyl<@nVs}?el~hyVmXP;TT{X)C4={UP_QF$#hAkx^(A z}JwSlY@Hp3{& zc`1`1p28-JQrv13!77%444TI9@y}eo!qGV>tWpo~N_W}T2@r4j4e_piO}qlENeJ~< z09KT()I76Nxiycf-Cge5rPagAKs3MED@Sd%+_x60wo-G$KHL_+)yU;iOvy z-bC9W-)4Qvz81RePSk9o+krE6G-ns>0M;~MO-J@vDX+Q%_%lYU(hzWFUdP!9oLQqd zxnm#zk7*d28&aOlPk5uYIjOn&oYXw7`hwHu!&F^B_ZUumys;RDlrnjph8?zx8V-=C zqa}Q&ucTrjbwIq?6`;Rw(TFVU7Sg>yElv;nzH1?O^YW6{_|eiSyu4H+C-|k3=Jd0r z%N46FFwm?8)AsV+7SfuIjh{#y_Rd&OPgtYz^gT5mQsDbm+byjdmW4JfZC$c)#hRt* zk=i?mwE>RXxXc^9#%J1by-dbNSTV)=uFDF*Mv%Z3#toQleRs*u) zBr>rR^;LNqKUcq+jN@{{Ns>&TYB)zqD0pNu5o|^9Z3G6V6aC~6?<*}P2l)LHW1eAT zj{|U4W+nW1U>wnTOJGyM50IZ1vXf}y>jUOm0)l%184U8)KpB5M@DG>p&Cx>Wk^SjR zutg#F@h4j*xI$=f3JuzLYwcKmbZQ+xFnK+c@HdkmGP5j3(@F%UCw{1D4B5@sPcQbI z0B+FAN>F$YKiAZl&0gc5HuWTBVCVkafcW6M48HLINMN^M&@tEGhk%ikY>+)fR}44? z#!9&3x{5d@aJ2|fnJRG33Mrnk+pP&Dp(ZQ|Yr>YWQweSYfvmpTWnqh{1@)8H40hN? z8zdZX6@ZzMvOoh`sns-vhO?bfF!UA}>TQ^yjoQsql!bJAjy9AbIiUzDd+aey$A+|t ziI$T(X)&n|cE^Gyzl|qfs zwo8dWpERI^sEUMP0o;i=`$K-^E<=G;yA2hf$FKnOn&yHlY=uNv0_Hp8!TQhwLjm6w z1Lh6RpFL2Ukee0)^q94StCe?n5gBk2xHA!H5JaNbK3ZhBa!>mfI9*}yT(`Th+$Po! zr)@4hex#+*>{9FPqD%Q7TXtm`hfPNJ)Sr>N)0NXKWNrc51g*ukA=ryRbgX#Ht~~%@ z5M-2|@Ie4kjqO6cPVD5dqLbaClQVV}2~$1V6M>%Yn;tZKnokL+-hK!h-wsxGMJA>E z!t_<7AnlytR;_2vgevE3oBR{>iVv*`eu~%=d9n<#xd47FEYc6pDJJAO{_Na&uAk?X z#_?B6Dy`3>Mw-{mn|g!j1yhuqH|++|1yl6iyneEsZe9!CSm{Rhcz{=m-4Rs+#gJqUJUMmQljmKyC_MT(#ILHkxwZ>Nyk7qh=c_Sr3G} z_ebLFA=6@4+#IoDxhHsRb%4Cj?_J$Q9_PPa-Ez|*v=ghogczvdEbRf`2{$GV1KoUu1RFc^TLTg=(o<{R{0T3g}zPF@dAjX-hw zv$Y1Xod&yG*@HZ}?%#4SrnDQO>Pm+fA= z0w24*+B=C~+Eni=0u?-RZbAL06kfgAPhR3TZ~kOV!ljoOF1VGb`^Iz;c3LkrTzuu0 zbxCtLi92VjA<+sre7@Aj4M_>N?x64*3dfR^Dl^<-3RWdNhI?Om{9}0EsP0nPd(@k- zXoWzHpCu(cdPS;|)`+7lZ-e^m9fUB*-KnY(xWhSM>L#r4RcX?jaA_6@Rg0neEPAyV zvMS`NCftMZK^uf0Z-&plv(&&FY;E=$>%e!_+&c>lVt>M`*TN;Xp4QQZJC$P#5G>lEGl8kR^M_5{2?r0R`AREHOv6%YVkp;Za* zn3|&nc=3*Ykl|-5e7{ECGg_WoKP+hi)FB=&!J`O1 zLU0*@7}+Eo3}h^v7n{} zWW=n6b{jD#Rl}hMLvOz(e1YQ-rpBH`yNzgv z6A`cZ=BO9*UW|IRCmhv&1qF`347zq%CIxV3$9ub*I05o!LJKWkH!%2rLbKN$Qk4SW zdqK-7e&sK#zmN}Ae{Q$gfmT2LW~rU9FlJ&8g)n9)6s%*5c>B(yB*y=`vw8ed6k4?X zJ-q|$EnJl=4rPvpyL+J6>_grXE@@g0I?REKdKy71l0gIqk!(W@|24>PM6xiCg(s2O Z{B+nq&Q1#C0w0b_^J5R%DWv=5e*x?5Fa7`k diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index df9a251..f5f8245 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -14,7 +14,6 @@ import time import os import re import socket -import csv class JointNameListener(Node): def __init__(self): @@ -92,9 +91,6 @@ class OSC_ROS2_interface(Node): self.new = False - self.latency = [] - - while True: try: print('+-' * 50) @@ -141,6 +137,7 @@ class OSC_ROS2_interface(Node): continue if robot: while True: + print('+-' * 50) set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower() if set_limits == 'y': while True: @@ -200,11 +197,12 @@ class OSC_ROS2_interface(Node): if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit): print("Invalid input. Lower limit must be less than upper limit.") continue - - if lower_limit: + lower_limit = float(lower_limit) if lower_limit!="" else None + upper_limit = float(upper_limit) if upper_limit!="" else None + if lower_limit!=None: if lower_limitlim[1][i]: while True: - sure = input(f"Are you sure you want to set the upper limit to {upper_limit} rad which is more than the default limit {lim[1][i]}(y/n): ").strip().lower() + sure = input(f"Are you sure you want to set the upper limit to {upper_limit} rad which is more than the default limit {lim[1][i]} (y/n)?: ").strip().lower() if sure == 'y': lim[1][i] = float(upper_limit) break @@ -224,7 +222,7 @@ class OSC_ROS2_interface(Node): print("Upper limit not changed.") break print("Invalid input. Please enter 'y' or 'n'.") - else: lim[0][i] = float(lower_limit) + else: lim[1][i] = float(upper_limit) self.robot.qlim = lim print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad") print("-" * 50) @@ -653,28 +651,26 @@ class OSC_ROS2_interface(Node): try: if self.desired is None or not(self.new): return - start_time = time.time() if self.desired[0] == "joint_positions": self.new = False self.send_joint_positions() - #return + return elif self.desired[0] == "tcp_coordinates": self.new = False self.send_tcp_coordinates() - #return + return elif self.desired[0] == "joint_trajectory": self.new = False self.send_joint_trajectory() - #return + return elif self.desired[0] == "cartesian_trajectory": self.new = False self.send_cartesian_trajectory() - #return + return else: self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.") return - self.latency.append(time.time()-start_time) except Exception as e: @@ -748,7 +744,7 @@ def main(): while True: try: print('-+'*50) - print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].") + print("The cost mask determines which coordinates are used for the IK.\nEach element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].") print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.") cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")] if sum(cost_mask) <= robot.n and len(cost_mask) == 6: @@ -757,7 +753,7 @@ def main(): print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.") except ValueError: print("Invalid input. Please enter integers only.") - print(f"Cost mask: {cost_mask}") + print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if cost_mask[i]==1]}") break elif use_urdf == 'n': node = JointNameListener() @@ -797,17 +793,6 @@ def main(): except KeyboardInterrupt: print("") finally: - file_path = "./latency_log.csv" - - # If file doesn't exist, create with header - if not os.path.exists(file_path): - with open(file_path, mode='w', newline='') as f: - writer = csv.writer(f) - writer.writerow('latency') - with open(file_path, mode='a', newline='') as f: - writer = csv.writer(f) - for i in node.latency: - writer.writerow([i]) node.destroy_node() rclpy.shutdown() osc_terminate() diff --git a/workspace/src/osc_ros2/setup.py b/workspace/src/osc_ros2/setup.py index 5b7fc71..1e38a71 100644 --- a/workspace/src/osc_ros2/setup.py +++ b/workspace/src/osc_ros2/setup.py @@ -14,11 +14,11 @@ setup( install_requires=[ 'setuptools', 'osc4py3', - 'roboticstoolbox-python==1.0.1', - 'numpy==1.23.5', - 'scipy==1.10.1', - 'spatialmath-python==1.0.0', - 'matplotlib==3.6.3',], + 'roboticstoolbox-python==1.1.1', + 'numpy==1.22.4', + 'scipy==1.7.3', + 'spatialmath-python==1.1.14', + 'matplotlib==3.4.3',], zip_safe=True, maintainer='Alexander Schaefer', maintainer_email='a.schaefer@tuhh.de', @@ -28,7 +28,6 @@ setup( entry_points={ 'console_scripts': [ 'interface = osc_ros2.osc_ros2:main', - 'interface1 = osc_ros2.osc_ros2_rpy:main', ], }, )