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

take FSM out as separate library

This commit is contained in:
Huang Zhenbiao 2025-02-27 19:02:21 +08:00
parent a8d1029a0c
commit 2b395cd2f3
43 changed files with 948 additions and 758 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
controllers/ocs2_quadruped_controller/launch/gazebo.launch.py
View File

@ -128,9 +128,6 @@ def generate_launch_description():
executable="parameter_bridge",
arguments=[
"/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
# "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
# "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
# "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
],
output="screen"
)

3
controllers/unitree_guide_controller/CMakeLists.txt
View File

@ -15,6 +15,7 @@ set(CONTROLLER_INCLUDE_DEPENDS
realtime_tools
std_msgs
control_input_msgs
controller_common
kdl_parser
)
@ -28,8 +29,6 @@ add_library(${PROJECT_NAME} SHARED
src/UnitreeGuideController.cpp
src/FSM/StatePassive.cpp
src/FSM/StateFixedDown.cpp
src/FSM/StateFixedStand.cpp
src/FSM/StateSwingTest.cpp
src/FSM/StateFreeStand.cpp

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

@ -4,12 +4,22 @@
#ifndef STATEBALANCETEST_H
#define STATEBALANCETEST_H
#include "FSMState.h"
#include <unitree_guide_controller/common/mathTypes.h>
#include "controller_common/FSM/FSMState.h"
class WaveGenerator;
class BalanceCtrl;
class QuadrupedRobot;
class Estimator;
struct CtrlComponent;
class StateBalanceTest final : public FSMState {
public:
explicit StateBalanceTest(CtrlComponent &ctrlComp);
explicit StateBalanceTest(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component);
void enter() override;

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

@ -5,35 +5,16 @@
#ifndef STATEFIXEDSTAND_H
#define STATEFIXEDSTAND_H
#include <rclcpp/time.hpp>
#include <controller_common/FSM/BaseFixedStand.h>
#include "FSMState.h"
class StateFixedStand final : public FSMState {
class StateFixedStand final : public BaseFixedStand {
public:
explicit StateFixedStand(CtrlComponent &ctrlComp,
explicit StateFixedStand(CtrlInterfaces &ctrl_interfaces,
const std::vector<double> &target_pos,
double kp,
double kd);
void enter() override;
void run() override;
void exit() override;
FSMStateName checkChange() override;
private:
double target_pos_[12] = {};
double start_pos_[12] = {};
rclcpp::Time start_time_;
double kp_, kd_;
double duration_ = 600; // steps
double percent_ = 0; //%
double phase = 0.0;
};

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

@ -4,12 +4,16 @@
#ifndef STATEFREESTAND_H
#define STATEFREESTAND_H
#include "FSMState.h"
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
#include "controller_common/FSM/FSMState.h"
struct CtrlComponent;
class StateFreeStand final : public FSMState {
public:
explicit StateFreeStand(CtrlComponent &ctrl_component);
explicit StateFreeStand(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component);
void enter() override;
@ -21,6 +25,7 @@ public:
private:
std::shared_ptr<QuadrupedRobot> &robot_model_;
void calc_body_target(float row, float pitch, float yaw, float height);
float row_max_, row_min_;
@ -35,5 +40,4 @@ private:
std::vector<KDL::Frame> init_foot_pos_; // 4 feet position in fr-foot frame
};
#endif //STATEFREESTAND_H

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

@ -5,12 +5,17 @@
#ifndef STATESWINGTEST_H
#define STATESWINGTEST_H
#include "FSMState.h"
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
#include "controller_common/FSM/FSMState.h"
struct CtrlComponent;
class StateSwingTest final : public FSMState {
public:
explicit StateSwingTest(CtrlComponent &ctrl_component);
explicit StateSwingTest(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component);
void enter() override;

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

@ -4,14 +4,14 @@
#ifndef STATETROTTING_H
#define STATETROTTING_H
#include <unitree_guide_controller/control/BalanceCtrl.h>
#include <unitree_guide_controller/gait/GaitGenerator.h>
#include "FSMState.h"
#include "controller_common/FSM/FSMState.h"
class StateTrotting final : public FSMState {
public:
explicit StateTrotting(CtrlComponent &ctrlComp);
explicit StateTrotting(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component);
void enter() override;

28
controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
View File

@ -7,14 +7,15 @@
#include <controller_interface/controller_interface.hpp>
#include <std_msgs/msg/string.hpp>
#include <unitree_guide_controller/FSM/FSMState.h>
#include <unitree_guide_controller/common/enumClass.h>
#include <controller_common/FSM/FSMState.h>
#include <controller_common/FSM/StatePassive.h>
#include <controller_common/FSM/StateFixedDown.h>
#include <controller_common/common/enumClass.h>
#include "control/CtrlComponent.h"
#include "FSM/StateBalanceTest.h"
#include "FSM/StateFixedDown.h"
#include "FSM/StateFixedStand.h"
#include "FSM/StateFreeStand.h"
#include "FSM/StatePassive.h"
#include "FSM/StateSwingTest.h"
#include "FSM/StateTrotting.h"
@ -62,7 +63,8 @@ namespace unitree_guide_controller {
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State &previous_state) override;
CtrlComponent ctrl_comp_;
CtrlComponent ctrl_component_;
CtrlInterfaces ctrl_interfaces_;
protected:
std::vector<std::string> joint_names_;
@ -99,11 +101,11 @@ namespace unitree_guide_controller {
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
command_interface_map_ = {
{"effort", &ctrl_comp_.joint_torque_command_interface_},
{"position", &ctrl_comp_.joint_position_command_interface_},
{"velocity", &ctrl_comp_.joint_velocity_command_interface_},
{"kp", &ctrl_comp_.joint_kp_command_interface_},
{"kd", &ctrl_comp_.joint_kd_command_interface_}
{"effort", &ctrl_interfaces_.joint_torque_command_interface_},
{"position", &ctrl_interfaces_.joint_position_command_interface_},
{"velocity", &ctrl_interfaces_.joint_velocity_command_interface_},
{"kp", &ctrl_interfaces_.joint_kp_command_interface_},
{"kd", &ctrl_interfaces_.joint_kd_command_interface_}
};
FSMMode mode_ = FSMMode::NORMAL;
@ -121,9 +123,9 @@ namespace unitree_guide_controller {
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
state_interface_map_ = {
{"position", &ctrl_comp_.joint_position_state_interface_},
{"effort", &ctrl_comp_.joint_effort_state_interface_},
{"velocity", &ctrl_comp_.joint_velocity_state_interface_}
{"position", &ctrl_interfaces_.joint_position_state_interface_},
{"effort", &ctrl_interfaces_.joint_effort_state_interface_},
{"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
};
};
}

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

@ -1,67 +1,20 @@
//
// Created by biao on 24-9-10.
// Created by tlab-uav on 25-2-27.
//
#ifndef INTERFACE_H
#define INTERFACE_H
#include <vector>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#ifndef CTRLCOMPONENT_H
#define CTRLCOMPONENT_H
#include <unitree_guide_controller/gait/WaveGenerator.h>
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
#include "BalanceCtrl.h"
#include "Estimator.h"
struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_torque_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kp_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kd_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_effort_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_;
control_input_msgs::msg::Inputs control_inputs_;
int frequency_{};
std::shared_ptr<QuadrupedRobot> robot_model_;
std::shared_ptr<Estimator> estimator_;
std::shared_ptr<BalanceCtrl> balance_ctrl_;
std::shared_ptr<WaveGenerator> wave_generator_;
CtrlComponent() = default;
void clear() {
joint_torque_command_interface_.clear();
joint_position_command_interface_.clear();
joint_velocity_command_interface_.clear();
joint_kd_command_interface_.clear();
joint_kp_command_interface_.clear();
joint_effort_state_interface_.clear();
joint_position_state_interface_.clear();
joint_velocity_state_interface_.clear();
imu_state_interface_.clear();
}
};
#endif //INTERFACE_H
#endif //CTRLCOMPONENT_H

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

@ -8,16 +8,16 @@
#include <kdl/frames.hpp>
#include <unitree_guide_controller/common/mathTypes.h>
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
#include "LowPassFilter.h"
struct CtrlInterfaces;
class WaveGenerator;
class QuadrupedRobot;
struct CtrlComponent;
class Estimator {
public:
explicit Estimator(CtrlComponent &ctrl_component);
explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component);
~Estimator() = default;
@ -105,7 +105,7 @@ public:
void update();
private:
CtrlComponent &ctrl_component_;
CtrlInterfaces &ctrl_interfaces_;
std::shared_ptr<QuadrupedRobot> &robot_model_;
std::shared_ptr<WaveGenerator> &wave_generator_;

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

@ -6,7 +6,7 @@
#ifndef WAVEGENERATOR_H
#define WAVEGENERATOR_H
#include <chrono>
#include <unitree_guide_controller/common/enumClass.h>
#include <controller_common/common/enumClass.h>
#include <unitree_guide_controller/common/mathTypes.h>
inline long long getSystemTime() {

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

@ -12,11 +12,11 @@
#include "RobotLeg.h"
struct CtrlComponent;
struct CtrlInterfaces;
class QuadrupedRobot {
public:
explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
explicit QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description,
const std::vector<std::string> &feet_names, const std::string &base_name);
~QuadrupedRobot() = default;
@ -91,7 +91,7 @@ public:
void update();
private:
CtrlComponent &ctrl_component_;
CtrlInterfaces &ctrl_interfaces_;
std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
KDL::Chain fr_chain_;

1
controllers/unitree_guide_controller/package.xml
View File

@ -11,6 +11,7 @@
<depend>backward_ros</depend>
<depend>controller_interface</depend>
<depend>controller_common</depend>
<depend>pluginlib</depend>
<depend>control_input_msgs</depend>
<depend>kdl_parser</depend>

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

@ -4,14 +4,21 @@
#include "unitree_guide_controller/FSM/StateBalanceTest.h"
#include "unitree_guide_controller/common/mathTools.h"
#include <unitree_guide_controller/UnitreeGuideController.h>
StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
ctrlComp),
estimator_(ctrlComp.estimator_),
robot_model_(ctrlComp.robot_model_),
balance_ctrl_(ctrlComp.balance_ctrl_),
wave_generator_(ctrl_comp_.wave_generator_) {
#include "unitree_guide_controller/common/mathTools.h"
#include "unitree_guide_controller/gait/WaveGenerator.h"
StateBalanceTest::StateBalanceTest(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component)
: FSMState(FSMStateName::BALANCETEST,
"balance test",
ctrl_interfaces),
estimator_(ctrl_component.estimator_),
robot_model_(ctrl_component.robot_model_),
balance_ctrl_(ctrl_component.balance_ctrl_),
wave_generator_(
ctrl_component.wave_generator_) {
_xMax = 0.05;
_xMin = -_xMax;
_yMax = 0.05;
@ -36,16 +43,16 @@ void StateBalanceTest::enter() {
}
void StateBalanceTest::run() {
pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.ly, _xMin, _xMax);
pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.lx, _yMin, _yMax);
pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.ry, _zMin, _zMax);
pcd_(0) = pcd_init_(0) + invNormalize(ctrl_interfaces_.control_inputs_.ly, _xMin, _xMax);
pcd_(1) = pcd_init_(1) - invNormalize(ctrl_interfaces_.control_inputs_.lx, _yMin, _yMax);
pcd_(2) = pcd_init_(2) + invNormalize(ctrl_interfaces_.control_inputs_.ry, _zMin, _zMax);
const float yaw = - invNormalize(ctrl_comp_.control_inputs_.rx, _yawMin, _yawMax);
const float yaw = -invNormalize(ctrl_interfaces_.control_inputs_.rx, _yawMin, _yawMax);
Rd_ = rotz(yaw) * init_rotation_;
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.8);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.8);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(0.8);
}
calcTorque();
@ -56,7 +63,7 @@ void StateBalanceTest::exit() {
}
FSMStateName StateBalanceTest::checkChange() {
switch (ctrl_comp_.control_inputs_.command) {
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 2:
@ -83,15 +90,16 @@ void StateBalanceTest::calcTorque() {
// 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_);
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_;
for (int i = 0; i < 4; 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));
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + j].get().set_value(
current_joints[i](j));
}
}
}

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

@ -1,60 +0,0 @@
//
// Created by tlab-uav on 24-9-11.
//
#include "unitree_guide_controller/FSM/StateFixedDown.h"
#include <cmath>
StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp,
const std::vector<double> &target_pos,
const double kp,
const double kd)
: FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrlComp),
kp_(kp), kd_(kd) {
duration_ = ctrl_comp_.frequency_ * 1.2;
for (int i = 0; i < 12; i++) {
target_pos_[i] = target_pos[i];
}
}
void StateFixedDown::enter() {
for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
}
ctrl_comp_.control_inputs_.command = 0;
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
}
}
void StateFixedDown::run() {
percent_ += 1 / duration_;
phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
}
}
void StateFixedDown::exit() {
percent_ = 0;
}
FSMStateName StateFixedDown::checkChange() {
if (percent_ < 1.5) {
return FSMStateName::FIXEDDOWN;
}
switch (ctrl_comp_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:
return FSMStateName::FIXEDSTAND;
default:
return FSMStateName::FIXEDDOWN;
}
}

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

