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
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 #include "path_composite.hpp"
00044 #include "utilities/error.h"
00045 #include <memory>
00046
00047 namespace KDL {
00048
00049
00050
00051
00052
00053
00054 double Path_Composite::Lookup(double s) const
00055 {
00056 assert(s>=-1e-12);
00057 assert(s<=pathlength+1e-12);
00058 if ( (cached_starts <=s) && ( s <= cached_ends) ) {
00059 return s - cached_starts;
00060 }
00061 double previous_s=0;
00062 for (unsigned int i=0;i<dv.size();++i) {
00063 if ((s <= dv[i])||(i == (dv.size()-1) )) {
00064 cached_index = i;
00065 cached_starts = previous_s;
00066 cached_ends = dv[i];
00067 return s - previous_s;
00068 }
00069 previous_s = dv[i];
00070 }
00071 return 0;
00072 }
00073
00074 Path_Composite::Path_Composite() {
00075 pathlength = 0;
00076 cached_starts = 0;
00077 cached_ends = 0;
00078 cached_index = 0;
00079 }
00080
00081 void Path_Composite::Add(Path* geom, bool aggregate ) {
00082 pathlength += geom->PathLength();
00083 dv.insert(dv.end(),pathlength);
00084 gv.insert( gv.end(),std::make_pair(geom,aggregate) );
00085 }
00086
00087 double Path_Composite::LengthToS(double length) {
00088 throw Error_MotionPlanning_Not_Applicable();
00089 return 0;
00090 }
00091
00092 double Path_Composite::PathLength() {
00093 return pathlength;
00094 }
00095
00096
00097 Frame Path_Composite::Pos(double s) const {
00098 s = Lookup(s);
00099 return gv[cached_index].first->Pos(s);
00100 }
00101
00102 Twist Path_Composite::Vel(double s,double sd) const {
00103 s = Lookup(s);
00104 return gv[cached_index].first->Vel(s,sd);
00105 }
00106
00107 Twist Path_Composite::Acc(double s,double sd,double sdd) const {
00108 s = Lookup(s);
00109 return gv[cached_index].first->Acc(s,sd,sdd);
00110 }
00111
00112 Path* Path_Composite::Clone() {
00113 std::auto_ptr<Path_Composite> comp( new Path_Composite() );
00114 for (unsigned int i = 0; i < dv.size(); ++i) {
00115 comp->Add(gv[i].first->Clone(), gv[i].second);
00116 }
00117 return comp.release();
00118 }
00119
00120 void Path_Composite::Write(std::ostream& os) {
00121 os << "COMPOSITE[ " << std::endl;
00122 os << " " << dv.size() << std::endl;
00123 for (unsigned int i=0;i<dv.size();i++) {
00124 gv[i].first->Write(os);
00125 }
00126 os << "]" << std::endl;
00127 }
00128
00129 int Path_Composite::GetNrOfSegments() {
00130 return dv.size();
00131 }
00132
00133 Path* Path_Composite::GetSegment(int i) {
00134 assert(i>=0);
00135 assert(i<dv.size());
00136 return gv[i].first;
00137 }
00138
00139 double Path_Composite::GetLengthToEndOfSegment(int i) {
00140 assert(i>=0);
00141 assert(i<dv.size());
00142 return dv[i];
00143 }
00144
00145 void Path_Composite::GetCurrentSegmentLocation(double s, int& segment_number,
00146 double& inner_s)
00147 {
00148 inner_s = Lookup(s);
00149 segment_number= cached_index;
00150 }
00151
00152 Path_Composite::~Path_Composite() {
00153 PathVector::iterator it;
00154 for (it=gv.begin();it!=gv.end();++it) {
00155 if (it->second)
00156 delete it->first;
00157 }
00158 }
00159
00160 }