23 std::ofstream update_log(
"update.log");
27 static double update_start_time = 0.0;
31 double max_condition_number = -1.0;
32 Joint* max_condition_number_joint = 0;
33 double max_sigma_ratio = -1.0;
34 Joint* max_sigma_ratio_joint = 0;
35 fVec condition_numbers;
37 double total_gamma_error = 0.0;
43 update_log <<
"Update no contact" << endl;
46 update_start_time = MPI_Wtime();
49 max_condition_number = -1.0;
50 max_sigma_ratio = -1.0;
51 max_condition_number_joint = 0;
52 max_sigma_ratio_joint = 0;
55 condition_numbers.
zero();
57 total_gamma_error = 0.0;
78 cerr <<
"[" << rank <<
"] disassembly t = " << MPI_Wtime()-update_start_time << endl;
84 cerr <<
"--- max condition number = " << max_condition_number <<
" at " << max_condition_number_joint->
name << endl;
85 cerr <<
"--- max sigma ratio = " << max_sigma_ratio <<
" at " << max_sigma_ratio_joint->
name << endl;
86 cerr <<
"--- total_gamma_error = " << total_gamma_error << endl;
98 void pSim::scatter_acc()
103 MPI_Bcast(MPI_BOTTOM, 1, all_acc_types[
i],
i, MPI_COMM_WORLD);
108 void pSubChain::scatter_acc()
133 MPI_Bcast(sendbuf, 6, MPI_DOUBLE, rank, MPI_COMM_WORLD);
135 MPI_Bcast(sendbuf, 6, MPI_DOUBLE, rank, MPI_COMM_WORLD);
168 static fVec3 rel_pos;
180 static fMat33 tcross, tmpT;
181 tcross.
cross(rel_pos);
182 tmpT.
mul(tcross, rel_att);
183 J(0,0) = rel_att(0,0),
J(0,1) = rel_att(1,0),
J(0,2) = rel_att(2,0);
184 J(1,0) = rel_att(0,1),
J(1,1) = rel_att(1,1),
J(1,2) = rel_att(2,1);
185 J(2,0) = rel_att(0,2),
J(2,1) = rel_att(1,2),
J(2,2) = rel_att(2,2);
186 J(0,3) = tmpT(0,0),
J(0,4) = tmpT(1,0),
J(0,5) = tmpT(2,0);
187 J(1,3) = tmpT(0,1),
J(1,4) = tmpT(1,1),
J(1,5) = tmpT(2,1);
188 J(2,3) = tmpT(0,2),
J(2,4) = tmpT(1,2),
J(2,5) = tmpT(2,2);
189 J(3,0) = 0.0,
J(3,1) = 0.0,
J(3,2) = 0.0;
190 J(4,0) = 0.0,
J(4,1) = 0.0,
J(4,2) = 0.0;
191 J(5,0) = 0.0,
J(5,1) = 0.0,
J(5,2) = 0.0;
192 J(3,3) = rel_att(0,0),
J(3,4) = rel_att(1,0),
J(3,5) = rel_att(2,0);
193 J(4,3) = rel_att(0,1),
J(4,4) = rel_att(1,1),
J(4,5) = rel_att(2,1);
194 J(5,3) = rel_att(0,2),
J(5,4) = rel_att(1,2),
J(5,5) = rel_att(2,2);
219 void pSubChain::recv_inertia()
230 MPI_Recv(
buf, 36, MPI_DOUBLE, rank, PSIM_TAG_LAMBDA, MPI_COMM_WORLD, &status);
239 double time1 = MPI_Wtime();
241 MPI_Recv(MPI_BOTTOM, 1, parent_lambda_type, rank, PSIM_TAG_LAMBDA, MPI_COMM_WORLD, &status);
243 double time2 = MPI_Wtime();
244 sim->inertia_wait_time += time2-time1;
249 void pSubChain::send_inertia(
int dest)
259 MPI_Send(
buf, 36, MPI_DOUBLE, dest, PSIM_TAG_LAMBDA, MPI_COMM_WORLD);
263 MPI_Send(MPI_BOTTOM, 1, parent_lambda_type, dest, PSIM_TAG_LAMBDA, MPI_COMM_WORLD);
271 if(
sim->rank != rank)
return;
278 static fMat JM(6, 6);
289 static fMat tJ(6, 6);
308 send_inertia(
parent->rank);
318 if(
sim->rank != rank)
return;
345 fMat U1(6,6), V1T(6,6), U2(6,6), V2T(6,6);
346 fVec sigma1(6), sigma2(6);
349 double s1 = sigma1.length(),
s2 = sigma2.
length();
350 if(
s1 > 1e-8 &&
s2 > 1e-8)
353 if(max_sigma_ratio < 0.0 || ratio > max_sigma_ratio)
355 max_sigma_ratio = ratio;
391 double cn = sigma(0) / sigma(
n_const-1);
392 if(max_condition_number < 0.0 || cn > max_condition_number)
394 max_condition_number = cn;
424 #else // #ifndef USE_DCA
433 for(
int j=0; j<
n_dof; j++)
437 for(
int j=0; j<6; j++)
493 static fMat LKi, KLj, GKLj;
529 #else // #ifndef USE_DCA
560 send_inertia(
parent->rank);
639 static fVec dv6(6), dv;
645 for(j=0; j<
n_dof; j++)
654 #ifndef USE_DCA // TODO: allow dca
699 static fVec KLf, Lf(6), Lf_temp[2], v(6), colf, colf_final(6);
715 Lf.
sub(Lf_temp[0], Lf_temp[1]);
719 #ifndef USE_DCA // TODO: allow DCA
798 cerr <<
"[" << rank <<
"] update_velocity end t = " << MPI_Wtime()-update_start_time << endl;
806 static fVec3 v1, v2, v3;
807 static fVec3 rel_pos;
853 static fVec3 a, c1, c2, g;
907 void pSubChain::recv_acc()
915 MPI_Recv(
buf, 6, MPI_DOUBLE, rank, PSIM_TAG_ACC, MPI_COMM_WORLD, &status);
919 double time1 = MPI_Wtime();
920 cerr <<
"[" <<
sim->rank <<
"] recv_acc(0) t = " << MPI_Wtime()-update_start_time << endl;
922 MPI_Recv(MPI_BOTTOM, 1, parent_acc_type, rank, PSIM_TAG_ACC, MPI_COMM_WORLD, &status);
924 cerr <<
"[" <<
sim->rank <<
"] recv_acc(1) t = " << MPI_Wtime()-update_start_time << endl;
925 double time2 = MPI_Wtime();
926 sim->acc_wait_time += time2-time1;
931 void pSubChain::send_acc(
int dest)
938 MPI_Send(
buf, 6, MPI_DOUBLE, dest, PSIM_TAG_ACC, MPI_COMM_WORLD);
942 cerr <<
"[" <<
sim->rank <<
"] send_acc(0) t = " << MPI_Wtime()-update_start_time << endl;
944 MPI_Send(MPI_BOTTOM, 1, parent_acc_type, dest, PSIM_TAG_ACC, MPI_COMM_WORLD);
946 cerr <<
"[" <<
sim->rank <<
"] send_acc(1) t = " << MPI_Wtime()-update_start_time << endl;
955 if(
sim->rank != rank)
return;
986 if(
sim->rank != rank)
return;
1005 for(j=0; j<
n_dof; j++)
1096 static fVec db(6), Wdb(6), IWRtau(6);
1105 for(j=0; j<
n_dof; j++)
1177 void pSubChain::recv_force()
1185 MPI_Recv(
buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1189 MPI_Recv(
buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1191 MPI_Recv(
buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1194 double time1 = MPI_Wtime();
1196 MPI_Recv(MPI_BOTTOM, 1, parent_force_type, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1198 double time2 = MPI_Wtime();
1199 sim->force_wait_time += time2-time1;
1204 void pSubChain::send_force(
int dest)
1211 MPI_Send(
buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1215 MPI_Send(
buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1217 MPI_Send(
buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1219 MPI_Send(MPI_BOTTOM, 1, parent_force_type, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1229 cerr <<
"---- " << target_joint->
name <<
": disassembly_leaf" << endl;
1231 static fVec3 total_f, total_n;
1233 static fMat33 att, t_att;
1235 static fVec allf(6);
1244 static fVec3 loc_f, loc_n,
f,
n, fn;
1245 static fVec3 jpos, rel_pos, pp;
1246 static fMat33 jatt, rel_att;
1258 rel_pos.
mul(t_att, pp);
1259 rel_att.
mul(t_att, jatt);
1260 f.mul(rel_att, loc_f);
1261 n.mul(rel_att, loc_n);
1264 cerr <<
"(f n) = " <<
f <<
n << endl;
1268 allf(0) = total_f(0);
1269 allf(1) = total_f(1);
1270 allf(2) = total_f(2);
1271 allf(3) = total_n(0);
1272 allf(4) = total_n(1);
1273 allf(5) = total_n(2);
1274 cerr <<
"total_f = " << total_f << endl;
1275 cerr <<
"total_n = " << total_n << endl;
1278 cerr <<
"Minv = " <<
links[0]->
Minv << endl;
1279 cerr <<
"acc = " <<
tran(
links[0]->acc) << endl;
1280 cerr <<
"link acc = " <<
tran(acc) << endl;
1288 update_log <<
"disassembly_body" << endl;
1291 if(
sim->rank != rank)
return;
1303 cerr <<
"[" <<
sim->rank <<
"] " <<
last_joint->
name <<
" enter disassembly t = " << MPI_Wtime()-update_start_time << endl;
1306 static fVec KLf, Lf(6), Lf_temp[2], v(6),
f, f_final(6);
1327 Lf.
sub(Lf_temp[0], Lf_temp[1]);
1347 #else // #ifndef USE_DCA (DCA test)
1348 static fVec vp(6), svp;
1378 f_final.
mul(Vhat, pp);