Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "chainiksolverpos_nr.hpp"
00023
00024 namespace KDL
00025 {
00026 ChainIkSolverPos_NR::ChainIkSolverPos_NR(const Chain& _chain,ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver,
00027 unsigned int _maxiter, double _eps):
00028 chain(_chain),iksolver(_iksolver),fksolver(_fksolver),delta_q(_chain.getNrOfJoints()),
00029 maxiter(_maxiter),eps(_eps)
00030 {
00031 }
00032
00033 int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
00034 {
00035 q_out = q_init;
00036
00037 unsigned int i;
00038 for(i=0;i<maxiter;i++){
00039 fksolver.JntToCart(q_out,f);
00040 delta_twist = diff(f,p_in);
00041 const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
00042 if (E_NOERROR > rc)
00043 return (error = E_IKSOLVER_FAILED);
00044
00045
00046 Add(q_out,delta_q,q_out);
00047 if(Equal(delta_twist,Twist::Zero(),eps))
00048
00049 return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
00050 }
00051 return (error = E_NO_CONVERGE);
00052 }
00053
00054 ChainIkSolverPos_NR::~ChainIkSolverPos_NR()
00055 {
00056 }
00057
00058 const char* ChainIkSolverPos_NR::strError(const int error) const
00059 {
00060 if (E_IKSOLVER_FAILED == error) return "Child IK solver failed";
00061 else return SolverI::strError(error);
00062 }
00063 }
00064