Blender V2.61 - r43446

WSDLSSolver.cpp

Go to the documentation of this file.
00001 
00004 /*
00005  * WDLSSolver.hpp.cpp
00006  *
00007  *  Created on: Jan 8, 2009
00008  *      Author: rubensmits
00009  */
00010 
00011 #include "WSDLSSolver.hpp"
00012 #include "kdl/utilities/svd_eigen_HH.hpp"
00013 #include <cstdio>
00014 
00015 namespace iTaSC {
00016 
00017 WSDLSSolver::WSDLSSolver() :
00018     m_ns(0), m_nc(0), m_nq(0)
00019 
00020 {
00021     // default maximum speed: 50 rad/s
00022     m_qmax = 50.0;
00023 }
00024 
00025 WSDLSSolver::~WSDLSSolver() {
00026 }
00027 
00028 bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc)
00029 {
00030     if (_nc == 0 || _nq == 0 || gc.size() != _nc)
00031         return false;
00032     m_nc = _nc;
00033     m_nq = _nq;
00034     m_ns = std::min(m_nc,m_nq);
00035     m_AWq = e_zero_matrix(m_nc,m_nq);
00036     m_WyAWq = e_zero_matrix(m_nc,m_nq);
00037     m_WyAWqt = e_zero_matrix(m_nq,m_nc);
00038     m_S = e_zero_vector(std::max(m_nc,m_nq));
00039     m_Wy_ydot = e_zero_vector(m_nc);
00040     m_ytask = gc;
00041     if (m_nq > m_nc) {
00042         m_transpose = true;
00043         m_temp = e_zero_vector(m_nc);
00044         m_U = e_zero_matrix(m_nc,m_nc);
00045         m_V = e_zero_matrix(m_nq,m_nc);
00046         m_WqV = e_zero_matrix(m_nq,m_nc);
00047     } else {
00048         m_transpose = false;
00049         m_temp = e_zero_vector(m_nq);
00050         m_U = e_zero_matrix(m_nc,m_nq);
00051         m_V = e_zero_matrix(m_nq,m_nq);
00052         m_WqV = e_zero_matrix(m_nq,m_nq);
00053     }
00054     return true;
00055 }
00056 
00057 bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
00058 {
00059     unsigned int i, j, l;
00060     e_scalar N, M;
00061 
00062     // Create the Weighted jacobian
00063     m_AWq.noalias() = A*Wq;
00064     for (i=0; i<m_nc; i++)
00065         m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
00066 
00067     // Compute the SVD of the weighted jacobian
00068     int ret;
00069     if (m_transpose) {
00070         m_WyAWqt = m_WyAWq.transpose();
00071         ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
00072     } else {
00073         ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
00074     }
00075     if(ret<0)
00076         return false;
00077 
00078     m_Wy_ydot = Wy.array() * ydot.array();
00079     m_WqV.noalias() = Wq*m_V;
00080     qdot.setZero();
00081     e_scalar maxDeltaS = e_scalar(0.0);
00082     e_scalar prevS = e_scalar(0.0);
00083     e_scalar maxS = e_scalar(1.0);
00084     for(i=0;i<m_ns;++i) {
00085         e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp;
00086         e_scalar S = m_S(i);
00087         bool prev;
00088         if (S < KDL::epsilon)
00089             break;
00090         Sinv = e_scalar(1.)/S;
00091         if (i > 0) {
00092             if ((prevS-S) > maxDeltaS) {
00093                 maxDeltaS = (prevS-S);
00094                 maxS = prevS;
00095             }
00096         }
00097         N = M = e_scalar(0.);
00098         for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) {
00099             if (prev == m_ytask[l]) {
00100                 norm += m_U(l,i)*m_U(l,i);
00101             } else {
00102                 N += std::sqrt(norm);
00103                 norm = m_U(l,i)*m_U(l,i);
00104             }
00105             prev = m_ytask[l];
00106         }
00107         N += std::sqrt(norm);
00108         for (j=0; j<m_nq; j++) {
00109             for (l=0, prev=m_ytask[0], norm=e_scalar(0.), mag=e_scalar(0.); l<m_nc; l++) {
00110                 if (prev == m_ytask[l]) {
00111                     norm += m_WyAWq(l,j)*m_WyAWq(l,j);
00112                 } else {
00113                     mag += std::sqrt(norm);
00114                     norm = m_WyAWq(l,j)*m_WyAWq(l,j);
00115                 }
00116                 prev = m_ytask[l];
00117             }
00118             mag += std::sqrt(norm);
00119             M += fabs(m_V(j,i))*mag;
00120         }
00121         M *= Sinv;
00122         alpha = m_U.col(i).dot(m_Wy_ydot);
00123         _qmax = (N < M) ? m_qmax*N/M : m_qmax;
00124         vmax = m_WqV.col(i).array().abs().maxCoeff();
00125         norm = fabs(Sinv*alpha*vmax);
00126         if (norm > _qmax) {
00127             damp = Sinv*alpha*_qmax/norm;
00128         } else {
00129             damp = Sinv*alpha;
00130         }
00131         qdot += m_WqV.col(i)*damp;
00132         prevS = S;
00133     }
00134     if (maxDeltaS == e_scalar(0.0))
00135         nlcoef = e_scalar(KDL::epsilon);
00136     else
00137         nlcoef = (maxS-maxDeltaS)/maxS;
00138     return true;
00139 }
00140 
00141 }