Stewart definitions. More...

#include <stewart.h>

Public Member Functions
	Stewart ()
	Default Constructor.

	Stewart (const Matrix InitPlat, bool Joint=true)
	Constructor. More...

	Stewart (const Stewart &x)
	Copy Constructor.

	Stewart (const std::string &filename, const std::string &PlatformName)

	~Stewart ()
	Destructor.

const Stewart &	operator= (const Stewart &x)

void	set_Joint (const bool _Joint)
	Set the position of the universal joint on the links.

void	set_q (const ColumnVector _q)
	Set the position of the platform.

void	set_dq (const ColumnVector _dq)
	Set the platform's speed.

void	set_ddq (const ColumnVector _ddq)
	Set the platform's acceleration.

void	set_pR (const ColumnVector _pR)
	Set the position of the center of mass of the platform.

void	set_pIp (const Matrix _pIp)
	Set the inertia matrix of the platform.

void	set_mp (const Real _mp)
	Set the mass of the platform.

bool	get_Joint () const
	Return the position of the universal joint (true if at base, false if at platform)

ReturnMatrix	get_q () const
	Return the position of the platform.

ReturnMatrix	get_dq () const
	Return the speed of the platform.

ReturnMatrix	get_ddq () const
	Return the acceleration of the platform.

ReturnMatrix	get_pR () const
	Return the postion of the center of mass of the platfom.

ReturnMatrix	get_pIp () const
	Return the inertia matrix of the platform.

Real	get_mp () const
	Return the mass of the platform.

void	Transform ()
	Call the functions corresponding to the basic parameters when q changes. More...

ReturnMatrix	Find_wRp ()
	Return the rotation matrix wRp. More...

ReturnMatrix	Find_Omega ()
	Return the angular speed of the platform. More...

ReturnMatrix	Find_Alpha ()
	Return the angular acceleration of the platform. More...

ReturnMatrix	jacobian ()
	Return the jacobian matrix of the platform. More...

ReturnMatrix	Find_InvJacob1 ()
	Return the first intermediate jacobian matrix (reverse) of the platform. More...

ReturnMatrix	Find_InvJacob2 ()
	Return the second intermediate jacobian matrix (reverse) of the platform. More...

ReturnMatrix	jacobian_dot ()
	Return time deriative of the inverse jacobian matrix of the platform. More...

ReturnMatrix	Find_dl ()
	Return the extension rate of the links in a vector. More...

ReturnMatrix	Find_ddl ()
	Return the extension acceleration of the links in a vector. More...

ReturnMatrix	Find_C (const Real Gravity=GRAVITY)
	Return intermediate matrix C for the dynamics calculations. More...

ReturnMatrix	Torque (const Real Gravity=GRAVITY)
	Return the torque vector of the platform. More...

ReturnMatrix	JointSpaceForceVct (const Real Gravity=GRAVITY)
	Return a vector containing the six actuation force components. More...

ReturnMatrix	InvPosKine ()
	Return the lenght of the links in a vector. More...

ReturnMatrix	ForwardKine (const ColumnVector guess_q, const ColumnVector l_given, const Real tolerance=0.001)
	Return the position vector of the platform (vector q) More...

ReturnMatrix	Find_h (const Real Gravity=GRAVITY)
	Return the intermediate matrix corresponding to the Coriolis and centrifugal + gravity force/torque components. More...

ReturnMatrix	Find_M ()
	Return the intermediate matrix corresponding to the inertia matrix of the machine. More...

ReturnMatrix	ForwardDyn (const ColumnVector Torque, const Real Gravity=GRAVITY)
	Return the acceleration vector of the platform (ddq) More...

void	Find_Mc_Nc_Gc (Matrix &Mc, Matrix &Nc, Matrix &Gc)
	Return(!) the intermediates matrix for forward dynamics with actuator dynamics. More...

ReturnMatrix	ForwardDyn_AD (const ColumnVector Command, const Real t)
	Return the acceleration of the platform (Stewart platform mechanism dynamics including actuator dynamics) More...

