stewart.cpp

Go to the documentation of this file.

/*
Copyright (C) 2004  Samuel Bélanger

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


Report problems and direct all questions to:

email: samuel.belanger@polymtl.ca or richard.gourdeau@polymtl.ca
*/



static const char rcsid[] = "$Id: stewart.cpp,v 1.6 2006/05/16 19:24:26 gourdeau Exp $";

#include "config.h"
#include "stewart.h"

#ifdef use_namespace
namespace ROBOOP {
#endif


LinkStewart::LinkStewart ()
{
    b = ColumnVector(3);
    b = 0.0;
    ap = ColumnVector(3);
    ap = 0.0;
    
    I1aa = 0.0;
    I1nn = 0.0;
    I2aa = 0.0;
    I2nn = 0.0;
    m1 = 0.0;
    m2 = 0.0;
    Lenght1 = 0.0;
    Lenght2 = 0.0;
    
    ColumnVector ZeroValue(3);
    ZeroValue<<0.0<<0.0<<0.0;
    
    UnitV = ZeroValue;
    aPos = ZeroValue;
    Vu = ZeroValue;
    Vc = ZeroValue;
    Vv = ZeroValue;
    da = ZeroValue;
    dda = ZeroValue;
    LOmega = ZeroValue;
    LAlpha = ZeroValue;
    ACM1 = ZeroValue;
    N = ZeroValue;
    gravity = ZeroValue;
    L = 0.0;
}

LinkStewart::LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q)
{       
    b = InitLink.Rows(1,3);
    ap = InitLink.Rows(4,6);
    I1aa = InitLink(7);
    I1nn = InitLink(8);
    I2aa = InitLink(9);
    I2nn = InitLink(10);
    m1 = InitLink(11);
    m2 = InitLink(12);
    Lenght1 = InitLink(13);
    Lenght2 = InitLink(14);

    ColumnVector ZeroValue(3);
    ZeroValue<<0.0<<0.0<<0.0;
    
    UnitV = ZeroValue;
    aPos = ZeroValue;
    Vc = ZeroValue;
    Vv = ZeroValue;
    da = ZeroValue;
    dda = ZeroValue;
    LOmega = ZeroValue;
    LAlpha = ZeroValue;
    ACM1 = ZeroValue;
    N = ZeroValue;
    gravity = ZeroValue;
    L = 0.0;
    
    aPos = Find_a(wRp,q);
    L = Find_Lenght();
    UnitV = Find_UnitV();
    Vu = Find_VctU();
}

LinkStewart::LinkStewart(const LinkStewart & x)
{
    b = x.b;
    ap = x.ap;
    I1aa = x.I1aa;
    I1nn = x.I1nn;
    I2aa = x.I2aa;
    I2nn = x.I2nn;
    m1 = x.m1;
    m2 = x.m2;
    Lenght1 = x.Lenght1;
    Lenght2 = x.Lenght2;

    UnitV = x.UnitV;
    aPos = x.aPos;
    Vu = x.Vu;
    Vc = x.Vc;
    Vv = x.Vv;
    da = x.da;
    dda = x.dda;
    LOmega = x.LOmega;
    LAlpha = x.LAlpha;
    ACM1 = x.ACM1;
    N = x.N;
    gravity = x.gravity;
    L = x.L;
}

LinkStewart::~LinkStewart()
{
}

const LinkStewart & LinkStewart::operator = (const LinkStewart& x)
{
    b = x.b;
    ap = x.ap;
    I1aa = x.I1aa;
    I1nn = x.I1nn;
    I2aa = x.I2aa;
    I2nn = x.I2nn;
    m1 = x.m1;
    m2 = x.m2;
    Lenght1 = x.Lenght1;
    Lenght2 = x.Lenght2;

    UnitV = x.UnitV;
    aPos = x.aPos;
    Vu = x.Vu;
    Vc = x.Vc;
    Vv = x.Vv;
    da = x.da;
    dda = x.dda;
    LOmega = x.LOmega;
    LAlpha = x.LAlpha;
    ACM1 = x.ACM1;
    N = x.N;
    gravity = x.gravity;
    L = x.L;
    return *this;
}

void LinkStewart::set_b(const ColumnVector  Newb)
{
    if(Newb.Nrows() == 3)
        b = Newb;
    else
        cerr<< "LinkStewart::set_b: wrong size in input vector."<< endl;
}

