2020-05-14 03:26:46 -03:00
|
|
|
#VRML_SIM R2020a utf8
|
2019-08-14 09:49:08 -03:00
|
|
|
WorldInfo {
|
|
|
|
gravity 0 -9.80665 0
|
2019-08-20 15:35:35 -03:00
|
|
|
physics "sitl_physics_env"
|
2019-08-14 09:49:08 -03:00
|
|
|
basicTimeStep 2
|
2020-05-14 03:26:46 -03:00
|
|
|
FPS 15
|
2019-08-14 09:49:08 -03:00
|
|
|
optimalThreadCount 4
|
|
|
|
randomSeed 52
|
|
|
|
}
|
2020-05-14 03:26:46 -03:00
|
|
|
Robot {
|
|
|
|
translation -1.949999999999999 0.23391237384410402 0
|
|
|
|
rotation 0 0 1 2.533555103014215e-15
|
|
|
|
children [
|
|
|
|
Emitter {
|
|
|
|
type "serial"
|
|
|
|
channel 1
|
|
|
|
}
|
|
|
|
]
|
|
|
|
name "supervisor_sync"
|
|
|
|
boundingObject Box {
|
|
|
|
size 0.1 0.1 0.1
|
|
|
|
}
|
|
|
|
physics Physics {
|
|
|
|
}
|
|
|
|
controller "ardupilot_SITL_Supervisor"
|
|
|
|
supervisor TRUE
|
|
|
|
}
|
|
|
|
Viewpoint {
|
|
|
|
orientation -0.9919331177114086 -0.11439530181889061 -0.054611399076472875 0.8971103745745552
|
|
|
|
position -0.9553539884549295 11.277792894156029 8.463112348647094
|
|
|
|
follow "quad_plus_sitl"
|
|
|
|
followType "Mounted Shot"
|
|
|
|
}
|
2019-08-14 09:49:08 -03:00
|
|
|
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)"
|
|
|
|
}
|
|
|
|
Background {
|
|
|
|
skyColor [
|
|
|
|
0.15 0.5 1
|
|
|
|
]
|
|
|
|
}
|
|
|
|
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)"
|
|
|
|
}
|
2019-08-20 15:35:35 -03:00
|
|
|
DEF DEF_VEHICLE Robot {
|
2020-05-14 03:26:46 -03:00
|
|
|
translation -0.027601 0.694307 0.005031
|
2019-08-14 09:49:08 -03:00
|
|
|
rotation 0 1 0 0.785388
|
|
|
|
children [
|
2020-05-14 03:26:46 -03:00
|
|
|
Receiver {
|
|
|
|
name "receiver_main"
|
|
|
|
type "serial"
|
|
|
|
channel 1
|
|
|
|
bufferSize 32
|
|
|
|
}
|
2019-08-20 15:35:35 -03:00
|
|
|
Emitter {
|
|
|
|
name "emitter_plugin"
|
|
|
|
description "commuicates with physics plugin"
|
|
|
|
}
|
2019-08-14 09:49:08 -03:00
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry Box {
|
|
|
|
size 0.1 0.1 0.1
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Camera {
|
|
|
|
translation 0 0.12 0
|
|
|
|
rotation 0 -1 0 2.356195
|
|
|
|
name "camera1"
|
|
|
|
width 128
|
|
|
|
height 128
|
|
|
|
}
|
|
|
|
Compass {
|
|
|
|
rotation 0 1 0 -0.7853983071795865
|
|
|
|
name "compass1"
|
|
|
|
}
|
|
|
|
GPS {
|
|
|
|
rotation 0 1 0 -0.785398
|
|
|
|
name "gps1"
|
|
|
|
}
|
|
|
|
Accelerometer {
|
|
|
|
rotation 0 1 0 -0.785398
|
|
|
|
name "accelerometer1"
|
|
|
|
}
|
|
|
|
Gyro {
|
|
|
|
rotation 0 1 0 -0.785398
|
|
|
|
name "gyro1"
|
|
|
|
}
|
|
|
|
InertialUnit {
|
|
|
|
rotation 0 -1 0 0.7853979999999999
|
|
|
|
name "inertial_unit"
|
|
|
|
}
|
|
|
|
Transform {
|
|
|
|
translation -0.09999999999999999 0 0
|
|
|
|
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
|
|
|
|
children [
|
|
|
|
Solid {
|
|
|
|
translation 0 0.35 0
|
|
|
|
rotation 1 0 0 1.5707959999999999
|
|
|
|
children [
|
|
|
|
Propeller {
|
|
|
|
shaftAxis 0 -1 0
|
|
|
|
thrustConstants -12.2583125 0
|
|
|
|
torqueConstants 18 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 1.1667874781290464
|
|
|
|
children [
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
diffuseColor 1 0 0.1
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry Cylinder {
|
|
|
|
height 0.002
|
|
|
|
radius 0.02
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
physics Physics {
|
|
|
|
mass 0.25
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry DEF DEF_ARM Cylinder {
|
|
|
|
height 0.1
|
|
|
|
radius 0.01
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
Transform {
|
|
|
|
translation 0 0 0.09999999999999999
|
|
|
|
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
|
|
|
|
children [
|
|
|
|
Solid {
|
|
|
|
translation 0 0.35 0
|
|
|
|
rotation 1 0 0 1.5707959999999999
|
|
|
|
children [
|
|
|
|
Propeller {
|
|
|
|
shaftAxis 0 1 0
|
|
|
|
thrustConstants 12.2583125 0
|
|
|
|
torqueConstants 18 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 5.370767303526115
|
|
|
|
children [
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
diffuseColor 1 0 0.1
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry Cylinder {
|
|
|
|
height 0.002
|
|
|
|
radius 0.02
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
name "solid(2)"
|
|
|
|
physics Physics {
|
|
|
|
mass 0.25
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
2020-05-14 03:26:46 -03:00
|
|
|
diffuseColor 1 0.09 0
|
2019-08-14 09:49:08 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry USE DEF_ARM
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
Transform {
|
|
|
|
translation 0.09999999999999999 0 0
|
|
|
|
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
|
|
|
|
children [
|
|
|
|
Solid {
|
|
|
|
translation 0 0.35 0
|
|
|
|
rotation 1 0 0 1.5707959999999999
|
|
|
|
children [
|
|
|
|
Propeller {
|
|
|
|
shaftAxis 0 -1 0
|
|
|
|
thrustConstants -12.2583125 0
|
|
|
|
torqueConstants 18 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 5.486397909883531
|
|
|
|
children [
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
diffuseColor 1 0 0.1
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry Cylinder {
|
|
|
|
height 0.002
|
|
|
|
radius 0.02
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
name "solid(1)"
|
|
|
|
physics Physics {
|
|
|
|
mass 0.25
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
diffuseColor 1 0.09999999999999999 0
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry USE DEF_ARM
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
Transform {
|
|
|
|
translation 0 0 -0.09999999999999999
|
|
|
|
rotation 1 0 0 -1.5707963071795863
|
|
|
|
children [
|
|
|
|
Solid {
|
|
|
|
translation 0 0.35 0
|
|
|
|
rotation 1 0 0 1.5707959999999999
|
|
|
|
children [
|
|
|
|
Propeller {
|
|
|
|
shaftAxis 0 1 0
|
|
|
|
thrustConstants 12.2583125 0
|
|
|
|
torqueConstants 18 0
|
|
|
|
device RotationalMotor {
|
|
|
|
name "motor4"
|
|
|
|
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 5.350616673324008
|
|
|
|
children [
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
diffuseColor 1 0 0.1
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry Cylinder {
|
|
|
|
height 0.002
|
|
|
|
radius 0.02
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
}
|
|
|
|
]
|
|
|
|
name "solid(3)"
|
|
|
|
physics Physics {
|
|
|
|
mass 0.25
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Shape {
|
|
|
|
appearance Appearance {
|
|
|
|
material Material {
|
|
|
|
diffuseColor 0.7999999999999999 0.7999999999999999 0.7999999999999999
|
|
|
|
}
|
|
|
|
}
|
|
|
|
geometry USE DEF_ARM
|
|
|
|
}
|
|
|
|
]
|
|
|
|
}
|
|
|
|
]
|
|
|
|
name "quad_x_sitl"
|
|
|
|
boundingObject Box {
|
|
|
|
size 0.1 0.1 0.1
|
|
|
|
}
|
|
|
|
physics Physics {
|
|
|
|
density -1
|
|
|
|
mass 1.5
|
|
|
|
centerOfMass [
|
|
|
|
0 0 0
|
|
|
|
]
|
|
|
|
}
|
|
|
|
rotationStep 0.261799
|
|
|
|
controller "ardupilot_SITL_QUAD"
|
2020-05-14 03:26:46 -03:00
|
|
|
controllerArgs "-p 5577 -df 0.01"
|
|
|
|
customData "1"
|
2019-08-14 09:49:08 -03:00
|
|
|
}
|
|
|
|
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)"
|
|
|
|
}
|