EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

can run main thread

This commit is contained in:
Zhenbiao Huang 2024-09-26 22:18:11 +08:00
parent 2052b3e9d1
commit d4f17e631e
4 changed files with 92 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -19,11 +19,11 @@ set(CONTROLLER_INCLUDE_DEPENDS
control_input_msgs control_input_msgs
angles angles
nav_msgs nav_msgs
qpOASES
) )
# find dependencies # find dependencies
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(qpOASES REQUIRED)
foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS}) foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED) find_package(${Dependency} REQUIRED)
endforeach () endforeach ()
@ -61,10 +61,12 @@ target_include_directories(${PROJECT_NAME}
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>" "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
PRIVATE PRIVATE
src) src)
ament_target_dependencies( ament_target_dependencies(${PROJECT_NAME}
${PROJECT_NAME} PUBLIC
${CONTROLLER_INCLUDE_DEPENDS} ${CONTROLLER_INCLUDE_DEPENDS}
) )
target_link_libraries(${PROJECT_NAME}
${qpOASES_LIBRARY_DIR}/libqpOASES.a
)
pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml) pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml)

31
controllers/ocs2_quadruped_controller/README.md
View File

@ -1,9 +1,38 @@
# OCS2 Quadruped Controller # OCS2 Quadruped Controller
This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control) and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2). This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control)
and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2).
## 2. Build ## 2. Build
### 2.1 Build Dependencies
* OCS2 ROS2 Libraries
```bash
colcon build --packages-up-to ocs2_legged_robot_ros
colcon build --packages-up-to ocs2_self_collision
```
* qpOASES
```bash
cd ~/ros2_ws/src/quadruped_ros2
git submodule update --init --recursive
```
also add below line to the Cmakelist.txt of qpOASES
```cmake
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
```
### 2.2 Build OCS2 Quadruped Controller
```bash ```bash
cd ~/ros2_ws cd ~/ros2_ws
colcon build --packages-up-to ocs2_quadruped_controller colcon build --packages-up-to ocs2_quadruped_controller
``` ```
## 3. Launch
* Go2 Robot
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go2_description ocs2_control.launch.py
```

94
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp
View File

@ -54,28 +54,27 @@ namespace ocs2::legged_robot {
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
const rclcpp::Duration &period) { const rclcpp::Duration &period) {
// State Estimate // State Estimate
updateStateEstimation(time, period); updateStateEstimation(time, period);
// Update the current state of the system // Update the current state of the system
mpcMrtInterface_->setCurrentObservation(currentObservation_); mpc_mrt_interface_->setCurrentObservation(current_observation_);
// Load the latest MPC policy // Load the latest MPC policy
mpcMrtInterface_->updatePolicy(); mpc_mrt_interface_->updatePolicy();
// Evaluate the current policy // Evaluate the current policy
vector_t optimizedState, optimizedInput; vector_t optimizedState, optimizedInput;
size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at. size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at.
mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, mpc_mrt_interface_->evaluatePolicy(current_observation_.time, current_observation_.state, optimizedState,
optimizedInput, plannedMode); optimizedInput, plannedMode);
// Whole body control // Whole body control
currentObservation_.input = optimizedInput; current_observation_.input = optimizedInput;
wbcTimer_.startTimer(); wbc_timer_.startTimer();
vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds()); vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds());
wbcTimer_.endTimer(); wbc_timer_.endTimer();
vector_t torque = x.tail(12); vector_t torque = x.tail(12);
@ -84,7 +83,7 @@ namespace ocs2::legged_robot {
legged_interface_->getCentroidalModelInfo()); legged_interface_->getCentroidalModelInfo());
// Safety check, if failed, stop the controller // Safety check, if failed, stop the controller
if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) { if (!safety_checker_->check(current_observation_, optimizedState, optimizedInput)) {
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller."); RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
} }
@ -100,6 +99,7 @@ namespace ocs2::legged_robot {
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
// Initialize OCS2 // Initialize OCS2
urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_); urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_);
task_file_ = auto_declare<std::string>("task_file", task_file_); task_file_ = auto_declare<std::string>("task_file", task_file_);
@ -145,7 +145,7 @@ namespace ocs2::legged_robot {
wbc_->loadTasksSetting(task_file_, verbose_); wbc_->loadTasksSetting(task_file_, verbose_);
// Safety Checker // Safety Checker
safetyChecker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo()); safety_checker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
@ -194,28 +194,29 @@ namespace ocs2::legged_robot {
} }
} }
if (mpcRunning_ == false) { if (mpc_running_ == false) {
// Initial state // Initial state
currentObservation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim)); current_observation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000)); updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000));
currentObservation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim)); current_observation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
currentObservation_.mode = STANCE; current_observation_.mode = STANCE;
TargetTrajectories target_trajectories({currentObservation_.time}, {currentObservation_.state}, const TargetTrajectories target_trajectories({current_observation_.time}, {current_observation_.state},
{currentObservation_.input}); {current_observation_.input});
// Set the first observation and command and wait for optimization to finish // Set the first observation and command and wait for optimization to finish
mpcMrtInterface_->setCurrentObservation(currentObservation_); mpc_mrt_interface_->setCurrentObservation(current_observation_);
mpcMrtInterface_->getReferenceManager().setTargetTrajectories(target_trajectories); mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ..."); RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
while (!mpcMrtInterface_->initialPolicyReceived()) { while (!mpc_mrt_interface_->initialPolicyReceived()) {
std::cout<<legged_interface_->mpcSettings().mrtDesiredFrequency_<<std::endl; std::cout<<"Waiting for the initial policy ..."<<std::endl;
mpcMrtInterface_->advanceMpc(); mpc_mrt_interface_->advanceMpc();
std::cout<<"Advance MPC"<<std::endl;
rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep(); rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
} }
RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received."); RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received.");
mpcRunning_ = true; mpc_running_ = true;
} }
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
@ -251,7 +252,7 @@ namespace ocs2::legged_robot {
mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(), mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
legged_interface_->getOptimalControlProblem(), legged_interface_->getOptimalControlProblem(),
legged_interface_->getInitializer()); legged_interface_->getInitializer());
rbdConversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(), rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo()); legged_interface_->getCentroidalModelInfo());
const std::string robotName = "legged_robot"; const std::string robotName = "legged_robot";
@ -263,40 +264,42 @@ namespace ocs2::legged_robot {
getGaitSchedule()); getGaitSchedule());
gait_manager_ptr->init(gait_file_); gait_manager_ptr->init(gait_file_);
// Todo Here maybe the reason of the nullPointer.
// ROS ReferenceManager // ROS ReferenceManager
// auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>( const auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
// robotName, legged_interface_->getReferenceManagerPtr()); robotName, legged_interface_->getReferenceManagerPtr());
// rosReferenceManagerPtr->subscribe(this->get_node()); // rosReferenceManagerPtr->subscribe(this->get_node());
// mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr); mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
// mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
// observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1); // observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
} }
void Ocs2QuadrupedController::setupMrt() { void Ocs2QuadrupedController::setupMrt() {
mpcMrtInterface_ = std::make_shared<MPC_MRT_Interface>(*mpc_); mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
mpcMrtInterface_->initRollout(&legged_interface_->getRollout()); mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
mpcTimer_.reset(); mpc_timer_.reset();
controllerRunning_ = true; controller_running_ = true;
mpcThread_ = std::thread([&] { mpc_thread_ = std::thread([&] {
while (controllerRunning_) { while (controller_running_) {
try { try {
executeAndSleep( executeAndSleep(
[&] { [&] {
if (mpcRunning_) { if (mpc_running_) {
mpcTimer_.startTimer(); mpc_timer_.startTimer();
mpcMrtInterface_->advanceMpc(); mpc_mrt_interface_->advanceMpc();
mpcTimer_.endTimer(); mpc_timer_.endTimer();
} }
}, },
legged_interface_->mpcSettings().mpcDesiredFrequency_); legged_interface_->mpcSettings().mpcDesiredFrequency_);
} catch (const std::exception &e) { } catch (const std::exception &e) {
controllerRunning_ = false; controller_running_ = false;
RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what()); RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
} }
} }
}); });
setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpcThread_); setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
RCLCPP_INFO(get_node()->get_logger(), "MRT initialized. MPC thread started.");
} }
void Ocs2QuadrupedController::setupStateEstimate() { void Ocs2QuadrupedController::setupStateEstimate() {
@ -304,18 +307,17 @@ namespace ocs2::legged_robot {
legged_interface_->getCentroidalModelInfo(), legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, ctrl_comp_, this->get_node()); *eeKinematicsPtr_, ctrl_comp_, this->get_node());
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
currentObservation_.time = 0; current_observation_.time = 0;
} }
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) { void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
measuredRbdState_ = ctrl_comp_.estimator_->update(time, period); measuredRbdState_ = ctrl_comp_.estimator_->update(time, period);
currentObservation_.time += period.seconds(); current_observation_.time += period.seconds();
const scalar_t yaw_last = currentObservation_.state(9); const scalar_t yaw_last = current_observation_.state(9);
currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(measuredRbdState_); current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measuredRbdState_);
currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance( current_observation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, currentObservation_.state(9)); yaw_last, current_observation_.state(9));
std::cout << ctrl_comp_.estimator_->getMode() << std::endl; current_observation_.mode = ctrl_comp_.estimator_->getMode();
currentObservation_.mode = ctrl_comp_.estimator_->getMode();
} }
} }

16
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h
View File

@ -102,7 +102,7 @@ namespace ocs2::legged_robot {
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_; rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
SystemObservation currentObservation_; SystemObservation current_observation_;
std::string task_file_; std::string task_file_;
std::string urdf_file_; std::string urdf_file_;
@ -116,20 +116,20 @@ namespace ocs2::legged_robot {
// Whole Body Control // Whole Body Control
std::shared_ptr<WbcBase> wbc_; std::shared_ptr<WbcBase> wbc_;
std::shared_ptr<SafetyChecker> safetyChecker_; std::shared_ptr<SafetyChecker> safety_checker_;
// Nonlinear MPC // Nonlinear MPC
std::shared_ptr<MPC_BASE> mpc_; std::shared_ptr<MPC_BASE> mpc_;
std::shared_ptr<MPC_MRT_Interface> mpcMrtInterface_; std::shared_ptr<MPC_MRT_Interface> mpc_mrt_interface_;
std::shared_ptr<CentroidalModelRbdConversions> rbdConversions_; std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
private: private:
vector_t measuredRbdState_; vector_t measuredRbdState_;
std::thread mpcThread_; std::thread mpc_thread_;
std::atomic_bool controllerRunning_{}, mpcRunning_{}; std::atomic_bool controller_running_{}, mpc_running_{};
benchmark::RepeatedTimer mpcTimer_; benchmark::RepeatedTimer mpc_timer_;
benchmark::RepeatedTimer wbcTimer_; benchmark::RepeatedTimer wbc_timer_;
}; };
} }