void LinkStewart::set_ap(const ColumnVector NewAp)
{
    if(NewAp.Nrows()== 3)
        ap = NewAp;
    else
        cerr<< "LinkStewart::set_Ap: wrong size in input vector."<< endl;
}

void LinkStewart::set_I1aa(const Real NewI1aa) 
 
{
    I1aa = NewI1aa;
}

void LinkStewart::set_I1nn(const Real NewI1nn) 

{
    I1nn = NewI1nn;
} 

void LinkStewart::set_I2aa(const Real NewI2aa) 

{
    I2aa = NewI2aa;
} 

void LinkStewart::set_I2nn(const Real NewI2nn) 

{
    I2nn = NewI2nn;
}

void LinkStewart::set_m1(const Real Newm1) 

{
    m1 = Newm1;
}

void LinkStewart::set_m2(const Real Newm2) 

{
    m2 = Newm2;
}

void LinkStewart::set_Lenght1(const Real NewLenght1)
{
    Lenght1 = NewLenght1;
}

void LinkStewart::set_Lenght2(const Real NewLenght2)
{
    Lenght2 = NewLenght2;
}

ReturnMatrix LinkStewart::get_ap() const
{
    return ap;
}

ReturnMatrix LinkStewart::get_b() const
{
    return b;
}

Real LinkStewart::get_I1aa() const
{
    return I1aa;
}

Real LinkStewart::get_I1nn() const
{
    return I1nn;
}

Real LinkStewart::get_I2aa() const
{
    return I2aa;
}

Real LinkStewart::get_I2nn() const
{
    return I2nn;
}

Real LinkStewart::get_m1() const
{
    return m1;
}

Real LinkStewart::get_m2() const
{
    return m2;
}

Real LinkStewart::get_Lenght1()  const
{
    return Lenght1;
}

Real LinkStewart::get_Lenght2() const
{
    return Lenght2;
}

void LinkStewart::LTransform(const Matrix wRp, const ColumnVector q)
{
    aPos = Find_a(wRp,q);
    L = Find_Lenght();
    UnitV = Find_UnitV();
    Vv = Find_VctV();
    Vc = Find_VctC();
}

void LinkStewart::d_LTransform(const ColumnVector dq, const ColumnVector Omega, 
                               const Real dl, const Real ddl)
{
    Matrix AngularKin;

    da = Find_da(dq,Omega);
    AngularKin = Find_AngularKin(dl, ddl);
    LOmega = AngularKin.Column(1);
    LAlpha = AngularKin.Column(2);
}

void LinkStewart::dd_LTransform(const ColumnVector ddq, const ColumnVector Omega, 
                                const ColumnVector Alpha, const Real dl, const Real ddl)
{
    Matrix AngularKin;
    
    dda = Find_dda(ddq,Omega,Alpha);
    AngularKin = Find_AngularKin(dl, ddl);
    LOmega = AngularKin.Column(1);
    LAlpha = AngularKin.Column(2);
}

void LinkStewart::tau_LTransform(const Real dl, const Real ddl,const Real Gravity)
{
    ACM1 = Find_ACM1(dl,ddl);
    N = Find_N(Gravity);
}

ReturnMatrix LinkStewart::Find_a(const Matrix wRp, const ColumnVector q)
{
    ColumnVector a;
    a = q.Rows(1,3) + wRp*ap;
    a.Release();  
    return a;
}

ReturnMatrix LinkStewart::Find_UnitV()
{
    Matrix Tmp (1,3);    
    Tmp = (aPos - b)/L;
    Tmp.Release(); 
    return Tmp;
}

ReturnMatrix LinkStewart::Find_da(const ColumnVector dq, const ColumnVector Omega)
{
    ColumnVector da;
    
    da = dq.Rows(1,3) + CrossProduct(Omega,aPos);
    
    da.Release(); 
    return da;
}

ReturnMatrix LinkStewart::Find_dda(const ColumnVector ddq,const ColumnVector Omega,const ColumnVector Alpha)
{
    ColumnVector dda;
    dda = ddq.Rows(1,3) + CrossProduct(Alpha,aPos) +
        CrossProduct(Omega,CrossProduct(Omega,aPos));
    
    dda.Release(); 
    return dda;
}

Real LinkStewart::Find_Lenght()
{
    return sqrt(DotProduct((aPos - b),(aPos - b)));
}

