AS: osc4py3

ros2_ws/src/joint_states_control/cart_to_angles.py

@@ -4,8 +4,15 @@ import roboticstoolbox as rtb
 def ik_sol(robot, pose):
 	ets = robot.ets()
 
-	Tep = ets.fkine(pose)
+	#Tep = ets.fkine(pose)
+
+	#sol = ets.ik_GN(Tep)
+
+	solver = rtb.IK_GN()
+	Tep = ets.fkine(pose)
+	sol = solver.solve(ets, Tep).q.tolist()
+
+	return sol
 
-	sol = ets.ik_LM(Tep)
 	if sol[1]: return sol[0].tolist()
 	return "no sol"

ros2_ws/src/joint_states_control/get_urdf.py

@@ -0,0 +1,55 @@
+import rclpy
+from rclpy.node import Node
+from rcl_interfaces.srv import GetParameters
+import roboticstoolbox as rtb
+
+class URDFRetriever(Node):
+    def __init__(self):
+        super().__init__('urdf_retriever')
+
+        # Create a parameter client to request the parameter from /robot_state_publisher
+        self.client = self.create_client(GetParameters, '/robot_state_publisher/get_parameters')
+
+        while not self.client.wait_for_service(timeout_sec=3.0):
+            self.get_logger().warn("⏳ Waiting for /robot_state_publisher parameter service...")
+
+        # Create and send a request to get 'robot_description'
+        self.request = GetParameters.Request()
+        self.request.names = ["robot_description"]
+
+        self.future = self.client.call_async(self.request)
+        rclpy.spin_until_future_complete(self, self.future)
+
+        # Extract the URDF
+        if self.future.result():
+            self.urdf_string = self.future.result().values[0].string_value
+            if self.urdf_string:
+                self.get_logger().info("Successfully retrieved URDF from /robot_state_publisher!")
+            else:
+                self.get_logger().error("URDF is empty")
+        else:
+            self.get_logger().error("Failed to get URDF from /robot_state_publisher.")
+
+def main():
+    rclpy.init()
+    node = URDFRetriever()
+    
+    urdf_string = node.urdf_string
+
+    if urdf_string:
+        # Save to a file
+        urdf_path = "/tmp/robot.urdf"
+        with open(urdf_path, "w") as file:
+            file.write(urdf_string)
+
+        print(f"✅ URDF saved to {urdf_path}")
+
+        # Load into Robotic Toolbox
+        robot = rtb.ERobot.URDF(urdf_path)
+        print(robot)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

ros2_ws/src/joint_states_control/move_scaled_joint_cart copy.py

@@ -0,0 +1,95 @@
+import rclpy
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from rcl_interfaces.srv import GetParameters
+import time
+from cart_to_angles import ik_sol
+import roboticstoolbox as rtb
+import xacro
+from spatialmath import SE3
+
+class ScaledJointTrajectoryPublisher(Node):
+    def __init__(self):
+        super().__init__('scaled_joint_trajectory_publisher')
+        
+        # Create a parameter client to request the parameter from /robot_state_publisher
+        self.client = self.create_client(GetParameters, '/robot_state_publisher/get_parameters')
+
+        while not self.client.wait_for_service(timeout_sec=3.0):
+            self.get_logger().warn("⏳ Waiting for /robot_state_publisher parameter service...")
+
+        # Create and send a request to get 'robot_description'
+        self.request = GetParameters.Request()
+        self.request.names = ["robot_description"]
+
+        self.future = self.client.call_async(self.request)
+        rclpy.spin_until_future_complete(self, self.future)
+
+        # Extract the URDF
+        if self.future.result():
+            self.urdf_string = self.future.result().values[0].string_value
+            if self.urdf_string:
+                self.get_logger().info("Successfully retrieved URDF from /robot_state_publisher!")
+            else:
+                self.get_logger().error("URDF is empty")
+        else:
+            self.get_logger().error("Failed to get URDF from /robot_state_publisher.")
+
+        
+        self.publisher = self.create_publisher(
+            JointTrajectory, 
+            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            10
+        )
+        time.sleep(1)  # Allow publisher to initialize
+        self.get_logger().info("Scaled Joint Trajectory Publisher Ready")
+
+    def send_trajectory(self, joint_positions, duration=5.0):
+        """Publish a joint trajectory command to move the UR10e."""
+        msg = JointTrajectory()
+        msg.joint_names = [
+            "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
+            "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
+        ]
+
+        point = JointTrajectoryPoint()
+        point.positions = joint_positions  # Target joint positions
+        point.time_from_start.sec = int(duration)  # Duration in seconds
+
+        msg.points.append(point)
+
+        self.publisher.publish(msg)
+
+
+def main():
+    rclpy.init()
+    node = ScaledJointTrajectoryPublisher()
+
+    urdf_string = node.urdf_string
+
+    # Save to a file
+    urdf_path = "/tmp/robot.urdf"
+    with open(urdf_path, "w") as file:
+        file.write(urdf_string)
+
+    print(f"URDF saved to {urdf_path}")
+
+    #robot = rtb.ERobot.URDF(urdf_path) # Load into Robotic Toolbox 
+    robot = rtb.ERobot.URDF('/BA/robot.urdf') # Load into Robotic Toolbox 
+
+    Tep = SE3.Trans(0.3, 0.3, 0.3) * SE3.OA([0, 1, 0], [0, 0, 1])
+    sol = robot.ik_LM(Tep)
+    sol = sol[0].tolist()
+    
+    print("Angles:")
+    print(f"{[val * (180/3.141) for val in sol]} (deg)")
+    print(f"{sol} (rad)")
+    # Move to a target position
+    node.send_trajectory(sol)
+
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

