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

used share_ptr rather than reference in CtrlComponent

This commit is contained in:
Zhenbiao Huang 2024-10-10 20:31:03 +08:00
parent 87b9759ddf
commit ff374d7005
25 changed files with 142 additions and 133 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/FSMState.h
View File

@ -7,7 +7,6 @@
#include <string>
#include <utility>
#include <rclcpp/time.hpp>
#include <unitree_guide_controller/common/enumClass.h>
#include <unitree_guide_controller/control/CtrlComponent.h>

8
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h
View File

@ -22,10 +22,10 @@ public:
private:
void calcTorque();
Estimator &estimator_;
QuadrupedRobot &robot_model_;
BalanceCtrl &balance_ctrl_;
WaveGenerator &wave_generator_;
std::shared_ptr<Estimator> &estimator_;
std::shared_ptr<QuadrupedRobot> &robot_model_;
std::shared_ptr<BalanceCtrl> &balance_ctrl_;
std::shared_ptr<WaveGenerator> &wave_generator_;
Vec3 pcd_, pcd_init_;
RotMat Rd_;

3
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h
View File

@ -5,6 +5,8 @@
#ifndef STATEFIXEDDOWN_H
#define STATEFIXEDDOWN_H
#include <rclcpp/time.hpp>
#include "FSMState.h"
class StateFixedDown final : public FSMState {
@ -18,6 +20,7 @@ public:
void exit() override;
FSMStateName checkChange() override;
private:
double target_pos_[12] = {
0.0473455, 1.22187, -2.44375, -0.0473455,

2
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
View File

@ -5,6 +5,8 @@
#ifndef STATEFIXEDSTAND_H
#define STATEFIXEDSTAND_H
#include <rclcpp/time.hpp>
#include "FSMState.h"
class StateFixedStand final : public FSMState {

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

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

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

@ -25,7 +25,7 @@ private:
void torqueCtrl() const;
QuadrupedRobot &robot_model_;
std::shared_ptr<QuadrupedRobot> &robot_model_;
float _xMin, _xMax;
float _yMin, _yMax;
float _zMin, _zMax;

9
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h
View File

@ -47,10 +47,11 @@ private:
*/
bool checkStepOrNot();
Estimator &estimator_;
QuadrupedRobot &robot_model_;
BalanceCtrl &balance_ctrl_;
WaveGenerator &wave_generator_;
std::shared_ptr<Estimator> &estimator_;
std::shared_ptr<QuadrupedRobot> &robot_model_;
std::shared_ptr<BalanceCtrl> &balance_ctrl_;
std::shared_ptr<WaveGenerator> &wave_generator_;
GaitGenerator gait_generator_;
// Robot State

6
controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h
View File

@ -5,17 +5,17 @@
#ifndef BALANCECTRL_H
#define BALANCECTRL_H
#include <memory>
#include "unitree_guide_controller/common/mathTypes.h"
class QuadrupedRobot;
class BalanceCtrl {
public:
explicit BalanceCtrl();
explicit BalanceCtrl(const std::shared_ptr<QuadrupedRobot>& robot);
~BalanceCtrl() = default;
void init(const QuadrupedRobot &robot);
/**
* Calculate the desired feet end force
* @param ddPcd desired body acceleration

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

@ -41,15 +41,13 @@ struct CtrlComponent {
control_input_msgs::msg::Inputs control_inputs_;
int frequency_{};
QuadrupedRobot robot_model_;
Estimator estimator_;
BalanceCtrl balance_ctrl_;
std::shared_ptr<QuadrupedRobot> robot_model_;
std::shared_ptr<Estimator> estimator_;
WaveGenerator wave_generator_;
std::shared_ptr<BalanceCtrl> balance_ctrl_;
std::shared_ptr<WaveGenerator> wave_generator_;
CtrlComponent() : robot_model_(*this),
estimator_(*this) {
}
CtrlComponent() = default;
void clear() {
joint_torque_command_interface_.clear();

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

@ -63,7 +63,7 @@ public:
* @return feet velocity in world frame
*/
Vec34 getFeetVel() {
const std::vector<KDL::Vector> feet_vel = robot_model_.getFeet2BVelocities();
const std::vector<KDL::Vector> feet_vel = robot_model_->getFeet2BVelocities();
Vec34 result;
for (int i(0); i < 4; ++i) {
result.col(i) = Vec3(feet_vel[i].data) + getVelocity();
@ -106,8 +106,8 @@ public:
private:
CtrlComponent &ctrl_component_;
QuadrupedRobot &robot_model_;
WaveGenerator &wave_generator_;
std::shared_ptr<QuadrupedRobot> &robot_model_;
std::shared_ptr<WaveGenerator> &wave_generator_;
Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)

6
controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCalc.h
View File

@ -5,6 +5,7 @@
#ifndef FOOTENDCTRL_H
#define FOOTENDCTRL_H
#include <memory>
#include <unitree_guide_controller/common/mathTypes.h>
@ -24,14 +25,13 @@ public:
private:
CtrlComponent &ctrl_component_;
QuadrupedRobot &robot_model_;
Estimator &estimator_;
std::shared_ptr<QuadrupedRobot> &robot_model_;
std::shared_ptr<Estimator> &estimator_;
Vec4 feet_radius_, feet_init_angle_;
double k_x_, k_y_, k_yaw_;
double t_stance_{}, t_swing_{};
};

5
controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h
View File

@ -5,6 +5,7 @@
#ifndef GAITGENERATOR_H
#define GAITGENERATOR_H
#include <memory>
#include <unitree_guide_controller/common/mathTypes.h>
#include "FeetEndCalc.h"
@ -66,8 +67,8 @@ private:
*/
[[nodiscard]] double cycloidZVelocity(double height, double phase) const;
WaveGenerator &wave_generator_;
Estimator &estimator_;
std::shared_ptr<WaveGenerator> &wave_generator_;
std::shared_ptr<Estimator> &estimator_;
FeetEndCalc feet_end_calc_;
double gait_height_{};

4
controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h
View File

@ -17,12 +17,10 @@ inline long long getSystemTime() {
class WaveGenerator {
public:
WaveGenerator();
WaveGenerator(double period, double st_ratio, const Vec4 &bias);
~WaveGenerator() = default;
void init(double period, double st_ratio, const Vec4 &bias);
void update();
[[nodiscard]] double get_t_stance() const { return period_ * st_ratio_; }

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

@ -16,13 +16,11 @@ struct CtrlComponent;
class QuadrupedRobot {
public:
explicit QuadrupedRobot(CtrlComponent &ctrl_component): ctrl_component_(ctrl_component) {
}
explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
const std::vector<std::string> &feet_names, const std::string &base_name);
~QuadrupedRobot() = default;
void init(const std::string &robot_description, const std::vector<std::string> &feet_names, const std::string& base_name);
/**
* Calculate the joint positions based on the foot end position
* @param pEe_list vector of foot-end position

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

@ -29,10 +29,10 @@ StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateN
}
void StateBalanceTest::enter() {
pcd_init_ = estimator_.getPosition();
pcd_init_ = estimator_->getPosition();
pcd_ = pcd_init_;
init_rotation_ = estimator_.getRotation();
wave_generator_.status_ = WaveStatus::STANCE_ALL;
init_rotation_ = estimator_->getRotation();
wave_generator_->status_ = WaveStatus::STANCE_ALL;
}
void StateBalanceTest::run() {
@ -52,7 +52,7 @@ void StateBalanceTest::run() {
}
void StateBalanceTest::exit() {
wave_generator_.status_ = WaveStatus::SWING_ALL;
wave_generator_->status_ = WaveStatus::SWING_ALL;
}
FSMStateName StateBalanceTest::checkChange() {
@ -67,28 +67,28 @@ FSMStateName StateBalanceTest::checkChange() {
}
void StateBalanceTest::calcTorque() {
const auto B2G_Rotation = estimator_.getRotation();
const auto B2G_Rotation = estimator_->getRotation();
const RotMat G2B_Rotation = B2G_Rotation.transpose();
const Vec3 pose_body = estimator_.getPosition();
const Vec3 vel_body = estimator_.getVelocity();
const Vec3 pose_body = estimator_->getPosition();
const Vec3 vel_body = estimator_->getVelocity();
// expected body acceleration
dd_pcd_ = Kp_p_ * (pcd_ - pose_body) + Kd_p_ * (Vec3(0, 0, 0) - vel_body);
// expected body angular acceleration
d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGyroGlobal());
Kd_w_ * (Vec3(0, 0, 0) - estimator_->getGyroGlobal());
// calculate foot force
const Vec34 pos_feet_2_body_global = estimator_.getFeetPos2Body();
const Vec34 force_feet_global = -balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
pos_feet_2_body_global, wave_generator_.contact_);
const Vec34 pos_feet_2_body_global = estimator_->getFeetPos2Body();
const Vec34 force_feet_global = -balance_ctrl_->calF(dd_pcd_, d_wbd_, B2G_Rotation,
pos_feet_2_body_global, wave_generator_->contact_);
const Vec34 force_feet_body = G2B_Rotation * force_feet_global;
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
std::vector<KDL::JntArray> current_joints = robot_model_->current_joint_pos_;
for (int i = 0; i < 4; i++) {
KDL::JntArray torque = robot_model_.getTorque(force_feet_body.col(i), i);
KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i);
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));

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

@ -24,8 +24,8 @@ void StateFreeStand::enter() {
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
}
init_joint_pos_ = robot_model_.current_joint_pos_;
init_foot_pos_ = robot_model_.getFeet2BPositions();
init_joint_pos_ = robot_model_->current_joint_pos_;
init_foot_pos_ = robot_model_->getFeet2BPositions();
fr_init_pos_ = init_foot_pos_[0];
@ -69,7 +69,7 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
for (int i = 0; i < 4; i++) {
goal_pos[i] = body_2_fr_pos * init_foot_pos_[i];
}
target_joint_pos_ = robot_model_.getQ(goal_pos);
target_joint_pos_ = robot_model_->getQ(goal_pos);
for (int i = 0; i < 4; i++) {
ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));

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

@ -7,7 +7,9 @@
#include "unitree_guide_controller/common/mathTools.h"
StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
FSMStateName::SWINGTEST, "swing test", ctrl_component), robot_model_(ctrl_component.robot_model_) {
FSMStateName::SWINGTEST, "swing test",
ctrl_component),
robot_model_(ctrl_component.robot_model_) {
_xMin = -0.15;
_xMax = 0.10;
_yMin = -0.15;
@ -29,8 +31,8 @@ void StateSwingTest::enter() {
Kp = KDL::Vector(20, 20, 50);
Kd = KDL::Vector(5, 5, 20);
init_joint_pos_ = robot_model_.current_joint_pos_;
init_foot_pos_ = robot_model_.getFeet2BPositions();
init_joint_pos_ = robot_model_->current_joint_pos_;
init_foot_pos_ = robot_model_->getFeet2BPositions();
target_foot_pos_ = init_foot_pos_;
fr_init_pos_ = init_foot_pos_[0];
@ -80,7 +82,7 @@ FSMStateName StateSwingTest::checkChange() {
void StateSwingTest::positionCtrl() {
target_foot_pos_[0] = fr_goal_pos_;
target_joint_pos_ = robot_model_.getQ(target_foot_pos_);
target_joint_pos_ = robot_model_->getQ(target_foot_pos_);
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 + 1].get().set_value(target_joint_pos_[i](1));
@ -89,14 +91,14 @@ void StateSwingTest::positionCtrl() {
}
void StateSwingTest::torqueCtrl() const {
const KDL::Frame fr_current_pos = robot_model_.getFeet2BPositions(0);
const KDL::Frame fr_current_pos = robot_model_->getFeet2BPositions(0);
const KDL::Vector pos_goal = fr_goal_pos_.p;
const KDL::Vector pos0 = fr_current_pos.p;
const KDL::Vector vel0 = robot_model_.getFeet2BVelocities(0);
const KDL::Vector vel0 = robot_model_->getFeet2BVelocities(0);
const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0;
KDL::JntArray torque0 = robot_model_.getTorque(force0, 0);
KDL::JntArray torque0 = robot_model_->getTorque(force0, 0);
for (int i = 0; i < 3; i++) {
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque0(i));

42
controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
View File

@ -27,10 +27,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
}
void StateTrotting::enter() {
pcd_ = estimator_.getPosition();
pcd_(2) = -estimator_.getFeetPos2Body()(2, 0);
pcd_ = estimator_->getPosition();
pcd_(2) = -estimator_->getFeetPos2Body()(2, 0);
v_cmd_body_.setZero();
yaw_cmd_ = estimator_.getYaw();
yaw_cmd_ = estimator_->getYaw();
Rd = rotz(yaw_cmd_);
w_cmd_global_.setZero();
@ -39,10 +39,10 @@ void StateTrotting::enter() {
}
void StateTrotting::run() {
pos_body_ = estimator_.getPosition();
vel_body_ = estimator_.getVelocity();
pos_body_ = estimator_->getPosition();
vel_body_ = estimator_->getVelocity();
B2G_RotMat = estimator_.getRotation();
B2G_RotMat = estimator_->getRotation();
G2B_RotMat = B2G_RotMat.transpose();
getUserCmd();
@ -55,16 +55,16 @@ void StateTrotting::run() {
calcQQd();
if (checkStepOrNot()) {
wave_generator_.status_ = WaveStatus::WAVE_ALL;
wave_generator_->status_ = WaveStatus::WAVE_ALL;
} else {
wave_generator_.status_ = WaveStatus::STANCE_ALL;
wave_generator_->status_ = WaveStatus::STANCE_ALL;
}
calcGain();
}
void StateTrotting::exit() {
wave_generator_.status_ = WaveStatus::SWING_ALL;
wave_generator_->status_ = WaveStatus::SWING_ALL;
}
FSMStateName StateTrotting::checkChange() {
@ -118,7 +118,7 @@ void StateTrotting::calcTau() {
Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_;
Vec3 d_wbd = kp_w_ * rotMatToExp(Rd * G2B_RotMat) +
Kd_w_ * (w_cmd_global_ - estimator_.getGyroGlobal());
Kd_w_ * (w_cmd_global_ - estimator_->getGyroGlobal());
dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3));
dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3));
@ -128,16 +128,16 @@ void StateTrotting::calcTau() {
d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40));
d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10));
const Vec34 pos_feet_body_global = estimator_.getFeetPos2Body();
const Vec34 pos_feet_body_global = estimator_->getFeetPos2Body();
Vec34 force_feet_global =
-balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_.contact_);
-balance_ctrl_->calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_->contact_);
Vec34 pos_feet_global = estimator_.getFeetPos();
Vec34 vel_feet_global = estimator_.getFeetVel();
Vec34 pos_feet_global = estimator_->getFeetPos();
Vec34 vel_feet_global = estimator_->getFeetVel();
for (int i(0); i < 4; ++i) {
if (wave_generator_.contact_(i) == 0) {
if (wave_generator_->contact_(i) == 0) {
force_feet_global.col(i) =
Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
@ -146,9 +146,9 @@ void StateTrotting::calcTau() {
Vec34 force_feet_body_ = G2B_RotMat * force_feet_global;
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
std::vector<KDL::JntArray> current_joints = robot_model_->current_joint_pos_;
for (int i = 0; i < 4; i++) {
KDL::JntArray torque = robot_model_.getTorque(force_feet_body_.col(i), i);
KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i);
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
}
@ -156,7 +156,7 @@ void StateTrotting::calcTau() {
}
void StateTrotting::calcQQd() {
const std::vector<KDL::Frame> pos_feet_body = robot_model_.getFeet2BPositions();
const std::vector<KDL::Frame> pos_feet_body = robot_model_->getFeet2BPositions();
Vec34 pos_feet_target, vel_feet_target;
for (int i(0); i < 4; ++i) {
@ -164,8 +164,8 @@ void StateTrotting::calcQQd() {
vel_feet_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
}
Vec12 q_goal = robot_model_.getQ(pos_feet_target);
Vec12 qd_goal = robot_model_.getQd(pos_feet_body, vel_feet_target);
Vec12 q_goal = robot_model_->getQ(pos_feet_target);
Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target);
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
@ -174,7 +174,7 @@ void StateTrotting::calcQQd() {
void StateTrotting::calcGain() const {
for (int i(0); i < 4; ++i) {
if (wave_generator_.contact_(i) == 0) {
if (wave_generator_->contact_(i) == 0) {
// swing gain
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);

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

@ -44,7 +44,7 @@ namespace unitree_guide_controller {
}
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();
// std::chrono::duration<double> time_diff = now - last_update_time_;
// last_update_time_ = now;
@ -53,9 +53,13 @@ namespace unitree_guide_controller {
// update_frequency_ = 1.0 / time_diff.count();
// RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
ctrl_comp_.robot_model_.update();
ctrl_comp_.wave_generator_.update();
ctrl_comp_.estimator_.update();
if (ctrl_comp_.robot_model_ == nullptr) {
return controller_interface::return_type::OK;
}
ctrl_comp_.robot_model_->update();
ctrl_comp_.wave_generator_->update();
ctrl_comp_.estimator_->update();
if (mode_ == FSMMode::NORMAL) {
current_state_->run();
@ -92,6 +96,11 @@ namespace unitree_guide_controller {
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
feet_names_ =
auto_declare<std::vector<std::string> >("feet_names", feet_names_);
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
} catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
@ -101,7 +110,7 @@ namespace unitree_guide_controller {
}
controller_interface::CallbackReturn UnitreeGuideController::on_configure(
const rclcpp_lifecycle::State &previous_state) {
const rclcpp_lifecycle::State & /*previous_state*/) {
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
// Handle message
@ -115,20 +124,18 @@ namespace unitree_guide_controller {
robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
"/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
[this](const std_msgs::msg::String::SharedPtr msg) {
ctrl_comp_.robot_model_.init(msg->data, feet_names_, base_name_);
ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
ctrl_comp_, msg->data, feet_names_, base_name_);
ctrl_comp_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_comp_.robot_model_);
});
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
ctrl_comp_.wave_generator_.init(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
ctrl_comp_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn
UnitreeGuideController::on_activate(const rclcpp_lifecycle::State &previous_state) {
UnitreeGuideController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
// clear out vectors in case of restart
ctrl_comp_.clear();
@ -171,27 +178,27 @@ namespace unitree_guide_controller {
}
controller_interface::CallbackReturn UnitreeGuideController::on_deactivate(
const rclcpp_lifecycle::State &previous_state) {
const rclcpp_lifecycle::State & /*previous_state*/) {
release_interfaces();
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn
UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn
UnitreeGuideController::on_error(const rclcpp_lifecycle::State &previous_state) {
UnitreeGuideController::on_error(const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn
UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
std::shared_ptr<FSMState> UnitreeGuideController::getNextState(FSMStateName stateName) const {
std::shared_ptr<FSMState> UnitreeGuideController::getNextState(const FSMStateName stateName) const {
switch (stateName) {
case FSMStateName::INVALID:
return state_list_.invalid;

8
controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
View File

@ -9,18 +9,16 @@
#include "quadProgpp/QuadProg++.hh"
BalanceCtrl::BalanceCtrl() {
mass_ = 15;
BalanceCtrl::BalanceCtrl(const std::shared_ptr<QuadrupedRobot> &robot) {
mass_ = robot->mass_;
alpha_ = 0.001;
beta_ = 0.1;
g_ << 0, 0, -9.81;
friction_ratio_ = 0.4;
friction_mat_ << 1, 0, friction_ratio_, -1, 0, friction_ratio_, 0, 1, friction_ratio_, 0, -1,
friction_ratio_, 0, 0, 1;
}
void BalanceCtrl::init(const QuadrupedRobot &robot) {
mass_ = robot.mass_;
pcb_ = Vec3(0.0, 0.0, 0.0);
Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();

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

@ -12,7 +12,9 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
robot_model_(ctrl_component.robot_model_),
wave_generator_(ctrl_component.wave_generator_) {
g_ << 0, 0, -9.81;
dt_ = 0.002;
dt_ = 1.0 / ctrl_component_.frequency_;
std::cout<<"dt: "<<dt_<<std::endl;
large_variance_ = 100;
for (int i(0); i < Qdig.rows(); ++i) {
Qdig(i) = i < 6 ? 0.0003 : 0.01;
@ -146,25 +148,25 @@ double Estimator::getYaw() const {
}
void Estimator::update() {
if (robot_model_.mass_ == 0) return;
if (robot_model_->mass_ == 0) return;
Q = QInit_;
R = RInit_;
foot_poses_ = robot_model_.getFeet2BPositions();
foot_vels_ = robot_model_.getFeet2BVelocities();
foot_poses_ = robot_model_->getFeet2BPositions();
foot_vels_ = robot_model_->getFeet2BVelocities();
feet_h_.setZero();
// Adjust the covariance based on foot contact and phase.
for (int i(0); i < 4; ++i) {
if (wave_generator_.contact_[i] == 0) {
if (wave_generator_->contact_[i] == 0) {
// foot not contact
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
R(24 + i, 24 + i) = large_variance_;
} else {
// foot contact
const double trust = windowFunc(wave_generator_.phase_[i], 0.2);
const double trust = windowFunc(wave_generator_->phase_[i], 0.2);
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
(1 + (1 - trust) * large_variance_) *
QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);

14
controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp
View File

@ -15,10 +15,10 @@ FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_c
}
void FeetEndCalc::init() {
t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
t_stance_ = ctrl_component_.wave_generator_->get_t_stance();
t_swing_ = ctrl_component_.wave_generator_->get_t_swing();
Vec34 feet_pos_body = estimator_.getFeetPos2Body();
Vec34 feet_pos_body = estimator_->getFeetPos2Body();
// Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_;
for (int i(0); i < 4; ++i) {
feet_radius_(i) =
@ -28,7 +28,7 @@ void FeetEndCalc::init() {
}
Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) {
Vec3 body_vel_global = estimator_.getVelocity();
Vec3 body_vel_global = estimator_->getVelocity();
Vec3 next_step;
next_step(0) = body_vel_global(0) * (1 - phase) * t_swing_ +
@ -39,8 +39,8 @@ Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl
k_y_ * (body_vel_global(1) - vxy_goal_global(1));
next_step(2) = 0;
const double yaw = estimator_.getYaw();
const double d_yaw = estimator_.getDYaw();
const double yaw = estimator_->getYaw();
const double d_yaw = estimator_->getDYaw();
const double next_yaw = d_yaw * (1 - phase) * t_swing_ + d_yaw * t_stance_ / 2 +
k_yaw_ * (d_yaw_global - d_yaw);
@ -49,7 +49,7 @@ Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl
next_step(1) +=
feet_radius_(index) * sin(yaw + feet_init_angle_(index) + next_yaw);
Vec3 foot_pos = estimator_.getPosition() + next_step;
Vec3 foot_pos = estimator_->getPosition() + next_step;
foot_pos(2) = 0.0;
return foot_pos;

26
controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp
View File

@ -22,21 +22,21 @@ void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const
void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
if (first_run_) {
start_p_ = estimator_.getFeetPos();
start_p_ = estimator_->getFeetPos();
first_run_ = false;
}
for (int i = 0; i < 4; i++) {
if (wave_generator_.contact_(i) == 1) {
if (wave_generator_.phase_(i) < 0.5) {
if (wave_generator_->contact_(i) == 1) {
if (wave_generator_->phase_(i) < 0.5) {
// foot contact the ground
start_p_.col(i) = estimator_.getFootPos(i);
start_p_.col(i) = estimator_->getFootPos(i);
}
feet_pos.col(i) = start_p_.col(i);
feet_vel.col(i).setZero();
} else {
// foot not contact, swing
end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i));
end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_->phase_(i));
feet_pos.col(i) = getFootPos(i);
feet_vel.col(i) = getFootVel(i);
}
@ -54,10 +54,10 @@ Vec3 GaitGenerator::getFootPos(const int i) {
Vec3 foot_pos;
foot_pos(0) =
cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_->phase_(i));
foot_pos(1) =
cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_.phase_(i));
cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_->phase_(i));
foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_->phase_(i));
return foot_pos;
}
@ -66,10 +66,10 @@ Vec3 GaitGenerator::getFootVel(const int i) {
Vec3 foot_vel;
foot_vel(0) =
cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_->phase_(i));
foot_vel(1) =
cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_.phase_(i));
cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_->phase_(i));
foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_->phase_(i));
return foot_vel;
}
@ -86,10 +86,10 @@ double GaitGenerator::cycloidZPosition(const double startZ, const double height,
double GaitGenerator::cycloidXYVelocity(const double startXY, const double endXY, const double phase) const {
const double phase_pi = 2 * M_PI * phase;
return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_.get_t_swing();
return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_->get_t_swing();
}
double GaitGenerator::cycloidZVelocity(const double height, const double phase) const {
const double phase_pi = 2 * M_PI * phase;
return height * M_PI * sin(phase_pi) / wave_generator_.get_t_swing();
return height * M_PI * sin(phase_pi) / wave_generator_->get_t_swing();
}

5
controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
View File

@ -8,14 +8,13 @@
#include <iostream>
WaveGenerator::WaveGenerator() {
WaveGenerator::WaveGenerator(double period, double st_ratio, const Vec4 &bias) {
phase_past_ << 0.5, 0.5, 0.5, 0.5;
contact_past_.setZero();
status_past_ = WaveStatus::SWING_ALL;
status_ = WaveStatus::SWING_ALL;
}
void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
period_ = period;
st_ratio_ = st_ratio;
bias_ = bias;

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

@ -6,8 +6,9 @@
#include "unitree_guide_controller/control/CtrlComponent.h"
#include "unitree_guide_controller/robot/QuadrupedRobot.h"
void QuadrupedRobot::init(const std::string &robot_description, const std::vector<std::string> &feet_names,
const std::string& base_name) {
QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
const std::vector<std::string> &feet_names,
const std::string &base_name) : ctrl_component_(ctrl_component) {
KDL::Tree robot_tree;
kdl_parser::treeFromString(robot_description, robot_tree);