@ -4,51 +4,17 @@
#include "unitree_guide_controller/FSM/StateFixedStand.h"
#include <cmath>
StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &target_pos,
StateFixedStand::StateFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector<double> &target_pos,
const double kp,
const double kd)
: FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp),
kp_(kp), kd_(kd) {
duration_ = ctrl_comp_.frequency_ * 1.2;
for (int i = 0; i < 12; i++) {
target_pos_[i] = target_pos[i];
}
}
void StateFixedStand::enter() {
for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
}
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
}
ctrl_comp_.control_inputs_.command = 0;
}
void StateFixedStand::run() {
percent_ += 1 / duration_;
phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
}
}
void StateFixedStand::exit() {
percent_ = 0;
: BaseFixedStand(ctrl_interfaces, target_pos, kp, kd) {
}
FSMStateName StateFixedStand::checkChange() {
if (percent_ < 1.5) {
return FSMStateName::FIXEDSTAND;
}
switch (ctrl_comp_.control_inputs_.command) {
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:

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

@ -3,11 +3,17 @@
//
#include "unitree_guide_controller/FSM/StateFreeStand.h"
#include <unitree_guide_controller/UnitreeGuideController.h>
#include "unitree_guide_controller/common/mathTools.h"
StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand",
ctrl_component),
robot_model_(ctrl_component.robot_model_) {
StateFreeStand::StateFreeStand(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component)
: FSMState(
FSMStateName::FREESTAND, "free stand",
ctrl_interfaces),
robot_model_(ctrl_component.robot_model_) {
row_max_ = 20 * M_PI / 180;
row_min_ = -row_max_;
pitch_max_ = 15 * M_PI / 180;
@ -20,8 +26,8 @@ StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStat
void StateFreeStand::enter() {
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(100);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(100);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5);
}
init_joint_pos_ = robot_model_->current_joint_pos_;
@ -33,21 +39,21 @@ void StateFreeStand::enter() {
foot_pos.p -= fr_init_pos_.p;
foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
}
ctrl_comp_.control_inputs_.command = 0;
ctrl_interfaces_.control_inputs_.command = 0;
}
void StateFreeStand::run() {
calc_body_target(invNormalize(ctrl_comp_.control_inputs_.lx, row_min_, row_max_),
invNormalize(ctrl_comp_.control_inputs_.ly, pitch_min_, pitch_max_),
invNormalize(ctrl_comp_.control_inputs_.rx, yaw_min_, yaw_max_),
invNormalize(ctrl_comp_.control_inputs_.ry, height_min_, height_max_));
calc_body_target(invNormalize(ctrl_interfaces_.control_inputs_.lx, row_min_, row_max_),
invNormalize(ctrl_interfaces_.control_inputs_.ly, pitch_min_, pitch_max_),
invNormalize(ctrl_interfaces_.control_inputs_.rx, yaw_min_, yaw_max_),
invNormalize(ctrl_interfaces_.control_inputs_.ry, height_min_, height_max_));
}
void StateFreeStand::exit() {
}
FSMStateName StateFreeStand::checkChange() {
switch (ctrl_comp_.control_inputs_.command) {
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:
@ -72,8 +78,11 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
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));
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 + 2].get().set_value(target_joint_pos_[i](2));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value(
target_joint_pos_[i](0));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value(
target_joint_pos_[i](1));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value(
target_joint_pos_[i](2));
}
}

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