Public Attributes
Matrix	wRp
	Rotation matrix describing the orientation of the platform.

Matrix	Jacobian
	Jacobian matrix.

Matrix	IJ1
	Inverse of the first intermediate Jacobian matrix.

Matrix	IJ2
	Inverse of the second intermediate Jacobian matrix.

ColumnVector	dl
	Rate of expension vector.

ColumnVector	ddl
	Acceleration of expension vector.

ColumnVector	Alpha
	Angular speed of the platform.

ColumnVector	Omega
	Angular acceleration of the platform.

Private Attributes
bool	UJointAtBase
	Gives the position of the universal joint (true if at base, false if at platform)

ColumnVector	q
	Platform position (xyz + euler angles)

ColumnVector	dq
	Platform speed.

ColumnVector	ddq
	Platform acceleration.

ColumnVector	pR
	Platform center of mass (in its own referential)

ColumnVector	gravity
	Gravity vector.

Matrix	pIp
	Platform Inertia (local ref.)

Real	mp
	Platform mass.

Real	p
	Pitch of the ballscrew (links)

Real	n
	Gear ratio (links motor)

Real	Js
	Moment of inertia (ballscrew)

Real	Jm
	Moment of inertia (motor)

Real	bs
	Viscous damping coefficient of the ballscrew.

Real	bm
	Viscous damping coefficient of the motor.

Real	Kb
	Motor back EMF.

Real	L
	Motor Inductance.

Real	R
	Motor armature resistance.

Real	Kt
	Motor torque.

LinkStewart	Links [6]
	Platform links.

Detailed Description

Stewart definitions.

Definition at line 140 of file stewart.h.

Constructor & Destructor Documentation

Stewart::Stewart	(	const Matrix	InitPlatt,
		bool	Joint = `true`
	)

Constructor.

Parameters

InitPlatt,:	Platform initialization matrix.
Joint,:	bool indicating where is the universal joint

Definition at line 850 of file stewart.cpp.

Member Function Documentation

ReturnMatrix Stewart::Find_Alpha ( )

Return the angular acceleration of the platform.

Eq:

$\alpha = \left( \begin{array}{ccc} 0 & \cos(\phi) & \cos(\theta)\sin(\phi) \\ 0 & \sin(\phi) & -\sin(\theta)\cos(\phi) \\ 1 & 0 & \cos(\theta) \end{array} \right) \left( \begin{array}{c} \ddot{\phi}\\ \ddot{\theta}\\ \ddot{\psi}\end{array} \right) + \left( \begin{array}{ccc} 0 & -\phi\sin(\phi) & \phi\cos(\phi)\sin(\theta) + \dot{\theta}\sin(\phi)\cos(\theta)\\ 0 & \phi\cos(\phi) & \phi\sin(\phi)\sin(\theta) - \dot{\theta}\cos(\phi)\cos(\theta) \\ 0 & 0 & -\dot{theta}\sin(\theta)\end{array} \right) \left( \begin{array}{c} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi}\end{array} \right)$

Where:

$\psi,\theta, \phi$ are the three Euler angles of the platform.
$\dot{\psi},\dot{\theta},\dot{\phi}$ are the three Euler angle speed of the platform.
$\ddot{\psi},\ddot{\theta},\ddot{\phi}$ are the three Euler angle acceleration of the platform.

Definition at line 1258 of file stewart.cpp.

ReturnMatrix Stewart::Find_C ( const Real Gravity = GRAVITY )

Return intermediate matrix C for the dynamics calculations.

Eqs:

$\ddot{x}_g = \ddot{x}+\alpha\times{\bar{r}}+\omega(\omega\times{\bar{r}})$

$\bar{r} = ^{w}R_{p}\cdot{^{p}\bar{r}}$

$\bar{I}_p = ^wR_p^p\bar{I}_p^wR_p^T$

