Merge pull request #24 from RonaldsonBellande/main

recal and controller
This commit is contained in:
Ronaldson Bellande 2024-07-27 11:26:02 -04:00 committed by GitHub
commit ff76008ae4
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
21 changed files with 281 additions and 439 deletions

View File

@ -1,36 +1,36 @@
# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework
## 🤖 Explore Humanoid Robot Intelligence with Us!
Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system.
[![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io)
[![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w)
[![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors)
## 🤖 Explore Humanoid Robot Intelligence with Us!
Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system.
## 📢 Mobile and ROS Control System
- [![Bellande's Internal Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_ios_api)
- [![Bellande's External Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_ios_api)
- [![Bellande's Functionality Mobile iOS API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_ios_api)
- [![Bellande's Internal Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_android_api)
- [![Bellande's External Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_android_api)
- [![Bellande's Functionality Mobile Android API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_android_api)
- [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api)
- [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api)
- [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api)
- [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_functionality_ros_api)
- [![Bellande's Algorithm through Bellande MODELS](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20MODELS-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_ros_models)
## 🔄 Run and Experiment with the build Bellande's Humanoid Robot
[![Bellande's Humanoid](https://img.shields.io/badge/Bellande's-Humanoid%20Robot%20Package-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_humanoid_robotics_package)
## 📢 Work with Bellande Algorithms and Models through Bellande's API!
[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API)
---
## 🧑‍💼 Work with Bellande Models through Bellande Framework!
[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Models%20through%20Bellande's%20Framework-Bellande%20Framework-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ROS-MODELS)
---
## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository
[![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/)
[![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension)
---
### 🚀 Key Repository Stats
- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community
@ -52,15 +52,7 @@ Explore our [repository website](https://robotics-sensors.github.io/bellande_hum
---
### 🔄 Updates and Versions
- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_framework)
- **Old Version/Previous Used for Different Context:** [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework)
---
# 🎉 Latest Release
[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_framework/releases/)
---
@ -73,7 +65,7 @@ The repository has recently undergone significant updates. Older commits and cod
🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors).
### 🧑‍💼 Maintainer
### 🧑‍💼 Maintainer & Author
Meet our dedicated author & maintainer, **Ronaldson Bellande**.

View File

@ -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}
)

View File

@ -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'},
)

View File

@ -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()

View File

@ -13,7 +13,7 @@
# the License.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_controller)
project(humanoid_robot_intelligence_control_system_external_sensors)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS

View File

@ -16,10 +16,10 @@ the License.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<name>humanoid_robot_intelligence_control_system_external_sensors</name>
<version>0.0.1</version>
<description>
This Package is for Detection Math
This Package is for External Sensors
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>

View File

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

View File

@ -1,82 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package humanoid_robot_intelligence_control_system_framework_common
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.2 (2023-10-03)
------------------
* Make it compatible for ROS1/ROS2
* Fix bugs
* Update package.xml and CMakeList.txt for to the latest versions
* Contributors & Maintainer: Ronaldson Bellande
0.3.1 (2023-09-27)
------------------
* Starting from this point it under a new license
* Fix errors and Issues
* Rename Repository for a completely different purpose
* Make it compatible with ROS/ROS2
* Upgrade version of all builds and make it more compatible
* Update package.xml and CMakeList.txt for to the latest versions
* Contributors & Maintainer: Ronaldson Bellande
0.2.9 (2018-03-22)
------------------
* none
0.2.8 (2018-03-20)
------------------
* tested for system dependencies
* Contributors: Pyo
0.2.7 (2018-03-15)
------------------
* change the License and package format to version 2
* Contributors: Pyo
0.2.6 (2017-08-09)
------------------
* none
0.2.5 (2017-06-09)
------------------
* none
0.2.4 (2017-06-07)
------------------
* none
0.2.3 (2017-05-23)
------------------
* updated the cmake file for ros install
* Contributors: SCH
0.2.2 (2017-04-24)
------------------
* updated for other packages
* Contributors: Zerom, Kayman
0.2.1 (2016-11-23)
------------------
* Merge the changes and update
* - dependencies fixed. (Pull requests `#26 <https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues/26>`_)
* - modified dependency problem.
* Contributors: Jay Song, Pyo, Zerom
0.2.0 (2016-08-31)
------------------
* updated CHANGLOG.rst for minor release
* rename ControlMode(CurrentControl -> TorqueControl)
* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_)
* Contributors: Zerom, Pyo
0.1.1 (2016-08-18)
------------------
* updated the package information
0.1.0 (2016-08-12)
------------------
* modified the package information for release
* Setting the license to BSD.
* add SensorState
add Singleton template
* Contributors: Jay Song, Zerom, Pyo

View File

@ -1,60 +0,0 @@
# 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.8)
project(humanoid_robot_intelligence_control_system_framework_common)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
humanoid_robot_intelligence_control_system_device
)
else()
find_package(ament_cmake REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS
roscpp
humanoid_robot_intelligence_control_system_device
)
endif()
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
include/${PROJECT_NAME}/motion_module.h
include/${PROJECT_NAME}/sensor_module.h
include/${PROJECT_NAME}/singleton.h
)
set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
install(TARGETS humanoid_robot_intelligence_control_system_framework_common
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

View File

@ -1,87 +0,0 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* 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.
*******************************************************************************/
/*
* motion_module.h
*
* Created on: 2016. 1. 15.
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_
#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_
#include <map>
#include <string>
#include "singleton.h"
#include "humanoid_robot_intelligence_control_system_device/robot.h"
#include "humanoid_robot_intelligence_control_system_device/dynamixel.h"
namespace humanoid_robot_intelligence_control_system_framework
{
enum ControlMode
{
PositionControl,
VelocityControl,
TorqueControl
};
class MotionModule
{
protected:
bool enable_;
std::string module_name_;
ControlMode control_mode_;
public:
std::map<std::string, DynamixelState *> result_;
virtual ~MotionModule() { }
std::string getModuleName() { return module_name_; }
ControlMode getControlMode() { return control_mode_; }
void setModuleEnable(bool enable)
{
if(this->enable_ == enable)
return;
this->enable_ = enable;
if(enable)
onModuleEnable();
else
onModuleDisable();
}
bool getModuleEnable() { return enable_; }
virtual void onModuleEnable() { }
virtual void onModuleDisable() { }
virtual void initialize(const int control_cycle_msec, Robot *robot) = 0;
virtual void process(std::map<std::string, Dynamixel *> dxls, std::map<std::string, double> sensors) = 0;
virtual void stop() = 0;
virtual bool isRunning() = 0;
};
}
#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */

View File

@ -1,57 +0,0 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* 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.
*******************************************************************************/
/*
* sensor_module.h
*
* Created on: 2016. 3. 30.
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_
#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_
#include <map>
#include <string>
#include "singleton.h"
#include "humanoid_robot_intelligence_control_system_device/robot.h"
#include "humanoid_robot_intelligence_control_system_device/dynamixel.h"
namespace humanoid_robot_intelligence_control_system_framework
{
class SensorModule
{
protected:
std::string module_name_;
public:
std::map<std::string, double> result_;
virtual ~SensorModule() { }
std::string getModuleName() { return module_name_; }
virtual void initialize(const int control_cycle_msec, Robot *robot) = 0;
virtual void process(std::map<std::string, Dynamixel *> dxls, std::map<std::string, Sensor *> sensors) = 0;
};
}
#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */

View File

@ -1,65 +0,0 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* 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.
*******************************************************************************/
/*
* singleton.h
*
* Created on: 2016. 5. 17.
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_
#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_
namespace humanoid_robot_intelligence_control_system_framework
{
template <class T>
class Singleton
{
private:
static T *unique_instance_;
protected:
Singleton() { }
Singleton(Singleton const&) { }
Singleton& operator=(Singleton const&) { return *this; }
public:
static T* getInstance()
{
if(unique_instance_ == NULL)
unique_instance_ = new T;
return unique_instance_;
}
static void destroyInstance()
{
if(unique_instance_)
{
delete unique_instance_;
unique_instance_ = NULL;
}
}
};
template <class T> T* Singleton<T>::unique_instance_ = NULL;
}
#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */

View File

@ -1,52 +0,0 @@
<?xml version="1.0"?>
<!--
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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_framework_common</name>
<version>0.3.2</version>
<description>The package contains commonly used headers for the
humanoid_robot_intelligence_control_system_framework</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_device</build_depend>
<build_export_depend condition="$ROS_VERSION == 1">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_device</build_export_depend>
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_device</exec_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend condition="$ROS_VERSION == 2">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_device</build_depend>
<build_export_depend condition="$ROS_VERSION == 2">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_device</build_export_depend>
<exec_depend condition="$ROS_VERSION == 2">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_device</exec_depend>
</package>

View File

@ -13,7 +13,7 @@
# the License.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_controller)
project(humanoid_robot_intelligence_control_system_internal_sensors)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS

View File

@ -16,10 +16,10 @@ the License.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<name>humanoid_robot_intelligence_control_system_internal_sensors</name>
<version>0.0.1</version>
<description>
This Package is for Detection Math
This Package is for Internal Sensors
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>

View File

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

View File

@ -0,0 +1,60 @@
# 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_structure_sensors)
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/PIDController.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/PIDController.py
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY config launch rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
endif()

View File

@ -0,0 +1,46 @@
<?xml version="1.0"?>
<!--
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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_structure_sensors</name>
<version>0.0.1</version>
<description>
This Package is for Internal Sensors
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

View File

@ -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 <https://www.gnu.org/licenses/>.
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/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_structure_sensors'],
package_dir={'': 'src'},
)
setup(**setup_args)

View File

@ -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