FancySafeBot 0.0.1
A safe robotics library
Loading...
Searching...
No Matches
Coordinate Transforms and Motion Vectors

Coordinate transformations and velocity and acceleration propagation. More...

Classes

struct  fsb::Transform
 Coordinate transform. More...
 
struct  fsb::MotionVector
 Motion vector. More...
 
struct  fsb::CartesianPva
 Cartesian pose, velocity and acceleration. More...
 

Functions

Vec3 fsb::vector_add (const Vec3 &v_a, const Vec3 &v_b)
 Vector addition.
 
Vec3 fsb::vector_subtract (const Vec3 &v_a, const Vec3 &v_b)
 Vector subtraction.
 
Vec3 fsb::vector_scale (real_t scalar, const Vec3 &vec)
 Scale vector.
 
Vec3 fsb::vector_multiply_elem (const Vec3 &v_a, const Vec3 &v_b)
 Element-wise vector multiplication.
 
Vec3 fsb::vector_cross (const Vec3 &v_a, const Vec3 &v_b)
 Vector cross product.
 
real_t fsb::vector_dot (const Vec3 &v_a, const Vec3 &v_b)
 Vector dot product.
 
real_t fsb::vector_norm (const Vec3 &vec)
 Vector norm.
 
Transform fsb::transform_identity ()
 Identity transform.
 
Transform fsb::transform_inverse (const Transform &transf)
 Get inverse of transform.
 
Transform fsb::coord_transform (const Transform &transf_a, const Transform &transf_b)
 Apply coordinate transform.
 
Transform fsb::coord_transform_inverse (const Transform &transf_a, const Transform &transf_b)
 Apply coordinate transform inverse.
 
Vec3 fsb::coord_transform_position (const Transform &transf, const Vec3 &pos)
 Apply coordinate transform to position vector.
 
Transform fsb::coord_transform_apply_error (const Transform &transf_initial, const MotionVector &offset)
 Apply motion vector offset to transform in space frame.
 
MotionVector fsb::coord_transform_get_error (const Transform &transf_initial, const Transform &transf_final)
 Get motion vector offset between two coordinate transforms in space frame.
 
Transform fsb::coord_transform_apply_body_offset (const Transform &transf_initial, const MotionVector &offset)
 Apply motion vector offset to transform in mobile frame.
 
MotionVector fsb::coord_transform_get_body_offset (const Transform &transf_initial, const Transform &transf_final)
 Get motion vector offset between two coordinate transforms in mobile frame.
 
MotionVector fsb::motion_transform_velocity (const Transform &parent_transf, const MotionVector &parent_velocity, const Transform &motion_transf, const MotionVector &motion_velocity)
 Get velocity of child frame with respect to a common frame given relative velocity between parent and child frames.
 
MotionVector fsb::motion_transform_acceleration (const Transform &parent_transf, const MotionVector &parent_velocity, const MotionVector &parent_acceleration, const Transform &motion_transf, const MotionVector &motion_velocity, const MotionVector &motion_acceleration)
 Get acceleration of child frame with respect to a common frame given relative motion between parent and child frames.
 
MotionVector fsb::motion_transform_space_to_body (const Transform &pose, const MotionVector &space_motion)
 Space to body-fixed motion.
 
MotionVector fsb::motion_transform_body_to_space (const Transform &pose, const MotionVector &body_motion)
 Body-fixed to space frame motion.
 
MotionVector fsb::motion_transform_velocity_position (const Transform &parent_transf, const MotionVector &parent_velocity, const Vec3 &child_position)
 Get velocity of child frame with respect to a common frame given transform between parent and child frames.
 
MotionVector fsb::motion_transform_acceleration_position (const Transform &parent_transf, const MotionVector &parent_velocity, const MotionVector &parent_acceleration, const Vec3 &child_position)
 Get acceleration of child frame with respect to a common frame given transform between parent and child frames.
 

Detailed Description

Coordinate transformations and velocity and acceleration propagation.

