still struggle with trot

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h

@@ -62,9 +62,9 @@ private:
     // Control Parameters
     double gait_height_;
     Vec3 pos_error_, vel_error_;
-    Mat3 Kpp, Kdp, Kdw;
+    Mat3 Kpp, Kdp, Kd_w_;
-    double _kpw;
+    double kp_w_;
-    Mat3 KpSwing, KdSwing;
+    Mat3 Kp_swing_, Kd_swing_;
     Vec2 _vxLim, _vyLim, _wyawLim;
 };
 

controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h

@@ -11,6 +11,7 @@
 
 #include "LowPassFilter.h"
 
+class WaveGenerator;
 class QuadrupedRobot;
 struct CtrlComponent;
 
@@ -106,10 +107,11 @@ public:
 private:
     CtrlComponent &ctrl_component_;
     QuadrupedRobot &robot_model_;
+    WaveGenerator &wave_generator_;
 
     Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
 
-    Eigen::Matrix<double, 3, 1> _u; // The input of estimator
+    Eigen::Matrix<double, 3, 1> u_; // The input of estimator
 
     Eigen::Matrix<double, 28, 1> _y; // The measurement value of output y
     Eigen::Matrix<double, 28, 1> y_hat_; // The prediction of output y

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h

@@ -5,14 +5,14 @@
 
 #ifndef WAVEGENERATOR_H
 #define WAVEGENERATOR_H
-#include <sys/time.h>
+#include <chrono>
 #include <unitree_guide_controller/common/enumClass.h>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 inline long long getSystemTime() {
-    timeval t{};
+    const auto now = std::chrono::system_clock::now();
-    gettimeofday(&t, nullptr);
+    const auto duration = now.time_since_epoch();
-    return 1000000 * t.tv_sec + t.tv_usec;
+    return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
 }
 
 class WaveGenerator {

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h

@@ -86,6 +86,7 @@ public:
     [[nodiscard]] std::vector<KDL::Vector> getFeet2BVelocities() const;
 
     double mass_ = 0;
+    Vec34 feet_pos_normal_stand_;
     std::vector<KDL::JntArray> current_joint_pos_;
     std::vector<KDL::JntArray> current_joint_vel_;
 

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -40,7 +40,7 @@ void StateBalanceTest::run() {
     pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
     pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
 
-    const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
+    const float yaw = - invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
     Rd_ = rotz(yaw) * init_rotation_;
 
     for (int i = 0; i < 12; i++) {

controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp

@@ -15,10 +15,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
     gait_height_ = 0.08;
     Kpp = Vec3(70, 70, 70).asDiagonal();
     Kdp = Vec3(10, 10, 10).asDiagonal();
-    _kpw = 780;
+    kp_w_ = 780;
-    Kdw = Vec3(70, 70, 70).asDiagonal();
+    Kd_w_ = Vec3(70, 70, 70).asDiagonal();
-    KpSwing = Vec3(400, 400, 400).asDiagonal();
+    Kp_swing_ = Vec3(300,300,300).asDiagonal();
-    KdSwing = Vec3(10, 10, 10).asDiagonal();
+    Kd_swing_ = Vec3(10, 10, 10).asDiagonal();
 
     _vxLim << -0.4, 0.4;
     _vyLim << -0.3, 0.3;
@@ -28,9 +28,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
 
 void StateTrotting::enter() {
     pcd_ = estimator_.getPosition();
+    pcd_(2) = -robot_model_.feet_pos_normal_stand_(2, 0);
     _vCmdBody.setZero();
     _yawCmd = estimator_.getYaw();
-    Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
+    Rd = rotz(_yawCmd);
     _wCmdGlobal.setZero();
 
     ctrl_comp_.control_inputs_.get().command = 0;
@@ -107,7 +108,7 @@ void StateTrotting::calcCmd() {
 
     /* Turning */
     _yawCmd = _yawCmd + _dYawCmd * dt_;
-    Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
+    Rd = rotz(_yawCmd);
     _wCmdGlobal(2) = _dYawCmd;
 }
 
@@ -116,8 +117,8 @@ void StateTrotting::calcTau() {
     vel_error_ = vel_target_ - vel_body_;
 
     Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_;
-    Vec3 d_wbd = _kpw * rotMatToExp(Rd * G2B_RotMat) +
+    Vec3 d_wbd = kp_w_ * rotMatToExp(Rd * G2B_RotMat) +
-                 Kdw * (_wCmdGlobal - estimator_.getGlobalGyro());
+                 Kd_w_ * (_wCmdGlobal - estimator_.getGlobalGyro());
 
     dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3));
     dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3));
@@ -127,9 +128,9 @@ void StateTrotting::calcTau() {
     d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40));
     d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10));
 
