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_FORCE_VECTOR_HPP__
       6                 :            : #define __RDL_FORCE_VECTOR_HPP__
       7                 :            : 
       8                 :            : /**
       9                 :            :  * @file ForceVector.hpp
      10                 :            :  * @brief Contains various geometric objects that have methods for transforming themselves into different frames of reference
      11                 :            :  */
      12                 :            : #include "rdl_dynamics/rdl_eigenmath.hpp"
      13                 :            : 
      14                 :            : namespace RobotDynamics
      15                 :            : {
      16                 :            : namespace Math
      17                 :            : {
      18                 :            : /**
      19                 :            :  * @class ForceVector
      20                 :            :  * @brief A ForceVector is a SpatialVector containing 3 moments and 3 linear forces.
      21                 :            :  */
      22                 :            : class ForceVector : public SpatialVector, public TransformableGeometricObject
      23                 :            : {
      24                 :            :   public:
      25                 :            :     /**
      26                 :            :      * @brief Constructor
      27                 :            :      * @param other
      28                 :            :      */
      29                 :            :     template <typename OtherDerived>
      30                 :            :     // cppcheck-suppress noExplicitConstructor
      31                 :      10811 :     ForceVector(const Eigen::MatrixBase<OtherDerived>& other) : SpatialVector(other)
      32                 :            :     {
      33                 :      10811 :     }
      34                 :            : 
      35                 :            :     /**
      36                 :            :      * Overloaded equal operator
      37                 :            :      * @param other
      38                 :            :      * @return Copied ForceVector
      39                 :            :      */
      40                 :            :     template <typename OtherDerived>
      41                 :          1 :     ForceVector& operator=(const Eigen::MatrixBase<OtherDerived>& other)
      42                 :            :     {
      43                 :          1 :         SpatialVector::operator=(other);
      44                 :          1 :         return *this;
      45                 :            :     }
      46                 :            : 
      47                 :            :     /**
      48                 :            :      * @brief Empty constructor
      49                 :            :      */
      50                 :      31490 :     EIGEN_STRONG_INLINE ForceVector() : SpatialVector(0., 0., 0., 0., 0., 0.)
      51                 :            :     {
      52                 :      31490 :     }
      53                 :            : 
      54                 :            :     /**
      55                 :            :      * @brief Get a copy of a ForceVector as type SpatialVector
      56                 :            :      */
      57                 :            :     EIGEN_STRONG_INLINE SpatialVector toSpatialVector() const
      58                 :            :     {
      59                 :            :         return *this;
      60                 :            :     }
      61                 :            : 
      62                 :            :     /**
      63                 :            :      * @brief Constructor
      64                 :            :      * @param mx x-Moment
      65                 :            :      * @param my y-Moment
      66                 :            :      * @param mz z-Moment
      67                 :            :      * @param fx x-Linear Force
      68                 :            :      * @param fy y-Linear Force
      69                 :            :      * @param fz z-Linear Force
      70                 :            :      */
      71                 :      36246 :     ForceVector(const double mx, const double my, const double mz, const double fx, const double fy, const double fz)
      72                 :      36246 :     {
      73                 :      36246 :         Base::_check_template_params();
      74                 :            : 
      75                 :      36246 :         (*this) << mx, my, mz, fx, fy, fz;
      76                 :      36246 :     }
      77                 :            : 
      78                 :            :     /**
      79                 :            :      * @brief Setter
      80                 :            :      * @param f
      81                 :            :      */
      82                 :       3583 :     EIGEN_STRONG_INLINE void set(const ForceVector& f)
      83                 :            :     {
      84                 :       3583 :         (*this) << f[0], f[1], f[2], f[3], f[4], f[5];
      85                 :       3583 :     }
      86                 :            : 
      87                 :            :     /**
      88                 :            :      * @brief Get reference to x-angular component
      89                 :            :      * @return Reference to x-angular component
      90                 :            :      */
      91                 :      10630 :     EIGEN_STRONG_INLINE double& mx()
      92                 :            :     {
      93                 :      10630 :         return this->operator[](0);
      94                 :            :     }
      95                 :            : 
      96                 :            :     /**
      97                 :            :      * @brief Get reference to y-angular component
      98                 :            :      * @return Reference to y-angular component
      99                 :            :      */
     100                 :      10630 :     EIGEN_STRONG_INLINE double& my()
     101                 :            :     {
     102                 :      10630 :         return this->operator[](1);
     103                 :            :     }
     104                 :            : 
     105                 :            :     /**
     106                 :            :      * @brief Get reference to z-angular component
     107                 :            :      * @return Reference to z-angular component
     108                 :            :      */
     109                 :      10630 :     EIGEN_STRONG_INLINE double& mz()
     110                 :            :     {
     111                 :      10630 :         return this->operator[](2);
     112                 :            :     }
     113                 :            : 
     114                 :            :     /**
     115                 :            :      * @brief Get copy of x-angular component
     116                 :            :      * @return Copy of x-angular component
     117                 :            :      */
     118                 :      13817 :     EIGEN_STRONG_INLINE double mx() const
     119                 :            :     {
     120                 :      13817 :         return this->operator[](0);
     121                 :            :     }
     122                 :            : 
     123                 :            :     /**
     124                 :            :      * @brief Get copy of y-angular component
     125                 :            :      * @return Copy of y-angular component
     126                 :            :      */
     127                 :      13817 :     EIGEN_STRONG_INLINE double my() const
     128                 :            :     {
     129                 :      13817 :         return this->operator[](1);
     130                 :            :     }
     131                 :            : 
     132                 :            :     /**
     133                 :            :      * @brief Get copy of z-angular component
     134                 :            :      * @return Copy of z-angular component
     135                 :            :      */
     136                 :      13817 :     EIGEN_STRONG_INLINE double mz() const
     137                 :            :     {
     138                 :      13817 :         return this->operator[](2);
     139                 :            :     }
     140                 :            : 
     141                 :            :     /**
     142                 :            :      * @brief Get reference to x-linear component
     143                 :            :      * @return Reference to x-linear component
     144                 :            :      */
     145                 :      10630 :     EIGEN_STRONG_INLINE double& fx()
     146                 :            :     {
     147                 :      10630 :         return this->operator[](3);
     148                 :            :     }
     149                 :            : 
     150                 :            :     /**
     151                 :            :      * @brief Get reference to y-linear component
     152                 :            :      * @return Reference to y-linear component
     153                 :            :      */
     154                 :      10626 :     EIGEN_STRONG_INLINE double& fy()
     155                 :            :     {
     156                 :      10626 :         return this->operator[](4);
     157                 :            :     }
     158                 :            : 
     159                 :            :     /**
     160                 :            :      * @brief Get reference to z-linear component
     161                 :            :      * @return Reference to z-linear component
     162                 :            :      */
     163                 :      10630 :     EIGEN_STRONG_INLINE double& fz()
     164                 :            :     {
     165                 :      10630 :         return this->operator[](5);
     166                 :            :     }
     167                 :            : 
     168                 :            :     /**
     169                 :            :      * @brief Get copy of x-linear component
     170                 :            :      * @return Copy of x-linear component
     171                 :            :      */
     172                 :      13817 :     EIGEN_STRONG_INLINE double fx() const
     173                 :            :     {
     174                 :      13817 :         return this->operator[](3);
     175                 :            :     }
     176                 :            : 
     177                 :            :     /**
     178                 :            :      * @brief Get copy of y-linear component
     179                 :            :      * @return Copy of y-linear component
     180                 :            :      */
     181                 :      13817 :     EIGEN_STRONG_INLINE double fy() const
     182                 :            :     {
     183                 :      13817 :         return this->operator[](4);
     184                 :            :     }
     185                 :            : 
     186                 :            :     /**
     187                 :            :      * @brief Get copy of z-linear component
     188                 :            :      * @return Copy of z-linear component
     189                 :            :      */
     190                 :      13817 :     EIGEN_STRONG_INLINE double fz() const
     191                 :            :     {
     192                 :      13817 :         return this->operator[](5);
     193                 :            :     }
     194                 :            : 
     195                 :            :     /**
     196                 :            :      * @brief Performs the following in place transform \f[
     197                 :            :      * f =
     198                 :            :      * \begin{bmatrix}
     199                 :            :      * X.E & -X.E(X.r\times) \\
     200                 :            :      * \mathbf{0} & X.E
     201                 :            :      * \end{bmatrix}
     202                 :            :      * f
     203                 :            :      * \f]
     204                 :            :      * @param X
     205                 :            :      */
     206                 :      29264 :     void transform(const SpatialTransform& X)
     207                 :            :     {
     208                 :      29264 :         this->setAngularPart(X.E * (this->getAngularPart() - X.r.cross(this->getLinearPart())));
     209                 :      29264 :         this->setLinearPart(X.E * this->getLinearPart());
     210                 :      29264 :     }
     211                 :            : 
     212                 :            :     /**
     213                 :            :      * @brief Copy then transform a ForceVector by \f[
     214                 :            :      * f_2 =
     215                 :            :      * \begin{bmatrix}
     216                 :            :      * X.E & -X.E(X.r\times) \\
     217                 :            :      * \mathbf{0} & X.E
     218                 :            :      * \end{bmatrix}
     219                 :            :      * f_1
     220                 :            :      * \f]
     221                 :            :      * @param X
     222                 :            :      * @return Copied, transformed ForceVector
     223                 :            :      */
     224                 :          2 :     ForceVector transform_copy(const SpatialTransform& X) const
     225                 :            :     {
     226                 :          2 :         ForceVector f = *this;
     227                 :          2 :         f.transform(X);
     228                 :          2 :         return f;
     229                 :            :     }
     230                 :            : 
     231                 :            :     /**
     232                 :            :      * @brief Overloaded plus-equals operator
     233                 :            :      * @return \f$ f_2=f_2+f_1 \f$
     234                 :            :      */
     235                 :         27 :     inline ForceVector operator+=(const ForceVector& v)
     236                 :            :     {
     237                 :         27 :         (*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());
     238                 :         27 :         return *this;
     239                 :            :     }
     240                 :            : };
     241                 :            : 
     242                 :            : /**
     243                 :            :  * @brief Operator for transforming a ForceVector. Calls the ForceVector::transform method.
     244                 :            :  * @param X SpatialTransform
     245                 :            :  * @param f ForceVector to be transformed
     246                 :            :  * @return Transformed ForceVector
     247                 :            :  */
     248                 :          1 : inline ForceVector operator*(const SpatialTransform& X, ForceVector f)
     249                 :            : {
     250                 :          1 :     f.transform(X);
     251                 :          1 :     return f;
     252                 :            : }
     253                 :            : typedef std::vector<ForceVector, Eigen::aligned_allocator<ForceVector>> ForceVectorV;
     254                 :            : }  // namespace Math
     255                 :            : }  // namespace RobotDynamics
     256                 :            : 
     257                 :            : #endif