DifferentialProbabilisticMotionModel.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_DIFFERENTIALPROBABILISTICMOTIONMODEL_H_
#define _MIRA_DIFFERENTIALPROBABILISTICMOTIONMODEL_H_

#include <math/Random.h>

#include <robot/RobotDataTypesExports.h>
#include <robot/ProbabilisticMotionModel.h>

namespace mira { namespace robot {


class MIRA_ROBOT_DATATYPES_EXPORT DifferentialProbabilisticMotionModel : public ProbabilisticMotionModel
{
public:
    struct MotionParameters
    {
        MotionParameters() :
            trans(0.0f), rot1(0.0f), rot2(0.0f),
            transStddev(0.0f), rot1Stddev(0.0f), rot2Stddev(0.0f) {}

        // means
        float trans;
        float rot1;
        float rot2;

        // std.devs
        float transStddev;
        float rot1Stddev;
        float rot2Stddev;
    };

public:

    DifferentialProbabilisticMotionModel();

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.property("AlphaRotRot", alphaRotRot,
                   "Alpha for rotation drift while rotating", 0.2f);
        r.property("AlphaRotTrans", alphaRotTrans,
                   "Alpha for translation drift while rotating", 0.2f);
        r.property("AlphaTransTrans", alphaTransTrans,
                   "Alpha for translation drift while translating", 0.2f);
        r.property("AlphaTransRot", alphaTransRot,
                   "Alpha for rotation drift while translating", 0.2f);
    }


public:

    MotionParameters calculateMotionParameters(const Pose2& p0, const Pose2& p1) const;


    virtual Pose2 sampleMotion(const Pose2& startPose, const Pose2& p0, const Pose2& p1) const;

    virtual void sampleMotion(const std::vector<Pose2>& startPoses, const Pose2& p0, const Pose2& p1,
                              std::vector<Pose2>& sampledPoses) const;

    Pose2 sampleMotion(const Pose2& startPose, const MotionParameters& parameters) const;

    PoseCov2 gaussianMotion(const PoseCov2& startPose, const MotionParameters& parameters) const;

public:

    float alphaRotRot;
    float alphaRotTrans;
    float alphaTransTrans;
    float alphaTransRot;
};


inline Pose2 DifferentialProbabilisticMotionModel::sampleMotion(const Pose2& startPose,
                                                                const MotionParameters& parameters) const
{
    float dTransSample = parameters.trans - (float)MIRA_RANDOM.normal(parameters.transStddev);
    float dRot1Sample = smallestAngleDifference(parameters.rot1,
                                                (float)MIRA_RANDOM.normal(parameters.rot1Stddev));
    float dRot2Sample = smallestAngleDifference(parameters.rot2,
                                                (float)MIRA_RANDOM.normal(parameters.rot2Stddev));

    return Pose2(startPose.x() + dTransSample * std::cos(startPose.phi() + dRot1Sample),
                 startPose.y() + dTransSample * std::sin(startPose.phi() + dRot1Sample),
                 startPose.phi() + SignedAnglef(dRot1Sample + dRot2Sample).rad());

}


}}

#endif