path_circle.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Erwin Aertbelien  Mon May 10 19:10:36 CEST 2004  path_circle.cxx
00003 
00004                         path_circle.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Erwin Aertbelien
00008     email                : erwin.aertbelien@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 /*****************************************************************************
00028  *  \author
00029  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00030  *
00031  *  \version
00032  *              ORO_Geometry V0.2
00033  *
00034  *      \par History
00035  *              - $log$
00036  *
00037  *      \par Release
00038  *              $Id: path_circle.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
00039  *              $Name:  $
00040  ****************************************************************************/
00041 
00042 
00043 #include "path_circle.hpp"
00044 #include "utilities/error.h"
00045 
00046 namespace KDL {
00047 
00048 
00049 
00050 Path_Circle::Path_Circle(const Frame& F_base_start,
00051                         const Vector& _V_base_center,
00052                         const Vector& V_base_p,
00053                         const Rotation& R_base_end,
00054                         double alpha,
00055                         RotationalInterpolation* _orient,
00056                         double _eqradius,
00057             bool _aggregate) :
00058                                 orient(_orient) ,
00059                                 eqradius(_eqradius),
00060                 aggregate(_aggregate)
00061                         {
00062                                         F_base_center.p = _V_base_center;
00063                                         orient->SetStartEnd(F_base_start.M,R_base_end);
00064                                         double oalpha = orient->Angle();
00065 
00066                                         Vector x(F_base_start.p - F_base_center.p);
00067                                         radius = x.Normalize();
00068                                         if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall();
00069                                         Vector tmpv(V_base_p-F_base_center.p);
00070                                         tmpv.Normalize();
00071                                         Vector z( x * tmpv);
00072                     double n = z.Normalize();
00073                                     if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane();
00074                                         F_base_center.M = Rotation(x,z*x,z);
00075                                         double dist = alpha*radius;
00076                                         // See what has the slowest eq. motion, and adapt
00077                                         // the other to this slower motion
00078                                         // use eqradius to transform between rot and transl.
00079                                         // the same as for lineair motion
00080                                         if (oalpha*eqradius > dist) {
00081                                                 // rotational_interpolation is the limitation
00082                                                 pathlength = oalpha*eqradius;
00083                                                 scalerot   = 1/eqradius;
00084                                                 scalelin   = dist/pathlength;
00085                                         } else {
00086                                                 // translation is the limitation
00087                                                 pathlength = dist;
00088                                                 scalerot   = oalpha/pathlength;
00089                                                 scalelin   = 1;
00090                                         }
00091                         }
00092 
00093 
00094 
00095 double Path_Circle::LengthToS(double length) {
00096         return length/scalelin;
00097 }
00098 
00099 
00100 double Path_Circle::PathLength() {
00101         return pathlength;
00102 }
00103 
00104 Frame Path_Circle::Pos(double s) const {
00105         double p = s*scalelin / radius;
00106         return Frame(orient->Pos(s*scalerot),
00107                          F_base_center*Vector(radius*cos(p),radius*sin(p),0)
00108                    );
00109 
00110 }
00111 
00112 Twist Path_Circle::Vel(double s,double sd) const {
00113         double p = s*scalelin  / radius;
00114         double v = sd*scalelin / radius;
00115         return Twist( F_base_center.M*Vector(-radius*sin(p)*v,radius*cos(p)*v,0),
00116                           orient->Vel(s*scalerot,sd*scalerot)
00117                    );
00118 }
00119 
00120 Twist Path_Circle::Acc(double s,double sd,double sdd) const {
00121         double p = s*scalelin / radius;
00122         double cp = cos(p);
00123         double sp = sin(p);
00124         double v = sd*scalelin / radius;
00125         double a = sdd*scalelin / radius;
00126         return Twist( F_base_center.M*Vector(
00127                                                 -radius*cp*v*v  -  radius*sp*a,
00128                                                 -radius*sp*v*v  +  radius*cp*a,
00129                                                 0
00130                                           ),
00131                           orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot)
00132                    );
00133 }
00134 
00135 Path* Path_Circle::Clone() {
00136         return new Path_Circle(
00137                 Pos(0),
00138                 F_base_center.p,
00139                 F_base_center.M.UnitY(),
00140                 orient->Pos(pathlength*scalerot),
00141                 pathlength*scalelin/radius/deg2rad,
00142                 orient->Clone(),
00143                 eqradius,
00144         aggregate
00145         );
00146 }
00147 
00148 Path_Circle::~Path_Circle() {
00149     if (aggregate)
00150         delete orient;
00151 }
00152 
00153 
00154 
00155 void Path_Circle::Write(std::ostream& os) {
00156         os << "CIRCLE[ ";
00157         os << "  " << Pos(0) << std::endl;
00158         os << "  " << F_base_center.p << std::endl;
00159         os << "  " << F_base_center.M.UnitY() << std::endl;
00160         os << "  " << orient->Pos(pathlength*scalerot) << std::endl;
00161         os << "  " << pathlength*scalelin/radius/deg2rad << std::endl;
00162         os << "  ";orient->Write(os);
00163         os << "  " << eqradius;
00164         os << "]"<< std::endl;
00165 }
00166 
00167 
00168 }
00169 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14