AS: updates for trajectroy control

workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env

@@ -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=/
+OLDPWD=/BA
-PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
+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

workspace/build/joint_control/joint_control.egg-info/SOURCES.txt

@@ -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/plugdata_cart.py
-joint_control/plugdata3.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

workspace/build/joint_control/joint_control.egg-info/entry_points.txt

@@ -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
+plugdata_cart = joint_control.plugdata_cart: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
+trajectory_server_cart_fast = joint_control.trajectory_server_cart_fast:main
+trajectory_server_cart_fast_1 = joint_control.trajectory_server_cart_fast_1:main
 

workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env

@@ -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

workspace/build/joint_info/joint_info.egg-info/SOURCES.txt

@@ -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

workspace/install/setup.bash

@@ -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)"

workspace/install/setup.ps1

@@ -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"

workspace/install/setup.sh

@@ -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"

workspace/install/setup.zsh

@@ -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)"

workspace/log/latest_build

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

workspace/src/joint_angles.py

@@ -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_positions1 = [0.3,0.3, 0.2, 0.0, 0.0, 0.0]
-    joint_positions2 = [-0.5,-0.6, 0.6,0.0,0.0, 0.0]
+    joint_positions2 = [0.3,0.1, 0.6, 0.0,0.0, 0.0]
-    joint_positions3 = [-0.5,0.6, 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.5,0.6, 0.2,0.0, 0.0, 0.0]
+    joint_positions4 = [0.4,0.3, 0.2, 0.0, 0.0, 0.0]
-    joint_positions5 = [-0.5,-0.6, 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")

workspace/src/joint_control/joint_control/trajectory_server_cart.py

@@ -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()
-
+    while True:
-    node = ScaledJointTrajectoryPublisher(joint_names, robot)
+        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:

workspace/src/joint_control/setup.py

@@ -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',
         ],
     },
 )