diff --git a/src/darwin_arm_kinematics.cpp b/src/darwin_arm_kinematics.cpp
index b298dc97579cdff3497fe2183fb562651b1fbdd8..2c21495e7111fda0df9ea6456bf1a657e73bd478 100644
--- a/src/darwin_arm_kinematics.cpp
+++ b/src/darwin_arm_kinematics.cpp
@@ -17,6 +17,11 @@ CDarwinArmKinematics::CDarwinArmKinematics()
this->iksolver_pos=NULL;
this->chain=NULL;
this->num_dof=0;
+ this->max_attempts=DEFAULT_MAX_IK_ATTEMPTS;
+ this->max_ik_tolerance=DEFAULT_MAX_IK_TOLERANCE;
+ this->max_iter=DEFAULT_MAX_FK_ITERATIONS;
+ this->max_fk_tolerance=DEFAULT_MAX_FK_TOLERANCE;
+ this->joint_names.clear();
}
double CDarwinArmKinematics::angle_norm(double angle)
@@ -44,6 +49,7 @@ void CDarwinArmKinematics::load_chain(std::string &filename)
if(extra<0) extra=0;
this->q_max.resize(6);
this->q_min.resize(6);
+ this->joint_names.clear();
if(this->chain!=NULL)
{
delete this->chain;
@@ -57,6 +63,7 @@ void CDarwinArmKinematics::load_chain(std::string &filename)
this->chain->addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH(iterator->params().a(),
iterator->params().alpha(),iterator->params().d(),iterator->params().theta())));
this->num_dof++;
+ this->joint_names.push_back(iterator->name());
}
/* add extra joints to get to 6 dof */
for(;i<6;i++)
@@ -75,7 +82,7 @@ void CDarwinArmKinematics::load_chain(std::string &filename)
this->iksolver_vel=new KDL::ChainIkSolverVel_pinv(*this->chain);
if(this->iksolver_pos!=NULL)
delete this->iksolver_pos;
- this->iksolver_pos=new KDL::ChainIkSolverPos_NR_JL(*this->chain,this->q_min,this->q_max,*this->fksolver,*this->iksolver_vel,100,1e-3);
+ this->iksolver_pos=new KDL::ChainIkSolverPos_NR_JL(*this->chain,this->q_min,this->q_max,*this->fksolver,*this->iksolver_vel,this->max_iter,this->max_fk_tolerance);
}catch(const xml_schema::exception& e){
std::ostringstream os;
os << e;
@@ -86,6 +93,56 @@ void CDarwinArmKinematics::load_chain(std::string &filename)
throw CDarwinRobotException(_HERE_,"Arm chain definition file does not exist");
}
+unsigned int CDarwinArmKinematics::get_num_dof(void)
+{
+ return this->num_dof;
+}
+
+std::vector<std::string> CDarwinArmKinematics::get_joint_names(void)
+{
+ return this->joint_names;
+}
+
+void CDarwinArmKinematics::set_ik_max_attempts(unsigned int attempts)
+{
+ this->max_attempts=attempts;
+}
+
+unsigned int CDarwinArmKinematics::get_ik_max_attempts(void)
+{
+ return this->max_attempts;
+}
+
+void CDarwinArmKinematics::set_ik_max_tolerance(double tolerance)
+{
+ this->max_ik_tolerance=tolerance;
+}
+
+double CDarwinArmKinematics::get_ik_max_tolerance(void)
+{
+ return this->max_ik_tolerance;
+}
+
+void CDarwinArmKinematics::set_fk_max_iterations(unsigned int iter)
+{
+ this->max_iter=iter;
+}
+
+unsigned int CDarwinArmKinematics::get_fk_max_iterations(void)
+{
+ return this->max_iter;
+}
+
+void CDarwinArmKinematics::set_fk_max_tolerance(double tolerance)
+{
+ this->max_fk_tolerance=tolerance;
+}
+
+double CDarwinArmKinematics::get_fk_max_tolerance(void)
+{
+ return this->max_fk_tolerance;
+}
+
void CDarwinArmKinematics::get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot)
{
double yaw=0.0,pitch=0.0,roll=0.0;
@@ -123,11 +180,12 @@ void CDarwinArmKinematics::get_forward_kinematics(std::vector<double> &angles,st
void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::vector<double> &rot,std::vector<double> &angles)
{
- int count=0,kinematics_status;
+ int kinematics_status;
KDL::JntArray q_init,joints;
KDL::Frame cartpos_sol;
KDL::Vector error;
- unsigned int i=0;
+ double max_error=0.0;
+ unsigned int i=0,count=0;
if(pos.size()!=3 || rot.size()!=3)
throw CDarwinRobotException(_HERE_,"Invalid position and/or rotation vector sizes");
@@ -147,9 +205,14 @@ void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::
if(kinematics_status<0)
throw CDarwinRobotException(_HERE_,"Impossible to find solution");
else
+ {
error=desired_frame.p-cartpos_sol.p;
- }while((fabs(error(0))>0.001 || fabs(error(1))>0.001 || fabs(error(2))>0.001) && count<50);
- if(count<50)
+ for(i=0;i<this->num_dof;i++)
+ if(fabs(error(i))>max_error)
+ max_error=fabs(error(i));
+ }
+ }while(max_error>this->max_ik_tolerance && count<this->max_attempts);
+ if(count<this->max_attempts)
{
angles.resize(this->num_dof);
for(i=0;i<this->num_dof;i++)
diff --git a/src/darwin_arm_kinematics.h b/src/darwin_arm_kinematics.h
index 6737deef9550e07890fb00ef23a6d682a8583140..66d8a68a055f4348b577fa29b0e092a5a56cd50c 100644
--- a/src/darwin_arm_kinematics.h
+++ b/src/darwin_arm_kinematics.h
@@ -8,6 +8,11 @@
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/frames_io.hpp>
+#define DEFAULT_MAX_IK_ATTEMPTS 50
+#define DEFAULT_MAX_IK_TOLERANCE 0.001
+#define DEFAULT_MAX_FK_ITERATIONS 100
+#define DEFAULT_MAX_FK_TOLERANCE 0.001
+
class CDarwinArmKinematics
{
private:
@@ -17,12 +22,30 @@ class CDarwinArmKinematics
KDL::ChainFkSolverPos_recursive *fksolver;
KDL::ChainIkSolverVel_pinv *iksolver_vel;
KDL::ChainIkSolverPos_NR_JL *iksolver_pos;
+ /* ik parameters */
+ unsigned int max_attempts;
+ double max_ik_tolerance;
+ /* fk parameters */
+ unsigned int max_iter;
+ double max_fk_tolerance;
+ /* general parameters */
unsigned int num_dof;
+ std::vector<std::string> joint_names;
protected:
double angle_norm(double angle);
public:
CDarwinArmKinematics();
void load_chain(std::string &filename);
+ unsigned int get_num_dof(void);
+ std::vector<std::string> get_joint_names(void);
+ void set_ik_max_attempts(unsigned int attempts);
+ unsigned int get_ik_max_attempts(void);
+ void set_ik_max_tolerance(double tolerance);
+ double get_ik_max_tolerance(void);
+ void set_fk_max_iterations(unsigned int iter);
+ unsigned int get_fk_max_iterations(void);
+ void set_fk_max_tolerance(double tolerance);
+ double get_fk_max_tolerance(void);
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);
~CDarwinArmKinematics();
diff --git a/src/darwin_leg_kinematics.cpp b/src/darwin_leg_kinematics.cpp
index 7cc2220bdf73b7353df7e053ed8875d632443fcf..8d0213506dca12da0e5fd41229ca30e7bdfc639c 100644
--- a/src/darwin_leg_kinematics.cpp
+++ b/src/darwin_leg_kinematics.cpp
@@ -17,6 +17,7 @@ CDarwinLegKinematics::CDarwinLegKinematics()
this->num_dof=0;
this->fwd_kin_transforms.clear();
this->fwd_kin_params.clear();
+ this->joint_names.clear();
}
double CDarwinLegKinematics::angle_norm(double angle)
@@ -100,6 +101,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
this->fwd_kin_params.clear();
this->fwd_kin_transforms.clear();
this->num_dof=0;
+ this->joint_names.clear();
for(iterator=chain->joint().begin(),i=0;iterator!=chain->joint().end();iterator++,i++)
{
params.alpha=iterator->params().alpha();
@@ -118,6 +120,7 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
transform(3,3)=1.0;
this->fwd_kin_transforms.push_back(transform);
this->num_dof++;
+ this->joint_names.push_back(iterator->name());
}
}catch(const xml_schema::exception& e){
std::ostringstream os;
@@ -129,6 +132,16 @@ void CDarwinLegKinematics::load_chain(std::string &filename)
throw CDarwinRobotException(_HERE_,"Leg chain definition file does not exist");
}
+unsigned int CDarwinLegKinematics::get_num_dof(void)
+{
+ return this->num_dof;
+}
+
+std::vector<std::string> CDarwinLegKinematics::get_joint_names(void)
+{
+ return this->joint_names;
+}
+
void CDarwinLegKinematics::get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot)
{
unsigned int i=0;
diff --git a/src/darwin_leg_kinematics.h b/src/darwin_leg_kinematics.h
index 29b82c6f39b97aa5940396812fbda318bc3c4dc5..c1d3587b1313ca950bbe04be37a36cfd9c0e232f 100644
--- a/src/darwin_leg_kinematics.h
+++ b/src/darwin_leg_kinematics.h
@@ -24,6 +24,7 @@ class CDarwinLegKinematics
{
private:
unsigned int num_dof;
+ std::vector<std::string> joint_names;
std::vector<Eigen::Matrix4d> fwd_kin_transforms;
std::vector<TDHParams> fwd_kin_params;
protected:
@@ -32,6 +33,8 @@ class CDarwinLegKinematics
public:
CDarwinLegKinematics();
void load_chain(std::string &filename);
+ unsigned int get_num_dof(void);
+ std::vector<std::string> get_joint_names(void);
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);
diff --git a/src/examples/darwin_arm_kin.cpp b/src/examples/darwin_arm_kin.cpp
index 8563c63c2910640bfc5c24c784bb1d4b7f8b6651..1b6567540949f0f3ce092ece7ebc0d005ab894f3 100644
--- a/src/examples/darwin_arm_kin.cpp
+++ b/src/examples/darwin_arm_kin.cpp
@@ -15,13 +15,22 @@ int main(int argc, char *argv[])
CDarwinArmKinematics kin;
kin.load_chain(kin_file);
- 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;
+ if(argc!=5)
+ {
+ std::cout << "Invalid number of parameters: darwin_arm_kin <joint0 angle> <joint1 angle> <joint2 angle> <joint3 angle>" << std::endl;
+ exit(-1);
+ }
+ else
+ {
+ 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;
+ }
return 0;
+
pos.push_back(0.0743671);
pos.push_back(0.0502218);
pos.push_back(0.0791303);
@@ -31,6 +40,7 @@ int main(int argc, char *argv[])
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;
return 0;
+
for(i=-1.5707;i<1.5707;i+=0.2)
{
for(j=-1.5707;j<1.5707;j+=0.2)
diff --git a/src/xml/darwin_kin_chain.xsd b/src/xml/darwin_kin_chain.xsd
index 1cb71d91c167b37efee67065344867611f7e33e9..191f127e27ac1097d41634ae94dedbb572adcb36 100644
--- a/src/xml/darwin_kin_chain.xsd
+++ b/src/xml/darwin_kin_chain.xsd
@@ -24,6 +24,8 @@ copyright : not copyrighted - public domain
<xsd:complexType name="joint_t">
<xsd:sequence>
+ <xsd:element name="name" type="xsd:string">
+ </xsd:element>
<xsd:element name="params" type="dh_t">
</xsd:element>
<xsd:element name="max_angle" type="xsd:float">
diff --git a/src/xml/left_arm.xml b/src/xml/left_arm.xml
index f4e2cd1d671bf29f4a5464636acce0c28296e182..f592fb9546ec6a4a0f087ffa3b07efef1335d7c3 100644
--- a/src/xml/left_arm.xml
+++ b/src/xml/left_arm.xml
@@ -3,6 +3,7 @@
<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
<joint>
+ <name>j_shoulder_pitch_l</name>
<params>
<a>-0.016</a>
<alpha>1.5707</alpha>
@@ -13,6 +14,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_shoulder_roll_l</name>
<params>
<a>-0.06</a>
<alpha>1.5707</alpha>
@@ -23,6 +25,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_elbow_l</name>
<params>
<a>-0.02165</a>
<alpha>1.5707</alpha>
@@ -33,6 +36,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_wrist_l</name>
<params>
<a>0</a>
<alpha>0</alpha>
diff --git a/src/xml/left_leg.xml b/src/xml/left_leg.xml
index 70113b2064fc050ec29db7b05ed33818b8ab4163..32522de8dbf1f8bc1251c00dc9b73d9379bee856 100644
--- a/src/xml/left_leg.xml
+++ b/src/xml/left_leg.xml
@@ -3,6 +3,7 @@
<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
<joint>
+ <name>j_hip_yaw_l</name>
<params>
<a>0.0</a>
<alpha>-1.5707</alpha>
@@ -13,6 +14,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_hip_roll_l</name>
<params>
<a>0.0</a>
<alpha>-1.5707</alpha>
@@ -23,6 +25,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_hip_pitch_l</name>
<params>
<a>0.093</a>
<alpha>0.0</alpha>
@@ -33,6 +36,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_knee_l</name>
<params>
<a>0.093</a>
<alpha>3.14159</alpha>
@@ -43,6 +47,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_ankle_pitch_l</name>
<params>
<a>0.0</a>
<alpha>1.5707</alpha>
@@ -53,6 +58,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_ankle_roll_l</name>
<params>
<a>0.0335</a>
<alpha>0.0</alpha>
diff --git a/src/xml/right_arm.xml b/src/xml/right_arm.xml
index 1b4e6a1411d42668d7a858f1b9670853fb92bc4e..e61d8e2f081ef4a4d18ad9c8cf40dd7028080430 100644
--- a/src/xml/right_arm.xml
+++ b/src/xml/right_arm.xml
@@ -3,6 +3,7 @@
<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
<joint>
+ <name>j_shoulder_pitch_r</name>
<params>
<a>-0.016</a>
<alpha>-1.5707</alpha>
@@ -13,6 +14,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_shoulder_roll_r</name>
<params>
<a>-0.06</a>
<alpha>1.5707</alpha>
@@ -23,6 +25,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_elbow_r</name>
<params>
<a>-0.02165</a>
<alpha>1.5707</alpha>
@@ -33,6 +36,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_wrist_r</name>
<params>
<a>0</a>
<alpha>0</alpha>
diff --git a/src/xml/right_leg.xml b/src/xml/right_leg.xml
index 8a2152512acd2b6e011800ace438afbad1055a1e..6d3acc97c18d8d6242f31a6edffa56ab67bfcdf1 100644
--- a/src/xml/right_leg.xml
+++ b/src/xml/right_leg.xml
@@ -3,6 +3,7 @@
<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
<joint>
+ <name>j_hip_yaw_r</name>
<params>
<a>0.0</a>
<alpha>-1.5707</alpha>
@@ -13,6 +14,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_hip_roll_r</name>
<params>
<a>0.0</a>
<alpha>1.5707</alpha>
@@ -23,6 +25,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_hip_pitch_r</name>
<params>
<a>0.093</a>
<alpha>0.0</alpha>
@@ -33,6 +36,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_knee_r</name>
<params>
<a>0.093</a>
<alpha>3.14159</alpha>
@@ -43,6 +47,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_ankle_pitch_r</name>
<params>
<a>0.0</a>
<alpha>-1.5707</alpha>
@@ -53,6 +58,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_ankle_roll_r</name>
<params>
<a>0.0335</a>
<alpha>0.0</alpha>