Webots的一些教程
2021/4/27 18:58:26
本文主要是介绍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;
}
如果有其他更多教程，会补充。
这篇关于Webots的一些教程的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！
- 2024-11-23Springboot应用的多环境打包入门
- 2024-11-23Springboot应用的生产发布入门教程
- 2024-11-23Python编程入门指南
- 2024-11-23Java创业入门：从零开始的编程之旅
- 2024-11-23Java创业入门：新手必读的Java编程与创业指南
- 2024-11-23Java对接阿里云智能语音服务入门详解
- 2024-11-23Java对接阿里云智能语音服务入门教程
- 2024-11-23JAVA对接阿里云智能语音服务入门教程
- 2024-11-23Java副业入门：初学者的简单教程
- 2024-11-23JAVA副业入门：初学者的实战指南
