/*!
@fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
const Real eps_, const Real lambda_max_, const Real dt);
@brief Constructor.
*/
Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
const Real eps_, const Real lambda_max_, const Real dt_):
dt(dt_),
eps(eps_),
lambda_max(lambda_max_),
mrobot(mrobot_)
{
robot_type = CLICK_mDH;
// Initialize with same joint position (and rates) has the robot.
q = mrobot.get_q();
qp = mrobot.get_qp();
qp_prev = qp;
Kpep = ColumnVector(3); Kpep = 0;
Koe0Quat = ColumnVector(3); Koe0Quat = 0;
v = ColumnVector(6); v = 0;
if(Kp_.Nrows()==3)
Kp = Kp_;
else
{
Kp = DiagonalMatrix(3); Kp = 0.0;
cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
}
if(Ko_.Nrows()==3)
Ko = Ko_;
else
{
Ko = DiagonalMatrix(3); Ko = 0.0;
cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
}
}
void Print(const DiagonalMatrix& X)
{
++PCN;
cout << "\nMatrix type: " << X.Type().Value() << " (";
cout << X.Nrows() << ", ";
cout << X.Ncols() << ")\n\n";
if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
int nr=X.Nrows(); int nc=X.Ncols();
for (int i=1; i<=nr; i++)
{
for (int j=1; j<i; j++) cout << "\t";
if (i<=nc) cout << X(i,i) << "\t";
cout << "\n";
}
cout << flush; ++PCZ;
}
static void tql1(DiagonalMatrix& D, DiagonalMatrix& E)
{
Tracer et("Evalue(tql1)");
Real eps = FloatingPointPrecision::Epsilon();
int n = D.Nrows(); int l;
for (l=1; l<n; l++) E.element(l-1) = E.element(l);
Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
for (l=0; l<n; l++)
{
int i,j;
Real& dl = D.element(l); Real& el = E.element(l);
Real h = eps * ( fabs(dl) + fabs(el) );
if (b < h) b = h;
int m;
for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
bool test = false;
for (j=0; j<30; j++)
{
if (m==l) { test = true; break; }
Real& dl1 = D.element(l+1);
Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
p = D.element(m); Real c = 1.0; Real s = 0.0;
for (i=m-1; i>=l; i--)
{
Real ei = E.element(i); Real di = D.element(i);
Real& ei1 = E.element(i+1);
g = c * ei; h = c * p;
if ( fabs(p) >= fabs(ei))
{
c = ei / p; r = sqrt(c*c + 1.0);
ei1 = s*p*r; s = c/r; c = 1.0/r;
}
else
{
c = p / ei; r = sqrt(c*c + 1.0);
ei1 = s * ei * r; s = 1.0/r; c /= r;
}
p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
}
el = s*p; dl = c*p;
if (fabs(el) <= b) { test = true; break; }
}
if (!test) Throw ( ConvergenceException(D) );
Real p = dl + f;
test = false;
for (i=l; i>0; i--)
{
if (p < D.element(i-1)) D.element(i) = D.element(i-1);
else { test = true; break; }
}
if (!test) i=0;
D.element(i) = p;
}
}
void getGeneralizedInverse(Matrix& G, Matrix& Gi) {
#ifdef DEBUG
cout << "\n\ngetGeneralizedInverse - Singular Value\n";
#endif
// Singular value decomposition method
// do SVD
Matrix U, V;
DiagonalMatrix D;
SVD(G,D,U,V); // X = U * D * V.t()
#ifdef DEBUG
cout << "D:\n";
cout << setw(9) << setprecision(6) << (D);
cout << "\n\n";
#endif
DiagonalMatrix Di;
Di << D.i();
#ifdef DEBUG
cout << "Di:\n";
cout << setw(9) << setprecision(6) << (Di);
cout << "\n\n";
#endif
int i=Di.Nrows();
for (; i>=1; i--) {
if (Di(i) > 1000.0) {
Di(i) = 0.0;
}
}
#ifdef DEBUG
cout << "Di with biggies zeroed out:\n";
cout << setw(9) << setprecision(6) << (Di);
cout << "\n\n";
#endif
//Matrix Gi;
Gi << (U * (Di * V.t()));
return;
}
void Clean(DiagonalMatrix& A, Real c)
{
int nr = A.Nrows();
for (int i=1; i<=nr; i++)
{ Real a = A(i,i); if ((a < c) && (a > -c)) A(i,i) = 0.0; }
}
static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z)
{
Tracer et("Evalue(tql2)");
Real eps = FloatingPointPrecision::Epsilon();
int n = D.Nrows(); Real* z = Z.Store(); int l;
for (l=1; l<n; l++) E.element(l-1) = E.element(l);
Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
for (l=0; l<n; l++)
{
int i,j;
Real& dl = D.element(l); Real& el = E.element(l);
Real h = eps * ( fabs(dl) + fabs(el) );
if (b < h) b = h;
int m;
for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
bool test = false;
for (j=0; j<30; j++)
{
if (m==l) { test = true; break; }
Real& dl1 = D.element(l+1);
Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
p = D.element(m); Real c = 1.0; Real s = 0.0;
for (i=m-1; i>=l; i--)
{
Real ei = E.element(i); Real di = D.element(i);
Real& ei1 = E.element(i+1);
g = c * ei; h = c * p;
if ( fabs(p) >= fabs(ei))
{
c = ei / p; r = sqrt(c*c + 1.0);
ei1 = s*p*r; s = c/r; c = 1.0/r;
}
else
{
c = p / ei; r = sqrt(c*c + 1.0);
ei1 = s * ei * r; s = 1.0/r; c /= r;
}
p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
Real* zki = z + i; Real* zki1 = zki + 1; int k = n;
while (k--)
{
h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h;
zki += n; zki1 += n;
}
}
el = s*p; dl = c*p;
if (fabs(el) <= b) { test = true; break; }
}
if (!test) Throw ( ConvergenceException(D) );
dl += f;
}
for (int i=0; i<n; i++)
{
int k = i; Real p = D.element(i);
for (int j=i+1; j<n; j++)
{ if (D.element(j) < p) { k = j; p = D.element(j); } }
if (k != i)
{
D.element(k) = D.element(i); D.element(i) = p; int j = n;
Real* zji = z + i; Real* zjk = z + k;
while (j--) { p = *zji; *zji = *zjk; *zjk = p; zji += n; zjk += n; }
}
}
}
