KinematicChain.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_CHAIN_H__
00020 #define __KINEMATIC_CHAIN_H__
00021 
00022 #include "RigidTransfo.h"
00023 
00024 #define KC_INVERTED
00025 #define BLOCK_KC
00026 #define WITH_LAST_LINK
00027 
00028 
00029 #define MAX_LINKS 20 
00031 //#define OLD_UPDATE
00032 
00033 enum modality_t{
00034   UNSPECIFIED_MODALITY = 0,
00035   TOUCH_MODALITY,
00036   VISION_MODALITY
00037 };
00038 
00047 class KinematicChain{
00048 
00049  protected:
00050 #ifdef BLOCK_KC
00051 
00054     RigidTransfo* joints_block[MAX_LINKS];//used of code optimization purposes
00055     
00060     RigidTransfo **joints;
00061     int reverse_block[MAX_LINKS];
00062     
00069     int *reverse;
00070 #else
00071     RigidTransfo* joints[MAX_LINKS];
00072     int reverse[MAX_LINKS];
00073 #endif
00074 
00075   int nb_joints;
00076 #ifdef WITH_LAST_LINK
00077   Translation *last_link; //last link
00078     int own_last_link; // flag indicating if last link belongs to a kinematic tree
00079 #endif
00080   modality_t modality;
00081   float tol;//inverse kin tolerance
00082 
00083 
00084     
00085 
00086 
00087  protected:
00088   void RotationStack(CQuat_t *qstack);
00089 
00098   void ForwardKinematicsStack(CVector3_t pos,CVector3_t *pstack);
00099 
00100 
00108     void InverseKinematicsStack(CVector3_t pos,CVector3_t *pstack);
00109 
00110 
00115     void InitKinematicStack(const CVector3_t local_pos,CVector3_t out)const;
00116 public:
00117     KinematicChain(int n=0);
00118     virtual ~KinematicChain();
00119 
00123     void FreeChain();
00124 
00128     int GetNbJoints()const{return nb_joints;};
00129     
00135     int SetLink(int i, CVector3_t link);
00136 
00137 #ifdef WITH_LAST_LINK
00138 
00142     void AddLastLink(Translation *trans)//{last_link=trans;}
00143     {if(last_link)delete last_link;last_link=trans;own_last_link =0;} //to avoid memory leak
00144  
00148     void SetLastLink(const CVector3_t llink){last_link->SetTranslation(llink);};
00149     
00150   Translation* GetLastLink()const{return last_link;};
00151 #endif
00152   int SetAllLinks(CVector3_t *links);
00153 
00154   // int GetLinks(int i, CVector3_t link);
00155 
00160   int SetRotationAxis(int i, CVector3_t axis);
00161 
00166     void SetRotationAxis(int i,float x,float y,float z)
00167     {CVector3_t a;v_set(x,y,z,a);SetRotationAxis(i,a);};
00168    
00174     int GetRotationAxis(int i, CVector3_t axis);
00175     
00181     float GetAngle(int i);
00182     
00187     void GetAngles(float *angles)
00188     {for(int i=0;i<nb_joints;i++)angles[i]=GetAngle(i);}
00189 
00194     void SetAngles(float *angles);
00195 
00201     void SetAngle(int i,float angle){joints[i]->SetAngle(angle);}
00202     
00208     int SetAllRotationAxes(CVector3_t *axes);
00209 
00213     void ScaleRate(float factor);
00214 
00222     void SetForwardKinematics(float *angles, CVector3_t pos, CVector3_t local_pos=NULL){
00223         SetAngles(angles);ForwardKinematics(pos,local_pos);};
00224     
00231     void ForwardKinematics(CVector3_t pos,  CVector3_t local_pos=NULL)const;
00232     
00241     float InverseKinPosCCD(CVector3_t pos);
00242 
00252     void InverseKinRotCCD(CQuat_t q);
00253     
00264     float InverseKinematicsCCD(CVector3_t pos,CQuat_t rot);
00265     
00275     void GetJacobian(float *jac);
00276 
00277 
00288     void GetFullJacobian(float *jac);
00289 
00290 
00299     float Update(float *angles, CVector3_t vision,CVector3_t local_pos=NULL)
00300     {return Update(angles,vision,FLT_MAX,local_pos);};
00301     
00302 
00314     float Update(float *angles, CVector3_t vision, float threshold,CVector3_t local_pos=NULL);
00315    
00316 
00332     float UpdateWithJacobian(float *angle_pos, float *angle_disp, CVector3_t visual_disp, 
00333                            float threshold, CVector3_t local_pos=NULL);
00334     
00335     
00349     float UpdateWithFullJacobian(float *angle_pos, float *angle_disp,RigidTransfo *visual_disp);
00350     float UpdateWithFullJacobian(float *angle_pos, float *angle_disp,float *visual_disp)
00351     {RigidTransfo r;r.Rotation::SetTransfo(visual_disp);
00352         return UpdateWithFullJacobian(angle_pos,angle_disp,&r);}
00353     void LinkRef(float *angle,int n, CVector3_t in, CVector3_t out);
00354     int GetLink(int i,CVector3_t link);
00355     RigidTransfo *GetTransfo(int i)const{return joints[i];}
00356     int AddJoint(RigidTransfo *t,int inv=1){reverse[nb_joints]=inv;joints[nb_joints++]=t;return nb_joints;}
00357     void Clear(){memset((void *)joints,0,MAX_LINKS*sizeof(RigidTransfo *));nb_joints=0;}
00358     void RandomAngles(float *angles){for(int i=0;i<nb_joints;i++)angles[i]=RND(pi)-pi/2;}
00359     void RandomAxes();
00360     void SetRandomAngles(){float a[MAX_LINKS]; RandomAngles(a);SetAngles(a);}
00361     modality_t GetModality()const {return modality;}
00362     void SetModality(modality_t mod){modality=mod;}
00363     
00371     void Transform(int i,CVector3_t in,CVector3_t out)const
00372     {if(reverse[i]==-1){joints[i]->InverseTransform(in,out);}else{joints[i]->Transform(in,out);}}
00373   
00374     void Rotate(int i,CVector3_t in,CVector3_t out)const
00375     {if(reverse[i]==-1){joints[i]->Rotation::InverseTransform(in,out);}
00376         else{joints[i]->Rotation::Transform(in,out);}}
00377     
00378     void CounterRotate(int i,CVector3_t in,CVector3_t out)const
00379     {if(reverse[i]!=-1){joints[i]->Rotation::InverseTransform(in,out);}
00380         else{joints[i]->Rotation::Transform(in,out);}}
00381     
00382     
00392     void Transform(int i,CVector3_t in,float angle,CVector3_t out);
00393     
00394     void InverseTransform(int i,CVector3_t in,CVector3_t out)const
00395     {if(reverse[i]==-1){joints[i]->Transform(in,out);}else{joints[i]->InverseTransform(in,out);}}
00396     
00397     void Translate(int i,CVector3_t in,CVector3_t out)const
00398     {if(reverse[i]==-1){joints[i]->Translation::InverseTransform(in,out);}else{joints[i]->Translation::Transform(in,out);}}
00399     
00400     void GetTranslation(int i, CVector3_t t)
00401     {if(reverse[i]==-1){joints[i]->GetInverseTranslation(t);}else{joints[i]->GetTranslation(t);}}
00402     
00403     void GetQuaternion(int i, CQuat_t q) const
00404     {joints[i]->GetQuaternion(q);if(reverse[i]==-1){q_inv(q,q);}}
00405     
00406     void GetInverseQuaternion(int i, CQuat_t q) const
00407     {joints[i]->GetQuaternion(q);if(reverse[i]!=-1){q_inv(q,q);}}
00408     
00409     void QuaternionAxisAngleDerivative(int i,CMatrix4_t m)
00410     {joints[i]->QuaternionAxisAngleDerivative(m);if(reverse[i]==-1)m_rescale(-1,m,m);} 
00411     
00412     void QuaternionAxisDerivative(int i,CMatrix4_t m)
00413     {joints[i]->QuaternionAxisDerivative(m);if(reverse[i]==-1)m_rescale(-1,m,m);}
00414 
00415     void QuaternionAngleDerivative(int i,CQuat_t q)
00416     {if(reverse[i]==-1)joints[i]->InverseQuaternionAngleDerivative(q);
00417         else joints[i]->QuaternionAngleDerivative(q);}
00418 
00419 #ifdef KC_INVERTED 
00420   int IsInverted(int i)const{return reverse[i];}
00421   void Invert(int i){reverse[i]*=-1;}
00422   void InvertAll(){for(int i=0;i<nb_joints;i++){Invert(i);}}
00423 #endif
00424  int Copy(const KinematicChain& kc);
00425  int RemoveJoint(){if(nb_joints>0){nb_joints--;return 1;}return 0;}
00426  void DeleteJoint(){if(nb_joints>0){delete joints[--nb_joints];joints[nb_joints]=NULL;}}
00427  float GetTolerance()const{return tol;}
00428     void SetTolerance(float t){tol = t;}
00433  void GlobalTransfo(RigidTransfo& rt);
00438  void GlobalRotation(Rotation& rot);
00439  void GlobalTranslation(CVector3_t trans){ForwardKinematics(trans);}
00440 
00444  virtual float CollisionDetection(float& l1, float& l2);
00445  float UpdateTouch(float *angle, float k0, float k1);
00446 
00447     float ScaleChain(float len);
00448     void PrintAxes(ostream& out);
00449     
00450 
00455     float *Serialize(float *data,int offset=0);
00456 
00461     int Deserialize(const float *data,int offset=0);
00462 
00469     int Deserialize(const float *data,int from, int to);
00470 };
00471 
00472 ostream& operator<<(ostream& out, const KinematicChain& kc);
00473 istream& operator>>(istream& in, KinematicChain& kc);
00474 inline void SkipComments(istream& in){while(in.peek()=='#'){ while(in.get()!='\n'){}}}
00475 #endif
 All Data Structures Functions Variables

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