ros2_ws/src/joint_states_control/move_scaled_joint_cart.py

@@ -1,13 +1,40 @@
 import rclpy
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+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):
         super().__init__('scaled_joint_trajectory_publisher')
+        
+        # Create a parameter client to request the parameter from /robot_state_publisher
+        self.client = self.create_client(GetParameters, '/robot_state_publisher/get_parameters')
+
+        while not self.client.wait_for_service(timeout_sec=3.0):
+            self.get_logger().warn("⏳ Waiting for /robot_state_publisher parameter service...")
+
+        # Create and send a request to get 'robot_description'
+        self.request = GetParameters.Request()
+        self.request.names = ["robot_description"]
+
+        self.future = self.client.call_async(self.request)
+        rclpy.spin_until_future_complete(self, self.future)
+
+        # Extract the URDF
+        if self.future.result():
+            self.urdf_string = self.future.result().values[0].string_value
+            if self.urdf_string:
+                self.get_logger().info("Successfully retrieved URDF from /robot_state_publisher!")
+            else:
+                self.get_logger().error("URDF is empty")
+        else:
+            self.get_logger().error("Failed to get URDF from /robot_state_publisher.")
+
+        
         self.publisher = self.create_publisher(
             JointTrajectory, 
             '/scaled_joint_trajectory_controller/joint_trajectory', 
@@ -32,15 +59,29 @@ class ScaledJointTrajectoryPublisher(Node):
 
         self.publisher.publish(msg)
 
+
 def main():
     rclpy.init()
     node = ScaledJointTrajectoryPublisher()
 
-    robot = rtb.models.URDF.UR10()
-    target_position = [0, 0, 300, 0, 0, 0]
+    urdf_string = node.urdf_string
 
+    # Save to a file
+    urdf_path = "/tmp/robot.urdf"
+    with open(urdf_path, "w") as file:
+        file.write(urdf_string)
+
+    print(f"URDF saved to {urdf_path}")
+
+    robot = rtb.ERobot.URDF(urdf_path) # Load into Robotic Toolbox 
+
+    target_position = [0.4, 0.4, 0.4, 0, 0, 0]
+    sol = ik_sol(robot,target_position)
+    print("Winkel:")
+    print([val * (180/3.141) for val in sol])
+    print(sol)
     # Move to a target position
-    node.send_trajectory(ik_sol(robot,target_position))
+    node.send_trajectory(sol)
 
     rclpy.spin(node)
     node.destroy_node()

ros2_ws/src/joint_states_control/test.py

@@ -1,8 +1,9 @@
-from cart_to_angles import ik_sol
+import os
 import roboticstoolbox as rtb
 
-robot = rtb.models.URDF.UR10()
+# Get the URDF from ROS 2
+os.system("ros2 param get /robot_state_publisher robot_description > urdf.xml")
 
-print(robot)
-
-print(ik_sol(robot, [0, 0, 200, 2,3, 1]))
+# Load the URDF using roboticstoolbox
+robot = rtb.ERobot.URDF("urdf.xml")
+print("Robot loaded:", robot)

ros2_ws/src/joint_states_control/test_urdf.py

@@ -0,0 +1,48 @@
+import rclpy
+from rclpy.node import Node
+import roboticstoolbox as rtb
+
+class GET_URDF(Node):
+    def __init__(self):
+        super().__init__('GET_URDF')
+
+        # Declare parameter and get it
+        self.urdf_value = self.get_parameter('robot_description')
+
+
+        if not self.urdf_value:
+            self.get_logger().error("URDF is empty! Check if robot_state_publisher is correctly publishing it.")
+            self.urdf_string = None
+        else:
+            #self.urdf_string = urdf_value
+            self.get_logger().info("Successfully got URDF from ROS 2.")
+
+    def get_urdf(self):
+        return self.urdf_value
+
+def main():
+    rclpy.init()
+    node = GET_URDF()
+    
+    print(node.get_urdf())
+    """
+    if urdf_string:
+        # Print the first 500 characters to check
+        print(f"URDF (first 500 chars):\n{urdf_string[:500]}")
+
+        # Save to file and use in Robotic Toolbox
+        urdf_path = "/tmp/robot.urdf"
+        with open(urdf_path, "w") as file:
+            file.write(urdf_string)
+
+        print(f"URDF saved to {urdf_path}")
+
+        # Load into Robotic Toolbox
+        robot = rtb.ERobot.URDF(urdf_path)
+        print(robot)
+"""
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()