ReturnMatrix LinkStewart::Find_VctU()
{
    ColumnVector u(3);
    
    u(1) = -UnitV(1)/sqrt(UnitV(1)*UnitV(1) + UnitV(3)*UnitV(3));
    u(2) = 0.0;
    u(3) = -UnitV(3)/sqrt(UnitV(1)*UnitV(1) + UnitV(3)*UnitV(3));
    
    u.Release();
    return u;
}

ReturnMatrix LinkStewart::Find_VctV()
{
    ColumnVector v;
    v = CrossProduct(Vu,UnitV)/(CrossProduct(Vu,UnitV).NormFrobenius());    

    v.Release(); 
    return v;
}

ReturnMatrix LinkStewart::Find_VctC()
{
    return CrossProduct(Vu, Vv);
}

ReturnMatrix LinkStewart::Find_AngularKin(const Real dl, const Real ddl)
{
    Matrix Temp(3,2);
    ColumnVector tmp_dda(3), omega(3), alpha(3);
    Real wu, wv, au, av;
    
    wu = DotProduct(-(da-dl*UnitV),Vv/(DotProduct(L*UnitV,Vc)));
    wv = DotProduct((da-dl*UnitV),Vu/(DotProduct(L*UnitV,Vc)));
    
    omega = wu*Vu + wv*Vv;
    
    tmp_dda = dda - wu*wv*L*CrossProduct(Vc,UnitV)-ddl*UnitV-2*dl*CrossProduct(omega,UnitV)
        -L*CrossProduct(omega,CrossProduct(omega,UnitV));
    
    au = DotProduct(-tmp_dda,Vv/(L*DotProduct(UnitV,Vc)));
    av = DotProduct(tmp_dda,Vu/(L*DotProduct(UnitV,Vc)));
    
    alpha = au*Vu + av*Vv + wu*wv*Vc;
    
    Temp.Column(1) = omega; Temp.Column(2) = alpha;
    
    Temp.Release(); return Temp;
}

ReturnMatrix LinkStewart::Find_N(const Real Gravity)
{
    ColumnVector  Accel2(3);
    Matrix Fn(3,1), N_N(3,1), I1(3,3), I2(3,3);
    
    gravity = 0;
    gravity(2) = -Gravity;

    Accel2 = Lenght2*CrossProduct(LOmega,CrossProduct(LOmega, UnitV)) 
             + Lenght2*CrossProduct(LAlpha,UnitV);
    
    IdentityMatrix Identity(3);
    
    I1 = I1aa*UnitV*UnitV.t() + I1nn*(Identity - UnitV*UnitV.t());
    I2 = I2aa*UnitV*UnitV.t() + I2nn*(Identity - UnitV*UnitV.t());
    
    N_N = -m1*(L-Lenght1)*CrossProduct(UnitV, gravity) - m2*Lenght2*CrossProduct(UnitV, gravity) +
        (I1+I2)*LAlpha - (I1+I2)*CrossProduct(LOmega,LOmega) 
        + m1*(L-Lenght1)*CrossProduct(UnitV,ACM1)
        +m2*Lenght2*CrossProduct(UnitV,Accel2);
    
    N_N.Release(); 
    return N_N;
}

ReturnMatrix LinkStewart::Moment()
{
    Matrix M(3,1);    
    M = DotProduct(N, UnitV)/DotProduct(Vc, UnitV);
    M.Release(); 
    return M;   
}

ReturnMatrix LinkStewart::NormalForce()
{
    Matrix Fn;
    Fn = (CrossProduct(N, UnitV) - CrossProduct(Moment(), UnitV))/L;    
    Fn.Release(); 
    return Fn;
}

ReturnMatrix LinkStewart::AxialForce(const Matrix J1, const ColumnVector C, 
                                     const int Index)
{
    Matrix Fa;
    ColumnVector tmp;
    tmp = (J1.t()*C);
    Fa = tmp(Index)*UnitV;
    
    Fa.Release(); 
    return Fa;
}

