Speed optimization in itasc when with armature with many bones and few targets. Thank...
authorBenoit Bolsee <benoit.bolsee@online.be>
Sun, 27 Sep 2009 16:20:42 +0000 (16:20 +0000)
committerBenoit Bolsee <benoit.bolsee@online.be>
Sun, 27 Sep 2009 16:20:42 +0000 (16:20 +0000)
intern/itasc/WDLSSolver.cpp
intern/itasc/WDLSSolver.hpp
intern/itasc/WSDLSSolver.cpp
intern/itasc/WSDLSSolver.hpp

index e8bfc95e5dd32b20c67e56fe2c630247ceeb1fb5..1d0efde54c93b5f39205cfcb2adfe4b0481bc3d7 100644 (file)
@@ -24,12 +24,22 @@ bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>&
        m_ns = std::min(nc,nq);
     m_AWq = e_zero_matrix(nc,nq);
     m_WyAWq = e_zero_matrix(nc,nq);
-    m_U = e_zero_matrix(nc,nq);
+    m_WyAWqt = e_zero_matrix(nq,nc);
        m_S = e_zero_vector(std::max(nc,nq));
-    m_temp = e_zero_vector(nq);
-    m_V = e_zero_matrix(nq,nq);
-    m_WqV = e_zero_matrix(nq,nq);
        m_Wy_ydot = e_zero_vector(nc);
+       if (nq > nc) {
+               m_transpose = true;
+           m_temp = e_zero_vector(nc);
+           m_U = e_zero_matrix(nc,nc);
+               m_V = e_zero_matrix(nq,nc);
+           m_WqV = e_zero_matrix(nq,nc);
+       } else {
+               m_transpose = false;
+           m_temp = e_zero_vector(nq);
+           m_U = e_zero_matrix(nc,nq);
+               m_V = e_zero_matrix(nq,nq);
+           m_WqV = e_zero_matrix(nq,nq);
+       }
     return true;
 }
 
@@ -42,7 +52,13 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd
                m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
 
     // Compute the SVD of the weighted jacobian
-       int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+       int ret;
+       if (m_transpose) {
+               m_WyAWqt = m_WyAWq.transpose();
+               ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
+       } else {
+               ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+       }
     if(ret<0)
         return false;
 
index 4418e73675c6f0cfc2bbe991ed15531babbe55d6..b56ad1ab2b8cee8c61db08ad84088493eb791198 100644 (file)
@@ -14,12 +14,13 @@ namespace iTaSC {
 
 class WDLSSolver: public iTaSC::Solver {
 private:
-    e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
+    e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV;
     e_vector m_S,m_temp,m_Wy_ydot;
     double m_lambda;
     double m_epsilon;
        double m_qmax;
        int m_ns;
+       bool m_transpose;
 public:
     WDLSSolver();
     virtual ~WDLSSolver();
index 971fb7f482eddbf3efda058ae6321fbfb50ab4ac..9f7ebed960adc4e76fcfc93bf9752b65954f2fbe 100644 (file)
@@ -31,13 +31,23 @@ bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<boo
        m_ns = std::min(m_nc,m_nq);
     m_AWq = e_zero_matrix(m_nc,m_nq);
     m_WyAWq = e_zero_matrix(m_nc,m_nq);
-    m_U = e_zero_matrix(m_nc,m_nq);
+    m_WyAWqt = e_zero_matrix(m_nq,m_nc);
        m_S = e_zero_vector(std::max(m_nc,m_nq));
-    m_temp = e_zero_vector(m_nq);
-    m_V = e_zero_matrix(m_nq,m_nq);
-    m_WqV = e_zero_matrix(m_nq,m_nq);
        m_Wy_ydot = e_zero_vector(m_nc);
        m_ytask = gc;
+       if (m_nq > m_nc) {
+               m_transpose = true;
+           m_temp = e_zero_vector(m_nc);
+           m_U = e_zero_matrix(m_nc,m_nc);
+               m_V = e_zero_matrix(m_nq,m_nc);
+           m_WqV = e_zero_matrix(m_nq,m_nc);
+       } else {
+               m_transpose = false;
+           m_temp = e_zero_vector(m_nq);
+           m_U = e_zero_matrix(m_nc,m_nq);
+               m_V = e_zero_matrix(m_nq,m_nq);
+           m_WqV = e_zero_matrix(m_nq,m_nq);
+       }
     return true;
 }
 
@@ -52,7 +62,13 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
                m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
 
     // Compute the SVD of the weighted jacobian
-       int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+       int ret;
+       if (m_transpose) {
+               m_WyAWqt = m_WyAWq.transpose();
+               ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
+       } else {
+               ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+       }
     if(ret<0)
         return false;
 
index 1341cf2af667bb64a67520d1e0b289e5ba4a5703..0b17f26ef47b10b4912aa989ef9a99afd4c5a7da 100644 (file)
@@ -14,11 +14,12 @@ namespace iTaSC {
 
 class WSDLSSolver: public iTaSC::Solver {
 private:
-    e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
+    e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV;
     e_vector m_S,m_temp,m_Wy_ydot;
        std::vector<bool> m_ytask;
        e_scalar m_qmax;
        unsigned int m_ns, m_nc, m_nq;
+       bool m_transpose;
 public:
     WSDLSSolver();
     virtual ~WSDLSSolver();