Body.h

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef RDL_BODY_H
#define RDL_BODY_H

#include "rdl_dynamics/SpatialAlgebraOperators.h"
#include "rdl_dynamics/RigidBodyInertia.hpp"
#include "rdl_dynamics/rdl_mathutils.h"
#include <assert.h>
#include <iostream>
#include "rdl_dynamics/RdlExceptions.hpp"

namespace RobotDynamics
{
struct Body
{
    Body() : mMass(0.), mCenterOfMass(0., 0., 0.), mInertia(Math::Matrix3d::Zero(3, 3)), mIsVirtual(false)
    {
    }

    Body(const Body& body) : mMass(body.mMass), mCenterOfMass(body.mCenterOfMass), mInertia(body.mInertia), mIsVirtual(body.mIsVirtual)
    {
    }

    Body& operator=(const Body& body)
    {
        if (this != &body)
        {
            mMass = body.mMass;
            mInertia = body.mInertia;
            mCenterOfMass = body.mCenterOfMass;
            mIsVirtual = body.mIsVirtual;
        }

        return *this;
    }

    Body(const double& mass, const Math::Vector3d& com, const Math::Vector3d& gyration_radii) : mMass(mass), mCenterOfMass(com), mIsVirtual(false)
    {
        mInertia = Math::Matrix3d(gyration_radii[0], 0., 0., 0., gyration_radii[1], 0., 0., 0., gyration_radii[2]);
    }

    Body(const double& mass, const Math::Vector3d& com, const Math::Matrix3d& inertia_C) : mMass(mass), mCenterOfMass(com), mInertia(inertia_C), mIsVirtual(false)
    {
    }

    void join(const Math::SpatialTransform& transform, const Body& other_body)
    {
        // nothing to do if we join a massles body to the current.
        if ((other_body.mMass == 0.) && (other_body.mInertia == Math::Matrix3d::Zero()))
        {
            return;
        }

        double other_mass = other_body.mMass;
        double new_mass = mMass + other_mass;

        if (new_mass == 0.)
        {
            throw RdlException("Error: cannot join bodies as both have zero mass!");
        }

        Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r;
        Math::Vector3d new_com = (1 / new_mass) * (mMass * mCenterOfMass + other_mass * other_com);

        // We have to transform the inertia of other_body to the new COM. This
        // is done in 4 steps:
        //
        // 1. Transform the inertia from other origin to other COM
        // 2. Rotate the inertia that it is aligned to the frame of this body
        // 3. Transform inertia of other_body to the origin of the frame of
        // this body
        // 4. Sum the two inertias
        // 5. Transform the summed inertia to the new COM

        Math::RigidBodyInertia other_rbi = Math::createFromMassComInertiaC(other_body.mMass, other_body.mCenterOfMass, other_body.mInertia);
        Math::RigidBodyInertia this_rbi = Math::createFromMassComInertiaC(mMass, mCenterOfMass, mInertia);

        Math::Matrix3d inertia_other = other_rbi.toMatrix().block<3, 3>(0, 0);

        // 1. Transform the inertia from other origin to other COM
        Math::Matrix3d other_com_cross = Math::toTildeForm(other_body.mCenterOfMass);
        Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross * other_com_cross.transpose();

        // 2. Rotate the inertia that it is aligned to the frame of this body
        Math::Matrix3d inertia_other_com_rotated = transform.E.transpose() * inertia_other_com * transform.E;

        // 3. Transform inertia of other_body to the origin of the frame of this body
        Math::Matrix3d inertia_other_com_rotated_this_origin = Math::parallel_axis(inertia_other_com_rotated, other_mass, other_com);

        // 4. Sum the two inertias
        Math::Matrix3d inertia_summed = Math::Matrix3d(this_rbi.toMatrix().block<3, 3>(0, 0)) + inertia_other_com_rotated_this_origin;

        // 5. Transform the summed inertia to the new COM
        Math::Matrix3d new_inertia = inertia_summed - new_mass * Math::toTildeForm(new_com) * Math::toTildeForm(new_com).transpose();

        *this = Body(new_mass, new_com, new_inertia);
    }

    ~Body()
    {
    }

    double mMass;

    Math::Vector3d mCenterOfMass;

    Math::Matrix3d mInertia;

    bool mIsVirtual;
};

struct FixedBody
{
    double mMass;

    Math::Vector3d mCenterOfMass;

    Math::Matrix3d mInertia;

    unsigned int mMovableParent;

    // fixed body.
    Math::SpatialTransform mParentTransform;
    Math::SpatialTransform mBaseTransform;

    static FixedBody CreateFromBody(const Body& body)
    {
        FixedBody fbody;

        fbody.mMass = body.mMass;
        fbody.mCenterOfMass = body.mCenterOfMass;
        fbody.mInertia = body.mInertia;

        return fbody;
    }
};
}  // namespace RobotDynamics

#endif  // ifndef RDL_BODY_H