stewart.h

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
*/

#ifndef __cplusplus
#error Must use C++
#endif
#ifndef STEWART_H
#define STEWART_H


static const char header_stewart_rcsid[] = "$Id: stewart.h,v 1.2 2006/05/16 16:11:15 gourdeau Exp $";

#include "utils.h"

#ifdef use_namespace
namespace ROBOOP {
  using namespace NEWMAT;
#endif


class LinkStewart {
    friend class Stewart;       
private:
    ColumnVector ap,    
                 b;     
    Real I1aa,          
         I1nn,          
         I2aa,          
         I2nn,          
         m1,            
         m2,            
         Lenght1,       
         Lenght2;       
public:

    ColumnVector UnitV,  
                 aPos,   
                 Vu,     
                 Vc,     
                 Vv,     
                 da,     
                 dda,    
                 LOmega, 
                 LAlpha, 
                 ACM1,   
                 M,      
                 N,      
                 gravity;
    Real L;              
        
    LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q);
    LinkStewart (const LinkStewart & x);
    LinkStewart ();

    ~LinkStewart ();
    const LinkStewart & operator = (const LinkStewart & x);

    void set_ap (const ColumnVector NewAp); 
    void set_b (const ColumnVector Newb); 
    void set_I1aa(const Real NewI1aa);
    void set_I1nn (const Real NewI1nn);
    void set_I2aa (const Real NewI2aa);
    void set_I2nn (const Real NewI2nn);
    void set_m1 (const Real Newm1);
    void set_m2 (const Real Newm2);
    void set_Lenght1 (const Real NewLenght1);
    void set_Lenght2 (const Real NewLenght2);

    ReturnMatrix get_ap() const;
    ReturnMatrix get_b() const;
    Real get_I1aa () const;
    Real get_I1nn () const;
    Real get_I2aa () const;
    Real get_I2nn () const;
    Real get_m1 () const;
    Real get_m2 () const;
    Real get_Lenght1() const;
    Real get_Lenght2 () const;

    void LTransform(const Matrix wRp, const ColumnVector q);
    void d_LTransform(const ColumnVector dq, const ColumnVector Omega, const Real dl, 
                      const Real ddl);
    void dd_LTransform(const ColumnVector ddq, const ColumnVector Omega, 
                       const ColumnVector Alpha, const Real dl, const Real ddl);
    void tau_LTransform(const Real dl, const Real ddl, const Real Gravity);
    ReturnMatrix Find_UnitV ();
    ReturnMatrix Find_a (const Matrix _wRp, const ColumnVector _q);
    ReturnMatrix Find_da (const ColumnVector dq,const ColumnVector Omega);      
    ReturnMatrix Find_dda (const ColumnVector ddq, const ColumnVector Omega, 
                           const ColumnVector Alpha);
    Real Find_Lenght ();

    ReturnMatrix Find_VctU ();
    ReturnMatrix Find_VctV ();
    ReturnMatrix Find_VctC ();
    ReturnMatrix Find_AngularKin (const Real dl, const Real ddl);
    ReturnMatrix NormalForce();
    ReturnMatrix AxialForce (const Matrix J1, const ColumnVector C, const int Index);    
    ReturnMatrix Find_N(const Real Gravity = GRAVITY);
    ReturnMatrix Moment();
    Real ActuationForce (const Matrix J1, const ColumnVector C, const int Index,
                         const Real Gravity = GRAVITY);
    ReturnMatrix Find_ACM1 (const Real dl, const Real ddl);
};

class Stewart {

private:
    bool UJointAtBase;  
    ColumnVector q,      
                 dq,     
                 ddq,    
                 pR,     
                 gravity;
    Matrix       pIp;   
    Real mp,            
         p,             
         n,             
         Js,            
         Jm,            
         bs,            
         bm,            
         Kb,            
         L,             
         R,             
         Kt;            
    LinkStewart Links[6]; 

 public:
    Matrix wRp,         
           Jacobian,    
           IJ1,         
           IJ2;         
    ColumnVector dl,    
                 ddl,   
                 Alpha, 
                 Omega; 

    Stewart ();
    Stewart (const Matrix InitPlat, bool Joint = true);
    Stewart (const Stewart & x);
    Stewart (const std::string & filename, const std::string & PlatformName);
    ~Stewart ();
    const Stewart & operator = (const Stewart& x);                              

    void set_Joint(const bool _Joint);
    void set_q (const ColumnVector _q);
    void set_dq (const ColumnVector _dq);
    void set_ddq (const ColumnVector _ddq);
    void set_pR (const ColumnVector _pR);
    void set_pIp (const Matrix _pIp);
    void set_mp (const Real _mp);
    bool get_Joint () const;
    ReturnMatrix get_q() const;
    ReturnMatrix get_dq() const;
    ReturnMatrix get_ddq() const;
    ReturnMatrix get_pR() const;
    ReturnMatrix get_pIp() const;
    Real get_mp () const;
    
    void Transform();
    ReturnMatrix Find_wRp ();
    ReturnMatrix Find_Omega ();
    ReturnMatrix Find_Alpha ();
    ReturnMatrix jacobian ();
    ReturnMatrix Find_InvJacob1 ();
    ReturnMatrix Find_InvJacob2 ();
    ReturnMatrix jacobian_dot();
    ReturnMatrix Find_dl ();
    ReturnMatrix Find_ddl ();
    ReturnMatrix Find_C(const Real Gravity = GRAVITY);
    ReturnMatrix Torque(const Real Gravity = GRAVITY);
    ReturnMatrix JointSpaceForceVct(const Real Gravity = GRAVITY);
    ReturnMatrix InvPosKine();
    ReturnMatrix ForwardKine(const ColumnVector guess_q, const ColumnVector l_given,
                             const Real tolerance = 0.001);
    ReturnMatrix Find_h(const Real Gravity = GRAVITY);
    ReturnMatrix Find_M();
    ReturnMatrix ForwardDyn(const ColumnVector Torque, const Real Gravity=GRAVITY);
    void Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc);
    ReturnMatrix ForwardDyn_AD(const ColumnVector Command, const Real t);
};

#ifdef use_namespace
}
#endif

#endif //Class Stewart