36 double _eps,
bool _position_ik)
39 , q_min_mimic(chain.getNrOfJoints())
41 , q_max_mimic(chain.getNrOfJoints())
42 , q_temp(chain.getNrOfJoints())
45 , delta_q(_chain.getNrOfJoints())
48 , position_ik(_position_ik)
56 for (std::size_t i = 0; i <
q_min.
rows(); ++i)
67 ROS_ERROR_NAMED(
"kdl",
"Mimic Joint info should be same size as number of joints in chain: %d",
72 for (std::size_t i = 0; i < _mimic_joints.size(); ++i)
76 ROS_ERROR_NAMED(
"kdl",
"Mimic Joint index should be less than number of joints in chain: %d",
116 bool lock_redundant_joints)
123 for (std::size_t i = 0; i < q_out.
rows(); ++i)
144 for (std::size_t i = 0; i < 6; ++i)
156 for (std::size_t i = 0; i <
q_temp.
rows(); ++i)
159 for (std::size_t j = 0; j <
q_min.
rows(); ++j)
165 for (std::size_t j = 0; j <
q_max.
rows(); ++j)
181 for (std::size_t i = 0; i <
q_temp.
rows(); ++i)
185 for (std::size_t i = 0; i < q_out.
rows(); ++i)
IMETHOD doubleVel diff(const doubleVel &a, const doubleVel &b, double dt=1.0)
void qMimicToq(const JntArray &q, JntArray &q_result)
#define ROS_DEBUG_STREAM_NAMED(name, args)
unsigned int rows() const
ChainIkSolverPos_NR_JL_Mimic(const Chain &chain, const JntArray &q_min, const JntArray &q_max, ChainFkSolverPos &fksolver, ChainIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6, bool position_ik=false)
ChainIkSolverVel & iksolver
virtual int CartToJntAdvanced(const JntArray &q_init, const Frame &p_in, JntArray &q_out, bool lock_redundant_joints)
std::vector< kdl_kinematics_plugin::JointMimic > mimic_joints
void qToqMimic(const JntArray &q, JntArray &q_result)
void Add(const JntArrayVel &src1, const JntArrayVel &src2, JntArrayVel &dest)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)=0
#define ROS_DEBUG_NAMED(name,...)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)=0
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
bool setMimicJoints(const std::vector< kdl_kinematics_plugin::JointMimic > &mimic_joints)
unsigned int getNrOfJoints() const
ChainFkSolverPos & fksolver
#define ROS_ERROR_NAMED(name,...)
~ChainIkSolverPos_NR_JL_Mimic()