KChainOrientationBodySchema.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 "KChainOrientationBodySchema.h"
00020 
00021 
00022 
00023 void  KChainOrientationBodySchema::Jacobian(Matrix& jac){
00024   Matrix mat(joint_angle_dim,6);
00025   GetFullJacobian(mat.Array());
00026   mat.Transpose(jac); // different storing convention in mathlib.h
00027   //  jac.Print();
00028 }
00029 
00030   
00031 
00032 
00033 
00034 
00035 // /**
00036 //  * @brief samples the workspace to find adequate weights
00037 //  */
00038 // void KChainOrientationBodySchema::GetWeights(cart_vec_t& cart_weights, joint_vec_t& joint_weights){
00039 //   joint_vec_t angles;
00040 //   CVector3_t kstack[MAX_LINKS];
00041 //   CVector3_t pos;
00042 //   int n =50; //number of samples
00043 // #ifdef WITH_LAST_LINK
00044 //   last_link->GetTranslation(pos);
00045 //   int last = nb_joints;
00046 // #else
00047 //   v_clear(pos);
00048 //   int last = nb_joints-1;  //assuming last rotation is irrelevant
00049 // #endif 
00050 //   cart_weights.Zero();
00051 //   joint_weights.Zero();
00052 //   cart_weights += 1;
00053 //   for(int i=0;i<n;i++){
00054 //     SetRandomAngle(angles);
00055 //     ForwardKinematicsStack(pos,kstack);
00056 //     for(int j=0;j<last;j++){ 
00057 //       joint_weights[j] += v_length(kstack[j+1]);
00058 //     }
00059 //   }
00060 //   joint_weights /=(float) n;
00061   
00062 //   for(int j=0;j<last;j++){ 
00063 //     joint_weights[j] = 1.f/joint_weights[j];
00064 //   }
00065 // }
00066 
00067 int KChainOrientationBodySchema::InverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){
00068   // int config[MAX_LINKS]; maybe later to try more sophistaicated search
00069   int done =ik_trials;
00070   joint_vec_t a;
00071   while(TryInverseKinematics(pos)>tol && done){
00072     // update config and done
00073     //   set angles
00074     SetRandomAngle(a);
00075     done--;
00076   }
00077   GetAngles(angles);
00078   return done;
00079 }
00080     
00081 
00082 float KChainOrientationBodySchema::TryInverseKinematics(const cart_vec_t position){
00083   int i,j;
00084   float cand,rdist=0,tdist=0,k=0.001; //rotation vs translation weight
00085   CVector3_t stack[MAX_LINKS];
00086   CVector3_t tar,newrod,rod,diff,pos;
00087   CQuat_t q1,q2,q3,q4,rot;
00088   
00089   // converting data format
00090   v_copy(position.GetArray(),pos);
00091   q_complete(position.GetArray()+3,rot);
00092 
00093   q_inv(rot,q1);
00094   for(i=0;i<nb_joints;i++){
00095     GetQuaternion(i,q2);
00096     q_multiply(q2,q1,q3);
00097     q_copy(q3,q1);
00098   } 
00099   for(j=0;j<50;j++){
00100 #ifdef WITH_LAST_LINK
00101     last_link->GetTranslation(rod);
00102 #else
00103     v_clear(rod);
00104 #endif
00105     InverseKinematicsStack(pos,stack);
00106     for(i=nb_joints-1;i>=0;i--){
00107       GetInverseQuaternion(i,q2);
00108       q_multiply(q2,q1,q3); //q1*inv(Ri)
00109       if(IsInverted(i)==-1){
00110         v_copy(stack[i],tar);
00111         Translate(i,rod,newrod); //getting to joint
00112         cand = joints[i]->MinimizePositionAndRotationAngle(tar,newrod,q3,k);
00113         cand = AngleInClosestRange(i,cand);
00114         joints[i]->SetAngle(-cand);// todo to check if it is - 
00115         Rotate(i,newrod,rod);   // updating "rod"
00116         v_sub(tar,rod,diff);
00117       }
00118       else{
00119         Rotate(i,stack[i+1],tar); //rotating back
00120         cand = joints[i]->MinimizePositionAndRotationAngle(tar,rod,q3,k);
00121         cand = AngleInClosestRange(i,cand);
00122         joints[i]->SetAngle(cand);
00123         Rotate(i,rod,newrod);
00124         Translate(i,newrod,rod);
00125         v_sub(tar,newrod,diff);
00126       }
00127       GetQuaternion(i,q2);
00128       q_multiply(q3,q2,q1);
00129       q_multiply(q2,q3,q4);
00130       rdist = v_length(q4);//rotation distance, only the first 3 components 
00131       tdist = v_length(diff);//translation distance
00132       //     cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl;
00133       if(tdist<tol && rdist<rot_tol){return rdist/rot_tol+tdist;}
00134     }
00135     q_multiply(rot,q1,q2);
00136     q_inv(rot,q3);
00137     q_multiply(q2,q3,q1);
00138   }
00139   return rdist/rot_tol + tdist;
00140 }
00141 
00142 
00143 #ifdef NO_JOINT_LIMITS
00144 
00145 int KChainOrientationBodySchema::InverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){
00146   float f;
00147   int it=0;
00148   CQuat_t q;
00149   v_copy(pos.GetArray()+3,q);
00150   q[3] = sqrtf(1-v_squ_length(q));
00151   f=InverseKinematicsCCD(pos.GetArray(),q); 
00152   if(f>tol){
00153     cout<<"can't find inverse kinematics"<<endl;
00154     return 0;
00155   }
00156   KinematicChain::GetAngles(angles.GetArray()); 
00157 //   if(!AnglesInRange(angles)){
00158 //     Matrix jac,pjac,mat(joint_angle_dim,joint_angle_dim);
00159 //     joint_vec_t dangle,middle;
00160 //     middle = (min_angle+max_angle)*0.5;  
00161 //     do{ // gets back to allowable range
00162 //       Jacobian(angles,jac);
00163 //       jac.Inverse(pjac); //pseudo-inverse
00164 //       if(!Matrix::IsInverseOk())
00165 //      {cout<<"bad matrix inversion in InverseKinematics"<<endl;}
00166 //       mat.Identity();
00167 //       mat -= pjac*jac;
00168 //       dangle = mat*(middle-angles)*0.01;
00169 //       angles += dangle;      
00170 //       //     cout<<"ik "<<angles<<endl;
00171 //       //mat.Print();
00172 //     }while(!AnglesInRange(angles) && it++ <100);
00173 //     if(it<=100){
00174 //       cout<<"can't bring target configuration in range"<<endl;
00175 //     }
00176 //     else{
00177 //       cout<<"in range"<<endl;
00178 //     }
00179 //     f=InverseKinPosCCD(pos.GetArray());
00180 //   }
00181   return f<=tol;
00182 }
00183 #endif
 All Data Structures Functions Variables

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