prasadnr / webots-quadcopter-python-sitl Goto Github PK

While the current code works properly, it does not consistently work with target_x and target_y as described in cyberbotics/webots#1071 . Once that issue is resolved, the functions will have to be modular.

See cyberbotics/webots#1081 for more details. After that official bug which I raised is resolved, this ODE physics issue will have to be closed and image processing has to be done (instead of GPS alone) to localise AruCo markers.

In the meanwhile, workarounds are necessary.

Currently there are few options which are being considered:

1. Attempting this problem with Webots if software development time is not that difficult for me.
2. Having ROS2 and Gazebo working properly (so that it can be easily ported to the hardwares directly).
3. Attempting custom simulator with appropriate graphics rendering (with 3D polygons).
4. Some other existing simulator.

Option 1 is about to be discarded as porting the algorithm to hardware requires extra efforts (and also, I had archived this repository as there were issues with the Webots software itself and I had raised appropriate "bugs" for that).

Option 3 can also be discarded as I am not really interested in creating another physics based software simulator just for this.

As of now, ROS2 and Gazebo might not work together properly on all of the operating systems. It does not look like Gazebo ROS2 integration page has been updated in some time. I see (as of now) the following:

http://gazebosim.org/tutorials?tut=ros2_overview

Not all functionality from ROS 1 has been ported to ROS 2 yet. You can check the progress on this issue.
If you're interested in porting official or your own custom plugins, check out the ROS2 migration contribution guide.

So, once I verify that I can use this on both Windows and Ubuntu, I will think about creating this simulator. If not, a lot has changed in software simulations since 2019! So, I might try looking some other simulator up.

In any case, a new repository will be created and this repository might not be updated.

I am creating this question as several people have asked this question to me.

https://github.com/PrasadNR/Webots-Quadcopter-Python-SITL/blob/master/controllers/mavic2proController/mavic2proController.py#L93-L96

front_left_motor_input = float(params["k_vertical_thrust"]) + vertical_input - roll_input - pitch_input + yaw_input
front_right_motor_input = float(params["k_vertical_thrust"]) + vertical_input + roll_input - pitch_input - yaw_input
rear_left_motor_input = float(params["k_vertical_thrust"]) + vertical_input - roll_input + pitch_input - yaw_input
rear_right_motor_input = float(params["k_vertical_thrust"]) + vertical_input + roll_input + pitch_input + yaw_input

I did not create this equation by myself; these were derived from the C++ equations I saw in Webots software https://github.com/cyberbotics/webots/blob/master/projects/robots/dji/mavic/controllers/mavic2pro/mavic2pro.c#L173-L180 (where I saw these equations without explanations or comments):

const double front_left_motor_input = k_vertical_thrust + vertical_input - roll_input - pitch_input + yaw_input;
const double front_right_motor_input = k_vertical_thrust + vertical_input + roll_input - pitch_input - yaw_input;
const double rear_left_motor_input = k_vertical_thrust + vertical_input - roll_input + pitch_input - yaw_input;
const double rear_right_motor_input = k_vertical_thrust + vertical_input + roll_input + pitch_input + yaw_input;
wb_motor_set_velocity(front_left_motor, front_left_motor_input);
wb_motor_set_velocity(front_right_motor, -front_right_motor_input);
wb_motor_set_velocity(rear_left_motor, -rear_left_motor_input);
wb_motor_set_velocity(rear_right_motor, rear_right_motor_input);

The intuition is like the one below:
k_vertical_thrust is for the quadcopter to just stay in the air without losing altitude. So, just for this discussion, we can assume that the quadcopter just stays in the air even when k_vertical_thrust = 0 (may be by magnetic fields that lets quadcopter "hover" without having to have any air-power). Then, if M₁, M₂, M₃ and M₄ are the four motors of the quadcopter, we can conveniently represent the 4 channel input mapped to these four motors' inputs.