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

achieved basic control on go1

This commit is contained in:
Huang Zhenbiao 2024-09-30 11:59:35 +08:00
parent 0ed329a44d
commit 4cb1657df5
7 changed files with 156 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -52,6 +52,7 @@ add_library(${PROJECT_NAME} SHARED
src/interface/LeggedInterface.cpp
src/control/GaitManager.cpp
src/control/TargetManager.cpp
)
target_include_directories(${PROJECT_NAME}

4
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h
View File

@ -9,7 +9,9 @@
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#include <ocs2_mpc/SystemObservation.h>
#include "TargetManager.h"
#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
struct CtrlComponent {
@ -39,9 +41,11 @@ struct CtrlComponent {
foot_force_state_interface_;
control_input_msgs::msg::Inputs control_inputs_;
ocs2::SystemObservation observation_;
int frequency_{};
std::shared_ptr<ocs2::legged_robot::StateEstimateBase> estimator_;
std::shared_ptr<ocs2::legged_robot::TargetManager> target_manager_;
CtrlComponent() {
}

57
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h Normal file
View File

@ -0,0 +1,57 @@
//
// Created by tlab-uav on 24-9-30.
//
#ifndef TARGETMANAGER_H
#define TARGETMANAGER_H
#include <memory>
#include <ocs2_mpc/SystemObservation.h>
#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h>
struct CtrlComponent;
namespace ocs2::legged_robot {
class TargetManager {
public:
TargetManager(CtrlComponent &ctrl_component,
const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
const std::string& task_file,
const std::string& reference_file);
~TargetManager() = default;
void update();
private:
TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose,
const SystemObservation &observation,
const scalar_t &targetReachingTime) {
// desired time trajectory
const scalar_array_t timeTrajectory{observation.time, targetReachingTime};
// desired state trajectory
vector_t currentPose = observation.state.segment<6>(6);
currentPose(2) = command_height_;
currentPose(4) = 0;
currentPose(5) = 0;
vector_array_t stateTrajectory(2, vector_t::Zero(observation.state.size()));
stateTrajectory[0] << vector_t::Zero(6), currentPose, default_joint_state_;
stateTrajectory[1] << vector_t::Zero(6), targetPose, default_joint_state_;
// desired input trajectory (just right dimensions, they are not used)
const vector_array_t inputTrajectory(2, vector_t::Zero(observation.input.size()));
return {timeTrajectory, stateTrajectory, inputTrajectory};
}
CtrlComponent &ctrl_component_;
std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_;
vector_t default_joint_state_{};
scalar_t command_height_{};
scalar_t time_to_target_{};
};
}
#endif //TARGETMANAGER_H

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

@ -58,8 +58,11 @@ namespace ocs2::legged_robot {
// State Estimate
updateStateEstimation(time, period);
// Compute target trajectory
ctrl_comp_.target_manager_->update();
// Update the current state of the system
mpc_mrt_interface_->setCurrentObservation(current_observation_);
mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
// Load the latest MPC policy
mpc_mrt_interface_->updatePolicy();
@ -67,24 +70,26 @@ namespace ocs2::legged_robot {
// Evaluate the current policy
vector_t optimized_state, optimized_input;
size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at.
mpc_mrt_interface_->evaluatePolicy(current_observation_.time, current_observation_.state, optimized_state,
mpc_mrt_interface_->evaluatePolicy(ctrl_comp_.observation_.time, ctrl_comp_.observation_.state, optimized_state,
optimized_input, planned_mode);
// Whole body control
current_observation_.input = optimized_input;
ctrl_comp_.observation_.input = optimized_input;
wbc_timer_.startTimer();
vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode, period.seconds());
vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode,
period.seconds());
wbc_timer_.endTimer();
vector_t torque = x.tail(12);
vector_t pos_des = centroidal_model::getJointAngles(optimized_state, legged_interface_->getCentroidalModelInfo());
vector_t pos_des = centroidal_model::getJointAngles(optimized_state,
legged_interface_->getCentroidalModelInfo());
vector_t vel_des = centroidal_model::getJointVelocities(optimized_input,
legged_interface_->getCentroidalModelInfo());
legged_interface_->getCentroidalModelInfo());
// Safety check, if failed, stop the controller
if (!safety_checker_->check(current_observation_, optimized_state, optimized_input)) {
if (!safety_checker_->check(ctrl_comp_.observation_, optimized_state, optimized_input)) {
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
for (int i = 0; i < joint_names_.size(); i++) {
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
@ -101,10 +106,10 @@ namespace ocs2::legged_robot {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(pos_des(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(6.0);
}
observation_publisher_->publish(ros_msg_conversions::createObservationMsg(current_observation_));
observation_publisher_->publish(ros_msg_conversions::createObservationMsg(ctrl_comp_.observation_));
return controller_interface::return_type::OK;
}
@ -190,7 +195,6 @@ namespace ocs2::legged_robot {
// assign command interfaces
for (auto &interface: command_interfaces_) {
std::string interface_name = interface.get_interface_name();
std::cout << "interface_name: " << interface.get_prefix_name() << std::endl;
if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else {
@ -211,16 +215,16 @@ namespace ocs2::legged_robot {
if (mpc_running_ == false) {
// Initial state
current_observation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
ctrl_comp_.observation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000));
current_observation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
current_observation_.mode = STANCE;
ctrl_comp_.observation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
ctrl_comp_.observation_.mode = STANCE;
const TargetTrajectories target_trajectories({current_observation_.time}, {current_observation_.state},
{current_observation_.input});
const TargetTrajectories target_trajectories({ctrl_comp_.observation_.time}, {ctrl_comp_.observation_.state},
{ctrl_comp_.observation_.input});
// Set the first observation and command and wait for optimization to finish
mpc_mrt_interface_->setCurrentObservation(current_observation_);
mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
while (!mpc_mrt_interface_->initialPolicyReceived()) {
@ -268,22 +272,17 @@ namespace ocs2::legged_robot {
rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo());
const std::string robotName = "legged_robot";
// Todo Handle Gait Receive.
// Gait receiver
// Initialize the reference manager
const auto gait_manager_ptr = std::make_shared<GaitManager>(
ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule());
gait_manager_ptr->init(gait_file_);
// Todo Here maybe the reason of the nullPointer.
// ROS ReferenceManager
const auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
robotName, legged_interface_->getReferenceManagerPtr());
rosReferenceManagerPtr->subscribe(get_node());
mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
ctrl_comp_.target_manager_ = std::make_shared<TargetManager>(ctrl_comp_,
legged_interface_->getReferenceManagerPtr(),
task_file_, reference_file_);
}
void Ocs2QuadrupedController::setupMrt() {
@ -319,17 +318,17 @@ namespace ocs2::legged_robot {
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, ctrl_comp_, this->get_node());
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
current_observation_.time = 0;
ctrl_comp_.observation_.time = 0;
}
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
measured_rbd_state_ = ctrl_comp_.estimator_->update(time, period);
current_observation_.time += period.seconds();
const scalar_t yaw_last = current_observation_.state(9);
current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
current_observation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, current_observation_.state(9));
current_observation_.mode = ctrl_comp_.estimator_->getMode();
ctrl_comp_.observation_.time += period.seconds();
const scalar_t yaw_last = ctrl_comp_.observation_.state(9);
ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, ctrl_comp_.observation_.state(9));
ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
}
}

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

@ -103,9 +103,6 @@ namespace ocs2::legged_robot {
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
rclcpp::Publisher<ocs2_msgs::msg::MpcObservation>::SharedPtr observation_publisher_;
SystemObservation current_observation_;
std::string task_file_;
std::string urdf_file_;
std::string reference_file_;

11
controllers/ocs2_quadruped_controller/src/SafetyChecker.h
View File

@ -5,7 +5,6 @@
#pragma once
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_mpc/SystemObservation.h>
namespace ocs2::legged_robot {
@ -14,15 +13,15 @@ namespace ocs2::legged_robot {
explicit SafetyChecker(const CentroidalModelInfo &info) : info_(info) {
}
bool check(const SystemObservation &observation, const vector_t & /*optimized_state*/,
const vector_t & /*optimized_input*/) {
[[nodiscard]] bool check(const SystemObservation &observation, const vector_t & /*optimized_state*/,
const vector_t & /*optimized_input*/) const {
return checkOrientation(observation);
}
protected:
bool checkOrientation(const SystemObservation &observation) {
vector_t pose = centroidal_model::getBasePose(observation.state, info_);
if (pose(5) > M_PI_2 || pose(5) < -M_PI_2) {
[[nodiscard]] bool checkOrientation(const SystemObservation &observation) const {
if (vector_t pose = centroidal_model::getBasePose(observation.state, info_);
pose(5) > M_PI_2 || pose(5) < -M_PI_2) {
std::cerr << "[SafetyChecker] Orientation safety check failed!" << std::endl;
return false;
}

56
controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp Normal file
View File

@ -0,0 +1,56 @@
//
// Created by tlab-uav on 24-9-30.
//
#include "ocs2_quadruped_controller/control/TargetManager.h"
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_robotic_tools/common/RotationTransforms.h>
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot {
TargetManager::TargetManager(CtrlComponent &ctrl_component,
const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
const std::string &task_file,
const std::string &reference_file)
: ctrl_component_(ctrl_component),
referenceManagerPtr_(referenceManagerPtr) {
default_joint_state_ = vector_t::Zero(12);
loadData::loadCppDataType(reference_file, "comHeight", command_height_);
loadData::loadEigenMatrix(reference_file, "defaultJointState", default_joint_state_);
loadData::loadCppDataType(task_file, "mpc.timeHorizon", time_to_target_);
}
void TargetManager::update() {
vector_t cmdGoal = vector_t::Zero(6);
cmdGoal[0] = ctrl_component_.control_inputs_.ly;
cmdGoal[1] = -ctrl_component_.control_inputs_.lx;
cmdGoal[2] = ctrl_component_.control_inputs_.ry;
cmdGoal[3] = -ctrl_component_.control_inputs_.rx;
const vector_t currentPose = ctrl_component_.observation_.state.segment<6>(6);
const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
vector_t cmdVelRot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3);
const scalar_t timeToTarget = time_to_target_;
const vector_t targetPose = [&]() {
vector_t target(6);
target(0) = currentPose(0) + cmdVelRot(0) * timeToTarget;
target(1) = currentPose(1) + cmdVelRot(1) * timeToTarget;
target(2) = command_height_;
target(3) = currentPose(3) + cmdGoal(3) * timeToTarget;
target(4) = 0;
target(5) = 0;
return target;
}();
const scalar_t targetReachingTime = ctrl_component_.observation_.time + timeToTarget;
auto trajectories =
targetPoseToTargetTrajectories(targetPose, ctrl_component_.observation_, targetReachingTime);
trajectories.stateTrajectory[0].head(3) = cmdVelRot;
trajectories.stateTrajectory[1].head(3) = cmdVelRot;
referenceManagerPtr_->setTargetTrajectories(std::move(trajectories));
}
}