diff --git a/src/darwin_leg_kinematics.cpp b/src/darwin_leg_kinematics.cpp
index 3640e8e7d8a95ce74e60e817bb993b0bea9070d3..7cc2220bdf73b7353df7e053ed8875d632443fcf 100644
--- a/src/darwin_leg_kinematics.cpp
+++ b/src/darwin_leg_kinematics.cpp
@@ -63,15 +63,15 @@ void CDarwinLegKinematics::get_transform_info(Eigen::Matrix4d &transform,Eigen::
   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(0)=atan2(transform(0,1),transform(1,1));
     rot(1)=asin(-transform(2,0));
-    rot(0)=0.0;
+    rot(2)=0.0;
   }
   else
   {
-    rot(2)=atan2(transform(2,1),transform(2,2));
+    rot(0)=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));
+    rot(2)=atan2(transform(1,0),transform(0,0));
   }
 }
 
@@ -139,6 +139,7 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st
     throw CDarwinRobotException(_HERE_,"Invalid angle vector size");
   else
   {
+    angles[0]*=-1.0;
     for(i=0;i<this->num_dof;i++)
     {
       this->fwd_kin_transforms[i](0,0)=cos(angles[i]+this->fwd_kin_params[i].theta);
@@ -151,11 +152,16 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st
       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,
+/*    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;*/
+    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();
+//    total_transform*=rotate.inverse();
+    total_transform=total_transform*rotate;
     this->zero_small_values(total_transform,0.001);
     this->get_transform_info(total_transform,position,rotation);
     pos.resize(3);
diff --git a/src/xml/left_leg.xml b/src/xml/left_leg.xml
index 7fc3379696177f16e74a224518628b749cca6f1d..70113b2064fc050ec29db7b05ed33818b8ab4163 100644
--- a/src/xml/left_leg.xml
+++ b/src/xml/left_leg.xml
@@ -7,7 +7,7 @@
       <a>0.0</a>
       <alpha>-1.5707</alpha>
       <d>-0.0315</d>
-      <theta>-1.5707</theta>
+      <theta>1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
     <min_angle>-1.5707</min_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>
diff --git a/src/xml/right_leg.xml b/src/xml/right_leg.xml
index 4ac05befef9b64a2e17a15c6389037ac2dca6152..8a2152512acd2b6e011800ace438afbad1055a1e 100644
--- a/src/xml/right_leg.xml
+++ b/src/xml/right_leg.xml
@@ -7,7 +7,7 @@
       <a>0.0</a>
       <alpha>-1.5707</alpha>
       <d>-0.0315</d>
-      <theta>-1.5707</theta>
+      <theta>1.5707</theta>
     </params>
     <max_angle>1.5707</max_angle>
     <min_angle>-1.5707</min_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>