@ -1,44 +0,0 @@
//
// Created by tlab-uav on 24-9-6.
//
#include "unitree_guide_controller/FSM/StatePassive.h"
#include <iostream>
#include <utility>
StatePassive::StatePassive(CtrlComponent &ctrlComp) : FSMState(
FSMStateName::PASSIVE, "passive", ctrlComp) {
}
void StatePassive::enter() {
for (auto i: ctrl_comp_.joint_torque_command_interface_) {
i.get().set_value(0);
}
for (auto i: ctrl_comp_.joint_position_command_interface_) {
i.get().set_value(0);
}
for (auto i: ctrl_comp_.joint_velocity_command_interface_) {
i.get().set_value(0);
}
for (auto i: ctrl_comp_.joint_kp_command_interface_) {
i.get().set_value(0);
}
for (auto i: ctrl_comp_.joint_kd_command_interface_) {
i.get().set_value(1);
}
ctrl_comp_.control_inputs_.command = 0;
}
void StatePassive::run() {
}
void StatePassive::exit() {
}
FSMStateName StatePassive::checkChange() {
if (ctrl_comp_.control_inputs_.command == 2) {
return FSMStateName::FIXEDDOWN;
}
return FSMStateName::PASSIVE;
}

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

@ -4,12 +4,17 @@
#include <iostream>
#include "unitree_guide_controller/FSM/StateSwingTest.h"
#include <unitree_guide_controller/control/CtrlComponent.h>
#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_) {
StateSwingTest::StateSwingTest(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component)
: FSMState(
FSMStateName::SWINGTEST, "swing test",
ctrl_interfaces),
robot_model_(ctrl_component.robot_model_) {
_xMin = -0.15;
_xMax = 0.10;
_yMin = -0.15;
@ -20,12 +25,12 @@ StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
void StateSwingTest::enter() {
for (int i = 0; i < 3; i++) {
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(3);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(2);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(3);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(2);
}
for (int i = 3; i < 12; i++) {
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(180);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5);
}
Kp = KDL::Vector(20, 20, 50);
@ -40,25 +45,25 @@ void StateSwingTest::enter() {
}
void StateSwingTest::run() {
if (ctrl_comp_.control_inputs_.ly > 0) {
fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x(),
if (ctrl_interfaces_.control_inputs_.ly > 0) {
fr_goal_pos_.p.x(invNormalize(ctrl_interfaces_.control_inputs_.ly, fr_init_pos_.p.x(),
fr_init_pos_.p.x() + _xMax, 0, 1));
} else {
fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
fr_goal_pos_.p.x(invNormalize(ctrl_interfaces_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
fr_init_pos_.p.x(), -1, 0));
}
if (ctrl_comp_.control_inputs_.lx > 0) {
fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y(),
if (ctrl_interfaces_.control_inputs_.lx > 0) {
fr_goal_pos_.p.y(invNormalize(ctrl_interfaces_.control_inputs_.lx, fr_init_pos_.p.y(),
fr_init_pos_.p.y() + _yMax, 0, 1));
} else {
fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
fr_goal_pos_.p.y(invNormalize(ctrl_interfaces_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
fr_init_pos_.p.y(), -1, 0));
}
if (ctrl_comp_.control_inputs_.ry > 0) {
fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z(),
if (ctrl_interfaces_.control_inputs_.ry > 0) {
fr_goal_pos_.p.z(invNormalize(ctrl_interfaces_.control_inputs_.ry, fr_init_pos_.p.z(),
fr_init_pos_.p.z() + _zMax, 0, 1));
} else {
fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
fr_goal_pos_.p.z(invNormalize(ctrl_interfaces_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
fr_init_pos_.p.z(), -1, 0));
}
@ -70,7 +75,7 @@ void StateSwingTest::exit() {
}
FSMStateName StateSwingTest::checkChange() {
switch (ctrl_comp_.control_inputs_.command) {
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:
@ -84,9 +89,9 @@ void StateSwingTest::positionCtrl() {
target_foot_pos_[0] = fr_goal_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));
ctrl_comp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
}
}
@ -101,6 +106,6 @@ void StateSwingTest::torqueCtrl() const {
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));
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque0(i));
}
}

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

