math.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_MATH_
#define SE2EZ_CORE_MATH_

#include <type_traits>
#include <vector>
#include <map>
#include <unordered_map>

#include <boost/math/constants/constants.hpp>

#include <ompl/util/RandomNumbers.h>

#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <kdl/jntarray.hpp>
#include <kdl/frames.hpp>

namespace se2ez
{
    /** \brief Helper functions and constants require for mathematical operations.
     */
    namespace math
    {
        /** \brief  Random number generation from OMPL. An instance of this class cannot be used by multiple
         * threads at once.
         */
        static ompl::RNG rng;

        static const double pi = boost::math::constants::pi<double>();
        static const double pipi = 2. * pi;
        static const double pi2 = pi / 2.;

        static const double eps = std::numeric_limits<double>::epsilon();
        static const double inf = std::numeric_limits<double>::infinity();
        static const double nan = std::numeric_limits<double>::quiet_NaN();

        /** \brief Converts a single double value into a 1 x 1 matrix.
         *  \param[in] x Value to convert.
         *  \return a 1-dimensional vector with x as the value.
         */
        Eigen::VectorXd toVector(double x);

        /** \brief Converts a vector of lists into a Matrix
         *  \param[in] vlist vector of lists to convert.
         *  \return a matrix with the values of the Eigen vectors.
         */
        Eigen::MatrixXd toMatrix(const std::vector<Eigen::VectorXd> &vlist);

        /** \brief Converts an initialize list into a vector
         *  \param[in] x Values to convert.
         *  \return a vector with the values of the list.
         */
        Eigen::VectorXd toVector(const std::initializer_list<double> &x);

        /** \brief Remap a value \e av in the interval \e a1, \e a2 to the interval \e b1, \e b2.
         *  \param[in] a1 Lower end of first interval.
         *  \param[in] a2 Upper end of first interval.
         *  \param[in] av Value in first interval.
         *  \param[in] b1 Lower end of second interval.
         *  \param[in] b2 Upper end of second interval.
         *  \return The remapped value in the second interval.
         */
        double remap(double a1, double a2, double av, double b1, double b2);

        /** \brief Clamp a value \e v between a range [a, b].
         *  \param[in] v Value to clamp.
         *  \param[in] a Lower bound.
         *  \param[in] b Upper bound.
         *  \return The clamped value.
         */
        double clamp(double v, double a, double b);

        /** \brief Return the minimum distance between two angles (wrapping from -pi to pi).
         *  \param[in] v1 First angle.
         *  \param[in] v2 Second angle.
         *  \return The distance between angles.
         */
        double angleDistance(double v1, double v2);

        /** \brief Normalize an angle between -pi to pi.
         *  \param[in] v The angle.
         *  \return The normalized angle.
         */
        double angleNormalize(double v);

        /** \brief Convert an angle to degrees.
         *  \param[in] v The angle in radians.
         *  \return The angle in degrees.
         */
        double toDegrees(double v);

        /** \brief Convert an angle to radians.
         *  \param[in] v The angle in degrees.
         *  \return The angle in radians.
         */
        double toRadians(double v);

        /** \brief Return a uniform random number between \e lower and \e upper.
         *  \param[in] lower Lower bound.
         *  \param[in] upper Upper bound.
         *  \return random number between lower and upper
         */
        double uniformReal(double lower, double upper);

        /** \brief Return a uniform random integer between lower and upper (inclusive).
         *  \param[in] lower Lower bound
         *  \param[in] upper Upper bound
         *  \return random number between lower and upper
         */
        int uniformInteger(int lower, int upper);

        /** \brief Return a random number sampled from a gaussain with given \e std and \e mean.
         *  \param[in] mean The mean of the gaussian.
         *  \param[in] std The standard deviation of the gaussian.
         *  \return random number sampled from gaussian
         */
        double gaussianReal(double mean, double std);
    }  // namespace math

    /** \brief Helper functions for transform manipulation.
     */
    namespace tf
    {
        /** \brief A template for a vector that stores Eigen objects.
         *  \tparam V Value for the vector (contains an Eigen type).
         */
        template <typename V>
        using EigenVector = std::vector<V, Eigen::aligned_allocator<V>>;

        /** \brief A template for a map that stores Eigen objects.
         *  \tparam K Key for the map (not an Eigen type).
         *  \tparam V Value for the map (contains an Eigen type).
         */
        template <typename K, typename V>
        using EigenMap = std::map<K, V,          //
                                  std::less<K>,  //
                                  Eigen::aligned_allocator<std::pair<const K, V>>>;