-    const Vec34 pos_feet2_b_global = estimator_.getFeetPos2Body();
+    const Vec34 pos_feet_body_global = estimator_.getFeetPos2Body();
     Vec34 force_feet_global =
-            -balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet2_b_global, wave_generator_.contact_);
+            -balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_.contact_);
 
 
     Vec34 pos_feet_global = estimator_.getFeetPos();
@@ -137,8 +138,8 @@ void StateTrotting::calcTau() {
     for (int i(0); i < 4; ++i) {
         if (wave_generator_.contact_(i) == 0) {
             force_feet_global.col(i) =
-                    KpSwing * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
+                    Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
-                    KdSwing * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
+                    Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
         }
     }
 
@@ -154,19 +155,19 @@ void StateTrotting::calcTau() {
 }
 
 void StateTrotting::calcQQd() {
-    const std::vector<KDL::Frame> pos_feet2_b = robot_model_.getFeet2BPositions();
+    const std::vector<KDL::Frame> pos_feet_body = robot_model_.getFeet2BPositions();
 
-    Vec34 pos_feet2_b_goal, vel_feet2_b_goal;
+    Vec34 pos_feet_target, vel_feet_target;
     for (int i(0); i < 4; ++i) {
-        pos_feet2_b_goal.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_);
+        pos_feet_target.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_);
-        vel_feet2_b_goal.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
+        vel_feet_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
-        // _velFeet2BGoal.col(i) = _G2B_RotMat * (_velFeetGlobalGoal.col(i) -
+        // vel_feet2_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_ - B2G_RotMat * skew(
-        // _velBody - _B2G_RotMat * (skew(_lowState->getGyro()) * _posFeet2B.col(i))
+        //                                             estimator_.getGyro()) * Vec3(pos_feet_body[i].p.data)
-        // );  //  c.f formula (6.12)
+        //                           ); //  c.f formula (6.12)
     }
 
