Branch data     Line data    Source code

       1                 :            : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
       2                 :            : // RDL - Robot Dynamics Library
       3                 :            : // Licensed under the zlib license. See LICENSE for more details.
       4                 :            : 
       5                 :            : #ifndef __RDL_SPATIAL_ACCELERATION_HPP__
       6                 :            : #define __RDL_SPATIAL_ACCELERATION_HPP__
       7                 :            : 
       8                 :            : #include "rdl_dynamics/SpatialMotion.hpp"
       9                 :            : 
      10                 :            : /**
      11                 :            :  * @file SpatialAcceleration.hpp
      12                 :            :  * @page spatial_acceleration Spatial Acceleration
      13                 :            :  *
      14                 :            :  * A RobotDynamics::Math::SpatialAcceleration represents a 6D acceleration vector with associated reference frames. It inherits from
      15                 :            :  * RobotDynamics::Math::SpatialMotion, and it is important to note that it will also inherit the RobotDynamics::Math::SpatialMotion::changeFrame
      16                 :            :  * method, but for an acceleration vector to change the frame it is expressed in, additional information is needed that is not available to the
      17                 :            :  * changeFrame method. Therefore it is important to note that for spatial accelerations, the changeFrame method will only return a correct
      18                 :            :  * result if there is no relative motion between the expressedInFrame and the new reference frame. If there is relative motion, you need to
      19                 :            :  * call the provided RobotDynamics::Math::SpatialAcceleration::changeFrameWithRelativeMotion method where you are able to specify the
      20                 :            :  * additional quantities required.
      21                 :            :  */
      22                 :            : 
      23                 :            : namespace RobotDynamics
      24                 :            : {
      25                 :            : namespace Math
      26                 :            : {
      27                 :            : /**
      28                 :            :  * @class SpatialAcceleration
      29                 :            :  * @ingroup reference_frame
      30                 :            :  * @brief SpatialAcceleration. For clarity, the ReferenceFrames are stated as follows. A spatial acceleration is
      31                 :            :  * the acceleration of the SpatialAcceleration::bodyFrame with respect to the SpatialAcceleration::baseFrame
      32                 :            :  * expressed in the SpatialAcceleration::expressedInFrame
      33                 :            :  */
      34                 :            : class SpatialAcceleration : public SpatialMotion
      35                 :            : {
      36                 :            :   public:
      37                 :            :     /**
      38                 :            :      * @brief Constructor. ReferenceFrame is initialized to nullptr and vector elements to 0.
      39                 :            :      */
      40                 :            :     SpatialAcceleration() : SpatialMotion()
      41                 :            :     {
      42                 :            :     }
      43                 :            : 
      44                 :            :     /**
      45                 :            :      * @brief Constructor
      46                 :            :      * @param bodyFrame The acceleration is of this frame
      47                 :            :      * @param baseFrame The acceleration is relative to this frame
      48                 :            :      * @param expressedInFrame The acceleration is expressed in this frame
      49                 :            :      * @param wx Angular x-coordinate
      50                 :            :      * @param wy Angular y-coordinate
      51                 :            :      * @param wz Angular z-coordinate
      52                 :            :      * @param vx Linear x-coordinate
      53                 :            :      * @param vy Linear y-coordinate
      54                 :            :      * @param vz Linear z-coordinate
      55                 :            :      */
      56                 :          6 :     SpatialAcceleration(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const double wx, const double wy, const double wz,
      57                 :            :                         const double vx, const double vy, const double vz)
      58                 :          6 :       : SpatialMotion(bodyFrame, baseFrame, expressedInFrame, wx, wy, wz, vx, vy, vz)
      59                 :            :     {
      60                 :          6 :     }
      61                 :            : 
      62                 :            :     /**
      63                 :            :      * @brief Constructor
      64                 :            :      * @param bodyFrame The acceleration is of this frame
      65                 :            :      * @param baseFrame The acceleration is relative to this frame
      66                 :            :      * @param expressedInFrame The acceleration is expressed in this frame
      67                 :            :      * @param w Vector containig the angular component of the acceleration
      68                 :            :      * @param v Vector containing the linear component of the acceleration
      69                 :            :      */
      70                 :            :     SpatialAcceleration(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const Vector3d& w, const Vector3d v)
      71                 :            :       : SpatialMotion(bodyFrame, baseFrame, expressedInFrame, w.x(), w.y(), w.z(), v.x(), v.y(), v.z())
      72                 :            :     {
      73                 :            :     }
      74                 :            : 
      75                 :            :     /**
      76                 :            :      * @brief Constructor
      77                 :            :      * @param bodyFrame The acceleration is of this frame
      78                 :            :      * @param baseFrame The acceleration is relative to this frame
      79                 :            :      * @param expressedInFrame The acceleration is expressed in this frame
      80                 :            :      * @param v
      81                 :            :      */
      82                 :       5990 :     SpatialAcceleration(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const SpatialVector& v)
      83                 :       5990 :       : SpatialMotion(bodyFrame, baseFrame, expressedInFrame, v)
      84                 :            :     {
      85                 :       5990 :     }
      86                 :            : 
      87                 :            :     /**
      88                 :            :      * @brief Constructor
      89                 :            :      * @param spatialAcceleration
      90                 :            :      */
      91                 :      14228 :     SpatialAcceleration(const SpatialAcceleration& spatialAcceleration)
      92                 :      14228 :       : SpatialMotion(spatialAcceleration.getBodyFrame(), spatialAcceleration.getBaseFrame(), spatialAcceleration.getReferenceFrame(), spatialAcceleration)
      93                 :            :     {
      94                 :      14228 :     }
      95                 :            : 
      96                 :          4 :     SpatialAcceleration& operator=(const SpatialAcceleration& a)
      97                 :            :     {
      98                 :          4 :         SpatialVector::operator=(a);
      99                 :          4 :         this->bodyFrame = a.getBodyFrame();
     100                 :          4 :         this->baseFrame = a.getBaseFrame();
     101                 :          4 :         this->referenceFrame = a.getReferenceFrame();
     102                 :          4 :         return *this;
     103                 :            :     }
     104                 :            : 
     105                 :            :     /**
     106                 :            :      * @brief Use this method to change the ReferenceFrame::expressedInFrame of a SpatialAcceleration if there is
     107                 :            :      * relative acceleration between the current frame and the desired frame. See V. Duindam 2.8(c) for how to
     108                 :            :      * transform a twist(i.e. SpatialMotion), and take the derivative.
     109                 :            :      * @param newFrame The new RobotDynamics::ReferenceFrame
     110                 :            :      * @param twistOfCurrentFrameWithRespectToNewFrame
     111                 :            :      * @param twistOfBodyWrtBaseExpressedInCurrent
     112                 :            :      */
     113                 :            :     void changeFrameWithRelativeMotion(ReferenceFramePtr newFrame, SpatialMotion twistOfCurrentFrameWithRespectToNewFrame,
     114                 :            :                                        const SpatialMotion& twistOfBodyWrtBaseExpressedInCurrent);
     115                 :            : 
     116                 :            :     /**
     117                 :            :      * @brief Add a SpatialAcceleration, \f$ a_sp=a_sp+vector \f$. Frame checks are performed to ensure frame
     118                 :            :      * rules are adhered to
     119                 :            :      * @param vector
     120                 :            :      */
     121                 :            :     void operator+=(const SpatialAcceleration& vector);
     122                 :            : 
     123                 :            :     /**
     124                 :            :      * @brief Subtract a SpatialAcceleration, \f$ a_sp=a_sp-vector \f$. Frame checks are performed to ensure frame
     125                 :            :      * rules are adhered to
     126                 :            :      * @param vector
     127                 :            :      */
     128                 :            :     void operator-=(const SpatialAcceleration& vector);
     129                 :            : };
     130                 :            : 
     131                 :            : /**
     132                 :            :  * @brief Add two SpatialAccelerations. Frame check will be performed to ensure frame rules are abided by
     133                 :            :  * @param v1
     134                 :            :  * @param v2
     135                 :            :  * @return A new SpatialAcceleration that is the addition of the two arguments.
     136                 :            :  */
     137                 :          1 : inline SpatialAcceleration operator+(SpatialAcceleration v1, const SpatialAcceleration& v2)
     138                 :            : {
     139                 :          1 :     v1 += v2;
     140                 :          1 :     return v1;
     141                 :            : }
     142                 :            : 
     143                 :            : /**
     144                 :            :  * @brief Subtract two SpatialAccelerations. Frame check will be performed to ensure frame rules are abided by
     145                 :            :  * @param v1
     146                 :            :  * @param v2
     147                 :            :  * @return A new SpatialAcceleration that is the subtraction of the second argument from the first.
     148                 :            :  */
     149                 :          2 : inline SpatialAcceleration operator-(SpatialAcceleration v1, const SpatialAcceleration& v2)
     150                 :            : {
     151                 :          2 :     v1 -= v2;
     152                 :          2 :     return v1;
     153                 :            : }
     154                 :            : typedef std::vector<SpatialAcceleration, Eigen::aligned_allocator<SpatialAcceleration>> SpatialAccelerationV;
     155                 :            : }  // namespace Math
     156                 :            : }  // namespace RobotDynamics
     157                 :            : 
     158                 :            : #endif  //__RDL_SPATIAL_ACCELERATION_HPP__