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),nj (chain.getNrOfJoints()),
00029 iksolver(_iksolver),fksolver(_fksolver),
00030 delta_q(_chain.getNrOfJoints()),
00031 maxiter(_maxiter),eps(_eps)
00032 {
00033 }
00034
00035 void ChainIkSolverPos_NR::updateInternalDataStructures() {
00036 nj = chain.getNrOfJoints();
00037 iksolver.updateInternalDataStructures();
00038 fksolver.updateInternalDataStructures();
00039 delta_q.resize(nj);
00040 }
00041
00042 int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
00043 {
00044 if (nj != chain.getNrOfJoints())
00045 return (error = E_NOT_UP_TO_DATE);
00046
00047 if(q_init.rows() != nj || q_out.rows() != nj)
00048 return (error = E_SIZE_MISMATCH);
00049
00050 q_out = q_init;
00051
00052 unsigned int i;
00053 for(i=0;i<maxiter;i++){
00054 if (E_NOERROR > fksolver.JntToCart(q_out,f) )
00055 return (error = E_FKSOLVERPOS_FAILED);
00056 delta_twist = diff(f,p_in);
00057 const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
00058 if (E_NOERROR > rc)
00059 return (error = E_IKSOLVER_FAILED);
00060
00061
00062 Add(q_out,delta_q,q_out);
00063 if(Equal(delta_twist,Twist::Zero(),eps))
00064
00065 return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
00066 }
00067 return (error = E_MAX_ITERATIONS_EXCEEDED);
00068 }
00069
00070 ChainIkSolverPos_NR::~ChainIkSolverPos_NR()
00071 {
00072 }
00073
00074 const char* ChainIkSolverPos_NR::strError(const int error) const
00075 {
00076 if (E_IKSOLVER_FAILED == error) return "Child IK solver failed";
00077 else if (E_FKSOLVERPOS_FAILED == error) return "Child FK solver failed";
00078 else return SolverI::strError(error);
00079 }
00080 }
00081