New Distance Sensor Feature

[xabierolaz]

Is your feature request related to a problem? Please describe.
No, brand new feature (Navigation Support)

Describe the solution you'd like
Train the model so it's able to takeoff and reach certain height (e.g. 20 meters)

Describe alternatives you've considered
Might get merged with #74 as a broader Navigation Support feature

Additional context
Future features as landing and wind support scheduled.

[wil3]

Hey @xabierolaz do you only need vertical distance support? If there isn't position support your agent wont be able to control the location it takes off too, only the height.

If there is no wind or disturbances a distance sensor and an IMU would allow you to takeoff vertically to a specific height. However if there is wind, without position hold the quad will drift. I would recommend starting simple with just the distance sensor and then going from there.

Also keep in mind these sequence of PRs add the functionality to GymFC but then you still need to inherit GymFC to train the controller and define your own state and reward function. How this is done is shown in the PPO example.

[xabierolaz]

full inputs needed are:

-orientation (pitch,yaw and roll)
-position (left,right, backwards,frontwards, up and down)

full output are:
4 throttles

I wasnt being specific, its exactly as you addressed. Will start from there, thanks !

[wil3]

In GymFC we only care about the sensors. It's the extended class you define the I/O. I'd start with just the distance sensor before adding a position sensor then we will have a good place to document the procedure for adding new sensors.

After you get more familiar with the code please define the changes into individual workable PRs and propose them here at way. Single large PRs are less like to be merged.

You can use the IMU sensor as an example it shouldn't be difficult. This will involve at least,

• a new message definition for the sensor
• Updated entry in the model.sdf
• Update to the cpp plugin
• Corresponding update to fc_env.py

[xabierolaz]

Thanks @wil3, starting today the way you suggesting and will post any update here

[wil3]

Reviewing the code it actually looks like there shouldn't need any modifications to fc_env.py

The state protobuf message just needs to be updated and then the python files generated using this script.

That was the point of this function to make it easy to add new sensors. It'll just add class attributes to whatever the names are defined in State.proto.

[wil3]

For the new distance sensor message this is a bit tricky because the current setup isn't ideal. Right now we have them defined in two places, in GymFC and within the actual sensor plugin, for example here. Ideally we'd have a separate repo with these definitions so there aren't multiple copies floating around.

I think a good alternative is to put them in a separate repo and then include it as a submodule so they are statically linked. Only other option is to dynamically link them at runtime however this will greatly increase the complexity and introduce more room for error. We can do the refactor after we have it working.

For the distance sensor you can add it to this repo if you like which I'm going to migrate over to this organization or your own repo and we can link to it in the GymFC readme, your call.

[xabierolaz]

Thanks for the insight @wil3, I will create a separate repo and add it to the one you linked

[Geo2001]

Thanks for your mention @xabierolaz , I am just seeking a controller that takes a target position in 2D ignoring the height, of course a height controller would be thoughtful contribution but not necessary for me, any insights about this ?

[xabierolaz]

@Geo2001 by 2D you mean navigation along a plane (forward/backward, left/right) ?

I think we might not need height sensor for that, although I'd recommend it because we would have to start the navigation from a certain height and mantain it. Otherwise, by ignoring the 3rd axis (height), the model could drift and become unpredictable. I'm not sure about this as I haven't started the training, maybe @wil3 could help you with this. We would have to adapt the reward in PPO. However, for your actual purpose a position sensor would be needed, such as GPS.

[Geo2001]

@xabierolaz You mean the feature isn’t currently available by the present models ?

[xabierolaz]

I've started updating the state protobuff message and updating the entry in the .sdf model

Distance Sensor Repository

Im using hokuyo laser sensor as a starting point, right now I have:

1-Updated entry in sdf model

2-Added message definition for the sensor by updating the state protobuff message

3-Generated plugin

4-Working on the cpp plugin part right now, any guide on how to update it?@wil3 Thanks!

@Geo2001 right now we have input about the ESC, IMU and MOTOR sensors
mail added in bio, however I recommend publishing all gymfc related work here

[Geo2001]

@xabierolaz can you please add your email or any communication method to your bio or here, we can do some collaborative work and push it here in that repo

