diff --git a/osc_recieve.py b/osc_recieve.py
index 4051558..b9ec47e 100644
--- a/osc_recieve.py
+++ b/osc_recieve.py
@@ -6,7 +6,7 @@ def handler(*args):
 
 osc_startup()
 osc_udp_server("0.0.0.0", 8000, "osc_server")
-osc_method("/coordinates", handler, argscheme=osm.OSCARG_DATAUNPACK)
+osc_method("/joint_angles", handler, argscheme=osm.OSCARG_MESSAGE)
 
 
 while True:
diff --git a/test/joint_states_recording.py b/test/joint_states_recording.py
index 84fbd92..ccea413 100644
--- a/test/joint_states_recording.py
+++ b/test/joint_states_recording.py
@@ -56,8 +56,8 @@ class JointStateLogger(Node):
                 writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
         with open(file_path_cart, mode='a', newline='') as f:
             writer = csv.writer(f)
-            [x,y,z] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:4])).t
-            [roll, pitch, yaw] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:4])).rpy()
+            [x,y,z] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:5])).t
+            [roll, pitch, yaw] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:5])).rpy()
             writer.writerow([timestamp, x, y, z, roll, pitch, yaw])
             
 
diff --git a/test/joint_trajectory_recording.py b/test/joint_trajectory_recording.py
index 3ee8c0b..12b08f6 100644
--- a/test/joint_trajectory_recording.py
+++ b/test/joint_trajectory_recording.py
@@ -59,9 +59,7 @@ class JointTrajectoryLogger(Node):
             for point in msg.points:
                 timestamp = recive_time + point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
                 T = self.robot.fkine(point.positions)
-                x = T.t[0]
-                y = T.t[1]
-                z = T.t[2]
+                [x,y,z] = T.t
                 roll, pitch, yaw = T.rpy()
                 row = [timestamp, x, y, z, roll, pitch, yaw]
                 writer.writerow(row)
diff --git a/workspace/src/joint_angles.py b/workspace/src/joint_angles.py
index f6a801c..42c70cf 100644
--- a/workspace/src/joint_angles.py
+++ b/workspace/src/joint_angles.py
@@ -10,15 +10,14 @@ def main():
     # Make client channels to send packets
     osc_udp_client("172.18.0.3", 8000, "osc_client")
 
-    robot = rtb.Robot.URDF("/BA/ur10e.urdf")
     # Example joint positions to send
-    joint_positions1 = [-0.4,0.6, 0.4, 0.0, 0.0, 0.0]
-    joint_positions2 = [-0.4,-0.6, 0.4, 0.0,0.0, 0.0]
-    joint_positions3 = [-0.4,-0.6, 0.6, 0.0, 0.0, 0.0]
-    joint_positions4 = [-0.4,-0.6, 0.2, 0.0, 0.0, 0.0]
-    joint_positions5 = [-0.4,0.6, 0.2, 0.0, 0.0, 0.0]
+    joint_positions1 = [0.0,0.0, 0.0, 0.0, 0.0, 0.0]
+    joint_positions2 = [-0.4,-1, 0.4, 0.0,0.0, 0.0]
+    joint_positions3 = [-0.4,-1, 0.2, 0.0, 0.0, 0.0]#, 6.0]
+    joint_positions4 = [-0.4,1, 0.2, 0.0, 0.0, 0.0]#, 6.0]
+    joint_positions5 = [-0.4,1, 0.4, 0.0, 0.0, 0.0]#, 6.0]
 
-    msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2])#, joint_positions3, joint_positions4, joint_positions5])
+    msg = oscbuildparse.OSCMessage("/joint_angles", None, [joint_positions1])#, joint_positions3])#, joint_positions4, joint_positions5])
     osc_send(msg, "osc_client")
     osc_process()
     print("Sending joint positions")
diff --git a/workspace/src/joint_control/joint_control/__pycache__/plugdata.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/plugdata.cpython-310.pyc
index f032c6c..93228bb 100644
Binary files a/workspace/src/joint_control/joint_control/__pycache__/plugdata.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/plugdata.cpython-310.pyc differ
diff --git a/workspace/src/joint_control/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc
index 1942a3c..ded605c 100644
Binary files a/workspace/src/joint_control/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc differ
diff --git a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new.cpython-310.pyc
index 1328bf5..b598f5b 100644
Binary files a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new.cpython-310.pyc differ
diff --git a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc
index 976cd89..02821aa 100644
Binary files a/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc and b/workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc differ
diff --git a/workspace/src/joint_control/joint_control/plugdata.py b/workspace/src/joint_control/joint_control/plugdata.py
index 7560a51..accb35d 100644
--- a/workspace/src/joint_control/joint_control/plugdata.py
+++ b/workspace/src/joint_control/joint_control/plugdata.py
@@ -7,6 +7,7 @@ from osc4py3 import oscmethod as osm
 import xml.etree.ElementTree as ET
 import time
 import numpy as np
