add swing test mode

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<KDL::JntArray> init_joint_pos_;
+    std::vector<KDL::JntArray> target_joint_pos_;
+
+    std::vector<KDL::JntArray> current_joint_pos_;
+    std::vector<KDL::JntArray> current_joint_vel_;
+
+    std::vector<KDL::Frame> init_foot_pos_;
+    std::vector<KDL::Frame> target_foot_pos_;
+
+    KDL::Frame fr_init_pos_;
+    KDL::Frame fr_goal_pos_;
 };
 
 

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_input_msgs::msg::Inputs> control_inputs_;
     int frequency_{};
-    std::shared_ptr<QuadrupedRobot> robot_model_;
 
-    CtrlComponent() : control_inputs_(default_inputs_) {
+    QuadrupedRobot default_robot_model_;
+    std::reference_wrapper<QuadrupedRobot> robot_model_;
+
+    CtrlComponent() : control_inputs_(default_inputs_), default_robot_model_(), robot_model_(default_robot_model_) {
     }
 
     void clear() {

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<typename T0, typename T1, typename T2>
+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

controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h

@@ -6,14 +6,18 @@
 #ifndef QUADRUPEDROBOT_H
 #define QUADRUPEDROBOT_H
 #include <string>
-#include <kdl_parser/kdl_parser/kdl_parser.hpp>
+#include <kdl_parser/kdl_parser.hpp>
 
 #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<KDL::Frame> getFeet2BPositions(const std::vector<KDL::JntArray> &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<Robotleg> robot_legs_;
+    std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
 
     KDL::Chain fr_chain_;
     KDL::Chain fl_chain_;

controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h

@@ -8,13 +8,15 @@
 
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
-#include <kdl_parser/kdl_parser/kdl_parser.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
 
-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<KDL::ChainFkSolverPos_recursive> fk_pose_solver_;
+    std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
     std::shared_ptr<KDL::ChainIkSolverPos_LMA> ik_pose_solver_;
+    std::shared_ptr<KDL::ChainIdSolver_RNE> id_solver_;
 };
 
 

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(
+        ctrlComp_.joint_kp_command_interface_[i].get().set_value(50.0);
-            phase * 50.0 + (1 - phase) * 20.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) {

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);
+            phase * 60.0 + (1 - phase) * 20.0);
-        ctrlComp_.joint_kd_command_interface_[i].get().set_value(8);
+        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) {

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -2,7 +2,9 @@
 // Created by biao on 24-9-12.
 //
 
+#include <iostream>
 #include <unitree_guide_controller/FSM/StateSwingTest.h>
+#include <unitree_guide_controller/common/mathTools.h>
 
 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<double, 3, Eigen::Dynamic> linear_jacobian = fr_jaco.data.topRows(3);
+    Eigen::Product<Eigen::Matrix<double, 3, -1>, Eigen::Matrix<double, -1, 1> > 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));
+    }
+}

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -4,7 +4,6 @@
 
 #include <unitree_guide_controller/UnitreeGuideController.h>
 #include <unitree_guide_controller/FSM/StatePassive.h>
-#include <kdl_parser/kdl_parser/kdl_parser.hpp>
 #include <unitree_guide_controller/robotics/QuadrupedRobot.h>
 
 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<QuadrupedRobot>(msg->data);
+                ctrl_comp_.robot_model_.get().init(msg->data);
             });
 
         get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);

controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp

@@ -2,9 +2,10 @@
 // Created by biao on 24-9-12.
 //
 
+#include <iostream>
 #include <unitree_guide_controller/robotics/QuadrupedRobot.h>
 
-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_.resize(4);
-    robot_legs_.emplace_back(fl_chain_);
+    robot_legs_[0] = std::make_shared<RobotLeg>(fr_chain_);
-    robot_legs_.emplace_back(rr_chain_);
+    robot_legs_[1] = std::make_shared<RobotLeg>(fl_chain_);
-    robot_legs_.emplace_back(rl_chain_);
+    robot_legs_[2] = std::make_shared<RobotLeg>(rr_chain_);
+    robot_legs_[3] = std::make_shared<RobotLeg>(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<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list,
@@ -31,7 +34,7 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
     std::vector<KDL::JntArray> 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<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
 std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const {
     std::vector<KDL::Frame> result;
     result.resize(4);
-    for (int i(0); i < 4; ++i) {
+    for (int i = 0; i < 4; i++) {
-        result.push_back(robot_legs_[i].calcPEe2B(joint_positions[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);
+}

controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp

@@ -2,28 +2,40 @@
 // Created by biao on 24-9-12.
 //
 
-#include <iostream>
+#include <memory>
 #include <unitree_guide_controller/robotics/RobotLeg.h>
 
-Robotleg::Robotleg(const KDL::Chain &chain) {
+RobotLeg::RobotLeg(const KDL::Chain &chain) {
     chain_ = chain;
 
     fk_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain);
     ik_pose_solver_ = std::make_shared<KDL::ChainIkSolverPos_LMA>(chain);
+    jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain);
+    id_solver_ = std::make_shared<KDL::ChainIdSolver_RNE>(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) {
+    fk_pose_solver_->JntToCart(joint_positions, pEe);
-        std::cerr << "Failed to calculate forward kinematics" << std::endl;
-    }
     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) {
+    ik_pose_solver_->CartToJnt(q_init, pEe, q);
-        std::cerr << "Failed to calculate inverse kinematics" << std::endl;
-    }
     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;
+}