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