RobWorkProject
Classes | Typedefs | Functions | Variables

Matrices, vectors, configurations, and more. More...

Classes

class  EAA< T >
 A class for representing an equivalent angle-axis rotation. More...
 
class  Function< RES_T, ARG_T >
 Interface for functions. More...
 
class  Function1Diff< RES_T, ARG_T, GRAD_T >
 Interface for functions which are 1 time differentiable. More...
 
class  InertiaMatrix< T >
 A 3x3 inertia matrix. More...
 
class  Jacobian
 A Jacobian class. A jacobian with m rows and n columns. More...
 
class  LinearAlgebra
 Collection of Linear Algebra functions. More...
 
class  Math
 Utility functions for the rw::math module. More...
 
class  Metric< T >
 Template interface for metrics on type T. More...
 
class  ManhattanMetric< T >
 Manhattan distance metric for vector types. More...
 
class  WeightedManhattanMetric< T >
 Weighted Manhattan distance metric for vector types. More...
 
class  EuclideanMetric< T >
 Euclidean distance metric for vector types. More...
 
class  WeightedEuclideanMetric< T >
 Weighted Euclidean metric for vector types. More...
 
class  InfinityMetric< T >
 Infinity norm distance metric for vector types. More...
 
class  WeightedInfinityMetric< T >
 Weighted infinity norm metric for vector types. More...
 
class  MahalanobisMetric< T >
 Mahalanobis distance metric for vector types. More...
 
class  Rotation3DAngleMetric< T >
 a distance metric over rotations. The distance between two rotations is the smalles angle that rotates the one into the other. More...
 
class  Transform3DAngleMetric< T >
 distance metrics between points in SE3. More...
 
class  MetricFactory
 Metric constructor functions. More...
 
class  MetricUtil
 Various metrics and other distance measures. More...
 
class  Polynomial< T >
 Representation of an ordinary polynomial with scalar coefficients (that can be both real and complex). More...
 
class  PolynomialND< Coef, Scalar >
 Representation of a polynomial that can have non-scalar coefficients (polynomial matrix). More...
 
class  PolynomialSolver
 Find solutions for roots of real and complex polynomial equations. More...
 
class  Pose2D< T >
 A Pose3D $ \mathbf{x}\in \mathbb{R}^6 $ describes a position and orientation in 3-dimensions. More...
 
class  Pose6D< T >
 A Pose6D $ \mathbf{x}\in \mathbb{R}^6 $ describes a position and orientation in 3-dimensions. More...
 
class  Quaternion< T >
 A Quaternion $ \mathbf{q}\in \mathbb{R}^4 $ a complex number used to describe rotations in 3-dimensional space. $ q_w+{\bf i}\ q_x+ {\bf j} q_y+ {\bf k}\ q_z $. More...
 
class  Random
 Generation of random numbers. More...
 
class  Rotation2D< T >
 A 2x2 rotation matrix $ \mathbf{R}\in SO(2) $. More...
 
class  Rotation3D< T >
 A 3x3 rotation matrix $ \mathbf{R}\in SO(3) $. More...
 
class  Rotation3DVector< T >
 An abstract base class for Rotation3D parameterisations. More...
 
class  RPY< T >
 A class for representing Roll-Pitch-Yaw Euler angle rotations. More...
 
class  Transform2D< T >
 A 4x4 homogeneous transform matrix $ \mathbf{T}\in SE(3) $. More...
 
class  Transform3D< T >
 A 4x4 homogeneous transform matrix $ \mathbf{T}\in SE(3) $. More...
 
class  Vector2D< T >
 A 2D vector $ \mathbf{v}\in \mathbb{R}^2 $. More...
 
class  Vector3D< T >
 A 3D vector $ \mathbf{v}\in \mathbb{R}^3 $. More...
 
class  VectorND< N, T >
 A N-Dimensional Vector. More...
 
class  VelocityScrew6D< T >
 Class for representing 6 degrees of freedom velocity screws. More...
 
class  Wrench6D< T >
 Class for representing 6 degrees of freedom wrenches. More...
 
class  FirstDifferences< RES_T, ARG_T, GRAD_T >
 
class  FunctionFactory
 
class  FunctionWithNumericalDerivative< RES_T, ARG_T, GRAD_T >
 
class  FunctionWrapper< RES_T, ARG_T >
 
class  Function1DiffWrapper< RES_T, ARG_T, GRAD_T >
 

Typedefs

typedef Metric< QQMetric
 Metrics on configurations.
 
typedef Metric< Transform3D<> > Transform3DMetric
 Metric on Transdform3D.
 
typedef rw::common::Ptr< FunctionWrapperPtr
 Smart pointer type.
 
typedef rw::common::Ptr< Function1DiffWrapperPtr
 Smart pointer type.
 

Functions

template<class T >
const Vector3D< T > cross (const Vector3D< T > &v, const EAA< T > &eaa)
 Calculates the cross product. More...
 
template<class Q , class T >
const EAA< Qcast (const EAA< T > &eaa)
 Casts EAA<T> to EAA<Q> More...
 
template<class Q >
InertiaMatrix< Qinverse (const InertiaMatrix< Q > &aRb)
 Calculates the inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $ of a rotation matrix. More...
 
