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 int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
00036 {
00037 if(q_init.rows() != nj || q_out.rows() != nj)
00038 return (error = E_SIZE_MISMATCH);
00039
00040 q_out = q_init;
00041
00042 unsigned int i;
00043 for(i=0;i<maxiter;i++){
00044 if (E_NOERROR > fksolver.JntToCart(q_out,f) )
00045 return (error = E_FKSOLVERPOS_FAILED);
00046 delta_twist = diff(f,p_in);
00047 const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
00048 if (E_NOERROR > rc)
00049 return (error = E_IKSOLVER_FAILED);
00050
00051
00052 Add(q_out,delta_q,q_out);
00053 if(Equal(delta_twist,Twist::Zero(),eps))
00054
00055 return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
00056 }
00057 return (error = E_MAX_ITERATIONS_EXCEEDED);
00058 }
00059
00060 ChainIkSolverPos_NR::~ChainIkSolverPos_NR()
00061 {
00062 }
00063
00064 const char* ChainIkSolverPos_NR::strError(const int error) const
00065 {
00066 if (E_IKSOLVER_FAILED == error) return "Child IK solver failed";
00067 else if (E_FKSOLVERPOS_FAILED == error) return "Child FK solver failed";
00068 else return SolverI::strError(error);
00069 }
00070 }
00071