can stand up

commands/keyboard_input/include/keyboard_input/KeyboardInput.h

@@ -21,11 +21,11 @@ T1 min(const T1 a, const T2 b) {
 }
 
 
-class Keyboardinput final : public rclcpp::Node {
+class KeyboardInput final : public rclcpp::Node {
 public:
-    Keyboardinput();
+    KeyboardInput();
 
-    ~Keyboardinput() override {
+    ~KeyboardInput() override {
         tcsetattr(STDIN_FILENO, TCSANOW, &old_tio_);
     }
 

commands/keyboard_input/readme.md

@@ -12,7 +12,7 @@ source ~/ros2_ws/install/setup.bash
 ros2 run keyboard_input keyboard_input
 ```
 
-## 1. Running Instructions
+## 1. Use Instructions
 ### 1.1 Control Mode
 * Passive Mode: Keyboard 1
 * Fixed Stand: Keyboard 2

commands/keyboard_input/src/KeyboardInput.cpp

@@ -4,9 +4,9 @@
 
 #include <keyboard_input/KeyboardInput.h>
 
-Keyboardinput::Keyboardinput() : Node("keyboard_input_node") {
+KeyboardInput::KeyboardInput() : Node("keyboard_input_node") {
     publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
-    timer_ = create_wall_timer(std::chrono::microseconds(100), std::bind(&Keyboardinput::timer_callback, this));
+    timer_ = create_wall_timer(std::chrono::microseconds(100), std::bind(&KeyboardInput::timer_callback, this));
     inputs_ = control_input_msgs::msg::Inputs();
 
     tcgetattr(STDIN_FILENO, &old_tio_);
@@ -16,7 +16,7 @@ Keyboardinput::Keyboardinput() : Node("keyboard_input_node") {
     RCLCPP_INFO(get_logger(), "Node initialized. Please input keys, press Ctrl+C to quit.");
 }
 
-void Keyboardinput::timer_callback() {
+void KeyboardInput::timer_callback() {
     if (kbhit()) {
         char key = getchar();
         check_command(key);
@@ -25,7 +25,7 @@ void Keyboardinput::timer_callback() {
     }
 }
 
-void Keyboardinput::check_command(const char key) {
+void KeyboardInput::check_command(const char key) {
     switch (key) {
         case '1':
             inputs_.command = 1; // L2_B
@@ -61,7 +61,7 @@ void Keyboardinput::check_command(const char key) {
     }
 }
 
-void Keyboardinput::check_value(char key) {
+void KeyboardInput::check_value(char key) {
     switch (key) {
         case 'w':
         case 'W':
@@ -101,7 +101,7 @@ void Keyboardinput::check_value(char key) {
     }
 }
 
-bool Keyboardinput::kbhit() {
+bool KeyboardInput::kbhit() {
     timeval tv = {0L, 0L};
     fd_set fds;
     FD_ZERO(&fds);
@@ -113,7 +113,7 @@ bool Keyboardinput::kbhit() {
 int main(int argc, char *argv[])
 {
     rclcpp::init(argc, argv);
-    auto node = std::make_shared<Keyboardinput>();
+    auto node = std::make_shared<KeyboardInput>();
     spin(node);
     rclcpp::shutdown();
     return 0;

controllers/unitree_guide_controller/CMakeLists.txt

@@ -25,6 +25,7 @@ endforeach ()
 add_library(${PROJECT_NAME} SHARED
         src/UnitreeGuideController.cpp
         src/FSM/StatePassive.cpp
+        src/FSM/StateFixedDown.cpp
         src/FSM/StateFixedStand.cpp
 )
 target_include_directories(${PROJECT_NAME}

controllers/unitree_guide_controller/config/controller.yaml

@@ -1,6 +1,7 @@
 unitree_guide_controller:
   ros__parameters:
     type: unitree_guide_controller/UnitreeGuideController
+    update_rate: 200  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h

@@ -7,6 +7,7 @@
 
 #include <string>
 #include <utility>
+#include <rclcpp/time.hpp>
 #include <unitree_guide_controller/common/enumClass.h>
 #include <unitree_guide_controller/common/interface.h>
 

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h

@@ -0,0 +1,37 @@
+//
+// Created by tlab-uav on 24-9-11.
+//
+
+#ifndef STATEFIXEDDOWN_H
+#define STATEFIXEDDOWN_H
+
+#include "FSMState.h"
+
+class StateFixedDown final : public FSMState {
+public:
+    explicit StateFixedDown(CtrlComponent ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+private:
+    double target_pos_[12] = {
+        0.0473455, 1.22187, -2.44375, -0.0473455,
+        1.22187, -2.44375, 0.0473455, 1.22187,
+        -2.44375, -0.0473455, 1.22187, -2.44375
+    };
+
+    double start_pos_[12] = {};
+    rclcpp::Time start_time_;
+
+    double duration_ = 6000; // steps
+    double percent_ = 0; //%
+    double phase = 0.0;
+};
+
+
+#endif //STATEFIXEDDOWN_H

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h

@@ -7,7 +7,7 @@
 
 #include "FSMState.h"
 
-class StateFixedStand : public FSMState {
+class StateFixedStand final : public FSMState {
 public:
     explicit StateFixedStand(CtrlComponent ctrlComp);
 
@@ -26,12 +26,10 @@ private:
         -1.21763, -0.00571868, 0.608813, -1.21763
     };
 
-    double start_pos_[12] = {
-        0.0473455, 1.22187, -2.44375, -0.0473455,
-        1.22187, -2.44375, 0.0473455, 1.22187,
-        -2.44375, -0.0473455, 1.22187, -2.44375
-    };
-    float duration_ = 600; // steps
+    double start_pos_[12] = {};
+    rclcpp::Time start_time_;
+
+    double duration_ = 6000; // steps
     double percent_ = 0; //%
     double phase = 0.0;
 };

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -10,6 +10,7 @@
 #include <unitree_guide_controller/FSM/FSMState.h>
 #include <unitree_guide_controller/common/enumClass.h>
 
+#include "FSM/StateFixedDown.h"
 #include "FSM/StateFixedStand.h"
 #include "FSM/StatePassive.h"
 
@@ -17,6 +18,7 @@ namespace unitree_guide_controller {
     struct FSMStateList {
         std::shared_ptr<FSMState> invalid;
         std::shared_ptr<StatePassive> passive;
+        std::shared_ptr<StateFixedDown> fixedDown;
         std::shared_ptr<StateFixedStand> fixedStand;
     };
 

controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h

@@ -9,6 +9,7 @@ enum class FSMStateName {
     // EXIT,
     INVALID,
     PASSIVE,
+    FIXEDDOWN,
     FIXEDSTAND,
     FREESTAND,
     TROTTING,

controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h

@@ -32,8 +32,10 @@ struct CtrlComponent {
 
     control_input_msgs::msg::Inputs default_inputs_;
     std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
+    int frequency_{};
 
-    CtrlComponent() : control_inputs_(default_inputs_) {}
+    CtrlComponent() : control_inputs_(default_inputs_) {
+    }
 
     void clear() {
         joint_effort_command_interface_.clear();

controllers/unitree_guide_controller/package.xml

@@ -14,6 +14,9 @@
     <depend>pluginlib</depend>
     <depend>control_input_msgs</depend>
 
+    <exec_depend>controller_manager</exec_depend>
+    <exec_depend>joint_state_broadcaster</exec_depend>
+
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_lint_common</test_depend>
 

controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp

@@ -0,0 +1,49 @@
+//
+// Created by tlab-uav on 24-9-11.
+//
+
+#include <cmath>
+#include <unitree_guide_controller/FSM/StateFixedDown.h>
+
+StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState(
+    FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) {
+    // duration_ = ctrlComp_.frequency_ * 15;
+}
+
+void StateFixedDown::enter() {
+    for (int i = 0; i < 12; i++) {
+        start_pos_[i] = ctrlComp_.joint_position_state_interface_[i].get().get_value();
+    }
+}
+
+void StateFixedDown::run() {
+    percent_ += 1 / duration_;
+    phase = std::tanh(percent_);
+    for (int i = 0; i < 12; i++) {
+        ctrlComp_.joint_position_command_interface_[i].get().set_value(
+            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_kd_command_interface_[i].get().set_value(3.5);
+    }
+}
+
+void StateFixedDown::exit() {
+    percent_ = 0;
+}
+
+FSMStateName StateFixedDown::checkChange() {
+    if (percent_ < 1) {
+        return FSMStateName::FIXEDDOWN;
+    }
+    switch (ctrlComp_.control_inputs_.get().command) {
+        case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
+            return FSMStateName::FIXEDSTAND;
+        default:
+            return FSMStateName::FIXEDDOWN;
+    }
+}

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -2,6 +2,8 @@
 // Created by biao on 24-9-10.
 //
 
+#include <cmath>
+#include <iostream>
 #include <unitree_guide_controller/FSM/StateFixedStand.h>
 
 StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
@@ -10,19 +12,34 @@ StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
 
 void StateFixedStand::enter() {
     for (int i = 0; i < 12; i++) {
-        ctrlComp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
+        start_pos_[i] = ctrlComp_.joint_position_state_interface_[i].get().get_value();
     }
 }
 
 void StateFixedStand::run() {
+    percent_ += 1 / duration_;
+    phase = std::tanh(percent_);
+    for (int i = 0; i < 12; i++) {
+        ctrlComp_.joint_position_command_interface_[i].get().set_value(
+            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_kd_command_interface_[i].get().set_value(8);
+    }
 }
 
 void StateFixedStand::exit() {
+    percent_ = 0;
 }
 
 FSMStateName StateFixedStand::checkChange() {
+    if (percent_ < 1) {
+        return FSMStateName::FIXEDSTAND;
+    }
     if (ctrlComp_.control_inputs_.get().command == 1) {
-        return FSMStateName::PASSIVE;
+        return FSMStateName::FIXEDDOWN;
     }
     return FSMStateName::FIXEDSTAND;
 }

controllers/unitree_guide_controller/src/FSM/StatePassive.cpp

@@ -3,7 +3,6 @@
 //
 
 #include <iostream>
-#include <ostream>
 #include <unitree_guide_controller/FSM/StatePassive.h>
 
 #include <utility>
@@ -26,7 +25,7 @@ void StatePassive::enter() {
         i.get().set_value(0);
     }
     for (auto i: ctrlComp_.joint_kd_command_interface_) {
-        i.get().set_value(3.5);
+        i.get().set_value(1);
     }
 }
 
@@ -38,7 +37,7 @@ void StatePassive::exit() {
 
 FSMStateName StatePassive::checkChange() {
     if (ctrlComp_.control_inputs_.get().command == 2) {
-        return FSMStateName::FIXEDSTAND;
+        return FSMStateName::FIXEDDOWN;
     }
     return FSMStateName::PASSIVE;
 }

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -51,7 +51,6 @@ namespace unitree_guide_controller {
 
             current_state_->enter();
             mode_ = FSMMode::NORMAL;
-            current_state_->run();
         }
 
         return controller_interface::return_type::OK;
@@ -85,6 +84,10 @@ namespace unitree_guide_controller {
                 ctrl_comp_.control_inputs_.get().rx = msg->rx;
                 ctrl_comp_.control_inputs_.get().ry = msg->ry;
             });
+
+        get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+        RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
+
         return CallbackReturn::SUCCESS;
     }
 
@@ -104,6 +107,7 @@ namespace unitree_guide_controller {
         }
 
         state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
+        state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);
         state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
 
         // Initialize FSM
@@ -143,6 +147,8 @@ namespace unitree_guide_controller {
                 return state_list_.invalid;
             case FSMStateName::PASSIVE:
                 return state_list_.passive;
+            case FSMStateName::FIXEDDOWN:
+                return state_list_.fixedDown;
             case FSMStateName::FIXEDSTAND:
                 return state_list_.fixedStand;
             // case FSMStateName::FREESTAND:

descriptions/go1_description/package.xml

@@ -7,6 +7,7 @@
     <maintainer email="laikago@unitree.cc">unitree</maintainer>
     <license>TODO</license>
 
+    <exec_depend>xacro</exec_depend>
     <exec_depend>joint_state_publisher</exec_depend>
     <exec_depend>robot_state_publisher</exec_depend>
     <exec_depend>imu_sensor_broadcaster</exec_depend>

descriptions/go2_description/launch/hardware.launch.py

@@ -36,7 +36,7 @@ def launch_setup(context, *args, **kwargs):
             name='robot_state_publisher',
             parameters=[
                 {
-                    'publish_frequency': 100.0,
+                    'publish_frequency': 20.0,
                     'use_tf_static': True,
                     'robot_description': robot_description,
                     'ignore_timestamp': True

descriptions/go2_description/package.xml

@@ -11,6 +11,7 @@
     <maintainer email="TODO@email.com"/>
     <license>BSD</license>
 
+    <exec_depend>xacro</exec_depend>
     <exec_depend>joint_state_publisher</exec_depend>
     <exec_depend>robot_state_publisher</exec_depend>
     <exec_depend>imu_sensor_broadcaster</exec_depend>

readme.md

@@ -1 +1,7 @@
 # Quadruped ROS2
+
+* rosdep
+```bash
+cd ~/ros2_ws
+rosdep install --from-paths src --ignore-src -r -y
+```