-    Vec12 q_goal = robot_model_.getQ(pos_feet2_b_goal);
+    Vec12 q_goal = robot_model_.getQ(pos_feet_target);
-    Vec12 qd_goal = robot_model_.getQd(pos_feet2_b, vel_feet2_b_goal);
+    Vec12 qd_goal = robot_model_.getQd(pos_feet_body, vel_feet_target);
     for (int i = 0; i < 12; i++) {
         ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
         ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
@@ -176,11 +177,13 @@ void StateTrotting::calcQQd() {
 void StateTrotting::calcGain() const {
     for (int i(0); i < 4; ++i) {
         if (wave_generator_.contact_(i) == 0) {
+            // swing gain
             for (int j = 0; j < 3; j++) {
                 ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
                 ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
             }
         } else {
+            // stable gain
             for (int j = 0; j < 3; j++) {
                 ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
                 ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -9,7 +9,8 @@
 #include "unitree_guide_controller/control/CtrlComponent.h"
 
 Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
-                                                      robot_model_(ctrl_component.robot_model_) {
+                                                      robot_model_(ctrl_component.robot_model_),
+                                                      wave_generator_(ctrl_component.wave_generator_) {
     g_ << 0, 0, -9.81;
     _dt = 0.002;
     _largeVariance = 100;
@@ -18,7 +19,7 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
     }
 
     x_hat_.setZero();
-    _u.setZero();
+    u_.setZero();
 
     A.setZero();
     A.block(0, 0, 3, 3) = I3;
@@ -155,19 +156,16 @@ void Estimator::update() {
     foot_vels_ = robot_model_.getFeet2BVelocities();
     _feetH.setZero();
 
-    const std::vector contact(4, 1);
-    const std::vector phase(4, 0.5);
-
     // Adjust the covariance based on foot contact and phase.
     for (int i(0); i < 4; ++i) {
-        if (contact[i] == 0) {
+        if (wave_generator_.contact_[i] == 0) {
             // foot not contact
             Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
             R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
             R(24 + i, 24 + i) = _largeVariance;
         } else {
             // foot contact
-            const double trust = windowFunc(phase[i], 0.2);
+            const double trust = windowFunc(wave_generator_.phase_[i], 0.2);
             Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
                     (1 + (1 - trust) * _largeVariance) *
                     QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);
@@ -209,8 +207,8 @@ void Estimator::update() {
     //                             ctrl_component_.imu_state_interface_[8].get().get_value(),
     //                             ctrl_component_.imu_state_interface_[9].get().get_value());
 
-    _u = rotation_ * acceleration_ + g_;
+    u_ = rotation_ * acceleration_ + g_;
-    x_hat_ = A * x_hat_ + B * _u;
+    x_hat_ = A * x_hat_ + B * u_;
     y_hat_ = C * x_hat_;
 
     // Update the measurement value

controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp

@@ -18,7 +18,8 @@ void FeetEndCtrl::init() {
     t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
     t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
 
-    Vec34 feet_pos_body = estimator_.getFeetPos2Body();
+    // Vec34 feet_pos_body = estimator_.getFeetPos2Body();
+    Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_;
     for (int i(0); i < 4; ++i) {
         feet_radius_(i) =
                 sqrt(pow(feet_pos_body(0, i), 2) + pow(feet_pos_body(1, i), 2));

controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp

@@ -8,10 +8,11 @@
 
 #include "unitree_guide_controller/control/CtrlComponent.h"
 
-GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : estimator_(ctrl_component.estimator_),
+GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : wave_generator_(ctrl_component.wave_generator_),
-                                                              feet_end_ctrl_(ctrl_component),
+                                                              estimator_(ctrl_component.estimator_),
-                                                              wave_generator_(ctrl_component.wave_generator_) {
+                                                              feet_end_ctrl_(ctrl_component) {
     first_run_ = true;
+    feet_end_ctrl_.init();
 }
 
 void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const double gait_height) {
@@ -28,8 +29,8 @@ void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
 
     for (int i = 0; i < 4; i++) {
         if (wave_generator_.contact_(i) == 1) {
-            // foot contact the ground
             if (wave_generator_.phase_(i) < 0.5) {
+                // foot contact the ground
                 start_p_.col(i) = estimator_.getFootPos(i);
             }
             feet_pos.col(i) = start_p_.col(i);
@@ -46,9 +47,9 @@ void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
 void GaitGenerator::restart() {
     first_run_ = true;
     vxy_goal_.setZero();
-    feet_end_ctrl_.init();
 }
 
+
 Vec3 GaitGenerator::getFootPos(const int i) {
     Vec3 foot_pos;
 

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -31,6 +31,8 @@ void QuadrupedRobot::init(const std::string &robot_description) {
     for (const auto &[fst, snd]: robot_tree.getSegments()) {
         mass_ += snd.segment.getInertia().getMass();
     }
+    feet_pos_normal_stand_ << 0.1881, 0.1881, -0.1881, -0.1881, -0.1300, 0.1300,
+            -0.1300, 0.1300, -0.3200, -0.3200, -0.3200, -0.3200;
 }
 
 std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list) const {

descriptions/go1_description/config/robot_control.yaml

@@ -1,9 +1,62 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 200  # Hz
+    update_rate: 500  # Hz
-    use_sim_time: true  # If running in simulation
 
     # Define the available controllers
     joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 500  # Hz
+    joints:
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
+      - FL_hip_joint
+      - FL_thigh_joint
+      - FL_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    imu_name: "imu_sensor"
+
+    imu_interfaces:
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z

descriptions/go1_description/launch/hardware.launch.py

@@ -3,8 +3,8 @@ import os
 import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.event_handlers import OnProcessExit
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -29,43 +29,68 @@ def launch_setup(context, *args, **kwargs):
             "robot_control.yaml",
         ]
     )
-    return [
-        Node(
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            name='robot_state_publisher',
-            parameters=[
-                {
-                    'publish_frequency': 20.0,
-                    'use_tf_static': True,
-                    'robot_description': robot_description,
-                    'ignore_timestamp': True
-                }
-            ],
-        ),
-        Node(
-            package="controller_manager",
-            executable="ros2_control_node",
-            parameters=[robot_controllers],
-            remappings=[
-                ("~/robot_description", "/robot_description"),
-            ],
-            output="both",
-        ),
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=["joint_state_broadcaster",
-                       "--controller-manager", "/controller_manager"],
-        ),
-        # Node(
-        #     package="controller_manager",
-        #     executable="spawner",
-        #     arguments=["imu_sensor_broadcaster",
-        #                "--controller-manager", "/controller_manager"],
-        # ),
-    ]
 
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+        ],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[unitree_guide_controller],
+            )
+        ),
+    ]
 
 def generate_launch_description():
     robot_type_arg = DeclareLaunchArgument(

descriptions/go1_description/xacro/ros2_control.xacro

@@ -151,6 +151,19 @@
             <state_interface name="effort"/>
         </joint>
 
+        <sensor name="imu_sensor">
+            <state_interface name="orientation.w"/>
+            <state_interface name="orientation.x"/>
+            <state_interface name="orientation.y"/>
+            <state_interface name="orientation.z"/>
+            <state_interface name="angular_velocity.x"/>
+            <state_interface name="angular_velocity.y"/>
+            <state_interface name="angular_velocity.z"/>
+            <state_interface name="linear_acceleration.x"/>
+            <state_interface name="linear_acceleration.y"/>
+            <state_interface name="linear_acceleration.z"/>
+        </sensor>
+
     </ros2_control>
 
 </robot>