Added the name of the joint to the XML file.

Added functions to configure the IK and FK problems for the arm.
Updated the example XML files.

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++)

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();

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;

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);

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)

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">

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>

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>

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>

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>