controller.h
Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 -------------------------------------------------------------------------------
00024 Revision_history:
00025 
00026 2004/07/13: Ethan Tira-Thompson
00027     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00028 
00029 2005/06/10: Etienne Lachance
00030     -The desired joint acceleration was missing in the computed torque method.
00031 
00032 2005/11/06: Etienne Lachance
00033     - No need to provide a copy constructor and the assignment operator 
00034       (operator=) for Impedance, Resolved_acc, Computed_torque_method and
00035       Proportional_Derivative classes. Instead we use the one provide by the
00036       compiler.
00037 -------------------------------------------------------------------------------
00038 */
00039 
00040 #ifndef CONTROLLER_H
00041 #define CONTROLLER_H
00042 
00048 
00049 static const char header_controller_rcsid[] = "$Id: controller.h,v 1.5 2006/05/16 16:11:15 gourdeau Exp $";
00050 
00051 
00052 #include "robot.h"
00053 #include "quaternion.h"
00054 
00055 #ifdef use_namespace
00056 namespace ROBOOP {
00057   using namespace NEWMAT;
00058 #endif
00059 
00061 #define WRONG_SIZE -1
00062 
00091 class Impedance{
00092 public:
00093    Impedance();
00094    Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
00095              const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
00096              const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_, 
00097              const DiagonalMatrix & Ko_);
00098    short set_Mp(const DiagonalMatrix & Mp_);
00099    short set_Mp(const Real MP_i, const short i);
00100    short set_Dp(const DiagonalMatrix & Dp_);
00101    short set_Dp(const Real Dp_i, const short i);
00102    short set_Kp(const DiagonalMatrix & Kp_);
00103    short set_Kp(const Real Kp_i, const short i);
00104    short set_Mo(const DiagonalMatrix & Mo_);
00105    short set_Mo(const Real Mo_i, const short i);
00106    short set_Do(const DiagonalMatrix & Do_);
00107    short set_Do(const Real Do_i, const short i);
00108    short set_Ko(const DiagonalMatrix & Ko_);
00109    short set_Ko(const Real Ko_i, const short i);
00110    short control(const ColumnVector & pdpp, const ColumnVector & pdp,
00111                  const ColumnVector & pd, const ColumnVector & wdp,
00112                  const ColumnVector & wd, const Quaternion & qd,
00113                  const ColumnVector & f, const ColumnVector & n,
00114                  const Real dt);
00115 
00116    Quaternion qc,          
00117               qcp,         
00118               qcp_prev,    
00119               qcd,         
00120              quat;         
00121    ColumnVector pc,        
00122                 pcp,       
00123                 pcpp,      
00124                 pcp_prev,  
00125                 pcpp_prev, 
00126                 pcd,       
00127                 pcdp,      
00128                 wc,        
00129                 wcp,       
00130                 wcp_prev,  
00131                 wcd;       
00132 private:
00133    DiagonalMatrix Mp,   
00134                   Dp,   
00135                   Kp,   
00136                   Mo,   
00137                   Do,   
00138                   Ko;   
00139    Matrix Ko_prime;     
00140 };
00141 
00171 class Resolved_acc {
00172 public:
00173    Resolved_acc(const short dof = 1);
00174    Resolved_acc(const Robot_basic & robot,
00175                 const Real Kvp, const Real Kpp,
00176                 const Real Kvo, const Real Kpo);
00177    void set_Kvp(const Real Kvp);
00178    void set_Kpp(const Real Kpp);
00179    void set_Kvo(const Real Kvo);
00180    void set_Kpo(const Real Kpo);
00181 
00182    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
00183                            const ColumnVector & pdp, const ColumnVector & pd,
00184                            const ColumnVector & wdp, const ColumnVector & wd,
00185                            const Quaternion & qd, const short link_pc,
00186                            const Real dt);
00187 private:
00188    double Kvp,                
00189           Kpp, 
00190           Kvo, 
00191           Kpo; 
00192    Matrix Rot;                
00193    ColumnVector zero3,        
00194                 qp,           
00195                 qpp,          
00196                 a,            
00197                 p,            
00198                 pp,           
00199                 quat_v_error; 
00200    Quaternion quat;           
00201 };
00202 
00203 
00220 class Computed_torque_method {
00221 public:
00222    Computed_torque_method(const short dof = 1);
00223    Computed_torque_method(const Robot_basic & robot,
00224                           const DiagonalMatrix & Kp, const DiagonalMatrix & Kd);
00225    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00226                            const ColumnVector & qpd,
00227                            const ColumnVector & qppd);
00228    short set_Kd(const DiagonalMatrix & Kd);
00229    short set_Kp(const DiagonalMatrix & Kp);
00230 
00231 private:
00232    int dof;            
00233    ColumnVector q,     
00234                 qp,    
00235                 qpp,   
00236                 zero3; 
00237    DiagonalMatrix Kp,  
00238                   Kd;  
00239 };
00240 
00241 
00252 class Proportional_Derivative {
00253 public:
00254    Proportional_Derivative(const short dof = 1);
00255    Proportional_Derivative(const Robot_basic & robot, const DiagonalMatrix & Kp, 
00256                            const DiagonalMatrix & Kd);
00257    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00258                            const ColumnVector & qpd);
00259    short set_Kd(const DiagonalMatrix & Kd);
00260    short set_Kp(const DiagonalMatrix & Kp);
00261 
00262 private:
00263    int dof;            
00264    ColumnVector q,     
00265                 qp,    
00266                 qpp,   
00267                 tau,   
00268                 zero3; 
00269    DiagonalMatrix Kp,  
00270                   Kd;  
00271 };
00272 
00273 #ifdef use_namespace
00274 }
00275 #endif
00276 
00277 #endif
00278 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32