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