chainiksolverpos_nr.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "chainiksolverpos_nr.hpp"
23 
24 namespace KDL
25 {
27  unsigned int _maxiter, double _eps):
28  chain(_chain),nj (chain.getNrOfJoints()),
29  iksolver(_iksolver),fksolver(_fksolver),
30  delta_q(_chain.getNrOfJoints()),
31  maxiter(_maxiter),eps(_eps)
32  {
33  }
34 
39  delta_q.resize(nj);
40  }
41 
42  int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
43  {
44  if (nj != chain.getNrOfJoints())
45  return (error = E_NOT_UP_TO_DATE);
46 
47  if(q_init.rows() != nj || q_out.rows() != nj)
48  return (error = E_SIZE_MISMATCH);
49 
50  q_out = q_init;
51 
52  unsigned int i;
53  for(i=0;i<maxiter;i++){
54  if (E_NOERROR > fksolver.JntToCart(q_out,f) )
55  return (error = E_FKSOLVERPOS_FAILED);
56  delta_twist = diff(f,p_in);
57  const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
58  if (E_NOERROR > rc)
59  return (error = E_IKSOLVER_FAILED);
60  // we chose to continue if the child solver returned a positive
61  // "error", which may simply indicate a degraded solution
62  Add(q_out,delta_q,q_out);
64  // converged, but possibly with a degraded solution
65  return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
66  }
67  return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge
68  }
69 
71  {
72  }
73 
74  const char* ChainIkSolverPos_NR::strError(const int error) const
75  {
76  if (E_IKSOLVER_FAILED == error) return "Child IK solver failed";
77  else if (E_FKSOLVERPOS_FAILED == error) return "Child FK solver failed";
78  else return SolverI::strError(error);
79  }
80 }
81 
static Twist Zero()
Definition: frames.hpp:291
virtual const char * strError(const int error) const
unsigned int rows() const
Definition: jntarray.cpp:72
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
Chain size changed.
Definition: solveri.hpp:97
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
Maximum number of iterations exceeded.
Definition: solveri.hpp:101
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
Input size does not match internal state.
Definition: solveri.hpp:99
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:82
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)=0
Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)
Definition: solveri.hpp:89
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)=0
virtual void updateInternalDataStructures()=0
ChainIkSolverPos_NR(const Chain &chain, ChainFkSolverPos &fksolver, ChainIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
Child FK solver failed.
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
static const int E_FKSOLVERPOS_FAILED
Child IK solver vel failed.
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
virtual void updateInternalDataStructures()=0
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
static const int E_IKSOLVER_FAILED
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
virtual void updateInternalDataStructures()
This abstract class encapsulates the inverse velocity solver for a KDL::Chain.
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Chain...


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:43