spatial Namespace Reference

Enumerations
enum	JointType { JointType::Prismatic, JointType::Revolute, JointType::FloatingBase, JointType::Nothing }

Functions
template<typename T >
SXform< T >	spatialRotation (CoordinateAxis axis, T theta)
template<typename T >
auto	motionCrossMatrix (const Eigen::MatrixBase< T > &v)
template<typename T >
auto	forceCrossMatrix (const Eigen::MatrixBase< T > &v)
template<typename T >
auto	motionCrossProduct (const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b)
template<typename T >
auto	forceCrossProduct (const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b)
template<typename T >
auto	sxformToHomogeneous (const Eigen::MatrixBase< T > &X)
template<typename T >
auto	homogeneousToSXform (const Eigen::MatrixBase< T > &H)
template<typename T , typename T2 >
auto	createSXform (const Eigen::MatrixBase< T > &R, const Eigen::MatrixBase< T2 > &r)
template<typename T >
auto	rotationFromSXform (const Eigen::MatrixBase< T > &X)
template<typename T >
auto	translationFromSXform (const Eigen::MatrixBase< T > &X)
template<typename T >
auto	invertSXform (const Eigen::MatrixBase< T > &X)
template<typename T >
SVec< T >	jointMotionSubspace (JointType joint, CoordinateAxis axis)
template<typename T >
Mat6< T >	jointXform (JointType joint, CoordinateAxis axis, T q)
template<typename T >
Mat3< typename T::Scalar >	rotInertiaOfBox (typename T::Scalar mass, const Eigen::MatrixBase< T > &dims)
template<typename T , typename T2 >
auto	spatialToLinearVelocity (const Eigen::MatrixBase< T > &v, const Eigen::MatrixBase< T2 > &x)
template<typename T >
auto	spatialToAngularVelocity (const Eigen::MatrixBase< T > &v)
template<typename T , typename T2 >
auto	spatialToLinearAcceleration (const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T2 > &v)
template<typename T , typename T2 , typename T3 >
auto	spatialToLinearAcceleration (const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T2 > &v, const Eigen::MatrixBase< T3 > &x)
template<typename T , typename T2 >
auto	sXFormPoint (const Eigen::MatrixBase< T > &X, const Eigen::MatrixBase< T2 > &p)
template<typename T , typename T2 >
auto	forceToSpatialForce (const Eigen::MatrixBase< T > &f, const Eigen::MatrixBase< T2 > &p)

Enumeration Type Documentation

enum spatial::JointType

strong

Enumerator
Prismatic
Revolute
FloatingBase
Nothing

Definition at line 21 of file spatial.h.

{ Prismatic, Revolute, FloatingBase, Nothing };

Function Documentation

template<typename T , typename T2 >

auto spatial::createSXform	(	const Eigen::MatrixBase< T > &	R,
		const Eigen::MatrixBase< T2 > &	r
	)

Create spatial coordinate transformation from rotation and translation

Definition at line 140 of file spatial.h.

