From e00fa0a38a9da18fd1d6c8f4b12eabd7aed40cbb Mon Sep 17 00:00:00 2001
From: Alexander Schaefer <alex@MacBook-Air-2.fritz.box>
Date: Mon, 7 Apr 2025 19:20:31 +0200
Subject: [PATCH] AS: connection with Pd

---
 .DS_Store                                     | Bin 6148 -> 8196 bytes
 robot.urdf                                    | 542 ------------------
 ros2_ws/install/local_setup.sh                |   2 +-
 ros2_ws/install/setup.bash                    |   5 -
 ros2_ws/install/setup.ps1                     |   3 -
 ros2_ws/install/setup.sh                      |   8 +-
 ros2_ws/install/setup.zsh                     |   5 -
 ros2_ws/log/latest_build                      |   2 +-
 ros2_ws/src/joint_states_control/get_urdf.py  |   2 +-
 .../move_scaled_joint_cart.py                 |   1 -
 .../colcon_command_prefix_setup_py.sh.env     |  14 +-
 .../joint_control.egg-info/SOURCES.txt        |  11 +
 .../joint_control.egg-info/entry_points.txt   |   4 +
 .../__pycache__/sitecustomize.cpython-310.pyc | Bin 293 -> 293 bytes
 .../colcon_command_prefix_setup_py.sh.env     |   7 +-
 .../joint_info.egg-info/SOURCES.txt           |   7 +
 .../__pycache__/sitecustomize.cpython-310.pyc | Bin 287 -> 287 bytes
 workspace/install/setup.bash                  |   5 -
 workspace/install/setup.ps1                   |   3 -
 workspace/install/setup.sh                    |   6 -
 workspace/install/setup.zsh                   |   5 -
 workspace/log/latest_build                    |   2 +-
 workspace/src/joint_angles.py                 |  32 +-
 .../cart_tcp_server.cpython-310.pyc           | Bin 3736 -> 3760 bytes
 .../joint_angles_server.cpython-310.pyc       | Bin 3269 -> 3679 bytes
 .../trajectory_server_cart.cpython-310.pyc    | Bin 4018 -> 4317 bytes
 .../joint_control/cart_tcp_server.py          |   6 +-
 .../joint_control/joint_angles_server.py      |  11 +-
 .../joint_control/trajectory_server_cart.py   |  75 ++-
 workspace/src/joint_control/setup.py          |   4 +
 30 files changed, 127 insertions(+), 635 deletions(-)
 delete mode 100644 robot.urdf

