LocalGuidance
A library that generates velocity references to follow a path.
robot_utils Namespace Reference

Namespaces

 catmull_rom
 
 geometry
 
 optimization
 
 physical_definitions
 
 traits
 

Classes

class  AverageForceCalibrator
 Calibrates the force offset using a Cumulative Moving Average filter. More...
 
class  BinaryChatteringCompensator
 
class  ContactDetectorBase
 
class  ContactDetectorThresholding
 
class  ContinuousTimeTransferFunction
 
class  CumulativeMovingAverageFilter
 Cumulative Moving Average Filter. More...
 
class  Delay
 
class  DiscretePidEigen
 
class  DiscreteTimeTransferFunction
 
class  Ellipsoid
 
class  FctLinChirp
 Linear chirp function. More...
 
class  FctLogChirp
 Log chirp function. More...
 
class  FilterButter
 FilterButter. More...
 
class  FilteredVariable
 
class  FilterMovAvg
 
class  FirstOrderDerivative
 First order discrete time derivative filter implementation. More...
 
class  FirstOrderFilter
 First order discrete time filter implementation. More...
 
class  ForceCalibratorBase
 
struct  ForceCalibratorCommand
 Command to control the force calibrator. More...
 
struct  ForceCalibratorStats
 Statistics of the force calibrator. More...
 
class  FunctionGeneratorBase
 
class  GaussianBaseTrajectory
 
class  GNSS
 
class  Joystick
 
class  NoneForceCalibrator
 This calibrator forwards the uncalibrated wrench. More...
 
class  PIDController
 
class  PIDControllerFeedForward
 
class  PidControllerQuaternion
 
class  PidEigen
 
struct  PIDGains
 
class  Profile
 
class  ProfileLogChirpUpSweep
 
class  ProfileRamp
 
class  ProfileSinusoid
 
class  ProfileStep
 
class  Schedule
 
class  SecondOrderFilteredVariable
 
class  Surface
 

Typedefs

using PIDGainsD = PIDGains< double >
 
using PIDGains3D = PIDGains< Eigen::Vector3d >
 
typedef FilteredVariable< double > FilteredDouble
 
using FirstOrderFilterD = FirstOrderFilter< double >
 Define some helper types for filters. More...
 
using FirstOrderFilterF = FirstOrderFilter< float >
 
using FilterMovAvgD = FilterMovAvg< double >
 
using FilterMovAvgF = FilterMovAvg< float >
 
using FirstOrderFilterEigenMatrix3d = FirstOrderFilter< Eigen::Matrix3d >
 
using FirstOrderFilterEigenVector3d = FirstOrderFilter< Eigen::Vector3d >
 
using FirstOrderFilterKindrVector3d = FirstOrderFilter< kindr::VectorTypeless3D >
 
using FirstOrderFilterKindrPosition = FirstOrderFilter< kindr::Position3D >
 
using FirstOrderFilterKindrLinearVelocity = FirstOrderFilter< kindr::Velocity3D >
 
using FirstOrderFilterKindrAngularVelocity = FirstOrderFilter< kindr::LocalAngularVelocityD >
 
template<typename ValueType , unsigned int Rows, unsigned int Cols>
using FirstOrderFilterEigenMatrixFixed = FirstOrderFilter< Eigen::Matrix< ValueType, Rows, Cols >>
 
template<typename T >
using VectorType = typename std::conditional< traits::is_eigen_matrix< T >::value, std::vector< T, Eigen::aligned_allocator< T >>, std::vector< T >>::type
 

Functions

Eigen::VectorXi integerSequence (const int low, const int high)
 
template<typename DerivedA , typename DerivedB >
void setAtSubscripts (Eigen::DenseBase< DerivedA > &filledObject, const Eigen::DenseBase< DerivedB > &includedObject, const Eigen::VectorXi &rowSubscripts, const Eigen::VectorXi &columnSubscripts)
 
template<typename DerivedA , typename DerivedB >
void addAtSubscripts (Eigen::DenseBase< DerivedA > &filledObject, const Eigen::DenseBase< DerivedB > &includedObject, const Eigen::VectorXi &rowSubscripts, const Eigen::VectorXi &columnSubscripts)
 
