r2 updated module

naoqi/modules/R2module/config/dh_head.cfg

@@ -1,7 +1,8 @@
 [ chain ] = 
 {
-T 0 0 126.5
 H 0 -M_PI/2 0 q1
 H 0 M_PI/2 0 q2
 T 53.9 0 67.9
 }
+# CoM-head shift
+T 0 0 126.5

naoqi/modules/R2module/config/dh_leftArm.cfg

@@ -1,18 +1,14 @@
 [ chain ] = 
 {
-T 0 98 100
-R 1 0 0 -M_PI/2
-H 0 0 0 q1
-R 1 0 0 M_PI/2
+H 0 M_PI/2 0 q1
 H 0 0 0 q2
-T 105 15 0
 R 0 0 1 -M_PI/2
 R 1 0 0 -M_PI/2
-H 0 0 0 q3
+H -15 M_PI/2 105 q3
 R 0 0 1 M_PI/2
-R 0 1 0 -M_PI/2
 H 0 0 0 q4
 T 113.7 0 12.31
-R 0 1 0 M_PI/2
+R 0 0 1 -M_PI/2
+R 1 0 0 -M_PI/2
 H 0 0 0 q5
 }

naoqi/modules/R2module/config/dh_rightArm.cfg

@@ -1,11 +1,14 @@
 [ chain ] = 
 {
-T 0 -98 100 100
-H 0 -M_PI/2 0 q1
-H 105 M_PI/2 -15 q2
+H 0 M_PI/2 0 q1
+H 0 0 0 q2
 R 0 0 1 -M_PI/2
-H 0 -M_PI/2 0 q3
-T 0 -12.31 0
+R 1 0 0 -M_PI/2
+H 15 M_PI/2 105 q3
+R 0 0 1 M_PI/2
 H 0 0 0 q4
-H 0 0 113.7 q5
+T 113.7 0 12.31
+R 0 0 1 -M_PI/2
+R 1 0 0 -M_PI/2
+H 0 0 0 q5
 }

naoqi/modules/R2module/include/libmath/Rutils.h

@@ -117,6 +117,8 @@ Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift);
 void Hz(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* mz);
 Eigen::Matrix4d homZ(double a, const Eigen::Vector3d& shift);
 
+Eigen::Matrix4d hTranslation(const Eigen::Vector3d& shift);
+
 /*
  * Compute the Moore-Penrose pseudo-inversion of matrix 'm' using SVD decomposition.
  * Optional value is the default tolerance for the singular values of 'm'.

naoqi/modules/R2module/include/r2Module.h

@@ -75,9 +75,11 @@ class R2Module : public AL::ALModule
     // Common base kinematic chain (left foot to body center)
     Rmath::KinChain* theKinChainLeftLeg;
     // All initialized tasks
-    std::map<std::string, Task*> taskMap;
+    std::map <std::string, Task*> taskMap;
     // Stack of tasks currently active
-    std::set<TaskBase*, taskCmp> taskSet;
+    std::set<Task*, taskCmp> taskSet;
+    // Tasks respective positions within the whole kinematic chain
+    std::map <Task*, int> taskLoc;
 
 public:
 

naoqi/modules/R2module/include/task.h

@@ -108,14 +108,39 @@ class Task : public TaskBase
     // The task kinematic chain
     Rmath::KinChain* theKinChain;
 
+    // Toggle positioning/velocity task
+    bool positioningActive;
+    // Toggle joint space/cartesian space control
+    bool jointControlActive;
+
+    // Discrete path to desired pose
+    std::vector<Eigen::VectorXd> path;
+    // Current progress in the path
+    int path_currentStep;
+
 public:
     // Constructor
     Task(int m, int n,  int _priority, ConfigReader theConfigReader, int _base, int _ee);
     // Destructor
     ~Task();
 
+    // Retrieve the current end-effector pose
+    Eigen::VectorXd getCurrentPose(const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()) const;
+    // Set a desired pose in the task space
+    void setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints = 1,
+                        const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity());
+    // Set a desired configuration in the joint space
+    void setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints);
+    // Reset the current target pose (either in joint or task space)
+    inline void resetDesiredPose()
+    {
+        positioningActive = false;
+        path.clear();
+    }
+
     // Update function
-    void update(const Eigen::VectorXd& q, double K, const Eigen::VectorXd& desiredPose, const Eigen::VectorXd& desiredVel);
+    void update(const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel,
+                double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity());
 };
 
 #endif

naoqi/modules/R2module/src/configReader.cpp

@@ -92,7 +92,7 @@ void ConfigReader::dhReader(std::string dh_file_path)
         else if (tokens.at(0) == "T")
         {
             Rmath::Transform* transl = new Rmath::Translation( to_double(tokens.at(1)),
-                                                               to_double(tokens.at(2)) ,
+                                                               to_double(tokens.at(2)),
                                                                to_double(tokens.at(3)) );
             transformations.push_back(transl);
         }
@@ -105,11 +105,12 @@ void ConfigReader::dhReader(std::string dh_file_path)
                                                          valueReader(tokens.at(4)) );
             transformations.push_back(rot);
         }
+
+        if (tokens.at(0) == "}") break;
     }
     cfg_dh.close();
 }
 
-
 void ConfigReader::paramsReader(std::string params_file_path)
 {
 

naoqi/modules/R2module/src/libmath/Rutils.cpp

@@ -21,6 +21,7 @@
 #include "libmath/transform.h"
 
 #define INFO(x) std::cout << "[Rmath] " << x << std::endl;
+//#define JACOBIAN_DEBUG std::cout << "[Rmath::Jacobian] " << x << std::endl;
 
 /** -------- Core tools -------- */
 