template<class Q , class T >
InertiaMatrix< Qcast (const InertiaMatrix< T > &rot)
 Casts InertiaMatrix<T> to InertiaMatrix<Q> More...
 
Polynomial operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > > &a, const PolynomialND< Eigen::Matrix< double, 3, 1 > > &b)
 Multiply 3D polynomial vector with 3D polynomial vector. More...
 
PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Vector3d > &polynomial, const Polynomial<> &p)
 Multiply 3D polynomial vector with a polynomial with scalar coefficients. More...
 
PolynomialND< Eigen::Matrix< double, 1, 3 > > operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > > &polynomial, const Polynomial<> &p)
 Multiply 3D polynomial vector with a polynomial with scalar coefficients. More...
 
PolynomialND< Eigen::Matrix3d > operator* (const PolynomialND< Eigen::Matrix3d > &polynomial, const Polynomial<> &p)
 Multiply 3D polynomial matrix with a polynomial with scalar coefficients. More...
 
Polynomial< float > operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &a, const PolynomialND< Eigen::Matrix< float, 3, 1 >, float > &b)
 Multiply 3D polynomial vector with 3D polynomial vector. More...
 
PolynomialND< Eigen::Vector3f, float > operator* (const PolynomialND< Eigen::Vector3f, float > &polynomial, const Polynomial< float > &p)
 Multiply 3D polynomial vector with a polynomial with scalar coefficients. More...
 
PolynomialND< Eigen::Matrix< float, 1, 3 >, float > operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &polynomial, const Polynomial< float > &p)
 Multiply 3D polynomial vector with a polynomial with scalar coefficients. More...
 
PolynomialND< Eigen::Matrix3f, float > operator* (const PolynomialND< Eigen::Matrix3f, float > &polynomial, const Polynomial< float > &p)
 Multiply 3D polynomial matrix with a polynomial with scalar coefficients. More...
 
PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Matrix3d > &A, const PolynomialND< Eigen::Vector3d > &b)
 Multiply 3D polynomial matrix with 3D polynomial vector. More...
 
PolynomialND< Eigen::Matrix< double, 1, 3 > > operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > > &a, const PolynomialND< Eigen::Matrix3d > &A)
 Multiply 3D polynomial vector with 3D polynomial matrix. More...
 
PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Matrix3d > &A, const Eigen::Vector3d &b)
 Multiply 3D polynomial matrix with 3D polynomial vector. More...
 
PolynomialND< Eigen::Matrix< double, 1, 3 > > operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > > &a, const Eigen::Matrix3d &A)
 Multiply 3D polynomial vector with 3D polynomial matrix. More...
 
PolynomialND< Eigen::Vector3f, float > operator* (const PolynomialND< Eigen::Matrix3f, float > &A, const PolynomialND< Eigen::Vector3f, float > &b)
 Multiply 3D polynomial matrix with 3D polynomial vector. More...
 
PolynomialND< Eigen::Matrix< float, 1, 3 >, float > operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &a, const PolynomialND< Eigen::Matrix3f, float > &A)
 Multiply 3D polynomial vector with 3D polynomial matrix. More...
 
PolynomialND< Eigen::Vector3f, float > operator* (const PolynomialND< Eigen::Matrix3f, float > &A, const Eigen::Vector3f &b)
 Multiply 3D polynomial matrix with 3D polynomial vector. More...
 
PolynomialND< Eigen::Matrix< float, 1, 3 >, float > operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &a, const Eigen::Matrix3f &A)
 Multiply 3D polynomial vector with 3D polynomial matrix. More...
 
template<class Q , class T >
const Pose6D< Qcast (const Pose6D< T > &pose)
 Casts Pose6D<T> to Pose6D<Q> More...
 
template<class Q , class T >
const Quaternion< Qcast (const Quaternion< T > &quaternion)
 Casts Quaternion<T> to Quaternion<Q> More...
 
template<class R , class T >
const Rotation2D< R > cast (const Rotation2D< T > &rot)
 Casts Rotation2D<T> to Rotation2D<Q> More...
 
template<class T >
const Rotation2D< T > transpose (const Rotation2D< T > &aRb)
 Find the transpose of aRb. More...
 
template<class Q , class T >
const RPY< Qcast (const RPY< T > &rpy)
 Casts RPY<T> to RPY<Q> More...
 
template<class Q , class T >
const Transform2D< Qcast (const Transform2D< T > &trans)
 Cast Transform2D<T> to Transform2D<Q> More...
 
template<class Q , class T >
const Transform3D< Qcast (const Transform3D< T > &trans)
 Cast Transform3D<T> to Transform3D<Q> More...
 
template<class T >
cross (const Vector2D< T > &v1, const Vector2D< T > &v2)
 Calculates the 2D vector cross product $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<class T >
double dot (const Vector2D< T > &v1, const Vector2D< T > &v2)
 Calculates the dot product $ \mathbf{v1} . \mathbf{v2} $. More...
 
template<class T >
double angle (const Vector2D< T > &v1, const Vector2D< T > &v2)
 calculates the ounter clock-wise angle from v1 to v2. the value returned will be in the interval [-2Pi,2Pi]
 
