fk.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  * fk.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.19
00012  */
00013 
00014 #include "chain.h"
00015 
00016 void Chain::CalcPosition()
00017 {
00018         root->calc_position();
00019 }
00020 
00021 void Joint::calc_position()
00022 {
00023         if(parent)
00024         {
00025 #if 0
00026                 // readable, but slower version
00027                 abs_pos = parent->abs_pos + parent->abs_att * rel_pos;
00028                 abs_att = parent->abs_att * rel_att;
00029 #else
00030                 // faster version
00031                 abs_pos.mul(parent->abs_att, rel_pos);
00032                 abs_pos += parent->abs_pos;
00033                 abs_att.mul(parent->abs_att, rel_att);
00034 #endif
00035 //              cout << name << ": " << abs_pos << endl;
00036         }
00037         child->calc_position();
00038         brother->calc_position();
00039 }
00040 
00041 void Chain::CalcVelocity()
00042 {
00043         root->calc_velocity();
00044 }
00045 
00046 void Joint::calc_velocity()
00047 {
00048         if(parent)
00049         {
00050                 static fMat33 t_rel_att;
00051                 static fVec3 v1, v2;
00052                 t_rel_att.tran(rel_att);
00053                 // compute loc_lin_vel
00054                 v1.cross(parent->loc_ang_vel, rel_pos);
00055                 v2.mul(t_rel_att, parent->loc_lin_vel);
00056                 loc_lin_vel.mul(t_rel_att, v1);
00057                 loc_lin_vel += v2;
00058                 loc_lin_vel += rel_lin_vel;
00059                 // compute loc_ang_vel
00060                 loc_ang_vel.mul(t_rel_att, parent->loc_ang_vel);
00061                 loc_ang_vel += rel_ang_vel;
00062                 // compute loc_com_vel
00063                 v1.cross(loc_ang_vel, loc_com);
00064                 loc_com_vel.add(loc_lin_vel, v1);
00065         }
00066         else
00067         {
00068                 loc_lin_vel.zero();
00069                 loc_ang_vel.zero();
00070         }
00071         child->calc_velocity();
00072         brother->calc_velocity();
00073 }
00074 
00075 void Chain::CalcAcceleration()
00076 {
00077         root->calc_acceleration();
00078 }
00079 
00080 void Joint::calc_acceleration()
00081 {
00082         if(parent)
00083         {
00084                 static fMat33 t_rel_att;
00085                 static fVec3 v1, v2, v3, v4;
00086                 t_rel_att.tran(rel_att);
00087                 // Éð»Ê±ñ1¡¦x
00088                 v1.mul(t_rel_att, parent->loc_ang_vel);
00089 //              v1 += loc_ang_vel;
00090                 v1 *= 2.0;
00091                 v2.cross(v1, rel_lin_vel);
00092                 loc_lin_acc.add(rel_lin_acc, v2);
00093                 v2.cross(parent->loc_ang_acc, rel_pos);
00094                 v1.add(parent->loc_lin_acc, v2);
00095                 v2.cross(parent->loc_ang_vel, rel_pos);
00096                 v3.cross(parent->loc_ang_vel, v2);
00097                 v1 += v3;
00098                 v4.mul(t_rel_att, v1);
00099                 loc_lin_acc += v4;
00100                 // ³Ñ±ñ1¡¦x
00101 //              v1.cross(loc_ang_vel, parent->rel_ang_vel);
00102                 v1.cross(loc_ang_vel, rel_ang_vel);
00103                 loc_ang_acc.add(rel_ang_acc, v1);
00104                 v1.mul(t_rel_att, parent->loc_ang_acc);
00105                 loc_ang_acc += v1;
00106                 // ½Å¿´£öÅð»Ê±ñ1¡¦x
00107                 v1.cross(loc_ang_acc, loc_com);
00108                 loc_com_acc.add(loc_lin_acc, v1);
00109                 v1.cross(loc_ang_vel, loc_com);
00110                 v2.cross(loc_ang_vel, v1);
00111                 loc_com_acc += v2;
00112 
00113 //              cerr << name << ": loc_lin_acc = " << loc_lin_acc << endl;
00114 //              cerr << name << ": loc_ang_acc = " << loc_ang_acc << endl;
00115 //              if(real)
00116 //                      cerr << name << ": acc = " << abs_att*loc_lin_acc << abs_att*loc_ang_acc << endl;
00117         }
00118         else
00119         {
00120                 // ¥ë¡¼¥È¥ê¥ó¥¯
00121                 // loc_lin_acc£õ"¡¦dÍ÷)¡¦¡¦x£åÂð€¡¦Ãà¡à¡¦Ëࡦ±êù硦¡¦Çà
00122                 loc_ang_acc.zero();
00123                 loc_com_acc.zero();
00124         }
00125         child->calc_acceleration();
00126         brother->calc_acceleration();
00127 }
00128 
00129 double Chain::TotalCOM(fVec3& com, const char* chname)
00130 {
00131         com.zero();
00132         double m = root->total_com(com, chname);
00133         com /= m;
00134         return m;
00135 }
00136 
00137 double Joint::total_com(fVec3& com, const char* chname)
00138 {
00139         int is_target = false;
00140         if(!chname)
00141         {
00142                 is_target = true;
00143         }
00144         else
00145         {
00146                 char* my_chname = CharName();
00147                 if(my_chname && !strcmp(my_chname, chname))
00148                 {
00149                         is_target = true;
00150                 }
00151         }
00152         fVec3 b_com, c_com;
00153         b_com.zero();
00154         c_com.zero();
00155         double ret = brother->total_com(b_com, chname) + child->total_com(c_com, chname);
00156 
00157         com.add(b_com, c_com);
00158         if(is_target)
00159         {
00160                 static fVec3 abs_com_pos, my_com;
00161                 ret += mass;
00162                 abs_com_pos.mul(abs_att, loc_com);
00163                 abs_com_pos += abs_pos;
00164                 my_com.mul(abs_com_pos, mass);
00165                 com += my_com;
00166         }
00167         return ret;
00168 }
00169 


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