Model submission for x500 sensor configuration 1 from CTU-CRAS-NORLAB.

submitted_models/ctu_cras_norlab_x500_sensor_config_1/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.1.2)
+
+project(ctu_cras_norlab_x500_sensor_config_1)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  message_runtime
+  std_msgs
+  geometry_msgs
+)
+
+find_package(ignition-gazebo4 REQUIRED)
+find_package(ignition-transport9 REQUIRED)
+find_package(ignition-common3 REQUIRED)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime roscpp std_msgs geometry_msgs
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(MRSMultirotorController SHARED
+  src/MRSMultirotorController.cpp
+  src/Common.cpp
+  src/SE3Controller.cpp
+)
+
+target_link_libraries(MRSMultirotorController
+  PUBLIC ${catkin_LIBRARIES}
+  PRIVATE ignition-plugin1::ignition-plugin1
+  PRIVATE ignition-gazebo4::ignition-gazebo4
+  PRIVATE ignition-common3::ignition-common3
+  PRIVATE ignition-transport9::ignition-transport9
+  PRIVATE Eigen3::Eigen
+)
+
+install(DIRECTORY launch meshes urdf materials worlds
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(FILES model.sdf model.config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(TARGETS MRSMultirotorController
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)

submitted_models/ctu_cras_norlab_x500_sensor_config_1/README.md

@@ -0,0 +1,58 @@
+# CTU-CRAS-NORLAB-X500
+This repository contains a virtual model of the X500 aerial platform of the CTU-CRAS-NORLAB team. Please see the [specifications](specifications.md) for more information.
+
+## Description
+X500 is a quadrotor UAV based on the Holybro X500 frame built for the Final Circuit of the DARPA Subterranean Challenge.
+Its sensory equipment consisting of 3D LiDAR, 2 RGBD cameras and 1 RGB camera allows reliable navigation through various environments, its small size facilitates flying through narrow passages.
+A two-battery system consisting of 2 Lipo 6750mAh 4-cell batteries connected in parallel can power the UAV for over 25 minutes of flight time.
+The propulsion system consists of T-MOTOR MN3510 KV700 motors and 13 inch propellers.
+
+## Usage Instructions
+The robot motion is controlled via standard cmd_vel commands.
+
+## Usage Rights
+No additional restrictions have to be taken into account for this configuration.
+
+## MRS multirotor controller
+
+* The code is based on the [multicopter_control](https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/multicopter_control) plugin from the [ignitionrobotics/ign-gazebo](https://github.com/ignitionrobotics/ign-gazebo).
+* The controller is based on the original work off [Lee, 2010](https://ieeexplore.ieee.org/iel5/5707200/5716927/05717652.pdf).
+* The controller's interface is compatible with [multicopter_control](https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/multicopter_control), therefore, it can be swapped without the need to modify loading of its parameters.
+* This controller was forked due to the lack of specifying a feedforward action, which is vital for precise UAV control [(link)](https://arxiv.org/abs/2008.08050). Controlling the UAV only through the desired velocity limits the available dynamics significantly.
+
+|                              |                            |
+|------------------------------|----------------------------|
+| Control without feed forward | ![](./.fig/without_ff.gif) |
+| Control with feed forward    | ![](./.fig/with_ff.gif)    |
+
+### Input
+
+As the [original controller](https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/multicopter_control), the MRS controller accepts the **geometry_msgs/Twist** command.
+The twist command contains desired linear velocities in the body frame of the UAV and the desired yaw rate.
+The control response of this controller should be identical as the response of the original controller.
+
+A new **optional** input is introduced, allowing users to set desired feedforward **acceleration** and **jerk**.
+When provided, the acceleration feedforward component is added to the desired force vector produced by the original velocity feedback.
+The jerk feedforward component is used to generate an appropriate desired attitude rate feedforward action.
+Both components can be provided in the form of ROS **geometry_msgs/Twist** message on the topic
+```
+/mrs_multirotor_controller/<ROBOT NAME>/feedforward
+```
+with the **geometry_msgs/Twist/linear** representing the desired acceleration, and **geometry_msgs/Twist/angular** representing the desired jerk.
+Both component are supposed to be supplied in the body frame of the UAV, similarly to the desired velocity.
+
+### Constraining the UAV dynamics
+
+The original acceleration saturation is applied to the sum of the result of the velocity feedback (as it was originally) and the feedforward acceleration reference.
+The desired jerk feedforward is left unconstrained.
+
+### Parameters
+
+The form and the function of the controller's paramaters was left unchanged from the [original controller](https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/multicopter_control).
+
+### Disclaimer
+
+The software is provided "as is" without any guarantees.
+The sofware was not developed nor is it suited for control of real-world robots.
+The software lacks safety features that should be employed in real world to mitigate abnormal situations and special cases during its execution!
+If you are interesting in controlling a real-world multirotor helicopter, please get inspiration, e.g., in the [mrs_uav_controllers](https://github.com/ctu-mrs/mrs_uav_controllers) and [mrs_uav_system](https://github.com/ctu-mrs/mrs_uav_system) packages.

submitted_models/ctu_cras_norlab_x500_sensor_config_1/include/Common.h

@@ -0,0 +1,69 @@
+#ifndef IGNITION_GAZEBO_SYSTEMS_MULTICOPTERVELOCITYCONTROL_COMMON_HH_
+#define IGNITION_GAZEBO_SYSTEMS_MULTICOPTERVELOCITYCONTROL_COMMON_HH_
+
+/* includes() //{ */
+
+#include <Eigen/Geometry>
+#include <vector>
+
+#include <sdf/sdf.hh>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/EntityComponentManager.hh>
+#include <ignition/gazebo/Model.hh>
+
+#include <Parameters.h>
+
+//}
+
+namespace ignition
+{
+
+namespace gazebo
+{
+
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+{
+namespace systems
+{
+namespace multicopter_control
+{
+
+struct EigenTwist
+{
+  Eigen::Vector3d linear;
+  Eigen::Vector3d angular;
+};
+
+struct FrameData
+{
+  Eigen::Isometry3d pose;
+  Eigen::Vector3d   linearVelocityWorld;
+  Eigen::Vector3d   angularVelocityBody;
+};
+
+RotorConfiguration loadRotorConfiguration(const EntityComponentManager &_ecm, const sdf::ElementPtr &_sdf, const Model &_model, const Entity &_comLink);
+
+std::optional<Eigen::Matrix4Xd> calculateAllocationMatrix(const RotorConfiguration &_rotorConfiguration);
+
+void createFrameDataComponents(EntityComponentManager &_ecm, const Entity &_link);
+
+std::optional<FrameData> getFrameData(const EntityComponentManager &_ecm, const Entity &_link, const NoiseParameters &_noise);
+
+inline Eigen::Matrix3d skewMatrixFromVector(const Eigen::Vector3d &_vector) {
+  Eigen::Matrix3d skewMatrix;
+  skewMatrix << 0, -_vector.z(), _vector.y(), _vector.z(), 0, -_vector.x(), -_vector.y(), _vector.x(), 0;
+  return skewMatrix;
+}
+
+inline Eigen::Vector3d vectorFromSkewMatrix(const Eigen::Matrix3d &_skewMatrix) {
+  return Eigen::Vector3d(_skewMatrix(2, 1), _skewMatrix(0, 2), _skewMatrix(1, 0));
+}
+
+}  // namespace multicopter_control
+}  // namespace systems
+}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+}  // namespace gazebo
+}  // namespace ignition
+
+#endif

submitted_models/ctu_cras_norlab_x500_sensor_config_1/include/Parameters.h

@@ -0,0 +1,51 @@
+#ifndef IGNITION_GAZEBO_SYSTEMS_MULTICOPTER_CONTROLLER_PARAMETERS_HH_
+#define IGNITION_GAZEBO_SYSTEMS_MULTICOPTER_CONTROLLER_PARAMETERS_HH_
+
+#include <Eigen/Geometry>
+#include <vector>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+{
+namespace systems
+{
+namespace multicopter_control
+{
+struct Rotor
+{
+  double angle;
+  double armLength;
+  double forceConstant;
+  double momentConstant;
+  int    direction;
+};
+
+using RotorConfiguration = std::vector<Rotor>;
+
+struct VehicleParameters
+{
+  double             mass;
+  Eigen::Matrix3d    inertia;
+  Eigen::Vector3d    gravity;
+  RotorConfiguration rotorConfiguration;
+};
+
+struct NoiseParameters
+{
+  Eigen::Vector3d linearVelocityMean;
+  Eigen::Vector3d linearVelocityStdDev;
+  Eigen::Vector3d angularVelocityMean;
+  Eigen::Vector3d angularVelocityStdDev;
+};
+
+}  // namespace multicopter_control
+}  // namespace systems
+}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+}  // namespace gazebo
+}  // namespace ignition
+
+#endif

submitted_models/ctu_cras_norlab_x500_sensor_config_1/include/SE3Controller.h

@@ -0,0 +1,70 @@
+#ifndef IGNITION_GAZEBO_SYSTEMS_MULTICOPTER_CONTROL_SE3CONTROLLER_HH_
+#define IGNITION_GAZEBO_SYSTEMS_MULTICOPTER_CONTROL_SE3CONTROLLER_HH_
+
+#include <Eigen/Geometry>
+#include <Common.h>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+{
+namespace systems
+{
+namespace multicopter_control
+{
+
+struct SE3ControllerParameters
+{
+  Eigen::Vector3d velocity_gain;
+  Eigen::Vector3d attitude_gain;
+  Eigen::Vector3d angular_rate_gain;
+  Eigen::Vector3d max_linear_acceleration;
+};
+
+struct SE3ControllerFeedforward
+{
+  Eigen::Vector3d acceleration;
+  Eigen::Vector3d jerk;
+};
+
+class SE3Controller {
+
+public:
+  SE3Controller(const SE3ControllerParameters &controller_parameters, const VehicleParameters &vehicle_parameters);
+
+  // the main function that returns the result
+  Eigen::VectorXd CalculateRotorVelocities(const FrameData &simulator_model_data, const EigenTwist &control_command,
+                                           const SE3ControllerFeedforward &feedforward_command) const;
+
+private:
+  // | ----------------------- parameters ----------------------- |
+
+  SE3ControllerParameters _controller_parameters_;
+  VehicleParameters       _vehicle_parameters_;
+
+  bool InitializeParameters();
+
+  // parameters computed in during initialization
+  Eigen::Vector3d  normalized_attitude_gain_;
+  Eigen::Vector3d  normalized_angular_rate_gain_;
+  Eigen::MatrixX4d angular_acc_to_rotor_velocities_;
+
+  // | -------------------- internal methods -------------------- |
+
+  Eigen::Vector3d ComputeDesiredAcceleration(const FrameData &simulator_model_data, const Eigen::Vector3d &vel_ref, const Eigen::Vector3d &acc_ref) const;
+  Eigen::Vector3d SO3Controller(const FrameData &simulator_model_data, const Eigen::Vector3d &des_acceleration, const Eigen::Vector3d &des_jerk,
+                                const double &des_yaw_rate) const;
+};
+
+}  // namespace multicopter_control
+
+}  // namespace systems
+}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+}  // namespace gazebo
+}  // namespace ignition
+
+#endif

submitted_models/ctu_cras_norlab_x500_sensor_config_1/launch/description.launch

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="name" doc="Name of Vehicle"/>
+  <param name="$(arg name)/robot_description" command="$(find xacro)/xacro '$(find ctu_cras_norlab_x500_sensor_config_1)/urdf/model.xacro' name:=$(arg name)"/>
+</launch>
+