Can I control the UAV using only rotors speed (which is mapped from "thrust and torques") to reach a given goal using Rotors package?

[IhabMohamed]

Hi everyone,

We have our own controller which takes as input the states (via odometry) and gives as output the desired thrust and torques. Then, we have mapped the output (Thrust and torques) into rotor velocities since our controller is implemented using Python.

Anyways, It has been realized that subscribing to odometry and publishing Actuators message are not enough to reach a goal. For instance, for hovering (i.e. pose 0, 0, 3), the drone stays in the desired position for a few seconds, then it flies somewhere else.

I do not know what should I do exactly:

• Should I publish the desired pose (Like in Lee Position Controller) or velocities, as well?
• Or, should I use the RollPitchYawRateThrust controller in cascaded with our controller? If I want to use this controller with mine, so our controller should publish the predicted Roll, Pitch, Yaw Rate, and Thrust as RollPitchYawRateThrust message, to the RollPitchYawRateThrust controller, am I right?
• Can I use only our controller? If so, what should I publish and subscribe exactly?

Please advise me on the right solution, in order to use the package properly.

I am looking forward to hearing from you.

Many thanks in advance,

[juanmed]

@IhabMohamed If you publish Actuator messages to the <mav_name>/command/motor_speed topic, the rotors will rotate at the indicated speeds.

If you are using full-state feedback control then you can subscribe to <mav_name>/odometry_sensor1/odometry topic and from there you can get position, linear velocity, orientation, and angular velocity.

I wrote my own high and low level controllers and I can confirm it is possible to make the drone follow a desired trajectory or simply hover indefinitely by computing the correct motors speeds.

Additionally, I would suggest refering to Koubaa, A. (2017). Robot Operating System (ROS) - The Complete Reference-Vol1 (Vol. 707). https://doi.org/10.1007/978-3-319-54927-9 Chapter 9 in order to understand RotorS. It can be difficult at the beginning but the tutorial is quite extensive and well written.

[gsilano]

Furthermore, I would like to suggest of reading other chapters on the same book volume and also on the following ones. Indeed, more chapters have used RotorS like as a basis for building their own ROS packages. Thus, you could learn more on RotorS on both the suggested chapter above and on the others.

[IhabMohamed]

@juanmed @gsilano, Many thanks for your interest and your kind suggestions. I am going through the recommended book. I have read the first chapter and the RotorS article, as well.

As @juanmed said I publish Actuator messages and subscribe to <mav_name>/odometry_sensor1/odometry to get the full-state. However, the MAV stays for a few seconds on the desired pose, then it fills down.

I guess there may be a problem with the states. Just for confirmation: regarding the odom message position and orientation are expressed in Inertia frame, while linear velocity and angular velocity are expressed in Body frame, am I right?

Do I need to subscribe to <mav_name>/command/trajectory as well? I know it gives information about the final pose, velocities, and accelerations.

BTW, the controller works perfectly as a script (without Gazebo), as you see in the shared figure https://drive.google.com/file/d/0B7JEIvRJLy7EUmo2NnZMMnU5MVVzTk1pekxYNl8zTVNLemln/view?usp=sharing

[gsilano]

Yes, you are right: both linear and angular velocities are expressed in the body frame. Therefore it is necessary to rotate these measurements to control the drone behavior.

The <mav_name>/command/trajectory topic provides the reference trajectory (expressed using waypoints) that the drone should follow.

Maybe there is something wrong with your code. I addressed the same problem when I developed CrazyS, a ROS package (it uses RotorS as a base) aimed to simulate the behavior of the Crazyflie 2.0 nano-quadrotor. I also published a book chapter on this (it will be available soon here) that described my package and the procedure by which you can add your drone in RotorS (both controller and 3D model).

Finally, I suggest you use this package in Gazebo if you are interested in simulating a cluttered environment (I thought this by looking at your screen).

[juanmed]

@gsilano Is the reference frame for each state defined in any document here on github or paper? I am quite surprised by your comment on the linear velocity being expressed in the body frame. I would have assured that linear velocity is expressed in the inertial frame, for two reasons:

First, following equations (6) and (7) in page 600 of the book I recommended, the position and linear velocity are expressed in the inertial frame, the orientation is expressed as the a quaternion that expresses the rotation of the body frame w.r.t inertial frame, and the angular velocity is expressed in the body frame.

Second, I have written a controller and have been using successfully in RotorS for trajectory tracking. I use the linear velocity as if it was expressed in the inertial frame (in other words, I do not rotate the measurements) and the performance is good.

After looking at the nav_msgs/Odometry definition, you statement makes sense, if the RotorS authors did following the definition that twist is expressed in the frame denoted by child_frame_id field in the message.

I will try rotating the linear velocity to see what happens.

[IhabMohamed]

@gsilano, Yes there is something wrong with my code, I am trying to figure it out.

So, since you have a good experience in Rotors package. Do you think that I have to publish the waypoints (through <mav_name>/command/trajectory) in conjunction with the actuators message?

Regarding CrazyS, it is a great job. I am pretty sure it will be useful for our team.

You are right, I am performing autonomous navigation in the forest, this package is also very interesting, thanks :-)

[IhabMohamed]

@juanmed, I guess the velocities are expressed in body frame, as I understood from Lee Controller code published by the Rotors authors (have a look at line 102

[gsilano]

@gsilano Is the reference frame for each state defined in any document here on github or paper? I am quite surprised by your comment on the linear velocity being expressed in the body frame. I would have assured that linear velocity is expressed in the inertial frame, for two reasons:

As you can see here, I rotated the linear velocity to perform my controller algorithm.

[gsilano]

So, since you have a good experience in Rotors package. Do you think that I have to publish the waypoints (through <mav_name>/command/trajectory) in conjunction with the actuators message?

Check out my code. I'm pointing to another repository, BebopS, but it also uses RotorS as a base.

You are right, I am performing autonomous navigation in the forest, this package is also very interesting, thanks :-)

Great! Write me an e-mail when you work will be available online :wink:

[juanmed]

@gsilano @IhabMohamed

Thanks for your answer. Yes, I checked the LeeController and you are right guys. I wonder why my controllers works then? haha Thanks!

[IhabMohamed]

@juanmed @gsilano, thanks guys, it was a great discussion at least I know that velocities are expressed in the body frame. @juanmed you are lucky that it works for you :-)

@gsilano, sure I will write to you if it works

[IhabMohamed]

Hi Everyone,

I figured out where was the problem. Actually, it was with the allocation matrix. As you can see in Lee Controller, they multiply the inverse of the allocation matrix with the inertia matrix. This has been done to make the tuning independently of the inertia matrix (i.e. dividing by inertia matrix).

So, what I have done is to consider the inertia equal to one in the dynamic model, in order to use the same form of the allocation matrix in conjunction with the inertia matrix. Also, I used the real value of the inertia matrix in the given equation, then considering the allocation matrix without the inertia, and it works well.