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