@@ -386,6 +387,23 @@ Eigen::Matrix4d Rmath::homZ(double a, const Eigen::Vector3d& shift)
     return hz;
 }
 
+Eigen::Matrix4d Rmath::hTranslation(const Eigen::Vector3d& shift)
+{
+    Eigen::Matrix4d hz;
+    // Using Eigen comma initializer
+    hz  <<   1,       0,       0,  shift(0),
+             0,       1,       0,  shift(1),
+             0,       0,       1,  shift(2),
+             0,       0,       0,         1;
+
+#ifdef CROP_TO_TOLERANCE
+    // Trim according to default numerical tolerance
+    trim(&hz);
+#endif
+
+    return hz;
+}
+
 /*
  * Compute the Moore-Penrose pseudo-inversion of matrix 'm' using SVD decomposition.
  * Optional value is the default tolerance for the singular values of 'm'.
@@ -506,7 +524,8 @@ void Rmath::DirectKin(const std::vector<Rmath::Transform*>& chain, Eigen::Matrix
     (*H) = Eigen::Matrix4d::Identity();
 
     // Go through the chain, compute i-to-i+1 transformation and post-multiply
-    for (unsigned int i=0; i < chain.size(); ++i) (*H) *= chain.at(i)->transform();
+    for (unsigned int i=0; i < chain.size(); ++i)
+        (*H) *= chain.at(i)->transform();
 
 #ifdef CROP_TO_TOLERANCE
     // Trim according to default numerical tolerance
@@ -582,6 +601,13 @@ void Rmath::geomJacobian(const std::vector<Rmath::Transform*>& chain, const std:
     // Initialization of J as a zero-matrix
     (*J) = Eigen::MatrixXd::Zero(taskSpaceDim, i_joints.size());
 
+//#ifdef JACOBIAN_DEBUG
+//    INFO( "\ttask dimension"<< taskSpaceDim );
+//    for(int i=0; i< chain.size(); ++i) chain.at(i)->print(std::cout) ;
+//    INFO( "\tnumber of joints: "<< i_joints.size() );
+//    for(int i=0; i< i_joints.size(); ++i) INFO( "\ti_joints: "<< i_joints.at(i) );
+//#endif
+
     // Default Z-axis in the base frame [ 0 , 0 , 1 ]'
     Eigen::Vector3d z = Eigen::Vector3d::UnitZ();
 
@@ -591,22 +617,31 @@ void Rmath::geomJacobian(const std::vector<Rmath::Transform*>& chain, const std:
     // Base-EE translation vector
     Eigen::Vector3d d_0e = H_ee.topRightCorner(3, 1);
 
+
     // Jacobian leftmost column
     Eigen::VectorXd J0(6,1); J0 << z.cross(d_0e), z;
     // Crop to required task space dimension
     J->col(0) = J0.head(taskSpaceDim);
 
+//#ifdef JACOBIAN_DEBUG
+//    INFO( "\tendeffector translation d0e: \n"<< H_ee.topRightCorner(3, 1) );
+//#endif
+
     // Go through the chain of transforms and build up J column-wise
     for (int i=1; i < i_joints.size(); ++i)
     {
         // Consider the subchain of transformation leading to joint i-1
         Eigen::Matrix4d currentH;
         std::vector<Rmath::Transform*> currentSubchain; //(chain.begin(), chain.begin()+i_joints.at(i-1));
-        for (int j=0; j <= i_joints.at(i)-1; ++j)
+        for (int j=0; j < i_joints.at(i); ++j)
             currentSubchain.push_back( chain.at(j) );
+
         // Compute forward kinematics up to joint i-1
         DirectKin(currentSubchain, &currentH);
 
+//#ifdef JACOBIAN_DEBUG
+//    INFO( "\tKinChain joint: "<<i<< std::endl << currentH.topRightCorner(3, 1) );
+//#endif
         // Extract current Z-axis in the reference frame of joint i-1
         Eigen::Vector3d currentZ = currentH.topLeftCorner(3,3) * z;
         // Extract base-to-joint-i translation vector

naoqi/modules/R2module/src/libmath/kinChain.cpp

@@ -240,15 +240,16 @@ void Rmath::KinChain::differential(int base, int ee, Eigen::MatrixXd* J, int tas
 
      // Collect all joint positions within the chain
      std::vector<int> q_indices;
-     int i = 0;
      std::map<int, Rmath::KinChain::Joint>::iterator selector = joints.begin();
+
      // Iterate over the joint container
      while(selector != joints.end())
      {
          // Skip joints out of the desired range
-         if ( (i >= base) && (i <= ee) )
-             q_indices.push_back(selector->first);
-         ++selector; ++i;
+         if ( selector->first >= base && selector->first <= ee )
+             q_indices.push_back(selector->first -base);
+
+         ++selector;
      }
 
      // Compute the Jacobian matrix

naoqi/modules/R2module/src/libmath/transform.cpp

@@ -100,7 +100,7 @@ void Rmath::Translation::parameters(std::map<char, double>* params)
  */
 Eigen::Matrix4d Rmath::Translation::transform() const
 {
-    return Rmath::homZ(0.0, values);
+    return Rmath::hTranslation(values);
 }
 
 /*