├── .gitattributes
├── Models
    ├── ReadmePhotos
    │   ├── Callback.PNG
    │   ├── CameraView.png
    │   ├── MATLABScript.PNG
    │   ├── ModelComponents.png
    │   └── ModelPhoto.PNG
    ├── obstacleAvoidanceUsingOpticalFlow.slx
    ├── startScript.m
    └── virtualworld
    │   ├── slv3.x3d
    │   ├── uav_arducopter.x3d
    │   ├── uav_environment.x3d
    │   ├── uav_heliport.png
    │   └── uav_protos.x3d
├── README.md
├── SECURITY.md
└── license.txt


/.gitattributes:
--------------------------------------------------------------------------------
1 | # Auto detect text files and perform LF normalization
2 | * text=auto
3 | 


--------------------------------------------------------------------------------
/Models/ReadmePhotos/Callback.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/ReadmePhotos/Callback.PNG


--------------------------------------------------------------------------------
/Models/ReadmePhotos/CameraView.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/ReadmePhotos/CameraView.png


--------------------------------------------------------------------------------
/Models/ReadmePhotos/MATLABScript.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/ReadmePhotos/MATLABScript.PNG


--------------------------------------------------------------------------------
/Models/ReadmePhotos/ModelComponents.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/ReadmePhotos/ModelComponents.png


--------------------------------------------------------------------------------
/Models/ReadmePhotos/ModelPhoto.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/ReadmePhotos/ModelPhoto.PNG


--------------------------------------------------------------------------------
/Models/obstacleAvoidanceUsingOpticalFlow.slx:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/obstacleAvoidanceUsingOpticalFlow.slx


--------------------------------------------------------------------------------
/Models/startScript.m:
--------------------------------------------------------------------------------
 1 | % Obstacle Avoidance using Optical Flow
 2 | % Copyright 2021 The MathWorks, Inc.
 3 | 
 4 | % Model Initialization
 5 | % Start Location [X Z Y]
 6 | dronestartlocation=[-1.5 0 4];
 7 | 
 8 | % Final Location [X Z Y]
 9 | helipadLocation = [14 0.29 4];
10 | 
11 | % Linear Velocity [Vx Vz Vy]
12 | linearVelocity = [0.6 0.05 .2]; 
13 | 
14 | % Angular Velocity during Optical Flow navigation
15 | % represented as [pitch yaw roll]
16 | angularVelocity = [0 0 -0.01]; 
17 | 
18 | % X-coordinate to switch from Optical flow controller to PID Controller
19 | switchingXLimit = 12.0;
20 | 
21 | % Angular Velocity during landing
22 | angularVelocityforLanding = [0 0 0.01];
23 | 
24 | % Hover Height before landing
25 | finalHoverHeight = 0.8;
26 | 
27 | % Threshold distance to execute the PID controller.
28 | delta = 0.1;
29 | 
30 | %Sample Time
31 | Ts = 0.1;
32 | 
33 | 
34 | 
35 | 


--------------------------------------------------------------------------------
/Models/virtualworld/slv3.x3d:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <!DOCTYPE X3D PUBLIC "ISO//Web3D//DTD X3D 3.3//EN" "http://www.web3d.org/specifications/x3d-3.3.dtd">
  3 | <X3D profile='Immersive' version='3.3' xmlns:xsd='http://www.w3.org/2001/XMLSchema-instance' xsd:noNamespaceSchemaLocation='http://www.web3d.org/specifications/x3d-3.3.xsd'>
  4 | <head>
  5 | </head>
  6 | <Scene>
  7 | <ExternProtoDeclare name='Arducopter' url='"uav_arducopter.x3d#Arducopter" '>
  8 | <field name='rotation' type='SFRotation' accessType='inputOutput'/>
  9 | <field name='translation' type='SFVec3f' accessType='inputOutput'/>
 10 | <field name='Shaft_Rot_1' type='SFRotation' accessType='inputOutput'/>
 11 | <field name='Propeller_Mode_1' type='SFInt32' accessType='inputOutput'/>
 12 | <field name='Shaft_Rot_2' type='SFRotation' accessType='inputOutput'/>
 13 | <field name='Propeller_Mode_2' type='SFInt32' accessType='inputOutput'/>
 14 | <field name='Shaft_Rot_3' type='SFRotation' accessType='inputOutput'/>
 15 | <field name='Propeller_Mode_3' type='SFInt32' accessType='inputOutput'/>
 16 | <field name='Shaft_Rot_4' type='SFRotation' accessType='inputOutput'/>
 17 | <field name='Propeller_Mode_4' type='SFInt32' accessType='inputOutput'/>
 18 | 
 19 | </ExternProtoDeclare>
 20 | 
 21 | <ExternProtoDeclare name='Pole' url='"uav_protos.x3d#Pole" '>
 22 | <field name='Color' type='SFColor' accessType='inputOutput'/>
 23 | <field name='Position' type='SFVec3f' accessType='inputOutput'/>
 24 | 
 25 | </ExternProtoDeclare>
 26 | 
 27 | <WorldInfo title='UAV Competition Scene' info='"Copyright 2012-2018 HUMUSOFT s.r.o. and The MathWorks, Inc." '>
 28 | </WorldInfo>
 29 | <ProximitySensor DEF='User'  size='1e+07 1e+07 1e+07'>
 30 | </ProximitySensor>
 31 | <!--avatarSize [0] smaller than default to avoid HUD clipping-->
 32 | <!--visibilityLimit 200000-->
 33 | <NavigationInfo type='"FLY" ' avatarSize='0.2 1.6 0.75'>
 34 | </NavigationInfo>
 35 | <DirectionalLight direction='-1 -1 -1' ambientIntensity='0.5' intensity='0.6'>
 36 | </DirectionalLight>
 37 | <DirectionalLight direction='1 -1 1' ambientIntensity='0.5' intensity='0.6'>
 38 | </DirectionalLight>
 39 | <Transform DEF='Camera_Overview'  translation='11 7 15' rotation='0 1 0 0.4'>
 40 |   <Viewpoint DEF='VP_Overview'  fieldOfView='0.7854' description='Overview' orientation='1 0 0 -0.5' position='0 0 0'>
 41 |   </Viewpoint>
 42 | </Transform>
 43 | <Transform DEF='Camera_Observe'  translation='-2.2 0.1 5.8' rotation='0 1 0 0'>
 44 |   <Transform translation='-1.8 1 0' rotation='0 1 0 -1.5708'>
 45 |     <Viewpoint fieldOfView='1.2' description='Observe UAV' orientation='1 0 0 -0.2' position='0 0 0'>
 46 |     </Viewpoint>
 47 |   </Transform>
 48 | </Transform>
 49 | <Transform DEF='Camera_TO'  translation='-5.5 1.7 7' rotation='0 1 0 -1.2'>
 50 |   <Viewpoint DEF='VP_TakeOff'  fieldOfView='0.7854' description='View on the Take-Off' orientation='1 0 0 -0.2' position='0 0 0'>
 51 |   </Viewpoint>
 52 | </Transform>
 53 | <Viewpoint DEF='VP_Top'  fieldOfView='0.7854' description='View from the Top' orientation='1 0 0 -1.5708' position='6 20 4'>
 54 | </Viewpoint>
 55 | <Transform DEF='Camera_Landing'  translation='22 4 4' rotation='0 1 0 1.5708'>
 56 |   <Viewpoint DEF='VP_Landing'  fieldOfView='0.7854' description='View on the Landing Area' orientation='1 0 0 -0.3' position='0 0 0'>
 57 |   </Viewpoint>
 58 | </Transform>
 59 | <Switch DEF='Triad_Visible' >
 60 |   <Transform DEF='Triad'  scale='10 10 10'>
 61 |     <Shape DEF='Triad_Shape' >
 62 |       <IndexedLineSet DEF='Triad_Geometry'  coordIndex='0 1 -1 0 2 -1 0 3 -1' colorPerVertex='false' colorIndex='0 1 2'>
 63 |         <Coordinate point='0 0 0 1 0 0 0 1 0 0 0 1'>
 64 |         </Coordinate>
 65 |         <Color color='1 0 0 0 1 0 0 0 1'>
 66 |         </Color>
 67 |       </IndexedLineSet>
 68 |     </Shape>
 69 |   </Transform>
 70 | </Switch>
 71 | <Transform DEF='Landing_Platform'  translation='14 0 4'>
 72 |   <Transform DEF='Landing_Block'  translation='0 0.2 0'>
 73 |     <Shape>
 74 |       <Appearance>
 75 |         <Material DEF='Landing_Mat'  diffuseColor='0.5 0.5 0.5'>
 76 |         </Material>
 77 |       </Appearance>
 78 |       <Cylinder radius='0.5' height='0.4'>
 79 |       </Cylinder>
 80 |     </Shape>
 81 |   </Transform>
 82 |   <Transform DEF='Helipad'  translation='0 0.4 0'>
 83 |     <Shape>
 84 |       <Appearance>
 85 |         <TextureTransform translation='-0.2 0.525' scale='0.7 1.05' rotation='-0.5' center='0.5 0.5'>
 86 |         </TextureTransform>
 87 |         <ImageTexture url='"uav_heliport.png" ' repeatS='false' repeatT='false'>
 88 |         </ImageTexture>
 89 |         <Material diffuseColor='0.5 0.5 0.5'>
 90 |         </Material>
 91 |       </Appearance>
 92 |       <Cylinder side='false' radius='0.5' height='0.01' bottom='false'>
 93 |       </Cylinder>
 94 |     </Shape>
 95 |   </Transform>
 96 | </Transform>
 97 | <Transform DEF='Poles'  translation='2 0 0'>
 98 |   <ProtoInstance DEF='Pole_1' name='Pole'>
 99 |   <fieldValue name='Color' value='1 0.5 0'/>
