KinematicTree.h

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #ifndef __KINEMATIC_TREE__
00020 #define __KINEMATIC_TREE__
00021 
00027 #define KINEMATIC_TREE_VIEW
00028 #define NO_CSOLID
00029 
00030 
00031 #ifdef NO_CSOLID
00032 typedef void *  pCSolidTree_t;
00033 #include "TreeParser.h"
00034 #else
00035 #include "SolidTree.h"
00036 #endif
00037 
00038 
00039 #include "KinematicChain.h"
00040 
00041 #define COLLISION
00042 #define WITH_RANGES
00043 
00044 #define MAX_CHAINS 15
00045 
00046 
00047 class ArticulatedTree;
00048 
00049 typedef ArticulatedTree  ArticulatedTree_t, *pArticulatedTree_t; 
00050 typedef vector<pArticulatedTree_t> ArticulatedTreeList_t;
00051 
00052 
00057 class ArticulatedTree
00058 {
00059  protected:
00060   RigidTransfo joint;
00061   ArticulatedTree *parent;
00062   ArticulatedTreeList_t children;
00063 
00064 #ifdef WITH_RANGES
00065   float range[2];
00066 #endif
00067 
00068 
00069  public:
00070   string name;
00071  public:
00072   ArticulatedTree();
00073 
00074   ArticulatedTree(const pTree tree);
00075   ArticulatedTree(const pArticulatedTree_t tree);
00076   virtual ~ArticulatedTree();
00077   void CopyTree(pArticulatedTree_t tree);
00078   void AddTree(pArticulatedTree_t ptree);
00079   virtual void StreamTree(ostream& out)const;
00080   int GetSize() const;
00081   //  void Invert(int i=0) {if(i*inverse<0){joint.Invert();inverse=i;}}
00082   pArticulatedTree_t FindJoint(string& name);
00083   pArticulatedTree_t FindSubTree(pArticulatedTree_t t, pArticulatedTree_t exclude=NULL);
00084  
00085   pArticulatedTree_t GetParent(){return parent;}
00086   ArticulatedTreeList_t *GetChildren(){return &children;}
00087   RigidTransfo *GetJoint(){return &joint;};
00088   void RandomAxis();
00089   void NoTranslation();
00090   void RandomAngle();
00091   void ZeroPosition();
00092   void Reshape(CVector3_t offset=NULL);
00093   void Reshape2(CVector3_t offset=NULL);
00094   void GetAxis(CVector3_t axis){joint.GetRotationAxis(axis);}
00100   int CheckPairsOfNodes(int (*f)(pArticulatedTree_t,pArticulatedTree_t,void *),void *arg);
00101   int CheckTreeNode1(pArticulatedTree_t tree,
00102                         int (*f)(pArticulatedTree_t,pArticulatedTree_t,void *),void *arg);
00103   int CheckTreeNode2(pArticulatedTree_t tree,
00104                         int (*f)(pArticulatedTree_t,pArticulatedTree_t,void *),void *arg);
00109   int GetAngleList(float *list);
00110   int SetAngleList(float *list);
00111 #ifdef WITH_RANGES
00112   int GetAngleRangeList(float *list_min, float *list_max);
00113     float GetLowerRange(){return range[0];}
00114     float GetUpperRange(){return range[1];}
00118     int FindAngleRanges(float *lower_range, float *upper_range,KinematicChain *chain);
00119 #endif
00120 
00126     int Serialize(float *data,int data_size)const;
00132     int Deserialize(const float *data,int data_size);
00133   void PrintList();
00134   void PrintAngles();
00135 
00136     // not used here
00137 #ifndef NO_CSOLID
00138   ArticulatedTree(const pCSolidTree_t stree);
00139   void FillCSolidTree(pCSolidTree_t solid);
00140  pCSolidTree_t CreateCSolidTreeStruct();
00141 #endif
00142 
00143 
00144 };
00145 
00146 
00147 
00152 class KinematicTree
00153 {
00154  protected:
00155   pTree xml_tree;
00156   pArticulatedTree_t root;
00157   pCSolidTree_t solid;
00158   KinematicChain chains[MAX_CHAINS];
00159   int nb_chains;
00160 
00161  protected:
00162   int LoadChainRec(pArticulatedTree_t from, pArticulatedTree_t to, KinematicChain *chain,
00163                    pArticulatedTree_t exclude=NULL)const;
00164 public:
00165     KinematicTree();
00166     KinematicTree(const char *filename);
00167     virtual ~KinematicTree();
00168     void FreeChains(){for(int i=0;i<MAX_CHAINS;i++)chains[i].FreeChain();}
00169     
00170 
00179   int LoadChain(pArticulatedTree_t from, pArticulatedTree_t to,
00180                 int i,modality_t mod=UNSPECIFIED_MODALITY);
00184  int LoadChain(pArticulatedTree_t from, pArticulatedTree_t to,
00185                 KinematicChain *chain,modality_t mod=UNSPECIFIED_MODALITY)const;
00189  int LoadChain(string& from, string& to, KinematicChain *chain,
00190                 modality_t mod=UNSPECIFIED_MODALITY);
00194  int LoadChain(string& from, string& to, int i,
00195                 modality_t mod=UNSPECIFIED_MODALITY); 
00199     int LoadChain(const char *from, const char *to, KinematicChain *chain,
00200                   modality_t mod=UNSPECIFIED_MODALITY)
00201     {string s1(from),s2(to);return  LoadChain(s1, s2, chain,mod);}
00205     int LoadChain(const char *from, const char *to,int i,
00206                   modality_t mod=UNSPECIFIED_MODALITY)
00207     {string s1(from),s2(to);return  LoadChain(s1, s2,i,mod);}
00208     
00209     KinematicChain *GetChain(int i){return &(chains[i]);}
00210     KinematicChain *GetNewChain(string& from, string& to,
00211                               modality_t mod=UNSPECIFIED_MODALITY);
00212   pArticulatedTree_t GetArticulatedTree(){return root;}
00213 
00219     int GetTreeSize(){return root?root->GetSize():0;}  
00220   virtual int CheckAllCollisions(KinematicTree *update_tree);
00221 
00233   static int CollisionDetection(pArticulatedTree_t t1, pArticulatedTree_t t2, void *arg);
00234 };
00235 
00236 
00237 ostream& operator<<(ostream& out, const ArticulatedTree& at);
00238 
00239 
00240 typedef struct {
00241   KinematicChain* kc;
00242   KinematicTree *kt;
00243   CVector3_t pos_stack[MAX_LINKS];
00244   CVector3_t *top;
00245   pArticulatedTree_t t1;
00246   pArticulatedTree_t t2;
00247   RigidTransfo rt;
00248   float dist;
00249   KinematicTree *kt_update;
00250   KinematicChain kc_update;
00251 } collision_arg_t;
00252 
00253 
00254 #endif
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:25 2010 for Body_Schema_Learning by  doxygen 1.6.1