Wrench.h

Go to the documentation of this file.

/*
 * Copyright (C) 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_WRENCH_H_
#define _MIRA_WRENCH_H_

#include <math/Eigen.h>

namespace mira { namespace robot {


template <typename ScalarType>
class Wrench2
{
    typedef Eigen::Matrix<ScalarType,2,1> VectorType;

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW


    Wrench2() {}

    Wrench2(const VectorType& f, ScalarType t) :
        force(f),
        torque(t)
    {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.property("Force", force, "The force");
        r.property("Torque", torque, "The torque");
    }


public:
    VectorType force;
    ScalarType torque;
};

template <typename ScalarType>
class Wrench3
{
    typedef Eigen::Matrix<ScalarType,3,1> VectorType;

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW


    Wrench3() {}

    Wrench3(const VectorType& f, const VectorType& t) :
        force(f),
        torque(t)
    {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.property("Force", force, "The force");
        r.property("Torque", torque, "The torque");
    }


public:
    VectorType force;
    VectorType torque;
};


template<typename ScalarType>
Wrench3<ScalarType> operator * (const Pose3& trans, const Wrench3<ScalarType>& w)
{
    Eigen::Matrix<ScalarType,3,3> rot = trans.r.toRotationMatrix();
    return Wrench3<ScalarType>(rot*w.force, rot*w.torque);
}

template<typename ScalarType>
Wrench3<ScalarType> operator ^ (const Pose3& trans, const Wrench3<ScalarType>& w)
{
    Eigen::Matrix<ScalarType,3,3> rot = trans.r.toRotationMatrix();
    return Wrench3<ScalarType>(rot*w.force, rot*(w.torque-trans.t.cross(w.torque)));
}


typedef Wrench2<float> Wrench2f;
typedef Wrench2<double> Wrench2d;

typedef Wrench3<float> Wrench3f;
typedef Wrench3<double> Wrench3d;


}}

#endif