nwegy/OSC_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AS: updates for trajectroy control

Browse Source
This commit is contained in:
Alexander Schaefer 2025-04-17 12:41:26 +02:00
parent e00fa0a38a
commit 648007498b
17 changed files with 60 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

11
workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env
View File

@ -1,13 +1,20 @@
AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
COLCON=1
COLCON_PREFIX_PATH=/BA/workspace/install
HOME=/root
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=/
PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
OLDPWD=/BA
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
PWD=/BA/workspace/build/joint_control
PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-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:/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

5
workspace/build/joint_control/joint_control.egg-info/SOURCES.txt
View File

@ -12,11 +12,12 @@ 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/plugdata_cart.py
joint_control/test.py
joint_control/trajectory_server.py
joint_control/trajectory_server_cart.py
joint_control/trajectory_server_cart_fast.py
joint_control/trajectory_server_cart_fast_1.py
joint_control.egg-info/PKG-INFO
joint_control.egg-info/SOURCES.txt
joint_control.egg-info/dependency_links.txt

5
workspace/build/joint_control/joint_control.egg-info/entry_points.txt
View File

@ -2,9 +2,10 @@
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
plugdata_cart = joint_control.plugdata_cart:main
test = joint_control.test:main
trajectory_server = joint_control.trajectory_server:main
trajectory_server_cart = joint_control.trajectory_server_cart:main
trajectory_server_cart_fast = joint_control.trajectory_server_cart_fast:main
trajectory_server_cart_fast_1 = joint_control.trajectory_server_cart_fast_1:main

BIN
workspace/build/joint_control/prefix_override/__pycache__/sitecustomize.cpython-310.pyc
View File

Binary file not shown.

4
workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env
View File

@ -1,4 +1,4 @@
AMENT_PREFIX_PATH=/opt/ros/humble:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control
AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
COLCON=1
COLCON_PREFIX_PATH=/BA/workspace/install
HOME=/root
@ -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=/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
PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/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:/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

7
workspace/build/joint_info/joint_info.egg-info/SOURCES.txt
View File

@ -1,13 +1,6 @@
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

BIN
workspace/build/joint_info/prefix_override/__pycache__/sitecustomize.cpython-310.pyc
View File

Binary file not shown.

5
workspace/install/setup.bash
View File

@ -17,6 +17,11 @@ _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)"

3
workspace/install/setup.ps1
View File

@ -21,6 +21,9 @@ 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"

6
workspace/install/setup.sh
View File

@ -29,6 +29,12 @@ _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"

5
workspace/install/setup.zsh
View File

@ -17,6 +17,11 @@ _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)"

2
workspace/log/latest_build
View File

@ -1 +1 @@
build_2025-04-07_10-59-55
build_2025-04-13_11-50-00

10
workspace/src/joint_angles.py
View File

@ -11,11 +11,11 @@ def main():
# Example joint positions to send
joint_positions1 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
joint_positions2 = [-0.5,-0.6, 0.6,0.0,0.0, 0.0]
joint_positions3 = [-0.5,0.6, 0.6,0.0, 0.0, 0.0]
joint_positions4 = [-0.5,0.6, 0.2,0.0, 0.0, 0.0]
joint_positions5 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
joint_positions1 = [0.3,0.3, 0.2, 0.0, 0.0, 0.0]
joint_positions2 = [0.3,0.1, 0.6, 0.0,0.0, 0.0]
joint_positions3 = [0.3,0.4, 0.6, 0.0, 0.0, 0.0]
joint_positions4 = [0.4,0.3, 0.2, 0.0, 0.0, 0.0]
joint_positions5 = [0.3,0.3, 0.2, 0.0, 0.0, 0.0]
msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2, joint_positions3, joint_positions4, joint_positions5])
osc_send(msg, "osc_client")

BIN
workspace/src/joint_control/joint_control/__pycache__/trajectory_server_cart.cpython-310.pyc
View File

Binary file not shown.

20
workspace/src/joint_control/joint_control/trajectory_server_cart.py
View File

@ -11,9 +11,10 @@ import time
class ScaledJointTrajectoryPublisher(Node):
"""Node to publish joint trajectories based on OSC messages."""
def __init__(self, joint_names, robot):
def __init__(self, joint_names, robot, cost_mask):
super().__init__('scaled_joint_trajectory_publisher')
self.cost_mask = cost_mask
self.robot = robot
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
@ -65,7 +66,7 @@ class ScaledJointTrajectoryPublisher(Node):
for j in range(steps):
print(f'j = {j}')
print(6)
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions, mask = self.cost_mask, joint_limits = True)
print(7)
if sol[1] == 1:
print(8)
@ -118,8 +119,19 @@ def main():
robot = rtb.ERobot.URDF(path_to_urdf)
print(robot)
rclpy.init()
node = ScaledJointTrajectoryPublisher(joint_names, robot)
while True:
try:
print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0) separated by spaces, of which <= {robot.n} are 1): ").split()]
if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
break
else:
print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
except ValueError:
print("Invalid input. Please enter integers only.")
print(f"Cost mask: {cost_mask}")
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask)
# Run both ROS 2 and OSC Server together
try:

4
workspace/src/joint_control/setup.py
View File

@ -25,10 +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',
'trajectory_server_cart_fast = joint_control.trajectory_server_cart_fast:main',
'plugdata = joint_control.plugdata:main',
'plugdata2 = joint_control.plugdata2:main',
'plugdata_cart = joint_control.plugdata_cart:main',
'test=joint_control.test:main',
'plugdata3 = joint_control.plugdata3:main',
],
},
)