Skip to content

Commit

Permalink
add polynomial path blending
Browse files Browse the repository at this point in the history
  • Loading branch information
Lars Berscheid committed Nov 19, 2020
1 parent b2ea4ba commit 4bd6f9e
Show file tree
Hide file tree
Showing 5 changed files with 257 additions and 167 deletions.
69 changes: 49 additions & 20 deletions include/movex/path/path.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,42 +27,71 @@ class Path {
std::tuple<std::shared_ptr<Segment>, double> get_local(double s) const {
auto ptr = std::lower_bound(cumulative_lengths.begin(), cumulative_lengths.end(), s);
size_t index = std::distance(cumulative_lengths.begin(), ptr);
index = std::min(index, segments.size() - 1);

auto segment = segments[index];
auto segment = segments.at(index);
double s_local = (index == 0) ? s : s - cumulative_lengths[index - 1];
return {segment, s_local};
}

inline double Power(double v, int e) {
return std::pow(v, e);
}

std::tuple<QuinticPolynomialSegment, Vector7d> blend_polynomial(const Vector7d& lb, const Vector7d& lm, const Vector7d& rb, const Vector7d& rm, const Vector7d& sMid, const Vector7d& maxDiff) {
Vector7d sAbs_ = (-8*(lb.array() + 2*maxDiff.array() - rb.array() + lm.array()*sMid.array()))/(3.*(lm - rm).array());
Vector7d sAbsMin = Vector7d::Constant(sAbs_.minCoeff());
std::tuple<std::shared_ptr<QuinticSegment>, double> blend_polynomial(const Vector7d& lb, const Vector7d& lm, const Vector7d& rb, const Vector7d& rm, double s_mid, double max_diff) {
Vector7d sAbs_ = ((-16*max_diff)/(3.*(lm - rm).array())).abs();
double s_abs_min = sAbs_.minCoeff();

Vector7d a = Vector7d::Zero();
Vector7d b = (lm - rm).array() / (16.*sAbsMin.array().pow(3));
Vector7d c = ((-lm + rm).array()*sMid.array())/(4.*sAbsMin.array().pow(3));
Vector7d d = (-3*(lm - rm).array()*(sAbsMin.array().pow(2) - sMid.array().pow(2)))/(8.*sAbsMin.array().pow(3));
Vector7d e = (lm.array()*(2*sAbsMin.array() - sMid.array())*(sAbsMin + sMid).array().pow(2) + rm.array()*(sAbsMin - sMid).array().pow(2)*(2*sAbsMin.array() + sMid.array()))/(4.*sAbsMin.array().pow(3));
Vector7d f = lb.array() - ((lm - rm).array()*(sAbsMin - sMid).array().pow(3)*(3*sAbsMin.array() + sMid.array()))/(16.*sAbsMin.array().pow(3));

auto blend = QuinticPolynomialSegment(a, b, c, d, e, f);
return {blend, sAbsMin};
Vector7d b = (lm - rm).array() / (16.*std::pow(s_abs_min, 3));
Vector7d c = (-lm + rm).array() / (4.*std::pow(s_abs_min, 2));
Vector7d d = Vector7d::Zero();
Vector7d e = lm;
Vector7d f = lb.array() + lm.array()*(-s_abs_min + s_mid);

return {std::make_shared<QuinticSegment>(a, b, c, d, e, f, 2*s_abs_min), s_abs_min};
}

public:
constexpr static size_t degrees_of_freedom {7};

explicit Path(const std::vector<PathPoint>& waypoints) {
double cumulative_length {0.0};
std::vector<std::shared_ptr<LineSegment>> line_segments;

for (size_t i = 0; i < waypoints.size() - 1; i += 1) {
auto segment = std::make_shared<LineSegment>(waypoints[i].point, waypoints[i+1].point);
cumulative_length += segment->get_length();
segments.emplace_back(segment);
cumulative_lengths.emplace_back(cumulative_length);
line_segments.emplace_back(segment);
}

double cumulative_length {0.0};
for (size_t i = 1; i < waypoints.size() - 1; i += 1) {
if (waypoints[i].blend_max_distance > 0.0) {
auto& left = line_segments[i - 1];
auto& right = line_segments[i];

Vector7d lm = (left->end - left->start) / left->get_length();
Vector7d rm = (right->end - right->start) / right->get_length();

auto [blend, s_abs] = blend_polynomial(left->start, lm, right->start, rm, left->get_length(), waypoints[i].blend_max_distance);
auto new_left = std::make_shared<LineSegment>(left->start, left->q(left->get_length() - s_abs));
auto new_right = std::make_shared<LineSegment>(right->q(s_abs), right->end);

cumulative_length += new_left->get_length();
segments.emplace_back(new_left);
cumulative_lengths.emplace_back(cumulative_length);

cumulative_length += 2 * s_abs;
segments.emplace_back(blend);
cumulative_lengths.emplace_back(cumulative_length);

right = new_right;

} else {
cumulative_length += line_segments[i - 1]->get_length();
segments.emplace_back(line_segments[i - 1]);
cumulative_lengths.emplace_back(cumulative_length);
}
}

cumulative_length += line_segments.back()->get_length();
segments.emplace_back(line_segments.back());
cumulative_lengths.emplace_back(cumulative_length);
length = cumulative_length;
}

Expand Down
39 changes: 10 additions & 29 deletions include/movex/path/segment.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@ struct Segment {


class LineSegment: public Segment {
public:
Vector7d start, end;

public:
explicit LineSegment(const Vector7d& start, const Vector7d&end): start(start), end(end) {
Vector7d diff = end - start;

Expand Down Expand Up @@ -56,24 +56,8 @@ class CircleSegment: public Segment {
Vector7d center, x, y;
double radius;

explicit CircleSegment(const Vector7d& start, const Vector7d& intersection, const Vector7d& end, double max_deviation) {
const Vector7d start_direction = (intersection - start).normalized();
const Vector7d end_direction = (end - intersection).normalized();

const double start_distance = (start - intersection).norm();
const double end_distance = (end - intersection).norm();

double distance = std::min((start - intersection).norm(), (end - intersection).norm());
const double angle = acos(start_direction.dot(end_direction));
explicit CircleSegment(const Vector7d& center, const Vector7d& start, double angle = 2 * M_PI) {

distance = std::min(distance, max_deviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle))); // enforce max deviation

radius = distance / tan(0.5 * angle);
length = angle * radius;

center = intersection + (end_direction - start_direction).normalized() * radius / cos(0.5 * angle);
x = (intersection - distance * start_direction - center).normalized();
y = start_direction;
}

double get_length() const {
Expand Down Expand Up @@ -102,11 +86,13 @@ class CircleSegment: public Segment {
};


class QuinticPolynomialSegment: public Segment {
Vector7d a, b, c, d, e, f;
class QuinticSegment: public Segment {
double s_length;

public:
explicit QuinticPolynomialSegment(const Vector7d& a, const Vector7d& b, const Vector7d& c, const Vector7d& d, const Vector7d& e, const Vector7d& f): a(a), b(b), c(c), d(d), e(e), f(f) {
Vector7d a, b, c, d, e, f;

explicit QuinticSegment(const Vector7d& a, const Vector7d& b, const Vector7d& c, const Vector7d& d, const Vector7d& e, const Vector7d& f, double s_length): a(a), b(b), c(c), d(d), e(e), f(f), s_length(s_length) {
// Numerical integration here
length = 1.0;
}
Expand All @@ -120,21 +106,16 @@ class QuinticPolynomialSegment: public Segment {
}

Vector7d pdq(double s) const {
return 5 * a * std::pow(s, 4) + 4 * b * std::pow(s, 3) + 3 * c * std::pow(s, 2) + 2 * d * s + e;
return e + s * (2 * d + s * (3 * c + s * (4 * b + s * 5 * a)));
}

Vector7d pddq(double s) const {
return 20 * a * std::pow(s, 3) + 12 * b * std::pow(s, 2) + 6 * c * s + 2 * d;
return 2 * d + s * (6 * c + s * (12 * b + s * 20 * a));
}

Vector7d pdddq(double s) const {
return 60 * a * std::pow(s, 2) + 24 * b * s + 6 * c;
return 6 * c + s * (24 * b + s * 60 * a);
}
};


class SplineSegment: public Segment {

};

} // namespace movex
Loading

0 comments on commit 4bd6f9e

Please sign in to comment.