Implemented and tested the inverse kinematics for the legs,

src/darwin_leg_kinematics.cpp

@@ -6,6 +6,7 @@
 #include <stdlib.h>
 #include <time.h>
 #include <math.h>
+#include <iostream>
 #ifdef _HAVE_XSD
 #include "xml/darwin_kin_chain.hxx"
 #endif
@@ -14,8 +15,8 @@ CDarwinLegKinematics::CDarwinLegKinematics()
 {
   srand(time(NULL));  
   this->num_dof=0;
-  this->fw_kin_transforms.clear();
-  this->fw_kin_params.clear();
+  this->fwd_kin_transforms.clear();
+  this->fwd_kin_params.clear();
 }
 
 double CDarwinLegKinematics::angle_norm(double angle)
@@ -29,6 +30,42 @@ double CDarwinLegKinematics::angle_norm(double angle)
   return angle;
 }
 
+Eigen::Matrix4d CDarwinLegKinematics::inv_kin_gen_transform(Eigen::Vector3d &pos,Eigen::Vector3d &rot)
+{
+  Eigen::Matrix4d transform=Eigen::Matrix4d::Identity();
+  double Cx = cos(rot(0));
+  double Cy = cos(rot(1));
+  double Cz = cos(rot(2));
+  double Sx = sin(rot(0));
+  double Sy = sin(rot(1));
+  double Sz = sin(rot(2));
+
+  transform(0,0)=Cz*Cy;
+  transform(0,1)=Cz*Sy*Sx-Sz*Cx;
+  transform(0,2)=Cz*Sy*Cx+Sz*Sx;
+  transform(0,3)=pos(0);
+  transform(1,0)=Sz*Cy;
+  transform(1,1)=Sz*Sy*Sx+Cz*Cx;
+  transform(1,2)=Sz*Sy*Cx-Cz*Sx;
+  transform(1,3)=pos(1);
+  transform(2,0)=-Sy;
+  transform(2,1)=Cy*Sx;
+  transform(2,2)=Cy*Cx;
+  transform(2,3)=pos(2);
+
+  return transform;
+}
+
+void CDarwinLegKinematics::zero_small_values(Eigen::Matrix4d &matrix,double threshold)
+{
+  unsigned int i,j;
+
+  for(i=0;i<4;i++) 
+    for(j=0;j<4;j++)
+      if(fabs(matrix(i,j))<threshold)
+        matrix(i,j)=0.0;
+}
+
 void CDarwinLegKinematics::load_chain(std::string &filename)
 {
   joints_t::joint_iterator iterator;
@@ -41,8 +78,8 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
   {
     try{
       std::auto_ptr<joints_t> chain(darwin_kin_chain(filename.c_str(), xml_schema::flags::dont_validate));
-      this->fw_kin_params.clear();
-      this->fw_kin_transforms.clear();
+      this->fwd_kin_params.clear();
+      this->fwd_kin_transforms.clear();
       this->num_dof=0;
       for(iterator=chain->joint().begin(),i=0;iterator!=chain->joint().end();iterator++,i++)
       {
@@ -50,7 +87,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
         params.a=iterator->params().a();
         params.d=iterator->params().d();
         params.theta=iterator->params().theta();
-        this->fw_kin_params.push_back(params);
+        this->fwd_kin_params.push_back(params);
         // precompute the transforms
         transform(2,0)=0.0;
         transform(2,1)=sin(params.alpha);
@@ -60,7 +97,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
         transform(3,1)=0.0;
         transform(3,2)=0.0;
         transform(3,3)=1.0;
-        this->fw_kin_transforms.push_back(transform);
+        this->fwd_kin_transforms.push_back(transform);
         this->num_dof++;
       }
     }catch(const xml_schema::exception& e){
@@ -76,7 +113,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
 void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot)
 {
   unsigned int i=0;
-  Eigen::Matrix4d total_transform=Eigen::Matrix4d::Identity();
+  Eigen::Matrix4d total_transform=Eigen::Matrix4d::Identity(),rotate;
 
   if(angles.size()!=this->num_dof)
     throw CDarwinRobotException(_HERE_,"Invalid angle vector size");
@@ -84,25 +121,32 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st
   {
     for(i=0;i<this->num_dof;i++)
     {
-      this->fw_kin_transforms[i](0,0)=cos(angles[i]+this->fw_kin_params[i].theta);
-      this->fw_kin_transforms[i](0,1)=-sin(angles[i]+this->fw_kin_params[i].theta)*cos(this->fw_kin_params[i].alpha);
-      this->fw_kin_transforms[i](0,2)=sin(angles[i]+this->fw_kin_params[i].theta)*sin(this->fw_kin_params[i].alpha);
-      this->fw_kin_transforms[i](0,3)=this->fw_kin_params[i].a*cos(angles[i]+this->fw_kin_params[i].theta);
-      this->fw_kin_transforms[i](1,0)=sin(angles[i]+this->fw_kin_params[i].theta);
-      this->fw_kin_transforms[i](1,1)=cos(angles[i]+this->fw_kin_params[i].theta)*cos(this->fw_kin_params[i].alpha);
-      this->fw_kin_transforms[i](1,2)=-cos(angles[i]+this->fw_kin_params[i].theta)*sin(this->fw_kin_params[i].alpha);
-      this->fw_kin_transforms[i](1,3)=this->fw_kin_params[i].a*sin(angles[i]+this->fw_kin_params[i].theta);
-      total_transform*=this->fw_kin_transforms[i];
+      this->fwd_kin_transforms[i](0,0)=cos(angles[i]+this->fwd_kin_params[i].theta);
+      this->fwd_kin_transforms[i](0,1)=-sin(angles[i]+this->fwd_kin_params[i].theta)*cos(this->fwd_kin_params[i].alpha);
+      this->fwd_kin_transforms[i](0,2)=sin(angles[i]+this->fwd_kin_params[i].theta)*sin(this->fwd_kin_params[i].alpha);
+      this->fwd_kin_transforms[i](0,3)=this->fwd_kin_params[i].a*cos(angles[i]+this->fwd_kin_params[i].theta);
+      this->fwd_kin_transforms[i](1,0)=sin(angles[i]+this->fwd_kin_params[i].theta);
+      this->fwd_kin_transforms[i](1,1)=cos(angles[i]+this->fwd_kin_params[i].theta)*cos(this->fwd_kin_params[i].alpha);
+      this->fwd_kin_transforms[i](1,2)=-cos(angles[i]+this->fwd_kin_params[i].theta)*sin(this->fwd_kin_params[i].alpha);
+      this->fwd_kin_transforms[i](1,3)=this->fwd_kin_params[i].a*sin(angles[i]+this->fwd_kin_params[i].theta);
+      total_transform*=this->fwd_kin_transforms[i];
     }
+    rotate << 0.0,0.0,1.0,0.0,
+              0.0,1.0,0.0,0.0,
+              -1.0,0.0,0.0,0.0,
+              0.0,0.0,0.0,1.0;
+    total_transform*=rotate.inverse();
+    this->zero_small_values(total_transform,0.001);
+
     pos.resize(3);
     pos[0]=total_transform(0,3);
     pos[1]=total_transform(1,3);
     pos[2]=total_transform(2,3);
     rot.resize(3);
-    if(total_transform(0,0)==0.0 && total_transform(1,0)==0.0)
+    if(fabs(total_transform(0,0))<0.001 && fabs(total_transform(1,0))<0.001)
     {
       rot[0]=atan2(total_transform(0,1),total_transform(1,1));
-      rot[1]=1.5707;  
+      rot[1]=asin(-total_transform(2,0));
       rot[2]=0.0;
     }
     else
@@ -116,12 +160,91 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st
 
 void CDarwinLegKinematics::get_inverse_kinematics(std::vector<double> &pos,std::vector<double> &rot,std::vector<double> &angles)
 {
+  Eigen::Matrix4d Tad,Tda,Tcd,Tdc,Tac;
+  Eigen::Vector3d vec,orientation,position;
+  double _Rac,_Acos,_Atan,_k,_l,_m,_n,_s,_c,_theta,tmp;
+
+  angles.resize(6);
+
+  orientation << rot[0], rot[1], rot[2];
+  position << pos[0], pos[1], pos[2]+DARWIN_PELVIS_LENGTH;
+  Tad=this->inv_kin_gen_transform(position,orientation);
+
+  vec(0)=pos[0]+Tad(0,2)*DARWIN_ANKLE_LENGTH;
+  vec(1)=pos[1]+Tad(1,2)*DARWIN_ANKLE_LENGTH;
+  vec(2)=(pos[2]+DARWIN_PELVIS_LENGTH)+Tad(2,2)*DARWIN_ANKLE_LENGTH;
+
+  // Get Knee
+  _Rac=vec.norm();
+
+  tmp=(pow(_Rac,2.0)-pow(DARWIN_THIGH_LENGTH,2.0)-pow(DARWIN_CALF_LENGTH,2.0))/(2.0*DARWIN_THIGH_LENGTH*DARWIN_CALF_LENGTH);
+  if(tmp>1.0) tmp=1.0;
+  else if(tmp<-1.0) tmp=1.0;
+  _Acos=acos(tmp);
+  if(isnan(_Acos)==1)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  // this is not the same for both legs
+  angles[3]=_Acos;
+
+  // Get Ankle Roll
+  if(Tad.determinant()==0.0)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  Tda=Tad.inverse();
+  _k=sqrt(pow(Tda(1,3),2.0)+pow(Tda(2,3),2.0));
+  _l=sqrt(pow(Tda(1,3),2.0)+pow(Tda(2,3)-DARWIN_ANKLE_LENGTH,2.0));
+  _m=(pow(_k,2.0)-pow(_l,2.0)-pow(DARWIN_ANKLE_LENGTH,2.0))/(2.0*_l*DARWIN_ANKLE_LENGTH);
+  if(_m>1.0) _m=1.0;
+  else if(_m<-1.0) _m=-1.0;
+  _Acos=acos(_m);
+  if(isnan(_Acos)==1)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  if(Tda(1,3)<0.0)
+    angles[5]=-_Acos;
+  else
+    angles[5]=_Acos;
+
+  // Get Hip Yaw
+  orientation << angles[5],0,0;
+  position << 0,0,-DARWIN_ANKLE_LENGTH;
+  Tcd=this->inv_kin_gen_transform(position,orientation);
+  if(Tcd.determinant()==0.0)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  Tdc=Tcd.inverse();
+  Tac=Tad*Tdc;
+
+  _Atan=atan2(-Tac(0,1),Tac(1,1));
+  if(isinf(_Atan)==1)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  angles[0]=_Atan;
+
+  // Get Hip Roll
+  _Atan=atan2(Tac(2,1),-Tac(0,1)*sin(angles[0])+Tac(1,1)*cos(angles[0]));
+  if(isinf(_Atan)==1)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  angles[1]=_Atan;
+
+  // Get Hip Pitch and Ankle Pitch
+  _Atan=atan2(Tac(0,2)*cos(angles[0])+Tac(1,2)*sin(angles[0]),Tac(0,0)*cos(angles[0])+Tac(1,0)*sin(angles[0]));
+  if(isinf(_Atan)==1)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  _theta=_Atan;
+  _k=sin(angles[3])*DARWIN_CALF_LENGTH;
+  _l=-DARWIN_THIGH_LENGTH-cos(angles[3])*DARWIN_CALF_LENGTH;
+  _m=cos(angles[0])*vec(0)+sin(angles[0])*vec(1);
+  _n=cos(angles[1])*vec(2)+sin(angles[0])*sin(angles[1])*vec(0)-cos(angles[0])*sin(angles[1])*vec(1);
+  _s=(_k*_n+_l*_m)/(pow(_k,2.0)+pow(_l,2.0));
+  _c=(_n-_k*_s)/_l;
+  _Atan=atan2(_s,_c);
+  if(isinf(_Atan)== 1)
+    throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
+  angles[2]=_Atan;
+  angles[4]=-(_theta-angles[3]-angles[2]);
 }
 
 CDarwinLegKinematics::~CDarwinLegKinematics()
 {
   this->num_dof=0;
-  this->fw_kin_transforms.clear();
-  this->fw_kin_params.clear();
+  this->fwd_kin_transforms.clear();
+  this->fwd_kin_params.clear();
 }
 

src/darwin_leg_kinematics.h

@@ -12,14 +12,23 @@ typedef struct{
   double theta;
 }TDHParams;
 
+#define DARWIN_PELVIS_LENGTH               0.029 //mm
+#define DARWIN_LEG_SIDE_OFFSET             0.037 //mm
+#define DARWIN_THIGH_LENGTH                0.093 //mm
+#define DARWIN_CALF_LENGTH                 0.093 //mm
+#define DARWIN_ANKLE_LENGTH                0.0335 //mm
+#define DARWIN_LEG_LENGTH                  (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm
+
 class CDarwinLegKinematics
 { 
   private:
     unsigned int num_dof;
-    std::vector<Eigen::Matrix4d> fw_kin_transforms;
-    std::vector<TDHParams> fw_kin_params;
+    std::vector<Eigen::Matrix4d> fwd_kin_transforms;
+    std::vector<TDHParams> fwd_kin_params;
   protected:
     double angle_norm(double angle);
+    Eigen::Matrix4d inv_kin_gen_transform(Eigen::Vector3d &pos,Eigen::Vector3d &rot);
+    void zero_small_values(Eigen::Matrix4d &matrix,double threshold);
   public:
     CDarwinLegKinematics();
     void load_chain(std::string &filename);

src/examples/darwin_leg_kin.cpp

@@ -3,11 +3,16 @@
 
 #include <iostream>
 
+#define     PI    3.14159
+
 std::string kin_file="../src/xml/right_leg.xml";
 
 int main(int argc, char *argv[])
 {
-  std::vector<double> joints_in(6),pos,rot_out;
+  double i,j,k,l,m,n,error;
+  int num_total=0,num_sol=0;
+  std::vector<double> joints_in(6),pos_in,rot_in;
+  std::vector<double> joints_out(6),pos_out,rot_out;
 
   try{
     CDarwinLegKinematics kin;
@@ -19,8 +24,61 @@ int main(int argc, char *argv[])
     joints_in[3]=atof(argv[4]);
     joints_in[4]=atof(argv[5]);
     joints_in[5]=atof(argv[6]);
-    kin.get_forward_kinematics(joints_in,pos,rot_out);
-    std::cout << pos[0] << "," << pos[1] << "," << pos[2] << std::endl;
+    kin.get_forward_kinematics(joints_in,pos_out,rot_out);
+    std::cout << "position:" << pos_out[0] << "," << pos_out[1] << "," << pos_out[2] << std::endl;
+    std::cout << "orientation:" << rot_out[0] << "," << rot_out[1] << "," << rot_out[2] << std::endl;
+    pos_in=pos_out;
+    rot_in=rot_out;
+    kin.get_inverse_kinematics(pos_in,rot_in,joints_out);
+    std::cout << "joints: " << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << "," << joints_out[3] << "," << joints_out[4] << "," << joints_out[5] << "," << std::endl;
+    for(i=-PI/4;i<PI/4;i+=0.2)
+    {
+      for(j=-PI/4;j<PI/4;j+=0.2)
+      {
+        for(k=-PI/4;k<PI/4;k+=0.2)
+        {
+          for(l=0;l<PI/4;l+=0.2)
+          {
+            for(m=-PI/4;m<PI/4;m+=0.2)
+            {
+              for(n=-PI/4;n<PI/4;n+=0.2)
+              {
+                num_total++;
+                joints_in[0]=i;
+                joints_in[1]=j;
+                joints_in[2]=k;
+                joints_in[3]=l;
+                joints_in[4]=m;
+                joints_in[5]=n;
+                try{
+                  std::cout << i << "," << j << "," << k << "," << l << "," << m << "," << n << std::endl;
+                  kin.get_forward_kinematics(joints_in,pos_out,rot_out);
+                  pos_in=pos_out;
+                  rot_in=rot_out;
+                  kin.get_inverse_kinematics(pos_in,rot_in,joints_out);
+                  error=0.0;
+                  error+=pow(i-joints_out[0],2.0);
+                  error+=pow(j-joints_out[1],2.0);
+                  error+=pow(k-joints_out[2],2.0);
+                  error+=pow(l-joints_out[3],2.0);
+                  error+=pow(m-joints_out[4],2.0);
+                  error+=pow(n-joints_out[5],2.0);
+                  error=sqrt(error);
+                  std::cout << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << "," << joints_out[3] << "," << joints_out[4] << "," << joints_out[5] << std::endl;
+                  if(error>0.1)
+                    std::cout << "Error: " << error << std::endl;
+                  else
+                    num_sol++;
+                }catch(CException &e){
+                  std::cout << e.what() << std::endl;
+                }
+              }
+            } 
+          }
+        }
+      }
+    }
+    std::cout << "Found " << num_sol << " solutions of " << num_total << " attempts" << std::endl;
 
   }catch(CException &e){
     std::cout << e.what() << std::endl;

src/xml/left_leg.xml

@@ -5,8 +5,8 @@
   <joint>
     <params>
       <a>0.0</a>
-      <alpha>1.5707</alpha>
-      <d>0.028652</d>
+      <alpha>-1.5707</alpha>
+      <d>-0.029</d>
       <theta>-1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
@@ -15,7 +15,7 @@
   <joint>
     <params>
       <a>0.0</a>
-      <alpha>-1.5707</alpha>
+      <alpha>1.5707</alpha>
       <d>0.0</d>
       <theta>1.5707</theta>
     </params>

src/xml/right_leg.xml

@@ -5,8 +5,8 @@
   <joint>
     <params>
       <a>0.0</a>
-      <alpha>1.5707</alpha>
-      <d>0.028652</d>
+      <alpha>-1.5707</alpha>
+      <d>-0.029</d>
       <theta>-1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
@@ -15,7 +15,7 @@
   <joint>
     <params>
       <a>0.0</a>
-      <alpha>1.5707</alpha>
+      <alpha>-1.5707</alpha>
       <d>0.0</d>
       <theta>1.5707</theta>
     </params>