From 04a47cc98f4e5fb28ac403cc866616777cd48593 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 23 Feb 2015 20:13:40 -0800 Subject: [PATCH] [hrplib/hrpUtil/MatrixSolvers] add calcSRInverse --- hrplib/hrpModel/JointPath.cpp | 19 +++++++------ hrplib/hrpModel/JointPath.h | 2 -- hrplib/hrpUtil/EigenTypes.h | 22 +-------------- hrplib/hrpUtil/MatrixSolvers.cpp | 47 ++++++++++++++++++++++++++++++++ hrplib/hrpUtil/MatrixSolvers.h | 1 + 5 files changed, 60 insertions(+), 31 deletions(-) diff --git a/hrplib/hrpModel/JointPath.cpp b/hrplib/hrpModel/JointPath.cpp index de6a5b1d8..bfb4a7387 100644 --- a/hrplib/hrpModel/JointPath.cpp +++ b/hrplib/hrpModel/JointPath.cpp @@ -17,11 +17,19 @@ #include "Link.h" #include #include +#include +#include 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; diff --git a/hrplib/hrpModel/JointPath.h b/hrplib/hrpModel/JointPath.h index 11288cd9e..b89ca5231 100644 --- a/hrplib/hrpModel/JointPath.h +++ b/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 joints; std::vector avoid_weight_gain; double sr_gain, manipulability_limit, manipulability_gain; diff --git a/hrplib/hrpUtil/EigenTypes.h b/hrplib/hrpUtil/EigenTypes.h index a01b4e4c5..bd0d42154 100644 --- a/hrplib/hrpUtil/EigenTypes.h +++ b/hrplib/hrpUtil/EigenTypes.h @@ -18,26 +18,6 @@ namespace hrp{ }; #include -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 #endif diff --git a/hrplib/hrpUtil/MatrixSolvers.cpp b/hrplib/hrpUtil/MatrixSolvers.cpp index 25e7f4457..01c0b3674 100644 --- a/hrplib/hrpUtil/MatrixSolvers.cpp +++ b/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) { diff --git a/hrplib/hrpUtil/MatrixSolvers.h b/hrplib/hrpUtil/MatrixSolvers.h index bcd124636..1f55fa12c 100644 --- a/hrplib/hrpUtil/MatrixSolvers.h +++ b/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); /**