Tinymal-B控制教程介绍4-方式二 仅开发导航算法

Tinymal-B团队将持续更新闭源软件固件,提高机器人运动能力,同时开发完事的API和SDK,开发机器人运动学配置和物理参数配置,方便爱好者将该原型系统部署于尺寸更大或类型不同的机器人研究雷达SLAM或视觉SLAM

如果你仅仅想采用Tinymal-B完成四足机器人导航、路径规划算法或智能交互算法的开发,那可以运行官方底层与步态控制固件,采用navigation_task项目进行二次开发,采用共享内存接口完成机器人轨迹、姿态、步态模式等指令的下达。
navigation_task中通过共享内存完成了对控制程序中机器人运动学、状态估计、足端力等数据的获取,并实时下达机器人步态模式、期望速度等指令。上述指令的下达我们可以采用两种方式来实现:1)模拟遥控器方式;2)SDK二次开发接口。

基于按键结构体开发

第一种方式即通过代码模拟上位机采集的PS2遥控按键,通过按键完成步态模式的切换,通过摇杆完成速度和姿态的调节,代码中遥控器数据发送代码为:
上图中向共享内存中传输了PS2遥控器对应的按键和摇杆数据,首先需要设置ocu.connect为1。其中rc_spd_w和rc_att_w为摇杆值,范围为-1~1对应摇杆行程的最小、最大值;rate_yaw_w为手柄后方两个扳机,范围为-1~1对应转向行程的最小、最大值;key_abxy对应手柄按键按下时为1,不按为0;key_ll和key_rr为遥控器上方左右两个按键,主要完成高度调节,按下为1,不按下为0;方向键上下为key_ud,则按下上为1,按下为-1,不按为0;方向键左右为key_lr,按下右为1,按下左为-1,不按为0;key_st为遥控器Start按键对应启动自动导航等SDK功能。则通过设计状态机,通过模拟按键触发过程既可以实现机器人步态的切换,通过gait_state可以实时确认机器人当前的状态,在不同步态中设定摇杆值则可以实现姿态和速度的调节,各模式定义如下依次递增:
{ M_SAFE=0, M_STAND_RC=1, M_STAND_IMU=2, M_STAND_PUSH=3, M_TROT=4, M_F_TROT=5, M_WALK=6, M_RECOVER, M_FALLING, M_CLIMB, M_WALKE, M_CRAWL, M_PRONK, M_BOUND, M_SOFT, M_C_TROT, M_C_JOG, M_ROLLING, M_ETL };
则基本按键切换逻辑如下表所示,状态切换均为触发方式:
下面给出一个基于按键触发的从站立到姿态自动调节再到蹲下的例子,使用该例子请打开:
#define DEMO_KEY 0
上电后按遥控器Back按键,则机器人将自动触发上述流程,下面给出了简单的测试代码,其主要就是操控按键结构体,并替代了默认读取上位机发送的数据:
void simple_test(float dt){ static int state=0; static float timer=0; switch(state){ case 0: if(ocu.key_back){ state++; printf("Simple API Start!\n"); } break; case 1: if(nav_rx.gait_state==M_SAFE)//init leg { nav_tx.key_x=1; nav_tx.key_rr=1; timer=0; state++; } break; case 2: timer+=dt; if(timer>0.6){ timer=0; state++; nav_tx.key_ll=1; } break; case 3://force gait enable timer+=dt; if(timer>0.6){ timer=0; state++; nav_tx.key_ll=0; } break; //-- case 4: if(nav_rx.gait_state!=M_SAFE) timer+=dt; if(timer>0.1){//stand timer=0; state++; nav_tx.key_x=0; nav_tx.key_rr=0; nav_tx.key_ll=1; } break; case 5: timer+=dt; if(timer>3) nav_tx.rc_rate_b[0]=0;//change pitch else if(timer>2) nav_tx.rc_rate_b[0]=1; else if(timer>1) nav_tx.rc_rate_b[0]=-1; if(timer>4){//sit down timer=0; state++; nav_tx.key_ll=0; nav_tx.key_rr=1; nav_tx.rc_rate_b[0]=0; } break; case 6: timer+=dt; if(timer>1){ timer=0; state=0; nav_tx.key_rr=0; nav_tx.key_ud=-1; printf("Simple API Done!\n"); } break; } if(state>0&&(ocu.key_ud==-1||ocu.key_y==1)) { printf("Simple API Reset!\n"); state=0; nav_tx.rc_spd_b[0]=0;// ocu.rc_spd_w[Xr]; nav_tx.rc_spd_b[1]=0;// ocu.rc_spd_w[Yr]; nav_tx.rc_rate_b[0] =0;// ocu.rc_att_w[PITr]; nav_tx.rc_rate_b[1] =0;// ocu.rc_att_w[ROLr]; nav_tx.rc_rate_b[2] =0;// ocu.rate_yaw_w; nav_tx.key_st= 0;// ocu.key_st; nav_tx.key_back= 0;// ocu.key_back; nav_tx.key_lr= 0;// ocu.key_lr; nav_tx.key_ud= 0;// ocu.key_ud; nav_tx.key_x= 0;// ocu.key_x; nav_tx.key_y= 0;// ocu.key_y; nav_tx.key_b= 0;// ocu.key_b; nav_tx.key_a= 0;// ocu.key_a; nav_tx.key_ll= 0;// ocu.key_ll; nav_tx.key_rr= 0;// ocu.key_rr; } }
注:为了保证断使能按键的有效性我们没有替换其输出:
nav_tx.connect = ocu.connect; nav_tx.ocu_mode = ocu.mode; nav_tx.key_ud= ocu.key_ud; nav_tx.key_y= ocu.key_y; simple_test(Get_Cycle_T(10));

基于SDK开发

另外,也可以采用SDK协议完成机器人模式的切换,即操作如下的数据结构直接设置期望步态,和机体真实的姿态角度、速度等状态参数,使用时请收下将moco_sdk.ros_connect设置为1:
该部分后续将持续更新,完善SDK部分!
2023-02-11