/usr/include/kdl/chainiksolvervel_wdls.hpp is in liborocos-kdl-dev 1.3.1+dfsg-1.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
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 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 | // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
#define KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include <Eigen/Core>
namespace KDL
{
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the weighted pseudo inverse with damped least-square to calculate the velocity
* transformation from Cartesian to joint space of a general
* KDL::Chain. It uses a svd-calculation based on householders
* rotations.
*
* J# = M_q*Vb*pinv_dls(Db)*Ub'*M_x
*
* where B = Mx*J*Mq
*
* and B = Ub*Db*Vb' is the SVD decomposition of B
*
* Mq and Mx represent, respectively, the joint-space and task-space weighting
* matrices.
* Please refer to the documentation of setWeightJS(const Eigen::MatrixXd& Mq)
* and setWeightTS(const Eigen::MatrixXd& Mx) for details on the effects of
* these matrices.
*
* For more details on Weighted Pseudo Inverse, see :
* 1) [Ben Israel 03] A. Ben Israel & T.N.E. Greville.
* Generalized Inverses : Theory and Applications,
* second edition. Springer, 2003. ISBN 0-387-00293-6.
*
* 2) [Doty 93] K. L. Doty, C. Melchiorri & C. Boniveto.
* A theory of generalized inverses applied to Robotics.
* The International Journal of Robotics Research,
* vol. 12, no. 1, pages 1-19, february 1993.
*
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel_wdls : public ChainIkSolverVel
{
public:
static const int E_SVD_FAILED = -100; //! SVD solver failed
/// solution converged but (pseudo)inverse is singular
static const int E_CONVERGE_PINV_SINGULAR = +100;
/**
* Constructor of the solver
*
* @param chain the chain to calculate the inverse velocity
* kinematics for
* @param eps if a singular value is below this value, its
* inverse is set to zero, default: 0.00001
* @param maxiter maximum iterations for the svd calculation,
* default: 150
*
*/
explicit ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
//=ublas::identity_matrix<double>
~ChainIkSolverVel_wdls();
/**
* Find an output joint velocity \a qdot_out, given a starting joint pose
* \a q_init and a desired cartesian velocity \a v_in
*
* @return
* E_NOERROR=svd solution converged in maxiter
* E_SVD_FAILED=svd solution failed
* E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse singular
*
* @note if E_CONVERGE_PINV_SINGULAR returned then converged and can
* continue motion, but have degraded solution
*
* @note If E_SVD_FAILED returned, then getSvdResult() returns the error
* code from the SVD algorithm.
*/
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
/**
* not (yet) implemented.
*
*/
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
/**
* Set the joint space weighting matrix
*
* @param weight_js joint space weighting symetric matrix,
* default : identity. M_q : This matrix being used as a
* weight for the norm of the joint space speed it HAS TO BE
* symmetric and positive definite. We can actually deal with
* matrices containing a symmetric and positive definite block
* and 0s otherwise. Taking a diagonal matrix as an example, a
* 0 on the diagonal means that the corresponding joints will
* not contribute to the motion of the system. On the other
* hand, the bigger the value, the most the corresponding
* joint will contribute to the overall motion. The obtained
* solution q_dot will actually minimize the weighted norm
* sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal
* with, it does not make sense to invert M_q but what is
* important is the physical meaning of all this : a joint
* that has a zero weight in M_q will not contribute to the
* motion of the system and this is equivalent to saying that
* it gets an infinite weight in the norm computation. For
* more detailed explanation : vincent.padois@upmc.fr
*/
void setWeightJS(const Eigen::MatrixXd& Mq);
/**
* Set the task space weighting matrix
*
* @param weight_ts task space weighting symetric matrix,
* default: identity M_x : This matrix being used as a weight
* for the norm of the error (in terms of task space speed) it
* HAS TO BE symmetric and positive definite. We can actually
* deal with matrices containing a symmetric and positive
* definite block and 0s otherwise. Taking a diagonal matrix
* as an example, a 0 on the diagonal means that the
* corresponding task coordinate will not be taken into
* account (ie the corresponding error can be really big). If
* the rank of the jacobian is equal to the number of task
* space coordinates which do not have a 0 weight in M_x, the
* weighting will actually not impact the results (ie there is
* an exact solution to the velocity inverse kinematics
* problem). In cases without an exact solution, the bigger
* the value, the most the corresponding task coordinate will
* be taken into account (ie the more the corresponding error
* will be reduced). The obtained solution will minimize the
* weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|).
* For more detailed explanation : vincent.padois@upmc.fr
*/
void setWeightTS(const Eigen::MatrixXd& Mx);
/**
* Set lambda
*/
void setLambda(const double lambda);
/**
* Set eps
*/
void setEps(const double eps_in);
/**
* Set maxIter
*/
void setMaxIter(const int maxiter_in);
/**
* Request the number of singular values of the jacobian that are < eps;
* if the number of near zero singular values is > jac.col()-jac.row(),
* then the jacobian pseudoinverse is singular
*/
unsigned int getNrZeroSigmas()const {return nrZeroSigmas;};
/**
* Request the minimum of the first six singular values
*/
double getSigmaMin()const {return sigmaMin;};
/**
* Request the value of lambda for the minimum
*/
double getLambda()const {return lambda;};
/**
* Request the scaled value of lambda for the minimum
* singular value 1-6
*/
double getLambdaScaled()const {return lambda_scaled;};
/**
* Retrieve the latest return code from the SVD algorithm
* @return 0 if CartToJnt() not yet called, otherwise latest SVD result code.
*/
int getSVDResult()const {return svdResult;};
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
private:
const Chain chain;
ChainJntToJacSolver jnt2jac;
Jacobian jac;
Eigen::MatrixXd U;
Eigen::VectorXd S;
Eigen::MatrixXd V;
double eps;
int maxiter;
Eigen::VectorXd tmp;
Eigen::MatrixXd tmp_jac;
Eigen::MatrixXd tmp_jac_weight1;
Eigen::MatrixXd tmp_jac_weight2;
Eigen::MatrixXd tmp_ts;
Eigen::MatrixXd tmp_js;
Eigen::MatrixXd weight_ts;
Eigen::MatrixXd weight_js;
double lambda;
double lambda_scaled;
unsigned int nrZeroSigmas ;
int svdResult;
double sigmaMin;
};
}
#endif
|