[hrplib/hrpUtil/MatrixSolvers] add calcSRInverse

hrplib/hrpModel/JointPath.cpp

@@ -17,11 +17,19 @@
 #include "Link.h"
 #include <hrpUtil/MatrixSolvers.h>
 #include <algorithm>
+#include <limits.h>
+#include <float.h>
 
 using namespace std;
 using namespace hrp;
 
 
+//#define DEBUG true
+#define DEBUG false
+#define deg2rad(x)((x)*M_PI/180)
+#define rad2deg(rad) (rad * 180 / M_PI)
+#define eps_eq(a, b, c)  (fabs((a)-(b)) <= c)
+
 JointPath::JointPath()
 {
     initialize();
@@ -52,7 +60,7 @@ void JointPath::initialize()
     manipulability_limit = 0.1;
     manipulability_gain = 0.001;
     maxIKPosErrorSqr = 1.0e-8;
-    maxIKRotErrorSqr = 1.0e-6:
+    maxIKRotErrorSqr = 1.0e-6;
     maxIKIteration = 50;
     isBestEffortIKMode = false;
     avoid_weight_gain.resize(numJoints());
@@ -297,11 +305,6 @@ void JointPath::setMaxIKError(double epos, double erot) {
   maxIKRotErrorSqr = erot*erot;
 }
 
-void JointPath::setMaxIKError(double e)
-{
-    maxIKErrorSqr = e * e;
-}
-
 void JointPath::setMaxIKIteration(int iter) {
     maxIKIteration = iter;
 }
@@ -571,7 +574,7 @@ bool JointPath::calcInverseKinematics2(const Vector3& end_p, const Matrix33& end
       }
 
       Vector3 dp(end_p - target->p);
-      Vector3 omega(target->R * omegaFromRotEx(target->R.transpose() * end_R));
+      Vector3 omega(target->R * omegaFromRot(target->R.transpose() * end_R));
       if ( dp.norm() > 0.1 ) dp = dp*0.1/dp.norm();
       if ( omega.norm() > 0.5 ) omega = omega*0.5/omega.norm();
 
@@ -607,7 +610,7 @@ bool JointPath::calcInverseKinematics2(const Vector3& end_p, const Matrix33& end
     if(!converged){
       std::cerr << "IK Fail, iter = " << iter << std::endl;
       Vector3 dp(end_p - target->p);
-      Vector3 omega(target->R * omegaFromRotEx(target->R.transpose() * end_R));
+      Vector3 omega(target->R * omegaFromRot(target->R.transpose() * end_R));
       const double errsqr = dp.dot(dp) + omega.dot(omega);
       if(isBestEffortIKMode){
         std::cerr << "  err : fabs(" << errsqr << " - " << errsqr0 << ") = " << fabs(errsqr-errsqr0) << " < " << maxIKErrorSqr << " BestEffortIKMode" << std::endl;

hrplib/hrpModel/JointPath.h

@@ -89,7 +89,6 @@ namespace hrp {
         virtual bool setManipulabilityLimit(double l) { manipulability_limit = l; }
         virtual bool setManipulabilityGain(double l) { manipulability_gain = l; }
         virtual void setMaxIKError(double epos, double erot);
-        virtual void setMaxIKError(double e);
         virtual void setMaxIKIteration(int iter);
 
       protected:
@@ -101,7 +100,6 @@ namespace hrp {
 
         double maxIKPosErrorSqr, maxIKRotErrorSqr;
         int maxIKIteration;
-        std::vector<Link*> joints;
         std::vector<double> avoid_weight_gain;
 	double sr_gain, manipulability_limit, manipulability_gain;
 

hrplib/hrpUtil/EigenTypes.h

@@ -18,26 +18,6 @@ namespace hrp{
 };
 
 #include <iostream>
-inline std::ostream& operator<<(std::ostream& out, hrp::dmatrix &a) {
-    const int c = a.rows();
-    const int n = a.cols();
-
-    for(int i = 0; i < c; i++){
-        out << "      :";
-        for(int j = 0; j < n; j++){
-            out << " " << std::setw(7) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << (a)(i,j);
-        }
-        out << std::endl;
-    }
-}
-
-inline std::ostream& operator<<(std::ostream& out, hrp::dvector &a) {
-    const int n = a.size();
-
-    for(int i = 0; i < n; i++){
-        out << std::setw(7) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << a(i) << " ";
-    }
-    out << std::endl;
-}
+#include <iomanip>
 
 #endif

hrplib/hrpUtil/MatrixSolvers.cpp

@@ -46,6 +46,28 @@ extern "C" void dgeev_(char const*jobvl, char const*jobvr, int *n, double *A,
 					  int *ldvl, double *vr, int *ldvr, double *work, int *lwork, int *info);
 
 
+inline std::ostream& operator<<(std::ostream& out, hrp::dmatrix &a) {
+    const int c = a.rows();
+    const int n = a.cols();
+
+    for(int i = 0; i < c; i++){
+        out << "      :";
+        for(int j = 0; j < n; j++){
+            out << " " << std::setw(7) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << (a)(i,j);
+        }
+        out << std::endl;
+    }
+}
+
+inline std::ostream& operator<<(std::ostream& out, hrp::dvector &a) {
+    const int n = a.size();
+
+    for(int i = 0; i < n; i++){
+        out << std::setw(7) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << a(i) << " ";
+    }
+    out << std::endl;
+}
+
 // originally in hrpCLAPACK.{cpp,h}
 // solveLinearEquation()
 // b = a * x, x = b^(-1) * a
@@ -338,6 +360,31 @@ namespace hrp {
 		}
 
 
+		//--- Calculation of SR-Inverse ---
+		int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w) {
+		    // J# = W Jt(J W Jt + kI)-1 (Weighted SR-Inverse)
+		    // SR-inverse :
+		    // Y. Nakamura and H. Hanafusa : "Inverse Kinematic Solutions With
+		    // Singularity Robustness for Robot Manipulator Control"
+		    // J. Dyn. Sys., Meas., Control  1986. vol 108, Issue 3, pp. 163--172.
+
+		    const int c = _a.rows(); // 6
+		    const int n = _a.cols(); // n
+
+		    if ( _w.cols() != n || _w.rows() != n ) {
+		        _w = dmatrix::Identity(n, n);
+		    }
+
+		    dmatrix at = _a.transpose();
+		    dmatrix a1(c, c);
+		    a1 = (_a * _w * at +  _sr_ratio * dmatrix::Identity(c,c)).inverse();
+
+		    //if (DEBUG) { dmatrix aat = _a * at; std::cerr << " a*at :" << std::endl << aat; }
+
+		    _a_sr  = _w * at * a1;
+		    //if (DEBUG) { dmatrix ii = _a * _a_sr; std::cerr << "    i :" << std::endl << ii; }
+		}
+
 		//--- Calculation of determinamt ---
 		double det(const dmatrix &_a)
 		{

hrplib/hrpUtil/MatrixSolvers.h

@@ -28,6 +28,7 @@ namespace hrp {
     HRP_UTIL_EXPORT int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio=1.0e-3);
 
     HRP_UTIL_EXPORT int calcEigenVectors(const dmatrix &_a, dmatrix  &_evec, dvector &_eval);
+    HRP_UTIL_EXPORT int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w);
     HRP_UTIL_EXPORT double det(const dmatrix &a);
 
     /**