        /** \brief A template for an unordered map that stores Eigen objects.
         *  \tparam K Key for the map (not an Eigen type).
         *  \tparam V Value for the map (contains an Eigen type).
         */
        template <typename K, typename V>
        using EigenHash = std::unordered_map<K, V,  //
                                             std::hash<K>, std::equal_to<K>,
                                             Eigen::aligned_allocator<std::pair<const K, V>>>;

        /** \name Eigen Isometry2d operations
            \{ */

        /** \brief Converts a translation (x, y) and rotation from X-axis (theta) into a transform.
         *  \param[in] x X coordinate of translation.
         *  \param[in] y Y coordinate of translation.
         *  \param[in] theta Rotation from the X-axis.
         *  \return Transform corresponding to input.
         */
        Eigen::Isometry2d toIsometry(double x, double y, double theta);

        /** \brief Gets rotation from a frame from the X-axis (t)
         *  \param[in] frame Frame to get rotation from.
         *  \return Angle of transformation.
         */
        double getRotation(const Eigen::Isometry2d &frame);

        /** \brief Converts a vector [x, y, t] composed of a translation (x, y) and rotation from X-axis (t)
         * into a transform.
         *  \param[in] vec Vector representation of transform.
         *  \return Transform corresponding to input.
         */
        Eigen::Isometry2d toIsometry(const Eigen::Vector3d &vec);

        /** \brief Return the transform distance between two transformations, \e w is the weight of the
         * euclidean distance, and (1-\e w) the weight of the rotational distance (wrapping from -pi to pi).
         * \param[in] t1 First transform.
         * \param[in] t2 Second transform.
         * \param[in] alpha Weight of the euclidean distance (must be between 0 and 1).
         * \return The weighted distance between the transforms.
         */
        double transformDistance(const Eigen::Isometry2d &t1,
                                 const Eigen::Isometry2d &t2 = Eigen::Isometry2d::Identity(),
                                 double alpha = 0.75);

        /** \brief Converts a transformation \e frame into a vector [x, y, t] composed of a translation (x, y)
         * and rotation from X-axis(t).
         *  \param[in] frame Transform to flatten.
         *  \return Vector representation of transform.
         */
        Eigen::Vector3d flattenIsometry(const Eigen::Isometry2d &frame);

        /** \} */

        /** \name KDL and Eigen Conversions
            \{ */

        /** \brief Converts an Eigen transformation into a KDL transformation.
         *  \param[in] iso Eigen transform.
         *  \return KDL transform.
         */
        KDL::Frame EigenToKDLFrame(const Eigen::Isometry2d &iso);

        /** \brief Converts an KDL transformation into a Eigen transformation.
         *  \param[in] frame KDL transform.
         *  \return Eigen transform.
         */
        Eigen::Isometry2d KDLToEigenFrame(const KDL::Frame &frame);

        /** \brief Converts an Eigen vector into a KDL vector.
         *  \param[in] vec Eigen vector.
         *  \return KDL vector.
         */
        KDL::JntArray EigenToKDLVec(const Eigen::VectorXd &vec);

        /** \brief Converts an KDL vector into a Eigen vector.
         *  \param[in] array KDL vector.
         *  \return Eigen vector.
         */
        Eigen::VectorXd KDLToEigenVec(const KDL::JntArray &array);

        /** \} */

        /** \name Debug & Output
            \{ */

        /** \brief Returns a string "[x, y, t]" of the translation (x, y) and rotation (t) component of a
         * transform.
         *  \param[in] frame Frame to print.
         *  \return Printable string of transform data.
         */
        std::string printFrame(const Eigen::Isometry2d &frame);

        /** \brief Returns a string "[x, y, t]" of the translation (x, y) and rotation (t) component of a
         * transform.
         *  \param[in] frame Frame to print.
         *  \return Printable string of transform data.
         */
        std::string printFrame(const KDL::Frame &frame);

        /** \brief Returns a string of a vector's contents.
         *  \param[in] v Vector to print.
         *  \param[in] precision Precision to print floating point values at.
         *  \return Printable string of vector.
         */
        std::string printVector(const Eigen::VectorXd &v, unsigned int precision = 4);

        /** \brief Returns a string of a matrix's contents.
         *  \param[in] v Matrix to print.
         *  \param[in] precision Precision to print floating point values at.
         *  \return Printable string of vector.
         */
        std::string printMatrix(const Eigen::MatrixXd &v, unsigned int precision = 4);

        /** \} */
    };  // namespace tf
};      // namespace se2ez

#endif