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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
/*
* Copyright (C) 2010 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
* Author: Eric Sauser
* email: eric.sauser@a3.epf.ch
* website: lasa.epfl.ch
*
* Permission is granted to copy, distribute, and/or modify this program
* under the terms of the GNU General Public License, version 2 or any
* later version published by the Free Software Foundation.
*
* 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
*/
#ifndef IKSUBSOLVER_H_
#define IKSUBSOLVER_H_
#include "MathLib.h"
#ifdef USE_MATHLIB_NAMESPACE
namespace MathLib {
#endif
/**
* \class IKSubSolver
*
* \ingroup MathLib
*
* \brief A class for solving inverse kinematic in a pseudo-inverse fashion with optimization
*
* This is a loose inverse kinematic algorithm... By loose, I mean that constraint are only satisfied if possible...
* This way some singularities can be solved pretty well...
*/
class IKSubSolver
{
public:
friend class IKGroupSolver;
public:
/// Constructor
IKSubSolver();
/// Destructor
virtual ~IKSubSolver();
/// Allows to print out debug message
void SetVerbose(bool verbose=true);
/// Initialization: prepare the system to process n DOFs and m constraints
void SetSizes(int dofs, int constraintsSize);
/// Set the thresholds for
void SetThresholds(REALTYPE loose, REALTYPE cut);
/// Sets the jacobian to the system
void SetJacobian(const Matrix & j);
/// Sets the weights on the degrees of freedom (useful if the redundancies are on the DOFs)
void SetDofsWeights(Matrix &m);
void SetDofsWeights(Vector &v);
/// Sets the weights on the constraints (useful if the redundancies are on the constraints)
void SetConstraintsWeights(Matrix &m);
void SetConstraintsWeights(Vector &v);
/// Sets the target values to reach (Size given by the number of constraints)
void SetTarget(Vector &v);
/// Compute the inverse kinematic solution
void Solve();
/// Get the result
Vector& GetOutput();
/// Get the actual target that output values produces
Vector& GetTargetOutput();
/// Get the error between produced target and the requested one
Vector& GetTargetError();
/// Get the squared norm of the error between produced target and the requested one
REALTYPE GetTargetErrorNorm();
REALTYPE GetTargetErrorNorm2();
/// Get the null space
Matrix& GetNullSpace();
/// Get the jacobian
Matrix& GetJacobian();
protected:
void Resize();
protected:
bool bVerbose;
int mConstraintsSize;
int mDofs;
Matrix mJacobian;
Matrix mDofsWeights;
Matrix mConstrWeights;
Vector mDesiredTarget;
Vector mOutputTarget;
Vector mErrorTarget;
Vector mOutput;
Vector mOutputTmp;
REALTYPE mLooseThreshold;
REALTYPE mCutThreshold;
Matrix mJW;
Matrix mWtJt;
Matrix mWtJtJW;
Matrix mTriMatrix;
Matrix mEigenVectors;
Matrix mEigenVectorsTranspose;
Matrix mRedEigenVectorsPtr;
Matrix mNullEigenVectorsPtr;
Matrix mRedEigenVectorsTransposePtr;
Matrix mNullEigenVectorsTransposePtr;
SharedMatrix mRedEigenVectors;
SharedMatrix mNullEigenVectors;
SharedMatrix mRedEigenVectorsTranspose;
SharedMatrix mNullEigenVectorsTranspose;
Vector mEigenValues;
SharedVector mRedEigenValues;
SharedVector mRedInvEigenValues;
Vector mRedEigenValuesPtr;
Vector mRedInvEigenValuesPtr;
Vector mCondNumbersVector;
int mEigenSteps;
int mRank;
Matrix mRedPseudoInverseTmpMatrixPtr;
Matrix mRedPseudoInversePtr;
Matrix mWeightedRedPseudoInversePtr;
SharedMatrix mRedPseudoInverseTmpMatrix;
SharedMatrix mRedPseudoInverse;
SharedMatrix mWeightedRedPseudoInverse;
};
#ifdef USE_MATHLIB_NAMESPACE
}
#endif
#endif /*IKSOLVER_H_*/
|
