28 const std::vector<std::string>& _endpoints,
31 unsigned int _maxiter,
double _eps) :
32 tree(_tree), q_min(_q_min), q_max(_q_max), iksolver(_iksolver),
33 fksolver(_fksolver), delta_q(tree.getNrOfJoints()),
34 endpoints(_endpoints), maxiter(_maxiter), eps(_eps)
36 for (
size_t i = 0; i <
endpoints.size(); i++) {
46 for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
52 for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it){
54 Frames::iterator f_it =
frames.find(f_des_it->first);
55 Twists::iterator delta_twist =
delta_twists.find(f_des_it->first);
58 delta_twist->second =
diff(f_it->second, f_des_it->second);
61 if (res <
eps)
return res;
65 for (
unsigned int j = 0; j <
q_min.
rows(); j++) {
66 if (q_out(j) <
q_min(j))
68 else if (q_out(j) >
q_max(j))
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)=0
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Tree...
unsigned int rows() const
TreeIkSolverPos_NR_JL(const Tree &tree, const std::vector< std::string > &endpoints, const JntArray &q_min, const JntArray &q_max, TreeFkSolverPos &fksolver, TreeIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
This class represents an fixed size array containing joint values of a KDL::Chain.
std::vector< std::string > endpoints
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
TreeFkSolverPos & fksolver
TreeIkSolverVel & iksolver
virtual double CartToJnt(const JntArray &q_init, const Frames &p_in, JntArray &q_out)
std::map< std::string, Frame > Frames
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)=0