100 |   <fieldValue name='Position' value='-0.5 0 3'/>
101 |   </ProtoInstance>
102 |   <ProtoInstance DEF='Pole_2' name='Pole'>
103 |   <fieldValue name='Color' value='1 0.5 0'/>
104 |   <fieldValue name='Position' value='-0.5 0 5.5'/>
105 |   </ProtoInstance>
106 |   <ProtoInstance DEF='Pole_3' name='Pole'>
107 |   <fieldValue name='Color' value='1 0.5 0'/>
108 |   <fieldValue name='Position' value='1.5 0 1.5'/>
109 |   </ProtoInstance>
110 |   <ProtoInstance DEF='Pole_4' name='Pole'>
111 |   <fieldValue name='Color' value='1 0.5 0'/>
112 |   <fieldValue name='Position' value='1.5 0 6.5'/>
113 |   </ProtoInstance>
114 |   <ProtoInstance DEF='Pole_5' name='Pole'>
115 |   <fieldValue name='Color' value='1 0.5 0'/>
116 |   <fieldValue name='Position' value='2 0 4'/>
117 |   </ProtoInstance>
118 |   <ProtoInstance DEF='Pole_6' name='Pole'>
119 |   <fieldValue name='Color' value='1 0.5 0'/>
120 |   <fieldValue name='Position' value='3 0 2'/>
121 |   </ProtoInstance>
122 |   <ProtoInstance DEF='Pole_7' name='Pole'>
123 |   <fieldValue name='Color' value='1 0.5 0'/>
124 |   <fieldValue name='Position' value='3.2 0 6'/>
125 |   </ProtoInstance>
126 |   <ProtoInstance DEF='Pole_8' name='Pole'>
127 |   <fieldValue name='Color' value='1 0.5 0'/>
128 |   <fieldValue name='Position' value='4 0 4.5'/>
129 |   </ProtoInstance>
130 |   <ProtoInstance DEF='Pole_9' name='Pole'>
131 |   <fieldValue name='Color' value='1 0.5 0'/>
132 |   <fieldValue name='Position' value='9 0 6'/>
133 |   </ProtoInstance>
134 |   <ProtoInstance DEF='Pole_10' name='Pole'>
135 |   <fieldValue name='Color' value='1 0.5 0'/>
136 |   <fieldValue name='Position' value='5.5 0 6'/>
137 |   </ProtoInstance>
138 |   <ProtoInstance DEF='Pole_11' name='Pole'>
139 |   <fieldValue name='Color' value='1 0.5 0'/>
140 |   <fieldValue name='Position' value='6 0 2.5'/>
141 |   </ProtoInstance>
142 |   <ProtoInstance DEF='Pole_12' name='Pole'>
143 |   <fieldValue name='Color' value='1 0.5 0'/>
144 |   <fieldValue name='Position' value='7 0 7'/>
145 |   </ProtoInstance>
146 |   <ProtoInstance DEF='Pole_13' name='Pole'>
147 |   <fieldValue name='Color' value='1 0.5 0'/>
148 |   <fieldValue name='Position' value='8 0 4'/>
149 |   </ProtoInstance>
150 |   <ProtoInstance DEF='Pole_14' name='Pole'>
151 |   <fieldValue name='Color' value='1 0.5 0'/>
152 |   <fieldValue name='Position' value='6 0 4.5'/>
153 |   </ProtoInstance>
154 |   <ProtoInstance DEF='Pole_15' name='Pole'>
155 |   <fieldValue name='Color' value='1 0.5 0'/>
156 |   <fieldValue name='Position' value='8.75 0 1.5'/>
157 |   </ProtoInstance>
158 |   <ProtoInstance DEF='Pole_16' name='Pole'>
159 |   <fieldValue name='Color' value='1 0.5 0'/>
160 |   <fieldValue name='Position' value='5 0 1'/>
161 |   </ProtoInstance>
162 | </Transform>
163 | <Transform DEF='UAV'  translation='-1.5 0 4'>
164 |   <!--translation	0 0 0-->
165 |   <Transform DEF='UAV_Col_Transf'  translation='0 0.16 0' rotation='0 1 0 0.785398'>
166 |     <Shape DEF='UAV_Col_Shape' >
167 |       <Appearance>
168 |         <Material transparency='1'>
169 |         </Material>
170 |       </Appearance>
171 |       <!--										geometry DEF UAV_Collision_Box Cylinder {-->
172 |       <!--												height	0.1-->
173 |       <!--												radius		0.4-->
174 |       <!--										}-->
175 |       <Box DEF='UAV_Collision_Box'  size='0.6 0.1 0.6'>
176 |       </Box>
177 |     </Shape>
178 |     <Transform translation='0 -0.16 0' rotation='0 1 0 -0.785398'>
179 |       <ProtoInstance DEF='UAV_Body' name='Arducopter'>
180 |       <fieldValue name='rotation' value='0 1 0 0'/>
181 |       <fieldValue name='translation' value='0 0 0'/>
182 |       <fieldValue name='Shaft_Rot_1' value='0 1 0 1'/>
183 |       <fieldValue name='Propeller_Mode_1' value='0'/>
184 |       <fieldValue name='Shaft_Rot_2' value='0 1 0 1.5'/>
185 |       <fieldValue name='Propeller_Mode_2' value='0'/>
186 |       <fieldValue name='Shaft_Rot_3' value='0 1 0 2'/>
187 |       <fieldValue name='Propeller_Mode_3' value='0'/>
188 |       <fieldValue name='Shaft_Rot_4' value='0 1 0 2.5'/>
189 |       <fieldValue name='Propeller_Mode_4' value='0'/>
190 |       </ProtoInstance>
191 |     </Transform>
192 |     <PrimitivePickSensor DEF='Sensor_Wall'  intersectionType='GEOMETRY' sortOrder='CLOSEST'>
193 |       <!--										geometry DEF UAV_Collision_Box Cylinder {-->
194 |       <!--												height	0.1-->
195 |       <!--												radius		0.4-->
196 |       <!--										}-->
197 |       <Box USE='UAV_Collision_Box'  containerField='pickingGeometry'>
198 |       </Box>
199 |       <Transform DEF='Start_Wall'  containerField='pickTarget'>
200 |         <Transform DEF='Wall_Block_1'  translation='0.1 0.5 4'>
201 |           <Shape>
202 |             <Appearance>
203 |               <!--specularColor .31 .28 .26-->
204 |               <Material DEF='Wall_Mat'  shininess='0.07' emissiveColor='0 0.1 0.11' diffuseColor='0 0.36 0.42' ambientIntensity='0.0133'>
205 |               </Material>
206 |             </Appearance>
207 |             <Box size='0.2 1 8'>
208 |             </Box>
209 |           </Shape>
210 |         </Transform>
211 |         <Transform DEF='Wall_Block_2'  translation='0.1 2.3 4'>
212 |           <Shape>
213 |             <Appearance>
214 |               <!--specularColor .31 .28 .26-->
215 |               <Material USE='Wall_Mat' >
216 |               </Material>
217 |             </Appearance>
218 |             <Box size='0.2 0.6 8'>
219 |             </Box>
220 |           </Shape>
221 |         </Transform>
222 |         <Transform DEF='Wall_Block_3'  translation='0.1 1.5 0.8'>
223 |           <Shape>
224 |             <Appearance>
225 |               <!--specularColor .31 .28 .26-->
226 |               <Material USE='Wall_Mat' >
227 |               </Material>
228 |             </Appearance>
229 |             <Box size='0.2 1 1.6'>
230 |             </Box>
231 |           </Shape>
232 |         </Transform>
233 |         <Transform DEF='Wall_Block_4'  translation='0.1 1.5 3.8'>
234 |           <Shape>
235 |             <Appearance>
236 |               <!--specularColor .31 .28 .26-->
237 |               <Material USE='Wall_Mat' >
238 |               </Material>
239 |             </Appearance>
240 |             <Box size='0.2 1 2.8'>
241 |             </Box>
242 |           </Shape>
243 |         </Transform>
244 |         <Transform DEF='Wall_Block_5'  translation='0.1 1.5 7.4'>
245 |           <Shape>
246 |             <Appearance>
247 |               <!--specularColor .31 .28 .26-->
248 |               <Material USE='Wall_Mat' >
249 |               </Material>
250 |             </Appearance>
251 |             <Box size='0.2 1 1.2'>
252 |             </Box>
253 |           </Shape>
254 |         </Transform>
255 |       </Transform>
256 |     </PrimitivePickSensor>
257 |     <PrimitivePickSensor DEF='Sensor_Start'  intersectionType='GEOMETRY' sortOrder='CLOSEST'>
258 |       <!--										geometry DEF UAV_Collision_Box Cylinder {-->
259 |       <!--												height	0.1-->
260 |       <!--												radius		0.4-->
261 |       <!--										}-->
262 |       <Box USE='UAV_Collision_Box'  containerField='pickingGeometry'>
263 |       </Box>
264 |       <!--Transparent window used to detect UAV passing the starting window in the wall-->
265 |       <Transform DEF='Start_Window'  containerField='pickTarget'>
266 |         <Shape DEF='Window' >
267 |           <Appearance>
268 |             <Material DEF='Window_Mat'  transparency='1' diffuseColor='0.1 0.8 0.1'>
269 |             </Material>
270 |           </Appearance>
271 |           <IndexedFaceSet coordIndex='0 1 2 3 -1' solid='false'>
272 |             <Coordinate point='0 1 5.2 0 1 6.8 0 2 6.8 0 2 5.2'>
273 |             </Coordinate>
274 |           </IndexedFaceSet>
275 |         </Shape>
276 |       </Transform>
277 |     </PrimitivePickSensor>
278 |     <PrimitivePickSensor DEF='Sensor_Poles'  intersectionType='GEOMETRY' sortOrder='CLOSEST'>
279 |       <!--										geometry DEF UAV_Collision_Box Cylinder {-->
280 |       <!--												height	0.1-->
281 |       <!--												radius		0.4-->
282 |       <!--										}-->
283 |       <Box USE='UAV_Collision_Box'  containerField='pickingGeometry'>
284 |       </Box>
285 |       <Transform USE='Poles'  containerField='pickTarget'>
286 |       </Transform>
287 |     </PrimitivePickSensor>
288 |     <PrimitivePickSensor DEF='Sensor_Landing'  intersectionType='GEOMETRY' sortOrder='CLOSEST'>
289 |       <!--										geometry DEF UAV_Collision_Box Cylinder {-->
290 |       <!--												height	0.1-->
291 |       <!--												radius		0.4-->
292 |       <!--										}-->
293 |       <Box USE='UAV_Collision_Box'  containerField='pickingGeometry'>
294 |       </Box>
295 |       <Transform USE='Helipad'  containerField='pickTarget'>
296 |       </Transform>
297 |     </PrimitivePickSensor>
298 |   </Transform>
299 |   <!--UAV Videocamera-->
300 |   <Transform rotation='0 1 0 -1.5708'>
301 |     <!--Horizontal FoV 0.97228 rad with aspect ratio 4:3 gives 0.7061 rad in shorter angle, per VRML97 standard-->
302 |     <Viewpoint DEF='UAV_Camera'  fieldOfView='0.7061' description='UAV Camera Looking Down' orientation='1 0 0 -1.5708' position='0 0 0'>
303 |     </Viewpoint>
304 |   </Transform>
305 | </Transform>
306 | <Transform DEF='UAV_Competition_Environment' >
307 |   <Inline url='"uav_environment.x3d" '>
308 |   </Inline>
309 | </Transform>
310 | <Transform DEF='HUDPos' >
311 |   <!--HUD Position Offset-->
312 |   <!--Offset from the user to the HUD geometry-->
313 |   <Transform DEF='HUDOffset'  translation='-0.45 0.28 -0.5'>
314 |     <Shape>
315 |       <Appearance>
316 |         <Material diffuseColor='1 1 1'>
317 |         </Material>
318 |       </Appearance>
319 |       <Text DEF='HUDText1'  string='"B" "A" '>
320 |         <FontStyle style='BOLD' size='0.05' family='"TYPEWRITER" '>
321 |         </FontStyle>
322 |       </Text>
323 |     </Shape>
324 |   </Transform>
325 | </Transform>
326 | <ROUTE fromNode='UAV' fromField='translation' toNode='Camera_Observe' toField='translation'/>
327 | 
328 | </Scene>
329 | </X3D>
330 | 