References ori::vectorToSkewMat(), and ori::X.

                                                {
  static_assert(T::ColsAtCompileTime == 3 && T::RowsAtCompileTime == 3,
                "Must have 3x3 matrix");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 matrix");
  Mat6<typename T::Scalar> X = Mat6<typename T::Scalar>::Zero();
  X.template topLeftCorner<3, 3>() = R;
  X.template bottomRightCorner<3, 3>() = R;
  X.template bottomLeftCorner<3, 3>() = -R * vectorToSkewMat(r);
  return X;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::forceCrossMatrix

(

const Eigen::MatrixBase< T > &

v

)

Compute spatial force cross product matrix. Prefer forceCrossProduct when possible

Definition at line 60 of file spatial.h.

References f().

                                                   {
  Mat6<typename T::Scalar> f;
  f << 0, -v(2), v(1), 0, -v(5), v(4), v(2), 0, -v(0), v(5), 0, -v(3), -v(1),
      v(0), 0, -v(4), v(3), 0, 0, 0, 0, 0, -v(2), v(1), 0, 0, 0, v(2), 0, -v(0),
      0, 0, 0, -v(1), v(0), 0;
  return f;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::forceCrossProduct	(	const Eigen::MatrixBase< T > &	a,
		const Eigen::MatrixBase< T > &	b
	)

Compute spatial force cross product. Faster than the matrix multiplication version

Definition at line 91 of file spatial.h.

                                                    {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  SVec<typename T::Scalar> mv;
  mv << b(2) * a(1) - b(1) * a(2) - b(4) * a(5) + b(5) * a(4),
      b(0) * a(2) - b(2) * a(0) + b(3) * a(5) - b(5) * a(3),
      b(1) * a(0) - b(0) * a(1) - b(3) * a(4) + b(4) * a(3),
      b(5) * a(1) - b(4) * a(2), b(3) * a(2) - b(5) * a(0),
      b(4) * a(0) - b(3) * a(1);
  return mv;
}

Here is the caller graph for this function:

template<typename T , typename T2 >

auto spatial::forceToSpatialForce	(	const Eigen::MatrixBase< T > &	f,
		const Eigen::MatrixBase< T2 > &	p
	)

Convert a force at a point to a spatial force

Parameters

f	: force
p	: point

Definition at line 348 of file spatial.h.

                                                       {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  SVec<typename T::Scalar> fs;
  fs.template topLeftCorner<3, 1>() = p.cross(f);
  fs.template bottomLeftCorner<3, 1>() = f;
  return fs;
}

Here is the caller graph for this function:

template<typename T >

auto spatial::homogeneousToSXform

(

const Eigen::MatrixBase< T > &

H

)

Convert a homogeneous coordinate transformation to a spatial one

Definition at line 124 of file spatial.h.

References ori::vectorToSkewMat(), and ori::X.

                                                      {
  static_assert(T::ColsAtCompileTime == 4 && T::RowsAtCompileTime == 4,
                "Must have 4x4 matrix");
  Mat3<typename T::Scalar> R = H.template topLeftCorner<3, 3>();
  Vec3<typename T::Scalar> translate = H.template topRightCorner<3, 1>();
  Mat6<typename T::Scalar> X = Mat6<typename T::Scalar>::Zero();
  X.template topLeftCorner<3, 3>() = R;
  X.template bottomLeftCorner<3, 3>() = vectorToSkewMat(translate) * R;
  X.template bottomRightCorner<3, 3>() = R;
  return X;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::invertSXform

(

const Eigen::MatrixBase< T > &

X

)

Invert a spatial transformation (much faster than matrix inverse)

Definition at line 181 of file spatial.h.

References createSXform(), ori::matToSkewVec(), and rotationFromSXform().

                                               {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  RotMat<typename T::Scalar> R = rotationFromSXform(X);
  Vec3<typename T::Scalar> r =
      -matToSkewVec(R.transpose() * X.template bottomLeftCorner<3, 3>());
  SXform<typename T::Scalar> Xinv = createSXform(R.transpose(), -R * r);
  return Xinv;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

SVec<T> spatial::jointMotionSubspace	(	JointType	joint,
		CoordinateAxis	axis
	)

Compute joint motion subspace vector

Definition at line 195 of file spatial.h.

References Prismatic, and Revolute.

                                                                  {
  Vec3<T> v(0, 0, 0);
  SVec<T> phi = SVec<T>::Zero();
  if (axis == CoordinateAxis::X)
    v(0) = 1;
  else if (axis == CoordinateAxis::Y)
    v(1) = 1;
  else
    v(2) = 1;

  if (joint == JointType::Prismatic)
    phi.template bottomLeftCorner<3, 1>() = v;
  else if (joint == JointType::Revolute)
    phi.template topLeftCorner<3, 1>() = v;
  else
    throw std::runtime_error("Unknown motion subspace");

  return phi;
}

template<typename T >

Mat6<T> spatial::jointXform	(	JointType	joint,
		CoordinateAxis	axis,
		T	q
	)

Compute joint transformation

Definition at line 219 of file spatial.h.

References createSXform(), Prismatic, Revolute, spatialRotation(), and ori::X.

                                                              {
  Mat6<T> X = Mat6<T>::Zero();
  if (joint == JointType::Revolute) {
    X = spatialRotation(axis, q);
  } else if (joint == JointType::Prismatic) {
    Vec3<T> v(0, 0, 0);
    if (axis == CoordinateAxis::X)
      v(0) = q;
    else if (axis == CoordinateAxis::Y)
      v(1) = q;
    else if (axis == CoordinateAxis::Z)
      v(2) = q;

    X = createSXform(RotMat<T>::Identity(), v);
  } else {
    throw std::runtime_error("Unknown joint xform\n");
  }
  return X;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::motionCrossMatrix

(

const Eigen::MatrixBase< T > &

v

)

Compute the spatial motion cross product matrix. Prefer motionCrossProduct when possible.

Definition at line 43 of file spatial.h.

                                                    {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  Mat6<typename T::Scalar> m;
  m << 0, -v(2), v(1), 0, 0, 0, v(2), 0, -v(0), 0, 0, 0, -v(1), v(0), 0, 0, 0,
      0,

      0, -v(5), v(4), 0, -v(2), v(1), v(5), 0, -v(3), v(2), 0, -v(0), -v(4),
      v(3), 0, -v(1), v(0), 0;
  return m;
}

Here is the caller graph for this function:

template<typename T >

auto spatial::motionCrossProduct	(	const Eigen::MatrixBase< T > &	a,
		const Eigen::MatrixBase< T > &	b
	)

Compute spatial motion cross product. Faster than the matrix multiplication version

Definition at line 73 of file spatial.h.

                                                     {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  SVec<typename T::Scalar> mv;
  mv << a(1) * b(2) - a(2) * b(1), a(2) * b(0) - a(0) * b(2),
      a(0) * b(1) - a(1) * b(0),
      a(1) * b(5) - a(2) * b(4) + a(4) * b(2) - a(5) * b(1),
      a(2) * b(3) - a(0) * b(5) - a(3) * b(2) + a(5) * b(0),
      a(0) * b(4) - a(1) * b(3) + a(3) * b(1) - a(4) * b(0);
  return mv;
}

Here is the caller graph for this function:

template<typename T >

auto spatial::rotationFromSXform

(

const Eigen::MatrixBase< T > &

X

)

Get rotation matrix from spatial transformation

Definition at line 157 of file spatial.h.

                                                     {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  RotMat<typename T::Scalar> R = X.template topLeftCorner<3, 3>();
  return R;
}

Here is the caller graph for this function:

template<typename T >

Mat3<typename T::Scalar> spatial::rotInertiaOfBox	(	typename T::Scalar	mass,
		const Eigen::MatrixBase< T > &	dims
	)

Construct the rotational inertia of a uniform density box with a given mass.

Parameters

mass	Mass of the box
dims	Dimensions of the box

Definition at line 245 of file spatial.h.

                                                                         {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  Mat3<typename T::Scalar> I =
      Mat3<typename T::Scalar>::Identity() * dims.norm() * dims.norm();
  for (int i = 0; i < 3; i++) I(i, i) -= dims(i) * dims(i);
  I = I * mass / 12;
  return I;
}

Here is the caller graph for this function:

template<typename T >

SXform<T> spatial::spatialRotation	(	CoordinateAxis	axis,
		T	theta
	)

Calculate the spatial coordinate transform from A to B where B is rotate by theta about axis.

Definition at line 28 of file spatial.h.

References ori::coordinateRotation(), and ori::X.

                                                        {
  static_assert(std::is_floating_point<T>::value,
                "must use floating point value");
  RotMat<T> R = coordinateRotation(axis, theta);
  SXform<T> X = SXform<T>::Zero();
  X.template topLeftCorner<3, 3>() = R;
  X.template bottomRightCorner<3, 3>() = R;
  return X;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::spatialToAngularVelocity

(

const Eigen::MatrixBase< T > &

v

)

Convert from spatial velocity to angular velocity.

Definition at line 277 of file spatial.h.

                                                           {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  Vec3<typename T::Scalar> vsAng = v.template topLeftCorner<3, 1>();
  return vsAng;
}

template<typename T , typename T2 >

auto spatial::spatialToLinearAcceleration	(	const Eigen::MatrixBase< T > &	a,
		const Eigen::MatrixBase< T2 > &	v
	)

Compute the classical lienear accleeration of a frame given its spatial acceleration and velocity

Definition at line 289 of file spatial.h.

                                                               {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 6,
                "Must have 6x1 vector");

  Vec3<typename T::Scalar> acc;
  // classical accleration = spatial linear acc + omega x v
  acc = a.template tail<3>() + v.template head<3>().cross(v.template tail<3>());
  return acc;
}

Here is the caller graph for this function:

template<typename T , typename T2 , typename T3 >

auto spatial::spatialToLinearAcceleration	(	const Eigen::MatrixBase< T > &	a,
		const Eigen::MatrixBase< T2 > &	v,
		const Eigen::MatrixBase< T3 > &	x
	)

Compute the classical lienear acceleration of a frame given its spatial acceleration and velocity

Definition at line 307 of file spatial.h.

References spatialToLinearVelocity().

                                                               {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T3::ColsAtCompileTime == 1 && T3::RowsAtCompileTime == 3,
                "Must have 3x1 vector");

  Vec3<typename T::Scalar> alin_x = spatialToLinearVelocity(a, x);
  Vec3<typename T::Scalar> vlin_x = spatialToLinearVelocity(v, x);

  // classical accleration = spatial linear acc + omega x v
  Vec3<typename T::Scalar> acc = alin_x + v.template head<3>().cross(vlin_x);
  return acc;
}

Here is the call graph for this function:

template<typename T , typename T2 >

auto spatial::spatialToLinearVelocity	(	const Eigen::MatrixBase< T > &	v,
		const Eigen::MatrixBase< T2 > &	x
	)

Convert from spatial velocity to linear velocity. Uses spatial velocity at the given point.

Definition at line 261 of file spatial.h.

                                                           {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  Vec3<typename T::Scalar> vsAng = v.template topLeftCorner<3, 1>();
  Vec3<typename T::Scalar> vsLin = v.template bottomLeftCorner<3, 1>();
  Vec3<typename T::Scalar> vLinear = vsLin + vsAng.cross(x);
  return vLinear;
}

Here is the caller graph for this function:

template<typename T , typename T2 >

auto spatial::sXFormPoint	(	const Eigen::MatrixBase< T > &	X,
		const Eigen::MatrixBase< T2 > &	p
	)

Apply spatial transformation to a point.

Definition at line 329 of file spatial.h.

References rotationFromSXform(), and translationFromSXform().

                                               {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 vector");

  Mat3<typename T::Scalar> R = rotationFromSXform(X);
  Vec3<typename T::Scalar> r = translationFromSXform(X);
  Vec3<typename T::Scalar> Xp = R * (p - r);
  return Xp;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::sxformToHomogeneous

(

const Eigen::MatrixBase< T > &

X

)

Convert a spatial transform to a homogeneous coordinate transformation

Definition at line 108 of file spatial.h.

References ori::matToSkewVec().

                                                      {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  Mat4<typename T::Scalar> H = Mat4<typename T::Scalar>::Zero();
  RotMat<typename T::Scalar> R = X.template topLeftCorner<3, 3>();
  Mat3<typename T::Scalar> skewR = X.template bottomLeftCorner<3, 3>();
  H.template topLeftCorner<3, 3>() = R;
  H.template topRightCorner<3, 1>() = matToSkewVec(skewR * R.transpose());
  H(3, 3) = 1;
  return H;
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T >

auto spatial::translationFromSXform

(

const Eigen::MatrixBase< T > &

X

)

Get translation vector from spatial transformation

Definition at line 168 of file spatial.h.

References ori::matToSkewVec(), and rotationFromSXform().

                                                        {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  RotMat<typename T::Scalar> R = rotationFromSXform(X);
  Vec3<typename T::Scalar> r =
      -matToSkewVec(R.transpose() * X.template bottomLeftCorner<3, 3>());
  return r;
}

Here is the call graph for this function:

Here is the caller graph for this function: