Merge pull request #21 from small-thinking/all-joint-move

Add all-joints move command

mnlm/client/gpt_control/dummy_command.py

@@ -0,0 +1,36 @@
+import json
+import os
+
+import requests
+
+
+def send_commands_to_service(json_file_path, service_url):
+    # Load the JSON commands from the specified file
+    with open(json_file_path, "r") as file:
+        commands = json.load(file)
+    print(f"Commands: {commands}")
+    # commands = f"""
+    # {{
+    #     "operations": []
+    # }}
+    # """
+    # Send the JSON commands to the HTTP service
+    response = requests.post(service_url, json=commands)
+
+    # Check the response status
+    if response.status_code == 200:
+        print("Commands sent successfully.")
+        print("Response:", response)
+    else:
+        print("Failed to send commands.")
+        print("Status Code:", response.status_code)
+        print("Response:", response.text)
+
+
+if __name__ == "__main__":
+    json_file_path = os.path.join(
+        os.path.dirname(os.path.dirname(__file__)), "knowledge/dummy_command.json"
+    )
+    service_url = "http://0.0.0.0:5678/robot_command"
+
+    send_commands_to_service(json_file_path, service_url)

mnlm/client/gpt_control/robot_arm.py

@@ -92,6 +92,10 @@ def translate_prompt_to_sequence(self, prompt: str) -> str:
                     {{
                         "operation": "move_single_servo",
                         "parameters": {{"id": "servo1", "angle": 90, "time": 500}}
+                    }},
+                    {{
+                        "operation": "move_all_servos",
+                        "parameters": {{"angles": [0, 90, 90, 45, 32, 0], "time": 500}}
                     }}
                 ]
             }}
