mirror of https://github.com/ArduPilot/ardupilot
SITL: adding Tricopter model in Webots
This commit is contained in:
mhefny
Francisco Ferreira
parent
47cb514446
commit
8310058c8c
|
|
@ -107,6 +107,8 @@ Webots::Webots(const char *frame_str) :
|
|||
output_type = OUTPUT_ROVER;
|
||||
} else if (strstr(frame_option, "-quad")) {
|
||||
output_type = OUTPUT_QUAD;
|
||||
} else if (strstr(frame_option, "-tri")) {
|
||||
output_type = OUTPUT_TRICOPTER;
|
||||
} else if (strstr(frame_option, "-pwm")) {
|
||||
output_type = OUTPUT_PWM;
|
||||
} else {
|
||||
|
|
@ -370,6 +372,34 @@ void Webots::output_rover(const struct sitl_input &input)
|
|||
sim_sock->send(buf, len);
|
||||
}
|
||||
|
||||
/*
|
||||
output control command assuming a 3 channels motors and 1 channel servo
|
||||
*/
|
||||
void Webots::output_tricopter(const struct sitl_input &input)
|
||||
{
|
||||
const float max_thrust = 1.0;
|
||||
float motors[3];
|
||||
const float servo = ((input.servos[6]-1000)/1000.0f - 0.5f);
|
||||
motors[0] = constrain_float(((input.servos[0]-1000)/1000.0f) * max_thrust, 0, max_thrust);
|
||||
motors[1] = constrain_float(((input.servos[1]-1000)/1000.0f) * max_thrust, 0, max_thrust);
|
||||
motors[2] = constrain_float(((input.servos[3]-1000)/1000.0f) * max_thrust, 0, max_thrust);
|
||||
|
||||
const float &m_right = motors[0];
|
||||
const float &m_left = motors[1];
|
||||
const float &m_servo = servo ;
|
||||
const float &m_back = motors[2];
|
||||
|
||||
// construct a JSON packet for motors
|
||||
char buf[200];
|
||||
const int len = snprintf(buf, sizeof(buf)-1, "{\"eng\": [%.3f, %.3f, %.3f, %.3f], \"wnd\": [%f, %3.1f, %1.1f, %2.1f]}\n",
|
||||
m_right, m_left, m_servo, m_back,
|
||||
input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
|
||||
//printf("\"eng\": [%.3f, %.3f, %.3f, %.3f]\n",m_right, m_left, m_servo, m_back);
|
||||
buf[len] = 0;
|
||||
|
||||
sim_sock->send(buf, len);
|
||||
}
|
||||
|
||||
/*
|
||||
output control command assuming a 4 channel quad
|
||||
*/
|
||||
|
|
@ -430,6 +460,9 @@ void Webots::output (const struct sitl_input &input)
|
|||
case OUTPUT_QUAD:
|
||||
output_quad(input);
|
||||
break;
|
||||
case OUTPUT_TRICOPTER:
|
||||
output_tricopter(input);
|
||||
break;
|
||||
case OUTPUT_PWM:
|
||||
output_pwm(input);
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -52,7 +52,8 @@ private:
|
|||
enum {
|
||||
OUTPUT_ROVER=1,
|
||||
OUTPUT_QUAD=2,
|
||||
OUTPUT_PWM=3
|
||||
OUTPUT_TRICOPTER=3,
|
||||
OUTPUT_PWM=4
|
||||
} output_type;
|
||||
|
||||
bool connect_sockets(void);
|
||||
|
|
@ -60,6 +61,7 @@ private:
|
|||
bool sensors_receive(void);
|
||||
void output_rover(const struct sitl_input &input);
|
||||
void output_quad(const struct sitl_input &input);
|
||||
void output_tricopter(const struct sitl_input &input);
|
||||
void output_pwm(const struct sitl_input &input);
|
||||
void report_FPS();
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
# Copyright 1996-2019 Cyberbotics Ltd.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
### Generic Makefile.include for Webots controllers, physics plugins, robot
|
||||
### window libraries, remote control libraries and other libraries
|
||||
### to be used with GNU make
|
||||
###
|
||||
### Platforms: Windows, macOS, Linux
|
||||
### Languages: C, C++
|
||||
###
|
||||
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
|
||||
### Edmund Ronald, Sergei Poskriakov
|
||||
###
|
||||
###-----------------------------------------------------------------------------
|
||||
###
|
||||
### This file is meant to be included from the Makefile files located in the
|
||||
### Webots projects subdirectories. It is possible to set a number of variables
|
||||
### to customize the build process, i.e., add source files, compilation flags,
|
||||
### include paths, libraries, etc. These variables should be set in your local
|
||||
### Makefile just before including this Makefile.include. This Makefile.include
|
||||
### should never be modified.
|
||||
###
|
||||
### Here is a description of the variables you may set in your local Makefile:
|
||||
###
|
||||
### ---- C Sources ----
|
||||
### if your program uses several C source files:
|
||||
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
|
||||
###
|
||||
### ---- C++ Sources ----
|
||||
### if your program uses several C++ source files:
|
||||
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
|
||||
###
|
||||
### ---- Compilation options ----
|
||||
### if special compilation flags are necessary:
|
||||
### CFLAGS = -Wno-unused-result
|
||||
###
|
||||
### ---- Linked libraries ----
|
||||
### if your program needs additional libraries:
|
||||
### INCLUDE = -I"/my_library_path/include"
|
||||
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
|
||||
###
|
||||
### ---- Linking options ----
|
||||
### if special linking flags are needed:
|
||||
### LFLAGS = -s
|
||||
###
|
||||
### ---- Webots included libraries ----
|
||||
### if you want to use the Webots C API in your C++ controller program:
|
||||
### USE_C_API = true
|
||||
### if you want to link with the Qt framework embedded in Webots:
|
||||
### QT = core gui widgets network
|
||||
###
|
||||
### ---- Debug mode ----
|
||||
### if you want to display the gcc command line for compilation and link, as
|
||||
### well as the rm command details used for cleaning:
|
||||
### VERBOSE = 1
|
||||
###
|
||||
###-----------------------------------------------------------------------------
|
||||
|
||||
C_SOURCES = ardupilot_SITL_TRICOPTER.c sockets.c sensors.c
|
||||
INCLUDE = -I"./"
|
||||
### Do not modify: this includes Webots global Makefile.include
|
||||
space :=
|
||||
space +=
|
||||
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
|
||||
include $(WEBOTS_HOME_PATH)/resources/Makefile.include
|
||||
Binary file not shown.
|
|
@ -0,0 +1,520 @@
|
|||
/*
|
||||
* File: ardupilot_SITL_TRICOPTER.c
|
||||
* Date: 18 Aug 2019
|
||||
* Description: integration with ardupilot SITL simulation.
|
||||
* Author: M.S.Hefny (HefnySco)
|
||||
* Modifications:
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* You may need to add include files like <webots/distance_sensor.h> or
|
||||
* <webots/differential_wheels.h>, etc.
|
||||
*/
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <webots/robot.h>
|
||||
#include <webots/supervisor.h>
|
||||
#include <webots/emitter.h>
|
||||
#include "ardupilot_SITL_TRICOPTER.h"
|
||||
#include "sockets.h"
|
||||
#include "sensors.h"
|
||||
|
||||
|
||||
|
||||
#define MOTOR_NUM 3
|
||||
|
||||
static WbDeviceTag motors[MOTOR_NUM];
|
||||
static WbDeviceTag servo;
|
||||
static WbDeviceTag gyro;
|
||||
static WbDeviceTag accelerometer;
|
||||
static WbDeviceTag compass;
|
||||
static WbDeviceTag gps;
|
||||
static WbDeviceTag camera;
|
||||
static WbDeviceTag inertialUnit;
|
||||
static WbDeviceTag emitter;
|
||||
static WbNodeRef world_info;
|
||||
|
||||
static const double *northDirection;
|
||||
static double v[MOTOR_NUM];
|
||||
static double servo_value = 0;
|
||||
static double servo_value_extra = 0;
|
||||
|
||||
int port;
|
||||
|
||||
|
||||
static int timestep;
|
||||
|
||||
|
||||
#ifdef DEBUG_USE_KB
|
||||
/*
|
||||
// Code used tp simulae motors using keys to make sure that sensors directions and motor torques and thrusts are all correct.
|
||||
// You can start this controller and use telnet instead of SITL to start the simulator.
|
||||
Then you can use Keyboard to emulate motor input.
|
||||
*/
|
||||
void process_keyboard ()
|
||||
{
|
||||
switch (wb_keyboard_get_key())
|
||||
{
|
||||
case 'Q': // Q key -> up & left
|
||||
v[0] = 0.0;
|
||||
v[1] = 0.0;
|
||||
v[2] = 0.0;
|
||||
servo_value_extra = 0.0;
|
||||
break;
|
||||
|
||||
case 'Y':
|
||||
v[0] = v[0] + 0.01;
|
||||
v[1] = v[1] + 0.01;
|
||||
v[2] = v[2] - 0.02;
|
||||
break;
|
||||
|
||||
case 'H':
|
||||
v[0] = v[0] - 0.01;
|
||||
v[1] = v[1] - 0.01;
|
||||
v[2] = v[2] + 0.02;
|
||||
break;
|
||||
|
||||
case 'G':
|
||||
v[0] = v[0] + 0.01;
|
||||
v[1] = v[1] - 0.01;
|
||||
break;
|
||||
|
||||
case 'J':
|
||||
v[0] = v[0] - 0.01;
|
||||
v[1] = v[1] + 0.01;
|
||||
break;
|
||||
|
||||
case 'W':
|
||||
for (int i=0; i<MOTOR_NUM;++i)
|
||||
{
|
||||
v[i] += 0.01;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'S':
|
||||
for (int i=0; i<MOTOR_NUM;++i)
|
||||
{
|
||||
v[i] -= 0.01;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'A':
|
||||
servo_value_extra = servo_value_extra + 0.01;
|
||||
break;
|
||||
|
||||
case 'D':
|
||||
servo_value_extra = servo_value_extra - 0.01;
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
|
||||
for (int i=0; i< MOTOR_NUM; ++i)
|
||||
{
|
||||
if (v[i] <=0) v[i]=0;
|
||||
if (v[i] >=600) v[i]=10;
|
||||
|
||||
wb_motor_set_position(motors[i], INFINITY);
|
||||
wb_motor_set_velocity(motors[i], v[i]);
|
||||
}
|
||||
|
||||
wb_motor_set_position (servo, servo_value_extra);
|
||||
wb_motor_set_velocity (servo, 100);
|
||||
|
||||
|
||||
printf ("Motors Internal right:%f left:%f back:%f servo:%f\n", v[0],v[1],v[2],servo_value);
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
// apply motor thrust.
|
||||
*/
|
||||
void update_controls()
|
||||
{
|
||||
/*
|
||||
1 N = 101.9716213 grams force
|
||||
Thrust = t1 * |omega| * omega - t2 * |omega| * V
|
||||
Where t1 and t2 are the constants specified in the thrustConstants field,
|
||||
omega is the motor angular velocity
|
||||
and V is the component of the linear velocity of the center of thrust along the shaft axis.
|
||||
|
||||
if Vehicle mass = 1 Kg. and we want omega = 1.0 to hover
|
||||
then (mass / 0.10197) / (4 motors) = t1
|
||||
|
||||
LINEAR_THRUST
|
||||
we also want throttle to be linear with thrust so we use sqrt to calculate omega from input.
|
||||
*/
|
||||
static float factor = 1.0f;
|
||||
static float offset = 0.0f;
|
||||
static float v[MOTOR_NUM];
|
||||
|
||||
#ifdef LINEAR_THRUST
|
||||
v[0] = sqrt(state.motors.w ) * factor + offset;
|
||||
v[1] = sqrt(state.motors.x ) * factor + offset;
|
||||
v[2] = sqrt(state.motors.z ) * factor + offset;
|
||||
#else
|
||||
v[0] = (state.motors.w ) * factor + offset;
|
||||
v[1] = (state.motors.x ) * factor + offset;
|
||||
v[2] = (state.motors.z ) * factor + offset;
|
||||
#endif
|
||||
|
||||
servo_value = -state.motors.y ;
|
||||
|
||||
for ( int i=0; i<MOTOR_NUM; ++i)
|
||||
{
|
||||
wb_motor_set_position(motors[i], INFINITY);
|
||||
wb_motor_set_velocity(motors[i], v[i]);
|
||||
}
|
||||
|
||||
#ifdef DEBUG_USE_KB
|
||||
wb_motor_set_position(servo, servo_value + servo_value_extra);
|
||||
#else
|
||||
wb_motor_set_position(servo, servo_value);
|
||||
#endif
|
||||
wb_motor_set_velocity(servo, 1000);
|
||||
|
||||
#ifdef DEBUG_MOTORS
|
||||
printf ("RAW R:%f L:%f SRV:%f B:%f\n", state.motors.w, state.motors.x, state.motors.y, state.motors.z);
|
||||
printf ("Motors R:%f L:%f SRV:%f B:%f\n", v[0], v[1], servo_value, v[2]);
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef WIND_SIMULATION
|
||||
|
||||
double linear_speed = sqrt(linear_velocity[0] * linear_velocity[0] + linear_velocity[1] * linear_velocity[1] + linear_velocity[2] * linear_velocity[2]);
|
||||
wind_webots_axis.w = state.wind.w + 0.01 * linear_speed * linear_speed;
|
||||
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
wind_webots_axis.x = state.wind.x - linear_velocity[0];
|
||||
wind_webots_axis.z = -state.wind.y - linear_velocity[2]; // "-state.wind.y" as angle 90 wind is from EAST.
|
||||
wind_webots_axis.y = state.wind.z - linear_velocity[1];
|
||||
}
|
||||
else
|
||||
{ // as in pyramids and any open map street world.
|
||||
wind_webots_axis.x = state.wind.y - linear_velocity[0]; // always add "linear_velocity" as there is no axis transformation here.
|
||||
wind_webots_axis.z = -state.wind.x - linear_velocity[2];
|
||||
wind_webots_axis.y = state.wind.z - linear_velocity[1];
|
||||
}
|
||||
|
||||
wb_emitter_send(emitter, &wind_webots_axis, sizeof(VECTOR4F));
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// data example: [my_controller_SITL] {"engines": [0.000, 0.000, 0.000, 0.000]}
|
||||
// the JSON parser is directly inspired by https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_Morse.cpp
|
||||
bool parse_controls(const char *json)
|
||||
{
|
||||
//state.timestamp = 1.0;
|
||||
#ifdef DEBUG_INPUT_DATA
|
||||
printf("%s\n", json);
|
||||
#endif
|
||||
|
||||
for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) {
|
||||
struct keytable *key;
|
||||
key = &keytable[i];
|
||||
//printf("search %s/%s\n", key->section, key->key);
|
||||
// look for section header
|
||||
const char *p = strstr(json, key->section);
|
||||
if (!p) {
|
||||
// we don't have this sensor
|
||||
continue;
|
||||
}
|
||||
p += strlen(key->section)+1;
|
||||
|
||||
// find key inside section
|
||||
p = strstr(p, key->key);
|
||||
if (!p) {
|
||||
printf("Failed to find key %s/%s\n", key->section, key->key);
|
||||
return false;
|
||||
}
|
||||
|
||||
p += strlen(key->key)+3;
|
||||
|
||||
switch (key->type)
|
||||
{
|
||||
case DATA_FLOAT:
|
||||
*((float *)key->ptr) = atof(p);
|
||||
#ifdef DEBUG_INPUT_DATA
|
||||
printf("GOT %s/%s\n", key->section, key->key);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case DATA_DOUBLE:
|
||||
*((double *)key->ptr) = atof(p);
|
||||
#ifdef DEBUG_INPUT_DATA
|
||||
printf("GOT %s/%s\n", key->section, key->key);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case DATA_VECTOR4F: {
|
||||
VECTOR4F *v = (VECTOR4F *)key->ptr;
|
||||
if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) {
|
||||
printf("Failed to parse Vector3f for %s %s/%s\n",p, key->section, key->key);
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
#ifdef DEBUG_INPUT_DATA
|
||||
printf("GOT %s/%s\n[%f, %f, %f, %f]\n ", key->section, key->key,v->w,v->x,v->y,v->z);
|
||||
#endif
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void run ()
|
||||
{
|
||||
|
||||
char send_buf[1000]; //1000 just a safe margin
|
||||
char command_buffer[200];
|
||||
fd_set rfds;
|
||||
while (wb_robot_step(timestep) != -1)
|
||||
{
|
||||
#ifdef DEBUG_USE_KB
|
||||
process_keyboard();
|
||||
#endif
|
||||
|
||||
if (fd == 0)
|
||||
{
|
||||
// if no socket wait till you get a socket
|
||||
fd = socket_accept(sfd);
|
||||
if (fd > 0)
|
||||
socket_set_non_blocking(fd);
|
||||
else if (fd < 0)
|
||||
break;
|
||||
}
|
||||
|
||||
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
printf("%s\n",send_buf);
|
||||
#endif
|
||||
|
||||
if (write(fd,send_buf,strlen(send_buf)) <= 0)
|
||||
{
|
||||
printf ("Send Data Error\n");
|
||||
}
|
||||
|
||||
if (fd)
|
||||
{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(fd, &rfds);
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0.05;
|
||||
tv.tv_usec = 0;
|
||||
int number = select(fd + 1, &rfds, NULL, NULL, &tv);
|
||||
if (number != 0)
|
||||
{
|
||||
// there is a valid connection
|
||||
|
||||
int n = recv(fd, (char *)command_buffer, 200, 0);
|
||||
if (n < 0) {
|
||||
#ifdef _WIN32
|
||||
int e = WSAGetLastError();
|
||||
if (e == WSAECONNABORTED)
|
||||
fprintf(stderr, "Connection aborted.\n");
|
||||
else if (e == WSAECONNRESET)
|
||||
fprintf(stderr, "Connection reset.\n");
|
||||
else
|
||||
fprintf(stderr, "Error reading from socket: %d.\n", e);
|
||||
#else
|
||||
if (errno)
|
||||
fprintf(stderr, "Error reading from socket: %d.\n", errno);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
if (n==0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (command_buffer[0] == 'e')
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (n > 0)
|
||||
{
|
||||
|
||||
//printf("Received %d bytes:\n", n);
|
||||
command_buffer[n] = 0;
|
||||
parse_controls (command_buffer);
|
||||
update_controls();
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
socket_cleanup();
|
||||
}
|
||||
|
||||
|
||||
void initialize (int argc, char *argv[])
|
||||
{
|
||||
|
||||
fd_set rfds;
|
||||
|
||||
port = 5599; // default port
|
||||
for (int i = 0; i < argc; ++i)
|
||||
{
|
||||
if (strcmp (argv[i],"-p")==0)
|
||||
{
|
||||
if (argc > i+1 )
|
||||
{
|
||||
port = atoi (argv[i+1]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sfd = create_socket_server(port);
|
||||
|
||||
/* necessary to initialize webots stuff */
|
||||
wb_robot_init();
|
||||
|
||||
// Get WorldInfo Node.
|
||||
WbNodeRef root, node;
|
||||
WbFieldRef children, field;
|
||||
int n, i;
|
||||
root = wb_supervisor_node_get_root();
|
||||
children = wb_supervisor_node_get_field(root, "children");
|
||||
n = wb_supervisor_field_get_count(children);
|
||||
printf("This world contains %d nodes:\n", n);
|
||||
for (i = 0; i < n; i++) {
|
||||
node = wb_supervisor_field_get_mf_node(children, i);
|
||||
if (wb_supervisor_node_get_type(node) == WB_NODE_WORLD_INFO)
|
||||
{
|
||||
world_info = node;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
node = wb_supervisor_field_get_mf_node(children, 0);
|
||||
field = wb_supervisor_node_get_field(node, "northDirection");
|
||||
northDirection = wb_supervisor_field_get_sf_vec3f(field);
|
||||
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
printf ("Axis Default Directions");
|
||||
}
|
||||
|
||||
printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]);
|
||||
|
||||
|
||||
// get Self Node
|
||||
self_node = wb_supervisor_node_get_self();
|
||||
|
||||
|
||||
// keybaard
|
||||
timestep = (int)wb_robot_get_basic_time_step();
|
||||
wb_keyboard_enable(timestep);
|
||||
|
||||
|
||||
|
||||
// inertialUnit
|
||||
inertialUnit = wb_robot_get_device("inertial_unit");
|
||||
wb_inertial_unit_enable(inertialUnit, timestep);
|
||||
|
||||
// gyro
|
||||
gyro = wb_robot_get_device("gyro1");
|
||||
wb_gyro_enable(gyro, timestep);
|
||||
|
||||
// accelerometer
|
||||
accelerometer = wb_robot_get_device("accelerometer1");
|
||||
wb_accelerometer_enable(accelerometer, timestep);
|
||||
|
||||
// compass
|
||||
compass = wb_robot_get_device("compass1");
|
||||
wb_compass_enable(compass, timestep);
|
||||
|
||||
// gps
|
||||
gps = wb_robot_get_device("gps1");
|
||||
wb_gps_enable(gps, timestep);
|
||||
|
||||
// camera
|
||||
camera = wb_robot_get_device("camera1");
|
||||
wb_camera_enable(camera, timestep);
|
||||
|
||||
#ifdef WIND_SIMULATION
|
||||
// emitter
|
||||
emitter = wb_robot_get_device("emitter_plugin");
|
||||
#endif
|
||||
|
||||
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3"};
|
||||
|
||||
// get motor device tags
|
||||
for (i = 0; i < MOTOR_NUM; i++) {
|
||||
motors[i] = wb_robot_get_device(MOTOR_NAMES[i]);
|
||||
v[i] = 0.0f;
|
||||
//assert(motors[i]);
|
||||
}
|
||||
|
||||
servo = wb_robot_get_device("servo_tail");
|
||||
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(sfd, &rfds);
|
||||
}
|
||||
/*
|
||||
* This is the main program.
|
||||
* The arguments of the main function can be specified by the
|
||||
* "controllerArgs" field of the Robot node
|
||||
*/
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
|
||||
|
||||
|
||||
initialize( argc, argv);
|
||||
|
||||
/*
|
||||
* You should declare here WbDeviceTag variables for storing
|
||||
* robot devices like this:
|
||||
* WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
|
||||
* WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
|
||||
*/
|
||||
|
||||
/* main loop
|
||||
* Perform simulation steps of TIME_STEP milliseconds
|
||||
* and leave the loop when the simulation is over
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* Read the sensors :
|
||||
* Enter here functions to read sensor data, like:
|
||||
* double val = wb_distance_sensor_get_value(my_sensor);
|
||||
*/
|
||||
|
||||
/* Process sensor data here */
|
||||
|
||||
/*
|
||||
* Enter here functions to send actuator commands, like:
|
||||
* wb_differential_wheels_set_speed(100.0,100.0);
|
||||
*/
|
||||
run();
|
||||
|
||||
|
||||
/* Enter your cleanup code here */
|
||||
|
||||
/* This is necessary to cleanup webots resources */
|
||||
wb_robot_cleanup();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
//#define DEBUG_MOTORS
|
||||
// #define DEBUG_SENSORS
|
||||
//#define DEBUG_USE_KB
|
||||
//#define DEBUG_INPUT_DATA
|
||||
// #define LINEAR_THRUST
|
||||
//#define WIND_SIMULATION
|
||||
|
||||
|
||||
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
|
||||
|
||||
|
||||
enum data_type {
|
||||
DATA_FLOAT,
|
||||
DATA_DOUBLE,
|
||||
DATA_VECTOR4F,
|
||||
};
|
||||
|
||||
struct vector4f
|
||||
{
|
||||
float w,x,y,z;
|
||||
};
|
||||
|
||||
|
||||
typedef struct vector4f VECTOR4F;
|
||||
|
||||
struct {
|
||||
double timestamp;
|
||||
VECTOR4F motors;
|
||||
VECTOR4F wind;
|
||||
/*
|
||||
struct {
|
||||
float speed; // m/s
|
||||
float direction; // degrees 0..360
|
||||
float turbulence;
|
||||
float dir_z; //degrees -90..90
|
||||
} wind;
|
||||
*/
|
||||
} state, last_state;
|
||||
|
||||
|
||||
|
||||
// table to aid parsing of JSON sensor data
|
||||
struct keytable {
|
||||
const char *section;
|
||||
const char *key;
|
||||
void *ptr;
|
||||
enum data_type type;
|
||||
|
||||
} keytable[2] = {
|
||||
//{ "", "timestamp", &state.timestamp, DATA_DOUBLE },
|
||||
{ "", "eng", &state.motors, DATA_VECTOR4F }, /*right,left,servo,back*/
|
||||
{ "", "wnd", &state.wind, DATA_VECTOR4F } /*speed,x,y,z in Ardupilot AXIS*/
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
w: wind speed
|
||||
x , y, z: wind direction.
|
||||
*/
|
||||
VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis;
|
||||
|
||||
|
|
@ -0,0 +1,194 @@
|
|||
#include <stdio.h>
|
||||
#include <sys/time.h>
|
||||
#include <webots/supervisor.h>
|
||||
#include "sensors.h"
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
#define M_PI2 6.28318530718
|
||||
|
||||
|
||||
/*
|
||||
https://discuss.ardupilot.org/t/copter-x-y-z-which-is-which/6823/2
|
||||
|
||||
Copy pasted what’s important:
|
||||
NED Coordinate System:
|
||||
|
||||
The x axis is aligned with the vector to the north pole (tangent to meridians).
|
||||
The y axis points to the east side (tangent to parallels)
|
||||
The z axis points to the center of the earth
|
||||
There is also Body Fixed Frame:
|
||||
Body Fixed Frame (Attached to the aircraft)
|
||||
|
||||
The x axis points in forward (defined by geometry and not by movement) direction. (= roll axis)
|
||||
The y axis points to the right (geometrically) (= pitch axis)
|
||||
The z axis points downwards (geometrically) (= yaw axis)
|
||||
In order to convert from Body Frame to NED what you need to call this function:
|
||||
|
||||
copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
|
||||
|
||||
|
||||
|
||||
|
||||
*/
|
||||
/*
|
||||
returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}}
|
||||
*/
|
||||
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf)
|
||||
{
|
||||
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]);
|
||||
}
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
|
||||
*/
|
||||
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf)
|
||||
{
|
||||
const double *north3D = wb_compass_get_values(compass);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426},
|
||||
*/
|
||||
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf)
|
||||
{
|
||||
|
||||
const double *north3D = wb_gps_get_values(gps);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
|
||||
*/
|
||||
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf)
|
||||
{
|
||||
//SHOULD BE CORRECT
|
||||
const double *a = wb_accelerometer_get_values(accelerometer);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
|
||||
|
||||
//sprintf(buf,"[0.0, 0.0, 0.0]");
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09]
|
||||
*/
|
||||
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf)
|
||||
{
|
||||
|
||||
const double *g = wb_gyro_get_values(gyro);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
|
||||
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf)
|
||||
{
|
||||
linear_velocity = wb_supervisor_node_get_velocity (nodeRef);
|
||||
if (linear_velocity != NULL)
|
||||
{
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit)
|
||||
{
|
||||
|
||||
/*
|
||||
{"timestamp": 1563544049.2840538,
|
||||
"vehicle.imu": {"timestamp": 1563544049.2673872,
|
||||
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
|
||||
"linear_acceleration": [0.005077465437352657, 0.22471386194229126, 9.807389259338379],
|
||||
"magnetic_field": [23088.71484375, 3875.498046875, -53204.48046875]},
|
||||
"vehicle.gps": {
|
||||
"timestamp": 1563544049.2673872,
|
||||
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635},
|
||||
"vehicle.velocity": {"timestamp": 1563544049.2673872,
|
||||
"linear_velocity": [-3.12359499377024e-10, -1.3824124955874595e-08, -6.033386625858839e-07],
|
||||
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
|
||||
"world_linear_velocity": [0.0, 0.0, -6.034970851942489e-07]},
|
||||
"vehicle.pose": {"timestamp": 1563544049.2673872,
|
||||
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635, "yaw": 8.899446402210742e-05, "pitch": -0.0005175824626348913, "roll": 0.022908702492713928}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
static char compass_buf [150];
|
||||
static char acc_buf [150];
|
||||
static char gyro_buf [150];
|
||||
static char gps_buf [150];
|
||||
static char inertial_buf [150];
|
||||
static char linear_velocity_buf [150];
|
||||
|
||||
char szTime[21];
|
||||
double time = wb_robot_get_time(); // current simulation time in [s]
|
||||
sprintf(szTime,"%f", time);
|
||||
|
||||
getGyro(gyro, northDirection, gyro_buf);
|
||||
getAcc(accelerometer, northDirection, acc_buf);
|
||||
getCompass(compass, northDirection, compass_buf);
|
||||
getGPS(gps, northDirection, gps_buf);
|
||||
getInertia (inertial_unit, northDirection, inertial_buf);
|
||||
getLinearVelocity(self_node, northDirection, linear_velocity_buf);
|
||||
|
||||
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
|
||||
|
||||
#include <webots/robot.h>
|
||||
#include <webots/keyboard.h>
|
||||
#include <webots/compass.h>
|
||||
#include <webots/accelerometer.h>
|
||||
#include <webots/inertial_unit.h>
|
||||
#include <webots/gps.h>
|
||||
#include <webots/gyro.h>
|
||||
#include <webots/motor.h>
|
||||
#include <webots/camera.h>
|
||||
|
||||
|
||||
WbNodeRef self_node;
|
||||
double *linear_velocity;
|
||||
|
||||
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf);
|
||||
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf);
|
||||
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf);
|
||||
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf);
|
||||
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf);
|
||||
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf);
|
||||
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit);
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
|
||||
#include "sockets.h"
|
||||
|
||||
|
||||
bool socket_init() {
|
||||
#ifdef _WIN32 /* initialize the socket API */
|
||||
WSADATA info;
|
||||
if (WSAStartup(MAKEWORD(1, 1), &info) != 0) {
|
||||
fprintf(stderr, "Cannot initialize Winsock.\n");
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool socket_set_non_blocking(int fd) {
|
||||
if (fd < 0)
|
||||
return false;
|
||||
#ifdef _WIN32
|
||||
unsigned long mode = 1;
|
||||
return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false;
|
||||
#else
|
||||
int flags = fcntl(fd, F_GETFL, 0) | O_NONBLOCK;
|
||||
return (fcntl(fd, F_SETFL, flags) == 0) ? true : false;
|
||||
#endif
|
||||
}
|
||||
|
||||
int socket_accept(int server_fd) {
|
||||
int cfd;
|
||||
struct sockaddr_in client;
|
||||
struct hostent *client_info;
|
||||
#ifndef _WIN32
|
||||
socklen_t asize;
|
||||
#else
|
||||
int asize;
|
||||
#endif
|
||||
asize = sizeof(struct sockaddr_in);
|
||||
cfd = accept(server_fd, (struct sockaddr *)&client, &asize);
|
||||
if (cfd == -1) {
|
||||
#ifdef _WIN32
|
||||
int e = WSAGetLastError();
|
||||
if (e == WSAEWOULDBLOCK)
|
||||
return 0;
|
||||
fprintf(stderr, "Accept error: %d.\n", e);
|
||||
#else
|
||||
if (errno == EWOULDBLOCK)
|
||||
return 0;
|
||||
fprintf(stderr, "Accept error: %d.\n", errno);
|
||||
#endif
|
||||
return -1;
|
||||
}
|
||||
client_info = gethostbyname((char *)inet_ntoa(client.sin_addr));
|
||||
printf("Accepted connection from: %s.\n", client_info->h_name);
|
||||
return cfd;
|
||||
}
|
||||
|
||||
bool socket_close(int fd) {
|
||||
#ifdef _WIN32
|
||||
return (closesocket(fd) == 0) ? true : false;
|
||||
#else
|
||||
return (close(fd) == 0) ? true : false;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool socket_cleanup() {
|
||||
#ifdef _WIN32
|
||||
return (WSACleanup() == 0) ? true : false;
|
||||
#else
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Creates a socket and bind it to port.
|
||||
*/
|
||||
int create_socket_server(int port) {
|
||||
int sfd, rc;
|
||||
struct sockaddr_in address;
|
||||
if (!socket_init())
|
||||
{
|
||||
fprintf (stderr, "socket_init failed");
|
||||
return -1;
|
||||
}
|
||||
sfd = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sfd == -1) {
|
||||
fprintf(stderr, "Cannot create socket.\n");
|
||||
return -1;
|
||||
}
|
||||
int one = 1;
|
||||
setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
|
||||
memset(&address, 0, sizeof(struct sockaddr_in));
|
||||
address.sin_family = AF_INET;
|
||||
address.sin_port = htons((unsigned short)port);
|
||||
address.sin_addr.s_addr = INADDR_ANY;
|
||||
rc = bind(sfd, (struct sockaddr *)&address, sizeof(struct sockaddr));
|
||||
if (rc == -1) {
|
||||
fprintf(stderr, "Cannot bind port %d.\n", port);
|
||||
socket_close(sfd);
|
||||
return -1;
|
||||
}
|
||||
if (listen(sfd, 1) == -1) {
|
||||
fprintf(stderr, "Cannot listen for connections.\n");
|
||||
socket_close(sfd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
printf ("socket initialized at port %d.\n", port);
|
||||
return sfd;
|
||||
}
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <winsock.h>
|
||||
#else
|
||||
#include <arpa/inet.h> /* definition of inet_ntoa */
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <netdb.h> /* definition of gethostbyname */
|
||||
#include <netinet/in.h> /* definition of struct sockaddr_in */
|
||||
#include <stdlib.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h> /* definition of close */
|
||||
#endif
|
||||
|
||||
int create_socket_server(int port);
|
||||
bool socket_cleanup();
|
||||
int socket_accept(int server_fd);
|
||||
bool socket_set_non_blocking(int fd);
|
||||
|
||||
int fd;
|
||||
int sfd;
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/bin/bash
|
||||
|
||||
# assume we start the script from the root directory
|
||||
ROOTDIR=$PWD
|
||||
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-tri --add-param-file=libraries/SITL/examples/Webots/tricopter.parm
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
FRAME_CLASS 7.000000
|
||||
FRAME_TYPE 0.000000
|
||||
ATC_ANG_PIT_P 1.0
|
||||
ATC_ANG_RLL_P 1.0
|
||||
ATC_ANG_YAW_P 0.5
|
||||
ATC_RAT_PIT_P 3.5
|
||||
ATC_RAT_RLL_P 3.5
|
||||
ATC_RAT_YAW_P 1.5
|
||||
ATC_RAT_YAW_I 0.03
|
||||
ATC_RAT_YAW_IMAX 0.50000
|
||||
SIM_WIND_DIR 90.0
|
||||
SIM_WIND_SPD 0.0
|
||||
SIM_WIND_TURB 0.0
|
||||
|
|
@ -0,0 +1,286 @@
|
|||
#VRML_SIM R2019b utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 2
|
||||
FPS 25
|
||||
optimalThreadCount 4
|
||||
randomSeed 52
|
||||
}
|
||||
DogHouse {
|
||||
translation 34.82 0.76 -24.56
|
||||
name "dog house(1)"
|
||||
}
|
||||
DogHouse {
|
||||
translation 161.819 0.75 -152.174
|
||||
name "dog house(2)"
|
||||
}
|
||||
DogHouse {
|
||||
translation 185.42 0.77 48.97
|
||||
name "dog house(5)"
|
||||
}
|
||||
Viewpoint {
|
||||
orientation 0.9997750421775041 -0.014997750480821456 -0.01499775048082146 4.712163997257285
|
||||
position 0.17712971811531414 14.83048200793912 -1.4201693307676047
|
||||
follow "tricopter"
|
||||
}
|
||||
Background {
|
||||
skyColor [
|
||||
0.15 0.5 1
|
||||
]
|
||||
cubemap Cubemap {
|
||||
}
|
||||
}
|
||||
Solid {
|
||||
translation 36.93 0.77 -37.93
|
||||
children [
|
||||
HouseWithGarage {
|
||||
}
|
||||
]
|
||||
}
|
||||
Solid {
|
||||
translation 192.76999999999998 0 64.98
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
children [
|
||||
HouseWithGarage {
|
||||
}
|
||||
]
|
||||
name "solid(1)"
|
||||
}
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -8.233889875751989e-05 0.666500515499142 -1.3598750857814472
|
||||
rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804
|
||||
children [
|
||||
Compass {
|
||||
name "compass1"
|
||||
}
|
||||
Camera {
|
||||
translation 0 0.25 0
|
||||
name "camera1"
|
||||
}
|
||||
Transform {
|
||||
translation -0.34 0 0
|
||||
rotation 0 1 0 1.5707959999999999
|
||||
children [
|
||||
HingeJoint {
|
||||
jointParameters HingeJointParameters {
|
||||
position -9.388038782122357e-12
|
||||
axis 0 0 1
|
||||
}
|
||||
device [
|
||||
RotationalMotor {
|
||||
name "servo_tail"
|
||||
maxVelocity 50000
|
||||
maxTorque 1000
|
||||
}
|
||||
]
|
||||
endPoint Solid {
|
||||
translation -4.884982810326628e-15 -4.1022629410960365e-12 1.8091922030330322e-13
|
||||
rotation 2.3470124341273664e-11 1 -3.708275057969111e-10 1.5707963071795863
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 11.44 0
|
||||
torqueConstants 1e-05 0
|
||||
device RotationalMotor {
|
||||
name "motor3"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 2.238367478129037
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 0 1 0.09999999999999999
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
name "solid(1)"
|
||||
boundingObject Box {
|
||||
size 0.01 0.01 0.01
|
||||
}
|
||||
physics Physics {
|
||||
mass 0.001
|
||||
}
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
Transform {
|
||||
translation 0.17 0 0.3
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -11.443 0
|
||||
torqueConstants 1e-05 0
|
||||
device RotationalMotor {
|
||||
name "motor1"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
Transform {
|
||||
translation 0.16 0 -0.3
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 11.443 0
|
||||
torqueConstants 1e-05 0
|
||||
device RotationalMotor {
|
||||
name "motor2"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
Emitter {
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
name "emitter_plugin"
|
||||
}
|
||||
InertialUnit {
|
||||
name "inertial_unit"
|
||||
}
|
||||
Gyro {
|
||||
name "gyro1"
|
||||
}
|
||||
Accelerometer {
|
||||
name "accelerometer1"
|
||||
}
|
||||
GPS {
|
||||
name "gps1"
|
||||
}
|
||||
Solid {
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
}
|
||||
}
|
||||
geometry Box {
|
||||
size 0.1 0.1 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
boundingObject Box {
|
||||
size 0.1 0.1 0.1
|
||||
}
|
||||
physics Physics {
|
||||
mass 0.6
|
||||
}
|
||||
}
|
||||
]
|
||||
name "tricopter"
|
||||
physics Physics {
|
||||
mass 0.001
|
||||
}
|
||||
controller "ardupilot_SITL_TRICOPTER"
|
||||
supervisor TRUE
|
||||
}
|
||||
DirectionalLight {
|
||||
direction 0 -1 0
|
||||
}
|
||||
UnevenTerrain {
|
||||
size 500 1 500
|
||||
}
|
||||
HouseWithGarage {
|
||||
translation 174.25 1.88 -157.5
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 0 2.35 -5.71
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 84.03999999999999 2.35 -5.81
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
name "advertising board(1)"
|
||||
}
|
||||
Loading…
Reference in New Issue