Body.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
#ifndef RBDL_BODY_H
#define RBDL_BODY_H
 
#include "rbdl/rbdl_math.h"
#include "rbdl/rbdl_mathutils.h"
#include <assert.h>
#include <iostream>
#include "rbdl/Logging.h"
#include "rbdl/rbdl_errors.h"
 
namespace RigidBodyDynamics
{
 
struct RBDL_DLLAPI Body {
  Body() :
    mMass (0.),
    mCenterOfMass (0., 0., 0.),
    mInertia (Math::Matrix3d::Zero()),
    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 Math::Scalar &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 Math::Scalar &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)
  {
#ifndef RBDL_USE_CASADI_MATH
    // nothing to do if we join a massles body to the current.
    if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) {
      return;
    }
#endif
 
    Math::Scalar other_mass = other_body.mMass;
    Math::Scalar new_mass = mMass + other_mass;
 
#ifndef RBDL_USE_CASADI_MATH
    if (new_mass == 0.) {
      throw Errors::RBDLError("Error: cannot join bodies as both have zero mass!\n");
    }
#endif
 
    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);
 
    LOG << "other_com = " << std::endl << other_com.transpose() << std::endl;
    LOG << "rotation = " << std::endl << transform.E << std::endl;
 
    // 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::SpatialRigidBodyInertia other_rbi =
      Math::SpatialRigidBodyInertia::createFromMassComInertiaC (other_body.mMass,
          other_body.mCenterOfMass, other_body.mInertia);
    Math::SpatialRigidBodyInertia this_rbi =
      Math::SpatialRigidBodyInertia::createFromMassComInertiaC (mMass, mCenterOfMass,
          mInertia);
 
    Math::Matrix3d inertia_other = other_rbi.toMatrix().block<3,3>(0,0);
    LOG << "inertia_other = " << std::endl << inertia_other << std::endl;
 
    // 1. Transform the inertia from other origin to other COM
    Math::Matrix3d other_com_cross = Math::VectorCrossMatrix(
                                       other_body.mCenterOfMass);
    Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross
                                       * other_com_cross.transpose();
    LOG << "inertia_other_com = " << std::endl << inertia_other_com << std::endl;
 
    // 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;
    LOG << "inertia_other_com_rotated = " << std::endl << inertia_other_com_rotated
        << std::endl;
 
    // 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);
    LOG << "inertia_other_com_rotated_this_origin = " << std::endl <<
        inertia_other_com_rotated_this_origin << std::endl;
 
    // 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;
    LOG << "inertia_summed  = " << std::endl << inertia_summed << std::endl;
 
    // 5. Transform the summed inertia to the new COM
    Math::Matrix3d new_inertia = inertia_summed - new_mass *
                                 Math::VectorCrossMatrix (new_com) * Math::VectorCrossMatrix(
                                   new_com).transpose();
 
    LOG << "new_mass = " << new_mass << std::endl;
    LOG << "new_com  = " << new_com.transpose() << std::endl;
    LOG << "new_inertia  = " << std::endl << new_inertia << std::endl;
 
    *this = Body (new_mass, new_com, new_inertia);
  }
 
  ~Body() {};
 
  Math::Scalar mMass;
  Math::Vector3d mCenterOfMass;
  Math::Matrix3d mInertia;
 
  bool mIsVirtual;
};
 
struct RBDL_DLLAPI FixedBody {
  Math::Scalar 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;
  }
};
 
}
 
/* RBDL_BODY_H */
#endif