@@ -111,7 +115,7 @@ def translate_prompt_to_sequence(self, prompt: str) -> str:
         if self.verbose:
             self.logger.info(f"Instruction: {instruction}")
         response = self.gpt_client.chat.completions.create(
-            model="gpt-3.5-turbo-1106",
+            model="gpt-3.5-turbo-0125",
             messages=[
                 {
                     "role": "system",

mnlm/client/knowledge/dummy_command.json

@@ -0,0 +1,16 @@
+{
+    "operations": [
+        {
+            "operation": "move_all_servos",
+            "parameters": {"angles": [0, 45, 60, 60, 0, 0, 0], "time": 500}
+        },
+        {
+            "operation": "move_single_servo",
+            "parameters": {"id": "servo0", "angle": -60, "time": 500}
+        },
+        {
+            "operation": "move_single_servo",
+            "parameters": {"id": "servo0", "angle": 60, "time": 500}
+        }
+    ]
+}

mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher.py → mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher_node.py

@@ -1,8 +1,9 @@
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import String, Float64
-from typing import Any, List
+from typing import Any, List, Dict
 import json
+import time
 
 
 class CommandDispatcherNode(Node):
@@ -62,9 +63,18 @@ def _process_command(self, operations: List[Any]):
                 )
             elif func_name == "set_rgb_light":  # set the RGB light
                 self.get_logger().warn(f"Setting RGB light to {parameters}")
+            elif func_name == "move_all_servos":
+                self._move_all_joints(
+                    servo_degrees=parameters.get("angles", [0, 0, 0, 0, 0, 0]),
+                    time=parameters.get("time", 0)
+                )
             else:
                 self.get_logger().error(f"Unknown action: {operation}")
-
+            seconds = 5
+            self._log_with_color("Waiting for {seconds} seconds before sending the next operation.")
+            time.sleep(seconds)
+
+
     def _move_single_joint(self, servo_id: str, angle: float, time: int):
         # Joints can only be servo0-servo5 and left_gripper_joint, right_gripper_joint
         if servo_id in ["servo0", "servo1", "servo2", "servo3", "servo4", "left_gripper_joint", "right_gripper_joint"]:
@@ -75,7 +85,34 @@ def _move_single_joint(self, servo_id: str, angle: float, time: int):
             self.get_logger().info(f"{msg.data} published to /joint_commands")
         else:
             self.get_logger().error(f"{self.node_name} Unknown joint: {servo_id}")
+
+    def _move_all_joints(self, servo_degrees: List[float], time: int):
+        # Joints can only be servo0-servo5 and left_gripper_joint, right_gripper_joint
+        if len(servo_degrees) == 7:
+            command = json.dumps({"angles": servo_degrees, "time": time})
+            msg = String()
+            msg.data = command
+            self.joint_command_pub.publish(msg)
+            self.get_logger().info(f"{msg.data} published to /joint_commands")
+        else:
+            self.get_logger().error(
+                f"{self.node_name} Incorrect number of servo degrees: {len(servo_degrees)}. Expected 7.")
+
 
+    def _log_with_color(self, msg: str, color="red"):
+        """Prints a log message with color."""
+        colors = {
+            "red": "\033[91m",
+            "green": "\033[92m",
+            "yellow": "\033[93m",
+            "blue": "\033[94m",
+            "magenta": "\033[95m",
+            "cyan": "\033[96m",
+            "white": "\033[97m",
+            "reset": "\033[0m"
+        }
+        color_code = colors.get(color, colors["reset"])
+        self.get_logger().error(f"{color_code}{msg}{colors['reset']}")
 
 def main(args=None):
     rclpy.init(args=args)

mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover.py → mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover_node.py

@@ -66,31 +66,34 @@ def _get_current_joint_positions(self, joint_names: List[str]):
 
     def _joint_command_callback(self, msg: String):
         self.get_logger().error(f"Received joint command: {msg.data}")
-        command_data = json.loads(msg.data)
 
-        joint_id = command_data["id"]
-        angle_in_degrees = command_data["angle"]
-        time_in_seconds = command_data["time"] / 1000.0  # Convert milliseconds to seconds
-
         # Read the current position from /joint_states topic
         positions = self._get_current_joint_positions(self.joint_names)
         self.get_logger().info(f"Current joint positions: {positions}")
         velocities = [0.0] * (self.num_joints + self.num_gripper_fingers)
 
-        # Update the angle for the specified joint, note the command is in degree and the position is in radians
-        if "servo" in joint_id:
+        # Parse the incoming command and take the appropriate action.
+        command_data = json.loads(msg.data)
+        if "id" in command_data:
+            joint_id = command_data["id"]
+            angle_in_degrees = command_data["angle"]
+            time_in_seconds = command_data["time"] / 1000.0  # Convert milliseconds to seconds
+
             target_servo_index = int(joint_id[-1])
             target_position = angle_in_degrees * (3.14159 / 180.0)  # Convert degrees to radians
             positions[target_servo_index] = target_position
-
-            # Only set the velocity of the target joint as 0.5
+            # Only set the velocity of the target joint as 0.8
             velocities[target_servo_index] = 0.5
-            # Send the command to the action server
-            response = self.send_goal_and_wait(positions=positions, velocities=velocities, time_from_start_sec=3)
-            if response:
-                self.get_logger().info(f"Successfully moved joint {joint_id} to {angle_in_degrees} degrees.")
-        else:
-            self.get_logger().error(f"Invalid joint ID: {joint_id}")
+        elif "angles" in command_data:
+            angles = command_data["angles"]
+            positions = [angle * (3.14159 / 180.0) for angle in angles]
+            # Set the velocity of all joints as 0.5
+            velocities = [0.5] * (self.num_joints + self.num_gripper_fingers)
+
+        # Send the command to the action server
+        response = self.send_goal_and_wait(positions=positions, velocities=velocities, time_from_start_sec=3)
+        if response:
+            self.get_logger().info(f"Successfully moved joint(s) to {positions} degrees.")
 
 
     def send_goal_and_wait(self, positions: List[float], velocities: List[float], time_from_start_sec: int) -> bool:
@@ -124,9 +127,9 @@ def send_goal_and_wait(self, positions: List[float], velocities: List[float], ti
     def _feedback_callback(self, feedback_msg):
         actual_positions = feedback_msg.feedback.actual.positions
         actual_velocities = feedback_msg.feedback.actual.velocities
-        self.get_logger().info(
-            f"Received feedback: positions: {actual_positions}, velocities: {actual_velocities}"
-        )
+        # self.get_logger().info(
+        #     f"Received feedback: positions: {actual_positions}, velocities: {actual_velocities}"
+        # )
 
 
 def main(args=None):

mnlm/robot/robot_arm_ws/src/robot_arm/setup.py

@@ -39,9 +39,9 @@
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
-            "command_receiver_node = robot_arm.command_receiver:main",
-            "command_dispatcher_node = robot_arm.command_dispatcher:main",
-            "joint_mover_node = robot_arm.joint_mover:main",
+            "command_receiver_node = robot_arm.command_receiver_node:main",
+            "command_dispatcher_node = robot_arm.command_dispatcher_node:main",
+            "joint_mover_node = robot_arm.joint_mover_node:main",
             "joint_move_script_node = robot_arm.joint_move_script:main",
         ],
     },