Solved a problem with the tolerance calculation in the inverse kinematics problem.

Chnaged the parameters of the arm kinematic chains.

src/darwin_arm_kinematics.cpp

@@ -196,6 +196,7 @@ void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
 
   do{
     count++;
+    max_error=0.0;
     for(i=0;i<this->num_dof;i++)
       q_init(i)=(rand()*3.14159/RAND_MAX)-1.5707;
     for(;i<6;i++)
@@ -207,7 +208,7 @@ void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
     else
     {
       error=desired_frame.p-cartpos_sol.p;
-      for(i=0;i<this->num_dof;i++)
+      for(i=0;i<3;i++)
         if(fabs(error(i))>max_error)
           max_error=fabs(error(i));
     }

src/examples/darwin_arm_kin.cpp

@@ -3,11 +3,11 @@
 
 #include <iostream>
 
-std::string kin_file="../src/xml/right_arm.xml";
+std::string kin_file="../src/xml/left_arm.xml";
 
 int main(int argc, char *argv[])
 {
-  std::vector<double> joints_in(4),joints_out(4),pos,rot_out,rot_in(3,0.0);
+  std::vector<double> joints_in(3),joints_out(3),pos,rot_out,rot_in(3,0.0);
   int num_total=0,num_sol=0;
   double i,j,k,l;
 
@@ -15,7 +15,7 @@ int main(int argc, char *argv[])
     CDarwinArmKinematics kin;
     kin.load_chain(kin_file);
 
-    if(argc!=5)
+    if(argc!=4)
     {
       std::cout << "Invalid number of parameters: darwin_arm_kin <joint0 angle> <joint1 angle> <joint2 angle> <joint3 angle>" << std::endl;
       exit(-1);
@@ -25,20 +25,20 @@ int main(int argc, char *argv[])
       joints_in[0]=atof(argv[1]);
       joints_in[1]=atof(argv[2]);
       joints_in[2]=atof(argv[3]);
-      joints_in[3]=atof(argv[4]);
       kin.get_forward_kinematics(joints_in,pos,rot_out);
       std::cout << pos[0] << "," << pos[1] << "," << pos[2] << std::endl;
+      std::cout << rot_out[0] << "," << rot_out[1] << "," << rot_out[2] << std::endl;
     }
-    return 0;
 
-    pos.push_back(0.0743671);
-    pos.push_back(0.0502218);
-    pos.push_back(0.0791303);
+    pos.resize(3);
+    pos[0]=0.05;
+    pos[1]=-0.09;
+    pos[2]=0.06;
     rot_in[0]=0;
     rot_in[1]=0;
     rot_in[2]=0;
     kin.get_inverse_kinematics(pos,rot_in,joints_out);
-    std::cout << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << "," << joints_out[3] << std::endl;
+    std::cout << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << std::endl;
     return 0;
 
     for(i=-1.5707;i<1.5707;i+=0.2)

src/xml/left_arm.xml

@@ -27,21 +27,10 @@
   <joint>
     <name>j_elbow_l</name>
     <params>
-      <a>-0.02165</a>
+      <a>-0.08165</a>
       <alpha>1.5707</alpha>
       <d>0</d>
-      <theta>0</theta>
-    </params>
-    <max_angle>1.5707</max_angle>
-    <min_angle>-1.5707</min_angle>
-  </joint>
-  <joint>
-    <name>j_wrist_l</name>
-    <params>
-      <a>0</a>
-      <alpha>0</alpha>
-      <d>0.056</d>
-      <theta>0</theta>
+      <theta>1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
     <min_angle>-1.5707</min_angle>

src/xml/right_arm.xml

@@ -27,21 +27,10 @@
   <joint>
     <name>j_elbow_r</name>
     <params>
-      <a>-0.02165</a>
+      <a>-0.08165</a>
       <alpha>1.5707</alpha>
       <d>0</d>
-      <theta>0</theta>
-    </params>
-    <max_angle>1.5707</max_angle>
-    <min_angle>-1.5707</min_angle>
-  </joint>
-  <joint>
-    <name>j_wrist_r</name>
-    <params>
-      <a>0</a>
-      <alpha>0</alpha>
-      <d>0.056</d>
-      <theta>0</theta>
+      <theta>1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
     <min_angle>-1.5707</min_angle>