intern/iksolver/intern/IK_QJacobian.h


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78


/* SPDX-License-Identifier: GPL-2.0-or-later
 * Copyright 2001-2002 NaN Holding BV. All rights reserved. */

/** \file
 * \ingroup intern_iksolver
 */

#pragma once

#include "IK_Math.h"

class IK_QJacobian {
 public:
  IK_QJacobian();
  ~IK_QJacobian();

  // Call once to initialize
  void ArmMatrices(int dof, int task_size);
  void SetDoFWeight(int dof, double weight);

  // Iteratively called
  void SetBetas(int id, int size, const Vector3d &v);
  void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight);

  void Invert();

  double AngleUpdate(int dof_id) const;
  double AngleUpdateNorm() const;

  // DoF locking for inner clamping loop
  void Lock(int dof_id, double delta);

  // Secondary task
  bool ComputeNullProjection();

  void Restrict(VectorXd &d_theta, MatrixXd &nullspace);
  void SubTask(IK_QJacobian &jacobian);

 private:
  void InvertSDLS();
  void InvertDLS();

  int m_dof, m_task_size;
  bool m_transpose;

  // the jacobian matrix and its null space projector
  MatrixXd m_jacobian, m_jacobian_tmp;
  MatrixXd m_nullspace;

  /// the vector of intermediate betas
  VectorXd m_beta;

  /// the vector of computed angle changes
  VectorXd m_d_theta;
  VectorXd m_d_norm_weight;

  /// space required for SVD computation
  VectorXd m_svd_w;
  MatrixXd m_svd_v;
  MatrixXd m_svd_u;

  VectorXd m_svd_u_beta;

  // space required for SDLS

  bool m_sdls;
  VectorXd m_norm;
  VectorXd m_d_theta_tmp;
  double m_min_damp;

  // null space task vector
  VectorXd m_alpha;

  // dof weighting
  VectorXd m_weight;
  VectorXd m_weight_sqrt;
};