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