diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py index d870a28..5263efe 100644 --- a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py @@ -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" ) diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index bd87bfc..846ba42 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h index a64d16d..5b2d166 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h @@ -4,12 +4,22 @@ #ifndef STATEBALANCETEST_H #define STATEBALANCETEST_H -#include "FSMState.h" +#include + +#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; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h index 090b47c..70085b2 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h @@ -5,35 +5,16 @@ #ifndef STATEFIXEDSTAND_H #define STATEFIXEDSTAND_H -#include +#include -#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 &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; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h index 77c12c8..b2a0884 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h @@ -4,12 +4,16 @@ #ifndef STATEFREESTAND_H #define STATEFREESTAND_H -#include "FSMState.h" +#include +#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 &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 init_foot_pos_; // 4 feet position in fr-foot frame }; - #endif //STATEFREESTAND_H diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h index 8fa4a1d..8714ea0 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h @@ -5,12 +5,17 @@ #ifndef STATESWINGTEST_H #define STATESWINGTEST_H -#include "FSMState.h" +#include +#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; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h index 678e742..8e932c3 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h @@ -4,14 +4,14 @@ #ifndef STATETROTTING_H #define STATETROTTING_H +#include #include - -#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; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index 02eea16..6eef0c9 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -7,14 +7,15 @@ #include #include -#include -#include +#include +#include +#include +#include +#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 joint_names_; @@ -99,11 +101,11 @@ namespace unitree_guide_controller { std::unordered_map< std::string, std::vector > *> 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 > *> 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_} }; }; } diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h index 97250c5..62ba1fb 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h @@ -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 -#include -#include -#include +#ifndef CTRLCOMPONENT_H +#define CTRLCOMPONENT_H #include -#include #include "BalanceCtrl.h" #include "Estimator.h" struct CtrlComponent { - std::vector > - joint_torque_command_interface_; - std::vector > - joint_position_command_interface_; - std::vector > - joint_velocity_command_interface_; - std::vector > - joint_kp_command_interface_; - std::vector > - joint_kd_command_interface_; - - - std::vector > - joint_effort_state_interface_; - std::vector > - joint_position_state_interface_; - std::vector > - joint_velocity_state_interface_; - - std::vector > - imu_state_interface_; - - control_input_msgs::msg::Inputs control_inputs_; - int frequency_{}; - std::shared_ptr robot_model_; std::shared_ptr estimator_; - std::shared_ptr balance_ctrl_; std::shared_ptr 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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h index 83585e0..60112df 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h @@ -8,16 +8,16 @@ #include #include #include - #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 &robot_model_; std::shared_ptr &wave_generator_; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h index 8efe00e..562f8b0 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h @@ -6,7 +6,7 @@ #ifndef WAVEGENERATOR_H #define WAVEGENERATOR_H #include -#include +#include #include inline long long getSystemTime() { diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h index fbaf54b..ac868ff 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h @@ -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 &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 > robot_legs_; KDL::Chain fr_chain_; diff --git a/controllers/unitree_guide_controller/package.xml b/controllers/unitree_guide_controller/package.xml index 29036de..595ff33 100644 --- a/controllers/unitree_guide_controller/package.xml +++ b/controllers/unitree_guide_controller/package.xml @@ -11,6 +11,7 @@ backward_ros controller_interface + controller_common pluginlib control_input_msgs kdl_parser diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index bc300f0..253009b 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -4,14 +4,21 @@ #include "unitree_guide_controller/FSM/StateBalanceTest.h" -#include "unitree_guide_controller/common/mathTools.h" +#include -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 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)); } } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp deleted file mode 100644 index d8e39f4..0000000 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// -// Created by tlab-uav on 24-9-11. -// - -#include "unitree_guide_controller/FSM/StateFixedDown.h" - -#include - -StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp, - const std::vector &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; - } -} diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index 222bec3..49704c9 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp @@ -4,51 +4,17 @@ #include "unitree_guide_controller/FSM/StateFixedStand.h" -#include - -StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector &target_pos, +StateFixedStand::StateFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector &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: diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index 0759ea3..5eefede 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp @@ -3,11 +3,17 @@ // #include "unitree_guide_controller/FSM/StateFreeStand.h" + +#include + #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)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp deleted file mode 100644 index e9381bf..0000000 --- a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// -// Created by tlab-uav on 24-9-6. -// - -#include "unitree_guide_controller/FSM/StatePassive.h" - -#include -#include - -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; -} diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index da35d01..c905fd1 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -4,12 +4,17 @@ #include #include "unitree_guide_controller/FSM/StateSwingTest.h" + +#include + #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)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index e8456e9..0d59c91 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp @@ -5,13 +5,18 @@ #include "unitree_guide_controller/FSM/StateTrotting.h" #include +#include +#include +#include -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); } } } diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index e7907cb..b328651 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -3,7 +3,8 @@ // #include "unitree_guide_controller/UnitreeGuideController.h" -#include "unitree_guide_controller/FSM/StatePassive.h" + +#include #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("stand_kp", stand_kp_); stand_kd_ = auto_declare("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(ctrl_comp_); + ctrl_component_.estimator_ = std::make_shared(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", 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( "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg) { - ctrl_comp_.robot_model_ = std::make_shared( - ctrl_comp_, msg->data, feet_names_, base_name_); - ctrl_comp_.balance_ctrl_ = std::make_shared(ctrl_comp_.robot_model_); + ctrl_component_.robot_model_ = std::make_shared( + ctrl_interfaces_, msg->data, feet_names_, base_name_); + ctrl_component_.balance_ctrl_ = std::make_shared(ctrl_component_.robot_model_); }); - ctrl_comp_.wave_generator_ = std::make_shared(0.45, 0.5, Vec4(0, 0.5, 0.5, 0)); + ctrl_component_.wave_generator_ = std::make_shared(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(ctrl_comp_); - state_list_.fixedDown = std::make_shared(ctrl_comp_, down_pos_, stand_kp_, stand_kd_); - state_list_.fixedStand = std::make_shared(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_); - state_list_.swingTest = std::make_shared(ctrl_comp_); - state_list_.freeStand = std::make_shared(ctrl_comp_); - state_list_.balanceTest = std::make_shared(ctrl_comp_); - state_list_.trotting = std::make_shared(ctrl_comp_); + state_list_.passive = std::make_shared(ctrl_interfaces_); + state_list_.fixedDown = std::make_shared(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_); + state_list_.fixedStand = std::make_shared(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_); + state_list_.swingTest = std::make_shared(ctrl_interfaces_, ctrl_component_); + state_list_.freeStand = std::make_shared(ctrl_interfaces_, ctrl_component_); + state_list_.balanceTest = std::make_shared(ctrl_interfaces_, ctrl_component_); + state_list_.trotting = std::make_shared(ctrl_interfaces_, ctrl_component_); // Initialize FSM current_state_ = state_list_.passive; diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index c6453f8..4874368 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -5,14 +5,15 @@ #include "unitree_guide_controller/control/Estimator.h" #include +#include -#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_; diff --git a/controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp b/controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp index 731ff1b..836f554 100644 --- a/controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp +++ b/controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp @@ -4,11 +4,13 @@ #include "unitree_guide_controller/gait/FeetEndCalc.h" -#include "unitree_guide_controller/control/CtrlComponent.h" +#include +#include -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; diff --git a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp index 54ef121..27b666c 100644 --- a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp +++ b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp @@ -5,12 +5,14 @@ #include "unitree_guide_controller/gait/GaitGenerator.h" #include +#include +#include +#include -#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; } diff --git a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp index 3a88c05..2dfb224 100644 --- a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp +++ b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp @@ -2,13 +2,11 @@ // Created by biao on 24-9-18. // -#include - #include "unitree_guide_controller/gait/WaveGenerator.h" #include -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(); diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 46cb104..31ced28 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -3,12 +3,12 @@ // #include -#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 &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; } } diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index 7465dbd..e1af91f 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -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: diff --git a/descriptions/unitree/go2_description/xacro/ft_sensor.xacro b/descriptions/unitree/go2_description/xacro/ft_sensor.xacro deleted file mode 100644 index aa714f9..0000000 --- a/descriptions/unitree/go2_description/xacro/ft_sensor.xacro +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - true - - true - - 1 - 500 - true - ${name}_ft - - child - - - - - - \ No newline at end of file diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index e408462..645acf2 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -1,324 +1,324 @@ - - - - gz_quadruped_hardware/GazeboSimSystem - + + + + gz_quadruped_hardware/GazeboSimSystem + - - - - - - + + + + + + - - - - + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - + + + + + + - + - - - $(find go2_description)/config/gazebo.yaml - - - - - - odom - base - 1000 - odom - 3 - odom_with_covariance - tf - + + + $(find go2_description)/config/gazebo.yaml + + + + + + odom + base + 1000 + odom + 3 + odom_with_covariance + tf + + + + + 0.6 + 0.6 + 1 + + + + + 1 + 500 + true + imu + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + true + + + + + + + + + + + + + 2.094 + + 1280 + 720 + + + 0.1 + 15 + + + gaussian + + 0.0 + 0.007 + + front_camera + camera/camera_info + + true + 15 + true + camera/image + - - - 0.6 - 0.6 - 1 + + + scan + 10 + lidar + + + + 640 + 1 + 1.396263 + 4.8869 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.05 + 10.0 + 0.01 + + + 1 + true + - - - - 1 - 500 - true - imu - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - - - true - - - - - - - - - - - - - 2.094 - - 1280 - 720 - - - 0.1 - 15 - - - gaussian - - 0.0 - 0.007 - - front_camera - camera/camera_info - - true - 15 - true - camera/image - - - - - scan - 10 - lidar - - - - 640 - 1 - 1.396263 - 4.8869 - - - 16 - 1 - -0.261799 - 0.261799 - - - - 0.05 - 10.0 - 0.01 - - - 1 - true - - - + diff --git a/hardwares/gz_quadruped_hardware/CMakeLists.txt b/hardwares/gz_quadruped_hardware/CMakeLists.txt index a4e6ac3..0b24c23 100644 --- a/hardwares/gz_quadruped_hardware/CMakeLists.txt +++ b/hardwares/gz_quadruped_hardware/CMakeLists.txt @@ -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) diff --git a/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro b/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro new file mode 100644 index 0000000..24676c8 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro @@ -0,0 +1,21 @@ + + + + + + + true + + true + + 1 + 500 + true + ${name}_foot_force + + child + + + + + \ No newline at end of file diff --git a/libraries/controller_common/CMakeLists.txt b/libraries/controller_common/CMakeLists.txt new file mode 100644 index 0000000..ea938fb --- /dev/null +++ b/libraries/controller_common/CMakeLists.txt @@ -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 + "$" + "$" + 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() \ No newline at end of file diff --git a/libraries/controller_common/include/controller_common/CtrlInterfaces.h b/libraries/controller_common/include/controller_common/CtrlInterfaces.h new file mode 100644 index 0000000..268c9ea --- /dev/null +++ b/libraries/controller_common/include/controller_common/CtrlInterfaces.h @@ -0,0 +1,56 @@ +// +// Created by biao on 24-9-10. +// + +#ifndef INTERFACE_H +#define INTERFACE_H + +#include +#include +#include +#include + +struct CtrlInterfaces { + std::vector > + joint_torque_command_interface_; + std::vector > + joint_position_command_interface_; + std::vector > + joint_velocity_command_interface_; + std::vector > + joint_kp_command_interface_; + std::vector > + joint_kd_command_interface_; + + + std::vector > + joint_effort_state_interface_; + std::vector > + joint_position_state_interface_; + std::vector > + joint_velocity_state_interface_; + + std::vector > + 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 diff --git a/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h b/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h new file mode 100644 index 0000000..911403f --- /dev/null +++ b/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h @@ -0,0 +1,40 @@ +// +// Created by biao on 24-9-10. +// + +#ifndef BASEFIXEDSTAND_H +#define BASEFIXEDSTAND_H + +#include + +#include "FSMState.h" + +class BaseFixedStand : public FSMState { +public: + BaseFixedStand(CtrlInterfaces &ctrl_interfaces, + const std::vector &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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h b/libraries/controller_common/include/controller_common/FSM/FSMState.h similarity index 73% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h rename to libraries/controller_common/include/controller_common/FSM/FSMState.h index f3a30f2..9804ea8 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h +++ b/libraries/controller_common/include/controller_common/FSM/FSMState.h @@ -7,17 +7,17 @@ #include #include -#include -#include +#include +#include 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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h b/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h similarity index 92% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h rename to libraries/controller_common/include/controller_common/FSM/StateFixedDown.h index bc48ad6..4b0d136 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h +++ b/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h @@ -11,7 +11,7 @@ class StateFixedDown final : public FSMState { public: - explicit StateFixedDown(CtrlComponent &ctrlComp, + explicit StateFixedDown(CtrlInterfaces &ctrl_interfaces, const std::vector &target_pos, double kp, double kd diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h b/libraries/controller_common/include/controller_common/FSM/StatePassive.h similarity index 84% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h rename to libraries/controller_common/include/controller_common/FSM/StatePassive.h index 9b186c4..744ac5e 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h +++ b/libraries/controller_common/include/controller_common/FSM/StatePassive.h @@ -9,7 +9,7 @@ class StatePassive final : public FSMState { public: - explicit StatePassive(CtrlComponent &ctrlComp); + explicit StatePassive(CtrlInterfaces &ctrl_interfaces); void enter() override; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h b/libraries/controller_common/include/controller_common/common/enumClass.h similarity index 96% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h rename to libraries/controller_common/include/controller_common/common/enumClass.h index ecddf66..d45c604 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h +++ b/libraries/controller_common/include/controller_common/common/enumClass.h @@ -16,6 +16,9 @@ enum class FSMStateName { SWINGTEST, BALANCETEST, + + OCS2, + RL }; enum class FSMMode { diff --git a/libraries/controller_common/package.xml b/libraries/controller_common/package.xml new file mode 100644 index 0000000..5e54765 --- /dev/null +++ b/libraries/controller_common/package.xml @@ -0,0 +1,22 @@ + + + + controller_common + 0.0.0 + Common libraries used by the controller + Huang Zhenbiao + Apache-2.0 + + ament_cmake + + hardware_interface + control_input_msgs + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/libraries/controller_common/src/FSM/BaseFixedStand.cpp b/libraries/controller_common/src/FSM/BaseFixedStand.cpp new file mode 100644 index 0000000..238830f --- /dev/null +++ b/libraries/controller_common/src/FSM/BaseFixedStand.cpp @@ -0,0 +1,59 @@ +// +// Created by biao on 24-9-10. +// + +#include "controller_common/FSM/BaseFixedStand.h" + +#include + +BaseFixedStand::BaseFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector &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; + } +} diff --git a/libraries/controller_common/src/FSM/StateFixedDown.cpp b/libraries/controller_common/src/FSM/StateFixedDown.cpp new file mode 100644 index 0000000..631e4bd --- /dev/null +++ b/libraries/controller_common/src/FSM/StateFixedDown.cpp @@ -0,0 +1,60 @@ +// +// Created by tlab-uav on 24-9-11. +// + +#include "controller_common/FSM/StateFixedDown.h" + +#include + +StateFixedDown::StateFixedDown(CtrlInterfaces &ctrl_interfaces, + const std::vector &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; + } +} diff --git a/libraries/controller_common/src/FSM/StatePassive.cpp b/libraries/controller_common/src/FSM/StatePassive.cpp new file mode 100644 index 0000000..964afe1 --- /dev/null +++ b/libraries/controller_common/src/FSM/StatePassive.cpp @@ -0,0 +1,43 @@ +// +// Created by tlab-uav on 25-2-27. +// + +#include "controller_common/FSM/StatePassive.h" + +#include + +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; +} diff --git a/libraries/gz_quadruped_playground/worlds/default.sdf b/libraries/gz_quadruped_playground/worlds/default.sdf index 06c7f4e..4306147 100644 --- a/libraries/gz_quadruped_playground/worlds/default.sdf +++ b/libraries/gz_quadruped_playground/worlds/default.sdf @@ -66,21 +66,21 @@ gz service -s /world/empty/create \ 1.0 + filename="gz-sim-sensors-system" + name="gz::sim::systems::Sensors"> ogre2 + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-user-commands-system" + name="gz::sim::systems::UserCommands"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster">