├── .gitignore ├── Docker ├── Dockerfile └── entrypoint.sh ├── LICENSE ├── README.md ├── images ├── jparse_concept_fig.png └── splash.png ├── jparse_collab_example.ipynb └── manipulator_control ├── CMakeLists.txt ├── __init__.py ├── config └── kinova_arm_controllers_gazebo.yaml ├── launch ├── franka_launch.launch ├── full_pose_trajectory.launch ├── jparse_april_tag_ros.launch ├── jparse_cpp.launch ├── kinova_gen3.launch ├── kinova_gen3_real.launch ├── kinova_gen3_real_visual_servoing.launch ├── kinova_vel_control.launch ├── line_extended_singular_traj.launch ├── manip_velocity_control.launch ├── panda_main_torque.launch ├── position_trajectory.launch ├── se3_type_2_singular_traj.launch ├── xarm_launch.launch ├── xarm_main_vel.launch ├── xarm_python_using_cpp.launch ├── xarm_real_launch.launch ├── xarm_real_spacemouse_base.launch └── xarm_spacemouse_teleop.launch ├── msg ├── JparseTerms.msg └── Matrow.msg ├── package.xml ├── rviz ├── jparse_final.rviz ├── kinova_gen3.rviz ├── panda_rviz.rviz └── xarm_rviz.rviz ├── scripts ├── device.py ├── extract_lfd_images_from_bag.py ├── input2action.py ├── main_experiment_kinova.py ├── panda_torque_main_experiment.py ├── position_trajectory_generator.py ├── se3_trajectory_generator.py ├── spacemouse.py ├── xarm_spacemouse.py ├── xarm_vel_main_experiment.py └── xarm_vel_python_using_cpp.py ├── setup.py ├── src ├── jparse.cpp └── manipulator_control │ └── jparse_cls.py └── srv └── JParseSrv.srv /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore all bagfiles 2 | *.bag 3 | *.DS_Store 4 | *.pyc 5 | *.html 6 | *.pdf 7 | *.mp4 8 | *.avi -------------------------------------------------------------------------------- /Docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # Official Dockerfile for J-PARSE 2 | 3 | FROM tiryoh/ros-desktop-vnc:noetic 4 | 5 | RUN set -x \ 6 | && apt-get update \ 7 | && apt-get install -y ros-noetic-moveit \ 8 | && apt-get install -y ros-noetic-moveit-servo \ 9 | && apt-get install -y ros-noetic-moveit-visual-tools \ 10 | && apt-get install -y ros-noetic-moveit-ros-visualization \ 11 | && apt-get install -y ros-noetic-graph-msgs \ 12 | && apt-get install -y ros-noetic-rosparam-shortcuts \ 13 | && apt-get install -y ros-noetic-control-toolbox \ 14 | && apt-get install -y gstreamer1.0-tools gstreamer1.0-libav libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-good1.0-dev gstreamer1.0-plugins-good gstreamer1.0-plugins-base \ 15 | && rm -rf /var/lib/apt/lists/* 16 | 17 | 18 | RUN set -x \ 19 | && pip3 install conan==1.59 \ 20 | && conan config set general.revisions_enabled=1 \ 21 | && conan profile new default --detect > /dev/null \ 22 | && conan profile update settings.compiler.libcxx=libstdc++11 default 23 | 24 | 25 | RUN rosdep update \ 26 | && echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc 27 | 28 | RUN apt-get update && apt-get install -y ros-noetic-ros-control ros-noetic-ros-controllers 29 | 30 | 31 | RUN /bin/bash -c 'conan config set general.revisions_enabled=1' 32 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; mkdir -p /ros_kortex_ws/src; cd ros_kortex_ws/src; git clone https://github.com/Kinovarobotics/ros_kortex; git clone https://github.com/Kinovarobotics/ros_kortex_vision; git clone https://github.com/ros-simulation/gazebo_ros_pkgs; cd ..; catkin_make' 33 | RUN echo "source /ros_kortex_ws/devel/setup.bash" >> ~/.bashrc 34 | 35 | 36 | # Install ros real sense 37 | RUN apt-get update && apt-get install -y ros-noetic-realsense2-camera && apt-get install -y ros-noetic-cv-bridge && apt install -y ros-noetic-sensor-msgs 38 | 39 | RUN apt-get install -y ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control 40 | 41 | # install xarm code 42 | RUN cd / && mkdir -p dev_ws/src && cd dev_ws/src 43 | 44 | RUN source /opt/ros/noetic/setup.bash && cd /ros_kortex_ws/src && git clone https://github.com/xArm-Developer/xarm_ros.git --recursive \ 45 | && git clone -b noetic-devel https://github.com/armlabstanford/hrl-kdl.git \ 46 | && git clone https://github.com/catkin/catkin_simple.git \ 47 | && cd xarm_ros && git pull && git submodule sync && git submodule update --init --remote \ 48 | && cd /ros_kortex_ws/src && rosdep update && rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y 49 | 50 | RUN source /opt/ros/noetic/setup.bash && cd /ros_kortex_ws && rm -rf build/ devel/ && catkin build && source devel/setup.bash 51 | # install franka panda gazebo 52 | # deps 53 | RUN apt install -y ros-$ROS_DISTRO-gazebo-ros-control ros-${ROS_DISTRO}-rospy-message-converter ros-${ROS_DISTRO}-effort-controllers ros-${ROS_DISTRO}-joint-state-controller ros-${ROS_DISTRO}-moveit ros-${ROS_DISTRO}-moveit-commander ros-${ROS_DISTRO}-moveit-visual-tools 54 | RUN apt install -y ros-${ROS_DISTRO}-libfranka 55 | 56 | RUN cd / && mkdir -p panda_ws/src && cd panda_ws/src 57 | RUN source /opt/ros/noetic/setup.bash && cd /panda_ws/src && git clone -b noetic-devel https://github.com/justagist/panda_simulator 58 | 59 | RUN cd /panda_ws/src/panda_simulator && ./build_ws.sh 60 | RUN pip install "numpy<1.24" && pip install numpy-quaternion==2020.5.11.13.33.35 && pip install numba 61 | 62 | RUN git clone https://github.com/xArm-Developer/xArm-Python-SDK.git && cd xArm-Python-SDK && python3 setup.py install 63 | 64 | RUN pip install hidapi==0.14.0.post4 65 | RUN apt install -y nano 66 | RUN pip install mpld3 packaging 67 | RUN apt install -y texlive texlive-latex-extra texlive-fonts-recommended dvipng 68 | 69 | RUN apt install ros-noetic-apriltag-ros -y 70 | 71 | COPY ./entrypoint.sh / 72 | ENTRYPOINT [ "/bin/bash", "-c", "/entrypoint.sh" ] 73 | 74 | ENV USER ubuntu 75 | ENV PASSWD ubuntu -------------------------------------------------------------------------------- /Docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Create User 4 | USER=${USER:-root} 5 | HOME=/root 6 | if [ "$USER" != "root" ]; then 7 | echo "* enable custom user: $USER" 8 | useradd --create-home --shell /bin/bash --user-group --groups adm,sudo "$USER" 9 | echo "$USER ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers 10 | if [ -z "$PASSWORD" ]; then 11 | echo " set default password to \"ubuntu\"" 12 | PASSWORD=ubuntu 13 | fi 14 | HOME="/home/$USER" 15 | echo "$USER:$PASSWORD" | /usr/sbin/chpasswd 2> /dev/null || echo "" 16 | cp -r /root/{.config,.gtkrc-2.0,.asoundrc} "$HOME" 2>/dev/null 17 | chown -R "$USER:$USER" "$HOME" 18 | [ -d "/dev/snd" ] && chgrp -R adm /dev/snd 19 | fi 20 | 21 | # VNC password 22 | VNC_PASSWORD=${PASSWORD:-ubuntu} 23 | 24 | mkdir -p "$HOME/.vnc" 25 | echo "$VNC_PASSWORD" | vncpasswd -f > "$HOME/.vnc/passwd" 26 | chmod 600 "$HOME/.vnc/passwd" 27 | chown -R "$USER:$USER" "$HOME" 28 | sed -i "s/password = WebUtil.getConfigVar('password');/password = '$VNC_PASSWORD'/" /usr/lib/novnc/app/ui.js 29 | 30 | # xstartup 31 | XSTARTUP_PATH="$HOME/.vnc/xstartup" 32 | cat << EOF > "$XSTARTUP_PATH" 33 | #!/bin/sh 34 | unset DBUS_SESSION_BUS_ADDRESS 35 | mate-session 36 | EOF 37 | chown "$USER:$USER" "$XSTARTUP_PATH" 38 | chmod 755 "$XSTARTUP_PATH" 39 | 40 | # vncserver launch 41 | VNCRUN_PATH="$HOME/.vnc/vnc_run.sh" 42 | cat << EOF > "$VNCRUN_PATH" 43 | #!/bin/sh 44 | 45 | # Workaround for issue when image is created with "docker commit". 46 | # Thanks to @SaadRana17 47 | # https://github.com/Tiryoh/docker-ros2-desktop-vnc/issues/131#issuecomment-2184156856 48 | 49 | if [ -e /tmp/.X1-lock ]; then 50 | rm -f /tmp/.X1-lock 51 | fi 52 | if [ -e /tmp/.X11-unix/X1 ]; then 53 | rm -f /tmp/.X11-unix/X1 54 | fi 55 | 56 | if [ $(uname -m) = "aarch64" ]; then 57 | LD_PRELOAD=/lib/aarch64-linux-gnu/libgcc_s.so.1 vncserver :1 -fg -geometry 1920x1080 -depth 24 58 | else 59 | vncserver :1 -fg -geometry 1920x1080 -depth 24 60 | fi 61 | EOF 62 | 63 | # Supervisor 64 | CONF_PATH=/etc/supervisor/conf.d/supervisord.conf 65 | cat << EOF > $CONF_PATH 66 | [supervisord] 67 | nodaemon=true 68 | user=root 69 | [program:vnc] 70 | command=gosu '$USER' bash '$VNCRUN_PATH' 71 | [program:novnc] 72 | command=gosu '$USER' bash -c "websockify --web=/usr/lib/novnc 80 localhost:5901" 73 | EOF 74 | 75 | # colcon 76 | BASHRC_PATH="$HOME/.bashrc" 77 | grep -F "source /opt/ros/$ROS_DISTRO/setup.bash" "$BASHRC_PATH" || echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> "$BASHRC_PATH" 78 | grep -F "source /ros_kortex_ws/devel/setup.bash" "$BASHRC_PATH" || echo "source /ros_kortex_ws/devel/setup.bash" >> "$BASHRC_PATH" 79 | 80 | 81 | 82 | grep -F "export ROS_AUTOMATIC_DISCOVERY_RANGE=" "$BASHRC_PATH" || echo "# export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST" >> "$BASHRC_PATH" 83 | chown "$USER:$USER" "$BASHRC_PATH" 84 | 85 | # Fix rosdep permission 86 | mkdir -p "$HOME/.ros" 87 | cp -r /root/.ros/rosdep "$HOME/.ros/rosdep" 88 | chown -R "$USER:$USER" "$HOME/.ros" 89 | 90 | # Add terminator shortcut 91 | mkdir -p "$HOME/Desktop" 92 | cat << EOF > "$HOME/Desktop/terminator.desktop" 93 | #!/usr/bin/env xdg-open 94 | [Desktop Entry] 95 | Name=Terminator 96 | Comment=Multiple terminals in one window 97 | TryExec=terminator 98 | Exec=terminator 99 | Icon=terminator 100 | Type=Application 101 | Categories=GNOME;GTK;Utility;TerminalEmulator;System; 102 | StartupNotify=true 103 | X-Ubuntu-Gettext-Domain=terminator 104 | X-Ayatana-Desktop-Shortcuts=NewWindow; 105 | Keywords=terminal;shell;prompt;command;commandline; 106 | [NewWindow Shortcut Group] 107 | Name=Open a New Window 108 | Exec=terminator 109 | TargetEnvironment=Unity 110 | EOF 111 | cat << EOF > "$HOME/Desktop/firefox.desktop" 112 | #!/usr/bin/env xdg-open 113 | [Desktop Entry] 114 | Version=1.0 115 | Name=Firefox Web Browser 116 | Name[ar]=متصفح الويب فَيَرفُكْس 117 | Name[ast]=Restolador web Firefox 118 | Name[bn]=ফায়ারফক্স ওয়েব ব্রাউজার 119 | Name[ca]=Navegador web Firefox 120 | Name[cs]=Firefox Webový prohlížeč 121 | Name[da]=Firefox - internetbrowser 122 | Name[el]=Περιηγητής Firefox 123 | Name[es]=Navegador web Firefox 124 | Name[et]=Firefoxi veebibrauser 125 | Name[fa]=مرورگر اینترنتی Firefox 126 | Name[fi]=Firefox-selain 127 | Name[fr]=Navigateur Web Firefox 128 | Name[gl]=Navegador web Firefox 129 | Name[he]=דפדפן האינטרנט Firefox 130 | Name[hr]=Firefox web preglednik 131 | Name[hu]=Firefox webböngésző 132 | Name[it]=Firefox Browser Web 133 | Name[ja]=Firefox ウェブ・ブラウザ 134 | Name[ko]=Firefox 웹 브라우저 135 | Name[ku]=Geroka torê Firefox 136 | Name[lt]=Firefox interneto naršyklė 137 | Name[nb]=Firefox Nettleser 138 | Name[nl]=Firefox webbrowser 139 | Name[nn]=Firefox Nettlesar 140 | Name[no]=Firefox Nettleser 141 | Name[pl]=Przeglądarka WWW Firefox 142 | Name[pt]=Firefox Navegador Web 143 | Name[pt_BR]=Navegador Web Firefox 144 | Name[ro]=Firefox – Navigator Internet 145 | Name[ru]=Веб-браузер Firefox 146 | Name[sk]=Firefox - internetový prehliadač 147 | Name[sl]=Firefox spletni brskalnik 148 | Name[sv]=Firefox webbläsare 149 | Name[tr]=Firefox Web Tarayıcısı 150 | Name[ug]=Firefox توركۆرگۈ 151 | Name[uk]=Веб-браузер Firefox 152 | Name[vi]=Trình duyệt web Firefox 153 | Name[zh_CN]=Firefox 网络浏览器 154 | Name[zh_TW]=Firefox 網路瀏覽器 155 | Comment=Browse the World Wide Web 156 | Comment[ar]=تصفح الشبكة العنكبوتية العالمية 157 | Comment[ast]=Restola pela Rede 158 | Comment[bn]=ইন্টারনেট ব্রাউজ করুন 159 | Comment[ca]=Navegueu per la web 160 | Comment[cs]=Prohlížení stránek World Wide Webu 161 | Comment[da]=Surf på internettet 162 | Comment[de]=Im Internet surfen 163 | Comment[el]=Μπορείτε να περιηγηθείτε στο διαδίκτυο (Web) 164 | Comment[es]=Navegue por la web 165 | Comment[et]=Lehitse veebi 166 | Comment[fa]=صفحات شبکه جهانی اینترنت را مرور نمایید 167 | Comment[fi]=Selaa Internetin WWW-sivuja 168 | Comment[fr]=Naviguer sur le Web 169 | Comment[gl]=Navegar pola rede 170 | Comment[he]=גלישה ברחבי האינטרנט 171 | Comment[hr]=Pretražite web 172 | Comment[hu]=A világháló böngészése 173 | Comment[it]=Esplora il web 174 | Comment[ja]=ウェブを閲覧します 175 | Comment[ko]=웹을 돌아 다닙니다 176 | Comment[ku]=Li torê bigere 177 | Comment[lt]=Naršykite internete 178 | Comment[nb]=Surf på nettet 179 | Comment[nl]=Verken het internet 180 | Comment[nn]=Surf på nettet 181 | Comment[no]=Surf på nettet 182 | Comment[pl]=Przeglądanie stron WWW 183 | Comment[pt]=Navegue na Internet 184 | Comment[pt_BR]=Navegue na Internet 185 | Comment[ro]=Navigați pe Internet 186 | Comment[ru]=Доступ в Интернет 187 | Comment[sk]=Prehliadanie internetu 188 | Comment[sl]=Brskajte po spletu 189 | Comment[sv]=Surfa på webben 190 | Comment[tr]=İnternet'te Gezinin 191 | Comment[ug]=دۇنيادىكى توربەتلەرنى كۆرگىلى بولىدۇ 192 | Comment[uk]=Перегляд сторінок Інтернету 193 | Comment[vi]=Để duyệt các trang web 194 | Comment[zh_CN]=浏览互联网 195 | Comment[zh_TW]=瀏覽網際網路 196 | GenericName=Web Browser 197 | GenericName[ar]=متصفح ويب 198 | GenericName[ast]=Restolador Web 199 | GenericName[bn]=ওয়েব ব্রাউজার 200 | GenericName[ca]=Navegador web 201 | GenericName[cs]=Webový prohlížeč 202 | GenericName[da]=Webbrowser 203 | GenericName[el]=Περιηγητής διαδικτύου 204 | GenericName[es]=Navegador web 205 | GenericName[et]=Veebibrauser 206 | GenericName[fa]=مرورگر اینترنتی 207 | GenericName[fi]=WWW-selain 208 | GenericName[fr]=Navigateur Web 209 | GenericName[gl]=Navegador Web 210 | GenericName[he]=דפדפן אינטרנט 211 | GenericName[hr]=Web preglednik 212 | GenericName[hu]=Webböngésző 213 | GenericName[it]=Browser web 214 | GenericName[ja]=ウェブ・ブラウザ 215 | GenericName[ko]=웹 브라우저 216 | GenericName[ku]=Geroka torê 217 | GenericName[lt]=Interneto naršyklė 218 | GenericName[nb]=Nettleser 219 | GenericName[nl]=Webbrowser 220 | GenericName[nn]=Nettlesar 221 | GenericName[no]=Nettleser 222 | GenericName[pl]=Przeglądarka WWW 223 | GenericName[pt]=Navegador Web 224 | GenericName[pt_BR]=Navegador Web 225 | GenericName[ro]=Navigator Internet 226 | GenericName[ru]=Веб-браузер 227 | GenericName[sk]=Internetový prehliadač 228 | GenericName[sl]=Spletni brskalnik 229 | GenericName[sv]=Webbläsare 230 | GenericName[tr]=Web Tarayıcı 231 | GenericName[ug]=توركۆرگۈ 232 | GenericName[uk]=Веб-браузер 233 | GenericName[vi]=Trình duyệt Web 234 | GenericName[zh_CN]=网络浏览器 235 | GenericName[zh_TW]=網路瀏覽器 236 | Keywords=Internet;WWW;Browser;Web;Explorer 237 | Keywords[ar]=انترنت;إنترنت;متصفح;ويب;وب 238 | Keywords[ast]=Internet;WWW;Restolador;Web;Esplorador 239 | Keywords[ca]=Internet;WWW;Navegador;Web;Explorador;Explorer 240 | Keywords[cs]=Internet;WWW;Prohlížeč;Web;Explorer 241 | Keywords[da]=Internet;Internettet;WWW;Browser;Browse;Web;Surf;Nettet 242 | Keywords[de]=Internet;WWW;Browser;Web;Explorer;Webseite;Site;surfen;online;browsen 243 | Keywords[el]=Internet;WWW;Browser;Web;Explorer;Διαδίκτυο;Περιηγητής;Firefox;Φιρεφοχ;Ιντερνετ 244 | Keywords[es]=Explorador;Internet;WWW 245 | Keywords[fi]=Internet;WWW;Browser;Web;Explorer;selain;Internet-selain;internetselain;verkkoselain;netti;surffaa 246 | Keywords[fr]=Internet;WWW;Browser;Web;Explorer;Fureteur;Surfer;Navigateur 247 | Keywords[he]=דפדפן;אינטרנט;רשת;אתרים;אתר;פיירפוקס;מוזילה; 248 | Keywords[hr]=Internet;WWW;preglednik;Web 249 | Keywords[hu]=Internet;WWW;Böngésző;Web;Háló;Net;Explorer 250 | Keywords[it]=Internet;WWW;Browser;Web;Navigatore 251 | Keywords[is]=Internet;WWW;Vafri;Vefur;Netvafri;Flakk 252 | Keywords[ja]=Internet;WWW;Web;インターネット;ブラウザ;ウェブ;エクスプローラ 253 | Keywords[nb]=Internett;WWW;Nettleser;Explorer;Web;Browser;Nettside 254 | Keywords[nl]=Internet;WWW;Browser;Web;Explorer;Verkenner;Website;Surfen;Online 255 | Keywords[pt]=Internet;WWW;Browser;Web;Explorador;Navegador 256 | Keywords[pt_BR]=Internet;WWW;Browser;Web;Explorador;Navegador 257 | Keywords[ru]=Internet;WWW;Browser;Web;Explorer;интернет;браузер;веб;файрфокс;огнелис 258 | Keywords[sk]=Internet;WWW;Prehliadač;Web;Explorer 259 | Keywords[sl]=Internet;WWW;Browser;Web;Explorer;Brskalnik;Splet 260 | Keywords[tr]=İnternet;WWW;Tarayıcı;Web;Gezgin;Web sitesi;Site;sörf;çevrimiçi;tara 261 | Keywords[uk]=Internet;WWW;Browser;Web;Explorer;Інтернет;мережа;переглядач;оглядач;браузер;веб;файрфокс;вогнелис;перегляд 262 | Keywords[vi]=Internet;WWW;Browser;Web;Explorer;Trình duyệt;Trang web 263 | Keywords[zh_CN]=Internet;WWW;Browser;Web;Explorer;网页;浏览;上网;火狐;Firefox;ff;互联网;网站; 264 | Keywords[zh_TW]=Internet;WWW;Browser;Web;Explorer;網際網路;網路;瀏覽器;上網;網頁;火狐 265 | Exec=firefox %u 266 | Terminal=false 267 | X-MultipleArgs=false 268 | Type=Application 269 | Icon=firefox 270 | Categories=GNOME;GTK;Network;WebBrowser; 271 | MimeType=text/html;text/xml;application/xhtml+xml;application/xml;application/rss+xml;application/rdf+xml;image/gif;image/jpeg;image/png;x-scheme-handler/http;x-scheme-handler/https;x-scheme-handler/ftp;x-scheme-handler/chrome;video/webm;application/x-xpinstall; 272 | StartupNotify=true 273 | Actions=new-window;new-private-window; 274 | 275 | [Desktop Action new-window] 276 | Name=Open a New Window 277 | Name[ar]=افتح نافذة جديدة 278 | Name[ast]=Abrir una ventana nueva 279 | Name[bn]=Abrir una ventana nueva 280 | Name[ca]=Obre una finestra nova 281 | Name[cs]=Otevřít nové okno 282 | Name[da]=Åbn et nyt vindue 283 | Name[de]=Ein neues Fenster öffnen 284 | Name[el]=Νέο παράθυρο 285 | Name[es]=Abrir una ventana nueva 286 | Name[fi]=Avaa uusi ikkuna 287 | Name[fr]=Ouvrir une nouvelle fenêtre 288 | Name[gl]=Abrir unha nova xanela 289 | Name[he]=פתיחת חלון חדש 290 | Name[hr]=Otvori novi prozor 291 | Name[hu]=Új ablak nyitása 292 | Name[it]=Apri una nuova finestra 293 | Name[ja]=新しいウィンドウを開く 294 | Name[ko]=새 창 열기 295 | Name[ku]=Paceyeke nû veke 296 | Name[lt]=Atverti naują langą 297 | Name[nb]=Åpne et nytt vindu 298 | Name[nl]=Nieuw venster openen 299 | Name[pt]=Abrir nova janela 300 | Name[pt_BR]=Abrir nova janela 301 | Name[ro]=Deschide o fereastră nouă 302 | Name[ru]=Новое окно 303 | Name[sk]=Otvoriť nové okno 304 | Name[sl]=Odpri novo okno 305 | Name[sv]=Öppna ett nytt fönster 306 | Name[tr]=Yeni pencere aç 307 | Name[ug]=يېڭى كۆزنەك ئېچىش 308 | Name[uk]=Відкрити нове вікно 309 | Name[vi]=Mở cửa sổ mới 310 | Name[zh_CN]=新建窗口 311 | Name[zh_TW]=開啟新視窗 312 | Exec=firefox -new-window 313 | 314 | [Desktop Action new-private-window] 315 | Name=Open a New Private Window 316 | Name[ar]=افتح نافذة جديدة للتصفح الخاص 317 | Name[ca]=Obre una finestra nova en mode d'incògnit 318 | Name[cs]=Otevřít nové anonymní okno 319 | Name[de]=Ein neues privates Fenster öffnen 320 | Name[el]=Νέο ιδιωτικό παράθυρο 321 | Name[es]=Abrir una ventana privada nueva 322 | Name[fi]=Avaa uusi yksityinen ikkuna 323 | Name[fr]=Ouvrir une nouvelle fenêtre de navigation privée 324 | Name[he]=פתיחת חלון גלישה פרטית חדש 325 | Name[hu]=Új privát ablak nyitása 326 | Name[it]=Apri una nuova finestra anonima 327 | Name[nb]=Åpne et nytt privat vindu 328 | Name[ru]=Новое приватное окно 329 | Name[sl]=Odpri novo okno zasebnega brskanja 330 | Name[sv]=Öppna ett nytt privat fönster 331 | Name[tr]=Yeni gizli pencere aç 332 | Name[uk]=Відкрити нове вікно у потайливому режимі 333 | Name[zh_TW]=開啟新隱私瀏覽視窗 334 | Exec=firefox -private-window 335 | EOF 336 | cat << EOF > "$HOME/Desktop/codium.desktop" 337 | #!/usr/bin/env xdg-open 338 | [Desktop Entry] 339 | Name=VSCodium 340 | Comment=Code Editing. Redefined. 341 | GenericName=Text Editor 342 | Exec=/usr/share/codium/codium --unity-launch %F 343 | Icon=vscodium 344 | Type=Application 345 | StartupNotify=false 346 | StartupWMClass=VSCodium 347 | Categories=TextEditor;Development;IDE; 348 | MimeType=text/plain;inode/directory;application/x-codium-workspace; 349 | Actions=new-empty-window; 350 | Keywords=vscode; 351 | 352 | [Desktop Action new-empty-window] 353 | Name=New Empty Window 354 | Exec=/usr/share/codium/codium --new-window %F 355 | Icon=vscodium 356 | EOF 357 | chmod +x "$HOME/Desktop/*.desktop" 358 | chown -R "$USER:$USER" "$HOME/Desktop" 359 | 360 | # clearup 361 | PASSWORD= 362 | VNC_PASSWORD= 363 | 364 | echo "============================================================================================" 365 | echo "NOTE: Before stopping to commit docker container to new docker image, log out first." 366 | echo -e 'See \e]8;;https://github.com/Tiryoh/docker-ros2-desktop-vnc/issue/131\e\\https://github.com/Tiryoh/docker-ros2-desktop-vnc/issue/131\e]8;;\e\\' 367 | echo "============================================================================================" 368 | 369 | exec /bin/tini -- supervisord -n -c /etc/supervisor/supervisord.conf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Assistive Robotics and Manipulation Laboratory 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 🦾 J-PARSE: Jacobian-based Projection Algorithm for Resolving Singularities Effectively in Inverse Kinematic Control of Serial Manipulators 2 | 3 | 4 | ### [Shivani Guptasarma](https://www.linkedin.com/in/shivani-guptasarma/), [Matthew Strong](https://peasant98.github.io/), [Honghao Zhen](https://www.linkedin.com/in/honghao-zhen/), and [Monroe Kennedy III](https://monroekennedy3.com/) 5 | 6 | 7 | _In Submission_ 8 | 9 | JPARSE splash 14 | 15 | 17 | 18 | [![Project](https://img.shields.io/badge/Project_Page-J_PARSE-blue)](https://jparse-manip.github.io) 19 | [![ArXiv](https://img.shields.io/badge/Arxiv-J_PARSE-red)](https://arxiv.org/abs/2505.00306) 20 | 21 | ## Quick Start with Docker 22 | 23 | To build the Docker image for the our environment, we use VNC docker, which allows for a graphical user interface displayable in the browser. 24 | 25 | ### Use the Public Docker Image (Recommended) 26 | 27 | We have created a public Docker image that you can pull! 28 | Steps: 29 | 30 | ```sh 31 | docker pull peasant98/jparse 32 | docker run --privileged -p 6080:80 --shm-size=512m -v :/home/ubuntu/Desktop/jparse_ws/src peasant98/jparse 33 | ``` 34 | 35 | ### Build the Image Yourself 36 | 37 | You can build the docker image yourself! To do so, follow the below steps: 38 | ```sh 39 | cd Docker 40 | docker build -t jparse . 41 | docker run --privileged -p 6080:80 --shm-size=512m -v :/home/ubuntu/Desktop/jparse_ws/src jparse 42 | 43 | ``` 44 | 45 | ### Note 46 | 47 | We are working on a Python package that you can easily import into your project. We envision the below: 48 | 49 | ```py 50 | import jparse 51 | ``` 52 | 53 | Stay tuned! 54 | 55 | ### Dependencies 56 | *Note: these are handled in the Docker image directly, and are already installed!* 57 | 58 | 1. [Catkin Simple](https://github.com/catkin/catkin_simple): https://github.com/catkin/catkin_simple 59 | 2. [HRL KDL](https://github.com/armlabstanford/hrl-kdl): https://github.com/armlabstanford/hrl-kdl 60 | 61 | 62 | ## Running Velocity Control (XArm) Example 63 | 64 | ### Simulation 65 | To run the XArm in simulation, first run 66 | ```bash 67 | roslaunch manipulator_control xarm_launch.launch 68 | ``` 69 | 70 | #### Run Desired Trajectory 71 | Next, run one of the trajectory generation scripts. This can either be the ellipse that has poses within and on the boundary of the reachable space of the arm (to test stability): 72 | ```bash 73 | roslaunch manipulator_control full_pose_trajectory.launch robot:=xarm 74 | ``` 75 | or for passing through the type-2 singularity (passing directly above the base link): 76 | ```bash 77 | roslaunch manipulator_control se3_type_2_singular_traj.launch robot:=xarm 78 | ``` 79 | To have more control over keypoints (stop at major and minor axis of ellipse), run 80 | ```bash 81 | roslaunch manipulator_control se3_type_2_singular_traj.launch robot:=xarm key_points_only_bool:=true frequency:=0.1 use_rotation:=false 82 | ``` 83 | or 84 | ```bash 85 | roslaunch manipulator_control full_pose_trajectory.launch robot:=xarm key_points_only_bool:=true frequency:=0.1 use_rotation:=false 86 | ``` 87 | (here frequency specifies how much time is spent at each keypoint). 88 | or 89 | ```bash 90 | roslaunch manipulator_control line_extended_singular_traj.launch robot:=xarm key_points_only_bool:=true frequency:=0.2 use_rotation:=false 91 | ``` 92 | (line trajectory that goes from over the robot, to out of reach in front of the robot.) 93 | 94 | #### Run Control Method 95 | ```bash 96 | roslaunch manipulator_control xarm_main_vel.launch is_sim:=true show_jparse_ellipses:=true phi_gain_position:=2.0 phi_gain_angular:=2.0 jparse_gamma:=0.2 method:=JParse 97 | ``` 98 | 99 | The arguments are 100 | | Parameter | Attribute Description | 101 | |------------|----------------------| 102 | | `is_sim` | Boolean for sim or real | 103 | | `show_jparse_ellipses` | Boolean for showing position JParse ellipsoids (for that method only) in rviz | 104 | | `phi_gain_position` | Kp gain for JParse singular direction position | 105 | | `phi_gain_angular` | Kp gain for JParse singular direction orientation | 106 | | `jparse_gamma` | JParse threshold value gamma | 107 | | `method` | "JParse", "JacobianPseudoInverse" (basic); "JacobianDampedLeastSquares"; "JacobianProjection"; "JacobianDynamicallyConsistentInverse" | 108 | 109 | 110 | ## Real Robot Velocity Control 111 | ### XArm Velocity Control Example 112 | To run on the physical Xarm, the update is to use 113 | ```bash 114 | roslaunch manipulator_control xarm_main_vel.launch is_sim:=false method:=JParse 115 | ``` 116 | Recommended methods for physical system (to avoid unsafe motion) is: "JParse", "JacobianDampedLeastSquares" 117 | 118 | 119 | ### Kinova Gen 3 Velocity Control Example 120 | Run the Kinova environment 121 | ```bash 122 | roslaunch manipulator_control kinova_gen3.launch 123 | ``` 124 | 125 | #### Select Trajectory 126 | Run desired Line Extended keypoints trajectory: 127 | ```bash 128 | roslaunch manipulator_control line_extended_singular_traj.launch robot:=kinova key_points_only_bool:=true frequency:=0.1 use_rotation:=false 129 | ``` 130 | 131 | Run elliptical keypoints trajectory 132 | ```bash 133 | roslaunch manipulator_control full_pose_trajectory.launch robot:=kinova key_points_only_bool:=true frequency:=0.06 use_rotation:=false 134 | ``` 135 | 136 | #### Select Control 137 | Run the Method: 138 | ```bash 139 | roslaunch manipulator_control kinova_vel_control.launch is_sim:=true show_jparse_ellipses:=true phi_gain_position:=2.0 phi_gain_angular:=2.0 jparse_gamma:=0.2 method:=JParse 140 | ``` 141 | 142 | ## Running JParse with the SpaceMouse controller 143 | 144 | You can also run JParse with a human teleoperator using a SpaceMouse controller. This will allow for a fun sandbox to verify JParse. 145 | 146 | We plan to extend this to a simple learning policy as well. The code for that (collecting data, training a policy, and running inference) will be published soon! 147 | 148 | To run, you can run 149 | 150 | ```sh 151 | # run the real robot 152 | roslaunch manipulator_control xarm_real_launch.launch using_spacemouse:=true 153 | 154 | # run the jparse method with or without jparse control 155 | roslaunch xarm_main_vel.launch use_space_mouse:=true use_space_mouse_jparse:={true|false} 156 | 157 | # run the spacemouse example!! Make sure the use_native_xarm_spacemouse argument is OPPOSITE of use_space_mouse_jparse. 158 | roslaunch xarm_spacemouse_teleop.launch use_native_xarm_spacemouse:={true|false} 159 | ``` 160 | 161 | ## Run C++ JParse publisher and service 162 | This allows for publishing JParse components and visualizing using a C++ script 163 | ```bash 164 | roslaunch manipulator_control jparse_cpp.launch jparse_gamma:=0.2 singular_direction_gain_position:=2.0 singular_direction_gain_angular:=2.0 165 | ``` 166 | 167 | The arguments are 168 | | Parameter | Attribute Description | 169 | |------------|----------------------| 170 | | `namespace` | namespace of the robot (e.g. xarm) | 171 | | `base_link_name` | Baselink frame | 172 | | `end_link_name` | end-effector frame | 173 | | `jparse_gamma` | JParse gamma value (0,1) | 174 | | `singular_direction_gain_position` | gains in singular direction for position | 175 | | `singular_direction_gain_angular` | gains in singular direction for orientation | 176 | | `run_as_service` | (boolean) true/false | 177 | 178 | For running as a service: 179 | ```bash 180 | roslaunch manipulator_control jparse_cpp.launch run_as_service:=true 181 | ``` 182 | Then to run service from a terminal (Xarm example): 183 | 184 | ```bash 185 | rosservice call /jparse_srv "gamma: 0.2 186 | singular_direction_gain_position: 2.0 187 | singular_direction_gain_angular: 2.0 188 | base_link_name: 'link_base' 189 | end_link_name: 'link_eef'" 190 | ``` 191 | To see versatility, simply change the kinematic chain for the JParse solution for that segment. To view options for your kinematic tree: 192 | ```bash 193 | rosrun rqt_tf_tree rqt_tf_tree 194 | ``` 195 | 196 | To test with the robot (using a python node to control the arm, with JParse coming from C++), first run script above, then: 197 | ```bash 198 | roslaunch manipulator_control xarm_python_using_cpp.launch is_sim:=true phi_gain_position:=2.0 phi_gain_angular:=2.0 jparse_gamma:=0.2 use_service_bool:=true 199 | ``` 200 | This has same parameters as the python version, but with the service versus message option. Message is faster/cleaner, but service is very versatile: 201 | | Parameter | Attribute Description | 202 | |------------|----------------------| 203 | | `use_service_bool` | True: use service, False: use message| 204 | | `jparse_gamma` | JParse gain (0,1)| 205 | | `phi_gain_position` | gain on position component| 206 | | `phi_gain_angular` | gain on angular component| 207 | | `is_sim` | use of sim versus real (boolean)| 208 | -------------------------------------------------------------------------------- /images/jparse_concept_fig.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/armlabstanford/jparse/6ae106a90ffbd9ae147a9957ed5bcef110fb2888/images/jparse_concept_fig.png -------------------------------------------------------------------------------- /images/splash.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/armlabstanford/jparse/6ae106a90ffbd9ae147a9957ed5bcef110fb2888/images/splash.png -------------------------------------------------------------------------------- /manipulator_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(manipulator_control) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_simple() 9 | 10 | 11 | ## Add support for C++11, supported in ROS Kinetic and newer 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 13 | 14 | find_package(Eigen3 REQUIRED) 15 | 16 | include_directories(include ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${PCL_LIBRARIES}) 17 | 18 | 19 | cs_add_executable(jparse src/jparse.cpp) 20 | 21 | 22 | cs_install() 23 | 24 | cs_export() 25 | #https://github.com/catkin/catkin_simple -------------------------------------------------------------------------------- /manipulator_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/armlabstanford/jparse/6ae106a90ffbd9ae147a9957ed5bcef110fb2888/manipulator_control/__init__.py -------------------------------------------------------------------------------- /manipulator_control/config/kinova_arm_controllers_gazebo.yaml: -------------------------------------------------------------------------------- 1 | gen3_arm_controller: 2 | follow_joint_trajectory: 3 | type: "robot_controllers/FollowJointTrajectoryController" 4 | joints: 5 | - joint_1 6 | - joint_2 7 | - joint_3 8 | - joint_4 9 | - joint_5 10 | - joint_6 11 | - joint_7 12 | gravity_compensation: 13 | type: "robot_controllers/GravityCompensation" 14 | root: "base_link" 15 | tip: "end_effector_link" 16 | autostart: true 17 | cartesian_twist: 18 | type: "robot_controllers/CartesianTwistController" 19 | root_name: "base_link" 20 | tip_name: "end_effector_link" 21 | velocity: 22 | type: "robot_controllers/VelocityController" 23 | joints: 24 | - joint_1 25 | - joint_2 26 | - joint_3 27 | - joint_4 28 | - joint_5 29 | - joint_6 30 | - joint_7 31 | weightless_torque: 32 | type: "robot_controllers/WeightlessTorqueController" 33 | root: "base_link" 34 | tip: "end_effector_link" 35 | autostart: false 36 | limits: 37 | # The joint limits are 10 degrees less than full motion. The gains ramp up to full motor torque at the limit 38 | - name: shoulder_pan_joint 39 | lo: -1.431 40 | hi: +1.431 41 | gain: 193.26 42 | - name: shoulder_lift_joint 43 | lo: -1.047 44 | hi: +1.343 45 | gain: 754.929 46 | - name: elbow_flex_joint 47 | lo: -2.076 48 | hi: +2.076 49 | gain: 379.18 50 | - name: wrist_flex_joint 51 | lo: -2.007 52 | hi: +2.007 53 | gain: 147.25 54 | torque_control_arm: 55 | type: "robot_controllers/TorqueControllerArm" 56 | root: "base_link" 57 | tip: "gripper_link" 58 | autostart: false 59 | limits: 60 | # The effort change cannot be more than the gain in one timestep 61 | - name: shoulder_pan_joint 62 | effort: 33.82 63 | - name: shoulder_lift_joint 64 | effort: 131.76 65 | - name: upperarm_roll_joint 66 | effort: 76.94 67 | - name: elbow_flex_joint 68 | effort: 66.18 69 | - name: forearm_roll_joint 70 | effort: 29.35 71 | - name: wrist_flex_joint 72 | effort: 25.7 73 | - name: wrist_roll_joint 74 | effort: 7.36 75 | 76 | arm_with_torso_controller: 77 | follow_joint_trajectory: 78 | type: "robot_controllers/FollowJointTrajectoryController" 79 | joints: 80 | - torso_lift_joint 81 | - shoulder_pan_joint 82 | - shoulder_lift_joint 83 | - upperarm_roll_joint 84 | - elbow_flex_joint 85 | - forearm_roll_joint 86 | - wrist_flex_joint 87 | - wrist_roll_joint 88 | 89 | torso_controller: 90 | follow_joint_trajectory: 91 | type: "robot_controllers/FollowJointTrajectoryController" 92 | joints: 93 | - torso_lift_joint 94 | 95 | head_controller: 96 | point_head: 97 | type: "robot_controllers/PointHeadController" 98 | follow_joint_trajectory: 99 | type: "robot_controllers/FollowJointTrajectoryController" 100 | joints: 101 | - head_pan_joint 102 | - head_tilt_joint 103 | 104 | base_controller: 105 | type: "robot_controllers/DiffDriveBaseController" 106 | max_velocity_x: 1.0 107 | max_acceleration_x: 0.75 108 | # hold position 109 | moving_threshold: -0.01 110 | rotating_threshold: -0.01 111 | # autostart to get odom 112 | autostart: true 113 | # use laser to only slowly collide with things 114 | laser_safety_dist: 1.0 115 | 116 | base_torque_controller: 117 | type: "robot_controllers/TorqueControllerBase" 118 | autostart: false 119 | 120 | arm_base_controller: 121 | type: "robot_controllers/TorqueControllerArmBase" 122 | autostart: false 123 | 124 | gripper_controller: 125 | gripper_action: 126 | type: "robot_controllers/ParallelGripperController" 127 | centering: 128 | p: 1000.0 129 | d: 0.0 130 | i: 0.0 131 | i_clamp: 0.0 132 | 133 | bellows_controller: 134 | type: "robot_controllers/ScaledMimicController" 135 | mimic_joint: "torso_lift_joint" 136 | controlled_joint: "bellows_joint" 137 | mimic_scale: 0.5 138 | autostart: true 139 | 140 | robot_driver: 141 | default_controllers: 142 | - "arm_controller/follow_joint_trajectory" 143 | - "arm_controller/gravity_compensation" 144 | - "arm_controller/cartesian_twist" 145 | - "arm_controller/velocity" 146 | - "arm_controller/weightless_torque" 147 | - "arm_controller/torque_control_arm" 148 | - "arm_with_torso_controller/follow_joint_trajectory" 149 | - "base_controller" 150 | - "head_controller/follow_joint_trajectory" 151 | - "head_controller/point_head" 152 | - "torso_controller/follow_joint_trajectory" 153 | - "base_torque_controller" 154 | - "arm_base_controller" 155 | 156 | 157 | gazebo: 158 | default_controllers: 159 | - "arm_controller/follow_joint_trajectory" 160 | - "arm_controller/gravity_compensation" 161 | - "arm_controller/cartesian_twist" 162 | - "arm_controller/velocity" 163 | - "arm_controller/weightless_torque" 164 | - "arm_controller/torque_control_arm" 165 | - "arm_with_torso_controller/follow_joint_trajectory" 166 | - "base_controller" 167 | - "head_controller/follow_joint_trajectory" 168 | - "head_controller/point_head" 169 | - "torso_controller/follow_joint_trajectory" 170 | - "base_torque_controller" 171 | - "arm_base_controller" 172 | - "gripper_controller/gripper_action" 173 | - "bellows_controller" 174 | l_wheel_joint: 175 | position: 176 | p: 0.0 177 | d: 0.0 178 | i: 0.0 179 | i_clamp: 0.0 180 | velocity: 181 | p: 8.85 182 | d: 0.0 183 | i: 0.5 184 | i_clamp: 6.0 185 | r_wheel_joint: 186 | position: 187 | p: 0.0 188 | d: 0.0 189 | i: 0.0 190 | i_clamp: 0.0 191 | velocity: 192 | p: 8.85 193 | d: 0.0 194 | i: 0.5 195 | i_clamp: 6.0 196 | torso_lift_joint: 197 | position: 198 | p: 1000.0 199 | d: 0.0 200 | i: 0.0 201 | i_clamp: 0.0 202 | velocity: 203 | p: 100000.0 204 | d: 0.0 205 | i: 0.0 206 | i_clamp: 0.0 207 | bellows_joint: 208 | position: 209 | p: 10.0 210 | d: 0.0 211 | i: 0.0 212 | i_clamp: 0.0 213 | velocity: 214 | p: 25.0 215 | d: 0.0 216 | i: 0.0 217 | i_clamp: 0.0 218 | head_pan_joint: 219 | position: 220 | p: 2.0 221 | d: 0.0 222 | i: 0.0 223 | i_clamp: 0.0 224 | velocity: 225 | p: 2.0 226 | d: 0.0 227 | i: 0.0 228 | i_clamp: 0.0 229 | head_tilt_joint: 230 | position: 231 | p: 10.0 232 | d: 0.0 233 | i: 0.0 234 | i_clamp: 0.0 235 | velocity: 236 | p: 3.0 237 | d: 0.0 238 | i: 0.0 239 | i_clamp: 0.0 240 | shoulder_pan_joint: 241 | position: 242 | p: 100.0 243 | d: 0.1 244 | i: 0.0 245 | i_clamp: 0.0 246 | velocity: 247 | p: 200.0 248 | d: 0.0 249 | i: 2.0 250 | i_clamp: 1.0 251 | shoulder_lift_joint: 252 | position: 253 | p: 100.0 254 | d: 0.1 255 | i: 0.0 256 | i_clamp: 0.0 257 | velocity: 258 | p: 200.0 259 | d: 0.0 260 | i: 0.0 261 | i_clamp: 0.0 262 | upperarm_roll_joint: 263 | position: 264 | p: 100.0 265 | d: 0.1 266 | i: 0.0 267 | i_clamp: 0.0 268 | velocity: 269 | p: 10.0 270 | d: 0.0 271 | i: 0.0 272 | i_clamp: 0.0 273 | elbow_flex_joint: 274 | position: 275 | p: 100.0 276 | d: 0.1 277 | i: 0.0 278 | i_clamp: 0.0 279 | velocity: 280 | p: 150.0 281 | d: 0.0 282 | i: 0.0 283 | i_clamp: 0.0 284 | forearm_roll_joint: 285 | position: 286 | p: 100.0 287 | d: 0.1 288 | i: 0.0 289 | i_clamp: 0.0 290 | velocity: 291 | p: 150.0 292 | d: 0.0 293 | i: 0.0 294 | i_clamp: 0.0 295 | wrist_flex_joint: 296 | position: 297 | p: 100.0 298 | d: 0.1 299 | i: 0.0 300 | i_clamp: 0.0 301 | velocity: 302 | p: 100.0 303 | d: 0.0 304 | i: 0.0 305 | i_clamp: 0.0 306 | wrist_roll_joint: 307 | position: 308 | p: 100.0 309 | d: 0.1 310 | i: 0.0 311 | i_clamp: 0.0 312 | velocity: 313 | p: 100.0 314 | d: 0.0 315 | i: 0.0 316 | i_clamp: 0.0 317 | l_gripper_finger_joint: 318 | position: 319 | p: 5000.0 320 | d: 0.0 321 | i: 0.0 322 | i_clamp: 0.0 323 | velocity: 324 | p: 0.0 325 | d: 0.0 326 | i: 0.0 327 | i_clamp: 0.0 328 | r_gripper_finger_joint: 329 | position: 330 | p: 5000.0 331 | d: 0.0 332 | i: 0.0 333 | i_clamp: 0.0 334 | velocity: 335 | p: 0.0 336 | d: 0.0 337 | i: 0.0 338 | i_clamp: 0.0 339 | 340 | -------------------------------------------------------------------------------- /manipulator_control/launch/franka_launch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /manipulator_control/launch/full_pose_trajectory.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /manipulator_control/launch/jparse_april_tag_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manipulator_control/launch/jparse_cpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /manipulator_control/launch/kinova_gen3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /manipulator_control/launch/kinova_gen3_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /manipulator_control/launch/kinova_gen3_real_visual_servoing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /manipulator_control/launch/kinova_vel_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /manipulator_control/launch/line_extended_singular_traj.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /manipulator_control/launch/manip_velocity_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /manipulator_control/launch/panda_main_torque.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /manipulator_control/launch/position_trajectory.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /manipulator_control/launch/se3_type_2_singular_traj.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /manipulator_control/launch/xarm_launch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manipulator_control/launch/xarm_main_vel.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /manipulator_control/launch/xarm_python_using_cpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /manipulator_control/launch/xarm_real_launch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /manipulator_control/launch/xarm_real_spacemouse_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 38 | 39 | 40 | 43 | 44 | 45 | 46 | 47 | 48 | 56 | 57 | 58 | 68 | 69 | 70 | 71 | 78 | 79 | 80 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /manipulator_control/launch/xarm_spacemouse_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manipulator_control/msg/JparseTerms.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | manipulator_control/Matrow[] jparse 3 | manipulator_control/Matrow[] jsafety_nullspace 4 | -------------------------------------------------------------------------------- /manipulator_control/msg/Matrow.msg: -------------------------------------------------------------------------------- 1 | float64[] row 2 | -------------------------------------------------------------------------------- /manipulator_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | manipulator_control 4 | 0.0.0 5 | The manipulator_control package 6 | Monroe Kennedy 7 | Monroe Kennedy 8 | 9 | 10 | MIT 11 | 12 | 13 | 14 | catkin 15 | catkin_simple 16 | message_generation 17 | roscpp 18 | rospy 19 | roscpp 20 | rospy 21 | 22 | std_msgs 23 | tf 24 | tf2 25 | tf2_ros 26 | sensor_msgs 27 | tf_conversions 28 | eigen_conversions 29 | visualization_msgs 30 | geometry_msgs 31 | robot_controllers_msgs 32 | kdl_parser 33 | orocos_kdl 34 | urdf 35 | robot_controllers_interface 36 | gazebo_msgs 37 | moveit_core 38 | kortex_driver 39 | kortex_gazebo 40 | kortex_vision 41 | 42 | 43 | message_generation 44 | 45 | moveit_commander 46 | actionlib_msgs 47 | 48 | 49 | message_runtime 50 | roscpp 51 | rospy 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /manipulator_control/rviz/jparse_final.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /MotionPlanning1 9 | - /MotionPlanning1/Scene Robot1 10 | - /MotionPlanning1/Planning Request1 11 | - /MotionPlanning1/Planned Path1 12 | - /Pose1 13 | - /MarkerArray1 14 | - /TF1 15 | - /TF1/Frames1 16 | - /TF1/Tree1 17 | Splitter Ratio: 0.7425600290298462 18 | Tree Height: 650 19 | - Class: rviz/Help 20 | Name: Help 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | Preferences: 27 | PromptSaveOnExit: true 28 | Toolbars: 29 | toolButtonStyle: 2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Acceleration_Scaling_Factor: 1 52 | Class: moveit_rviz_plugin/MotionPlanning 53 | Enabled: false 54 | JointsTab_Use_Radians: true 55 | Move Group Namespace: "" 56 | MoveIt_Allow_Approximate_IK: false 57 | MoveIt_Allow_External_Program: false 58 | MoveIt_Allow_Replanning: false 59 | MoveIt_Allow_Sensor_Positioning: false 60 | MoveIt_Planning_Attempts: 10 61 | MoveIt_Planning_Time: 5 62 | MoveIt_Use_Cartesian_Path: false 63 | MoveIt_Use_Constraint_Aware_IK: true 64 | MoveIt_Workspace: 65 | Center: 66 | X: 0 67 | Y: 0 68 | Z: 0 69 | Size: 70 | X: 2 71 | Y: 2 72 | Z: 2 73 | Name: MotionPlanning 74 | Planned Path: 75 | Color Enabled: false 76 | Interrupt Display: false 77 | Links: 78 | All Links Enabled: true 79 | Expand Joint Details: false 80 | Expand Link Details: false 81 | Expand Tree: false 82 | Link Tree Style: Links in Alphabetic Order 83 | Loop Animation: false 84 | Robot Alpha: 0.5 85 | Robot Color: 150; 50; 150 86 | Show Robot Collision: false 87 | Show Robot Visual: true 88 | Show Trail: false 89 | State Display Time: 0.05 s 90 | Trail Step Size: 1 91 | Trajectory Topic: move_group/display_planned_path 92 | Use Sim Time: false 93 | Planning Metrics: 94 | Payload: 1 95 | Show Joint Torques: false 96 | Show Manipulability: false 97 | Show Manipulability Index: false 98 | Show Weight Limit: false 99 | TextHeight: 0.07999999821186066 100 | Planning Request: 101 | Colliding Link Color: 255; 0; 0 102 | Goal State Alpha: 1 103 | Goal State Color: 250; 128; 0 104 | Interactive Marker Size: 0 105 | Joint Violation Color: 255; 0; 255 106 | Planning Group: xarm7 107 | Query Goal State: true 108 | Query Start State: false 109 | Show Workspace: false 110 | Start State Alpha: 1 111 | Start State Color: 0; 255; 0 112 | Planning Scene Topic: move_group/monitored_planning_scene 113 | Robot Description: robot_description 114 | Scene Geometry: 115 | Scene Alpha: 1 116 | Scene Color: 50; 230; 50 117 | Scene Display Time: 0.20000000298023224 118 | Show Scene Geometry: true 119 | Voxel Coloring: Z-Axis 120 | Voxel Rendering: Occupied Voxels 121 | Scene Robot: 122 | Attached Body Color: 150; 50; 150 123 | Links: 124 | All Links Enabled: true 125 | Expand Joint Details: false 126 | Expand Link Details: false 127 | Expand Tree: false 128 | Link Tree Style: Links in Alphabetic Order 129 | Robot Alpha: 0.5 130 | Show Robot Collision: false 131 | Show Robot Visual: true 132 | Value: false 133 | Velocity_Scaling_Factor: 1 134 | - Alpha: 1 135 | Class: rviz/RobotModel 136 | Collision Enabled: false 137 | Enabled: true 138 | Links: 139 | All Links Enabled: true 140 | Expand Joint Details: false 141 | Expand Link Details: false 142 | Expand Tree: false 143 | Link Tree Style: Links in Alphabetic Order 144 | link1: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | link2: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | link3: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | link4: 160 | Alpha: 1 161 | Show Axes: false 162 | Show Trail: false 163 | Value: true 164 | link5: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | Value: true 169 | link6: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | link7: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | link_base: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | link_eef: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | world: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Name: RobotModel 193 | Robot Description: robot_description 194 | TF Prefix: "" 195 | Update Interval: 0 196 | Value: true 197 | Visual Enabled: true 198 | - Alpha: 1 199 | Axes Length: 0.20000000298023224 200 | Axes Radius: 0.029999999329447746 201 | Class: rviz/Pose 202 | Color: 255; 25; 0 203 | Enabled: true 204 | Head Length: 0.30000001192092896 205 | Head Radius: 0.10000000149011612 206 | Name: Pose 207 | Queue Size: 10 208 | Shaft Length: 1 209 | Shaft Radius: 0.05000000074505806 210 | Shape: Axes 211 | Topic: /target_pose 212 | Unreliable: false 213 | Value: true 214 | - Class: rviz/MarkerArray 215 | Enabled: true 216 | Marker Topic: /jparse_ellipsoid_marker 217 | Name: MarkerArray 218 | Namespaces: 219 | J_proj: true 220 | J_safety: true 221 | J_singular: true 222 | Queue Size: 1 223 | Value: true 224 | - Attached Body Color: 150; 50; 150 225 | Class: moveit_rviz_plugin/RobotState 226 | Collision Enabled: false 227 | Enabled: true 228 | Links: 229 | All Links Enabled: true 230 | Expand Joint Details: false 231 | Expand Link Details: false 232 | Expand Tree: false 233 | Link Tree Style: Links in Alphabetic Order 234 | link1: 235 | Alpha: 1 236 | Show Axes: false 237 | Show Trail: false 238 | Value: true 239 | link2: 240 | Alpha: 1 241 | Show Axes: false 242 | Show Trail: false 243 | Value: true 244 | link3: 245 | Alpha: 1 246 | Show Axes: false 247 | Show Trail: false 248 | Value: true 249 | link4: 250 | Alpha: 1 251 | Show Axes: false 252 | Show Trail: false 253 | Value: true 254 | link5: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | Value: true 259 | link6: 260 | Alpha: 1 261 | Show Axes: false 262 | Show Trail: false 263 | Value: true 264 | link7: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | link_base: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | link_eef: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | world: 279 | Alpha: 1 280 | Show Axes: false 281 | Show Trail: false 282 | Name: RobotState 283 | Robot Alpha: 1 284 | Robot Description: robot_description 285 | Robot State Topic: display_robot_state 286 | Show All Links: true 287 | Show Highlights: true 288 | Value: true 289 | Visual Enabled: true 290 | - Alpha: 1 291 | Autocompute Intensity Bounds: false 292 | Autocompute Value Bounds: 293 | Max Value: 10 294 | Min Value: -10 295 | Value: true 296 | Axis: Z 297 | Channel Name: temperature 298 | Class: rviz/Temperature 299 | Color: 255; 255; 255 300 | Color Transformer: "" 301 | Decay Time: 0 302 | Enabled: true 303 | Invert Rainbow: true 304 | Max Color: 255; 255; 255 305 | Max Intensity: 100 306 | Min Color: 0; 0; 0 307 | Min Intensity: 0 308 | Name: Temperature 309 | Position Transformer: "" 310 | Queue Size: 10 311 | Selectable: true 312 | Size (Pixels): 3 313 | Size (m): 0.009999999776482582 314 | Style: Flat Squares 315 | Topic: "" 316 | Unreliable: false 317 | Use Fixed Frame: true 318 | Use rainbow: true 319 | Value: true 320 | - Class: rviz/TF 321 | Enabled: true 322 | Filter (blacklist): "" 323 | Filter (whitelist): "" 324 | Frame Timeout: 15 325 | Frames: 326 | All Enabled: false 327 | link1: 328 | Value: false 329 | link2: 330 | Value: false 331 | link3: 332 | Value: false 333 | link4: 334 | Value: false 335 | link5: 336 | Value: false 337 | link6: 338 | Value: false 339 | link7: 340 | Value: false 341 | link_base: 342 | Value: false 343 | link_eef: 344 | Value: true 345 | world: 346 | Value: false 347 | Marker Alpha: 1 348 | Marker Scale: 1 349 | Name: TF 350 | Show Arrows: true 351 | Show Axes: true 352 | Show Names: true 353 | Tree: 354 | world: 355 | link_base: 356 | link1: 357 | link2: 358 | link3: 359 | link4: 360 | link5: 361 | link6: 362 | link7: 363 | link_eef: 364 | {} 365 | Update Interval: 0 366 | Value: true 367 | - Class: rviz/Marker 368 | Enabled: true 369 | Marker Topic: /robot_action_vector 370 | Name: Marker 371 | Namespaces: 372 | robot_action_vector: true 373 | Queue Size: 100 374 | Value: true 375 | Enabled: true 376 | Global Options: 377 | Background Color: 48; 48; 48 378 | Default Light: true 379 | Fixed Frame: world 380 | Frame Rate: 30 381 | Name: root 382 | Tools: 383 | - Class: rviz/Interact 384 | Hide Inactive Objects: true 385 | - Class: rviz/MoveCamera 386 | - Class: rviz/Select 387 | Value: true 388 | Views: 389 | Current: 390 | Class: rviz/XYOrbit 391 | Distance: 1.7171660661697388 392 | Enable Stereo Rendering: 393 | Stereo Eye Separation: 0.05999999865889549 394 | Stereo Focal Distance: 1 395 | Swap Stereo Eyes: false 396 | Value: false 397 | Field of View: 0.7853981852531433 398 | Focal Point: 399 | X: 0.11356700211763382 400 | Y: 0.10592000186443329 401 | Z: 2.2351800055275817e-07 402 | Focal Shape Fixed Size: true 403 | Focal Shape Size: 0.05000000074505806 404 | Invert Z Axis: false 405 | Name: Current View 406 | Near Clip Distance: 0.009999999776482582 407 | Pitch: -0.04020166024565697 408 | Target Frame: world 409 | Yaw: 1.9467729330062866 410 | Saved: ~ 411 | Window Geometry: 412 | Displays: 413 | collapsed: false 414 | Height: 885 415 | Help: 416 | collapsed: false 417 | Hide Left Dock: false 418 | Hide Right Dock: false 419 | MotionPlanning: 420 | collapsed: false 421 | MotionPlanning - Trajectory Slider: 422 | collapsed: false 423 | QMainWindow State: 000000ff00000000fd00000001000000000000020c0000031bfc0200000007fb000000100044006900730070006c006100790073010000003d0000031b000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001410000017d0000017d00ffffff0000056e0000031b00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 424 | Views: 425 | collapsed: false 426 | Width: 1920 427 | X: 0 428 | Y: 28 429 | -------------------------------------------------------------------------------- /manipulator_control/rviz/kinova_gen3.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /MarkerArray1 11 | - /MarkerArray1/Namespaces1 12 | - /Pose1 13 | Splitter Ratio: 0.5 14 | Tree Height: 346 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Image 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Class: rviz/RobotModel 60 | Collision Enabled: false 61 | Enabled: true 62 | Links: 63 | All Links Enabled: true 64 | Expand Joint Details: false 65 | Expand Link Details: false 66 | Expand Tree: false 67 | Link Tree Style: "" 68 | base_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | bracelet_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | camera_color_frame: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | camera_depth_frame: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | camera_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | end_effector_link: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | forearm_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | half_arm_1_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | half_arm_2_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | shoulder_link: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | spherical_wrist_1_link: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | spherical_wrist_2_link: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | tool_frame: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Name: RobotModel 129 | Robot Description: robot_description 130 | TF Prefix: "" 131 | Update Interval: 0 132 | Value: true 133 | Visual Enabled: true 134 | - Class: rviz/TF 135 | Enabled: true 136 | Filter (blacklist): "" 137 | Filter (whitelist): "" 138 | Frame Timeout: 2 139 | Frames: 140 | All Enabled: false 141 | base_link: 142 | Value: true 143 | bracelet_link: 144 | Value: false 145 | camera_color_frame: 146 | Value: false 147 | camera_depth_frame: 148 | Value: false 149 | camera_link: 150 | Value: false 151 | end_effector_link: 152 | Value: true 153 | forearm_link: 154 | Value: false 155 | half_arm_1_link: 156 | Value: false 157 | half_arm_2_link: 158 | Value: false 159 | shoulder_link: 160 | Value: false 161 | spherical_wrist_1_link: 162 | Value: false 163 | spherical_wrist_2_link: 164 | Value: false 165 | tag_1: 166 | Value: true 167 | tool_frame: 168 | Value: false 169 | Marker Alpha: 1 170 | Marker Scale: 1 171 | Name: TF 172 | Show Arrows: true 173 | Show Axes: true 174 | Show Names: false 175 | Tree: 176 | base_link: 177 | shoulder_link: 178 | half_arm_1_link: 179 | half_arm_2_link: 180 | forearm_link: 181 | spherical_wrist_1_link: 182 | spherical_wrist_2_link: 183 | bracelet_link: 184 | end_effector_link: 185 | camera_link: 186 | camera_color_frame: 187 | tag_1: 188 | {} 189 | camera_depth_frame: 190 | {} 191 | tool_frame: 192 | {} 193 | Update Interval: 0 194 | Value: true 195 | - Class: rviz/Marker 196 | Enabled: true 197 | Marker Topic: /visualization_marker 198 | Name: Marker 199 | Namespaces: 200 | {} 201 | Queue Size: 100 202 | Value: true 203 | - Class: rviz/MarkerArray 204 | Enabled: true 205 | Marker Topic: /jparse_ellipsoid_marker 206 | Name: MarkerArray 207 | Namespaces: 208 | J_proj: true 209 | J_safety: true 210 | Queue Size: 100 211 | Value: true 212 | - Class: rviz/Camera 213 | Enabled: false 214 | Image Rendering: background and overlay 215 | Image Topic: "" 216 | Name: Camera 217 | Overlay Alpha: 0.5 218 | Queue Size: 2 219 | Transport Hint: raw 220 | Unreliable: false 221 | Value: false 222 | Visibility: 223 | "": true 224 | Grid: true 225 | Marker: true 226 | MarkerArray: true 227 | RobotModel: true 228 | TF: true 229 | Value: true 230 | Zoom Factor: 1 231 | - Class: rviz/Image 232 | Enabled: true 233 | Image Topic: /camera/color/image_raw 234 | Max Value: 1 235 | Median window: 5 236 | Min Value: 0 237 | Name: Image 238 | Normalize Range: true 239 | Queue Size: 2 240 | Transport Hint: raw 241 | Unreliable: false 242 | Value: true 243 | - Alpha: 1 244 | Axes Length: 0.20000000298023224 245 | Axes Radius: 0.009999999776482582 246 | Class: rviz/Pose 247 | Color: 255; 25; 0 248 | Enabled: true 249 | Head Length: 0.30000001192092896 250 | Head Radius: 0.10000000149011612 251 | Name: Pose 252 | Queue Size: 10 253 | Shaft Length: 1 254 | Shaft Radius: 0.05000000074505806 255 | Shape: Axes 256 | Topic: /target_pose_servoing 257 | Unreliable: false 258 | Value: true 259 | Enabled: true 260 | Global Options: 261 | Background Color: 243; 243; 243 262 | Default Light: true 263 | Fixed Frame: base_link 264 | Frame Rate: 30 265 | Name: root 266 | Tools: 267 | - Class: rviz/Interact 268 | Hide Inactive Objects: true 269 | - Class: rviz/MoveCamera 270 | - Class: rviz/Select 271 | - Class: rviz/FocusCamera 272 | - Class: rviz/Measure 273 | - Class: rviz/SetInitialPose 274 | Theta std deviation: 0.2617993950843811 275 | Topic: /initialpose 276 | X std deviation: 0.5 277 | Y std deviation: 0.5 278 | - Class: rviz/SetGoal 279 | Topic: /move_base_simple/goal 280 | - Class: rviz/PublishPoint 281 | Single click: true 282 | Topic: /clicked_point 283 | Value: true 284 | Views: 285 | Current: 286 | Class: rviz/Orbit 287 | Distance: 2.0648767948150635 288 | Enable Stereo Rendering: 289 | Stereo Eye Separation: 0.05999999865889549 290 | Stereo Focal Distance: 1 291 | Swap Stereo Eyes: false 292 | Value: false 293 | Field of View: 0.7853981852531433 294 | Focal Point: 295 | X: 0.3303897976875305 296 | Y: -0.10178785771131516 297 | Z: 0.046957336366176605 298 | Focal Shape Fixed Size: true 299 | Focal Shape Size: 0.05000000074505806 300 | Invert Z Axis: false 301 | Name: Current View 302 | Near Clip Distance: 0.009999999776482582 303 | Pitch: 0.3747974634170532 304 | Target Frame: 305 | Yaw: 1.3253982067108154 306 | Saved: ~ 307 | Window Geometry: 308 | Camera: 309 | collapsed: false 310 | Displays: 311 | collapsed: false 312 | Height: 836 313 | Hide Left Dock: false 314 | Hide Right Dock: true 315 | Image: 316 | collapsed: false 317 | QMainWindow State: 000000ff00000000fd00000004000000000000016b000002a6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000228000000bb0000001600fffffffb0000000a0049006d0061006700650100000228000000bb0000001600ffffff000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000033f000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 318 | Selection: 319 | collapsed: false 320 | Time: 321 | collapsed: false 322 | Tool Properties: 323 | collapsed: false 324 | Views: 325 | collapsed: true 326 | Width: 1200 327 | X: 648 328 | Y: 28 329 | -------------------------------------------------------------------------------- /manipulator_control/rviz/panda_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /MarkerArray1 11 | - /MarkerArray1/Namespaces1 12 | Splitter Ratio: 0.5 13 | Tree Height: 539 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/RobotModel 59 | Collision Enabled: false 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: "" 67 | panda_1_hand: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | panda_1_hand_sc: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | panda_1_hand_tcp: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | panda_1_leftfinger: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | panda_1_link0: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | panda_1_link0_sc: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | panda_1_link1: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | panda_1_link1_sc: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | panda_1_link2: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | panda_1_link2_sc: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | panda_1_link3: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | panda_1_link3_sc: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | panda_1_link4: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | panda_1_link4_sc: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | panda_1_link5: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | panda_1_link5_sc: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | panda_1_link6: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | panda_1_link6_sc: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | panda_1_link7: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | panda_1_link7_sc: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | panda_1_link8: 158 | Alpha: 1 159 | Show Axes: false 160 | Show Trail: false 161 | panda_1_rightfinger: 162 | Alpha: 1 163 | Show Axes: false 164 | Show Trail: false 165 | Value: true 166 | world: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | Name: RobotModel 171 | Robot Description: robot_description 172 | TF Prefix: "" 173 | Update Interval: 0 174 | Value: true 175 | Visual Enabled: true 176 | - Class: rviz/TF 177 | Enabled: true 178 | Filter (blacklist): "" 179 | Filter (whitelist): "" 180 | Frame Timeout: 15 181 | Frames: 182 | All Enabled: false 183 | panda_1_EE: 184 | Value: false 185 | panda_1_K: 186 | Value: false 187 | panda_1_NE: 188 | Value: false 189 | panda_1_hand: 190 | Value: true 191 | panda_1_hand_sc: 192 | Value: false 193 | panda_1_hand_tcp: 194 | Value: false 195 | panda_1_leftfinger: 196 | Value: false 197 | panda_1_link0: 198 | Value: true 199 | panda_1_link0_sc: 200 | Value: false 201 | panda_1_link1: 202 | Value: false 203 | panda_1_link1_sc: 204 | Value: false 205 | panda_1_link2: 206 | Value: false 207 | panda_1_link2_sc: 208 | Value: false 209 | panda_1_link3: 210 | Value: false 211 | panda_1_link3_sc: 212 | Value: false 213 | panda_1_link4: 214 | Value: false 215 | panda_1_link4_sc: 216 | Value: false 217 | panda_1_link5: 218 | Value: false 219 | panda_1_link5_sc: 220 | Value: false 221 | panda_1_link6: 222 | Value: false 223 | panda_1_link6_sc: 224 | Value: false 225 | panda_1_link7: 226 | Value: false 227 | panda_1_link7_sc: 228 | Value: false 229 | panda_1_link8: 230 | Value: false 231 | panda_1_rightfinger: 232 | Value: false 233 | world: 234 | Value: false 235 | Marker Alpha: 1 236 | Marker Scale: 1 237 | Name: TF 238 | Show Arrows: true 239 | Show Axes: true 240 | Show Names: false 241 | Tree: 242 | world: 243 | panda_1_link0: 244 | panda_1_link0_sc: 245 | {} 246 | panda_1_link1: 247 | panda_1_link1_sc: 248 | {} 249 | panda_1_link2: 250 | panda_1_link2_sc: 251 | {} 252 | panda_1_link3: 253 | panda_1_link3_sc: 254 | {} 255 | panda_1_link4: 256 | panda_1_link4_sc: 257 | {} 258 | panda_1_link5: 259 | panda_1_link5_sc: 260 | {} 261 | panda_1_link6: 262 | panda_1_link6_sc: 263 | {} 264 | panda_1_link7: 265 | panda_1_link7_sc: 266 | {} 267 | panda_1_link8: 268 | panda_1_NE: 269 | panda_1_EE: 270 | panda_1_K: 271 | {} 272 | panda_1_hand: 273 | panda_1_hand_sc: 274 | {} 275 | panda_1_hand_tcp: 276 | {} 277 | panda_1_leftfinger: 278 | {} 279 | panda_1_rightfinger: 280 | {} 281 | Update Interval: 0 282 | Value: true 283 | - Class: rviz/Marker 284 | Enabled: true 285 | Marker Topic: /visualization_marker 286 | Name: Marker 287 | Namespaces: 288 | se3_trajectory: true 289 | Queue Size: 100 290 | Value: true 291 | - Class: rviz/MarkerArray 292 | Enabled: true 293 | Marker Topic: /jparse_ellipsoid_marker 294 | Name: MarkerArray 295 | Namespaces: 296 | J_proj: true 297 | J_safety: true 298 | J_singular: true 299 | Queue Size: 100 300 | Value: true 301 | Enabled: true 302 | Global Options: 303 | Background Color: 243; 243; 243 304 | Default Light: true 305 | Fixed Frame: panda_1_link0 306 | Frame Rate: 30 307 | Name: root 308 | Tools: 309 | - Class: rviz/Interact 310 | Hide Inactive Objects: true 311 | - Class: rviz/MoveCamera 312 | - Class: rviz/Select 313 | - Class: rviz/FocusCamera 314 | - Class: rviz/Measure 315 | - Class: rviz/SetInitialPose 316 | Theta std deviation: 0.2617993950843811 317 | Topic: /initialpose 318 | X std deviation: 0.5 319 | Y std deviation: 0.5 320 | - Class: rviz/SetGoal 321 | Topic: /move_base_simple/goal 322 | - Class: rviz/PublishPoint 323 | Single click: true 324 | Topic: /clicked_point 325 | Value: true 326 | Views: 327 | Current: 328 | Class: rviz/Orbit 329 | Distance: 2.705379009246826 330 | Enable Stereo Rendering: 331 | Stereo Eye Separation: 0.05999999865889549 332 | Stereo Focal Distance: 1 333 | Swap Stereo Eyes: false 334 | Value: false 335 | Field of View: 0.7853981852531433 336 | Focal Point: 337 | X: 0 338 | Y: 0 339 | Z: 0 340 | Focal Shape Fixed Size: true 341 | Focal Shape Size: 0.05000000074505806 342 | Invert Z Axis: false 343 | Name: Current View 344 | Near Clip Distance: 0.009999999776482582 345 | Pitch: 0.36979612708091736 346 | Target Frame: 347 | Yaw: 0.8153983354568481 348 | Saved: ~ 349 | Window Geometry: 350 | Displays: 351 | collapsed: false 352 | Height: 836 353 | Hide Left Dock: false 354 | Hide Right Dock: true 355 | QMainWindow State: 000000ff00000000fd00000004000000000000016b000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000033f000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 356 | Selection: 357 | collapsed: false 358 | Time: 359 | collapsed: false 360 | Tool Properties: 361 | collapsed: false 362 | Views: 363 | collapsed: true 364 | Width: 1200 365 | X: 659 366 | Y: 28 367 | -------------------------------------------------------------------------------- /manipulator_control/rviz/xarm_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /MarkerArray1 12 | - /MarkerArray1/Namespaces1 13 | Splitter Ratio: 0.5 14 | Tree Height: 536 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Class: rviz/RobotModel 60 | Collision Enabled: false 61 | Enabled: true 62 | Links: 63 | All Links Enabled: true 64 | Expand Joint Details: false 65 | Expand Link Details: false 66 | Expand Tree: false 67 | Link Tree Style: Links in Alphabetic Order 68 | link1: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | link2: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | link3: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | link4: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | link5: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | link6: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | link7: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | link_base: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | link_eef: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | world: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Name: RobotModel 117 | Robot Description: robot_description 118 | TF Prefix: "" 119 | Update Interval: 0 120 | Value: true 121 | Visual Enabled: true 122 | - Class: rviz/Marker 123 | Enabled: true 124 | Marker Topic: /visualization_marker 125 | Name: Marker 126 | Namespaces: 127 | se3_trajectory: true 128 | Queue Size: 100 129 | Value: true 130 | - Class: rviz/TF 131 | Enabled: true 132 | Filter (blacklist): "" 133 | Filter (whitelist): "" 134 | Frame Timeout: 15 135 | Frames: 136 | All Enabled: false 137 | link1: 138 | Value: false 139 | link2: 140 | Value: false 141 | link3: 142 | Value: false 143 | link4: 144 | Value: false 145 | link5: 146 | Value: false 147 | link6: 148 | Value: false 149 | link7: 150 | Value: false 151 | link_base: 152 | Value: true 153 | link_eef: 154 | Value: true 155 | world: 156 | Value: false 157 | Marker Alpha: 1 158 | Marker Scale: 1 159 | Name: TF 160 | Show Arrows: true 161 | Show Axes: true 162 | Show Names: false 163 | Tree: 164 | world: 165 | link_base: 166 | link1: 167 | link2: 168 | link3: 169 | link4: 170 | link5: 171 | link6: 172 | link7: 173 | link_eef: 174 | {} 175 | Update Interval: 0 176 | Value: true 177 | - Class: rviz/MarkerArray 178 | Enabled: true 179 | Marker Topic: /jparse_ellipsoid_marker 180 | Name: MarkerArray 181 | Namespaces: 182 | {} 183 | Queue Size: 2 184 | Value: true 185 | - Class: rviz/Marker 186 | Enabled: true 187 | Marker Topic: /robot_action_vector 188 | Name: Marker 189 | Namespaces: 190 | {} 191 | Queue Size: 100 192 | Value: true 193 | - Class: rviz/MarkerArray 194 | Enabled: true 195 | Marker Topic: /jparse_ellipsoid_marker_cpp 196 | Name: MarkerArray 197 | Namespaces: 198 | J_proj: true 199 | J_safety: true 200 | Queue Size: 100 201 | Value: true 202 | Enabled: true 203 | Global Options: 204 | Background Color: 255; 255; 255 205 | Default Light: true 206 | Fixed Frame: link_base 207 | Frame Rate: 30 208 | Name: root 209 | Tools: 210 | - Class: rviz/Interact 211 | Hide Inactive Objects: true 212 | - Class: rviz/MoveCamera 213 | - Class: rviz/Select 214 | - Class: rviz/FocusCamera 215 | - Class: rviz/Measure 216 | - Class: rviz/SetInitialPose 217 | Theta std deviation: 0.2617993950843811 218 | Topic: /initialpose 219 | X std deviation: 0.5 220 | Y std deviation: 0.5 221 | - Class: rviz/SetGoal 222 | Topic: /move_base_simple/goal 223 | - Class: rviz/PublishPoint 224 | Single click: true 225 | Topic: /clicked_point 226 | Value: true 227 | Views: 228 | Current: 229 | Class: rviz/Orbit 230 | Distance: 1.7798486948013306 231 | Enable Stereo Rendering: 232 | Stereo Eye Separation: 0.05999999865889549 233 | Stereo Focal Distance: 1 234 | Swap Stereo Eyes: false 235 | Value: false 236 | Field of View: 0.7853981852531433 237 | Focal Point: 238 | X: 0.22760114073753357 239 | Y: -0.09859215468168259 240 | Z: 0.294298380613327 241 | Focal Shape Fixed Size: true 242 | Focal Shape Size: 0.05000000074505806 243 | Invert Z Axis: false 244 | Name: Current View 245 | Near Clip Distance: 0.009999999776482582 246 | Pitch: 0.18479715287685394 247 | Target Frame: 248 | Yaw: 1.5104079246520996 249 | Saved: ~ 250 | Window Geometry: 251 | Displays: 252 | collapsed: false 253 | Height: 833 254 | Hide Left Dock: false 255 | Hide Right Dock: true 256 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 257 | Selection: 258 | collapsed: false 259 | Time: 260 | collapsed: false 261 | Tool Properties: 262 | collapsed: false 263 | Views: 264 | collapsed: true 265 | Width: 1200 266 | X: 716 267 | Y: 28 268 | -------------------------------------------------------------------------------- /manipulator_control/scripts/device.py: -------------------------------------------------------------------------------- 1 | import abc # for abstract base class definitions 2 | 3 | 4 | class Device(metaclass=abc.ABCMeta): 5 | """ 6 | Base class for all robot controllers. 7 | Defines basic interface for all controllers to adhere to. 8 | """ 9 | 10 | @abc.abstractmethod 11 | def start_control(self): 12 | """ 13 | Method that should be called externally before controller can 14 | start receiving commands. 15 | """ 16 | raise NotImplementedError 17 | 18 | @abc.abstractmethod 19 | def get_controller_state(self): 20 | """Returns the current state of the device, a dictionary of pos, orn, grasp, and reset.""" 21 | raise NotImplementedError -------------------------------------------------------------------------------- /manipulator_control/scripts/extract_lfd_images_from_bag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import rosbag 5 | import os 6 | import cv2 7 | import numpy as np 8 | from cv_bridge import CvBridge 9 | from sensor_msgs.msg import Image 10 | import rospkg 11 | import sys 12 | 13 | def get_bagfile_path(bagname): 14 | rospack = rospkg.RosPack() 15 | package_path = rospack.get_path('manipulator_control') 16 | bagfile_path = os.path.join(package_path, 'bag', 'xarm_real_bags', 'LfD', bagname) 17 | 18 | if not os.path.exists(bagfile_path): 19 | raise FileNotFoundError(f"Bag file '{bagfile_path}' not found.") 20 | 21 | return bagfile_path 22 | 23 | def save_image(image_msg, output_dir, index): 24 | bridge = CvBridge() 25 | try: 26 | # Convert ROS Image message to OpenCV BGR format 27 | cv_image = bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8") 28 | filename = os.path.join(output_dir, f"image_{index:05d}.png") 29 | cv2.imwrite(filename, cv_image) 30 | rospy.loginfo(f"Saved image to {filename}") 31 | return cv_image 32 | except Exception as e: 33 | rospy.logerr(f"Failed to convert and save image: {e}") 34 | return None 35 | 36 | def process_bag(input_bag_path): 37 | if not os.path.exists(input_bag_path): 38 | rospy.logerr(f"Bag file not found: {input_bag_path}") 39 | return 40 | 41 | # Create an output folder at the same level as the bag file 42 | output_dir = os.path.join(os.path.dirname(input_bag_path), 'images') 43 | os.makedirs(output_dir, exist_ok=True) 44 | 45 | rospy.loginfo(f"Saving images to: {output_dir}") 46 | 47 | index = 0 48 | frames = [] 49 | timestamps = [] 50 | 51 | try: 52 | with rosbag.Bag(input_bag_path, 'r') as bag: 53 | for topic, msg, t in bag.read_messages(): 54 | if topic == "/camera/rgb/image_raw":# and isinstance(msg, Image): 55 | cv_image = save_image(msg, output_dir, index) 56 | if cv_image is not None: 57 | frames.append(cv_image) 58 | timestamps.append(msg.header.stamp.to_sec()) 59 | index += 1 60 | 61 | rospy.loginfo(f"Extracted and saved {index} images") 62 | 63 | # If there are frames, create a video 64 | if len(frames) > 1: 65 | # Calculate average time difference to define FPS 66 | time_diffs = np.diff(timestamps) 67 | avg_time_diff = np.mean(time_diffs) 68 | fps = round(1.0 / avg_time_diff) if avg_time_diff > 0 else 30 69 | 70 | rospy.loginfo(f"Average time difference: {avg_time_diff:.4f} seconds => FPS: {fps}") 71 | 72 | # Get video dimensions from the first frame 73 | height, width, _ = frames[0].shape 74 | video_path = os.path.join(output_dir, 'output_video.avi') 75 | 76 | # Use the MJPG codec and .avi container, which tends to avoid color issues 77 | fourcc = cv2.VideoWriter_fourcc(*'MJPG') 78 | video_writer = cv2.VideoWriter(video_path, fourcc, fps, (width, height)) 79 | 80 | # Write frames in BGR format directly to the AVI file 81 | for frame in frames: 82 | video_writer.write(frame) 83 | 84 | video_writer.release() 85 | rospy.loginfo(f"Saved MJPEG video to {video_path}") 86 | 87 | except Exception as e: 88 | rospy.logerr(f"Error reading bag file: {e}") 89 | 90 | if __name__ == '__main__': 91 | rospy.init_node('image_extraction_node') 92 | 93 | if len(sys.argv) < 2: 94 | rospy.logerr("Usage: rosrun image_extraction.py ") 95 | sys.exit(1) 96 | 97 | bagname = sys.argv[1] 98 | 99 | try: 100 | input_bag_path = get_bagfile_path(bagname) 101 | rospy.loginfo(f"Reading bag file: {input_bag_path}") 102 | process_bag(input_bag_path) 103 | except Exception as e: 104 | rospy.logerr(f"Error: {e}") 105 | sys.exit(1) -------------------------------------------------------------------------------- /manipulator_control/scripts/input2action.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utility functions for grabbing user inputs 3 | """ 4 | 5 | import numpy as np 6 | 7 | # import robosuite as suite 8 | # import robosuite.utils.transform_utils as T 9 | # from robosuite.devices import * 10 | # from robosuite.models.robots import * 11 | # from robosuite.robots import * 12 | 13 | from spacemouse import SpaceMouse 14 | 15 | 16 | def input2action(device, robot, active_arm="right", env_configuration=None): 17 | """ 18 | Converts an input from an active device into a valid action sequence that can be fed into an env.step() call 19 | 20 | If a reset is triggered from the device, immediately returns None. Else, returns the appropriate action 21 | 22 | Args: 23 | device (Device): A device from which user inputs can be converted into actions. Can be either a Spacemouse or 24 | Keyboard device class 25 | 26 | robot (Robot): Which robot we're controlling 27 | 28 | active_arm (str): Only applicable for multi-armed setups (e.g.: multi-arm environments or bimanual robots). 29 | Allows inputs to be converted correctly if the control type (e.g.: IK) is dependent on arm choice. 30 | Choices are {right, left} 31 | 32 | env_configuration (str or None): Only applicable for multi-armed environments. Allows inputs to be converted 33 | correctly if the control type (e.g.: IK) is dependent on the environment setup. Options are: 34 | {bimanual, single-arm-parallel, single-arm-opposed} 35 | 36 | Returns: 37 | 2-tuple: 38 | 39 | - (None or np.array): Action interpreted from @device including any gripper action(s). None if we get a 40 | reset signal from the device 41 | - (None or int): 1 if desired close, -1 if desired open gripper state. None if get a reset signal from the 42 | device 43 | 44 | """ 45 | 46 | 47 | state = device.get_controller_state() 48 | # Note: Devices output rotation with x and z flipped to account for robots starting with gripper facing down 49 | # Also note that the outputted rotation is an absolute rotation, while outputted dpos is delta pos 50 | # Raw delta rotations from neutral user input is captured in raw_drotation (roll, pitch, yaw) 51 | dpos, rotation, raw_drotation, grasp, reset = ( 52 | state["dpos"], 53 | state["rotation"], 54 | state["raw_drotation"], 55 | state["grasp"], 56 | state["reset"], 57 | ) 58 | 59 | # If we're resetting, immediately return None 60 | if reset: 61 | return None, None 62 | 63 | # Get controller reference 64 | #controller = robot.controller if not isinstance(robot, Bimanual) else robot.controller[active_arm] 65 | 66 | #gripper_dof = robot.gripper.dof 67 | 68 | # First process the raw drotation 69 | drotation = raw_drotation[[1, 0, 2]] 70 | 71 | #elif controller.name == "OSC_POSE": 72 | # Flip z 73 | drotation[2] = -drotation[2] 74 | # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device 75 | drotation = drotation * 50 76 | dpos = dpos * 125 77 | 78 | # map 0 to -1 (open) and map 1 to 1 (closed) 79 | grasp = 1 if grasp else -1 80 | 81 | #action = np.concatenate([dpos, drotation, [grasp] * gripper_dof]) 82 | action = np.concatenate([dpos, drotation]) 83 | 84 | # Return the action and grasp 85 | return action, grasp -------------------------------------------------------------------------------- /manipulator_control/scripts/position_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import math 5 | from visualization_msgs.msg import Marker 6 | from geometry_msgs.msg import Pose, PoseStamped 7 | 8 | class OvalTrajectory: 9 | def __init__(self): 10 | # Initialize ROS node 11 | rospy.init_node('oval_trajectory_generator', anonymous=True) 12 | 13 | # RViz Marker publisher 14 | self.marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10) 15 | 16 | # Pose publisher 17 | self.pose_pub = rospy.Publisher('/target_pose', PoseStamped, queue_size=10) 18 | 19 | # Parameters for the oval trajectory 20 | self.center = rospy.get_param('~center', [0.0, 0.0, 0.0]) # Center of the oval [x, y, z] 21 | if type(self.center) is str: 22 | self.center = [float(x) for x in self.center.strip('[]').split(',')] # Convert string to list of floats 23 | 24 | self.major_axis = float(rospy.get_param('~major_axis', "1.0")) # Length of the major axis 25 | self.minor_axis = float(rospy.get_param('~minor_axis', "0.5")) # Length of the minor axis 26 | self.pitch_axis = float(rospy.get_param('~pitch_axis', "0.0")) # Length of the pitch axis 27 | self.plane = rospy.get_param('~plane', "xy") # Plane of the oval ('xy', 'yz', or 'xz') 28 | self.frequency = float(rospy.get_param('~frequency', "0.2")) # Frequency of the trajectory (Hz) 29 | self.base_frame = rospy.get_param('~base_frame', "base_link") # Base frame for the trajectory 30 | 31 | # Time tracking 32 | self.start_time = rospy.Time.now() 33 | 34 | # Start the trajectory generation loop 35 | self.trajectory_loop() 36 | 37 | def generate_position(self, time_elapsed): 38 | """Generate the 3D position of the point on the oval.""" 39 | angle = 2 * math.pi * self.frequency * time_elapsed 40 | 41 | if self.plane == 'xy': 42 | x = self.center[0] + self.major_axis * math.cos(angle) 43 | y = self.center[1] + self.minor_axis * math.sin(angle) 44 | z = self.center[2] + self.pitch_axis * math.cos(angle) 45 | elif self.plane == 'yz': 46 | x = self.center[0] 47 | y = self.center[1] + self.major_axis * math.cos(angle) 48 | z = self.center[2] + self.minor_axis * math.sin(angle) 49 | elif self.plane == 'xz': 50 | x = self.center[0] + self.major_axis * math.cos(angle) 51 | y = self.center[1] 52 | z = self.center[2] + self.minor_axis * math.sin(angle) 53 | else: 54 | rospy.logwarn("Invalid plane specified. Defaulting to 'xy'.") 55 | x = self.center[0] + self.major_axis * math.cos(angle) 56 | y = self.center[1] + self.minor_axis * math.sin(angle) 57 | z = self.center[2] 58 | 59 | return x, y, z 60 | 61 | def publish_marker(self, position): 62 | """Publish the current goal position as an RViz marker.""" 63 | marker = Marker() 64 | marker.header.frame_id = self.base_frame 65 | marker.header.stamp = rospy.Time.now() 66 | marker.ns = "oval_trajectory" 67 | marker.id = 0 68 | marker.type = Marker.SPHERE 69 | marker.action = Marker.ADD 70 | marker.pose.position.x = position[0] 71 | marker.pose.position.y = position[1] 72 | marker.pose.position.z = position[2] 73 | marker.pose.orientation.x = 0.0 74 | marker.pose.orientation.y = 0.0 75 | marker.pose.orientation.z = 0.0 76 | marker.pose.orientation.w = 1.0 77 | marker.scale.x = 0.1 78 | marker.scale.y = 0.1 79 | marker.scale.z = 0.1 80 | marker.color.r = 1.0 81 | marker.color.g = 0.0 82 | marker.color.b = 0.0 83 | marker.color.a = 1.0 84 | 85 | self.marker_pub.publish(marker) 86 | 87 | def publish_pose(self, position): 88 | """Publish the current goal position as a geometry_msgs/Pose message.""" 89 | pose = PoseStamped() 90 | pose.header.frame_id = self.base_frame 91 | pose.header.stamp = rospy.Time.now() 92 | pose.pose.position.x = position[0] 93 | pose.pose.position.y = position[1] 94 | pose.pose.position.z = position[2] 95 | pose.pose.orientation.x = 0.0 96 | pose.pose.orientation.y = 0.0 97 | pose.pose.orientation.z = 0.0 98 | pose.pose.orientation.w = 1.0 99 | 100 | self.pose_pub.publish(pose) 101 | 102 | def trajectory_loop(self): 103 | """Main loop for trajectory generation and marker/pose publishing.""" 104 | rate = rospy.Rate(50) # 50 Hz control loop 105 | 106 | while not rospy.is_shutdown(): 107 | # Calculate elapsed time 108 | elapsed_time = (rospy.Time.now() - self.start_time).to_sec() 109 | 110 | # Generate current position on the oval 111 | position = self.generate_position(elapsed_time) 112 | 113 | # Publish the marker and pose 114 | self.publish_marker(position) 115 | self.publish_pose(position) 116 | 117 | # Sleep to maintain loop rate 118 | rate.sleep() 119 | 120 | 121 | if __name__ == '__main__': 122 | try: 123 | OvalTrajectory() 124 | except rospy.ROSInterruptException: 125 | rospy.loginfo("Oval trajectory generator node terminated.") -------------------------------------------------------------------------------- /manipulator_control/scripts/se3_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import math 5 | import numpy as np 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import PoseStamped, Pose 8 | from tf.transformations import quaternion_from_euler, quaternion_matrix 9 | 10 | class SE3Trajectory: 11 | def __init__(self): 12 | # Initialize ROS node 13 | rospy.init_node('se3_trajectory_generator', anonymous=True) 14 | 15 | # RViz Marker publisher 16 | self.marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10) 17 | 18 | # Pose publisher 19 | self.pose_pub = rospy.Publisher('/target_pose', PoseStamped, queue_size=10) 20 | 21 | # Parameters for position trajectory 22 | self.center = rospy.get_param('~center', [0.0, 0.0, 0.0]) # Center of the oval [x, y, z] 23 | if type(self.center) is str: 24 | self.center = [float(x) for x in self.center.strip('[]').split(',')] # Convert string to list of floats 25 | 26 | self.robot = rospy.get_param('~robot', "panda") # Robot type ('panda' or 'xarm' or 'kinova') 27 | 28 | self.key_points_only_bool = rospy.get_param('~key_points_only_bool', "False") # Boolean to determine if only key points should be published 29 | self.use_rotation = rospy.get_param('~use_rotation', "True") # Boolean to determine if rotation should be used 30 | 31 | self.major_axis = float(rospy.get_param('~major_axis', "1.0")) # Length of the major axis 32 | self.minor_axis = float(rospy.get_param('~minor_axis', "0.5")) # Length of the minor axis 33 | self.pitch_axis = float(rospy.get_param('~pitch_axis', "0.0")) # Length of the pitch axis 34 | 35 | 36 | self.plane = rospy.get_param('~plane', "xy") # Plane of the oval ('xy', 'yz', or 'xz') 37 | self.frequency = float(rospy.get_param('~frequency', "0.2")) # Frequency of the trajectory (Hz) 38 | 39 | # Parameters for orientation trajectory 40 | self.orientation_major_axis = float(rospy.get_param('~orientation_major_axis', "0.3")) 41 | self.orientation_minor_axis = float(rospy.get_param('~orientation_minor_axis', "0.1")) 42 | self.orientation_frequency = float(rospy.get_param('~orientation_frequency', "0.1")) # Frequency of orientation change 43 | 44 | #Get the base frame for the trajectory 45 | self.base_frame = rospy.get_param('~base_frame', "base_link") # Base frame for the trajectory 46 | 47 | #set scale for marker arrow length 48 | self.arrow_scale = 0.25 49 | 50 | # Time tracking 51 | self.start_time = rospy.Time.now() 52 | 53 | # Start the trajectory generation loop 54 | self.trajectory_loop() 55 | 56 | def discrete_positions_around_ellipse(self, time_elapsed): 57 | # Period (time spent at each critical point) 58 | period = 1 / self.frequency 59 | # Determine which critical point to select based on time_elapsed 60 | critical_point_index = int((time_elapsed % (4 * period)) // period) 61 | 62 | if self.plane == 'xy': 63 | if critical_point_index == 0: # Positive major axis 64 | x = self.center[0] + self.major_axis 65 | y = self.center[1] 66 | z = self.center[2] 67 | elif critical_point_index == 1: # Positive minor axis 68 | x = self.center[0] 69 | y = self.center[1] + self.minor_axis 70 | z = self.center[2] 71 | elif critical_point_index == 2: # Negative major axis 72 | x = self.center[0] - self.major_axis 73 | y = self.center[1] 74 | z = self.center[2] 75 | elif critical_point_index == 3: # Negative minor axis 76 | x = self.center[0] 77 | y = self.center[1] - self.minor_axis 78 | z = self.center[2] 79 | 80 | elif self.plane == 'yz': 81 | if critical_point_index == 0: # Positive major axis 82 | x = self.center[0] 83 | y = self.center[1] + self.major_axis 84 | z = self.center[2] 85 | elif critical_point_index == 1: # Positive minor axis 86 | x = self.center[0] 87 | y = self.center[1] 88 | z = self.center[2] + self.minor_axis 89 | elif critical_point_index == 2: # Negative major axis 90 | x = self.center[0] 91 | y = self.center[1] - self.major_axis 92 | z = self.center[2] 93 | elif critical_point_index == 3: # Negative minor axis 94 | x = self.center[0] 95 | y = self.center[1] 96 | z = self.center[2] - self.minor_axis 97 | 98 | elif self.plane == 'xz': 99 | if critical_point_index == 0: # Positive major axis 100 | x = self.center[0] + self.major_axis 101 | y = self.center[1] 102 | z = self.center[2] 103 | elif critical_point_index == 1: # Positive minor axis 104 | x = self.center[0] 105 | y = self.center[1] 106 | z = self.center[2] + self.minor_axis 107 | elif critical_point_index == 2: # Negative major axis 108 | x = self.center[0] - self.major_axis 109 | y = self.center[1] 110 | z = self.center[2] 111 | elif critical_point_index == 3: # Negative minor axis 112 | x = self.center[0] 113 | y = self.center[1] 114 | z = self.center[2] - self.minor_axis 115 | 116 | else: 117 | rospy.logwarn("Invalid plane specified. Defaulting to 'xy'.") 118 | if critical_point_index == 0: # Positive major axis 119 | x = self.center[0] + self.major_axis 120 | y = self.center[1] 121 | z = self.center[2] 122 | elif critical_point_index == 1: # Positive minor axis 123 | x = self.center[0] 124 | y = self.center[1] + self.minor_axis 125 | z = self.center[2] 126 | elif critical_point_index == 2: # Negative major axis 127 | x = self.center[0] - self.major_axis 128 | y = self.center[1] 129 | z = self.center[2] 130 | elif critical_point_index == 3: # Negative minor axis 131 | x = self.center[0] 132 | y = self.center[1] 133 | z = self.center[2] - self.minor_axis 134 | 135 | return x, y, z 136 | 137 | def generate_position(self, time_elapsed): 138 | """Generate the 3D position of the point on the oval.""" 139 | if self.key_points_only_bool: 140 | x, y, z = self.discrete_positions_around_ellipse(time_elapsed) 141 | 142 | else: 143 | angle = 2 * math.pi * self.frequency * time_elapsed 144 | 145 | if self.plane == 'xy': 146 | x = self.center[0] + self.major_axis * math.cos(angle) 147 | y = self.center[1] + self.minor_axis * math.sin(angle) 148 | z = self.center[2] + self.pitch_axis * math.cos(angle) 149 | elif self.plane == 'yz': 150 | x = self.center[0] 151 | y = self.center[1] + self.major_axis * math.cos(angle) 152 | z = self.center[2] + self.minor_axis * math.sin(angle) 153 | elif self.plane == 'xz': 154 | x = self.center[0] + self.major_axis * math.cos(angle) 155 | y = self.center[1] 156 | z = self.center[2] + self.minor_axis * math.sin(angle) 157 | else: 158 | rospy.logwarn("Invalid plane specified. Defaulting to 'xy'.") 159 | x = self.center[0] + self.major_axis * math.cos(angle) 160 | y = self.center[1] + self.minor_axis * math.sin(angle) 161 | z = self.center[2] 162 | 163 | 164 | 165 | return x, y, z 166 | 167 | def generate_orientation(self, time_elapsed): 168 | """ 169 | Generate the orientation as a quaternion tracing an oval. 170 | this function returns a quaternion representing the orientation of the goal pose. 171 | the type of the returned value is a list of 4 floats representing the quaternion in the order [x, y, z, w]. 172 | """ 173 | angle = 2 * math.pi * self.orientation_frequency * time_elapsed 174 | 175 | if self.use_rotation: 176 | roll = np.pi #keep roll constant 177 | pitch = self.orientation_major_axis * math.cos(angle) # Oval in pitch 178 | yaw = self.orientation_minor_axis * math.sin(angle) # Oval in yaw 179 | else: 180 | if self.robot == "panda" or self.robot == "xarm": 181 | roll = np.pi 182 | pitch = 0 #self.orientation_major_axis 183 | yaw = 0 #self.orientation_minor_axis 184 | elif self.robot == "kinova": 185 | roll = np.pi/2 186 | pitch = 0 187 | yaw = np.pi/2 188 | 189 | # Generate quaternion from roll, pitch, yaw 190 | quaternion = quaternion_from_euler(roll, pitch, yaw) 191 | return quaternion 192 | 193 | def publish_marker(self, position, orientation): 194 | """Publish the current goal position and orientation as RViz markers representing axes.""" 195 | # Convert quaternion to rotation matrix 196 | rotation_matrix = quaternion_matrix(orientation) 197 | 198 | # Define axis vectors (1 unit length) 199 | x_axis = rotation_matrix[:3, 0] # Red 200 | y_axis = rotation_matrix[:3, 1] # Green 201 | z_axis = rotation_matrix[:3, 2] # Blue 202 | 203 | # Publish arrows for each axis 204 | self.publish_arrow(position, x_axis, [1.0, 0.0, 0.0, 1.0], 0) # Red 205 | self.publish_arrow(position, y_axis, [0.0, 1.0, 0.0, 1.0], 1) # Green 206 | self.publish_arrow(position, z_axis, [0.0, 0.0, 1.0, 1.0], 2) # Blue 207 | 208 | def publish_arrow(self, position, axis_vector, color, marker_id): 209 | """Publish an arrow marker for a given axis.""" 210 | marker = Marker() 211 | marker.header.frame_id = self.base_frame 212 | marker.header.stamp = rospy.Time.now() 213 | marker.ns = "se3_trajectory" 214 | marker.id = marker_id 215 | marker.type = Marker.ARROW 216 | marker.action = Marker.ADD 217 | 218 | # Arrow start and end points 219 | marker.points = [] 220 | start_point = Pose().position 221 | start_point.x = position[0] 222 | start_point.y = position[1] 223 | start_point.z = position[2] 224 | end_point = Pose().position 225 | end_point.x = position[0] + self.arrow_scale*axis_vector[0] 226 | end_point.y = position[1] + self.arrow_scale*axis_vector[1] 227 | end_point.z = position[2] + self.arrow_scale*axis_vector[2] 228 | marker.points.append(start_point) 229 | marker.points.append(end_point) 230 | 231 | # Arrow properties 232 | marker.scale.x = 0.02 # Shaft diameter 233 | marker.scale.y = 0.05 # Head diameter 234 | marker.scale.z = 0.05 # Head length 235 | marker.color.r = color[0] 236 | marker.color.g = color[1] 237 | marker.color.b = color[2] 238 | marker.color.a = color[3] 239 | 240 | self.marker_pub.publish(marker) 241 | 242 | def publish_pose(self, position, orientation): 243 | """Publish the current goal position and orientation as a geometry_msgs/Pose message.""" 244 | pose = PoseStamped() 245 | pose.header.frame_id = self.base_frame 246 | pose.header.stamp = rospy.Time.now() 247 | pose.pose.position.x = position[0] 248 | pose.pose.position.y = position[1] 249 | pose.pose.position.z = position[2] 250 | pose.pose.orientation.x = orientation[0] 251 | pose.pose.orientation.y = orientation[1] 252 | pose.pose.orientation.z = orientation[2] 253 | pose.pose.orientation.w = orientation[3] 254 | 255 | self.pose_pub.publish(pose) 256 | 257 | def trajectory_loop(self): 258 | """Main loop for trajectory generation and marker/pose publishing.""" 259 | rate = rospy.Rate(50) # 50 Hz control loop 260 | 261 | while not rospy.is_shutdown(): 262 | # Calculate elapsed time 263 | elapsed_time = (rospy.Time.now() - self.start_time).to_sec() 264 | 265 | # Generate position and orientation 266 | position = self.generate_position(elapsed_time) 267 | orientation = self.generate_orientation(elapsed_time) 268 | 269 | # Publish the marker and pose 270 | self.publish_marker(position, orientation) 271 | self.publish_pose(position, orientation) 272 | 273 | # Sleep to maintain loop rate 274 | rate.sleep() 275 | 276 | 277 | if __name__ == '__main__': 278 | try: 279 | SE3Trajectory() 280 | except rospy.ROSInterruptException: 281 | rospy.loginfo("SE(3) trajectory generator node terminated.") -------------------------------------------------------------------------------- /manipulator_control/scripts/spacemouse.py: -------------------------------------------------------------------------------- 1 | """Driver class for SpaceMouse controller. 2 | 3 | This class provides a driver support to SpaceMouse on macOS. 4 | In particular, we assume you are using a SpaceMouse Wireless by default. 5 | 6 | To set up a new SpaceMouse controller: 7 | 1. Download and install driver from https://www.3dconnexion.com/service/drivers.html 8 | 2. Install hidapi library through pip 9 | (make sure you run uninstall hid first if it is installed). 10 | 3. Make sure SpaceMouse is connected before running the script 11 | 4. (Optional) Based on the model of SpaceMouse, you might need to change the 12 | vendor id and product id that correspond to the device. 13 | 14 | For Linux support, you can find open-source Linux drivers and SDKs online. 15 | See http://spacenav.sourceforge.net/ 16 | 17 | """ 18 | 19 | import threading 20 | import time 21 | from collections import namedtuple 22 | 23 | import numpy as np 24 | import math 25 | 26 | try: 27 | import hid 28 | except ModuleNotFoundError as exc: 29 | raise ImportError( 30 | "Unable to load module hid, required to interface with SpaceMouse. " 31 | "Only macOS is officially supported. Install the additional " 32 | "requirements with `pip install -r requirements-extra.txt`" 33 | ) from exc 34 | 35 | from device import Device 36 | #from robosuite.utils.transform_utils import rotation_matrix 37 | 38 | AxisSpec = namedtuple("AxisSpec", ["channel", "byte1", "byte2", "scale"]) 39 | 40 | SPACE_MOUSE_SPEC = { 41 | "x": AxisSpec(channel=1, byte1=1, byte2=2, scale=1), 42 | "y": AxisSpec(channel=1, byte1=3, byte2=4, scale=-1), 43 | "z": AxisSpec(channel=1, byte1=5, byte2=6, scale=-1), 44 | "roll": AxisSpec(channel=1, byte1=7, byte2=8, scale=-1), 45 | "pitch": AxisSpec(channel=1, byte1=9, byte2=10, scale=-1), 46 | "yaw": AxisSpec(channel=1, byte1=11, byte2=12, scale=1), 47 | } 48 | 49 | def unit_vector(data, axis=None, out=None): 50 | """ 51 | Returns ndarray normalized by length, i.e. eucledian norm, along axis. 52 | 53 | E.g.: 54 | >>> v0 = numpy.random.random(3) 55 | >>> v1 = unit_vector(v0) 56 | >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 57 | True 58 | 59 | >>> v0 = numpy.random.rand(5, 4, 3) 60 | >>> v1 = unit_vector(v0, axis=-1) 61 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 62 | >>> numpy.allclose(v1, v2) 63 | True 64 | 65 | >>> v1 = unit_vector(v0, axis=1) 66 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 67 | >>> numpy.allclose(v1, v2) 68 | True 69 | 70 | >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float32) 71 | >>> unit_vector(v0, axis=1, out=v1) 72 | >>> numpy.allclose(v1, v2) 73 | True 74 | 75 | >>> list(unit_vector([])) 76 | [] 77 | 78 | >>> list(unit_vector([1.0])) 79 | [1.0] 80 | 81 | Args: 82 | data (np.array): data to normalize 83 | axis (None or int): If specified, determines specific axis along data to normalize 84 | out (None or np.array): If specified, will store computation in this variable 85 | 86 | Returns: 87 | None or np.array: If @out is not specified, will return normalized vector. Otherwise, stores the output in @out 88 | """ 89 | if out is None: 90 | data = np.array(data, dtype=np.float32, copy=True) 91 | if data.ndim == 1: 92 | data /= math.sqrt(np.dot(data, data)) 93 | return data 94 | else: 95 | if out is not data: 96 | out[:] = np.array(data, copy=False) 97 | data = out 98 | length = np.atleast_1d(np.sum(data * data, axis)) 99 | np.sqrt(length, length) 100 | if axis is not None: 101 | length = np.expand_dims(length, axis) 102 | data /= length 103 | if out is None: 104 | return data 105 | 106 | def rotation_matrix(angle, direction, point=None): 107 | """ 108 | Returns matrix to rotate about axis defined by point and direction. 109 | 110 | E.g.: 111 | >>> angle = (random.random() - 0.5) * (2*math.pi) 112 | >>> direc = numpy.random.random(3) - 0.5 113 | >>> point = numpy.random.random(3) - 0.5 114 | >>> R0 = rotation_matrix(angle, direc, point) 115 | >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) 116 | >>> is_same_transform(R0, R1) 117 | True 118 | 119 | >>> R0 = rotation_matrix(angle, direc, point) 120 | >>> R1 = rotation_matrix(-angle, -direc, point) 121 | >>> is_same_transform(R0, R1) 122 | True 123 | 124 | >>> I = numpy.identity(4, numpy.float32) 125 | >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) 126 | True 127 | 128 | >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, 129 | ... direc, point))) 130 | True 131 | 132 | Args: 133 | angle (float): Magnitude of rotation 134 | direction (np.array): (ax,ay,az) axis about which to rotate 135 | point (None or np.array): If specified, is the (x,y,z) point about which the rotation will occur 136 | 137 | Returns: 138 | np.array: 4x4 homogeneous matrix that includes the desired rotation 139 | """ 140 | sina = math.sin(angle) 141 | cosa = math.cos(angle) 142 | direction = unit_vector(direction[:3]) 143 | # rotation matrix around unit vector 144 | R = np.array(((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=np.float32) 145 | R += np.outer(direction, direction) * (1.0 - cosa) 146 | direction *= sina 147 | R += np.array( 148 | ( 149 | (0.0, -direction[2], direction[1]), 150 | (direction[2], 0.0, -direction[0]), 151 | (-direction[1], direction[0], 0.0), 152 | ), 153 | dtype=np.float32, 154 | ) 155 | M = np.identity(4) 156 | M[:3, :3] = R 157 | if point is not None: 158 | # rotation not around origin 159 | point = np.array(point[:3], dtype=np.float32, copy=False) 160 | M[:3, 3] = point - np.dot(R, point) 161 | return M 162 | 163 | def to_int16(y1, y2): 164 | """ 165 | Convert two 8 bit bytes to a signed 16 bit integer. 166 | 167 | Args: 168 | y1 (int): 8-bit byte 169 | y2 (int): 8-bit byte 170 | 171 | Returns: 172 | int: 16-bit integer 173 | """ 174 | x = (y1) | (y2 << 8) 175 | if x >= 32768: 176 | x = -(65536 - x) 177 | return x 178 | 179 | 180 | def scale_to_control(x, axis_scale=350.0, min_v=-1.0, max_v=1.0): 181 | """ 182 | Normalize raw HID readings to target range. 183 | 184 | Args: 185 | x (int): Raw reading from HID 186 | axis_scale (float): (Inverted) scaling factor for mapping raw input value 187 | min_v (float): Minimum limit after scaling 188 | max_v (float): Maximum limit after scaling 189 | 190 | Returns: 191 | float: Clipped, scaled input from HID 192 | """ 193 | x = x / axis_scale 194 | x = min(max(x, min_v), max_v) 195 | return x 196 | 197 | 198 | def convert(b1, b2): 199 | """ 200 | Converts SpaceMouse message to commands. 201 | 202 | Args: 203 | b1 (int): 8-bit byte 204 | b2 (int): 8-bit byte 205 | 206 | Returns: 207 | float: Scaled value from Spacemouse message 208 | """ 209 | return scale_to_control(to_int16(b1, b2)) 210 | 211 | 212 | class SpaceMouse(Device): 213 | """ 214 | A minimalistic driver class for SpaceMouse with HID library. 215 | 216 | Note: Use hid.enumerate() to view all USB human interface devices (HID). 217 | Make sure SpaceMouse is detected before running the script. 218 | You can look up its vendor/product id from this method. 219 | 220 | Args: 221 | vendor_id (int): HID device vendor id 222 | product_id (int): HID device product id 223 | pos_sensitivity (float): Magnitude of input position command scaling 224 | rot_sensitivity (float): Magnitude of scale input rotation commands scaling 225 | """ 226 | 227 | def __init__( 228 | self, 229 | vendor_id=9583, 230 | product_id= 50741, 231 | pos_sensitivity=1.0, 232 | rot_sensitivity=1.0, 233 | ): 234 | 235 | print("Opening SpaceMouse device") 236 | self.vendor_id = vendor_id 237 | self.product_id = product_id 238 | self.device = hid.device() 239 | self.device.open(self.vendor_id, self.product_id) # SpaceMouse 240 | 241 | self.pos_sensitivity = pos_sensitivity 242 | self.rot_sensitivity = rot_sensitivity 243 | 244 | print("Manufacturer: %s" % self.device.get_manufacturer_string()) 245 | print("Product: %s" % self.device.get_product_string()) 246 | 247 | # 6-DOF variables 248 | self.x, self.y, self.z = 0, 0, 0 249 | self.roll, self.pitch, self.yaw = 0, 0, 0 250 | 251 | self._display_controls() 252 | 253 | self.single_click_and_hold = False 254 | 255 | self._control = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 256 | self._reset_state = 0 257 | self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]]) 258 | self._enabled = False 259 | 260 | # launch a new listener thread to listen to SpaceMouse 261 | self.thread = threading.Thread(target=self.run) 262 | self.thread.daemon = True 263 | self.thread.start() 264 | 265 | @staticmethod 266 | def _display_controls(): 267 | """ 268 | Method to pretty print controls. 269 | """ 270 | 271 | def print_command(char, info): 272 | char += " " * (30 - len(char)) 273 | print("{}\t{}".format(char, info)) 274 | 275 | print("") 276 | print_command("Control", "Command") 277 | print_command("Right button", "reset simulation") 278 | print_command("Left button (hold)", "close gripper") 279 | print_command("Move mouse laterally", "move arm horizontally in x-y plane") 280 | print_command("Move mouse vertically", "move arm vertically") 281 | print_command("Twist mouse about an axis", "rotate arm about a corresponding axis") 282 | print("") 283 | 284 | def _reset_internal_state(self): 285 | """ 286 | Resets internal state of controller, except for the reset signal. 287 | """ 288 | self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]]) 289 | # Reset 6-DOF variables 290 | self.x, self.y, self.z = 0, 0, 0 291 | self.roll, self.pitch, self.yaw = 0, 0, 0 292 | # Reset control 293 | self._control = np.zeros(6) 294 | # Reset grasp 295 | self.single_click_and_hold = False 296 | 297 | def start_control(self): 298 | """ 299 | Method that should be called externally before controller can 300 | start receiving commands. 301 | """ 302 | self._reset_internal_state() 303 | self._reset_state = 0 304 | self._enabled = True 305 | 306 | def get_controller_state(self): 307 | """ 308 | Grabs the current state of the 3D mouse. 309 | 310 | Returns: 311 | dict: A dictionary containing dpos, orn, unmodified orn, grasp, and reset 312 | """ 313 | dpos = self.control[:3] * 0.005 * self.pos_sensitivity 314 | roll, pitch, yaw = self.control[3:] * 0.005 * self.rot_sensitivity 315 | 316 | # convert RPY to an absolute orientation 317 | drot1 = rotation_matrix(angle=-pitch, direction=[1.0, 0, 0], point=None)[:3, :3] 318 | drot2 = rotation_matrix(angle=roll, direction=[0, 1.0, 0], point=None)[:3, :3] 319 | drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.0], point=None)[:3, :3] 320 | 321 | self.rotation = self.rotation.dot(drot1.dot(drot2.dot(drot3))) 322 | 323 | return dict( 324 | dpos=dpos, 325 | rotation=self.rotation, 326 | raw_drotation=np.array([roll, pitch, yaw]), 327 | grasp=self.control_gripper, 328 | reset=self._reset_state, 329 | ) 330 | 331 | def run(self): 332 | """Listener method that keeps pulling new messages.""" 333 | 334 | t_last_click = -1 335 | 336 | while True: 337 | d = self.device.read(13) 338 | if d is not None and self._enabled: 339 | 340 | if self.product_id == 50741: 341 | ## logic for older spacemouse model 342 | 343 | if d[0] == 1: ## readings from 6-DoF sensor 344 | self.y = convert(d[1], d[2]) 345 | self.x = convert(d[3], d[4]) 346 | self.z = convert(d[5], d[6]) * -1.0 347 | 348 | elif d[0] == 2: 349 | 350 | self.roll = convert(d[1], d[2]) 351 | self.pitch = convert(d[3], d[4]) 352 | self.yaw = convert(d[5], d[6]) 353 | 354 | self._control = [ 355 | self.x, 356 | self.y, 357 | self.z, 358 | self.roll, 359 | self.pitch, 360 | self.yaw, 361 | ] 362 | else: 363 | ## default logic for all other spacemouse models 364 | 365 | if d[0] == 1: ## readings from 6-DoF sensor 366 | self.y = convert(d[1], d[2]) 367 | self.x = convert(d[3], d[4]) 368 | self.z = convert(d[5], d[6]) * -1.0 369 | 370 | self.roll = convert(d[7], d[8]) 371 | self.pitch = convert(d[9], d[10]) 372 | self.yaw = convert(d[11], d[12]) 373 | 374 | self._control = [ 375 | self.x, 376 | self.y, 377 | self.z, 378 | self.roll, 379 | self.pitch, 380 | self.yaw, 381 | ] 382 | 383 | if d[0] == 3: ## readings from the side buttons 384 | 385 | # press left button 386 | if d[1] == 1: 387 | t_click = time.time() 388 | elapsed_time = t_click - t_last_click 389 | t_last_click = t_click 390 | self.single_click_and_hold = True 391 | 392 | # release left button 393 | if d[1] == 0: 394 | self.single_click_and_hold = False 395 | 396 | # right button is for reset 397 | if d[1] == 2: 398 | self._reset_state = 1 399 | self._enabled = False 400 | self._reset_internal_state() 401 | 402 | @property 403 | def control(self): 404 | """ 405 | Grabs current pose of Spacemouse 406 | 407 | Returns: 408 | np.array: 6-DoF control value 409 | """ 410 | return np.array(self._control) 411 | 412 | @property 413 | def control_gripper(self): 414 | """ 415 | Maps internal states into gripper commands. 416 | 417 | Returns: 418 | float: Whether we're using single click and hold or not 419 | """ 420 | if self.single_click_and_hold: 421 | return 1.0 422 | return 0 423 | 424 | 425 | if __name__ == "__main__": 426 | 427 | space_mouse = SpaceMouse() 428 | print(space_mouse.get_controller_state()) 429 | 430 | while True: 431 | time.sleep(1) 432 | print(space_mouse.get_controller_state()) -------------------------------------------------------------------------------- /manipulator_control/scripts/xarm_spacemouse.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from spacemouse import SpaceMouse 4 | from input2action import input2action 5 | import numpy as np 6 | 7 | from sensor_msgs.msg import JointState 8 | 9 | from xarm.wrapper import XArmAPI 10 | from std_msgs.msg import Bool 11 | from geometry_msgs.msg import TwistStamped, PoseStamped 12 | from scipy.spatial.transform import Rotation 13 | 14 | def euler_to_quaternion(roll, pitch, yaw): 15 | rot = Rotation.from_euler('xyz', [roll, pitch, yaw]) 16 | return rot.as_quat() 17 | 18 | class Spacemouse2Xarm: 19 | def __init__(self): 20 | rospy.init_node('spacemouse2xarm') 21 | ip = rospy.get_param('~robot_ip', '192.168.1.233') 22 | 23 | self.arm = XArmAPI(ip) 24 | self.arm.motion_enable(enable=True) 25 | self.arm.set_mode(1) 26 | self.arm.set_state(0) 27 | self.device = SpaceMouse(pos_sensitivity=1.0, rot_sensitivity=1.0) 28 | self.device.start_control() 29 | self.locked = False 30 | self.last_grasp_press_time = rospy.Time.now().to_nsec() 31 | 32 | # publisher for pose and twist action to send to JParse controller 33 | self.action_pub = rospy.Publisher('robot_action', TwistStamped, queue_size=10) 34 | self.position_action_pub = rospy.Publisher('robot_position_action', PoseStamped, queue_size=10) 35 | self.reset_sub = rospy.Subscriber('reset_xarm', Bool, self.reset_callback) 36 | self.is_resetting = False 37 | self.lock_hand = rospy.Publisher('lock_hand', Bool, queue_size=10) 38 | 39 | self.timer = rospy.Timer(rospy.Duration(1/30.0), self.timer_callback) 40 | 41 | # reset 42 | self.is_resetting = True 43 | self.arm.motion_enable(enable=True) 44 | self.arm.set_mode(0) 45 | self.arm.set_state(0) 46 | self.arm.set_position(x=331, y=20.3, z=308, roll=173, pitch=0, yaw=0, 47 | speed=100, is_radian=False, wait=True) 48 | self.arm.motion_enable(enable=True) 49 | 50 | self.arm.set_mode(5) 51 | self.arm.set_state(0) 52 | self.is_resetting = False 53 | 54 | 55 | self.joint_states_pub = rospy.Publisher('/joint_states', JointState, queue_size=10) 56 | 57 | self.use_native_xarm_spacemouse = rospy.get_param('~use_native_xarm_spacemouse', False) 58 | 59 | 60 | 61 | def reset_callback(self, msg): 62 | """ 63 | a reset callback function to reset the robot to a predefined position 64 | """ 65 | if msg.data: 66 | self.is_resetting = True 67 | self.arm.motion_enable(enable=True) 68 | self.arm.set_mode(0) 69 | self.arm.set_state(0) 70 | self.arm.set_position(x=331, y=20.3, z=308, roll=173, pitch=0, yaw=0, 71 | speed=100, is_radian=False, wait=True) 72 | self.arm.motion_enable(enable=True) 73 | self.arm.set_mode(1) 74 | self.arm.set_state(0) 75 | self.is_resetting = False 76 | 77 | def action_to_cartesian_velocity(self, actions): 78 | """ 79 | converts space mouse actions to cartesian velocities 80 | """ 81 | if actions is None: 82 | return 0, 0, 0, 0, 0, 0 83 | 84 | dt = 1000 / 30.0 85 | scale = 100 86 | ang_scale = 1 87 | vx = -scale * actions[0] / dt 88 | vy = -scale * actions[1] / dt 89 | vz = scale * actions[2] / dt 90 | wx = -ang_scale * actions[3] 91 | wy = -ang_scale * actions[4] 92 | wz = ang_scale * actions[5] 93 | return vx, vy, vz, wx, wy, wz 94 | 95 | def timer_callback(self, event): 96 | cur_pose = self.arm.get_position()[1] 97 | cur_pose = np.array(cur_pose) 98 | actions, grasp = input2action(device=self.device, robot="xArm") 99 | 100 | if grasp is None: 101 | grasp = 0 102 | 103 | grasp = 850 - 860 * grasp 104 | 105 | # get joint states 106 | status, joint_states = self.arm.get_joint_states(is_radian=True) 107 | joint_pos = joint_states[0] 108 | joint_vel = joint_states[1] 109 | joint_torque = joint_states[2] 110 | 111 | 112 | vx, vy, vz, wx, wy, wz = self.action_to_cartesian_velocity(actions) 113 | cur_pose = cur_pose + np.array([vx, vy, vz, wx, wy, wz]) 114 | action_pose_msg = PoseStamped() 115 | action_pose_msg.pose.position.x = cur_pose[0] 116 | action_pose_msg.pose.position.y = cur_pose[1] 117 | action_pose_msg.pose.position.z = cur_pose[2] 118 | roll, pitch, yaw = cur_pose[3], cur_pose[4], cur_pose[5] 119 | q = euler_to_quaternion(roll, pitch, yaw) 120 | action_pose_msg.pose.orientation.x = q[0] 121 | action_pose_msg.pose.orientation.y = q[1] 122 | action_pose_msg.pose.orientation.z = q[2] 123 | action_pose_msg.pose.orientation.w = q[3] 124 | action_pose_msg.header.stamp = rospy.Time.now() 125 | 126 | self.position_action_pub.publish(action_pose_msg) 127 | twist_msg = TwistStamped() 128 | twist_msg.twist.linear.x = vx 129 | twist_msg.twist.linear.y = vy 130 | twist_msg.twist.linear.z = vz 131 | twist_msg.twist.angular.x = wx 132 | twist_msg.twist.angular.y = wy 133 | twist_msg.twist.angular.z = wz 134 | twist_msg.header.stamp = rospy.Time.now() 135 | 136 | 137 | position_velocity = np.array([vx, vy, vz]) 138 | position_velocity_norm = np.linalg.norm(position_velocity) 139 | if position_velocity_norm > 0.05: 140 | position_velocity = position_velocity / (position_velocity_norm * 0.05) 141 | 142 | vx, vy, vz = position_velocity 143 | 144 | self.action_pub.publish(twist_msg) 145 | current_time_ns = rospy.Time.now().to_nsec() 146 | 147 | 148 | # grasp control 149 | if grasp == 1: 150 | if (current_time_ns - self.last_grasp_press_time) > 5e8: 151 | self.last_grasp_press_time = current_time_ns 152 | bool_msg = Bool() 153 | bool_msg.data = not self.locked 154 | self.locked = bool_msg.data 155 | self.lock_hand.publish(bool_msg) 156 | if not self.is_resetting: 157 | v_scaling = 1000 158 | vx = vx * v_scaling 159 | vy = vy * v_scaling 160 | vz = vz * v_scaling 161 | if self.use_native_xarm_spacemouse: 162 | # use the native xarm cartesian velocity controller. 163 | self.arm.vc_set_cartesian_velocity([vx, vy, vz, wx, wy, wz], is_radian=True) 164 | ret_grip = self.arm.set_gripper_position(grasp) 165 | 166 | def spin(self): 167 | rospy.spin() 168 | self.arm.disconnect() 169 | 170 | if __name__ == '__main__': 171 | node = Spacemouse2Xarm() 172 | node.spin() 173 | -------------------------------------------------------------------------------- /manipulator_control/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['manipulator_control'], 6 | package_dir={'': 'src'} 7 | ) 8 | 9 | setup(**d) -------------------------------------------------------------------------------- /manipulator_control/src/jparse.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include // for std::stringstream 4 | 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | 36 | class JPARSE 37 | { 38 | public: 39 | JPARSE(ros::NodeHandle& nh); 40 | void Jnt_state_callback(const sensor_msgs::JointStateConstPtr& msg); 41 | void Jparse_calculation(const Eigen::MatrixXd& J, Eigen::MatrixXd& J_parse, Eigen::MatrixXd& J_safety_nullspace, std::vector& jparse_singular_index, Eigen::MatrixXd& U_safety, Eigen::VectorXd& S_new_safety, Eigen::MatrixXd& U_new_proj, Eigen::VectorXd& S_new_proj, Eigen::MatrixXd& U_new_sing, Eigen::VectorXd& Phi, double& gamma, double& singular_direction_gain_position, double& singular_direction_gain_angular); 42 | void Publish_JPARSE(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace); 43 | void JPARSE_visualization(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace, const std::vector& jparse_singular_index, const Eigen::MatrixXd& U_safety, const Eigen::VectorXd& S_new_safety, const Eigen::MatrixXd& U_new_proj, const Eigen::VectorXd& S_new_proj, const Eigen::MatrixXd& U_new_sing, const Eigen::VectorXd& Phi); 44 | void matrix_to_msg(const Eigen::MatrixXd& mat, std::vector& msg_rows); 45 | 46 | Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& J, double tol = 1e-6); 47 | 48 | bool handleJparse( 49 | manipulator_control::JParseSrv::Request& req, 50 | manipulator_control::JParseSrv::Response& resp 51 | ); 52 | 53 | private: 54 | ros::NodeHandle nh_; 55 | ros::Subscriber joint_state_sub_; 56 | ros::Publisher jparse_pub_; 57 | ros::Publisher jparse_markers_pub_; 58 | std::string root_, tip_; 59 | 60 | urdf::Model model_; 61 | KDL::Tree kdl_tree_; 62 | KDL::Chain kdl_chain_; 63 | KDL::Chain kdl_chain_service_; //for service option 64 | boost::shared_ptr kdl_chain_dynamics_; 65 | boost::shared_ptr jac_solver_; 66 | boost::shared_ptr jac_solver_service_; //for service option 67 | 68 | KDL::JntArrayVel positions_; 69 | std::vector joint_names_; 70 | 71 | tf2_ros::Buffer tfBuffer; 72 | std::unique_ptr tfListener; 73 | 74 | double gamma_; //Jparse threshold gamma 75 | double singular_direction_gain_position_; 76 | double singular_direction_gain_angular_; 77 | 78 | //for service option 79 | bool have_last_msg_ = false; 80 | std::mutex last_msg_mutex_; 81 | sensor_msgs::JointStateConstPtr joint_state_msg_service_; 82 | ros::ServiceServer jparse_service_; 83 | 84 | }; 85 | 86 | JPARSE::JPARSE(ros::NodeHandle& nh) : nh_(nh) 87 | { 88 | 89 | ros::NodeHandle pnh("~"); 90 | 91 | bool run_as_service = false; 92 | pnh.param("run_as_service", run_as_service, false); 93 | 94 | // Always subscribe, so we cache last_J_ 95 | joint_state_sub_ = nh_.subscribe("joint_states", 1, 96 | &JPARSE::Jnt_state_callback, this); 97 | 98 | if (run_as_service) 99 | { 100 | // advertise the service 101 | jparse_service_ = nh_.advertiseService("jparse_srv", 102 | &JPARSE::handleJparse, this); 103 | ROS_INFO("JPARSE: running as service 'jparse_srv'"); 104 | } 105 | 106 | pnh.param("base_link_name", root_, "base_link"); 107 | pnh.param("end_link_name", tip_, "end_effector_link"); 108 | 109 | nh_.param("jparse_gamma", gamma_, 0.2); 110 | nh_.param("singular_direction_gain_position", singular_direction_gain_position_, 1.0); 111 | nh_.param("singular_direction_gain_angular", singular_direction_gain_angular_, 1.0); 112 | 113 | if (!model_.initParam("/robot_description") || 114 | !kdl_parser::treeFromUrdfModel(model_, kdl_tree_)) 115 | { 116 | ROS_ERROR("Failed to load /robot_description or build KDL tree"); 117 | return; 118 | } 119 | 120 | //for getting the end-effector pose 121 | tfBuffer.setUsingDedicatedThread(true); 122 | tfListener.reset(new tf2_ros::TransformListener(tfBuffer)); 123 | 124 | if (!kdl_tree_.getChain(root_, tip_, kdl_chain_)) 125 | { 126 | ROS_ERROR("Failed to extract KDL chain from %s to %s", root_.c_str(), tip_.c_str()); 127 | return; 128 | } 129 | 130 | kdl_chain_dynamics_.reset(new KDL::ChainDynParam(kdl_chain_, KDL::Vector(0, 0, -9.81))); 131 | jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_)); 132 | positions_ = KDL::JntArrayVel(kdl_chain_.getNrOfJoints()); 133 | 134 | KDL::SetToZero(positions_.q); 135 | KDL::SetToZero(positions_.qdot); 136 | 137 | for (size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i) 138 | { 139 | KDL::Joint joint = kdl_chain_.getSegment(i).getJoint(); 140 | if (joint.getType() != KDL::Joint::None) 141 | joint_names_.push_back(joint.getName()); 142 | } 143 | 144 | joint_state_sub_ = nh_.subscribe("joint_states", 1, &JPARSE::Jnt_state_callback, this); 145 | jparse_pub_ = nh_.advertise("jparse_output", 1); 146 | jparse_markers_pub_ = nh_.advertise("/jparse_ellipsoid_marker_cpp", 1); 147 | } 148 | 149 | void JPARSE::Jnt_state_callback(const sensor_msgs::JointStateConstPtr& msg) 150 | { 151 | std::vector q, dq; 152 | for (const auto& joint_name : joint_names_) 153 | { 154 | auto it = std::find(msg->name.begin(), msg->name.end(), joint_name); 155 | if (it != msg->name.end()) 156 | { 157 | size_t idx = std::distance(msg->name.begin(), it); 158 | q.push_back(msg->position[idx]); 159 | dq.push_back(msg->velocity[idx]); 160 | } 161 | } 162 | 163 | if (q.size() != joint_names_.size()) return; 164 | 165 | for (size_t i = 0; i < joint_names_.size(); ++i) 166 | { 167 | positions_.q(i) = q[i]; 168 | positions_.qdot(i) = dq[i]; 169 | } 170 | 171 | KDL::Jacobian J_kdl(joint_names_.size()); 172 | jac_solver_->JntToJac(positions_.q, J_kdl); 173 | Eigen::MatrixXd J = J_kdl.data; 174 | 175 | //store a copy for the service 176 | { 177 | std::lock_guard guard(last_msg_mutex_); 178 | joint_state_msg_service_ = msg; 179 | have_last_msg_ = true; 180 | } 181 | 182 | Eigen::MatrixXd J_parse, J_safety_nullspace; 183 | //handle variable size kinematic chain 184 | Eigen::JacobiSVD svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); 185 | int n = svd.singularValues().size(); 186 | std::vector jparse_singular_index(n, 0); // Elements in this list are 0 if non-singular, 1 if singular 187 | Eigen::MatrixXd U_safety, U_new_proj, U_new_sing; 188 | Eigen::VectorXd S_new_safety, S_new_proj, Phi; 189 | 190 | Jparse_calculation(J, J_parse, J_safety_nullspace, jparse_singular_index, U_safety, S_new_safety, U_new_proj, S_new_proj, U_new_sing, Phi, gamma_, singular_direction_gain_position_, singular_direction_gain_angular_); 191 | Publish_JPARSE(msg->header, J_parse, J_safety_nullspace); 192 | 193 | Eigen::MatrixXd J_position = J.block(0, 0, 3, J.cols()); //extract J_v 194 | Eigen::MatrixXd J_parse_position, J_safety_nullspace_position; 195 | 196 | //handle variable size kinematic chain 197 | Eigen::JacobiSVD svd_position(J_position, Eigen::ComputeFullU | Eigen::ComputeFullV); 198 | int n_position = svd_position.singularValues().size(); 199 | std::vector jparse_singular_index_position(n_position, 0); // Elements in this list are 0 if non-singular, 1 if singular 200 | Eigen::MatrixXd U_safety_position, U_new_proj_position, U_new_sing_position; 201 | Eigen::VectorXd S_new_safety_position, S_new_proj_position, Phi_position; 202 | 203 | Jparse_calculation(J_position, J_parse_position, J_safety_nullspace_position, jparse_singular_index_position, U_safety_position, S_new_safety_position, U_new_proj_position, S_new_proj_position, U_new_sing_position, Phi_position, gamma_, singular_direction_gain_position_, singular_direction_gain_angular_); 204 | JPARSE_visualization(msg->header, J_parse_position, J_safety_nullspace_position, jparse_singular_index_position, U_safety_position, S_new_safety_position, U_new_proj_position, S_new_proj_position, U_new_sing_position, Phi_position); 205 | } 206 | 207 | Eigen::MatrixXd JPARSE::pseudoInverse(const Eigen::MatrixXd& J, double tol) 208 | { 209 | Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); 210 | const Eigen::VectorXd& singularValues = svd.singularValues(); 211 | Eigen::MatrixXd S_pinv = Eigen::MatrixXd::Zero(svd.matrixV().cols(), svd.matrixU().cols()); 212 | 213 | for (int i = 0; i < singularValues.size(); ++i) 214 | { 215 | if (singularValues(i) > tol) 216 | { 217 | S_pinv(i, i) = 1.0 / singularValues(i); 218 | } 219 | } 220 | 221 | return svd.matrixV() * S_pinv * svd.matrixU().transpose(); 222 | } 223 | 224 | void JPARSE::Jparse_calculation(const Eigen::MatrixXd& J, Eigen::MatrixXd& J_parse, Eigen::MatrixXd& J_safety_nullspace, std::vector& jparse_singular_index, Eigen::MatrixXd& U_safety, Eigen::VectorXd& S_new_safety, Eigen::MatrixXd& U_new_proj, Eigen::VectorXd& S_new_proj, Eigen::MatrixXd& U_new_sing, Eigen::VectorXd& Phi, double& gamma, double& singular_direction_gain_position, double& singular_direction_gain_angular) 225 | { 226 | /* 227 | Steps are as follows: 228 | 1. Find the SVD of J 229 | 2. Find the adjusted condition number and Jparse singular index 230 | 3. Find the Projection Jacobian 231 | 4. Find the Safety Jacobian 232 | 5. Find the singular direction projection components 233 | 6. Find the pseudo inverse of J_safety and J_proj 234 | 7. Find Jparse 235 | 8. Find the null space of J_safety 236 | */ 237 | 238 | //1. Find the SVD of J 239 | Eigen::MatrixXd U, V; 240 | Eigen::VectorXd S; 241 | Eigen::JacobiSVD svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); 242 | U = svd.matrixU(); 243 | S = svd.singularValues(); 244 | V = svd.matrixV(); 245 | 246 | if (U.rows() == U.cols() && U.determinant() < 0.0) { 247 | int k = U.cols() - 1; // pick the last column 248 | U.col(k) *= -1.0; 249 | V.col(k) *= -1.0; 250 | } 251 | 252 | U_safety = U; //Safety Jacobian shares the same U as the SVD of J 253 | 254 | //2. find the adjusted condition number 255 | double max_singular_value = S.maxCoeff(); 256 | std::vector adjusted_condition_numbers(S.size()); 257 | for (int i = 0; i < S.size(); ++i) 258 | { 259 | adjusted_condition_numbers[i] = S(i) / max_singular_value; 260 | } 261 | 262 | //3. Find the Projection Jacobian 263 | std::vector valid_indices; 264 | 265 | for (int i = 0; i < S.size(); ++i) 266 | { 267 | // keep only the elements whose singular value is greater than the threshold 268 | if (S(i) > gamma * max_singular_value) 269 | { 270 | valid_indices.push_back(i); 271 | }else{ 272 | jparse_singular_index[i] = 1; // set the index to 1 if the singular value is less than the threshold 273 | } 274 | } 275 | 276 | U_new_proj = Eigen::MatrixXd(U.rows(), valid_indices.size()); 277 | S_new_proj = Eigen::VectorXd(valid_indices.size()); 278 | 279 | for (size_t i = 0; i < valid_indices.size(); ++i) 280 | { 281 | U_new_proj.col(i) = U.col(valid_indices[i]); 282 | S_new_proj(i) = S(valid_indices[i]); 283 | } 284 | Eigen::MatrixXd S_new_proj_matrix = Eigen::MatrixXd::Zero(U_new_proj.cols(), V.rows()); 285 | for (int i = 0; i < S_new_proj.size(); ++i) 286 | { 287 | S_new_proj_matrix(i, i) = S_new_proj(i); 288 | } 289 | Eigen::MatrixXd J_proj = U_new_proj * S_new_proj_matrix * V.transpose(); 290 | 291 | //4. Find the Safety Jacobian 292 | S_new_safety = Eigen::VectorXd(S.size()); 293 | for (int i = 0; i < S.size(); ++i) 294 | { 295 | //if the singular value is greater than the threshold, keep it otherwise set it to the threshold 296 | if ((S(i) / max_singular_value) > gamma) 297 | { 298 | S_new_safety(i) = S(i); 299 | } 300 | else 301 | { 302 | S_new_safety(i) = gamma * max_singular_value; 303 | } 304 | } 305 | 306 | 307 | Eigen::MatrixXd S_new_safety_matrix = Eigen::MatrixXd::Zero(U.rows(), V.cols()); 308 | for (int i = 0; i < S_new_safety.size(); ++i) 309 | { 310 | S_new_safety_matrix(i, i) = S_new_safety(i); 311 | } 312 | Eigen::MatrixXd J_safety = U * S_new_safety_matrix * V.transpose(); 313 | 314 | //5. Find the singular direction projection components 315 | Eigen::MatrixXd Phi_matrix, Kp_singular, Phi_singular; 316 | std::vector valid_indices_sing; 317 | bool set_empty_bool = true; // set to true if the valid indices are empty 318 | for (int i = 0; i < S.size(); ++i) 319 | { 320 | if (adjusted_condition_numbers[i] <= gamma) 321 | { 322 | set_empty_bool = false; 323 | valid_indices_sing.push_back(i); 324 | } 325 | } 326 | 327 | U_new_sing = Eigen::MatrixXd(U.rows(), valid_indices_sing.size()); 328 | Phi = Eigen::VectorXd(valid_indices_sing.size()); 329 | 330 | if (set_empty_bool==false) 331 | { 332 | for (size_t i = 0; i < valid_indices_sing.size(); ++i) 333 | { 334 | U_new_sing.col(i) = U.col(valid_indices_sing[i]); 335 | Phi(i) = adjusted_condition_numbers[valid_indices_sing[i]] / gamma; 336 | } 337 | 338 | Phi_matrix = Eigen::MatrixXd::Zero(Phi.size(), Phi.size()); 339 | for (int i = 0; i < Phi.size(); ++i) 340 | { 341 | Phi_matrix(i, i) = Phi(i); 342 | } 343 | 344 | Kp_singular = Eigen::MatrixXd::Zero(U.rows(), U.cols()); 345 | for (int i = 0; i < 3; ++i) 346 | { 347 | Kp_singular(i, i) = singular_direction_gain_position; 348 | } 349 | if (Kp_singular.cols() > 3) //checks if position only (J_v) or full jacobian 350 | { 351 | for (int i = 3; i < 6; ++i) 352 | { 353 | Kp_singular(i, i) = singular_direction_gain_angular; 354 | } 355 | } 356 | Phi_singular = U_new_sing * Phi_matrix * U_new_sing.transpose() * Kp_singular; // put it all together 357 | } 358 | 359 | //6. Find pseudo inverse of J_safety and J_proj 360 | Eigen::MatrixXd J_safety_pinv = pseudoInverse(J_safety); 361 | Eigen::MatrixXd J_proj_pinv = pseudoInverse(J_proj); 362 | 363 | //7. Find the Jparse 364 | if (set_empty_bool==false) 365 | { 366 | J_parse = J_safety_pinv * J_proj * J_proj_pinv + J_safety_pinv * Phi_singular; 367 | } 368 | else 369 | { 370 | J_parse = J_safety_pinv * J_proj * J_proj_pinv; 371 | } 372 | 373 | //8. Find the null space of J_safety 374 | J_safety_nullspace = Eigen::MatrixXd::Identity(J_safety.cols(), J_safety.cols()) - J_safety_pinv * J_safety; 375 | 376 | } 377 | 378 | void JPARSE::JPARSE_visualization(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace, const std::vector& jparse_singular_index, const Eigen::MatrixXd& U_safety, const Eigen::VectorXd& S_new_safety, const Eigen::MatrixXd& U_new_proj, const Eigen::VectorXd& S_new_proj, const Eigen::MatrixXd& U_new_sing, const Eigen::VectorXd& Phi) 379 | { 380 | //This script takes in the J_Parse matricies and visualizes them in RVIZ; this is done for position. 381 | 382 | //Get the end-effector position and orientation 383 | geometry_msgs::TransformStamped transformStamped; 384 | try 385 | { 386 | transformStamped = tfBuffer.lookupTransform(root_, tip_, ros::Time(0)); 387 | } 388 | catch (tf2::TransformException& ex) 389 | { 390 | ROS_WARN("%s", ex.what()); 391 | return; 392 | } 393 | 394 | 395 | //1. create a marker array 396 | visualization_msgs::MarkerArray marker_array; 397 | visualization_msgs::Marker J_safety_marker; 398 | visualization_msgs::Marker J_proj_marker; 399 | visualization_msgs::Marker J_singular_marker; 400 | 401 | //2. Set up the J_safety_marker 402 | J_safety_marker.header = header; 403 | J_safety_marker.header.frame_id = root_; 404 | J_safety_marker.ns = "J_safety"; 405 | J_safety_marker.id = 0; 406 | J_safety_marker.type = visualization_msgs::Marker::SPHERE; 407 | J_safety_marker.action = visualization_msgs::Marker::ADD; 408 | //set the marker pose position to the end effector position 409 | J_safety_marker.pose.position.x = transformStamped.transform.translation.x; 410 | J_safety_marker.pose.position.y = transformStamped.transform.translation.y; 411 | J_safety_marker.pose.position.z = transformStamped.transform.translation.z; 412 | double ellipsoid_scale = 0.25; 413 | 414 | double safety_value_1 = std::max(0.001, S_new_safety(0)); 415 | J_safety_marker.scale.x = ellipsoid_scale * safety_value_1; 416 | 417 | double safety_value_2 = std::max(0.001, S_new_safety(1)); 418 | J_safety_marker.scale.y = ellipsoid_scale * safety_value_2; 419 | 420 | double safety_value_3 = std::max(0.001, S_new_safety(2)); 421 | J_safety_marker.scale.z = ellipsoid_scale * safety_value_3; 422 | 423 | Eigen::Matrix3d R = U_safety.block<3,3>(0, 0); 424 | 425 | 426 | Eigen::Quaterniond q_safety(R); 427 | //normalize the quaternion 428 | q_safety.normalize(); // optional if R is already perfectly orthonormal 429 | J_safety_marker.pose.orientation.x = q_safety.x(); 430 | J_safety_marker.pose.orientation.y = q_safety.y(); 431 | J_safety_marker.pose.orientation.z = q_safety.z(); 432 | J_safety_marker.pose.orientation.w = q_safety.w(); 433 | 434 | J_safety_marker.color.a = 0.7; 435 | J_safety_marker.color.r = 1.0; 436 | J_safety_marker.color.g = 0.0; 437 | J_safety_marker.color.b = 0.0; 438 | 439 | // Add the J_safety_marker to the marker array 440 | marker_array.markers.push_back(J_safety_marker); 441 | 442 | 443 | // Determine if J_proj and J_singular exist 444 | bool J_proj_exists = false; 445 | bool J_singular_exists = false; 446 | int number_of_singular_directions = 0; 447 | for (int i = 0; i < 3; ++i) { 448 | if (jparse_singular_index[i] == 0) { 449 | // some directions are non-singular 450 | J_proj_exists = true; 451 | } else if (jparse_singular_index[i] == 1) { 452 | // some directions are singular 453 | J_singular_exists = true; 454 | number_of_singular_directions++; 455 | } 456 | } 457 | 458 | 459 | 460 | 461 | //2. setup the J_proj_marker if it exists 462 | if(J_proj_exists==true){ 463 | // Set up the J_proj_marker 464 | J_proj_marker.header = header; 465 | J_proj_marker.header.frame_id = root_; 466 | J_proj_marker.ns = "J_proj"; 467 | J_proj_marker.id = 1; 468 | J_proj_marker.type = visualization_msgs::Marker::SPHERE; 469 | J_proj_marker.action = visualization_msgs::Marker::ADD; 470 | 471 | 472 | if(jparse_singular_index[0]==0){ 473 | double proj_value_1 = std::max(0.001, S_new_proj(0)); 474 | J_proj_marker.scale.x = ellipsoid_scale * proj_value_1; 475 | }else{ 476 | J_proj_marker.scale.x = 0.001; 477 | } 478 | 479 | if(jparse_singular_index[1]==0){ 480 | double proj_value_2 = std::max(0.001, S_new_proj(1)); 481 | J_proj_marker.scale.y = ellipsoid_scale * proj_value_2; 482 | }else{ 483 | J_proj_marker.scale.y = 0.001; 484 | } 485 | 486 | if(jparse_singular_index[2]==0){ 487 | double proj_value_3 = std::max(0.001, S_new_proj(2)); 488 | J_proj_marker.scale.z = ellipsoid_scale * proj_value_3; 489 | }else{ 490 | J_proj_marker.scale.z = 0.001; 491 | } 492 | 493 | J_proj_marker.pose.position.x = transformStamped.transform.translation.x; 494 | J_proj_marker.pose.position.y = transformStamped.transform.translation.y; 495 | J_proj_marker.pose.position.z = transformStamped.transform.translation.z; 496 | 497 | Eigen::Matrix3d R_proj = U_safety.block<3,3>(0, 0); 498 | Eigen::Quaterniond q_proj(R_proj); 499 | 500 | //normalize the quaternion 501 | q_proj.normalize(); // optional if R is already perfectly orthonormal 502 | J_proj_marker.pose.orientation.x = q_proj.x(); 503 | J_proj_marker.pose.orientation.y = q_proj.y(); 504 | J_proj_marker.pose.orientation.z = q_proj.z(); 505 | J_proj_marker.pose.orientation.w = q_proj.w(); 506 | 507 | J_proj_marker.color.a = 0.7; 508 | J_proj_marker.color.r = 0.0; 509 | J_proj_marker.color.g = 0.0; 510 | J_proj_marker.color.b = 1.0; 511 | } 512 | 513 | // Add the J_proj_marker to the marker array 514 | if(J_proj_exists==true){ 515 | marker_array.markers.push_back(J_proj_marker); 516 | } 517 | 518 | 519 | //3. setup the J_singular_marker if it exists 520 | 521 | if (J_singular_exists) 522 | { 523 | // Extract end-effector position once 524 | geometry_msgs::Point ee_pos; 525 | ee_pos.x = transformStamped.transform.translation.x; 526 | ee_pos.y = transformStamped.transform.translation.y; 527 | ee_pos.z = transformStamped.transform.translation.z; 528 | 529 | double arrow_scale = 1.0; 530 | 531 | for (int idx = 0; idx < number_of_singular_directions; ++idx) 532 | { 533 | visualization_msgs::Marker marker; 534 | // --- Header & identity --- 535 | marker.header = header; // copy stamp & frame_id 536 | marker.header.frame_id = root_; 537 | marker.ns = "J_singular"; 538 | marker.id = idx + 2; 539 | marker.type = visualization_msgs::Marker::ARROW; 540 | marker.action = visualization_msgs::Marker::ADD; 541 | marker.lifetime = ros::Duration(0.1); 542 | 543 | // --- Start point --- 544 | geometry_msgs::Point start_point = ee_pos; 545 | 546 | // --- Decide arrow direction --- 547 | Eigen::Vector3d u_col = U_new_sing.block<3,1>(0, idx); // first 3 rows of column 548 | double dot = ee_pos.x * u_col.x() 549 | + ee_pos.y * u_col.y() 550 | + ee_pos.z * u_col.z(); 551 | 552 | Eigen::Vector3d arrow_dir = (dot < 0.0 ? u_col : -u_col); 553 | 554 | // --- End point --- 555 | double mag = std::abs(Phi(idx)); 556 | geometry_msgs::Point end_point; 557 | end_point.x = ee_pos.x + arrow_scale * arrow_dir.x() * mag; 558 | end_point.y = ee_pos.y + arrow_scale * arrow_dir.y() * mag; 559 | end_point.z = ee_pos.z + arrow_scale * arrow_dir.z() * mag; 560 | 561 | // --- Push points (clear just in case) --- 562 | marker.points.clear(); 563 | marker.points.push_back(start_point); 564 | marker.points.push_back(end_point); 565 | 566 | // --- Fixed arrow sizing --- 567 | marker.scale.x = 0.01; // shaft diameter 568 | marker.scale.y = 0.05; // head diameter 569 | marker.scale.z = 0.05; // head length 570 | 571 | // --- Color = solid red --- 572 | marker.color.r = 1.0; 573 | marker.color.g = 0.0; 574 | marker.color.b = 0.0; 575 | marker.color.a = 1.0; 576 | 577 | marker_array.markers.push_back(marker); 578 | } 579 | 580 | } 581 | 582 | //publish the marker array 583 | jparse_markers_pub_.publish(marker_array); 584 | } 585 | 586 | bool JPARSE::handleJparse( 587 | manipulator_control::JParseSrv::Request& req, 588 | manipulator_control::JParseSrv::Response& res) 589 | { 590 | // find the jacobian for the joints specified in the request 591 | 592 | std::string root_service = req.base_link_name; 593 | std::string tip_service = req.end_link_name; 594 | std::vector joint_names; 595 | double gamma_service = req.gamma; 596 | double singular_direction_gain_position_service = req.singular_direction_gain_position; 597 | double singular_direction_gain_angular_service = req.singular_direction_gain_angular; 598 | sensor_msgs::JointStateConstPtr msg; 599 | 600 | ROS_INFO("JPARSE service: Received request for base_link: %s, end_link: %s", root_service.c_str(), tip_service.c_str()); 601 | //setup the KDL chain 602 | if (!kdl_tree_.getChain(root_service, tip_service, kdl_chain_service_)) 603 | { 604 | ROS_ERROR("Failed to extract KDL chain from %s to %s", root_service.c_str(), tip_service.c_str()); 605 | return false; 606 | } 607 | jac_solver_service_.reset(new KDL::ChainJntToJacSolver(kdl_chain_service_)); 608 | 609 | KDL::JntArrayVel positions = KDL::JntArrayVel(kdl_chain_service_.getNrOfJoints()); 610 | 611 | KDL::SetToZero(positions.q); 612 | KDL::SetToZero(positions.qdot); 613 | 614 | for (size_t i = 0; i < kdl_chain_service_.getNrOfSegments(); ++i) 615 | { 616 | KDL::Joint joint = kdl_chain_service_.getSegment(i).getJoint(); 617 | if (joint.getType() != KDL::Joint::None) 618 | joint_names.push_back(joint.getName()); 619 | } 620 | 621 | 622 | //send over the copy of the joint state message 623 | { 624 | std::lock_guard guard(last_msg_mutex_); 625 | if (!have_last_msg_) { 626 | ROS_WARN("No joint state yet, refusing service"); 627 | return false; 628 | } 629 | msg = joint_state_msg_service_; 630 | have_last_msg_ = false; 631 | joint_state_msg_service_.reset(); 632 | } 633 | 634 | std::vector q, dq; 635 | for (const auto& joint_name : joint_names) 636 | { 637 | auto it = std::find(msg->name.begin(), msg->name.end(), joint_name); 638 | if (it != msg->name.end()) 639 | { 640 | size_t idx = std::distance(msg->name.begin(), it); 641 | q.push_back(msg->position[idx]); 642 | dq.push_back(msg->velocity[idx]); 643 | } 644 | } 645 | 646 | if (q.size() != joint_names.size()) 647 | { 648 | ROS_ERROR("Joint state message does not contain all joint names"); 649 | return false; 650 | } 651 | 652 | for (size_t i = 0; i < joint_names.size(); ++i) 653 | { 654 | positions.q(i) = q[i]; 655 | positions.qdot(i) = dq[i]; 656 | } 657 | 658 | KDL::Jacobian J_kdl(joint_names.size()); 659 | jac_solver_service_->JntToJac(positions.q, J_kdl); 660 | Eigen::MatrixXd J = J_kdl.data; 661 | 662 | // Now run usual pipeline on J 663 | Eigen::MatrixXd J_parse, J_safety_nullspace; 664 | 665 | //handle any kinematic chain size 666 | Eigen::JacobiSVD svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); 667 | int n = svd.singularValues().size(); 668 | std::vector jparse_singular_index(n, 0);// Elements in this list are 0 if non-singular, 1 if singular 669 | 670 | Eigen::MatrixXd U_safety, U_new_proj, U_new_sing; 671 | Eigen::VectorXd S_new_safety, S_new_proj, Phi; 672 | 673 | Jparse_calculation(J, J_parse, J_safety_nullspace, jparse_singular_index, U_safety, S_new_safety, U_new_proj, S_new_proj, U_new_sing, Phi, gamma_service, singular_direction_gain_position_service, singular_direction_gain_angular_service); 674 | 675 | // Fill response 676 | matrix_to_msg(J_parse, res.jparse); 677 | matrix_to_msg(J_safety_nullspace, res.jsafety_nullspace); 678 | return true; 679 | } 680 | 681 | void JPARSE::matrix_to_msg(const Eigen::MatrixXd& mat, std::vector& msg_rows) 682 | { 683 | msg_rows.clear(); 684 | for (int i = 0; i < mat.rows(); ++i) 685 | { 686 | manipulator_control::Matrow row; 687 | for (int j = 0; j < mat.cols(); ++j) 688 | { 689 | row.row.push_back(mat(i, j)); 690 | } 691 | msg_rows.push_back(row); 692 | } 693 | } 694 | 695 | void JPARSE::Publish_JPARSE(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace) 696 | { 697 | manipulator_control::JparseTerms msg; 698 | msg.header = header; 699 | 700 | matrix_to_msg(J_parse, msg.jparse); 701 | matrix_to_msg(J_safety_nullspace, msg.jsafety_nullspace); 702 | 703 | jparse_pub_.publish(msg); 704 | } 705 | 706 | 707 | int main(int argc, char** argv) 708 | { 709 | ros::init(argc, argv, "jparse_cpp_node"); 710 | ros::NodeHandle nh; 711 | JPARSE parser(nh); 712 | 713 | ros::AsyncSpinner spinner(2); // AsyncSpinner with 2 threads lets your subscriber and service, callbacks run concurrently. 714 | spinner.start(); 715 | ros::waitForShutdown(); 716 | return 0; 717 | } 718 | 719 | -------------------------------------------------------------------------------- /manipulator_control/src/manipulator_control/jparse_cls.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import numpy as np 4 | #access pykdl for jacobian and other matricies 5 | import PyKDL as kdl 6 | from urdf_parser_py.urdf import Robot 7 | from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model 8 | from pykdl_utils.kdl_kinematics import KDLKinematics 9 | from pykdl_utils.kdl_kinematics import kdl_to_mat 10 | from pykdl_utils.kdl_kinematics import joint_list_to_kdl 11 | #for plotting ellipsoids 12 | from visualization_msgs.msg import Marker, MarkerArray 13 | from geometry_msgs.msg import Quaternion, Pose, Point, PoseStamped 14 | 15 | 16 | class JParseClass(object): 17 | def __init__(self, base_link="base_link", end_link="end_effector_link"): 18 | # Initialize any necessary variables or parameters here 19 | """ 20 | Base link: The base link of the robot. 21 | End link: The end link of the robot. 22 | """ 23 | self.base_link = base_link 24 | self.end_link = end_link 25 | self.urdf = Robot.from_parameter_server() 26 | self.kdl_tree = kdl_tree_from_urdf_model(self.urdf) 27 | self.chain = self.kdl_tree.getChain(base_link, end_link) 28 | self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain) 29 | self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain) 30 | self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl, self._ik_v_kdl) 31 | self._jac_kdl = kdl.ChainJntToJacSolver(self.chain) 32 | self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector(0,0,-9.81)) 33 | self.kdl_kin = KDLKinematics(self.urdf, base_link, end_link) 34 | self.num_joints = self.kdl_kin.num_joints 35 | self.joint_names = self.kdl_kin.get_joint_names() 36 | self.marker_pub = rospy.Publisher('/jparse_ellipsoid_marker', MarkerArray, queue_size=10) 37 | 38 | self.J_prev = None 39 | self.J_prev_time = None 40 | 41 | def jacobian(self, q=[]): 42 | """ 43 | Computes the Jacobian matrix for the given joint configuration. 44 | 45 | Parameters: 46 | q (list): A list of joint positions. 47 | 48 | Returns: 49 | numpy.ndarray: The Jacobian matrix as a numpy array. 50 | 51 | This function converts the joint positions to a KDL joint array, computes the Jacobian using KDL, 52 | and then converts the resulting KDL Jacobian matrix to a numpy array. 53 | """ 54 | j_kdl = kdl.Jacobian(self.num_joints) 55 | q_kdl = joint_list_to_kdl(q) 56 | self._jac_kdl.JntToJac(q_kdl, j_kdl) 57 | J_mat = kdl_to_mat(j_kdl) #converts kdl to numpy matrix 58 | return J_mat 59 | 60 | def jacobian_dot(self, q=[], position_only=False , approx=True): 61 | """ 62 | Computes the time derivative of the Jacobian matrix for the given joint configuration and joint velocities. 63 | 64 | Parameters: 65 | q (list): A list of joint positions. 66 | 67 | Returns: 68 | numpy.ndarray: The time derivative of the Jacobian matrix as a numpy array. 69 | """ 70 | J = self.jacobian(q) 71 | if position_only == True: 72 | #if we are only interested in the position, return the first 3 rows of the jacobian 73 | J= J[:3, :] 74 | 75 | if approx == True: 76 | #This is the approximate method for calculating the time derivative of the jacobian 77 | J_dot = np.zeros(J.shape) 78 | q_plus = q.copy() 79 | q_minus = q.copy() 80 | for i in range(self.num_joints): 81 | q_plus[i] += 0.0001 82 | q_minus[i] -= 0.0001 83 | J_plus = self.jacobian(q_plus) 84 | J_minus = self.jacobian(q_minus) 85 | J_dot = (J_plus - J_minus) / 0.0002 86 | return J_dot 87 | else: 88 | #This is the exact method for calculating the time derivative of the jacobian 89 | if self.J_prev is None: 90 | self.J_prev = J 91 | self.J_prev_time = rospy.Time.now() 92 | J_dot = np.zeros(J.shape) 93 | else: 94 | dt = (rospy.Time.now() - self.J_prev_time).to_sec() 95 | J_dot = (J - self.J_prev) / dt 96 | self.J_prev = J 97 | return J_dot 98 | 99 | def svd_compose(self,U,S,Vt): 100 | """ 101 | This function takes SVD: U,S,V and recomposes them for J 102 | """ 103 | Zero_concat = np.zeros((U.shape[0],Vt.shape[0]-len(S))) 104 | Sfull = np.zeros((U.shape[1],Vt.shape[0])) 105 | for row in range(Sfull.shape[0]): 106 | for col in range(Sfull.shape[1]): 107 | if row == col: 108 | if row < len(S): 109 | Sfull[row,col] = S[row] 110 | J_new =np.matrix(U)*Sfull*np.matrix(Vt) 111 | return J_new 112 | 113 | def manipulability_measure(self, q=[], use_inv_cond_number=False): 114 | """ 115 | This function computes the manipulability measure for the given joint configuration. 116 | """ 117 | #if we are using the manipulability measure, return that 118 | J = self.jacobian(q) 119 | return np.sqrt(np.linalg.det(J @ J.T)) 120 | 121 | def inverse_condition_number(self, q=[]): 122 | """ 123 | This function computes the inverse of the condition number for the given joint configuration. 124 | """ 125 | J = self.jacobian(q) 126 | U, S, Vt = np.linalg.svd(J) 127 | #find the min and max singular values 128 | sigma_min = np.min(S) 129 | sigma_max = np.max(S) 130 | inv_cond_number = sigma_min/sigma_max 131 | return inv_cond_number 132 | 133 | def JacobianPseudoInverse(self, q=[], position_only=False, jac_nullspace_bool = False): 134 | """ 135 | This function computes the pseudo-inverse of the Jacobian matrix for the given joint configuration. 136 | """ 137 | J = self.jacobian(q) 138 | if position_only == True: 139 | #if we are only interested in the position, return the first 3 rows of the jacobian 140 | J= J[:3, :] 141 | J_pinv = np.linalg.pinv(J) 142 | if jac_nullspace_bool == True: 143 | #Find the nullspace of the jacobian 144 | J_nullspace = np.eye(J.shape[1]) - np.linalg.pinv(J) @ J 145 | return J_pinv, J_nullspace 146 | return J_pinv 147 | 148 | def JParse(self, q=[], gamma=0.1, position_only=False, jac_nullspace_bool = False , singular_direction_gain_position=1, singular_direction_gain_angular=1, verbose=False, publish_jparse_ellipses=False, internal_marker_flag=False, end_effector_pose=None): 149 | """ 150 | This function computes the JParse matrix for the given joint configuration and gamma value. 151 | This function should return the JParse matrix as a numpy array. 152 | - gamma is the threshold value for the adjusted condition number. 153 | - position_only is a boolean flag to indicate whether to use only the position part of the Jacobian. 154 | - singular_direction_gain is the gain for the singular direction projection matrix. Nominal values range from 1 to 10. 155 | """ 156 | #obtain the original jacobian 157 | J = self.jacobian(q) 158 | 159 | if position_only == True: 160 | #if we are only interested in the position, return the first 3 rows of the jacobian 161 | J= J[:3, :] 162 | 163 | #Perform the SVD decomposition of the jacobian 164 | U, S, Vt = np.linalg.svd(J) 165 | #Find the adjusted condition number 166 | sigma_max = np.max(S) 167 | adjusted_condition_numbers = [sig / sigma_max for sig in S] 168 | 169 | if verbose == True: 170 | print("adjusted_condition_numbers:", adjusted_condition_numbers) 171 | #Find the projection Jacobian 172 | U_new_proj = [] 173 | S_new_proj = [] 174 | for col in range(len(S)): 175 | if S[col] > gamma*sigma_max: 176 | #Singular row 177 | U_new_proj.append(np.matrix(U[:,col]).T) 178 | S_new_proj.append(S[col]) 179 | U_new_proj = np.concatenate(U_new_proj,axis=0).T 180 | J_proj = self.svd_compose(U_new_proj, S_new_proj, Vt) 181 | 182 | #Find the safety jacboian 183 | S_new_safety = [s if (s/sigma_max) > gamma else gamma*sigma_max for s in S] 184 | J_safety = self.svd_compose(U,S_new_safety,Vt) 185 | 186 | #Find the singular direction projection components 187 | U_new_sing = [] 188 | Phi = [] #these will be the ratio of s_i/s_max 189 | set_empty_bool = True 190 | for col in range(len(S)): 191 | if adjusted_condition_numbers[col] <= gamma: 192 | set_empty_bool = False 193 | U_new_sing.append(np.matrix(U[:,col]).T) 194 | Phi.append(adjusted_condition_numbers[col]/gamma) #division by gamma for s/(s_max * gamma), gives smooth transition for Kp =1.0; 195 | 196 | #set an empty Phi_singular matrix, populate if there were any adjusted 197 | #condition numbers below the threshold 198 | Phi_singular = np.zeros(U.shape) #initialize the singular projection matrix 199 | 200 | if verbose == True: 201 | print("set_empty_bool:", set_empty_bool) 202 | if set_empty_bool == False: 203 | #construct the new U, as there were singular directions 204 | U_new_sing = np.matrix(np.concatenate(U_new_sing,axis=0)).T 205 | Phi_mat = np.matrix(np.diag(Phi)) 206 | if position_only == True: 207 | gains = np.array([singular_direction_gain_position]*3, dtype=float) 208 | else: 209 | gains = np.array([singular_direction_gain_position]*3 + [singular_direction_gain_angular]*3, dtype=float) 210 | Kp_singular = np.diag(gains) 211 | # Now put it all together: 212 | Phi_singular = U_new_sing @ Phi_mat @ U_new_sing.T @ Kp_singular 213 | if verbose == True: 214 | print("Kp_singular:", Kp_singular) 215 | print("Phi_mat shape:", Phi_mat.shape, "Phi_mat:", Phi_mat) 216 | print("U_new_sing shape:", U_new_sing.shape, "U_new_sing:", U_new_sing) 217 | 218 | #Obtain psuedo-inverse of the safety jacobian and the projection jacobian 219 | J_safety_pinv = np.linalg.pinv(J_safety) 220 | J_proj_pinv = np.linalg.pinv(J_proj) 221 | 222 | if set_empty_bool == False: 223 | J_parse = J_safety_pinv @ J_proj @ J_proj_pinv + J_safety_pinv @ Phi_singular 224 | else: 225 | J_parse = J_safety_pinv @ J_proj @ J_proj_pinv 226 | 227 | #Publish the JParse ellipses 228 | ellipse_dict = {"J_safety_u": U, "J_safety_s": S_new_safety, "J_proj_u": U_new_proj, "J_proj_s": S_new_proj, "J_singular_u": U_new_sing, "J_singular_s": Phi} 229 | if internal_marker_flag == True: 230 | #this is internal for jparse marker display 231 | return ellipse_dict 232 | 233 | if publish_jparse_ellipses == True: 234 | #only shows position ellipses 235 | self.publish_position_Jparse_ellipses(q=q, gamma=gamma, jac_nullspace_bool=False, singular_direction_gain_position=singular_direction_gain_position, singular_direction_gain_angular=singular_direction_gain_angular, verbose=verbose, publish_jparse_ellipses=publish_jparse_ellipses, end_effector_pose=end_effector_pose) 236 | 237 | 238 | if jac_nullspace_bool == True: 239 | #Find the nullspace of the jacobian 240 | J_safety_nullspace = np.eye(J_safety.shape[1]) - J_safety_pinv @ J_safety 241 | return J_parse, J_safety_nullspace 242 | 243 | return J_parse 244 | 245 | ''' 246 | The following are only useful for the dynamics terms (e.g. torque control) 247 | They are not required to obtain the JParse matrix 248 | ''' 249 | def publish_position_Jparse_ellipses(self, q=[], gamma=0.1, jac_nullspace_bool = False , singular_direction_gain_position=1, singular_direction_gain_angular=1, verbose=False, publish_jparse_ellipses=False, end_effector_pose=None): 250 | """ 251 | this function displays the key ellipses for the JParse 252 | """ 253 | #obtain the key matricies 254 | ellipse_marker_array = MarkerArray() 255 | 256 | ellipse_dict = self.JParse(q=q, gamma=gamma, position_only=True, jac_nullspace_bool=jac_nullspace_bool, singular_direction_gain_position=singular_direction_gain_position, singular_direction_gain_angular=singular_direction_gain_angular, verbose=verbose, publish_jparse_ellipses=False, internal_marker_flag=True, end_effector_pose=end_effector_pose) 257 | frame_id = self.base_link 258 | #add safety jacobian 259 | J_safety_marker = self.generate_jparse_ellipses(mat_type="J_safety", U_mat=ellipse_dict["J_safety_u"], S_vect=ellipse_dict["J_safety_s"], marker_ns="J_safety", end_effector_pose=end_effector_pose, frame_id=frame_id) 260 | ellipse_marker_array.markers.append(J_safety_marker[0]) 261 | #if there are feasible directions, add J_proj 262 | if len(ellipse_dict["J_proj_s"]) > 0: 263 | #Pass in the full U to ensure the ellipse shows up at all, we will handle with approximate scaling for singular directions (make them 0.001) 264 | J_proj_marker = self.generate_jparse_ellipses(mat_type="J_proj", U_mat=ellipse_dict["J_safety_u"], S_vect=ellipse_dict["J_proj_s"], marker_ns="J_proj", end_effector_pose=end_effector_pose, frame_id=frame_id) 265 | ellipse_marker_array.markers.append(J_proj_marker[0]) 266 | #if there are singular directions, add them 267 | J_singular_marker = self.generate_jparse_ellipses(mat_type="J_singular", U_mat=ellipse_dict["J_singular_u"], S_vect=ellipse_dict["J_singular_s"], end_effector_pose=end_effector_pose, frame_id=frame_id) 268 | if len(J_singular_marker) > 0: 269 | for idx in range(len(J_singular_marker)): 270 | ellipse_marker_array.markers.append(J_singular_marker[idx]) 271 | self.marker_pub.publish(ellipse_marker_array) 272 | 273 | def generate_jparse_ellipses(self, mat_type=None, U_mat=None, S_vect=None, marker_ns="ellipse" , frame_id="base_link", end_effector_pose=None): 274 | #This function takes in the singular value directions and plots ellipses or vectors 275 | Marker_list = [] 276 | pose = PoseStamped() 277 | if mat_type in ["J_safety", "J_proj"]: 278 | marker = Marker() 279 | #Create the marker 280 | marker.header.frame_id = frame_id 281 | marker.header.stamp = rospy.Time.now() 282 | marker.ns = marker_ns 283 | if mat_type == "J_safety": 284 | marker.id = 0 285 | marker_ns = "J_safety" 286 | elif mat_type == "J_proj": 287 | marker.id = 1 288 | marker_ns = "J_proj" 289 | marker.type = Marker.SPHERE 290 | marker.action = Marker.ADD 291 | 292 | #set the position of the marker 293 | marker.pose.position.x = end_effector_pose.pose.position.x 294 | marker.pose.position.y = end_effector_pose.pose.position.y 295 | marker.pose.position.z = end_effector_pose.pose.position.z 296 | 297 | #set the scale based on singular values (adjust factor if needed); in practice, the ellipsoid should flatten, but for easy visualization, we make these dimensions very very small 298 | ellipsoid_scale = 0.25 299 | marker.scale.x = 0.001 300 | marker.scale.y = 0.001 301 | marker.scale.z = 0.001 302 | if len(S_vect) == 1: 303 | marker.scale.x = ellipsoid_scale*np.max([S_vect[0], 0.001]) 304 | elif len(S_vect) == 2: 305 | marker.scale.x = ellipsoid_scale*np.max([S_vect[0], 0.001]) 306 | marker.scale.y = ellipsoid_scale*np.max([S_vect[1], 0.001]) 307 | elif len(S_vect) >= 3: 308 | marker.scale.x = ellipsoid_scale*np.max([S_vect[0], 0.001]) 309 | marker.scale.y = ellipsoid_scale*np.max([S_vect[1], 0.001]) 310 | marker.scale.z = ellipsoid_scale*np.max([S_vect[2], 0.001]) 311 | 312 | # Convert U (rotation matrix) to quaternion 313 | q = self.rotation_matrix_to_quaternion(U_mat) 314 | marker.pose.orientation = q 315 | 316 | # Set color (RGBA) 317 | if mat_type == "J_safety": 318 | marker.color.r = 1.0 319 | marker.color.g = 0.0 320 | marker.color.b = 0.0 321 | marker.color.a = 0.7 #transparency 322 | elif mat_type == "J_proj": 323 | marker.color.r = 0.0 324 | marker.color.g = 0.0 325 | marker.color.b = 1.0 326 | marker.color.a = 0.7 #transparency 327 | Marker_list.append(marker) 328 | 329 | elif mat_type == "J_singular": 330 | 331 | for idx in range(len(S_vect)): 332 | #Create the marker 333 | marker = Marker() 334 | marker_ns = "J_singular" 335 | marker.header.frame_id = frame_id 336 | marker.header.stamp = rospy.Time.now() 337 | marker.ns = marker_ns 338 | marker.id = idx+2 339 | marker.type = Marker.ARROW 340 | marker.action = Marker.ADD 341 | marker.lifetime = rospy.Duration(0.1) # or rclpy.duration.Duration(seconds=0.1) in ROS 2 342 | 343 | #Arrow start point 344 | start_point = Point() 345 | start_point.x = end_effector_pose.pose.position.x 346 | start_point.y = end_effector_pose.pose.position.y 347 | start_point.z = end_effector_pose.pose.position.z 348 | 349 | #arrow end point 350 | arrow_scale = 1.0 351 | end_point = Point() 352 | # if points away from origin 353 | if (end_effector_pose.pose.position.x*U_mat[0,idx] + end_effector_pose.pose.position.y*U_mat[1,idx] + end_effector_pose.pose.position.z*U_mat[2,idx]) < 0: 354 | arrow_x = U_mat[0,idx] 355 | arrow_y = U_mat[1,idx] 356 | arrow_z = U_mat[2,idx] 357 | else: 358 | arrow_x = -U_mat[0,idx] 359 | arrow_y = -U_mat[1,idx] 360 | arrow_z = -U_mat[2,idx] 361 | end_point.x = end_effector_pose.pose.position.x + arrow_scale*arrow_x*np.abs(S_vect[idx]) 362 | end_point.y = end_effector_pose.pose.position.y + arrow_scale*arrow_y*np.abs(S_vect[idx]) 363 | end_point.z = end_effector_pose.pose.position.z + arrow_scale*arrow_z*np.abs(S_vect[idx]) 364 | 365 | marker.points.append(start_point) 366 | marker.points.append(end_point) 367 | 368 | # Scale (arrow thickness and head size) 369 | marker.scale.x = 0.01 # Shaft diameter 370 | marker.scale.y = 0.05 # Head diameter 371 | marker.scale.z = 0.05 # Head length 372 | 373 | # Set color based on principal axis 374 | marker.color.r = 1.0 375 | marker.color.g = 0.0 376 | marker.color.b = 0.0 377 | marker.color.a = 1.0 # Opaque 378 | 379 | Marker_list.append(marker) 380 | 381 | return Marker_list 382 | 383 | def rotation_matrix_to_quaternion(self, R): 384 | """ 385 | Convert a 3x3 rotation matrix to a quaternion. 386 | :param R: 3x3 rotation matrix 387 | :return: geometry_msgs/Quaternion 388 | """ 389 | q = Quaternion() 390 | trace = np.trace(R) 391 | 392 | if trace > 0: 393 | S = np.sqrt(trace + 1.0) * 2 394 | qw = 0.25 * S 395 | qx = (R[2, 1] - R[1, 2]) / S 396 | qy = (R[0, 2] - R[2, 0]) / S 397 | qz = (R[1, 0] - R[0, 1]) / S 398 | elif (R[0, 0] > R[1, 1]) and (R[0, 0] > R[2, 2]): 399 | S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 400 | qw = (R[2, 1] - R[1, 2]) / S 401 | qx = 0.25 * S 402 | qy = (R[0, 1] + R[1, 0]) / S 403 | qz = (R[0, 2] + R[2, 0]) / S 404 | elif R[1, 1] > R[2, 2]: 405 | S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 406 | qw = (R[0, 2] - R[2, 0]) / S 407 | qx = (R[0, 1] + R[1, 0]) / S 408 | qy = 0.25 * S 409 | qz = (R[1, 2] + R[2, 1]) / S 410 | else: 411 | S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 412 | qw = (R[1, 0] - R[0, 1]) / S 413 | qx = (R[0, 2] + R[2, 0]) / S 414 | qy = (R[1, 2] + R[2, 1]) / S 415 | qz = 0.25 * S 416 | 417 | # Normalize quaternion 418 | norm = np.sqrt(qw**2 + qx**2 + qy**2 + qz**2) 419 | q.x = qx / norm 420 | q.y = qy / norm 421 | q.z = qz / norm 422 | q.w = qw / norm 423 | return q 424 | 425 | def cart_inertia(self, q=[]): 426 | """ 427 | This is not needed for the main JParse class, but is included here for reference. 428 | """ 429 | H = self.inertia(q) 430 | J = self.jacobian(q) 431 | J_pinv = np.linalg.pinv(J) 432 | J_pinv_transpose = J_pinv.T 433 | return J_pinv_transpose @ H @ J_pinv 434 | 435 | def inertia(self, q=[]): 436 | """ 437 | This is not needed for the main JParse class, but is included here for reference. 438 | """ 439 | h_kdl = kdl.JntSpaceInertiaMatrix(self.num_joints) 440 | self._dyn_kdl.JntToMass(joint_list_to_kdl(q), h_kdl) 441 | return kdl_to_mat(h_kdl) 442 | 443 | def coriolis(self,q=[], qdot=[]): 444 | """ 445 | This is not needed for the main JParse class, but is included here for reference. 446 | """ 447 | q = q #list 448 | qdot = qdot #list 449 | q_cori = [0.0 for idx in range(len(q))] 450 | q_kdl = joint_list_to_kdl(q) 451 | qdot_kdl = joint_list_to_kdl(qdot) 452 | q_cori_kdl = joint_list_to_kdl(q_cori) 453 | self._dyn_kdl.JntToCoriolis(q_kdl, qdot_kdl, q_cori_kdl) 454 | q_cori_kdl = np.array([q_cori_kdl[i] for i in range(q_cori_kdl.rows())]) 455 | q_cori_kdl = np.matrix(q_cori_kdl).T 456 | return q_cori_kdl 457 | 458 | def gravity(self,q=[]): 459 | """ 460 | This is not needed for the main JParse class, but is included here for reference. 461 | """ 462 | q_grav = [0.0 for idx in range(len(q))] 463 | q_kdl = joint_list_to_kdl(q) 464 | q_grav_kdl = joint_list_to_kdl(q_grav) 465 | self._dyn_kdl.JntToGravity(q_kdl,q_grav_kdl) 466 | q_grav_kdl = np.array([q_grav_kdl[i] for i in range(q_grav_kdl.rows())]) 467 | q_grav_kdl = np.matrix(q_grav_kdl).T 468 | return q_grav_kdl 469 | 470 | ''' 471 | Baseline implementations for comparison 472 | ''' 473 | def jacobian_damped_least_squares(self, q=[], damping=0.01, jac_nullspace_bool=False): 474 | """ 475 | COMPARISON METHOD (not used in JPARSE) 476 | This function computes the damped least squares pseudo-inverse of the Jacobian matrix for the given joint configuration. 477 | """ 478 | J = self.jacobian(q) 479 | J_dls = np.linalg.inv(J.T @ J + damping**2 * np.eye(J.shape[1])) @ J.T 480 | if jac_nullspace_bool == False: 481 | return J_dls 482 | J_dls_nullspace = np.eye(J.shape[1]) - J_dls @ J 483 | return J_dls, J_dls_nullspace 484 | 485 | def jacobian_transpose(self, q=[]): 486 | """ 487 | COMPARISON METHOD (not used in JPARSE) 488 | This function computes the transpose of the Jacobian matrix for the given joint configuration. 489 | """ 490 | J = self.jacobian(q) 491 | J_transpose = J.T 492 | J_transpose_nullspace = np.eye(J_transpose.shape[0]) - J_transpose @ np.linalg.pinv(J_transpose) 493 | return J_transpose, J_transpose_nullspace 494 | 495 | def jacobian_projection(self, q=[], gamma=0.1, jac_nullspace_bool=False): 496 | """ 497 | This function computes the projection of the Jacobian matrix onto feasible (non-singular) directions for the given joint configuration. 498 | """ 499 | J = self.jacobian(q) 500 | #Perform the SVD decomposition of the jacobian 501 | U, S, Vt = np.linalg.svd(J) 502 | #Find the adjusted condition number 503 | sigma_max = np.max(S) 504 | #Find the projection Jacobian 505 | U_new_proj = [] 506 | S_new_proj = [] 507 | for col in range(len(S)): 508 | if S[col] > gamma*sigma_max: 509 | U_new_proj.append(np.matrix(U[:,col]).T) 510 | S_new_proj.append(S[col]) 511 | else: 512 | rospy.loginfo("Singular row found during projection") 513 | print("the ratio of s_i/s_max is:", S[col]/sigma_max) 514 | 515 | U_new_proj = np.concatenate(U_new_proj,axis=0).T 516 | J_proj = self.svd_compose(U_new_proj, S_new_proj, Vt) 517 | #if there is no nullspace, return the projection jacobian 518 | if jac_nullspace_bool == False: 519 | return J_proj 520 | J_proj_nullspace = np.eye(J_proj.shape[1]) - np.linalg.pinv(J_proj)@J_proj 521 | return J_proj, J_proj_nullspace 522 | 523 | def jacobian_nullspace_decoupled(self, q=[], dq=[], identity_weight_matrix=False): 524 | """ 525 | COMPARISON METHOD (not used in JPARSE) 526 | This function computes the decoupled nullspace of the Jacobian matrix for the given joint configuration. 527 | This method is described in the paper by Chang and Khatib (1995). 528 | """ 529 | J_proj = self.jacobian_projection(q=q, gamma=0.05) 530 | J = self.jacobian(q) 531 | J_method = J.T 532 | Mass_matrix = self.inertia(q) 533 | if identity_weight_matrix == True: 534 | #temporarily make the mass matrix the identity matrix (better performance than the actual mass matrix) 535 | Mass_matrix = np.eye(Mass_matrix.shape[0]) 536 | Minv = np.linalg.inv(Mass_matrix) 537 | Lambda = np.linalg.pinv(J_proj @ Minv @ J_proj.T) 538 | J_bar = Minv @ J_proj.T @ Lambda 539 | J_proj_nullspace = Mass_matrix - J.T @ J_bar.T @ Mass_matrix 540 | #note that the objective and damping terms are not included in this implementation, they need to be multiplied by the nullspace matrix 541 | return J_method, J_proj_nullspace 542 | 543 | def jacobian_dci(self, q=[], dq=[], identity_weight_matrix=False): 544 | """ 545 | COMPARISON METHOD (not used in JPARSE) 546 | This function computes the dynamically consistent generalized inverse 547 | """ 548 | J = self.jacobian(q) 549 | J_proj = self.jacobian_projection(q=q, gamma=0.05) 550 | Mass_matrix = self.inertia(q) 551 | Minv = np.linalg.inv(Mass_matrix) 552 | Lambda = np.linalg.pinv(J_proj @ Minv @ J_proj.T) 553 | J_method = Minv @ J_proj.T @ Lambda 554 | J_proj_nullspace = np.eye(J_proj.shape[1]) - J_method @ J 555 | return J_method, J_proj_nullspace 556 | 557 | 558 | def main(): 559 | rospy.init_node('jparse_test', anonymous=True) 560 | jparse = JParseClass() 561 | q = [0, 0, 0, 0, 0, 0] 562 | print("JParse:", jparse.JParse(q=q)) 563 | print("JParse:", jparse.JParse(q=q, position_only=True)) 564 | print("Jacobian:", jparse.jacobian(q=q)) 565 | print("Jacobian:", jparse.jacobian(q=q, position_only=True)) 566 | print("Jacobian Pseudo Inverse:", jparse.JacobianPseudoInverse(q=q)) 567 | print("Jacobian Pseudo Inverse:", jparse.JacobianPseudoInverse(q=q, position_only=True)) 568 | print("Jacobian Damped Least Squares:", jparse.jacobian_damped_least_squares(q=q)) 569 | print("Jacobian Damped Least Squares:", jparse.jacobian_damped_least_squares(q=q, jac_nullspace_bool=True)) 570 | print("Jacobian Transpose:", jparse.jacobian_transpose(q=q)) 571 | 572 | if __name__ == '__main__': 573 | try: 574 | main() 575 | except rospy.ROSInterruptException: 576 | pass 577 | 578 | -------------------------------------------------------------------------------- /manipulator_control/srv/JParseSrv.srv: -------------------------------------------------------------------------------- 1 | float64 gamma 2 | float64 singular_direction_gain_position 3 | float64 singular_direction_gain_angular 4 | string base_link_name 5 | string end_link_name 6 | --- 7 | manipulator_control/Matrow[] jparse 8 | manipulator_control/Matrow[] jsafety_nullspace --------------------------------------------------------------------------------