template<class T >
const Vector2D< T > normalize (const Vector2D< T > &v)
 Returns the normalized vector $\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} $. More...
 
template<class Q , class T >
const Vector2D< Qcast (const Vector2D< T > &v)
 Casts Vector2D<T> to Vector2D<Q> More...
 
template<class T >
norm1 (const VelocityScrew6D< T > &screw)
 Takes the 1-norm of the velocity screw. All elements both angular and linear are given the same weight. More...
 
template<class T >
norm2 (const VelocityScrew6D< T > &screw)
 Takes the 2-norm of the velocity screw. All elements both angular and linear are given the same weight. More...
 
template<class T >
normInf (const VelocityScrew6D< T > &screw)
 Takes the infinite norm of the velocity screw. All elements both angular and linear are given the same weight. More...
 
template<class Q , class T >
const VelocityScrew6D< Qcast (const VelocityScrew6D< T > &vs)
 Casts VelocityScrew6D<T> to VelocityScrew6D<Q> More...
 
template<class T >
norm1 (const Wrench6D< T > &wrench)
 Takes the 1-norm of the wrench. All elements both force and torque are given the same weight. More...
 
template<class T >
norm2 (const Wrench6D< T > &wrench)
 Takes the 2-norm of the wrench. All elements both force and tporque are given the same weight. More...
 
template<class T >
normInf (const Wrench6D< T > &wrench)
 Takes the infinite norm of the wrench. All elements both force and torque are given the same weight. More...
 
template<class Q , class T >
const Wrench6D< Qcast (const Wrench6D< T > &vs)
 Casts Wrench6D<T> to Wrench6D<Q> More...
 
 FunctionWrapper (typename boost::function< RES_T(ARG_T)> f)
 Constructor. More...
 
virtual ~FunctionWrapper ()
 Destructor.
 
virtual RES_T f (ARG_T q)
 Returns function value for arguments q. More...
 
boost::function< RES_T(ARG_T)> getCallback ()
 Get the wrapped callback.
 
 Function1DiffWrapper (typename boost::function< RES_T(ARG_T)> f, typename boost::function< GRAD_T(ARG_T)> df)
 Constructor. More...
 
virtual ~Function1DiffWrapper ()
 Destructor.
 
virtual GRAD_T df (ARG_T q)
 
boost::function< RES_T(ARG_T)> getGradientCallback ()
 Get the wrapped callback.
 
const VelocityScrew6D operator* (const Jacobian &Jq, const Q &dq)
 Calculates velocity vector. More...
 
const Q operator* (const Jacobian &JqInv, const VelocityScrew6D<> &v)
 Calculates joint velocities. More...
 
const Jacobian operator* (const Jacobian &j1, const Jacobian &j2)
 Multiplies jacobians $ \mathbf{J} = \mathbf{J}_1 * \mathbf{J}_2 $. More...
 
std::ostream & operator<< (std::ostream &out, const Jacobian &v)
 Streaming operator.
 
const Jacobian operator* (const Rotation3D<> &r, const Jacobian &v)
 Rotates each column of v by r. More...
 
template<>
std::pair< typename LinearAlgebra::EigenMatrix< double >::type, typename LinearAlgebra::EigenVector< double >::type > eigenDecompositionSymmetric (const Eigen::MatrixXd &Am1)
 
template<>
std::pair< typename LinearAlgebra::EigenMatrix< std::complex< double > >::type, typename LinearAlgebra::EigenVector< std::complex< double > >::type > eigenDecomposition (const Eigen::MatrixXd &Am1)
 
template<class T >
std::ostream & operator<< (std::ostream &out, const Pose6D< T > &v)
 Streaming operator.
 
template<class T >
std::ostream & operator<< (std::ostream &out, const Quaternion< T > &v)
 Streaming operator.
 
template<class T >
const Rotation2D< T > inverse (const Rotation2D< T > &aRb)
 The inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $ of a rotation matrix. More...
 
template<class Q , class T >
const Rotation3D< Qcast (const Rotation3D< T > &rot)
 Casts Rotation3D<T> to Rotation3D<Q> More...
 
template<class T >
const Rotation3D< T > inverse (const Rotation3D< T > &aRb)
 Calculates the inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $ of a rotation matrix. More...
 
template<class T >
std::ostream & operator<< (std::ostream &os, const Rotation3D< T > &r)
 Writes rotation matrix to stream. More...
 
template<class T >
const Transform2D< T > inverse (const Transform2D< T > &aTb)
 Calculates $ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} $. More...
 
template<class T >
const Transform3D< T > inverse (const Transform3D< T > &aTb)
 Calculates $ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} $. More...
 
template<class T >
bool operator== (const Vector2D< T > &a, const Vector2D< T > &b)
 Compares a and b for equality. More...
 
template<class T >
bool operator!= (const Vector2D< T > &a, const Vector2D< T > &b)
 Compares a and b for inequality. More...
 
template<class T >
const Vector3D< T > cross (const Vector3D< T > &v1, const Vector3D< T > &v2)
 Calculates the 3D vector cross product $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<class T >
