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  q_min_(nr_of_jnts),
39  q_max_(nr_of_jnts),
40  q_dot_max_(nr_of_jnts),
41  fksolver_(fksolver),
42  iksolver_(iksolver),
43  q_dot_(nr_of_jnts)
44 {
45  q_min_ = q_min;
46  q_max_ = q_max;
47  q_dot_max_ = q_dot_max;
48  x_dot_trans_max_ = x_dot_trans_max;
49  x_dot_rot_max_ = x_dot_rot_max;
50 
51  for (size_t i = 0; i < endpoints.size(); i++)
52  {
53  frames_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
54  delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
55  }
56 }
57 
58 
60 {}
61 
62 
63 double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
64 {
65  q_out = q_in;
66 
67  // First check, if all elements in p_in are available
68  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
69  if(frames_.find(f_des_it->first)==frames_.end())
70  return -2;
71 
72  for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
73  {
74  // Get all iterators for this endpoint
75  Frames::iterator f_it = frames_.find(f_des_it->first);
76  Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
77 
78  fksolver_.JntToCart(q_out, f_it->second, f_it->first);
79  twist_ = diff(f_it->second, f_des_it->second);
80 
81  // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
82  // And scales them, if necessary
84 
85  delta_twists_it->second = twist_;
86  }
87 
88  double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
89 
90  if(res<0)
91  return res;
92  //If we got here q_out is definitely of the right size
93  if(q_out.rows()!=q_min_.rows() || q_out.rows()!=q_max_.rows() || q_out.rows()!= q_dot_max_.rows())
94  return -1;
95 
96  // Checks, if joint velocities (q_dot_) exceed their maximum and scales them, if necessary
98 
99  // Integrate
100  Add(q_out, q_dot_, q_out);
101 
102  // Limit joint positions
103  for (unsigned int j = 0; j < q_min_.rows(); j++)
104  {
105  if (q_out(j) < q_min_(j))
106  q_out(j) = q_min_(j);
107  else if (q_out(j) > q_max_(j))
108  q_out(j) = q_max_(j);
109  }
110 
111  return res;
112 }
113 
114 
116 {
117  // check, if one (or more) joint velocities exceed the maximum value
118  // and if so, safe the biggest overshoot for scaling q_dot_ properly
119  // to keep the direction of the velocity vector the same
120  double rel_os, rel_os_max = 0.0; // relative overshoot and the biggest relative overshoot
121  bool max_exceeded = false;
122 
123  for (unsigned int i = 0; i < q_dot_.rows(); i++)
124  {
125  if ( q_dot_(i) > q_dot_max_(i) )
126  {
127  max_exceeded = true;
128  rel_os = (q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
129  if ( rel_os > rel_os_max )
130  {
131  rel_os_max = rel_os;
132  }
133  }
134  else if ( q_dot_(i) < -q_dot_max_(i) )
135  {
136  max_exceeded = true;
137  rel_os = (-q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
138  if ( rel_os > rel_os_max)
139  {
140  rel_os_max = rel_os;
141  }
142  }
143  }
144 
145  // scales q_out, if one joint exceeds the maximum value
146  if ( max_exceeded == true )
147  {
148  Multiply(q_dot_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_dot_);
149  }
150 }
151 
152 
154 {
155  double x_dot_trans, x_dot_rot; // vector lengths
156  x_dot_trans = sqrt( pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2));
157  x_dot_rot = sqrt( pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2));
158 
159  if ( x_dot_trans > x_dot_trans_max_ || x_dot_rot > x_dot_rot_max_ )
160  {
161  if ( x_dot_trans > x_dot_rot )
162  {
163  twist_.vel = twist_.vel * ( x_dot_trans_max_ / x_dot_trans );
164  twist_.rot = twist_.rot * ( x_dot_trans_max_ / x_dot_trans );
165  }
166  else if ( x_dot_rot > x_dot_trans )
167  {
168  twist_.vel = twist_.vel * ( x_dot_rot_max_ / x_dot_rot );
169  twist_.rot = twist_.rot * ( x_dot_rot_max_ / x_dot_rot );
170  }
171  }
172 }
173 
174 } // namespace
175 
static Twist Zero()
Definition: frames.hpp:291
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)=0
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Tree...
unsigned int rows() const
Definition: jntarray.cpp:72
Vector vel
The velocity of that point.
Definition: frames.hpp:722
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
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)
double z() const
Definition: frames.hpp:78
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:82
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
double y() const
Definition: frames.hpp:77
double x() const
Definition: frames.hpp:76
std::map< std::string, Frame > Frames
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:361
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
static Frame Identity()
Definition: frames.hpp:701
virtual double CartToJnt(const JntArray &q_in, const Frames &p_in, JntArray &q_out)
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)=0
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:92


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44