00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "ik.h"
00015
00016 #ifdef MEASURE_TIME
00017 #include <time.h>
00018 double calc_jacobian_time = 0;
00019 double solve_ik_time = 0;
00020 double high_constraint_time = 0;
00021 double low_constraint_time = 0;
00022 #endif
00023
00024 double IK::Update(double timestep)
00025 {
00026 double ret;
00027 CalcPosition();
00028 #ifdef MEASURE_TIME
00029 clock_t t1 = clock();
00030 #endif
00031 calc_jacobian();
00032 calc_feedback();
00033 #ifdef MEASURE_TIME
00034 clock_t t2 = clock();
00035 calc_jacobian_time += (double)(t2 - t1) / (double)CLOCKS_PER_SEC;
00036 #endif
00037 ret = solve_ik();
00038 #ifdef MEASURE_TIME
00039 solve_ik_time += (double)(clock() - t2) / (double)CLOCKS_PER_SEC;
00040 #endif
00041 Integrate(timestep);
00042 CalcPosition();
00043 return ret;
00044 }
00045
00046 double IK::Update(double max_timestep, double min_timestep, double max_integ_error)
00047 {
00048 double ret;
00049 double new_timestep = max_timestep;
00050 for(int i=0; i<2; i++)
00051 {
00052 CalcPosition();
00053 calc_jacobian();
00054 calc_feedback();
00055 ret = solve_ik();
00056 IntegrateAdaptive(new_timestep, i, min_timestep, max_integ_error);
00057 }
00058
00059 CalcPosition();
00060 return ret;
00061 }
00062
00063
00064
00065
00066 int IK::calc_jacobian()
00067 {
00068 int i;
00069 for(i=0; i<n_constraints; i++)
00070 {
00071 constraints[i]->calc_jacobian();
00072 }
00073 return 0;
00074 }
00075
00076 int IKConstraint::calc_jacobian()
00077 {
00078 J.resize(n_const, ik->NumDOF());
00079 J.zero();
00080 calc_jacobian(ik->Root());
00081 return 0;
00082 }
00083
00084 int IKConstraint::calc_jacobian(Joint* cur)
00085 {
00086 if(!cur) return 0;
00087 switch(cur->j_type)
00088 {
00089 case JROTATE:
00090 calc_jacobian_rotate(cur);
00091 break;
00092 case JSLIDE:
00093 calc_jacobian_slide(cur);
00094 break;
00095 case JSPHERE:
00096 calc_jacobian_sphere(cur);
00097 break;
00098 case JFREE:
00099 calc_jacobian_free(cur);
00100 break;
00101 default:
00102 break;
00103 }
00104 calc_jacobian(cur->child);
00105 calc_jacobian(cur->brother);
00106 return 0;
00107 }
00108
00109
00110
00111
00112 int IK::calc_feedback()
00113 {
00114 int i;
00115 for(i=0; i<n_constraints; i++)
00116 {
00117 constraints[i]->calc_feedback();
00118 }
00119 return 0;
00120 }
00121
00122
00123
00124
00125 int IK::copy_jacobian()
00126 {
00127 int i, m;
00128 for(m=0; m<N_PRIORITY_TYPES; m++)
00129 n_const[m] = 0;
00130 n_all_const = 0;
00131
00132 for(i=0; i<n_constraints; i++)
00133 {
00134 constraints[i]->count_constraints();
00135 }
00136 if(n_all_const == 0) return 0;
00137
00138 for(m=0; m<N_PRIORITY_TYPES; m++)
00139 {
00140 if(n_const[m] > 0)
00141 {
00142 J[m].resize(n_const[m], n_dof);
00143 J[m].zero();
00144 fb[m].resize(n_const[m]);
00145 fb[m].zero();
00146 weight[m].resize(n_const[m]);
00147 weight[m] = 1.0;
00148 }
00149 }
00150
00151
00152 for(i=0; i<n_constraints; i++)
00153 constraints[i]->copy_jacobian();
00154 return 0;
00155 }
00156
00157 double IK::solve_ik()
00158 {
00159 int i, j;
00160 double current_max_condnum = -1.0;
00161 copy_jacobian();
00162 if(n_all_const > 0)
00163 {
00165
00166 int n_high_const;
00167 fMat Jhigh;
00168 fMat wJhigh;
00169 fVec fb_high;
00170 fVec weight_high;
00171 int* is_high_const = 0;
00172 double cond_number = 1.0;
00173
00174
00175 if(n_const[HIGH_IF_POSSIBLE] > 0)
00176 {
00177 is_high_const = new int [n_const[HIGH_IF_POSSIBLE]];
00178
00179 for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
00180 is_high_const[i] = true;
00181
00182 int search_phase = 0;
00183 while(1)
00184 {
00185 n_high_const = n_const[HIGH_PRIORITY];
00186 for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
00187 {
00188 if(is_high_const[i]) n_high_const++;
00189 }
00190 Jhigh.resize(n_high_const, n_dof);
00191 wJhigh.resize(n_high_const, n_dof);
00192 fb_high.resize(n_high_const);
00193 weight_high.resize(n_high_const);
00194 if(n_const[HIGH_PRIORITY] > 0)
00195 {
00196
00197 for(i=0; i<n_const[HIGH_PRIORITY]; i++)
00198 {
00199 fb_high(i) = fb[HIGH_PRIORITY](i);
00200 weight_high(i) = weight[HIGH_PRIORITY](i);
00201 for(j=0; j<n_dof; j++)
00202 {
00203 Jhigh(i, j) = J[HIGH_PRIORITY](i, j);
00204 wJhigh(i, j) = Jhigh(i, j) * weight_high(i) / joint_weights(j);
00205 }
00206 }
00207 }
00208 int count = 0;
00209
00210 for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
00211 {
00212 if(is_high_const[i])
00213 {
00214 fb_high(n_const[HIGH_PRIORITY]+count) = fb[HIGH_IF_POSSIBLE](i);
00215 weight_high(n_const[HIGH_PRIORITY]+count) = weight[HIGH_IF_POSSIBLE](i);
00216 for(j=0; j<n_dof; j++)
00217 {
00218 Jhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j);
00219 wJhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j) * weight[HIGH_IF_POSSIBLE](i) / joint_weights(j);
00220 }
00221 count++;
00222 }
00223 }
00224
00225 fMat U(n_high_const, n_high_const), VT(n_dof, n_dof);
00226 fVec s;
00227 int s_size;
00228 if(n_high_const < n_dof) s_size = n_high_const;
00229 else s_size = n_dof;
00230 s.resize(s_size);
00231 wJhigh.svd(U, s, VT);
00232 double condnum_limit = max_condnum * 100.0;
00233 if(s(s_size-1) > s(0)/(max_condnum*condnum_limit))
00234 cond_number = s(0) / s(s_size-1);
00235 else
00236 cond_number = condnum_limit;
00237 if(current_max_condnum < 0.0 || cond_number > current_max_condnum)
00238 {
00239 current_max_condnum = cond_number;
00240 }
00241 if(n_high_const <= n_const[HIGH_PRIORITY]) break;
00242
00243 if(cond_number > max_condnum)
00244 {
00245 int reduced = false;
00246 for(i=n_constraints-1; i>=0; i--)
00247 {
00248 if(constraints[i]->enabled &&
00249 constraints[i]->priority == HIGH_IF_POSSIBLE &&
00250 constraints[i]->i_const >= 0 &&
00251 constraints[i]->GetType() == HANDLE_CONSTRAINT &&
00252 is_high_const[constraints[i]->i_const])
00253 {
00254 IKHandle* h = (IKHandle*)constraints[i];
00255 if(search_phase ||
00256 (!search_phase && h->joint->DescendantDOF() > 0))
00257 {
00258 for(j=0; j<constraints[i]->n_const; j++)
00259 {
00260 is_high_const[constraints[i]->i_const + j] = false;
00261 }
00262 constraints[i]->is_dropped = true;
00263
00264 reduced = true;
00265 break;
00266 }
00267 }
00268 }
00269 if(!reduced) search_phase++;
00270 }
00271 else break;
00272 }
00273 }
00274 else
00275 {
00276 n_high_const = n_const[HIGH_PRIORITY];
00277 Jhigh.resize(n_high_const, n_dof);
00278 wJhigh.resize(n_high_const, n_dof);
00279 fb_high.resize(n_high_const);
00280 weight_high.resize(n_high_const);
00281 if(n_high_const > 0)
00282 {
00283 Jhigh.set(J[HIGH_PRIORITY]);
00284 fb_high.set(fb[HIGH_PRIORITY]);
00285 weight_high.set(weight[HIGH_PRIORITY]);
00286 }
00287 }
00288 #if 0
00289
00290
00291 if(current_max_condnum > max_condnum)
00292 fb_high.zero();
00293 else
00294 {
00295 double k = (current_max_condnum-max_condnum)/(1.0-max_condnum);
00296 fb_high *= k;
00297 cerr << current_max_condnum << ", " << k << endl;
00298 }
00301 if(current_max_condnum < 0.0) current_max_condnum = 1.0;
00302 #endif
00303 int n_low_const = n_all_const - n_high_const;
00304 int low_first = 0, count = 0;
00305 fMat Jlow(n_low_const, n_dof);
00306 fVec fb_low(n_low_const);
00307 fVec weight_low(n_low_const);
00308 for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
00309 {
00310 if(!is_high_const[i])
00311 {
00312 fb_low(count) = fb[HIGH_IF_POSSIBLE](i);
00313 weight_low(count) = weight[HIGH_IF_POSSIBLE](i);
00314 for(j=0; j<n_dof; j++)
00315 {
00316 Jlow(count, j) = J[HIGH_IF_POSSIBLE](i, j);
00317 }
00318 count++;
00319 }
00320 }
00321 low_first = count;
00322 double* p = fb_low.data() + low_first;
00323 double* q = fb[LOW_PRIORITY].data();
00324 double* r = weight_low.data() + low_first;
00325 double* s = weight[LOW_PRIORITY].data();
00326 for(i=0; i<n_const[LOW_PRIORITY]; p++, q++, r++, s++, i++)
00327 {
00328
00329 *p = *q;
00330 *r = *s;
00331 double* a = Jlow.data() + low_first + i;
00332 int a_row = Jlow.row();
00333 double* b = J[LOW_PRIORITY].data() + i;
00334 int b_row = J[LOW_PRIORITY].row();
00335 for(j=0; j<n_dof; a+=a_row, b+=b_row, j++)
00336 {
00337
00338 *a = *b;
00339 }
00340 }
00341 if(is_high_const) delete[] is_high_const;
00342
00343 fVec jvel(n_dof);
00344 fVec jvel0(n_dof);
00345 fVec fb_low_0(n_low_const), dfb(n_low_const), y(n_dof);
00346 fMat Jinv(n_dof, n_high_const), W(n_dof, n_dof), JW(n_low_const, n_dof);
00347 fVec w_error(n_low_const), w_norm(n_dof);
00348
00349 double damping = 0.1;
00350
00351 w_error.set(weight_low);
00352 w_norm = 1.0;
00353
00354
00355
00356
00357 #ifdef MEASURE_TIME
00358 clock_t t1 = clock();
00359 #endif
00360
00361 if(n_high_const > 0)
00362 {
00363 for(i=0; i<n_dof; i++)
00364 {
00365 int a_row = wJhigh.row();
00366 double* a = wJhigh.data() + a_row*i;
00367 int b_row = Jhigh.row();
00368 double* b = Jhigh.data() + b_row*i;
00369 double* c = joint_weights.data() + i;
00370 double* d = weight_high.data();
00371 for(j=0; j<n_high_const; a++, b++, d++, j++)
00372 {
00373
00374 *a = *b * *d / *c;
00375 }
00376 }
00377 fVec w_fb_high(fb_high);
00378 for(i=0; i<n_high_const; i++)
00379 w_fb_high(i) *= weight_high(i);
00380 Jinv.p_inv(wJhigh);
00381 jvel0.mul(Jinv, w_fb_high);
00382 W.mul(Jinv, wJhigh);
00383 for(i=0; i<n_dof; i++)
00384 {
00385 double* w = W.data() + i;
00386 double a = joint_weights(i);
00387 for(j=0; j<n_dof; w+=n_dof, j++)
00388 {
00389 if(i==j) *w -= 1.0;
00390 *w /= -a;
00391 }
00392 jvel0(i) /= a;
00393 }
00394 JW.mul(Jlow, W);
00395 }
00396 else
00397 {
00398 jvel0.zero();
00399 JW.set(Jlow);
00400 }
00401 #ifdef MEASURE_TIME
00402 clock_t t2 = clock();
00403 high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC;
00404 #endif
00405
00406 if(n_low_const > 0)
00407 {
00408 fb_low_0.mul(Jlow, jvel0);
00409 dfb.sub(fb_low, fb_low_0);
00410 y.lineq_sr(JW, w_error, w_norm, damping, dfb);
00411 if(n_high_const > 0) jvel.mul(W, y);
00412 else jvel.set(y);
00413
00414
00415
00416 }
00417 else
00418 {
00419 jvel.zero();
00420 }
00421
00422 jvel += jvel0;
00423 #ifdef MEASURE_TIME
00424 clock_t t3 = clock();
00425 low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC;
00426 #endif
00427 SetJointVel(jvel);
00428
00429 }
00430 if(current_max_condnum < 0.0) current_max_condnum = 1.0;
00431 return current_max_condnum;
00432 }
00433
00434 int IKConstraint::count_constraints()
00435 {
00436 i_const = -1;
00437 if(enabled)
00438 {
00439 i_const = ik->n_const[priority];
00440 ik->n_const[priority] += n_const;
00441 ik->n_all_const += n_const;
00442 }
00443 return 0;
00444 }
00445
00446 int IKConstraint::copy_jacobian()
00447 {
00448 if(i_const < 0) return -1;
00449 int n_dof = ik->NumDOF();
00450 int i, j, m;
00451 is_dropped = false;
00452 for(m=0; m<IK::N_PRIORITY_TYPES; m++)
00453 {
00454 if(priority == (IK::Priority)m && enabled)
00455 {
00456 fMat& targetJ = ik->J[m];
00457 int target_row = targetJ.row();
00458 int row = J.row();
00459 double *a, *b;
00460 for(i=0; i<n_const; i++)
00461 {
00462 ik->fb[m](i_const+i) = fb(i);
00463 if(weight.size() == n_const)
00464 ik->weight[m](i_const+i) = weight(i);
00465 else
00466 ik->weight[m](i_const+i) = 1.0;
00467 a = targetJ.data() + i_const + i;
00468 b = J.data() + i;
00469 for(j=0; j<n_dof; a+=target_row, b+=row, j++)
00470 {
00471
00472 *a = *b;
00473 }
00474 }
00475 break;
00476 }
00477 }
00478 return 0;
00479 }
00480
00481