KinematicTree Class Reference

This class contains and ArticulatedTree and the KinematicChains to adapt it. More...

#include <KinematicTree.h>

Inherited by EmbodiedKinematicTree.

Public Member Functions

 KinematicTree (const char *filename)
void FreeChains ()
int LoadChain (pArticulatedTree_t from, pArticulatedTree_t to, int i, modality_t mod=UNSPECIFIED_MODALITY)
 Loads a chain that refers to a path in the kinematic tree as described in Hersch et al.
int LoadChain (pArticulatedTree_t from, pArticulatedTree_t to, KinematicChain *chain, modality_t mod=UNSPECIFIED_MODALITY) const
 valid only for a direct link
int LoadChain (string &from, string &to, KinematicChain *chain, modality_t mod=UNSPECIFIED_MODALITY)
int LoadChain (string &from, string &to, int i, modality_t mod=UNSPECIFIED_MODALITY)
int LoadChain (const char *from, const char *to, KinematicChain *chain, modality_t mod=UNSPECIFIED_MODALITY)
int LoadChain (const char *from, const char *to, int i, modality_t mod=UNSPECIFIED_MODALITY)
KinematicChainGetChain (int i)
KinematicChainGetNewChain (string &from, string &to, modality_t mod=UNSPECIFIED_MODALITY)
pArticulatedTree_t GetArticulatedTree ()
int GetTreeSize ()
 yields the size of a kinematicTree(number of nodes)
virtual int CheckAllCollisions (KinematicTree *update_tree)

Static Public Member Functions

static int CollisionDetection (pArticulatedTree_t t1, pArticulatedTree_t t2, void *arg)
 detects if there is a collision between link t1 and t2 and possibly updates a articulated tree accordingly.

Protected Member Functions

int LoadChainRec (pArticulatedTree_t from, pArticulatedTree_t to, KinematicChain *chain, pArticulatedTree_t exclude=NULL) const
 finds the path joining two nodes of a tree and builds the corresponding kinematic chain.

Protected Attributes

pTree xml_tree
pArticulatedTree_t root
pCSolidTree_t solid
KinematicChain chains [MAX_CHAINS]
int nb_chains

Detailed Description

This class contains and ArticulatedTree and the KinematicChains to adapt it.

Definition at line 152 of file KinematicTree.h.


Member Function Documentation

int KinematicTree::CollisionDetection ( pArticulatedTree_t  t1,
pArticulatedTree_t  t2,
void *  arg 
) [static]

detects if there is a collision between link t1 and t2 and possibly updates a articulated tree accordingly.

detects if there is a collision between link t1 and t2.

This works by creating a KinematicChain from t1 to t2 and computing the position of link t1 in the frame of ref of t2. Consecutive links are discarded, as they touch each other at the joint

Parameters:
dist the distance below which contact is assumed
t1 the first link
t2 the second link
Returns:
1 if there is a collision, 0 otherwise

Definition at line 656 of file KinematicTree.cpp.

00656                                                                                             {
00657   collision_arg_t *state;
00658   // CVector3_t v,v2,p1,p2;
00659   float k1,k2,distance=-1.0f;
00660   state = (collision_arg_t *) arg;
00661   //  KinematicChain kc;
00662   //  KinematicTree kt;
00663   //  RigidTransfo& rt = state->rt;
00664   if(t1->GetParent()==t2 || t2->GetParent()==t1){return 0;}
00665   if(t1->GetParent()==state->t1 && t2 == state->t2 && 0) {// and t2 is not a subtree of t1 -> just add a transformation  for optimizing processing speed
00666    //  rt = t1->GetJoint();
00667 //     state->kc->AddJoint(rt,-1);
00668 //     //  rt->InverseTransform(state->pos_stack[state->top],state->pos_stack[state->top-1]); 
00669 //     //  state->top--;
00670 //     rt = rt
00671 //     state->t1 = t1;
00672   }
00673   else{
00674     state->kt->LoadChain(t1,t2,state->kc, TOUCH_MODALITY);   
00675 }
00676 
00677    distance = state->kc->CollisionDetection(k1,k2);
00678   if(distance<0){
00679     return 0;
00680   }
00681   if(distance<=state->dist){
00682     //    cout<<t1->name<<" "<<t2->name<<" "<<distance<<endl;
00683     if(state->kt_update){
00684       float angles[MAX_LINKS];
00685       state->kc->GetAngles(angles);
00686        state->kt_update->LoadChain(t1->name,t2->name,&(state->kc_update),TOUCH_MODALITY);      
00687        state->kc_update.UpdateTouch(angles,k1,k2);
00688     }    
00689     return 1;
00690   }
00691   else{
00692     return 0;
00693   }
00694 }

int KinematicTree::GetTreeSize (  )  [inline]

yields the size of a kinematicTree(number of nodes)

Returns:
the number of nodes in a tree

Definition at line 219 of file KinematicTree.h.

Referenced by ReachingThread::init().

00219 {return root?root->GetSize():0;}  