Function Documentation

◆ coord_transform()

Transform fsb::coord_transform ( const Transform transf_a,
const Transform transf_b 
)

Apply coordinate transform.

The resulting orientation quaternion is

\[ q = q_A q_B \]

and the position is

\[ p = p_A + C(q_A) p_B \]

Parameters
transf_aTransform A
transf_bTransform B
Returns
Transform A applied to B

◆ coord_transform_apply_body_offset()

Transform fsb::coord_transform_apply_body_offset ( const Transform transf_initial,
const MotionVector offset 
)

Apply motion vector offset to transform in mobile frame.

Parameters
transf_initialInitial transform
offsetOffset motion vector of final transform with respect to initial transform
Returns
Final transform with mobile frame offset applied

◆ coord_transform_apply_error()

Transform fsb::coord_transform_apply_error ( const Transform transf_initial,
const MotionVector offset 
)

Apply motion vector offset to transform in space frame.

Parameters
transf_initialInitial transform
offsetOffset motion vector in space frame
Returns
Final transform including offset applied in space frame

◆ coord_transform_get_body_offset()

MotionVector fsb::coord_transform_get_body_offset ( const Transform transf_initial,
const Transform transf_final 
)

Get motion vector offset between two coordinate transforms in mobile frame.

Parameters
transf_initialInitial transform
transf_finalFinal transform with mobile frame offset applied
Returns
Offset motion vector of final transform with respect to initial transform

◆ coord_transform_get_error()

MotionVector fsb::coord_transform_get_error ( const Transform transf_initial,
const Transform transf_final 
)

Get motion vector offset between two coordinate transforms in space frame.

Parameters
transf_initialCoordinate transform applied after motion offset
transf_finalFinal transform including offset applied in space frame
Returns
Offset motion vector in space frame

◆ coord_transform_inverse()

Transform fsb::coord_transform_inverse ( const Transform transf_a,
const Transform transf_b 
)

Apply coordinate transform inverse.

The resulting orientation quaternion is

\[ q = q^{-1}_A q_B \]

and the position is

\[ p = C(q^{-1}_A) (p_B - p_A) \]

Parameters
transf_aTransform A
transf_bTransform B
Returns
Transform inverse of A applied to B

◆ coord_transform_position()

Vec3 fsb::coord_transform_position ( const Transform transf,
const Vec3 pos 
)

Apply coordinate transform to position vector.

Parameters
transfCoordinate transform
posPosition vector
Returns
Transformed position vector

◆ motion_transform_acceleration()

MotionVector fsb::motion_transform_acceleration ( const Transform parent_transf,
const MotionVector parent_velocity,
const MotionVector parent_acceleration,
const Transform motion_transf,
const MotionVector motion_velocity,
const MotionVector motion_acceleration 
)

Get acceleration of child frame with respect to a common frame given relative motion between parent and child frames.

Parameters
parent_transfTransform of parent frame with respect to common frame
parent_velocityVelocity of parent frame with respect to common frame
parent_accelerationAcceleration of parent frame with respect to common frame
motion_transfTransform of child frame with respect to parent frame
motion_velocityVelocity of child frame with respect to parent frame
motion_accelerationAcceleration of child frame with respect to parent frame
Returns
Acceleration of child frame with respect to common frame

◆ motion_transform_acceleration_position()

MotionVector fsb::motion_transform_acceleration_position ( const Transform parent_transf,
const MotionVector parent_velocity,
const MotionVector parent_acceleration,
const Vec3 child_position 
)

Get acceleration of child frame with respect to a common frame given transform between parent and child frames.

Parameters
parent_transfTransform of parent frame with respect to common frame
parent_velocityVelocity of parent frame with respect to common frame
parent_accelerationAcceleration of parent frame with respect to common frame
child_positionPosition of child frame with respect to parent frame
Returns
Acceleration of child frame with respect to common frame

◆ motion_transform_body_to_space()

MotionVector fsb::motion_transform_body_to_space ( const Transform pose,
const MotionVector body_motion 
)

