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_line.hpp"
00044
00045 namespace KDL {
00046
00047 Path_Line::Path_Line(const Frame& startpos,
00048 const Frame& endpos,
00049 RotationalInterpolation* _orient,
00050 double _eqradius,
00051 bool _aggregate ):
00052 orient(_orient),
00053 V_base_start(startpos.p),
00054 V_base_end(endpos.p),
00055 eqradius(_eqradius),
00056 aggregate(_aggregate)
00057 {
00058 V_start_end = V_base_end - V_base_start;
00059 double dist = V_start_end.Normalize();
00060 orient->SetStartEnd(startpos.M,endpos.M);
00061 double alpha = orient->Angle();
00062
00063
00064
00065
00066
00067
00068 if ( alpha != 0 && alpha*eqradius > dist) {
00069
00070 pathlength = alpha*eqradius;
00071 scalerot = 1/eqradius;
00072 scalelin = dist/pathlength;
00073 } else if ( dist != 0 ) {
00074
00075 pathlength = dist;
00076 scalerot = alpha/pathlength;
00077 scalelin = 1;
00078 } else {
00079
00080 pathlength = 0;
00081 scalerot = 1;
00082 scalelin = 1;
00083 }
00084 }
00085
00086 Path_Line::Path_Line(const Frame& startpos,
00087 const Twist& starttwist,
00088 RotationalInterpolation* _orient,
00089 double _eqradius,
00090 bool _aggregate ):
00091 orient(_orient),
00092 V_base_start(startpos.p),
00093 V_base_end(startpos.p + starttwist.vel),
00094 eqradius(_eqradius),
00095 aggregate(_aggregate)
00096 {
00097
00098
00099
00100
00101
00102 V_start_end = V_base_end - V_base_start;
00103 double dist = V_start_end.Normalize();
00104 orient->SetStartEnd(startpos.M, (startpos*Frame( Rotation::Rot(starttwist.rot, starttwist.rot.Norm() ), starttwist.vel )).M);
00105 double alpha = orient->Angle();
00106
00107
00108
00109
00110
00111 if ( alpha != 0 && alpha*eqradius > dist) {
00112
00113 pathlength = alpha*eqradius;
00114 scalerot = 1/eqradius;
00115 scalelin = dist/pathlength;
00116 } else if ( dist != 0 ) {
00117
00118 pathlength = dist;
00119 scalerot = alpha/pathlength;
00120 scalelin = 1;
00121 } else {
00122
00123 pathlength = 0;
00124 scalerot = 1;
00125 scalelin = 1;
00126 }
00127 }
00128
00129 double Path_Line::LengthToS(double length) {
00130 return length/scalelin;
00131 }
00132 double Path_Line::PathLength(){
00133 return pathlength;
00134 }
00135 Frame Path_Line::Pos(double s) const {
00136 return Frame(orient->Pos(s*scalerot),V_base_start + V_start_end*s*scalelin );
00137 }
00138
00139 Twist Path_Line::Vel(double s,double sd) const {
00140 return Twist( V_start_end*sd*scalelin, orient->Vel(s*scalerot,sd*scalerot) );
00141 }
00142
00143 Twist Path_Line::Acc(double s,double sd,double sdd) const {
00144 return Twist( V_start_end*sdd*scalelin, orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) );
00145 }
00146
00147
00148 Path_Line::~Path_Line() {
00149 if (aggregate)
00150 delete orient;
00151 }
00152
00153 Path* Path_Line::Clone() {
00154 if (aggregate )
00155 return new Path_Line(
00156 Frame(orient->Pos(0),V_base_start),
00157 Frame(orient->Pos(pathlength*scalerot),V_base_end),
00158 orient->Clone(),
00159 eqradius,
00160 true
00161 );
00162
00163 return new Path_Line(
00164 Frame(orient->Pos(0),V_base_start),
00165 Frame(orient->Pos(pathlength*scalerot),V_base_end),
00166 orient,
00167 eqradius,
00168 false
00169 );
00170
00171 }
00172
00173 void Path_Line::Write(std::ostream& os) {
00174 os << "LINE[ ";
00175 os << " " << Frame(orient->Pos(0),V_base_start) << std::endl;
00176 os << " " << Frame(orient->Pos(pathlength*scalerot),V_base_end) << std::endl;
00177 os << " ";orient->Write(os);
00178 os << " " << eqradius;
00179 os << "]" << std::endl;
00180 }
00181
00182
00183 }
00184