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

refact with reference

This commit is contained in:
Huang Zhenbiao 2024-09-18 13:52:37 +08:00
parent e390835458
commit da5be83eda
11 changed files with 65 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h
View File

@ -20,6 +20,7 @@ public:
FSMStateName checkChange() override; FSMStateName checkChange() override;
private: private:
QuadrupedRobot &robot_model_;
void calc_body_target(float row, float pitch, float yaw, float height); void calc_body_target(float row, float pitch, float yaw, float height);
float row_max_, row_min_; float row_max_, row_min_;

4
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h
View File

@ -10,7 +10,7 @@
class StateSwingTest final : public FSMState { class StateSwingTest final : public FSMState {
public: public:
explicit StateSwingTest(CtrlComponent &ctrlComp); explicit StateSwingTest(CtrlComponent &ctrl_component);
void enter() override; void enter() override;
@ -21,11 +21,11 @@ public:
FSMStateName checkChange() override; FSMStateName checkChange() override;
private: private:
void positionCtrl(); void positionCtrl();
void torqueCtrl() const; void torqueCtrl() const;
QuadrupedRobot &robot_model_;
float _xMin, _xMax; float _xMin, _xMax;
float _yMin, _yMax; float _yMin, _yMax;
float _zMin, _zMax; float _zMin, _zMax;

19
controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h
View File

@ -42,20 +42,15 @@ struct CtrlComponent {
std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_; std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
int frequency_{}; int frequency_{};
QuadrupedRobot default_robot_model_; QuadrupedRobot robot_model_;
std::reference_wrapper<QuadrupedRobot> robot_model_; Estimator estimator_;
BalanceCtrl balance_ctrl_;
Estimator default_estimator_; WaveGenerator wave_generator_;
std::reference_wrapper<Estimator> estimator_;
BalanceCtrl default_balance_ctrl_; CtrlComponent() : control_inputs_(default_inputs_),
std::reference_wrapper<BalanceCtrl> balance_ctrl_; robot_model_(*this),
estimator_(*this) {
WaveGenerator default_wave_generator_;
std::reference_wrapper<WaveGenerator> wave_generator_;
CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_),
estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_), wave_generator_(default_wave_generator_) {
} }
void clear() { void clear() {

8
controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h
View File

@ -10,11 +10,12 @@
#include "LowPassFilter.h" #include "LowPassFilter.h"
class QuadrupedRobot;
struct CtrlComponent; struct CtrlComponent;
class Estimator { class Estimator {
public: public:
Estimator(); explicit Estimator(CtrlComponent &ctrl_component);
~Estimator() = default; ~Estimator() = default;
@ -68,9 +69,12 @@ public:
return rotation_ * gyro_; return rotation_ * gyro_;
} }
void update(const CtrlComponent &ctrlComp); void update();
private: private:
CtrlComponent &ctrl_component_;
QuadrupedRobot &robot_model_;
Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
Eigen::Matrix<double, 3, 1> _u; // The input of estimator Eigen::Matrix<double, 3, 1> _u; // The input of estimator

6
controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h
View File

@ -16,7 +16,8 @@ struct CtrlComponent;
class QuadrupedRobot { class QuadrupedRobot {
public: public:
explicit QuadrupedRobot() = default; explicit QuadrupedRobot(CtrlComponent &ctrl_component): ctrl_component_(ctrl_component) {
}
~QuadrupedRobot() = default; ~QuadrupedRobot() = default;
@ -84,9 +85,10 @@ public:
std::vector<KDL::JntArray> current_joint_pos_; std::vector<KDL::JntArray> current_joint_pos_;
std::vector<KDL::JntArray> current_joint_vel_; std::vector<KDL::JntArray> current_joint_vel_;
void update(const CtrlComponent &ctrlComp); void update();
private: private:
CtrlComponent &ctrl_component_;
std::vector<std::shared_ptr<RobotLeg> > robot_legs_; std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
KDL::Chain fr_chain_; KDL::Chain fr_chain_;

10
controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
View File

@ -4,13 +4,13 @@
#include "unitree_guide_controller/FSM/StateBalanceTest.h" #include "unitree_guide_controller/FSM/StateBalanceTest.h"
#include <unitree_guide_controller/common/mathTools.h> #include "unitree_guide_controller/common/mathTools.h"
StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test", StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
ctrlComp), ctrlComp),
estimator_(ctrlComp.estimator_.get()), estimator_(ctrlComp.estimator_),
robot_model_(ctrlComp.robot_model_.get()), robot_model_(ctrlComp.robot_model_),
balance_ctrl_(ctrlComp.balance_ctrl_.get()) { balance_ctrl_(ctrlComp.balance_ctrl_) {
_xMax = 0.05; _xMax = 0.05;
_xMin = -_xMax; _xMin = -_xMax;
_yMax = 0.05; _yMax = 0.05;

10
controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
View File

@ -6,7 +6,8 @@
#include "unitree_guide_controller/common/mathTools.h" #include "unitree_guide_controller/common/mathTools.h"
StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand", StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand",
ctrl_component) { ctrl_component),
robot_model_(ctrl_component.robot_model_) {
row_max_ = 20 * M_PI / 180; row_max_ = 20 * M_PI / 180;
row_min_ = -row_max_; row_min_ = -row_max_;
pitch_max_ = 15 * M_PI / 180; pitch_max_ = 15 * M_PI / 180;
@ -23,8 +24,8 @@ void StateFreeStand::enter() {
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
} }
init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_; init_joint_pos_ = robot_model_.current_joint_pos_;
init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions(); init_foot_pos_ = robot_model_.getFeet2BPositions();
fr_init_pos_ = init_foot_pos_[0]; fr_init_pos_ = init_foot_pos_[0];
@ -35,7 +36,6 @@ void StateFreeStand::enter() {
} }
void StateFreeStand::run() { void StateFreeStand::run() {
ctrl_comp_.robot_model_.get().update(ctrl_comp_);
calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_), calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_),
invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_), invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_),
invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_), invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_),
@ -68,7 +68,7 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
goal_pos[i] = body_2_fr_pos * init_foot_pos_[i]; goal_pos[i] = body_2_fr_pos * init_foot_pos_[i];
} }
target_joint_pos_ = ctrl_comp_.robot_model_.get().getQ(goal_pos); target_joint_pos_ = robot_model_.getQ(goal_pos);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));

