Functions.h

Go to the documentation of this file.

 
#ifndef INCLUDE_RML_FUNCTIONS_H_
#define INCLUDE_RML_FUNCTIONS_H_
 
#include "EulerRPY.h"
#include "RMLExceptions.h"
#include "Types.h"
#include <algorithm>
#include <array>
#include <iostream>
#include <vector>
 
namespace rml {
struct PlaneParameters {
    double A, B, C, D;
 
    PlaneParameters()
        : A(0)
        , B(0)
        , C(0)
        , D(0)
    {
    }
};
Eigen::Vector3d ReducedVersorLemma(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2);
 
Eigen::Vector3d VersorLemma(const Eigen::RotationMatrix& r1, const Eigen::RotationMatrix& r2);
Eigen::Vector3d VersorLemma(const EulerRPY& v1, const EulerRPY& v2);
Eigen::Vector6d CartesianError(const Eigen::TransformationMatrix& in1, const Eigen::TransformationMatrix& in2);
Eigen::Vector6d CartesianError(const Eigen::Vector6d& v1, const Eigen::Vector6d& v2);
double DecreasingBellShapedFunction(double xmin, double xmax, double ymin, double ymax, double x) noexcept(false);
double IncreasingBellShapedFunction(double xmin, double xmax, double ymin, double ymax, double x) noexcept(false);
void SaturateScalar(double sat, double& value);
void SaturateVector(const double sat, Eigen::VectorXd& vector);
 
void SaturateVector(const Eigen::VectorXd& upper_limits, const Eigen::VectorXd& lower_limits, Eigen::VectorXd& vect);
 
 
double DistancePointToPlane(const Eigen::Vector3d& point, const PlaneParameters& planeParams);
Eigen::Vector3d ClosestPointOnPlane(const Eigen::Vector3d& point, const PlaneParameters& planeParams);
Eigen::Matrix3d Vect3ToSkew(const Eigen::Vector3d& t);
Eigen::Matrix6d RigidBodyMatrix(const Eigen::Vector3d& transl);
Eigen::MatrixXd ChangeJacobianObserver(Eigen::MatrixXd J, Eigen::MatrixXd Jobserver, Eigen::Vector3d CartesianError);
template <class MatT>
static bool eigen_norm_compare(MatT& a, MatT& b)
{
    return (a.norm() < b.norm());
}
template <class MatT>
MatT GreatestNormElement(const MatT& vect1, const MatT& vect2, const MatT& vect3)
{
 
    std::vector<MatT> vecs = { vect1, vect2, vect3 };
    return *std::max_element(vecs.begin(), vecs.end(), eigen_norm_compare<MatT>);
}
template <typename T>
int sgn(T val)
{
    return (T(0) < val) - (val < T(0));
}
}
 
#endif /* INCLUDE_RML_FUNCTIONS_H_ */