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