void cross (const Vector3D< T > &v1, const Vector3D< T > &v2, Vector3D< T > &dst)
 Calculates the 3D vector cross product $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<class T >
dot (const Vector3D< T > &v1, const Vector3D< T > &v2)
 Calculates the dot product $ \mathbf{v1} . \mathbf{v2} $. More...
 
template<class T >
const Vector3D< T > normalize (const Vector3D< T > &v)
 Returns the normalized vector $\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} $. In case $ \|mathbf{v}\| = 0$ the zero vector is returned. More...
 
template<class T >
double angle (const Vector3D< T > &v1, const Vector3D< T > &v2, const Vector3D< T > &n)
 Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $ with n determining the sign. More...
 
template<class T >
double angle (const Vector3D< T > &v1, const Vector3D< T > &v2)
 Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<class Q , class T >
const Vector3D< Qcast (const Vector3D< T > &v)
 Casts Vector3D<T> to Vector3D<Q> More...
 
template<size_t ND, class T >
const VectorND< ND, T > cross (const VectorND< ND, T > &v1, const VectorND< ND, T > &v2)
 Calculates the 3D VectorND cross product $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<size_t ND, class T >
void cross (const VectorND< ND, T > &v1, const VectorND< ND, T > &v2, VectorND< ND, T > &dst)
 Calculates the 3D VectorND cross product $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<size_t ND, class T >
dot (const VectorND< ND, T > &v1, const VectorND< ND, T > &v2)
 Calculates the dot product $ \mathbf{v1} . \mathbf{v2} $. More...
 
template<size_t ND, class T >
const VectorND< ND, T > normalize (const VectorND< ND, T > &v)
 Returns the normalized VectorND $\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} $. In case $ \|mathbf{v}\| = 0$ the zero VectorND is returned. More...
 
template<size_t ND, class T >
double angle (const VectorND< ND, T > &v1, const VectorND< ND, T > &v2, const VectorND< ND, T > &n)
 Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $ with n determining the sign. More...
 
template<size_t ND, class T >
double angle (const VectorND< ND, T > &v1, const VectorND< ND, T > &v2)
 Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $. More...
 
template<size_t ND, class Q , class T >
const VectorND< ND, Qcast (const VectorND< ND, T > &v)
 Casts VectorND<N,T> to VectorND<Q> More...
 

Variables

const double Pi = 3.1415926535897932384626433832795
 
const double Inch2Meter = 0.0254
 
const double Meter2Inch = 1 / Inch2Meter
 
const double Deg2Rad = Pi / 180
 
const double Rad2Deg = 180 / Pi
 

Detailed Description

Matrices, vectors, configurations, and more.

Function Documentation

◆ angle() [1/4]

double angle ( const VectorND< ND, T > &  v1,
const VectorND< ND, T > &  v2,
const VectorND< ND, T > &  n 
)
related

Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $ with n determining the sign.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
n[in] $ \mathbf{n} $
Returns
the angle

◆ angle() [2/4]

double angle ( const VectorND< ND, T > &  v1,
const VectorND< ND, T > &  v2 
)
related

Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the angle

◆ angle() [3/4]

double angle ( const Vector3D< T > &  v1,
const Vector3D< T > &  v2,
const Vector3D< T > &  n 
)
related

Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $ with n determining the sign.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
n[in] $ \mathbf{n} $
Returns
the angle

◆ angle() [4/4]

double angle ( const Vector3D< T > &  v1,
const Vector3D< T > &  v2 
)
related

Calculates the angle from $ \mathbf{v1}$ to $ \mathbf{v2} $ around the axis defined by $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the angle

◆ cast() [1/14]

const RPY<Q> rw::math::cast ( const RPY< T > &  rpy)

Casts RPY<T> to RPY<Q>

Parameters
rpy[in] RPY with type T
Returns
RPY with type Q

◆ cast() [2/14]

const Pose6D<Q> rw::math::cast ( const Pose6D< T > &  pose)

Casts Pose6D<T> to Pose6D<Q>

Parameters
pose[in] Pose6D with type T
Returns
Pose6D with type Q

◆ cast() [3/14]

const Transform2D<Q> rw::math::cast ( const Transform2D< T > &  trans)

Cast Transform2D<T> to Transform2D<Q>

Parameters
trans[in] Transform2D with type T
Returns
Transform2D with type Q

◆ cast() [4/14]

const EAA<Q> rw::math::cast ( const EAA< T > &  eaa)

Casts EAA<T> to EAA<Q>

Parameters
eaa[in] EAA with type T
Returns
EAA with type Q

◆ cast() [5/14]

const Rotation2D<R> rw::math::cast ( const Rotation2D< T > &  rot)

Casts Rotation2D<T> to Rotation2D<Q>

Parameters
rot[in] Rotation2D with type T
Returns
Rotation2D with type R

◆ cast() [6/14]

InertiaMatrix<Q> rw::math::cast ( const InertiaMatrix< T > &  rot)

Casts InertiaMatrix<T> to InertiaMatrix<Q>

Parameters
rot[in] InertiaMatrix with type T
Returns
InertiaMatrix with type Q

◆ cast() [7/14]

const Vector2D<Q> rw::math::cast ( const Vector2D< T > &  v)

