31 nj(chain.getNrOfJoints()),
33 U(Eigen::MatrixXd::Zero(6,nj)),
34 S(Eigen::VectorXd::Zero(nj)),
35 V(Eigen::MatrixXd::Zero(nj,nj)),
38 tmp(Eigen::VectorXd::Zero(nj)),
39 tmp_jac(Eigen::MatrixXd::Zero(6,nj)),
40 tmp_jac_weight1(Eigen::MatrixXd::Zero(6,nj)),
41 tmp_jac_weight2(Eigen::MatrixXd::Zero(6,nj)),
42 tmp_ts(Eigen::MatrixXd::Zero(6,6)),
43 tmp_js(Eigen::MatrixXd::Zero(nj,nj)),
44 weight_ts(Eigen::MatrixXd::Identity(6,6)),
45 weight_js(Eigen::MatrixXd::Identity(nj,nj)),
58 Eigen::MatrixXd z6nj = Eigen::MatrixXd::Zero(6,
nj);
59 Eigen::VectorXd znj = Eigen::VectorXd::Zero(
nj);
60 Eigen::MatrixXd znjnj = Eigen::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);
69 weight_js.conservativeResizeLike(Eigen::MatrixXd::Identity(
nj,
nj));
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
unsigned int columns() 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.
Eigen::MatrixXd weight_js
unsigned int rows() const
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)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
represents both translational and rotational velocities.
virtual void updateInternalDataStructures()
unsigned int getNrOfJoints() const
virtual void updateInternalDataStructures()
Internal svd calculation failed.
Eigen::MatrixXd tmp_jac_weight2
int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon)
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
virtual const char * strError(const int error) const
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 nrZeroSigmas
void setMaxIter(const int maxiter_in)