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
156
157
158
159
160
161
162
163
164
165
166
167
168
|
/*
* 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 IKGROUPSOLVER_H_
#define IKGROUPSOLVER_H_
#include "MathLib.h"
#include "IKSubSolver.h"
#ifdef USE_MATHLIB_NAMESPACE
namespace MathLib {
#endif
/**
* \class IKGroupSolver
*
* \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 IKGroupSolver
{
public:
/// Constructor
IKGroupSolver();
/// Destructor
virtual ~IKGroupSolver();
/// Allows to print out debug message
void SetVerbose(bool verbose=true);
void SetSizes(int dofs);
/// Sets the weights on the degrees of freedom (useful if the redundancies are on the DOFs)
void SetDofsWeights(Vector &v);
//void SetDofsWeights(Matrix &m);
/// Add a solver item with given constraints size and return the sovler id
int AddSolverItem(const int constraintsSize);
/// Set jacobian dofs indices for the given solver
void SetDofsIndices(const vector<unsigned int> & dofsIndex, int solverId = 0);
/// Set the priority of the given solver
void SetPriority(int priority, int solverId = 0);
/// Set the thresholds for the given solver (all by default)
void SetThresholds(REALTYPE loose, REALTYPE cut, int solverId = -1);
/// Sets the jacobian for the given solver (if provided, dofs indices will be used)
void SetJacobian(const Matrix & j, int solverId = 0);
/// Sets the weights on the constraints for the given solver (useful if the redundancies are on the constraints)
void SetConstraintsWeights(Matrix &m, int solverId = 0);
void SetConstraintsWeights(Vector &v, int solverId = 0);
/// Sets the target values to reach for the given solver
void SetTarget(const Vector &v, int solverId = 0);
/// Enable or disable the given solver
void Enable(bool enable=true, int solverId = 0);
/// Suspend or resume the given solver (used in conjunction with Enable)
void Suspend(bool suspend=true, int solverId = 0);
/// Get if the given solver is enabled
bool IsEnabled(int solverId = 0);
/// Sets the target for the null space (Size given by the number of DOFs)
void SetNullTarget(const Vector &null);
/// Removes all constraints limits on the outputs
void ClearLimits();
/**
* \brief Sets the constraints limits on the putput
* \param low Vector for low bounds values
* \param high Vector for high bounds values
*/
void SetLimits(const Vector &low, const Vector &high);
void ComputePriorities();
void Solve();
void Resize();
/// Get the result
Vector& GetOutput();
/// Get the error between produced target and the requested one
Vector& GetTargetError(int solverId = 0);
/// Get the actual target that output values produces
Vector& GetTargetOutput(int solverId = 0);
/// Get the squared norm of the error between produced target and the requested one
REALTYPE GetTargetErrorNorm();
REALTYPE GetTargetErrorNorm2();
protected:
typedef struct{
IKSubSolver mSolver;
IndicesVector mDofsIndex;
int mPriority;
Vector mDesiredTarget;
Vector mActualTarget;
Vector mOutputTarget;
Vector mErrorTarget;
Vector mOutput;
bool bEnabled;
bool bSuspended;
}IKSolverItem;
vector<IKSolverItem> mIKItems;
vector<int> mSortedPriorityIds;
bool bVerbose;
bool bComputePriorities;
int mConstraintsSize;
int mDofs;
Vector mNullTarget;
Vector mLimits[2];
Vector mCurrLimits[2];
Vector mLimitsOffset;
Matrix mDofsWeights;
Matrix mInvDofsWeights;
Matrix mCurrDofsWeights;
Matrix mCurrWeights;
Matrix mCurrWeightsTranspose;
Vector mLimitsOffsetTarget;
Vector mOutput;
Vector mStepOutput;
Vector mOutputOffset;
Vector mOutputLimitsError;
Vector tmpCsV;
Vector tmpDsV;
Vector tmpDsV2;
Matrix tmpMat;
};
#ifdef USE_MATHLIB_NAMESPACE
}
#endif
#endif
|
