trajectory.cpp
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00049 #endif
00050
00051 #ifndef M_PI_2
00052 #define M_PI_2 1.57079632679489661923
00053 #endif
00054
00055 using namespace Robot;
00056 using namespace Base;
00057
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
00151 if(pcTrajectory) delete (pcTrajectory);
00152 pcTrajectory = new KDL::Trajectory_Composite();
00153
00154
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
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
00171 switch((*it)->Type){
00172 case Waypoint::LINE:
00173 case Waypoint::PTP:{
00174 KDL::Frame Next = toFrame((*it)->EndPos);
00175
00176 bool Cont = (*it)->Cont && !(it==--vpcWaypoints.end());
00177
00178 if(Cont && pcRoundComp==0){
00179 pcRoundComp = new KDL::Path_RoundedComposite(3,
00180 3,
00181 new KDL::RotationalInterpolation_SingleAxis()
00182 );
00183
00184 pcVelPrf = new KDL::VelocityProfile_Trap((*it)->Velocity,(*it)->Accelaration);
00185 pcRoundComp->Add(Last);
00186 pcRoundComp->Add(Next);
00187
00188
00189 }else if (Cont && pcRoundComp){
00190 pcRoundComp->Add(Next);
00191
00192
00193 }else if (Cont==false && pcRoundComp){
00194
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
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
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
00237 std::string CleanName = Name;
00238 if (!CleanName.empty() && CleanName[0] >= 48 && CleanName[0] <= 57)
00239 CleanName[0] = '_';
00240
00241 for (std::string::iterator it = CleanName.begin(); it != CleanName.end(); ++it) {
00242 if (!((*it>=48 && *it<=57) ||
00243 (*it>=65 && *it<=90) ||
00244 (*it>=97 && *it<=122)))
00245 *it = '_';
00246 }
00247
00248
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
00255 return CleanName;
00256 }
00257 else {
00258
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) {
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
00309 reader.readElement("Trajectory");
00310
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