$C = \left( \begin{array}{c} m_pG - m_p\ddot{x}_g-\sum{F_i^n}\\ m_p\bar{r}\times{G}-m_p(\bar{r}\times{\ddot{x}_g}-\bar{I}_p\alpha+\bar{I}_p\omega\times{\omega}-\sum{a_{wi}\times{F_i^n}}-\sum{M_i}\end{array} \right)$

Where:

$\ddot{x}_g$ is the acceleration of the platform center of mass.
$\ddot{x}$ is the acceleration of the platform center (first three elements of the ddq vector).
$\alpha$ is the angular acceleration of the platform.
$\bar{r}$ is the platform center of mass in the world referential.
$\omega$ is the angular speed of the platform.
is the rotational matrix of the two referentials (world and platform).
$^{p}\bar{r}$ is the vector of the center of mass of the platform with reference to the local frame (platform).
$^p\bar{I}_p$ is the constant mass moments of inertia of the platform with reference to the local frame (platform).
is the mass of the platform.
G is the gravity.
is the normal force transferred from the platform to the link.
$\bar{I}_p$ is the constant mass moments of inertia of the platform in the world referential.
$a_{wi}$ is the position of the attachment point of each link to the platform in the world referential.
is the moment transferred from the platform to the link (not present is the spherical joint is at the platform end).

Definition at line 1509 of file stewart.cpp.

References Omega().

ReturnMatrix Stewart::Find_ddl ( )

Return the extension acceleration of the links in a vector.

Eq:

$\ddot{l} = J^{-1}\ddot{q} + \frac{dJ^{-1}}{dt}\dot{q}$

Where:

$J^{-1}$ is the inverse jacobian matrix of the platform
$\ddot{q}$ is the ddq vector

Definition at line 1469 of file stewart.cpp.

ReturnMatrix Stewart::Find_dl ( )

Return the extension rate of the links in a vector.

Eq:

$\dot{l} = J^{-1}\dot{q}$

Where:

$J^{-1}$ is the inverse Jacobian matrix of the platform
$\dot{q}$ is the dq vector

Definition at line 1448 of file stewart.cpp.

ReturnMatrix Stewart::Find_h ( const Real Gravity = GRAVITY )

Return the intermediate matrix corresponding to the Coriolis and centrifugal + gravity force/torque components.

Parameters

Gravity,: Gravity (9.81)

h is found by setting the ddq vector to zero and then calling the torque routine. The vector returned by Torque() is equal to h.

Definition at line 1643 of file stewart.cpp.

ReturnMatrix Stewart::Find_InvJacob1 ( )

Return the first intermediate jacobian matrix (reverse) of the platform.

Eq:

$J_1^{-1} = \left( \begin{array}{cc} n_1^T & (a_{w1}\times{n_1})^T\\ \vdots & \vdots\\ n_6^T & (a_{w6}\times{n_6})^T\end{array} \right)$

Where:

to are the unit vector of the links
$a_{w1}$ to $a_{w6}$ are the attachment point of the links to the platform in the world referential

Definition at line 1313 of file stewart.cpp.

ReturnMatrix Stewart::Find_InvJacob2 ( )

Return the second intermediate jacobian matrix (reverse) of the platform.

Eq:

$J_2^{-1} = \left( \begin{array}{cccccc} 1&0&0&0&0&0\\ 0&1&0&0&0&0\\ 0&0&1&0&0&0\\ 0&0&0&0&\cos\phi&\sin\phi\sin\theta\\ 0&0&0&0&\sin\phi&-\cos\phi\sin\theta\\ 0&0&0&1&0&\cos\theta\end{array} \right)$

Where:

$\phi$ and $\theta$ are two of the euler angle of the platform (vector q)

Definition at line 1341 of file stewart.cpp.

ReturnMatrix Stewart::Find_M ( )

Return the intermediate matrix corresponding to the inertia matrix of the machine.

M is found by setting the dq and Gravity vectors to zero and the ddq vector to zero except for the ith element that is set to one. Then, the ith row of M is equal to the matrix returned by Torque().

Definition at line 1660 of file stewart.cpp.

void Stewart::Find_Mc_Nc_Gc	(	Matrix &	Mc,
		Matrix &	Nc,
		Matrix &	Gc
	)

Return(!) the intermediates matrix for forward dynamics with actuator dynamics.

Parameters

Mc,:	Inertia matrix of the machine
Nc,:	Coriolis and centrifugal force/torque component
Gc,:	Gravity force/torque component

Eq:

$K_a = \frac{p}{2\pi n}I_{6\times{6}}$ $M_a = \frac{2\pi}{np}(J_s+n^2J_m)I_{6\times{6}}$ $V_a = \frac{2\pi}{np}(b_s+n^2b_m)I_{6\times{6}}$ $M_c = K_aJ^TM+M_aJ^{-1}$ $N_c = K_aJ^TN+(V_aJ^{-1}+M_a\frac{dJ^{-1}}{dt})dq$ $G_c = K_aJ^TG$ Where:

p is the pitch of the ballscrew.
n is the gear ratio.
$I_{6\times{6}}$ is the Identity matrix.
is the mass moment of inertia of the ballscrew.
is the mass moment of inertia of the motor.
is the viscous damping coefficient of the ballscrew.
is the viscous damping coefficient of the motor.
J is the Jacobian matrix of the platform.

Definition at line 1733 of file stewart.cpp.

ReturnMatrix Stewart::Find_Omega ( )

Return the angular speed of the platform.

Eq:

$\omega = \left( \begin{array}{ccc} 0 & \cos(\phi) & \cos(\theta)\sin(\phi) \\ 0 & \sin(\phi) & -\sin(\theta)\cos(\phi) \\ 1 & 0 & \cos(\theta) \end{array} \right) \left( \begin{array}{c} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi}\end{array} \right)$

