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;
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;
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 
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),
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),
147  orient->Clone(),
148  eqradius,
149  aggregate
150  );
151 }
152 
154  if (aggregate)
155  delete orient;
156 }
157 
158 void Path_Circle::Write(std::ostream& os) {
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;
163  os << " " << orient->Pos(pathlength*scalerot) << 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 }
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
virtual void Write(std::ostream &os)
virtual double PathLength()
double Normalize(double eps=epsilon)
Definition: frames.cpp:147
virtual Twist Vel(double s, double sd) const
virtual RotationalInterpolation * Clone() const =0
double LengthToS(double length)
virtual Frame Pos(double s) const
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:48
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
virtual Path * Clone()
represents both translational and rotational velocities.
Definition: frames.hpp:720
virtual Rotation Pos(double theta) const =0
virtual double Angle()=0
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
RotationalInterpolation * orient
Definition: path_circle.hpp:64
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Vector p
origine of the Frame
Definition: frames.hpp:572
virtual Vector Vel(double theta, double thetad) const =0
virtual Twist Acc(double s, double sd, double sdd) const
virtual void SetStartEnd(Rotation start, Rotation end)=0
virtual ~Path_Circle()
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
const double deg2rad
the value pi/180
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313


orocos_kdl
Author(s):
autogenerated on Tue Sep 1 2020 03:18:51