template<typename TypeA , typename DerivedB >
void addTripletsAtSubscripts (std::vector< Eigen::Triplet< TypeA >> &tripletList, const Eigen::DenseBase< DerivedB > &includedObject, const Eigen::VectorXi &rowSubscripts, const Eigen::VectorXi &columnSubscripts)
 
template<typename ValueType >
constexpr ValueType getDefaultValue (typename std::enable_if< !std::is_arithmetic< ValueType >::value &&!traits::is_eigen_matrix< ValueType >::value >::type *=0)
 
template<typename ValueType >
constexpr ValueType getDefaultValue (typename std::enable_if< std::is_arithmetic< ValueType >::value >::type *=0)
 
template<typename ValueType >
constexpr ValueType getDefaultValue (typename std::enable_if< traits::is_eigen_matrix< ValueType >::value >::type *=0)
 
kindr::HomTransformQuatD getPoseEndToReferenceFromTwistReferenceToStartInStartFrame (const kindr::HomTransformQuatD &poseStartToReference, const kindr::TwistLocalD &twistReferenceToStartInStartFrame, double timeHorizonInSeconds)
 
kindr::HomTransformQuatD getApproximatedPoseEndToReferenceFromTwistReferenceToStartInStartFrame (const kindr::HomTransformQuatD &poseStartToReference, const kindr::TwistLocalD &TwistReferenceToStartInStartFrame, double timeHorizonInSeconds)
 
Eigen::MatrixXd pseudoInverseAdaptiveDls (const Eigen::MatrixXd &A, double maxDampingFactor=0.02, double singularRegionDimension=0.06)
 
void boundToRange (double *v, double min, double max)
 
void boundToRange (int *v, int min, int max)
 
double boundToRange (double v, double min, double max)
 
double mapTo01Range (double v, double min, double max)
 
double mapToUniformRange (double v, double min, double max)
 
double linearlyInterpolate (double v1, double v2, double t1, double t2, double t)
 
Eigen::Vector3d linearlyInterpolate (const Eigen::Vector3d &v1, const Eigen::Vector3d &v2, double t1, double t2, double t)
 
kindr::Position3D linearlyInterpolate (const kindr::Position3D &v1, const kindr::Position3D &v2, double t1, double t2, double t)
 
template<typename T >
mapInRange (T x, T in_min, T in_max, T out_min, T out_max)
 
int intmod (int n, int b)
 Map an integer n to the periodic interval (0, b-1). More...
 
int intmodRange (int n, int a, int b)
 Map an integer n to the interval [a, b]. More...
 
bool areNear (double a, double b, double tol=1e-6)
 Check if two values are numerically close. More...
 
bool isEqualOrLargerThan (double a, double b, double tol=1e-6)
 True if a is equal or larger than the b. More...
 
bool isEqualOrSmallerThan (double a, double b, double tol=1e-6)
 True if a is equal or smaller than the b. More...
 
bool isLargerThan (double a, double b, double tol=1e-6)
 True if a is larger than the b. More...
 
bool isSmallerThan (double a, double b, double tol=1e-6)
 True if a smaller than the b. More...
 
double computeNorm (double x, double y)
 
double computeNorm (double x, double y, double z)
 
void mapToUnitInterval (double &phase, double tol=1e-6)
 map a phase event to the interval [0,1). More...
 
void mapToInterval (double &phase, double interval_start, double tol=1e-6)
 map a phase event to the interval [start,start+1.0]. More...
 
double getPhaseDifference (double phase1, double phase2, double tol=1e-6)
 Assuming that phase1 happens before phase2, this function will return. More...
 
void alphaFilter (Eigen::Vector3d &vector, const Eigen::Vector3d &vectorRef, double weight, double weightRef)
 
template<typename T >
generateRandomNumber (T min=std::numeric_limits< T >::lowest(), T max=std::numeric_limits< T >::max())
 
template<typename Container_ >
void applyAbs (Container_ &c)
 
template<typename Container_ >
Container_ abs (const Container_ &c)
 
template<typename Container_ >
double absPeak (const Container_ &c)
 
template<typename Container_ >
double mean (const Container_ &c)
 
template<typename Container_ >
double rms (const Container_ &c)
 