Casts Vector2D<T> to Vector2D<Q>

Parameters
v[in] Vector2D with type T
Returns
Vector2D with type Q

◆ cast() [8/14]

const VectorND< ND, Q > cast ( const VectorND< ND, T > &  v)
related

Casts VectorND<N,T> to VectorND<Q>

Parameters
v[in] VectorND with type T
Returns
VectorND with type Q

◆ cast() [9/14]

const Rotation3D< Q > cast ( const Rotation3D< T > &  rot)
related

Casts Rotation3D<T> to Rotation3D<Q>

Parameters
rot[in] Rotation3D with type T
Returns
Rotation3D with type Q

◆ cast() [10/14]

const Vector3D< Q > cast ( const Vector3D< T > &  v)
related

Casts Vector3D<T> to Vector3D<Q>

Parameters
v[in] Vector3D with type T
Returns
Vector3D with type Q

◆ cast() [11/14]

const Transform3D<Q> rw::math::cast ( const Transform3D< T > &  trans)

Cast Transform3D<T> to Transform3D<Q>

Parameters
trans[in] Transform3D with type T
Returns
Transform3D with type Q

◆ cast() [12/14]

const Wrench6D<Q> rw::math::cast ( const Wrench6D< T > &  vs)

Casts Wrench6D<T> to Wrench6D<Q>

Parameters
vs[in] Wrench6D with type T
Returns
Wrench6D with type Q

◆ cast() [13/14]

const VelocityScrew6D<Q> rw::math::cast ( const VelocityScrew6D< T > &  vs)

Casts VelocityScrew6D<T> to VelocityScrew6D<Q>

Parameters
vs[in] VelocityScrew6D with type T
Returns
VelocityScrew6D with type Q

◆ cast() [14/14]

const Quaternion<Q> rw::math::cast ( const Quaternion< T > &  quaternion)
inline

Casts Quaternion<T> to Quaternion<Q>

Parameters
quaternion[in] Quarternion with type T
Returns
Quaternion with type Q

◆ cross() [1/6]

const Vector3D< T > cross ( const Vector3D< T > &  v,
const EAA< T > &  eaa 
)

Calculates the cross product.

Parameters
v[in] a 3D vector
eaa[in] a 3D eaa vector
Returns
the resulting 3D vector

◆ cross() [2/6]

T rw::math::cross ( const Vector2D< T > &  v1,
const Vector2D< T > &  v2 
)

Calculates the 2D vector cross product $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the cross product $ \mathbf{v1} \times \mathbf{v2} $

The 2D vector cross product is defined as:

$ \mathbf{v1} \times \mathbf{v2} = v1_x * v2_y - v1_y * v2_x $

◆ cross() [3/6]

const VectorND< ND, T > cross ( const VectorND< ND, T > &  v1,
const VectorND< ND, T > &  v2 
)
related

Calculates the 3D VectorND cross product $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the 3D VectorND cross product $ \mathbf{v1} \times \mathbf{v2} $

The 3D VectorND cross product is defined as: $ \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right] $

◆ cross() [4/6]

void cross ( const VectorND< ND, T > &  v1,
const VectorND< ND, T > &  v2,
VectorND< ND, T > &  dst 
)
related

Calculates the 3D VectorND cross product $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
dst[out] the 3D VectorND cross product $ \mathbf{v1} \times \mathbf{v2} $

The 3D VectorND cross product is defined as: $ \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right] $

◆ cross() [5/6]

const Vector3D< T > cross ( const Vector3D< T > &  v1,
const Vector3D< T > &  v2 
)
related

Calculates the 3D vector cross product $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the 3D vector cross product $ \mathbf{v1} \times \mathbf{v2} $

The 3D vector cross product is defined as: $ \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right] $

◆ cross() [6/6]

void cross ( const Vector3D< T > &  v1,
const Vector3D< T > &  v2,
Vector3D< T > &  dst 
)
related

Calculates the 3D vector cross product $ \mathbf{v1} \times \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
dst[out] the 3D vector cross product $ \mathbf{v1} \times \mathbf{v2} $

The 3D vector cross product is defined as: $ \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right] $

◆ df()

virtual GRAD_T df ( ARG_T  q)
inlinevirtual

Redirects gradient evaluation to the wrapped callback.

Implements Function1Diff< RES_T, ARG_T, GRAD_T >.

◆ dot() [1/3]

double rw::math::dot ( const Vector2D< T > &  v1,
const Vector2D< T > &  v2 
)

Calculates the dot product $ \mathbf{v1} . \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the dot product $ \mathbf{v1} . \mathbf{v2} $

◆ dot() [2/3]

T dot ( const VectorND< ND, T > &  v1,
const VectorND< ND, T > &  v2 
)
related

Calculates the dot product $ \mathbf{v1} . \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the dot product $ \mathbf{v1} . \mathbf{v2} $

◆ dot() [3/3]

T dot ( const Vector3D< T > &  v1,
const Vector3D< T > &  v2 
)
related

Calculates the dot product $ \mathbf{v1} . \mathbf{v2} $.

Parameters
v1[in] $ \mathbf{v1} $
v2[in] $ \mathbf{v2} $
Returns
the dot product $ \mathbf{v1} . \mathbf{v2} $

