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

better fsm switch logic for unitree controller

This commit is contained in:
Huang Zhenbiao 2024-09-29 20:27:02 +08:00
parent 66c20b4050
commit e0020b4bfe
11 changed files with 60 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
commands/keyboard_input/include/keyboard_input/KeyboardInput.h
View File

@ -41,6 +41,9 @@ private:
rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_; rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
bool just_published_ = false;
int reset_count_ = 0;
float sensitivity_left_ = 0.05; float sensitivity_left_ = 0.05;
float sensitivity_right_ = 0.05; float sensitivity_right_ = 0.05;
termios old_tio_{}, new_tio_{}; termios old_tio_{}, new_tio_{};

13
commands/keyboard_input/src/KeyboardInput.cpp
View File

@ -26,8 +26,21 @@ void KeyboardInput::timer_callback() {
inputs_.ly = 0; inputs_.ly = 0;
inputs_.rx = 0; inputs_.rx = 0;
inputs_.ry = 0; inputs_.ry = 0;
reset_count_ = 100;
} }
publisher_->publish(inputs_); publisher_->publish(inputs_);
just_published_ = true;
} else {
if (just_published_) {
reset_count_ -= 1;
if (reset_count_ == 0) {
just_published_ = false;
if (inputs_.command != 0) {
inputs_.command = 0;
publisher_->publish(inputs_);
}
}
}
} }
} }

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

