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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
|
/*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*/
/** \file
* \ingroup 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 it's 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;
};
|