--------------------------------------------------------------------------------
/Models/virtualworld/uav_arducopter.x3d:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <!DOCTYPE X3D PUBLIC "ISO//Web3D//DTD X3D 3.3//EN" "http://www.web3d.org/specifications/x3d-3.3.dtd">
  3 | <X3D profile='Immersive' version='3.3' xmlns:xsd='http://www.w3.org/2001/XMLSchema-instance' xsd:noNamespaceSchemaLocation='http://www.web3d.org/specifications/x3d-3.3.xsd'>
  4 | <head>
  5 | </head>
  6 | <!--Arducopter model-->
  7 | <!--PROTO to be used in the UAV scene-->
  8 | <!--Copyright 2012-2018 HUMUSOFT s.r.o. and The MathWorks, Inc.-->
  9 | <Scene>
 10 | <ProtoDeclare name='Copter_Arm'>
 11 | <ProtoInterface>
 12 | <field name='Shaft_Rotation' type='SFRotation' accessType='inputOutput' value='0 1 0 0'/>
 13 | <field name='Propeller_Mode' type='SFInt32' accessType='inputOutput' value='0'/>
 14 | <field name='Arm_Rot' type='SFRotation' accessType='initializeOnly' value='0 1 0 0'/>
 15 | <field name='Prop_Right_Left' type='SFInt32' accessType='initializeOnly' value='0'/>
 16 | <field name='Rod_Red_Gray' type='SFInt32' accessType='initializeOnly' value='0'/>
 17 | </ProtoInterface>
 18 | <ProtoBody>
 19 |   <Transform>
 20 |   <IS>
 21 |   <connect protoField='Arm_Rot' nodeField='rotation' />
 22 |   </IS>
 23 |     <Transform DEF='Rod'  translation='0.15 0 0'>
 24 |       <Switch>
 25 |       <IS>
 26 |       <connect protoField='Rod_Red_Gray' nodeField='whichChoice' />
 27 |       </IS>
 28 |         <!--red rod-->
 29 |         <Shape>
 30 |           <Appearance>
 31 |             <Material emissiveColor='0.2 0 0' diffuseColor='0.7 0.1 0.1'>
 32 |             </Material>
 33 |           </Appearance>
 34 |           <Box size='0.3 0.016 0.016'>
 35 |           </Box>
 36 |         </Shape>
 37 |         <!--gray rod-->
 38 |         <Shape>
 39 |           <Appearance>
 40 |             <Material specularColor='0.63 0.92 1' diffuseColor='0.102 0.506 0.784' ambientIntensity='0'>
 41 |             </Material>
 42 |           </Appearance>
 43 |           <Box size='0.3 0.016 0.016'>
 44 |           </Box>
 45 |         </Shape>
 46 |       </Switch>
 47 |     </Transform>
 48 |     <Transform DEF='Plexi_Arc'  translation='0.015 0.008 0.008'>
 49 |       <Shape>
 50 |         <Appearance DEF='Plexi' >
 51 |           <Material transparency='0.5' specularColor='1 0.94 0.54' shininess='0.83' emissiveColor='0.4 0.4 0.4' diffuseColor='0.5 0.5 0.5' ambientIntensity='0'>
 52 |           </Material>
 53 |         </Appearance>
 54 |         <Extrusion spine='0 0 0.002 0 0 0' crossSection='0.072 0 0.0718 0.0056 0.0711 0.0113 0.07 0.0168 0.0685 0.0222 0.0665 0.0276 0.0642 0.0327 0.0614 0.0376 0.0582 0.0423 0.0547 0.0468 0.0509 0.0509 0.0468 0.0547 0.0423 0.0582 0.0376 0.0614 0.0327 0.0642 0.0276 0.0665 0.0222 0.0685 0.0168 0.07 0.0113 0.0711 0.0056 0.0718 0 0.072 0 0.084 0.0066 0.0837 0.0131 0.083 0.0196 0.0817 0.026 0.0799 0.0321 0.0776 0.0381 0.0748 0.0439 0.0716 0.0494 0.068 0.0546 0.0639 0.0594 0.0594 0.0639 0.0546 0.068 0.0494 0.0716 0.0439 0.0748 0.0381 0.0776 0.0321 0.0799 0.026 0.0817 0.0196 0.083 0.0131 0.0837 0.0066 0.084 0 0.084 -0.016 0.078 -0.05 0.072 -0.05 0.072 0'>
 55 |         </Extrusion>
 56 |       </Shape>
 57 |     </Transform>
 58 |     <Transform DEF='Propeller_Assembly'  translation='0.3 0.015 0'>
 59 |       <!--Main motor-->
 60 |       <Transform>
 61 |         <Shape>
 62 |           <Appearance DEF='Black_Metal' >
 63 |             <Material specularColor='0.5 0.5 0.5' diffuseColor='0 0 0' ambientIntensity='0'>
 64 |             </Material>
 65 |           </Appearance>
 66 |           <Cylinder radius='0.019' height='0.015'>
 67 |           </Cylinder>
 68 |         </Shape>
 69 |         <Transform translation='0 0.025 0'>
 70 |           <Shape>
 71 |             <Appearance DEF='Blue_Metal' >
 72 |               <Material specularColor='0.63 0.92 1' diffuseColor='0 0 0.5' ambientIntensity='0'>
 73 |               </Material>
 74 |             </Appearance>
 75 |             <Cylinder radius='0.019' height='0.035'>
 76 |             </Cylinder>
 77 |           </Shape>
 78 |         </Transform>
 79 |       </Transform>
 80 |       <!--Main shaft-->
 81 |       <!--rotation 1 0 0 1.5708-->
 82 |       <Transform translation='0 0.05 0'>
 83 |         <Shape>
 84 |           <Appearance DEF='Aluminium' >
 85 |             <Material specularColor='0.39 0.39 0.39' shininess='0.11' diffuseColor='0.51 0.51 0.51' ambientIntensity='0.25'>
 86 |             </Material>
 87 |           </Appearance>
 88 |           <Cylinder radius='0.004' height='0.02'>
 89 |           </Cylinder>
 90 |         </Shape>
 91 |       </Transform>
 92 |       <!--Main propeller bed-->
 93 |       <!--rotation 1 0 0 1.5708-->
 94 |       <Transform translation='0 0.05 0'>
 95 |         <Shape>
 96 |           <Appearance USE='Aluminium' >
 97 |           </Appearance>
 98 |           <Cylinder radius='0.009' height='0.004'>
 99 |           </Cylinder>