Where:

$\psi,\theta, \phi$ are the three Euler angles of the platform.
$\dot{\psi},\dot{\theta},\dot{\phi}$ are the three Euler angle speed of the platform.

Definition at line 1220 of file stewart.cpp.

ReturnMatrix Stewart::Find_wRp ( )

Return the rotation matrix wRp.

Eq of the matrix:

$wRp = \left( \begin{array}{ccc} \cos(\psi)\cos(\phi)-\cos(\theta)\sin(\phi)\sin(\psi) & -\sin(\psi)\cos(\phi)-\cos(\theta)\sin(\phi)\cos(\psi) & \sin(\theta)\sin(\phi) \\ \cos(\psi)\sin(\phi)+\cos(\theta)\cos(\phi)\sin(\psi) & -\sin(\psi)\sin(\phi)+\cos(\theta)\cos(\phi)\cos(\psi) & -\sin(\theta)\cos(\phi) \\ \sin(\psi)\sin(\theta) & \cos(\psi)\sin(\theta) & \cos(\theta) \end{array} \right)$

Where:

$\psi,\theta, \phi,$ are the three Euler angles of the platform.

Definition at line 1183 of file stewart.cpp.

ReturnMatrix Stewart::ForwardDyn	(	const ColumnVector	T,
		const Real	Gravity = `GRAVITY`
	)

Return the acceleration vector of the platform (ddq)

Parameters

T,:	torque vector
Gravity,:	Gravity (9.81)

Eq:

$ddq = M^{-1}(\tau-h)$

Where:

M is from Find_M() routine.
$\tau$ is the torque vector.
h is from Find_h() routine.

Definition at line 1698 of file stewart.cpp.

ReturnMatrix Stewart::ForwardDyn_AD	(	const ColumnVector	Command,
		const Real	t
	)

Return the acceleration of the platform (Stewart platform mechanism dynamics including actuator dynamics)

Parameters

Command,:	Vector of the 6 motors voltages.
t,:	period of time use to find the currents (di/dt)

Voltages with back emf:

$V' = V-J^{-1}\dot{q}(\frac{2\pi}{p})K_b$

Currents:

