KDL  1.3.0
chainiksolvervel_wdls.hpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #ifndef KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
23 #define KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
24 
25 #include "chainiksolver.hpp"
26 #include "chainjnttojacsolver.hpp"
27 #include <Eigen/Core>
28 
29 namespace KDL
30 {
64  {
65  public:
66  static const int E_SVD_FAILED = -100;
67  static const int E_CONVERGE_PINV_SINGULAR = +100;
69 
82  explicit ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
83  //=ublas::identity_matrix<double>
85 
101  virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
106  virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
107 
130  void setWeightJS(const Eigen::MatrixXd& Mq);
131 
155  void setWeightTS(const Eigen::MatrixXd& Mx);
156 
160  void setLambda(const double lambda);
164  void setEps(const double eps_in);
168  void setMaxIter(const int maxiter_in);
169 
175  unsigned int getNrZeroSigmas()const {return nrZeroSigmas;};
176 
180  double getSigmaMin()const {return sigmaMin;};
181 
185  double getLambda()const {return lambda;};
186 
191  double getLambdaScaled()const {return lambda_scaled;};
192 
197  int getSVDResult()const {return svdResult;};
198 
200  virtual const char* strError(const int error) const;
201 
202  private:
203  const Chain chain;
206  Eigen::MatrixXd U;
207  Eigen::VectorXd S;
208  Eigen::MatrixXd V;
209  double eps;
210  int maxiter;
211  Eigen::VectorXd tmp;
212  Eigen::MatrixXd tmp_jac;
213  Eigen::MatrixXd tmp_jac_weight1;
214  Eigen::MatrixXd tmp_jac_weight2;
215  Eigen::MatrixXd tmp_ts;
216  Eigen::MatrixXd tmp_js;
217  Eigen::MatrixXd weight_ts;
218  Eigen::MatrixXd weight_js;
219  double lambda;
221  unsigned int nrZeroSigmas ;
223  double sigmaMin;
224  };
225 }
226 #endif
227 
int svdResult
Definition: chainiksolvervel_wdls.hpp:222
ChainIkSolverVel_wdls(const Chain &chain, double eps=0.00001, int maxiter=150)
Constructor of the solver.
Definition: chainiksolvervel_wdls.cpp:28
static const int E_SVD_FAILED
Definition: chainiksolvervel_wdls.hpp:66
Eigen::MatrixXd tmp_jac
Definition: chainiksolvervel_wdls.hpp:212
double eps
Definition: chainiksolvervel_wdls.hpp:209
Definition: jacobian.hpp:35
This class encapsulates a serial kinematic interconnection structure.
Definition: chain.hpp:35
double lambda_scaled
Definition: chainiksolvervel_wdls.hpp:220
Eigen::MatrixXd tmp_ts
Definition: chainiksolvervel_wdls.hpp:215
Eigen::MatrixXd V
Definition: chainiksolvervel_wdls.hpp:208
double getLambdaScaled() const
Request the scaled value of lambda for the minimum singular value 1-6.
Definition: chainiksolvervel_wdls.hpp:191
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: chainiksolvervel_wdls.cpp:176
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
int maxiter
Definition: chainiksolvervel_wdls.hpp:210
double sigmaMin
Definition: chainiksolvervel_wdls.hpp:223
~ChainIkSolverVel_wdls()
Definition: chainiksolvervel_wdls.cpp:53
unsigned int getNrZeroSigmas() const
Request the number of singular values of the jacobian that are < eps; if the number of near zero sing...
Definition: chainiksolvervel_wdls.hpp:175
Eigen::MatrixXd weight_js
Definition: chainiksolvervel_wdls.hpp:218
static const int E_CONVERGE_PINV_SINGULAR
SVD solver failed.
Definition: chainiksolvervel_wdls.hpp:68
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian ve...
Definition: chainiksolvervel_wdls.cpp:80
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:41
represents both translational and rotational velocities.
Definition: frames.hpp:720
Eigen::MatrixXd U
Definition: chainiksolvervel_wdls.hpp:206
double getLambda() const
Request the value of lambda for the minimum.
Definition: chainiksolvervel_wdls.hpp:185
Eigen::MatrixXd tmp_js
Definition: chainiksolvervel_wdls.hpp:216
const Chain chain
Definition: chainiksolvervel_wdls.hpp:203
Definition: articulatedbodyinertia.cpp:28
void setWeightTS(const Eigen::MatrixXd &Mx)
Set the task space weighting matrix.
Definition: chainiksolvervel_wdls.cpp:61
Eigen::VectorXd S
Definition: chainiksolvervel_wdls.hpp:207
virtual int CartToJnt(const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
not (yet) implemented.
Definition: chainiksolvervel_wdls.hpp:106
Eigen::MatrixXd tmp_jac_weight2
Definition: chainiksolvervel_wdls.hpp:214
Definition: jntarrayvel.hpp:45
Eigen::VectorXd tmp
Definition: chainiksolvervel_wdls.hpp:211
void setEps(const double eps_in)
Set eps.
Definition: chainiksolvervel_wdls.cpp:70
ChainJntToJacSolver jnt2jac
Definition: chainiksolvervel_wdls.hpp:204
void setLambda(const double lambda)
Set lambda.
Definition: chainiksolvervel_wdls.cpp:65
Eigen::MatrixXd weight_ts
Definition: chainiksolvervel_wdls.hpp:217
Jacobian jac
Definition: chainiksolvervel_wdls.hpp:205
int getSVDResult() const
Retrieve the latest return code from the SVD algorithm.
Definition: chainiksolvervel_wdls.hpp:197
Eigen::MatrixXd tmp_jac_weight1
Definition: chainiksolvervel_wdls.hpp:213
Implementation of a inverse velocity kinematics algorithm based on the weighted pseudo inverse with d...
Definition: chainiksolvervel_wdls.hpp:63
Definition: framevel.hpp:197
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:122
void setWeightJS(const Eigen::MatrixXd &Mq)
Set the joint space weighting matrix.
Definition: chainiksolvervel_wdls.cpp:57
unsigned int nrZeroSigmas
Definition: chainiksolvervel_wdls.hpp:221
void setMaxIter(const int maxiter_in)
Set maxIter.
Definition: chainiksolvervel_wdls.cpp:75
double getSigmaMin() const
Request the minimum of the first six singular values.
Definition: chainiksolvervel_wdls.hpp:180
This abstract class encapsulates the inverse velocity solver for a KDL::Chain.
Definition: chainiksolver.hpp:65
double lambda
Definition: chainiksolvervel_wdls.hpp:219