@ -38,8 +38,7 @@ struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_; imu_state_interface_;
control_input_msgs::msg::Inputs default_inputs_; control_input_msgs::msg::Inputs control_inputs_;
std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
int frequency_{}; int frequency_{};
QuadrupedRobot robot_model_; QuadrupedRobot robot_model_;
@ -48,8 +47,7 @@ struct CtrlComponent {
WaveGenerator wave_generator_; WaveGenerator wave_generator_;
CtrlComponent() : control_inputs_(default_inputs_), CtrlComponent() : robot_model_(*this),
robot_model_(*this),
estimator_(*this) { estimator_(*this) {
} }

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

@ -36,11 +36,11 @@ void StateBalanceTest::enter() {
} }
void StateBalanceTest::run() { void StateBalanceTest::run() {
pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax); pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.ly, _xMin, _xMax);
pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax); pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.lx, _yMin, _yMax);
pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax); pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.ry, _zMin, _zMax);
const float yaw = - invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax); const float yaw = - invNormalize(ctrl_comp_.control_inputs_.rx, _yawMin, _yawMax);
Rd_ = rotz(yaw) * init_rotation_; Rd_ = rotz(yaw) * init_rotation_;
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
@ -56,7 +56,7 @@ void StateBalanceTest::exit() {
} }
FSMStateName StateBalanceTest::checkChange() { FSMStateName StateBalanceTest::checkChange() {
switch (ctrl_comp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
case 2: case 2:

4
controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
View File

@ -15,7 +15,7 @@ void StateFixedDown::enter() {
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
} }
ctrl_comp_.control_inputs_.get().command = 0; ctrl_comp_.control_inputs_.command = 0;
} }
void StateFixedDown::run() { void StateFixedDown::run() {
@ -39,7 +39,7 @@ FSMStateName StateFixedDown::checkChange() {
if (percent_ < 1.5) { if (percent_ < 1.5) {
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
} }
switch (ctrl_comp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::PASSIVE; return FSMStateName::PASSIVE;
case 2: case 2:

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

@ -15,7 +15,7 @@ void StateFixedStand::enter() {
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value(); start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
} }
ctrl_comp_.control_inputs_.get().command = 0; ctrl_comp_.control_inputs_.command = 0;
} }
void StateFixedStand::run() { void StateFixedStand::run() {
@ -40,8 +40,10 @@ FSMStateName StateFixedStand::checkChange() {
if (percent_ < 1.5) { if (percent_ < 1.5) {
return FSMStateName::FIXEDSTAND; return FSMStateName::FIXEDSTAND;
} }
switch (ctrl_comp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::PASSIVE;
case 2:
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
case 3: case 3:
return FSMStateName::FREESTAND; return FSMStateName::FREESTAND;

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

@ -33,23 +33,23 @@ void StateFreeStand::enter() {
foot_pos.p -= fr_init_pos_.p; foot_pos.p -= fr_init_pos_.p;
foot_pos.M = KDL::Rotation::RPY(0, 0, 0); foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
} }
ctrl_comp_.control_inputs_.get().command = 0; ctrl_comp_.control_inputs_.command = 0;
} }
void StateFreeStand::run() { void StateFreeStand::run() {
calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_), calc_body_target(invNormalize(ctrl_comp_.control_inputs_.lx, row_min_, row_max_),
invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_), invNormalize(ctrl_comp_.control_inputs_.ly, pitch_min_, pitch_max_),
invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_), invNormalize(ctrl_comp_.control_inputs_.rx, yaw_min_, yaw_max_),
invNormalize(ctrl_comp_.control_inputs_.get().ry, height_min_, height_max_)); invNormalize(ctrl_comp_.control_inputs_.ry, height_min_, height_max_));
} }
void StateFreeStand::exit() { void StateFreeStand::exit() {
} }
FSMStateName StateFreeStand::checkChange() { FSMStateName StateFreeStand::checkChange() {
switch (ctrl_comp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::PASSIVE;
case 2: case 2:
return FSMStateName::FIXEDSTAND; return FSMStateName::FIXEDSTAND;
default: default:

4
controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
View File

@ -27,7 +27,7 @@ void StatePassive::enter() {
for (auto i: ctrl_comp_.joint_kd_command_interface_) { for (auto i: ctrl_comp_.joint_kd_command_interface_) {
i.get().set_value(1); i.get().set_value(1);
} }
ctrl_comp_.control_inputs_.get().command = 0; ctrl_comp_.control_inputs_.command = 0;
} }
void StatePassive::run() { void StatePassive::run() {
@ -37,7 +37,7 @@ void StatePassive::exit() {
} }
FSMStateName StatePassive::checkChange() { FSMStateName StatePassive::checkChange() {
if (ctrl_comp_.control_inputs_.get().command == 2) { if (ctrl_comp_.control_inputs_.command == 2) {
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
} }
return FSMStateName::PASSIVE; return FSMStateName::PASSIVE;

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

@ -38,25 +38,25 @@ void StateSwingTest::enter() {
} }
void StateSwingTest::run() { void StateSwingTest::run() {
if (ctrl_comp_.control_inputs_.get().ly > 0) { if (ctrl_comp_.control_inputs_.ly > 0) {
fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x(), fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x(),
fr_init_pos_.p.x() + _xMax, 0, 1)); fr_init_pos_.p.x() + _xMax, 0, 1));
} else { } else {
fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x() + _xMin, fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
fr_init_pos_.p.x(), -1, 0)); fr_init_pos_.p.x(), -1, 0));
} }
if (ctrl_comp_.control_inputs_.get().lx > 0) { if (ctrl_comp_.control_inputs_.lx > 0) {
fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y(), fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y(),
fr_init_pos_.p.y() + _yMax, 0, 1)); fr_init_pos_.p.y() + _yMax, 0, 1));
} else { } else {
fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y() + _yMin, fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
fr_init_pos_.p.y(), -1, 0)); fr_init_pos_.p.y(), -1, 0));
} }
if (ctrl_comp_.control_inputs_.get().ry > 0) { if (ctrl_comp_.control_inputs_.ry > 0) {
fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z(), fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z(),
fr_init_pos_.p.z() + _zMax, 0, 1)); fr_init_pos_.p.z() + _zMax, 0, 1));
} else { } else {
fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin, fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
fr_init_pos_.p.z(), -1, 0)); fr_init_pos_.p.z(), -1, 0));
} }
@ -68,9 +68,9 @@ void StateSwingTest::exit() {
} }
FSMStateName StateSwingTest::checkChange() { FSMStateName StateSwingTest::checkChange() {
switch (ctrl_comp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::PASSIVE;
case 2: case 2:
return FSMStateName::FIXEDSTAND; return FSMStateName::FIXEDSTAND;
default: default:

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

@ -34,7 +34,7 @@ void StateTrotting::enter() {
Rd = rotz(yaw_cmd_); Rd = rotz(yaw_cmd_);
w_cmd_global_.setZero(); w_cmd_global_.setZero();
ctrl_comp_.control_inputs_.get().command = 0; ctrl_comp_.control_inputs_.command = 0;
gait_generator_.restart(); gait_generator_.restart();
} }
@ -68,9 +68,9 @@ void StateTrotting::exit() {
} }
FSMStateName StateTrotting::checkChange() { FSMStateName StateTrotting::checkChange() {
switch (ctrl_comp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::PASSIVE;
case 2: case 2:
return FSMStateName::FIXEDSTAND; return FSMStateName::FIXEDSTAND;
default: default:
@ -80,12 +80,12 @@ FSMStateName StateTrotting::checkChange() {
void StateTrotting::getUserCmd() { void StateTrotting::getUserCmd() {
/* Movement */ /* Movement */
v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, v_x_limit_(0), v_x_limit_(1)); v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.ly, v_x_limit_(0), v_x_limit_(1));
v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, v_y_limit_(0), v_y_limit_(1)); v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.lx, v_y_limit_(0), v_y_limit_(1));
v_cmd_body_(2) = 0; v_cmd_body_(2) = 0;
/* Turning */ /* Turning */
d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.get().rx, w_yaw_limit_(0), w_yaw_limit_(1)); d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.rx, w_yaw_limit_(0), w_yaw_limit_(1));
d_yaw_cmd_ = 0.9 * d_yaw_cmd_past_ + (1 - 0.9) * d_yaw_cmd_; d_yaw_cmd_ = 0.9 * d_yaw_cmd_past_ + (1 - 0.9) * d_yaw_cmd_;
d_yaw_cmd_past_ = d_yaw_cmd_; d_yaw_cmd_past_ = d_yaw_cmd_;
} }

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

@ -105,11 +105,11 @@ namespace unitree_guide_controller {
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>( control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
// Handle message // Handle message
ctrl_comp_.control_inputs_.get().command = msg->command; ctrl_comp_.control_inputs_.command = msg->command;
ctrl_comp_.control_inputs_.get().lx = msg->lx; ctrl_comp_.control_inputs_.lx = msg->lx;
ctrl_comp_.control_inputs_.get().ly = msg->ly; ctrl_comp_.control_inputs_.ly = msg->ly;
ctrl_comp_.control_inputs_.get().rx = msg->rx; ctrl_comp_.control_inputs_.rx = msg->rx;
ctrl_comp_.control_inputs_.get().ry = msg->ry; ctrl_comp_.control_inputs_.ry = msg->ry;
}); });
robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>( robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(