ROBOOP, A Robotics Object Oriented Package in C++
clik.h
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 Reference:
24 
25 CLIK: closed loop inverse kinematics
26 
27 [1] S. Chiaverini, B. Siciliano, "The Unit Quaternion: A Useful Tool for
28  Inverse Kinematics of Robot Manipulators", Systems Analysis, Modelling
29  and Simulation, vol. 35, pp.45-60, 1999.
30 
31 [2] F. Caccavale, S. Chiaverini, B. Siciliano, "Second-Order Kinematic Control
32  of Robot Manipulators with Jacobian Damped Least-Squares Inverse: Theory
33  and Experiments", IEEE/ASME Trans on Mechatronics, vol 2, no. 3,
34  pp. 188-194, 1997.
35 
36 [3] S. Chiaverini, B. Siciliano, "Review of the Damped Least-Squares Inverse
37  Kinematics with Experiments on an Industrial Robot Manipulator" IEEE Trans
38  on Control Systems Technology, vol 2, no 2, june 1994
39 
40 [4] C. Natale, "Quaternion-Based Representation of Rigid Bodies Orientation",
41  PRISMA LAB, PRISMA Technical Report no. 97-05, Oct 1997.
42 
43 The algorithme is based on [1], which is of first order.
44 
45 -------------------------------------------------------------------------------
46 Revision_history:
47 
48 2004/07/01: Etienne Lachance
49  -Added doxygen documentation.
50 
51 2004/07/01: Ethan Tira-Thompson
52  -Added support for newmat's use_namespace #define, using ROBOOP namespace
53 
54 2006/01/21: Etienne Lachance
55  -No need to include quaternion.h.
56 */
57 
58 
59 #ifndef CLIK_H
60 #define CLIK_H
61 
67 #include "robot.h"
68 
69 #ifdef use_namespace
70 namespace ROBOOP {
71  using namespace NEWMAT;
72 #endif
73 
74 
76 #define CLICK_DH 1
77 
78 #define CLICK_mDH 2
79 
80 #define CLICK_mDH_min_para 3
81 
82 
84 class Clik {
85 public:
86  Clik(){}
87  Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
88  const Real eps_=0.04, const Real lambda_max_=0.04, const Real dt=1.0);
89  Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
90  const Real eps_=0.04, const Real lambda_max_=0.04, const Real dt=1.0);
91  Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
92  const DiagonalMatrix & Ko_, const Real eps_=0.04, const Real lambda_max_=0.04,
93  const Real dt=1.0);
94  Clik(const Clik & x);
95  ~Clik(){}
96  Clik & operator=(const Clik & x);
97  void q_qdot(const Quaternion & qd, const ColumnVector & pd,
98  const ColumnVector & pddot, const ColumnVector & wd,
99  ColumnVector & q, ColumnVector & qp);
100 private:
101  int endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & pddot,
102  const Quaternion & qd, const ColumnVector & wd);
103 
104  Real
105  dt,
106  eps,
107  lambda_max;
108  short robot_type;
109  Robot robot;
110  mRobot mrobot;
111  mRobot_min_para mrobot_min_para;
112  DiagonalMatrix Kp,
113  Ko;
114 
115  ColumnVector q ,
116  qp,
117  qp_prev,
118  Kpep,
119  Koe0Quat,
120  v;
121 };
122 
123 #ifdef use_namespace
124 }
125 #endif
126 
127 #endif