KDL  1.4.0
chainidsolver_recursive_newton_euler.hpp
Go to the documentation of this file.
1 // Copyright (C) 2009 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 KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP
23 #define KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP
24 
25 #include "chainidsolver.hpp"
26 
27 namespace KDL{
41  public:
47  ChainIdSolver_RNE(const Chain& chain,Vector grav);
49 
60  int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques);
61 
63  virtual void updateInternalDataStructures();
64 
65  private:
66  const Chain& chain;
67  unsigned int nj;
68  unsigned int ns;
69  std::vector<Frame> X;
70  std::vector<Twist> S;
71  std::vector<Twist> v;
72  std::vector<Twist> a;
73  std::vector<Wrench> f;
75  };
76 }
77 
78 #endif
KDL::ChainIdSolver_RNE::S
std::vector< Twist > S
Definition: chainidsolver_recursive_newton_euler.hpp:70
KDL::ChainIdSolver_RNE
Recursive newton euler inverse dynamics solver.
Definition: chainidsolver_recursive_newton_euler.hpp:40
KDL::JntArray
Definition: jntarray.hpp:70
KDL::ChainIdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
Function to calculate from Cartesian forces to joint torques.
Definition: chainidsolver_recursive_newton_euler.cpp:44
KDL::ChainIdSolver_RNE::ag
Twist ag
Definition: chainidsolver_recursive_newton_euler.hpp:74
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Segment::getJoint
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
frames_io.hpp
KDL::ChainIdSolver_RNE::f
std::vector< Wrench > f
Definition: chainidsolver_recursive_newton_euler.hpp:73
chainidsolver_recursive_newton_euler.hpp
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
KDL::ChainIdSolver_RNE::X
std::vector< Frame > X
Definition: chainidsolver_recursive_newton_euler.hpp:69
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::ChainIdSolver_RNE::chain
const Chain & chain
Definition: chainidsolver_recursive_newton_euler.hpp:66
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::Segment::pose
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
chainidsolver.hpp
KDL::ChainIdSolver_RNE::a
std::vector< Twist > a
Definition: chainidsolver_recursive_newton_euler.hpp:72
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Request the 6D-velocity of the tip of the segment, given the joint position q and the joint velocity ...
Definition: segment.cpp:62
KDL::Wrenches
std::vector< Wrench > Wrenches
Definition: chainidsolver.hpp:33
KDL::Joint::None
@ None
Definition: joint.hpp:47
KDL::ChainIdSolver_RNE::ChainIdSolver_RNE
ChainIdSolver_RNE(const Chain &chain, Vector grav)
Constructor for the solver, it will allocate all the necessary memory.
Definition: chainidsolver_recursive_newton_euler.cpp:27
KDL::Joint::getType
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
KDL::ChainIdSolver_RNE::nj
unsigned int nj
Definition: chainidsolver_recursive_newton_euler.hpp:67
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::ChainIdSolver
This abstract class encapsulates the inverse dynamics solver for a KDL::Chain.
Definition: chainidsolver.hpp:41
KDL::ChainIdSolver_RNE::v
std::vector< Twist > v
Definition: chainidsolver_recursive_newton_euler.hpp:71
KDL::Chain
Definition: chain.hpp:35
KDL::Segment::getInertia
const RigidBodyInertia & getInertia() const
Request the inertia of the segment.
Definition: segment.hpp:128
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
KDL::ChainIdSolver_RNE::ns
unsigned int ns
Definition: chainidsolver_recursive_newton_euler.hpp:68
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainIdSolver_RNE::~ChainIdSolver_RNE
~ChainIdSolver_RNE()
Definition: chainidsolver_recursive_newton_euler.hpp:48
KDL::ChainIdSolver_RNE::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainidsolver_recursive_newton_euler.cpp:34