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_roundedcomposite.hpp"
00044 #include "path_line.hpp"
00045 #include "path_circle.hpp"
00046 #include "utilities/error.h"
00047 #include "utilities/scoped_ptr.hpp"
00048 #include <memory>
00049
00050
00051 namespace KDL {
00052
00053
00054
00055 Path_RoundedComposite::Path_RoundedComposite(Path_Composite* _comp,
00056 double _radius, double _eqradius, RotationalInterpolation* _orient,
00057 bool _aggregate,int _nrofpoints):
00058 comp(_comp), radius(_radius), eqradius(_eqradius), orient(_orient), nrofpoints(_nrofpoints), aggregate(_aggregate) {
00059 }
00060
00061 Path_RoundedComposite::Path_RoundedComposite(double _radius,double _eqradius,RotationalInterpolation* _orient, bool _aggregate) :
00062 comp( new Path_Composite()), radius(_radius),eqradius(_eqradius), orient(_orient), aggregate(_aggregate)
00063 {
00064 nrofpoints = 0;
00065 if (eqradius<=0) {
00066 throw Error_MotionPlanning_Not_Feasible(1);
00067 }
00068 }
00069
00070 void Path_RoundedComposite::Add(const Frame& F_base_point) {
00071 double eps = 1E-7;
00072 if (nrofpoints == 0) {
00073 F_base_start = F_base_point;
00074 } else if (nrofpoints == 1) {
00075 F_base_via = F_base_point;
00076 } else {
00077
00078
00079 Vector ab = F_base_via.p - F_base_start.p;
00080 Vector bc = F_base_point.p - F_base_via.p;
00081 double abdist = ab.Norm();
00082 double bcdist = bc.Norm();
00083 if (abdist < eps) {
00084 throw Error_MotionPlanning_Not_Feasible(2);
00085 }
00086 if (bcdist < eps) {
00087 throw Error_MotionPlanning_Not_Feasible(3);
00088 }
00089
00090 double alpha = acos(std::max(-1., std::min(dot(ab, bc) / abdist / bcdist, 1.)));
00091 if ((PI - alpha) < eps) {
00092 throw Error_MotionPlanning_Not_Feasible(4);
00093 }
00094 if (alpha < eps) {
00095
00096 comp->Add(
00097 new Path_Line(F_base_start, F_base_via, orient->Clone(),
00098 eqradius));
00099 F_base_start = F_base_via;
00100 F_base_via = F_base_point;
00101 } else {
00102 double d = radius / tan((PI - alpha) / 2);
00103 if (d >= abdist)
00104 throw Error_MotionPlanning_Not_Feasible(5);
00105
00106 if (d >= bcdist)
00107 throw Error_MotionPlanning_Not_Feasible(6);
00108
00109 scoped_ptr < Path
00110 > line1(
00111 new Path_Line(F_base_start, F_base_via,
00112 orient->Clone(), eqradius));
00113 scoped_ptr < Path
00114 > line2(
00115 new Path_Line(F_base_via, F_base_point,
00116 orient->Clone(), eqradius));
00117 Frame F_base_circlestart = line1->Pos(line1->LengthToS(abdist - d));
00118 Frame F_base_circleend = line2->Pos(line2->LengthToS(d));
00119
00120 Vector V_base_t = ab * (ab * bc);
00121 V_base_t.Normalize();
00122 comp->Add(
00123 new Path_Line(F_base_start, F_base_circlestart,
00124 orient->Clone(), eqradius));
00125 comp->Add(
00126 new Path_Circle(F_base_circlestart,
00127 F_base_circlestart.p - V_base_t * radius,
00128 F_base_circleend.p, F_base_circleend.M, alpha,
00129 orient->Clone(), eqradius));
00130
00131 F_base_start = F_base_circleend;
00132 F_base_via = F_base_point;
00133 }
00134 }
00135
00136 nrofpoints++;
00137 }
00138
00139 void Path_RoundedComposite::Finish() {
00140 if (nrofpoints >= 1) {
00141 comp->Add(
00142 new Path_Line(F_base_start, F_base_via, orient->Clone(),
00143 eqradius));
00144 }
00145 }
00146
00147 double Path_RoundedComposite::LengthToS(double length) {
00148 return comp->LengthToS(length);
00149 }
00150
00151
00152 double Path_RoundedComposite::PathLength() {
00153 return comp->PathLength();
00154 }
00155
00156 Frame Path_RoundedComposite::Pos(double s) const {
00157 return comp->Pos(s);
00158 }
00159
00160 Twist Path_RoundedComposite::Vel(double s, double sd) const {
00161 return comp->Vel(s, sd);
00162 }
00163
00164 Twist Path_RoundedComposite::Acc(double s, double sd, double sdd) const {
00165 return comp->Acc(s, sd, sdd);
00166 }
00167
00168 void Path_RoundedComposite::Write(std::ostream& os) {
00169 comp->Write(os);
00170 }
00171
00172 int Path_RoundedComposite::GetNrOfSegments() {
00173 return comp->GetNrOfSegments();
00174 }
00175
00176 Path* Path_RoundedComposite::GetSegment(int i) {
00177 return comp->GetSegment(i);
00178 }
00179
00180 double Path_RoundedComposite::GetLengthToEndOfSegment(int i) {
00181 return comp->GetLengthToEndOfSegment(i);
00182 }
00183
00184 void Path_RoundedComposite::GetCurrentSegmentLocation(double s,
00185 int& segment_number, double& inner_s) {
00186 comp->GetCurrentSegmentLocation(s,segment_number,inner_s);
00187 }
00188
00189
00190
00191 Path_RoundedComposite::~Path_RoundedComposite() {
00192 if (aggregate)
00193 delete orient;
00194 delete comp;
00195 }
00196
00197
00198 Path* Path_RoundedComposite::Clone() {
00199 return new Path_RoundedComposite(static_cast<Path_Composite*>(comp->Clone()),radius,eqradius,orient->Clone(), true, nrofpoints);
00200 }
00201
00202 }