integ.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  * integ.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.07.11
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                 // save derivatives at t
00032                 // save initial value/vel and compute value/vel at t+dt
00033                 // set value/vel in the middle
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                 // compute and save value/vel after timestep
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                 // compare x1 and x2
00065                 total_error = sqrt(value_error+vel_error) / (n_value+n_dof);
00066                 // integrate with new timestep
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         // save derivatives
00131         for(i=0; i<n_value; i++)
00132         {
00133                 j_value_dot[step][i] = *all_value_dot[i];
00134         }
00135         // prepare for the next step
00136         if(step == 0)
00137         {
00138                 for(i=0; i<n_value; i++)
00139                 {
00140                         init_value[i] = *all_value[i];  // save init value
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         // save derivatives
00175         for(i=0; i<n_dof; i++)
00176         {
00177                 j_acc_p[step][i] = *all_vel_dot[i];
00178         }
00179         // prepare for the next step
00180         if(step == 0)
00181         {
00182                 for(i=0; i<n_dof; i++)
00183                 {
00184                         init_vel[i] = *all_vel[i];  // save init vel
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         // save derivatives
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         // prepare for the next step
00229         if(step == 0)
00230         {
00231                 for(i=0; i<n_value; i++)
00232                 {
00233                         init_value[i] = *all_value[i];  // save init value
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];  // save init vel
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         // compute p_lin_vel, p_ep_dot, p_ang_vel, p_lin_acc, p_ang_acc
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         // compute rel_att, rel_lin_vel, rel_ang_vel,
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 }


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:54