Real LinkStewart::ActuationForce(const Matrix J1, const ColumnVector C, 
                                 const int Index, const Real Gravity)
{
    Real f, fa;

    ColumnVector Fa(3);
    gravity = 0;
    gravity(2) = -Gravity;
    
    Fa = AxialForce(J1, C, Index);
    
    if (Fa(1) != 0)
        fa = Fa(1)/UnitV(1);
    else if (Fa(2) != 0)
        fa = Fa(2)/UnitV(2);
    else if (Fa(3) != 0)
        fa = Fa(3)/UnitV(3);
    else
        fa = 0;
    
    f = m1*DotProduct(ACM1,UnitV) - fa - m1*DotProduct(gravity, UnitV);
    
    return f;
}
ReturnMatrix LinkStewart::Find_ACM1(const Real dl, const Real ddl)
{
   ColumnVector Accel1;
    
    Accel1 = (L-Lenght1)*CrossProduct(LOmega, CrossProduct(LOmega, UnitV)) +
        (L-Lenght1)*CrossProduct(LAlpha,UnitV) +
        CrossProduct(2*LOmega,dl*UnitV)+ddl*UnitV;
    
    Accel1.Release(); 
    return Accel1;
}


Stewart::Stewart()
{
    q = ColumnVector(6);
    q = 0.0;
    dq = ColumnVector(6);
    dq = 0.0;
    ddq = ColumnVector(6);
    ddq = 0.0;
    pR = ColumnVector(3);
    pR = 0.0;
    gravity = pR;
    pIp = Matrix(3,3);
    pIp = 0.0;
    mp = 0.0;
    p = 0.0;
    n = 0.0;
    Js = 0.0;
    Jm = 0.0;
    bs = 0.0;
    bm = 0.0;
    p = 0.0;
    n = 0.0;
    Kb = 0.0;
    L = 0.0;
    R = 0.0;
    Kt = 0.0;
}

Stewart::Stewart(const Matrix InitPlatt, bool Joint)
{
    ColumnVector InitLink (14);
    Matrix Inertia (3,3);
    
    pR = InitPlatt.SubMatrix(7,7,1,3).t();
    
    Inertia = 0.0;
    Inertia(1,1) = InitPlatt(7,4);
    Inertia(2,2) = InitPlatt(7,5);
    Inertia(3,3) = InitPlatt(7,6);
    pIp = Inertia;
    mp = InitPlatt(7,7);
    Js = InitPlatt(7,8);
    Jm = InitPlatt(7,9);
    bs = InitPlatt(7,10);
    bm = InitPlatt(7,11);
    p = InitPlatt(7,12);
    n = InitPlatt(7,13);
    Kb = InitPlatt(7,14);
    L = InitPlatt(7,15);
    R = InitPlatt(7,16);
    Kt = InitPlatt(7,17);
    
    UJointAtBase = Joint;
    
    q = ColumnVector(6); q <<0.0 <<0.0 <<1.0 <<0.0 <<M_PI/2 <<0.0;
    dq = ColumnVector(6); dq =0.0;
    ddq = ColumnVector(6); ddq =0.0;

    wRp = Find_wRp();

    gravity = ColumnVector(3);
    gravity = 0;
    
    for (int i =0; i<6; i++)
    {
        InitLink = InitPlatt.Row(i+1).t();
        Links[i] = LinkStewart (InitLink,wRp,q);
    }
}

