DifferentialRobotModel.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_DIFFERENTIALROBOTMODEL_H_
#define _MIRA_DIFFERENTIALROBOTMODEL_H_

#include <math/Random.h>
#include <robot/UnicycleBasedRobotModel.h>
#include <robot/DifferentialProbabilisticMotionModel.h>

namespace mira { namespace robot {


class MIRA_ROBOT_DATATYPES_EXPORT DifferentialRobotModel :
    public UnicycleBasedRobotModel,
    public DifferentialProbabilisticMotionModel
{
    MIRA_OBJECT(DifferentialRobotModel)

public:

    DifferentialRobotModel();

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, UnicycleBasedRobotModel);
        r.property("WheelDistance", wheelDist,
                   "Distance of the main drive wheels [m]. Must be greater than zero.");

        MIRA_REFLECT_BASE(r, DifferentialProbabilisticMotionModel);
    }


public:

    virtual Pose2 localKinematics(const Velocity2& v, float dt) const;

    virtual float predictStandstillDistance(const Velocity2& v) const;

    virtual float predictStandstillRotation(const Velocity2& v) const;


public:

    SignedAnglef angleFromTotalDistance(float totalLeft, float totalRight) const;


    Velocity2 convert2Velocity(float vLeft, float vRight) const;

    std::pair<float, float> convert2LRWheelSpeed(const Velocity2& v) const;

    float getMaxVelocity() const;


public:

    float wheelDist;
};

typedef boost::shared_ptr<DifferentialRobotModel> DifferentialRobotModelPtr;


// KEEP THE FOLLOWING CODE INLINE FOR PERFORMANCE REASONS !!!

inline Pose2 DifferentialRobotModel::localKinematics(const Velocity2& v,
                                                     float dt) const
{
    assert(dt >= 0.0f);

    float dx,dy;
    float dphi = dt * v.r.angle();

    if(std::abs(dphi)>0.001f) {
        // compute curvature of arc, and assume circular movement
        float R = v.x() / v.r.angle();
        dx = sinf(dphi) * R;
        dy = (1-cosf(dphi)) * R;
    } else {
        // assume translational movement, only
        float s = dt * v.t.x();
        dx = cosf(0.5f*dphi) * s;
        dy = sinf(0.5f*dphi) * s;
    }
    return Pose2(dx,dy,dphi);
}

inline float DifferentialRobotModel::predictStandstillDistance(const Velocity2& v) const
{
    assert(maxEmergencyDeceleration > 0.0f);

    // v = a*t
    // t = v/a
    // s = 0.5 * a * t^2
    // s = 0.5 * v^2 / a
    float a = maxEmergencyDeceleration;
    return 0.5f * (v.x()*v.x()) / a;
}

inline float DifferentialRobotModel::predictStandstillRotation(const Velocity2& v) const
{
    assert(maxRotDeceleration > 0.0f);

    float a = maxRotDeceleration;
    float rot = 0.5f * (v.phi()*v.phi()) / a;
    return (v.phi() > 0 ?  rot : -rot);
}


inline SignedAnglef DifferentialRobotModel::angleFromTotalDistance(float totalLeft,
                                                                   float totalRight) const
{
    assert(wheelDist > 0.0f);

    float outlineLength = two_pi<float>() * wheelDist;
    return SignedAnglef(fmod((totalRight - totalLeft)/wheelDist, outlineLength));
}

inline Velocity2 DifferentialRobotModel::convert2Velocity(float vLeft,
                                                          float vRight) const
{
    assert(wheelDist > 0.0f);
    return Velocity2((vRight + vLeft)*0.5f, 0.0f, (vRight-vLeft)/wheelDist);
}

inline std::pair<float, float> DifferentialRobotModel::convert2LRWheelSpeed(const Velocity2& v) const
{
    float vTrans = v.x();
    float vRot   = v.r.angle();
    vRot *= 0.5f*wheelDist;
    return std::make_pair(vTrans-vRot, vTrans+vRot);
}

inline float DifferentialRobotModel::getMaxVelocity() const
{
    return std::max(std::abs(maxBackwardVelocity), std::abs(maxForwardVelocity));
}


}}

#endif