treeidsolver_recursive_newton_euler.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00025 
00026 
00027 
00028 #include "treeidsolver_recursive_newton_euler.hpp"
00029 #include "kdl/frames_io.hpp"
00030 
00031 #include <deque>
00032 using namespace std;
00033 
00034 namespace KDL{
00035     
00036     TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree_,Vector grav)
00037     :tree(tree_)
00038     {
00039                 root_name = tree.getRootSegment()->first;
00040                 
00041         ag=-Twist(grav,Vector::Zero());
00042         const SegmentMap& sm = tree.getSegments();
00043         
00044         int jnt =0;
00045         for( SegmentMap::const_iterator i=sm.begin(); i!=sm.end(); ++i ){
00046                         const Segment& s = i->second.segment;
00047                         const Joint& j = s.getJoint();
00048                         if( j.getType() != Joint::None ){
00049                                 JntEntry& je = jntdb[ j.getName() ];
00050                                 je.idx = jnt++;
00051                         }
00052                                 
00053                                 
00054                         const string& name = i->first;
00055                         db[ name ]; // create the element here, but don't have anything to fill in
00056                         /*
00057                         X[ name ];
00058                         S[ name ];
00059                         v[ name ];
00060                         a[ name ];
00061                         f[ name ];
00062                         f_ext_map[ name ];
00063                         */
00064                 }
00065     }
00066 
00067     int TreeIdSolver_RNE::CartToJnt(const std::vector<double> &q, const std::vector<double> &q_dot, const std::vector<double> &q_dotdot, const Wrenches& f_ext,JntArray &torques)
00068     {
00069         
00070         {
00071                         const SegmentMap& sm = tree.getSegments();
00072                         int cnt =0;
00073                         for( SegmentMap::const_iterator i=sm.begin(); i!=sm.end(); ++i ){
00074                                 Entry& e = db[ i->first ];
00075                                 e.f_ext = f_ext[ cnt++ ];
00076                         }
00077                 }
00078         
00079         
00082         deque< SegmentMap::const_iterator > open;
00083         vector< SegmentMap::const_iterator > processed; 
00084         open.push_back( tree.getRootSegment() );
00085 
00086         //Sweep from root to leaf
00087         while( !open.empty() ){
00088                         SegmentMap::const_iterator te = open.front();
00089                         open.pop_front();
00090                         processed.push_back( te );
00091                         for( std::vector< SegmentMap::const_iterator >::const_iterator i=te->second.children.begin();
00092                              i != te->second.children.end();
00093                              ++i )
00094                         {
00095                                 open.push_back( *i );
00096                         }
00097                         
00098                         
00099                         const Segment& seg = te->second.segment;
00100                         const Joint& jnt = seg.getJoint();
00101                         
00102             double q_,qdot_,qdotdot_;
00103             if(jnt.getType() !=Joint::None){
00104                                 
00105                                 int idx = jntdb[ jnt.getName() ].idx;
00106                 q_=q[idx];
00107                 qdot_=q_dot[idx];
00108                 qdotdot_=q_dotdot[idx];
00109             }else
00110                 q_=qdot_=qdotdot_=0.0;
00111                 
00112             Entry& e = db[seg.getName()];
00113                 
00114             Frame& eX  = e.X;
00115             Twist& eS  = e.S;
00116             Twist& ev  = e.v;
00117             Twist& ea  = e.a;
00118             Wrench& ef = e.f;
00119             Wrench& ef_ext = e.f_ext;
00120             
00121             string parent_name = "fake";
00122             if( te->first != root_name )
00123                                 parent_name = te->second.parent->first;
00124             
00125             Entry& parent_entry = db[parent_name];
00126             Twist& parent_a = parent_entry.a;
00127             Twist& parent_v = parent_entry.v;
00128             
00129             //Calculate segment properties: X,S,vj,cj
00130             eX=seg.pose(q_);//Remark this is the inverse of the 
00131                             //frame for transformations from 
00132                             //the parent to the current coord frame
00133             //Transform velocity and unit velocity to segment frame
00134             Twist vj=eX.M.Inverse(seg.twist(q_,qdot_));
00135             eS=eX.M.Inverse(seg.twist(q_,1.0));
00136             //We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
00137             //calculate velocity and acceleration of the segment (in segment coordinates)
00138             if( te->first == root_name ){
00139                 ev=vj;
00140                 ea=eX.Inverse(ag)+eS*qdotdot_+ev*vj;
00141             }else{
00142                 ev=eX.Inverse(parent_v)+vj;
00143                 ea=eX.Inverse(parent_a)+eS*qdotdot_+ev*vj;
00144             }
00145             //Calculate the force for the joint
00146             //Collect RigidBodyInertia and external forces
00147             RigidBodyInertia Ii=seg.getInertia();
00148             ef=Ii*ea+ev*(Ii*ev)-ef_ext;
00149             //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
00150         }
00151         // process processed back to front...        
00152         //Sweep from leaf to root
00153         int size = processed.size()-1;
00154         for(int i=size; i>0; i--){
00155                         SegmentMap::const_iterator te = processed[i];
00156                         const Segment& seg = te->second.segment;
00157                         const Joint& jnt = seg.getJoint();
00158                         Entry& e = db[seg.getName()];
00159                         Frame& eX = e.X;
00160             Twist& eS = e.S;
00161             Wrench& ef = e.f;
00162             string parent_name = te->second.parent->first;
00163                         Entry& parent_e = db[parent_name];
00164                         Wrench& pre_f = parent_e.f;
00165                         
00166                         
00167             if(jnt.getType()!=Joint::None)
00168                 jntdb[jnt.getName()].torque=dot(eS,ef);
00169             pre_f +=eX*ef;
00170         }
00171         // do root element
00172         // probably could hard code this lookup to make a little bit faster
00173         { 
00174                         const TreeElement& te = tree.getRootSegment()->second;
00175                         const Segment& seg = te.segment;
00176                         const Joint& jnt = seg.getJoint();
00177                         Entry& e = db[root_name];
00178                         if( jnt.getType() != Joint::None )
00179                                 jntdb[jnt.getName()].torque = dot( e.S, e.f );
00180                 }
00181                 for( map<string, JntEntry>::const_iterator i= jntdb.begin(); i!= jntdb.end(); ++i ){
00182                         torques(i->second.idx) = i->second.torque;
00183                 }
00184         return 0;
00185     }
00186 
00187     int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
00188     {
00189         {
00190                         const SegmentMap& sm = tree.getSegments();
00191                         int cnt =0;
00192                         for( SegmentMap::const_iterator i=sm.begin(); i!=sm.end(); ++i ){
00193                                 Entry& e = db[ i->first ];
00194                                 e.f_ext = f_ext[ cnt++ ];
00195                         }
00196                 }
00197         
00200         deque< SegmentMap::const_iterator > open;
00201         vector< SegmentMap::const_iterator > processed; 
00202         open.push_back( tree.getRootSegment() );
00203 
00204         //Sweep from root to leaf
00205         while( !open.empty() ){
00206                         SegmentMap::const_iterator te = open.front();
00207                         open.pop_front();
00208                         processed.push_back( te );
00209                         for( std::vector< SegmentMap::const_iterator >::const_iterator i=te->second.children.begin();
00210                              i != te->second.children.end();
00211                              ++i )
00212                         {
00213                                 open.push_back( *i );
00214                         }
00215                         
00216                         
00217                         const Segment& seg = te->second.segment;
00218                         const Joint& jnt = seg.getJoint();
00219                         
00220             double q_,qdot_,qdotdot_;
00221             if(jnt.getType() !=Joint::None){
00222                                 
00223                                 int idx = jntdb[ jnt.getName() ].idx;
00224                 q_=q(idx);
00225                 qdot_=q_dot(idx);
00226                 qdotdot_=q_dotdot(idx);
00227             }else
00228                 q_=qdot_=qdotdot_=0.0;
00229                 
00230             Entry& e = db[seg.getName()];
00231                 
00232             Frame& eX  = e.X;
00233             Twist& eS  = e.S;
00234             Twist& ev  = e.v;
00235             Twist& ea  = e.a;
00236             Wrench& ef = e.f;
00237             Wrench& ef_ext = e.f_ext;
00238             
00239             string parent_name = "fake";
00240             if( te->first != root_name )
00241                                 parent_name = te->second.parent->first;
00242             
00243             Entry& parent_entry = db[parent_name];
00244             Twist& parent_a = parent_entry.a;
00245             Twist& parent_v = parent_entry.v;
00246             
00247             //Calculate segment properties: X,S,vj,cj
00248             eX=seg.pose(q_);//Remark this is the inverse of the 
00249                             //frame for transformations from 
00250                             //the parent to the current coord frame
00251             //Transform velocity and unit velocity to segment frame
00252             Twist vj=eX.M.Inverse(seg.twist(q_,qdot_));
00253             eS=eX.M.Inverse(seg.twist(q_,1.0));
00254             //We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
00255             //calculate velocity and acceleration of the segment (in segment coordinates)
00256             if( te->first == root_name ){
00257                 ev=vj;
00258                 ea=eX.Inverse(ag)+eS*qdotdot_+ev*vj;
00259             }else{
00260                 ev=eX.Inverse(parent_v)+vj;
00261                 ea=eX.Inverse(parent_a)+eS*qdotdot_+ev*vj;
00262             }
00263             //Calculate the force for the joint
00264             //Collect RigidBodyInertia and external forces
00265             RigidBodyInertia Ii=seg.getInertia();
00266             ef=Ii*ea+ev*(Ii*ev)-ef_ext;
00267             //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
00268         }
00269         // process processed back to front...        
00270         //Sweep from leaf to root
00271         int size = processed.size()-1;
00272         for(int i=size; i>0; i--){
00273                         SegmentMap::const_iterator te = processed[i];
00274                         const Segment& seg = te->second.segment;
00275                         const Joint& jnt = seg.getJoint();
00276                         Entry& e = db[seg.getName()];
00277                         Frame& eX = e.X;
00278             Twist& eS = e.S;
00279             Wrench& ef = e.f;
00280             string parent_name = te->second.parent->first;
00281                         Entry& parent_e = db[parent_name];
00282                         Wrench& pre_f = parent_e.f;
00283                         
00284                         
00285             if(jnt.getType()!=Joint::None)
00286                 jntdb[jnt.getName()].torque=dot(eS,ef);
00287             pre_f +=eX*ef;
00288         }
00289         // do root element
00290         // probably could hard code this lookup to make a little bit faster
00291         { 
00292                         const TreeElement& te = tree.getRootSegment()->second;
00293                         const Segment& seg = te.segment;
00294                         const Joint& jnt = seg.getJoint();
00295                         Entry& e = db[root_name];
00296                         if( jnt.getType() != Joint::None )
00297                                 jntdb[jnt.getName()].torque = dot( e.S, e.f );
00298                 }
00299                 for( map<string, JntEntry>::const_iterator i= jntdb.begin(); i!= jntdb.end(); ++i ){
00300                         torques(i->second.idx) = i->second.torque;
00301                 }
00302         return 0;
00303     }
00304 
00305 
00306 }//namespace


r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Mon Oct 6 2014 02:48:44