66 lines
1.8 KiB
C++
66 lines
1.8 KiB
C++
/**
|
|
* @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;
|
|
}
|
|
};
|
|
|
|
|
|
};
|