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_DYNAMICS_FRAME_ORIENTATION_HPP__
       6                 :            : #define __RDL_DYNAMICS_FRAME_ORIENTATION_HPP__
       7                 :            : 
       8                 :            : /**
       9                 :            :  * @file FrameOrientation.hpp
      10                 :            :  * @page frame_orientation Frame Orientation
      11                 :            :  *
      12                 :            :  * The RobotDynamics::Math::FrameOrientation object is a class for storing an orientation w.r.t a reference frame
      13                 :            :  */
      14                 :            : 
      15                 :            : #include <rdl_dynamics/FrameObject.hpp>
      16                 :            : #include <rdl_dynamics/rdl_eigenmath.hpp>
      17                 :            : 
      18                 :            : namespace RobotDynamics
      19                 :            : {
      20                 :            : namespace Math
      21                 :            : {
      22                 :            : /**
      23                 :            :  * @class FrameOrientation
      24                 :            :  * @ingroup reference_frame
      25                 :            :  * @brief A Frame object that represents an orientation(quaternion) relative to a reference frame
      26                 :            :  */
      27                 :            : class FrameOrientation : public FrameObject, public Quaternion
      28                 :            : {
      29                 :            :   public:
      30                 :            :     FrameOrientation() : FrameObject(nullptr), Quaternion(0., 0., 0., 1.)
      31                 :            :     {
      32                 :            :     }
      33                 :            : 
      34                 :            :     explicit FrameOrientation(ReferenceFramePtr referenceFrame) : FrameObject(referenceFrame), Quaternion(0., 0., 0., 1.)
      35                 :            :     {
      36                 :            :     }
      37                 :            : 
      38                 :          3 :     FrameOrientation(ReferenceFramePtr referenceFrame, Quaternion quat) : FrameObject(referenceFrame), Quaternion(quat)
      39                 :            :     {
      40                 :          3 :     }
      41                 :            : 
      42                 :            :     FrameOrientation(ReferenceFramePtr referenceFrame, double x, double y, double z, double w) : FrameObject(referenceFrame), Quaternion(x, y, z, w)
      43                 :            :     {
      44                 :            :     }
      45                 :            : 
      46                 :          3 :     FrameOrientation(ReferenceFramePtr referenceFrame, const Matrix3d& E) : FrameObject(referenceFrame), Quaternion(E)
      47                 :            :     {
      48                 :          3 :     }
      49                 :            : 
      50                 :          1 :     FrameOrientation(ReferenceFramePtr referenceFrame, const AxisAngle& axis_angle) : FrameObject(referenceFrame), Quaternion(axis_angle)
      51                 :            :     {
      52                 :          1 :     }
      53                 :            : 
      54                 :            :     FrameOrientation(ReferenceFramePtr referenceFrame, Vector3d axis, double angle) : FrameObject(referenceFrame), Quaternion(axis, angle)
      55                 :            :     {
      56                 :            :     }
      57                 :            : 
      58                 :          7 :     Math::TransformableGeometricObject* getTransformableGeometricObject()
      59                 :            :     {
      60                 :          7 :         return this;
      61                 :            :     }
      62                 :            : 
      63                 :            :     [[deprecated("Use FrameOrientation(ReferenceFramePtr, Quaternion) instead.")]] void setIncludingFrame(const Quaternion& q, ReferenceFramePtr referenceFrame)
      64                 :            :     {
      65                 :            :         *this = FrameOrientation(referenceFrame, q);
      66                 :            :     }
      67                 :            : 
      68                 :            :     [[deprecated("Use FrameOrientation(ReferenceFramePtr, Matrix3d) instead.")]] void setIncludingFrame(const Matrix3d& E, ReferenceFramePtr referenceFrame)
      69                 :            :     {
      70                 :            :         *this = FrameOrientation(referenceFrame, E);
      71                 :            :     }
      72                 :            : 
      73                 :            :     [[deprecated("Use FrameOrientation(ReferenceFramePtr, AxisAngle) instead.")]] void setIncludingFrame(const AxisAngle& axis_angle, ReferenceFramePtr referenceFrame)
      74                 :            :     {
      75                 :            :         *this = FrameOrientation(referenceFrame, axis_angle);
      76                 :            :     }
      77                 :            : 
      78                 :            :     [[deprecated("Use FrameOrientation(ReferenceFramePtr, Vector3d axis, double angle) instead.")]] void setIncludingFrame(Vector3d axis, double angle,
      79                 :            :                                                                                                                            ReferenceFramePtr referenceFrame)
      80                 :            :     {
      81                 :            :         *this = FrameOrientation(referenceFrame, axis, angle);
      82                 :            :     }
      83                 :            : 
      84                 :            :     void setOrientation(const Quaternion& o)
      85                 :            :     {
      86                 :            :         x() = o.x();
      87                 :            :         y() = o.y();
      88                 :            :         z() = o.z();
      89                 :            :         w() = o.z();
      90                 :            :     }
      91                 :            : 
      92                 :          1 :     void setOrientation(const Matrix3d& E)
      93                 :            :     {
      94                 :          1 :         Quaternion o(E);
      95                 :          1 :         x() = o.x();
      96                 :          1 :         y() = o.y();
      97                 :          1 :         z() = o.z();
      98                 :          1 :         w() = o.z();
      99                 :          1 :     }
     100                 :            : 
     101                 :          1 :     void setOrientation(const AxisAngle& axis_angle)
     102                 :            :     {
     103                 :          1 :         Quaternion o(axis_angle);
     104                 :          1 :         x() = o.x();
     105                 :          1 :         y() = o.y();
     106                 :          1 :         z() = o.z();
     107                 :          1 :         w() = o.z();
     108                 :          1 :     }
     109                 :            : 
     110                 :            :     void setOrientation(Vector3d axis, double angle)
     111                 :            :     {
     112                 :            :         Quaternion o(axis, angle);
     113                 :            :         x() = o.x();
     114                 :            :         y() = o.y();
     115                 :            :         z() = o.z();
     116                 :            :         w() = o.z();
     117                 :            :     }
     118                 :            : 
     119                 :          3 :     FrameOrientation changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
     120                 :            :     {
     121                 :          3 :         FrameOrientation ret = *this;
     122                 :          3 :         ret.changeFrame(referenceFrame);
     123                 :          3 :         return ret;
     124                 :          0 :     }
     125                 :            : };
     126                 :            : }  // namespace Math
     127                 :            : }  // namespace RobotDynamics
     128                 :            : 
     129                 :            : #endif  //__RDL_DYNAMICS_FRAME_ORIENTATION_HPP__