orocos_kdl
treeidsolver_recursive_newton_euler.cpp
Go to the documentation of this file.
1 // Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Franco Fusco <franco dot fusco at ls2n dot fr>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
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 
23 #include "frames_io.hpp"
24 #include <stdexcept>
25 
26 namespace KDL{
27 
29  tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments())
30  {
31  ag=-Twist(grav,Vector::Zero());
33  }
34 
36  nj = tree.getNrOfJoints();
39  }
40 
42  const SegmentMap& segments = tree.getSegments();
43  for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) {
44  X[seg->first] = Frame();
45  S[seg->first] = Twist();
46  v[seg->first] = Twist();
47  a[seg->first] = Twist();
48  f[seg->first] = Wrench();
49  }
50  }
51 
52  int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques)
53  {
54  //Check that the tree was not modified externally
55  if(nj != tree.getNrOfJoints() || ns != tree.getNrOfSegments())
56  return (error = E_NOT_UP_TO_DATE);
57 
58  //Check sizes of joint vectors
59  if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj)
60  return (error = E_SIZE_MISMATCH);
61 
62  try {
63  //Do the recursion here
64  rne_step(tree.getRootSegment(), q, q_dot, q_dotdot, f_ext, torques);
65  }
66  catch(const std::out_of_range&) {
67  //If in rne_step we get an out_of_range exception it means that some call
68  //to map::at failed. This can happen only if updateInternalDataStructures
69  //was not called after changing something in the tree. Note that it
70  //should be impossible to reach this point as consistency of the tree is
71  //checked above.
72  return (error = E_NOT_UP_TO_DATE);
73  }
74  return (error = E_NOERROR);
75  }
76 
77 
78  void TreeIdSolver_RNE::rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques) {
79  const Segment& seg = GetTreeElementSegment(segment->second);
80  const std::string& segname = segment->first;
81  const std::string& parname = GetTreeElementParent(segment->second)->first;
82 
83  //Do forward calculations involving velocity & acceleration of this segment
84  double q_, qdot_, qdotdot_;
85  unsigned int j = GetTreeElementQNr(segment->second);
86  if(seg.getJoint().getType()!=Joint::Fixed) {
87  q_ = q(j);
88  qdot_ = q_dot(j);
89  qdotdot_ = q_dotdot(j);
90  }
91  else
92  q_ = qdot_ = qdotdot_ = 0.0;
93 
94  //Calculate segment properties: X,S,vj,cj
95 
96  //Remark this is the inverse of the frame for transformations from the parent to the current coord frame
97  X.at(segname) = seg.pose(q_);
98 
99  //Transform velocity and unit velocity to segment frame
100  Twist vj = X.at(segname).M.Inverse( seg.twist(q_,qdot_) );
101  S.at(segname) = X.at(segname).M.Inverse( seg.twist(q_,1.0) );
102 
103  //calculate velocity and acceleration of the segment (in segment coordinates)
104  if(segment == tree.getRootSegment()) {
105  v.at(segname) = vj;
106  a.at(segname) = X.at(segname).Inverse(ag) + S.at(segname)*qdotdot_+ v.at(segname)*vj;
107  }
108  else {
109  v.at(segname) = X.at(segname).Inverse(v.at(parname)) + vj;
110  a.at(segname) = X.at(segname).Inverse(a.at(parname)) + S.at(segname)*qdotdot_ + v.at(segname)*vj;
111  }
112 
113  //Calculate the force for the joint
114  //Collect RigidBodyInertia and external forces
115  const RigidBodyInertia& I = seg.getInertia();
116  f.at(segname) = I*a.at(segname) + v.at(segname)*(I*v.at(segname));
117  if(f_ext.find(segname) != f_ext.end())
118  f.at(segname) = f.at(segname) - f_ext.at(segname);
119 
120  //propagate calculations over each child segment
121  SegmentMap::const_iterator child;
122  for (unsigned int i = 0; i < GetTreeElementChildren(segment->second).size(); i++) {
123  child = GetTreeElementChildren(segment->second)[i];
124  rne_step(child, q, q_dot, q_dotdot, f_ext, torques);
125  }
126 
127  //do backward calculations involving wrenches and joint efforts
128 
129  //If there is a moving joint, evaluate its effort
130  if(seg.getJoint().getType()!=Joint::Fixed) {
131  torques(j) = dot(S.at(segname), f.at(segname));
132  torques(j) += seg.getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia
133  }
134 
135  //add reaction forces to parent segment
136  if(segment != tree.getRootSegment())
137  f.at(parname) = f.at(parname) + X.at(segname)*f.at(segname);
138  }
139 }//namespace
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
std::string
KDL::TreeIdSolver_RNE::tree
const Tree & tree
Definition: treeidsolver_recursive_newton_euler.hpp:72
KDL::TreeIdSolver_RNE::ns
unsigned int ns
Definition: treeidsolver_recursive_newton_euler.hpp:74
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
KDL::TreeIdSolver_RNE::rne_step
void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
One recursion step.
Definition: treeidsolver_recursive_newton_euler.cpp:78
std::map::find
T find(T... args)
frames_io.hpp
KDL::TreeIdSolver_RNE::S
std::map< std::string, Twist > S
Definition: treeidsolver_recursive_newton_euler.hpp:76
KDL::TreeIdSolver_RNE::ag
Twist ag
Definition: treeidsolver_recursive_newton_euler.hpp:80
KDL::TreeIdSolver_RNE::TreeIdSolver_RNE
TreeIdSolver_RNE(const Tree &tree, Vector grav)
Definition: treeidsolver_recursive_newton_euler.cpp:28
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
KDL::TreeIdSolver_RNE::f
std::map< std::string, Wrench > f
Definition: treeidsolver_recursive_newton_euler.hpp:79
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
GetTreeElementParent
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
GetTreeElementChildren
#define GetTreeElementChildren(tree_element)
Definition: tree.hpp:59
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::TreeIdSolver_RNE::v
std::map< std::string, Twist > v
Definition: treeidsolver_recursive_newton_euler.hpp:77
stdexcept
KDL::TreeIdSolver_RNE::X
std::map< std::string, Frame > X
Definition: treeidsolver_recursive_newton_euler.hpp:75
KDL::TreeIdSolver_RNE::nj
unsigned int nj
Definition: treeidsolver_recursive_newton_euler.hpp:73
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Tree::getSegments
const SegmentMap & getSegments() const
Definition: tree.hpp:218
KDL::Tree::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: tree.hpp:168
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
std::map::at
T at(T... args)
dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
treeidsolver_recursive_newton_euler.hpp
KDL::TreeIdSolver_RNE::initAuxVariables
void initAuxVariables()
Helper function to initialize private members X, S, v, a, f.
Definition: treeidsolver_recursive_newton_euler.cpp:41
KDL::Joint::getInertia
const double & getInertia() const
Definition: joint.hpp:220
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
std::map< std::string, TreeElement >
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
KDL::Segment
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
Definition: segment.hpp:46
std::map::begin
T begin(T... args)
std::out_of_range
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
std::map::end
T end(T... args)
KDL::TreeIdSolver_RNE::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: treeidsolver_recursive_newton_euler.cpp:35
GetTreeElementSegment
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
KDL::TreeIdSolver_RNE::a
std::map< std::string, Twist > a
Definition: treeidsolver_recursive_newton_euler.hpp:78
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::Segment::getInertia
const RigidBodyInertia & getInertia() const
Definition: segment.hpp:128
KDL::TreeIdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
Definition: treeidsolver_recursive_newton_euler.cpp:52
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
GetTreeElementQNr
#define GetTreeElementQNr(tree_element)
Definition: tree.hpp:61