100 |         </Shape>
101 |       </Transform>
102 |       <!--Main Rotor-->
103 |       <!--end of switch-->
104 |       <Transform translation='0 0.05 0'>
105 |         <Switch DEF='Rotor_Mode' >
106 |         <IS>
107 |         <connect protoField='Propeller_Mode' nodeField='whichChoice' />
108 |         </IS>
109 |           <!--end of choice 1 - cylinder approximation only-->
110 |           <!--full rotor - rotation to be controlled-->
111 |           <Transform DEF='MainRotor' >
112 |           <IS>
113 |           <connect protoField='Shaft_Rotation' nodeField='rotation' />
114 |           </IS>
115 |             <Group DEF='TwoBlades' >
116 |               <Switch DEF='Right_Left' >
117 |               <IS>
118 |               <connect protoField='Prop_Right_Left' nodeField='whichChoice' />
119 |               </IS>
120 |                 <Transform DEF='Blades_Right' >
121 |                   <Transform DEF='Blade_1R'  translation='0.0045 0.00375 0.006' scale='1 2 2' rotation='1 0 0 1.83'>
122 |                     <Shape DEF='Blade1R_Shape' >
123 |                       <Appearance DEF='BladeColor' >
124 |                         <Material specularColor='0.1 0.1 0.1' emissiveColor='0.1 0.1 0.1' diffuseColor='0.1 0.1 0.1' ambientIntensity='0'>
125 |                         </Material>
126 |                       </Appearance>
127 |                       <Extrusion spine='0.0008 0 0 0.1 0 0' crossSection='-0.002 0 0.0012 -0.0002 0.002 -0.0004 0.0032 -0.0006 0.0044 -0.0007 0.0056 -0.0006 0.006 -0.0005 0.0064 -0.0004 0.00662 0 0.0064 0.00032 0.006 0.00036 0.0056 0.0004 0.0044 0.0004 0.0032 0.0004 0.002 0.00036 0.0012 0.0002 -0.002 0' creaseAngle='0.8' ccw='false'>
128 |                       </Extrusion>
129 |                     </Shape>
130 |                   </Transform>
131 |                   <Transform DEF='Blade_2R'  translation='-0.0045 0.00375 -0.006' scale='1 2 2' rotation='1 0 0 1.31'>
132 |                     <Shape DEF='Blade2R_Shape' >
133 |                       <Appearance USE='BladeColor' >
134 |                       </Appearance>
135 |                       <Extrusion spine='-0.0008 0 0 -0.1 0 0' crossSection='-0.002 0 0.0012 -0.0002 0.002 -0.0004 0.0032 -0.0006 0.0044 -0.0007 0.0056 -0.0006 0.006 -0.0005 0.0064 -0.0004 0.00662 0 0.0064 0.00032 0.006 0.00036 0.0056 0.0004 0.0044 0.0004 0.0032 0.0004 0.002 0.00036 0.0012 0.0002 -0.002 0' creaseAngle='0.8' ccw='false'>
136 |                       </Extrusion>
137 |                     </Shape>
138 |                   </Transform>
139 |                 </Transform>
140 |                 <Transform DEF='Blades_Left' >
141 |                   <Transform DEF='Blade_1L'  translation='-0.105 0.00375 0.006' scale='1 2 2' rotation='1 0 0 1.83'>
142 |                     <Shape DEF='Blade1L_Shape' >
143 |                       <Appearance USE='BladeColor' >
144 |                       </Appearance>
145 |                       <Extrusion spine='0.0008 0 0 0.1 0 0' crossSection='-0.002 0 0.0012 -0.0002 0.002 -0.0004 0.0032 -0.0006 0.0044 -0.0007 0.0056 -0.0006 0.006 -0.0005 0.0064 -0.0004 0.00662 0 0.0064 0.00032 0.006 0.00036 0.0056 0.0004 0.0044 0.0004 0.0032 0.0004 0.002 0.00036 0.0012 0.0002 -0.002 0' creaseAngle='0.8' ccw='false'>
146 |                       </Extrusion>
147 |                     </Shape>
148 |                   </Transform>
149 |                   <Transform DEF='Blade_2LR'  translation='0.105 0.00375 -0.006' scale='1 2 2' rotation='1 0 0 1.31'>
150 |                     <Shape DEF='Blade2L_Shape' >
151 |                       <Appearance USE='BladeColor' >
152 |                       </Appearance>
153 |                       <Extrusion spine='-0.0008 0 0 -0.1 0 0' crossSection='-0.002 0 0.0012 -0.0002 0.002 -0.0004 0.0032 -0.0006 0.0044 -0.0007 0.0056 -0.0006 0.006 -0.0005 0.0064 -0.0004 0.00662 0 0.0064 0.00032 0.006 0.00036 0.0056 0.0004 0.0044 0.0004 0.0032 0.0004 0.002 0.00036 0.0012 0.0002 -0.002 0' creaseAngle='0.8' ccw='false'>
154 |                       </Extrusion>
155 |                     </Shape>
156 |                   </Transform>
157 |                 </Transform>
158 |               </Switch>
159 |             </Group>
160 |           </Transform>
161 |           <!--end of choice 0 - static rotor-->
162 |           <Group>
163 |             <!--Main propeller cylinder approximation-->
164 |             <!--rotation 1 0 0 1.5708-->
165 |             <Transform translation='0 0.005 0'>
166 |               <Shape>
167 |                 <Appearance>
168 |                   <Material transparency='0.8' specularColor='0.2 0.2 0.2' shininess='0.05' diffuseColor='0.1 0.1 0.1'>
169 |                   </Material>
170 |                 </Appearance>
171 |                 <Cylinder radius='0.105' height='0.005'>
172 |                 </Cylinder>
173 |               </Shape>
174 |             </Transform>
175 |           </Group>
176 |         </Switch>
177 |       </Transform>
178 |       <!--Main shaft bolt-->
179 |       <!--rotation 1 0 0 1.5708-->
180 |       <Transform translation='0 0.064 0'>
181 |         <Shape>
182 |           <Appearance USE='Aluminium' >
183 |           </Appearance>
184 |           <Cone height='0.012' bottomRadius='0.009'>
185 |           </Cone>
186 |         </Shape>
187 |       </Transform>
188 |     </Transform>
189 |   </Transform>
190 | </ProtoBody></ProtoDeclare>
191 | <ProtoDeclare name='Arducopter'>
192 | <ProtoInterface>
193 | <field name='Propeller_Mode_4' type='SFInt32' accessType='inputOutput' value='0'/>
194 | <field name='Shaft_Rot_4' type='SFRotation' accessType='inputOutput' value='0 1 0 2.5'/>
195 | <field name='Propeller_Mode_3' type='SFInt32' accessType='inputOutput' value='0'/>
196 | <field name='Shaft_Rot_3' type='SFRotation' accessType='inputOutput' value='0 1 0 2'/>
197 | <field name='Propeller_Mode_2' type='SFInt32' accessType='inputOutput' value='0'/>
198 | <field name='Shaft_Rot_2' type='SFRotation' accessType='inputOutput' value='0 1 0 1.5'/>
199 | <field name='Propeller_Mode_1' type='SFInt32' accessType='inputOutput' value='0'/>
200 | <field name='Shaft_Rot_1' type='SFRotation' accessType='inputOutput' value='0 1 0 1'/>
201 | <field name='translation' type='SFVec3f' accessType='inputOutput' value='0 0 0'/>
202 | <field name='rotation' type='SFRotation' accessType='inputOutput' value='0 1 0 0'/>
203 | </ProtoInterface>
204 | <ProtoBody>
205 |   <Transform DEF='ACopter' >
206 |   <IS>
207 |   <connect protoField='translation' nodeField='translation' />
208 |   <connect protoField='rotation' nodeField='rotation' />
209 |   </IS>
210 |     <!--End of body-->
211 |     <Transform DEF='Body_Offset'  translation='0 0.1 0'>
212 |       <!--Place the copter on the surface - Y offset if any-->
213 |       <Transform DEF='Copter_Body'  translation='0 0.042 0'>
214 |         <ProtoInstance DEF='Arm_1' name='Copter_Arm'>
215 |         <fieldValue name='Arm_Rot' value='0 1 0 0'/>
216 |         <fieldValue name='Prop_Right_Left' value='1'/>
217 |         <fieldValue name='Rod_Red_Gray' value='0'/>
218 |         <IS>
219 |         <connect protoField='Shaft_Rot_1' nodeField='Shaft_Rotation' />
220 |         <connect protoField='Propeller_Mode_1' nodeField='Propeller_Mode' />
221 |         </IS>
222 |         </ProtoInstance>
223 |         <ProtoInstance DEF='Arm_2' name='Copter_Arm'>
224 |         <fieldValue name='Arm_Rot' value='0 1 0 1.5708'/>
225 |         <fieldValue name='Prop_Right_Left' value='0'/>
226 |         <fieldValue name='Rod_Red_Gray' value='1'/>
227 |         <IS>
228 |         <connect protoField='Shaft_Rot_2' nodeField='Shaft_Rotation' />
229 |         <connect protoField='Propeller_Mode_2' nodeField='Propeller_Mode' />
230 |         </IS>
231 |         </ProtoInstance>
232 |         <ProtoInstance DEF='Arm_3' name='Copter_Arm'>
233 |         <fieldValue name='Arm_Rot' value='0 1 0 3.14159'/>
234 |         <fieldValue name='Prop_Right_Left' value='1'/>
235 |         <fieldValue name='Rod_Red_Gray' value='1'/>
236 |         <IS>
237 |         <connect protoField='Shaft_Rot_3' nodeField='Shaft_Rotation' />
238 |         <connect protoField='Propeller_Mode_3' nodeField='Propeller_Mode' />
239 |         </IS>
240 |         </ProtoInstance>
241 |         <ProtoInstance DEF='Arm_4' name='Copter_Arm'>
242 |         <fieldValue name='Arm_Rot' value='0 1 0 4.71239'/>
243 |         <fieldValue name='Prop_Right_Left' value='0'/>
244 |         <fieldValue name='Rod_Red_Gray' value='1'/>
245 |         <IS>
246 |         <connect protoField='Shaft_Rot_4' nodeField='Shaft_Rotation' />
247 |         <connect protoField='Propeller_Mode_4' nodeField='Propeller_Mode' />
248 |         </IS>
249 |         </ProtoInstance>
250 |         <Transform DEF='Body' >
251 |           <Transform DEF='Blue_Box'  translation='0 -0.022 0'>
252 |             <Shape>
253 |               <Appearance>
254 |                 <Material diffuseColor='0.2 0.3 0.8'>
255 |                 </Material>
256 |               </Appearance>
257 |               <Box size='0.12 0.025 0.05'>
258 |               </Box>
259 |             </Shape>
260 |           </Transform>
261 |           <Transform DEF='Rhomb_Base' >
262 |             <Transform DEF='RB_Lower'  translation='0 -0.01 0'>
263 |               <Shape DEF='RB_Shape' >
264 |                 <Appearance DEF='Grey_Metal' >
265 |                   <!--specularColor .29 .29 .29-->
266 |                   <Material shininess='0.11' diffuseColor='0.51 0.51 0.51' ambientIntensity='0.15'>
267 |                   </Material>
268 |                 </Appearance>
269 |                 <Extrusion spine='0 0 0 0 0.002 0' crossSection='0.008 0.08 0.08 0.008 0.08 -0.008 0.008 -0.08 -0.008 -0.08 -0.08 -0.008 -0.08 0.008 -0.008 0.08 0.008 0.08'>
270 |                 </Extrusion>
271 |               </Shape>
272 |             </Transform>
273 |             <Transform DEF='RB_Upper'  translation='0 0.01 0'>
274 |               <Shape USE='RB_Shape' >
275 |               </Shape>
276 |             </Transform>
277 |           </Transform>
278 |           <Transform DEF='Yellow_Box'  translation='0 0.025 0'>
279 |             <Shape>
280 |               <Appearance>
281 |                 <Material diffuseColor='0.7 0.5 0'>
282 |                 </Material>
283 |               </Appearance>
284 |               <Box size='0.09 0.025 0.09'>
285 |               </Box>
286 |             </Shape>
287 |           </Transform>
288 |           <Transform DEF='Gray_Box'  translation='0 0.05 0'>
289 |             <Shape>
290 |               <Appearance>
291 |                 <Material diffuseColor='0.4 0.4 0.4'>
292 |                 </Material>
293 |               </Appearance>
294 |               <Box size='0.07 0.03 0.07'>
295 |               </Box>
296 |             </Shape>
297 |           </Transform>
298 |           <Transform DEF='Plexi_Cylinder'  translation='0 0.086 0'>
299 |             <Shape>
300 |               <Appearance DEF='Plexi' >
301 |                 <Material transparency='0.28' specularColor='1 0.94 0.54' shininess='0.83' emissiveColor='0.4 0.4 0.4' diffuseColor='0.29 0.29 0.29' ambientIntensity='0'>
302 |                 </Material>
303 |               </Appearance>
304 |               <Cylinder radius='0.018' height='0.012'>
305 |               </Cylinder>
306 |             </Shape>
307 |           </Transform>
308 |         </Transform>
309 |       </Transform>
310 |     </Transform>
311 |     <!--LOD that shows a red transparent Sphere from distance &gt; 5m-->
312 |     <LOD range='5'>
313 |       <Group>
314 |       </Group>
315 |       <Transform DEF='Aura' >
316 |         <Shape>
317 |           <Appearance>
318 |             <Material transparency='0.8' diffuseColor='1 0 0'>
319 |             </Material>
320 |           </Appearance>
321 |           <Sphere>
322 |           </Sphere>
323 |         </Shape>
324 |       </Transform>
325 |     </LOD>
326 |     <Transform DEF='Camera_Ride'  translation='-1.8 1 0' rotation='0 1 0 -1.5708'>
327 |       <Viewpoint DEF='VP_Ride'  description='Ride on the UAV Tail' position='0 0 0' orientation='1 0 0 -0.4' fieldOfView='1.2'>
328 |       </Viewpoint>
329 |     </Transform>
330 |     <Transform DEF='Camera_BirdsEye'  rotation='0 1 0 -1.5708'>
331 |       <Viewpoint DEF='VP_BirdsEye'  description='Bird&apos;s Eye' position='0 0.3 0' orientation='1 0 0 -0.2' fieldOfView='1.2'>
332 |       </Viewpoint>
333 |     </Transform>
334 |   </Transform>
335 | </ProtoBody></ProtoDeclare>
336 | </Scene>
337 | </X3D>
338 | 