◆ f()

virtual RES_T f ( ARG_T  q)
inlinevirtual

Returns function value for arguments q.

Redirects function call to the wrapped callback.

Implements Function< RES_T, ARG_T >.

◆ Function1DiffWrapper()

Function1DiffWrapper ( typename boost::function< RES_T(ARG_T)>  f,
typename boost::function< GRAD_T(ARG_T)>  df 
)
inline

Constructor.

Parameters
f[in] a callback that returns the function value
df[in] a callback that returns the function derivative

◆ FunctionWrapper()

FunctionWrapper ( typename boost::function< RES_T(ARG_T)>  f)
inline

Constructor.

Parameters
f[in] a callback that returns the function value

◆ inverse() [1/5]

const Transform2D< T > inverse ( const Transform2D< T > &  aTb)
related

Calculates $ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} $.

Parameters
aTb[in] the transform matrix $ \robabx{a}{b}{\mathbf{T}} $
Returns
$ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} $

$ \robabx{a}{b}{\mathbf{T}}^{-1} = \left[ \begin{array}{cc} \robabx{a}{b}{\mathbf{R}}^{T} & - \robabx{a}{b}{\mathbf{R}}^{T} \robabx{a}{b}{\mathbf{d}} \\ \begin{array}{ccc}0 & 0 & 0\end{array} & 1 \end{array} \right] $

◆ inverse() [2/5]

InertiaMatrix<Q> rw::math::inverse ( const InertiaMatrix< Q > &  aRb)

Calculates the inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $ of a rotation matrix.

Parameters
aRb[in] the rotation matrix $ \robabx{a}{b}{\mathbf{R}} $
Returns
the matrix inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $

$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} = \robabx{a}{b}{\mathbf{R}}^T $

◆ inverse() [3/5]

const Rotation2D< T > inverse ( const Rotation2D< T > &  aRb)
related

The inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $ of a rotation matrix.

Parameters
aRb[in] the rotation matrix $ \robabx{a}{b}{\mathbf{R}} $
Returns
the matrix inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $

$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} = \robabx{a}{b}{\mathbf{R}}^T $

◆ inverse() [4/5]

const Rotation3D< T > inverse ( const Rotation3D< T > &  aRb)
related

Calculates the inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $ of a rotation matrix.

See also
Rotation3D::inverse() for a faster version that modifies the existing rotation object instead of allocating a new one.
Parameters
aRb[in] the rotation matrix $ \robabx{a}{b}{\mathbf{R}} $
Returns
the matrix inverse $ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} $

$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} = \robabx{a}{b}{\mathbf{R}}^T $

◆ inverse() [5/5]

const Transform3D< T > inverse ( const Transform3D< T > &  aTb)
related

Calculates $ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} $.

Parameters
aTb[in] the transform matrix $ \robabx{a}{b}{\mathbf{T}} $
Returns
$ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} $

$ \robabx{a}{b}{\mathbf{T}}^{-1} = \left[ \begin{array}{cc} \robabx{a}{b}{\mathbf{R}}^{T} & - \robabx{a}{b}{\mathbf{R}}^{T} \robabx{a}{b}{\mathbf{d}} \\ \begin{array}{ccc}0 & 0 & 0\end{array} & 1 \end{array} \right] $

◆ norm1() [1/2]

T rw::math::norm1 ( const Wrench6D< T > &  wrench)

Takes the 1-norm of the wrench. All elements both force and torque are given the same weight.

Parameters
wrench[in] the wrench
Returns
the 1-norm

◆ norm1() [2/2]

T rw::math::norm1 ( const VelocityScrew6D< T > &  screw)

Takes the 1-norm of the velocity screw. All elements both angular and linear are given the same weight.

Parameters
screw[in] the velocity screw
Returns
the 1-norm

◆ norm2() [1/2]

T rw::math::norm2 ( const Wrench6D< T > &  wrench)

Takes the 2-norm of the wrench. All elements both force and tporque are given the same weight.

Parameters
wrench[in] the wrench
Returns
the 2-norm

◆ norm2() [2/2]

T rw::math::norm2 ( const VelocityScrew6D< T > &  screw)

Takes the 2-norm of the velocity screw. All elements both angular and linear are given the same weight.

Parameters
screw[in] the velocity screw
Returns
the 2-norm

◆ normalize() [1/3]

const Vector2D<T> rw::math::normalize ( const Vector2D< T > &  v)

Returns the normalized vector $\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} $.

If $ \| \mathbf{v} \| = 0$ then the zero vector is returned.

Parameters
v[in] $ \mathbf{v} $ which should be normalized
Returns
the normalized vector $ \mathbf{n} $

◆ normalize() [2/3]

const VectorND< ND, T > normalize ( const VectorND< ND, T > &  v)
related

Returns the normalized VectorND $\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} $. In case $ \|mathbf{v}\| = 0$ the zero VectorND is returned.

Parameters
v[in] $ \mathbf{v} $ which should be normalized
Returns
the normalized VectorND $ \mathbf{n} $

◆ normalize() [3/3]

const Vector3D< T > normalize ( const Vector3D< T > &  v)
related