@ -5,13 +5,18 @@
#include "unitree_guide_controller/FSM/StateTrotting.h"
#include <unitree_guide_controller/common/mathTools.h>
#include <unitree_guide_controller/control/CtrlComponent.h>
#include <unitree_guide_controller/control/Estimator.h>
#include <unitree_guide_controller/gait/WaveGenerator.h>
StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::TROTTING, "trotting", ctrlComp),
estimator_(ctrlComp.estimator_),
robot_model_(ctrlComp.robot_model_),
balance_ctrl_(ctrlComp.balance_ctrl_),
wave_generator_(ctrl_comp_.wave_generator_),
gait_generator_(ctrlComp) {
StateTrotting::StateTrotting(CtrlInterfaces &ctrl_interfaces,
CtrlComponent &ctrl_component) : FSMState(FSMStateName::TROTTING, "trotting",
ctrl_interfaces),
estimator_(ctrl_component.estimator_),
robot_model_(ctrl_component.robot_model_),
balance_ctrl_(ctrl_component.balance_ctrl_),
wave_generator_(ctrl_component.wave_generator_),
gait_generator_(ctrl_component) {
gait_height_ = 0.08;
Kpp = Vec3(70, 70, 70).asDiagonal();
Kdp = Vec3(10, 10, 10).asDiagonal();
@ -23,7 +28,7 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
v_x_limit_ << -0.4, 0.4;
v_y_limit_ << -0.3, 0.3;
w_yaw_limit_ << -0.5, 0.5;
dt_ = 1.0 / ctrl_comp_.frequency_;
dt_ = 1.0 / ctrl_interfaces_.frequency_;
}
void StateTrotting::enter() {
@ -34,7 +39,7 @@ void StateTrotting::enter() {
Rd = rotz(yaw_cmd_);
w_cmd_global_.setZero();
ctrl_comp_.control_inputs_.command = 0;
ctrl_interfaces_.control_inputs_.command = 0;
gait_generator_.restart();
}
@ -68,7 +73,7 @@ void StateTrotting::exit() {
}
FSMStateName StateTrotting::checkChange() {
switch (ctrl_comp_.control_inputs_.command) {
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:
@ -80,12 +85,12 @@ FSMStateName StateTrotting::checkChange() {
void StateTrotting::getUserCmd() {
/* Movement */
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_.lx, v_y_limit_(0), v_y_limit_(1));
v_cmd_body_(0) = invNormalize(ctrl_interfaces_.control_inputs_.ly, v_x_limit_(0), v_x_limit_(1));
v_cmd_body_(1) = -invNormalize(ctrl_interfaces_.control_inputs_.lx, v_y_limit_(0), v_y_limit_(1));
v_cmd_body_(2) = 0;
/* Turning */
d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.rx, w_yaw_limit_(0), w_yaw_limit_(1));
d_yaw_cmd_ = -invNormalize(ctrl_interfaces_.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_past_ = d_yaw_cmd_;
}
@ -139,8 +144,8 @@ void StateTrotting::calcTau() {
for (int i(0); i < 4; ++i) {
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));
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));
}
}
@ -150,7 +155,7 @@ void StateTrotting::calcTau() {
for (int i = 0; i < 4; 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));
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
}
}
}
@ -167,8 +172,8 @@ void StateTrotting::calcQQd() {
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));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(q_goal(i));
std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
}
}
@ -177,14 +182,14 @@ void StateTrotting::calcGain() const {
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);
ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
}
} else {
// stable gain
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);
}
}
}

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

