KDL 1.5.1
Loading...
Searching...
No Matches
chainiksolverpos_nr.hpp
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#ifndef KDLCHAINIKSOLVERPOS_NR_HPP
23#define KDLCHAINIKSOLVERPOS_NR_HPP
24
25#include "chainiksolver.hpp"
26#include "chainfksolver.hpp"
27
28namespace KDL {
29
39 {
40 public:
41 static const int E_IKSOLVER_FAILED = -100;
42 static const int E_FKSOLVERPOS_FAILED = -101;
43
60 unsigned int maxiter=100,double eps=1e-6);
62
74 virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
75
77 virtual const char* strError(const int error) const;
78
80 virtual void updateInternalDataStructures();
81 private:
82 const Chain& chain;
83
84 unsigned int nj;
90
91 unsigned int maxiter;
92 double eps;
93 };
94
95}
96
97#endif
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Chain.
Definition: chainfksolver.hpp:41
Implementation of a general inverse position kinematics algorithm based on Newton-Raphson iterations ...
Definition: chainiksolverpos_nr.hpp:39
unsigned int maxiter
Definition: chainiksolverpos_nr.hpp:91
ChainIkSolverVel & iksolver
Definition: chainiksolverpos_nr.hpp:85
static const int E_FKSOLVERPOS_FAILED
Child IK solver vel failed.
Definition: chainiksolverpos_nr.hpp:42
unsigned int nj
Definition: chainiksolverpos_nr.hpp:84
double eps
Definition: chainiksolverpos_nr.hpp:92
JntArray delta_q
Definition: chainiksolverpos_nr.hpp:87
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
Find an output joint pose q_out, given a starting joint pose q_init and a desired cartesian pose p_in...
Definition: chainiksolverpos_nr.cpp:42
~ChainIkSolverPos_NR()
Definition: chainiksolverpos_nr.cpp:70
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainiksolverpos_nr.cpp:35
Twist delta_twist
Definition: chainiksolverpos_nr.hpp:89
Frame f
Definition: chainiksolverpos_nr.hpp:88
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: chainiksolverpos_nr.cpp:74
static const int E_IKSOLVER_FAILED
Definition: chainiksolverpos_nr.hpp:41
const Chain & chain
Definition: chainiksolverpos_nr.hpp:82
ChainFkSolverPos & fksolver
Definition: chainiksolverpos_nr.hpp:86
Definition: chainiksolver.hpp:42
Definition: chainiksolver.hpp:66
Definition: chain.hpp:35
Definition: frames.hpp:570
Definition: jntarray.hpp:70
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
represents both translational and rotational velocities.
Definition: frames.hpp:720
Definition: articulatedbodyinertia.cpp:26