Webots的一些教程

Webots 的一些教程

Webots 的官网有比较详细的API 介绍,可以参考:
Webots API (用到的函数这里都有)
Webots 官网指导 (有教程)
然后软件里也有各种例子,可以参考。

附上一个之前写过的代码,供参考。(但Webots的版本升级后,函数有些可能会不同,仅供参考。)

#include <webots/robot.h>
#include <webots/supervisor.h>
#include <webots/gyro.h>
#include <webots/motor.h>
#include <stdio.h>
#include <math.h>
#include <webots/position_sensor.h>
#include <webots/accelerometer.h>

#define PI 3.1415926539
#define TIME_STEP 10 //in ms
#define INF 9999999.0
#define GRAVITY 9.81 //in m s-2
#define MASS 90 //in kg
#define THIGH_LENGTH 0.35 //in metres
#define SHIN_LENGTH 0.35 //in metres
#define HALF_BWIDTH 0.25 //in metres
#define HALF_BLENGTH 0.5 //in metres

//METHOD 1: TILT BALANCE WALK: walk/run faster by reducing front hip angle and increase frequency.


int main(int argc, char **argv)
{
    wb_robot_init();
    
    
    WbDeviceTag hip[4], hiproll[4], knee[4], gyro, accel;
    WbDeviceTag hip_sensor[4],hiproll_sensor[4],knee_sensor[4];
    
    FILE *OUT,*MOTOR;
    
    OUT=fopen("C:/Users/think/Desktop/my_project/controllers/flight/OUT.txt","w");
    MOTOR=fopen("C:/Users/think/Desktop/my_project/controllers/flight/MOTOR.txt","w");
   
    int timestep = 0,cumul_time = 0, i, phase=0;
    const double *gyro_values, *accel_values;
    double com_roll=0, com_pitch=0, com_yaw=0, x_velocity=0, global_x_accel, z_height;
    double ref_xvel;
    double left_height, right_height;
    double hip_angle[4], hiproll_angle[4], knee_angle[4], ankle_angle[4], ankleroll_angle[4];
    double frequency,air_time,sweep_angle,forward_sweep_ratio;
    double hip_force,hiproll_force,knee_force;
    int cycle = 1;
    
    //define reference values / Characteristics of gait
    //Independent Variables
    frequency = 2.2;
    sweep_angle = 0.05*PI;
    forward_sweep_ratio = 0;
    
    //Dependent Variables
    air_time = 200/frequency; // in ms
    ref_xvel = 0.1;
    
    //Index of joints:
    // 0 - Left Front
    // 1 - Right Front
    // 2 - Left Back
    // 3 - Right Back
    //Assign Wb id tags to actuators
    hip[0] = wb_robot_get_device("LF_hip");
    hiproll[0] = wb_robot_get_device("LF_hiproll");
    knee[0] = wb_robot_get_device("LF_knee");
    hip[1] = wb_robot_get_device("RF_hip");
    hiproll[1] = wb_robot_get_device("RF_hiproll");
    knee[1] = wb_robot_get_device("RF_knee");
    hip[2] = wb_robot_get_device("LB_hip");
    hiproll[2] = wb_robot_get_device("LB_hiproll");
    knee[2] = wb_robot_get_device("LB_knee");
    hip[3] = wb_robot_get_device("RB_hip");
    hiproll[3] = wb_robot_get_device("RB_hiproll");
    knee[3] = wb_robot_get_device("RB_knee");
    
     //Assign Wb id tags to actuators sensors
    hip_sensor[0]=wb_robot_get_device("LF_hip_sensor");
    hiproll_sensor[0]=wb_robot_get_device("LF_hiproll_sensor");
    knee_sensor[0]=wb_robot_get_device("LF_knee_sensor");
    hip_sensor[1]=wb_robot_get_device("RF_hip_sensor");
    hiproll_sensor[1]=wb_robot_get_device("RF_hiproll_sensor");
    knee_sensor[1]=wb_robot_get_device("RF_knee_sensor");
    hip_sensor[2]=wb_robot_get_device("LB_hip_sensor");
    hiproll_sensor[2]=wb_robot_get_device("LB_hiproll_sensor");
    knee_sensor[2]=wb_robot_get_device("LB_knee_sensor");
    hip_sensor[3]=wb_robot_get_device("RB_hip_sensor");
    hiproll_sensor[3]=wb_robot_get_device("RB_hiproll_sensor");
    knee_sensor[3]=wb_robot_get_device("RB_knee_sensor");
    
    //Enable position detection of motors and force detection
    for(i=0;i<=3;i++){
    
      wb_position_sensor_enable(hip_sensor[i],TIME_STEP);
      wb_position_sensor_enable(hiproll_sensor[i],TIME_STEP);
      wb_position_sensor_enable(knee_sensor[i],TIME_STEP);
      
      wb_motor_enable_force_feedback(hip[i],TIME_STEP);
      wb_motor_enable_force_feedback(hiproll[i],TIME_STEP);
      wb_motor_enable_force_feedback(knee[i],TIME_STEP);
    }
    
    //Assign Wb id tags to gyroscope&accelerometer and enable them
    gyro = wb_robot_get_device("gyroscope");
    accel = wb_robot_get_device("accelerometer");
    wb_gyro_enable(gyro,TIME_STEP);
    wb_accelerometer_enable(accel,TIME_STEP);
    
    while (wb_robot_step(TIME_STEP) != -1) {
      //Calculate angular positions using gyroscope **ASSUME ROBOT STARTS IN PERFECT ORIENTATION**
      gyro_values = wb_gyro_get_values(gyro);
      
      com_roll += gyro_values[0]*0.001*TIME_STEP; // x axis
      com_pitch += (-gyro_values[2])*0.001*TIME_STEP; // y axis
        //com_pitch += (gyro_values[1])*0.001*TIME_STEP; // y axis
      com_yaw += gyro_values[1]*0.001*TIME_STEP; // z axis
      //fprintf(OUT,"%f %f %f\n",com_pitch,com_roll,com_yaw);
      //fprintf(OUT,"%f %f %f\n",com_pitch,com_roll,com_yaw);
      
      //Get all actuated joint positions
      for(i=0;i<=3;i++){
        
        hip_angle[i]=wb_position_sensor_get_value(hip_sensor[i]);
        hiproll_angle[i]=wb_position_sensor_get_value(hiproll_sensor[i]);
        knee_angle[i]=wb_position_sensor_get_value(knee_sensor[i]);
        //fprintf(OUT,"%d ) %f %f %f\n",i,hip_angle[i],hiproll_angle[i],knee_angle[i]);
      }
      
      //Calculate ankle DoF positions
      for(i=0;i<=3;i++){
        ankle_angle[i] = -com_pitch - hip_angle[i] - knee_angle[i];
        ankleroll_angle[i] = -com_roll - hiproll_angle[i];
        //fprintf(OUT,"%d ) %f %f\n",i,ankle_angle[i],ankleroll_angle[i]);
      }
      
      //Phase alternation (0 - In air, 1 - RF&LB, 2 - In air, 3 - LF&RB)
	  switch(phase){
      	case 0:
      		if(timestep >= air_time)	phase = 1;
      		break;
      	case 1:
      		if(timestep >= (0.5/frequency)*1000){	
			  phase = 2;
			  timestep = 0;
			}
      		break;
      	case 2:
      		if(timestep >= air_time)	phase = 3;
      		break;
      	case 3:
      		if(timestep >= (0.5/frequency)*1000){	
			  phase = 0;
			  timestep = 0;
        //fprintf(OUT,"\nNEW CYCLE\n");
        //fprintf(MOTOR,"\nNEW CYCLE\n");
        cycle++;
			}
			break;
		default: break;
	  }
      
      //Update x_velocity
      accel_values = wb_accelerometer_get_values(accel);
      global_x_accel= ( accel_values[0] - GRAVITY*sin(com_pitch) ) / (cos(com_pitch));
      x_velocity += global_x_accel*0.001*TIME_STEP;
      if(cycle == 5)
      fprintf(OUT,"%f\n",x_velocity);
      
      //Calculate z_height of body CoM (Which pair of legs to use depends on phase)
      if(phase == 1){
        left_height = SHIN_LENGTH*cos(ankle_angle[2]) + THIGH_LENGTH*cos(ankle_angle[2] + knee_angle[2]);
        right_height = SHIN_LENGTH*cos(ankle_angle[1]) + THIGH_LENGTH*cos(ankle_angle[1] + knee_angle[1]);
        z_height = 0.5*( right_height*cos(ankleroll_angle[1]) + left_height*cos(ankleroll_angle[2]) );
      }else if(phase == 3){
        left_height = SHIN_LENGTH*cos(ankle_angle[0]) + THIGH_LENGTH*cos(ankle_angle[0] + knee_angle[0]);
        right_height = SHIN_LENGTH*cos(ankle_angle[3]) + THIGH_LENGTH*cos(ankle_angle[3] + knee_angle[3]);
        z_height = 0.5*( right_height*cos(ankleroll_angle[3]) + left_height*cos(ankleroll_angle[0]) );
      }
      //fprintf(OUT,"%f\n",z_height);
      
      
      //Actuations
      
      
      //Hop on the spot for 2 periods
      if(cumul_time <= (2/frequency)*1000){
      	if(phase == 0 || phase == 1){
        
        	wb_motor_set_position(hip[0],-3*PI/8);
        	wb_motor_set_position(hip[1],-PI/8);
        	wb_motor_set_position(hip[2],PI/8);
        	wb_motor_set_position(hip[3],3*PI/8);
        	wb_motor_set_position(knee[0],3*PI/4);
        	wb_motor_set_position(knee[1],PI/4);
        	wb_motor_set_position(knee[2],-PI/4);
        	wb_motor_set_position(knee[3],-3*PI/4);
        	wb_motor_set_position(hiproll[0],0);
        	wb_motor_set_position(hiproll[1],0);
        	wb_motor_set_position(hiproll[2],0);
        	wb_motor_set_position(hiproll[3],0);
        
        	 wb_motor_set_control_pid(hip[0],10,0,0);
        	 wb_motor_set_control_pid(hip[1],10,0,0);
        	 wb_motor_set_control_pid(hip[2],10,0,0);
        	 wb_motor_set_control_pid(hip[3],10,0,0);
        	 wb_motor_set_control_pid(knee[0],10,0,0);
        	 wb_motor_set_control_pid(knee[1],10,0,0);
        	 wb_motor_set_control_pid(knee[2],10,0,0);
        	 wb_motor_set_control_pid(knee[3],10,0,0);
        	 wb_motor_set_control_pid(hiproll[0],10,0,0);
        	 wb_motor_set_control_pid(hiproll[1],10,0,0);
        	 wb_motor_set_control_pid(hiproll[2],10,0,0);
        	 wb_motor_set_control_pid(hiproll[3],10,0,0);
        
      	}else if(phase == 2 || phase == 3){
        	wb_motor_set_position(hip[0],-PI/8);
        	wb_motor_set_position(hip[1],-3*PI/8);
        	wb_motor_set_position(hip[2],3*PI/8);
        	wb_motor_set_position(hip[3],PI/8);
        	wb_motor_set_position(knee[0],PI/4);
        	wb_motor_set_position(knee[1],3*PI/4);
        	wb_motor_set_position(knee[2],-3*PI/4);
        	wb_motor_set_position(knee[3],-PI/4);
        	wb_motor_set_position(hiproll[0],0);
        	wb_motor_set_position(hiproll[1],0);
        	wb_motor_set_position(hiproll[2],0);
        	wb_motor_set_position(hiproll[3],0);
        
        	wb_motor_set_control_pid(hip[0],10,0,0);
        	wb_motor_set_control_pid(hip[1],10,0,0);
        	wb_motor_set_control_pid(hip[2],10,0,0);
        	wb_motor_set_control_pid(hip[3],10,0,0);
        	wb_motor_set_control_pid(knee[0],10,0,0);
        	wb_motor_set_control_pid(knee[1],10,0,0);
        	wb_motor_set_control_pid(knee[2],10,0,0);
        	wb_motor_set_control_pid(knee[3],10,0,0);
        	wb_motor_set_control_pid(hiproll[0],10,0,0);
        	wb_motor_set_control_pid(hiproll[1],10,0,0);
        	wb_motor_set_control_pid(hiproll[2],10,0,0);
        	wb_motor_set_control_pid(hiproll[3],10,0,0);
      	}
        cumul_time += 10;
	  }else{
      	//Duty Movement positions
        if(phase == 0){
        	wb_motor_set_position(hip[0],-3*PI/8);
        	wb_motor_set_position(hip[1],-PI/8+forward_sweep_ratio*sweep_angle);
        	wb_motor_set_position(hip[2],PI/8+forward_sweep_ratio*sweep_angle);
        	wb_motor_set_position(hip[3],3*PI/8);
        	wb_motor_set_position(knee[0],3*PI/4);
        	wb_motor_set_position(knee[1],PI/4);
        	wb_motor_set_position(knee[2],-PI/4);
        	wb_motor_set_position(knee[3],-3*PI/4);
        	wb_motor_set_position(hiproll[0],0);
        	wb_motor_set_position(hiproll[1],0);
        	wb_motor_set_position(hiproll[2],0);
        	wb_motor_set_position(hiproll[3],0);
        
        	wb_motor_set_control_pid(hip[0],10,0,0);
        	wb_motor_set_control_pid(hip[1],10,0,0);
        	wb_motor_set_control_pid(hip[2],10,0,0);
        	wb_motor_set_control_pid(hip[3],10,0,0);
        	wb_motor_set_control_pid(knee[0],10,0,0);
        	wb_motor_set_control_pid(knee[1],10,0,0);
        	wb_motor_set_control_pid(knee[2],10,0,0);
        	wb_motor_set_control_pid(knee[3],10,0,0);
        	wb_motor_set_control_pid(hiproll[0],10,0,0);
        	wb_motor_set_control_pid(hiproll[1],10,0,0);
        	wb_motor_set_control_pid(hiproll[2],10,0,0);
        	wb_motor_set_control_pid(hiproll[3],10,0,0);
        
      	}else if(phase == 1){
        	wb_motor_set_position(hip[0],-3*PI/8);
        	wb_motor_set_position(hip[1],-PI/8-(1-forward_sweep_ratio)*sweep_angle);
        	wb_motor_set_position(hip[2],PI/8-(1-forward_sweep_ratio)*sweep_angle);
        	wb_motor_set_position(hip[3],3*PI/8);
        	wb_motor_set_position(knee[0],3*PI/4);
        	wb_motor_set_position(knee[1],PI/4);
        	wb_motor_set_position(knee[2],-PI/4);
        	wb_motor_set_position(knee[3],-3*PI/4);
        	wb_motor_set_position(hiproll[0],0);
        	wb_motor_set_position(hiproll[1],0);
        	wb_motor_set_position(hiproll[2],0);
        	wb_motor_set_position(hiproll[3],0);
        
        	wb_motor_set_control_pid(hip[0],10,0,0);
        	wb_motor_set_control_pid(hip[1],10,0,0);
        	wb_motor_set_control_pid(hip[2],10,0,0);
        	wb_motor_set_control_pid(hip[3],10,0,0);
        	wb_motor_set_control_pid(knee[0],10,0,0);
        	wb_motor_set_control_pid(knee[1],10,0,0);
        	wb_motor_set_control_pid(knee[2],10,0,0);
        	wb_motor_set_control_pid(knee[3],10,0,0);
        	wb_motor_set_control_pid(hiproll[0],10,0,0);
        	wb_motor_set_control_pid(hiproll[1],10,0,0);
        	wb_motor_set_control_pid(hiproll[2],10,0,0);
        	wb_motor_set_control_pid(hiproll[3],10,0,0);
        	
      	}else if(phase == 2){
        	wb_motor_set_position(hip[0],-PI/8+forward_sweep_ratio*sweep_angle);
        	wb_motor_set_position(hip[1],-3*PI/8);
        	wb_motor_set_position(hip[2],3*PI/8);
        	wb_motor_set_position(hip[3],PI/8+forward_sweep_ratio*sweep_angle);
        	wb_motor_set_position(knee[0],PI/4);
        	wb_motor_set_position(knee[1],3*PI/4);
        	wb_motor_set_position(knee[2],-3*PI/4);
        	wb_motor_set_position(knee[3],-PI/4);
        	wb_motor_set_position(hiproll[0],0);
        	wb_motor_set_position(hiproll[1],0);
        	wb_motor_set_position(hiproll[2],0);
        	wb_motor_set_position(hiproll[3],0);
        
        	wb_motor_set_control_pid(hip[0],10,0,0);
        	wb_motor_set_control_pid(hip[1],10,0,0);
        	wb_motor_set_control_pid(hip[2],10,0,0);
        	wb_motor_set_control_pid(hip[3],10,0,0);
        	wb_motor_set_control_pid(knee[0],10,0,0);
        	wb_motor_set_control_pid(knee[1],10,0,0);
        	wb_motor_set_control_pid(knee[2],10,0,0);
        	wb_motor_set_control_pid(knee[3],10,0,0);
        	wb_motor_set_control_pid(hiproll[0],10,0,0);
        	wb_motor_set_control_pid(hiproll[1],10,0,0);
        	wb_motor_set_control_pid(hiproll[2],10,0,0);
        	wb_motor_set_control_pid(hiproll[3],10,0,0);
        	
      	}else if(phase == 3){
        	wb_motor_set_position(hip[0],-PI/8-(1-forward_sweep_ratio)*sweep_angle);
        	wb_motor_set_position(hip[1],-3*PI/8);
        	wb_motor_set_position(hip[2],3*PI/8);
        	wb_motor_set_position(hip[3],PI/8-(1-forward_sweep_ratio)*sweep_angle);
        	wb_motor_set_position(knee[0],PI/4);
        	wb_motor_set_position(knee[1],3*PI/4);
        	wb_motor_set_position(knee[2],-3*PI/4);
        	wb_motor_set_position(knee[3],-PI/4);
        	wb_motor_set_position(hiproll[0],0);
        	wb_motor_set_position(hiproll[1],0);
        	wb_motor_set_position(hiproll[2],0);
        	wb_motor_set_position(hiproll[3],0);
        
        	wb_motor_set_control_pid(hip[0],10,0,0);
        	wb_motor_set_control_pid(hip[1],10,0,0);
            wb_motor_set_control_pid(hip[2],10,0,0);
        	wb_motor_set_control_pid(hip[3],10,0,0);
        	wb_motor_set_control_pid(knee[0],10,0,0);
        	wb_motor_set_control_pid(knee[1],10,0,0);
        	wb_motor_set_control_pid(knee[2],10,0,0);
        	wb_motor_set_control_pid(knee[3],10,0,0);
        	wb_motor_set_control_pid(hiproll[0],10,0,0);
        	wb_motor_set_control_pid(hiproll[1],10,0,0);
        	wb_motor_set_control_pid(hiproll[2],10,0,0);
        	wb_motor_set_control_pid(hiproll[3],10,0,0);
        }
      }
      //Get Motor force info
      if(cycle == 5){
      for(i=0;i<=3;i++){
        hip_force = wb_servo_get_motor_force_feedback(hip[i]);
        hiproll_force = wb_servo_get_motor_force_feedback(hiproll[i]);
        knee_force = wb_servo_get_motor_force_feedback(knee[i]);
        fprintf(MOTOR,"%f\t%f\t%f\t",hip_force,hiproll_force,knee_force);
      }
      fprintf(MOTOR,"\n");
      }
      timestep +=10;
      
    }
    
    fclose(OUT);
    fclose(MOTOR);
    wb_robot_cleanup();

    return 0;
}

如果有其他更多教程,会补充。

上一篇:Flash拼图游戏


下一篇:selenium+cookie自动登录新浪微博(二)