29 inline double PYTHAG(
double a,
double b) {
35 return at*
sqrt(1.0+ct*ct);
41 return bt*
sqrt(1.0+ct*ct);
47 inline double SIGN(
double a,
double b) {
48 return ((b) >= 0.0 ? fabs(a) : -fabs(a));
64 const int rows = jac.
rows();
67 int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
69 bool flag,maxarg1,maxarg2;
70 double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
76 for(i=rows;i<cols;i++)
81 for (i=0;i<cols;i++) {
86 for (k=i;k<rows;k++) scale += fabs(U[k](i));
90 for (k=i;k<rows;k++) {
98 for (j=ppi;j<cols;j++) {
100 for (s=0.0,k=i;k<rows;k++) s += U[k](i)*U[k](j);
103 for (k=i;k<rows;k++) U[k](j) += f*U[k](i);
105 for (k=i;k<rows;k++) U[k](i) *= scale;
111 if ((i <rows) && (i+1 != cols)) {
113 for (k=ppi;k<cols;k++) scale += fabs(U[i](k));
115 for (k=ppi;k<cols;k++) {
117 s += U[i](k)*U[i](k);
123 for (k=ppi;k<cols;k++)
tmp(k)=U[i](k)/h;
124 for (j=ppi;j<rows;j++) {
125 for (s=0.0,k=ppi;k<cols;k++) s += U[j](k)*U[i](k);
126 for (k=ppi;k<cols;k++) U[j](k) += s*
tmp(k);
128 for (k=ppi;k<cols;k++) U[i](k) *= scale;
132 maxarg2=(fabs(w(i))+fabs(
tmp(i)));
133 anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
136 for (i=cols-1;i>=0;i--) {
139 for (j=ppi;j<cols;j++) v[j](i)=(U[i](j)/U[i](ppi))/g;
140 for (j=ppi;j<cols;j++) {
141 for (s=0.0,k=ppi;k<cols;k++) s += U[i](k)*v[k](j);
142 for (k=ppi;k<cols;k++) v[k](j) += s*v[k](i);
145 for (j=ppi;j<cols;j++) v[i](j)=v[j](i)=0.0;
152 for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
155 for (j=ppi;j<cols;j++) U[i](j)=0.0;
158 for (j=ppi;j<cols;j++) {
159 for (s=0.0,k=ppi;k<rows;k++) s += U[k](i)*U[k](j);
161 for (k=i;k<rows;k++) U[k](j) += f*U[k](i);
163 for (j=i;j<rows;j++) U[j](i) *= g;
165 for (j=i;j<rows;j++) U[j](i)=0.0;
171 for (k=cols-1;k>=0;k--) {
172 for (its=1;its<=maxiter;its++) {
174 for (ppi=k;ppi>=0;ppi--) {
176 if ((fabs(
tmp(ppi))+anorm) == anorm) {
180 if ((fabs(w(nm)+anorm) == anorm))
break;
185 for (i=ppi;i<=k;i++) {
188 if ((fabs(f)+anorm) == anorm)
break;
195 for (j=0;j<rows;j++) {
208 for (j=0;j<cols;j++) v[j](k)=-v[j](k);
217 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
220 f=((x-z)*(x+z)+h*((y/(f+
SIGN(g,f)))-h))/x;
224 for (j=ppi;j<=nm;j++) {
238 for (jj=0;jj<cols;jj++) {
253 for (jj=0;jj<rows;jj++) {
double SIGN(double a, double b)
This class represents an fixed size array containing joint values of a KDL::Chain.
unsigned int columns() const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
SVD_HH(const Jacobian &jac)
double PYTHAG(double a, double b)
int calculate(const Jacobian &jac, std::vector< JntArray > &U, JntArray &w, std::vector< JntArray > &v, int maxiter)
unsigned int rows() const