@ -3,7 +3,8 @@
//
#include "unitree_guide_controller/UnitreeGuideController.h"
#include "unitree_guide_controller/FSM/StatePassive.h"
#include <unitree_guide_controller/gait/WaveGenerator.h>
#include "unitree_guide_controller/robot/QuadrupedRobot.h"
namespace unitree_guide_controller {
@ -53,13 +54,13 @@ namespace unitree_guide_controller {
// update_frequency_ = 1.0 / time_diff.count();
// RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
if (ctrl_comp_.robot_model_ == nullptr) {
if (ctrl_component_.robot_model_ == nullptr) {
return controller_interface::return_type::OK;
}
ctrl_comp_.robot_model_->update();
ctrl_comp_.wave_generator_->update();
ctrl_comp_.estimator_->update();
ctrl_component_.robot_model_->update();
ctrl_component_.wave_generator_->update();
ctrl_component_.estimator_->update();
if (mode_ == FSMMode::NORMAL) {
current_state_->run();
@ -103,10 +104,10 @@ namespace unitree_guide_controller {
stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_);
ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
ctrl_component_.estimator_ = std::make_shared<Estimator>(ctrl_interfaces_, ctrl_component_);
} catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
@ -120,22 +121,22 @@ namespace unitree_guide_controller {
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
ctrl_comp_.control_inputs_.command = msg->command;
ctrl_comp_.control_inputs_.lx = msg->lx;
ctrl_comp_.control_inputs_.ly = msg->ly;
ctrl_comp_.control_inputs_.rx = msg->rx;
ctrl_comp_.control_inputs_.ry = msg->ry;
ctrl_interfaces_.control_inputs_.command = msg->command;
ctrl_interfaces_.control_inputs_.lx = msg->lx;
ctrl_interfaces_.control_inputs_.ly = msg->ly;
ctrl_interfaces_.control_inputs_.rx = msg->rx;
ctrl_interfaces_.control_inputs_.ry = msg->ry;
});
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_ = std::make_shared<QuadrupedRobot>(
ctrl_comp_, msg->data, feet_names_, base_name_);
ctrl_comp_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_comp_.robot_model_);
ctrl_component_.robot_model_ = std::make_shared<QuadrupedRobot>(
ctrl_interfaces_, msg->data, feet_names_, base_name_);
ctrl_component_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_component_.robot_model_);
});
ctrl_comp_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
ctrl_component_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
return CallbackReturn::SUCCESS;
}
@ -143,7 +144,7 @@ namespace unitree_guide_controller {
controller_interface::CallbackReturn
UnitreeGuideController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
// clear out vectors in case of restart
ctrl_comp_.clear();
ctrl_interfaces_.clear();
// assign command interfaces
for (auto &interface: command_interfaces_) {
@ -158,20 +159,20 @@ namespace unitree_guide_controller {
// assign state interfaces
for (auto &interface: state_interfaces_) {
if (interface.get_prefix_name() == imu_name_) {
ctrl_comp_.imu_state_interface_.emplace_back(interface);
ctrl_interfaces_.imu_state_interface_.emplace_back(interface);
} else {
state_interface_map_[interface.get_interface_name()]->push_back(interface);
}
}
// Create FSM List
state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_);
state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);
state_list_.trotting = std::make_shared<StateTrotting>(ctrl_comp_);
state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_);
state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_interfaces_, ctrl_component_);
state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_interfaces_, ctrl_component_);
state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_interfaces_, ctrl_component_);
state_list_.trotting = std::make_shared<StateTrotting>(ctrl_interfaces_, ctrl_component_);
// Initialize FSM
current_state_ = state_list_.passive;

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