Stewart::Stewart(const string & FileName, const string & PlatFormName)
{
    pR = ColumnVector(3);
    pR = 0.0;
    gravity = pR;
    pIp = Matrix(3,3);
    pIp = 0.0;
    mp = 0.0;
    
    ColumnVector InitLink(14);
    
    Config platData;
    ifstream inconffile(FileName.c_str(), std::ios::in);
    if (platData.read_conf(inconffile))
    {
        cerr << "Stewart::Stewart: can not read input config file" << endl;
    }
    
    platData.select(PlatFormName, "mp", mp);
    platData.select(PlatFormName, "Joint", UJointAtBase);
    platData.select(PlatFormName, "pRx", pR(1));
    platData.select(PlatFormName, "pRy", pR(2));
    platData.select(PlatFormName, "pRz", pR(3));
    platData.select(PlatFormName, "Ixx", pIp(1,1));
    platData.select(PlatFormName, "Iyy", pIp(2,2));
    platData.select(PlatFormName, "Izz", pIp(3,3));
    platData.select(PlatFormName, "Js", Js);
    platData.select(PlatFormName, "Jm", Jm);
    platData.select(PlatFormName, "bs", bs);
    platData.select(PlatFormName, "bm", bm);
    platData.select(PlatFormName, "p", p);
    platData.select(PlatFormName, "n", n);
    platData.select(PlatFormName, "Kb", Kb);
    platData.select(PlatFormName, "L", L);
    platData.select(PlatFormName, "R", R);
    platData.select(PlatFormName, "Kt", Kt);

    q = ColumnVector(6); q <<0.0 <<0.0 <<1.0 <<0.0 <<M_PI/2 <<0.0;
    dq = ColumnVector(6); dq =0.0;
    ddq = ColumnVector(6); ddq =0.0;
    
    wRp = Find_wRp();
    
    for(int j = 1; j <= 6; j++)
    {
        string platformName_link;
        ostringstream ostr;
        ostr << PlatFormName << "_LINK" << j;
        platformName_link = ostr.str();
        
        platData.select(platformName_link, "bx", InitLink(1));
        platData.select(platformName_link, "by", InitLink(2));
        platData.select(platformName_link, "bz", InitLink(3));
        platData.select(platformName_link, "ax", InitLink(4));
        platData.select(platformName_link, "ay", InitLink(5));
        platData.select(platformName_link, "az", InitLink(6));
        platData.select(platformName_link, "Iaa1", InitLink(7));
        platData.select(platformName_link, "Inn1", InitLink(8));
        platData.select(platformName_link, "Iaa2", InitLink(9));
        platData.select(platformName_link, "Inn2", InitLink(10));
        platData.select(platformName_link, "m1", InitLink(11));
        platData.select(platformName_link, "m2", InitLink(12));
        platData.select(platformName_link, "L1", InitLink(13));
        platData.select(platformName_link, "L2", InitLink(14));

        Links[j-1] = LinkStewart(InitLink,wRp,q);       
    }
}

Stewart::Stewart(const Stewart & x)
{
    for(int i=0;i<6;i++)
        Links[i] = x.Links[i];
    UJointAtBase = x.UJointAtBase;
    q = x.q;
    dq = x.dq;
    ddq = x.ddq;
    pR = x.pR;
    pIp = x.pIp;
    mp = x.mp;
    p = x.p;
    n = x.n;
    Js = x.Js;
    Jm = x.Jm;
    bs = x.bs;
    bm = x.bm;
    Kb = x.Kb;
    L = x.L;
    R = x.R;
    Kt = x.Kt;
    gravity = x.gravity;
}


Stewart::~Stewart()
{       
}

const Stewart & Stewart::operator = (const Stewart& x)
{
    for (int i=0; i<6; i++)
        Links[i] = x.Links[i];
    UJointAtBase = x.UJointAtBase;
    q = x.q;
    dq = x.dq;
    ddq = x.ddq;
    pR = x.pR;
    pIp = x.pIp;
    mp = x.mp;
    p = x.p;
    n = x.n;
    Js = x.Js;
    Jm = x.Jm;
    bs = x.bs;
    bm = x.bm;
    Kb = x.Kb;
    L = x.L;
    R = x.R;
    Kt = x.Kt;
    gravity = x.gravity;
    return *this;
}

void Stewart::set_Joint(const bool Joint) 

{
    UJointAtBase = Joint;
}

void Stewart::set_q(const ColumnVector _q)
{
    if(_q.Nrows()== 6)
    {
        q = _q;
        Transform();
    }
    else
        cerr<< "Stewart::set_q: wrong size in input vector."<< endl;
}

void Stewart::set_dq(const ColumnVector dq_)
{
    if(dq_.Nrows()== 6)
    {
        dq = dq_;
        
        Omega = Find_Omega();
        dl = Find_dl();
        ddl =  Find_ddl();
        
        for(int i =0; i<6; i++)
            Links[i].d_LTransform(dq,Omega, dl(i+1),ddl(i+1));
    }
    else
        cerr<< "Stewart::set_dq: wrong size in input vector."<< endl;   
}

void Stewart::set_ddq(const ColumnVector _ddq)
{
    if(_ddq.Nrows()== 6)
    {
        ddq = _ddq;
        
        Omega = Find_Omega();
        Alpha = Find_Alpha();
        ddl = Find_ddl();
        for(int i =0; i<6; i++)
            Links[i].dd_LTransform(ddq,Omega,Alpha, dl(i+1),ddl(i+1));  
    }
    else
        cerr<< "Stewart::set_ddq: wrong size in input vector."<< endl;
}