+import os
 
 class ScaledJointTrajectoryPublisher(Node):
     """Node to publish joint trajectories based on OSC messages."""
@@ -44,15 +45,87 @@ class ScaledJointTrajectoryPublisher(Node):
         # Register OSC handler
         osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
 
+        while True:
+            try:
+                self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
+                self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
+                self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
+                
+                if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
+                    print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
+                    continue
+                
+                if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
+                (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
+                (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
+                    print("Invalid input. Lower limit must be less than upper limit for each axis.")
+                    continue
+                
+                print(f"Current limits:")
+                print(f"x: {self.x_limits}")
+                print(f"y: {self.y_limits}")
+                print(f"z: {self.z_limits}")
+                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
+                if confirm == 'y':
+                    break
+                elif confirm == 'n':
+                    print("Please re-enter the limits.")
+                else:
+                    print("Invalid input. Please enter 'y' or 'n'.")
+            except ValueError:
+                print("Invalid input. Please enter numeric values only.")
+        
+        # Ask the user if they want to set new joint limits
+        update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
+        if update_limits == 'y':
+            for joint_name in self.joint_names:
+                while True:
+                    try:
+                        print(f"Current position limits for joint '{joint_name}': {self.robot.links[joint_name]} rad")
+                        lower_limit = input(f"Enter the new lower limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
+                        upper_limit = input(f"Enter the new upper limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
+                        
+                        if lower_limit is not None and upper_limit is not None and lower_limit >= upper_limit:
+                            print("Invalid input. Lower limit must be less than upper limit.")
+                            continue
+                        
+                        if lower_limit:
+                            self.robot.links[joint_name][0] = float(lower_limit)
+                        if upper_limit:
+                            self.robot.links[joint_name][1] = float(upper_limit)
+                        break
+                    except ValueError:
+                        print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
+            
 
         self.hz = float(input("Enter the desired refresh frequency (Hz): "))
         # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
         self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
 
     def joint_angles_handler(self, *args):
-        """Handles incoming OSC messages for joint positions."""
-        print(f"Received joint angles: {args}")
-        self.desired_joint_positions = [float(i) for i in list(args)]
+    # Ensure the desired joint positions are within the specified limits
+
+        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
+        if self.x_limits[0] is not None:
+            x = max(self.x_limits[0], x)
+        if self.x_limits[1] is not None:
+            x = min(self.x_limits[1], x)
+        if self.y_limits[0] is not None:
+            y = max(self.y_limits[0], y)
+        if self.y_limits[1] is not None:
+            y = min(self.y_limits[1], y)
+        if self.z_limits[0] is not None:
+            z = max(self.z_limits[0], z)
+        if self.z_limits[1] is not None:
+            z = min(self.z_limits[1], z)
+
+        if x != args[0] or y != args[1] or z != args[2]:
+            self.get_logger().warn(
+            f"Desired joint positions adjusted to fit within limits: "
+            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
+            )
+
+        self.desired_joint_positions = [x, y, z, r, p, yaw]
         
 
     def joint_states_callback(self, msg):
@@ -89,8 +162,16 @@ class ScaledJointTrajectoryPublisher(Node):
 
 def main():
     """Main function to get joint names and start the ROS 2 & OSC system."""
-    robot_urdf = input("Enter the path to the URDF file: ")
-    tree = ET.parse(robot_urdf)
+    while True:
+        path_to_urdf = input("Enter the path to the URDF file: ")
+        if os.path.isfile(path_to_urdf):
+            if not path_to_urdf.endswith('.urdf'):
+                print("The file is not a URDF file. Please enter a valid URDF file.")
+                continue
+            break
+        else:
+            print("Invalid path. Please enter a valid 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']
     
diff --git a/workspace/src/joint_control/joint_control/plugdata_cart_fix.py b/workspace/src/joint_control/joint_control/plugdata_cart_fix.py
index a86d62f..a045cce 100644
--- a/workspace/src/joint_control/joint_control/plugdata_cart_fix.py
+++ b/workspace/src/joint_control/joint_control/plugdata_cart_fix.py
@@ -47,22 +47,106 @@ class ScaledJointTrajectoryPublisher(Node):
         print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
         # Register OSC handler
         osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
-
+        set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
+        if set_limits == 'y':
+            while True:
+                        try:
+                            self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
+                            self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
+                            self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
+                            
+                            if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
+                                print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
+                                continue
+                            
+                            if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
+                            (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
+                            (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
+                                print("Invalid input. Lower limit must be less than upper limit for each axis.")
+                                continue
+                            
+                            print(f"Current limits:")
+                            print(f"x: {self.x_limits}")
+                            print(f"y: {self.y_limits}")
+                            print(f"z: {self.z_limits}")
+                            con = True
+                            while con:
+                                confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
+                                if confirm == 'y':
+                                    break
+                                elif confirm == 'n':
+                                    print("Please re-enter the limits.")
+                                    con = False
+                                else:
+                                    print("Invalid input. Please enter 'y' or 'n'.")
+                            if con: break
+                        except ValueError:
+                            print("Invalid input. Please enter numeric values only.")
+                
+                # Ask the user if they want to set new joint limits
+        update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
+        if update_limits == 'y':
+            for i in range(len(self.joint_names)):
+                while True:
+                    try:
+                        lim = self.robot.qlim.copy()
+                        # Find the link corresponding to the joint name
+                        print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
+                        lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
+                        upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
+                        
+                        if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
+                            print("Invalid input. Lower limit must be less than upper limit.")
+                            continue
+                            
+                        if lower_limit:
+                            lim[0][i] = float(lower_limit)
+                        if upper_limit:
+                            lim[1][i] = float(upper_limit)
+                        self.robot.qlim = lim
+                        print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
+                        print("-" * 50)
+                        break
+                    except ValueError:
+                        print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
+                    
 
         self.hz = float(input("Enter the desired refresh frequency (Hz): "))
         # Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
         self.create_timer(1/self.hz, self.update_position)  # Timer to call osc_process periodically
 
     def joint_angles_handler(self, *args):
-        """Handles incoming OSC messages for joint positions."""
-        self.desired_joint_positions = [float(i) for i in list(args)]
+    # Ensure the desired joint positions are within the specified limits
+        print("received joint angles")
+        x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
+        if self.x_limits[0] is not None:
+            x = max(self.x_limits[0], x)
+        if self.x_limits[1] is not None:
+            x = min(self.x_limits[1], x)
+        if self.y_limits[0] is not None:
+            y = max(self.y_limits[0], y)
+        if self.y_limits[1] is not None:
+            y = min(self.y_limits[1], y)
+        if self.z_limits[0] is not None:
+            z = max(self.z_limits[0], z)
+        if self.z_limits[1] is not None:
+            z = min(self.z_limits[1], z)
+
+        if x != args[0] or y != args[1] or z != args[2]:
+            self.get_logger().warn(
+            f"Desired joint positions adjusted to fit within limits: "
+            f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
+            )
+
+        self.desired_joint_positions = [x, y, z, r, p, yaw]
         
 
     def joint_states_callback(self, msg):
         """Callback function to handle incoming joint states."""
         joint_position_dict = dict(zip(msg.name, msg.position))
         self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
-
+        joint_position_dict = dict(zip(msg.name, msg.velocity))
+        self.current_joint_velocities = [joint_position_dict[name] for name in self.joint_names]
 
     def update_position(self):
         if self.desired_joint_positions == self.previous_desired:
@@ -84,8 +168,24 @@ class ScaledJointTrajectoryPublisher(Node):
             if steps < 2: steps = 2
             cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
             for j in range(steps):
-                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True) if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
+                sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
                 if sol[1] == 1:
+                    fowards = self.robot.fkine_all(sol[0])
+                    out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
+                    if np.any(out_of_bounds):
+                        #print(fowards.t)
+                        #indices = np.where(out_of_bounds)[0]
+                        #print(f"indices: {indices}")
+                        self.get_logger().warn("One or more links moved out of bounds!")
+                        '''
+                        for i in indices:
+                            try:
+                                print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
+                            except IndexError:
+                                print(f"index {i} is out of bounds, but no corresponding joint found.")
+                        self.previous_desired = self.desired_joint_positions
+                        '''
+                        break
                     duration = 0
                     prev = self.current_joint_positions if j == 0 else prev_sol
                     for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
@@ -102,8 +202,10 @@ class ScaledJointTrajectoryPublisher(Node):
                     point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
                     msg.points.append(point)
                 else:
-                    print('IK could not find a solution!')
+                    print(f'IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!')
                     prev_sol = self.current_joint_positions
+        if len(msg.points) == 0:
+            return
         msg.header.stamp = self.get_clock().now().to_msg()
         self.publisher.publish(msg)
         self.previous_desired = self.desired_joint_positions
diff --git a/workspace/src/joint_control/joint_control/trajectory_server_new.py b/workspace/src/joint_control/joint_control/trajectory_server_new.py
index 162bcd9..bcdeb07 100644
--- a/workspace/src/joint_control/joint_control/trajectory_server_new.py
+++ b/workspace/src/joint_control/joint_control/trajectory_server_new.py
@@ -63,7 +63,7 @@ class ScaledJointTrajectoryPublisher(Node):
             viapoints = []
             msg = JointTrajectory()
             msg.joint_names = self.joint_names
-            steps_per_m = 1
+            steps_per_m = 4
             for i in range(len(args)-1):
                 x, y, z, roll, pitch, yaw = args[i]
                 x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
@@ -79,7 +79,7 @@ class ScaledJointTrajectoryPublisher(Node):
                         prev_sol = list(sol[0])
                     else: print('IK could not find a solution!')
             dt = 0.01
-            tacc = 0.1
+            tacc = 0.5
             print(f'length viapoints: {len(viapoints)}')
             traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[1 * i for i in self.joint_velocity_limits])
             print(len(traj.q))
@@ -91,7 +91,7 @@ class ScaledJointTrajectoryPublisher(Node):
                 point = JointTrajectoryPoint()
                 point.positions = list(traj.q[i])
                 point.time_from_start.sec = int(traj.t[i])
-                point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
+                point.time_from_start.nanosec = int(((traj.t[i] - int(traj.t[i])) * 1e9))
                 #point.time_from_start = rclpy.duration.Duration(seconds=traj.t[i]).to_msg()
                 msg.points.append(point)
             msg.header.stamp = self.get_clock().now().to_msg()
diff --git a/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py b/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py
index f4a8263..9c8a5fc 100644
--- a/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py
+++ b/workspace/src/joint_control/joint_control/trajectory_server_new_cart.py
@@ -22,6 +22,31 @@ class ScaledJointTrajectoryPublisher(Node):
 
         if self.trajectroy_topic_name == "":
             self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
+        print(f"Using topic name: {self.trajectroy_topic_name}")
+        print("--------------------------------------------------------------------------------------------------------------------------------")
+        while True:
+            try:
+                self.speed = input("Enter your desired speed of the tcp (in m/s): ")
+                if self.speed == '':
+                    self.speed = 1
+                else:
+                    self.speed = float(self.speed)
+                break
+            except ValueError:
+                print("Invalid input. Please enter a number.")
+                continue
+        while True:
+            try:
+                self.t_acc = input("Enter how fast you want the tcp to reach that velocity (in s). \nRemember! If the acceleration time is to short the robot might not be able to accelerate fast enough: ")
+                if self.t_acc == '':
+                    self.t_acc = 2
+                else:
+                    self.t_acc = float(self.t_acc)
+                break
+            except ValueError:
+                print("Invalid input. Please enter a number.")
+                continue
+
 
         # ROS2 Publisher
         self.publisher = self.create_publisher(
@@ -60,44 +85,28 @@ class ScaledJointTrajectoryPublisher(Node):
         try:
             print("Received joint positions")
             viapoints = np.array([list(i) for i in args])
-            print(1)
             msg = JointTrajectory()
-            print(2)
             msg.joint_names = self.joint_names
-            print(3)
             x,y,z = self.robot.fkine(self.current_joint_positions).t
-            r,p,y = self.robot.fkine(self.current_joint_positions).rpy()
-            q0 = [x, y, z, r, p, y]
-            print(4)
-            traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=1, qdmax=[0.1]*len(self.joint_names))
-            print(traj.q)
-            print(5)
+            r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy()
+            q0 = [x, y, z, r, p, yaw]
+            traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=self.t_acc, qdmax=self.speed)
             msg.points = []
-            print(6)
-            prev_sol = q0
+            prev_sol = self.current_joint_positions
             for i in range(len(traj.q)):
-                point = JointTrajectoryPoint()
-                print(8)
-                T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:])
+                T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz')
                 sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True)
-                print(9)
                 if sol[1] == 1:
-                    print(10)
+                    point = JointTrajectoryPoint()
                     point.positions = list(sol[0])
-                    print(11)
                     point.time_from_start.sec = int(traj.t[i])
-                    print(12)
                     point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
-                    print(13)
                     msg.points.append(point)
-                    print(14)
                     prev_sol = list(sol[0])
-                    print(15)
                 else: print('IK could not find a solution!')
-            print(16)
             msg.header.stamp = self.get_clock().now().to_msg()
-            print(17)
             self.publisher.publish(msg)
+            print(f'lenght msg.points: {len(msg.points)}')
 
             print('published')
         except Exception as e:
@@ -114,6 +123,7 @@ def main():
             break
         else:
             print("Invalid path. Please enter a valid path to the URDF file.")
+    print("--------------------------------------------------------------------------------------------------------------------------------")
     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']
@@ -151,6 +161,7 @@ def main():
         except ValueError:
             print("Invalid input. Please enter integers only.")
     print(f"Cost mask: {cost_mask}")
+    print("--------------------------------------------------------------------------------------------------------------------------------")
     node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
 
     # Run both ROS 2 and OSC Server together