--------------------------------------------------------------------------------
/Models/virtualworld/uav_environment.x3d:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <!DOCTYPE X3D PUBLIC "ISO//Web3D//DTD X3D 3.3//EN" "http://www.web3d.org/specifications/x3d-3.3.dtd">
  3 | <X3D profile='Immersive' version='3.3' xmlns:xsd='http://www.w3.org/2001/XMLSchema-instance' xsd:noNamespaceSchemaLocation='http://www.web3d.org/specifications/x3d-3.3.xsd'>
  4 | <head>
  5 | </head>
  6 | <!--IMAV scene static environment-->
  7 | <!--Copyright 2012-2018 HUMUSOFT s.r.o. and The MathWorks, Inc.-->
  8 | <Scene>
  9 | <!--DEF SkyDome Transform {-->
 10 | <!--	translation	0 0 0-->
 11 | <!--	children [-->
 12 | <!--		Inline {-->
 13 | <!--			url ["SkyDome.x3d"]-->
 14 | <!--		}-->
 15 | <!--	]-->
 16 | <!--}-->
 17 | <Background groundAngle='0.9 1.5 1.57' groundColor='0 0.8 0 0.17255 0.81961 0.18824 0.46667 0.81961 0.44706 0.62353 0.67059 0.6' skyAngle='0.9 1.5 1.57' skyColor='0 0.2 0.7 0 0.5 0.7 0 0.5 1 0 1 1'>
 18 | </Background>
 19 | <Transform DEF='Static_Objects' >
 20 |   <Transform DEF='Grid' >
 21 |     <Inline url='"*sl3dlib/objects/Components/Grids/Grid_201x201.x3d" '>
 22 |     </Inline>
 23 |   </Transform>
 24 |   <Shape DEF='Ground' >
 25 |     <!--repeatS	TRUE-->
 26 |     <Appearance>
 27 |       <TextureTransform scale='1 -1'>
 28 |       </TextureTransform>
 29 |       <ImageTexture url='"*sl3dlib/objects/Textures/General/Gravel.jpg" '>
 30 |       </ImageTexture>
 31 |     </Appearance>
 32 |     <IndexedFaceSet coordIndex='3 2 1 0 -1' colorPerVertex='false'>
 33 |       <Coordinate point='-225 -0.1 -225 225 -0.1 -225 225 -0.1 225 -225 -0.1 225'>
 34 |       </Coordinate>
 35 |     </IndexedFaceSet>
 36 |   </Shape>
 37 |   <Transform DEF='Start_Zone'  translation='-3.5 0 0'>
 38 |     <Shape DEF='Start_Rect' >
 39 |       <Appearance>
 40 |         <TextureTransform scale='100 100'>
 41 |         </TextureTransform>
 42 |         <ImageTexture url='"*sl3dlib/objects/Textures/General/Granite.jpg" '>
 43 |         </ImageTexture>
 44 |         <!--      specularColor .31 .28 .26-->
 45 |         <Material DEF='StartZone_Mat'  shininess='0.07' emissiveColor='0.07 0.15 0.14' diffuseColor='0.34 0.71 0.69' ambientIntensity='0.05'>
 46 |         </Material>
 47 |       </Appearance>
 48 |       <IndexedFaceSet coordIndex='3 2 1 0 -1'>
 49 |         <Coordinate point='0.2 0 0.2 3.3 0 0.2 3.3 0 7.8 0.2 0 7.8'>
 50 |         </Coordinate>
 51 |       </IndexedFaceSet>
 52 |     </Shape>
 53 |     <Shape DEF='Start_Border' >
 54 |       <Appearance>
 55 |         <Material>
 56 |         </Material>
 57 |       </Appearance>
 58 |       <IndexedFaceSet coordIndex='3 2 1 0 -1 7 6 5 4 -1 11 10 9 8 -1 15 14 13 12 -1'>
 59 |         <Coordinate point='0 0 0 0.2 0 0 0.2 0 8 0 0 8 0 0 0 3.5 0 0 3.5 0 0.2 0 0 0.2 3.3 0 0 3.5 0 0 3.5 0 8 3.3 0 8 0 0 7.8 3.5 0 7.8 3.5 0 8 0 0 8'>
 60 |         </Coordinate>
 61 |       </IndexedFaceSet>
 62 |     </Shape>
 63 |     <Transform DEF='Take_Off'  translation='2 0.05 4'>
 64 |       <Shape>
 65 |         <Appearance>
 66 |           <TextureTransform translation='-0.2 0.525' scale='0.7 1.05' rotation='-0.5' center='0.5 0.5'>
 67 |           </TextureTransform>
 68 |           <ImageTexture url='"uav_heliport.png" ' repeatS='false' repeatT='false'>
 69 |           </ImageTexture>
 70 |           <Material diffuseColor='0.5 0.5 0.5'>
 71 |           </Material>
 72 |         </Appearance>
 73 |         <Cylinder side='false' radius='0.5' height='0.1' bottom='false'>
 74 |         </Cylinder>
 75 |       </Shape>
 76 |     </Transform>
 77 |   </Transform>
 78 |   <Transform DEF='Obstacle_Zone' >
 79 |     <Shape DEF='Obstacle_Rect' >
 80 |       <Appearance>
 81 |         <TextureTransform scale='50 50'>
 82 |         </TextureTransform>
 83 |         <ImageTexture url='"*sl3dlib/objects/Textures/General/Granite.jpg" '>
 84 |         </ImageTexture>
 85 |         <Material DEF='ObstZone_Mat'  shininess='0' diffuseColor='0.37 0.5 0.33' ambientIntensity='0.5'>
 86 |         </Material>
 87 |       </Appearance>
 88 |       <IndexedFaceSet coordIndex='3 2 1 0 -1'>
 89 |         <Coordinate point='0.2 0 0.2 11.8 0 0.2 11.8 0 7.8 0.2 0 7.8'>
 90 |         </Coordinate>
 91 |       </IndexedFaceSet>
 92 |     </Shape>
 93 |     <Shape DEF='Obstacle_Border' >
 94 |       <Appearance>
 95 |         <Material DEF='Red'  shininess='0' diffuseColor='1 1 1' ambientIntensity='0.5'>
 96 |         </Material>
 97 |       </Appearance>
 98 |       <IndexedFaceSet coordIndex='3 2 1 0 -1 7 6 5 4 -1 11 10 9 8 -1 15 14 13 12 -1'>
 99 |         <Coordinate point='0 0 0 0.2 0 0 0.2 0 8 0 0 8 0 0 0 12 0 0 12 0 0.2 0 0 0.2 11.8 0 0 12 0 0 12 0 8 11.8 0 8 0 0 7.8 12 0 7.8 12 0 8 0 0 8'>