void Stewart::set_pR(const ColumnVector _pR)
{
    if(_pR.Nrows()== 3)
        pR = _pR;
    else
        cerr<< "Stewart::set_pR: wrong size in input vector."<< endl;
}

void Stewart::set_pIp(const Matrix _pIp)
{
    if((_pIp.Nrows()== 3)&&(_pIp.Ncols() == 3))
        pIp = _pIp;
    else
        cerr<< "Stewart::set_pIp: wrong size in input vector."<< endl;
}

void Stewart::set_mp (const Real _mp)
{
    mp = _mp;
} 
    
bool Stewart::get_Joint () const
{
    return UJointAtBase;
}

ReturnMatrix Stewart::get_q () const
{
    return q;
}

ReturnMatrix Stewart::get_dq () const
{
    return dq;
}

ReturnMatrix Stewart::get_ddq () const
{
    return ddq;
}

ReturnMatrix Stewart::get_pR () const
{
    return pR;
}

ReturnMatrix Stewart::get_pIp () const
{
    return pIp;
}

Real Stewart::get_mp() const
{
    return mp;
}

void Stewart::Transform()
{
    wRp = Find_wRp();
    
    for(int i=0; i<6; i++)
        Links[i].LTransform(wRp, q);
    
    IJ1 = Find_InvJacob1();
    IJ2 = Find_InvJacob2();
    Jacobian = jacobian();      
}

ReturnMatrix Stewart::Find_wRp ()
{
    Matrix _wRp(3,3);
    
    _wRp(1,1) = cos(q(6))*cos(q(4)) - cos(q(5))*sin(q(4))*sin(q(6)); 
    _wRp(1,2) = -sin(q(6))*cos(q(4)) - cos(q(5))*sin(q(4))*cos(q(6));
    _wRp(1,3) = sin(q(5))*sin(q(4));
    _wRp(2,1) = sin(q(6))*cos(q(4)) + cos(q(5))*sin(q(4))*cos(q(6));
    _wRp(2,2) = -sin(q(6))*sin(q(4)) + cos(q(6))*cos(q(4))*cos(q(5));
    _wRp(2,3) = -sin(q(5))*cos(q(4));
    _wRp(3,1) = sin(q(6))*sin(q(5));
    _wRp(3,2) = sin(q(5))*cos(q(6));
    _wRp(3,3) = cos(q(5));
    
    _wRp.Release(); 
    return _wRp;
}

ReturnMatrix Stewart::Find_Omega()
{
    ColumnVector w(3);
    
    w(1) = cos(q(4))*dq(5) + sin(q(4))*cos(q(5))*dq(6);
    w(2) = sin(q(4))*dq(5) - cos(q(4))*sin(q(5))*dq(6);
    w(3) = dq(4) + cos(q(5))*dq(6);
    
    w.Release(); 
    return w;
}

ReturnMatrix Stewart::Find_Alpha()
{
    Matrix A, Temp(3,3),Temp2(3,3);
    
    Temp = 0.0; Temp(3,1) = 1; Temp(1,2) = cos(q(4)); Temp(2,2) = sin(q(4));
    Temp(1,3) = sin(q(4))*cos(q(5)); Temp(2,3)=-cos(q(4))*sin(q(5)); Temp(3,3) = cos(q(5));
    
    Temp2 = 0.0; Temp2(1,2) = -dq(4)*sin(q(4)); Temp2(2,2) = dq(4)*cos(q(4));
    Temp2(1,3) = dq(4)*cos(q(4))*sin(q(5))+dq(5)*sin(q(4))*cos(q(5));
    Temp2(2,3) = dq(4)*sin(q(4))*sin(q(5))-dq(5)*cos(q(4))*cos(q(5));
    Temp2(3,3) = -dq(5)*sin(q(5));
    
    A = Temp*ddq.Rows(4,6) + Temp2*dq.Rows(4,6);
    
    A.Release(); 
    return A;
}

ReturnMatrix Stewart::jacobian()
{
    Matrix _Jacobi;
    
    _Jacobi = (IJ1*IJ2).i();
    
    _Jacobi.Release(); 
    return _Jacobi;
}

ReturnMatrix Stewart::Find_InvJacob1()
{
    Matrix tmp_Jacobi1 (6,6);
    
    for(int i = 0; i<6; i++)
        tmp_Jacobi1.Row(i+1) = Links[i].UnitV.t() | CrossProduct(wRp*Links[i].ap,Links[i].UnitV).t();
    
    tmp_Jacobi1.Release(); 
    return tmp_Jacobi1;
}

