From eefef8d028db637142a2ba6bdb3bf2c49f951fa3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu> Date: Mon, 15 Aug 2016 18:52:22 +0200 Subject: [PATCH] Implemented and tested the inverse kinematics for the legs, --- src/darwin_leg_kinematics.cpp | 163 ++++++++++++++++++++++++++++---- src/darwin_leg_kinematics.h | 13 ++- src/examples/darwin_leg_kin.cpp | 64 ++++++++++++- src/xml/left_leg.xml | 6 +- src/xml/right_leg.xml | 6 +- 5 files changed, 221 insertions(+), 31 deletions(-) diff --git a/src/darwin_leg_kinematics.cpp b/src/darwin_leg_kinematics.cpp index 4a55817..e441a1d 100644 --- a/src/darwin_leg_kinematics.cpp +++ b/src/darwin_leg_kinematics.cpp @@ -6,6 +6,7 @@ #include <stdlib.h> #include <time.h> #include <math.h> +#include <iostream> #ifdef _HAVE_XSD #include "xml/darwin_kin_chain.hxx" #endif @@ -14,8 +15,8 @@ CDarwinLegKinematics::CDarwinLegKinematics() { srand(time(NULL)); this->num_dof=0; - this->fw_kin_transforms.clear(); - this->fw_kin_params.clear(); + this->fwd_kin_transforms.clear(); + this->fwd_kin_params.clear(); } double CDarwinLegKinematics::angle_norm(double angle) @@ -29,6 +30,42 @@ double CDarwinLegKinematics::angle_norm(double angle) return angle; } +Eigen::Matrix4d CDarwinLegKinematics::inv_kin_gen_transform(Eigen::Vector3d &pos,Eigen::Vector3d &rot) +{ + Eigen::Matrix4d transform=Eigen::Matrix4d::Identity(); + double Cx = cos(rot(0)); + double Cy = cos(rot(1)); + double Cz = cos(rot(2)); + double Sx = sin(rot(0)); + double Sy = sin(rot(1)); + double Sz = sin(rot(2)); + + transform(0,0)=Cz*Cy; + transform(0,1)=Cz*Sy*Sx-Sz*Cx; + transform(0,2)=Cz*Sy*Cx+Sz*Sx; + transform(0,3)=pos(0); + transform(1,0)=Sz*Cy; + transform(1,1)=Sz*Sy*Sx+Cz*Cx; + transform(1,2)=Sz*Sy*Cx-Cz*Sx; + transform(1,3)=pos(1); + transform(2,0)=-Sy; + transform(2,1)=Cy*Sx; + transform(2,2)=Cy*Cx; + transform(2,3)=pos(2); + + return transform; +} + +void CDarwinLegKinematics::zero_small_values(Eigen::Matrix4d &matrix,double threshold) +{ + unsigned int i,j; + + for(i=0;i<4;i++) + for(j=0;j<4;j++) + if(fabs(matrix(i,j))<threshold) + matrix(i,j)=0.0; +} + void CDarwinLegKinematics::load_chain(std::string &filename) { joints_t::joint_iterator iterator; @@ -41,8 +78,8 @@ void CDarwinLegKinematics::load_chain(std::string &filename) { try{ std::auto_ptr<joints_t> chain(darwin_kin_chain(filename.c_str(), xml_schema::flags::dont_validate)); - this->fw_kin_params.clear(); - this->fw_kin_transforms.clear(); + this->fwd_kin_params.clear(); + this->fwd_kin_transforms.clear(); this->num_dof=0; for(iterator=chain->joint().begin(),i=0;iterator!=chain->joint().end();iterator++,i++) { @@ -50,7 +87,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename) params.a=iterator->params().a(); params.d=iterator->params().d(); params.theta=iterator->params().theta(); - this->fw_kin_params.push_back(params); + this->fwd_kin_params.push_back(params); // precompute the transforms transform(2,0)=0.0; transform(2,1)=sin(params.alpha); @@ -60,7 +97,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename) transform(3,1)=0.0; transform(3,2)=0.0; transform(3,3)=1.0; - this->fw_kin_transforms.push_back(transform); + this->fwd_kin_transforms.push_back(transform); this->num_dof++; } }catch(const xml_schema::exception& e){ @@ -76,7 +113,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename) void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot) { unsigned int i=0; - Eigen::Matrix4d total_transform=Eigen::Matrix4d::Identity(); + Eigen::Matrix4d total_transform=Eigen::Matrix4d::Identity(),rotate; if(angles.size()!=this->num_dof) throw CDarwinRobotException(_HERE_,"Invalid angle vector size"); @@ -84,25 +121,32 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st { for(i=0;i<this->num_dof;i++) { - this->fw_kin_transforms[i](0,0)=cos(angles[i]+this->fw_kin_params[i].theta); - this->fw_kin_transforms[i](0,1)=-sin(angles[i]+this->fw_kin_params[i].theta)*cos(this->fw_kin_params[i].alpha); - this->fw_kin_transforms[i](0,2)=sin(angles[i]+this->fw_kin_params[i].theta)*sin(this->fw_kin_params[i].alpha); - this->fw_kin_transforms[i](0,3)=this->fw_kin_params[i].a*cos(angles[i]+this->fw_kin_params[i].theta); - this->fw_kin_transforms[i](1,0)=sin(angles[i]+this->fw_kin_params[i].theta); - this->fw_kin_transforms[i](1,1)=cos(angles[i]+this->fw_kin_params[i].theta)*cos(this->fw_kin_params[i].alpha); - this->fw_kin_transforms[i](1,2)=-cos(angles[i]+this->fw_kin_params[i].theta)*sin(this->fw_kin_params[i].alpha); - this->fw_kin_transforms[i](1,3)=this->fw_kin_params[i].a*sin(angles[i]+this->fw_kin_params[i].theta); - total_transform*=this->fw_kin_transforms[i]; + this->fwd_kin_transforms[i](0,0)=cos(angles[i]+this->fwd_kin_params[i].theta); + this->fwd_kin_transforms[i](0,1)=-sin(angles[i]+this->fwd_kin_params[i].theta)*cos(this->fwd_kin_params[i].alpha); + this->fwd_kin_transforms[i](0,2)=sin(angles[i]+this->fwd_kin_params[i].theta)*sin(this->fwd_kin_params[i].alpha); + this->fwd_kin_transforms[i](0,3)=this->fwd_kin_params[i].a*cos(angles[i]+this->fwd_kin_params[i].theta); + this->fwd_kin_transforms[i](1,0)=sin(angles[i]+this->fwd_kin_params[i].theta); + this->fwd_kin_transforms[i](1,1)=cos(angles[i]+this->fwd_kin_params[i].theta)*cos(this->fwd_kin_params[i].alpha); + this->fwd_kin_transforms[i](1,2)=-cos(angles[i]+this->fwd_kin_params[i].theta)*sin(this->fwd_kin_params[i].alpha); + 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, + 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(); + this->zero_small_values(total_transform,0.001); + 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(total_transform(0,0)==0.0 && total_transform(1,0)==0.0) + 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]=1.5707; + rot[1]=asin(-total_transform(2,0)); rot[2]=0.0; } else @@ -116,12 +160,91 @@ void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,st void CDarwinLegKinematics::get_inverse_kinematics(std::vector<double> &pos,std::vector<double> &rot,std::vector<double> &angles) { + Eigen::Matrix4d Tad,Tda,Tcd,Tdc,Tac; + Eigen::Vector3d vec,orientation,position; + double _Rac,_Acos,_Atan,_k,_l,_m,_n,_s,_c,_theta,tmp; + + angles.resize(6); + + 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); + + vec(0)=pos[0]+Tad(0,2)*DARWIN_ANKLE_LENGTH; + vec(1)=pos[1]+Tad(1,2)*DARWIN_ANKLE_LENGTH; + vec(2)=(pos[2]+DARWIN_PELVIS_LENGTH)+Tad(2,2)*DARWIN_ANKLE_LENGTH; + + // Get Knee + _Rac=vec.norm(); + + tmp=(pow(_Rac,2.0)-pow(DARWIN_THIGH_LENGTH,2.0)-pow(DARWIN_CALF_LENGTH,2.0))/(2.0*DARWIN_THIGH_LENGTH*DARWIN_CALF_LENGTH); + if(tmp>1.0) tmp=1.0; + else if(tmp<-1.0) tmp=1.0; + _Acos=acos(tmp); + if(isnan(_Acos)==1) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + // this is not the same for both legs + angles[3]=_Acos; + + // Get Ankle Roll + if(Tad.determinant()==0.0) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + Tda=Tad.inverse(); + _k=sqrt(pow(Tda(1,3),2.0)+pow(Tda(2,3),2.0)); + _l=sqrt(pow(Tda(1,3),2.0)+pow(Tda(2,3)-DARWIN_ANKLE_LENGTH,2.0)); + _m=(pow(_k,2.0)-pow(_l,2.0)-pow(DARWIN_ANKLE_LENGTH,2.0))/(2.0*_l*DARWIN_ANKLE_LENGTH); + if(_m>1.0) _m=1.0; + else if(_m<-1.0) _m=-1.0; + _Acos=acos(_m); + if(isnan(_Acos)==1) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + if(Tda(1,3)<0.0) + angles[5]=-_Acos; + else + angles[5]=_Acos; + + // Get Hip Yaw + orientation << angles[5],0,0; + position << 0,0,-DARWIN_ANKLE_LENGTH; + Tcd=this->inv_kin_gen_transform(position,orientation); + if(Tcd.determinant()==0.0) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + Tdc=Tcd.inverse(); + Tac=Tad*Tdc; + + _Atan=atan2(-Tac(0,1),Tac(1,1)); + if(isinf(_Atan)==1) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + angles[0]=_Atan; + + // Get Hip Roll + _Atan=atan2(Tac(2,1),-Tac(0,1)*sin(angles[0])+Tac(1,1)*cos(angles[0])); + if(isinf(_Atan)==1) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + angles[1]=_Atan; + + // Get Hip Pitch and Ankle Pitch + _Atan=atan2(Tac(0,2)*cos(angles[0])+Tac(1,2)*sin(angles[0]),Tac(0,0)*cos(angles[0])+Tac(1,0)*sin(angles[0])); + if(isinf(_Atan)==1) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + _theta=_Atan; + _k=sin(angles[3])*DARWIN_CALF_LENGTH; + _l=-DARWIN_THIGH_LENGTH-cos(angles[3])*DARWIN_CALF_LENGTH; + _m=cos(angles[0])*vec(0)+sin(angles[0])*vec(1); + _n=cos(angles[1])*vec(2)+sin(angles[0])*sin(angles[1])*vec(0)-cos(angles[0])*sin(angles[1])*vec(1); + _s=(_k*_n+_l*_m)/(pow(_k,2.0)+pow(_l,2.0)); + _c=(_n-_k*_s)/_l; + _Atan=atan2(_s,_c); + if(isinf(_Atan)== 1) + throw CDarwinRobotException(_HERE_,"Impossible to find solution"); + angles[2]=_Atan; + angles[4]=-(_theta-angles[3]-angles[2]); } CDarwinLegKinematics::~CDarwinLegKinematics() { this->num_dof=0; - this->fw_kin_transforms.clear(); - this->fw_kin_params.clear(); + this->fwd_kin_transforms.clear(); + this->fwd_kin_params.clear(); } diff --git a/src/darwin_leg_kinematics.h b/src/darwin_leg_kinematics.h index 0ebcd76..5ff9247 100644 --- a/src/darwin_leg_kinematics.h +++ b/src/darwin_leg_kinematics.h @@ -12,14 +12,23 @@ typedef struct{ double theta; }TDHParams; +#define DARWIN_PELVIS_LENGTH 0.029 //mm +#define DARWIN_LEG_SIDE_OFFSET 0.037 //mm +#define DARWIN_THIGH_LENGTH 0.093 //mm +#define DARWIN_CALF_LENGTH 0.093 //mm +#define DARWIN_ANKLE_LENGTH 0.0335 //mm +#define DARWIN_LEG_LENGTH (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm + class CDarwinLegKinematics { private: unsigned int num_dof; - std::vector<Eigen::Matrix4d> fw_kin_transforms; - std::vector<TDHParams> fw_kin_params; + std::vector<Eigen::Matrix4d> fwd_kin_transforms; + 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); diff --git a/src/examples/darwin_leg_kin.cpp b/src/examples/darwin_leg_kin.cpp index c527a0b..0d67a7a 100644 --- a/src/examples/darwin_leg_kin.cpp +++ b/src/examples/darwin_leg_kin.cpp @@ -3,11 +3,16 @@ #include <iostream> +#define PI 3.14159 + std::string kin_file="../src/xml/right_leg.xml"; int main(int argc, char *argv[]) { - std::vector<double> joints_in(6),pos,rot_out; + double i,j,k,l,m,n,error; + int num_total=0,num_sol=0; + std::vector<double> joints_in(6),pos_in,rot_in; + std::vector<double> joints_out(6),pos_out,rot_out; try{ CDarwinLegKinematics kin; @@ -19,8 +24,61 @@ int main(int argc, char *argv[]) joints_in[3]=atof(argv[4]); joints_in[4]=atof(argv[5]); joints_in[5]=atof(argv[6]); - kin.get_forward_kinematics(joints_in,pos,rot_out); - std::cout << pos[0] << "," << pos[1] << "," << pos[2] << std::endl; + kin.get_forward_kinematics(joints_in,pos_out,rot_out); + std::cout << "position:" << pos_out[0] << "," << pos_out[1] << "," << pos_out[2] << std::endl; + std::cout << "orientation:" << rot_out[0] << "," << rot_out[1] << "," << rot_out[2] << std::endl; + pos_in=pos_out; + rot_in=rot_out; + kin.get_inverse_kinematics(pos_in,rot_in,joints_out); + std::cout << "joints: " << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << "," << joints_out[3] << "," << joints_out[4] << "," << joints_out[5] << "," << std::endl; + for(i=-PI/4;i<PI/4;i+=0.2) + { + for(j=-PI/4;j<PI/4;j+=0.2) + { + for(k=-PI/4;k<PI/4;k+=0.2) + { + for(l=0;l<PI/4;l+=0.2) + { + for(m=-PI/4;m<PI/4;m+=0.2) + { + for(n=-PI/4;n<PI/4;n+=0.2) + { + num_total++; + joints_in[0]=i; + joints_in[1]=j; + joints_in[2]=k; + joints_in[3]=l; + joints_in[4]=m; + joints_in[5]=n; + try{ + std::cout << i << "," << j << "," << k << "," << l << "," << m << "," << n << std::endl; + kin.get_forward_kinematics(joints_in,pos_out,rot_out); + pos_in=pos_out; + rot_in=rot_out; + kin.get_inverse_kinematics(pos_in,rot_in,joints_out); + error=0.0; + error+=pow(i-joints_out[0],2.0); + error+=pow(j-joints_out[1],2.0); + error+=pow(k-joints_out[2],2.0); + error+=pow(l-joints_out[3],2.0); + error+=pow(m-joints_out[4],2.0); + error+=pow(n-joints_out[5],2.0); + error=sqrt(error); + std::cout << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << "," << joints_out[3] << "," << joints_out[4] << "," << joints_out[5] << std::endl; + if(error>0.1) + std::cout << "Error: " << error << std::endl; + else + num_sol++; + }catch(CException &e){ + std::cout << e.what() << std::endl; + } + } + } + } + } + } + } + std::cout << "Found " << num_sol << " solutions of " << num_total << " attempts" << std::endl; }catch(CException &e){ std::cout << e.what() << std::endl; diff --git a/src/xml/left_leg.xml b/src/xml/left_leg.xml index 23eecbd..9267610 100644 --- a/src/xml/left_leg.xml +++ b/src/xml/left_leg.xml @@ -5,8 +5,8 @@ <joint> <params> <a>0.0</a> - <alpha>1.5707</alpha> - <d>0.028652</d> + <alpha>-1.5707</alpha> + <d>-0.029</d> <theta>-1.5707</theta> </params> <max_angle>1.5707</max_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 f5cef18..975bbb6 100644 --- a/src/xml/right_leg.xml +++ b/src/xml/right_leg.xml @@ -5,8 +5,8 @@ <joint> <params> <a>0.0</a> - <alpha>1.5707</alpha> - <d>0.028652</d> + <alpha>-1.5707</alpha> + <d>-0.029</d> <theta>-1.5707</theta> </params> <max_angle>1.5707</max_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