dynamic_orientation.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */
#ifndef TAPROOT_DYNAMIC_ORIENTATION_HPP_
#define TAPROOT_DYNAMIC_ORIENTATION_HPP_
 
#include "tap/algorithms/cmsis_mat.hpp"
 
#include "angular_velocity.hpp"
#include "orientation.hpp"
 
namespace tap::algorithms::transforms
{
class DynamicOrientation
{
public:
    inline DynamicOrientation(
        const float roll,
        const float pitch,
        const float yaw,
        const float rollVel,
        const float pitchVel,
        const float yawVel)
        : orientation(Orientation::fromEulerAngles(roll, pitch, yaw)),
          angularVelocity(AngularVelocity::skewMatFromAngVel(rollVel, pitchVel, yawVel))
    {
    }
 
    inline DynamicOrientation(Orientation&& orientation, AngularVelocity&& angularVelocity)
        : orientation(std::move(orientation.matrix_)),
          angularVelocity(std::move(angularVelocity.matrix_))
    {
    }
 
    inline DynamicOrientation(Orientation& orientation, AngularVelocity& angularVelocity)
        : orientation(orientation.matrix_),
          angularVelocity(angularVelocity.matrix_)
    {
    }
 
    inline DynamicOrientation(
        const CMSISMat<3, 3>&& orientation,
        const CMSISMat<3, 3>&& angularVelocity)
        : orientation(std::move(orientation)),
          angularVelocity(std::move(angularVelocity))
    {
    }
 
    /* Costly; use rvalue reference whenever possible */
    inline DynamicOrientation(
        const CMSISMat<3, 3>& orientation,
        const CMSISMat<3, 3>& angularVelocity)
        : orientation(orientation),
          angularVelocity(angularVelocity)
    {
    }
 
    DynamicOrientation compose(const DynamicOrientation& other) const
    {
        return DynamicOrientation(
            this->orientation * other.orientation,
            this->angularVelocity +
                this->orientation * other.angularVelocity * this->orientation.transpose());
    }
 
    DynamicOrientation inverse() const
    {
        return DynamicOrientation(
            this->orientation.transpose(),
            -(this->orientation.transpose() * this->angularVelocity * this->orientation));
    }
 
    inline Orientation getOrientation() const { return Orientation(orientation); }
 
    inline AngularVelocity getAngularVelocity() const { return AngularVelocity(angularVelocity); }
 
    inline float roll() const { return atan2(orientation.data[7], orientation.data[8]); }
 
    inline float pitch() const { return asinf(-orientation.data[6]); }
 
    inline float yaw() const { return atan2(orientation.data[3], orientation.data[0]); }
 
    inline float getRollVelocity() const { return angularVelocity.data[0 * 3 + 2]; }
 
    inline float getPitchVelocity() const { return -angularVelocity.data[1 * 3 + 2]; }
 
    inline float getYawVelocity() const { return -angularVelocity.data[0 * 3 + 1]; }
 
    friend class Transform;
 
private:
    CMSISMat<3, 3> orientation;
 
    CMSISMat<3, 3> angularVelocity;
 
};  // class DynamicOrientation
}  // namespace tap::algorithms::transforms
 
#endif  // TAPROOT_DYNAMIC_ORIENTATION_HPP_