31 nj(chain.getNrOfJoints()),
33 U(MatrixXd::Zero(6,nj)),
34 S(VectorXd::Zero(nj)),
35 V(MatrixXd::Zero(nj,nj)),
38 tmp(VectorXd::Zero(nj)),
39 tmp_jac(MatrixXd::Zero(6,nj)),
40 tmp_jac_weight1(MatrixXd::Zero(6,nj)),
41 tmp_jac_weight2(MatrixXd::Zero(6,nj)),
42 tmp_ts(MatrixXd::Zero(6,6)),
43 tmp_js(MatrixXd::Zero(nj,nj)),
44 weight_ts(MatrixXd::Identity(6,6)),
45 weight_js(MatrixXd::Identity(nj,nj)),
58 MatrixXd z6nj = MatrixXd::Zero(6,
nj);
59 VectorXd znj = VectorXd::Zero(
nj);
60 MatrixXd znjnj = MatrixXd::Zero(
nj,
nj);
61 U.conservativeResizeLike(z6nj);
62 S.conservativeResizeLike(znj);
63 V.conservativeResizeLike(znjnj);
64 tmp.conservativeResizeLike(znj);
65 tmp_jac.conservativeResizeLike(z6nj);
68 tmp_js.conservativeResizeLike(znjnj);
110 if (Sout.size() !=
S.size())
149 qdot_out.
data.setZero() ;
170 sum+=
tmp_ts(j,i)*v_in(j);
ChainIkSolverVel_wdls(const Chain &chain, double eps=0.00001, int maxiter=150)
int setWeightJS(const Eigen::MatrixXd &Mq)
unsigned int rows() const
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
int setWeightTS(const Eigen::MatrixXd &Mx)
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
virtual const char * strError(const int error) const
This class represents an fixed size array containing joint values of a KDL::Chain.
Input size does not match internal state.
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
unsigned int columns() const
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
represents both translational and rotational velocities.
virtual void updateInternalDataStructures()
virtual void updateInternalDataStructures()
Internal svd calculation failed.
Eigen::MatrixXd tmp_jac_weight2
unsigned int getNrOfJoints() const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void setEps(const double eps_in)
ChainJntToJacSolver jnt2jac
int getSigma(Eigen::VectorXd &Sout)
void setLambda(const double lambda)
Eigen::MatrixXd weight_ts
Eigen::MatrixXd tmp_jac_weight1
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
int error
Latest error, initialized to E_NOERROR in constructor.
unsigned int rows() const
unsigned int nrZeroSigmas
void setMaxIter(const int maxiter_in)