LeeVelocityController.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_MULTICOPTER_CONTROL_LEEVELOCITYCONTROLLER_HH_
#define GZ_SIM_SYSTEMS_MULTICOPTER_CONTROL_LEEVELOCITYCONTROLLER_HH_
 
#include <Eigen/Geometry>
#include <memory>
#include "gz/sim/config.hh"
 
#include "Common.hh"
#include "LeeVelocityController.hh"
 
namespace gz
{
namespace sim
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems
{
namespace multicopter_control
{
  struct LeeVelocityControllerParameters
  {
    Eigen::Vector3d velocityGain;
    Eigen::Vector3d attitudeGain;
    Eigen::Vector3d angularRateGain;
    Eigen::Vector3d maxLinearAcceleration;
  };
 
  class LeeVelocityController
  {
    public: static std::unique_ptr<LeeVelocityController> MakeController(
                const LeeVelocityControllerParameters &_controllerParams,
                const VehicleParameters &_vehicleParams);
 
    public: void CalculateRotorVelocities(
                 const FrameData &_frameData,
                 const EigenTwist &_cmdVel,
                 Eigen::VectorXd &_rotorVelocities) const;
 
    private: LeeVelocityController() = default;
 
    private: Eigen::Vector3d ComputeDesiredAcceleration(
                 const FrameData &_frameData, const EigenTwist &_cmdVel) const;
 
    private: Eigen::Vector3d ComputeDesiredAngularAcc(
                 const FrameData &_frameData, const EigenTwist &_cmdVel,
                 const Eigen::Vector3d &_acceleration) const;
 
    private: bool InitializeParameters();
 
    private: LeeVelocityControllerParameters controllerParameters;
 
    private: VehicleParameters vehicleParameters;
 
    private: Eigen::Vector3d normalizedAttitudeGain;
 
    private: Eigen::Vector3d normalizedAngularRateGain;
 
    private: Eigen::MatrixX4d angularAccToRotorVelocities;
  };
}  // namespace multicopter_control
}  // namespace systems
}  // namespace GZ_SIM_VERSION_NAMESPACE
}  // namespace sim
}  // namespace gz
 
#endif