Merge pull request #3 from DeepRoboticsLab/test

fix timestamp bug in idle and standup states

Author: HaoKa1

src/M20_sdk_deploy/state_machine/quadruped_wheel/idle_state.hpp

@@ -21,8 +21,8 @@ namespace qw {
         bool first_enter_flag_ = true;
         VecXf joint_pos_, joint_vel_, joint_tau_;
         Vec3f rpy_, acc_, omg_;
-        float enter_state_time_ = 10000.;
-        float last_print_time = 0;
+        double enter_state_time_ = 10000.;
+        double last_print_time = 0;
 
         void GetProprioceptiveData() {
             joint_pos_ = ri_ptr_->GetJointPosition();

src/M20_sdk_deploy/state_machine/quadruped_wheel/standup_state.hpp

@@ -16,10 +16,10 @@ namespace qw{
 class StandUpState : public StateBase{
 private:
     VecXf init_joint_pos_, init_joint_vel_, current_joint_pos_, current_joint_vel_;
-    float time_stamp_record_, run_time_;
+    double time_stamp_record_, run_time_;
     VecXf goal_joint_pos_, kp_, kd_;
     MatXf joint_cmd_;
-    float stand_duration_ = 2.;
+    double stand_duration_ = 2.;
 
     const float init_hipx_pos_ = Deg2Rad(0.);
     const float set_wheel_kd_ = 1.;