Simulate the alternate tripod gait using Webots

本文介绍三脚架对换式步态 （alternate tripod gait）的实现

┌───────────┐          ^ right
│ 0   1   2 │          │ 
│ 3   4   5 │ front <──┘
└───────────┘

一侧的前后两足和另一侧的中间足构成一个三脚架，每一时刻有一个三脚架处于支撑相，另一个三脚架出于摆动相，每隔半个周期进行切换。

足沿着轴摆动时，我们把处于支撑相的这段叫做小摆(smallSwap)，把处于摆动相的这段叫做大摆(bigSwap)。

#define RATE 		1.0/6  // 小摆占圆周的比率
#define smallSwap	2 * M_PI * RATE
#define bigSwap		2 * M_PI * (1-RATE)

步态涉及两个状态，一个是奇数号三脚架大摆，偶数号三脚架小摆；另一个是奇数号三脚架小摆，偶数号三脚架大摆。

const double posSwap [NUM_STATES][NUM_SERVOS] = {
	{smallSwap	,bigSwap  	,smallSwap	,bigSwap  	,smallSwap	,bigSwap  },
	{bigSwap  	,smallSwap	,bigSwap  	,smallSwap	,bigSwap  	,smallSwap}
};

初始时六足偶数号在前（进入支撑相），奇数号在后（离开地面进入摆动相）。准备进入第一个状态。

double  pos[NUM_SERVOS] = {
	-smallSwap/2,+smallSwap/2,-smallSwap/2,
	+smallSwap/2,-smallSwap/2,+smallSwap/2
};

主循环每隔一定周期调整一次各个足的位置，进入下一个状态。

while(1) {
#define AJUST_PERIOD 50
	elapsed++;
	if(elapsed % AJUST_PERIOD!=0) continue;
	state = !state; 
	
	for (i = 0; i < NUM_SERVOS; i++ ){
		servo_set_position(servos[i], pos[i]);
		pos[i] += posSwap[state][i];
	}
}

调整两侧足的运行方向，两侧足同向后运行，就可以实现倒退；一侧足向前，一侧足向后，就可以实现原地转弯。

const int 	goForward	[] = { 1, 1, 1, 1, 1, 1};
const int 	goBackward	[] = {-1,-1,-1,-1,-1,-1};
const int 	turnLeft	[] = { 1, 1, 1,-1,-1,-1};
const int 	turnRight	[] = {-1,-1,-1, 1, 1, 1};

加上转向，完整的仿真程序如下：

#include <webots/robot.h>
#include <webots/servo.h>
#include <stdio.h>

#define NUM_SERVOS 	6
#define NUM_STATES 	2

#define RATE 	 	1.0/6
#define smallSwap	2 * M_PI * RATE
#define bigSwap		2 * M_PI * (1-RATE)

#define TIME_STEP 	16
#define AJUST_PERIOD 	50

int main() {
	const char *SERVO_NAMES[] = {
        "servo_r0",
        "servo_r1",
        "servo_r2",
        "servo_l0",
        "servo_l1",
        "servo_l2"
	};
	WbDeviceTag servos[NUM_SERVOS];
  
	const int 	goForward	[] = { 1, 1, 1, 1, 1, 1};
	const int 	goBackward	[] = {-1,-1,-1,-1,-1,-1};
	const int 	turnLeft	[] = { 1, 1, 1,-1,-1,-1};
	const int 	turnRight	[] = {-1,-1,-1, 1, 1, 1};

	const double posSwap [NUM_STATES][NUM_SERVOS] = {
		{smallSwap	,bigSwap  	,smallSwap	,bigSwap  	,smallSwap	,bigSwap  },
		{bigSwap  	,smallSwap	,bigSwap  	,smallSwap	,bigSwap  	,smallSwap}
	};
	double  pos[NUM_SERVOS] = {
		-smallSwap/2,+smallSwap/2,-smallSwap/2,
		+smallSwap/2,-smallSwap/2,+smallSwap/2
	};
	int i;
	
	// 初始化Webots链接舵机
	wb_robot_init();
  
	for (i = 0; i < NUM_SERVOS; i++) {
		servos[i] = wb_robot_get_device(SERVO_NAMES[i]);
		if (!servos[i])
			printf("could not find servo: %s\n",SERVO_NAMES[i]);
	}
	
	const int* dir = turnRight;//turnLeft;//goBackward;// goForward;
	int  state;
	int  elapsed = 0;
	
	// 初始位置
	for (i = 0; i < NUM_SERVOS; i++ )
		pos[i] =  dir[i] * pos[i];
	// 主循环
	while(wb_robot_step(TIME_STEP)!=-1) { 
	    elapsed++;
		if(elapsed % AJUST_PERIOD!=0) continue;
		state = (elapsed / AJUST_PERIOD + 1) % NUM_STATES;
		for (i = 0; i < NUM_SERVOS; i++ ){
			wb_servo_set_position(servos[i], pos[i]);
			pos[i] += dir[i] * posSwap[state][i]; 
		}
	}
	wb_robot_cleanup();
	return 0;
}

仿真结果如下：