$I = \frac{I_{6\times{6}}}{L}e^{(-R\cdot{t}/L)}V'$

Motor torque:

$\tau_m = IK_t$

Platform acceleration:

$\ddot{q} = M_c^{-1}(\tau_m - Nc - Gc)$

Where:

J is the Jacobian matrix of the platform.
$\dot{q}$ is the dq vector.
p is the pitch of the ballscrew.
is the motor back emf constant.
L is the motor armature inductance.
R is the motor armature resistance.
is the motor torque constant.
, and are from Find_Mc_Nc_Gc().

Definition at line 1789 of file stewart.cpp.

ReturnMatrix Stewart::ForwardKine	(	const ColumnVector	guess_q,
		const ColumnVector	l_given,
		const Real	tolerance = `0.001`
	)

Return the position vector of the platform (vector q)

Parameters

guess_q,:	Approximation of real position
l_given,:	Lenght of the 6 links
tolerance,:	Ending criterion

The Newton-Raphson method is used to solve the forward kinematic problem. It is a numerical iterative technic that simplify the solution. An approximation of the answer has to be guess for this method to work.

Eq:

$q_i = q_{i-1}-J_{q_{i-1}}(l_{q_{i-1}}-l)$

Where:

is the position vector of the platform at the ith iteration.
$q_{i-1}$ is the position vector of the platform at the (i-1)th iteration.
$J_{q_{i-1}}$ is the Jacobian matrix of the platform at the position of the $q_{i-1}$ vector.
$l_{q_{i-1}}$ is the lenght vector of the links at the (i-1)th position of the platform.
l is the real lenght vector of the links.

Definition at line 1564 of file stewart.cpp.

ReturnMatrix Stewart::InvPosKine ( )

Return the lenght of the links in a vector.

The goal of the inverse kinematic is to find the lenght of each of the six links from the position of the platform (X,Y,Z, $\psi$ , $\theta$ , $\phi$ ).

Definition at line 1426 of file stewart.cpp.

ReturnMatrix Stewart::jacobian ( )

Return the jacobian matrix of the platform.

Eq:

$J = J_1^{-1}J_2^{-1}$

Where:

and are intermediate matrix(Find_InvJacob1(), Find_InvJacob2())

Definition at line 1287 of file stewart.cpp.

ReturnMatrix Stewart::jacobian_dot ( )

Return time deriative of the inverse jacobian matrix of the platform.

Eq:

$\frac{dJ^{-1}}{dt} = \frac{dJ_1^{-1}}{dt}J_2^{-1}+J_1^{-1}\frac{dJ_2^{-1}}{dt}$

$\frac{dJ_1^{-1}}{dt} = \left( \begin{array}{cc} (\omega_1\times{n_1})^T & ((\omega\times{a_{w1}})\times{n_1}+a_{w1}\times{(\omega_1\times{n_1})})^T\\ \vdots & \vdots\\ (\omega_6\times{n_6})^T & ((\omega\times{a_{w6}})\times{n_6}+a_{w6}\times{(\omega_6\times{n_6})})^T\end{array} \right)$

$\frac{dJ_2^{-1}}{dt} = \left( \begin{array}{cccccc} 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & -\dot{\phi}\sin\phi & \dot{\phi}\cos\phi\sin\theta + \dot{\theta}\sin\phi\cos\theta\\ 0 & 0 & 0 & 0 & \dot{\phi}\cos\phi & \dot{\phi}\sin\phi\sin\theta + \dot{\theta}\cos\phi\cos\theta\\ 0 & 0 & 0 & 0 & 0 &-\dot{\theta}\sin\theta\end{array} \right)$

Where:

$\omega_i$ is the angular speed vector of each link
n is the unit vector of the link
$\omega$ is the angular speed vector of the platform
$a_{wi}$ is the position vector of the attachment point of the link to the platform
$\phi$ and $\theta$ are two of the Euler angle (vector q)
$\dot{\phi}$ and $\dot{\theta}$ are two of the Euler angle speed (vector dq)