@ -5,14 +5,15 @@
#include "unitree_guide_controller/control/Estimator.h"
#include <unitree_guide_controller/common/mathTools.h>
#include <unitree_guide_controller/control/CtrlComponent.h>
#include "unitree_guide_controller/control/CtrlComponent.h"
#include "controller_common/CtrlInterfaces.h"
Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component) : ctrl_interfaces_(ctrl_interfaces),
robot_model_(ctrl_component.robot_model_),
wave_generator_(ctrl_component.wave_generator_) {
g_ << 0, 0, -9.81;
dt_ = 1.0 / ctrl_component_.frequency_;
dt_ = 1.0 / ctrl_interfaces.frequency_;
std::cout << "dt: " << dt_ << std::endl;
large_variance_ = 100;
@ -181,19 +182,19 @@ void Estimator::update() {
}
Quat quat;
quat << ctrl_component_.imu_state_interface_[0].get().get_value(),
ctrl_component_.imu_state_interface_[1].get().get_value(),
ctrl_component_.imu_state_interface_[2].get().get_value(),
ctrl_component_.imu_state_interface_[3].get().get_value();
quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(),
ctrl_interfaces_.imu_state_interface_[1].get().get_value(),
ctrl_interfaces_.imu_state_interface_[2].get().get_value(),
ctrl_interfaces_.imu_state_interface_[3].get().get_value();
rotation_ = quatToRotMat(quat);
gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(),
ctrl_component_.imu_state_interface_[5].get().get_value(),
ctrl_component_.imu_state_interface_[6].get().get_value();
gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_value(),
ctrl_interfaces_.imu_state_interface_[5].get().get_value(),
ctrl_interfaces_.imu_state_interface_[6].get().get_value();
acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(),
ctrl_component_.imu_state_interface_[8].get().get_value(),
ctrl_component_.imu_state_interface_[9].get().get_value();
acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_value(),
ctrl_interfaces_.imu_state_interface_[8].get().get_value(),
ctrl_interfaces_.imu_state_interface_[9].get().get_value();
u_ = rotation_ * acceleration_ + g_;
x_hat_ = A * x_hat_ + B * u_;

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

@ -4,11 +4,13 @@
#include "unitree_guide_controller/gait/FeetEndCalc.h"
#include "unitree_guide_controller/control/CtrlComponent.h"
#include <unitree_guide_controller/control/CtrlComponent.h>
#include <unitree_guide_controller/control/Estimator.h>
FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
robot_model_(ctrl_component.robot_model_),
estimator_(ctrl_component.estimator_) {
FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component)
: ctrl_component_(ctrl_component),
robot_model_(ctrl_component.robot_model_),
estimator_(ctrl_component.estimator_) {
k_x_ = 0.005;
k_y_ = 0.005;
k_yaw_ = 0.005;

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

@ -5,12 +5,14 @@
#include "unitree_guide_controller/gait/GaitGenerator.h"
#include <utility>
#include <unitree_guide_controller/control/CtrlComponent.h>
#include <unitree_guide_controller/control/Estimator.h>
#include <unitree_guide_controller/gait/WaveGenerator.h>
#include "unitree_guide_controller/control/CtrlComponent.h"
GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : wave_generator_(ctrl_component.wave_generator_),
estimator_(ctrl_component.estimator_),
feet_end_calc_(ctrl_component) {
GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component)
: wave_generator_(ctrl_component.wave_generator_),
estimator_(ctrl_component.estimator_),
feet_end_calc_(ctrl_component) {
first_run_ = true;
}

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

@ -2,13 +2,11 @@
// Created by biao on 24-9-18.
//
#include <utility>
#include "unitree_guide_controller/gait/WaveGenerator.h"
#include <iostream>
WaveGenerator::WaveGenerator(double period, double st_ratio, const Vec4 &bias) {
WaveGenerator::WaveGenerator(const double period, const double st_ratio, const Vec4 &bias) {
phase_past_ << 0.5, 0.5, 0.5, 0.5;
contact_past_.setZero();

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

@ -3,12 +3,12 @@
//
#include <iostream>
#include "unitree_guide_controller/control/CtrlComponent.h"
#include "controller_common/CtrlInterfaces.h"
#include "unitree_guide_controller/robot/QuadrupedRobot.h"
QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
QuadrupedRobot::QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description,
const std::vector<std::string> &feet_names,
const std::string &base_name) : ctrl_component_(ctrl_component) {
const std::string &base_name) : ctrl_interfaces_(ctrl_interfaces) {
KDL::Tree robot_tree;
kdl_parser::treeFromString(robot_description, robot_tree);
@ -114,15 +114,15 @@ void QuadrupedRobot::update() {
if (mass_ == 0) return;
for (int i = 0; i < 4; i++) {
KDL::JntArray pos_array(3);
pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value();
pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value();
pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value();
pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_value();
pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_value();
pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_value();
current_joint_pos_[i] = pos_array;
KDL::JntArray vel_array(3);
vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value();
vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_value();
vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
current_joint_vel_[i] = vel_array;
}
}

8
descriptions/unitree/go2_description/config/gazebo.yaml
View File

@ -77,10 +77,10 @@ ocs2_quadruped_controller:
foot_force_name: "foot_force"
foot_force_interfaces:
- FL_ft_sensor
- RL_ft_sensor
- FR_ft_sensor
- RR_ft_sensor
- FL_foot_force
- RL_foot_force
- FR_foot_force
- RR_foot_force
unitree_guide_controller:
ros__parameters:

22
descriptions/unitree/go2_description/xacro/ft_sensor.xacro
View File

@ -1,22 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ft_sensor" params="name">
<gazebo reference="${name}_foot_fixed">
<!-- Enable feedback for this joint -->
<provideFeedback>true</provideFeedback>
<!-- Prevent ros2_control from lumping this fixed joint with others -->
<disableFixedJointLumping>true</disableFixedJointLumping>
<sensor name="${name}_ft_sensor" type="force_torque">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>${name}_ft</topic>
<force_torque>
<frame>child</frame>
<!-- <measure_direction>child_to_parent</measure_direction>-->
</force_torque>
</sensor>
</gazebo>
</xacro:macro>
</robot>

604
descriptions/unitree/go2_description/xacro/gazebo.xacro
View File

@ -1,324 +1,324 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go2_description)/xacro/ft_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware>
<xacro:include filename="$(find gz_quadruped_hardware)/xacro/foot_force_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware>
<joint name="FR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<joint name="FR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
<sensor name="foot_force">
<state_interface name="FR_ft_sensor"/>
<state_interface name="FL_ft_sensor"/>
<state_interface name="RR_ft_sensor"/>
<state_interface name="RL_ft_sensor"/>
</sensor>
<sensor name="foot_force">
<state_interface name="FR_foot_force"/>
<state_interface name="FL_foot_force"/>
<state_interface name="RR_foot_force"/>
<state_interface name="RL_foot_force"/>
</sensor>
</ros2_control>
</ros2_control>
<gazebo>
<plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
<plugin filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>1000</odom_publish_frequency>
<odom_topic>odom</odom_topic>
<dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic>
<tf_topic>tf</tf_topic>
</plugin>
<gazebo>
<plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
<plugin filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>1000</odom_publish_frequency>
<odom_topic>odom</odom_topic>
<dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic>
<tf_topic>tf</tf_topic>
</plugin>
</gazebo>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:foot_force_sensor name="FL"/>
<xacro:foot_force_sensor name="RL"/>
<xacro:foot_force_sensor name="FR"/>
<xacro:foot_force_sensor name="RR"/>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">
<gazebo reference="front_camera">
<sensor name="front_camera" type="camera">
<camera>
<horizontal_fov>2.094</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
</image>
<clip>
<near>0.1</near>
<far>15</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<optical_frame_id>front_camera</optical_frame_id>
<camera_info_topic>camera/camera_info</camera_info_topic>
</camera>
<always_on>true</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<topic>camera/image</topic>
</sensor>
</gazebo>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<gazebo reference="lidar">
<sensor name='L1_lidar' type='gpu_lidar'>
<topic>scan</topic>
<update_rate>10</update_rate>
<gz_frame_id>lidar</gz_frame_id>
<lidar>
<scan>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>1.396263</min_angle>
<max_angle>4.8869</max_angle>
</horizontal>
<vertical>
<samples>16</samples>
<resolution>1</resolution>
<min_angle>-0.261799</min_angle>
<max_angle>0.261799</max_angle>
</vertical>
</scan>
<range>
<min>0.05</min>
<max>10.0</max>
<resolution>0.01</resolution>
</range>
</lidar>
<alwaysOn>1</alwaysOn>
<visualize>true</visualize>
</sensor>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:ft_sensor name="FL"/>
<xacro:ft_sensor name="RL"/>
<xacro:ft_sensor name="FR"/>
<xacro:ft_sensor name="RR"/>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">
<gazebo reference="front_camera">
<sensor name="front_camera" type="camera">
<camera>
<horizontal_fov>2.094</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
</image>
<clip>
<near>0.1</near>
<far>15</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<optical_frame_id>front_camera</optical_frame_id>
<camera_info_topic>camera/camera_info</camera_info_topic>
</camera>
<always_on>true</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<topic>camera/image</topic>
</sensor>
</gazebo>
<gazebo reference="lidar">
<sensor name='L1_lidar' type='gpu_lidar'>
<topic>scan</topic>
<update_rate>10</update_rate>
<gz_frame_id>lidar</gz_frame_id>
<lidar>
<scan>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>1.396263</min_angle>
<max_angle>4.8869</max_angle>
</horizontal>
<vertical>
<samples>16</samples>
<resolution>1</resolution>
<min_angle>-0.261799</min_angle>
<max_angle>0.261799</max_angle>
</vertical>
</scan>
<range>
<min>0.05</min>
<max>10.0</max>
<resolution>0.01</resolution>
</range>
</lidar>
<alwaysOn>1</alwaysOn>
<visualize>true</visualize>
</sensor>
</gazebo>
</xacro:if>
</xacro:if>
</robot>

67
hardwares/gz_quadruped_hardware/CMakeLists.txt
View File

@ -3,18 +3,18 @@ project(gz_quadruped_hardware)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# Default to C11
if(NOT CMAKE_C_STANDARD)
if (NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 11)
endif()
endif ()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
endif ()
# Compiler options
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
endif ()
# Find dependencies
find_package(ament_cmake REQUIRED)
@ -38,58 +38,63 @@ add_library(${PROJECT_NAME}-system SHARED
)
target_link_libraries(${PROJECT_NAME}-system
gz-sim::gz-sim
gz-plugin::register
gz-sim::gz-sim
gz-plugin::register
)
ament_target_dependencies(${PROJECT_NAME}-system
ament_index_cpp
controller_manager
hardware_interface
pluginlib
rclcpp
yaml_cpp_vendor
rclcpp_lifecycle
ament_index_cpp
controller_manager
hardware_interface
pluginlib
rclcpp
yaml_cpp_vendor
rclcpp_lifecycle
)
#########
add_library(gz_quadruped_plugins SHARED
src/gz_system.cpp
src/gz_system.cpp
)
ament_target_dependencies(gz_quadruped_plugins
rclcpp_lifecycle
hardware_interface
rclcpp
rclcpp_lifecycle
hardware_interface
rclcpp
)
target_link_libraries(gz_quadruped_plugins
gz-sim::gz-sim
gz-sim::gz-sim
)
## Install
install(TARGETS
gz_quadruped_plugins
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
gz_quadruped_plugins
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY
include/
DESTINATION include
include/
DESTINATION include
)
install(
DIRECTORY xacro
DESTINATION share/${PROJECT_NAME}/
)
# Testing and linting
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
if (BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif ()
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME}-system gz_quadruped_plugins)
# Install directories
install(TARGETS ${PROJECT_NAME}-system
DESTINATION lib
DESTINATION lib
)
pluginlib_export_plugin_description_file(gz_quadruped_hardware gz_quadruped_hardware.xml)

21
hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="foot_force_sensor" params="name">
<gazebo reference="${name}_foot_fixed">
<!-- Enable feedback for this joint -->
<provideFeedback>true</provideFeedback>
<!-- Prevent ros2_control from lumping this fixed joint with others -->
<disableFixedJointLumping>true</disableFixedJointLumping>
<sensor name="${name}_foot_force" type="force_torque">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>${name}_foot_force</topic>
<force_torque>
<frame>child</frame>
</force_torque>
</sensor>
</gazebo>
</xacro:macro>
</robot>

58
libraries/controller_common/CMakeLists.txt Normal file
View File

@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.8)
project(controller_common)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif ()
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(dependencies
hardware_interface
control_input_msgs
rclcpp
)
find_package(ament_cmake REQUIRED)
foreach (Dependency IN ITEMS ${dependencies})
find_package(${Dependency} REQUIRED)
endforeach ()
add_library(${PROJECT_NAME} SHARED
src/FSM/StatePassive.cpp
src/FSM/BaseFixedStand.cpp
src/FSM/StateFixedDown.cpp
)
target_include_directories(${PROJECT_NAME}
PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
PRIVATE
src)
ament_target_dependencies(
${PROJECT_NAME} PUBLIC
${dependencies}
)
install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(
TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS})
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_package()

