From 365ac0a29408ca39a5eff6f3ca3431573882e94a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Wed, 24 Aug 2016 23:35:27 +0200
Subject: [PATCH] Corrected some denabit hartenberg parameter of both legs.
Changed the sign of the pelvis yaw angle.
---
src/darwin_leg_kinematics.cpp | 18 ++++++++++++------
src/xml/left_leg.xml | 4 ++--
src/xml/right_leg.xml | 4 ++--
3 files changed, 16 insertions(+), 10 deletions(-)
diff --git a/src/darwin_leg_kinematics.cpp b/src/darwin_leg_kinematics.cpp
index 3640e8e..7cc2220 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 7fc3379..70113b2 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 4ac05be..8a21525 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>
--
GitLab