Common.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2019 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
 
#ifndef GZ_SIM_SYSTEMS_MULTICOPTERVELOCITYCONTROL_COMMON_HH_
#define GZ_SIM_SYSTEMS_MULTICOPTERVELOCITYCONTROL_COMMON_HH_
 
#include <Eigen/Geometry>
#include <optional>
#include <vector>
 
#include <sdf/sdf.hh>
 
#include "gz/sim/config.hh"
#include "gz/sim/EntityComponentManager.hh"
#include "gz/sim/Model.hh"
 
#include "Parameters.hh"
 
namespace gz
{
namespace sim
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems
{
namespace multicopter_control
{
  struct EigenTwist
  {
    Eigen::Vector3d linear;
    Eigen::Vector3d angular;
  };
 
  struct FrameData
  {
    // Even though this struct contains Eigen objects, None of them are
    // fixed-size vectorizable, so there is no need to override the new operator
    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 GZ_SIM_VERSION_NAMESPACE
}  // namespace sim
}  // namespace gz
 
#endif