diff --git a/mnlm/client/gpt_control/dummy_command.py b/mnlm/client/gpt_control/dummy_command.py new file mode 100644 index 0000000..018510a --- /dev/null +++ b/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) diff --git a/mnlm/client/gpt_control/robot_arm.py b/mnlm/client/gpt_control/robot_arm.py index 096dc65..ec10f11 100644 --- a/mnlm/client/gpt_control/robot_arm.py +++ b/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", diff --git a/mnlm/client/knowledge/dummy_command.json b/mnlm/client/knowledge/dummy_command.json new file mode 100644 index 0000000..1bc8ab6 --- /dev/null +++ b/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} + } + ] +} \ No newline at end of file diff --git a/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher.py b/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher_node.py similarity index 68% rename from mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher.py rename to mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher_node.py index ac42295..881ce1f 100644 --- a/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_dispatcher.py +++ b/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) diff --git a/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_receiver.py b/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_receiver_node.py similarity index 100% rename from mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_receiver.py rename to mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/command_receiver_node.py diff --git a/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover.py b/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover_node.py similarity index 82% rename from mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover.py rename to mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover_node.py index 00480da..ae2e8ea 100644 --- a/mnlm/robot/robot_arm_ws/src/robot_arm/robot_arm/joint_mover.py +++ b/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): diff --git a/mnlm/robot/robot_arm_ws/src/robot_arm/setup.py b/mnlm/robot/robot_arm_ws/src/robot_arm/setup.py index 0063ada..1289864 100644 --- a/mnlm/robot/robot_arm_ws/src/robot_arm/setup.py +++ b/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", ], },