$search
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 2006/01/21: Etienne Lachance 00055 -No need to include quaternion.h. 00056 */ 00057 00058 00059 #ifndef CLIK_H 00060 #define CLIK_H 00061 00067 00068 static const char header_clik_rcsid[] = "$Id: clik.h,v 1.6 2006/05/16 16:11:15 gourdeau Exp $"; 00069 00070 #include "robot.h" 00071 00072 #ifdef use_namespace 00073 namespace ROBOOP { 00074 using namespace NEWMAT; 00075 #endif 00076 00077 00079 #define CLICK_DH 1 00080 00081 #define CLICK_mDH 2 00082 00083 #define CLICK_mDH_min_para 3 00084 00085 00087 class Clik { 00088 public: 00089 Clik(){} 00090 Clik(const Robot & robot_, 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 & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_, 00093 const Real eps_=0.04, const Real lambda_max_=0.04, const Real dt=1.0); 00094 Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_, 00095 const DiagonalMatrix & Ko_, const Real eps_=0.04, const Real lambda_max_=0.04, 00096 const Real dt=1.0); 00097 Clik(const Clik & x); 00098 ~Clik(){} 00099 Clik & operator=(const Clik & x); 00100 void q_qdot(const Quaternion & qd, const ColumnVector & pd, 00101 const ColumnVector & pddot, const ColumnVector & wd, 00102 ColumnVector & q, ColumnVector & qp); 00103 private: 00104 int endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & pddot, 00105 const Quaternion & qd, const ColumnVector & wd); 00106 00107 Real 00108 dt, 00109 eps, 00110 lambda_max; 00111 short robot_type; 00112 Robot robot; 00113 mRobot mrobot; 00114 mRobot_min_para mrobot_min_para; 00115 DiagonalMatrix Kp, 00116 Ko; 00117 00118 ColumnVector q , 00119 qp, 00120 qp_prev, 00121 Kpep, 00122 Koe0Quat, 00123 v; 00124 }; 00125 00126 #ifdef use_namespace 00127 } 00128 #endif 00129 00130 #endif