#include "robot.h"

Include dependency graph for quaternion.h:

This graph shows which files directly or indirectly include this file:

Classes
class	Quaternion
	Quaternion class definition. More...
Defines
#define	BASE_FRAME 0
#define	BODY_FRAME 1
#define	EPSILON 0.0000001
Functions
short	Integ_quat (Quaternion &dquat_present, Quaternion &dquat_past, Quaternion &quat, const Real dt)
	Trapezoidal quaternion integration.
Real	Integ_Trap_quat_s (const Quaternion &present, Quaternion &past, const Real dt)
	Trapezoidal quaternion scalar part integration.
ReturnMatrix	Integ_Trap_quat_v (const Quaternion &present, Quaternion &past, const Real dt)
	Trapezoidal quaternion vector part integration.
ReturnMatrix	Omega (const Quaternion &q, const Quaternion &q_dot)
	Return angular velocity from a quaternion and it's time derivative.
Quaternion	operator* (const Real c, const Quaternion &rhs)
	Overload * operator, multiplication by a scalar.
Quaternion	operator* (const Quaternion &lhs, const Real c)
	Overload * operator, multiplication by a scalar.
Quaternion	operator/ (const Real c, const Quaternion &rhs)
	Overload / operator, division by a scalar.
Quaternion	operator/ (const Quaternion &lhs, const Real c)
Quaternion	Slerp (const Quaternion &q0, const Quaternion &q1, const Real t)
	Spherical Linear Interpolation.
Quaternion	Slerp_prime (const Quaternion &q0, const Quaternion &q1, const Real t)
	Spherical Linear Interpolation derivative.
