diff --git a/.DS_Store b/.DS_Store index 8aa99a7..34c963e 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/robot.urdf b/robot.urdf deleted file mode 100644 index bf21727..0000000 --- a/robot.urdf +++ /dev/null @@ -1,542 +0,0 @@ - - - - - - - - - - - - - ur_robot_driver/URPositionHardwareInterface - 172.18.0.2 - /opt/ros/humble/share/ur_client_library/resources/external_control.urscript - /opt/ros/humble/share/ur_robot_driver/resources/rtde_output_recipe.txt - /opt/ros/humble/share/ur_robot_driver/resources/rtde_input_recipe.txt - False - 50001 - 50002 - 0.0.0.0 - 50004 - 50003 - - True - 2000 - 0.03 - False - calib_5119701370761913513 - 0 - 0 - 115200 - 1 - 1.5 - 3.5 - /tmp/ttyUR - 54321 - 2 - - - - - - - 0.0 - - - - - - - - - - -1.57 - - - - - - - - - - 0.0 - - - - - - - - - - -1.57 - - - - - - - - - - 0.0 - - - - - - - - - - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros2_ws/install/local_setup.sh b/ros2_ws/install/local_setup.sh index e8ab9d5..07a0147 100644 --- a/ros2_ws/install/local_setup.sh +++ b/ros2_ws/install/local_setup.sh @@ -6,7 +6,7 @@ # since a plain shell script can't determine its own path when being sourced # either use the provided COLCON_CURRENT_PREFIX # or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/ros2_ws/install" +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/BA/ros2_ws/install" if [ -z "$COLCON_CURRENT_PREFIX" ]; then if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 diff --git a/ros2_ws/install/setup.bash b/ros2_ws/install/setup.bash index 10ea0f7..4e927c6 100644 --- a/ros2_ws/install/setup.bash +++ b/ros2_ws/install/setup.bash @@ -17,11 +17,6 @@ _colcon_prefix_chain_bash_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - # source this prefix # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" diff --git a/ros2_ws/install/setup.ps1 b/ros2_ws/install/setup.ps1 index 558e9b9..fe4e838 100644 --- a/ros2_ws/install/setup.ps1 +++ b/ros2_ws/install/setup.ps1 @@ -21,9 +21,6 @@ function _colcon_prefix_chain_powershell_source_script { } } -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" - # source this prefix $env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) _colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/ros2_ws/install/setup.sh b/ros2_ws/install/setup.sh index 48ebbe1..b77c122 100644 --- a/ros2_ws/install/setup.sh +++ b/ros2_ws/install/setup.sh @@ -7,7 +7,7 @@ # since a plain shell script can't determine its own path when being sourced # either use the provided COLCON_CURRENT_PREFIX # or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/ros2_ws/install +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/BA/ros2_ws/install if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then @@ -29,12 +29,6 @@ _colcon_prefix_chain_sh_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - # source this prefix # setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" diff --git a/ros2_ws/install/setup.zsh b/ros2_ws/install/setup.zsh index 54799fd..2901efe 100644 --- a/ros2_ws/install/setup.zsh +++ b/ros2_ws/install/setup.zsh @@ -17,11 +17,6 @@ _colcon_prefix_chain_zsh_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - # source this prefix # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" diff --git a/ros2_ws/log/latest_build b/ros2_ws/log/latest_build index d5cf0b3..6d48b87 120000 --- a/ros2_ws/log/latest_build +++ b/ros2_ws/log/latest_build @@ -1 +1 @@ -build_2025-01-29_16-04-47 \ No newline at end of file +build_2025-03-27_14-22-13 \ No newline at end of file diff --git a/ros2_ws/src/joint_states_control/get_urdf.py b/ros2_ws/src/joint_states_control/get_urdf.py index 4c7d262..bf52a83 100644 --- a/ros2_ws/src/joint_states_control/get_urdf.py +++ b/ros2_ws/src/joint_states_control/get_urdf.py @@ -38,7 +38,7 @@ def main(): if urdf_string: # Save to a file - urdf_path = "/BA/robot.urdf" + urdf_path = "/BA/robot_ur5.urdf" with open(urdf_path, "w") as file: file.write(urdf_string) diff --git a/ros2_ws/src/joint_states_control/move_scaled_joint_cart.py b/ros2_ws/src/joint_states_control/move_scaled_joint_cart.py index e52596a..d756320 100644 --- a/ros2_ws/src/joint_states_control/move_scaled_joint_cart.py +++ b/ros2_ws/src/joint_states_control/move_scaled_joint_cart.py @@ -5,7 +5,6 @@ from rcl_interfaces.srv import GetParameters import time from cart_to_angles import ik_sol import roboticstoolbox as rtb -import xacro class ScaledJointTrajectoryPublisher(Node): def __init__(self): diff --git a/workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env b/workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env index 384b422..7d6308d 100644 --- a/workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env +++ b/workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env @@ -1,21 +1,13 @@ -AMENT_PREFIX_PATH=/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble COLCON=1 -COLCON_PREFIX_PATH=/BA/workspace/install HOME=/root -HOSTNAME=3230bc57d699 +HOSTNAME=0e38e264ac6b LANG=C.UTF-8 LC_ALL=C.UTF-8 -LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36: -OLDPWD=/BA/workspace/src -PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin +OLDPWD=/ +PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin PWD=/BA/workspace/build/joint_control -PYTHONPATH=/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages ROS_DISTRO=humble -ROS_LOCALHOST_ONLY=0 -ROS_PYTHON_VERSION=3 -ROS_VERSION=2 SHLVL=1 TERM=xterm _=/usr/bin/colcon -_colcon_cd_root=/opt/ros/foxy/ diff --git a/workspace/build/joint_control/joint_control.egg-info/SOURCES.txt b/workspace/build/joint_control/joint_control.egg-info/SOURCES.txt index 8c9d108..a1c6326 100644 --- a/workspace/build/joint_control/joint_control.egg-info/SOURCES.txt +++ b/workspace/build/joint_control/joint_control.egg-info/SOURCES.txt @@ -1,9 +1,20 @@ package.xml setup.cfg setup.py +../../build/joint_control/joint_control.egg-info/PKG-INFO +../../build/joint_control/joint_control.egg-info/SOURCES.txt +../../build/joint_control/joint_control.egg-info/dependency_links.txt +../../build/joint_control/joint_control.egg-info/entry_points.txt +../../build/joint_control/joint_control.egg-info/requires.txt +../../build/joint_control/joint_control.egg-info/top_level.txt +../../build/joint_control/joint_control.egg-info/zip-safe joint_control/__init__.py joint_control/cart_tcp_server.py joint_control/joint_angles_server.py +joint_control/plugdata.py +joint_control/plugdata2.py +joint_control/plugdata3.py +joint_control/test.py joint_control/trajectory_server.py joint_control/trajectory_server_cart.py joint_control.egg-info/PKG-INFO diff --git a/workspace/build/joint_control/joint_control.egg-info/entry_points.txt b/workspace/build/joint_control/joint_control.egg-info/entry_points.txt index 09de273..375b957 100644 --- a/workspace/build/joint_control/joint_control.egg-info/entry_points.txt +++ b/workspace/build/joint_control/joint_control.egg-info/entry_points.txt @@ -1,6 +1,10 @@ [console_scripts] cart_coords = joint_control.cart_tcp_server:main joint_control = joint_control.joint_angles_server:main +plugdata = joint_control.plugdata:main +plugdata2 = joint_control.plugdata2:main +plugdata3 = joint_control.plugdata3:main +test = joint_control.test:main trajectory_server = joint_control.trajectory_server:main trajectory_server_cart = joint_control.trajectory_server_cart:main diff --git a/workspace/build/joint_control/prefix_override/__pycache__/sitecustomize.cpython-310.pyc b/workspace/build/joint_control/prefix_override/__pycache__/sitecustomize.cpython-310.pyc index 0123d59..e7d083a 100644 Binary files a/workspace/build/joint_control/prefix_override/__pycache__/sitecustomize.cpython-310.pyc and b/workspace/build/joint_control/prefix_override/__pycache__/sitecustomize.cpython-310.pyc differ diff --git a/workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env b/workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env index cf8da85..4145848 100644 --- a/workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env +++ b/workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env @@ -1,8 +1,8 @@ -AMENT_PREFIX_PATH=/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble +AMENT_PREFIX_PATH=/opt/ros/humble:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control COLCON=1 COLCON_PREFIX_PATH=/BA/workspace/install HOME=/root -HOSTNAME=3230bc57d699 +HOSTNAME=0e38e264ac6b LANG=C.UTF-8 LC_ALL=C.UTF-8 LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib @@ -10,7 +10,7 @@ LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd OLDPWD=/BA/workspace/src PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin PWD=/BA/workspace/build/joint_info -PYTHONPATH=/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages +PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages ROS_DISTRO=humble ROS_LOCALHOST_ONLY=0 ROS_PYTHON_VERSION=3 @@ -18,4 +18,3 @@ ROS_VERSION=2 SHLVL=1 TERM=xterm _=/usr/bin/colcon -_colcon_cd_root=/opt/ros/foxy/ diff --git a/workspace/build/joint_info/joint_info.egg-info/SOURCES.txt b/workspace/build/joint_info/joint_info.egg-info/SOURCES.txt index d7cdacc..df11c2c 100644 --- a/workspace/build/joint_info/joint_info.egg-info/SOURCES.txt +++ b/workspace/build/joint_info/joint_info.egg-info/SOURCES.txt @@ -1,6 +1,13 @@ package.xml setup.cfg setup.py +../../build/joint_info/joint_info.egg-info/PKG-INFO +../../build/joint_info/joint_info.egg-info/SOURCES.txt +../../build/joint_info/joint_info.egg-info/dependency_links.txt +../../build/joint_info/joint_info.egg-info/entry_points.txt +../../build/joint_info/joint_info.egg-info/requires.txt +../../build/joint_info/joint_info.egg-info/top_level.txt +../../build/joint_info/joint_info.egg-info/zip-safe joint_info/__init__.py joint_info/osc_joint_states_pub.py joint_info/osc_joint_states_sub.py diff --git a/workspace/build/joint_info/prefix_override/__pycache__/sitecustomize.cpython-310.pyc b/workspace/build/joint_info/prefix_override/__pycache__/sitecustomize.cpython-310.pyc index 4e7b071..041af0f 100644 Binary files a/workspace/build/joint_info/prefix_override/__pycache__/sitecustomize.cpython-310.pyc and b/workspace/build/joint_info/prefix_override/__pycache__/sitecustomize.cpython-310.pyc differ diff --git a/workspace/install/setup.bash b/workspace/install/setup.bash index 10ea0f7..4e927c6 100644 --- a/workspace/install/setup.bash +++ b/workspace/install/setup.bash @@ -17,11 +17,6 @@ _colcon_prefix_chain_bash_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - # source this prefix # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" diff --git a/workspace/install/setup.ps1 b/workspace/install/setup.ps1 index 558e9b9..fe4e838 100644 --- a/workspace/install/setup.ps1 +++ b/workspace/install/setup.ps1 @@ -21,9 +21,6 @@ function _colcon_prefix_chain_powershell_source_script { } } -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" - # source this prefix $env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) _colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/workspace/install/setup.sh b/workspace/install/setup.sh index fa9641d..7f6c219 100644 --- a/workspace/install/setup.sh +++ b/workspace/install/setup.sh @@ -29,12 +29,6 @@ _colcon_prefix_chain_sh_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - # source this prefix # setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" diff --git a/workspace/install/setup.zsh b/workspace/install/setup.zsh index 54799fd..2901efe 100644 --- a/workspace/install/setup.zsh +++ b/workspace/install/setup.zsh @@ -17,11 +17,6 @@ _colcon_prefix_chain_zsh_source_script() { fi } -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - # source this prefix # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" diff --git a/workspace/log/latest_build b/workspace/log/latest_build index 6703708..9a2733d 120000 --- a/workspace/log/latest_build +++ b/workspace/log/latest_build @@ -1 +1 @@ -build_2025-03-24_10-47-50 \ No newline at end of file +build_2025-04-07_10-59-55 \ No newline at end of file diff --git a/workspace/src/joint_angles.py b/workspace/src/joint_angles.py index 92d5c7c..1e8225b 100644 --- a/workspace/src/joint_angles.py +++ b/workspace/src/joint_angles.py @@ -1,12 +1,13 @@ from osc4py3.as_eventloop import * from osc4py3 import oscbuildparse +import time def main(): # Start the OSC system osc_startup() # Make client channels to send packets - osc_udp_client("127.0.0.1", 8000, "osc_client") + osc_udp_client("172.18.0.3", 8000, "osc_client") # Example joint positions to send @@ -17,12 +18,33 @@ def main(): joint_positions5 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0] msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2, joint_positions3, joint_positions4, joint_positions5]) - print("Sending joint positions") - # Send the OSC message osc_send(msg, "osc_client") - print("Sent joint positions") osc_process() + print("Sending joint positions") + ''' + time.sleep(2) + msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions2) + osc_send(msg, "osc_client") + osc_process() + print("Sent joint positions2") + time.sleep(3) + msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions3) + osc_send(msg, "osc_client") + osc_process() + print("Sent joint positions3") + time.sleep(3) + msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions4) + osc_send(msg, "osc_client") + osc_process() + print("Sent joint positions4") + time.sleep(3) + msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions5) + osc_send(msg, "osc_client") + osc_process() + print("Sent joint positions5") + time.sleep(3) + ''' osc_terminate() if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/workspace/src/joint_control/joint_control/__pycache__/cart_tcp_server.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/cart_tcp_server.cpython-310.pyc index 6db002c..34c1f9a 100644 Binary files a/workspace/src/joint_control/joint_control/__pycache__/cart_tcp_server.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/cart_tcp_server.cpython-310.pyc differ diff --git a/workspace/src/joint_control/joint_control/__pycache__/joint_angles_server.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/joint_angles_server.cpython-310.pyc index 8c4e103..de62ed3 100644 Binary files a/workspace/src/joint_control/joint_control/__pycache__/joint_angles_server.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/joint_angles_server.cpython-310.pyc differ diff --git a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_cart.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_cart.cpython-310.pyc index e253862..6277c39 100644 Binary files a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_cart.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_cart.cpython-310.pyc differ diff --git a/workspace/src/joint_control/joint_control/cart_tcp_server.py b/workspace/src/joint_control/joint_control/cart_tcp_server.py index 1eb8a12..6a8f244 100644 --- a/workspace/src/joint_control/joint_control/cart_tcp_server.py +++ b/workspace/src/joint_control/joint_control/cart_tcp_server.py @@ -33,7 +33,7 @@ class ScaledJointTrajectoryPublisher(Node): def tcp_coordinates_handler(self, *args): """Handles incoming OSC messages for tcp position.""" - #time1 = time.time() + time1 = time.time() if len(args) == len(self.joint_positions): x, y, z, roll, pitch, yaw = args duration = 4.0 # Default duration @@ -62,10 +62,10 @@ class ScaledJointTrajectoryPublisher(Node): if sol[1]: joint_positions = list(sol[0]) self.send_trajectory(joint_positions, duration) - print(f"Computed joint positions: {joint_positions}") + #print(f"Computed joint positions: {joint_positions}") else: print("Inverse kinematics failed") - #print(f"Frequency: {1/(time.time() - time1)} Hz") + print(f"Frequency: {1/(time.time() - time1)} Hz") def send_trajectory(self, joint_positions, duration=4.0): """Publish a joint trajectory command to move the robot.""" diff --git a/workspace/src/joint_control/joint_control/joint_angles_server.py b/workspace/src/joint_control/joint_control/joint_angles_server.py index c5a7733..002c9ac 100644 --- a/workspace/src/joint_control/joint_control/joint_angles_server.py +++ b/workspace/src/joint_control/joint_control/joint_angles_server.py @@ -29,8 +29,10 @@ class ScaledJointTrajectoryPublisher(Node): def joint_angles_handler(self, *args): """Handles incoming OSC messages for joint positions.""" + print(args) if len(args) == len(self.joint_positions): - self.joint_positions = args + self.joint_positions = list(args) + print(self.joint_positions) self.send_trajectory(self.joint_positions) elif len(args) == len(self.joint_positions) + 1: self.joint_positions = args[:-1] @@ -39,11 +41,12 @@ class ScaledJointTrajectoryPublisher(Node): - def send_trajectory(self, joint_positions, duration=3.0): + def send_trajectory(self, joint_positions, duration=0.01): """Publish a joint trajectory command to move the robot.""" msg = JointTrajectory() msg.joint_names = self.joint_names point = JointTrajectoryPoint() + joint_positions = [float(joint) for joint in joint_positions] point.positions = joint_positions # Updated joint positions point.time_from_start.sec = int(duration) point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) @@ -55,8 +58,8 @@ class ScaledJointTrajectoryPublisher(Node): def main(): """Main function to get joint names and start the ROS 2 & OSC system.""" - - tree = ET.parse('/BA/robot.urdf') + robot_urdf = input("Enter the path to the URDF file: ") + tree = ET.parse(robot_urdf) root = tree.getroot() joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic'] diff --git a/workspace/src/joint_control/joint_control/trajectory_server_cart.py b/workspace/src/joint_control/joint_control/trajectory_server_cart.py index 2b998f8..00f08f7 100644 --- a/workspace/src/joint_control/joint_control/trajectory_server_cart.py +++ b/workspace/src/joint_control/joint_control/trajectory_server_cart.py @@ -15,20 +15,26 @@ class ScaledJointTrajectoryPublisher(Node): super().__init__('scaled_joint_trajectory_publisher') self.robot = robot + self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ") + + if self.trajectroy_topic_name == "": + self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' # ROS2 Publisher self.publisher = self.create_publisher( JointTrajectory, - '/scaled_joint_trajectory_controller/joint_trajectory', + self.trajectroy_topic_name, 10 ) # Store received joint positions self.joint_names = joint_names + + self.port = 8000 # UDP port osc_startup() - osc_udp_server("0.0.0.0", 8000, "osc_server") - print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates") + osc_udp_server("0.0.0.0", self.port, "osc_server") + print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates") # Register OSC handler osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK) @@ -41,26 +47,53 @@ class ScaledJointTrajectoryPublisher(Node): joint_positions = [0.0] * len(self.joint_names) steps = 30 vel = 0.4 - if len(args[0]) == len(self.joint_names): + if True: #len(args[0]) == len(self.joint_names): n=2.0 for i in range(len(args)-1): + print(f'i = {i}') x, y, z, roll, pitch, yaw = args[i] + print(1) Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz') + print(2) x, y, z, roll, pitch, yaw = args[i+1] + print(3) Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz') + print(4) cart_traj = rtb.ctraj(Tep1, Tep2, steps) - for j in range(steps-1): + print(cart_traj) + print(5) + for j in range(steps): + print(f'j = {j}') + print(6) sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions) - dist = np.linalg.norm(cart_traj[j].t - cart_traj[j+1].t) - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - joint_positions = list(sol[0]) - point.time_from_start.sec = int(n) - point.time_from_start.nanosec = int((n - int(n)) * 1e9) - n+=dist/vel - n+=0.1 - msg.points.append(point) + print(7) + if sol[1] == 1: + print(8) + if j == 0: dist = vel*n + else: dist = np.linalg.norm(cart_traj[j].t - cart_traj[j-1].t) + print(9) + point = JointTrajectoryPoint() + print(10) + point.positions = list(sol[0]) + print(11) + joint_positions = list(sol[0]) + print(12) + point.time_from_start.sec = int(n) + print(13) + point.time_from_start.nanosec = int((n - int(n)) * 1e9) + print(14) + n+=dist/vel + print(16) + msg.points.append(point) + print(17) + else: print('IK could not find a solution!') + print(18) + self.publisher.publish(msg) + print(19) + print(f"published joint positions {msg.points[-1]}") + print(f'Frequency: {round(1/(time.time()-time1),2)} Hz') +''' elif len(args[0]) == len(self.joint_names) + 1: for i in range(len(args)): x, y, z, roll, pitch, yaw, timetag = args[i] @@ -72,20 +105,18 @@ class ScaledJointTrajectoryPublisher(Node): sol = self.robot.ik_LM(Tep, q0=joint_positions) else: - print("Invalid number or format of arguments") - - self.publisher.publish(msg) - print("published joint positions") - print(f'Frequency: {round(1/(time.time()-time1),2)} Hz') + print("Invalid number or format of arguments")''' + def main(): """Main function to get joint names and start the ROS 2 & OSC system.""" - - tree = ET.parse('/BA/robot.urdf') + path_to_urdf = input("Enter the path to the URDF file: ") + tree = ET.parse(path_to_urdf) root = tree.getroot() joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic'] - robot = rtb.ERobot.URDF('/BA/robot.urdf') + robot = rtb.ERobot.URDF(path_to_urdf) + print(robot) rclpy.init() node = ScaledJointTrajectoryPublisher(joint_names, robot) diff --git a/workspace/src/joint_control/setup.py b/workspace/src/joint_control/setup.py index 4ac5034..3dfd1d9 100644 --- a/workspace/src/joint_control/setup.py +++ b/workspace/src/joint_control/setup.py @@ -25,6 +25,10 @@ setup( 'cart_coords = joint_control.cart_tcp_server:main', 'trajectory_server = joint_control.trajectory_server:main', 'trajectory_server_cart = joint_control.trajectory_server_cart:main', + 'plugdata = joint_control.plugdata:main', + 'plugdata2 = joint_control.plugdata2:main', + 'test=joint_control.test:main', + 'plugdata3 = joint_control.plugdata3:main', ], }, )