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  {
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 }
133  return pathlength;
134 }
135 Frame Path_Line::Pos(double s) const {
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 
154  if (aggregate )
155  return new Path_Line(
158  orient->Clone(),
159  eqradius,
160  true
161  );
162  // else :
163  return new Path_Line(
165  Frame(orient->Pos(pathlength*scalerot),V_base_end),
166  orient,
167  eqradius,
168  false
169  );
170 
171 }
172 
173 void Path_Line::Write(std::ostream& os) {
174  os << "LINE[ ";
175  os << " " << Frame(orient->Pos(0),V_base_start) << std::endl;
176  os << " " << Frame(orient->Pos(pathlength*scalerot),V_base_end) << std::endl;
177  os << " ";orient->Write(os);
178  os << " " << eqradius;
179  os << "]" << std::endl;
180 }
181 
182 
183 }
184 
double scalerot
Definition: path_line.hpp:74
Vector V_start_end
Definition: path_line.hpp:67
Vector V_base_end
Definition: path_line.hpp:66
double Normalize(double eps=epsilon)
Definition: frames.cpp:148
Vector vel
The velocity of that point.
Definition: frames.hpp:722
Vector V_base_start
Definition: path_line.hpp:65
virtual RotationalInterpolation * Clone() const =0
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
Path_Line(const Frame &F_base_start, const Frame &F_base_end, RotationalInterpolation *orient, double eqradius, bool _aggregate=true)
Definition: path_line.cpp:47
represents both translational and rotational velocities.
Definition: frames.hpp:720
virtual void Write(std::ostream &os)
Definition: path_line.cpp:173
virtual Rotation Pos(double theta) const =0
virtual double Angle()=0
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
double LengthToS(double length)
Definition: path_line.cpp:129
double Norm() const
Definition: frames.cpp:118
virtual Path * Clone()
Definition: path_line.cpp:153
virtual Frame Pos(double s) const
Definition: path_line.cpp:135
virtual Vector Vel(double theta, double thetad) const =0
virtual double PathLength()
Definition: path_line.cpp:132
virtual void SetStartEnd(Rotation start, Rotation end)=0
double pathlength
Definition: path_line.hpp:72
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
virtual void Write(std::ostream &os) const =0
virtual Vector Acc(double theta, double thetad, double thetadd) const =0
virtual Twist Acc(double s, double sd, double sdd) const
Definition: path_line.cpp:143
virtual ~Path_Line()
Definition: path_line.cpp:148
virtual Twist Vel(double s, double sd) const
Definition: path_line.cpp:139
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:294
RotationalInterpolation * orient
Definition: path_line.hpp:62
double eqradius
Definition: path_line.hpp:69
double scalelin
Definition: path_line.hpp:73


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36