ReturnMatrix Stewart::Find_InvJacob2()
{
    Matrix tmp_Jacobi2;

    tmp_Jacobi2 = IdentityMatrix(6);
    tmp_Jacobi2(4,4) = 0;
    tmp_Jacobi2(6,4) = 1;
    tmp_Jacobi2(4,5) = cos(q(4));
    tmp_Jacobi2(5,5) = sin(q(4));
    tmp_Jacobi2(4,6) = sin(q(4))*sin(q(5));
    tmp_Jacobi2(5,6) = -cos(q(4))*sin(q(5));
    tmp_Jacobi2(6,6) = cos(q(5));

    tmp_Jacobi2.Release(); 
    return tmp_Jacobi2;
}
ReturnMatrix Stewart::jacobian_dot()
{
    Matrix tmp_dJ2(6,6), tmp_dn(6,3), tmp_sol(6,3), tmp_dJ1(6,6), tmp_dJ(6,6);
    ColumnVector VctNorm, a;

    tmp_dJ2 = 0.0;
    tmp_dJ2(4,5) = -dq(4)*sin(q(4));
    tmp_dJ2(5,5) = dq(4)*cos(q(4));
    tmp_dJ2(4,6) = dq(4)*cos(q(4))*sin(q(5))+dq(5)*sin(q(4))*cos(q(5));
    tmp_dJ2(5,6) = dq(4)*sin(q(4))*sin(q(5))-dq(5)*cos(q(4))*cos(q(5));
    tmp_dJ2(6,6) = -dq(5)*sin(q(5));
    
    for (int i = 0; i<6; i++)
    {
        tmp_dn.Row(i+1) = ((Links[i].UnitV*Links[i].L - 
                            dl(i+1)*Links[i].UnitV)/Links[i].L).t();
        tmp_sol.Row(i+1) = (CrossProduct(CrossProduct(Omega,Links[i].aPos.t()),
                                         Links[i].UnitV.t()) +
                            (CrossProduct(Links[i].aPos,tmp_dn.Row(i+1)))).t();
    }
    
    for (int j = 1; j < 7; j++)
        for(int k = 1; k < 4; k++)
        {
            tmp_dJ1(j,k) = tmp_dn(j,k);
            tmp_dJ1(j,k+3) = tmp_sol(j,k);
        }
    
    tmp_dJ = tmp_dJ1*IJ2 + IJ1*tmp_dJ2;
    
    tmp_dJ.Release(); 
    return tmp_dJ;
}
ReturnMatrix Stewart::InvPosKine()
{
   ColumnVector Vct_L(6);

    for (int i = 1; i < 7; i++)
        Vct_L(i) = Links[i-1].L;
    
    Vct_L.Release(); return Vct_L;
}

ReturnMatrix Stewart::Find_dl()
{
    ColumnVector tmp_dl;
    tmp_dl = Jacobian.i()*dq;

    tmp_dl.Release(); 
    return tmp_dl;
}

ReturnMatrix Stewart::Find_ddl()
{
    ColumnVector tmp_ddl;
    tmp_ddl = Jacobian.i() * ddq + jacobian_dot() * dq;    
    tmp_ddl.Release(); 
    return tmp_ddl;
}

ReturnMatrix Stewart::Find_C(const Real Gravity)
{
    Matrix C(6,1), I(3,3);
    ColumnVector Sum(3), Sum2(3), Sum3(3), ddxg(3), LNormalForce(3);

    gravity = 0;
    gravity(2) = -Gravity;
    
    ddxg = ddq.Rows(1,3) + CrossProduct(Alpha,wRp*pR) + CrossProduct(Omega,CrossProduct(Omega,wRp*pR));
    I = wRp*pIp*(wRp.t());
    
    Sum = 0.0;
    Sum2 = 0.0;
    Sum3 = 0.0;
    for (int i=0; i<6; i++)
    {   
        LNormalForce = Links[i].NormalForce();
        Sum = Sum + LNormalForce;
        Sum2 = Sum2 + CrossProduct(Links[i].aPos,LNormalForce);
        if(!UJointAtBase)
        {
            Sum3 = Sum3 +Links[i].Moment();
        }
    }
    
    C.Rows(1,3) = mp*gravity - mp*ddxg - Sum;
    C.Rows(4,6) = mp*CrossProduct(wRp*pR, gravity) - mp*CrossProduct(wRp*pR,ddxg)-I*Alpha
        + I*CrossProduct(Omega,Omega)- Sum2 - Sum3;
    
    C.Release(); 
    return C;   
}

