orocos_kdl
path_line.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_line.cxx
3 
4  path_line.cxx - description
5  -------------------
6  begin : Mon May 10 2004
7  copyright : (C) 2004 Erwin Aertbelien
8  email : erwin.aertbelien@mech.kuleuven.ac.be
9 
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., 59 Temple Place, *
24  * Suite 330, Boston, MA 02111-1307 USA *
25  * *
26  ***************************************************************************/
27 /*****************************************************************************
28  * \author
29  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
30  *
31  * \version
32  * ORO_Geometry V0.2
33  *
34  * \par History
35  * - $log$
36  *
37  * \par Release
38  * $Id: path_line.cpp,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
39  * $Name: $
40  ****************************************************************************/
41 
42 
43 #include "path_line.hpp"
44 
45 namespace KDL {
46 
47 Path_Line::Path_Line(const Frame& startpos,
48  const Frame& endpos,
49  RotationalInterpolation* _orient,
50  double _eqradius,
51  bool _aggregate ):
52  orient(_orient),
53  V_base_start(startpos.p),
54  V_base_end(endpos.p),
55  eqradius(_eqradius),
56  aggregate(_aggregate)
57  {
58  V_start_end = V_base_end - V_base_start;
59  double dist = V_start_end.Normalize();
60  orient->SetStartEnd(startpos.M,endpos.M);
61  double alpha = orient->Angle();
62 
63  // See what has the slowest eq. motion, and adapt
64  // the other to this slower motion
65  // use eqradius to transform between rot and transl.
66 
67  // Only modify if non zero (prevent division by zero)
68  if ( alpha != 0 && alpha*eqradius > dist) {
69  // rotational_interpolation is the limitation
70  pathlength = alpha*eqradius;
71  scalerot = 1/eqradius;
72  scalelin = dist/pathlength;
73  } else if ( dist != 0 ) {
74  // translation is the limitation
75  pathlength = dist;
76  scalerot = alpha/pathlength;
77  scalelin = 1;
78  } else {
79  // both were zero
80  pathlength = 0;
81  scalerot = 1;
82  scalelin = 1;
83  }
84  }
85 
86 Path_Line::Path_Line(const Frame& startpos,
87  const Twist& starttwist,
88  RotationalInterpolation* _orient,
89  double _eqradius,
90  bool _aggregate ):
91  orient(_orient),
92  V_base_start(startpos.p),
93  V_base_end(startpos.p + starttwist.vel),
94  eqradius(_eqradius),
95  aggregate(_aggregate)
96  {
97  // startframe and starttwist are expressed in Wo.
98  // after 1 time unit, startframe has translated over starttwist.vel
99  // and rotated over starttwist.rot.Norm() (both vectors can be zero)
100  // Thus the frame on the path after 1 time unit is defined by
101  // startframe.Integrate(starttwist, 1);
103  double dist = V_start_end.Normalize(); // distance traveled during 1 time unit
104  orient->SetStartEnd(startpos.M, (startpos*Frame( Rotation::Rot(starttwist.rot, starttwist.rot.Norm() ), starttwist.vel )).M);
105  double alpha = orient->Angle(); // rotation during 1 time unit
106 
107  // See what has the slowest eq. motion, and adapt
108  // the other to this slower motion
109  // use eqradius to transform between rot and transl.
110  // Only modify if non zero (prevent division by zero)
111  if ( alpha != 0 && alpha*eqradius > dist) {
112  // rotational_interpolation is the limitation
113  pathlength = alpha*eqradius;
114  scalerot = 1/eqradius;
115  scalelin = dist/pathlength;
116  } else if ( dist != 0 ) {
117  // translation is the limitation
118  pathlength = dist;
119  scalerot = alpha/pathlength;
120  scalelin = 1;
121  } else {
122  // both were zero
123  pathlength = 0;
124  scalerot = 1;
125  scalelin = 1;
126  }
127  }
128 
129 double Path_Line::LengthToS(double length) {
130  return length/scalelin;
131 }
132 double Path_Line::PathLength(){
133  return pathlength;
134 }
135 Frame Path_Line::Pos(double s) const {
136  return Frame(orient->Pos(s*scalerot),V_base_start + V_start_end*s*scalelin );
137 }
138 
139 Twist Path_Line::Vel(double s,double sd) const {
140  return Twist( V_start_end*sd*scalelin, orient->Vel(s*scalerot,sd*scalerot) );
141 }
142 
143 Twist Path_Line::Acc(double s,double sd,double sdd) const {
144  return Twist( V_start_end*sdd*scalelin, orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) );
145 }
146 
147 
149  if (aggregate)
150  delete orient;
151 }
152 
153 Path* Path_Line::Clone() {
154  if (aggregate )
155  return new Path_Line(
156  Frame(orient->Pos(0),V_base_start),
158  orient->Clone(),
159  eqradius,
160  true
161  );
162  // else :
163  return new Path_Line(
164  Frame(orient->Pos(0),V_base_start),
166  orient,
168  false
169  );
170 
171 }
172 
174  os << "LINE[ ";
175  os << " " << Frame(orient->Pos(0),V_base_start) << std::endl;
177  os << " ";orient->Write(os);
178  os << " " << eqradius;
179  os << "]" << std::endl;
180 }
181 
182 
183 }
184 
KDL::RotationalInterpolation
Definition: rotational_interpolation.hpp:100
KDL::Path_Line::V_base_start
Vector V_base_start
Definition: path_line.hpp:143
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::Path_Line::~Path_Line
virtual ~Path_Line()
Definition: path_line.cpp:186
KDL::Path_Line::eqradius
double eqradius
Definition: path_line.hpp:147
KDL::RotationalInterpolation::SetStartEnd
virtual void SetStartEnd(Rotation start, Rotation end)=0
KDL::RotationalInterpolation::Vel
virtual Vector Vel(double theta, double thetad) const =0
KDL::Path_Line::scalerot
double scalerot
Definition: path_line.hpp:152
KDL::Vector::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:142
KDL::Path_Line::Acc
virtual Twist Acc(double s, double sd, double sdd) const
Definition: path_line.cpp:181
KDL::Path_Line::Clone
virtual Path * Clone()
Definition: path_line.cpp:191
KDL::Vector::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:172
KDL::Path_Line::V_base_end
Vector V_base_end
Definition: path_line.hpp:144
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Path_Line::PathLength
virtual double PathLength()
Definition: path_line.cpp:170
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::RotationalInterpolation::Write
virtual void Write(std::ostream &os) const =0
KDL::Path_Line::aggregate
bool aggregate
Definition: path_line.hpp:154
std::ostream
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::RotationalInterpolation::Angle
virtual double Angle()=0
KDL::Path_Line::Path_Line
Path_Line(const Frame &F_base_start, const Frame &F_base_end, RotationalInterpolation *orient, double eqradius, bool _aggregate=true)
Definition: path_line.cpp:85
KDL::Path_Line::Write
virtual void Write(std::ostream &os)
Definition: path_line.cpp:211
KDL::Path_Line::orient
RotationalInterpolation * orient
Definition: path_line.hpp:140
KDL::RotationalInterpolation::Acc
virtual Vector Acc(double theta, double thetad, double thetadd) const =0
path_line.hpp
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Path_Line::LengthToS
double LengthToS(double length)
Definition: path_line.cpp:167
std::endl
T endl(T... args)
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:318
KDL::Path_Line::Vel
virtual Twist Vel(double s, double sd) const
Definition: path_line.cpp:177
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::Path_Line::scalelin
double scalelin
Definition: path_line.hpp:151
KDL::Path_Line::V_start_end
Vector V_start_end
Definition: path_line.hpp:145
KDL::RotationalInterpolation::Clone
virtual RotationalInterpolation * Clone() const =0
KDL::Path_Line::pathlength
double pathlength
Definition: path_line.hpp:150
KDL::Path_Line::Pos
virtual Frame Pos(double s) const
Definition: path_line.cpp:173
KDL::RotationalInterpolation::Pos
virtual Rotation Pos(double theta) const =0