int KinematicTree::LoadChain ( const char *  from,
const char *  to,
int  i,
modality_t  mod = UNSPECIFIED_MODALITY 
) [inline]
Returns:
the number of joints in the chain

Definition at line 205 of file KinematicTree.h.

References LoadChain().

Referenced by LoadChain().

00207     {string s1(from),s2(to);return  LoadChain(s1, s2,i,mod);}

int KinematicTree::LoadChain ( const char *  from,
const char *  to,
KinematicChain chain,
modality_t  mod = UNSPECIFIED_MODALITY 
) [inline]
Returns:
the number of joints in the chain

Definition at line 199 of file KinematicTree.h.

References LoadChain().

Referenced by LoadChain().

00201     {string s1(from),s2(to);return  LoadChain(s1, s2, chain,mod);}

int KinematicTree::LoadChain ( string &  from,
string &  to,
int  i,
modality_t  mod = UNSPECIFIED_MODALITY 
)
Returns:
the number of joints in the chain

Definition at line 619 of file KinematicTree.cpp.

References LoadChain().

00619                                                                             {
00620   return LoadChain(from,to,&(chains[i]),mod);
00621 }

int KinematicTree::LoadChain ( string &  from,
string &  to,
KinematicChain chain,
modality_t  mod = UNSPECIFIED_MODALITY 
)
Returns:
the number of joints in the chain

Definition at line 622 of file KinematicTree.cpp.

References LoadChain().

00622                                                                                         {
00623   pArticulatedTree_t f, t;
00624   f=root->FindJoint(from); 
00625   t=root->FindJoint(to);
00626   if(f && t){
00627     return LoadChain(f,t,kc,mod);
00628   }
00629   else{
00630     return -1;
00631   }
00632 }

int KinematicTree::LoadChain ( pArticulatedTree_t  from,
pArticulatedTree_t  to,
KinematicChain kc,
modality_t  mod = UNSPECIFIED_MODALITY 
) const

valid only for a direct link

Returns:
the number of joints in the chain

Extract the kinematic chain. The chain is assumed to end with a translation (the last rotation is discarded if the corresponding transformation is not inverted).

Definition at line 544 of file KinematicTree.cpp.

References KinematicChain::AddLastLink(), and LoadChainRec().

00545                                                                      {
00546 
00547  
00548     kc->Clear();
00549     kc->SetModality(mod);
00550     //    kc->AddLastLink(to->GetJoint());
00551     int ret= LoadChainRec(from,to,kc);
00552     if(ret){
00553         if(kc->IsInverted(ret-1)==1){//not inverted
00554             kc->AddLastLink(kc->GetTransfo(ret-1));
00555             kc->RemoveJoint();
00556             return ret-1;
00557         }
00558     }
00559     return ret;
00560 }

int KinematicTree::LoadChain ( pArticulatedTree_t  from,
pArticulatedTree_t  to,
int  i,
modality_t  mod = UNSPECIFIED_MODALITY 
) [inline]

Loads a chain that refers to a path in the kinematic tree as described in Hersch et al.

(2008), IJHR. ifdef WITH_LAST_LINK, the last rotation is not taken into account, only the translation is put into last_link

Returns:
the number of joints in the chain

Definition at line 577 of file KinematicTree.cpp.

Referenced by ReachingThread::init(), and LoadChain().

00578                                                     {
00579   return LoadChain(from,to,&(chains[i]),mod);
00580 }

int KinematicTree::LoadChainRec ( pArticulatedTree_t  from,
pArticulatedTree_t  to,
KinematicChain chain,
pArticulatedTree_t  exclude = NULL 
) const [protected]

finds the path joining two nodes of a tree and builds the corresponding kinematic chain.

The two nodes must belong to the same tree to avoid infinite recursion.

Parameters:
starting node
final node
chain where to put the KinematicChain
Returns:
the number of nodes in the path

Definition at line 592 of file KinematicTree.cpp.

References ArticulatedTree::FindSubTree(), and KinematicChain::GetNbJoints().

Referenced by LoadChain().

00593                                                                                       {
00594   if(to==from){
00595       if(chain->GetNbJoints()){ //the chain is on two branches - 
00596           return chain->GetNbJoints();
00597       }
00598       else{//the chain is on the same branch
00599           cout<<"adding "<<to->name<<endl;
00600           return chain->AddJoint(to->GetJoint(),1);
00601       }     
00602   }
00603   // from is not above to
00604   if(from->FindSubTree(to,exclude)==NULL){
00605     chain->AddJoint(from->GetJoint(),-1);
00606     cout<<"adding "<<from->name<<endl;
00607     if(from->GetParent()==to){
00608         cout<<"adding "<<to->name<<endl;
00609         return chain->AddJoint(to->GetJoint(),-1);
00610     }
00611     return LoadChainRec(from->GetParent(),to,chain,from);
00612   }
00613   LoadChainRec(from,to->GetParent(),chain);
00614   cout<<"adding "<<to->name<<endl;
00615   return chain->AddJoint(to->GetJoint(),1);
00616 }


The documentation for this class was generated from the following files:
 All Data Structures Functions Variables

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