43 int _num_redundant_joints,
bool _position_ik,
double _eps,
47 , jac(chain.getNrOfJoints())
48 , U(6, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
49 , S(chain.getNrOfJoints() - _num_mimic_joints)
50 , V(chain.getNrOfJoints() - _num_mimic_joints, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
51 , tmp(chain.getNrOfJoints() - _num_mimic_joints)
52 , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints)
53 , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints)
54 , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints))
55 , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
56 , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints))
57 , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
58 , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints)
59 , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)
60 , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints)
64 , num_mimic_joints(_num_mimic_joints)
65 , position_ik(_position_ik)
66 , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
67 , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
68 , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
69 chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
70 , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
71 , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
72 , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
73 , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
74 chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
75 , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
76 , num_redundant_joints(_num_redundant_joints)
77 , redundant_joints_locked(false)
93 for (std::size_t i = 0; i < mimic_joints.size(); ++i)
103 const std::vector<unsigned int>& redundant_joints_map_index)
107 ROS_ERROR(
"Map index size: %d does not match expected size. " 108 "No. of joints: %d, num_mimic_joints: %d, num_redundant_joints: %d",
114 for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
125 jac_reduced_l.
data.setZero();
138 jac_locked.
data.setZero();
148 qdot_out.
data.setZero();
184 rows = jac_locked.
rows();
189 for (i = 0; i < jac_locked.
columns(); i++)
192 for (j = 0; j < rows; j++)
208 for (i = 0; i < jac_locked.
columns(); i++)
211 for (j = 0; j < jac_locked.
columns(); j++)
294 for (j = 0; j < rows; j++)
297 sum +=
U[j](i) * v_in(j);
304 tmp(i) = sum * (fabs(
S(i)) <
eps ? 0.0 : 1.0 /
S(i));
316 sum +=
V[i](j) *
tmp(j);
Eigen::MatrixXd V_translate_locked
Eigen::MatrixXd U_translate_locked
#define ROS_DEBUG_STREAM_NAMED(name, args)
Eigen::VectorXd S_translate_locked
unsigned int num_redundant_joints
Eigen::VectorXd tmp_translate
Eigen::VectorXd tmp_locked
std::vector< kdl_kinematics_plugin::JointMimic > mimic_joints_
bool jacToJacReduced(const Jacobian &jac, Jacobian &jac_mimic)
ChainJntToJacSolver jnt2jac
std::vector< unsigned int > locked_joints_map_index
bool setRedundantJointsMapIndex(const std::vector< unsigned int > &redundant_joints_map_index)
Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i...
Eigen::MatrixXd V_translate
unsigned int columns() const
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_mimic(const Chain &chain, int num_mimic_joints=0, int num_redundant_joints=0, bool position_ik=false, double eps=0.00001, int maxiter=150)
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
std::vector< JntArray > V
void setColumn(unsigned int i, const Twist &t)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
unsigned int getNrOfJoints() const
Twist getColumn(unsigned int i) const
virtual int CartToJntRedundant(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
std::vector< JntArray > U
~ChainIkSolverVel_pinv_mimic()
bool setMimicJoints(const std::vector< kdl_kinematics_plugin::JointMimic > &_mimic_joints)
Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in ...
JntArray qdot_out_reduced_locked
JntArray qdot_out_reduced
int calculate(const Jacobian &jac, std::vector< JntArray > &U, JntArray &w, std::vector< JntArray > &v, int maxiter)
Eigen::VectorXd S_translate
bool jacToJacLocked(const Jacobian &jac, Jacobian &jac_locked)
Eigen::MatrixXd U_translate
Eigen::VectorXd tmp_translate_locked
unsigned int rows() const
bool redundant_joints_locked