orocos_kdl
treeiksolverpos_online.cpp
Go to the documentation of this file.
1 // Copyright (C) 2011 PAL Robotics S.L. All rights reserved.
2 // Copyright (C) 2007-2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
3 // Copyright (C) 2008 Mikael Mayer
4 // Copyright (C) 2008 Julia Jesse
5 
6 // Version: 1.0
7 // Author: Marcus Liebhardt
8 // This class has been derived from the KDL::TreeIkSolverPos_NR_JL class
9 // by Julia Jesse, Mikael Mayer and Ruben Smits
10 
11 // This library is free software; you can redistribute it and/or
12 // modify it under the terms of the GNU Lesser General Public
13 // License as published by the Free Software Foundation; either
14 // version 2.1 of the License, or (at your option) any later version.
15 
16 // This library is distributed in the hope that it will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 // Lesser General Public License for more details.
20 
21 // You should have received a copy of the GNU Lesser General Public
22 // License along with this library; if not, write to the Free Software
23 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
24 
26 
27 namespace KDL {
28 
30  const std::vector<std::string>& endpoints,
31  const JntArray& q_min,
32  const JntArray& q_max,
33  const JntArray& q_dot_max,
34  const double x_dot_trans_max,
35  const double x_dot_rot_max,
36  TreeFkSolverPos& fksolver,
37  TreeIkSolverVel& iksolver) :
38  fksolver_(fksolver),
39  iksolver_(iksolver),
40  q_dot_(static_cast<unsigned int>(nr_of_jnts))
41 {
42  q_min_ = q_min;
43  q_max_ = q_max;
44  q_dot_max_ = q_dot_max;
45  x_dot_trans_max_ = x_dot_trans_max;
46  x_dot_rot_max_ = x_dot_rot_max;
47 
48  for (size_t i = 0; i < endpoints.size(); i++)
49  {
50  frames_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
51  delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
52  }
53 }
54 
55 
57 {}
58 
59 
60 double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
61 {
62  q_out = q_in;
63 
64  // First check, if all elements in p_in are available
65  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
66  if(frames_.find(f_des_it->first)==frames_.end())
67  return -2;
68 
69  for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
70  {
71  // Get all iterators for this endpoint
72  Frames::iterator f_it = frames_.find(f_des_it->first);
73  Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
74 
75  fksolver_.JntToCart(q_out, f_it->second, f_it->first);
76  twist_ = diff(f_it->second, f_des_it->second);
77 
78  // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
79  // And scales them, if necessary
81 
82  delta_twists_it->second = twist_;
83  }
84 
85  double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
86 
87  if(res<0)
88  return res;
89  //If we got here q_out is definitely of the right size
90  if(q_out.rows()!=q_min_.rows() || q_out.rows()!=q_max_.rows() || q_out.rows()!= q_dot_max_.rows())
91  return -1;
92 
93  // Checks, if joint velocities (q_dot_) exceed their maximum and scales them, if necessary
95 
96  // Integrate
97  Add(q_out, q_dot_, q_out);
98 
99  // Limit joint positions
100  for (unsigned int j = 0; j < q_min_.rows(); j++)
101  {
102  if (q_out(j) < q_min_(j))
103  q_out(j) = q_min_(j);
104  else if (q_out(j) > q_max_(j))
105  q_out(j) = q_max_(j);
106  }
107 
108  return res;
109 }
110 
111 
113 {
114  // check, if one (or more) joint velocities exceed the maximum value
115  // and if so, safe the biggest overshoot for scaling q_dot_ properly
116  // to keep the direction of the velocity vector the same
117  double rel_os, rel_os_max = 0.0; // relative overshoot and the biggest relative overshoot
118  bool max_exceeded = false;
119 
120  for (unsigned int i = 0; i < q_dot_.rows(); i++)
121  {
122  if ( q_dot_(i) > q_dot_max_(i) )
123  {
124  max_exceeded = true;
125  rel_os = (q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
126  if ( rel_os > rel_os_max )
127  {
128  rel_os_max = rel_os;
129  }
130  }
131  else if ( q_dot_(i) < -q_dot_max_(i) )
132  {
133  max_exceeded = true;
134  rel_os = (-q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
135  if ( rel_os > rel_os_max)
136  {
137  rel_os_max = rel_os;
138  }
139  }
140  }
141 
142  // scales q_out, if one joint exceeds the maximum value
143  if ( max_exceeded == true )
144  {
145  Multiply(q_dot_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_dot_);
146  }
147 }
148 
149 
151 {
152  double x_dot_trans, x_dot_rot; // vector lengths
153  x_dot_trans = sqrt( pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2));
154  x_dot_rot = sqrt( pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2));
155 
156  if ( x_dot_trans > x_dot_trans_max_ || x_dot_rot > x_dot_rot_max_ )
157  {
158  if ( x_dot_trans > x_dot_rot )
159  {
160  twist_.vel = twist_.vel * ( x_dot_trans_max_ / x_dot_trans );
161  twist_.rot = twist_.rot * ( x_dot_trans_max_ / x_dot_trans );
162  }
163  else if ( x_dot_rot > x_dot_trans )
164  {
165  twist_.vel = twist_.vel * ( x_dot_rot_max_ / x_dot_rot );
166  twist_.rot = twist_.rot * ( x_dot_rot_max_ / x_dot_rot );
167  }
168  }
169 }
170 
171 } // namespace
172 
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::pow
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:383
KDL::Vector::y
double y() const
Definition: frames.inl:76
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::JntArray
Definition: jntarray.hpp:69
KDL::TreeIkSolverPos_Online::CartToJnt
virtual double CartToJnt(const JntArray &q_in, const Frames &p_in, JntArray &q_out)
Definition: treeiksolverpos_online.cpp:60
KDL::TreeFkSolverPos
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Tree.
Definition: treefksolver.hpp:45
std::vector< std::string >
std::map::find
T find(T... args)
std::vector::size
T size(T... args)
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
KDL::Add
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:80
KDL::TreeIkSolverVel
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
Definition: treeiksolver.hpp:54
KDL::TreeIkSolverPos_Online::q_max_
JntArray q_max_
Definition: treeiksolverpos_online.hpp:93
KDL::Multiply
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:90
KDL::TreeIkSolverPos_Online::q_dot_
JntArray q_dot_
Definition: treeiksolverpos_online.hpp:99
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::TreeIkSolverPos_Online::iksolver_
TreeIkSolverVel & iksolver_
Definition: treeiksolverpos_online.hpp:98
KDL::TreeIkSolverPos_Online::~TreeIkSolverPos_Online
~TreeIkSolverPos_Online()
Definition: treeiksolverpos_online.cpp:56
KDL::Vector::z
double z() const
Definition: frames.inl:77
KDL::TreeIkSolverPos_Online::frames_
Frames frames_
Definition: treeiksolverpos_online.hpp:101
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::TreeIkSolverPos_Online::x_dot_trans_max_
double x_dot_trans_max_
Definition: treeiksolverpos_online.hpp:95
KDL::TreeIkSolverVel::CartToJnt
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)=0
KDL::TreeFkSolverPos::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)=0
KDL::TreeIkSolverPos_Online::TreeIkSolverPos_Online
TreeIkSolverPos_Online(const double &nr_of_jnts, const std::vector< std::string > &endpoints, const JntArray &q_min, const JntArray &q_max, const JntArray &q_dot_max, const double x_dot_trans_max, const double x_dot_rot_max, TreeFkSolverPos &fksolver, TreeIkSolverVel &iksolver)
Definition: treeiksolverpos_online.cpp:29
KDL::TreeIkSolverPos_Online::q_min_
JntArray q_min_
Definition: treeiksolverpos_online.hpp:92
KDL::TreeIkSolverPos_Online::twist_
Twist twist_
Definition: treeiksolverpos_online.hpp:100
std::map< std::string, Frame >
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Twist::Zero
static Twist Zero()
Definition: frames.inl:290
KDL::TreeIkSolverPos_Online::q_dot_max_
JntArray q_dot_max_
Definition: treeiksolverpos_online.hpp:94
std::map::begin
T begin(T... args)
std::map::insert
T insert(T... args)
KDL::TreeIkSolverPos_Online::x_dot_rot_max_
double x_dot_rot_max_
Definition: treeiksolverpos_online.hpp:96
std::map::end
T end(T... args)
KDL::TreeIkSolverPos_Online::enforceJointVelLimits
void enforceJointVelLimits()
Definition: treeiksolverpos_online.cpp:112
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::TreeIkSolverPos_Online::fksolver_
TreeFkSolverPos & fksolver_
Definition: treeiksolverpos_online.hpp:97
treeiksolverpos_online.hpp
KDL::Vector::x
double x() const
Definition: frames.inl:75
KDL::TreeIkSolverPos_Online::delta_twists_
Twists delta_twists_
Definition: treeiksolverpos_online.hpp:102
KDL::TreeIkSolverPos_Online::enforceCartVelLimits
void enforceCartVelLimits()
Definition: treeiksolverpos_online.cpp:150