45 double _eps,
bool _position_ik)
48 , q_min_mimic(chain.getNrOfJoints())
50 , q_max_mimic(chain.getNrOfJoints())
51 , q_temp(chain.getNrOfJoints())
54 , delta_q(_chain.getNrOfJoints())
57 , position_ik(_position_ik)
65 for (std::size_t i = 0; i <
q_min.
rows(); ++i)
76 ROS_ERROR_NAMED(
"lma",
"Mimic Joint info should be same size as number of joints in chain: %d",
81 for (std::size_t i = 0; i < _mimic_joints.size(); ++i)
85 ROS_ERROR_NAMED(
"lma",
"Mimic Joint index should be less than number of joints in chain: %d",
129 while (q_out(i) > 2 *
M_PI)
130 q_out(i) -= 2 *
M_PI;
132 while (q_out(i) < -2 *
M_PI)
133 q_out(i) += 2 *
M_PI;
139 bool obeys_limits =
true;
142 if ((q_out(i) < (
q_min(i) - 0.0001)) || (q_out(i) > (
q_max(i) + 0.0001)))
145 obeys_limits =
false;
147 <<
" to " <<
q_max(i));
155 bool lock_redundant_joints)
std::vector< lma_kinematics_plugin::JointMimic > mimic_joints
#define ROS_DEBUG_STREAM_NAMED(name, args)
unsigned int rows() const
void harmonize(JntArray &q_out)
bool obeysLimits(const KDL::JntArray &q_out)
ChainIkSolverPos_LMA_JL_Mimic(const Chain &chain, const JntArray &q_min, const JntArray &q_max, ChainFkSolverPos &fksolver, ChainIkSolverPos_LMA &iksolver, unsigned int maxiter=100, double eps=1e-6, bool position_ik=false)
virtual int CartToJntAdvanced(const JntArray &q_init, const Frame &p_in, JntArray &q_out, bool lock_redundant_joints)
#define ROS_DEBUG_NAMED(name,...)
void qMimicToq(const JntArray &q, JntArray &q_result)
bool setMimicJoints(const std::vector< lma_kinematics_plugin::JointMimic > &mimic_joints)
void qToqMimic(const JntArray &q, JntArray &q_result)
unsigned int getNrOfJoints() const
#define ROS_ERROR_NAMED(name,...)
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
~ChainIkSolverPos_LMA_JL_Mimic()
ChainIkSolverPos_LMA & iksolver
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)