56
libraries/controller_common/include/controller_common/CtrlInterfaces.h Normal file
View File

@ -0,0 +1,56 @@
//
// Created by biao on 24-9-10.
//
#ifndef INTERFACE_H
#define INTERFACE_H
#include <vector>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
struct CtrlInterfaces {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_torque_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kp_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kd_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_effort_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_;
control_input_msgs::msg::Inputs control_inputs_;
int frequency_{};
CtrlInterfaces() = default;
void clear() {
joint_torque_command_interface_.clear();
joint_position_command_interface_.clear();
joint_velocity_command_interface_.clear();
joint_kd_command_interface_.clear();
joint_kp_command_interface_.clear();
joint_effort_state_interface_.clear();
joint_position_state_interface_.clear();
joint_velocity_state_interface_.clear();
imu_state_interface_.clear();
}
};
#endif //INTERFACE_H

40
libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h Normal file
View File

@ -0,0 +1,40 @@
//
// Created by biao on 24-9-10.
//
#ifndef BASEFIXEDSTAND_H
#define BASEFIXEDSTAND_H
#include <rclcpp/time.hpp>
#include "FSMState.h"
class BaseFixedStand : public FSMState {
public:
BaseFixedStand(CtrlInterfaces &ctrl_interfaces,
const std::vector<double> &target_pos,
double kp,
double kd);
void enter() override;
void run() override;
void exit() override;
FSMStateName checkChange() override;
protected:
double target_pos_[12] = {};
double start_pos_[12] = {};
rclcpp::Time start_time_;
double kp_, kd_;
double duration_ = 600; // steps
double percent_ = 0; //%
double phase = 0.0;
};
#endif //BASEFIXEDSTAND_H