Body-fixed to space frame motion.

Parameters
posePose of coordinate frame
body_motionMotion in body-fixed frame
Returns
Motion in space frame

◆ motion_transform_space_to_body()

MotionVector fsb::motion_transform_space_to_body ( const Transform pose,
const MotionVector space_motion 
)

Space to body-fixed motion.

Parameters
posePose of coordinate frame
space_motionMotion in space frame
Returns
Motion in body-fixed frame

◆ motion_transform_velocity()

MotionVector fsb::motion_transform_velocity ( const Transform parent_transf,
const MotionVector parent_velocity,
const Transform motion_transf,
const MotionVector motion_velocity 
)

Get velocity of child frame with respect to a common frame given relative velocity between parent and child frames.

Parameters
parent_transfTransform of parent frame with respect to common frame
parent_velocityVelocity of parent frame with respect to common frame
motion_transfTransform of child frame with respect to parent frame
motion_velocityVelocity of child frame with respect to parent frame
Returns
Velocity of child frame with respect to common frame

◆ motion_transform_velocity_position()

MotionVector fsb::motion_transform_velocity_position ( const Transform parent_transf,
const MotionVector parent_velocity,
const Vec3 child_position 
)

Get velocity of child frame with respect to a common frame given transform between parent and child frames.

Parameters
parent_transfTransform of parent frame with respect to common frame
parent_velocityVelocity of parent frame with respect to common frame
child_positionPosition of child frame with respect to parent frame
Returns
Velocity of child frame with respect to common frame

◆ transform_identity()

Transform fsb::transform_identity ( )

Identity transform.

Rotation is unit quaternion with zero x, y, and z components Zero vector translation

Returns
Identity transform

◆ transform_inverse()

Transform fsb::transform_inverse ( const Transform transf)

Get inverse of transform.

Parameters
transfInput transform
Returns
Inverse of transform

◆ vector_add()

Vec3 fsb::vector_add ( const Vec3 v_a,
const Vec3 v_b 
)

Vector addition.

\( \mathbf{v}_a + \mathbf{v}_b \)

Parameters
v_aVector a
v_bVector b
Returns
Vector addition of vector a and b

◆ vector_cross()

Vec3 fsb::vector_cross ( const Vec3 v_a,
const Vec3 v_b 
)

Vector cross product.

\( \mathbf{v}_a \times \mathbf{v}_b \)

Parameters
v_aVector a
v_bVector b
Returns
Cross product of a and b

◆ vector_dot()

real_t fsb::vector_dot ( const Vec3 v_a,
const Vec3 v_b 
)

Vector dot product.

\( \mathbf{v}_a \cdot \mathbf{v}_b \)

Parameters
v_aVector a
v_bVector b
Returns
Dot product of a and b

◆ vector_multiply_elem()

Vec3 fsb::vector_multiply_elem ( const Vec3 v_a,
const Vec3 v_b 
)

Element-wise vector multiplication.

\( \mathbf{v}_a \cdot \mathbf{v}_b \)

Parameters
v_aVector a
v_bVector b
Returns
Element-wise multiplication of a and b

◆ vector_norm()

real_t fsb::vector_norm ( const Vec3 vec)

Vector norm.

\( \| \mathbf{v} \| = \sqrt{v_x^2 + v_y^2 + v_z^2} \)

Parameters
vecVector input
Returns
2-norm of vector

◆ vector_scale()

Vec3 fsb::vector_scale ( real_t  scalar,
const Vec3 vec 
)

Scale vector.

\( s \mathbf{v} \)

Parameters
scalarScalar value
vecVector to scale
Returns
Scaled Vector

◆ vector_subtract()

Vec3 fsb::vector_subtract ( const Vec3 v_a,
const Vec3 v_b 
)

Vector subtraction.

\( \mathbf{v}_a - \mathbf{v}_b \)

Parameters
v_aVector a
v_bVector b
Returns
Vector subtraction of b from a