Momentum.hpp

Go to the documentation of this file.

/*
 * RDL - Robot Dynamics Library
 * Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_MOMENTUM_HPP__
#define __RDL_MOMENTUM_HPP__

#include "rdl_dynamics/ForceVector.hpp"
#include "rdl_dynamics/MotionVector.hpp"
#include "rdl_dynamics/RigidBodyInertia.hpp"

namespace RobotDynamics
{
namespace Math
{
class Momentum : public ForceVector
{
  public:
    Momentum() : ForceVector()
    {
    }

    Momentum(const double kx, const double ky, const double kz, const double lx, const double ly, const double lz) : ForceVector(kx, ky, kz, lx, ly, lz)
    {
    }

    Momentum(const Vector3d& k, const Vector3d l) : ForceVector(k.x(), k.y(), k.z(), l.x(), l.y(), l.z())
    {
    }

    Momentum(const Momentum& momentum) : ForceVector(momentum)
    {
    }

    explicit Momentum(const ForceVector& forceVector) : ForceVector(forceVector)
    {
    }

    Momentum(const RigidBodyInertia& inertia, const MotionVector& vector) : ForceVector()
    {
        computeMomentum(inertia, vector);
    }

    Momentum transform_copy(const SpatialTransform& X) const
    {
        Momentum m = *this;
        m.transform(X);
        return m;
    }

    inline void set(const RigidBodyInertia& inertia, const MotionVector& vector)
    {
        computeMomentum(inertia, vector);
    }

    EIGEN_STRONG_INLINE double& kx()
    {
        return this->data()[0];
    }

    EIGEN_STRONG_INLINE double& ky()
    {
        return this->data()[1];
    }

    EIGEN_STRONG_INLINE double& kz()
    {
        return this->data()[2];
    }

    EIGEN_STRONG_INLINE double kx() const
    {
        return this->data()[0];
    }

    EIGEN_STRONG_INLINE double ky() const
    {
        return this->data()[1];
    }

    EIGEN_STRONG_INLINE double kz() const
    {
        return this->data()[2];
    }

    EIGEN_STRONG_INLINE double& lx()
    {
        return this->data()[3];
    }

    EIGEN_STRONG_INLINE double& ly()
    {
        return this->data()[4];
    }

    EIGEN_STRONG_INLINE double& lz()
    {
        return this->data()[5];
    }

    EIGEN_STRONG_INLINE double lx() const
    {
        return this->data()[3];
    }

    EIGEN_STRONG_INLINE double ly() const
    {
        return this->data()[4];
    }

    EIGEN_STRONG_INLINE double lz() const
    {
        return this->data()[5];
    }

    inline Momentum operator+=(const Momentum& v)
    {
        (*this) << (this->mx() += v.mx()), (this->my() += v.my()), (this->mz() += v.mz()), (this->fx() += v.fx()), (this->fy() += v.fy()), (this->fz() += v.fz());
        return *this;
    }

    inline Momentum operator-=(const Momentum& v)
    {
        (*this) << (this->mx() -= v.mx()), (this->my() -= v.my()), (this->mz() -= v.mz()), (this->fx() -= v.fx()), (this->fy() -= v.fy()), (this->fz() -= v.fz());
        return *this;
    }

    EIGEN_STRONG_INLINE double operator*(const MotionVector& vector)
    {
        return this->dot(vector) * 0.5;
    }

  protected:
    inline void computeMomentum(const RigidBodyInertia& I, const MotionVector& v)
    {
        (*this) << I.Ixx * v[0] + I.Iyx * v[1] + I.Izx * v[2] + I.h[1] * v[5] - I.h[2] * v[4], I.Iyx * v[0] + I.Iyy * v[1] + I.Izy * v[2] - I.h[0] * v[5] + I.h[2] * v[3],
            I.Izx * v[0] + I.Izy * v[1] + I.Izz * v[2] + I.h[0] * v[4] - I.h[1] * v[3], -I.h[1] * v[2] + I.h[2] * v[1] + I.m * v[3],
            I.h[0] * v[2] - I.h[2] * v[0] + I.m * v[4], -I.h[0] * v[1] + I.h[1] * v[0] + I.m * v[5];
    }
};

static inline Momentum operator*(const RigidBodyInertia& I, const MotionVector& v)
{
    return Momentum(I, v);
}

static inline Momentum operator+(Momentum m1, const Momentum& m2)
{
    m1 += m2;
    return m1;
}

static inline Momentum operator-(Momentum m1, const Momentum& m2)
{
    m1 -= m2;
    return m1;
}

}  // namespace Math
}  // namespace RobotDynamics

#endif  // ifndef __RDL_MOMENTUM_HPP__