30 nj(chain.getNrOfJoints()),
32 U(MatrixXd::Zero(6,nj)),
33 S(VectorXd::Zero(nj)),
34 Sinv(VectorXd::Zero(nj)),
35 V(MatrixXd::Zero(nj,nj)),
36 tmp(VectorXd::Zero(nj)),
37 tmp2(VectorXd::Zero(nj)),
52 U(MatrixXd::Zero(6,
nj)),
53 S(VectorXd::Zero(
nj)),
55 V(MatrixXd::Zero(
nj,
nj)),
56 tmp(VectorXd::Zero(
nj)),
69 U.conservativeResizeLike(MatrixXd::Zero(6,
nj));
70 S.conservativeResizeLike(VectorXd::Zero(
nj));
71 Sinv.conservativeResizeLike(VectorXd::Zero(
nj));
72 V.conservativeResizeLike(MatrixXd::Zero(
nj,
nj));
73 tmp.conservativeResizeLike(VectorXd::Zero(
nj));
74 tmp2.conservativeResizeLike(VectorXd::Zero(
nj));
102 qdot_out.
data.setZero() ;
113 for (i = 0; i <
nj; ++i) {
114 Sinv(i) = fabs(
S(i))<
eps ? 0.0 : 1.0/
S(i);
116 for (i = 0; i < 6; ++i) {
120 qdot_out.
data =
V *
Sinv.asDiagonal() *
U.transpose() *
tmp.head(6);
142 for (i = 0; i <
nj; ++i) {
143 double qd = q_in(i) -
opt_pos(i);
150 for (i = 0; i <
nj; ++i) {
155 tmp2 =
V *
Sinv.asDiagonal() *
U.transpose() *
U *
S.asDiagonal() *
V.transpose() *
tmp;
157 for (i = 0; i <
nj; ++i) {
168 if (
nj != _weights.
rows())
176 if (
nj != _opt_pos.
rows())
unsigned int rows() const
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
~ChainIkSolverVel_pinv_nso()
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
This class represents an fixed size array containing joint values of a KDL::Chain.
Input size does not match internal state.
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
ChainIkSolverVel_pinv_nso(const Chain &chain, const JntArray &opt_pos, const JntArray &weights, double eps=0.00001, int maxiter=150, double alpha=0.25)
represents both translational and rotational velocities.
virtual void updateInternalDataStructures()
virtual int setAlpha(const double alpha)
Internal svd calculation failed.
virtual int setOptPos(const JntArray &opt_pos)
unsigned int getNrOfJoints() const
virtual int setWeights(const JntArray &weights)
ChainJntToJacSolver jnt2jac
virtual void updateInternalDataStructures()
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
int error
Latest error, initialized to E_NOERROR in constructor.