From f956dd771ff1da60e1f4306e8a8838c28075841b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Tue, 16 May 2017 00:31:36 +0200
Subject: [PATCH] Solved a problem with the tolerance calculation in the
inverse kinematics problem. Chnaged the parameters of the arm kinematic
chains.
---
src/darwin_arm_kinematics.cpp | 3 ++-
src/examples/darwin_arm_kin.cpp | 18 +++++++++---------
src/xml/left_arm.xml | 15 ++-------------
src/xml/right_arm.xml | 15 ++-------------
4 files changed, 15 insertions(+), 36 deletions(-)
diff --git a/src/darwin_arm_kinematics.cpp b/src/darwin_arm_kinematics.cpp
index 2c21495..ee1bc22 100644
--- a/src/darwin_arm_kinematics.cpp
+++ b/src/darwin_arm_kinematics.cpp
@@ -196,6 +196,7 @@ void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
do{
count++;
+ max_error=0.0;
for(i=0;i<this->num_dof;i++)
q_init(i)=(rand()*3.14159/RAND_MAX)-1.5707;
for(;i<6;i++)
@@ -207,7 +208,7 @@ void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
else
{
error=desired_frame.p-cartpos_sol.p;
- for(i=0;i<this->num_dof;i++)
+ for(i=0;i<3;i++)
if(fabs(error(i))>max_error)
max_error=fabs(error(i));
}
diff --git a/src/examples/darwin_arm_kin.cpp b/src/examples/darwin_arm_kin.cpp
index 1b65675..f94b6ba 100644
--- a/src/examples/darwin_arm_kin.cpp
+++ b/src/examples/darwin_arm_kin.cpp
@@ -3,11 +3,11 @@
#include <iostream>
-std::string kin_file="../src/xml/right_arm.xml";
+std::string kin_file="../src/xml/left_arm.xml";
int main(int argc, char *argv[])
{
- std::vector<double> joints_in(4),joints_out(4),pos,rot_out,rot_in(3,0.0);
+ std::vector<double> joints_in(3),joints_out(3),pos,rot_out,rot_in(3,0.0);
int num_total=0,num_sol=0;
double i,j,k,l;
@@ -15,7 +15,7 @@ int main(int argc, char *argv[])
CDarwinArmKinematics kin;
kin.load_chain(kin_file);
- if(argc!=5)
+ if(argc!=4)
{
std::cout << "Invalid number of parameters: darwin_arm_kin <joint0 angle> <joint1 angle> <joint2 angle> <joint3 angle>" << std::endl;
exit(-1);
@@ -25,20 +25,20 @@ int main(int argc, char *argv[])
joints_in[0]=atof(argv[1]);
joints_in[1]=atof(argv[2]);
joints_in[2]=atof(argv[3]);
- joints_in[3]=atof(argv[4]);
kin.get_forward_kinematics(joints_in,pos,rot_out);
std::cout << pos[0] << "," << pos[1] << "," << pos[2] << std::endl;
+ std::cout << rot_out[0] << "," << rot_out[1] << "," << rot_out[2] << std::endl;
}
- return 0;
- pos.push_back(0.0743671);
- pos.push_back(0.0502218);
- pos.push_back(0.0791303);
+ pos.resize(3);
+ pos[0]=0.05;
+ pos[1]=-0.09;
+ pos[2]=0.06;
rot_in[0]=0;
rot_in[1]=0;
rot_in[2]=0;
kin.get_inverse_kinematics(pos,rot_in,joints_out);
- std::cout << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << "," << joints_out[3] << std::endl;
+ std::cout << joints_out[0] << "," << joints_out[1] << "," << joints_out[2] << std::endl;
return 0;
for(i=-1.5707;i<1.5707;i+=0.2)
diff --git a/src/xml/left_arm.xml b/src/xml/left_arm.xml
index f592fb9..ed8c57e 100644
--- a/src/xml/left_arm.xml
+++ b/src/xml/left_arm.xml
@@ -27,21 +27,10 @@
<joint>
<name>j_elbow_l</name>
<params>
- <a>-0.02165</a>
+ <a>-0.08165</a>
<alpha>1.5707</alpha>
<d>0</d>
- <theta>0</theta>
- </params>
- <max_angle>1.5707</max_angle>
- <min_angle>-1.5707</min_angle>
- </joint>
- <joint>
- <name>j_wrist_l</name>
- <params>
- <a>0</a>
- <alpha>0</alpha>
- <d>0.056</d>
- <theta>0</theta>
+ <theta>1.5707</theta>
</params>
<max_angle>1.5707</max_angle>
<min_angle>-1.5707</min_angle>
diff --git a/src/xml/right_arm.xml b/src/xml/right_arm.xml
index e61d8e2..0135e4d 100644
--- a/src/xml/right_arm.xml
+++ b/src/xml/right_arm.xml
@@ -27,21 +27,10 @@
<joint>
<name>j_elbow_r</name>
<params>
- <a>-0.02165</a>
+ <a>-0.08165</a>
<alpha>1.5707</alpha>
<d>0</d>
- <theta>0</theta>
- </params>
- <max_angle>1.5707</max_angle>
- <min_angle>-1.5707</min_angle>
- </joint>
- <joint>
- <name>j_wrist_r</name>
- <params>
- <a>0</a>
- <alpha>0</alpha>
- <d>0.056</d>
- <theta>0</theta>
+ <theta>1.5707</theta>
</params>
<max_angle>1.5707</max_angle>
<min_angle>-1.5707</min_angle>
--
GitLab