ReturnMatrix Stewart::ForwardKine(const ColumnVector guess_q, const ColumnVector l_given, 
                                  const Real tolerance)
{
    ColumnVector next_q, tmp_long(6);
    Real Diff = 1;
    
    q = guess_q;
    while (Diff>tolerance)
    {
        for(int i=0; i<6; i++)
            tmp_long(i+1) = Links[i].L - l_given(i+1);
        
        next_q = q - Jacobian*(tmp_long);
        Diff = (next_q - q).MaximumAbsoluteValue();
        
        set_q(next_q);
    }
    next_q.Release(); 
    return next_q;
}

ReturnMatrix Stewart::JointSpaceForceVct(const Real Gravity)
{
    Matrix F(6,1), C, IJ1(6,6);

    IJ1 = Find_InvJacob1();
    C = Find_C(Gravity);
    
    for (int i =0; i<6; i++)
    {
        F(i+1,1) = Links[i].ActuationForce(IJ1, C, i+1, Gravity);
    }
    
    F.Release(); 
    return F;
}

ReturnMatrix Stewart::Torque(const Real Gravity)
{
    Matrix T;
    
    for(int i=0;i<6;i++)
        Links[i].tau_LTransform(dl(i+1), ddl(i+1), Gravity);
    
    T = Jacobian.i().t()*JointSpaceForceVct(Gravity);
    
    T.Release(); 
    return T;
}
ReturnMatrix Stewart::Find_h(const Real Gravity)
{
    ColumnVector _ddq(6);
    _ddq = 0.0;
    set_ddq(_ddq);
    return Torque(Gravity);
}

ReturnMatrix Stewart::Find_M()
{
    Matrix M(6,6);
    ColumnVector _ddq(6), _dq(6), tmpdq(6);

    tmpdq = dq;
    _dq = 0.0;

    set_dq(_dq);
    
    for (int i = 1; i < 7; i++)
    {
        _ddq = 0.0;
        _ddq(i) = 1.0;
        set_ddq(_ddq);
        M.Column(i) = Torque (0.0);
    }
    set_dq(tmpdq);
        
    M.Release(); 
    return M;
}

ReturnMatrix Stewart::ForwardDyn(const ColumnVector T, const Real Gravity)
{
    ColumnVector _ddq;
     
    _ddq = Find_M().i()*(T - Find_h(Gravity));
    
    _ddq.Release();  
    return _ddq;
}

void Stewart::Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc)
{
    Matrix G, Ka(6,6), Ma(6,6), Va(6,6), dJacobian(6,6);
    dJacobian = jacobian_dot();
    
    Ka = p/(2*M_PI*n)*IdentityMatrix(6);
    Ma = (2*M_PI/(n*p))*(Js + n*n*Jm)*IdentityMatrix(6);
    Va = (2*M_PI/(n*p))*(bs + n*n*bm)*IdentityMatrix(6);
    
    Mc = Ka*Jacobian.t() * Find_M() + Ma*Jacobian.i();
    
    Nc = Ka*Jacobian.t()*Find_h(0.0) + (Va*Jacobian.i()+Ma*dJacobian)*dq;
    
    ColumnVector _dq(6), _tmpdq(6);
    _dq = 0.0;
    _tmpdq = dq;
    set_dq(_dq);
    
    Gc = Ka *Jacobian.t()*Find_h();
    set_dq(_tmpdq);
}

ReturnMatrix Stewart::ForwardDyn_AD(const ColumnVector Command, const Real t)
{
    Matrix _ddq;
    Matrix Nc,Gc,Mc, tmp1,tmp2;
    
    Find_Mc_Nc_Gc(Mc,Nc,Gc);
    
    tmp1 = (Command - (Jacobian.i()*dq*(2*M_PI/p)*Kb));
    tmp2 = (IdentityMatrix(6)*Kt/L*exp(-R*t/L))*tmp1;

    _ddq = Mc.i()*(tmp2 - Nc - Gc);
    
    _ddq.Release();  
    return _ddq;
}

#ifdef use_namespace
}
#endif