Merge branch 'blender2.7'
[blender.git] / intern / itasc / WSDLSSolver.cpp
1 /** \file itasc/WSDLSSolver.cpp
2  *  \ingroup itasc
3  */
4 /*
5  * WDLSSolver.hpp.cpp
6  *
7  *  Created on: Jan 8, 2009
8  *      Author: rubensmits
9  */
10
11 #include "WSDLSSolver.hpp"
12 #include "kdl/utilities/svd_eigen_HH.hpp"
13 #include <cstdio>
14
15 namespace iTaSC {
16
17 WSDLSSolver::WSDLSSolver() :
18         m_ns(0), m_nc(0), m_nq(0)
19
20 {
21         // default maximum speed: 50 rad/s
22         m_qmax = 50.0;
23 }
24
25 WSDLSSolver::~WSDLSSolver() {
26 }
27
28 bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc)
29 {
30         if (_nc == 0 || _nq == 0 || gc.size() != _nc)
31                 return false;
32         m_nc = _nc;
33         m_nq = _nq;
34         m_ns = std::min(m_nc,m_nq);
35     m_AWq = e_zero_matrix(m_nc,m_nq);
36     m_WyAWq = e_zero_matrix(m_nc,m_nq);
37     m_WyAWqt = e_zero_matrix(m_nq,m_nc);
38         m_S = e_zero_vector(std::max(m_nc,m_nq));
39         m_Wy_ydot = e_zero_vector(m_nc);
40         m_ytask = gc;
41         if (m_nq > m_nc) {
42                 m_transpose = true;
43             m_temp = e_zero_vector(m_nc);
44             m_U = e_zero_matrix(m_nc,m_nc);
45                 m_V = e_zero_matrix(m_nq,m_nc);
46             m_WqV = e_zero_matrix(m_nq,m_nc);
47         } else {
48                 m_transpose = false;
49             m_temp = e_zero_vector(m_nq);
50             m_U = e_zero_matrix(m_nc,m_nq);
51                 m_V = e_zero_matrix(m_nq,m_nq);
52             m_WqV = e_zero_matrix(m_nq,m_nq);
53         }
54     return true;
55 }
56
57 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)
58 {
59         unsigned int i, j, l;
60         e_scalar N, M;
61
62         // Create the Weighted jacobian
63         m_AWq.noalias() = A*Wq;
64         for (i=0; i<m_nc; i++)
65                 m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
66
67     // Compute the SVD of the weighted jacobian
68         int ret;
69         if (m_transpose) {
70                 m_WyAWqt = m_WyAWq.transpose();
71                 ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
72         } else {
73                 ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
74         }
75         if(ret<0)
76                 return false;
77
78         m_Wy_ydot = Wy.array() * ydot.array();
79         m_WqV.noalias() = Wq*m_V;
80         qdot.setZero();
81         e_scalar maxDeltaS = e_scalar(0.0);
82         e_scalar prevS = e_scalar(0.0);
83         e_scalar maxS = e_scalar(1.0);
84         for(i=0;i<m_ns;++i) {
85                 e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp;
86                 e_scalar S = m_S(i);
87                 bool prev;
88                 if (S < KDL::epsilon)
89                         break;
90                 Sinv = e_scalar(1.)/S;
91                 if (i > 0) {
92                         if ((prevS-S) > maxDeltaS) {
93                                 maxDeltaS = (prevS-S);
94                                 maxS = prevS;
95                         }
96                 }
97                 N = M = e_scalar(0.);
98                 for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) {
99                         if (prev == m_ytask[l]) {
100                                 norm += m_U(l,i)*m_U(l,i);
101                         } else {
102                                 N += std::sqrt(norm);
103                                 norm = m_U(l,i)*m_U(l,i);
104                         }
105                         prev = m_ytask[l];
106                 }
107                 N += std::sqrt(norm);
108                 for (j=0; j<m_nq; j++) {
109                         for (l=0, prev=m_ytask[0], norm=e_scalar(0.), mag=e_scalar(0.); l<m_nc; l++) {
110                                 if (prev == m_ytask[l]) {
111                                         norm += m_WyAWq(l,j)*m_WyAWq(l,j);
112                                 } else {
113                                         mag += std::sqrt(norm);
114                                         norm = m_WyAWq(l,j)*m_WyAWq(l,j);
115                                 }
116                                 prev = m_ytask[l];
117                         }
118                         mag += std::sqrt(norm);
119                         M += fabs(m_V(j,i))*mag;
120                 }
121                 M *= Sinv;
122                 alpha = m_U.col(i).dot(m_Wy_ydot);
123                 _qmax = (N < M) ? m_qmax*N/M : m_qmax;
124                 vmax = m_WqV.col(i).array().abs().maxCoeff();
125                 norm = fabs(Sinv*alpha*vmax);
126                 if (norm > _qmax) {
127                         damp = Sinv*alpha*_qmax/norm;
128                 } else {
129                         damp = Sinv*alpha;
130                 }
131                 qdot += m_WqV.col(i)*damp;
132                 prevS = S;
133         }
134         if (maxDeltaS == e_scalar(0.0))
135                 nlcoef = e_scalar(KDL::epsilon);
136         else
137                 nlcoef = (maxS-maxDeltaS)/maxS;
138     return true;
139 }
140
141 }