Returns the normalized vector $\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} $. In case $ \|mathbf{v}\| = 0$ the zero vector is returned.

Parameters
v[in] $ \mathbf{v} $ which should be normalized
Returns
the normalized vector $ \mathbf{n} $

◆ normInf() [1/2]

T rw::math::normInf ( const Wrench6D< T > &  wrench)

Takes the infinite norm of the wrench. All elements both force and torque are given the same weight.

Parameters
wrench[in] the wrench
Returns
the infinite norm

◆ normInf() [2/2]

T rw::math::normInf ( const VelocityScrew6D< T > &  screw)

Takes the infinite norm of the velocity screw. All elements both angular and linear are given the same weight.

Parameters
screw[in] the velocity screw
Returns
the infinite norm

◆ operator!=()

bool operator!= ( const Vector2D< T > &  a,
const Vector2D< T > &  b 
)
related

Compares a and b for inequality.

Parameters
a[in]
b[in]
Returns
True if a and b are different, false otherwise.

◆ operator*() [1/20]

const VelocityScrew6D operator* ( const Jacobian Jq,
const Q dq 
)
related

Calculates velocity vector.

Parameters
Jq[in] the jacobian $ \mathbf{J}_{\mathbf{q}} $
dq[in] the joint velocity vector $ \dot{\mathbf{q}} $
Returns
the velocity vector $ \mathbf{\nu} $

◆ operator*() [2/20]

const Q operator* ( const Jacobian JqInv,
const VelocityScrew6D<> &  v 
)
related

Calculates joint velocities.

Parameters
JqInv[in] the inverse jacobian $ \mathbf{J}_{\mathbf{q}}^{-1} $
v[in] the velocity vector $ \mathbf{\nu} $
Returns
the joint velocity vector $ \dot{\mathbf{q}} $

◆ operator*() [3/20]

const Jacobian operator* ( const Jacobian j1,
const Jacobian j2 
)
related

Multiplies jacobians $ \mathbf{J} = \mathbf{J}_1 * \mathbf{J}_2 $.

Parameters
j1[in] $ \mathbf{J}_1 $
j2[in] $ \mathbf{J}_2 $
Returns
$ \mathbf{J} $

◆ operator*() [4/20]

const Jacobian operator* ( const Rotation3D<> &  r,
const Jacobian v 
)
related

Rotates each column of v by r.

The Jacobian must be of height 6.

◆ operator*() [5/20]

PolynomialND<Eigen::Vector3d> rw::math::operator* ( const PolynomialND< Eigen::Matrix3d > &  A,
const PolynomialND< Eigen::Vector3d > &  b 
)

Multiply 3D polynomial matrix with 3D polynomial vector.

Parameters
A[in] the matrix expression.
b[in] the vector expression.
Returns
a 3D polynomial vector.

◆ operator*() [6/20]

PolynomialND<Eigen::Matrix<double,1,3> > rw::math::operator* ( const PolynomialND< Eigen::Matrix< double, 1, 3 > > &  a,
const PolynomialND< Eigen::Matrix3d > &  A 
)

Multiply 3D polynomial vector with 3D polynomial matrix.

Parameters
a[in] the vector expression.
A[in] the matrix expression.
Returns
a 3D polynomial vector.

◆ operator*() [7/20]

PolynomialND<Eigen::Vector3d> rw::math::operator* ( const PolynomialND< Eigen::Matrix3d > &  A,
const Eigen::Vector3d &  b 
)

Multiply 3D polynomial matrix with 3D polynomial vector.

Parameters
A[in] the matrix expression.
b[in] the vector expression.
Returns
a 3D polynomial vector.

◆ operator*() [8/20]

PolynomialND<Eigen::Matrix<double,1,3> > rw::math::operator* ( const PolynomialND< Eigen::Matrix< double, 1, 3 > > &  a,
const Eigen::Matrix3d &  A 
)

Multiply 3D polynomial vector with 3D polynomial matrix.

Parameters
a[in] the vector expression.
A[in] the matrix expression.
Returns
a 3D polynomial vector.

◆ operator*() [9/20]

PolynomialND<Eigen::Vector3f,float> rw::math::operator* ( const PolynomialND< Eigen::Matrix3f, float > &  A,
const PolynomialND< Eigen::Vector3f, float > &  b 
)

Multiply 3D polynomial matrix with 3D polynomial vector.

Parameters
A[in] the matrix expression.
b[in] the vector expression.
Returns
a 3D polynomial vector.

◆ operator*() [10/20]

PolynomialND<Eigen::Matrix<float,1,3>,float> rw::math::operator* ( const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &  a,
const PolynomialND< Eigen::Matrix3f, float > &  A 
)

Multiply 3D polynomial vector with 3D polynomial matrix.

Parameters
a[in] the vector expression.
A[in] the matrix expression.
Returns
a 3D polynomial vector.

◆ operator*() [11/20]

PolynomialND<Eigen::Vector3f,float> rw::math::operator* ( const PolynomialND< Eigen::Matrix3f, float > &  A,
const Eigen::Vector3f &  b 
)

Multiply 3D polynomial matrix with 3D polynomial vector.

