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
21 * \ingroup iksolver
22 */
23
24 #include "IK_QJacobian.h"
25
IK_QJacobian()26 IK_QJacobian::IK_QJacobian() : m_sdls(true), m_min_damp(1.0)
27 {
28 }
29
~IK_QJacobian()30 IK_QJacobian::~IK_QJacobian()
31 {
32 }
33
ArmMatrices(int dof,int task_size)34 void IK_QJacobian::ArmMatrices(int dof, int task_size)
35 {
36 m_dof = dof;
37 m_task_size = task_size;
38
39 m_jacobian.resize(task_size, dof);
40 m_jacobian.setZero();
41
42 m_alpha.resize(dof);
43 m_alpha.setZero();
44
45 m_nullspace.resize(dof, dof);
46
47 m_d_theta.resize(dof);
48 m_d_theta_tmp.resize(dof);
49 m_d_norm_weight.resize(dof);
50
51 m_norm.resize(dof);
52 m_norm.setZero();
53
54 m_beta.resize(task_size);
55
56 m_weight.resize(dof);
57 m_weight_sqrt.resize(dof);
58 m_weight.setOnes();
59 m_weight_sqrt.setOnes();
60
61 if (task_size >= dof) {
62 m_transpose = false;
63
64 m_jacobian_tmp.resize(task_size, dof);
65
66 m_svd_u.resize(task_size, dof);
67 m_svd_v.resize(dof, dof);
68 m_svd_w.resize(dof);
69
70 m_svd_u_beta.resize(dof);
71 }
72 else {
73 // use the SVD of the transpose jacobian, it works just as well
74 // as the original, and often allows using smaller matrices.
75 m_transpose = true;
76
77 m_jacobian_tmp.resize(dof, task_size);
78
79 m_svd_u.resize(task_size, task_size);
80 m_svd_v.resize(dof, task_size);
81 m_svd_w.resize(task_size);
82
83 m_svd_u_beta.resize(task_size);
84 }
85 }
86
SetBetas(int id,int,const Vector3d & v)87 void IK_QJacobian::SetBetas(int id, int, const Vector3d &v)
88 {
89 m_beta[id + 0] = v.x();
90 m_beta[id + 1] = v.y();
91 m_beta[id + 2] = v.z();
92 }
93
SetDerivatives(int id,int dof_id,const Vector3d & v,double norm_weight)94 void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
95 {
96 m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id];
97 m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id];
98 m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id];
99
100 m_d_norm_weight[dof_id] = norm_weight;
101 }
102
Invert()103 void IK_QJacobian::Invert()
104 {
105 if (m_transpose) {
106 // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
107 // so J = U*W*Vt and Jinv = V*Winv*Ut
108 Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(),
109 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
ComputeNullProjection()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
SubTask(IK_QJacobian & jacobian)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
Restrict(VectorXd & d_theta,MatrixXd & nullspace)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
InvertSDLS()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)
288 dofdamp = 1.0;
289
290 m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
291 }
292
293 if (damp < m_min_damp)
294 m_min_damp = damp;
295 }
296
297 // weight + prevent from doing angle updates with angles > max_angle_change
298 double max_angle = 0.0, abs_angle;
299
300 for (j = 0; j < m_dof; j++) {
301 m_d_theta[j] *= m_weight[j];
302
303 abs_angle = fabs(m_d_theta[j]);
304
305 if (abs_angle > max_angle)
306 max_angle = abs_angle;
307 }
308
309 if (max_angle > max_angle_change) {
310 double damp = (max_angle_change) / (max_angle_change + max_angle);
311
312 for (j = 0; j < m_dof; j++)
313 m_d_theta[j] *= damp;
314 }
315 }
316
InvertDLS()317 void IK_QJacobian::InvertDLS()
318 {
319 // Compute damped least squares inverse of pseudo inverse
320 // Compute damping term lambda
321
322 // Note when lambda is zero this is equivalent to the
323 // least squares solution. This is fine when the m_jjt is
324 // of full rank. When m_jjt is near to singular the least squares
325 // inverse tries to minimize |J(dtheta) - dX)| and doesn't
326 // try to minimize dTheta. This results in eratic changes in angle.
327 // Damped least squares minimizes |dtheta| to try and reduce this
328 // erratic behaviour.
329
330 // We don't want to use the damped solution everywhere so we
331 // only increase lamda from zero as we approach a singularity.
332
333 // find the smallest non-zero W value, anything below epsilon is
334 // treated as zero
335
336 double epsilon = 1e-10;
337 double max_angle_change = 0.1;
338 double x_length = sqrt(m_beta.dot(m_beta));
339
340 int i, j;
341 double w_min = std::numeric_limits<double>::max();
342
343 for (i = 0; i < m_svd_w.size(); i++) {
344 if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min)
345 w_min = m_svd_w[i];
346 }
347
348 // compute lambda damping term
349
350 double d = x_length / max_angle_change;
351 double lambda;
352
353 if (w_min <= d / 2)
354 lambda = d / 2;
355 else if (w_min < d)
356 lambda = sqrt(w_min * (d - w_min));
357 else
358 lambda = 0.0;
359
360 lambda *= lambda;
361
362 if (lambda > 10)
363 lambda = 10;
364
365 // immediately multiply with Beta, so we can do matrix*vector products
366 // rather than matrix*matrix products
367
368 // compute Ut*Beta
369 m_svd_u_beta = m_svd_u.transpose() * m_beta;
370
371 m_d_theta.setZero();
372
373 for (i = 0; i < m_svd_w.size(); i++) {
374 if (m_svd_w[i] > epsilon) {
375 double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
376
377 // compute V*Winv*Ut*Beta
378 m_svd_u_beta[i] *= wInv;
379
380 for (j = 0; j < m_d_theta.size(); j++)
381 m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i];
382 }
383 }
384
385 for (j = 0; j < m_d_theta.size(); j++)
386 m_d_theta[j] *= m_weight[j];
387 }
388
Lock(int dof_id,double delta)389 void IK_QJacobian::Lock(int dof_id, double delta)
390 {
391 int i;
392
393 for (i = 0; i < m_task_size; i++) {
394 m_beta[i] -= m_jacobian(i, dof_id) * delta;
395 m_jacobian(i, dof_id) = 0.0;
396 }
397
398 m_norm[dof_id] = 0.0; // unneeded
399 m_d_theta[dof_id] = 0.0;
400 }
401
AngleUpdate(int dof_id) const402 double IK_QJacobian::AngleUpdate(int dof_id) const
403 {
404 return m_d_theta[dof_id];
405 }
406
AngleUpdateNorm() const407 double IK_QJacobian::AngleUpdateNorm() const
408 {
409 int i;
410 double mx = 0.0, dtheta_abs;
411
412 for (i = 0; i < m_d_theta.size(); i++) {
413 dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
414 if (dtheta_abs > mx)
415 mx = dtheta_abs;
416 }
417
418 return mx;
419 }
420
SetDoFWeight(int dof,double weight)421 void IK_QJacobian::SetDoFWeight(int dof, double weight)
422 {
423 m_weight[dof] = weight;
424 m_weight_sqrt[dof] = sqrt(weight);
425 }
426