AS: cleanup

workspace/src/osc_ros2/osc_ros2/osc_ros2.py

@@ -198,6 +198,7 @@ class OSC_ROS2_interface(Node):
                     break
                 elif update_limits == 'n':
                     self.joint_lim = None
+                    break
                 print("Invalid input. Please enter 'y' or 'n'.")
                 
             print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}')
@@ -343,7 +344,20 @@ class OSC_ROS2_interface(Node):
             self.get_logger().fatal(f"speed_scaling_handler: {e}")
     
     def joint_trajectory_handler(self, *args):
-        pass
+        try:
+                if len(args[0]) == 6:
+                    points = [[float(j) for j in i] for i in args]
+                elif len(args[0]) >= 7:
+                    points = [[float(j) for j in i[:6]] for i in args]
+                    self.get_logger().warn(f"joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
+                else:
+                    self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.")
+                    return
+
+                self.desired = ["joint_trajectory"] + points
+                self.new = True
+        except Exception as e:
+            self.get_logger().fatal(f"joint_trajectory_handler: {e}")
 
     def joint_position_handler(self, address, *args):
         """Handles incoming OSC messages for joint positions."""
@@ -421,7 +435,20 @@ class OSC_ROS2_interface(Node):
     def cartesian_trajectory_handler(self, *args):
         """Handles incoming OSC messages for cartesian trajectory."""
         if self.robot:
-            pass
+            try:
+                if len(args[0]) == 6:
+                    points = [[float(j) for j in i] for i in args]
+                elif len(args[0]) >= 7:
+                    points = [[float(j) for j in i[:6]] for i in args]
+                    self.get_logger().warn(f"cartesian_trajectory_handler: Duration is not supported for cartesian trajectory yet. Ignoring duration.")
+                else:
+                    self.get_logger().warn(f"cartesian_trajectory_handler: Invalid number of arguments for cartesian trajectory. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args[0])}.")
+                    return
+
+                self.desired = ["cartesian_trajectory"] + points
+                self.new = True
+            except Exception as e:
+                self.get_logger().fatal(f"cartesian_trajectory_handler: {e}")
         else:
             self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.")
             return
@@ -774,11 +801,60 @@ class OSC_ROS2_interface(Node):
             self.get_logger().fatal(f"send_tcp_coordinates: {e}")
 
     def send_joint_trajectory(self):
-        pass
-        self.previous_desired = None
+        
+        try:
+            self.new = False
+            viapoints = np.array([i for i in self.desired[1:]])
+            msg = JointTrajectory()
+            msg.joint_names = self.joint_names
+            traj = rtb.mstraj(viapoints, q0 = self.current_joint_positions ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=[self.speed_scaling*i for i in self.joint_velocity_limits.values()])
+            msg.points = []
+            for i in range(len(traj.q)):
+                    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)
+                    msg.points.append(point)
+            msg.header.stamp = self.get_clock().now().to_msg()
+            self.publisher.publish(msg)
+            self.previous_desired = None
+        except Exception as e:
+            print(f'Error in joint_angles_handler: {e}')
 
     def send_cartesian_trajectory(self):
-        pass
+        
+        try:
+            self.new = False
+            viapoints = np.array([i[:6] for i in self.desired[1:]])
+            msg = JointTrajectory()
+            msg.joint_names = self.joint_names
+            x,y,z = self.robot.fkine(self.current_joint_positions).t
+            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=1.5/self.speed_scaling, qdmax=2*self.speed_scaling)
+            msg.points = []
+            prev_sol = self.current_joint_positions
+            n = max(int(len(traj.q)/200), 1)
+            for i in range(len(traj.q)):
+                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)
+                if sol[1] == 1:
+                    point = JointTrajectoryPoint()
+                    point.positions = list(sol[0])
+                    point.time_from_start.sec = int(traj.t[i])
+                    if traj.t[i] == 0:
+                        continue
+                    point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
+                    msg.points.append(point)
+                    prev_sol = list(sol[0])
+                else: self.get_logger().warn(f"send_cartesian_trajectory: IK could not find a solution for (x,y,z) = {traj.q[i][:3]} and (r,p,y) = {traj.q[i][3:]}!")
+            msg.header.stamp = self.get_clock().now().to_msg()
+            msg.points = msg.points[::n]
+            self.publisher.publish(msg)
+            self.previous_desired = None
+        except Exception as e:
+            print(f'Error in joint_angles_handler: {e}')
+
         self.previous_desired = None
 
     def update_position(self):
@@ -966,7 +1042,6 @@ def main():
         print("")
     finally:
         node.destroy_node()
-        rclpy.shutdown()
         osc_terminate()
 
 if __name__ == '__main__':