KChainBodySchema.cpp

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 #include "TreeParser.h"
00020 #include "KChainBodySchema.h"
00021 
00022 
00023 //  KChainBodySchema::KChainBodySchema(char *filename){
00024 //      telescopic
00025 //  }
00026 
00027 
00028 // KChainBodySchema::KChainBodySchema(const KinematicChain& chain){
00029 //   int i,n;
00030 //   n = chain.GetNbJoints();
00031 //   for(i=0;i<n;i++){
00032 //     AddJoint( new RigidTransfo(*(chain.GetTransfo(i))),chain.IsInverted(i));
00033 //   }
00034 //   modality = chain.GetModality();
00035 //   tol =chain.GetTolerance();
00036 // }
00037 
00038 void  KChainBodySchema::Jacobian(Matrix& jac){
00039   Matrix mat(1,4*joint_angle_dim,true);
00040   int n = nb_joints;
00041   jac.Resize(3,n,false);
00042   GetJacobian(mat.Array());
00043   for(int i=0;i<n;i++){
00044     for(int j=0;j<3;j++){
00045       jac.Array()[j*n+i] = mat.Array()[i*4+j];
00046     }
00047   }
00048 }
00049 
00050 
00054 void KChainBodySchema::GetWeights(cart_vec_t& cart_weights, joint_vec_t& joint_weights){
00055   joint_vec_t angles;
00056   CVector3_t kstack[MAX_LINKS];
00057   CVector3_t pos;
00058   int n =50; //number of samples
00059 #ifdef WITH_LAST_LINK
00060   last_link->GetTranslation(pos);
00061   int last = nb_joints;
00062 #else
00063   v_clear(pos);
00064   int last = nb_joints-1;
00065 #endif
00066   cart_weights.Zero();
00067   joint_weights.Zero();
00068   cart_weights += 1;
00069   for(int i=0;i<n;i++){
00070     SetRandomAngle(angles);
00071     ForwardKinematicsStack(pos,kstack);
00072     for(int j=0;j<last;j++){
00073       joint_weights[j] += v_length(kstack[j+1]);
00074     }
00075   }
00076   joint_weights /=(float) n;
00077 
00078   for(int j=0;j<last;j++){
00079     if(joint_weights[j]<epsilon){ //avoiding deviding by 0 
00080       joint_weights[j] = 100000;
00081     }
00082     else{
00083       joint_weights[j] = 1.f/joint_weights[j];
00084     }
00085     
00086   }
00087   joint_weights.Print();
00088     
00089 }
00090 
00091 
00092 int KChainBodySchema::InverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){
00093   int done =ik_trials;
00094   float dist;
00095   joint_vec_t a;
00096   while(done){
00097       dist = TryInverseKinematics(pos,angles);
00098       cout<<"ikdist "<<dist<<endl;
00099       if(dist<=tol){
00100           return done;
00101       }
00102       else{
00103           SetRandomAngle(a);//new start
00104           done--;
00105       }
00106   }
00107   return done;
00108   //  return TryInverseKinematics(pos,config,angles)<tol;   
00109 
00110 }
00111     
00112 
00113 
00114 float KChainBodySchema::TryInverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){
00115  CVector3_t stack[MAX_LINKS];
00116   CVector3_t tar,newrod,rod,diff;
00117   float dist=-1.0f;
00118   float cand; //candidate
00119   for(int j=0;j<20;j++){
00120     InverseKinematicsStack(pos.GetArray(),stack);
00121 #ifdef WITH_LAST_LINK
00122     last_link->GetTranslation(rod);
00123     if(telescopic){// useful for pointing to, or gazing to)
00124         v_normalize(rod,newrod);
00125         float f=v_dot(newrod,stack[nb_joints]);
00126         if(f>0){
00127             v_scale(newrod,f,rod);
00128         }
00129     }
00130 #else
00131     v_clear(rod);
00132 #endif
00133         
00134     for(int i=nb_joints-1;i>=0;i--){
00135       if(IsInverted(i)==-1){
00136           v_copy(stack[i],tar);
00137           Translate(i,rod,newrod); //getting to joint
00138           cand =joints[i]->AimingAngle(tar,newrod);
00139         //      joints[i]->InvertRotationAngle();
00140           cand = AngleInClosestRange(i,cand);
00141           joints[i]->SetAngle(-cand);//check if that's right
00142           Rotate(i,newrod,rod);   // updating "rod"
00143           v_sub(tar,rod,diff);
00144       }
00145       else{
00146           Rotate(i,stack[i+1],tar); //rotating back
00147           cand =joints[i]->AimingAngle(tar,newrod);
00148           cand = AngleInClosestRange(i,cand);
00149           joints[i]->SetAngle(cand);
00150           Rotate(i,rod,newrod);
00151           Translate(i,newrod,rod);
00152           v_sub(tar,newrod,diff);
00153       }
00154       dist=v_length(diff);
00155       if(dist<=tol)
00156           {
00157               GetAngles(angles);//trial
00158               return dist;
00159           }
00160       //     cout<<v_length(diff)<<endl;;
00161     }
00162   }
00163   GetAngles(angles);
00164   return dist;
00165 }
00166 
00167 
00168 
00169 void KChainBodySchema::Load(const char *filename){
00170   //#ifdef NOT_YET   
00171   Tree *xml_tree = new Tree();
00172   Tree *child = xml_tree;
00173   CVector3_t v;
00174   float f, min_a,max_a;
00175   int index=0;
00176 
00177   xml_tree->LoadFromFile(filename);
00178   while (child){ 
00179     string name = child->GetData();
00180     cout<<"adding "<<name<<endl;
00181     Tree_List *subTrees = child->GetSubTrees();
00182     child =NULL;
00183     RigidTransfo *rt =new RigidTransfo();
00184     for(unsigned int i=0;i<(*subTrees).size();i++){
00185       if((*subTrees)[i]->GetName().compare("Axis")==0){
00186         istringstream s((*subTrees)[i]->GetData());
00187         s >> v[0]>>v[1]>>v[2];
00188         rt->SetRotationAxis(v);
00189       }
00190       if((*subTrees)[i]->GetName().compare("Angle")==0){
00191         istringstream s((*subTrees)[i]->GetData());
00192         s >> f;
00193         rt->SetAngle(f*deg2rad);
00194       }
00195       if((*subTrees)[i]->GetName().compare("Position")==0){
00196         istringstream s((*subTrees)[i]->GetData());
00197         s >> v[0]>>v[1]>>v[2];
00198         rt->SetTranslation(v);
00199       }
00200       if((*subTrees)[i]->GetName().compare("Range")==0){
00201         istringstream s((*subTrees)[i]->GetData());
00202         s>>min_a>>max_a;
00203         //      cout<<range[0]<<" "<<range[1]<<endl;
00204       }
00205       if((*subTrees)[i]->GetName().compare("Children")==0){
00206         Tree_List *children = (*subTrees)[i]->GetSubTrees();
00207         //      cout<<" children "<<children->size()<<endl;
00208         if(children->size()>0){
00209           child = (*children)[0];
00210           index++;
00211         }
00212       }
00213    }
00214     if(rt->AxisOk()){ //there is a rotation
00215      min_angle.Resize(nb_joints+1);
00216      max_angle.Resize(nb_joints+1);
00217      min_angle[nb_joints] = min_a;
00218      max_angle[nb_joints] = max_a;
00219      AddJoint(rt);
00220     }
00221 #ifdef WITH_LAST_LINK
00222     else{           // it is the last link
00223       SetLastLink(rt->GetTranslation());
00224       delete rt;
00225     }
00226 #endif
00227   }
00228   min_angle *= deg2rad;
00229   max_angle *= deg2rad;
00230   delete xml_tree;
00231   //#endif
00232 }
00233 
00234 
00235 
00236 #ifdef OLD_INV_KIN
00237 int KChainBodySchema::InverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){
00238   float f;
00239   int it=0;
00240   f=InverseKinPosCCD(pos.GetArray()); 
00241   if(f>tol){
00242     cout<<"can't find inverse kinematics"<<endl;
00243     return 0;
00244   }
00245   KinematicChain::GetAngles(angles.GetArray()); 
00246   if(!AnglesInRange(angles)){
00247     Matrix jac,pjac,mat(joint_angle_dim,joint_angle_dim);
00248     joint_vec_t dangle,middle;
00249     middle = (min_angle+max_angle)*0.5;  
00250     do{ // gets back to allowable range
00251       Jacobian(angles,jac);
00252       jac.Inverse(pjac); //pseudo-inverse
00253       if(!Matrix::IsInverseOk())
00254         {cout<<"bad matrix inversion in InverseKinematics"<<endl;}
00255       mat.Identity();
00256       mat -= pjac*jac;
00257       dangle = mat*(middle-angles)*0.01;
00258       angles += dangle;      
00259       //     cout<<"ik "<<angles<<endl;
00260       //mat.Print();
00261     }while(!AnglesInRange(angles) && it++ <500);
00262     if(it<=100){
00263       cout<<"can't bring target configuration in range"<<endl;
00264     }
00265     else{
00266       cout<<"in range"<<endl;
00267     }
00268     f=InverseKinPosCCD(pos.GetArray());
00269   }
00270   return f<=tol;
00271 }
00272 
00273 
00274 
00275 #endif
 All Data Structures Functions Variables

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