template<typename Container_ >
double var (const Container_ &c)
 
template<typename Container_ >
double stdev (const Container_ &c)
 
double my_ran0 (int *idum)
 
double my_gasdev (int *idum)
 
double gaussian (double value, double std)
 
double normalPdf (const Eigen::VectorXd &value, const Eigen::VectorXd &mean, const Eigen::MatrixXd &cov)
 
template<unsigned int N_>
double normalPdf (const Eigen::Matrix< double, N_, 1 > &value, const Eigen::Matrix< double, N_, 1 > &mean, const Eigen::Matrix< double, N_, N_ > &cov)
 
double normalPdf (double value, double mean, double std)
 
double normalCdf (double value, double mean, double std)
 
double sampleNormal ()
 
double sampleUniform ()
 
double inverseNormal (double prob, double mean, double sd)
 
Eigen::Matrix3d getPerspectiveTransform (const std::vector< Eigen::Vector3d > &src, const std::vector< Eigen::Vector3d > &dst)
 
Eigen::Matrix3d getAffineTransform (const std::vector< Eigen::Vector3d > &src, const std::vector< Eigen::Vector3d > &dst)
 

Detailed Description

Simple Moving Average Filter (FIR) Computes the unweighted mean of the previous n samples.

Typedef Documentation

using robot_utils::FilterMovAvgD = typedef FilterMovAvg<double>
using robot_utils::FilterMovAvgF = typedef FilterMovAvg<float>

Define some helper types for filters.

template<typename ValueType , unsigned int Rows, unsigned int Cols>
using robot_utils::FirstOrderFilterEigenMatrixFixed = typedef FirstOrderFilter<Eigen::Matrix<ValueType, Rows, Cols>>
using robot_utils::FirstOrderFilterKindrAngularVelocity = typedef FirstOrderFilter<kindr::LocalAngularVelocityD>
using robot_utils::FirstOrderFilterKindrVector3d = typedef FirstOrderFilter<kindr::VectorTypeless3D>
using robot_utils::PIDGains3D = typedef PIDGains<Eigen::Vector3d>
using robot_utils::PIDGainsD = typedef PIDGains<double>
template<typename T >
using robot_utils::VectorType = typedef typename std::conditional<traits::is_eigen_matrix<T>::value, std::vector<T, Eigen::aligned_allocator<T>>, std::vector<T>>::type

Function Documentation

template<typename Container_ >
Container_ robot_utils::abs ( const Container_ &  c)
inline
template<typename Container_ >
double robot_utils::absPeak ( const Container_ &  c)
inline
template<typename DerivedA , typename DerivedB >
void robot_utils::addAtSubscripts ( Eigen::DenseBase< DerivedA > &  filledObject,
const Eigen::DenseBase< DerivedB > &  includedObject,
const Eigen::VectorXi &  rowSubscripts,
const Eigen::VectorXi &  columnSubscripts 
)
inline
template<typename TypeA , typename DerivedB >
void robot_utils::addTripletsAtSubscripts ( std::vector< Eigen::Triplet< TypeA >> &  tripletList,
const Eigen::DenseBase< DerivedB > &  includedObject,
const Eigen::VectorXi &  rowSubscripts,
const Eigen::VectorXi &  columnSubscripts 
)
inline
void robot_utils::alphaFilter ( Eigen::Vector3d &  vector,
const Eigen::Vector3d &  vectorRef,
double  weight,
double  weightRef 
)
inline
template<typename Container_ >
void robot_utils::applyAbs ( Container_ &  c)
inline
bool robot_utils::areNear ( double  a,
double  b,
double  tol = 1e-6 
)
inline

Check if two values are numerically close.

void robot_utils::boundToRange ( double *  v,
double  min,
double  max 
)
inline
void robot_utils::boundToRange ( int *  v,
int  min,
int  max 
)
inline
double robot_utils::boundToRange ( double  v,
double  min,
double  max 
)
inline
double robot_utils::computeNorm ( double  x,
double  y 
)
inline
double robot_utils::computeNorm ( double  x,
double  y,
double  z 
)
inline
double robot_utils::gaussian ( double  value,
double  std 
)
template<typename T >
T robot_utils::generateRandomNumber ( min = std::numeric_limits<T>::lowest(),
max = std::numeric_limits<T>::max() 
)
Eigen::Matrix3d robot_utils::getAffineTransform ( const std::vector< Eigen::Vector3d > &  src,
const std::vector< Eigen::Vector3d > &  dst 
)

