initial commit

2026-10-22 09:23:54 +08:00
commit 63f557a39f
4448 changed files with 584487 additions and 0 deletions
@@ -0,0 +1,65 @@
+/**
+ * @file joint_damping_state.hpp
+ * @brief joint passive control state
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+#include "state_base.h"
+
+namespace qw{
+class JointDampingState : public StateBase{
+private:
+    float time_record_, run_time_;
+    MatXf joint_cmd_;
+public:
+    JointDampingState(const RobotName& robot_name, const std::string& state_name, 
+        std::shared_ptr<ControllerData> data_ptr):StateBase(robot_name, state_name, data_ptr){
+            if (robot_name_ == RobotName::FireAnt) {
+                VecXf kd_ = VecXf::Zero(8);
+                for (int leg = 0; leg < 4; ++leg) {
+                    kd_(2 * leg) = 2.0f;
+                    kd_(2 * leg + 1) = 0.6f;
+                }
+                joint_cmd_ = MatXf::Zero(8, 5);
+                joint_cmd_.col(2) = kd_;
+                return;
+            }
+            Vec4f one_leg_kd;
+            one_leg_kd << cp_ptr_->swing_leg_kd_, 1.;
+            VecXf kd_ = one_leg_kd.replicate(4, 1);
+            joint_cmd_ = MatXf::Zero(16, 5);
+            joint_cmd_.col(2) = kd_;
+        }
+    ~JointDampingState(){}
+
+    virtual void OnEnter() {
+        time_record_ = ri_ptr_->GetInterfaceTimeStamp();
+        run_time_ = ri_ptr_->GetInterfaceTimeStamp();
+        msfb_.UpdateCurrentState(RobotMotionState::JointDamping);
+    };
+    virtual void OnExit() {
+
+    }
+    virtual void Run() {
+        run_time_ = ri_ptr_->GetInterfaceTimeStamp();
+        ri_ptr_->SetJointCommand(joint_cmd_);
+    }
+    virtual bool LoseControlJudge() {
+        return false;
+    }
+    virtual StateName GetNextStateName() {
+        if(run_time_ - time_record_ < 3.){
+            return StateName::kJointDamping;
+        }
+        return StateName::kIdle;
+    }
+};
+
+
+};