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