From 7e9e1e346c4af6caa6c157106ae1a1b0c5a0c683 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Thu, 12 Sep 2024 20:04:20 +0800 Subject: [PATCH] add swing test mode --- .../FSM/StateSwingTest.h | 20 ++++ .../common/interface.h | 6 +- .../common/mathTools.h | 14 +++ .../robotics/QuadrupedRobot.h | 20 +++- .../robotics/RobotLeg.h | 22 ++++- .../src/FSM/StateFixedDown.cpp | 5 +- .../src/FSM/StateFixedStand.cpp | 6 +- .../src/FSM/StateSwingTest.cpp | 95 +++++++++++++++++++ .../src/UnitreeGuideController.cpp | 3 +- .../src/robotics/QuadrupedRobot.cpp | 38 +++++--- .../src/robotics/RobotLeg.cpp | 32 +++++-- 11 files changed, 222 insertions(+), 39 deletions(-) create mode 100644 controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.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 eaba2ed..28e3e11 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 @@ -21,9 +21,29 @@ public: FSMStateName checkChange() override; private: + + void currentJointPos(); + void positionCtrl(); + + void torqueCtrl(); + float _xMin, _xMax; float _yMin, _yMax; float _zMin, _zMax; + + KDL::Vector Kp, Kd; + + 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_; + + KDL::Frame fr_init_pos_; + KDL::Frame fr_goal_pos_; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h index d940de0..2cb7a85 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h @@ -34,9 +34,11 @@ struct CtrlComponent { control_input_msgs::msg::Inputs default_inputs_; std::reference_wrapper control_inputs_; int frequency_{}; - std::shared_ptr robot_model_; - CtrlComponent() : control_inputs_(default_inputs_) { + QuadrupedRobot default_robot_model_; + std::reference_wrapper robot_model_; + + CtrlComponent() : control_inputs_(default_inputs_), default_robot_model_(), robot_model_(default_robot_model_) { } void clear() { diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h new file mode 100644 index 0000000..d91f726 --- /dev/null +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h @@ -0,0 +1,14 @@ +// +// Created by tlab-uav on 24-9-12. +// + +#ifndef MATHTOOLS_H +#define MATHTOOLS_H + +template +T1 invNormalize(const T0 value, const T1 min, const T2 max, + const double minLim = -1, const double maxLim = 1) { + return (value - minLim) * (max - min) / (maxLim - minLim) + min; +} + +#endif //MATHTOOLS_H 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 2aab7c3..94f8ce6 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 @@ -6,14 +6,18 @@ #ifndef QUADRUPEDROBOT_H #define QUADRUPEDROBOT_H #include -#include +#include #include "RobotLeg.h" class QuadrupedRobot { public: - explicit QuadrupedRobot(const std::string &robot_description); + explicit QuadrupedRobot() = default; + + ~QuadrupedRobot() = default; + + void init(const std::string &robot_description); /** * Calculate the joint positions based on the foot end position @@ -31,10 +35,18 @@ public: */ [[nodiscard]] std::vector getFeet2BPositions(const std::vector &joint_positions) const; -protected: + [[nodiscard]] KDL::Frame getFeet2BPositions(const KDL::JntArray &joint_positions, int index) const; + + [[nodiscard]] KDL::Jacobian getJacobian(const KDL::JntArray &joint_positions, int index) const; + + [[nodiscard]] KDL::JntArray getTorque(const KDL::JntArray &joint_positions, + const KDL::JntArray &joint_velocities, + const KDL::Wrenches &force, int index) const; + +private: double mass_; - std::vector robot_legs_; + std::vector > robot_legs_; KDL::Chain fr_chain_; KDL::Chain fl_chain_; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h index b1bdea3..bbe0874 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h @@ -8,13 +8,15 @@ #include #include -#include +#include +#include +#include -class Robotleg { +class RobotLeg { public: - explicit Robotleg(const KDL::Chain &chain); + explicit RobotLeg(const KDL::Chain &chain); - ~Robotleg() = default; + ~RobotLeg() = default; /** * Use forward kinematic to calculate the Pose of End effector to Body frame. @@ -31,10 +33,22 @@ public: */ [[nodiscard]] KDL::JntArray calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const; + /** + * Calculate the current jacobian matrix. + * @param joint_positions Leg joint positions + * @return jacobian matrix + */ + [[nodiscard]] KDL::Jacobian calcJaco(const KDL::JntArray &joint_positions) const; + + [[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities, + const KDL::Wrenches& force) const; + protected: KDL::Chain chain_; std::shared_ptr fk_pose_solver_; + std::shared_ptr jac_solver_; std::shared_ptr ik_pose_solver_; + std::shared_ptr id_solver_; }; diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp index 8257425..3b93c07 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp @@ -24,8 +24,7 @@ void StateFixedDown::run() { phase * target_pos_[i] + (1 - phase) * start_pos_[i]); ctrlComp_.joint_velocity_command_interface_[i].get().set_value(0); ctrlComp_.joint_effort_command_interface_[i].get().set_value(0); - ctrlComp_.joint_kp_command_interface_[i].get().set_value( - phase * 50.0 + (1 - phase) * 20.0); + ctrlComp_.joint_kp_command_interface_[i].get().set_value(50.0); ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5); } } @@ -35,7 +34,7 @@ void StateFixedDown::exit() { } FSMStateName StateFixedDown::checkChange() { - if (percent_ < 1) { + if (percent_ < 2) { return FSMStateName::FIXEDDOWN; } switch (ctrlComp_.control_inputs_.get().command) { diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index a57c678..d1e0dad 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp @@ -25,8 +25,8 @@ void StateFixedStand::run() { ctrlComp_.joint_velocity_command_interface_[i].get().set_value(0); ctrlComp_.joint_effort_command_interface_[i].get().set_value(0); ctrlComp_.joint_kp_command_interface_[i].get().set_value( - phase * 50.0 + (1 - phase) * 20.0); - ctrlComp_.joint_kd_command_interface_[i].get().set_value(8); + phase * 60.0 + (1 - phase) * 20.0); + ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5); } } @@ -35,7 +35,7 @@ void StateFixedStand::exit() { } FSMStateName StateFixedStand::checkChange() { - if (percent_ < 1) { + if (percent_ < 2) { return FSMStateName::FIXEDSTAND; } switch (ctrlComp_.control_inputs_.get().command) { diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index c81bbeb..6142b64 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -2,7 +2,9 @@ // Created by biao on 24-9-12. // +#include #include +#include StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState( FSMStateName::SWINGTEST, "swing test", std::move(ctrlComp)) { @@ -15,9 +17,54 @@ StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState( } void StateSwingTest::enter() { + for (int i = 0; i < 3; i++) { + ctrlComp_.joint_kp_command_interface_[i].get().set_value(3); + ctrlComp_.joint_kd_command_interface_[i].get().set_value(2); + } + for (int i = 3; i < 12; i++) { + ctrlComp_.joint_kp_command_interface_[i].get().set_value(180); + ctrlComp_.joint_kd_command_interface_[i].get().set_value(5); + } + + 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_; + + init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_); + target_foot_pos_ = init_foot_pos_; + fr_init_pos_ = init_foot_pos_[0]; + fr_goal_pos_ = fr_init_pos_; } void StateSwingTest::run() { + if (ctrlComp_.control_inputs_.get().ly > 0) { + fr_goal_pos_.p.x(invNormalize(ctrlComp_.control_inputs_.get().ly, fr_init_pos_.p.x(), + fr_init_pos_.p.x() + _xMax, 0, 1)); + } else { + fr_goal_pos_.p.x(invNormalize(ctrlComp_.control_inputs_.get().ly, fr_init_pos_.p.x() + _xMin, + fr_init_pos_.p.x(), -1, 0)); + } + if (ctrlComp_.control_inputs_.get().lx > 0) { + fr_goal_pos_.p.y(invNormalize(ctrlComp_.control_inputs_.get().lx, fr_init_pos_.p.y(), + fr_init_pos_.p.y() + _yMax, 0, 1)); + } else { + fr_goal_pos_.p.y(invNormalize(ctrlComp_.control_inputs_.get().lx, fr_init_pos_.p.y() + _yMin, + fr_init_pos_.p.y(), -1, 0)); + } + if (ctrlComp_.control_inputs_.get().ry > 0) { + fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z(), + fr_init_pos_.p.z() + _zMax, 0, 1)); + } else { + fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin, + fr_init_pos_.p.z(), -1, 0)); + } + positionCtrl(); + torqueCtrl(); } void StateSwingTest::exit() { @@ -33,3 +80,51 @@ FSMStateName StateSwingTest::checkChange() { return FSMStateName::SWINGTEST; } } + +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_); + 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)); + } +} + +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); + + 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; + 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); + + 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 164abdc..8094b85 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -4,7 +4,6 @@ #include #include -#include #include namespace unitree_guide_controller { @@ -91,7 +90,7 @@ namespace unitree_guide_controller { "~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg) { // Handle message - ctrl_comp_.robot_model_ = std::make_shared(msg->data); + ctrl_comp_.robot_model_.get().init(msg->data); }); get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); diff --git a/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp index 6fb71a0..5fb9735 100644 --- a/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp @@ -2,9 +2,10 @@ // Created by biao on 24-9-12. // +#include #include -QuadrupedRobot::QuadrupedRobot(const std::string &robot_description) { +void QuadrupedRobot::init(const std::string &robot_description) { KDL::Tree robot_tree; kdl_parser::treeFromString(robot_description, robot_tree); @@ -13,17 +14,19 @@ QuadrupedRobot::QuadrupedRobot(const std::string &robot_description) { robot_tree.getChain("base", "RR_foot", rr_chain_); robot_tree.getChain("base", "RL_foot", rl_chain_); - robot_legs_.emplace_back(fr_chain_); - robot_legs_.emplace_back(fl_chain_); - robot_legs_.emplace_back(rr_chain_); - robot_legs_.emplace_back(rl_chain_); + robot_legs_.resize(4); + robot_legs_[0] = std::make_shared(fr_chain_); + robot_legs_[1] = std::make_shared(fl_chain_); + robot_legs_[2] = std::make_shared(rr_chain_); + robot_legs_[3] = std::make_shared(rl_chain_); + + std::cout << "robot_legs_.size(): " << robot_legs_.size() << std::endl; // calculate total mass from urdf - double totoal_mass = 0.0; + mass_ = 0; for (const auto &[fst, snd]: robot_tree.getSegments()) { - totoal_mass += snd.segment.getInertia().getMass(); + mass_ += snd.segment.getInertia().getMass(); } - mass_ = totoal_mass; } std::vector QuadrupedRobot::getQ(const std::vector &pEe_list, @@ -31,7 +34,7 @@ std::vector QuadrupedRobot::getQ(const std::vector &p std::vector result; result.resize(4); for (int i(0); i < 4; ++i) { - result.push_back(robot_legs_[i].calcQ(pEe_list[i], q_init[i])); + result[i] = robot_legs_[i]->calcQ(pEe_list[i], q_init[i]); } return result; } @@ -39,8 +42,21 @@ std::vector QuadrupedRobot::getQ(const std::vector &p std::vector QuadrupedRobot::getFeet2BPositions(const std::vector &joint_positions) const { std::vector result; result.resize(4); - for (int i(0); i < 4; ++i) { - result.push_back(robot_legs_[i].calcPEe2B(joint_positions[i])); + for (int i = 0; i < 4; i++) { + result[i] = robot_legs_[i]->calcPEe2B(joint_positions[i]); } return result; } + +KDL::Frame QuadrupedRobot::getFeet2BPositions(const KDL::JntArray &joint_positions, const int index) const { + return robot_legs_[index]->calcPEe2B(joint_positions); +} + +KDL::Jacobian QuadrupedRobot::getJacobian(const KDL::JntArray &joint_positions, const int index) const { + return robot_legs_[index]->calcJaco(joint_positions); +} + +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); +} diff --git a/controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp b/controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp index 1839552..e2c3af4 100644 --- a/controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp +++ b/controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp @@ -2,28 +2,40 @@ // Created by biao on 24-9-12. // -#include +#include #include -Robotleg::Robotleg(const KDL::Chain &chain) { +RobotLeg::RobotLeg(const KDL::Chain &chain) { chain_ = chain; fk_pose_solver_ = std::make_shared(chain); ik_pose_solver_ = std::make_shared(chain); + jac_solver_ = std::make_shared(chain); + id_solver_ = std::make_shared(chain, KDL::Vector(0, 0, -9.81)); } -KDL::Frame Robotleg::calcPEe2B(const KDL::JntArray &joint_positions) const { +KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const { KDL::Frame pEe; - if (fk_pose_solver_->JntToCart(joint_positions, pEe) < 0) { - std::cerr << "Failed to calculate forward kinematics" << std::endl; - } + fk_pose_solver_->JntToCart(joint_positions, pEe); return pEe; } -KDL::JntArray Robotleg::calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const { +KDL::JntArray RobotLeg::calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const { KDL::JntArray q(chain_.getNrOfJoints()); - if (ik_pose_solver_->CartToJnt(q_init, pEe, q) < 0) { - std::cerr << "Failed to calculate inverse kinematics" << std::endl; - } + ik_pose_solver_->CartToJnt(q_init, pEe, q); return q; } + +KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const { + KDL::Jacobian jacobian(chain_.getNrOfJoints()); + jac_solver_->JntToJac(joint_positions, jacobian); + return jacobian; +} + +KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities, + const KDL::Wrenches &force) const { + KDL::JntArray torque(chain_.getNrOfJoints()); + id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force, + torque); + return torque; +}