diff --git a/commands/keyboard_input/src/KeyboardInput.cpp b/commands/keyboard_input/src/KeyboardInput.cpp index d9ba2f2..74d4da7 100644 --- a/commands/keyboard_input/src/KeyboardInput.cpp +++ b/commands/keyboard_input/src/KeyboardInput.cpp @@ -21,6 +21,12 @@ void KeyboardInput::timer_callback() { char key = getchar(); check_command(key); if (inputs_.command == 0) check_value(key); + else { + inputs_.lx = 0; + inputs_.ly = 0; + inputs_.rx = 0; + inputs_.ry = 0; + } publisher_->publish(inputs_); } } @@ -110,11 +116,10 @@ bool KeyboardInput::kbhit() { } -int main(int argc, char *argv[]) -{ +int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared(); spin(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index b7cb616..1f0909d 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(${PROJECT_NAME} SHARED src/FSM/StateFixedDown.cpp src/FSM/StateFixedStand.cpp src/FSM/StateSwingTest.cpp + src/FSM/StateFreeStand.cpp src/robotics/QuadrupedRobot.cpp src/robotics/RobotLeg.cpp ) 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 new file mode 100644 index 0000000..addeaf3 --- /dev/null +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h @@ -0,0 +1,38 @@ +// +// Created by tlab-uav on 24-9-13. +// + +#ifndef STATEFREESTAND_H +#define STATEFREESTAND_H +#include "FSMState.h" + + +class StateFreeStand final : public FSMState { +public: + explicit StateFreeStand(CtrlComponent ctrl_component); + + void enter() override; + + void run() override; + + void exit() override; + + FSMStateName checkChange() override; + +private: + void calc_body_target(float row, float pitch, float yaw, float height); + + float row_max_, row_min_; + float pitch_max_, pitch_min_; + float yaw_max_, yaw_min_; + float height_max_, height_min_; + + std::vector init_joint_pos_; + std::vector target_joint_pos_; + + KDL::Frame fr_init_pos_; + 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 28e3e11..a98c979 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 @@ -22,10 +22,9 @@ public: private: - void currentJointPos(); void positionCtrl(); - void torqueCtrl(); + void torqueCtrl() const; float _xMin, _xMax; float _yMin, _yMax; @@ -36,9 +35,6 @@ private: std::vector init_joint_pos_; std::vector target_joint_pos_; - std::vector current_joint_pos_; - std::vector current_joint_vel_; - std::vector init_foot_pos_; std::vector target_foot_pos_; 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 848c830..3c67fb3 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -12,6 +12,7 @@ #include "FSM/StateFixedDown.h" #include "FSM/StateFixedStand.h" +#include "FSM/StateFreeStand.h" #include "FSM/StatePassive.h" #include "FSM/StateSwingTest.h" @@ -21,6 +22,7 @@ namespace unitree_guide_controller { std::shared_ptr passive; std::shared_ptr fixedDown; std::shared_ptr fixedStand; + std::shared_ptr freeStand; std::shared_ptr swingTest; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h index 94f8ce6..8eb1b0f 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h @@ -11,6 +11,8 @@ #include "RobotLeg.h" +struct CtrlComponent; + class QuadrupedRobot { public: explicit QuadrupedRobot() = default; @@ -22,29 +24,47 @@ public: /** * Calculate the joint positions based on the foot end position * @param pEe_list vector of foot-end position - * @param q_init vector of current joint positions * @return */ - [[nodiscard]] std::vector getQ(const std::vector &pEe_list, - const std::vector &q_init) const; + [[nodiscard]] std::vector getQ(const std::vector &pEe_list) const; /** * Calculate the foot end position based on joint positions - * @param joint_positions vector of current joint positions + * @param joint_positions vector of joint positions * @return vector of foot-end position */ [[nodiscard]] std::vector getFeet2BPositions(const std::vector &joint_positions) const; - [[nodiscard]] KDL::Frame getFeet2BPositions(const KDL::JntArray &joint_positions, int index) const; + /** + * Calculate the foot end position based on joint positions + * @param index leg index + * @return foot-end position + */ + [[nodiscard]] KDL::Frame getFeet2BPositions(int index) const; - [[nodiscard]] KDL::Jacobian getJacobian(const KDL::JntArray &joint_positions, int index) const; + /** + * Calculate the Jacobian matrix based on joint positions + * @param index leg index + * @return Jacobian matrix + */ + [[nodiscard]] KDL::Jacobian getJacobian(int index) const; - [[nodiscard]] KDL::JntArray getTorque(const KDL::JntArray &joint_positions, - const KDL::JntArray &joint_velocities, - const KDL::Wrenches &force, int index) const; + /** + * Calculate the torque based on joint positions, joint velocities and external force + * @param force external force + * @param index leg index + * @return torque + */ + [[nodiscard]] KDL::JntArray getTorque( + const KDL::Wrenches &force, int index) const; + + std::vector current_joint_pos_; + std::vector current_joint_vel_; + + void update(const CtrlComponent &ctrlComp); private: - double mass_; + double mass_ = 0; std::vector > robot_legs_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index d1e0dad..eb7ee9b 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp @@ -41,7 +41,9 @@ FSMStateName StateFixedStand::checkChange() { switch (ctrlComp_.control_inputs_.get().command) { case 1: return FSMStateName::FIXEDDOWN; - case 8: + case 3: + return FSMStateName::FREESTAND; + case 9: return FSMStateName::SWINGTEST; default: return FSMStateName::FIXEDSTAND; diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp new file mode 100644 index 0000000..d5f3687 --- /dev/null +++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp @@ -0,0 +1,79 @@ +// +// Created by tlab-uav on 24-9-13. +// + +#include +#include + +StateFreeStand::StateFreeStand(CtrlComponent ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand", + std::move(ctrl_component)) { + row_max_ = 20 * M_PI / 180; + row_min_ = -row_max_; + pitch_max_ = 15 * M_PI / 180; + pitch_min_ = -pitch_max_; + yaw_max_ = 20 * M_PI / 180; + yaw_min_ = -yaw_max_; + height_max_ = 0.1; + height_min_ = -height_max_; +} + +void StateFreeStand::enter() { + for (int i = 0; i < 12; i++) { + ctrlComp_.joint_kp_command_interface_[i].get().set_value(180); + ctrlComp_.joint_kd_command_interface_[i].get().set_value(5); + } + ctrlComp_.robot_model_.get().update(ctrlComp_); + + init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_; + init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_); + + + fr_init_pos_ = init_foot_pos_[0]; + for (auto &foot_pos: init_foot_pos_) { + foot_pos.p -= fr_init_pos_.p; + foot_pos.M = KDL::Rotation::RPY(0, 0, 0); + } +} + +void StateFreeStand::run() { + ctrlComp_.robot_model_.get().update(ctrlComp_); + calc_body_target(invNormalize(ctrlComp_.control_inputs_.get().lx, row_min_, row_max_), + invNormalize(ctrlComp_.control_inputs_.get().ly, pitch_min_, pitch_max_), + invNormalize(ctrlComp_.control_inputs_.get().rx, yaw_min_, yaw_max_), + invNormalize(ctrlComp_.control_inputs_.get().ry, height_min_, height_max_)); +} + +void StateFreeStand::exit() { +} + +FSMStateName StateFreeStand::checkChange() { + switch (ctrlComp_.control_inputs_.get().command) { + case 1: + return FSMStateName::FIXEDDOWN; + case 2: + return FSMStateName::FIXEDSTAND; + default: + return FSMStateName::FREESTAND; + } +} + +void StateFreeStand::calc_body_target(const float row, const float pitch, + const float yaw, const float height) { + KDL::Frame fr_2_body_pos; + fr_2_body_pos.p = -fr_init_pos_.p; + fr_2_body_pos.p.z(fr_2_body_pos.p.z() + height); + fr_2_body_pos.M = KDL::Rotation::RPY(row, pitch, yaw); + + const KDL::Frame body_2_fr_pos = fr_2_body_pos.Inverse(); + std::vector goal_pos(4, KDL::Frame::Identity()); + for (int i = 0; i < 4; i++) { + goal_pos[i] = body_2_fr_pos * init_foot_pos_[i]; + } + target_joint_pos_ = ctrlComp_.robot_model_.get().getQ(goal_pos); + + for (int i = 0; i < 4; i++) { + ctrlComp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); + ctrlComp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); + ctrlComp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2)); + } +} diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index 6142b64..dc8a70a 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -29,11 +29,8 @@ void StateSwingTest::enter() { Kp = KDL::Vector(20, 20, 50); Kd = KDL::Vector(5, 5, 20); - current_joint_pos_.resize(4); - current_joint_vel_.resize(4); - - currentJointPos(); - init_joint_pos_ = current_joint_pos_; + ctrlComp_.robot_model_.get().update(ctrlComp_); + init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_; init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_); target_foot_pos_ = init_foot_pos_; @@ -63,6 +60,8 @@ void StateSwingTest::run() { fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin, fr_init_pos_.p.z(), -1, 0)); } + + ctrlComp_.robot_model_.get().update(ctrlComp_); positionCtrl(); torqueCtrl(); } @@ -81,25 +80,9 @@ FSMStateName StateSwingTest::checkChange() { } } -void StateSwingTest::currentJointPos() { - for (int i = 0; i < 4; i++) { - KDL::JntArray pos_array(3); - pos_array(0) = ctrlComp_.joint_position_state_interface_[i * 3].get().get_value(); - pos_array(1) = ctrlComp_.joint_position_state_interface_[i * 3 + 1].get().get_value(); - pos_array(2) = ctrlComp_.joint_position_state_interface_[i * 3 + 2].get().get_value(); - current_joint_pos_[i] = pos_array; - - KDL::JntArray vel_array(3); - vel_array(0) = ctrlComp_.joint_velocity_state_interface_[i * 3].get().get_value(); - vel_array(1) = ctrlComp_.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); - vel_array(2) = ctrlComp_.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); - current_joint_vel_[i] = vel_array; - } -} - void StateSwingTest::positionCtrl() { target_foot_pos_[0] = fr_goal_pos_; - target_joint_pos_ = ctrlComp_.robot_model_.get().getQ(target_foot_pos_, init_joint_pos_); + target_joint_pos_ = ctrlComp_.robot_model_.get().getQ(target_foot_pos_); for (int i = 0; i < 4; i++) { ctrlComp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); ctrlComp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); @@ -107,22 +90,21 @@ void StateSwingTest::positionCtrl() { } } -void StateSwingTest::torqueCtrl() { - const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(current_joint_pos_[0], 0); - KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(current_joint_pos_[0], 0); +void StateSwingTest::torqueCtrl() const { + const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(0); + KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(0); const KDL::Vector pos_goal = fr_goal_pos_.p; const KDL::Vector pos0 = fr_current_pos.p; const Eigen::Matrix linear_jacobian = fr_jaco.data.topRows(3); Eigen::Product, Eigen::Matrix > vel_eigen = - linear_jacobian * current_joint_vel_[0].data; + linear_jacobian * ctrlComp_.robot_model_.get().current_joint_vel_[0].data; const KDL::Vector vel0(vel_eigen(0), vel_eigen(1), vel_eigen(2)); const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * (-vel0); const KDL::Wrench wrench0(force0, KDL::Vector::Zero()); // 假设力矩为零 const KDL::Wrenches wrenches(1, wrench0); // 创建一个包含 wrench0 的容器 - KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(current_joint_pos_[0], current_joint_vel_[0], - wrenches, 0); + KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, 0); for (int i = 0; i < 3; i++) { ctrlComp_.joint_effort_command_interface_[i].get().set_value(torque0(i)); diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index 8094b85..f9fe0f9 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -118,6 +118,7 @@ namespace unitree_guide_controller { state_list_.fixedDown = std::make_shared(ctrl_comp_); state_list_.fixedStand = std::make_shared(ctrl_comp_); state_list_.swingTest = std::make_shared(ctrl_comp_); + state_list_.freeStand = std::make_shared(ctrl_comp_); // Initialize FSM current_state_ = state_list_.passive; @@ -160,8 +161,8 @@ namespace unitree_guide_controller { return state_list_.fixedDown; case FSMStateName::FIXEDSTAND: return state_list_.fixedStand; - // case FSMStateName::FREESTAND: - // return state_list_.freeStand; + case FSMStateName::FREESTAND: + return state_list_.freeStand; // case FSMStateName::TROTTING: // return state_list_.trotting; case FSMStateName::SWINGTEST: diff --git a/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp index 5fb9735..f8a5337 100644 --- a/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp @@ -3,6 +3,7 @@ // #include +#include #include void QuadrupedRobot::init(const std::string &robot_description) { @@ -20,6 +21,9 @@ void QuadrupedRobot::init(const std::string &robot_description) { robot_legs_[2] = std::make_shared(rr_chain_); robot_legs_[3] = std::make_shared(rl_chain_); + current_joint_pos_.resize(4); + current_joint_vel_.resize(4); + std::cout << "robot_legs_.size(): " << robot_legs_.size() << std::endl; // calculate total mass from urdf @@ -29,12 +33,11 @@ void QuadrupedRobot::init(const std::string &robot_description) { } } -std::vector QuadrupedRobot::getQ(const std::vector &pEe_list, - const std::vector &q_init) const { +std::vector QuadrupedRobot::getQ(const std::vector &pEe_list) const { std::vector result; result.resize(4); for (int i(0); i < 4; ++i) { - result[i] = robot_legs_[i]->calcQ(pEe_list[i], q_init[i]); + result[i] = robot_legs_[i]->calcQ(pEe_list[i], current_joint_pos_[i]); } return result; } @@ -48,15 +51,31 @@ std::vector QuadrupedRobot::getFeet2BPositions(const std::vectorcalcPEe2B(joint_positions); +KDL::Frame QuadrupedRobot::getFeet2BPositions(const int index) const { + return robot_legs_[index]->calcPEe2B(current_joint_pos_[index]); } -KDL::Jacobian QuadrupedRobot::getJacobian(const KDL::JntArray &joint_positions, const int index) const { - return robot_legs_[index]->calcJaco(joint_positions); +KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const { + return robot_legs_[index]->calcJaco(current_joint_pos_[index]); } -KDL::JntArray QuadrupedRobot::getTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities, - const KDL::Wrenches &force, const int index) const { - return robot_legs_[index]->calcTorque(joint_positions, joint_velocities, force); +KDL::JntArray QuadrupedRobot::getTorque( + const KDL::Wrenches &force, const int index) const { + return robot_legs_[index]->calcTorque(current_joint_pos_[index], current_joint_vel_[index], force); +} + +void QuadrupedRobot::update(const CtrlComponent &ctrlComp) { + for (int i = 0; i < 4; i++) { + KDL::JntArray pos_array(3); + pos_array(0) = ctrlComp.joint_position_state_interface_[i * 3].get().get_value(); + pos_array(1) = ctrlComp.joint_position_state_interface_[i * 3 + 1].get().get_value(); + pos_array(2) = ctrlComp.joint_position_state_interface_[i * 3 + 2].get().get_value(); + current_joint_pos_[i] = pos_array; + + KDL::JntArray vel_array(3); + vel_array(0) = ctrlComp.joint_velocity_state_interface_[i * 3].get().get_value(); + vel_array(1) = ctrlComp.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); + vel_array(2) = ctrlComp.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); + current_joint_vel_[i] = vel_array; + } }