Initial revision
[blender.git] / intern / iksolver / intern / IK_QJacobianSolver.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  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 #include "IK_QJacobianSolver.h"
33
34 #include "TNT/svd.h"
35
36 using namespace std;
37
38         IK_QJacobianSolver *
39 IK_QJacobianSolver::
40 New(
41 ){
42         return new IK_QJacobianSolver();
43 }
44
45         bool
46 IK_QJacobianSolver::
47 Solve(
48         IK_QChain &chain,
49         const MT_Vector3 &g_position,
50         const MT_Vector3 &g_pose,
51         const MT_Scalar tolerance,
52         const int max_iterations,
53         const MT_Scalar max_angle_change
54 ){
55
56         const vector<IK_QSegment> & segs = chain.Segments();
57         if (segs.size() == 0) return false;
58         
59         // compute the goal direction from the base of the chain
60         // in global coordinates
61
62         MT_Vector3 goal_dir = g_position - segs[0].GlobalSegmentStart();
63         
64
65         const MT_Scalar chain_max_extension = chain.MaxExtension();
66
67         bool do_parallel_check(false);
68
69         if (chain_max_extension < goal_dir.length()) {
70                 do_parallel_check = true;
71         }
72
73         goal_dir.normalize();
74
75
76         ArmMatrices(chain.DoF());
77
78         for (int iterations = 0; iterations < max_iterations; iterations++) {
79                 
80                 // check to see if the chain is pointing in the right direction
81
82                 if (iterations%32 && do_parallel_check && ParallelCheck(chain,goal_dir)) {
83
84                         return false;
85                 }
86                 
87                 MT_Vector3 end_effector = chain.EndEffector();
88                 MT_Vector3 d_pos = g_position - end_effector;
89                 const MT_Scalar x_length = d_pos.length();
90                 
91                 if (x_length < tolerance) {
92                         return true;
93                 }       
94
95                 chain.ComputeJacobian();
96
97         try {
98                     ComputeInverseJacobian(chain,x_length,max_angle_change);
99         }
100         catch(...) {
101             return false;
102         }
103                 
104                 ComputeBetas(chain,d_pos);
105                 UpdateChain(chain);
106                 chain.UpdateGlobalTransformations();
107         }
108
109
110         return false;
111 };
112
113 IK_QJacobianSolver::
114 ~IK_QJacobianSolver(
115 ){
116         // nothing to do
117 }
118
119
120 IK_QJacobianSolver::
121 IK_QJacobianSolver(
122 ){
123         // nothing to do
124 };
125  
126         void
127 IK_QJacobianSolver::
128 ComputeBetas(
129         IK_QChain &chain,
130         const MT_Vector3 d_pos
131 ){      
132
133         m_beta = 0;
134
135         m_beta[0] = d_pos.x();
136         m_beta[1] = d_pos.y();
137         m_beta[2] = d_pos.z();
138         
139         TNT::matmult(m_d_theta,m_svd_inverse,m_beta);
140
141 };      
142
143
144         int
145 IK_QJacobianSolver::
146 ComputeInverseJacobian(
147         IK_QChain &chain,
148         const MT_Scalar x_length,
149         const MT_Scalar max_angle_change
150 ) {
151
152         int dimension = 0;
153
154         m_svd_u = MT_Scalar(0);
155
156         // copy first 3 rows of jacobian into m_svd_u
157
158         int row, column;
159
160         for (row = 0; row < 3; row ++) {
161                 for (column = 0; column < chain.Jacobian().num_cols(); column ++) {
162                         m_svd_u[row][column] = chain.Jacobian()[row][column];
163                 }
164         }
165
166         m_svd_w = MT_Scalar(0);
167         m_svd_v = MT_Scalar(0);
168
169     m_svd_work_space = MT_Scalar(0);
170
171         TNT::SVD(m_svd_u,m_svd_w,m_svd_v,m_svd_work_space);
172
173         // invert the SVD and compute inverse
174
175         TNT::transpose(m_svd_v,m_svd_v_t);
176         TNT::transpose(m_svd_u,m_svd_u_t);
177
178         // Compute damped least squares inverse of pseudo inverse
179         // Compute damping term lambda
180
181         // Note when lambda is zero this is equivalent to the
182         // least squares solution. This is fine when the m_jjt is
183         // of full rank. When m_jjt is near to singular the least squares
184         // inverse tries to minimize |J(dtheta) - dX)| and doesn't 
185         // try to minimize  dTheta. This results in eratic changes in angle.
186         // Damped least squares minimizes |dtheta| to try and reduce this
187         // erratic behaviour.
188
189         // We don't want to use the damped solution everywhere so we
190         // only increase lamda from zero as we approach a singularity.
191
192         // find the smallest non-zero m_svd_w value
193
194         int i = 0;
195
196         MT_Scalar w_min = MT_INFINITY;
197
198         // anything below epsilon is treated as zero
199
200         MT_Scalar epsilon = MT_Scalar(1e-10);
201
202         for ( i = 0; i <m_svd_w.size() ; i++) {
203
204                 if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) {
205                         w_min = m_svd_w[i];
206                 }
207         }
208
209         MT_Scalar lambda(0);
210
211         MT_Scalar d = x_length/max_angle_change;
212
213         if (w_min <= d/2) {
214                 lambda = d/2;
215         } else
216         if (w_min < d) {
217                 lambda = sqrt(w_min*(d - w_min));
218         } else {
219                 lambda = MT_Scalar(0);
220         }
221
222
223         lambda *= lambda;
224
225         for (i= 0; i < m_svd_w.size(); i++) {
226                 if (m_svd_w[i] < epsilon) {
227                         m_svd_w[i] = MT_Scalar(0);
228                 } else {
229                         m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
230                 }
231         }
232
233
234         TNT::matmultdiag(m_svd_temp1,m_svd_w,m_svd_u_t);
235         TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1);
236
237         return dimension;
238
239 }
240
241         void
242 IK_QJacobianSolver::
243 UpdateChain(
244         IK_QChain &chain
245 ){
246
247         // iterate through the set of angles and 
248         // update their values from the d_thetas
249
250         vector<IK_QSegment> &segs = chain.Segments(); 
251         
252         int seg_ind = 0;
253         for (seg_ind = 0;seg_ind < segs.size(); seg_ind++) {
254         
255                 MT_Vector3 dq;
256                 dq[0] = m_d_theta[3*seg_ind];
257                 dq[1] = m_d_theta[3*seg_ind + 1];
258                 dq[2] = m_d_theta[3*seg_ind + 2];
259                 segs[seg_ind].IncrementAngle(dq);       
260         }
261
262 };
263         
264         void
265 IK_QJacobianSolver::
266 ArmMatrices(
267         int dof
268 ){
269
270         m_beta.newsize(dof);
271         m_d_theta.newsize(dof);
272
273         m_svd_u.newsize(dof,dof);
274         m_svd_v.newsize(dof,dof);
275         m_svd_w.newsize(dof);
276
277     m_svd_work_space.newsize(dof);
278
279         m_svd_u = MT_Scalar(0);
280         m_svd_v = MT_Scalar(0);
281         m_svd_w = MT_Scalar(0);
282
283         m_svd_u_t.newsize(dof,dof);
284         m_svd_v_t.newsize(dof,dof);
285         m_svd_w_diag.newsize(dof,dof);
286         m_svd_inverse.newsize(dof,dof);
287         m_svd_temp1.newsize(dof,dof);
288
289 };
290
291         bool
292 IK_QJacobianSolver::
293 ParallelCheck(
294         const IK_QChain &chain,
295         const MT_Vector3 goal_dir
296 ) const {
297         
298         // compute the start of the chain in global coordinates
299         const vector<IK_QSegment> &segs = chain.Segments(); 
300
301         int num_segs = segs.size();
302         
303         if (num_segs == 0) {
304                 return false;
305         }
306
307         MT_Scalar crossp_sum = 0;
308
309         int i;
310         for (i = 0; i < num_segs; i++) {
311                 MT_Vector3 global_seg_direction = segs[i].GlobalSegmentEnd() - 
312                         segs[i].GlobalSegmentStart();
313
314                 global_seg_direction.normalize();       
315
316                 MT_Scalar crossp = (global_seg_direction.cross(goal_dir)).length();
317                 crossp_sum += MT_Scalar(fabs(crossp));
318         }
319
320         if (crossp_sum < MT_Scalar(0.01)) {
321                 return true;
322         } else {
323                 return false;
324         }
325 }
326
327