100 |         </Coordinate>
101 |       </IndexedFaceSet>
102 |     </Shape>
103 |   </Transform>
104 |   <Transform DEF='Safe_Zone'  translation='12 0 0'>
105 |     <Shape DEF='Safe_Rect' >
106 |       <Appearance>
107 |         <TextureTransform scale='100 100'>
108 |         </TextureTransform>
109 |         <ImageTexture url='"*sl3dlib/objects/Textures/General/Granite.jpg" '>
110 |         </ImageTexture>
111 |         <!--      specularColor .31 .28 .26-->
112 |         <Material DEF='SafeZone_Mat'  shininess='0.07' emissiveColor='0.07 0.15 0.14' diffuseColor='0.34 0.71 0.69' ambientIntensity='0.05'>
113 |         </Material>
114 |       </Appearance>
115 |       <IndexedFaceSet coordIndex='3 2 1 0 -1'>
116 |         <Coordinate point='0.2 0 0.2 3.8 0 0.2 3.8 0 7.8 0.2 0 7.8'>
117 |         </Coordinate>
118 |       </IndexedFaceSet>
119 |     </Shape>
120 |     <Shape DEF='Safe_Border' >
121 |       <Appearance>
122 |         <Material shininess='0' diffuseColor='1 1 1' ambientIntensity='0.5'>
123 |         </Material>
124 |       </Appearance>
125 |       <IndexedFaceSet coordIndex='3 2 1 0 -1 7 6 5 4 -1 11 10 9 8 -1 15 14 13 12 -1'>
126 |         <Coordinate point='0 0 0 0.2 0 0 0.2 0 8 0 0 8 0 0 0 4 0 0 4 0 0.2 0 0 0.2 3.8 0 0 4 0 0 4 0 8 3.8 0 8 0 0 7.8 4 0 7.8 4 0 8 0 0 8'>
127 |         </Coordinate>
128 |       </IndexedFaceSet>
129 |     </Shape>
130 |   </Transform>
131 | </Transform>
132 | </Scene>
133 | </X3D>
134 | 