Estimates the affine transformation matrix

Parameters
srclist of three source points src(i)=(x_i, y_i, 1.0), i=0,1,2
dstlist of three destination points dst(i)=(x_i, y_i, 1.0), i=0,1,2
Returns
transformation matrix
kindr::HomTransformQuatD robot_utils::getApproximatedPoseEndToReferenceFromTwistReferenceToStartInStartFrame ( const kindr::HomTransformQuatD &  poseStartToReference,
const kindr::TwistLocalD &  TwistReferenceToStartInStartFrame,
double  timeHorizonInSeconds 
)

Computes the approximated final pose of a frame moving at constant frame-fixed twist.

Computes the approximated final pose of a frame moving at constant frame-fixed twist given its start pose. The solution is based on an approximation valid for small angles of rotation, i.e. given the product of the time horizon and the magnitude of the angular velocity is small. Namely, we use the following approximation:

finalPose = initialPose + initialOrientation * eulerVectorToRotationMatrix(t*angularVelocityInMovingFrame) * linearVelocityInMovingFrame * t;

Note: A frame moving at constant frame-fixed twist follows a helical path. By contrast, if moving at constant reference twist, it would travel along a straight line. However, for large rotations this function predicts a spiral path.

Note: The function getPoseEndToReferenceFromTwistReferenceToStartInStartFrame should be preferred, as it has comparable computational complexity, but provides the exact/analytic solution.

Parameters
poseStartToReferencepose start frame to reference frame
twistReferenceToStartInStartFrametwist (constant) of start frame w.r.t. reference frame in start frame
timeHorizonInSecondsend time minus start time
Returns
pose end frame to reference frame
template<typename ValueType >
constexpr ValueType robot_utils::getDefaultValue ( typename std::enable_if< !std::is_arithmetic< ValueType >::value &&!traits::is_eigen_matrix< ValueType >::value >::type *  = 0)
template<typename ValueType >
constexpr ValueType robot_utils::getDefaultValue ( typename std::enable_if< std::is_arithmetic< ValueType >::value >::type *  = 0)
template<typename ValueType >
constexpr ValueType robot_utils::getDefaultValue ( typename std::enable_if< traits::is_eigen_matrix< ValueType >::value >::type *  = 0)
Eigen::Matrix3d robot_utils::getPerspectiveTransform ( const std::vector< Eigen::Vector3d > &  src,
const std::vector< Eigen::Vector3d > &  dst 
)

Estimates the perspective transformation matrix (homography)

Calculates coefficients of perspective transformation which maps (xi,yi) to (ui,vi), (i=1,2,3,4):

The function calculates the 3 3 matrix of a perspective transform so that:

{bmatrix} t_i x'_i \ t_i y'_i \ t_i {bmatrix} = {map_matrix} {bmatrix} x_i \ y_i \ 1 {bmatrix}

where

dst(i)=(x'_i,y'_i), src(i)=(x_i, y_i), i=0,1,2,3

Parameters
srclist of four source points src(i)=(x_i, y_i, 1.0), i=0,1,2,3
dstlist of four destination points dst(i)=(x_i, y_i, 1.0), i=0,1,2,3
Returns
transformation matrix
double robot_utils::getPhaseDifference ( double  phase1,
double  phase2,
double  tol = 1e-6 
)
inline

Assuming that phase1 happens before phase2, this function will return.

kindr::HomTransformQuatD robot_utils::getPoseEndToReferenceFromTwistReferenceToStartInStartFrame ( const kindr::HomTransformQuatD &  poseStartToReference,
const kindr::TwistLocalD &  twistReferenceToStartInStartFrame,
double  timeHorizonInSeconds 
)

Computes the final pose of a frame moving at constant frame-fixed twist.

Computes the final pose of a frame moving at constant frame-fixed twist given its start pose. The pose is exact in the sense that the analytic solution is used for its computation.

