SystemModel.cpp

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/
 */
 
#include "SystemModel.h"
 
#include <manif/SO3.h>
 
template <typename floatT>
typename SystemModelSE3<floatT>::ObsT
SystemModelSE3<floatT>::observationFunction(const StateT& state)
{
    ObsT observation = state.pose.log().coeffs();
    return observation;
}
 
template <typename floatT>
typename SystemModelSE3<floatT>::StateT
SystemModelSE3<floatT>::propagationFunction(const StateT& state,
                                            const ControlT& control,
                                            const ControlNoiseT& noise,
                                            FloatT dt)
{
    StateT new_state;
    new_state.pose = state.pose.template rplus(control.velocity * dt);
    return new_state;
}
 
template <typename floatT>
typename SystemModelSE3<floatT>::SigmaPointsT
SystemModelSE3<floatT>::inverseRetraction(const StateT& state1, const StateT& state2)
{
    SigmaPointsT sigma;
    sigma = state2.pose.lminus(state1.pose).coeffs();
    return sigma;
}
 
template <typename floatT>
typename SystemModelSE3<floatT>::StateT
SystemModelSE3<floatT>::retraction(const StateT& state, const SigmaPointsT& sigmaPoints)
{
    StateT new_state;
    manif::SE3Tangent<FloatT> tan;
    tan.coeffs() << sigmaPoints;
    auto e = tan.exp();
    new_state.pose = state.pose.lplus(tan);
    return new_state;
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
template <typename floatT>
typename SystemModelSE3xV<floatT>::ObsT
SystemModelSE3xV<floatT>::observationFunction(const SystemModelSE3xV::StateT& state)
{
    ObsT observation = state.pose.log().coeffs();
    return observation;
}
 
template <typename floatT>
typename SystemModelSE3xV<floatT>::StateT
SystemModelSE3xV<floatT>::propagationFunction(const SystemModelSE3xV::StateT& state,
                                              const SystemModelSE3xV::ControlT& control,
                                              const SystemModelSE3xV::ControlNoiseT& noise,
                                              FloatT dt)
{
    // transform acceleration into global frame
    // TODO: figure out what to do here; probably something with adjoint
    //    Eigen::Matrix<FloatT, 3, 1> local_acc = control.acceleration.segment(0, 3) + noise.template block<3, 1>(3, 0);
    //    Eigen::Matrix<FloatT, 3, 1> acc = state.pose.asSO3().template act(local_acc);
 
    StateT new_state;
    //    manif::SO3<FloatT> rotation = state.pose.asSO3().lplus(
    //            manif::SO3Tangent<FloatT>(control.acceleration.segment(3, 3) * dt + noise.segment(0, 3)));
    //    Eigen::Matrix<FloatT, 3, 1> position = state.pose.translation() + state.velocity * dt + 0.5 * control * dt * dt;
    new_state.pose =
        state.pose.template lplus(state.velocity * dt + 0.5 * control.acceleration * dt * dt);
    new_state.velocity = state.velocity + control.acceleration * dt;
    return new_state;
}
 
template <typename floatT>
typename SystemModelSE3xV<floatT>::StateT
SystemModelSE3xV<floatT>::retraction(const SystemModelSE3xV::StateT& state,
                                     const SystemModelSE3xV::SigmaPointsT& sigmaPoints)
{
    StateT new_state;
    manif::SE3Tangent<FloatT> tan;
    tan.coeffs() << sigmaPoints.template segment<6>(0);
    new_state.pose = state.pose.lplus(tan);
    tan.coeffs() << sigmaPoints.template segment<6>(6);
    // TODO: this is probably not correct, i.e. there needs to be some interaction between velocity and pose
    new_state.velocity += tan;
    return new_state;
}
 
template <typename floatT>
typename SystemModelSE3xV<floatT>::SigmaPointsT
SystemModelSE3xV<floatT>::inverseRetraction(const SystemModelSE3xV::StateT& state1,
                                            const SystemModelSE3xV::StateT& state2)
{
    SigmaPointsT sigma;
    sigma.template segment<6>(0) = state2.pose.lminus(state1.pose).coeffs();
    // TODO: this is probably not correct; probably one cannot subtract two tangent vectors at two different poses
    //  from each other
    sigma.template segment<6>(6) = (state2.velocity - state1.velocity).coeffs();
    return sigma;
}
 
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
template <typename floatT>
typename SystemModelSO3xR3<floatT>::ObsT
SystemModelSO3xR3<floatT>::observationFunction(const SystemModelSO3xR3::StateT& state)
{
    ObsT observation;
    observation.template segment<3>(0) = state.position;
    observation.template segment<3>(3) = state.orientation.log().coeffs();
    return observation;
}
 
template <typename floatT>
typename SystemModelSO3xR3<floatT>::StateT
SystemModelSO3xR3<floatT>::propagationFunction(const SystemModelSO3xR3::StateT& state,
                                               const SystemModelSO3xR3::ControlT& control,
                                               const SystemModelSO3xR3::ControlNoiseT& noise,
                                               FloatT dt)
{
    StateT new_state;
    new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
    new_state.position = state.position + control.euclidean_velocity * dt;
    return new_state;
}
 
template <typename floatT>
typename SystemModelSO3xR3<floatT>::StateT
SystemModelSO3xR3<floatT>::retraction(const SystemModelSO3xR3::StateT& state,
                                      const SystemModelSO3xR3::SigmaPointsT& sigmaPoints)
{
    StateT new_state;
    manif::SO3Tangent<FloatT> tan;
    tan.coeffs() << sigmaPoints.template segment<3>(3);
    new_state.orientation = state.orientation.lplus(tan);
 
    new_state.position = state.position + sigmaPoints.template segment<3>(0);
    return new_state;
}
 
template <typename floatT>
typename SystemModelSO3xR3<floatT>::SigmaPointsT
SystemModelSO3xR3<floatT>::inverseRetraction(const SystemModelSO3xR3::StateT& state1,
                                             const SystemModelSO3xR3::StateT& state2)
{
    SigmaPointsT sigma;
    // TODO: check if right order of substraction
    sigma.template segment<3>(0) = state1.position - state2.position;
    sigma.template segment<3>(3) = state2.orientation.lminus(state1.orientation).coeffs();
    return sigma;
}
 
 
template struct SystemModelSE3<float>;
template struct SystemModelSE3<double>;
 
template struct SystemModelSE3xV<float>;
template struct SystemModelSE3xV<double>;
 
template struct SystemModelSO3xR3<float>;
template struct SystemModelSO3xR3<double>;