add free stand

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,8 +116,7 @@ bool KeyboardInput::kbhit() {
 }
 
 
-int main(int argc, char *argv[])
+int main(int argc, char *argv[]) {
-{
     rclcpp::init(argc, argv);
     auto node = std::make_shared<KeyboardInput>();
     spin(node);

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
 )

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<KDL::JntArray> init_joint_pos_;
+    std::vector<KDL::JntArray> target_joint_pos_;
+
+    KDL::Frame fr_init_pos_;
+    std::vector<KDL::Frame> init_foot_pos_; // 4 feet position in fr-foot frame
+};
+
+
+#endif //STATEFREESTAND_H

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<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_;
 

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<StatePassive> passive;
         std::shared_ptr<StateFixedDown> fixedDown;
         std::shared_ptr<StateFixedStand> fixedStand;
+        std::shared_ptr<StateFreeStand> freeStand;
 
         std::shared_ptr<StateSwingTest> swingTest;
     };

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<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list,
+    [[nodiscard]] std::vector<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list) const;
-                                                  const std::vector<KDL::JntArray> &q_init) 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<KDL::Frame> getFeet2BPositions(const std::vector<KDL::JntArray> &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,
+     * 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<KDL::JntArray> current_joint_pos_;
+    std::vector<KDL::JntArray> current_joint_vel_;
+
+    void update(const CtrlComponent &ctrlComp);
+
 private:
-    double mass_;
+    double mass_ = 0;
 
     std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
 

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;

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -0,0 +1,79 @@
+//
+// Created by tlab-uav on 24-9-13.
+//
+
+#include <unitree_guide_controller/common/mathTools.h>
+#include <unitree_guide_controller/FSM/StateFreeStand.h>
+
+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));
+    }
+}

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);
+    ctrlComp_.robot_model_.get().update(ctrlComp_);
-    current_joint_vel_.resize(4);
+    init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
-
-    currentJointPos();
-    init_joint_pos_ = 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() {
+void StateSwingTest::torqueCtrl() const {
-    const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(current_joint_pos_[0], 0);
+    const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(0);
-    KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(current_joint_pos_[0], 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<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;
+            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],
+    KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, 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

@@ -118,6 +118,7 @@ namespace unitree_guide_controller {
         state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);
         state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
         state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
+        state_list_.freeStand = std::make_shared<StateFreeStand>(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:
+            case FSMStateName::FREESTAND:
-            //     return state_list_.freeStand;
+                return state_list_.freeStand;
             // case FSMStateName::TROTTING:
             //     return state_list_.trotting;
             case FSMStateName::SWINGTEST:

controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp

@@ -3,6 +3,7 @@
 //
 
 #include <iostream>
+#include <unitree_guide_controller/common/interface.h>
 #include <unitree_guide_controller/robotics/QuadrupedRobot.h>
 
 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<RobotLeg>(rr_chain_);
     robot_legs_[3] = std::make_shared<RobotLeg>(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<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list,
+std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list) const {
-                                                const std::vector<KDL::JntArray> &q_init) const {
     std::vector<KDL::JntArray> 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<KDL::Frame> QuadrupedRobot::getFeet2BPositions(const std::vector<KDL
     return result;
 }
 
-KDL::Frame QuadrupedRobot::getFeet2BPositions(const KDL::JntArray &joint_positions, const int index) const {
+KDL::Frame QuadrupedRobot::getFeet2BPositions(const int index) const {
-    return robot_legs_[index]->calcPEe2B(joint_positions);
+    return robot_legs_[index]->calcPEe2B(current_joint_pos_[index]);
 }
 
-KDL::Jacobian QuadrupedRobot::getJacobian(const KDL::JntArray &joint_positions, const int index) const {
+KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const {
-    return robot_legs_[index]->calcJaco(joint_positions);
+    return robot_legs_[index]->calcJaco(current_joint_pos_[index]);
 }
 
-KDL::JntArray QuadrupedRobot::getTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities,
+KDL::JntArray QuadrupedRobot::getTorque(
     const KDL::Wrenches &force, const int index) const {
-    return robot_legs_[index]->calcTorque(joint_positions, joint_velocities, force);
+    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;
+    }
 }