From aa9a66a20bcd2cf1cdfbb664dfd3b5f407a224e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Mon, 11 Jan 2021 13:52:08 +0100 Subject: [PATCH 1/3] Add t_start and t_end to extremum candidates. --- .../polynomial_optimization_linear_impl.h | 48 ++++++++++++------- 1 file changed, 30 insertions(+), 18 deletions(-) diff --git a/mav_trajectory_generation/include/mav_trajectory_generation/impl/polynomial_optimization_linear_impl.h b/mav_trajectory_generation/include/mav_trajectory_generation/impl/polynomial_optimization_linear_impl.h index ae36113d..4f7c6f96 100644 --- a/mav_trajectory_generation/include/mav_trajectory_generation/impl/polynomial_optimization_linear_impl.h +++ b/mav_trajectory_generation/include/mav_trajectory_generation/impl/polynomial_optimization_linear_impl.h @@ -414,32 +414,44 @@ void PolynomialOptimization<_N>:: computeSegmentMaximumMagnitudeCandidatesBySampling( const Segment& segment, double t_start, double t_stop, double dt, std::vector* candidates) { - Eigen::VectorXd value_old, value_start; - value_start = segment.evaluate(t_start - dt, Derivative); - - value_old = segment.evaluate(t_start, Derivative); - - // Determine initial direction from t_start -dt to t_start. - // t_start may be an extremum, especially for start and end vertices! - double direction = value_old.norm() - value_start.norm(); - - // Continue with direction from t_start to t_start + dt until t_stop + dt. - // Again, there may be an extremum at t_stop (e.g. end vertex). - for (double t = t_start + dt; t < t_stop + dt; t += dt) { - Eigen::VectorXd value_new; + CHECK_NOTNULL(candidates); + // Start is candidate. + candidates->push_back(t_start); + + // Determine initial direction from t_start to t_start + dt. + auto t_old = t_start + dt; + auto value_new = segment.evaluate(t_old, Derivative); + auto value_old = segment.evaluate(t_start, Derivative); + auto direction = value_new.norm() - value_old.norm(); + + // Continue with direction from t_start + dt to t_start + 2 dt until t_stop. + bool last_sample = false; + for (double t = t_start + dt + dt; t <= t_stop; t += dt) { + // Update direction. + value_old = value_new; value_new = segment.evaluate(t, Derivative); - - double direction_new = value_new.norm() - value_old.norm(); + auto direction_new = value_new.norm() - value_old.norm(); if (std::signbit(direction) != std::signbit(direction_new)) { - Eigen::VectorXd value_deriv = segment.evaluate(t - dt, Derivative + 1); + auto value_deriv = segment.evaluate(t_old, Derivative + 1); if (value_deriv.norm() < 1e-2) { - candidates->push_back(t - dt); // extremum was at last dt + candidates->push_back(t_old); // extremum was at last dt } } - value_old = value_new; direction = direction_new; + t_old = t; + + // Check last sample before t_stop. + if ((t + dt) > t_stop && !last_sample) { + t = t_stop - dt; + last_sample = true; + } + } + + // End is candidates. + if (candidates->back() != t_stop) { + candidates->push_back(t_stop); } } From e9975a43ccbd16f600f0702682b3eb1914cf5d1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Mon, 11 Jan 2021 13:52:35 +0100 Subject: [PATCH 2/3] Beautify coefficient printing. --- mav_trajectory_generation/src/segment.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mav_trajectory_generation/src/segment.cpp b/mav_trajectory_generation/src/segment.cpp index 1370cfe8..8aa578ed 100644 --- a/mav_trajectory_generation/src/segment.cpp +++ b/mav_trajectory_generation/src/segment.cpp @@ -63,6 +63,7 @@ void printSegment(std::ostream& stream, const Segment& s, int derivative) { stream << " coefficients for " << positionDerivativeToString(derivative) << ": " << std::endl; for (int i = 0; i < s.D(); ++i) { + stream << "dim " << i << ": " << std::endl; stream << s[i].getCoefficients(derivative) << std::endl; } } @@ -211,11 +212,11 @@ bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append, // Get common polynomial order. const int new_N = std::max(segment_to_append.N(), N_); const int new_D = D_ + segment_to_append.D(); - + // Create temporary segments to scale polynomials if necessary. Segment current_segment = *this; Segment segment_to_append_temp = segment_to_append; - + // Scale segment polynomials to the longer segment time. const double new_time = std::max(time_, segment_to_append.getTime()); if (time_ < new_time && new_time > 0.0){ @@ -227,7 +228,7 @@ bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append, segment_to_append_temp[d].scalePolynomialInTime(segment_to_append.getTime() / new_time); } } - + *new_segment = Segment(new_N, new_D); if (N_ == segment_to_append.N()) { From f0517ffa33c922a2cc72c76127ced2cab7d1eb80 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Mon, 11 Jan 2021 13:53:09 +0100 Subject: [PATCH 3/3] Fix check resolution. Consider Nyquist theorem. --- .../test/test_polynomial_optimization.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/mav_trajectory_generation/test/test_polynomial_optimization.cpp b/mav_trajectory_generation/test/test_polynomial_optimization.cpp index 5b1cf1ba..3a228a49 100644 --- a/mav_trajectory_generation/test/test_polynomial_optimization.cpp +++ b/mav_trajectory_generation/test/test_polynomial_optimization.cpp @@ -335,24 +335,18 @@ TEST_P(PolynomialOptimizationTests, ExtremaOfMagnitude) { std::vector res_sampling; time_sampling.Start(); + const double dt = 0.01; opt.computeSegmentMaximumMagnitudeCandidatesBySampling( - s, 0, s.getTime(), 0.01, &res_sampling); + s, 0, s.getTime(), dt, &res_sampling); time_sampling.Stop(); - // First check that the candidates are ACTUAL extrema. - // Do this by evaluating the derivative+1 of the segment. - for (double candidate : res_sampling) { - Eigen::VectorXd sampled = s.evaluate(candidate, kDerivative + 1); - EXPECT_NEAR(0.0, sampled.norm(), 0.01) << "Sampled Result: Time: " - << candidate; - } - - constexpr double check_tolerance = 0.01; + const double check_tolerance = 2 * dt; // Nyquist resolution bool success = checkExtrema(res_sampling, res, check_tolerance); if (!success) { std::cout << "############CHECK XTREMA FAILED: \n"; std::cout << "segment idx: " << segment_idx << "/" << segments.size() - << " time: " << s.getTime() << std::endl; + << " time: " << s.getTime() << std::endl + << "segment: " << s << std::endl; std::cout << "analytically found: "; for (const double& t : res) {