Quaternion	Squad (const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
	Spherical Cubic Interpolation.
Quaternion	Squad_prime (const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
	Spherical Cubic Interpolation derivative.
Variables
static const char	header_quat_rcsid [] = "$Id: quaternion.h,v 1.12 2005/11/15 19:25:58 gourdeau Exp $"
	RCS/CVS version.

Detailed Description

Quaternion class.

Definition in file quaternion.h.

Define Documentation

#define BASE_FRAME 0

Definition at line 84 of file quaternion.h.

#define BODY_FRAME 1

Definition at line 85 of file quaternion.h.

#define EPSILON 0.0000001

Definition at line 86 of file quaternion.h.

Function Documentation

short Integ_quat	(	Quaternion &	dquat_present,
		Quaternion &	dquat_past,
		Quaternion &	quat,
		const Real	dt
	)

Trapezoidal quaternion integration.

Definition at line 585 of file quaternion.cpp.

Real Integ_Trap_quat_s	(	const Quaternion &	present,
		Quaternion &	past,
		const Real	dt
	)

Trapezoidal quaternion scalar part integration.

Definition at line 612 of file quaternion.cpp.

ReturnMatrix Integ_Trap_quat_v	(	const Quaternion &	present,
		Quaternion &	past,
		const Real	dt
	)

Trapezoidal quaternion vector part integration.

Definition at line 621 of file quaternion.cpp.

ReturnMatrix Omega	(	const Quaternion &	q,
		const Quaternion &	q_dot
	)

Return angular velocity from a quaternion and it's time derivative.

See Quaternion::dot for explanation.

Definition at line 560 of file quaternion.cpp.

Quaternion operator*	(	const Real	c,
		const Quaternion &	q
	)

Overload * operator, multiplication by a scalar.

$q = [s, v]$ and let $r \in R$ . Then $rq = qr = [r, 0][s, v] = [rs, rv]$

The result is not necessarily a unit quaternion even if $q$ is a unit quaternions.

Definition at line 516 of file quaternion.cpp.

Quaternion operator*	(	const Quaternion &	lhs,
		const Real	c
	)

Overload * operator, multiplication by a scalar.

Definition at line 533 of file quaternion.cpp.

Quaternion operator/	(	const Real	c,
		const Quaternion &	q
	)

Overload / operator, division by a scalar.

Same explanation as multiplication by scaler.

Definition at line 542 of file quaternion.cpp.

Quaternion operator/	(	const Quaternion &	lhs,
		const Real	c
	)

Definition at line 555 of file quaternion.cpp.

Quaternion Slerp	(	const Quaternion &	q0,
		const Quaternion &	q1,
		const Real	t
	)

Spherical Linear Interpolation.

Cite_:Dam

The quaternion $q(t)$ interpolate the quaternions $q_0$ and $q_1$ given the parameter $t$ along the quaternion sphere.

$q(t) = c_0(t)q_0 + c_1(t)q_1$

where $c_0$ and $c_1$ are real functions with $0\leq t \leq 1$ . As $t$ varies between 0 and 1. the values $q(t)$ varies uniformly along the circular arc from $q_0$ and $q_1$ . The angle between $q(t)$ and $q_0$ is $\cos(t\theta)$ and the angle between $q(t)$ and $q_1$ is $\cos((1-t)\theta)$ . Taking the dot product of $q(t)$ and $q_0$ yields

$\cos(t\theta) = c_0(t) + \cos(\theta)c_1(t)$

and taking the dot product of $q(t)$ and $q_1$ yields

$\cos((1-t)\theta) = \cos(\theta)c_0(t) + c_1(t)$

These are two equations with $c_0$ and $c_1$ . The solution is

$c_0 = \frac{\sin((1-t)\theta)}{\sin(\theta)}$

$c_1 = \frac{\sin(t\theta)}{sin(\theta)}$

The interpolation is then

$Slerp(q_0, q_1, t) = \frac{q_0\sin((1-t)\theta)+q_1\sin(t\theta)}{\sin(\theta)}$

If $q_0$ and $q_1$ are unit quaternions the $q(t)$ is also a unit quaternions. For unit quaternions we have

$Slerp(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t$

For t = 0 and t = 1 we have

$q_0 = Slerp(q_0, q_1, 0)$

$q_1 = Slerp(q_0, q_1, 1)$

It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). This choice avoids extra spinning caused by the interpolated rotations.

Definition at line 631 of file quaternion.cpp.

Quaternion Slerp_prime	(	const Quaternion &	q0,
		const Quaternion &	q1,
		const Real	t
	)

Spherical Linear Interpolation derivative.

Cite_: Dam

The derivative of the function $q^t$ where $q$ is a constant unit quaternion is

$\frac{d}{dt}q^t = q^t log(q)$

Using the preceding equation the Slerp derivative is then

$Slerp'(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t log(q_0^{-1}q_1)$

It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). This choice avoids extra spinning caused by the interpolated rotations. The result is not necessary a unit quaternion.

Definition at line 692 of file quaternion.cpp.

Quaternion Squad	(	const Quaternion &	p,
		const Quaternion &	a,
		const Quaternion &	b,
		const Quaternion &	q,
		const Real	t
	)

Spherical Cubic Interpolation.

Cite_: Dam

Let four quaternions be $q_i$ (p), $s_i$ (a), $s_{i+1}$ (b) and $q_{i+1}$ (q) be the ordered vertices of a quadrilateral. Obtain c from $q_i$ to $q_{i+1}$ interpolation. Obtain d from $s_i$ to $s_{i+1}$ interpolation. Obtain e, the final result, from c to d interpolation.

$Squad(q_i, s_i, s_{i+1}, q_{i+1}, t) = Slerp(Slerp(q_i,q_{i+1},t),Slerp(s_i,s_{i+1},t), 2t(1-t))$

The intermediate quaternion $s_i$ and $s_{i+1}$ are given by

$s_i = q_i exp\Big ( - \frac{log(q_i^{-1}q_{i+1}) + log(q_i^{-1}q_{i-1})}{4}\Big )$

Definition at line 725 of file quaternion.cpp.

Quaternion Squad_prime	(	const Quaternion &	p,
		const Quaternion &	a,
		const Quaternion &	b,
		const Quaternion &	q,
		const Real	t
	)

Spherical Cubic Interpolation derivative.

Cite_: www.magic-software.com

The derivative of the function $q^t$ where $q$ is a constant unit quaternion is

$\frac{d}{dt}q^t = q^t log(q)$

Recalling that $log(q) = [0, v\theta]$ (see Quaternion::Log()). If the power is a function we have

$\frac{d}{dt}q^{f(t)} = f'(t)q^{f(t)}log(q)$

If $q$ is a function of time and the power is differentiable function of time we have

$\frac{d}{dt}(q(t))^{f(t)} = f'(t)(q(t))^{f(t)}log(q) + f(t)(q(t))^{f(t)-1}q'(t)$

Using these last three equations Squad derivative can be define. Let $U(t)=Slerp(p,q,t)$ , $V(t)=Slerp(q,b,t)$ , $W(t)=U(t)^{-1}V(t)$ . We then have $Squad(p,a,b,q,t)=Slerp(U(t),V(t),2t(1-t))=U(t)W(t)^{2t(1-t)}$