update.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, 04.02.25
00012  */
00013 
00014 #include "psim.h"
00015 #include <limits>
00016 
00017 //#define PSIM_TEST
00018 
00020 
00021 #ifdef VERBOSE
00022 #include <fstream>
00023 std::ofstream update_log("update.log");
00024 #endif
00025 
00026 #ifdef TIMING_CHECK
00027 static double update_start_time = 0.0;
00028 #endif
00029 
00030 #ifdef PSIM_TEST
00031 double max_condition_number = -1.0;
00032 Joint* max_condition_number_joint = 0;
00033 double max_sigma_ratio = -1.0;
00034 Joint* max_sigma_ratio_joint = 0;
00035 fVec condition_numbers;
00036 fVec sigma_ratios;
00037 double total_gamma_error = 0.0;
00038 #endif
00039 
00040 int pSim::Update()
00041 {
00042 #ifdef VERBOSE
00043         update_log << "Update no contact" << endl;
00044 #endif
00045 #ifdef TIMING_CHECK
00046         update_start_time = MPI_Wtime();
00047 #endif
00048 #ifdef PSIM_TEST
00049         max_condition_number = -1.0;
00050         max_sigma_ratio = -1.0;
00051         max_condition_number_joint = 0;
00052         max_sigma_ratio_joint = 0;
00053         condition_numbers.resize(n_dof);
00054         sigma_ratios.resize(n_dof);
00055         condition_numbers.zero();
00056         sigma_ratios.zero();
00057         total_gamma_error = 0.0;
00058 #endif
00059         /**** assembly ****/
00060         // position-dependent variables
00061         update_position();
00062 #if 1
00063         if(do_connect)
00064         {
00065                 //
00066                 // do collision computation if needed
00067                 //
00068                 update_collision();
00069                 // don't forget to recompute link velocities
00070                 CalcVelocity();
00071                 do_connect = false;
00072         }
00073 #endif
00074         // velocity-dependent variables
00075         update_velocity();
00076 
00077 #ifdef TIMING_CHECK
00078         cerr << "[" << rank << "] disassembly t = " << MPI_Wtime()-update_start_time << endl;
00079 #endif
00080         /**** disassembly ****/
00081         disassembly();
00082         
00083 #ifdef PSIM_TEST
00084         cerr << "--- max condition number = " << max_condition_number << " at " << max_condition_number_joint->name << endl;
00085         cerr << "--- max sigma ratio = " << max_sigma_ratio << " at " << max_sigma_ratio_joint->name << endl;
00086         cerr << "--- total_gamma_error = " << total_gamma_error << endl;
00087 //      cerr << "condition_numbers = " << tran(condition_numbers) << endl;
00088 //      cerr << "sigma_ratios = " << tran(sigma_ratios) << endl;
00089 #endif
00090 #ifdef USE_MPI
00091         // scatter results
00092         scatter_acc();
00093 #endif
00094         return 0;
00095 }
00096 
00097 #ifdef USE_MPI
00098 void pSim::scatter_acc()
00099 {
00100         int i;
00101         for(i=0; i<size; i++)
00102         {
00103                 MPI_Bcast(MPI_BOTTOM, 1, all_acc_types[i], i, MPI_COMM_WORLD);
00104         }
00105         subchains->scatter_acc();
00106 }
00107 
00108 void pSubChain::scatter_acc()
00109 {
00110         if(!this) return;
00111         if(last_joint && last_joint->t_given)
00112         {
00113                 switch(last_joint->j_type)
00114                 {
00115                 case JROTATE:
00116                 case JSLIDE:
00117                         last_joint->SetJointAcc(acc_final(0));
00118 //                      cerr << last_joint->name << ": " << acc_final(0) << endl;
00119                         break;
00120                 case JSPHERE:
00121                         last_joint->SetJointAcc(acc_final(0), acc_final(1), acc_final(2));
00122 //                      cerr << last_joint->name << ": " << tran(acc_final) << endl;
00123                         break;
00124                 case JFREE:
00125                         last_joint->SetJointAcc(acc_final(0), acc_final(1), acc_final(2),
00126                                                                         acc_final(3), acc_final(4), acc_final(5));
00127 //                      cerr << last_joint->name << ": " << tran(acc_final) << endl;
00128                         break;
00129                 }
00130 #if 0
00131                 double* sendbuf;
00132                 sendbuf = last_pjoints[0]->f_final.data();
00133                 MPI_Bcast(sendbuf, 6, MPI_DOUBLE, rank, MPI_COMM_WORLD);
00134                 sendbuf = last_pjoints[1]->f_final.data();
00135                 MPI_Bcast(sendbuf, 6, MPI_DOUBLE, rank, MPI_COMM_WORLD);
00136 #endif
00137         }
00138         if(children[0]) children[0]->scatter_acc();
00139         if(children[1] && children[0] != children[1]) children[1]->scatter_acc();
00140 }
00141 #endif
00142 
00146 void pSim::update_position()
00147 {
00148         int i;
00149         for(i=0; i<n_joint; i++)
00150         {
00151 #ifdef USE_MPI
00152                 if(joint_info[i].pjoints[0]->subchain && rank == joint_info[i].pjoints[0]->subchain->rank)
00153 #endif
00154                         joint_info[i].pjoints[0]->calc_jacobian();
00155 #ifdef USE_MPI
00156                 if(joint_info[i].pjoints[1]->subchain && rank == joint_info[i].pjoints[1]->subchain->rank)
00157 #endif
00158                         joint_info[i].pjoints[1]->calc_jacobian();
00159         }
00160         // should come after computing Jacobians of all pjoints
00161         subchains->calc_inertia();
00162 }
00163 
00164 void pJoint::calc_jacobian()
00165 {
00166         if(parent_side)
00167         {
00168                 static fVec3 rel_pos;
00169                 static fMat33 rel_att;
00170                 if(link_side == joint->real)
00171                 {
00172                         rel_pos.set(joint->rpos_real);
00173                         rel_att.set(joint->ratt_real);
00174                 }
00175                 else
00176                 {
00177                         rel_pos.set(joint->rel_pos);
00178                         rel_att.set(joint->rel_att);
00179                 }
00180                 static fMat33 tcross, tmpT;
00181                 tcross.cross(rel_pos);
00182                 tmpT.mul(tcross, rel_att);
00183                 J(0,0) = rel_att(0,0), J(0,1) = rel_att(1,0), J(0,2) = rel_att(2,0);
00184                 J(1,0) = rel_att(0,1), J(1,1) = rel_att(1,1), J(1,2) = rel_att(2,1);
00185                 J(2,0) = rel_att(0,2), J(2,1) = rel_att(1,2), J(2,2) = rel_att(2,2);
00186                 J(0,3) = tmpT(0,0), J(0,4) = tmpT(1,0), J(0,5) = tmpT(2,0);
00187                 J(1,3) = tmpT(0,1), J(1,4) = tmpT(1,1), J(1,5) = tmpT(2,1);
00188                 J(2,3) = tmpT(0,2), J(2,4) = tmpT(1,2), J(2,5) = tmpT(2,2);
00189                 J(3,0) = 0.0, J(3,1) = 0.0, J(3,2) = 0.0;
00190                 J(4,0) = 0.0, J(4,1) = 0.0, J(4,2) = 0.0;
00191                 J(5,0) = 0.0, J(5,1) = 0.0, J(5,2) = 0.0;
00192                 J(3,3) = rel_att(0,0), J(3,4) = rel_att(1,0), J(3,5) = rel_att(2,0);
00193                 J(4,3) = rel_att(0,1), J(4,4) = rel_att(1,1), J(4,5) = rel_att(2,1);
00194                 J(5,3) = rel_att(0,2), J(5,4) = rel_att(1,2), J(5,5) = rel_att(2,2);
00195 //              update_log << joint->name << ": J = " << J << endl;
00196         }
00197 }
00198 
00199 void pSubChain::calc_inertia()
00200 {
00201         if(!this) return;
00202         if(!last_joint)  // single link
00203         {
00204                 calc_inertia_leaf();
00205         }
00206         else  // have last joint
00207         {
00208                 // process children first
00209                 children[0]->calc_inertia();
00210                 if(children[1] != children[0])
00211                 {
00212                         children[1]->calc_inertia();
00213                 }
00214                 calc_inertia_body();
00215         }
00216 }
00217 
00218 #ifdef USE_MPI
00219 void pSubChain::recv_inertia()
00220 {
00221         // recv from subchain's rank
00222         MPI_Status status;
00223 #if 0
00224         int i, j;
00225         for(i=0; i<n_outer_joints; i++)
00226         {
00227                 for(j=i; j<n_outer_joints; j++)
00228                 {
00229                         double* buf = Lambda[i][j].data();
00230                         MPI_Recv(buf, 36, MPI_DOUBLE, rank, PSIM_TAG_LAMBDA, MPI_COMM_WORLD, &status);
00231                         if(i != j)
00232                         {
00233                                 Lambda[j][i].tran(Lambda[i][j]);
00234                         }
00235                 }
00236         }
00237 #else
00238 #ifdef TIMING_CHECK
00239         double time1 = MPI_Wtime();
00240 #endif
00241         MPI_Recv(MPI_BOTTOM, 1, parent_lambda_type, rank, PSIM_TAG_LAMBDA, MPI_COMM_WORLD, &status);
00242 #ifdef TIMING_CHECK
00243         double time2 = MPI_Wtime();
00244         sim->inertia_wait_time += time2-time1;
00245 #endif
00246 #endif
00247 }
00248 
00249 void pSubChain::send_inertia(int dest)
00250 {
00251         // send to dest
00252 #if 0
00253         int i, j;
00254         for(i=0; i<n_outer_joints; i++)
00255         {
00256                 for(j=i; j<n_outer_joints; j++)
00257                 {
00258                         double* buf = Lambda[i][j].data();
00259                         MPI_Send(buf, 36, MPI_DOUBLE, dest, PSIM_TAG_LAMBDA, MPI_COMM_WORLD);
00260                 }
00261         }
00262 #else
00263         MPI_Send(MPI_BOTTOM, 1, parent_lambda_type, dest, PSIM_TAG_LAMBDA, MPI_COMM_WORLD);
00264 #endif
00265 }
00266 #endif
00267 
00268 void pSubChain::calc_inertia_leaf()
00269 {
00270 #ifdef USE_MPI
00271         if(sim->rank != rank) return;
00272 #endif
00273         int i, j;
00274         // Lambda
00275 //      update_log << links[0]->joint->name << ": calc_inertia_leaf" << endl;
00276         for(i=0; i<n_outer_joints; i++)
00277         {
00278                 static fMat JM(6, 6);
00279                 if(outer_joints[i]->parent_side) // link is parent side -> compute JM
00280                 {
00281                         JM.mul(outer_joints[i]->J, outer_joints[i]->plink->Minv);
00282                 }
00283                 else // link is child side -> J is identity
00284                 {
00285                         JM.set(outer_joints[i]->plink->Minv);
00286                 }
00287                 for(j=i; j<n_outer_joints; j++)  // half only
00288                 {
00289                         static fMat tJ(6, 6);
00290                         if(outer_joints[j]->parent_side)
00291                         {
00292                                 tJ.tran(outer_joints[j]->J);
00293                                 Lambda[i][j].mul(JM, tJ);
00294                         }
00295                         else
00296                         {
00297                                 Lambda[i][j].set(JM);
00298                         }
00299 //                      update_log << "Lambda[" << i << "][" << j << "] = " << Lambda[i][j] << endl;
00300                         if(i != j)  // copy transpose
00301                                 Lambda[j][i].tran(Lambda[i][j]);
00302                 }
00303         }
00304 #ifdef USE_MPI
00305         if(parent && sim->rank != parent->rank)
00306         {
00307 //              cerr << sim->rank << ": link (" << links[0]->joint->name << "): send_inertia to " << parent->rank << endl;
00308                 send_inertia(parent->rank);
00309         }
00310 #endif
00311 }
00312 
00313 void pSubChain::calc_inertia_body()
00314 {
00315         // for space
00316 //      if(!children[1]) return;
00317 #ifdef USE_MPI
00318         if(sim->rank != rank) return;
00319         if(children[0] && sim->rank != children[0]->rank)
00320         {
00321 //              cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[0]->rank << endl;
00322                 children[0]->recv_inertia();
00323         }
00324         if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
00325         {
00326 //              cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[1]->rank << endl;
00327                 children[1]->recv_inertia();
00328         }
00329 #endif
00330         int i, j;
00331 //      cerr << "---- " << last_joint->name << ": calc_inertia_body" << endl;
00332         // P
00333         if(children[1])
00334         {
00335                 P.add(children[0]->Lambda[last_index[0]][last_index[0]], children[1]->Lambda[last_index[1]][last_index[1]]);
00336         }
00337         else
00338         {
00339                 P.set(children[0]->Lambda[last_index[0]][last_index[0]]);
00340         }
00341 //      cerr << "Lambda[0] = " << children[0]->Lambda[last_index[0]][last_index[0]] << endl;
00342 //      cerr << "Lambda[1] = " << children[1]->Lambda[last_index[1]][last_index[1]] << endl;
00343 #ifdef PSIM_TEST
00344         {
00345                 fMat U1(6,6), V1T(6,6), U2(6,6), V2T(6,6);
00346                 fVec sigma1(6), sigma2(6);
00347                 children[0]->Lambda[last_index[0]][last_index[0]].svd(U1, sigma1, V1T);
00348                 children[1]->Lambda[last_index[1]][last_index[1]].svd(U2, sigma2, V2T);
00349                 double s1 = sigma1.length(), s2 = sigma2.length();
00350                 if(s1 > 1e-8 && s2 > 1e-8)
00351                 {
00352                         double ratio = (s1 < s2) ? s2/s1 : s1/s2;
00353                         if(max_sigma_ratio < 0.0 || ratio > max_sigma_ratio)
00354                         {
00355                                 max_sigma_ratio = ratio;
00356                                 max_sigma_ratio_joint = last_joint;
00357                         }
00358                         sigma_ratios(last_joint->i_dof) =  ratio;
00359 //                      cerr << last_joint->name << ": " << s1 << ", " << s2 << " -> " << ratio << endl;
00360                 }
00361         }
00362 #endif
00363         if(children[0] == children[1])
00364         {
00365                 P -= children[0]->Lambda[last_index[0]][last_index[1]];
00366                 P -= children[0]->Lambda[last_index[1]][last_index[0]];
00367         }
00368         // P should be symmetric
00369 //      P += tran(P);
00370 //      P *= 0.5;
00371         P.symmetric();
00372 #ifndef USE_DCA
00373         // Gamma
00374         if(n_const > 0)
00375         {
00376                 for(i=0; i<n_const; i++)
00377                 {
00378                         for(j=0; j<n_const; j++)
00379                                 Gamma(i, j) = P(const_index[i], const_index[j]);
00380                 }
00381                 if(children[0] != children[1])
00382                 {
00383                         // Gamma is symmetric, positive-definite
00384                         if(Gamma_inv.inv_posv(Gamma))
00385                                 Gamma_inv.inv_svd(Gamma);
00386 //                      Gamma_inv.inv_porfs(Gamma);
00387 #ifdef PSIM_TEST
00388                         fMat U(n_const, n_const), VT(n_const, n_const);
00389                         fVec sigma(n_const);
00390                         Gamma.svd(U, sigma, VT);
00391                         double cn = sigma(0) / sigma(n_const-1);
00392                         if(max_condition_number < 0.0 || cn > max_condition_number)
00393                         {
00394                                 max_condition_number = cn;
00395                                 max_condition_number_joint = last_joint;
00396                         }
00397                         condition_numbers(last_joint->i_dof) = cn;
00398 //                      cerr << "condition_number = " << cn << endl;
00399 //                      fMat Gamma_inv2(n_const, n_const);
00400 //                      Gamma_inv2.inv_svd(Gamma, 2000);
00401 //                      cerr << last_joint->name << ": " << cn << endl;
00402 //                      fMat I(n_const, n_const);
00403 //                      I.identity();
00404 //                      cerr << last_joint->name << ": " << I - Gamma*Gamma_inv << endl;
00405 //                      cerr << last_joint->name << ": " << Gamma_inv - Gamma_inv2 << endl;
00406 #endif
00407                 }
00408                 else
00409                 {
00410                         // Gamma may be singular
00411                         Gamma_inv.inv_svd(Gamma);
00412 //                      cerr << Gamma_inv * Gamma << endl;
00413                 }
00414         }
00415         // W, IW
00416         W.zero();
00417         for(i=0; i<n_const; i++)
00418         {
00419                 for(int j=0; j<n_const; j++)
00420                 {
00421                         W(const_index[i], const_index[j]) = -Gamma_inv(i, j);
00422                 }
00423         }
00424 #else // #ifndef USE_DCA
00425         static fMat SV, VSV;
00426         SV.resize(n_dof, 6);
00427         VSV.resize(n_dof, 6);
00428         Vhat.inv_posv(P);
00429         if(n_dof > 0)
00430         {
00431                 for(i=0; i<n_dof; i++)
00432                 {
00433                         for(int j=0; j<n_dof; j++)
00434                         {
00435                                 SVS(i,j) = Vhat(joint_index[i], joint_index[j]);
00436                         }
00437                         for(int j=0; j<6; j++)
00438                         {
00439                                 SV(i,j) = Vhat(joint_index[i], j);
00440                         }
00441                 }
00442 //              cerr << "SVS = " << SVS << endl;
00443                 VSV.lineq(SVS, SV);
00444 //              cerr << "VSV = " << VSV << endl;
00445                 W.mul(tran(SV), VSV);
00446 //              W *= -1.0;
00447         }
00448         else
00449         {
00450                 W.zero();
00451         }
00452         W -= Vhat;
00453 #endif
00454 //      cerr << "P = " << P << endl;
00455 //      cerr << "W = " << W << endl;
00456         IW.mul(P, W);
00457         for(i=0; i<6; i++)
00458         {
00459                 IW(i, i) += 1.0;
00460         }
00461 //      cerr << "IW = " << IW << endl;
00462         // Lambda
00463         if(n_const == 0)
00464         {
00465                 for(i=0; i<n_outer_joints; i++)
00466                 {
00467                         int org_i = outer_joints_origin[i];
00468                         int index_i = outer_joints_index[i];
00469                         for(j=i; j<n_outer_joints; j++)
00470                         {
00471                                 int org_j = outer_joints_origin[j];
00472                                 int index_j = outer_joints_index[j];
00473                                 if(children[org_i] == children[org_j])
00474                                 {
00475                                         Lambda[i][j].set(children[org_i]->Lambda[index_i][index_j]);
00476                                 }
00477                                 if(i != j)
00478                                 {
00479                                         Lambda[j][i].tran(Lambda[i][j]);
00480                                 }
00481                         }
00482                 }
00483         }
00484         else
00485         {
00486                 for(i=0; i<n_outer_joints; i++)
00487                 {
00488                         int org_i = outer_joints_origin[i];
00489                         int index_i = outer_joints_index[i];
00490                         fMat& Lambda_i = children[org_i]->Lambda[index_i][last_index[org_i]];
00491 #ifndef USE_DCA
00492                         int m, n;
00493                         static fMat LKi, KLj, GKLj;
00494                         LKi.resize(6, n_const);
00495                         KLj.resize(n_const, 6);
00496                         GKLj.resize(n_const, 6);
00497                         for(m=0; m<6; m++)
00498                         {
00499                                 for(n=0; n<n_const; n++)
00500                                         LKi(m, n) = Lambda_i(m, const_index[n]);
00501                         }
00502                         for(j=i; j<n_outer_joints; j++)
00503                         {
00504                                 int org_j = outer_joints_origin[j];
00505                                 int index_j = outer_joints_index[j];
00506                                 fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
00507                                 for(m=0; m<n_const; m++)
00508                                 {
00509                                         for(n=0; n<6; n++)
00510                                                 KLj(m, n) = Lambda_j(const_index[m], n);
00511                                 }
00512                                 GKLj.mul(Gamma_inv, KLj);
00513 //                              GKLj.lineq_posv(Gamma, KLj);
00514                                 Lambda[i][j].mul(LKi, GKLj);
00515                                 if(children[org_i] == children[org_j])
00516                                 {
00517                                         Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
00518                                         Lambda[i][j] *= -1.0;
00519                                 }
00520                                 if(i != j)
00521                                 {
00522                                         Lambda[j][i].tran(Lambda[i][j]);
00523                                 }
00524                                 else
00525                                 {
00526                                         Lambda[i][j].symmetric();
00527                                 }
00528                         }
00529 #else  // #ifndef USE_DCA
00530                         for(j=i; j<n_outer_joints; j++)
00531                         {
00532                                 int org_j = outer_joints_origin[j];
00533                                 int index_j = outer_joints_index[j];
00534                                 fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
00535                                 static fMat WL(6,6);
00536                                 WL.mul(W, Lambda_j);
00537                                 WL *= -1.0;
00538                                 Lambda[i][j].mul(Lambda_i, WL);
00539                                 if(children[org_i] == children[org_j])
00540                                 {
00541                                         Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
00542                                         Lambda[i][j] *= -1.0;
00543                                 }
00544                                 if(i != j)
00545                                 {
00546                                         Lambda[j][i].tran(Lambda[i][j]);
00547                                 }
00548                                 else
00549                                 {
00550                                         Lambda[i][j].symmetric();
00551                                 }
00552                         }
00553 #endif
00554                 }
00555         }
00556 #ifdef USE_MPI
00557         if(parent && sim->rank != parent->rank)
00558         {
00559 //              cerr << sim->rank << ": joint (" << last_joint->name << "): send_inertia to " << parent->rank << endl;
00560                 send_inertia(parent->rank);
00561         }
00562 #endif
00563 }
00564 
00568 void pSim::update_collision()
00569 {
00570         calc_dvel();
00571         col_disassembly();
00572 }
00573 
00574 void pSim::calc_dvel()
00575 {
00576         int i;
00577         for(i=0; i<n_joint; i++)
00578         {
00579                 joint_info[i].pjoints[0]->calc_dvel();
00580                 joint_info[i].pjoints[1]->calc_dvel();
00581         }
00582         subchains->calc_dvel();
00583 }
00584 
00585 void pJoint::calc_dvel()
00586 {
00587         if(parent_side)
00588         {
00589                 static fVec v(6);
00590                 v(0) = link_side->loc_lin_vel(0);
00591                 v(1) = link_side->loc_lin_vel(1);
00592                 v(2) = link_side->loc_lin_vel(2);
00593                 v(3) = link_side->loc_ang_vel(0);
00594                 v(4) = link_side->loc_ang_vel(1);
00595                 v(5) = link_side->loc_ang_vel(2);
00596                 dvel.mul(J, v);
00597         }
00598         else
00599         {
00600                 dvel(0) = joint->loc_lin_vel(0);
00601                 dvel(1) = joint->loc_lin_vel(1);
00602                 dvel(2) = joint->loc_lin_vel(2);
00603                 dvel(3) = joint->loc_ang_vel(0);
00604                 dvel(4) = joint->loc_ang_vel(1);
00605                 dvel(5) = joint->loc_ang_vel(2);
00606         }
00607 }
00608 
00609 void pSubChain::calc_dvel()
00610 {
00611         if(!this) return;
00612         if(!last_joint)  // single link
00613         {
00614                 calc_dvel_leaf();
00615         }
00616         else  // have last joint
00617         {
00618                 // process children first
00619                 children[0]->calc_dvel();
00620                 if(children[1] != children[0]) children[1]->calc_dvel();
00621                 calc_dvel_body();
00622         }
00623 }
00624 
00625 void pSubChain::calc_dvel_leaf()
00626 {
00627         int i;
00628         for(i=0; i<n_outer_joints; i++)
00629         {
00630                 vel_temp[i].set(outer_joints[i]->dvel);
00631         }
00632 }
00633 
00634 void pSubChain::calc_dvel_body()
00635 {
00636         if(!children[1]) return;
00637         int i, j;
00638         // compute f_temp
00639         static fVec dv6(6), dv;
00640         static fMat PK;
00641         PK.resize(6, n_dof);
00642         dv.resize(n_const);
00643         for(i=0; i<6; i++)
00644         {
00645                 for(j=0; j<n_dof; j++)
00646                         PK(i, j) = P(i, joint_index[j]);
00647         }
00648         // + child_side - parent_side ?
00649         dv6.sub(children[0]->vel_temp[last_index[0]], children[1]->vel_temp[last_index[1]]);
00650         // actually we could save some computation by
00651         // selecting const rows first
00652         for(i=0; i<n_const; i++)
00653                 dv(i) = -dv6(const_index[i]);
00654 #ifndef USE_DCA  // TODO: allow dca
00655         colf_temp.mul(Gamma_inv, dv);
00656 #endif
00657         // compute new velocity at all outer joints
00658         static fVec f(6);
00659         f.zero();
00660         for(i=0; i<n_const; i++)
00661                 f(const_index[i]) = colf_temp(i);
00662         for(i=0; i<n_outer_joints; i++)
00663         {
00664                 int org = outer_joints_origin[i];
00665                 int index = outer_joints_index[i];
00666                 int ilast = last_index[org];
00667                 vel_temp[i].mul(children[org]->Lambda[index][ilast], f);
00668                 if(org == 1)
00669                 {
00670                         vel_temp[i] *= -1.0;
00671                 }
00672                 vel_temp[i] += children[org]->vel_temp[index];
00673         }
00674 }
00675 
00676 void pSim::col_disassembly()
00677 {
00678         subchains->col_disassembly();
00679 }
00680 
00681 void pSubChain::col_disassembly()
00682 {
00683         if(!this) return;
00684         if(last_joint)
00685         {
00686                 col_disassembly_body();
00687                 // process children last
00688                 children[0]->col_disassembly();
00689                 if(children[1] != children[0]) children[1]->col_disassembly();
00690         }
00691 }
00692 
00693 void pSubChain::col_disassembly_body()
00694 {
00695         // for space
00696         if(!children[1]) return;
00697         int i;
00698         // compute final constraint force
00699         static fVec KLf, Lf(6), Lf_temp[2], v(6), colf, colf_final(6);
00700         KLf.resize(n_const);
00701         Lf_temp[0].resize(6);
00702         Lf_temp[1].resize(6);
00703         colf.resize(n_const);
00704         Lf_temp[0].zero();
00705         Lf_temp[1].zero();
00706         for(i=0; i<n_outer_joints; i++)
00707         {
00708                 int org = outer_joints_origin[i];
00709                 int index = outer_joints_index[i];
00710                 fMat& Lambda_i = children[org]->Lambda[last_index[org]][index];
00711                 // first multiply and increment
00712                 v.mul(Lambda_i, children[org]->outer_joints[index]->colf_final);
00713                 Lf_temp[org] += v;
00714         }
00715         Lf.sub(Lf_temp[0], Lf_temp[1]);
00716         // then extract rows
00717         for(i=0; i<n_const; i++)
00718                 KLf(i) = Lf(const_index[i]);
00719 #ifndef USE_DCA  // TODO: allow DCA
00720         colf.mul(Gamma_inv, KLf);
00721 #endif
00722         colf_temp -= colf;
00723         colf_final.zero();
00724         for(i=0; i<n_const; i++)
00725                 colf_final(const_index[i]) = colf_temp(i);
00726         last_pjoints[0]->colf_final.set(colf_final);
00727         last_pjoints[1]->colf_final.neg(colf_final);
00728         // compute link velocities
00729         last_pjoints[0]->vel_final.mul(children[0]->Lambda[last_index[0]][last_index[0]], last_pjoints[0]->colf_final);
00730         last_pjoints[0]->vel_final += Lf_temp[0];
00731         last_pjoints[0]->vel_final += children[0]->vel_temp[last_index[0]];
00732         if(children[1])
00733         {
00734                 last_pjoints[1]->vel_final.mul(children[1]->Lambda[last_index[1]][last_index[1]], last_pjoints[1]->colf_final);
00735                 last_pjoints[1]->vel_final += Lf_temp[1];
00736                 last_pjoints[1]->vel_final += children[1]->vel_temp[last_index[1]];
00737         }
00738         if(children[0] == children[1])
00739         {
00740                 v.mul(children[1]->Lambda[last_index[0]][last_index[1]], last_pjoints[1]->colf_final);
00741                 last_pjoints[0]->vel_final += v;
00742                 v.mul(children[0]->Lambda[last_index[1]][last_index[0]], last_pjoints[0]->colf_final);
00743                 last_pjoints[1]->vel_final += v;
00744         }
00745         // compute joint velocity
00746         v.sub(last_pjoints[0]->vel_final, last_pjoints[1]->vel_final);
00747 //      cerr << last_joint->name << ": v = " << tran(v) << endl;
00748         switch(last_joint->j_type)
00749         {
00750         case JROTATE:
00751         case JSLIDE:
00752                 last_joint->SetJointVel(v(axis));
00753                 break;
00754         case JSPHERE:
00755                 last_joint->SetJointVel(fVec3(v(3), v(4), v(5)));
00756                 break;
00757         case JFREE:
00758                 last_joint->SetJointVel(fVec3(v(0), v(1), v(2)), fVec3(v(3), v(4), v(5)));
00759                 break;
00760         default:
00761                 break;
00762         }
00763 }
00764 
00768 void pSim::update_velocity()
00769 {
00770         int i;
00771         for(i=0; i<n_joint; i++)
00772         {
00773 #ifdef USE_MPI
00774                 if(joint_info[i].pjoints[0]->subchain && rank == joint_info[i].pjoints[0]->subchain->rank)
00775 #endif
00776                 {
00777                         joint_info[i].pjoints[0]->calc_jdot();
00778                 }
00779 #ifdef USE_MPI
00780                 if(joint_info[i].pjoints[1]->subchain && rank == joint_info[i].pjoints[1]->subchain->rank)
00781 #endif
00782                 {
00783                         joint_info[i].pjoints[1]->calc_jdot();
00784                 }
00785                 if(joint_info[i].plink
00786 #ifdef USE_MPI
00787                    && joint_info[i].plink->subchain &&
00788                    rank == joint_info[i].plink->subchain->rank
00789 #endif
00790                    )
00791                 {
00792                         joint_info[i].plink->calc_acc(Root()->loc_lin_acc);
00793                 }
00794         }
00795         // should come after computing Jacobians of all pjoints
00796         subchains->calc_acc();
00797 #ifdef TIMING_CHECK
00798         cerr << "[" << rank << "] update_velocity end t = " << MPI_Wtime()-update_start_time << endl;
00799 #endif
00800 }
00801 
00802 void pJoint::calc_jdot()
00803 {
00804         if(parent_side)
00805         {
00806                 static fVec3 v1, v2, v3;
00807                 static fVec3 rel_pos;
00808                 static fMat33 rel_att;
00809                 if(link_side == joint->real)
00810                 {
00811                         rel_pos.set(joint->rpos_real);
00812                         rel_att.set(joint->ratt_real);
00813                 }
00814                 else
00815                 {
00816                         rel_pos.set(joint->rel_pos);
00817                         rel_att.set(joint->rel_att);
00818                 }
00819                 v1.cross(link_side->loc_ang_vel, rel_pos);
00820                 v2.cross(link_side->loc_ang_vel, v1);
00821                 v3.mul(v2, rel_att);
00822 
00823 //              if(!joint->real)
00824                 {
00825 //                      v1.set(joint->loc_ang_vel);
00826                         v1.mul(link_side->loc_ang_vel, rel_att);
00827                         // linear joint motion
00828                         v2.cross(v1, joint->rel_lin_vel);
00829                         v2 *= 2.0;
00830                         v3 += v2;
00831                         // angular joint motion
00832                         v2.cross(v1, joint->rel_ang_vel);
00833                 }
00834 
00835                 Jdot(0) = v3(0);
00836                 Jdot(1) = v3(1);
00837                 Jdot(2) = v3(2);
00838                 Jdot(3) = v2(0);
00839                 Jdot(4) = v2(1);
00840                 Jdot(5) = v2(2);
00841 //              cerr << joint->name << ": jdot = " << tran(Jdot) << endl;
00842 //              cerr << "rel_pos = " << rel_pos << endl;
00843 //              cerr << "rel_lin_vel = " << joint->rel_lin_vel << endl;
00844 //              cerr << "rel_ang_vel = " << joint->rel_ang_vel << endl;
00845         }
00846         else // child side
00847         {
00848         }
00849 }
00850 
00851 void pLink::calc_acc(const fVec3& g0)
00852 {
00853         static fVec3 a, c1, c2, g;
00854         static fVec3 v1, v2;
00855         g.mul(g0, joint->abs_att);
00856         // nonlinear acc
00857         v1.cross(joint->loc_ang_vel, joint->loc_com);
00858         a.cross(joint->loc_ang_vel, v1);
00859         a += g;
00860         // c1
00861         c1.mul(joint->mass, a);
00862         // c2
00863         v1.mul(joint->inertia, joint->loc_ang_vel);
00864         v2.cross(joint->loc_ang_vel, v1);
00865         c2.cross(joint->loc_com, c1);
00866         c2 += v2;
00867         // external force/moment around com
00868         c1 -= joint->ext_force;
00869         c2 -= joint->ext_moment;
00870 #ifdef VERBOSE
00871         update_log << joint->name << ": external force/moment = " << joint->ext_force << "/" << joint->ext_moment << endl;
00872 #endif
00873 //      v1.cross(joint->ext_force, joint->loc_com);
00874 //      c2 -= v1;
00875         // -c
00876         c(0) = -c1(0);
00877         c(1) = -c1(1);
00878         c(2) = -c1(2);
00879         c(3) = -c2(0);
00880         c(4) = -c2(1);
00881         c(5) = -c2(2);
00882         // bias acc
00883         acc.mul(Minv, c);
00884 //      cerr << joint->name << ": acc = " << tran(acc) << endl;
00885 }
00886 
00887 void pSubChain::calc_acc()
00888 {
00889         if(!this) return;
00890         if(!last_joint)  // single link
00891         {
00892                 calc_acc_leaf();
00893         }
00894         else  // have last joint
00895         {
00896                 // process children first
00897                 children[0]->calc_acc();
00898                 if(children[1] != children[0])
00899                 {
00900                         children[1]->calc_acc();
00901                 }
00902                 calc_acc_body();
00903         }
00904 }
00905 
00906 #ifdef USE_MPI
00907 void pSubChain::recv_acc()
00908 {
00909         MPI_Status status;
00910 #if 0
00911         int i;
00912         for(i=0; i<n_outer_joints; i++)
00913         {
00914                 double* buf = acc_temp[i].data();
00915                 MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_ACC, MPI_COMM_WORLD, &status);
00916         }
00917 #else
00918 #ifdef TIMING_CHECK
00919         double time1 = MPI_Wtime();
00920         cerr << "[" << sim->rank << "] recv_acc(0) t = " << MPI_Wtime()-update_start_time << endl;
00921 #endif
00922         MPI_Recv(MPI_BOTTOM, 1, parent_acc_type, rank, PSIM_TAG_ACC, MPI_COMM_WORLD, &status);
00923 #ifdef TIMING_CHECK
00924         cerr << "[" << sim->rank << "] recv_acc(1) t = " << MPI_Wtime()-update_start_time << endl;
00925         double time2 = MPI_Wtime();
00926         sim->acc_wait_time += time2-time1;
00927 #endif
00928 #endif
00929 }
00930 
00931 void pSubChain::send_acc(int dest)
00932 {
00933 #if 0
00934         int i;
00935         for(i=0; i<n_outer_joints; i++)
00936         {
00937                 double* buf = acc_temp[i].data();
00938                 MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_ACC, MPI_COMM_WORLD);
00939         }
00940 #else
00941 #ifdef TIMING_CHECK
00942         cerr << "[" << sim->rank << "] send_acc(0) t = " << MPI_Wtime()-update_start_time << endl;
00943 #endif
00944         MPI_Send(MPI_BOTTOM, 1, parent_acc_type, dest, PSIM_TAG_ACC, MPI_COMM_WORLD);
00945 #ifdef TIMING_CHECK
00946         cerr << "[" << sim->rank << "] send_acc(1) t = " << MPI_Wtime()-update_start_time << endl;
00947 #endif
00948 #endif
00949 }
00950 #endif
00951 
00952 void pSubChain::calc_acc_leaf()
00953 {
00954 #ifdef USE_MPI
00955         if(sim->rank != rank) return;
00956 #endif
00957         // compute bias acc at all outer joints
00958 //      update_log << "--- " << links[0]->joint->name << ": calc_acc_leaf" << endl;
00959         int i;
00960         for(i=0; i<n_outer_joints; i++)
00961         {
00962                 if(outer_joints[i]->parent_side)
00963                 {
00964                         acc_temp[i].mul(outer_joints[i]->J, links[0]->acc);
00965                         acc_temp[i] += outer_joints[i]->Jdot;
00966                 }
00967                 else
00968                 {
00969 //                      acc_temp[i].set(links[0]->acc);
00970                         acc_temp[i].add(links[0]->acc, outer_joints[i]->Jdot);
00971                 }
00972 //              update_log << "acc_temp[" << i << "] = " << tran(acc_temp[i]) << endl;
00973         }
00974 #ifdef USE_MPI
00975         if(parent && sim->rank != parent->rank)
00976         {
00977                 send_acc(parent->rank);
00978         }
00979 #endif
00980 }
00981 
00982 void pSubChain::calc_acc_body()
00983 {
00984 //      if(!children[1]) return;
00985 #ifdef USE_MPI
00986         if(sim->rank != rank) return;
00987         if(children[0] && sim->rank != children[0]->rank)
00988         {
00989                 children[0]->recv_acc();
00990         }
00991         if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
00992         {
00993                 children[1]->recv_acc();
00994         }
00995 #endif
00996 //      update_log << "--- " << last_joint->name << ": calc_acc_body" << endl;
00997         int i, j;
00998         // compute f_temp
00999         static fVec da;
01000         static fMat PK;
01001         PK.resize(6, n_dof);
01002         da.resize(n_const);
01003         for(i=0; i<6; i++)
01004         {
01005                 for(j=0; j<n_dof; j++)
01006                         PK(i, j) = P(i, joint_index[j]);
01007         }
01008         if(last_joint->n_dof > 0)
01009         {
01010                 switch(last_joint->j_type)
01011                 {
01012                 case JROTATE:
01013                 case JSLIDE:
01014                         tau(0) = last_joint->tau;
01015                         break;
01016                 case JSPHERE:
01017                         tau(0) = last_joint->tau_n(0);
01018                         tau(1) = last_joint->tau_n(1);
01019                         tau(2) = last_joint->tau_n(2);
01020                         break;
01021                 case JFREE:
01022                         tau(0) = last_joint->tau_f(0);
01023                         tau(1) = last_joint->tau_f(1);
01024                         tau(2) = last_joint->tau_f(2);
01025                         tau(3) = last_joint->tau_n(0);
01026                         tau(4) = last_joint->tau_n(1);
01027                         tau(5) = last_joint->tau_n(2);
01028                         break;
01029                 default:
01030                         break;
01031                 }
01032                 da6.mul(PK, tau);
01033         }
01034         else
01035                 da6.zero();
01036 //      cerr << "da6(0) = " << tran(da6) << endl;
01037         // + child_side - parent_side ?
01038         da6 += children[0]->acc_temp[last_index[0]];
01039 //      cerr << "da6(1) = " << tran(da6) << endl;
01040         if(children[1])
01041                 da6 -= children[1]->acc_temp[last_index[1]];
01042 //      cerr << "da6(2) = " << tran(da6) << endl;
01043         // motion controlled joints
01044         if(!last_joint->t_given)
01045         {
01046                 switch(last_joint->j_type)
01047                 {
01048                 case JROTATE:
01049                 case JSLIDE:
01050                         da6(axis) -= last_joint->qdd;
01051 //                      update_log << last_joint->name << ": qdd = " << last_joint->qdd << endl;
01052                         break;
01053                 case JSPHERE:
01054                         da6(3) -= last_joint->rel_ang_acc(0);
01055                         da6(4) -= last_joint->rel_ang_acc(1);
01056                         da6(5) -= last_joint->rel_ang_acc(2);
01057                         break;
01058                 case JFREE:
01059                         da6(0) -= last_joint->rel_lin_acc(0);
01060                         da6(1) -= last_joint->rel_lin_acc(1);
01061                         da6(2) -= last_joint->rel_lin_acc(2);
01062                         da6(3) -= last_joint->rel_ang_acc(0);
01063                         da6(4) -= last_joint->rel_ang_acc(1);
01064                         da6(5) -= last_joint->rel_ang_acc(2);
01065                         break;
01066                 default:
01067                         break;
01068                 }
01069         }
01070         static fVec f(6);
01071 //      cerr << "Gamma = " << Gamma << endl;
01072 //      cerr << "Gamma_inv = " << Gamma_inv << endl;
01073 #if 0
01074         // actually we could save some computation by
01075         // selecting const rows first
01076         for(i=0; i<n_const; i++)
01077                 da(i) = -da6(const_index[i]);
01078         f_temp.mul(Gamma_inv, da);
01079 //      f_temp.lineq_posv(Gamma, da);
01080         // compute acc at all outer joints
01081         for(i=0; i<n_dof; i++)
01082                 f(joint_index[i]) = tau(i);
01083         for(i=0; i<n_const; i++)
01084                 f(const_index[i]) = f_temp(i);
01085 //      cerr << "da = " << tran(da) << endl;
01086 //      cerr << "f_temp = " << tran(f_temp) << endl;
01087 //      cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
01088 #else
01089 #if 0
01090         f.mul(W, da6);
01091         for(i=0; i<n_dof; i++)
01092         {
01093                 f(joint_index[i]) += tau(i);
01094         }
01095 #else
01096         static fVec db(6), Wdb(6), IWRtau(6);
01097         static fMat IWR;
01098         IWR.resize(6, n_dof);
01099         db.set(children[0]->acc_temp[last_index[0]]);
01100         if(children[1])
01101                 db -= children[1]->acc_temp[last_index[1]];
01102         Wdb.mul(W, db);
01103         for(i=0; i<6; i++)
01104         {
01105                 for(j=0; j<n_dof; j++)
01106                 {
01107                         IWR(i, j) = IW(joint_index[j], i);
01108                 }
01109         }
01110         IWRtau.mul(IWR, tau);
01111 //      cerr << "W = " << tran(W) << endl;
01112 //      update_log << "db = " << tran(db) << endl;
01113 //      cerr << "Wdb = " << tran(Wdb) << endl;
01114 //      cerr << "IWRtau = " << tran(IWRtau) << endl;
01115         f.add(Wdb, IWRtau);
01116 //      update_log << "f = " << tran(f) << endl;
01117         
01118 #ifdef PSIM_TEST
01119 
01120         for(i=0; i<n_const; i++)
01121         {
01122                 da(i) = -da6(const_index[i]);
01123                 f_temp(i) = f(const_index[i]);
01124         }
01125 //      cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
01126         total_gamma_error += (Gamma*f_temp-da) * (Gamma*f_temp-da);
01128 #endif
01129 #endif
01130 #endif
01131         for(i=0; i<n_outer_joints; i++)
01132         {
01133                 int org = outer_joints_origin[i];
01134                 int index = outer_joints_index[i];
01135                 int ilast = last_index[org];
01136                 acc_temp[i].mul(children[org]->Lambda[index][ilast], f);
01137                 if(org == 1)
01138                 {
01139                         acc_temp[i] *= -1.0;
01140                 }
01141                 acc_temp[i] += children[org]->acc_temp[index];
01142 //              update_log << "acc_temp[" << i << "] = " << tran(acc_temp[i]) << endl;
01143         }
01144 #ifdef USE_MPI
01145         if(parent && sim->rank != parent->rank)
01146         {
01147                 send_acc(parent->rank);
01148         }
01149 #endif
01150 }
01151 
01155 void pSim::disassembly()
01156 {
01157         subchains->disassembly();
01158 }
01159 
01160 void pSubChain::disassembly()
01161 {
01162         if(!this) return;
01163         if(last_joint)
01164         {
01165                 disassembly_body();
01166                 // process children last
01167                 children[0]->disassembly();
01168                 if(children[1] != children[0]) children[1]->disassembly();
01169         }
01170         else 
01171         {
01172 //              disassembly_leaf();
01173         }
01174 }
01175 
01176 #ifdef USE_MPI
01177 void pSubChain::recv_force()
01178 {
01179         MPI_Status status;
01180 #if 0
01181         int i;
01182         for(i=0; i<n_outer_joints; i++)
01183         {
01184                 double* buf = outer_joints[i]->f_final.data();
01185                 MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
01186         }
01187         double* buf;
01188         buf = last_pjoints[0]->f_final.data();
01189         MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
01190         buf = last_pjoints[1]->f_final.data();
01191         MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
01192 #else
01193 #ifdef TIMING_CHECK
01194         double time1 = MPI_Wtime();
01195 #endif
01196         MPI_Recv(MPI_BOTTOM, 1, parent_force_type, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
01197 #ifdef TIMING_CHECK
01198         double time2 = MPI_Wtime();
01199         sim->force_wait_time += time2-time1;
01200 #endif
01201 #endif
01202 }
01203 
01204 void pSubChain::send_force(int dest)
01205 {
01206 #if 0
01207         int i;
01208         for(i=0; i<n_outer_joints; i++)
01209         {
01210                 double* buf = outer_joints[i]->f_final.data();
01211                 MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
01212         }
01213         double* buf;
01214         buf = last_pjoints[0]->f_final.data();
01215         MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
01216         buf = last_pjoints[1]->f_final.data();
01217         MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
01218 #else
01219         MPI_Send(MPI_BOTTOM, 1, parent_force_type, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
01220 #endif
01221 }
01222 #endif
01223 
01224 void pSubChain::disassembly_leaf()
01225 {
01226         Joint* target_joint = links[0]->joint;
01227 //      if(!target_joint->parent) return;  // skip space
01228         // convert all forces/moments to joint frame
01229         cerr << "---- " << target_joint->name << ": disassembly_leaf" << endl;
01230         int i;
01231         static fVec3 total_f, total_n;
01232         static fVec3 pos;
01233         static fMat33 att, t_att;
01234         static fVec acc(6);
01235         static fVec allf(6);
01236         total_f.zero();
01237         total_n.zero();
01238         acc.zero();
01239         pos.set(target_joint->abs_pos);
01240         att.set(target_joint->abs_att);
01241         t_att.tran(att);
01242         for(i=0; i<n_outer_joints; i++)
01243         {
01244                 static fVec3 loc_f, loc_n, f, n, fn;
01245                 static fVec3 jpos, rel_pos, pp;
01246                 static fMat33 jatt, rel_att;
01247                 cerr << "outer[" << i << "]: " << outer_joints[i]->joint->name << endl;
01248                 cerr << "f_final = " << tran(outer_joints[i]->f_final) << endl;
01249                 loc_f(0) = outer_joints[i]->f_final(0);
01250                 loc_f(1) = outer_joints[i]->f_final(1);
01251                 loc_f(2) = outer_joints[i]->f_final(2);
01252                 loc_n(0) = outer_joints[i]->f_final(3);
01253                 loc_n(1) = outer_joints[i]->f_final(4);
01254                 loc_n(2) = outer_joints[i]->f_final(5);
01255                 jpos.set(outer_joints[i]->joint->abs_pos);
01256                 jatt.set(outer_joints[i]->joint->abs_att);
01257                 pp.sub(jpos, pos);
01258                 rel_pos.mul(t_att, pp);
01259                 rel_att.mul(t_att, jatt);
01260                 f.mul(rel_att, loc_f);  // force
01261                 n.mul(rel_att, loc_n);
01262                 fn.cross(rel_pos, f);
01263                 n += fn;
01264                 cerr << "(f n) = " << f << n << endl;
01265                 total_f += f;
01266                 total_n += n;
01267         }
01268         allf(0) = total_f(0);
01269         allf(1) = total_f(1);
01270         allf(2) = total_f(2);
01271         allf(3) = total_n(0);
01272         allf(4) = total_n(1);
01273         allf(5) = total_n(2);
01274         cerr << "total_f = " << total_f << endl;
01275         cerr << "total_n = " << total_n << endl;
01276         acc.lineq_posv(links[0]->M, allf);
01277         acc += links[0]->acc;
01278         cerr << "Minv = " << links[0]->Minv << endl;
01279         cerr << "acc = " << tran(links[0]->acc) << endl;
01280         cerr << "link acc = " << tran(acc) << endl;
01281 }
01282 
01283 void pSubChain::disassembly_body()
01284 {
01285         // for space
01286         if(!children[1]) return;
01287 #ifdef VERBOSE
01288         update_log << "disassembly_body" << endl;
01289 #endif
01290 #ifdef USE_MPI
01291         if(sim->rank != rank) return;
01292         if(parent && sim->rank != parent->rank)
01293         {
01294 #ifdef TIMING_CHECK
01295                 cerr << "[" << sim->rank << "] " << last_joint->name << ": recv force from " << parent->last_joint->name << " [" << parent->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
01296 #endif
01297                 parent->recv_force();
01298         }
01299 #endif
01300         int i;
01301 #ifdef TIMING_CHECK
01302         if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
01303                 cerr << "[" << sim->rank << "] " << last_joint->name << " enter disassembly t = " << MPI_Wtime()-update_start_time << endl;
01304 #endif
01305         // compute final constraint force
01306         static fVec KLf, Lf(6), Lf_temp[2], v(6), f, f_final(6);
01307         KLf.resize(n_const);
01308         Lf_temp[0].resize(6);
01309         Lf_temp[1].resize(6);
01310         f.resize(n_const);
01311         Lf_temp[0].zero();
01312         Lf_temp[1].zero();
01313         for(i=0; i<n_outer_joints; i++)
01314         {
01315                 int org = outer_joints_origin[i];
01316                 int index = outer_joints_index[i];
01317                 fMat& Lambda_i = children[org]->Lambda[last_index[org]][index];
01318                 // first multiply and increment
01319 //              v.mul(Lambda_i, children[org]->outer_joints[index]->f_final);
01320                 v.mul(Lambda_i, outer_joints[i]->f_final);
01321 #ifdef VERBOSE
01322 //              update_log << "children[" << org << "]->Lambda[" << last_index[org] << "][" << index << "] = " << Lambda_i << endl;
01323                 update_log << outer_joints[i]->joint->name << ": f_final[" << i << "] = " << tran(outer_joints[i]->f_final) << endl;
01324 #endif
01325                 Lf_temp[org] += v;
01326         }
01327         Lf.sub(Lf_temp[0], Lf_temp[1]);
01328 //      update_log << "Lf_temp[0] = " << tran(Lf_temp[0]) << endl;
01329 //      update_log << "Lf_temp[1] = " << tran(Lf_temp[1]) << endl;
01330         // all test codes removed on 02/09/2007
01331         static fVec pp(6);
01332 #ifndef USE_DCA
01333         // new formulation
01334         pp.add(da6, Lf);
01335         f_final.mul(W, pp);
01336         for(i=0; i<n_dof; i++)
01337                 f_final(joint_index[i]) += tau(i);
01338         last_pjoints[0]->f_final.set(f_final);
01339         last_pjoints[1]->f_final.neg(f_final);
01340         v.mul(IW, pp);
01341         for(i=0; i<n_dof; i++)
01342         {
01343                 acc_final(i) = v(joint_index[i]);
01344         }
01345         last_joint->joint_f.set(fVec3(f_final(0), f_final(1), f_final(2)));
01346         last_joint->joint_n.set(fVec3(f_final(3), f_final(4), f_final(5)));
01347 #else  // #ifndef USE_DCA (DCA test)
01348         static fVec vp(6), svp;
01349         svp.resize(n_dof);
01350         pp.set(da6);
01351         pp += Lf;
01352         vp.mul(Vhat, pp);
01353         for(i=0; i<n_dof; i++)
01354         {
01355                 svp(i) = tau(i) + vp(joint_index[i]);
01356         }
01357         acc_final.lineq_posv(SVS, svp);
01358 //      cerr << "SVS = " << SVS << endl;
01359 //      cerr << "svp = " << tran(svp) << endl;
01360 //      cerr << "acc_final = " << tran(acc_final) << endl;
01361         v.zero();
01362         switch(last_joint->j_type)
01363         {
01364         case JROTATE:
01365         case JSLIDE:
01366                 v(axis) = acc_final(0);
01367                 break;
01368         case JSPHERE:
01369                 v(3) = acc_final(0);
01370                 v(4) = acc_final(1);
01371                 v(5) = acc_final(2);
01372                 break;
01373         case JFREE:
01374                 v.set(acc_final);
01375                 break;
01376         }
01377         pp -= v;
01378         f_final.mul(Vhat, pp);
01379         last_pjoints[0]->f_final.neg(f_final);
01380         last_pjoints[1]->f_final.set(f_final);
01381 #endif
01382 #ifndef USE_MPI
01383         if(last_joint->t_given) {
01384           switch(last_joint->j_type) {
01385             case JROTATE:
01386           case JSLIDE:
01387             last_joint->SetJointAcc(v(axis));
01388             //          cerr << last_joint->name << ": " << v(axis) << endl;
01389             break;
01390           case JSPHERE:
01391             last_joint->SetJointAcc(v(3), v(4), v(5));
01392             break;
01393           case JFREE:
01394             last_joint->SetJointAcc(v(0), v(1), v(2), v(3), v(4), v(5));
01395 #ifdef VERBOSE
01396             update_log << last_joint->name << ": " << tran(v) << endl;
01397 #endif
01398             break;
01399           default:
01400             break;
01401           }
01402         }
01403 #endif
01404 #ifdef USE_MPI
01405         if(children[0] && sim->rank != children[0]->rank)
01406         {
01407 #ifdef TIMING_CHECK
01408                 cerr << "[" << sim->rank << "] " << last_joint->name << ": send force to " << children[0]->last_joint->name << " [" << children[0]->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
01409 #endif
01410                 send_force(children[0]->rank);
01411         }
01412         if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
01413         {
01414 #ifdef TIMING_CHECK
01415                 cerr << "[" << sim->rank << "] " << last_joint->name << ": send force to " << children[1]->last_joint->name << " [" << children[1]->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
01416 #endif
01417                 send_force(children[1]->rank);
01418         }
01419 #endif
01420 }


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