00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 static const char rcsid[] = "$Id: clik.cpp,v 1.5 2004/09/26 19:55:35 ejt Exp $";
00034
00035
00036
00037
00038
00039
00040 #include "clik.h"
00041
00042 #ifdef use_namespace
00043 namespace ROBOOP {
00044 using namespace NEWMAT;
00045 #endif
00046
00047
00048
00049
00050
00051
00052 Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00053 const Real eps_, const Real lambda_max_, const Real dt_):
00054 dt(dt_),
00055 eps(eps_),
00056 lambda_max(lambda_max_),
00057 robot(robot_)
00058 {
00059 robot_type = CLICK_DH;
00060
00061 q = robot.get_q();
00062 qp = robot.get_qp();
00063 qp_prev = qp;
00064 Kpep = ColumnVector(3); Kpep = 0;
00065 Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00066 v = ColumnVector(6); v = 0;
00067
00068 if(Kp_.Nrows()==3)
00069 Kp = Kp_;
00070 else
00071 {
00072 Kp = DiagonalMatrix(3); Kp = 0.0;
00073 cerr << "Clik::Clik-->Robot, Kp if not 3x3, set gain to 0." << endl;
00074 }
00075 if(Ko_.Nrows()==3)
00076 Ko = Ko_;
00077 else
00078 {
00079 Ko = DiagonalMatrix(3); Ko = 0.0;
00080 cerr << "Clik::Clik-->Robot, Ko if not 3x3, set gain to 0." << endl;
00081 }
00082 }
00083
00084
00085
00086
00087
00088
00089
00090 Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00091 const Real eps_, const Real lambda_max_, const Real dt_):
00092 dt(dt_),
00093 eps(eps_),
00094 lambda_max(lambda_max_),
00095 mrobot(mrobot_)
00096 {
00097 robot_type = CLICK_mDH;
00098
00099 q = mrobot.get_q();
00100 qp = mrobot.get_qp();
00101 qp_prev = qp;
00102 Kpep = ColumnVector(3); Kpep = 0;
00103 Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00104 v = ColumnVector(6); v = 0;
00105
00106 if(Kp_.Nrows()==3)
00107 Kp = Kp_;
00108 else
00109 {
00110 Kp = DiagonalMatrix(3); Kp = 0.0;
00111 cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
00112 }
00113 if(Ko_.Nrows()==3)
00114 Ko = Ko_;
00115 else
00116 {
00117 Ko = DiagonalMatrix(3); Ko = 0.0;
00118 cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
00119 }
00120 }
00121
00122
00123
00124
00125
00126
00127
00128 Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
00129 const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_,
00130 const Real dt_):
00131 dt(dt_),
00132 eps(eps_),
00133 lambda_max(lambda_max_),
00134 mrobot_min_para(mrobot_min_para_)
00135 {
00136 robot_type = CLICK_mDH_min_para;
00137
00138 q = mrobot_min_para.get_q();
00139 qp = mrobot_min_para.get_qp();
00140 qp_prev = qp;
00141 Kpep = ColumnVector(3); Kpep = 0;
00142 Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00143 v = ColumnVector(6); v = 0;
00144
00145 if(Kp_.Nrows()==3)
00146 Kp = Kp_;
00147 else
00148 {
00149 Kp = DiagonalMatrix(3); Kp = 0.0;
00150 cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
00151 }
00152 if(Ko_.Nrows()==3)
00153 Ko = Ko_;
00154 else
00155 {
00156 Ko = DiagonalMatrix(3); Ko = 0.0;
00157 cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
00158 }
00159 }
00160
00161
00162 Clik::Clik(const Clik & x)
00163
00164 {
00165 robot_type = x.robot_type;
00166 switch(robot_type)
00167 {
00168 case CLICK_DH:
00169 robot = x.robot;
00170 break;
00171 case CLICK_mDH:
00172 mrobot = x.mrobot;
00173 break;
00174 case CLICK_mDH_min_para:
00175 mrobot_min_para = x.mrobot_min_para;
00176 break;
00177 }
00178 eps = x.eps;
00179 lambda_max = x.lambda_max;
00180 dt = x.dt;
00181 q = x.q;
00182 qp = x.qp;
00183 qp_prev = x.qp_prev;
00184 Kpep = x.Kpep;
00185 Koe0Quat = x.Koe0Quat;
00186 Kp = x.Kp;
00187 Ko = x.Ko;
00188 v = x.v;
00189 }
00190
00191 Clik & Clik::operator=(const Clik & x)
00192
00193 {
00194 robot_type = x.robot_type;
00195 switch(robot_type)
00196 {
00197 case CLICK_DH:
00198 robot = x.robot;
00199 break;
00200 case CLICK_mDH:
00201 mrobot = x.mrobot;
00202 break;
00203 case CLICK_mDH_min_para:
00204 mrobot_min_para = x.mrobot_min_para;
00205 break;
00206 }
00207 eps = x.eps;
00208 lambda_max = x.lambda_max;
00209 dt = x.dt;
00210 q = x.q;
00211 qp = x.qp;
00212 qp_prev = x.qp_prev;
00213 Kpep = x.Kpep;
00214 Koe0Quat = x.Koe0Quat;
00215 Kp = x.Kp;
00216 Ko = x.Ko;
00217 v = x.v;
00218
00219 return *this;
00220 }
00221
00222
00223 int Clik::endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & ,
00224 const Quaternion & qqqd, const ColumnVector & )
00225
00226
00227
00228
00229
00230
00231
00232 {
00233 ColumnVector p;
00234 Matrix R;
00235
00236 switch(robot_type)
00237 {
00238 case CLICK_DH:
00239 robot.set_q(q);
00240 robot.kine(R, p);
00241 break;
00242 case CLICK_mDH:
00243 mrobot.set_q(q);
00244 mrobot.kine(R, p);
00245 break;
00246 case CLICK_mDH_min_para:
00247 mrobot_min_para.set_q(q);
00248 mrobot_min_para.kine(R, p);
00249 break;
00250 }
00251 Kpep = Kp*(pd - p);
00252 Quaternion qq(R);
00253
00254 Quaternion qqd;
00255
00256 if(qq.dot_prod(qqqd) < 0)
00257 qqd = qqqd*(-1);
00258 else
00259 qqd = qqqd;
00260
00261
00262 Koe0Quat = Ko*(qq.s()*qqd.v() - qqd.s()*qq.v() + x_prod_matrix(qq.v())*qqd.v());
00263
00264 return 0;
00265 }
00266
00267
00268 void Clik::q_qdot(const Quaternion & qd, const ColumnVector & pd,
00269 const ColumnVector & pdd, const ColumnVector & wd,
00270 ColumnVector & q_, ColumnVector & qp_)
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280 {
00281 v.SubMatrix(1,3,1,1) = pdd + Kpep;
00282 v.SubMatrix(4,6,1,1) = wd + Koe0Quat;
00283
00284 switch(robot_type)
00285 {
00286 case CLICK_DH:
00287 robot.set_q(q);
00288 qp = robot.jacobian_DLS_inv(eps, lambda_max)*v;
00289 break;
00290 case CLICK_mDH:
00291 mrobot.set_q(q);
00292 qp = mrobot.jacobian_DLS_inv(eps, lambda_max)*v;
00293 break;
00294 case CLICK_mDH_min_para:
00295 mrobot_min_para.set_q(q);
00296 qp = mrobot_min_para.jacobian_DLS_inv(eps, lambda_max)*v;
00297 break;
00298 }
00299
00300 q = q + Integ_Trap(qp, qp_prev, dt);
00301 endeff_pos_ori_err(pd, pdd, qd, wd);
00302
00303 q_ = q;
00304 qp_ = qp;
00305 }
00306
00307 #ifdef use_namespace
00308 }
00309 #endif