id.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  * id.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.08.29
00012  */
00013 
00014 #include "chain.h"
00015 
00016 void Chain::InvDyn(fVec& tau)
00017 {
00018         CalcAcceleration();
00019         root->inv_dyn();
00020         root->calc_joint_force(tau);
00021 }
00022 
00023 void Joint::inv_dyn()
00024 {
00025         inv_dyn_1();
00026         child->inv_dyn();
00027         brother->inv_dyn();
00028         inv_dyn_2();
00029         //      cerr << name << ": force = " << joint_f << joint_n << endl;
00030 }
00031 
00032 void Joint::inv_dyn_1()
00033 {
00034         // compute total force around com
00035         static fVec3 v1, v2;
00036         total_f.mul(mass, loc_com_acc);
00037         v1.mul(inertia, loc_ang_vel);
00038         v2.cross(loc_ang_vel, v1);
00039         total_n.mul(inertia, loc_ang_acc);
00040         total_n += v2;
00041 
00042         joint_f.zero();
00043         joint_n.zero();
00044 }
00045 
00046 void Joint::inv_dyn_2()
00047 {
00048         static fVec3 v1;
00049         joint_f += total_f;
00050         joint_n += total_n;
00051         v1.cross(loc_com, joint_f);
00052         joint_n += v1;
00053         // external force/moment
00054         joint_f -= ext_force;
00055         joint_n -= ext_moment;
00056 //      cout << name << ": joint_f = " << joint_f << ", joint_n = " << joint_n << endl;
00057         if(parent)
00058         {
00059                 // force
00060                 v1.mul(rel_att, joint_f);
00061                 parent->joint_f += v1;
00062                 // moment
00063                 v1.mul(rel_att, joint_n);
00064                 parent->joint_n += v1;
00065                 static fVec3 p, f;
00066                 f.mul(rel_att, joint_f);
00067                 p.sub(parent->loc_com, rel_pos);
00068                 v1.cross(p, f);
00069                 parent->joint_n -= v1;
00070         }
00071 }
00072 
00073 void Joint::calc_joint_force(fVec& tau)
00074 {
00075         double t;
00076 //      cerr << name << ": force = " << joint_f << joint_n << endl;
00077         if(i_dof >= 0)
00078         {
00079                 switch(j_type)
00080                 {
00081                 case JROTATE:
00082                         t = axis * joint_n;
00083                         tau(i_dof) = t;
00084                         break;
00085                 case JSLIDE:
00086                         t = axis * joint_f;
00087                         tau(i_dof) = t;
00088                         break;
00089                 case JSPHERE:
00090                         tau(i_dof+0) = joint_n(0);
00091                         tau(i_dof+1) = joint_n(1);
00092                         tau(i_dof+2) = joint_n(2);
00093                         break;
00094                 case JFREE:
00095                         tau(i_dof+0) = joint_f(0);
00096                         tau(i_dof+1) = joint_f(1);
00097                         tau(i_dof+2) = joint_f(2);
00098                         tau(i_dof+3) = joint_n(0);
00099                         tau(i_dof+4) = joint_n(1);
00100                         tau(i_dof+5) = joint_n(2);
00101                         break;
00102                 default:
00103                         break;
00104                 }
00105         }
00106         child->calc_joint_force(tau);
00107         brother->calc_joint_force(tau);
00108 }
00109 


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