Parameters
A[in] the matrix expression.
b[in] the vector expression.
Returns
a 3D polynomial vector.

◆ operator*() [12/20]

PolynomialND<Eigen::Matrix<float,1,3>,float> rw::math::operator* ( const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &  a,
const Eigen::Matrix3f &  A 
)

Multiply 3D polynomial vector with 3D polynomial matrix.

Parameters
a[in] the vector expression.
A[in] the matrix expression.
Returns
a 3D polynomial vector.

◆ operator*() [13/20]

Polynomial rw::math::operator* ( const PolynomialND< Eigen::Matrix< double, 1, 3 > > &  a,
const PolynomialND< Eigen::Matrix< double, 3, 1 > > &  b 
)

Multiply 3D polynomial vector with 3D polynomial vector.

Parameters
a[in] first polynomial vector (row vector).
b[in] second polynomial vector (column vector).
Returns
a polynomial with scalar coefficients.

◆ operator*() [14/20]

PolynomialND<Eigen::Vector3d> rw::math::operator* ( const PolynomialND< Eigen::Vector3d > &  polynomial,
const Polynomial<> &  p 
)

Multiply 3D polynomial vector with a polynomial with scalar coefficients.

Parameters
polynomial[in] the polynomial vector.
p[in] polynomial with scalar coefficients.
Returns
a 3D polynomial vector.

◆ operator*() [15/20]

PolynomialND<Eigen::Matrix<double,1,3> > rw::math::operator* ( const PolynomialND< Eigen::Matrix< double, 1, 3 > > &  polynomial,
const Polynomial<> &  p 
)

Multiply 3D polynomial vector with a polynomial with scalar coefficients.

Parameters
polynomial[in] the polynomial vector.
p[in] polynomial with scalar coefficients.
Returns
a 3D polynomial vector.

◆ operator*() [16/20]

PolynomialND<Eigen::Matrix3d> rw::math::operator* ( const PolynomialND< Eigen::Matrix3d > &  polynomial,
const Polynomial<> &  p 
)

Multiply 3D polynomial matrix with a polynomial with scalar coefficients.

Parameters
polynomial[in] the polynomial matrix.
p[in] polynomial with scalar coefficients.
Returns
a 3D polynomial matrix.

◆ operator*() [17/20]

Polynomial<float> rw::math::operator* ( const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &  a,
const PolynomialND< Eigen::Matrix< float, 3, 1 >, float > &  b 
)

Multiply 3D polynomial vector with 3D polynomial vector.

Parameters
a[in] first polynomial vector (row vector).
b[in] second polynomial vector (column vector).
Returns
a polynomial with scalar coefficients.

◆ operator*() [18/20]

PolynomialND<Eigen::Vector3f,float> rw::math::operator* ( const PolynomialND< Eigen::Vector3f, float > &  polynomial,
const Polynomial< float > &  p 
)

Multiply 3D polynomial vector with a polynomial with scalar coefficients.

Parameters
polynomial[in] the polynomial vector.
p[in] polynomial with scalar coefficients.
Returns
a 3D polynomial vector.

◆ operator*() [19/20]

PolynomialND<Eigen::Matrix<float,1,3>,float> rw::math::operator* ( const PolynomialND< Eigen::Matrix< float, 1, 3 >, float > &  polynomial,
const Polynomial< float > &  p 
)

Multiply 3D polynomial vector with a polynomial with scalar coefficients.

Parameters
polynomial[in] the polynomial vector.
p[in] polynomial with scalar coefficients.
Returns
a 3D polynomial vector.

◆ operator*() [20/20]

PolynomialND<Eigen::Matrix3f,float> rw::math::operator* ( const PolynomialND< Eigen::Matrix3f, float > &  polynomial,
const Polynomial< float > &  p 
)

Multiply 3D polynomial matrix with a polynomial with scalar coefficients.

Parameters
polynomial[in] the polynomial matrix.
p[in] polynomial with scalar coefficients.
Returns
a 3D polynomial matrix.

◆ operator<<()

std::ostream & operator<< ( std::ostream &  os,
const Rotation3D< T > &  r 
)
related

Writes rotation matrix to stream.

Parameters
os[in/out] output stream to use
r[in] rotation matrix to print
Returns
the updated output stream

◆ operator==()

bool operator== ( const Vector2D< T > &  a,
const Vector2D< T > &  b 
)
related

Compares a and b for equality.

Parameters
a[in]
b[in]
Returns
True if a equals b, false otherwise.

◆ transpose()

const Rotation2D<T> rw::math::transpose ( const Rotation2D< T > &  aRb)

Find the transpose of aRb.

The transpose of a rotation matrix is the same as the inverse.

Variable Documentation

◆ Deg2Rad

const double Deg2Rad = Pi / 180

Convert degrees to radians

◆ Inch2Meter

const double Inch2Meter = 0.0254

Converts inch to meter

◆ Meter2Inch

const double Meter2Inch = 1 / Inch2Meter

Converts meter to inch

◆ Pi

const double Pi = 3.1415926535897932384626433832795

Definition of Pi

◆ Rad2Deg

const double Rad2Deg = 180 / Pi

Converts radians to degrees