KDL 1.5.1
Loading...
Searching...
No Matches
chainiksolverpos_lma.hpp
Go to the documentation of this file.
1#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
2#define KDL_CHAINIKSOLVERPOS_GN_HPP
8/**************************************************************************
9 begin : May 2012
10 copyright : (C) 2012 Erwin Aertbelien
11 email : firstname.lastname@mech.kuleuven.ac.be
12
13 History (only major changes)( AUTHOR-Description ) :
14
15 ***************************************************************************
16 * This library is free software; you can redistribute it and/or *
17 * modify it under the terms of the GNU Lesser General Public *
18 * License as published by the Free Software Foundation; either *
19 * version 2.1 of the License, or (at your option) any later version. *
20 * *
21 * This library is distributed in the hope that it will be useful, *
22 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
24 * Lesser General Public License for more details. *
25 * *
26 * You should have received a copy of the GNU Lesser General Public *
27 * License along with this library; if not, write to the Free Software *
28 * Foundation, Inc., 59 Temple Place, *
29 * Suite 330, Boston, MA 02111-1307 USA *
30 * *
31 ***************************************************************************/
32
33
34#include "chainiksolver.hpp"
35#include "chain.hpp"
36#include <Eigen/Dense>
37
38namespace KDL
39{
40
65{
66private:
67 typedef double ScalarType;
68 typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
69 typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
70public:
71
72 static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
73 static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;
74
96 const KDL::Chain& _chain,
97 const Eigen::Matrix<double,6,1>& _L,
98 double _eps=1E-5,
99 int _maxiter=500,
100 double _eps_joints=1E-15
101 );
102
109 const KDL::Chain& _chain,
110 double _eps=1E-5,
111 int _maxiter=500,
112 double _eps_joints=1E-15
113 );
114
126 virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);
127
131 virtual ~ChainIkSolverPos_LMA();
132
138 void compute_fwdpos(const VectorXq& q);
139
145 void compute_jacobian(const VectorXq& q);
146
151 void display_jac(const KDL::JntArray& jval);
152
155
157 virtual const char* strError(const int error) const;
158
159private:
161 unsigned int nj;
162 unsigned int ns;
163
164public:
165
166
171
176
181
186
191
198
211
216private:
217 // additional specification of the inverse position kinematics problem:
218 unsigned int maxiter;
219 double eps;
221 Eigen::Matrix<ScalarType,6,1> L;
222
223
224
225 // state of compute_fwdpos and compute_jacobian:
226 std::vector<KDL::Frame> T_base_jointroot;
227 std::vector<KDL::Frame> T_base_jointtip;
228 // need 2 vectors because of the somewhat strange definition of segment.hpp
229 // you could also recompute jointtip out of jointroot,
230 // but then you'll need more expensive cos/sin functions.
231
232
233 // the following are state of CartToJnt that is pre-allocated:
234
238 Eigen::LDLT<MatrixXq> ldlt;
239 Eigen::JacobiSVD<MatrixXq> svd;
243};
244
245
246
247
248
249} // namespace KDL
250
251
252
253
254
255
256#endif
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
Definition chainiksolverpos_lma.hpp:65
void updateInternalDataStructures()
Update the internal data structures.
Definition chainiksolverpos_lma.cpp:122
void compute_jacobian(const VectorXq &q)
for internal use only.
Definition chainiksolverpos_lma.cpp:158
double ScalarType
Definition chainiksolverpos_lma.hpp:67
VectorXq original_Aii
Definition chainiksolverpos_lma.hpp:242
static const int E_INCREMENT_JOINTS_TOO_SMALL
Definition chainiksolverpos_lma.hpp:73
MatrixXq A
Definition chainiksolverpos_lma.hpp:236
Eigen::LDLT< MatrixXq > ldlt
Definition chainiksolverpos_lma.hpp:238
KDL::Frame T_base_head
for internal use only.
Definition chainiksolverpos_lma.hpp:210
bool display_information
display information on each iteration step to the console.
Definition chainiksolverpos_lma.hpp:215
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Definition chainiksolverpos_lma.cpp:187
ChainIkSolverPos_LMA(const KDL::Chain &_chain, const Eigen::Matrix< double, 6, 1 > &_L, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15)
constructs an ChainIkSolverPos_LMA solver.
Definition chainiksolverpos_lma.cpp:50
static const int E_GRADIENT_JOINTS_TOO_SMALL
Definition chainiksolverpos_lma.hpp:72
Eigen::JacobiSVD< MatrixXq > svd
Definition chainiksolverpos_lma.hpp:239
Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixXq
Definition chainiksolverpos_lma.hpp:68
const KDL::Chain & chain
Definition chainiksolverpos_lma.hpp:160
VectorXq diffq
Definition chainiksolverpos_lma.hpp:240
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition chainiksolverpos_lma.cpp:317
VectorXq q
Definition chainiksolverpos_lma.hpp:235
unsigned int maxiter
Definition chainiksolverpos_lma.hpp:218
std::vector< KDL::Frame > T_base_jointroot
Definition chainiksolverpos_lma.hpp:226
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
Definition chainiksolverpos_lma.hpp:180
VectorXq lastSV
contains the last values for the singular values of the weighted Jacobian after an execution of CartT...
Definition chainiksolverpos_lma.hpp:190
VectorXq tmp
Definition chainiksolverpos_lma.hpp:237
std::vector< KDL::Frame > T_base_jointtip
Definition chainiksolverpos_lma.hpp:227
VectorXq q_new
Definition chainiksolverpos_lma.hpp:241
double lastDifference
contains the last value for after an execution of CartToJnt.
Definition chainiksolverpos_lma.hpp:175
void compute_fwdpos(const VectorXq &q)
for internal use only.
Definition chainiksolverpos_lma.cpp:141
double eps_joints
Definition chainiksolverpos_lma.hpp:220
Eigen::Matrix< ScalarType, 6, 1 > L
Definition chainiksolverpos_lma.hpp:221
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Definition chainiksolverpos_lma.hpp:170
unsigned int ns
Definition chainiksolverpos_lma.hpp:162
double eps
Definition chainiksolverpos_lma.hpp:219
unsigned int nj
Definition chainiksolverpos_lma.hpp:161
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq
Definition chainiksolverpos_lma.hpp:69
MatrixXq jac
for internal use only.
Definition chainiksolverpos_lma.hpp:197
void display_jac(const KDL::JntArray &jval)
for internal use only.
Definition chainiksolverpos_lma.cpp:177
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
Definition chainiksolverpos_lma.hpp:185
VectorXq grad
for internal use only.
Definition chainiksolverpos_lma.hpp:204
virtual ~ChainIkSolverPos_LMA()
destructor.
Definition chainiksolverpos_lma.cpp:139
Definition chainiksolver.hpp:42
Definition chain.hpp:35
Definition frames.hpp:570
Definition jntarray.hpp:70
int error
Latest error, initialized to E_NOERROR in constructor.
Definition solveri.hpp:149
Definition articulatedbodyinertia.cpp:26