Homepage Demos Overview Downloads Tutorials Reference
Credits

clik.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 Reference:
00024 
00025 CLIK: closed loop inverse kinematics
00026 
00027 [1] S. Chiaverini, B. Siciliano, "The Unit Quaternion: A Useful Tool for
00028     Inverse Kinematics of Robot Manipulators", Systems Analysis, Modelling
00029     and Simulation, vol. 35, pp.45-60, 1999.
00030 
00031 [2] F. Caccavale, S. Chiaverini, B. Siciliano, "Second-Order Kinematic Control 
00032     of Robot Manipulators with Jacobian Damped Least-Squares Inverse: Theory
00033     and Experiments", IEEE/ASME Trans on Mechatronics, vol 2, no. 3, 
00034     pp. 188-194, 1997.
00035 
00036 [3] S. Chiaverini, B. Siciliano, "Review of the Damped Least-Squares Inverse 
00037     Kinematics with Experiments on an Industrial Robot Manipulator" IEEE Trans
00038     on Control Systems Technology, vol 2, no 2, june 1994
00039 
00040 [4] C. Natale, "Quaternion-Based Representation of Rigid Bodies Orientation",
00041     PRISMA LAB, PRISMA Technical Report no. 97-05, Oct 1997.
00042 
00043 The algorithme is based on [1], which is of first order.     
00044 
00045 -------------------------------------------------------------------------------
00046 Revision_history:
00047 
00048 2004/07/01: Etienne Lachance
00049    -Added doxygen documentation.
00050 
00051 2004/07/01: Ethan Tira-Thompson
00052     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00053 */
00054 
00055 
00056 #ifndef CLIK_H
00057 #define CLIK_H
00058 
00059 /*!
00060   @file clik.h
00061   @brief Header file for Clik class definitions.
00062 */
00063 
00064 //! @brief RCS/CVS version.
00065 static const char header_clik_rcsid[] = "$Id: clik.h,v 1.4 2004/07/14 02:32:11 ejt Exp $";
00066 
00067 #include "robot.h"
00068 #include "quaternion.h"
00069 
00070 #ifdef use_namespace
00071 namespace ROBOOP {
00072   using namespace NEWMAT;
00073 #endif
00074 
00075 
00076 //! @brief Using Clik under DH notation.
00077 #define CLICK_DH           1
00078 //! @brief Using Clik under modified DH notation.
00079 #define CLICK_mDH          2
00080 //! @brief Using Clik under modified DH notation with minimum intertial parameters
00081 #define CLICK_mDH_min_para 3
00082 
00083 
00084 //! @brief Handle Closed Loop Inverse Kinematics scheme.
00085 class Clik {
00086 public:
00087    Clik(){}
00088    Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00089         const Real eps_=0.04, const Real lambda_max_=0.04, const Real dt=1.0);
00090    Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00091         const Real eps_=0.04, const Real lambda_max_=0.04, const Real dt=1.0);
00092    Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
00093         const DiagonalMatrix & Ko_, const Real eps_=0.04, const Real lambda_max_=0.04,
00094         const Real dt=1.0);
00095    Clik(const Clik & x);
00096    ~Clik(){}
00097    Clik & operator=(const Clik & x);
00098    void q_qdot(const Quaternion & qd, const ColumnVector & pd,
00099                const ColumnVector & pddot, const ColumnVector & wd,
00100                ColumnVector & q, ColumnVector & qp);
00101 private:
00102    int endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & pddot,
00103                           const Quaternion & qd, const ColumnVector & wd);
00104 
00105    Real 
00106      dt,                //!< Time frame.
00107      eps,               //!< Range of singular region in Jacobian DLS inverse.
00108      lambda_max;        //!< Damping factor in Jacobian DLS inverse.
00109    short  robot_type;   //!< Robot type used.
00110    Robot robot;         //!< Robot instance.
00111    mRobot mrobot;       //!< mRobot instance.
00112    mRobot_min_para mrobot_min_para; //!< mRobot_min_para instance.
00113    DiagonalMatrix Kp,   //!< Position error gain.
00114                   Ko;   //!< Orientation error gain.
00115 
00116    ColumnVector q ,       //!< Clik joint position.
00117                 qp,       //!< Clik joint velocity.
00118                 qp_prev,  //!< Clik previous joint velocity.
00119                 Kpep,     //!< Kp times position error.
00120                 Koe0Quat, //!< Ko times orientation error (quaternion vector part).
00121                 v;        //!< Quaternion vector part.
00122 };
00123 
00124 #ifdef use_namespace
00125 }
00126 #endif
00127 
00128 #endif

ROBOOP v1.21a
Generated Tue Nov 23 16:35:50 2004 by Doxygen 1.3.9.1