--------------------------------------------------------------------------------
/Models/virtualworld/uav_heliport.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/mathworks-robotics/obstacle-avoidance-using-camera/d2735e13c1962e880ec1da6b56a4c813ce96e576/Models/virtualworld/uav_heliport.png


--------------------------------------------------------------------------------
/Models/virtualworld/uav_protos.x3d:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <!DOCTYPE X3D PUBLIC "ISO//Web3D//DTD X3D 3.3//EN" "http://www.web3d.org/specifications/x3d-3.3.dtd">
 3 | <X3D profile='Immersive' version='3.3' xmlns:xsd='http://www.w3.org/2001/XMLSchema-instance' xsd:noNamespaceSchemaLocation='http://www.web3d.org/specifications/x3d-3.3.xsd'>
 4 | <head>
 5 | </head>
 6 | <Scene>
 7 | <ProtoDeclare name='Pole'>
 8 | <ProtoInterface>
 9 | <field name='Position' type='SFVec3f' accessType='inputOutput' value='0 0 0'/>
10 | <field name='Color' type='SFColor' accessType='inputOutput' value='0.4 0.4 0'/>
11 | </ProtoInterface>
12 | <ProtoBody>
13 |   <Transform>
14 |   <IS>
15 |   <connect protoField='Position' nodeField='translation' />
16 |   </IS>
17 |     <Transform DEF='Pole_Base'  translation='0 0.1 0'>
18 |       <Shape>
19 |         <Appearance>
20 |           <Material emissiveColor='0.2 0.2 0.2' diffuseColor='0.6 0.6 0.6'>
21 |           </Material>
22 |         </Appearance>
23 |         <Cylinder radius='0.25' height='0.2'>
24 |         </Cylinder>
25 |       </Shape>
26 |     </Transform>
27 |     <Transform DEF='Pole_Main'  translation='0 1.2 0'>
28 |       <Shape>
29 |         <Appearance>
30 |           <Material shininess='0.5' ambientIntensity='0.5'>
31 |           <IS>
32 |           <connect protoField='Color' nodeField='diffuseColor' />
33 |           </IS>
34 |           </Material>
35 |         </Appearance>
36 |         <Cylinder radius='0.2' height='2.5'>
37 |         </Cylinder>
38 |       </Shape>
39 |     </Transform>
40 |     <Transform DEF='Pole_Band'  translation='0 1.5 0'>
41 |       <Shape>
42 |         <Appearance>
43 |           <Material emissiveColor='0.2 0.2 0.2' diffuseColor='0 0 1'>
44 |           </Material>
45 |         </Appearance>
46 |         <Cylinder radius='0.205' height='0.15'>
47 |         </Cylinder>
48 |       </Shape>
49 |     </Transform>
50 |   </Transform>
51 | </ProtoBody></ProtoDeclare>
52 | </Scene>
53 | </X3D>
54 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # Obstacle Avoidance with Camera Sensor using Simulink
  2 | 
  3 | This submission contains the implementation of [optical flow algorithm](https://www.mathworks.com/help/vision/ref/opticalflow.html) for obstacle avoidance. Here, the drone to traverses through a [Simulink&reg; 3D Animation](https://www.mathworks.com/products/3d-animation.html) arena using optical flow algorithm calculated using the drone's front view camera.
  4 | This model aims at helping you to get started to use computer vision along with control systems in your ground/aerial robots.
  5 | Using this model, you can get your UAV/UGV to avoid obstacles using the front-facing (bird's eye view) vision data of the vehicle.
  6 | 
  7 | Check out the video explaining this example:
  8 | [https://www.youtube.com/watch?v=YTmq13xGnLg](https://www.youtube.com/watch?v=YTmq13xGnLg)
  9 | 
 10 | ## About the model ##
 11 | 
 12 | The model shows the workflow to implement an optical flow algorithm for navigating a drone through the arena which has poles as the obstacles. 
 13 | 
 14 | 
 15 | The following steps describe the workflow:
 16 | 
 17 | 1. Getting the input image from the on-board camera.
 18 | 2. Calculating the optical flow lines from the image using *Horn-Schunck* method
 19 | 3. Implementing an algorithm for steering the vehicle from the calculated flow lines 
 20 | 4. Implementing a switching based logic using [Stateflow&reg; ](https://www.mathworks.com/products/stateflow.html) to switch from optical flow algorithm to a PID Control algorithm.
 21 | 5. Visualizing the drone navigating the obstacles through the VR Sink block (used to set the UAV position/rotation and visual properties of virtual world objects) associated with the virtual world.
 22 | 
 23 | 
 24 | When you open the model and execute it, the drone will take off from the helipad, navigate by evading the obstacles, switch to PID Controller once it detects that it is close to the landing area and eventually, land on the landing helipad. 
 25 | 
 26 | 
 27 | <img src="Models/ReadmePhotos/ModelComponents.png" width="800" title="Locations and naming of the model components explained using the side view of the Simulink 3D environment">
 28 | 
 29 | 
 30 | ## Model ##
 31 | 
 32 | **obstacleAvoidanceUsingOpticalFlow.slx** contains a model which implements Optical Flow algorithm for navigating through obstacles and PID Control algorithm for Drone Landing.
 33 | 
 34 | **startScript.mlx** contains the constants used in the model.
 35 | 
 36 | **virtualworld** folder contains the files required for showing the quadcopter in the Simulink 3D Animation World
 37 | 
 38 | **uav_environment.wrl** shows the three-dimensional environment used for the simulation. 
 39 | 
 40 | *Note:* Before running the model, please make sure all these files are in the current folder
 41 | 
 42 | ## How to run the model? ##
 43 | - Open the Simulink model *obstacleAvoidanceUsingOpticalFlow.slx*. 
 44 | 
 45 | - The model automatically adds the *virtualworld* folder to the path. Check *PostLoadFcn* [Model Callback](https://www.mathworks.com/help/simulink/ug/model-callbacks.html). 
 46 | 
 47 | - The model automatically loads *startScript.m* that initializes the parameters required to run the model.Check *InitFcn* [Model Callback](https://www.mathworks.com/help/simulink/ug/model-callbacks.html). 
 48 | 
 49 | <img src="Models/ReadmePhotos/Callback.PNG" width="350" height="250" title="startScript.m called from the Initialization model callback"> 
 50 | 
 51 | - Run the model.
 52 | 
 53 | ## Assumptions in the model ##
 54 | 
 55 | - While designing the system, the drone is considered as a point mass and the drone dynamics are not considered.
 56 | - The feedback from the drone is taken from the Simulink 3D Animation sensors.
 57 | - The model assumes that the user knows at what point the controller switches from using optical flow to using PID controller.
 58 | - The model also assumes that the x-coordinate landing location is greater than the x-coordinate of the point of transition from optical flow controller to PID Controlelr.
 59 | 
 60 | *Note:* Do not change the *Bird's Eye* View in the *On-board Camera* and *Virtual Reality Sensors* block. This will affect the optical flow algorithm since we need the images from the front camera.
 61 | 
 62 | <img src="Models/ReadmePhotos/CameraView.png" width="800" title="Do not change the view from the Camera or Virtual Reality Sensor">
 63 | 
 64 | 
 65 | 
 66 | ## About the Subsystems ##
 67 | 
 68 | <img src="Models/ReadmePhotos/ModelPhoto.PNG" width="800" height="300" title="Model structure">
 69 | 
 70 | 
 71 | - *Sensor Inputs:* Simulink 3D Animation Sensor inputs (camera view and estimated position)
 72 | - *Optical Flow Lines Calculation:* Calculates the optical flow lines for the image using 'Horn-Schunk' method.
 73 | - *Optical Flow Algorithm for Obstacle Avoidance:* Controller for steering the vehicle: 
 74 |     - *FlowDifferences* MATLAB&reg; Function: The Optical Flow on the right and left is calculated by the system. 
 75 |     - *Optical flow algorithm and PID Controller for landing:*
 76 |         - *DronePositionEstimation:*
 77 |             1. *OpticalFlowAlgorithm:* The sign of the linear velocity (*V_Y*)is assigned depending on Optical Flow on Left and Right
 78 |             2. *ControlAlgorithm:* Switch to the PID controller to hover above the destination coordinate.
 79 |             3. *DroneLanding:* Descend the drone to the floor.
 80 |         - *DroneOrientationEstimation:* 
 81 |             1. *OpticalFlowAlgorithm:* The sign of the angular velocity (*W_Y*)is assigned depending on Optical Flow on Left and Right
 82 |             2. *ControlAlgorithm:* Find the orientation from angular velocities
 83 | - *3D Visualization:* Once the Position and Orientation outputs are estimated in the model, they are sent to the VR Sink block which are then used to set the UAV position / rotation. 
 84 | - *Distance between landing helipad and present location:* This subsystem calculates the distance between the current point and the Helipad Lcoation. If the drone gets closer to the helipad, it stops the simulation. The propeller will be turned off since the sensor at the landing helipad will get activated.
 85 | 
 86 | ## How to change the start location, land location and other parameters? ##
 87 | All the variables used in the model are lcoated in the *startScript.m* file. To change any of the paramters, change the variable value in the file. You can *Run* the file to ensure that the variables are updated.
 88 | 
 89 | <img src="Models/ReadmePhotos/MATLABScript.PNG" width="500" title="MATLAB Script with the values of the initialized variables">
 90 | 
 91 | 
 92 | 
 93 | 
 94 | - *droneStartLocation*: The location from where the drone will take off.
 95 | - *helipadLocation*: The location where the drone will land. Presently this is assigned as the center ocation of the helipad. This can be modified to a value where the drone needs to land.
 96 | - *linearVelocity*: The linear velocity [V_X V_Z V_Y] during optical flow navigation.
 97 | - *angularVelocity*: The angular velocity [W_X W_Z W_Y] during optical flow navigation.
 98 | - *angularVelocityforLanding*: The angular velocity of the drone during landing.
 99 | - *switchingXLimit*: The X-coordinate to switch from Optical flow controller to PID Controller
100 | - *finalHoverHeight*: The height to reach over the helipad before landing.
101 | - *delta*: The threshold distance to execute the PID controller. The drone will start landing if the present coordinate of the drone is within this *delta* value.
102 | 
103 | 
104 | 
105 | ## How to tune linear and angular velocities? ##
106 | 
107 | The linear and angular velocities defined in *startScript.mlx* need to tuned if the Simulink 3D Animation environment is modified.  The following are the steps to tune the same:
108 | 1. Ensure that with a given linear velocity, the drone doesn't move out of the arena. 
109 | 2. *V_Y* is the velocity which will be determined depending on the output from the optical flow algorithm. Here, the magnitude of V_Y remains the same but just the sign is changed. If:
110 | 	- *vleftflow* > *vrightflow*, then *V_Y* is positive. Hence, the drone will steer to its right.
111 | 	- *vrightflow* > *vleftflow*, then *V_Y* is negative. Hence, the drone will steer to its left.  
112 | 3. Similarly, the angular velocity *W_Y* will be determined depending on the output from the optical flow algorithm. Here, the magnitude of W_Y remains the same but just the sign is changed.If:
113 | 	- *vleftflow* > *vrightflow* then *W_Y* is negative (clockwise)
114 | 	- *vrightflow* > *vleftflow* then *W_Y* is positive (anti-clockwise)
115 | 4. If the drone was not be able to bypass all the obstacles with the given *V_Y*, change *V_Y* along with some incremental change in roll velocity. 
116 | 5. Always try with a smaller angular velocity values to ensure less oscillations since the decisions are made depending on the flow differences.For example, if there are lots of obstacles on both the sides of the drone, the sign of *flowDifference* will flip rapidly. In case the angular velocities are high here, the drone will have too many oscillations which are not desirable.
117 | 
118 | *Note*: The first component in linear velocity is the *V_X*, the second component in linear velocity is the *V_Z*, the third component in linear velocity is the *V_Y*.
119 | 
120 | ## Product Requirements ##
121 | 
122 | The model use the following MathWorks products:
123 | 
124 | 1. MATLAB
125 | 2. Simulink&reg;
126 | 3. Simulink 3D Animation
127 | 4. Stateflow
128 | 5. Aerospace Blockset&trade;
129 | 7. Aerospace Toolbox
130 | 8. Computer Vision Toolbox&trade;
131 | 
132 | In case of any questions, please reach out to us at roboticsarena@mathworks.com.
133 | 
134 | Copyright 2021 The MathWorks, Inc.
135 | 
136 | 


--------------------------------------------------------------------------------
/SECURITY.md:
--------------------------------------------------------------------------------
1 | # Reporting Security Vulnerabilities 
2 | 
3 | If you believe you have discovered a security vulnerability, please report it to 
4 | [security@mathworks.com](mailto:security@mathworks.com). Please see 
5 | [MathWorks Vulnerability Disclosure Policy for Security Researchers](https://www.mathworks.com/company/aboutus/policies_statements/vulnerability-disclosure-policy.html) 
6 | for additional information.  


--------------------------------------------------------------------------------
/license.txt:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2020, The MathWorks, Inc.
 2 | All rights reserved.
 3 | 
 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 5 | 
 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 8 | 3. In all cases, the software is, and all modifications and derivatives of the software shall be, licensed to you solely for use in conjunction with MathWorks products and service offerings. 
 9 | 
10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


--------------------------------------------------------------------------------