orocos_kdl
path_circle.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_circle.cxx
3 
4  path_circle.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_circle.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
39  * $Name: $
40  ****************************************************************************/
41 
42 
43 #include "path_circle.hpp"
44 #include "utilities/error.h"
45 
46 namespace KDL {
47 
48 Path_Circle::Path_Circle(const Frame& F_base_start,
49  const Vector& _V_base_center,
50  const Vector& V_base_p,
51  const Rotation& R_base_end,
52  double alpha,
53  RotationalInterpolation* _orient,
54  double _eqradius,
55  bool _aggregate) :
56  orient(_orient) ,
57  eqradius(_eqradius),
58  aggregate(_aggregate)
59 {
60  F_base_center.p = _V_base_center;
61  orient->SetStartEnd(F_base_start.M,R_base_end);
62  double oalpha = orient->Angle();
63 
64  Vector x(F_base_start.p - F_base_center.p);
65  radius = x.Normalize();
66  if (radius < epsilon) {
67  if (aggregate)
68  delete orient;
69  throw Error_MotionPlanning_Circle_ToSmall();
70  }
71  Vector tmpv(V_base_p-F_base_center.p);
72  tmpv.Normalize();
73  Vector z( x * tmpv);
74  double n = z.Normalize();
75  if (n < epsilon) {
76  if (aggregate)
77  delete orient;
78  throw Error_MotionPlanning_Circle_No_Plane();
79  }
80  F_base_center.M = Rotation(x,z*x,z);
81  double dist = alpha*radius;
82  // See what has the slowest eq. motion, and adapt
83  // the other to this slower motion
84  // use eqradius to transform between rot and transl.
85  // the same as for lineair motion
86  if (oalpha*eqradius > dist) {
87  // rotational_interpolation is the limitation
88  pathlength = oalpha*eqradius;
89  scalerot = 1/eqradius;
90  scalelin = dist/pathlength;
91  } else {
92  // translation is the limitation
93  pathlength = dist;
94  scalerot = oalpha/pathlength;
95  scalelin = 1;
96  }
97 }
98 
99 
100 double Path_Circle::LengthToS(double length) {
101  return length/scalelin;
102 }
103 
104 
105 double Path_Circle::PathLength() {
106  return pathlength;
107 }
108 
109 Frame Path_Circle::Pos(double s) const {
110  double p = s*scalelin / radius;
111  return Frame(orient->Pos(s*scalerot),
112  F_base_center*Vector(radius*cos(p),radius*sin(p),0)
113  );
114 
115 }
116 
117 Twist Path_Circle::Vel(double s,double sd) const {
118  double p = s*scalelin / radius;
119  double v = sd*scalelin / radius;
120  return Twist( F_base_center.M*Vector(-radius*sin(p)*v,radius*cos(p)*v,0),
121  orient->Vel(s*scalerot,sd*scalerot)
122  );
123 }
124 
125 Twist Path_Circle::Acc(double s,double sd,double sdd) const {
126  double p = s*scalelin / radius;
127  double cp = cos(p);
128  double sp = sin(p);
129  double v = sd*scalelin / radius;
130  double a = sdd*scalelin / radius;
131  return Twist( F_base_center.M*Vector(
132  -radius*cp*v*v - radius*sp*a,
133  -radius*sp*v*v + radius*cp*a,
134  0
135  ),
136  orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot)
137  );
138 }
139 
141  return new Path_Circle(
142  Pos(0),
148  eqradius,
149  aggregate
150  );
151 }
152 
154  if (aggregate)
155  delete orient;
156 }
157 
159  os << "CIRCLE[ ";
160  os << " " << Pos(0) << std::endl;
161  os << " " << F_base_center.p << std::endl;
162  os << " " << F_base_center.M.UnitY() << std::endl;
164  os << " " << pathlength*scalelin/radius/deg2rad << std::endl;
165  os << " ";orient->Write(os);
166  os << " " << eqradius;
167  os << "]"<< std::endl;
168 }
169 
170 }
KDL::Path_Circle::Write
virtual void Write(std::ostream &os)
Definition: path_circle.cpp:196
KDL::RotationalInterpolation::Vel
virtual Vector Vel(double theta, double thetad) const =0
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::Path_Circle::F_base_center
Frame F_base_center
Definition: path_circle.hpp:146
KDL::Path_Circle::Vel
virtual Twist Vel(double s, double sd) const
Definition: path_circle.cpp:155
KDL::Path_Circle::Acc
virtual Twist Acc(double s, double sd, double sdd) const
Definition: path_circle.cpp:163
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL::deg2rad
const double deg2rad
the value pi/180
Definition: utility.cxx:19
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Path_Circle::~Path_Circle
virtual ~Path_Circle()
Definition: path_circle.cpp:191
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
KDL::Path_Circle::aggregate
bool aggregate
Definition: path_circle.hpp:156
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::RotationalInterpolation::Write
virtual void Write(std::ostream &os) const =0
KDL::Path_Circle::orient
RotationalInterpolation * orient
Definition: path_circle.hpp:142
std::ostream
error.h
KDL::RotationalInterpolation::Acc
virtual Vector Acc(double theta, double thetad, double thetadd) const =0
KDL::Path_Circle::pathlength
double pathlength
Definition: path_circle.hpp:152
KDL::Path_Circle::PathLength
virtual double PathLength()
Definition: path_circle.cpp:143
std::endl
T endl(T... args)
KDL::Path_Circle::radius
double radius
Definition: path_circle.hpp:145
KDL::Rotation::UnitY
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:524
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::Path_Circle::Path_Circle
Path_Circle(const Frame &F_base_start, const Vector &V_base_center, const Vector &V_base_p, const Rotation &R_base_end, double alpha, RotationalInterpolation *otraj, double eqradius, bool _aggregate=true)
Definition: path_circle.cpp:86
path_circle.hpp
KDL::RotationalInterpolation::Clone
virtual RotationalInterpolation * Clone() const =0
KDL::Path_Circle::eqradius
double eqradius
Definition: path_circle.hpp:149
KDL::Path_Circle::Clone
virtual Path * Clone()
Definition: path_circle.cpp:178
KDL::Path
Definition: path.hpp:96
KDL::Path_Circle::scalerot
double scalerot
Definition: path_circle.hpp:154
KDL::Path_Circle::LengthToS
double LengthToS(double length)
Definition: path_circle.cpp:138
KDL::Path_Circle::Pos
virtual Frame Pos(double s) const
Definition: path_circle.cpp:147
KDL::RotationalInterpolation::Pos
virtual Rotation Pos(double theta) const =0
KDL::Path_Circle::scalelin
double scalelin
Definition: path_circle.hpp:153