29 nj(chain.getNrOfJoints()),
32 transpose(nj>6),toggle(true),
36 U(MatrixXd::Identity(m,m)),
37 V(MatrixXd::Identity(n,n)),
41 UY(VectorXd::Zero(6)),
42 SUY(VectorXd::Zero(nj)),
56 U.conservativeResizeLike(MatrixXd::Identity(
m,
m));
57 V.conservativeResizeLike(MatrixXd::Identity(
n,
n));
58 B.conservativeResize(
m,
n);
59 S.conservativeResize(
n);
60 tempi.conservativeResize(
m);
61 SUY.conservativeResizeLike(VectorXd::Zero(
nj));
84 for(
unsigned int i=0;i<6;i++)
87 for(
unsigned int i=0;i<
m;i++){
88 for(
unsigned int j=0;j<
n;j++)
101 for (
unsigned int i = 0; i <
n; i++){
int svd_eigen_Macie(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::MatrixXd &B, Eigen::VectorXd &tempi, double threshold, bool toggle)
unsigned int rows() const
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
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.
~ChainIkSolverVel_pinv_givens()
ChainIkSolverVel_pinv_givens(const Chain &chain)
Input size does not match internal state.
represents both translational and rotational velocities.
virtual void updateInternalDataStructures()
double min(double a, double b)
unsigned int getNrOfJoints() const
ChainJntToJacSolver jnt2jac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
int error
Latest error, initialized to E_NOERROR in constructor.
double max(double a, double b)
virtual void updateInternalDataStructures()
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)