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