SystemModel.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ROBDEKON
 * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
 * @date       18.03.21
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 *
 * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
 * See also:
 *      - https://arxiv.org/pdf/2002.00878.pdf
 *      - https://ieeexplore.ieee.org/document/8206066/
 */
 
 
 
#ifndef ROBDEKON_SYSTEMMODEL_H
#define ROBDEKON_SYSTEMMODEL_H
 
#include <Eigen/Dense>
 
#define MANIF_NO_DEBUG
#include <manif/SE3.h>
#include <manif/SO3.h>
 
template <typename floatT>
struct StateSE3 {
    manif::SE3<floatT> pose;
};
 
template <typename floatT>
struct ControlSE3 {
   typename manif::SE3<floatT>::Tangent velocity;
};
 
template <typename floatT>
struct StateSE3xV {
    manif::SE3<floatT> pose;
    typename manif::SE3<floatT>::Tangent velocity;
};
 
template <typename floatT>
struct StateSO3xR3 {
    manif::SO3<floatT> orientation;
    Eigen::Matrix<floatT, 3, 1> position;
};
 
template <typename floatT>
struct ControlSO3xR3 {
    typename manif::SO3<floatT>::Tangent angular_velocity;
    Eigen::Matrix<floatT, 3, 1> euclidean_velocity;
};
 
 
template <typename floatT>
struct ControlSE3xV {
    Eigen::Matrix<floatT, 6, 1> acceleration;
};
 
template<typename floatT>
struct SystemModelSE3
{
    static_assert(std::is_floating_point_v<floatT>);
    struct dim {
        static constexpr long state = 6, control = 6, obs = 6;
    };
 
    using FloatT = floatT;
    using StateT = StateSE3<FloatT>;
    using ControlT = ControlSE3<FloatT>;
    using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
    using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
    using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
 
    static ObsT observationFunction(const StateT& state);
 
    static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
 
    static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
 
    static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
 
};
 
template<typename floatT>
struct SystemModelSO3xR3
{
    static_assert(std::is_floating_point_v<floatT>);
    struct dim {
        static constexpr long state = 6, control = 6, obs = 6;
    };
 
    using FloatT = floatT;
    using StateT = StateSO3xR3<FloatT>;
    using ControlT = ControlSO3xR3<FloatT>;
    using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
    using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
    using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
 
    static ObsT observationFunction(const StateT& state);
 
    static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
 
    static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
 
    static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
 
};
 
template<typename floatT>
struct SystemModelSE3xV
{
    static_assert(std::is_floating_point_v<floatT>);
    struct dim {
        static constexpr long state = 12, control = 6, obs = 6;
    };
 
    using FloatT = floatT;
    using StateT = StateSE3xV<FloatT>;
    using ControlT = ControlSE3xV<FloatT>;
    using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
    using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
    using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
 
    static ObsT observationFunction(const StateT& state);
 
    static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
 
    static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
 
    static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
 
};
 
extern template struct SystemModelSE3<float>;
extern template struct SystemModelSE3<double>;
extern template struct SystemModelSO3xR3<float>;
extern template struct SystemModelSO3xR3<double>;
extern template struct SystemModelSE3xV<float>;
extern template struct SystemModelSE3xV<double>;
#endif //ROBDEKON_SYSTEMMODEL_H