SensorValueHolonomicPlatform.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    RobotAPI
 * @author     Raphael ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <ArmarXCore/core/util/algorithm.h>
 
#include "SensorValueBase.h"
 
namespace armarx
{
    class SensorValueHolonomicPlatformVelocity : virtual public SensorValueBase
    {
    public:
        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float velocityX;
        float velocityY;
        float velocityRotation;
 
        static SensorValueInfo<SensorValueHolonomicPlatformVelocity>
        GetClassMemberInfo()
        {
            SensorValueInfo<SensorValueHolonomicPlatformVelocity> svi;
            svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityX, "velocityX");
            svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityY, "velocityY");
            svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityRotation,
                                  "velocityRotation");
            return svi;
        }
    };
 
    class SensorValueHolonomicPlatformAcceleration : virtual public SensorValueBase
    {
    public:
        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float accelerationX;
        float accelerationY;
        float accelerationRotation;
 
        void
        deriveAccelerationFromVelocityDelta(float dvx, float dvy, float dvrot, float dt)
        {
            accelerationX = dvx / dt;
            accelerationY = dvy / dt;
            accelerationRotation = dvrot / dt;
        }
 
        static SensorValueInfo<SensorValueHolonomicPlatformAcceleration>
        GetClassMemberInfo()
        {
            SensorValueInfo<SensorValueHolonomicPlatformAcceleration> svi;
            svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationX,
                                  "accelerationX");
            svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationY,
                                  "accelerationY");
            svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationRotation,
                                  "accelerationRotation");
            return svi;
        }
    };
 
    /**
     * @brief The robot's position relative to its initial pose when starting the robot unit based on odometry information. 
     * 
     * Note: The relative position undergoes a significant drift over time.
     */
    class SensorValueHolonomicPlatformRelativePosition : virtual public SensorValueBase
    {
    public:
        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
 
            float relativePositionX = 0;
        float relativePositionY = 0;
        float relativePositionRotation = 0;
 
        static SensorValueInfo<SensorValueHolonomicPlatformRelativePosition>
        GetClassMemberInfo()
        {
            SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> svi;
            svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionX,
                                  "relativePositionX");
            svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionY,
                                  "relativePositionY");
            svi.addMemberVariable(
                &SensorValueHolonomicPlatformRelativePosition::relativePositionRotation,
                "relativePositionRotation");
            return svi;
        }
    };
 
    // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
    // Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
 
    // class SensorValueHolonomicPlatformAbsolutePosition : virtual public SensorValueBase
    // {
    // public:
    //     DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
    //     float absolutePositionX;
    //     float absolutePositionY;
    //     float absolutePositionRotation;
    //     static SensorValueInfo<SensorValueHolonomicPlatformAbsolutePosition> GetClassMemberInfo()
    //     {
    //         SensorValueInfo<SensorValueHolonomicPlatformAbsolutePosition> svi;
    //         svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionX, "absolutePositionX");
    //         svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionY, "absolutePositionY");
    //         svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionRotation, "absolutePositionRotation");
    //         return svi;
    //     }
    // };
 
 
    class SensorValueHolonomicPlatform :
        virtual public SensorValueHolonomicPlatformVelocity,
        virtual public SensorValueHolonomicPlatformAcceleration,
        virtual public SensorValueHolonomicPlatformRelativePosition
    {
    public:
        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
 
            void
            setVelocitiesAndDeriveAcceleration(float vx, float vy, float vrot, float dt)
        {
            deriveAccelerationFromVelocityDelta(
                vx - velocityX, vy - velocityY, vrot - velocityRotation, dt);
            velocityX = vx;
            velocityY = vy;
            velocityRotation = vrot;
        }
 
        static SensorValueInfo<SensorValueHolonomicPlatform>
        GetClassMemberInfo()
        {
            SensorValueInfo<SensorValueHolonomicPlatform> svi;
            svi.addBaseClass<SensorValueHolonomicPlatformVelocity>();
            svi.addBaseClass<SensorValueHolonomicPlatformAcceleration>();
            svi.addBaseClass<SensorValueHolonomicPlatformRelativePosition>();
            return svi;
        }
    };
 
    // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
    // Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
 
    // class SensorValueHolonomicPlatformWithAbsolutePosition : virtual public SensorValueHolonomicPlatform, virtual public SensorValueHolonomicPlatformAbsolutePosition
    // {
    // public:
    //     DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
    //     static SensorValueInfo<SensorValueHolonomicPlatformWithAbsolutePosition> GetClassMemberInfo()
    //     {
    //         SensorValueInfo<SensorValueHolonomicPlatformWithAbsolutePosition> svi;
    //         svi.addBaseClass<SensorValueHolonomicPlatform>();
    //         svi.addBaseClass<SensorValueHolonomicPlatformAbsolutePosition>();
    //         return svi;
    //     }
    // };
} // namespace armarx