17
controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
View File

@ -6,8 +6,8 @@
#include "unitree_guide_controller/FSM/StateSwingTest.h" #include "unitree_guide_controller/FSM/StateSwingTest.h"
#include "unitree_guide_controller/common/mathTools.h" #include "unitree_guide_controller/common/mathTools.h"
StateSwingTest::StateSwingTest(CtrlComponent &ctrlComp): FSMState( StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
FSMStateName::SWINGTEST, "swing test", ctrlComp) { FSMStateName::SWINGTEST, "swing test", ctrl_component), robot_model_(ctrl_component.robot_model_) {
_xMin = -0.15; _xMin = -0.15;
_xMax = 0.10; _xMax = 0.10;
_yMin = -0.15; _yMin = -0.15;
@ -29,8 +29,8 @@ void StateSwingTest::enter() {
Kp = KDL::Vector(20, 20, 50); Kp = KDL::Vector(20, 20, 50);
Kd = KDL::Vector(5, 5, 20); Kd = KDL::Vector(5, 5, 20);
init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_; init_joint_pos_ = robot_model_.current_joint_pos_;
init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions(); init_foot_pos_ = robot_model_.getFeet2BPositions();
target_foot_pos_ = init_foot_pos_; target_foot_pos_ = init_foot_pos_;
fr_init_pos_ = init_foot_pos_[0]; fr_init_pos_ = init_foot_pos_[0];
@ -60,7 +60,6 @@ void StateSwingTest::run() {
fr_init_pos_.p.z(), -1, 0)); fr_init_pos_.p.z(), -1, 0));
} }
ctrl_comp_.robot_model_.get().update(ctrl_comp_);
positionCtrl(); positionCtrl();
torqueCtrl(); torqueCtrl();
} }
@ -81,7 +80,7 @@ FSMStateName StateSwingTest::checkChange() {
void StateSwingTest::positionCtrl() { void StateSwingTest::positionCtrl() {
target_foot_pos_[0] = fr_goal_pos_; target_foot_pos_[0] = fr_goal_pos_;
target_joint_pos_ = ctrl_comp_.robot_model_.get().getQ(target_foot_pos_); target_joint_pos_ = robot_model_.getQ(target_foot_pos_);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
@ -90,14 +89,14 @@ void StateSwingTest::positionCtrl() {
} }
void StateSwingTest::torqueCtrl() const { void StateSwingTest::torqueCtrl() const {
const KDL::Frame fr_current_pos = ctrl_comp_.robot_model_.get().getFeet2BPositions(0); const KDL::Frame fr_current_pos = robot_model_.getFeet2BPositions(0);
const KDL::Vector pos_goal = fr_goal_pos_.p; const KDL::Vector pos_goal = fr_goal_pos_.p;
const KDL::Vector pos0 = fr_current_pos.p; const KDL::Vector pos0 = fr_current_pos.p;
const KDL::Vector vel0 = ctrl_comp_.robot_model_.get().getFeet2BVelocities(0); const KDL::Vector vel0 = robot_model_.getFeet2BVelocities(0);
const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0; const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0;
KDL::JntArray torque0 = ctrl_comp_.robot_model_.get().getTorque(force0, 0); KDL::JntArray torque0 = robot_model_.getTorque(force0, 0);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
ctrl_comp_.joint_effort_command_interface_[i].get().set_value(torque0(i)); ctrl_comp_.joint_effort_command_interface_[i].get().set_value(torque0(i));

9
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -41,7 +41,6 @@ namespace unitree_guide_controller {
controller_interface::return_type UnitreeGuideController:: controller_interface::return_type UnitreeGuideController::
update(const rclcpp::Time &time, const rclcpp::Duration &period) { update(const rclcpp::Time &time, const rclcpp::Duration &period) {
// auto now = std::chrono::steady_clock::now(); // auto now = std::chrono::steady_clock::now();
// std::chrono::duration<double> time_diff = now - last_update_time_; // std::chrono::duration<double> time_diff = now - last_update_time_;
// last_update_time_ = now; // last_update_time_ = now;
@ -51,8 +50,8 @@ namespace unitree_guide_controller {
// RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_); // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
ctrl_comp_.robot_model_.get().update(ctrl_comp_); ctrl_comp_.robot_model_.update();
ctrl_comp_.estimator_.get().update(ctrl_comp_); ctrl_comp_.estimator_.update();
if (mode_ == FSMMode::NORMAL) { if (mode_ == FSMMode::NORMAL) {
current_state_->run(); current_state_->run();
@ -108,8 +107,8 @@ namespace unitree_guide_controller {
robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>( robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
"~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), "~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
[this](const std_msgs::msg::String::SharedPtr msg) { [this](const std_msgs::msg::String::SharedPtr msg) {
ctrl_comp_.robot_model_.get().init(msg->data); ctrl_comp_.robot_model_.init(msg->data);
ctrl_comp_.balance_ctrl_.get().init(ctrl_comp_.robot_model_.get()); ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
}); });
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);

31
controllers/unitree_guide_controller/src/control/Estimator.cpp
View File

@ -6,7 +6,8 @@
#include "unitree_guide_controller/control/CtrlComponent.h" #include "unitree_guide_controller/control/CtrlComponent.h"
#include "unitree_guide_controller/common/mathTools.h" #include "unitree_guide_controller/common/mathTools.h"
Estimator::Estimator() { Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
robot_model_(ctrl_component.robot_model_) {
g_ = KDL::Vector(0, 0, -9.81); g_ = KDL::Vector(0, 0, -9.81);
_dt = 0.002; _dt = 0.002;
_largeVariance = 100; _largeVariance = 100;
@ -138,14 +139,14 @@ Estimator::Estimator() {
low_pass_filters_[2] = std::make_shared<LowPassFilter>(0.02, 3.0); low_pass_filters_[2] = std::make_shared<LowPassFilter>(0.02, 3.0);
} }
void Estimator::update(const CtrlComponent &ctrlComp) { void Estimator::update() {
if (ctrlComp.robot_model_.get().mass_ == 0) return; if (robot_model_.mass_ == 0) return;
Q = QInit_; Q = QInit_;
R = RInit_; R = RInit_;
foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions(); foot_poses_ = robot_model_.getFeet2BPositions();
foot_vels_ = ctrlComp.robot_model_.get().getFeet2BVelocities(); foot_vels_ = robot_model_.getFeet2BVelocities();
_feetH.setZero(); _feetH.setZero();
const std::vector contact(4, 1); const std::vector contact(4, 1);
@ -175,18 +176,18 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
} }
// Acceleration from imu as system input // Acceleration from imu as system input
rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[1].get().get_value(), rotation_ = KDL::Rotation::Quaternion(ctrl_component_.imu_state_interface_[1].get().get_value(),
ctrlComp.imu_state_interface_[2].get().get_value(), ctrl_component_.imu_state_interface_[2].get().get_value(),
ctrlComp.imu_state_interface_[3].get().get_value(), ctrl_component_.imu_state_interface_[3].get().get_value(),
ctrlComp.imu_state_interface_[0].get().get_value()); ctrl_component_.imu_state_interface_[0].get().get_value());
gyro_ = KDL::Vector(ctrlComp.imu_state_interface_[4].get().get_value(), gyro_ = KDL::Vector(ctrl_component_.imu_state_interface_[4].get().get_value(),
ctrlComp.imu_state_interface_[5].get().get_value(), ctrl_component_.imu_state_interface_[5].get().get_value(),
ctrlComp.imu_state_interface_[6].get().get_value()); ctrl_component_.imu_state_interface_[6].get().get_value());
acceleration_ = KDL::Vector(ctrlComp.imu_state_interface_[7].get().get_value(), acceleration_ = KDL::Vector(ctrl_component_.imu_state_interface_[7].get().get_value(),
ctrlComp.imu_state_interface_[8].get().get_value(), ctrl_component_.imu_state_interface_[8].get().get_value(),
ctrlComp.imu_state_interface_[9].get().get_value()); ctrl_component_.imu_state_interface_[9].get().get_value());
_u = Vec3((rotation_ * acceleration_ + g_).data); _u = Vec3((rotation_ * acceleration_ + g_).data);
x_hat_ = A * x_hat_ + B * _u; x_hat_ = A * x_hat_ + B * _u;

14
controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp
View File

@ -83,19 +83,19 @@ std::vector<KDL::Vector> QuadrupedRobot::getFeet2BVelocities() const {
return result; return result;
} }
void QuadrupedRobot::update(const CtrlComponent &ctrlComp) { void QuadrupedRobot::update() {
if (mass_ == 0) return; if (mass_ == 0) return;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
KDL::JntArray pos_array(3); KDL::JntArray pos_array(3);
pos_array(0) = ctrlComp.joint_position_state_interface_[i * 3].get().get_value(); pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value();
pos_array(1) = ctrlComp.joint_position_state_interface_[i * 3 + 1].get().get_value(); pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value();
pos_array(2) = ctrlComp.joint_position_state_interface_[i * 3 + 2].get().get_value(); pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value();
current_joint_pos_[i] = pos_array; current_joint_pos_[i] = pos_array;
KDL::JntArray vel_array(3); KDL::JntArray vel_array(3);
vel_array(0) = ctrlComp.joint_velocity_state_interface_[i * 3].get().get_value(); vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value();
vel_array(1) = ctrlComp.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
vel_array(2) = ctrlComp.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
current_joint_vel_[i] = vel_array; current_joint_vel_[i] = vel_array;
} }
} }