trajectory.cpp

Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (c) Jürgen Riegel          (juergen.riegel@web.de) 2002     *
00003  *                                                                         *
00004  *   This file is part of the FreeCAD CAx development system.              *
00005  *                                                                         *
00006  *   This library is free software; you can redistribute it and/or         *
00007  *   modify it under the terms of the GNU Library General Public           *
00008  *   License as published by the Free Software Foundation; either          *
00009  *   version 2 of the License, or (at your option) any later version.      *
00010  *                                                                         *
00011  *   This library  is distributed in the hope that it will be useful,      *
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00014  *   GNU Library General Public License for more details.                  *
00015  *                                                                         *
00016  *   You should have received a copy of the GNU Library General Public     *
00017  *   License along with this library; see the file COPYING.LIB. If not,    *
00018  *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
00019  *   Suite 330, Boston, MA  02111-1307, USA                                *
00020  *                                                                         *
00021  ***************************************************************************/
00022 
00023 
00024 #include "PreCompiled.h"
00025 
00026 #ifndef _PreComp_
00027 #endif
00028 
00029 #include <Base/Writer.h>
00030 #include <Base/Reader.h>
00031 #include <Base/Exception.h>
00032 
00033 #include "kdl_cp/chain.hpp"
00034 #include "kdl_cp/path_line.hpp"
00035 #include "kdl_cp/path_roundedcomposite.hpp"
00036 #include "kdl_cp/trajectory_composite.hpp"
00037 #include "kdl_cp/rotational_interpolation_sa.hpp"
00038 #include "kdl_cp/velocityprofile_trap.hpp"
00039 #include "kdl_cp/trajectory_segment.hpp"
00040 #include "kdl_cp/path_roundedcomposite.hpp"
00041 #include "kdl_cp/utilities/error.h"
00042 
00043 #include "Trajectory.h"
00044 #include "RobotAlgos.h"
00045 
00046 #ifndef M_PI
00047     #define M_PI 3.14159265358979323846
00048     #define M_PI    3.14159265358979323846 /* pi */
00049 #endif
00050 
00051 #ifndef M_PI_2
00052     #define M_PI_2  1.57079632679489661923 /* pi/2 */
00053 #endif
00054 
00055 using namespace Robot;
00056 using namespace Base;
00057 //using namespace KDL;
00058 
00059 
00060 TYPESYSTEM_SOURCE(Robot::Trajectory , Base::Persistence);
00061 
00062 Trajectory::Trajectory()
00063 :pcTrajectory(0)
00064 {
00065 
00066 }
00067 
00068 Trajectory::Trajectory(const Trajectory& Trac)
00069 :vpcWaypoints(Trac.vpcWaypoints.size()),pcTrajectory(0)
00070 {
00071     operator=(Trac);
00072 }
00073 
00074 Trajectory::~Trajectory()
00075 {
00076     for(std::vector<Waypoint*>::iterator it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it)
00077         delete ( *it );
00078     delete pcTrajectory;
00079 }
00080 
00081 Trajectory &Trajectory::operator=(const Trajectory& Trac)
00082 {
00083     for(std::vector<Waypoint*>::iterator it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it)
00084         delete ( *it );
00085     vpcWaypoints.clear();
00086     vpcWaypoints.resize(Trac.vpcWaypoints.size());
00087 
00088     int i=0;
00089     for (std::vector<Waypoint*>::const_iterator it=Trac.vpcWaypoints.begin();it!=Trac.vpcWaypoints.end();++it,i++)
00090         vpcWaypoints[i] = new Waypoint(**it);
00091 
00092     generateTrajectory();
00093     return *this;
00094 }
00095 
00096 double Trajectory::getLength(int n) const
00097 {
00098     if(pcTrajectory)
00099         if(n<0)
00100             return pcTrajectory->GetPath()->PathLength();
00101         else
00102             return pcTrajectory->Get(n)->GetPath()->PathLength();
00103     else
00104         return 0;
00105 }
00106 
00107 double Trajectory::getDuration(int n) const
00108 {
00109     if(pcTrajectory)
00110         if(n<0)
00111             return pcTrajectory->Duration();
00112         else
00113             return pcTrajectory->Get(n)->Duration();
00114     else
00115         return 0;
00116 }
00117 
00118 Placement Trajectory::getPosition(double time)const
00119 {
00120     if(pcTrajectory)
00121         return Placement(toPlacement(pcTrajectory->Pos(time)));
00122     else
00123         return Placement();
00124 }
00125 
00126 double Trajectory::getVelocity(double time)const
00127 {
00128     if(pcTrajectory){
00129         KDL::Vector vec = pcTrajectory->Vel(time).vel;
00130         Base::Vector3d vec2(vec[0],vec[1],vec[2]);
00131         return vec2.Length();
00132     }else
00133         return 0;
00134 }
00135 
00136 void Trajectory::deleteLast(unsigned int n)
00137 {
00138     for(unsigned int i=0;i<=n;i++){
00139         delete(*vpcWaypoints.rbegin());
00140         vpcWaypoints.pop_back();
00141     }
00142 
00143 }
00144 
00145 
00146 void Trajectory::generateTrajectory(void)
00147 {
00148     if(vpcWaypoints.size()==0)return;
00149 
00150     // delete the old and create a new one
00151     if(pcTrajectory) delete (pcTrajectory);
00152     pcTrajectory = new KDL::Trajectory_Composite();
00153 
00154     // pointer to the pieces while iterating
00155     KDL::Trajectory_Segment     *pcTrak;
00156     KDL::Path                   *pcPath;
00157     KDL::VelocityProfile        *pcVelPrf;
00158     KDL::Path_RoundedComposite  *pcRoundComp=0;
00159     KDL::Frame                  Last;
00160 
00161     try {
00162         // handle the first waypoint special
00163         bool first=true;
00164 
00165         for(std::vector<Waypoint*>::const_iterator it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it){
00166             if(first){
00167                 Last = toFrame((*it)->EndPos);
00168                 first = false;
00169             }else{
00170                 // destinct the type of movement
00171                 switch((*it)->Type){
00172                     case Waypoint::LINE:
00173                     case Waypoint::PTP:{
00174                         KDL::Frame Next = toFrame((*it)->EndPos);
00175                         // continues the movement until no continus waypoint or the end
00176                         bool Cont = (*it)->Cont && !(it==--vpcWaypoints.end());
00177                         // start of a continue block
00178                         if(Cont && pcRoundComp==0){
00179                             pcRoundComp = new KDL::Path_RoundedComposite(3,
00180                                                         3,
00181                                                         new KDL::RotationalInterpolation_SingleAxis()
00182                                                         );
00183                             // the velocity of the first waypoint is used
00184                             pcVelPrf = new KDL::VelocityProfile_Trap((*it)->Velocity,(*it)->Accelaration);
00185                             pcRoundComp->Add(Last);
00186                             pcRoundComp->Add(Next);
00187 
00188                         // continue a continues block
00189                         }else if (Cont && pcRoundComp){
00190                             pcRoundComp->Add(Next);
00191 
00192                         // end a continous block
00193                         }else if (Cont==false && pcRoundComp){
00194                             // add the last one
00195                             pcRoundComp->Add(Next);
00196                             pcRoundComp->Finish();
00197                             pcVelPrf->SetProfile(0,pcRoundComp->PathLength());
00198                             pcTrak = new KDL::Trajectory_Segment(pcRoundComp,pcVelPrf);
00199                             pcRoundComp = 0;
00200 
00201                         // normal block
00202                         }else if (Cont==false && pcRoundComp==0){
00203                             pcPath = new KDL::Path_Line(Last,
00204                                                         Next,
00205                                                         new KDL::RotationalInterpolation_SingleAxis(),
00206                                                         1.0,
00207                                                         true
00208                                                         );
00209                             pcVelPrf = new KDL::VelocityProfile_Trap((*it)->Velocity,(*it)->Accelaration);
00210                             pcVelPrf->SetProfile(0,pcPath->PathLength());
00211                             pcTrak = new KDL::Trajectory_Segment(pcPath,pcVelPrf);
00212                         }
00213                         Last = Next;
00214                         break;}
00215                     case Waypoint::WAIT:
00216                         break;
00217                         default:
00218                         break;
00219                 }
00220                 // add the segment if no continous block is runing
00221                 if(!pcRoundComp)
00222                     pcTrajectory->Add(pcTrak);
00223             }
00224         }
00225     }catch (KDL::Error &e) {
00226         throw Base::Exception(e.Description());
00227     }
00228 
00229 }
00230 
00231 std::string Trajectory::getUniqueWaypointName(const char *Name) const
00232 {
00233     if (!Name || *Name == '\0')
00234         return std::string();
00235 
00236     // check for first character whether it's a digit
00237     std::string CleanName = Name;
00238     if (!CleanName.empty() && CleanName[0] >= 48 && CleanName[0] <= 57)
00239         CleanName[0] = '_';
00240     // strip illegal chars
00241     for (std::string::iterator it = CleanName.begin(); it != CleanName.end(); ++it) {
00242         if (!((*it>=48 && *it<=57) ||  // number
00243              (*it>=65 && *it<=90)  ||  // uppercase letter
00244              (*it>=97 && *it<=122)))   // lowercase letter
00245              *it = '_'; // it's neither number nor letter
00246     }
00247 
00248     // name in use?
00249     std::vector<Robot::Waypoint*>::const_iterator it;
00250     for(it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it)
00251         if((*it)->Name == CleanName) break;
00252   
00253     if (it == vpcWaypoints.end()) {
00254         // if not, name is OK
00255         return CleanName;
00256     }
00257     else {
00258         // find highest suffix
00259         int nSuff = 0;
00260         for(it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it) {
00261             const std::string &ObjName = (*it)->Name;
00262             if (ObjName.substr(0, CleanName.length()) == CleanName) { // same prefix
00263                 std::string clSuffix(ObjName.substr(CleanName.length()));
00264                 if (clSuffix.size() > 0) {
00265                     std::string::size_type nPos = clSuffix.find_first_not_of("0123456789");
00266                     if (nPos==std::string::npos)
00267                         nSuff = std::max<int>(nSuff, std::atol(clSuffix.c_str()));
00268                 }
00269             }
00270         }
00271 
00272         std::stringstream str;
00273         str << CleanName << (nSuff + 1);
00274         return str.str();
00275     }
00276 }
00277 
00278 void Trajectory::addWaypoint(const Waypoint &WPnt)
00279 {
00280     std::string UniqueName = getUniqueWaypointName(WPnt.Name.c_str());
00281     Waypoint *tmp = new Waypoint(WPnt);
00282     tmp->Name = UniqueName;
00283     vpcWaypoints.push_back(tmp);
00284 }
00285 
00286 
00287 
00288 
00289 unsigned int Trajectory::getMemSize (void) const
00290 {
00291         return 0;
00292 }
00293 
00294 void Trajectory::Save (Writer &writer) const
00295 {
00296     writer.Stream() << writer.ind() << "<Trajectory count=\"" <<  getSize() <<"\">" << std::endl;
00297     writer.incInd();
00298     for(unsigned int i = 0;i<getSize(); i++)
00299         vpcWaypoints[i]->Save(writer);
00300     writer.decInd();
00301     writer.Stream() << writer.ind() << "</Trajectory>" << std::endl ;
00302 
00303 }
00304 
00305 void Trajectory::Restore(XMLReader &reader)
00306 {
00307     vpcWaypoints.clear();
00308     // read my element
00309     reader.readElement("Trajectory");
00310     // get the value of my Attribute
00311     int count = reader.getAttributeAsInteger("count");
00312     vpcWaypoints.resize(count);
00313 
00314     for (int i = 0; i < count; i++) {
00315         Waypoint *tmp = new Waypoint();
00316         tmp->Restore(reader);
00317         vpcWaypoints[i] = tmp;
00318     }
00319     generateTrajectory();
00320 }
00321 
00322 
00323 
00324 
00325  

Generated on Wed Nov 23 19:00:49 2011 for FreeCAD by  doxygen 1.6.1