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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
|
/*
* $Id$
* ***** BEGIN GPL LICENSE BLOCK *****
*
* 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.
*
* The Original Code is: all of this file.
*
* Original Author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL LICENSE BLOCK *****
*/
/** \file iksolver/intern/IK_QJacobian.h
* \ingroup iksolver
*/
#ifndef NAN_INCLUDED_IK_QJacobian_h
#define NAN_INCLUDED_IK_QJacobian_h
#include "TNT/cmat.h"
#include <vector>
#include "MT_Vector3.h"
class IK_QJacobian
{
public:
typedef TNT::Matrix<MT_Scalar> TMatrix;
typedef TNT::Vector<MT_Scalar> TVector;
IK_QJacobian();
~IK_QJacobian();
// Call once to initialize
void ArmMatrices(int dof, int task_size);
void SetDoFWeight(int dof, MT_Scalar weight);
// Iteratively called
void SetBetas(int id, int size, const MT_Vector3& v);
void SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight);
void Invert();
MT_Scalar AngleUpdate(int dof_id) const;
MT_Scalar AngleUpdateNorm() const;
// DoF locking for inner clamping loop
void Lock(int dof_id, MT_Scalar delta);
// Secondary task
bool ComputeNullProjection();
void Restrict(TVector& d_theta, TMatrix& null);
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
TMatrix m_jacobian, m_jacobian_tmp;
TMatrix m_null;
/// the vector of intermediate betas
TVector m_beta;
/// the vector of computed angle changes
TVector m_d_theta;
TVector m_d_norm_weight;
/// space required for SVD computation
TVector m_svd_w;
TMatrix m_svd_v;
TMatrix m_svd_u;
TVector m_work1;
TVector m_work2;
TMatrix m_svd_u_t;
TVector m_svd_u_beta;
// space required for SDLS
bool m_sdls;
TVector m_norm;
TVector m_d_theta_tmp;
MT_Scalar m_min_damp;
// null space task vector
TVector m_alpha;
// dof weighting
TVector m_weight;
TVector m_weight_sqrt;
};
#endif
|