Note: A frame moving at constant frame-fixed twist follows a helical path. By contrast, if moving at constant reference twist, it would travel along a straight line.

Parameters
poseStartToReferencepose start frame to reference frame
twistReferenceToStartInStartFrametwist (constant) of start frame w.r.t. reference frame in start frame
timeHorizonInSecondsend time minus start time
Returns
pose end frame to reference frame
Eigen::VectorXi robot_utils::integerSequence ( const int  low,
const int  high 
)
inline
int robot_utils::intmod ( int  n,
int  b 
)
inline

Map an integer n to the periodic interval (0, b-1).

int robot_utils::intmodRange ( int  n,
int  a,
int  b 
)
inline

Map an integer n to the interval [a, b].

double robot_utils::inverseNormal ( double  prob,
double  mean,
double  sd 
)
bool robot_utils::isEqualOrLargerThan ( double  a,
double  b,
double  tol = 1e-6 
)
inline

True if a is equal or larger than the b.

bool robot_utils::isEqualOrSmallerThan ( double  a,
double  b,
double  tol = 1e-6 
)
inline

True if a is equal or smaller than the b.

bool robot_utils::isLargerThan ( double  a,
double  b,
double  tol = 1e-6 
)
inline

True if a is larger than the b.

bool robot_utils::isSmallerThan ( double  a,
double  b,
double  tol = 1e-6 
)
inline

True if a smaller than the b.

double robot_utils::linearlyInterpolate ( double  v1,
double  v2,
double  t1,
double  t2,
double  t 
)
inline
Eigen::Vector3d robot_utils::linearlyInterpolate ( const Eigen::Vector3d &  v1,
const Eigen::Vector3d &  v2,
double  t1,
double  t2,
double  t 
)
inline
kindr::Position3D robot_utils::linearlyInterpolate ( const kindr::Position3D &  v1,
const kindr::Position3D &  v2,
double  t1,
double  t2,
double  t 
)
inline
template<typename T >
T robot_utils::mapInRange ( x,
in_min,
in_max,
out_min,
out_max 
)
double robot_utils::mapTo01Range ( double  v,
double  min,
double  max 
)
inline
void robot_utils::mapToInterval ( double &  phase,
double  interval_start,
double  tol = 1e-6 
)
inline

map a phase event to the interval [start,start+1.0].

double robot_utils::mapToUniformRange ( double  v,
double  min,
double  max 
)
inline
void robot_utils::mapToUnitInterval ( double &  phase,
double  tol = 1e-6 
)
inline

map a phase event to the interval [0,1).

template<typename Container_ >
double robot_utils::mean ( const Container_ &  c)
inline
double robot_utils::my_gasdev ( int *  idum)
double robot_utils::my_ran0 ( int *  idum)
double robot_utils::normalCdf ( double  value,
double  mean,
double  std 
)
double robot_utils::normalPdf ( const Eigen::VectorXd &  value,
const Eigen::VectorXd &  mean,
const Eigen::MatrixXd &  cov 
)
template<unsigned int N_>
double robot_utils::normalPdf ( const Eigen::Matrix< double, N_, 1 > &  value,
const Eigen::Matrix< double, N_, 1 > &  mean,
const Eigen::Matrix< double, N_, N_ > &  cov 
)
double robot_utils::normalPdf ( double  value,
double  mean,
double  std 
)
Eigen::MatrixXd robot_utils::pseudoInverseAdaptiveDls ( const Eigen::MatrixXd &  A,
double  maxDampingFactor = 0.02,
double  singularRegionDimension = 0.06 
)
template<typename Container_ >
double robot_utils::rms ( const Container_ &  c)
inline
double robot_utils::sampleNormal ( )
double robot_utils::sampleUniform ( )
template<typename DerivedA , typename DerivedB >
void robot_utils::setAtSubscripts ( Eigen::DenseBase< DerivedA > &  filledObject,
const Eigen::DenseBase< DerivedB > &  includedObject,
const Eigen::VectorXi &  rowSubscripts,
const Eigen::VectorXi &  columnSubscripts 
)
inline
template<typename Container_ >
double robot_utils::stdev ( const Container_ &  c)
inline
template<typename Container_ >
double robot_utils::var ( const Container_ &  c)
inline