namespace Segment {
+static inline double to_deg(double angle) {
+ return angle/M_PI*180;
+}
+
+static inline double to_rad(double angle) {
+ return angle/180.0*M_PI;
+}
+
/**
* Stright segment of the trajectory.
*/
double Ax, Bx, Cx, Dx, Ex, Fx; // constants representing x-axis polynom
double Ay, By, Cy, Dy, Ey, Fy; // constants representing y-axis polynom
double length; // segment length
- Point *edge; // approximated edge
- double distance; // distance form p1 to edge (or p2 to edge as well)
- double omega; // central angle of the edge
- double m; // starting and final speed vector 'length' parameter
+ Point *corner; // Corner point this spline is substituted for
+ double distance; // distance form p1 to the corner (or p2 to the corner as well)
+ double gamma; // central angle of the corner
+ double m; // size of starting and final speed vector (parameter of spline shaping)
double param0, param1; // parameter value at the begining and end of the segment (initially 0 and 1)
double vc; // speed in the center (lowest - according to clothoid approximation)
double tc; // time in the center
public:
- Spline(Point *_p1, Point *_p2, Point *_edge ) :
- p1(_p1), p2(_p2), edge(_edge) {
+ Spline(Point *_p1, Point *_p2, Point *_corner ) :
+ p1(_p1), p2(_p2), corner(_corner) {
- const double A_SHAPE = 6860; // constants for shaping a clothoid like spline
+ const double A_SHAPE = 6860; // constants for shaping a clothoid like spline (derived by heuristics TODO: describe the source)
const double B_SHAPE = 4.4;
- double omegaDeg; // central angle in degrees
param0 = 0;
param1 = 1;
- // Count distance
- distance = p1->distanceTo(*edge);
- if (distance != p2->distanceTo(*edge))
+ // Calculate distance
+ distance = p1->distanceTo(*corner);
+ if (fabs(p2->distanceTo(*corner) - distance) < 1e-3)
dbgPrintf("Error: distances must be equal!");
- // Count central angle
- double angle1 = edge->angleTo(*p1);
- double angle2 = edge->angleTo(*p2);
- omega = fabs(angle1 - angle2);
- if (omega > M_PI)
- omega = (2*M_PI) - omega; // !!!
- omegaDeg = (omega*180/M_PI);
- if (omegaDeg < 7)
+ // Calculate central angle
+ double angle1 = corner->angleTo(*p1);
+ double angle2 = corner->angleTo(*p2);
+ gamma = fabs(angle1 - angle2);
+ if (gamma > M_PI)
+ gamma = (2*M_PI) - gamma; // !!!
+ if (gamma < to_rad(7.0))
{
- m = 0.0423 * omegaDeg + 0.008;
-// printf("---- too small curve %lf - m=%lf ", omegaDeg, m);
+ // Heurictics - TODO: link to matlab
+ m = 0.0423 * to_deg(gamma) + 0.008;
+ // FIXME: Multiply by distance
+// printf("---- too small curve %lf - m=%lf ", gammaDeg, m);
}
else
{
// Count vectors to shape splnine like a clothoid using eliptic approximation of relation
- m = sqrt(B_SHAPE - pow((omegaDeg-180)/180*2*M_PI, 2)/A_SHAPE); // experimental result
+ m = sqrt(B_SHAPE - pow(gamma-M_PI, 2)/A_SHAPE); // experimental result
m = m * distance;
}
- // Count polynom constants
+ // Calculate polynom constants (TODO: link to m-file)
double x0 = p1->x;
double y0 = p1->y;
double x1 = p2->x;
Ey = yy0;
Fy = y0;
- // count length (using numeric integration, there is no better way)
- const double SENS = 100; // sensitivity of numeric integration
- double dl;
- Point * pp1 = new Point(0, 0);
- Point * pp2 = new Point(0, 0);
- getPointAtParam(param0, pp2);
+ // Calculate length (using numeric integration, there is no better way)
+ const int SENS = 100; // sensitivity of numeric integration
+ Point pp1 = Point(0, 0);
+ Point pp2 = Point(0, 0);
+ getPointAtParam(param0, &pp2);
length = 0;
- for (double i = (param0 + (param1-param0)/SENS);
- i <= param1;
- i+=((param1-param0)/SENS) ) {
- pp1->x = pp2->x;
- pp1->y = pp2->y;
- getPointAtParam(i, pp2);
- dl = pp1->distanceTo(*pp2);
+ for (int i = 1; i <= SENS; i++) {
+ double par = (double)i/SENS*(param1-param0);
+ double dl;
+ pp1.x = pp2.x;
+ pp1.y = pp2.y;
+ getPointAtParam(par, &pp2);
+ dl = pp1.distanceTo(pp2);
length += dl;
}
virtual bool isSpline() const { return true; }
/**
- * Finds maximal speeds vc, v1 and v2 (in the middle, in the beginning and in the end).
+ * Finds maximal speeds vc, v1 and v2 (in the middle, in the beginning and at the end).
* The speed profile looks like a letter 'v' because of clothoid's curvature profile shape.
* This is a simplified approach, but quick. Other constraints play a significant role.
* @TODO better acquaint with the constraint meaning
* @TODO display constraints with on-line speed and accs values
*/
virtual void setMaxV(const TrajectoryConstraints &constr) {
- double maxrParam; // point with the smallest radius of curvature
- double maxr; // the radius
+ double minrParam; // point with the smallest radius of curvature
+ double minr; // the radius
if (param0 <= 0.5 && param1 >=0.5)
- maxrParam = 0.5;
+ minrParam = 0.5;
else {
if (param0 > 0.5)
- maxrParam = param0;
+ minrParam = param0;
if (param1 < 0.5)
- maxrParam = param1;
+ minrParam = param1;
}
- maxr = fabs(getRadiusAtParam(maxrParam));
+ minr = fabs(getRadiusAtParam(minrParam));
vc = constr.maxv;
- vc = fmin(vc, constr.maxomega * maxr);
+ vc = fmin(vc, constr.maxomega * minr);
// angular speed depends on radius
- vc = fmin(vc, sqrt(constr.maxcenacc * maxr));
+ vc = fmin(vc, sqrt(constr.maxcenacc * minr));
- vc = fmin(vc, constr.maxangacc*length/2*maxr);
+ // TODO: How was this derived? Check units!
+ vc = fmin(vc, constr.maxangacc*length/2*minr);
// vc ... speed in the middle of the spline
#ifdef MATLAB_MEX_FILE
mexEvalString(str);
#endif
- v1 = constr.maxv/2;
- v2 = constr.maxv/2;
+ // Find optimal v1 and v2 by binary chopping
+ v1 = constr.maxv/2.0;
+ v2 = constr.maxv/2.0;
double maxvc = vc;
- double step = v1 / 2;
- double stepc = vc / 2;
+ double step = v1 / 2.0;
+ double stepc = vc / 2.0;
// speed optimalisation
for (int i =0; i<30; i++) {
*/
double dist2param(double distance) {
double par;
-
+ // FIXME: This cannot be so simple! Parameter is not linear to
+ // distance.
if (distance >= 0) {
par = (param1-param0)*distance/length;
par += param0;
getPointAt(-distance, newEnd);
if (distance > 0) { // end being cut off
p2 = newEnd;
- param1 -= (param1-param0)*distance/length;
+ param1 -= (param1-param0)*distance/length; // FIXME: Use dist2param()
// (param1 - newParam1) / (param1-param0) == distance/length
} else { // begining being cut off
p1 = newEnd;
distance = -distance;
- param0 += (param1-param0)*distance/length;
+ param0 += (param1-param0)*distance/length; // FIXME: Use dist2param()
// (newParam0 - param0) / (param1-param0) == distance/length
}
length -= distance;
}
Point * pt = new Point();
- getPointAt(distance, pt);
+ getPointAt(distance, pt); // FIXME: Avoid this time consuming calculations
// getPointAtParam(param0+(param1-param0)*fraction, pt);
rp.x = pt->x;
rp.y = pt->y;
t1 = time;
+ // FIXME: What's this?
double dpar = fmin(param1, 0.5) - fmin(param0, 0.5);
double ds = dpar * length;
double vp = (v1+vc) / 2;
projects / eurobot / public.git / commitdiff