From 8ea9a2276056e2cba655d3d7aaed907f14970397 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 25 Jul 2024 15:02:40 -0400 Subject: [PATCH] latest pushes --- .../CMakeLists.txt | 62 ++++ .../package.xml | 46 +++ .../setup.py | 26 ++ .../src/HumanoidRobotController.py | 327 ++++++++++++++++++ .../src/MovementController.py | 73 ++++ 5 files changed, 534 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_movement/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_movement/package.xml create mode 100644 humanoid_robot_intelligence_control_system_movement/setup.py create mode 100644 humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py create mode 100644 humanoid_robot_intelligence_control_system_movement/src/MovementController.py diff --git a/humanoid_robot_intelligence_control_system_movement/CMakeLists.txt b/humanoid_robot_intelligence_control_system_movement/CMakeLists.txt new file mode 100644 index 0000000..ad4b2c6 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/CMakeLists.txt @@ -0,0 +1,62 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_movement) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/MovementController.py + src/HumanoidRobotController.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/MovementController.py + src/HumanoidRobotController.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_movement/package.xml b/humanoid_robot_intelligence_control_system_movement/package.xml new file mode 100644 index 0000000..7cc7687 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_movement + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_movement/setup.py b/humanoid_robot_intelligence_control_system_movement/setup.py new file mode 100644 index 0000000..1450a13 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=["src/MovementController.py", "src/HumanoidRobotController.py"], + packages=['humanoid_robot_intelligence_control_system_movement'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py b/humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py new file mode 100644 index 0000000..13adf37 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/src/HumanoidRobotController.py @@ -0,0 +1,327 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import time +import math +from typing import Dict, List, Tuple, Any + +from humanoid_robot_intelligence_control_system_controller.PIDController import PIDController +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig +from humanoid_robot_intelligence_control_system_movement.MovementController import MovementController + + +class HumanoidRobotController: + def __init__(self, config: TrackerConfig, controller_type: str = "PID"): + self.config = config + self.controller_type = controller_type + self.controllers: Dict[str, Any] = {} + self.joint_positions: Dict[str, float] = {} + self.target_positions: Dict[str, float] = {} + self.movement_controller = MovementController(config, controller_type) + self.current_balance = 0.0 + self.current_speed = 0.0 + self.current_angle = 0.0 + self.current_position = (0.0, 0.0) + self.last_joint_positions = {} + self.control_loop_time = 0.01 # 100 Hz + + self.initialize_controllers() + self.initialize_sensors() + + + def initialize_controllers(self): + joints = [ + "head_pan", "head_tilt", + "left_shoulder_pitch", "left_shoulder_roll", "left_elbow", + "right_shoulder_pitch", "right_shoulder_roll", "right_elbow", + "left_hip_yaw", "left_hip_roll", "left_hip_pitch", "left_knee", "left_ankle_pitch", "left_ankle_roll", + "right_hip_yaw", "right_hip_roll", "right_hip_pitch", "right_knee", "right_ankle_pitch", "right_ankle_roll" + ] + + for joint in joints: + self.controllers[joint] = self._create_controller(joint) + self.joint_positions[joint] = 0.0 + self.target_positions[joint] = 0.0 + + self.controllers["balance"] = self._create_controller("balance") + self.controllers["walk_forward"] = self._create_controller("walk_forward") + self.controllers["turn"] = self._create_controller("turn") + + self.roll_controller = self._create_controller("roll") + self.pitch_controller = self._create_controller("pitch") + + + def _create_controller(self, controller_name: str) -> Any: + if self.controller_type == "PID": + return PIDController( + getattr(self.config, f"get_{controller_name}_pid_gains")(), + f"{controller_name}_controller", + output_limits=self.config.get_joint_limits(controller_name) + ) + elif self.controller_type == "BellandeController": + return BellandeController( + getattr(self.config, f"get_{controller_name}_bellande_params")(), + f"{controller_name}_controller" + ) + else: + raise ValueError(f"Unsupported controller type: {self.controller_type}") + + + def initialize_sensors(self): + self.imu = None + self.power_sensor = None + self.joint_sensors = None + + + def update_config(self, new_config: TrackerConfig): + self.config = new_config + for joint, controller in self.controllers.items(): + self._update_controller(controller, joint) + self.movement_controller.update_config(new_config) + + + def _update_controller(self, controller: Any, controller_name: str): + if self.controller_type == "PID": + controller.update_config(getattr(self.config, f"get_{controller_name}_pid_gains")()) + elif self.controller_type == "BellandeController": + controller.update_config(getattr(self.config, f"get_{controller_name}_bellande_params")()) + + + def set_target_position(self, joint: str, position: float): + if joint in self.target_positions: + self.target_positions[joint] = position + else: + raise ValueError(f"Unknown joint: {joint}") + + + def update_joint_position(self, joint: str, position: float): + if joint in self.joint_positions: + self.joint_positions[joint] = position + else: + raise ValueError(f"Unknown joint: {joint}") + + + def compute_joint_control(self, joint: str) -> float: + if joint not in self.controllers: + raise ValueError(f"Unknown joint: {joint}") + + return self.controllers[joint].compute( + self.target_positions[joint], + self.joint_positions[joint] + ) + + + def compute_all_joint_controls(self) -> Dict[str, float]: + return {joint: self.compute_joint_control(joint) for joint in self.joint_positions.keys()} + + + def compute_balance_control(self, current_balance: float) -> float: + return self.controllers["balance"].compute(0, current_balance) + + + def compute_walk_forward_control(self, current_speed: float, target_speed: float) -> float: + return self.controllers["walk_forward"].compute(target_speed, current_speed) + + + def compute_turn_control(self, current_angle: float, target_angle: float) -> float: + return self.controllers["turn"].compute(target_angle, current_angle) + + + def reset_all_controllers(self): + for controller in self.controllers.values(): + controller.reset() + + + def update_robot_state(self, joint_positions: Dict[str, float], balance: float, speed: float, angle: float, position: Tuple[float, float]): + for joint, position in joint_positions.items(): + self.update_joint_position(joint, position) + + self.current_balance = balance + self.current_speed = speed + self.current_angle = angle + self.current_position = position + + + def compute_full_robot_control(self, target_speed: float, target_angle: float, target_position: Tuple[float, float]) -> Dict[str, float]: + joint_controls = self.compute_all_joint_controls() + balance_control = self.compute_balance_control(self.current_balance) + walk_control = self.compute_walk_forward_control(self.current_speed, target_speed) + turn_control = self.compute_turn_control(self.current_angle, target_angle) + + translation, rotation = self.movement_controller.compute_path_control(self.current_position, target_position) + + return { + "joints": joint_controls, + "balance": balance_control, + "walk": walk_control, + "turn": turn_control, + "translation": translation, + "rotation": rotation + } + + + def execute_control(self, control: Dict[str, float]): + try: + # Send joint control commands + for joint, value in control['joints'].items(): + self.robot_interface.set_joint_position(joint, value) + + # Send balance control command + self.robot_interface.set_balance_adjustment(control['balance']) + + # Send walk and turn commands + self.robot_interface.set_walk_speed(control['walk']) + self.robot_interface.set_turn_rate(control['turn']) + + # Send translation and rotation commands + self.robot_interface.set_body_translation(control['translation']) + self.robot_interface.set_body_rotation(control['rotation']) + + # Verify that commands were executed + if not self.robot_interface.verify_last_command(): + raise Exception("Failed to execute control commands") + + except Exception as e: + print(f"Error executing control: {e}") + self.emergency_stop() + + + def get_robot_state(self) -> Dict[str, Any]: + joint_positions = self.joint_sensors.get_positions() + imu_data = self.imu.get_data() + return { + "joint_positions": joint_positions, + "balance": imu_data.acceleration.z, + "speed": math.sqrt(imu_data.velocity.x**2 + imu_data.velocity.y**2), + "angle": math.atan2(imu_data.orientation.y, imu_data.orientation.x), + "position": (imu_data.position.x, imu_data.position.y) + } + + + def balance_adjustment(self): + roll, pitch, yaw = self.imu.get_orientation() + roll_rate, pitch_rate, yaw_rate = self.imu.get_angular_velocity() + + roll_correction = self.roll_controller.compute(0, roll) + pitch_correction = self.pitch_controller.compute(0, pitch) + + self.set_target_position("left_hip_roll", self.joint_positions["left_hip_roll"] - roll_correction) + self.set_target_position("right_hip_roll", self.joint_positions["right_hip_roll"] + roll_correction) + self.set_target_position("left_ankle_roll", self.joint_positions["left_ankle_roll"] - roll_correction) + self.set_target_position("right_ankle_roll", self.joint_positions["right_ankle_roll"] + roll_correction) + self.set_target_position("left_ankle_pitch", self.joint_positions["left_ankle_pitch"] - pitch_correction) + self.set_target_position("right_ankle_pitch", self.joint_positions["right_ankle_pitch"] - pitch_correction) + + if abs(pitch) > self.config.get_max_stable_pitch(): + hip_pitch_correction = self.pitch_controller.compute(0, pitch) * 0.5 + self.set_target_position("left_hip_pitch", self.joint_positions["left_hip_pitch"] + hip_pitch_correction) + self.set_target_position("right_hip_pitch", self.joint_positions["right_hip_pitch"] + hip_pitch_correction) + + damping_factor = self.config.get_angular_velocity_damping_factor() + self.set_target_position("left_hip_roll", self.joint_positions["left_hip_roll"] - roll_rate * damping_factor) + self.set_target_position("right_hip_roll", self.joint_positions["right_hip_roll"] + roll_rate * damping_factor) + self.set_target_position("left_ankle_roll", self.joint_positions["left_ankle_roll"] - roll_rate * damping_factor) + self.set_target_position("right_ankle_roll", self.joint_positions["right_ankle_roll"] + roll_rate * damping_factor) + self.set_target_position("left_ankle_pitch", self.joint_positions["left_ankle_pitch"] - pitch_rate * damping_factor) + self.set_target_position("right_ankle_pitch", self.joint_positions["right_ankle_pitch"] - pitch_rate * damping_factor) + + + def check_joint_status(self) -> bool: + for joint, position in self.joint_positions.items(): + limits = self.config.get_joint_limits(joint) + if position < limits[0] or position > limits[1]: + print(f"Joint {joint} out of limits: {position}") + return False + + if joint in self.last_joint_positions: + velocity = (position - self.last_joint_positions[joint]) / self.control_loop_time + if abs(velocity) > self.config.get_max_joint_velocity(joint): + print(f"Joint {joint} velocity too high: {velocity}") + return False + + self.last_joint_positions = self.joint_positions.copy() + return True + + + def check_power_status(self) -> bool: + voltage = self.power_sensor.get_voltage() + current = self.power_sensor.get_current() + + if voltage < self.config.get_min_voltage(): + print(f"Voltage too low: {voltage}V") + return False + if current > self.config.get_max_current(): + print(f"Current too high: {current}A") + return False + + return True + + + def check_sensor_status(self) -> bool: + return self.imu.is_operational() and self.power_sensor.is_operational() and self.joint_sensors.is_operational() + + + def self_diagnosis(self) -> Dict[str, bool]: + return { + "joints": self.check_joint_status(), + "sensors": self.check_sensor_status(), + "power": self.check_power_status(), + } + + + def emergency_stop(self): + self.stop() + self.reset_all_controllers() + + print("Emergency stop activated!") + + + def stop(self): + zero_control = {key: 0.0 for key in self.controllers.keys()} + zero_control.update({"translation": 0.0, "rotation": 0.0}) + self.execute_control(zero_control) + + + def run_control_loop(self): + while True: + try: + loop_start_time = time.time() + + state = self.get_robot_state() + self.update_robot_state(**state) + self.balance_adjustment() + + diagnosis = self.self_diagnosis() + if not all(diagnosis.values()): + print("System check failed. Initiating emergency stop.") + self.emergency_stop() + break + + control = self.compute_full_robot_control(self.current_speed, self.current_angle, self.current_position) + self.execute_control(control) + + loop_end_time = time.time() + loop_duration = loop_end_time - loop_start_time + if loop_duration < self.control_loop_time: + time.sleep(self.control_loop_time - loop_duration) + + except Exception as e: + print(f"Unexpected error: {e}") + self.emergency_stop() + break + + print("HumanoidRobotController shutting down.") diff --git a/humanoid_robot_intelligence_control_system_movement/src/MovementController.py b/humanoid_robot_intelligence_control_system_movement/src/MovementController.py new file mode 100644 index 0000000..d64d2c6 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_movement/src/MovementController.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math +from typing import Tuple + +from humanoid_robot_intelligence_control_system_detection.PIDController import PIDController +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig + + +class MovementController: + def __init__(self, config: TrackerConfig, controller_type: str = "PID"): + self.config = config + self.controller_type = controller_type + self.translation_controller = self._create_controller("translation") + self.rotation_controller = self._create_controller("rotation") + self.path_controller = self._create_controller("path") + + def _create_controller(self, controller_name: str) -> Any: + if self.controller_type == "PID": + return PIDController( + getattr(self.config, f"get_{controller_name}_pid_gains")(), + f"{controller_name}_controller", + output_limits=(-1, 1) + ) + elif self.controller_type == "BellandeController": + return BellandeController( + getattr(self.config, f"get_{controller_name}_bellande_params")(), + f"{controller_name}_controller" + ) + else: + raise ValueError(f"Unsupported controller type: {self.controller_type}") + + def update_config(self, new_config: TrackerConfig): + self.config = new_config + self._update_controller(self.translation_controller, "translation") + self._update_controller(self.rotation_controller, "rotation") + self._update_controller(self.path_controller, "path") + + def _update_controller(self, controller: Any, controller_name: str): + if self.controller_type == "PID": + controller.update_config(getattr(self.config, f"get_{controller_name}_pid_gains")()) + elif self.controller_type == "BellandeController": + controller.update_config(getattr(self.config, f"get_{controller_name}_bellande_params")()) + + def compute_translation_control(self, current_position: float, target_position: float) -> float: + return self.translation_controller.compute(target_position, current_position) + + def compute_rotation_control(self, current_angle: float, target_angle: float) -> float: + return self.rotation_controller.compute(target_angle, current_angle) + + def compute_path_control(self, current_position: Tuple[float, float], target_position: Tuple[float, float]) -> Tuple[float, float]: + dx = target_position[0] - current_position[0] + dy = target_position[1] - current_position[1] + distance = (dx**2 + dy**2)**0.5 + angle = math.atan2(dy, dx) + translation = self.translation_controller.compute(0, -distance) + rotation = self.rotation_controller.compute(angle, 0) + return translation, rotation