diff --git a/.DS_Store b/.DS_Store
index 8aa99a7c3f90137f572163dea2aab2b62bc76a22..34c963ef83e8d10391bb43c5e54186a8ea78de5f 100644
GIT binary patch
delta 369
zcmZoMXmOBWU|?W$DortDU;r^WfEYvza8E20o2aMA$h$FMH}hr%jz7$c**Q2SHn1@A
zZsuVrVX9|mC}PNGC}xNUlgXYr`N>H+`AHzXKoezw^qT)*0Aw*Ra50nu)f+MxFr+f*
zf!QexX{dS_)}!g+0IM^_u08^&*a?W&LoE6KAIRopC<i*Gh#?#3tOABahGd3RR4W*}
z(X7CtW3mTJ1Ba23g^q%Wk;UW-EcWtP6qN-R<>ln(r2~y)+-%Epk(o<^8_0GA1>9yq
aj_=Hq`9(Y@`}1&cFhatVVRJms9A*IfHeA~P

delta 104
zcmZp1XfcprU|?W$DortDU=RQ@Ie-{Mvv5r;6q~50$jH7iU^g=(`(_@25~htM?Tm}r
pIXDEFfhvGNfE!4-f)sBo{LVa?U&a%pm4OLj49H}L&G9^Qm;rO#5E}ph

diff --git a/robot.urdf b/robot.urdf
deleted file mode 100644
index bf21727..0000000
--- a/robot.urdf
+++ /dev/null
@@ -1,542 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from /opt/ros/humble/share/ur_description/urdf/ur.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="ur10e">
-  <!--
-    Base UR robot series xacro macro.
-
-    NOTE this is NOT a URDF. It cannot directly be loaded by consumers
-    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
-    (but note that .xacro must still be processed by the xacro command).
-
-    This file models the base kinematic chain of a UR robot, which then gets
-    parameterised by various configuration files to convert it into a UR3(e),
-    UR5(e), UR10(e) or UR16e.
-
-    NOTE the default kinematic parameters (i.e., link lengths, frame locations,
-    offsets, etc) do not correspond to any particular robot. They are defaults
-    only. There WILL be non-zero offsets between the Forward Kinematics results
-    in TF (i.e., robot_state_publisher) and the values reported by the Teach
-    Pendant.
-
-    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
-    parameter MUST point to a .yaml file containing the appropriate values for
-    the targeted robot.
-
-    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
-    described in the readme of that repository to extract the kinematic
-    calibration from the controller and generate the required .yaml file.
-
-    Main author of the migration to yaml configs Ludovic Delval.
-
-    Contributors to previous versions (in no particular order)
-
-     - Denis Stogl
-     - Lovro Ivanov
-     - Felix Messmer
-     - Kelsey Hawkins
-     - Wim Meeussen
-     - Shaun Edwards
-     - Nadia Hammoudeh Garcia
-     - Dave Hershberger
-     - G. vd. Hoorn
-     - Philip Long
-     - Dave Coleman
-     - Miguel Prada
-     - Mathias Luedtke
-     - Marcel Schnirring
-     - Felix von Drigalski
-     - Felix Exner
-     - Jimmy Da Silva
-     - Ajit Krisshna N L
-     - Muhammad Asif Rana
-  -->
-  <!--
-    NOTE the macro defined in this file is NOT part of the public API of this
-          package. Users CANNOT rely on this file being available, or stored in
-          this location. Nor can they rely on the existence of the macro.
-  -->
-  <!-- create link fixed to the "world" -->
-  <link name="world"/>
-  <ros2_control name="ur10e" type="system">
-    <hardware>
-      <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
-      <param name="robot_ip">172.18.0.2</param>
-      <param name="script_filename">/opt/ros/humble/share/ur_client_library/resources/external_control.urscript</param>
-      <param name="output_recipe_filename">/opt/ros/humble/share/ur_robot_driver/resources/rtde_output_recipe.txt</param>
-      <param name="input_recipe_filename">/opt/ros/humble/share/ur_robot_driver/resources/rtde_input_recipe.txt</param>
-      <param name="headless_mode">False</param>
-      <param name="reverse_port">50001</param>
-      <param name="script_sender_port">50002</param>
-      <param name="reverse_ip">0.0.0.0</param>
-      <param name="script_command_port">50004</param>
-      <param name="trajectory_port">50003</param>
-      <param name="tf_prefix"></param>
-      <param name="non_blocking_read">True</param>
-      <param name="servoj_gain">2000</param>
-      <param name="servoj_lookahead_time">0.03</param>
-      <param name="use_tool_communication">False</param>
-      <param name="kinematics/hash">calib_5119701370761913513</param>
-      <param name="tool_voltage">0</param>
-      <param name="tool_parity">0</param>
-      <param name="tool_baud_rate">115200</param>
-      <param name="tool_stop_bits">1</param>
-      <param name="tool_rx_idle_chars">1.5</param>
-      <param name="tool_tx_idle_chars">3.5</param>
-      <param name="tool_device_name">/tmp/ttyUR</param>
-      <param name="tool_tcp_port">54321</param>
-      <param name="keep_alive_count">2</param>
-    </hardware>
-    <joint name="shoulder_pan_joint">
-      <command_interface name="position"/>
-      <command_interface name="velocity"/>
-      <state_interface name="position">
-        <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="shoulder_lift_joint">
-      <command_interface name="position"/>
-      <command_interface name="velocity"/>
-      <state_interface name="position">
-        <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">-1.57</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="elbow_joint">
-      <command_interface name="position"/>
-      <command_interface name="velocity"/>
-      <state_interface name="position">
-        <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="wrist_1_joint">
-      <command_interface name="position"/>
-      <command_interface name="velocity"/>
-      <state_interface name="position">
-        <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">-1.57</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="wrist_2_joint">
-      <command_interface name="position"/>
-      <command_interface name="velocity"/>
-      <state_interface name="position">
-        <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="wrist_3_joint">
-      <command_interface name="position"/>
-      <command_interface name="velocity"/>
-      <state_interface name="position">
-        <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <sensor name="tcp_fts_sensor">
-      <state_interface name="force.x"/>
-      <state_interface name="force.y"/>
-      <state_interface name="force.z"/>
-      <state_interface name="torque.x"/>
-      <state_interface name="torque.y"/>
-      <state_interface name="torque.z"/>
-    </sensor>
-    <sensor name="tcp_pose">
-      <state_interface name="position.x"/>
-      <state_interface name="position.y"/>
-      <state_interface name="position.z"/>
-      <state_interface name="orientation.x"/>
-      <state_interface name="orientation.y"/>
-      <state_interface name="orientation.z"/>
-      <state_interface name="orientation.w"/>
-    </sensor>
-    <!-- NOTE The following are joints used only for testing with fake hardware and will change in the future -->
-    <gpio name="speed_scaling">
-      <state_interface name="speed_scaling_factor"/>
-      <param name="initial_speed_scaling_factor">1</param>
-      <command_interface name="target_speed_fraction_cmd"/>
-      <command_interface name="target_speed_fraction_async_success"/>
-    </gpio>
-    <gpio name="gpio">
-      <command_interface name="standard_digital_output_cmd_0"/>
-      <command_interface name="standard_digital_output_cmd_1"/>
-      <command_interface name="standard_digital_output_cmd_2"/>
-      <command_interface name="standard_digital_output_cmd_3"/>
-      <command_interface name="standard_digital_output_cmd_4"/>
-      <command_interface name="standard_digital_output_cmd_5"/>
-      <command_interface name="standard_digital_output_cmd_6"/>
-      <command_interface name="standard_digital_output_cmd_7"/>
-      <command_interface name="standard_digital_output_cmd_8"/>
-      <command_interface name="standard_digital_output_cmd_9"/>
-      <command_interface name="standard_digital_output_cmd_10"/>
-      <command_interface name="standard_digital_output_cmd_11"/>
-      <command_interface name="standard_digital_output_cmd_12"/>
-      <command_interface name="standard_digital_output_cmd_13"/>
-      <command_interface name="standard_digital_output_cmd_14"/>
-      <command_interface name="standard_digital_output_cmd_15"/>
-      <command_interface name="standard_digital_output_cmd_16"/>
-      <command_interface name="standard_digital_output_cmd_17"/>
-      <command_interface name="standard_analog_output_cmd_0"/>
-      <command_interface name="standard_analog_output_cmd_1"/>
-      <command_interface name="analog_output_domain_cmd"/>
-      <command_interface name="tool_voltage_cmd"/>
-      <command_interface name="io_async_success"/>
-      <state_interface name="digital_output_0"/>
-      <state_interface name="digital_output_1"/>
-      <state_interface name="digital_output_2"/>
-      <state_interface name="digital_output_3"/>
-      <state_interface name="digital_output_4"/>
-      <state_interface name="digital_output_5"/>
-      <state_interface name="digital_output_6"/>
-      <state_interface name="digital_output_7"/>
-      <state_interface name="digital_output_8"/>
-      <state_interface name="digital_output_9"/>
-      <state_interface name="digital_output_10"/>
-      <state_interface name="digital_output_11"/>
-      <state_interface name="digital_output_12"/>
-      <state_interface name="digital_output_13"/>
-      <state_interface name="digital_output_14"/>
-      <state_interface name="digital_output_15"/>
-      <state_interface name="digital_output_16"/>
-      <state_interface name="digital_output_17"/>
-      <state_interface name="digital_input_0"/>
-      <state_interface name="digital_input_1"/>
-      <state_interface name="digital_input_2"/>
-      <state_interface name="digital_input_3"/>
-      <state_interface name="digital_input_4"/>
-      <state_interface name="digital_input_5"/>
-      <state_interface name="digital_input_6"/>
-      <state_interface name="digital_input_7"/>
-      <state_interface name="digital_input_8"/>
-      <state_interface name="digital_input_9"/>
-      <state_interface name="digital_input_10"/>
-      <state_interface name="digital_input_11"/>
-      <state_interface name="digital_input_12"/>
-      <state_interface name="digital_input_13"/>
-      <state_interface name="digital_input_14"/>
-      <state_interface name="digital_input_15"/>
-      <state_interface name="digital_input_16"/>
-      <state_interface name="digital_input_17"/>
-      <state_interface name="standard_analog_output_0"/>
-      <state_interface name="standard_analog_output_1"/>
-      <state_interface name="standard_analog_input_0"/>
-      <state_interface name="standard_analog_input_1"/>
-      <state_interface name="analog_io_type_0"/>
-      <state_interface name="analog_io_type_1"/>
-      <state_interface name="analog_io_type_2"/>
-      <state_interface name="analog_io_type_3"/>
-      <state_interface name="tool_mode"/>
-      <state_interface name="tool_output_voltage"/>
-      <state_interface name="tool_output_current"/>
-      <state_interface name="tool_temperature"/>
-      <state_interface name="tool_analog_input_0"/>
-      <state_interface name="tool_analog_input_1"/>
-      <state_interface name="tool_analog_input_type_0"/>
-      <state_interface name="tool_analog_input_type_1"/>
-      <state_interface name="robot_mode"/>
-      <state_interface name="robot_status_bit_0"/>
-      <state_interface name="robot_status_bit_1"/>
-      <state_interface name="robot_status_bit_2"/>
-      <state_interface name="robot_status_bit_3"/>
-      <state_interface name="safety_mode"/>
-      <state_interface name="safety_status_bit_0"/>
-      <state_interface name="safety_status_bit_1"/>
-      <state_interface name="safety_status_bit_2"/>
-      <state_interface name="safety_status_bit_3"/>
-      <state_interface name="safety_status_bit_4"/>
-      <state_interface name="safety_status_bit_5"/>
-      <state_interface name="safety_status_bit_6"/>
-      <state_interface name="safety_status_bit_7"/>
-      <state_interface name="safety_status_bit_8"/>
-      <state_interface name="safety_status_bit_9"/>
-      <state_interface name="safety_status_bit_10"/>
-      <state_interface name="program_running"/>
-    </gpio>
-    <gpio name="payload">
-      <command_interface name="mass"/>
-      <command_interface name="cog.x"/>
-      <command_interface name="cog.y"/>
-      <command_interface name="cog.z"/>
-      <command_interface name="payload_async_success"/>
-    </gpio>
-    <gpio name="resend_robot_program">
-      <command_interface name="resend_robot_program_cmd"/>
-      <command_interface name="resend_robot_program_async_success"/>
-    </gpio>
-    <gpio name="hand_back_control">
-      <command_interface name="hand_back_control_cmd"/>
-      <command_interface name="hand_back_control_async_success"/>
-    </gpio>
-    <gpio name="zero_ftsensor">
-      <command_interface name="zero_ftsensor_cmd"/>
-      <command_interface name="zero_ftsensor_async_success"/>
-    </gpio>
-    <gpio name="system_interface">
-      <state_interface name="initialized"/>
-    </gpio>
-    <gpio name="get_robot_software_version">
-      <state_interface name="get_version_major"/>
-      <state_interface name="get_version_minor"/>
-      <state_interface name="get_version_build"/>
-      <state_interface name="get_version_bugfix"/>
-    </gpio>
-  </ros2_control>
-  <!-- Add URDF transmission elements (for ros_control) -->
-  <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
-  <!-- Placeholder for ros2_control transmission which don't yet exist -->
-  <!-- links -  main serial chain -->
-  <link name="base_link"/>
-  <link name="base_link_inertia">
-    <visual>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/base.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/base.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="4.0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
-    </inertial>
-  </link>
-  <link name="shoulder_link">
-    <visual>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/shoulder.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/shoulder.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="7.369"/>
-      <origin rpy="1.570796326794897 0 0" xyz="0.021 -0.027 0.0"/>
-      <inertia ixx="0.03408" ixy="2e-05" ixz="-0.00425" iyy="0.03529" iyz="8e-05" izz="0.02156"/>
-    </inertial>
-  </link>
-  <link name="upper_arm_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/upperarm.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/upperarm.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="13.051"/>
-      <origin rpy="0 0 0" xyz="-0.2327 0.0 0.158"/>
-      <inertia ixx="0.02814" ixy="5e-05" ixz="-0.01561" iyy="0.77068" iyz="2e-05" izz="0.76943"/>
-    </inertial>
-  </link>
-  <link name="forearm_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/forearm.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/forearm.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="3.989"/>
-      <origin rpy="0 0 0" xyz="-0.33155 0.0 0.068"/>
-      <inertia ixx="0.01014" ixy="8e-05" ixz="0.00916" iyy="0.30928" iyz="0.0" izz="0.30646"/>
-    </inertial>
-  </link>
-  <link name="wrist_1_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/wrist1.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/wrist1.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="2.1"/>
-      <origin rpy="1.570796326794897 0 0" xyz="0.0 -0.018 0.007"/>
-      <inertia ixx="0.00296" ixy="-1e-05" ixz="0.0" iyy="0.00222" iyz="-0.00024" izz="0.00258"/>
-    </inertial>
-  </link>
-  <link name="wrist_2_link">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/wrist2.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.12"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/wrist2.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="1.98"/>
-      <origin rpy="-1.570796326794897 0 0" xyz="0.0 0.018 -0.007"/>
-      <inertia ixx="0.00296" ixy="-1e-05" ixz="0.0" iyy="0.00222" iyz="-0.00024" izz="0.00258"/>
-    </inertial>
-  </link>
-  <link name="wrist_3_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/visual/wrist3.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur10e/collision/wrist3.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.615"/>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.026"/>
-      <inertia ixx="0.0004" ixy="0.0" ixz="0.0" iyy="0.00041" iyz="0.0" izz="0.00034"/>
-    </inertial>
-  </link>
-  <!-- base_joint fixes base_link to the environment -->
-  <joint name="base_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="world"/>
-    <child link="base_link"/>
-  </joint>
-  <!-- joints - main serial chain -->
-  <joint name="base_link-base_link_inertia" type="fixed">
-    <parent link="base_link"/>
-    <child link="base_link_inertia"/>
-    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
-           frames of the robot/controller have X+ pointing backwards.
-           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
-           link/frame) to introduce the necessary rotation over Z (of pi rad).
-      -->
-    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-  </joint>
-  <joint name="shoulder_pan_joint" type="revolute">
-    <parent link="base_link_inertia"/>
-    <child link="shoulder_link"/>
-    <origin rpy="0 0 0" xyz="0 0 0.1807"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
-    <safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="shoulder_lift_joint" type="revolute">
-    <parent link="shoulder_link"/>
-    <child link="upper_arm_link"/>
-    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
-    <safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="elbow_joint" type="revolute">
-    <parent link="upper_arm_link"/>
-    <child link="forearm_link"/>
-    <origin rpy="0 0 0" xyz="-0.6127 0 0"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
-    <safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-2.991592653589793" soft_upper_limit="2.991592653589793"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="wrist_1_joint" type="revolute">
-    <parent link="forearm_link"/>
-    <child link="wrist_1_link"/>
-    <origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="54.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
-    <safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="wrist_2_joint" type="revolute">
-    <parent link="wrist_1_link"/>
-    <child link="wrist_2_link"/>
-    <origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="54.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
-    <safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="wrist_3_joint" type="revolute">
-    <parent link="wrist_2_link"/>
-    <child link="wrist_3_link"/>
-    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="54.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
-    <safety_controller k_position="20" k_velocity="0.0" soft_lower_limit="-6.133185307179586" soft_upper_limit="6.133185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <link name="ft_frame"/>
-  <joint name="wrist_3_link-ft_frame" type="fixed">
-    <parent link="wrist_3_link"/>
-    <child link="ft_frame"/>
-    <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
-  </joint>
-  <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
-  <link name="base"/>
-  <joint name="base_link-base_fixed_joint" type="fixed">
-    <!-- Note the rotation over Z of pi radians - as base_link is REP-103
-           aligned (i.e., has X+ forward, Y+ left and Z+ up), this is needed
-           to correctly align 'base' with the 'Base' coordinate system of
-           the UR controller.
-      -->
-    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="base"/>
-  </joint>
-  <!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
-  <link name="flange"/>
-  <joint name="wrist_3-flange" type="fixed">
-    <parent link="wrist_3_link"/>
-    <child link="flange"/>
-    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
-  </joint>
-  <!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
-  <link name="tool0"/>
-  <joint name="flange-tool0" type="fixed">
-    <!-- default toolframe - X+ left, Y+ up, Z+ front -->
-    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
-    <parent link="flange"/>
-    <child link="tool0"/>
-  </joint>
-</robot>
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 0123d59340c4f58db0faac58b7be887b97c5b59b..e7d083a46329f6bcb4fe817734bfcfab50f8a4c5 100644
GIT binary patch
delta 20
acmZ3=w3LZEpO=@50SL4=eBQ_{#Rvc|2n3P<

delta 20
acmZ3=w3LZEpO=@50SLk@9&Y58Vgvv!3j{L&

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 4e7b0719a592bfe19571d3fc586c975335dd1631..041af0f1a7ad01af3028dc4eb49b339932b906e8 100644
GIT binary patch
delta 20
acmbQwG@pq(pO=@50SFwbo^Iq8VFUmzAp}tX

delta 20
acmbQwG@pq(pO=@50SLk^9&Y3oVFUmyA_O7;

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 6db002c94b4eb569e17e2c6569fc9c8c0dae660c..34c1f9aabb9c59449c14fc909794551cb27038df 100644
GIT binary patch
delta 502
zcmY*WL2DE-7|oYtCYhbdBHLQKEVYAJVJW4kJ$aE{+Ir|gMeraDLo=?k?T$7xDlQ2<
zEO>GenTsI8?mtlWf9S#69zFOw{02cZ@Vzg2yyU$kA2+^isOL(x2##-We-6Y~b<4X@
zxDr^0SmiH-ip<EII9h>)2_TXooxmwI>lU8)jydi;=R4t~1{V~)|8<&PA*kI!t@u}q
zO21;7O#q1I4@t#mo@JI_SZ~fMKJ<pZ^{udSz_h3RDP1M5h1OyyZPTe1Q|dw+YT6|x
zsREqIE}4LfESa@zWP>>cQgAd08WZd-P5)fR(sj}!pNI}W{|(TO#=A#pQTKhb*S%M~
z%1oKXr+JZ$(keeG<ELqUmg#YLV6yX<S#k2_PCRDu(YPNjd6kc{`iH;2qD&88Y{^qI
zD3@MC->hSCeC;Z-l6K${_^<}6ASs9ItfM52pbe76u$YPu0DJXjunF7s-QYcJFTMw_
zDI6~jqubEQnsa56jMBVFlKz@`T)$Jr%A<x3%NLk2J;S8W%#B*LcQ4;Z-fT6{{H1Aj
KoBjGh``|Y%K6i5f

delta 440
zcmY+Bze)r#5XLi;-E4MmSL8fT^!x*&!r2H4Y9}ISAz~qjg>o)N&s)7yb1Q-*SX?a@
zX#^3puodOr!3VIk6B{4Hte|LMX1)o($(N9)f$M?jD2f7N7tX&|`ExYsUWB&uY%JMm
zzS@)$1ud|dB(@%)EGXDy8a369b~V$U0L>ev|D3=cV%f4~y}zs_S&D+T0ElqbNJ{S<
zg^DWYo~AT$_7YdQiYc#3g)7)47Krw(HA_5|RS@1K>Ochodx_jhxmCQEv_R(+x${*{
z@p}YfEv&vFxA<#>BKXneoS9{3tZ(6{etc@4dAE6FB`gQ#gUyGbEZJATIkBuZ&5XA)
z+;7`F?F0rzC;&$XMrc0b7(xhn2=RQ{Y{6jr#XA9*G_(E?jGGPr3dY-?{u#n@dpn$h
z{7!Z#uT<)Btx>6zi*nsOM2)VsY&+LHj?-FvSdY_vIcml_CW@=ycSvjXU77uc%*N%M
INjet40OF`)+yDRo

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 8c4e103b692113fbbfe005735150204ae326a869..de62ed32ad28d80aa64939ce9f1e9fa232b85d02 100644
GIT binary patch
delta 1143
zcmaJ<TWb?R6rP#AO*Xq}tMSs{C2Cbly&#H3rAobmC{(M6h%B*_*2bFXW>gwC^dSm@
z_#mS%im(9@R1k~5L45K_A0!AO2ohApU!dP?st+PgIN#3szH?4y&Y73JAC_b)zV8yO
zukXK49P?*0cNu-E9<s}H$BUfthR!^KCP_paloCnx7f4F1LC>*pKoZeW46Q^YV2Djr
zL=%B13O_bA*VGk=Ff=F3(Z8w*x`?3Sk%$UYSl3xE@&XpEpGyX9^^zt_O4uFmK6#M*
zfi9-4-%;nRZBCov$ZbP4t%Yl?Z~>gKTSE_^Wll|2Wvi=NEQ>&fg;H@$R>EoZ&RR1+
z2c5Hdlv5W!PCe==#~$Wav(FBEtX<osM(o_oa|B7!Aaz61&eiqRA_Etvo$E>istIrI
zqQbg{_U$DNT1WdLGtioyw9|@k{>UuZfdkI@&B=scOa#v&E73Xh6!bw78KqS0MrP7Y
zDqNj}Bcx+5GUm86eE&~#JT%5vEqidJoyOtYo(3N<MghN<?wJW2g;leF2~<68*J)?=
zx7kHDO2VZO`f1A=pQ;q(V>Rur^#^r=&d!qBZuP_2pl03d(sl++y9+P@BgNORhueAx
zZ9N2)bNc84wcR<T?m1fr`ao@jRaXLR#?g9CZCEBj54*)M4~qKY4AM1f6(6E~s=$Y4
zdO`<DW3WHV!j8lCz^ZQnoi|#?dy82_n#mYqqe<#aFkxUgEH(xDnT*JgR%|y3o|zM2
zfOWIBvz7(Nn<U&UQw^<HCG=ld!oD@5-(w<HSDzqd-y||UJxmb<-s?>CBq?`YAty*=
z#(q-RiZjW#OBWM-^uOA_v#yZ-RBiX+g1VC$Ub$jlSr)^*JX_3PD9E#USxNTCkM7-{
zAD^5mZqKVvsXMuF0KG^i*nwr*-2gbKn(k`4UcGXc^X`^4S-x;ds?Y9C-)gyG<xE8e
zmqIbFs_Aw9K7?1d06250dXnBW?=+ki4}$ZB$#M|%_k<(rTlz-#A<)TaLLCt9RM)+2
L&JbVZQuF68lF;>N

delta 848
zcmaJ;&1(}u6rVRU*^lgQ(#A+zRNA0cBGRY`{g9|_dh`o=2%eUjX-gs|FqzsmY{(&s
zig+45Rbd03L=WQMAYSari}WHMz4;&1H``J{#J9XR?|a|xz3<0Ajb&OvkS0i<D?ir0
zc^@(_DZJHh=!1{P*jMOnVnmS$`hb!&@V!T6K7xpZ$T2m(0RRzn>90HiBLs8jA`n!t
zM|2G9|J0zDjgbKbqL|?O);4)f`v<*DzhbJRw^K8hZvNau>ZBLf<+7@`nsXCHE{AY1
zIgDaA+g@pkP|4-SN=>!omOP4HJz`C|CMQQN7}B?``5qcb&b1ob*AkgR$&o9_?FI@(
z`lQPYtcZo6C|WpP#6zZdgyTiLkqRxN@7tq9K%N}<?T9s;*p0YZ3HC_GUc`*9d~w9=
zDmcW*5Fgidd)i&Yzmk=moq64NPU&|}l_vnGJ6$MX<ScSKhr*%`Sorxc3O@BA51lFf
zo{#INZVpcBS8f>!de<%Yd}(8P4f&7^L<c#6`2`h4Vi0(_E{++~0qG`JdWe^f@g4(M
zL`YqP7^emV$F~ghF6&(B<k4pjh%70mZz8<mMogrH_1xUj#CoJ%sFG&DiAryk0h2J`
z3EQ0Z4ryjLNR>n^_LIgu4w5%xnh9s=zp;N;ZIGhdS-7!WZ%(YPH)~3t@Mg1_(!zX6
zw(hpn)VdU_deeIWMg7Y@17$twpN7l&x<3wC-S(dW*IsbWo58KLFQP~@y%=1`-9mNO
l3B&u?I1G!!vZ5acPlm3dJK3)^39_WeGqd)2XCw`L?>EM&wxa+5

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 e2538627825b62b343c23577a576e481b1fd9f12..6277c3913e6d786b25c9a127de0275efeefcc8c8 100644
GIT binary patch
delta 2172
zcmaJ?U1(fI6rQ>N`~Q>O&2EyWH?b{l)fz!)8*AFyA6u$L+Dfgcmt^O*+1t(D&D>er
zEOS{Q1wjm2F9>ZZY<v?`&?m(=LGY#Z!M7{EyMhS5`Jna8B<YV2&SlP=`OY_U4>RY?
z`D*(wQ&~TgNg`->_xyh8RQY;#0^hv;L|9B9Vv)o(^m@e|CCSSQJ@w_+6^!4De#GTI
zQ;D*QIdU|PJdLPet`N}0e4sN!qWZcDh{*-dB7ivwmRv;`u@o_Zks#Kp3bQmxu#A|m
z;#-I$Z>y^q&@4#-mScIZK+<c#Bj_xuXHLgcK_hxbu}>a9)@ClX*`jMRzte2k?fE64
z+E*5vjYWxjezVPNM&~`Z!F;-62aA5UMeGIF4qRdJXYCk8uPD#$YaZH;P|uv%BfmK5
z2Ms51>Dw;tjh&UF9k6-ITw?oedy_WX`#kzyxx2R*>p{2U(%5pGX1mE8C)TOI;Imk7
zwmV%Gmp6T&{)!`=b%e9gXX=cub$rUApVgh38Za8&Q;!%WxDA?%PH1oK&Vnj>V1r0w
zLrLPKVqi^Gu!a?E{B4+qVklW$iGI-bXDh(6puY$`_&Cbx`;Wps8llN_rg6NAiCIOf
z_$Yewuh3$cSlqgWZo^e#1lMHM&>KdW2$NwdOoy2;3k*}(q9@JqLV_gMloASafTkq+
zg<dl;NpCUwTZ}v~G6H3ZL6YZqo<k^v1%`P+qL^gmsB>NV@=;iX6**qq@&#sj!GHir
zd1+qY#Tw!oDXb~s2-u3!HbTrABIae~Lrlj29qCKBNJ<cm-*SVS;B2X#XBjU$$Zc%m
z<;5rtp@zJC6|Lj21WU`(ox&;#%P_0RS&@&BQVkA^mmTrb(MQox1FR~s5{E<LWv^d$
zDgf5DKqd`UM8-;T8?Wq)f_+@rxyei17)N^%+zloZ5>vhEpvKGV;^Z*dCa~rAL7Uvr
zmUn@^T{4k3HW=sXCes!e!?R?H!xm(^h8{Y|$Jhih85*E<#FZ)<M{s!x<k^mmJR>_H
zt9C`-nQs5wC5Rf)4h=ELp4POVw`29{O0SM%-KWH*Lj?>&RR~Q8UGhBO4GmC%r!3D5
zO<@vYNX0(GgwVtjGfWAQ79s;e{?5H~=kQQO`0&}hkOdIEZ6{CJ4VkHJpV=3iZDP;c
zf#2$~rr(|#76oxc5XYhqtlCrd@E){Y>Ns_myKcL&GHdr#`$X?_eOs)t=8{XRK&HZN
zT8e(McH><1kL6Xx7O~msF0`7#qR7~8o75|GGI}S`@6#zkmttCI;~?tL0}q6ySlGZ8
z&O;W8Jk)RrXQ5ko2lVRZn1#{8X|Pz*CdMMRL5j6vmfq}Q#fA>~jg?=mnl7fLC`>Na
z#p%f3#~H_IwC01L;kH_i^N@$|b|E?)WmDDuI2Fg=fPTM;s}a|61bXlr2!;r}1tG-D
zK;a?@FvkOR9fb-}h<XhIWuOsl9o`9GsZ95b0X*2i)0o9I8Euf+>+&h*Dj-Uz_vU&f
z$oGDF9nm?Kk?(xM%W|FQ#8^}0V*s{b<~(X=G{kZ%FJagKaBJg{xq#ZaE9gAp`XDd2
zP7DfiEQ-B|b@qR4TdR`b!{)MRGX3)O)PpL~nP*TId|4S@J#+Lp1XjzPwWD78lkrC{
zJ5C;ekYEjesWaD&_GWh0m*Lbd-Mk^%HwPiiQzESVE`u7;pd!Fe*GH%b;AkZ?IeHM3
z*f@4ZmH<usOGaSyY36YBN9OT!Mw}MTKq+v#lw5=$rLG%m)b|<Pc>Ki%THA+|`;TYZ
z(ma$DFbDfUSWpi{Y(cO@7qWYb?}~L$gapZP>Q&l{Ze(A>6Vad9r~6D8bFwZq8v*nE
z)`EYPN?%g}_|u$kErD0`fZ*xv?owxkz5qJiFNBCHD)y(Y5D(nCpCYk!V96)lmOF=m
cAuE;zH7hS;yNIJfE}2x$S$V6P#M-@o0SZF>Q~&?~

delta 1870
zcma)7O^g&p6t3$3>6z~7nVnyDfg$oYV%Uuum6ag7@~4R$00Dn+n4M~t8D^$;t80**
zRA&+g5-*!T`hpmf856H2n0U~5(Zq`ejn_^z#*5+L)dPvaSH0{a=)tPISFh{6`upm8
z)jx0hY^&{AmWiNzef77++s+Mp3g5i3Cvb>C5?9fi1-D4dOA>wc_Ll{W&xSwY{4tJ3
zGE;~|<gv_AP30K0#`Gb=h#5p7D$#~A*a@N!F|;NzIBF$Xs%4SHDtHJwiK=FJUfL1<
zD9z6H^_jhKZ64f;P-J?3qvpHxvP=8lpN!jz&+3%9q~die4`OFun3<Ug=j0!-9Xd)m
zJgQ8GT-hFeqReX<I1jbMKa_WNSzy{AehE|xYmy>MSXAvVP1mFpHgG}e?+VZ1E#XbI
zV2dyV<fs_)Y50}8Z=NX}6Sa(n7%bG1hZq3)j%b0-RN)(9qL;vX2%Wnb7=VURMsI6D
zA~1twkP5884)<x3+W^p2DTjizh#-0q{a4grJ<4H;k;&8C;p$ipGEC*_#wy2ytiZFC
z;u)STBd!p0RSG7U)v`zFwZ4`k+O<_$P6>^1kYkx43i2$=C)Q5G^R<bGUxC7MTptWE
z&$sfmg7^?J#3e+MIXGV2U*tN^acvSkj{=?RBo()Et28R}{1`5Kd1ZN~yYz3!#8xgz
z*D-x=9kdv*(;I;4F<^-Yhy?|KS#=$a;zP`cMeJ+R|G=)ejvddy%_M{31I&u+cwNWK
zyO)3IDh!jf;3@bwjV8G)jsbm6%w0?}Y>Ma%jnEizse~pG$quCt5t;Z1m;hY1fXli5
z)5q%JTSomN#Xy`z5K#`^6<?8jWu;%mk?K+6QZS_DnK;6|b7$`NTOWw>#Y@NU-iOlP
z@k;k{z1<|0Zm+Z8(uzkb7d+ajvx;}2Qm2c(j@xB^zZ`mr@}7S32z8fxZnv>ATe&O2
zT79{4tY6(6DXiIXX%2d*EL0}^Jn<xC%CCu5$q{?fmwF5BrhhR~sn_e0YKfM^1#>Wn
zbTN|E7bE$UyF{nN;8<-IRsqx#ApU7khGbw3^2fmn)B;Ej>ULZLv7tAIE8w?r3Tg?&
z09*P~x;K?|AbLA^MVtX~(tYWkVWwa{Whd@G6DQ;Q$-@JYyz!cmRjW1Hb>DBe?RKs9
zk8N!dA@SaHuu>qtNM8dfu%eTvqPGFEQB<%*#LFOl1PZPe6ATFg1Q<zU6kzeRT}2!Z
zMlw;xkk>;TNK9#|BP|Y;nLaYc;<Za~iOWMNQ2Gb@Ip{UV2ne2xUqT)7GkyzKC5ViD
z{_ZoVYbl6XD~B+QhkkqQjeG-j)9<1)kjtZVJhMJ>#xIHhNLnD7aq@BJBfdw5#h$QW
zy_UA8=jW!Wx8Sk8JxVTw*Q}2xAHC7Y+z%{Z4X?9w;7mAg@7R1EAfe)pfv8Er1+T~8
zHEu-ccr_bdx3^@U0%xQhJ|X6fl-ExjIud?s&#5unLM^>}UIqG5*NqhFd5o?-#$t@p
z^&kNr+kn!kH@hdm?LQ03lm#DB;d|-5*~?;#QLBl7S`8T53cpF8#l>*anH~_~Ovh(y
zHhkuJ?FH`&jklKcm+GupZ$r{tq*W28LNYC_&}p#gPN4)VsF<Ig66)cWHmDvM`#T=#
iwcP_Eh5ap{4ETRIz}FPc;;aFNsE%YDF^VQu?*9!;^{l-B

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