10
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h → libraries/controller_common/include/controller_common/FSM/FSMState.h
View File

@ -7,17 +7,17 @@
#include <string>
#include <utility>
#include <unitree_guide_controller/common/enumClass.h>
#include <unitree_guide_controller/control/CtrlComponent.h>
#include <controller_common/common/enumClass.h>
#include <controller_common/CtrlInterfaces.h>
class FSMState {
public:
virtual ~FSMState() = default;
FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlComponent &ctrl_comp)
FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlInterfaces &ctrl_interfaces)
: state_name(state_name),
state_name_string(std::move(state_name_string)),
ctrl_comp_(ctrl_comp) {
ctrl_interfaces_(ctrl_interfaces) {
}
virtual void enter() = 0;
@ -32,7 +32,7 @@ public:
std::string state_name_string;
protected:
CtrlComponent &ctrl_comp_;
CtrlInterfaces &ctrl_interfaces_;
};
#endif //FSMSTATE_H

2
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h → libraries/controller_common/include/controller_common/FSM/StateFixedDown.h
View File

@ -11,7 +11,7 @@
class StateFixedDown final : public FSMState {
public:
explicit StateFixedDown(CtrlComponent &ctrlComp,
explicit StateFixedDown(CtrlInterfaces &ctrl_interfaces,
const std::vector<double> &target_pos,
double kp,
double kd

2
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h → libraries/controller_common/include/controller_common/FSM/StatePassive.h
View File

@ -9,7 +9,7 @@
class StatePassive final : public FSMState {
public:
explicit StatePassive(CtrlComponent &ctrlComp);
explicit StatePassive(CtrlInterfaces &ctrl_interfaces);
void enter() override;

3
controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h → libraries/controller_common/include/controller_common/common/enumClass.h
View File

@ -16,6 +16,9 @@ enum class FSMStateName {
SWINGTEST,
BALANCETEST,
OCS2,
RL
};
enum class FSMMode {

22
libraries/controller_common/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>controller_common</name>
<version>0.0.0</version>
<description>Common libraries used by the controller</description>
<maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>hardware_interface</depend>
<depend>control_input_msgs</depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

59
libraries/controller_common/src/FSM/BaseFixedStand.cpp Normal file
View File

@ -0,0 +1,59 @@
//
// Created by biao on 24-9-10.
//
#include "controller_common/FSM/BaseFixedStand.h"
#include <cmath>
BaseFixedStand::BaseFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector<double> &target_pos,
const double kp,
const double kd)
: FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrl_interfaces),
kp_(kp), kd_(kd) {
duration_ = ctrl_interfaces_.frequency_ * 1.2;
for (int i = 0; i < 12; i++) {
target_pos_[i] = target_pos[i];
}
}
void BaseFixedStand::enter() {
for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
}
for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_);
}
ctrl_interfaces_.control_inputs_.command = 0;
}
void BaseFixedStand::run() {
percent_ += 1 / duration_;
phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
}
}
void BaseFixedStand::exit() {
percent_ = 0;
}
FSMStateName BaseFixedStand::checkChange() {
if (percent_ < 1.5) {
return FSMStateName::FIXEDSTAND;
}
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:
return FSMStateName::FIXEDDOWN;
default:
return FSMStateName::FIXEDSTAND;
}
}

60
libraries/controller_common/src/FSM/StateFixedDown.cpp Normal file
View File

@ -0,0 +1,60 @@
//
// Created by tlab-uav on 24-9-11.
//
#include "controller_common/FSM/StateFixedDown.h"
#include <cmath>
StateFixedDown::StateFixedDown(CtrlInterfaces &ctrl_interfaces,
const std::vector<double> &target_pos,
const double kp,
const double kd)
: FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrl_interfaces),
kp_(kp), kd_(kd) {
duration_ = ctrl_interfaces_.frequency_ * 1.2;
for (int i = 0; i < 12; i++) {
target_pos_[i] = target_pos[i];
}
}
void StateFixedDown::enter() {
for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
}
ctrl_interfaces_.control_inputs_.command = 0;
for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_);
}
}
void StateFixedDown::run() {
percent_ += 1 / duration_;
phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
}
}
void StateFixedDown::exit() {
percent_ = 0;
}
FSMStateName StateFixedDown::checkChange() {
if (percent_ < 1.5) {
return FSMStateName::FIXEDDOWN;
}
switch (ctrl_interfaces_.control_inputs_.command) {
case 1:
return FSMStateName::PASSIVE;
case 2:
return FSMStateName::FIXEDSTAND;
default:
return FSMStateName::FIXEDDOWN;
}
}

43
libraries/controller_common/src/FSM/StatePassive.cpp Normal file
View File

@ -0,0 +1,43 @@
//
// Created by tlab-uav on 25-2-27.
//
#include "controller_common/FSM/StatePassive.h"
#include <iostream>
StatePassive::StatePassive(CtrlInterfaces &ctrl_interfaces) : FSMState(
FSMStateName::PASSIVE, "passive", ctrl_interfaces) {
}
void StatePassive::enter() {
for (auto i: ctrl_interfaces_.joint_torque_command_interface_) {
std::ignore = i.get().set_value(0);
}
for (auto i: ctrl_interfaces_.joint_position_command_interface_) {
std::ignore = i.get().set_value(0);
}
for (auto i: ctrl_interfaces_.joint_velocity_command_interface_) {
std::ignore = i.get().set_value(0);
}
for (auto i: ctrl_interfaces_.joint_kp_command_interface_) {
std::ignore = i.get().set_value(0);
}
for (auto i: ctrl_interfaces_.joint_kd_command_interface_) {
std::ignore = i.get().set_value(1);
}
ctrl_interfaces_.control_inputs_.command = 0;
}
void StatePassive::run() {
}
void StatePassive::exit() {
}
FSMStateName StatePassive::checkChange() {
if (ctrl_interfaces_.control_inputs_.command == 2) {
return FSMStateName::FIXEDDOWN;
}
return FSMStateName::PASSIVE;
}

16
libraries/gz_quadruped_playground/worlds/default.sdf
View File

@ -66,21 +66,21 @@ gz service -s /world/empty/create \
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<light type="directional" name="sun">