[wil3]

@xabierolaz per my previous advice I suggest breaking the task into small workable PRs to make it easier for me to help you. Step 1 would be a PR that integrates the capability of adding the distance sensor into gymfc which it looks you are well on your way with from # 2 above. Step 2 would be to actually implement a distance sensor.

Once you have step 1 done, open the PR up so we can start the discussion.

[xabierolaz]

Hi @wil3, step 1 PR is ready , have sent you an invitation to it
https://github.com/xabierolaz/Distance-sensor-capabilities

And this would be step 2 PR, implementing the distance sensor, once adding the distance sensor into gymfc is confirmed
https://github.com/xabierolaz/gymfc-distance-sensor/blob/master/README.md

[wil3]

Hey @xabierolaz those aren't PRs, those are separate projects. PRs (pull requests) are opened for the repo the code will be merged to go through a review process (and CI) before merged.

If you want to keep you're distance sensor plugin separate we can link to the repo in the GymFC README.md. Changes that must be made to GymFC need to be opened in a PR for example updating the state message (https://github.com/xabierolaz/gymfc-distance-sensor/blob/master/gymfc/envs/assets/gazebo/plugins/msgs/State.proto#L28). If this is new to you I suggest reading up on how to open a PR in github.

[xabierolaz]

Hi @wil3 , I misspeled PR as public repository, now it's properly commited as a pull request, thanks for the patience

[xabierolaz]

@wil3 I have updated the state protobuf message as you might see in the PR and then the python files generated using that script.
Im opening next PR as you suggested me earlier so we can discuss the next CPP plugin part

[xabierolaz]

@wil3 closed previous PR and added a new one with just the needed file modifications for the new sensor capabilities

[xabierolaz]

Hi @wil3 ,

A work colleague has started working on the project too, and is starting from the last part of the whole process (when the lidar is working in GymFC and the NN trained).Reading the thesis he asked me some some questions:

1. When the training is finished and we have the controller lets say Mathek F7 with the NN burned in.
   As the LIDAR is a external sensor outside the controller, How will we do to read those values to the controller (how does neuroflight actually do it with the iMU for example?) will it be via SPI port or whats the optimal way to do it?
   Thanks

PS. The PR for new sensor capabilities is up now.

[xabierolaz]

Hi @wil3 . I am still working on the navigation support, just have been learning some drone fundamentals and trying other methods. I have one question though.
Do we have access to individual motors in GYMFC?
I would need the algorythm to control the 4 motors as the only ouput so it gets to a certain height.

Thanks

[wil3]

Hey @xabierolaz yep that's what the step function does https://github.com/wil3/gymfc/blob/master/gymfc/envs/fc_env.py#L237

In the tests directory you can see example uses of it.

[Pranav-India]

Hi @wil3

I a, trying to add the distance sensor
I have added distance.proto with only one parameter which is distance from ground.
Generated both ".pb.cc" and ".pb.h" files.

I need to edit sdf file , In sensors section I added distance sensor and it's one parameter below imu ,
Now Do we need ".so" file file this sensor as well?
I looked for the distance sensor ".so" files but all of them are for ros.
Do I need to write the "" part in the sdf file? if yes then where can I find the ".so" file

[wil3]

Hi @Pranav-India, are you caught up on this thread? The sensor plugin has been discussed (for e.g., here).

Yes, there is no distance sensor (yet), it needs to be created and added to the SDF file.

My suggestion would be to get caught up with whats been discussed in this thread and understand how the GymFC IMU plugin works, its will be the same process. Once the sensor has been created it can be integrated into GymFC.

[xabierolaz]

Hi @wil3

Regarding the distance sensor,

This is a summary of all the files that have been changed or created in both gymfc and gymfc-aircraft-plugins repositories.

Before making any pull to #88 ,is there any missing file or change that needs to be done before getting into the .sdf?

Thanks

GYMFC REPOSITORY

Inside gymfc/envs/assets/gazebo/plugins/msgs
Distance.proto (added)

Inside \gymfc\envs\assets\gazebo\plugins
Cmakelists.txt (added)
FlightControllerPlugin.cpp (updated)
FlightControllerPlugin.hh (updated)

