latest pushes

humanoid_robot_intelligence_control_system_controller/CMakeLists.txt

@@ -37,6 +37,7 @@ if($ENV{ROS_VERSION} EQUAL 1)
 
   catkin_install_python(PROGRAMS
     src/PIDController.py
+    src/BellandeController.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   )
 
@@ -49,6 +50,7 @@ else()
 
   install(PROGRAMS
     src/PIDController.py
+    src/BellandeController.py
     DESTINATION lib/${PROJECT_NAME}
   )
 

humanoid_robot_intelligence_control_system_controller/setup.py

@@ -18,7 +18,7 @@ from catkin_pkg.python_setup import generate_distutils_setup
 
 # fetch values from package.xml
 setup_args = generate_distutils_setup(
-    scripts=['src/PIDController.py'],
+    scripts=['src/PIDController.py', "src/BellandeController.py"],
     packages=['humanoid_robot_intelligence_control_system_controller'],
     package_dir={'': 'src'},
 )

humanoid_robot_intelligence_control_system_controller/src/BellandeController.py

@@ -0,0 +1,69 @@
+#!/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 <https://www.gnu.org/licenses/>.
+
+import os
+import time
+from typing import Tuple
+
+ros_version = os.getenv("ROS_VERSION")
+
+if ros_version == "1":
+    import rospy
+    from std_msgs.msg import Float64
+elif ros_version == "2":
+    import rclpy
+    from rclpy.node import Node
+    from std_msgs.msg import Float64
+else:
+    raise ValueError("Invalid ROS version. Set ROS_VERSION environment variable to '1' or '2'.")
+
+class BellandeController:
+    def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))):
+        self.kp, self.ki, self.kd = gains
+        self.name = name
+        self.output_limits = output_limits
+        self.reset()
+
+        if ros_version == "1":
+            rospy.init_node('bellande_controller', anonymous=True)
+            self.subscriber = rospy.Subscriber('control_output', Float64, self.callback)
+        elif ros_version == "2":
+            rclpy.init()
+            self.node = Node('bellande_controller')
+            self.subscriber = self.node.create_subscription(Float64, 'control_output', self.callback, 10)
+
+    def reset(self):
+        self.last_error = 0
+        self.integral = 0
+        self.last_time = time.time()
+
+    def update_config(self, gains: Tuple[float, float, float]):
+        self.kp, self.ki, self.kd = gains
+
+    def callback(self, msg):
+        print(f"Received control output: {msg.data}")
+
+    def run(self):
+        if ros_version == "1":
+            rospy.spin()
+        elif ros_version == "2":
+            rclpy.spin(self.node)
+
+    def shutdown(self):
+        if ros_version == "2":
+            self.node.destroy_node()
+            rclpy.shutdown()

humanoid_robot_intelligence_control_system_internal_sensors/src/cpu.py

@@ -0,0 +1,50 @@
+#!/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 <https://www.gnu.org/licenses/>.
+
+import time
+from typing import Dict, List, Tuple
+
+class PIDController:
+    def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))):
+        self.kp, self.ki, self.kd = gains
+        self.name = name
+        self.output_limits = output_limits
+        self.reset()
+
+    def reset(self):
+        self.last_error = 0
+        self.integral = 0
+        self.last_time = time.time()
+
+    def compute(self, setpoint: float, process_variable: float) -> float:
+        current_time = time.time()
+        dt = current_time - self.last_time
+        error = setpoint - process_variable
+        
+        self.integral += error * dt
+        derivative = (error - self.last_error) / dt if dt > 0 else 0
+
+        output = self.kp * error + self.ki * self.integral + self.kd * derivative
+        output = max(min(output, self.output_limits[1]), self.output_limits[0])
+
+        self.last_error = error
+        self.last_time = current_time
+
+        return output
+
+    def update_config(self, gains: Tuple[float, float, float]):
+        self.kp, self.ki, self.kd = gains