int hrp::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; } }
bool JointPathEx::calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull) { const int n = numJoints(); hrp::dmatrix w = hrp::dmatrix::Identity(n,n); // // wmat/weight: weighting joint angle weight // // w_i = 1 + | dH/dt | if d|dH/dt| >= 0 // = 1 if d|dH/dt| < 0 // dH/dt = (t_max - t_min)^2 (2t - t_max - t_min) // / 4 (t_max - t)^2 (t - t_min)^2 // // T. F. Chang and R.-V. Dubey: "A weighted least-norm solution based // scheme for avoiding joint limits for redundant manipulators", in IEEE // Trans. On Robotics and Automation, 11((2):286-292, April 1995. // for ( int j = 0; j < n ; j++ ) { double jang = joints[j]->q; double jmax = joints[j]->ulimit; double jmin = joints[j]->llimit; double e = deg2rad(1); if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) { } else if ( eps_eq(jang, jmax,e) ) { jang = jmax - e; } else if ( eps_eq(jang, jmin,e) ) { jang = jmin + e; } double r; if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) { r = DBL_MAX; } else { r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) ); if (std::isnan(r)) r = 0; } // If use_inside_joint_weight_retrieval = true (true by default), use T. F. Chang and R.-V. Dubeby weight retrieval inward. // Otherwise, joint weight is always calculated from limit value to resolve https://github.com/fkanehiro/hrpsys-base/issues/516. if (( r - avoid_weight_gain[j] ) >= 0 ) { w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) ); } else { if (use_inside_joint_weight_retrieval) w(j, j) = optional_weight_vector[j] * 1.0; else w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) ); } avoid_weight_gain[j] = r; } if ( DEBUG ) { std::cerr << " cost :"; for(int j = 0; j < n; j++ ) { std::cerr << std::setw(8) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << avoid_weight_gain[j]; } std::cerr << std::endl; std::cerr << " optw :"; for(int j = 0; j < n; j++ ) { std::cerr << std::setw(8) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << optional_weight_vector[j]; } std::cerr << std::endl; std::cerr << " w :"; for(int j = 0; j < n; j++ ) { std::cerr << std::setw(8) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << w(j, j); } std::cerr << std::endl; } double manipulability = sqrt((J*J.transpose()).determinant()); double k = 0; if ( manipulability < manipulability_limit ) { k = manipulability_gain * pow((1 - ( manipulability / manipulability_limit )), 2); } if ( DEBUG ) { std::cerr << " manipulability = " << manipulability << " < " << manipulability_limit << ", k = " << k << " -> " << sr_gain * k << std::endl; } calcSRInverse(J, Jinv, sr_gain * k, w); Jnull = ( hrp::dmatrix::Identity(n, n) - Jinv * J); return true; }