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

add gait manager

This commit is contained in:
Huang Zhenbiao 2024-09-26 17:28:18 +08:00
parent 3149acf2ed
commit c4262b42da
8 changed files with 409 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -51,6 +51,8 @@ add_library(${PROJECT_NAME} SHARED
src/interface/LeggedRobotPreComputation.cpp
src/interface/LeggedInterface.cpp
src/control/GaitManager.cpp
)
target_include_directories(${PROJECT_NAME}
PUBLIC

42
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h Normal file
View File

@ -0,0 +1,42 @@
//
// Created by tlab-uav on 24-9-26.
//
#ifndef GAITMANAGER_H
#define GAITMANAGER_H
#include <ocs2_legged_robot/gait/GaitSchedule.h>
#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
#include "CtrlComponent.h"
namespace ocs2::legged_robot {
class GaitManager final : public SolverSynchronizedModule {
public:
GaitManager(CtrlComponent &ctrl_component,
std::shared_ptr<GaitSchedule> gait_schedule_ptr);
void preSolverRun(scalar_t initTime, scalar_t finalTime,
const vector_t &currentState,
const ReferenceManagerInterface &referenceManager) override;
void postSolverRun(const PrimalSolution &primalSolution) override {
}
void init(const std::string &gait_file);
private:
void getTargetGait();
CtrlComponent &ctrl_component_;
std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
ModeSequenceTemplate target_gait_;
bool gait_updated_{false};
bool verbose_{false};
std::vector<ModeSequenceTemplate> gait_list_;
std::vector<std::string> gait_name_list_;
};
}
#endif //GAITMANAGER_H

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

@ -13,6 +13,7 @@
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
#include <ocs2_sqp/SqpMpc.h>
#include <angles/angles.h>
#include <ocs2_quadruped_controller/control/GaitManager.h>
namespace ocs2::legged_robot {
using config_type = controller_interface::interface_configuration_type;
@ -53,13 +54,9 @@ namespace ocs2::legged_robot {
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
const rclcpp::Duration &period) {
// State Estimate
const vector_t rbd_state = ctrl_comp_.estimator_->update(time, period);
currentObservation_.time += period.seconds();
const scalar_t yaw_last = currentObservation_.state(9);
currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(rbd_state);
currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(yaw_last, currentObservation_.state(9));
currentObservation_.mode = stateEstimate_->getMode();
updateStateEstimation(time, period);
// Update the current state of the system
mpcMrtInterface_->setCurrentObservation(currentObservation_);
@ -70,19 +67,21 @@ namespace ocs2::legged_robot {
// Evaluate the current policy
vector_t optimizedState, optimizedInput;
size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at.
mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, optimizedInput, plannedMode);
mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState,
optimizedInput, plannedMode);
// Whole body control
currentObservation_.input = optimizedInput;
wbcTimer_.startTimer();
vector_t x = wbc_->update(optimizedState, optimizedInput, rbd_state, plannedMode, period.seconds());
vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds());
wbcTimer_.endTimer();
vector_t torque = x.tail(12);
vector_t posDes = centroidal_model::getJointAngles(optimizedState, legged_interface_->getCentroidalModelInfo());
vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, legged_interface_->getCentroidalModelInfo());
vector_t velDes = centroidal_model::getJointVelocities(optimizedInput,
legged_interface_->getCentroidalModelInfo());
// Safety check, if failed, stop the controller
if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) {
@ -105,6 +104,7 @@ namespace ocs2::legged_robot {
urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_);
task_file_ = auto_declare<std::string>("task_file", task_file_);
reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
gait_file_ = auto_declare<std::string>("gait_file", gait_file_);
verbose_ = false;
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
@ -194,6 +194,30 @@ namespace ocs2::legged_robot {
}
}
if (mpcRunning_ == false) {
// Initial state
currentObservation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000));
currentObservation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
currentObservation_.mode = STANCE;
TargetTrajectories target_trajectories({currentObservation_.time}, {currentObservation_.state},
{currentObservation_.input});
// Set the first observation and command and wait for optimization to finish
mpcMrtInterface_->setCurrentObservation(currentObservation_);
mpcMrtInterface_->getReferenceManager().setTargetTrajectories(target_trajectories);
RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
while (!mpcMrtInterface_->initialPolicyReceived()) {
std::cout<<legged_interface_->mpcSettings().mrtDesiredFrequency_<<std::endl;
mpcMrtInterface_->advanceMpc();
rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
}
RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received.");
mpcRunning_ = true;
}
return CallbackReturn::SUCCESS;
}
@ -234,16 +258,16 @@ namespace ocs2::legged_robot {
// Todo Handle Gait Receive.
// Gait receiver
// auto gaitReceiverPtr =
// std::make_shared<GaitReceiver>(this->get_node(),
// legged_interface_->getSwitchedModelReferenceManagerPtr()->
// getGaitSchedule(), robotName);
const auto gait_manager_ptr = std::make_shared<GaitManager>(
ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule());
gait_manager_ptr->init(gait_file_);
// ROS ReferenceManager
// auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
// robotName, legged_interface_->getReferenceManagerPtr());
// rosReferenceManagerPtr->subscribe(this->get_node());
// mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
// mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
// mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
// observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
}
@ -282,6 +306,17 @@ namespace ocs2::legged_robot {
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
currentObservation_.time = 0;
}
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
measuredRbdState_ = ctrl_comp_.estimator_->update(time, period);
currentObservation_.time += period.seconds();
const scalar_t yaw_last = currentObservation_.state(9);
currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(measuredRbdState_);
currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, currentObservation_.state(9));
std::cout << ctrl_comp_.estimator_->getMode() << std::endl;
currentObservation_.mode = ctrl_comp_.estimator_->getMode();
}
}
#include "pluginlib/class_list_macros.hpp"

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