Inside gymfc/envs/assets/gazebo/plugins
Distance.pb.h (added)
Distance.pb.cc (added)
(They are in my /build folder now, but as its built after installation, dont know where to place them)

AIRCRAFT PLUGINS REPOSITORY (gymfc-aircraft-plugins)

Inside gymfc-aircraft-plugins/msgs
Distance.proto (added)

Inside gymfc-aircraft-plugins/src
gazebo_distance_plugin.cpp (added)
gazebo_distance_plugin.h_ (added)

Distance.pb.h (added, same file as in gazebo/plugins)
Distance.pb.cc (added, same file as in gazebo/plugins)
(They are in my /build folder now, but as its built after installation, dont know where to place them)

[Pranav-India]

Hi @wil3

I have written the Distance.proto file as follows:

syntax = "proto2";
package sensor_msgs.msgs;
import "Float.proto";
message Distance
{ required float Ground_distance = 1; }
I am have a small confusion like in Imu.proto
we have :
required gazebo.msgs.Quaternion orientation = 1;
repeated float orientation_covariance = 2 [packed=true]

Do I need "gazebo.msgs.Float" as a replacement of "gazebo.msgs.Quaternion" since we don't need something like Quaternion for the distance calculation ?

[wil3]

@xabierolaz The first thing I'd do is open up a PR for the gymfc-aircraft-plugins changes. That should merge first because the gymfc changes depend on it. But yea for glance those look like those are the correct files. Also don't forget to modify CMakeLists.txt.

@Pranav-India no you shouldn't need to import Float.proto, can look at the ESC and others that use float. Honestly I can't remember what that Float proto is for I'll have to look into it. Its probably to send a single value.

To start can you two please open an issue in the gymfc-aircraft-plugins repo for discussion particular to that change. Then create a branch linked to that issue following the same guidelines as gymfc. It will be easier for me to guide you if I can see the code.

[xabierolaz]

Hi @wil3
gymfc-aircraft-plugins PR is up with all errors fixed! (wil3/gymfc-aircraft-plugins#5)

Now we are working on the GymFC part, right now we got:
FILES CHANGED/ADDED

• FlightControllerPlugin.cpp

• FlightControllerPlugin.hh

• Corresponding update to fc_env (I know you said above that it wouldn't be necessary to change it, but checking the code)
  https://github.com/xabierolaz/gymfc/blob/master/gymfc/envs/fc_env.py#L131
  Its mandatory to have the distance sensor as a valid sensor to make it work

  VALID_SENSORS = ["esc", "imu", "battery", "distance"]

• Distance.proto

• State.proto update

• Model.sdf. Actually figuring it out, as almost every tutorial shows how to use it with ROS, but that wouldnt work in GymFC so following this generic gazebo tutorial http://gazebosim.org/tutorials?cat=guided_i&tut=guided_i1

[xabierolaz]

Hi @wil3, I got a couple of questions about adding the laser sensor in the sdf file regarding GymFC

Here is the modified NF1 copter with the laser sensor in case you want to verify the problem nf1.zip

I have added a base link in the sdf file of the nf1 drone and added the laser code there.

1. If I start gazebo normally (regular, not gymfc env) and load the nf1 with sensor I have added, the sensor works (shows the distance output in the topic visulation window 'gazebo/default/nf1/base/sensor/scan' ) (Image 1)

NF1's attached sensor detects and collides the box in a default gazebo environment

However, if I run any test in gymfc/tests with same modified nf1 sdf it shows the topic but laser rays pass through the object i.e. don't detect the object.

NF1's attached sensor does not detect nor collide the box in GymFCs test_start_sim test environment

What is the reason? is it because of the plugin?

2. I can see the topic named "gazebo/default/nf1/base/sensor/scan" which is suppose to give the distance as a output . I have created a distance.proto file with a single varible(distance from ground) , now through the plugin I need to attach the 'gazebo/default/nf1/base/sensor/scan' output to the variable defined in the proto file. But I am not sure on how to access that topic in the file..
   when that plugin cpp is made I will be able to create .so file and add that into the sdf file.

[wil3]

I only have time to address one thing at a time. Once the plugin gets merged I'll revisit the GymFC integration.