From b0608740525b532d402a55ff52fc7397bdd08c37 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 22 Aug 2016 01:05:03 +0200
Subject: [PATCH] Added static functions to handle transform matrices. Solved a
bug in the euler angles representation. Added some kinematic parameters to
the header files.
---
src/darwin_leg_kinematics.cpp | 44 +++++++++++++++++++++--------------
src/darwin_leg_kinematics.h | 6 +++--
src/xml/left_leg.xml | 2 +-
src/xml/right_leg.xml | 2 +-
4 files changed, 33 insertions(+), 21 deletions(-)
diff --git a/src/darwin_leg_kinematics.cpp b/src/darwin_leg_kinematics.cpp
index e441a1d..3640e8e 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 5ff9247..29b82c6 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 9267610..7fc3379 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 975bbb6..4ac05be 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>
--
GitLab