37 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H 38 #define KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H 40 #include "kdl/chainiksolver.hpp" 41 #include "kdl/chainjnttojacsolver.hpp" 42 #include "kdl/utilities/svd_HH.hpp" 43 #include "kdl/utilities/svd_eigen_HH.hpp" 58 class ChainIkSolverVel_pinv_mimic :
public ChainIkSolverVel
82 virtual int CartToJnt(
const JntArray& q_in,
const Twist& v_in, JntArray& qdot_out);
84 virtual int CartToJntRedundant(
const JntArray& q_in,
const Twist& v_in, JntArray& qdot_out);
103 bool setMimicJoints(
const std::vector<lma_kinematics_plugin::JointMimic>& _mimic_joints);
135 std::vector<JntArray>
U;
137 std::vector<JntArray>
V;
std::vector< lma_kinematics_plugin::JointMimic > mimic_joints_
Eigen::MatrixXd V_translate_locked
Eigen::MatrixXd U_translate_locked
Eigen::VectorXd S_translate_locked
unsigned int num_redundant_joints
Eigen::VectorXd tmp_translate
Eigen::VectorXd tmp_locked
bool jacToJacReduced(const Jacobian &jac, Jacobian &jac_mimic)
ChainJntToJacSolver jnt2jac
void lockRedundantJoints()
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
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 CartToJnt(const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
void unlockRedundantJoints()
std::vector< JntArray > V
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
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
Eigen::VectorXd S_translate
bool jacToJacLocked(const Jacobian &jac, Jacobian &jac_locked)
Eigen::MatrixXd U_translate
Eigen::VectorXd tmp_translate_locked
bool redundant_joints_locked