@ -67,6 +67,9 @@ namespace ocs2::legged_robot {
void setupStateEstimate();
void updateStateEstimation(const rclcpp::Time &time,
const rclcpp::Duration &period);
CtrlComponent ctrl_comp_;
std::vector<std::string> joint_names_;
std::vector<std::string> command_interface_types_;
@ -104,6 +107,7 @@ namespace ocs2::legged_robot {
std::string task_file_;
std::string urdf_file_;
std::string reference_file_;
std::string gait_file_;
bool verbose_;
@ -119,9 +123,9 @@ namespace ocs2::legged_robot {
std::shared_ptr<MPC_MRT_Interface> mpcMrtInterface_;
std::shared_ptr<CentroidalModelRbdConversions> rbdConversions_;
std::shared_ptr<StateEstimateBase> stateEstimate_;
private:
vector_t measuredRbdState_;
std::thread mpcThread_;
std::atomic_bool controllerRunning_{}, mpcRunning_{};
benchmark::RepeatedTimer mpcTimer_;

51
controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp Normal file
View File

@ -0,0 +1,51 @@
//
// Created by tlab-uav on 24-9-26.
//
#include <utility>
#include "ocs2_quadruped_controller/control/GaitManager.h"
#include <ocs2_core/misc/LoadData.h>
namespace ocs2::legged_robot {
GaitManager::GaitManager(CtrlComponent &ctrl_component,
std::shared_ptr<GaitSchedule> gait_schedule_ptr)
: ctrl_component_(ctrl_component),
gait_schedule_ptr_(std::move(gait_schedule_ptr)),
target_gait_({0.0, 1.0}, {STANCE}) {
}
void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime,
const vector_t &currentState,
const ReferenceManagerInterface &referenceManager) {
getTargetGait();
if (gait_updated_) {
const auto timeHorizon = finalTime - initTime;
gait_schedule_ptr_->insertModeSequenceTemplate(target_gait_, finalTime,
timeHorizon);
gait_updated_ = false;
}
}
void GaitManager::init(const std::string &gait_file) {
gait_name_list_.clear();
loadData::loadStdVector(gait_file, "list", gait_name_list_, verbose_);
gait_list_.clear();
for (const auto &name: gait_name_list_) {
gait_list_.push_back(loadModeSequenceTemplate(gait_file, name, verbose_));
}
RCLCPP_INFO(rclcpp::get_logger("gait_manager"), "GaitManager is ready.");
}
void GaitManager::getTargetGait() {
std::cout << "ctrl_component_.control_inputs_.command: " << ctrl_component_.control_inputs_.command << std::endl;
if (ctrl_component_.control_inputs_.command == 0) return;
target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1];
RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
gait_updated_ = true;
}
}

1
controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp
View File

@ -12,7 +12,6 @@
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot {
using namespace legged_robot;
StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics,

255
descriptions/go2_description/config/ocs2/gait.info Normal file
View File

@ -0,0 +1,255 @@
list
{
[0] stance
[1] trot
[2] standing_trot
[3] flying_trot
[4] pace
[5] standing_pace
[6] dynamic_walk
[7] static_walk
[8] amble
[9] lindyhop
[10] skipping
[11] pawup
}
stance
{
modeSequence
{
[0] STANCE
}
switchingTimes
{
[0] 0.0
[1] 0.5
}
}
trot
{
modeSequence
{
[0] LF_RH
[1] RF_LH
}
switchingTimes
{
[0] 0.0
[1] 0.3
[2] 0.6
}
}
standing_trot
{
modeSequence
{
[0] LF_RH
[1] STANCE
[2] RF_LH
[3] STANCE
}
switchingTimes
{
[0] 0.00
[1] 0.25
[2] 0.3
[3] 0.55
[4] 0.6
}
}
flying_trot
{
modeSequence
{
[0] LF_RH
[1] FLY
[2] RF_LH
[3] FLY
}
switchingTimes
{
[0] 0.00
[1] 0.15
[2] 0.2
[3] 0.35
[4] 0.4
}
}
pace
{
modeSequence
{
[0] LF_LH
[1] FLY
[2] RF_RH
[3] FLY
}
switchingTimes
{
[0] 0.0
[1] 0.28
[2] 0.30
[3] 0.58
[4] 0.60
}
}
standing_pace
{
modeSequence
{
[0] LF_LH
[1] STANCE
[2] RF_RH
[3] STANCE
}
switchingTimes
{
[0] 0.0
[1] 0.30
[2] 0.35
[3] 0.65
[4] 0.70
}
}
dynamic_walk
{
modeSequence
{
[0] LF_RF_RH
[1] RF_RH
[2] RF_LH_RH
[3] LF_RF_LH
[4] LF_LH
[5] LF_LH_RH
}
switchingTimes
{
[0] 0.0
[1] 0.2
[2] 0.3
[3] 0.5
[4] 0.7
[5] 0.8
[6] 1.0
}
}
static_walk
{
modeSequence
{
[0] LF_RF_RH
[1] RF_LH_RH
[2] LF_RF_LH
[3] LF_LH_RH
}
switchingTimes
{
[0] 0.0
[1] 0.3
[2] 0.6
[3] 0.9
[4] 1.2
}
}
amble
{
modeSequence
{
[0] RF_LH
[1] LF_LH
[2] LF_RH
[3] RF_RH
}
switchingTimes
{
[0] 0.0
[1] 0.15
[2] 0.40
[3] 0.55
[4] 0.80
}
}
lindyhop
{
modeSequence
{
[0] LF_RH
[1] STANCE
[2] RF_LH
[3] STANCE
[4] LF_LH
[5] RF_RH
[6] LF_LH
[7] STANCE
[8] RF_RH
[9] LF_LH
[10] RF_RH
[11] STANCE
}
switchingTimes
{
[0] 0.00 ; Step 1
[1] 0.35 ; Stance
[2] 0.45 ; Step 2
[3] 0.80 ; Stance
[4] 0.90 ; Tripple step
[5] 1.125 ;
[6] 1.35 ;
[7] 1.70 ; Stance
[8] 1.80 ; Tripple step
[9] 2.025 ;
[10] 2.25 ;
[11] 2.60 ; Stance
[12] 2.70 ;
}
}
skipping
{
modeSequence
{
[0] LF_RH
[1] FLY
[2] LF_RH
[3] FLY
[4] RF_LH
[5] FLY
[6] RF_LH
[7] FLY
}
switchingTimes
{
[0] 0.00
[1] 0.27
[2] 0.30
[3] 0.57
[4] 0.60
[5] 0.87
[6] 0.90
[7] 1.17
[8] 1.20
}
}
pawup
{
modeSequence
{
[0] RF_LH_RH
}
switchingTimes
{
[0] 0.0
[1] 2.0
}
}

4
descriptions/go2_description/launch/ocs2_control.launch.py
View File

@ -54,7 +54,9 @@ def launch_setup(context, *args, **kwargs):
'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
'task.info'),
'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
'ocs2', 'reference.info')
'ocs2', 'reference.info'),
'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
'ocs2', 'gait.info')
}],
output="both",
)