22 #ifndef KDL_CHAIN_IKSOLVERVEL_WDLS_HPP 23 #define KDL_CHAIN_IKSOLVERVEL_WDLS_HPP 167 void setEps(
const double eps_in);
188 int getSigma(Eigen::VectorXd& Sout);
ChainIkSolverVel_wdls(const Chain &chain, double eps=0.00001, int maxiter=150)
double getLambdaScaled() const
int setWeightJS(const Eigen::MatrixXd &Mq)
int setWeightTS(const Eigen::MatrixXd &Mx)
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
This class represents an fixed size array containing joint values of a KDL::Chain.
virtual int CartToJnt(const JntArray &, const FrameVel &, JntArrayVel &)
virtual const char * strError(const int error) const
Eigen::MatrixXd weight_js
static const int E_CONVERGE_PINV_SINGULAR
solution converged but (pseudo)inverse is singular
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
represents both translational and rotational velocities.
virtual void updateInternalDataStructures()
Eigen::MatrixXd tmp_jac_weight2
void setEps(const double eps_in)
ChainJntToJacSolver jnt2jac
int getSigma(Eigen::VectorXd &Sout)
void setLambda(const double lambda)
Eigen::MatrixXd weight_ts
unsigned int getNrZeroSigmas() const
Eigen::MatrixXd tmp_jac_weight1
double getSigmaMin() const
int error
Latest error, initialized to E_NOERROR in constructor.
unsigned int nrZeroSigmas
void setMaxIter(const int maxiter_in)
This abstract class encapsulates the inverse velocity solver for a KDL::Chain.