diff --git a/src/darwin_leg_kinematics.cpp b/src/darwin_leg_kinematics.cpp
index e441a1d8f20e4fee9c6fd4f1c4e8de5390e256d2..3640e8e7d8a95ce74e60e817bb993b0bea9070d3 100644
--- a/src/darwin_leg_kinematics.cpp
+++ b/src/darwin_leg_kinematics.cpp
@@ -30,7 +30,7 @@ double CDarwinLegKinematics::angle_norm(double angle)
   return angle;
 }
 
-Eigen::Matrix4d CDarwinLegKinematics::inv_kin_gen_transform(Eigen::Vector3d &pos,Eigen::Vector3d &rot)
+Eigen::Matrix4d CDarwinLegKinematics::generate_transform(Eigen::Vector3d &pos,Eigen::Vector3d &rot)
 {
   Eigen::Matrix4d transform=Eigen::Matrix4d::Identity();
   double Cx = cos(rot(0));
@@ -56,6 +56,25 @@ Eigen::Matrix4d CDarwinLegKinematics::inv_kin_gen_transform(Eigen::Vector3d &pos
   return transform;
 }
 
+void CDarwinLegKinematics::get_transform_info(Eigen::Matrix4d &transform,Eigen::Vector3d &pos,Eigen::Vector3d &rot)
+{
+  pos(0)=transform(0,3);
+  pos(1)=transform(1,3);
+  pos(2)=transform(2,3);
+  if(fabs(transform(0,0))<0.001 && fabs(transform(1,0))<0.001)
+  {
+    rot(2)=atan2(transform(0,1),transform(1,1));
+    rot(1)=asin(-transform(2,0));
+    rot(0)=0.0;
+  }
+  else
+  {
+    rot(2)=atan2(transform(2,1),transform(2,2));
+    rot(1)=atan2(-transform(2,0),sqrt(pow(transform(0,0),2)+pow(transform(1,0),2)));
+    rot(0)=atan2(transform(1,0),transform(0,0));
+  }
+}
+
 void CDarwinLegKinematics::zero_small_values(Eigen::Matrix4d &matrix,double threshold)
 {
   unsigned int i,j;
@@ -114,6 +133,7 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st
 {
   unsigned int i=0;
   Eigen::Matrix4d total_transform=Eigen::Matrix4d::Identity(),rotate;
+  Eigen::Vector3d position,rotation;
 
   if(angles.size()!=this->num_dof)
     throw CDarwinRobotException(_HERE_,"Invalid angle vector size");
@@ -137,23 +157,13 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st
               0.0,0.0,0.0,1.0;
     total_transform*=rotate.inverse();
     this->zero_small_values(total_transform,0.001);
-
+    this->get_transform_info(total_transform,position,rotation);
     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(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]=asin(-total_transform(2,0));
-      rot[2]=0.0;
-    }
-    else
+    for(i=0;i<3;i++) 
     {
-      rot[0]=atan2(total_transform(2,1),total_transform(2,2));
-      rot[1]=atan2(-total_transform(2,0),sqrt(pow(total_transform(0,0),2)+pow(total_transform(1,0),2)));  
-      rot[2]=atan2(total_transform(1,0),total_transform(0,0));
+      pos[i]=position(i);
+      rot[i]=rotation(i);
     }
   }
 }
@@ -168,7 +178,7 @@ void CDarwinLegKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
 
   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);
+  Tad=this->generate_transform(position,orientation);
 
   vec(0)=pos[0]+Tad(0,2)*DARWIN_ANKLE_LENGTH;
   vec(1)=pos[1]+Tad(1,2)*DARWIN_ANKLE_LENGTH;
@@ -206,7 +216,7 @@ void CDarwinLegKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
   // Get Hip Yaw
   orientation << angles[5],0,0;
   position << 0,0,-DARWIN_ANKLE_LENGTH;
-  Tcd=this->inv_kin_gen_transform(position,orientation);
+  Tcd=this->generate_transform(position,orientation);
   if(Tcd.determinant()==0.0)
     throw CDarwinRobotException(_HERE_,"Impossible to find solution"); 
   Tdc=Tcd.inverse();
diff --git a/src/darwin_leg_kinematics.h b/src/darwin_leg_kinematics.h
index 5ff9247bbd9f804dcf325828e2053a90aefebdf1..29b82c6f39b97aa5940396812fbda318bc3c4dc5 100644
--- a/src/darwin_leg_kinematics.h
+++ b/src/darwin_leg_kinematics.h
@@ -12,7 +12,8 @@ typedef struct{
   double theta;
 }TDHParams;
 
-#define DARWIN_PELVIS_LENGTH               0.029 //mm
+#define DARWIN_PELVIS_BASE_LINK_OFFSET     0.0907//mm
+#define DARWIN_PELVIS_LENGTH               0.0315//mm
 #define DARWIN_LEG_SIDE_OFFSET             0.037 //mm
 #define DARWIN_THIGH_LENGTH                0.093 //mm
 #define DARWIN_CALF_LENGTH                 0.093 //mm
@@ -27,11 +28,12 @@ class CDarwinLegKinematics
     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);
+    static Eigen::Matrix4d generate_transform(Eigen::Vector3d &pos,Eigen::Vector3d &rot);
+    static void get_transform_info(Eigen::Matrix4d &transform,Eigen::Vector3d &pos,Eigen::Vector3d &rot);
     void get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot);
     void get_inverse_kinematics(std::vector<double> &pos,std::vector<double> &rot,std::vector<double> &angles);
     ~CDarwinLegKinematics();
diff --git a/src/xml/left_leg.xml b/src/xml/left_leg.xml
index 92676109ff547bb1df6d296bd416208b17ea73d4..7fc3379696177f16e74a224518628b749cca6f1d 100644
--- a/src/xml/left_leg.xml
+++ b/src/xml/left_leg.xml
@@ -6,7 +6,7 @@
     <params>
       <a>0.0</a>
       <alpha>-1.5707</alpha>
-      <d>-0.029</d>
+      <d>-0.0315</d>
       <theta>-1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
diff --git a/src/xml/right_leg.xml b/src/xml/right_leg.xml
index 975bbb6f6caafee5323bae81a43f6f1b81711e3d..4ac05befef9b64a2e17a15c6389037ac2dca6152 100644
--- a/src/xml/right_leg.xml
+++ b/src/xml/right_leg.xml
@@ -6,7 +6,7 @@
     <params>
       <a>0.0</a>
       <alpha>-1.5707</alpha>
-      <d>-0.029</d>
+      <d>-0.0315</d>
       <theta>-1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>