Merge pull request #24 from RonaldsonBellande/main
recal and controller
This commit is contained in:
commit
ff76008ae4
46
README.md
46
README.md
@ -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**.
|
||||
|
||||
|
@ -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}
|
||||
)
|
||||
|
||||
|
@ -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'},
|
||||
)
|
||||
|
@ -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()
|
@ -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
|
||||
|
@ -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>
|
||||
|
@ -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'},
|
||||
)
|
||||
|
||||
|
@ -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
|
@ -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}
|
||||
)
|
@ -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_ */
|
@ -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_ */
|
@ -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_ */
|
@ -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>
|
@ -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
|
||||
|
@ -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>
|
||||
|
@ -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'},
|
||||
)
|
||||
|
||||
|
@ -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()
|
@ -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>
|
@ -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)
|
@ -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
|
Loading…
Reference in New Issue
Block a user