Merging r49287 through r49299 from trunk into soc-2011-tomato
[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 #include "TNT/svd.h"
36
37 IK_QJacobian::IK_QJacobian()
38 : m_sdls(true), m_min_damp(1.0)
39 {
40 }
41
42 IK_QJacobian::~IK_QJacobian()
43 {
44 }
45
46 void IK_QJacobian::ArmMatrices(int dof, int task_size)
47 {
48         m_dof = dof;
49         m_task_size = task_size;
50
51         m_jacobian.newsize(task_size, dof);
52         m_jacobian = 0;
53
54         m_alpha.newsize(dof);
55         m_alpha = 0;
56
57         m_null.newsize(dof, dof);
58
59         m_d_theta.newsize(dof);
60         m_d_theta_tmp.newsize(dof);
61         m_d_norm_weight.newsize(dof);
62
63         m_norm.newsize(dof);
64         m_norm = 0.0;
65
66         m_beta.newsize(task_size);
67
68         m_weight.newsize(dof);
69         m_weight_sqrt.newsize(dof);
70         m_weight = 1.0;
71         m_weight_sqrt = 1.0;
72
73         if (task_size >= dof) {
74                 m_transpose = false;
75
76                 m_jacobian_tmp.newsize(task_size, dof);
77
78                 m_svd_u.newsize(task_size, dof);
79                 m_svd_v.newsize(dof, dof);
80                 m_svd_w.newsize(dof);
81
82                 m_work1.newsize(task_size);
83                 m_work2.newsize(dof);
84
85                 m_svd_u_t.newsize(dof, task_size);
86                 m_svd_u_beta.newsize(dof);
87         }
88         else {
89                 // use the SVD of the transpose jacobian, it works just as well
90                 // as the original, and often allows using smaller matrices.
91                 m_transpose = true;
92
93                 m_jacobian_tmp.newsize(dof, task_size);
94
95                 m_svd_u.newsize(task_size, task_size);
96                 m_svd_v.newsize(dof, task_size);
97                 m_svd_w.newsize(task_size);
98
99                 m_work1.newsize(dof);
100                 m_work2.newsize(task_size);
101
102                 m_svd_u_t.newsize(task_size, task_size);
103                 m_svd_u_beta.newsize(task_size);
104         }
105 }
106
107 void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v)
108 {
109         m_beta[id] = v.x();
110         m_beta[id+1] = v.y();
111         m_beta[id+2] = v.z();
112 }
113
114 void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight)
115 {
116         m_jacobian[id][dof_id] = v.x()*m_weight_sqrt[dof_id];
117         m_jacobian[id+1][dof_id] = v.y()*m_weight_sqrt[dof_id];
118         m_jacobian[id+2][dof_id] = v.z()*m_weight_sqrt[dof_id];
119
120         m_d_norm_weight[dof_id] = norm_weight;
121 }
122
123 void IK_QJacobian::Invert()
124 {
125         if (m_transpose) {
126                 // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
127                 // so J = U*W*Vt and Jinv = V*Winv*Ut
128                 TNT::transpose(m_jacobian, m_jacobian_tmp);
129                 TNT::SVD(m_jacobian_tmp, m_svd_v, m_svd_w, m_svd_u, m_work1, m_work2);
130         }
131         else {
132                 // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
133                 // so Jinv = V*Winv*Ut
134                 m_jacobian_tmp = m_jacobian;
135                 TNT::SVD(m_jacobian_tmp, m_svd_u, m_svd_w, m_svd_v, m_work1, m_work2);
136         }
137
138         if (m_sdls)
139                 InvertSDLS();
140         else
141                 InvertDLS();
142 }
143
144 bool IK_QJacobian::ComputeNullProjection()
145 {
146         MT_Scalar epsilon = 1e-10;
147         
148         // compute null space projection based on V
149         int i, j, rank = 0;
150         for (i = 0; i < m_svd_w.size(); i++)
151                 if (m_svd_w[i] > epsilon)
152                         rank++;
153
154         if (rank < m_task_size)
155                 return false;
156
157         TMatrix basis(m_svd_v.num_rows(), rank);
158         TMatrix basis_t(rank, m_svd_v.num_rows());
159         int b = 0;
160
161         for (i = 0; i < m_svd_w.size(); i++)
162                 if (m_svd_w[i] > epsilon) {
163                         for (j = 0; j < m_svd_v.num_rows(); j++)
164                                 basis[j][b] = m_svd_v[j][i];
165                         b++;
166                 }
167         
168         TNT::transpose(basis, basis_t);
169         TNT::matmult(m_null, basis, basis_t);
170
171         for (i = 0; i < m_null.num_rows(); i++)
172                 for (j = 0; j < m_null.num_cols(); j++)
173                         if (i == j)
174                                 m_null[i][j] = 1.0 - m_null[i][j];
175                         else
176                                 m_null[i][j] = -m_null[i][j];
177         
178         return true;
179 }
180
181 void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
182 {
183         if (!ComputeNullProjection())
184                 return;
185
186         // restrict lower priority jacobian
187         jacobian.Restrict(m_d_theta, m_null);
188
189         // add angle update from lower priority
190         jacobian.Invert();
191
192         // note: now damps secondary angles with minimum damping value from
193         // SDLS, to avoid shaking when the primary task is near singularities,
194         // doesn't work well at all
195         int i;
196         for (i = 0; i < m_d_theta.size(); i++)
197                 m_d_theta[i] = m_d_theta[i] + /*m_min_damp**/jacobian.AngleUpdate(i);
198 }
199
200 void IK_QJacobian::Restrict(TVector& d_theta, TMatrix& null)
201 {
202         // subtract part already moved by higher task from beta
203         TVector beta_sub(m_beta.size());
204
205         TNT::matmult(beta_sub, m_jacobian, d_theta);
206         m_beta = m_beta - beta_sub;
207
208         // note: should we be using the norm of the unrestricted jacobian for SDLS?
209         
210         // project jacobian on to null space of higher priority task
211         TMatrix jacobian_copy(m_jacobian);
212         TNT::matmult(m_jacobian, jacobian_copy, null);
213 }
214
215 void IK_QJacobian::InvertSDLS()
216 {
217         // Compute the dampeds least squeares pseudo inverse of J.
218         //
219         // Since J is usually not invertible (most of the times it's not even
220         // square), the psuedo inverse is used. This gives us a least squares
221         // solution.
222         //
223         // This is fine when the J*Jt is of full rank. When J*Jt is near to
224         // singular the least squares inverse tries to minimize |J(dtheta) - dX)|
225         // and doesn't try to minimize  dTheta. This results in eratic changes in
226         // angle. The damped least squares minimizes |dtheta| to try and reduce this
227         // erratic behaviour.
228         //
229         // The selectively damped least squares (SDLS) is used here instead of the
230         // DLS. The SDLS damps individual singular values, instead of using a single
231         // damping term.
232
233         MT_Scalar max_angle_change = MT_PI/4.0;
234         MT_Scalar epsilon = 1e-10;
235         int i, j;
236
237         m_d_theta = 0;
238         m_min_damp = 1.0;
239
240         for (i = 0; i < m_dof; i++) {
241                 m_norm[i] = 0.0;
242                 for (j = 0; j < m_task_size; j+=3) {
243                         MT_Scalar n = 0.0;
244                         n += m_jacobian[j][i]*m_jacobian[j][i];
245                         n += m_jacobian[j+1][i]*m_jacobian[j+1][i];
246                         n += m_jacobian[j+2][i]*m_jacobian[j+2][i];
247                         m_norm[i] += sqrt(n);
248                 }
249         }
250
251         for (i = 0; i<m_svd_w.size(); i++) {
252                 if (m_svd_w[i] <= epsilon)
253                         continue;
254
255                 MT_Scalar wInv = 1.0/m_svd_w[i];
256                 MT_Scalar alpha = 0.0;
257                 MT_Scalar N = 0.0;
258
259                 // compute alpha and N
260                 for (j=0; j<m_svd_u.num_rows(); j+=3) {
261                         alpha += m_svd_u[j][i]*m_beta[j];
262                         alpha += m_svd_u[j+1][i]*m_beta[j+1];
263                         alpha += m_svd_u[j+2][i]*m_beta[j+2];
264
265                         // note: for 1 end effector, N will always be 1, since U is
266                         // orthogonal, .. so could be optimized
267                         MT_Scalar tmp;
268                         tmp = m_svd_u[j][i]*m_svd_u[j][i];
269                         tmp += m_svd_u[j+1][i]*m_svd_u[j+1][i];
270                         tmp += m_svd_u[j+2][i]*m_svd_u[j+2][i];
271                         N += sqrt(tmp);
272                 }
273                 alpha *= wInv;
274
275                 // compute M, dTheta and max_dtheta
276                 MT_Scalar M = 0.0;
277                 MT_Scalar max_dtheta = 0.0, abs_dtheta;
278
279                 for (j = 0; j < m_d_theta.size(); j++) {
280                         MT_Scalar v = m_svd_v[j][i];
281                         M += MT_abs(v)*m_norm[j];
282
283                         // compute tmporary dTheta's
284                         m_d_theta_tmp[j] = v*alpha;
285
286                         // find largest absolute dTheta
287                         // multiply with weight to prevent unnecessary damping
288                         abs_dtheta = MT_abs(m_d_theta_tmp[j])*m_weight_sqrt[j];
289                         if (abs_dtheta > max_dtheta)
290                                 max_dtheta = abs_dtheta;
291                 }
292
293                 M *= wInv;
294
295                 // compute damping term and damp the dTheta's
296                 MT_Scalar gamma = max_angle_change;
297                 if (N < M)
298                         gamma *= N/M;
299
300                 MT_Scalar damp = (gamma < max_dtheta)? gamma/max_dtheta: 1.0;
301
302                 for (j = 0; j < m_d_theta.size(); j++) {
303                         // slight hack: we do 0.80*, so that if there is some oscillation,
304                         // the system can still converge (for joint limits). also, it's
305                         // better to go a little to slow than to far
306                         
307                         MT_Scalar dofdamp = damp/m_weight[j];
308                         if (dofdamp > 1.0) dofdamp = 1.0;
309                         
310                         m_d_theta[j] += 0.80*dofdamp*m_d_theta_tmp[j];
311                 }
312
313                 if (damp < m_min_damp)
314                         m_min_damp = damp;
315         }
316
317         // weight + prevent from doing angle updates with angles > max_angle_change
318         MT_Scalar max_angle = 0.0, abs_angle;
319
320         for (j = 0; j<m_dof; j++) {
321                 m_d_theta[j] *= m_weight[j];
322
323                 abs_angle = MT_abs(m_d_theta[j]);
324
325                 if (abs_angle > max_angle)
326                         max_angle = abs_angle;
327         }
328         
329         if (max_angle > max_angle_change) {
330                 MT_Scalar damp = (max_angle_change)/(max_angle_change + max_angle);
331
332                 for (j = 0; j<m_dof; j++)
333                         m_d_theta[j] *= damp;
334         }
335 }
336
337 void IK_QJacobian::InvertDLS()
338 {
339         // Compute damped least squares inverse of pseudo inverse
340         // Compute damping term lambda
341
342         // Note when lambda is zero this is equivalent to the
343         // least squares solution. This is fine when the m_jjt is
344         // of full rank. When m_jjt is near to singular the least squares
345         // inverse tries to minimize |J(dtheta) - dX)| and doesn't 
346         // try to minimize  dTheta. This results in eratic changes in angle.
347         // Damped least squares minimizes |dtheta| to try and reduce this
348         // erratic behaviour.
349
350         // We don't want to use the damped solution everywhere so we
351         // only increase lamda from zero as we approach a singularity.
352
353         // find the smallest non-zero W value, anything below epsilon is
354         // treated as zero
355
356         MT_Scalar epsilon = 1e-10;
357         MT_Scalar max_angle_change = 0.1;
358         MT_Scalar x_length = sqrt(TNT::dot_prod(m_beta, m_beta));
359
360         int i, j;
361         MT_Scalar w_min = MT_INFINITY;
362
363         for (i = 0; i <m_svd_w.size() ; i++) {
364                 if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
365                         w_min = m_svd_w[i];
366         }
367         
368         // compute lambda damping term
369
370         MT_Scalar d = x_length/max_angle_change;
371         MT_Scalar lambda;
372
373         if (w_min <= d/2)
374                 lambda = d/2;
375         else if (w_min < d)
376                 lambda = sqrt(w_min*(d - w_min));
377         else
378                 lambda = 0.0;
379
380         lambda *= lambda;
381
382         if (lambda > 10)
383                 lambda = 10;
384
385         // immediately multiply with Beta, so we can do matrix*vector products
386         // rather than matrix*matrix products
387
388         // compute Ut*Beta
389         TNT::transpose(m_svd_u, m_svd_u_t);
390         TNT::matmult(m_svd_u_beta, m_svd_u_t, m_beta);
391
392         m_d_theta = 0.0;
393
394         for (i = 0; i < m_svd_w.size(); i++) {
395                 if (m_svd_w[i] > epsilon) {
396                         MT_Scalar wInv = m_svd_w[i]/(m_svd_w[i]*m_svd_w[i] + lambda);
397
398                         // compute V*Winv*Ut*Beta
399                         m_svd_u_beta[i] *= wInv;
400
401                         for (j = 0; j<m_d_theta.size(); j++)
402                                 m_d_theta[j] += m_svd_v[j][i]*m_svd_u_beta[i];
403                 }
404         }
405
406         for (j = 0; j<m_d_theta.size(); j++)
407                 m_d_theta[j] *= m_weight[j];
408 }
409
410 void IK_QJacobian::Lock(int dof_id, MT_Scalar delta)
411 {
412         int i;
413
414         for (i = 0; i < m_task_size; i++) {
415                 m_beta[i] -= m_jacobian[i][dof_id]*delta;
416                 m_jacobian[i][dof_id] = 0.0;
417         }
418
419         m_norm[dof_id] = 0.0; // unneeded
420         m_d_theta[dof_id] = 0.0;
421 }
422
423 MT_Scalar IK_QJacobian::AngleUpdate(int dof_id) const
424 {
425         return m_d_theta[dof_id];
426 }
427
428 MT_Scalar IK_QJacobian::AngleUpdateNorm() const
429 {
430         int i;
431         MT_Scalar mx = 0.0, dtheta_abs;
432
433         for (i = 0; i < m_d_theta.size(); i++) {
434                 dtheta_abs = MT_abs(m_d_theta[i]*m_d_norm_weight[i]);
435                 if (dtheta_abs > mx)
436                         mx = dtheta_abs;
437         }
438         
439         return mx;
440 }
441
442 void IK_QJacobian::SetDoFWeight(int dof, MT_Scalar weight)
443 {
444         m_weight[dof] = weight;
445         m_weight_sqrt[dof] = sqrt(weight);
446 }
447