Branch data     Line data    Source code

       1                 :            : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
       2                 :            : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
       3                 :            : // RDL - Robot Dynamics Library
       4                 :            : // Licensed under the zlib license. See LICENSE for more details.
       5                 :            : 
       6                 :            : #ifndef __RDL_SPATIAL_RIGID_BODY_INERTIA_HPP__
       7                 :            : #define __RDL_SPATIAL_RIGID_BODY_INERTIA_HPP__
       8                 :            : 
       9                 :            : /**
      10                 :            :  * @file SpatialRigidBodyInertia.hpp
      11                 :            :  * @page spatial_inertia Spatial Inertia
      12                 :            :  *
      13                 :            :  * A RobotDynamics::Math::SpatialInertia is a RobotDynamics::Math::RigidBodyInertia with an associated reference frame
      14                 :            :  */
      15                 :            : 
      16                 :            : #include "rdl_dynamics/FrameObject.hpp"
      17                 :            : #include "rdl_dynamics/RigidBodyInertia.hpp"
      18                 :            : 
      19                 :            : namespace RobotDynamics
      20                 :            : {
      21                 :            : namespace Math
      22                 :            : {
      23                 :            : /**
      24                 :            :  * @class SpatialInertia
      25                 :            :  * @ingroup reference_frame
      26                 :            :  * @brief A Math::SpatialInertia is a RigidBodyInertia explicitly expressed in a RobotDynamics::ReferenceFrame. The
      27                 :            :  **frame
      28                 :            :  * a Math::SpatialInertia is expressed in can be changed by calling RobotDynamics::FrameObject::changeFrame.
      29                 :            :  */
      30                 :            : class SpatialInertia : public RigidBodyInertia, public FrameObject
      31                 :            : {
      32                 :            :   public:
      33                 :            :     /**
      34                 :            :      * @brief Empty constructor. Initializes FrameObject::referenceFrame to nullptr
      35                 :            :      */
      36                 :            :     SpatialInertia() : RigidBodyInertia(), FrameObject(nullptr)
      37                 :            :     {
      38                 :            :     }
      39                 :            : 
      40                 :            :     /**
      41                 :            :      * @brief Constructor
      42                 :            :      * @param referenceFrame
      43                 :            :      */
      44                 :       1286 :     explicit SpatialInertia(ReferenceFramePtr referenceFrame) : RigidBodyInertia(), FrameObject(referenceFrame)
      45                 :            :     {
      46                 :       1286 :     }
      47                 :            : 
      48                 :            :     /**
      49                 :            :      * @brief Constructor
      50                 :            :      * @param referenceFrame
      51                 :            :      * @param mass
      52                 :            :      * @param com_mass Mass scaled center of mass vector
      53                 :            :      * @param inertia 3x3 inertia tensor
      54                 :            :      */
      55                 :            :     SpatialInertia(ReferenceFramePtr referenceFrame, double mass, const Vector3d& com_mass, const Matrix3d& inertia)
      56                 :            :       : RigidBodyInertia(mass, com_mass, inertia), FrameObject(referenceFrame)
      57                 :            :     {
      58                 :            :     }
      59                 :            : 
      60                 :            :     /**
      61                 :            :      * @brief Constructor
      62                 :            :      * @param referenceFrame
      63                 :            :      * @param m
      64                 :            :      * @param h Mass scaled center of mass
      65                 :            :      * @param Ixx
      66                 :            :      * @param Iyx
      67                 :            :      * @param Iyy
      68                 :            :      * @param Izx
      69                 :            :      * @param Izy
      70                 :            :      * @param Izz
      71                 :            :      */
      72                 :          5 :     SpatialInertia(ReferenceFramePtr referenceFrame, double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx,
      73                 :            :                    const double Izy, const double Izz)
      74                 :          5 :       : RigidBodyInertia(m, h, Ixx, Iyx, Iyy, Izx, Izy, Izz), FrameObject(referenceFrame)
      75                 :            :     {
      76                 :          5 :     }
      77                 :            : 
      78                 :            :     /**
      79                 :            :      * @brief Constructor
      80                 :            :      * @param referenceFrame
      81                 :            :      * @param inertia
      82                 :            :      */
      83                 :      10842 :     SpatialInertia(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia) : RigidBodyInertia(inertia), FrameObject(referenceFrame)
      84                 :            :     {
      85                 :      10842 :     }
      86                 :            : 
      87                 :            :     /**
      88                 :            :      * @brief Copy constructor
      89                 :            :      * @param inertia
      90                 :            :      */
      91                 :      28383 :     SpatialInertia(const SpatialInertia& inertia) : RigidBodyInertia(inertia), FrameObject(inertia.referenceFrame)
      92                 :            :     {
      93                 :      28383 :     }
      94                 :            : 
      95                 :        150 :     SpatialInertia& operator=(const SpatialInertia& I)
      96                 :            :     {
      97                 :        150 :         RigidBodyInertia::operator=(I);
      98                 :        150 :         this->referenceFrame = I.getReferenceFrame();
      99                 :        150 :         return *this;
     100                 :            :     }
     101                 :            : 
     102                 :          2 :     TransformableGeometricObject* getTransformableGeometricObject()
     103                 :            :     {
     104                 :          2 :         return this;
     105                 :            :     }
     106                 :            : 
     107                 :            :     /**
     108                 :            :      * @brief Get a copy of a Math::SpatialInertia as type Math::RigidBodyInertia
     109                 :            :      * @return A copy as type Math::RigidBodyInertia
     110                 :            :      */
     111                 :            :     RigidBodyInertia toRigidBodyInertia() const
     112                 :            :     {
     113                 :            :         return RigidBodyInertia(this->m, this->h, this->Ixx, this->Iyx, this->Iyy, this->Izx, this->Izy, this->Izz);
     114                 :            :     }
     115                 :            : 
     116                 :            :     /**
     117                 :            :      * @brief Overloaded += operator. Performs frame checks
     118                 :            :      * @param spatialInertia
     119                 :            :      */
     120                 :          1 :     void operator+=(const SpatialInertia& spatialInertia)
     121                 :            :     {
     122                 :          1 :         checkReferenceFramesMatch(&spatialInertia);
     123                 :          1 :         this->RigidBodyInertia::operator+=(spatialInertia);
     124                 :          1 :     }
     125                 :            : 
     126                 :       1470 :     RobotDynamics::Math::SpatialForce operator*(const RobotDynamics::Math::SpatialAcceleration& a)
     127                 :            :     {
     128                 :       1470 :         referenceFrame->checkReferenceFramesMatch(a.getReferenceFrame());
     129                 :       1470 :         return RobotDynamics::Math::SpatialForce(referenceFrame, this->timesSpatialVector(a));  // this->RigidBodyInertia::operator*(*this, a));
     130                 :            :     }
     131                 :            : };
     132                 :            : 
     133                 :            : /**
     134                 :            :  * @brief Adds two Math::SpatialInertia together. Performs frame checks.
     135                 :            :  * @param inertia_a
     136                 :            :  * @param inertia_b
     137                 :            :  * @return The addition of the two arguments
     138                 :            :  */
     139                 :          1 : inline SpatialInertia operator+(SpatialInertia inertia_a, const SpatialInertia& inertia_b)
     140                 :            : {
     141                 :          1 :     inertia_a += inertia_b;
     142                 :          1 :     return inertia_a;
     143                 :            : }
     144                 :            : typedef std::vector<SpatialInertia, Eigen::aligned_allocator<SpatialInertia>> SpatialInertiaV;
     145                 :            : }  // namespace Math
     146                 :            : }  // namespace RobotDynamics
     147                 :            : 
     148                 :            : #endif  //__SPATIAL_RIGID_BODY_INERTIA_HPP__