ROBOOP, A Robotics Object Oriented Package in C++: Stewart Class Reference

Definition at line 143 of file stewart.h.


Public Member Functions
	Stewart ()
	Default Constructor.
	Stewart (const Matrix InitPlat, bool Joint=true)
	Constructor.
	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.
ReturnMatrix	Find_wRp ()
	Return the rotation matrix wRp.
ReturnMatrix	Find_Omega ()
	Return the angular speed of the platform.
ReturnMatrix	Find_Alpha ()
	Return the angular acceleration of the platform.
ReturnMatrix	jacobian ()
	Return the jacobian matrix of the platform.
ReturnMatrix	Find_InvJacob1 ()
	Return the first intermediate jacobian matrix (reverse) of the platform.
ReturnMatrix	Find_InvJacob2 ()
	Return the second intermediate jacobian matrix (reverse) of the platform.
ReturnMatrix	jacobian_dot ()
	Return time deriative of the inverse jacobian matrix of the platform.
ReturnMatrix	Find_dl ()
	Return the extension rate of the links in a vector.
ReturnMatrix	Find_ddl ()
	Return the extension acceleration of the links in a vector.
ReturnMatrix	Find_C (const Real Gravity=GRAVITY)
	Return intermediate matrix C for the dynamics calculations.
ReturnMatrix	Torque (const Real Gravity=GRAVITY)
	Return the torque vector of the platform.
ReturnMatrix	JointSpaceForceVct (const Real Gravity=GRAVITY)
	Return a vector containing the six actuation force components.
ReturnMatrix	InvPosKine ()
	Return the lenght of the links in a vector.
ReturnMatrix	ForwardKine (const ColumnVector guess_q, const ColumnVector l_given, const Real tolerance=0.001)
	Return the position vector of the platform (vector q).
ReturnMatrix	Find_h (const Real Gravity=GRAVITY)
	Return the intermediate matrix corresponding to the Coriolis and centrifugal + gravity force/torque components.
ReturnMatrix	Find_M ()
	Return the intermediate matrix corresponding to the inertia matrix of the machine.
ReturnMatrix	ForwardDyn (const ColumnVector Torque, const Real Gravity=GRAVITY)
	Return the acceleration vector of the platform (ddq).
void	Find_Mc_Nc_Gc (Matrix &Mc, Matrix &Nc, Matrix &Gc)
	Return(!) the intermediates matrix for forward dynamics with actuator dynamics.
ReturnMatrix	ForwardDyn_AD (const ColumnVector Command, const Real t)
	Return the acceleration of the platform (Stewart platform mechanism dynamics including actuator dynamics).
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.

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 853 of file stewart.cpp.

References bm, bs, ddq, dq, Find_wRp(), gravity, Jm, Js, Kb, Kt, L, Links, M_PI, mp, n, p, pIp, pR, q, R, UJointAtBase, and wRp.

Member Function Documentation

const Stewart & Stewart::operator= ( const Stewart & x )

Definition at line 1004 of file stewart.cpp.

References bm, bs, ddq, dq, gravity, Jm, Js, Kb, Kt, L, Links, mp, n, p, pIp, pR, q, R, and UJointAtBase.

void Stewart::Transform ( )

Call the functions corresponding to the basic parameters when q changes.

These functions are called by Transform:

Definition at line 1160 of file stewart.cpp.

References Find_InvJacob1(), Find_InvJacob2(), Find_wRp(), IJ1, IJ2, jacobian(), Jacobian, Links, q, and wRp.

Referenced by set_q().

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 1186 of file stewart.cpp.

References q.

Referenced by Stewart(), and Transform().

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 1223 of file stewart.cpp.

References dq, and q.

Referenced by set_ddq(), and set_dq().

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 1261 of file stewart.cpp.

References ddq, dq, and q.

Referenced by set_ddq().

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 1290 of file stewart.cpp.

References IJ1, and IJ2.

Referenced by Transform().

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 1316 of file stewart.cpp.

References Links, LinkStewart::UnitV, and wRp.

Referenced by JointSpaceForceVct(), and Transform().

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 1344 of file stewart.cpp.

References q.

Referenced by Transform().

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)

Definition at line 1389 of file stewart.cpp.

References dl, dq, IJ1, IJ2, L, LinkStewart::L, Links, Omega, q, and LinkStewart::UnitV.

Referenced by Find_ddl(), and Find_Mc_Nc_Gc().

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 1451 of file stewart.cpp.

References dq, and Jacobian.

Referenced by set_dq().

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 1472 of file stewart.cpp.

References ddq, dq, Jacobian, and jacobian_dot().

Referenced by set_ddq(), and set_dq().

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 1512 of file stewart.cpp.

References Alpha, ddq, gravity, Links, LinkStewart::Moment(), mp, Omega, pIp, pR, UJointAtBase, and wRp.

Referenced by JointSpaceForceVct().

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

Return the torque vector of the platform.

Parameters:

Gravity,:

Gravity (9.81)

Eq:

$\tau = J^{-T}F$

Where:

is the Jacobian matrix of the platform.
F is the joint space force vector (JointSpaceForceVct()).

Definition at line 1625 of file stewart.cpp.

References ddl, dl, Jacobian, JointSpaceForceVct(), and Links.

Referenced by Find_h(), Find_M(), and stewartmain().

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

Return a vector containing the six actuation force components.

Parameters:

Gravity,:

Gravity (9.81)

See the description of LinkStewart::ActuationForce().

Definition at line 1595 of file stewart.cpp.

References Find_C(), Find_InvJacob1(), IJ1, and Links.

Referenced by Torque().

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 1429 of file stewart.cpp.

References L, and Links.

Referenced by stewartmain().

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 1567 of file stewart.cpp.

References Jacobian, L, Links, q, and set_q().

Referenced by stewartmain().

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 1646 of file stewart.cpp.

References set_ddq(), and Torque().

Referenced by Find_Mc_Nc_Gc(), and ForwardDyn().

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 1663 of file stewart.cpp.

References dq, set_ddq(), set_dq(), and Torque().

Referenced by Find_Mc_Nc_Gc(), and ForwardDyn().

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 1701 of file stewart.cpp.

References Find_h(), and Find_M().

Referenced by stewartmain().

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: