Multicopter Attitude and Position Controller

[mihirk284]

Desired behavior

The current gz-sim implements a system that incorporates a multicopter plugin based on the RotorS Simulator's setup. This plugin is only used to command the velocity of the robot in the vehicle frame. Currently, attitude controllers that are able to command the desired roll, pitch, yaw rate, and thrust are not a part of the simulator. I want to propose addition of a Position controller and Attitude Controller based on the same paper that the current LeeVelocityController is implemented from.

One major reason for this recommendation is that it would be extremely easy for users to implement their own software to use multirotors in simulation directly by providing position commands (in the world frame), thus allowing them to develop software to command robots without having to explicitly write a controller that commands robot velocities.

Alternatives considered

I have considered a rotors_control-like structure that uses ROS nodes that implement the controller externally and provide motor commands to the actuators. However the inbuilt functionality of directly reading the RotorConfiguration and robot mass/inertia properties from the SDF reduces room for errors and thus, implementation of the position and attitude controllers as a simulator plugin appears as a natural solution to the problem.

I am interested in developing this further for our own use and if the Gazebo Team is interested, I would be happy to open a pull-request to this repository for it to be merged with the open-source simulator. The added feature (with basic working functionality, with some room for improvements) can be seen on the following commit to the fork of gz-sim repo: https://github.com/ntnu-arl/gz-sim/tree/dev/multicopter_control.

Implementation suggestion

I am in the process of implementing the controller myself for internal use at our research group, however it would be great if these can be incorporated within the open-source simulator. The current implementation that I am working with has created two different systems that include multicopter_attitude_control and multicopter_position_control systems. These are similar in nature to the current multicopter_system that controls only the velocity in vehicle frame for the robot, however the attitude control system commands the attitude in the vehicle frame, and the position control system is able to command the position in the world frame (thus exploiting available ground truth pose information available in the simulator).

The current implementation (in progress) can be found here: https://github.com/ntnu-arl/gz-sim/tree/dev/multicopter_control, however if this is planned to be incorporated in the simulator, it can be changed as per your recommendations.

Additional Context

---

[azeey]

@mihirk284 this is a great idea. Please go ahead and create the PR. One thing I'm thinking about is how we can consolidate the controllers so that we can have one controller that accepts a parameter as to whether it's velocity, position, or attitude. Do you think that's feasible?

[mihirk284]

Hi @azeey, thanks for replying I think this should be doable. I will update this on my end an open a PR soon enough so we can iterate over it.

Ideally this would involve renaming of classes that exist, would this API-breaking change be a problem? Or should this exist in parallel as another system, while the existing velocity controller remains unmodified?

[azeey]

The API break should be okay as long as the plugin name and how it's used in SDFormat doesn't change.

[knmcguire]

Hi! Just wanted to check what the process was on this issue. I have been working before on similar controllers for gazebo a while ago, but don't want to re-invent the wheel here :) and perhaps it would be good to work together on these things.

@mihirk284 is there anything I or anybody in the ROS aerial robotics community can help out with to make your changes more PR ready? Happy to help!

[mihirk284]

Hei @azeey and @knmcguire!

Apologies on the slow response time on the issue.

We implemented a hacky version for acceleration, velocity, attitude controller for our lab use on this branch. And I faced some questions regarding the implementation of this feature for the official gazebo repository.

I have a few questions and would like some guidance on the same for implementing the controllers:

1. What gazebo messages are most appropriate to command roll-pitch-yawrate-thrust, position-yaw, acceleration-yawrate and body-rates-thrust setpoint controllers? Given than each of these require 4 dimensions, would it be a good idea to define new messages for this or we can use existing ones?
2. Would it be a good idea to have the multicopter_control combined together as one plugin that can be referenced with filename="gz-sim-multicopter-control-system" within the SDF file as is done now, but a new field is added within the block for the controller type (for example <controllerType>attitude</controllerType>)?
3. Is it recommended to merge the functionalities into a base class that the position, velocity, attitude and body rate controllers inherit from?

I would appreciate some guidance on the above to implement these features.

Thank you

[knmcguire]

Ah I won't be able to answer all of these as I'm not part of the Gazebo core development team, so I guess I can give some suggestions.

1- I can't really find a good gazebo message here, except for that the position one would likely need to be done with Pose. And for the others perhaps for the time being a simple float array of 4. All of these messages have also gz-ros-bridge equivelants so that is imporant to consider too.

Mind that there is a REP 147 for message standards of aerial vehicles which seems to suggest to use Twist messages for this as well but it's not officially implemented and there is a need to improve this even further. But on the other hand, if all is reused in twist, that might fit well with your second suggestion.

2- I like it! But might be a bit restrictive for anyone that would like to do things like 'height fixed and velocity in the horizontal axis control' (which is a common use case). Any chance that this could be configured per body axis or is that way too difficult?

3- I would say so yes? If it is reusing the same type of controllers but the input and gains are different.

So these are just suggestions from the aerial robotics side. From what is better from a gazebo side that is perhaps something that @azeey could better answer. I'll also share this within the aerial ROS group to see if anybody else have some suggestions.

[azeey]

I'm a little confused about what's being proposed. The title and the PR description only mention two new controllers: attitude and position. Your question about message types implies there are other controllers. Can you confirm what types of controllers you're proposing to implement?

I'll try to answer assuming that you're creating other new controllers:

1. I think defining new semantically meaningful messages would be ideal (see https://docs.ros.org/en/iron/Concepts/Basic/About-Topics.html#strongly-typed) . I see that a Twist message is suggested in REP 147 some of these, but I would not be in favor of that since, for example, the unit of thrust (Newtons), would not be properly conveyed in a Twist message. https://docs.ros.org/en/melodic/api/mav_msgs/html/index-msg.html has a list of messages that align with the controllers you are proposing. Maybe we can adopt those? Unfortunately, mav_msgs is not available in ROS 2. Some messages, e.g. Actuators has already been migrated, but not all, so we'll have to do something about that for ros_gz_bridge. Possibly add them to https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_interfaces, but that wouldn't be ideal.
2. I like @knmcguire's suggestion
3. Maximizing code reuse is definitely a good idea, but it could be done by adding functions to https://github.com/gazebosim/gz-sim/blob/6af74459f4b0f5a2a33fa112e24ac50633a90d3b/src/systems/multicopter_control/Common.hh as well. If it makes sense to use inheritance, go for it, but if you can avoid it, that would be my preference.