32 int _num_redundant_joints,
bool _position_ik,
double _eps,
36 , jac(chain.getNrOfJoints())
37 , U(6,
JntArray(chain.getNrOfJoints() - _num_mimic_joints))
38 , S(chain.getNrOfJoints() - _num_mimic_joints)
39 , V(chain.getNrOfJoints() - _num_mimic_joints,
JntArray(chain.getNrOfJoints() - _num_mimic_joints))
40 , tmp(chain.getNrOfJoints() - _num_mimic_joints)
41 , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints)
42 , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints)
43 , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints))
44 , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
45 , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints))
46 , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
47 , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints)
48 , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)
49 , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints)
53 , num_mimic_joints(_num_mimic_joints)
54 , position_ik(_position_ik)
55 , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
56 , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
57 , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
58 chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
59 , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
60 , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
61 , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
62 , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
63 chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
64 , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
65 , num_redundant_joints(_num_redundant_joints)
66 , redundant_joints_locked(false)
82 for (std::size_t i = 0; i < mimic_joints.size(); ++i)
92 const std::vector<unsigned int>& redundant_joints_map_index)
96 ROS_ERROR(
"Map index size: %d does not match expected size. " 97 "No. of joints: %d, num_mimic_joints: %d, num_redundant_joints: %d",
103 for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
114 jac_reduced_l.
data.setZero();
127 jac_locked.
data.setZero();
137 qdot_out.
data.setZero();
181 for (j = 0; j < rows; j++)
283 for (j = 0; j < rows; j++)
286 sum +=
U[j](i) * v_in(j);
293 tmp(i) = sum * (fabs(
S(i)) <
eps ? 0.0 : 1.0 /
S(i));
305 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)
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
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
unsigned int rows() const
bool redundant_joints_locked