Fix too much memory usage for Cycles attribute map.
[blender.git] / intern / iksolver / intern / IK_QJacobian.cpp
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Original Author: Laurence
24  * Contributor(s): Brecht
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 /** \file iksolver/intern/IK_QJacobian.cpp
30  *  \ingroup iksolver
31  */
32
33
34 #include "IK_QJacobian.h"
35
36 IK_QJacobian::IK_QJacobian()
37         : m_sdls(true), m_min_damp(1.0)
38 {
39 }
40
41 IK_QJacobian::~IK_QJacobian()
42 {
43 }
44
45 void IK_QJacobian::ArmMatrices(int dof, int task_size)
46 {
47         m_dof = dof;
48         m_task_size = task_size;
49
50         m_jacobian.resize(task_size, dof);
51         m_jacobian.setZero();
52
53         m_alpha.resize(dof);
54         m_alpha.setZero();
55
56         m_nullspace.resize(dof, dof);
57
58         m_d_theta.resize(dof);
59         m_d_theta_tmp.resize(dof);
60         m_d_norm_weight.resize(dof);
61
62         m_norm.resize(dof);
63         m_norm.setZero();
64
65         m_beta.resize(task_size);
66
67         m_weight.resize(dof);
68         m_weight_sqrt.resize(dof);
69         m_weight.setOnes();
70         m_weight_sqrt.setOnes();
71
72         if (task_size >= dof) {
73                 m_transpose = false;
74
75                 m_jacobian_tmp.resize(task_size, dof);
76
77                 m_svd_u.resize(task_size, dof);
78                 m_svd_v.resize(dof, dof);
79                 m_svd_w.resize(dof);
80
81                 m_svd_u_beta.resize(dof);
82         }
83         else {
84                 // use the SVD of the transpose jacobian, it works just as well
85                 // as the original, and often allows using smaller matrices.
86                 m_transpose = true;
87
88                 m_jacobian_tmp.resize(dof, task_size);
89
90                 m_svd_u.resize(task_size, task_size);
91                 m_svd_v.resize(dof, task_size);
92                 m_svd_w.resize(task_size);
93
94                 m_svd_u_beta.resize(task_size);
95         }
96 }
97
98 void IK_QJacobian::SetBetas(int id, int, const Vector3d& v)
99 {
100         m_beta[id + 0] = v.x();
101         m_beta[id + 1] = v.y();
102         m_beta[id + 2] = v.z();
103 }
104
105 void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight)
106 {
107         m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id];
108         m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id];
109         m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id];
110
111         m_d_norm_weight[dof_id] = norm_weight;
112 }
113
114 void IK_QJacobian::Invert()
115 {
116         if (m_transpose) {
117                 // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
118                 // so J = U*W*Vt and Jinv = V*Winv*Ut
119                 Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
120                 m_svd_u = svd.matrixV();
121                 m_svd_w = svd.singularValues();
122                 m_svd_v = svd.matrixU();
123         }
124         else {
125                 // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
126                 // so Jinv = V*Winv*Ut
127                 Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
128                 m_svd_u = svd.matrixU();
129                 m_svd_w = svd.singularValues();
130                 m_svd_v = svd.matrixV();
131         }
132
133         if (m_sdls)
134                 InvertSDLS();
135         else
136                 InvertDLS();
137 }
138
139 bool IK_QJacobian::ComputeNullProjection()
140 {
141         double epsilon = 1e-10;
142         
143         // compute null space projection based on V
144         int i, j, rank = 0;
145         for (i = 0; i < m_svd_w.size(); i++)
146                 if (m_svd_w[i] > epsilon)
147                         rank++;
148
149         if (rank < m_task_size)
150                 return false;
151
152         MatrixXd basis(m_svd_v.rows(), rank);
153         int b = 0;
154
155         for (i = 0; i < m_svd_w.size(); i++)
156                 if (m_svd_w[i] > epsilon) {
157                         for (j = 0; j < m_svd_v.rows(); j++)
158                                 basis(j, b) = m_svd_v(j, i);
159                         b++;
160                 }
161         
162         m_nullspace = basis * basis.transpose();
163
164         for (i = 0; i < m_nullspace.rows(); i++)
165                 for (j = 0; j < m_nullspace.cols(); j++)
166                         if (i == j)
167                                 m_nullspace(i, j) = 1.0 - m_nullspace(i, j);
168                         else
169                                 m_nullspace(i, j) = -m_nullspace(i, j);
170         
171         return true;
172 }
173
174 void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
175 {
176         if (!ComputeNullProjection())
177                 return;
178
179         // restrict lower priority jacobian
180         jacobian.Restrict(m_d_theta, m_nullspace);
181
182         // add angle update from lower priority
183         jacobian.Invert();
184
185         // note: now damps secondary angles with minimum damping value from
186         // SDLS, to avoid shaking when the primary task is near singularities,
187         // doesn't work well at all
188         int i;
189         for (i = 0; i < m_d_theta.size(); i++)
190                 m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i);
191 }
192
193 void IK_QJacobian::Restrict(VectorXd& d_theta, MatrixXd& nullspace)
194 {
195         // subtract part already moved by higher task from beta
196         m_beta = m_beta - m_jacobian * d_theta;
197
198         // note: should we be using the norm of the unrestricted jacobian for SDLS?
199         
200         // project jacobian on to null space of higher priority task
201         m_jacobian = m_jacobian * nullspace;
202 }
203
204 void IK_QJacobian::InvertSDLS()
205 {
206         // Compute the dampeds least squeares pseudo inverse of J.
207         //
208         // Since J is usually not invertible (most of the times it's not even
209         // square), the psuedo inverse is used. This gives us a least squares
210         // solution.
211         //
212         // This is fine when the J*Jt is of full rank. When J*Jt is near to
213         // singular the least squares inverse tries to minimize |J(dtheta) - dX)|
214         // and doesn't try to minimize  dTheta. This results in eratic changes in
215         // angle. The damped least squares minimizes |dtheta| to try and reduce this
216         // erratic behaviour.
217         //
218         // The selectively damped least squares (SDLS) is used here instead of the
219         // DLS. The SDLS damps individual singular values, instead of using a single
220         // damping term.
221
222         double max_angle_change = M_PI / 4.0;
223         double epsilon = 1e-10;
224         int i, j;
225
226         m_d_theta.setZero();
227         m_min_damp = 1.0;
228
229         for (i = 0; i < m_dof; i++) {
230                 m_norm[i] = 0.0;
231                 for (j = 0; j < m_task_size; j += 3) {
232                         double n = 0.0;
233                         n += m_jacobian(j, i) * m_jacobian(j, i);
234                         n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i);
235                         n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i);
236                         m_norm[i] += sqrt(n);
237                 }
238         }
239
240         for (i = 0; i < m_svd_w.size(); i++) {
241                 if (m_svd_w[i] <= epsilon)
242                         continue;
243
244                 double wInv = 1.0 / m_svd_w[i];
245                 double alpha = 0.0;
246                 double N = 0.0;
247
248                 // compute alpha and N
249                 for (j = 0; j < m_svd_u.rows(); j += 3) {
250                         alpha += m_svd_u(j, i) * m_beta[j];
251                         alpha += m_svd_u(j + 1, i) * m_beta[j + 1];
252                         alpha += m_svd_u(j + 2, i) * m_beta[j + 2];
253
254                         // note: for 1 end effector, N will always be 1, since U is
255                         // orthogonal, .. so could be optimized
256                         double tmp;
257                         tmp = m_svd_u(j, i) * m_svd_u(j, i);
258                         tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i);
259                         tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i);
260                         N += sqrt(tmp);
261                 }
262                 alpha *= wInv;
263
264                 // compute M, dTheta and max_dtheta
265                 double M = 0.0;
266                 double max_dtheta = 0.0, abs_dtheta;
267
268                 for (j = 0; j < m_d_theta.size(); j++) {
269                         double v = m_svd_v(j, i);
270                         M += fabs(v) * m_norm[j];
271
272                         // compute tmporary dTheta's
273                         m_d_theta_tmp[j] = v * alpha;
274
275                         // find largest absolute dTheta
276                         // multiply with weight to prevent unnecessary damping
277                         abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
278                         if (abs_dtheta > max_dtheta)
279                                 max_dtheta = abs_dtheta;
280                 }
281
282                 M *= wInv;
283
284                 // compute damping term and damp the dTheta's
285                 double gamma = max_angle_change;
286                 if (N < M)
287                         gamma *= N / M;
288
289                 double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
290
291                 for (j = 0; j < m_d_theta.size(); j++) {
292                         // slight hack: we do 0.80*, so that if there is some oscillation,
293                         // the system can still converge (for joint limits). also, it's
294                         // better to go a little to slow than to far
295                         
296                         double dofdamp = damp / m_weight[j];
297                         if (dofdamp > 1.0) dofdamp = 1.0;
298                         
299                         m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
300                 }
301
302                 if (damp < m_min_damp)
303                         m_min_damp = damp;
304         }
305
306         // weight + prevent from doing angle updates with angles > max_angle_change
307         double max_angle = 0.0, abs_angle;
308
309         for (j = 0; j < m_dof; j++) {
310                 m_d_theta[j] *= m_weight[j];
311
312                 abs_angle = fabs(m_d_theta[j]);
313
314                 if (abs_angle > max_angle)
315                         max_angle = abs_angle;
316         }
317         
318         if (max_angle > max_angle_change) {
319                 double damp = (max_angle_change) / (max_angle_change + max_angle);
320
321                 for (j = 0; j < m_dof; j++)
322                         m_d_theta[j] *= damp;
323         }
324 }
325
326 void IK_QJacobian::InvertDLS()
327 {
328         // Compute damped least squares inverse of pseudo inverse
329         // Compute damping term lambda
330
331         // Note when lambda is zero this is equivalent to the
332         // least squares solution. This is fine when the m_jjt is
333         // of full rank. When m_jjt is near to singular the least squares
334         // inverse tries to minimize |J(dtheta) - dX)| and doesn't 
335         // try to minimize  dTheta. This results in eratic changes in angle.
336         // Damped least squares minimizes |dtheta| to try and reduce this
337         // erratic behaviour.
338
339         // We don't want to use the damped solution everywhere so we
340         // only increase lamda from zero as we approach a singularity.
341
342         // find the smallest non-zero W value, anything below epsilon is
343         // treated as zero
344
345         double epsilon = 1e-10;
346         double max_angle_change = 0.1;
347         double x_length = sqrt(m_beta.dot(m_beta));
348
349         int i, j;
350         double w_min = std::numeric_limits<double>::max();
351
352         for (i = 0; i < m_svd_w.size(); i++) {
353                 if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
354                         w_min = m_svd_w[i];
355         }
356         
357         // compute lambda damping term
358
359         double d = x_length / max_angle_change;
360         double lambda;
361
362         if (w_min <= d / 2)
363                 lambda = d / 2;
364         else if (w_min < d)
365                 lambda = sqrt(w_min * (d - w_min));
366         else
367                 lambda = 0.0;
368
369         lambda *= lambda;
370
371         if (lambda > 10)
372                 lambda = 10;
373
374         // immediately multiply with Beta, so we can do matrix*vector products
375         // rather than matrix*matrix products
376
377         // compute Ut*Beta
378         m_svd_u_beta = m_svd_u.transpose() * m_beta;
379
380         m_d_theta.setZero();
381
382         for (i = 0; i < m_svd_w.size(); i++) {
383                 if (m_svd_w[i] > epsilon) {
384                         double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
385
386                         // compute V*Winv*Ut*Beta
387                         m_svd_u_beta[i] *= wInv;
388
389                         for (j = 0; j < m_d_theta.size(); j++)
390                                 m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i];
391                 }
392         }
393
394         for (j = 0; j < m_d_theta.size(); j++)
395                 m_d_theta[j] *= m_weight[j];
396 }
397
398 void IK_QJacobian::Lock(int dof_id, double delta)
399 {
400         int i;
401
402         for (i = 0; i < m_task_size; i++) {
403                 m_beta[i] -= m_jacobian(i, dof_id) * delta;
404                 m_jacobian(i, dof_id) = 0.0;
405         }
406
407         m_norm[dof_id] = 0.0; // unneeded
408         m_d_theta[dof_id] = 0.0;
409 }
410
411 double IK_QJacobian::AngleUpdate(int dof_id) const
412 {
413         return m_d_theta[dof_id];
414 }
415
416 double IK_QJacobian::AngleUpdateNorm() const
417 {
418         int i;
419         double mx = 0.0, dtheta_abs;
420
421         for (i = 0; i < m_d_theta.size(); i++) {
422                 dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
423                 if (dtheta_abs > mx)
424                         mx = dtheta_abs;
425         }
426         
427         return mx;
428 }
429
430 void IK_QJacobian::SetDoFWeight(int dof, double weight)
431 {
432         m_weight[dof] = weight;
433         m_weight_sqrt[dof] = sqrt(weight);
434 }
435