controller.h

Go to the documentation of this file.

/*
Copyright (C) 2002-2004  Etienne Lachance

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: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca

-------------------------------------------------------------------------------
Revision_history:

2004/07/13: Ethan Tira-Thompson
    -Added support for newmat's use_namespace #define, using ROBOOP namespace

2005/06/10: Etienne Lachance
    -The desired joint acceleration was missing in the computed torque method.

2005/11/06: Etienne Lachance
    - No need to provide a copy constructor and the assignment operator 
      (operator=) for Impedance, Resolved_acc, Computed_torque_method and
      Proportional_Derivative classes. Instead we use the one provide by the
      compiler.
-------------------------------------------------------------------------------
*/

#ifndef CONTROLLER_H
#define CONTROLLER_H


static const char header_controller_rcsid[] = "$Id: controller.h,v 1.5 2006/05/16 16:11:15 gourdeau Exp $";


#include "robot.h"
#include "quaternion.h"

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

#define WRONG_SIZE -1

class Impedance{
public:
   Impedance();
   Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
             const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
             const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_, 
             const DiagonalMatrix & Ko_);
   short set_Mp(const DiagonalMatrix & Mp_);
   short set_Mp(const Real MP_i, const short i);
   short set_Dp(const DiagonalMatrix & Dp_);
   short set_Dp(const Real Dp_i, const short i);
   short set_Kp(const DiagonalMatrix & Kp_);
   short set_Kp(const Real Kp_i, const short i);
   short set_Mo(const DiagonalMatrix & Mo_);
   short set_Mo(const Real Mo_i, const short i);
   short set_Do(const DiagonalMatrix & Do_);
   short set_Do(const Real Do_i, const short i);
   short set_Ko(const DiagonalMatrix & Ko_);
   short set_Ko(const Real Ko_i, const short i);
   short control(const ColumnVector & pdpp, const ColumnVector & pdp,
                 const ColumnVector & pd, const ColumnVector & wdp,
                 const ColumnVector & wd, const Quaternion & qd,
                 const ColumnVector & f, const ColumnVector & n,
                 const Real dt);

   Quaternion qc,          
              qcp,         
              qcp_prev,    
              qcd,         
             quat;         
   ColumnVector pc,        
                pcp,       
                pcpp,      
                pcp_prev,  
                pcpp_prev, 
                pcd,       
                pcdp,      
                wc,        
                wcp,       
                wcp_prev,  
                wcd;       
private:
   DiagonalMatrix Mp,   
                  Dp,   
                  Kp,   
                  Mo,   
                  Do,   
                  Ko;   
   Matrix Ko_prime;     
};

class Resolved_acc {
public:
   Resolved_acc(const short dof = 1);
   Resolved_acc(const Robot_basic & robot,
                const Real Kvp, const Real Kpp,
                const Real Kvo, const Real Kpo);
   void set_Kvp(const Real Kvp);
   void set_Kpp(const Real Kpp);
   void set_Kvo(const Real Kvo);
   void set_Kpo(const Real Kpo);

   ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
                           const ColumnVector & pdp, const ColumnVector & pd,
                           const ColumnVector & wdp, const ColumnVector & wd,
                           const Quaternion & qd, const short link_pc,
                           const Real dt);
private:
   double Kvp,                
          Kpp, 
          Kvo, 
          Kpo; 
   Matrix Rot;                
   ColumnVector zero3,        
                qp,           
                qpp,          
                a,            
                p,            
                pp,           
                quat_v_error; 
   Quaternion quat;           
};


class Computed_torque_method {
public:
   Computed_torque_method(const short dof = 1);
   Computed_torque_method(const Robot_basic & robot,
                          const DiagonalMatrix & Kp, const DiagonalMatrix & Kd);
   ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
                           const ColumnVector & qpd,
                           const ColumnVector & qppd);
   short set_Kd(const DiagonalMatrix & Kd);
   short set_Kp(const DiagonalMatrix & Kp);

private:
   int dof;            
   ColumnVector q,     
                qp,    
                qpp,   
                zero3; 
   DiagonalMatrix Kp,  
                  Kd;  
};


class Proportional_Derivative {
public:
   Proportional_Derivative(const short dof = 1);
   Proportional_Derivative(const Robot_basic & robot, const DiagonalMatrix & Kp, 
                           const DiagonalMatrix & Kd);
   ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
                           const ColumnVector & qpd);
   short set_Kd(const DiagonalMatrix & Kd);
   short set_Kp(const DiagonalMatrix & Kp);

private:
   int dof;            
   ColumnVector q,     
                qp,    
                qpp,   
                tau,   
                zero3; 
   DiagonalMatrix Kp,  
                  Kd;  
};

#ifdef use_namespace
}
#endif

#endif