系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对四足机器人步态状态FSM管理：电机失能模式、躯干翻身\收腿\恢复站立步态、WBC平衡站立步态、站立过程步态、对角小跑的运动步态做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
一、电机失能模式SM_State_Passive
功能
在进入该状态时把腿部控制器的所有控制量都置为0，然后正常进入失能运行状态启动电机失能，关节电机没有可控制指令，机械狗就趴下来了
.
.
1.onEnter（）【进入该状态时要执行的行为】
/**
*功能：进入状态时要执行的行为
*/
template <typename T>
void FSM_State_Passive<T>::onEnter()
{
this->nextStateName = this->stateName; //默认是不过渡
this->transitionData.zero(); //重置转换数据
}
.
.
2.run()【运行该状态的正常行为】
/**
* 功能：调用要在每个控制循环迭代中执行的函数
*/
template <typename T>
void FSM_State_Passive<T>::run()
{
testTransition(); //什么都不做，所有的命令都应该以0开头
}
/**
* 功能：处理机器人在状态之间的实际转换。在转换完成时返回true。
* @return 如果转换完成，则为true
*/
template <typename T>
TransitionData<T> FSM_State_Passive<T>::testTransition()
{
this->transitionData.done = true;
return this->transitionData;
}
.
.
3.checkTransition()【检查是否有触发状态转换器–用于状态检查】
/**
* 功能：状态转移检查：管理哪些状态可以由用户命令或状态事件触发器切换
* @return 要转换为的枚举FSM状态名
*/
template <typename T>
FSM_StateName FSM_State_Passive<T>::checkTransition()
{
this->nextStateName = this->stateName; //把这一次的状态名称赋值给下一次的状态名称
iter++;
switch ((int)this->_data->controlParameters->control_mode) //切换FSM控制模式
{
case K_PASSIVE: //基于状态的转换的正常操作
break;
case K_JOINT_PD:
this->nextStateName = FSM_StateName::JOINT_PD;
break;
case K_STAND_UP:
this->nextStateName = FSM_StateName::STAND_UP;
break;
case K_RECOVERY_STAND:
this->nextStateName = FSM_StateName::RECOVERY_STAND;
break;
default:
std::cout << "[CONTROL FSM] Bad Request: Cannot transition from "
<< K_PASSIVE << " to "
<< this->_data->controlParameters->control_mode << std::endl;
}
return this->nextStateName; //得到下一个状态
}
.
.
4.transition（）【管理特定于状态的转换–用于状态转换】
/**
*功能：状态转移， 处理机器人在状态之间的实际转换
* @return 如果转换完成，则为true
*/
template <typename T>
TransitionData<T> FSM_State_Passive<T>::transition()
{
this->transitionData.done = true; //完成过渡
return this->transitionData; //将转换数据返回到FSM
}
.
.
5.onExit()【退出该状态时要执行的行为】
/**
*在退出状态时清除状态信息。
*/
template <typename T>
void FSM_State_Passive<T>::onExit()
{
//退出时无需清理
}
.
.
二、躯干翻身、收腿、恢复站立状态FSM_State_RecoveryStand
功能
四足机器人摔倒时，实现先把腿部折叠起来，然后躯干翻身、恢复站立的过程
.
.
1.onEnter（）【进入该状态时要执行的行为】
（1）构造函数：定义所有相关的初始化关节位置控制数据
/**
*功能：向传递状态特定信息的FSM状态的构造函数
* @param _controlFSMData 定义所有相关的初始化关节位置控制数据
*/
template <typename T>
FSM_State_RecoveryStand<T>::FSM_State_RecoveryStand(ControlFSMData<T>* _controlFSMData)
: FSM_State<T>(_controlFSMData, FSM_StateName::STAND_UP, "STAND_UP")
{
this->checkSafeOrientation = false;//设置预控安全检查
this->checkPDesFoot = false; //位置控制安全检查
this->checkForceFeedForward = false;
zero_vec3.setZero();
/*目标配置*/
//（1）匍匐状态的关节位置的初始参数
fold_jpos[0] << -0.0f, -1.4f, 2.7f;
fold_jpos[1] << 0.0f, -1.4f, 2.7f;
fold_jpos[2] << -0.0f, -1.4f, 2.7f;
fold_jpos[3] << 0.0f, -1.4f, 2.7f;
//（2）站力状态的关节位置的初始参数
for(size_t i(0); i<4; ++i)
{
stand_jpos[i] << 0.f, -.8f, 1.6f;
}
//（3）滚动状态的关节位置的初始参数
rolling_jpos[0] << 1.5f, -1.6f, 2.77f;
rolling_jpos[1] << 1.3f, -3.1f, 2.77f;
rolling_jpos[2] << 1.5f, -1.6f, 2.77f;
rolling_jpos[3] << 1.3f, -3.1f, 2.77f;
f_ff << 0.f, 0.f, -25.f;
}
/**
*功能：进入状态时要执行的行为
*/
template <typename T>
void FSM_State_RecoveryStand<T>::onEnter()
{
this->nextStateName = this->stateName;//默认为不过渡
this->transitionData.zero(); //重置转换数据
iter = 0; //重置迭代计数器
_state_iter = 0;
for(size_t i(0); i < 4; ++i) //初始配置，位置
{
initial_jpos[i] = this->_data->_legController->datas[i].q;
}
T body_height = this->_data->_stateEstimator->getResult().position[2];
_flag = FoldLegs;
if( !_UpsideDown() ) //正确定位
{
if ( (0.2 < body_height) && (body_height < 0.45) )
{
printf("[Recovery Balance] body height is %f; Stand Up \n", body_height);
_flag = StandUp;
}
else
{
printf("[Recovery Balance] body height is %f; Folding legs \n", body_height);
}
}
else //不正确定位
{
printf("[Recovery Balance] UpsideDown (%d) \n", _UpsideDown() );
}
_motion_start_iter = 0;
}
判断躯干位姿是否正确
/**
*功能：判断是否正确定位
*/
template <typename T>
bool FSM_State_RecoveryStand<T>::_UpsideDown()
{
if(this->_data->_stateEstimator->getResult().rBody(2,2) < 0)
{
return true;
}
return false;
}
.
.
2.run()【运行该状态的正常行为】
/**
*功能：调用要在每个控制循环迭代中执行的函数。
*/
template <typename T>
void FSM_State_RecoveryStand<T>::run()
{
switch(_flag)
{
case StandUp://（3）从匍匐状态转换成站立状态
_StandUp(_state_iter - _motion_start_iter);
break;
case FoldLegs://（1）折叠腿部
_FoldLegs(_state_iter - _motion_start_iter);
break;
case RollOver://（2）翻身动作
_RollOver(_state_iter - _motion_start_iter);
break;
}
++_state_iter;
}
（1）滚动翻身过程运动函数
/**
*功能：滚动翻身过程运动函数
*/
template <typename T>
void FSM_State_RecoveryStand<T>::_RollOver(const int & curr_iter)
{
for(size_t i(0); i<4; ++i)
{
_SetJPosInterPts(curr_iter, rollover_ramp_iter, i,
initial_jpos[i], rolling_jpos[i]);
}
if(curr_iter > rollover_ramp_iter + rollover_settle_iter)
{
_flag = FoldLegs;
for(size_t i(0); i<4; ++i) initial_jpos[i] = rolling_jpos[i];
_motion_start_iter = _state_iter+1;
}
}
（2）把四条腿折叠起来动作函数
/**
*功能：把四条腿折叠起来动作函数
*/
template <typename T>
void FSM_State_RecoveryStand<T>::_FoldLegs(const int & curr_iter)
{
for(size_t i(0); i<4; ++i) //设置四个关节位置，计算关节位置，并执行控制
{
_SetJPosInterPts(curr_iter, fold_ramp_iter, i,
initial_jpos[i], fold_jpos[i]);
}
if(curr_iter >= fold_ramp_iter + fold_settle_iter)//如果现在折叠的角度过大
{
if(_UpsideDown())//向上倒
{
_flag = RollOver;
for(size_t i(0); i<4; ++i) initial_jpos[i] = fold_jpos[i];
}
else //向下倒
{
_flag = StandUp;
for(size_t i(0); i<4; ++i) initial_jpos[i] = fold_jpos[i];
}
_motion_start_iter = _state_iter + 1;
}
}
（3）从收腿到恢复站立过程运动函数
/**
*功能：从匍匐到恢复站立过程运动函数
*/
template <typename T>
void FSM_State_RecoveryStand<T>::_StandUp(const int & curr_iter)
{
T body_height = this->_data->_stateEstimator->getResult().position[2];
bool something_wrong(false);
if( _UpsideDown() || (body_height < 0.1 ) )
{
something_wrong = true;
}
if( (curr_iter > floor(standup_ramp_iter*0.7) ) && something_wrong)//如果因为某种原因身体高度太低，即使站起来的动作快结束了，当紧急停止处于其他状态中间时可能发生
{
for(size_t i(0); i < 4; ++i)
{
initial_jpos[i] = this->_data->_legController->datas[i].q;
}
_flag = FoldLegs;
_motion_start_iter = _state_iter+1;
printf("[Recovery Balance - Warning] body height is still too low (%f) or UpsideDown (%d); Folding legs \n",
body_height, _UpsideDown() );
}
else //正常情况
{
for(size_t leg(0); leg<4; ++leg)
{
_SetJPosInterPts(curr_iter, standup_ramp_iter,
leg, initial_jpos[leg], stand_jpos[leg]);
}
}
//机器人的前馈质量。
//for(int i = 0; i < 4; i++)
//this->_data->_legController->commands[i].forceFeedForward = f_ff;
Vec4<T> se_contactState(0.5,0.5,0.5,0.5);
this->_data->_stateEstimator->setContactPhase(se_contactState);
}
（4）计算关节位置，并执行控制
/**
*功能：计算关节位置，并执行控制
*/
template <typename T>
void FSM_State_RecoveryStand<T>::_SetJPosInterPts(
const size_t & curr_iter, size_t max_iter, int leg,
const Vec3<T> & ini, const Vec3<T> & fin)
{
float a(0.f);
float b(1.f);
if(curr_iter <= max_iter) //如果我们完成插值
{
b = (float)curr_iter/(float)max_iter;
a = 1.f - b;
}
Vec3<T> inter_pos = a * ini + b * fin; //计算设定值
this->jointPDControl(leg, inter_pos, zero_vec3); //执行控制
}
.
.
3.checkTransition()【检查是否有触发状态转换器–用于状态检查】
/**
* 功能：状态转移检查：管理哪些状态可以由用户命令或状态事件触发器切换
* @return 要转换为的枚举FSM状态名
*/
template <typename T>
FSM_StateName FSM_State_RecoveryStand<T>::checkTransition()
{
this->nextStateName = this->stateName; //把这一次的状态名称赋值给下一次的状态名称
iter++;
switch ((int)this->_data->controlParameters->control_mode)//切换FSM控制模式
{
case K_RECOVERY_STAND:
break;
case K_LOCOMOTION:
this->nextStateName = FSM_StateName::LOCOMOTION;
break;
case K_PASSIVE:
this->nextStateName = FSM_StateName::PASSIVE;
break;
case K_BALANCE_STAND:
this->nextStateName = FSM_StateName::BALANCE_STAND;
break;
case K_BACKFLIP:
this->nextStateName = FSM_StateName::BACKFLIP;
break;
case K_FRONTJUMP:
this->nextStateName = FSM_StateName::FRONTJUMP;
break;
case K_VISION:
this->nextStateName = FSM_StateName::VISION;
break;
default:
std::cout << "[CONTROL FSM] Bad Request: Cannot transition from "
<< K_RECOVERY_STAND << " to "
<< this->_data->controlParameters->control_mode << std::endl;
}
return this->nextStateName;//获取下一个状态
}
.
.
4.transition（）【管理特定于状态的转换–用于状态转换】
/**
*功能：状态转移， 处理机器人在状态之间的实际转换
* @return 如果转换完成，则为true
*/
template <typename T>
TransitionData<T> FSM_State_RecoveryStand<T>::transition()
{
//完成转换
switch (this->nextStateName)
{
case FSM_StateName::PASSIVE:
this->transitionData.done = true;
break;
case FSM_StateName::BALANCE_STAND:
this->transitionData.done = true;
break;
case FSM_StateName::LOCOMOTION:
this->transitionData.done = true;
break;
case FSM_StateName::BACKFLIP:
this->transitionData.done = true;
break;
case FSM_StateName::FRONTJUMP:
this->transitionData.done = true;
break;
case FSM_StateName::VISION:
this->transitionData.done = true;
break;
default:
std::cout << "[CONTROL FSM] Something went wrong in transition"
<< std::endl;
}
return this->transitionData;//将转换数据返回到FSM
}
.
.
5.onExit()【退出该状态时要执行的行为】
/**
* 功能：在退出状态时清除状态信息。
*/
template <typename T>
void FSM_State_RecoveryStand<T>::onExit()
{
//退出时无需清理
}
.
.
三、WBC平衡站立步态FSM_State_BalanceStand
功能
四足机器人平衡站立，使用了wbc的全身控制方式
.
1.onEnter（）【进入该状态时要执行的行为】
/*
* 功能：向传递状态特定信息的FSM状态的构造函数 通用FSM状态构造函数
* @param _controlFSMData 保存所有相关的控制数据
*/
template <typename T>
FSM_State_BalanceStand<T>::FSM_State_BalanceStand(
ControlFSMData<T>* _controlFSMData)
: FSM_State<T>(_controlFSMData, FSM_StateName::BALANCE_STAND,"BALANCE_STAND")
{
this->turnOnAllSafetyChecks(); //设置预控安全检查
this->checkPDesFoot = false; //关闭脚踏pos命令，因为它在WBC中设置为操作任务
this->footFeedForwardForces = Mat34<T>::Zero(); //将GRF初始化为0s
_wbc_ctrl = new LocomotionCtrl<T>(_controlFSMData->_quadruped->buildModel());
_wbc_data = new LocomotionCtrlData<T>();
_wbc_ctrl->setFloatingBaseWeight(1000.);
}
/*
* 功能：进入站立平衡状态时要执行的行为
*/
template <typename T>
void FSM_State_BalanceStand<T>::onEnter()
{
this->nextStateName = this->stateName;//默认为不过渡
this->transitionData.zero();//重置转换数据
this->_data->_gaitScheduler->gaitData._nextGait = GaitType::STAND;//始终将步态设置为站在这种状态
_ini_body_pos = (this->_data->_stateEstimator->getResult()).position;
if(_ini_body_pos[2] < 0.2)
{
_ini_body_pos[2] = 0.3;
}
last_height_command = _ini_body_pos[2];
_ini_body_ori_rpy = (this->_data->_stateEstimator->getResult()).rpy;
_body_weight = this->_data->_quadruped->_bodyMass * 9.81;
}
.
.
2.run()【运行该状态的正常行为】
/**
* 功能：调用要在每个控制循环迭代中执行的函数。
*/
template <typename T>
void FSM_State_BalanceStand<T>::run()
{
Vec4<T> contactState;
contactState<< 0.5, 0.5, 0.5, 0.5; //四条腿都要接触
this->_data->_stateEstimator->setContactPhase(contactState);
BalanceStandStep();
}
/**
* 功能：计算平衡站立的时候，每只脚的腿部控制器的命令。
*/
template <typename T>
void FSM_State_BalanceStand<T>::BalanceStandStep()
{
_wbc_data->pBody_des = _ini_body_pos; //初始化躯干位置
_wbc_data->vBody_des.setZero(); //设置躯干的线速度为0
_wbc_data->aBody_des.setZero(); //设置躯干的加速度为0
_wbc_data->pBody_RPY_des = _ini_body_ori_rpy; //初始化躯干rpy方向
//计算躯干的方向和高度
if(this->_data->controlParameters->use_rc) //若在使用手柄，计算躯干的方向和高度
{
const rc_control_settings* rc_cmd = this->_data->_desiredStateCommand->rcCommand;
//使用手柄时方向计算
_wbc_data->pBody_RPY_des[0] = rc_cmd->rpy_des[0]*1.4;
_wbc_data->pBody_RPY_des[1] = rc_cmd->rpy_des[1]*0.46;
_wbc_data->pBody_RPY_des[2] -= rc_cmd->rpy_des[2];
//使用手柄时高度计算
_wbc_data->pBody_des[2] += 0.12 * rc_cmd->height_variation;
}
else //若不使用手柄，计算躯干的方向和高度
{
//不使用手柄时方向计算
_wbc_data->pBody_RPY_des[0] =
0.6* this->_data->_desiredStateCommand->gamepadCommand->leftStickAnalog[0];
_wbc_data->pBody_RPY_des[1] =
0.6*this->_data->_desiredStateCommand->gamepadCommand->rightStickAnalog[0];
_wbc_data->pBody_RPY_des[2] -=
this->_data->_desiredStateCommand->gamepadCommand->rightStickAnalog[1];
//不使用手柄时高度
_wbc_data->pBody_des[2] +=
0.12 * this->_data->_desiredStateCommand->gamepadCommand->rightStickAnalog[0];
}
_wbc_data->vBody_Ori_des.setZero(); //设置躯干线速度为0
for(size_t i(0); i<4; ++i) //设置每条腿的参数
{
_wbc_data->pFoot_des[i].setZero(); //脚的位置设置为0
_wbc_data->vFoot_des[i].setZero(); //脚的速度设置为0
_wbc_data->aFoot_des[i].setZero(); //脚的加速度设置为0
_wbc_data->Fr_des[i].setZero(); //
_wbc_data->Fr_des[i][2] = _body_weight/4.;
_wbc_data->contact_state[i] = true;
}
if(this->_data->_desiredStateCommand->trigger_pressed)
{
_wbc_data->pBody_des[2] = 0.05;
if(last_height_command - _wbc_data->pBody_des[2] > 0.001)
{
_wbc_data->pBody_des[2] = last_height_command - 0.001;
}
}
last_height_command = _wbc_data->pBody_des[2];
_wbc_ctrl->run(_wbc_data, *this->_data);
}
.
.
3.checkTransition()【检查是否有触发状态转换器–用于状态检查】
/**
* 功能：管理可以由用户命令或状态事件触发器转换为哪些状态。
* @return 要转换为的枚举FSM状态名
*/
template <typename T>
FSM_StateName FSM_State_BalanceStand<T>::checkTransition()
{
_iter++;//获取下一个状态
switch ((int)this->_data->controlParameters->control_mode) //切换FSM控制模式
{
case K_BALANCE_STAND:
//基于状态的转换的正常操作
//需要一个工作状态估计器
/*
if (velocity > v_max)
{
this->nextStateName = FSM_StateName::LOCOMOTION; //通知该状态下一个状态即将到来
this->transitionDuration = 0.0; //根据要求即时转换到移动状态
this->_data->_gaitScheduler->gaitData._nextGait = GaitType::TROT;//将调度程序中的下一步设置为
}*/
//测试：就地显示非用户请求的自动转换
/*if (_iter >= 5458)
{
this->nextStateName = FSM_StateName::LOCOMOTION;
this->_data->controlParameters->control_mode = K_LOCOMOTION;
this->transitionDuration = 0.0;
this->_data->_gaitScheduler->gaitData._nextGait =
GaitType::AMBLE; // TROT; // Or get whatever is in
// main_control_settings
_iter = 0;
}
*/
break;
case K_LOCOMOTION:
this->nextStateName = FSM_StateName::LOCOMOTION; //要求更改平衡架
this->transitionDuration = 0.0; // 根据要求即时转换到移动状态
this->_data->_gaitScheduler->gaitData._nextGait = GaitType::TROT; //将调度程序中的下一步设置为
break;
case K_PASSIVE:
this->nextStateName = FSM_StateName::PASSIVE;
this->transitionDuration = 0.0; //过渡时间很快
break;
case K_VISION:
this->nextStateName = FSM_StateName::VISION;
this->transitionDuration = 0.0; //过渡时间很快
break;
case K_RECOVERY_STAND:
this->nextStateName = FSM_StateName::RECOVERY_STAND;
this->transitionDuration = 0.0; //过渡时间很快
break;
case K_BACKFLIP:
this->nextStateName = FSM_StateName::BACKFLIP;
this->transitionDuration = 0.;
break;
default:
std::cout << "[CONTROL FSM] Bad Request: Cannot transition from "
<< K_BALANCE_STAND << " to "
<< this->_data->controlParameters->control_mode << std::endl;
}
return this->nextStateName; //将下一个状态名返回给FSM
}
.
.
4.transition（）【管理特定于状态的转换–用于状态转换】
/**
*功能：处理robot在状态之间的实际转换。
* @return 如果转换完成，则为true
*/
template <typename T>
TransitionData<T> FSM_State_BalanceStand<T>::transition()
{
switch (this->nextStateName) //切换FSM控制模式
{
case FSM_StateName::LOCOMOTION: //平衡站立模式
BalanceStandStep();
_iter++;
if (_iter >= this->transitionDuration * 1000)
{
this->transitionData.done = true;
}
else
{
this->transitionData.done = false;
}
break;
case FSM_StateName::PASSIVE: //人为调试模式
this->turnOffAllSafetyChecks();
this->transitionData.done = true;
break;
case FSM_StateName::RECOVERY_STAND: //恢复站立模式
this->transitionData.done = true;
break;
case FSM_StateName::BACKFLIP: //后空翻模式
this->transitionData.done = true;
break;
case FSM_StateName::VISION: //视觉模式
this->transitionData.done = true;
break;
default:
std::cout << "[CONTROL FSM] Something went wrong in transition"
<< std::endl;
}
return this->transitionData; //将转换数据返回到FSM
}
.
.
5.onExit()【退出该状态时要执行的行为】
/**
* 功能：在退出状态时清除状态信息。
*/
template <typename T>
void FSM_State_BalanceStand<T>::onExit()
{
_iter = 0;
}
四、站立过程步态FSM_State_StandUp
功能
四足机器人从匍匐的状态转换成站立的状态过程，然后保持四足站立的状态不动（该四足站立没有抗干扰的功能）
.
.
1.onEnter（）【进入该状态时要执行的行为】
使用腿部控制器反馈当前腿部关节的位置来设置站立步态的初始位置状态
/**
*功能：进入状态时要执行的行为
*/
template <typename T>
void FSM_State_StandUp<T>::onEnter()
{
this->nextStateName = this->stateName; //默认为不转换
this->transitionData.zero(); //重置转换数据
iter = 0; //重置迭代计数器
for(size_t leg(0); leg<4; ++leg) //设置4个腿的位置
{
_ini_foot_pos[leg] = this->_data->_legController->datas[leg].p;
}
}
.
.
2.run()【运行该状态的正常行为】
原理
- 给定站立的高度，根据运动控制周期把站立过程的占空比不断增大，直到完成站立状态（占空比为1）
- 给定增益Kp/Kd参数，根据占空比计算脚的z位置，按照控制周期，把脚的控制参数发送给腿部控制器执行
/**
* 调用要在每个控制循环迭代中执行的函数。
* 原理
* 给定站立的高度，根据运动控制周期把站立过程的占空比不断增大，直到完成站立状态（占空比为1）
* 给定增益Kp/Kd参数，根据占空比计算脚的z位置，按照控制周期，把脚的控制参数发送给腿部控制器执行
*/
template <typename T>
void FSM_State_StandUp<T>::run()
{
if(this->_data->_quadruped->_robotType == RobotType::MINI_CHEETAH) //仅仅只有MINI_CHEETAH
{
T hMax = 0.25;//站立高度
T progress = 2 * iter * this->_data->controlParameters->controller_dt;
if (progress > 1.) //站立高度过程占空比限幅
{ progress = 1.; }
for(int i = 0; i < 4; i++) //更新腿的信息
{
this->_data->_legController->commands[i].kpCartesian = Vec3<T>(500, 500, 500).asDiagonal(); //Kp返回参数
this->_data->_legController->commands[i].kdCartesian = Vec3<T>(8, 8, 8).asDiagonal(); //Kd返回参数
this->_data->_legController->commands[i].pDes = _ini_foot_pos[i]; //脚的位置x不变
this->_data->_legController->commands[i].pDes[2] = //脚的位置z高度，按照周期不断变高
progress*(-hMax) + (1. - progress) * _ini_foot_pos[i][2];
}
}
}
.
.
3.checkTransition()【检查是否有触发状态转换器–用于状态检查】
/**
* 功能：状态转移检查：管理哪些状态可以由用户命令或状态事件触发器切换
* @return 要转换为的枚举FSM状态名
*/
template <typename T>
FSM_StateName FSM_State_StandUp<T>::checkTransition()
{
this->nextStateName = this->stateName;//把这一次的状态名称赋值给下一次的状态名称
iter++;
switch ((int)this->_data->controlParameters->control_mode) //切换FSM控制模式
{
case K_STAND_UP:
break;
case K_BALANCE_STAND:
this->nextStateName = FSM_StateName::BALANCE_STAND;
break;
case K_LOCOMOTION:
this->nextStateName = FSM_StateName::LOCOMOTION;
break;
case K_VISION:
this->nextStateName = FSM_StateName::VISION;
break;
case K_PASSIVE:
this->nextStateName = FSM_StateName::PASSIVE;
break;
default:
std::cout << "[CONTROL FSM] Bad Request: Cannot transition from "
<< K_PASSIVE << " to "
<< this->_data->controlParameters->control_mode << std::endl;
}
return this->nextStateName; //获取下一个状态
}
.
.
4.transition（）【管理特定于状态的转换–用于状态转换】
/**
*功能：状态转移， 处理机器人在状态之间的实际转换
* @return 如果转换完成，则为true
*/
template <typename T>
TransitionData<T> FSM_State_StandUp<T>::transition()
{
switch (this->nextStateName) //完成转换
{
case FSM_StateName::PASSIVE:
this->transitionData.done = true;
break;
case FSM_StateName::BALANCE_STAND:
this->transitionData.done = true;
break;
case FSM_StateName::LOCOMOTION:
this->transitionData.done = true;
break;
case FSM_StateName::VISION:
this->transitionData.done = true;
break;
default:
std::cout << "[CONTROL FSM] Something went wrong in transition"
<< std::endl;
}
return this->transitionData;//将转换数据返回到FSM
}
.
.
5.onExit()【退出该状态时要执行的行为】
/**
* 功能：在退出状态时清除状态信息。
*/
template <typename T>
void FSM_State_StandUp<T>::onExit()
{
// 退出时无需清理
}
.
.
五、对角小跑的运动步态FSM_State_Locomotion
功能
实现四足机器人的对角小跑的功能，实现算法主要包括支撑腿的反作用MPC预测、摆动腿的足端规划、躯干的状态估计，步态相位控制、腿部正逆运动学解算等等
.
.
1.onEnter（）【进入该状态时要执行的行为】
构造函数
/**
* 功能：将状态特定信息传递给通用FSM状态构造函数
* @param _controlFSMData 保存所有相关的控制数据
* 步骤：
* （0）根据机型实例化MPC控制器
* （1）开启所有的安全检查
* （2）关闭足部pos命令，因为它在WBC中被设置为操作任务
* （3）将GRF初始化为零
* （4）将footstep初始化为零
* （5）建立模型
* （6）发布运动信息
*/
template <typename T>
FSM_State_Locomotion<T>::FSM_State_Locomotion(ControlFSMData<T> *_controlFSMData)
: FSM_State<T>(_controlFSMData, FSM_StateName::LOCOMOTION, "LOCOMOTION")
{
//（1）根据机器人机型，创建MPC控制器
if (_controlFSMData->_quadruped->_robotType == RobotType::MINI_CHEETAH) //实例化MINI_CHEETAHMPC控制器
{
//在这里可以改MPC的迭代次数、MPC的预测未来状态数量、控制周期三个形参。默认的参数请到"ConvexMPCLocomotion.h"里面改
//上面的27是MPC计算的时间间隔27ms，相当于控制频率
cMPCOld = new ConvexMPCLocomotion(_controlFSMData->controlParameters->controller_dt,
27 / (1000. * _controlFSMData->controlParameters->controller_dt),
_controlFSMData->userParameters);
}
else if (_controlFSMData->_quadruped->_robotType == RobotType::CHEETAH_3)//实例化CHEETAH_3MPC控制器
{
cMPCOld = new ConvexMPCLocomotion(_controlFSMData->controlParameters->controller_dt,
33 / (1000. * _controlFSMData->controlParameters->controller_dt),
_controlFSMData->userParameters);
}
else //既不是MINI_CHEETAH，也不是CHEETAH_3，机型对应不上，报错
{
assert(false);
}
this->turnOnAllSafetyChecks(); //（2）开启所有的安全检查
this->checkPDesFoot = false; //（3）关闭足部位置检查命令，因为它在WBC中被设置为操作任务
this->footFeedForwardForces = Mat34<T>::Zero(); //（4）将反作用力GRF初始化为零
this->footstepLocations = Mat34<T>::Zero(); //（5）将footstep初始化为零
_wbc_ctrl = new LocomotionCtrl<T>(_controlFSMData->_quadruped->buildModel()); //（6）建立机器人模型
_wbc_data = new LocomotionCtrlData<T>(); //（7）发布运动信息
}
/**
*功能：进入状态时要执行的行为
*/
template <typename T>
void FSM_State_Locomotion<T>::onEnter()
{
this->nextStateName = this->stateName; //默认是不转换状态
this->transitionData.zero(); //重置转换数据
cMPCOld->initialize(); //初始化CMPC
this->_data->_gaitScheduler->gaitData._nextGait = GaitType::TROT;//切换状态时默认trot步态
printf("[FSM LOCOMOTION] On Enter\n");
}
.
.
2.run()【运行该状态的正常行为】
第一步：使用MPC预测足端的反作用力
原理参考我另外一篇博客
【四足机器人–支撑相足端反作用力预测】（5）ConvexMPCLocomotion代码解析
第二步：使用WBC求解每个关节的扭矩、速度和位置
第三步：使用腿部控制器运行每个关节的扭矩、速度和位置
/**
* 功能：调用要在每个控制循环迭代中执行的函数
*/
template <typename T>
void FSM_State_Locomotion<T>::run()
{
LocomotionControlStep(); //为这个迭代调用移动控制逻辑
}
/**
* 功能：控制运动状态：通过调用适当的平衡控制器，并解析每个姿态或摆动腿的结果，计算每个脚的腿部控制器的命令。
* 备注：原理很重要
*/
template <typename T>
void FSM_State_Locomotion<T>::LocomotionControlStep()
{
///*（1）【很重要！！】运行CMPC控制器，得到每条腿的反作用力*
cMPCOld->run<T>(*this->_data); //MPC控制器，输入_data数据，输出反作用力
///*(2)【很重要！！】运行WBC控制器*//
//（1）定义期望位置、期望速度、KP\KD，并用腿部控制器数据更新
//1）定义期望位置、期望速度、KP\KD
Vec3<T> pDes_backup[4];
Vec3<T> vDes_backup[4];
Mat3<T> Kp_backup[4];
Mat3<T> Kd_backup[4];
//2）用腿部控制器数据更新期望位置、期望速度、KP\KD
for (int leg(0); leg < 4; ++leg)
{
pDes_backup[leg] = this->_data->_legController->commands[leg].pDes; //腿部控制器更新每条腿的位置
vDes_backup[leg] = this->_data->_legController->commands[leg].vDes; //腿部控制器更新每条腿的速度
Kp_backup[leg] = this->_data->_legController->commands[leg].kpCartesian;//腿部控制器更新每条腿的KP
Kd_backup[leg] = this->_data->_legController->commands[leg].kdCartesian;//腿部控制器更新每条腿的KD
}
//（2）把MPC控制器输出的数据传给WBC控制器的躯干数据和腿部数据中
if (this->_data->userParameters->use_wbc > 0.9)
{
//1）把MPC控制器输出的数据传给WBC控制器的躯干数据中
_wbc_data->pBody_des = cMPCOld->pBody_des; //1）把MPC的期望躯干位置传给WBC的期望位置
_wbc_data->vBody_des = cMPCOld->vBody_des; //2）把MPC的期望躯干速度传给WBC的期望速度
_wbc_data->aBody_des = cMPCOld->aBody_des; //3）把MPC的期望躯干加速度传给WBC的期望加速度
_wbc_data->pBody_RPY_des = cMPCOld->pBody_RPY_des; //4）把MPC的期望躯干欧拉角传给WBC的期望欧拉角
_wbc_data->vBody_Ori_des = cMPCOld->vBody_Ori_des; //5）把MPC的期望躯干方向传给WBC的期望方向
//2）把MPC控制器输出的数据传给WBC控制器的腿部数据中
for (size_t i(0); i < 4; ++i)
{
_wbc_data->pFoot_des[i] = cMPCOld->pFoot_des[i]; //1）把MPC的期望腿部位置传给WBC的期望位置
_wbc_data->vFoot_des[i] = cMPCOld->vFoot_des[i]; //2）把MPC的期望腿部速度传给WBC的期望速度
_wbc_data->aFoot_des[i] = cMPCOld->aFoot_des[i]; //3）把MPC的期望腿部加速度传给WBC的期望加速度
_wbc_data->Fr_des[i] = cMPCOld->Fr_des[i]; //4）把MPC的期望腿部反作用力传给WBC的期望反作用力【重要！！！】
}
//3）把MPC的期望腿部接触状态传给WBC的腿部接触状态
_wbc_data->contact_state = cMPCOld->contact_state;
//（3）运行WBC控制器【重要！！！】
_wbc_ctrl->run(_wbc_data, *this->_data); //运行WBC控制器【重要！！！！！！】
}
//*（3）更新腿部控制命令*
for (int leg(0); leg < 4; ++leg)//腿控制器
{
//this->_data->_legController->commands[leg].pDes = pDes_backup[leg]; //位置指令
this->_data->_legController->commands[leg].vDes = vDes_backup[leg]; //速度指令
//this->_data->_legController->commands[leg].kpCartesian = Kp_backup[leg];//笛卡尔KP增益
this->_data->_legController->commands[leg].kdCartesian = Kd_backup[leg]; //笛卡尔KD增益
}
}
.
.
3.checkTransition()【检查是否有触发状态转换器–用于状态检查】
extern rc_control_settings rc_control;
/**
* 功能：检查能不能进行状态转移，即管理哪些状态可以由用户命令或状态事件触发器切换。
* @return 要转换为的枚举FSM状态名
*/
template <typename T>
FSM_StateName FSM_State_Locomotion<T>::checkTransition()
{
iter++;//获取下一个状态
if (locomotionSafe())//当前姿态安全才可以切换状态
{
switch ((int)this->_data->controlParameters->control_mode) //切换FSM控制模式
{
case K_LOCOMOTION:
break;
case K_BALANCE_STAND:
this->nextStateName = FSM_StateName::BALANCE_STAND; //要求更改平衡表
this->transitionDuration = 0.0; //过渡时间很快
break;
case K_PASSIVE: //啥都不干
this->nextStateName = FSM_StateName::PASSIVE; //要求更改平衡表
this->transitionDuration = 0.0; //过渡时间是直接的
break;
case K_STAND_UP: //站起
this->nextStateName = FSM_StateName::STAND_UP;
this->transitionDuration = 0.;
break;
case K_RECOVERY_STAND: //摔倒翻身
this->nextStateName = FSM_StateName::RECOVERY_STAND;
this->transitionDuration = 0.;
break;
case K_VISION:
this->nextStateName = FSM_StateName::VISION;
this->transitionDuration = 0.;
break;
default:
std::cout << "[CONTROL FSM] Bad Request: Cannot transition from "
<< K_LOCOMOTION << " to "
<< this->_data->controlParameters->control_mode << std::endl;
}
}
else ///当前姿态不安全，不可以切换状态，强制设置为恢复站立状态
{
this->nextStateName = FSM_StateName::RECOVERY_STAND;//强制设置为恢复站立状态
this->transitionDuration = 0.;
rc_control.mode = RC_mode::RECOVERY_STAND;
}
return this->nextStateName; //将下一个状态名返回给FSM
}
.
.
4.transition（）【管理特定于状态的转换–用于状态转换】
/**
* 功能：状态转移，即处理机器人在状态之间的实际转换。
* @return 在转换完成时返回true。
*/
template <typename T>
TransitionData<T> FSM_State_Locomotion<T>::transition()
{
switch (this->nextStateName) //切换FSM控制模式
{
case FSM_StateName::BALANCE_STAND: //站立平衡状态
LocomotionControlStep(); //从移动转换到站立时缓慢过度状态 状态转换完成
iter++;
if (iter >= this->transitionDuration * 1000)//大于转换时间，表示已经切换完成
{
this->transitionData.done = true;
}
else //小于转换时间，表示没有切换完成
{
this->transitionData.done = false;
}
break;
case FSM_StateName::PASSIVE:
this->turnOffAllSafetyChecks();
this->transitionData.done = true;
break;
case FSM_StateName::STAND_UP:
this->transitionData.done = true;
break;
case FSM_StateName::RECOVERY_STAND:
this->transitionData.done = true;
break;
case FSM_StateName::VISION:
this->transitionData.done = true;
break;
default:
std::cout << "[CONTROL FSM] Something went wrong in transition"
<< std::endl;
}
return this->transitionData; //将转换数据返回到FSM
}
/**
* 功能：移动是否安全有效
* @return 在有效时返回true,超过设定限幅参数返回false
* 步骤：
* （1）获取状态估计器的所有数据
* （2）当状态估计器返回的roll角度（即rpy[0]）大于最大的翻滚角roll，报警并返回无效
* （3）当状态估计器返回的pitch角度（即rpy[2]）大于最大的翻滚角pitch，报警并返回无效
*
* （4）获取腿部控制器hip、y-position两个数据
* （5）如果hip超过限制，报警并返回无效
* （6）y-position超过限制，报警并返回无效
*
* （7）获取腿部控制器速度数据
* （8）腿的速度超过限制，报警并返回无效
*/
template <typename T>
bool FSM_State_Locomotion<T>::locomotionSafe()
{
auto &seResult = this->_data->_stateEstimator->getResult();//（1）获取状态估计器的所有数据
const T max_roll = 40; //定义翻滚角roll的最大角度
const T max_pitch = 40; //定义平移角pitch的最大角度
if (std::fabs(seResult.rpy[0]) > ori::deg2rad(max_roll)) //（2）当状态估计器返回的roll角度（即rpy[0]）大于最大的翻滚角roll，报警并返回无效
{
printf("Unsafe locomotion: roll is %.3f degrees (max %.3f)\n", ori::rad2deg(seResult.rpy[0]), max_roll);
return false;
}
if (std::fabs(seResult.rpy[1]) > ori::deg2rad(max_pitch)) //（3）当状态估计器返回的pitch角度（即rpy[2]）大于最大的翻滚角pitch，报警并返回无效
{
printf("Unsafe locomotion: pitch is %.3f degrees (max %.3f)\n", ori::rad2deg(seResult.rpy[1]), max_pitch);
return false;
}
for (int leg = 0; leg < 4; leg++)//四条腿都要检查
{
auto p_leg = this->_data->_legController->datas[leg].p;//（4）获取腿部控制器hip、y-position两个数据
if (p_leg[2] > 0) //（5）如果hip超过限制，报警并返回无效
{
printf("Unsafe locomotion: leg %d is above hip (%.3f m)\n", leg, p_leg[2]);
return false;
}
if (std::fabs(p_leg[1] > 0.18)) //（6）y-position超过限制，报警并返回无效
{
printf("Unsafe locomotion: leg %d's y-position is bad (%.3f m)\n", leg, p_leg[1]);
return false;
}
auto v_leg = this->_data->_legController->datas[leg].v.norm();//（7）获取腿部控制器速度数据
if (std::fabs(v_leg) > 9.) //（8）腿的速度超过限制，报警并返回无效
{
printf("Unsafe locomotion: leg %d is moving too quickly (%.3f m/s)\n", leg, v_leg);
return false;
}
}
return true;
}
.
.
5.onExit()【退出该状态时要执行的行为】
/**
* 功能：在退出状态时清除状态信息。
*/
template <typename T>
void FSM_State_Locomotion<T>::onExit()
{
iter = 0; //退出时无需清理
}
.
.
总结
参考资料
版权声明：本文为qq_35635374原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。