update_ik.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * update.cpp
00011  * Create: Katsu Yamane, 03.07.10
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 //      cerr << new_timestep << endl;
00059         CalcPosition();
00060         return ret;
00061 }
00062 
00063 /*
00064  * calc_jacobian
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  * calc_feedback
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  * solve_ik
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         // count number of constraints
00132         for(i=0; i<n_constraints; i++)
00133         {
00134                 constraints[i]->count_constraints();
00135         }
00136         if(n_all_const == 0) return 0;
00137         // ¹ÔÎë!E¥Ù¥¯¥È¥ë¤ê¹à 
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         // ³Æ¹´Áå"Ëá§ãR¥Ó¥¢¥ó¡¦¥Õ¥£¡¼¥É¥Ð¥Ã¥¯ÁæÁ٤깸¤â"
00151         // Âå"¡¦ÇësÎë!E¥Ù¥¯¥È¥ë£õ"¡¦
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                 // check rank when HIGH_IF_POSSIBLE constraints have high priority
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 //              cerr << "---" << endl;
00174 //              cerr << n_const[HIGH_PRIORITY] << " " << n_const[HIGH_IF_POSSIBLE] << " " << n_const[LOW_PRIORITY] << endl;
00175                 if(n_const[HIGH_IF_POSSIBLE] > 0)
00176                 {
00177                         is_high_const = new int [n_const[HIGH_IF_POSSIBLE]];
00178                         // initialize
00179                         for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
00180                                 is_high_const[i] = true;
00181                         // search
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                                         // set fb and J of higher priority pins
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                                 // set fb and J of medium priority pins
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                                 // singular value decomposition
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                                 // remove some constraints
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 //                                                              cerr << "r" << flush;
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                 // adjust feedback according to the condition number
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 //                      fb_low(low_first+i) = fb[LOW_PRIORITY](i);
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 //                              Jlow(low_first+i, j) = J[LOW_PRIORITY](i, j);
00338                                 *a = *b;
00339                         }
00340                 }
00341                 if(is_high_const) delete[] is_high_const;
00342 
00343                 fVec jvel(n_dof);   // ³û¼â1¡¦x
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                 // weighted
00349                 double damping = 0.1;
00350 //              w_error = 1.0;
00351                 w_error.set(weight_low);
00352                 w_norm = 1.0;
00353 //              w_norm.set(joint_weights);
00354 //              cerr << "joint_weights = " << joint_weights << endl;
00355 //              cerr << "weight_high = " << weight_high << endl;
00356 //              cerr << "weight_low = " << weight_low << endl;
00357 #ifdef MEASURE_TIME
00358                 clock_t t1 = clock();
00359 #endif
00360                 // ¹ó/¡¦¾å¡¦€ÅæéòÉçïàïêvZ
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 //                                      wJhigh(j, i) = Jhigh(j, i) / joint_weights(i);
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                 // Ǥ¡¦#x¥¯¥È¥ë¹à¤ê³×Z
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 //                      fVec error(n_low_const);
00414 //                      error = dfb-Jlow*jvel;
00415 //                      cerr << dfb*dfb << "->" << error*error << endl;
00416                 }
00417                 else
00418                 {
00419                         jvel.zero();
00420                 }
00421                 // ³û¼â1¡¦x
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 //              cerr << fb_high - Jhigh * jvel << endl;
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 //                                      targetJ(i_const+i, j) = J(i, j);
00472                                         *a = *b;
00473                                 }
00474                         }
00475                         break;
00476                 }
00477         }
00478         return 0;
00479 }
00480 
00481 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57