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