Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "chain.h"
00015
00024 int Chain::IntegrateAdaptive(double& timestep, int step, double min_timestep, double max_integ_error)
00025 {
00026 root->pre_integrate();
00027 int i;
00028 double half_step = timestep * 0.5;
00029 if(step == 0)
00030 {
00031
00032
00033
00034 for(i=0; i<n_value; i++)
00035 {
00036 j_value_dot[0][i] = *all_value_dot[i];
00037 init_value[i] = *all_value[i];
00038 j_value_dot[1][i] = init_value[i] + j_value_dot[0][i] * timestep;
00039 *all_value[i] += j_value_dot[0][i] * half_step;
00040 }
00041 for(i=0; i<n_dof; i++)
00042 {
00043 j_acc_p[0][i] = *all_vel_dot[i];
00044 init_vel[i] = *all_vel[i];
00045 j_acc_p[1][i] = init_vel[i] + j_acc_p[0][i] * timestep;
00046 *all_vel_dot[i] += j_acc_p[0][i] * half_step;
00047 }
00048 }
00049 else if(step == 1)
00050 {
00051
00052 double d, value_error = 0.0, vel_error = 0.0, total_error;
00053 for(i=0; i<n_value; i++)
00054 {
00055 d = *all_value[i] + *all_value_dot[i] * half_step - j_value_dot[1][i];
00056 value_error += d*d;
00057 }
00058 for(i=0; i<n_dof; i++)
00059 {
00060 d = *all_vel[i] + *all_vel_dot[i] * half_step - j_acc_p[1][i];
00061 vel_error += d*d;
00062 }
00063 double new_timestep;
00064
00065 total_error = sqrt(value_error+vel_error) / (n_value+n_dof);
00066
00067 new_timestep = timestep * sqrt(max_integ_error / total_error);
00068 if(new_timestep < min_timestep)
00069 new_timestep = min_timestep;
00070 if(new_timestep > timestep)
00071 new_timestep = timestep;
00072 timestep = new_timestep;
00073 for(i=0; i<n_value; i++)
00074 {
00075 *all_value[i] = init_value[i] + j_value_dot[0][i] * timestep;
00076 }
00077 for(i=0; i<n_dof; i++)
00078 {
00079 *all_vel[i] = init_vel[i] + j_acc_p[0][i] * timestep;
00080 }
00081 }
00082 root->post_integrate();
00083 return 0;
00084 }
00085
00086 int Chain::IntegrateValue(double timestep)
00087 {
00088 root->pre_integrate();
00089 int i;
00090 for(i=0; i<n_value; i++)
00091 {
00092 *all_value[i] += timestep * *all_value_dot[i];
00093 }
00094 root->post_integrate();
00095 return 0;
00096 }
00097
00098 int Chain::IntegrateVelocity(double timestep)
00099 {
00100 root->pre_integrate();
00101 int i;
00102 for(i=0; i<n_dof; i++)
00103 {
00104 *all_vel[i] += timestep * *all_vel_dot[i];
00105 }
00106 root->post_integrate();
00107 return 0;
00108 }
00109
00110 int Chain::Integrate(double timestep)
00111 {
00112 root->pre_integrate();
00113 int i;
00114 for(i=0; i<n_value; i++)
00115 {
00116 *all_value[i] += timestep * *all_value_dot[i];
00117 }
00118 for(i=0; i<n_dof; i++)
00119 {
00120 *all_vel[i] += timestep * *all_vel_dot[i];
00121 }
00122 root->post_integrate();
00123 return 0;
00124 }
00125
00126 int Chain::IntegrateRK4Value(double timestep, int step)
00127 {
00128 root->pre_integrate();
00129 int i;
00130
00131 for(i=0; i<n_value; i++)
00132 {
00133 j_value_dot[step][i] = *all_value_dot[i];
00134 }
00135
00136 if(step == 0)
00137 {
00138 for(i=0; i<n_value; i++)
00139 {
00140 init_value[i] = *all_value[i];
00141 *all_value[i] += j_value_dot[0][i] * timestep * 0.5;
00142 }
00143 }
00144 else if(step == 1)
00145 {
00146 for(i=0; i<n_value; i++)
00147 {
00148 *all_value[i] = init_value[i] + j_value_dot[1][i] * timestep * 0.5;
00149 }
00150 }
00151 else if(step == 2)
00152 {
00153 for(i=0; i<n_value; i++)
00154 {
00155 *all_value[i] = init_value[i] + j_value_dot[2][i] * timestep;
00156 }
00157 }
00158 else if(step == 3)
00159 {
00160 for(i=0; i<n_value; i++)
00161 {
00162 *all_value[i] = init_value[i] +
00163 (j_value_dot[0][i] + 2.0*(j_value_dot[1][i] + j_value_dot[2][i]) + j_value_dot[3][i]) * timestep / 6.0;
00164 }
00165 }
00166 root->post_integrate();
00167 return 0;
00168 }
00169
00170 int Chain::IntegrateRK4Velocity(double timestep, int step)
00171 {
00172 root->pre_integrate();
00173 int i;
00174
00175 for(i=0; i<n_dof; i++)
00176 {
00177 j_acc_p[step][i] = *all_vel_dot[i];
00178 }
00179
00180 if(step == 0)
00181 {
00182 for(i=0; i<n_dof; i++)
00183 {
00184 init_vel[i] = *all_vel[i];
00185 *all_vel[i] += j_acc_p[0][i] * timestep * 0.5;
00186 }
00187 }
00188 else if(step == 1)
00189 {
00190 for(i=0; i<n_dof; i++)
00191 {
00192 *all_vel[i] = init_vel[i] + j_acc_p[1][i] * timestep * 0.5;
00193 }
00194 }
00195 else if(step == 2)
00196 {
00197 for(i=0; i<n_dof; i++)
00198 {
00199 *all_vel[i] = init_vel[i] + j_acc_p[2][i] * timestep;
00200 }
00201 }
00202 else if(step == 3)
00203 {
00204 for(i=0; i<n_dof; i++)
00205 {
00206 *all_vel[i] = init_vel[i] +
00207 (j_acc_p[0][i] + 2.0*(j_acc_p[1][i] + j_acc_p[2][i]) + j_acc_p[3][i]) * timestep / 6.0;
00208 }
00209 }
00210 root->post_integrate();
00211 return 0;
00212 }
00213
00214 int Chain::IntegrateRK4(double timestep, int step)
00215 {
00216 root->pre_integrate();
00217 int i;
00218 double half_step = timestep * 0.5;
00219
00220 for(i=0; i<n_value; i++)
00221 {
00222 j_value_dot[step][i] = *all_value_dot[i];
00223 }
00224 for(i=0; i<n_dof; i++)
00225 {
00226 j_acc_p[step][i] = *all_vel_dot[i];
00227 }
00228
00229 if(step == 0)
00230 {
00231 for(i=0; i<n_value; i++)
00232 {
00233 init_value[i] = *all_value[i];
00234 *all_value[i] += j_value_dot[0][i] * half_step;
00235 }
00236 for(i=0; i<n_dof; i++)
00237 {
00238 init_vel[i] = *all_vel[i];
00239 *all_vel[i] += j_acc_p[0][i] * half_step;
00240 }
00241 }
00242 else if(step == 1)
00243 {
00244 for(i=0; i<n_value; i++)
00245 {
00246 *all_value[i] = init_value[i] + j_value_dot[1][i] * half_step;
00247 }
00248 for(i=0; i<n_dof; i++)
00249 {
00250 *all_vel[i] = init_vel[i] + j_acc_p[1][i] * half_step;
00251 }
00252 }
00253 else if(step == 2)
00254 {
00255 for(i=0; i<n_value; i++)
00256 {
00257 *all_value[i] = init_value[i] + j_value_dot[2][i] * timestep;
00258 }
00259 for(i=0; i<n_dof; i++)
00260 {
00261 *all_vel[i] = init_vel[i] + j_acc_p[2][i] * timestep;
00262 }
00263 }
00264 else if(step == 3)
00265 {
00266 for(i=0; i<n_value; i++)
00267 {
00268 *all_value[i] = init_value[i] +
00269 (j_value_dot[0][i] + 2.0*(j_value_dot[1][i] + j_value_dot[2][i]) + j_value_dot[3][i]) * timestep / 6.0;
00270 }
00271 for(i=0; i<n_dof; i++)
00272 {
00273 *all_vel[i] = init_vel[i] +
00274 (j_acc_p[0][i] + 2.0*(j_acc_p[1][i] + j_acc_p[2][i]) + j_acc_p[3][i]) * timestep / 6.0;
00275 }
00276 }
00277 root->post_integrate();
00278 return 0;
00279 }
00280
00281 int Joint::pre_integrate()
00282 {
00283
00284 switch(j_type)
00285 {
00286 case JROTATE:
00287 case JSLIDE:
00288 break;
00289 case JSPHERE:
00290 p_ang_vel.mul(rel_att, rel_ang_vel);
00291 p_ep_dot.angvel2epdot(rel_ep, rel_ang_vel);
00292 p_ang_acc.mul(rel_att, rel_ang_acc);
00293 break;
00294 case JFREE:
00295 p_lin_vel.mul(rel_att, rel_lin_vel);
00296 p_ang_vel.mul(rel_att, rel_ang_vel);
00297 p_ep_dot.angvel2epdot(rel_ep, rel_ang_vel);
00298 p_lin_acc.mul(rel_att, rel_lin_acc);
00299 p_ang_acc.mul(rel_att, rel_ang_acc);
00300 break;
00301 default:
00302 break;
00303 }
00304 child->pre_integrate();
00305 brother->pre_integrate();
00306 return 0;
00307 }
00308
00309 int Joint::post_integrate()
00310 {
00311
00312 fMat33 ratt;
00313 switch(j_type)
00314 {
00315 case JROTATE:
00316 case JSLIDE:
00317 SetJointValue(q);
00318 SetJointVel(qd);
00319 break;
00320 case JSPHERE:
00321 rel_ep.unit();
00322 rel_att.set(rel_ep);
00323 ratt.tran(rel_att);
00324 rel_ang_vel.mul(ratt, p_ang_vel);
00325 break;
00326 case JFREE:
00327 rel_ep.unit();
00328 rel_att.set(rel_ep);
00329 ratt.tran(rel_att);
00330 rel_lin_vel.mul(ratt, p_lin_vel);
00331 rel_ang_vel.mul(ratt, p_ang_vel);
00332 break;
00333 default:
00334 break;
00335 }
00336 child->post_integrate();
00337 brother->post_integrate();
00338 return 0;
00339 }