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.
Vec3	fsb::vector_abs (const Vec3 &vec)
	Element-wise vector absolute value.
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_a	Transform A
transf_b	Transform 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_initial	Initial transform
offset	Offset 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_initial	Initial transform
offset	Offset 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_initial	Initial transform
transf_final	Final 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_initial	Coordinate transform applied after motion offset
transf_final	Final 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_a	Transform A
transf_b	Transform 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

transf	Coordinate transform
pos	Position 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_transf	Transform of parent frame with respect to common frame
parent_velocity	Velocity of parent frame with respect to common frame
parent_acceleration	Acceleration of parent frame with respect to common frame
motion_transf	Transform of child frame with respect to parent frame
motion_velocity	Velocity of child frame with respect to parent frame
motion_acceleration	Acceleration 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_transf	Transform of parent frame with respect to common frame
parent_velocity	Velocity of parent frame with respect to common frame
parent_acceleration	Acceleration of parent frame with respect to common frame
child_position	Position 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

pose	Pose of coordinate frame
body_motion	Motion 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

pose	Pose of coordinate frame
space_motion	Motion 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_transf	Transform of parent frame with respect to common frame
parent_velocity	Velocity of parent frame with respect to common frame
motion_transf	Transform of child frame with respect to parent frame
motion_velocity	Velocity 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_transf	Transform of parent frame with respect to common frame
parent_velocity	Velocity of parent frame with respect to common frame
child_position	Position 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

transf

Input transform

Returns

Inverse of transform

vector_abs()

Vec3 fsb::vector_abs

(

const Vec3 &

vec

)

Element-wise vector absolute value.

\( [|v_x|, |v_y|, |v_z|]^T \)

Parameters

vec	Vector input

Returns

Element-wise absolute value of vector

vector_add()

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

Vector addition.

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

Parameters

v_a	Vector a
v_b	Vector 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_a	Vector a
v_b	Vector 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_a	Vector a
v_b	Vector 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_a	Vector a
v_b	Vector 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

vec	Vector input

Returns

2-norm of vector

vector_scale()

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

Scale vector.

\( s \mathbf{v} \)

Parameters

scalar	Scalar value
vec	Vector 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_a	Vector a
v_b	Vector b

Returns

Vector subtraction of b from a