chainiksolverpos_nr_jl.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 // Copyright (C) 2008 Mikael Mayer
3 // Copyright (C) 2008 Julia Jesse
4 
5 // Version: 1.0
6 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
7 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
25 
26 #include <limits>
27 
28 namespace KDL
29 {
30  ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver,
31  unsigned int _maxiter, double _eps):
32  chain(_chain), nj(chain.getNrOfJoints()),
33  q_min(_q_min), q_max(_q_max),
34  iksolver(_iksolver), fksolver(_fksolver),
35  delta_q(_chain.getNrOfJoints()),
36  maxiter(_maxiter),eps(_eps)
37  {
38 
39  }
40 
42  unsigned int _maxiter, double _eps):
43  chain(_chain), nj(chain.getNrOfJoints()),
44  q_min(nj), q_max(nj),
45  iksolver(_iksolver), fksolver(_fksolver),
46  delta_q(nj),
47  maxiter(_maxiter),eps(_eps)
48  {
51  }
52 
55  q_min.data.conservativeResizeLike(Eigen::VectorXd::Constant(nj,std::numeric_limits<double>::min()));
56  q_max.data.conservativeResizeLike(Eigen::VectorXd::Constant(nj,std::numeric_limits<double>::max()));
59  delta_q.resize(nj);
60  }
61 
62  int ChainIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
63  {
64  if(nj != chain.getNrOfJoints())
65  return (error = E_NOT_UP_TO_DATE);
66 
67  if(nj != q_init.rows() || nj != q_out.rows() || nj != q_min.rows() || nj != q_max.rows())
68  return (error = E_SIZE_MISMATCH);
69 
70  q_out = q_init;
71 
72  unsigned int i;
73  for(i=0;i<maxiter;i++){
74  if ( fksolver.JntToCart(q_out,f) < 0)
75  return (error = E_FKSOLVERPOS_FAILED);
76  delta_twist = diff(f,p_in);
77 
79  break;
80 
81  if ( iksolver.CartToJnt(q_out,delta_twist,delta_q) < 0)
82  return (error = E_IKSOLVERVEL_FAILED);
83  Add(q_out,delta_q,q_out);
84 
85  for(unsigned int j=0; j<q_min.rows(); j++) {
86  if(q_out(j) < q_min(j))
87  q_out(j) = q_min(j);
88  }
89 
90 
91  for(unsigned int j=0; j<q_max.rows(); j++) {
92  if(q_out(j) > q_max(j))
93  q_out(j) = q_max(j);
94  }
95  }
96 
97  if(i!=maxiter)
98  return (error = E_NOERROR);
99  else
100  return (error = E_MAX_ITERATIONS_EXCEEDED);
101  }
102 
103  int ChainIkSolverPos_NR_JL::setJointLimits(const JntArray& q_min_in, const JntArray& q_max_in) {
104  if (q_min_in.rows() != nj || q_max_in.rows() != nj)
105  return (error = E_SIZE_MISMATCH);
106  q_min = q_min_in;
107  q_max = q_max_in;
108  return (error = E_NOERROR);
109  }
110 
112  {
113  }
114 
115  const char* ChainIkSolverPos_NR_JL::strError(const int error) const
116  {
117  if (E_FKSOLVERPOS_FAILED == error) return "Internal forward position solver failed.";
118  else if (E_IKSOLVERVEL_FAILED == error) return "Internal inverse velocity solver failed.";
119  else return SolverI::strError(error);
120  }
121 }
122 
static Twist Zero()
Definition: frames.hpp:291
unsigned int rows() const
Definition: jntarray.cpp:72
int setJointLimits(const JntArray &q_min, const JntArray &q_max)
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
Chain size changed.
Definition: solveri.hpp:97
ChainIkSolverPos_NR_JL(const Chain &chain, const JntArray &q_min, const JntArray &q_max, ChainFkSolverPos &fksolver, ChainIkSolverVel &iksolver, unsigned int maxiter=100, double eps=1e-6)
Child FK solver failed.
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
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
Eigen::VectorXd data
Definition: jntarray.hpp:72
static const int E_FKSOLVERPOS_FAILED
Child IK solver vel failed.
double min(double a, double b)
Definition: utility.h:212
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
void resize(unsigned int newSize)
Definition: jntarray.cpp:55
const char * strError(const int error) const
virtual void updateInternalDataStructures()=0
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
double max(double a, double b)
Definition: utility.h:204
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
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