remove gitignore

2025-04-22 17:10:24 +02:00
parent 97cd67a008
commit 33f21d3096
519 changed files with 18046 additions and 0 deletions
--- a/workspace/build/joint_control/build/lib/joint_control/init.py
+++ b/workspace/build/joint_control/build/lib/joint_control/init.py
--- a/workspace/build/joint_control/build/lib/joint_control/cart_tcp_server.py
+++ b/workspace/build/joint_control/build/lib/joint_control/cart_tcp_server.py
@@ -0,0 +1,108 @@
+import rclpy
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from osc4py3.as_eventloop import *
+from osc4py3 import oscmethod as osm
+import roboticstoolbox as rtb
+import spatialmath as sm
+import xml.etree.ElementTree as ET
+import time
+
+class ScaledJointTrajectoryPublisher(Node):
+    """Node to publish joint trajectories based on OSC messages."""
+    def __init__(self, robot, joint_names):
+        super().__init__('scaled_joint_trajectory_publisher')
+        
+        # ROS2 Publisher
+        self.publisher = self.create_publisher(
+            JointTrajectory, 
+            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            10
+        )
+
+        # Store received joint positions
+        self.joint_positions = [0.0] * len(joint_names)
+        self.joint_names = joint_names
+        self.robot = robot
+
+        osc_startup()
+        osc_udp_server("0.0.0.0", 8000, "osc_server")
+        print("Server started on 0.0.0.0:8000 \nready to receive messages in the following format: /tcp_coordinates [x, y, z, roll, pitch, yaw] optional: duration as last argument: /tcp_coordinates [x, y, z, roll, pitch, yaw, duration]")
+        # Register OSC handler
+        osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
+
+    def tcp_coordinates_handler(self, *args):
+        """Handles incoming OSC messages for tcp position."""
+        time1 = time.time()
+        if len(args) == len(self.joint_positions):
+            x, y, z, roll, pitch, yaw = args
+            duration = 4.0  # Default duration
+        elif len(args) == len(self.joint_positions) + 1:
+            x, y, z, roll, pitch, yaw, duration = args
+        else:
+            print("Invalid number of arguments")
+            return
+
+        # Create the desired end-effector pose
+        Tep = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+
+        # Compute the inverse kinematics to get the joint angles
+        #time1 = time.time()
+        #sol = self.robot.ikine_LM(Tep)
+        #print(f"Time taken for ERobot: {time.time() - time1}")
+        #time1 = time.time()
+        #sol = self.robot.ikine_LM(Tep, q0=self.joint_positions)
+        #print(f"Time taken for ERobot with initial guess: {time.time() - time1}")
+        #time1 = time.time()
+        #sol = self.robot.ets().ikine_LM(Tep)
+        #print(f"Time taken for ETS: {time.time() - time1}")
+        #time1 = time.time()
+        sol = self.robot.ik_LM(Tep, q0=self.joint_positions)
+        #print(f"Time taken for ETS with initial guess: {time.time() - time1}")
+        if sol[1]:
+            joint_positions = list(sol[0])
+            self.send_trajectory(joint_positions, duration)
+            #print(f"Computed joint positions: {joint_positions}")
+        else:
+            print("Inverse kinematics failed")
+        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."""
+        msg = JointTrajectory()
+        msg.joint_names = self.joint_names
+        point = JointTrajectoryPoint()
+        point.positions = joint_positions  # Updated joint positions
+        point.time_from_start.sec = int(duration)
+        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
+        
+        msg.points.append(point)
+        self.publisher.publish(msg)
+        print(f"Updated joint positions: {joint_positions}")
+
+def main():
+    """Main function to get joint names and start the ROS 2 & OSC system."""
+
+    tree = ET.parse('/BA/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']
+    robot = rtb.ERobot.URDF('/BA/robot.urdf')
+
+    rclpy.init()
+
+    node = ScaledJointTrajectoryPublisher(robot.ets(), joint_names)
+    
+    # Run both ROS 2 and OSC Server together
+    try:
+        while rclpy.ok():
+            osc_process()  # Handle one OSC request at a time
+            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
+    except KeyboardInterrupt:
+        print("")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        osc_terminate()
+
+if __name__ == '__main__':
+    main()
--- a/workspace/build/joint_control/build/lib/joint_control/joint_angles_server.py
+++ b/workspace/build/joint_control/build/lib/joint_control/joint_angles_server.py
@@ -0,0 +1,80 @@
+import rclpy
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from osc4py3.as_eventloop import *
+from osc4py3 import oscmethod as osm
+import xml.etree.ElementTree as ET
+
+class ScaledJointTrajectoryPublisher(Node):
+    """Node to publish joint trajectories based on OSC messages."""
+    def __init__(self, joint_names):
+        super().__init__('scaled_joint_trajectory_publisher')
+        
+        # ROS2 Publisher
+        self.publisher = self.create_publisher(
+            JointTrajectory, 
+            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            10
+        )
+
+        # Store received joint positions
+        self.joint_positions = [0.0] * len(joint_names)
+        self.joint_names = joint_names
+
+        osc_startup()
+        osc_udp_server("0.0.0.0", 8000, "osc_server")
+        print("Server started on 0.0.0.0:8000 \nready to receive messages in the following format: \n   /joint_angles [joint_positions]; optional: duration as last element, default is 3sec")
+        # Register OSC handler
+        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
+
+    def joint_angles_handler(self, *args):
+        """Handles incoming OSC messages for joint positions."""
+        if len(args) == len(self.joint_positions): 
+            self.joint_positions = args
+            self.send_trajectory(self.joint_positions)
+        elif len(args) == len(self.joint_positions) + 1:
+            self.joint_positions = args[:-1]
+            self.send_trajectory(self.joint_positions, args[-1])
+            print(f'Duration: {args[-1]}')
+
+
+
+    def send_trajectory(self, joint_positions, duration=3.0):
+        """Publish a joint trajectory command to move the robot."""
+        msg = JointTrajectory()
+        msg.joint_names = self.joint_names
+        point = JointTrajectoryPoint()
+        point.positions = joint_positions  # Updated joint positions
+        point.time_from_start.sec = int(duration)
+        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
+        
+        msg.points.append(point)
+        self.publisher.publish(msg)
+        print(f"Updated joint positions: {self.joint_positions}")
+
+
+def main():
+    """Main function to get joint names and start the ROS 2 & OSC system."""
+
+    tree = ET.parse('/BA/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']
+
+    rclpy.init()
+
+    node = ScaledJointTrajectoryPublisher(joint_names)
+    
+    # Run both ROS 2 and OSC Server together
+    try:
+        while rclpy.ok():
+            osc_process()  # Handle one OSC request at a time
+            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
+    except KeyboardInterrupt:
+        print("")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        osc_terminate()
+
+if __name__ == '__main__':
+    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server.py
@@ -0,0 +1,78 @@
+import rclpy
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from osc4py3.as_eventloop import *
+from osc4py3 import oscmethod as osm
+import xml.etree.ElementTree as ET
+
+class ScaledJointTrajectoryPublisher(Node):
+    """Node to publish joint trajectories based on OSC messages."""
+    def __init__(self, joint_names):
+        super().__init__('scaled_joint_trajectory_publisher')
+        
+        # ROS2 Publisher
+        self.publisher = self.create_publisher(
+            JointTrajectory, 
+            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            10
+        )
+
+        # Store received joint positions
+        self.joint_positions = []
+        self.joint_names = joint_names
+
+        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 [joint_positions] optional: duration as last argument")
+        # Register OSC handler
+        osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
+
+    def joint_angles_handler(self, *args):
+        """Handles incoming OSC messages for joint positions."""
+        msg = JointTrajectory()
+        msg.joint_names = self.joint_names
+        n=2
+        for arg in args:
+            if len(arg) == len(self.joint_names): 
+                point = JointTrajectoryPoint()
+                point.positions = list(arg)
+                point.time_from_start.sec = n
+                n+=2
+                point.time_from_start.nanosec = 0
+                msg.points.append(point)
+            elif len(arg) == len(self.joint_names) + 1:
+                point = JointTrajectoryPoint()
+                point.positions = list(arg[:-1])
+                point.time_from_start.sec = int(arg[-1])
+                point.time_from_start.nanosec = int((arg[-1] - int(arg[-1])) * 1e9)
+                msg.points.append(point)
+        
+        self.publisher.publish(msg)
+        print("published joint positions")
+
+
+def main():
+    """Main function to get joint names and start the ROS 2 & OSC system."""
+
+    tree = ET.parse('/BA/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']
+
+    rclpy.init()
+
+    node = ScaledJointTrajectoryPublisher(joint_names)
+    
+    # Run both ROS 2 and OSC Server together
+    try:
+        while rclpy.ok():
+            osc_process()  # Handle one OSC request at a time
+            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
+    except KeyboardInterrupt:
+        print("")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        osc_terminate()
+
+if __name__ == '__main__':
+    main()
--- a/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart.py
+++ b/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart.py
@@ -0,0 +1,106 @@
+import rclpy
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from osc4py3.as_eventloop import *
+from osc4py3 import oscmethod as osm
+import xml.etree.ElementTree as ET
+import roboticstoolbox as rtb
+import spatialmath as sm
+import numpy as np
+import time
+
+class ScaledJointTrajectoryPublisher(Node):
+    """Node to publish joint trajectories based on OSC messages."""
+    def __init__(self, joint_names, robot):
+        super().__init__('scaled_joint_trajectory_publisher')
+        
+        self.robot = robot
+
+        # ROS2 Publisher
+        self.publisher = self.create_publisher(
+            JointTrajectory, 
+            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            10
+        )
+
+        # Store received joint positions
+        self.joint_names = joint_names
+
+        osc_startup()
+        osc_udp_server("0.0.0.0", 6080, "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")
+        # Register OSC handler
+        osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
+
+    def joint_angles_handler(self, *args):
+        """Handles incoming OSC messages for joint positions."""
+        time1 = time.time()
+        print("Received joint positions")
+        msg = JointTrajectory()
+        msg.joint_names = self.joint_names
+        joint_positions = [0.0] * len(self.joint_names)
+        steps = 30
+        vel = 0.4
+        if len(args[0]) == len(self.joint_names):
+            n=2.0
+            for i in range(len(args)-1):
+                x, y, z, roll, pitch, yaw = args[i]
+                Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+                x, y, z, roll, pitch, yaw = args[i+1]
+                Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+                cart_traj = rtb.ctraj(Tep1, Tep2, steps)
+                for j in range(steps-1):
+                    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)
+
+        elif len(args[0]) == len(self.joint_names) + 1:
+            for i in range(len(args)):
+                x, y, z, roll, pitch, yaw, timetag = args[i]
+                Tep = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+                x, y, z, roll, pitch, yaw = args[i+1][:-1]
+                Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+                cart_traj = rtb.ctraj(Tep, Tep2, steps)
+                for Tep in cart_traj:
+                    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')
+
+
+def main():
+    """Main function to get joint names and start the ROS 2 & OSC system."""
+
+    tree = ET.parse('/BA/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']
+    robot = rtb.ERobot.URDF('/BA/robot.urdf')
+    rclpy.init()
+
+    node = ScaledJointTrajectoryPublisher(joint_names, robot)
+
+    # Run both ROS 2 and OSC Server together
+    try:
+        while rclpy.ok():
+            osc_process()  # Handle one OSC request at a time
+            rclpy.spin_once(node, timeout_sec=0.1)  # Process ROS callbacks
+    except KeyboardInterrupt:
+        print("")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        osc_terminate()
+
+if __name__ == '__main__':
+    main()