From f6705f7b54936e6c04f0d14e7a2c9da412575774 Mon Sep 17 00:00:00 2001 From: Kotakku Date: Sat, 27 Jan 2024 21:33:31 +0900 Subject: [PATCH 1/3] =?UTF-8?q?CutmulRom=E6=9B=B2=E7=B7=9A=E3=81=BE?= =?UTF-8?q?=E3=81=A7=E5=AE=9F=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + example/path_planning/spline.cpp | 48 ++ include/cpp_robotics/path_planning/spline.hpp | 547 ------------------ .../path_planning/spline_path.hpp | 547 ++++++++++++++++++ 4 files changed, 596 insertions(+), 547 deletions(-) create mode 100644 example/path_planning/spline.cpp delete mode 100644 include/cpp_robotics/path_planning/spline.hpp create mode 100644 include/cpp_robotics/path_planning/spline_path.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index db4074cbf..549bd7a77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,7 @@ set(example_files path_planning/fmt_star path_planning/dwa path_planning/navigation_diffbot + path_planning/spline unit/unit_example ) diff --git a/example/path_planning/spline.cpp b/example/path_planning/spline.cpp new file mode 100644 index 000000000..942dccad1 --- /dev/null +++ b/example/path_planning/spline.cpp @@ -0,0 +1,48 @@ +#include +#include +#include + +int main() +{ + using namespace cpp_robotics; + namespace plt = matplotlibcpp; + + std::vector waypoints = + { + {0, 0}, + {1, 2}, + {2, 3}, + {3, 1}, + {4, 1}, + {5, 0}, + {4, -1}, + {2, -2}, + {1, -1}, + {0, 0}, + }; + + CatumullRomSplinePath spline(waypoints, true); + + std::vector x, y; + const double length = spline.length(); + for (double i = 0; i < length; i += 0.05) + { + Eigen::Vector2d pos = spline.position(i); + x.push_back(pos[0]); + y.push_back(pos[1]); + } + plt::plot(x, y); + + x.clear(); + y.clear(); + // waypoints + for (const auto& p : waypoints) + { + x.push_back(p[0]); + y.push_back(p[1]); + } + plt::plot(x, y, "o"); + plt::show(); + + return 0; +} \ No newline at end of file diff --git a/include/cpp_robotics/path_planning/spline.hpp b/include/cpp_robotics/path_planning/spline.hpp deleted file mode 100644 index 89e8ae435..000000000 --- a/include/cpp_robotics/path_planning/spline.hpp +++ /dev/null @@ -1,547 +0,0 @@ -#include -#include -#include -#include - -#include -#include - -#include "../utility/math_utils.hpp" -#include "../vector/vector2.hpp" -#include "../vector/vector4.hpp" - -namespace cpp_robotics -{ - -/** - * @brief スプライン曲線用関数郡 - * - */ -namespace spline -{ - struct spline_c - { - Vector4d xb, yb; - }; - - /////////////////////////////////////////// base spline functions /////////////////////////////////////////// - spline_c bezier_spline(const Vector2d &p0, const Vector2d &p1, const Vector2d &p2, const Vector2d &p3) - { - return { - Vector4d{p0.x, p1.x, p2.x, p3.x}, - Vector4d{p0.y, p1.y, p2.y, p3.y}}; - } - - spline_c hermite_spline(const Vector2d &p0, const Vector2d &p1, const Vector2d &v0, const Vector2d &v1) - { - Vector2d bp1 = p0 + (1.0 / 3.0) * v0; - Vector2d bp2 = p1 - (1.0 / 3.0) * v1; - - return bezier_spline(p0, bp1, bp2, p1); - } - - spline_c catumull_spline(const Vector2d &p0, const Vector2d &p1, const Vector2d &p2, const Vector2d &p3) - { - Vector2d bp1 = p1 + (1.0 / 6.0) * (p2 - p0); - Vector2d bp2 = p2 - (1.0 / 6.0) * (p3 - p1); - - return bezier_spline(p1, bp1, bp2, p2); - } - - // f(t) = a t^3 + b t^2 + c t + d -> bezier - spline_c cubic_function_to_bezier(const Vector2d &a, const Vector2d &b, const Vector2d &c, const Vector2d &d) - { - Vector2d bp1 = d + (1.0 / 3.0) * c; - Vector2d bp2 = (1.0 / 3.0) * b + (2.0 / 3.0) * c + d; - Vector2d bp3 = a + b + c + d; - - return bezier_spline(d, bp1, bp2, bp3); - } - - /////////////////////////////////////////// bezier weigth & evaluate functions /////////////////////////////////////////// - Vector4d bezier_weight(const double t) - { - double s = 1.0 - t; - - double t2 = t * t; - double t3 = t2 * t; - - double s2 = s * s; - double s3 = s2 * s; - - return Vector4d(s3, 3.0 * s2 * t, 3.0 * s * t2, t3); - } - - Vector4d bezier_weight(const Vector4d t) - { - return Vector4d( - t.x - 3.0 * t.y + 3.0 * t.z - t.w, - 3.0 * t.y - 6.0 * t.z + 3.0 * t.w, - 3.0 * t.z - 3.0 * t.w, - t.w); - } - - inline Vector2d evaluate(const spline_c &spline, const Vector4d &w) - { - return { - Vector4d::dot(spline.xb, w), - Vector4d::dot(spline.yb, w)}; - } - - /////////////////////////////////////////// position, velocity, acceleration /////////////////////////////////////////// - Vector2d position(const spline_c &spline, double t) - { - return evaluate(spline, bezier_weight(t)); - } - - Vector2d velocity(const spline_c &spline, double t) - { - Vector4d dt(0.0, 1.0, 2.0 * t, 3.0 * t * t); - return evaluate(spline, bezier_weight(dt)); - } - - Vector2d acceleration(const spline_c &spline, double t) - { - Vector4d dt(0.0, 0.0, 2.0, 6.0 * t); - return evaluate(spline, bezier_weight(t)); - } - - /////////////////////////////////////////// split /////////////////////////////////////////// - inline void split(const Vector4d &spline, Vector4d &spline0, Vector4d &spline1, double t) - { - // assumption: seg = (P0, P1, P2, P3) - double q0 = lerp(spline.x, spline.y, t); - double q1 = lerp(spline.y, spline.z, t); - double q2 = lerp(spline.z, spline.w, t); - - double r0 = lerp(q0, q1, t); - double r1 = lerp(q1, q2, t); - - double s0 = lerp(r0, r1, t); - - double sx = spline.x; // support aliasing - double sw = spline.w; - - spline0 = Vector4d(sx, q0, r0, s0); - spline1 = Vector4d(s0, r1, q2, sw); - } - - // Optimised for t=0.5 - inline void split(const Vector4d &spline, Vector4d &spline0, Vector4d &spline1) - { - double q0 = (spline.x + spline.y) * 0.5; // x + y / 2 - double q1 = (spline.y + spline.z) * 0.5; // y + z / 2 - double q2 = (spline.z + spline.w) * 0.5; // z + w / 2 - - double r0 = (q0 + q1) * 0.5; // x + 2y + z / 4 - double r1 = (q1 + q2) * 0.5; // y + 2z + w / 4 - - double s0 = (r0 + r1) * 0.5; // q0 + 2q1 + q2 / 4 = x+y + 2(y+z) + z+w / 8 = x + 3y + 3z + w - - double sx = spline.x; // support aliasing - double sw = spline.w; - - spline0 = Vector4d(sx, q0, r0, s0); - spline1 = Vector4d(s0, r1, q2, sw); - } - - void split(const spline_c& spline, spline_c& spline0, spline_c& spline1) - { - split(spline.xb, spline0.xb, spline1.xb); - split(spline.yb, spline0.yb, spline1.yb); - } - - void split(const spline_c& spline, spline_c& spline0, spline_c& spline1, double t) - { - split(spline.xb, spline0.xb, spline1.xb, t); - split(spline.yb, spline0.yb, spline1.yb, t); - } - - /////////////////////////////////////////// length /////////////////////////////////////////// - double length_estimate(const spline_c &s, double *error) - { - // Our convex hull is p0, p1, p2, p3, so p0_p3 is our minimum possible length, and p0_p1 + p1_p2 + p2_p3 our maximum. - double d03 = square(s.xb.x - s.xb.w) + square(s.yb.x - s.yb.w); - - double d01 = square(s.xb.x - s.xb.y) + square(s.yb.x - s.yb.y); - double d12 = square(s.xb.y - s.xb.z) + square(s.yb.y - s.yb.z); - double d23 = square(s.xb.z - s.xb.w) + square(s.yb.z - s.yb.w); - - double minLength = std::sqrt(d03); - double maxLength = std::sqrt(d01) + std::sqrt(d12) + std::sqrt(d23); - - minLength *= 0.5; - maxLength *= 0.5; - - *error = maxLength - minLength; - return minLength + maxLength; - } - - double length(const spline_c &s, double maxError) - { - double error; - double len = length_estimate(s, &error); - - if (error > maxError) - { - spline_c s0; - spline_c s1; - - split(s, s0, s1); - - return length(s0, maxError) + length(s1, maxError); - } - - return len; - } - - double length(const spline_c &s, double t0, double t1, double maxError) - { - assert(t0 >= 0.0 && t0 < 1.0); - assert(t1 >= 0.0 && t1 <= 1.0); - assert(t0 <= t1); - - spline_c s0, s1; - - if (t0 == 0.0) - { - if (t1 == 1.0) - return length(s, maxError); - - split(s, s0, s1, t1); - return length(s0, maxError); - } - else - { - split(s, s0, s1, t0); - - if (t1 == 1.0) - return length(s1, maxError); - - split(s1, s0, s1, (t1 - t0) / (1.0 - t0)); - return length(s0, maxError); - } - } - - /////////////////////////////////////////// curvature /////////////////////////////////////////// - double curvature(const spline_c &spline, double t) - { - Vector2d v = velocity(spline, t); - Vector2d a = acceleration(spline, t); - - double avCrossLen = std::abs(v.x * a.y - v.y * a.x); - double vLen = v.norm(); - - if (vLen == 0.0) - return 1e10; - - return avCrossLen / (vLen * vLen * vLen); - } -} // namespace spline - -/** - * @brief 2次元のスプライン曲線のインターフェイスクラス - * - */ -class Spline2D -{ -public: - Spline2D() = default; - - bool is_empty() - { - return _is_empty; - } - - size_t size() - { - return _size; - } - - size_t point_num() - { - return size()+1; - } - - double length() - { - return _all_length; - } - - double length(size_t i) - { - if(i >= size()) - return 0; - - return _spline[i].length; - } - - Vector2d position(double length) - { - assert(_is_empty == false); - assert(length >= 0); - assert(length <= _all_length); - - segment_info_t segment = get_segmet_idx_length(length); - return spline::position(_spline[segment.i].coeff, segment.t); - } - - Vector2d spline_position(double t) - { - assert(_is_empty == false); - assert(t >= 0); - assert(t <= static_cast(size())); - - segment_info_t segment = get_segmet_idx(t); - return spline::position(_spline[segment.i].coeff, segment.t); - } - - Vector2d spline_velocity(double t) - { - assert(_is_empty == false); - assert(t >= 0); - assert(t <= static_cast(size())); - - size_t i = std::floor(t); - t -= static_cast(i); - return spline::velocity(_spline[i].coeff, t); - } - - Vector2d spline_acceleration(double t) - { - assert(_is_empty == false); - assert(t >= 0); - assert(t <= static_cast(size())); - - size_t i = std::floor(t); - t -= static_cast(i); - return spline::acceleration(_spline[i].coeff, t); - } - -protected: - struct segment_t - { - spline::spline_c coeff; - double length; - std::vector split_lengths; - }; - - struct segment_info_t - { - size_t i; - double t; - }; - - segment_info_t get_segmet_idx(const double t) - { - segment_info_t result; - result.i = std::min(static_cast(std::floor(t)), size()-1); - result.t = t - static_cast(result.i); - return result; - } - - segment_info_t get_segmet_idx_length(double length) - { - segment_info_t result; - result.i = 0; - result.t = 1.0; - while (length > _spline[result.i].length) - { - length -= _spline[result.i].length; - result.i++; - } - - double dt = 1.0 / _spline[result.i].split_lengths.size(); - for(size_t i = 1; i < _spline[result.i].split_lengths.size(); i++) - { - if(length < _spline[result.i].split_lengths[i]) - { - double diff = length - _spline[result.i].split_lengths[i-1]; - double seg_diff = _spline[result.i].split_lengths[i] - _spline[result.i].split_lengths[i-1]; - result.t = static_cast(i) * dt + (diff/seg_diff) * dt; - return result; - } - } - - return result; - } - - std::vector _spline; - bool _is_empty; - size_t _size; - double _all_length; -}; - -/** - * @brief Catumull曲線 - * - */ -class CatumullRom2D : public Spline2D -{ -public: - CatumullRom2D(std::vector& points, bool trajectory_loop = false, const double error = 0.01) - { - const size_t p_size = points.size(); - - _is_empty = false; - switch (p_size) - { - case 0: - _is_empty = true; - return; - case 1: - { - Vector2d& p0 = points[0]; - _spline.push_back({spline::catumull_spline(p0, p0, p0, p0), 0, {}}); - return; - } - case 2: - _size = 1; - _spline.resize(_size); - calcu_segment(_spline[0], points, error, 0, 0, 0, 1, 1); - _all_length = _spline[0].length; - return; - } - - _size = p_size - 1; - _spline.resize(_size); - - if(trajectory_loop) - calcu_segment(_spline[0], points, error, 0, p_size-2, 0, 1, 2); - else - calcu_segment(_spline[0], points, error, 0, 0, 0, 1, 2); - for (size_t i = 0; i < p_size - 3; i++) - { - calcu_segment(_spline[i+1], points, error, i, 0, 1, 2, 3); - } - if(trajectory_loop) - calcu_segment(_spline[_size-1], points, error, 0, p_size-3, p_size-2, p_size-1, 1); - else - calcu_segment(_spline[_size-1], points, error, p_size-3, 0, 1, 2, 2); - - _all_length = 0; - for(auto& seg : _spline) - { - _all_length += seg.length; - } - } - -private: - void calcu_segment(segment_t& seg, std::vector& points, const double error, const size_t start, const size_t i0 = 0, const size_t i1 = 1, const size_t i2 = 2, const size_t i3 = 3) - { - Vector2d &p0 = points[start + i0]; - Vector2d &p1 = points[start + i1]; - Vector2d &p2 = points[start + i2]; - Vector2d &p3 = points[start + i3]; - - seg.coeff = spline::catumull_spline(p0, p1, p2, p3); - seg.length = spline::length(seg.coeff, error); - seg.split_lengths.resize(100); - for(size_t i = 0; i < 100; i++) - { - seg.split_lengths[i] = spline::length(seg.coeff, 0.0, static_cast(i) / 100.0, error); - } - } -}; - -/** - * @brief 3次スプライン曲線 - * - */ -class CubicSpline : public Spline2D -{ -public: - CubicSpline(std::vector& points, const double error = 0.01) - { - const size_t p_size = points.size(); - std::vector w; - - _is_empty = false; - if(p_size < 2) - { - _is_empty = true; - return; - } - - _size = p_size - 1; - _spline.resize(p_size); - w.resize(p_size); - - for(size_t xy = 0; xy < 2; xy++) - { - // 0次の係数 - for(size_t i = 0; i <= _size; i++) - { - spline(i, xy).x = p(points, i, xy); - } - - // 2次の係数 - spline(0, xy).z = spline(_size, xy).z = 0.0; - for(size_t i = 1; i < _size; i++) - { - spline(i, xy).z = 3.0 * (spline(i-1, xy).x - 2.0 * spline(i, xy).x + spline(i+1, xy).x); - } - // 左下消去 - w[0] = 0.0; - for(size_t i = 1; i < _size; i++) - { - double temp = 4.0 - w[i-1]; - spline(i, xy).z = (spline(i, xy).z - spline(i-1, xy).z)/temp; - w[i] = 1.0 / temp; - } - - // 右下消去 - for(size_t i = _size-1; i > 0; i--) { - spline(i, xy).z = spline(i, xy).z - spline(i+1, xy).z * w[i]; - } - - // 1次と3次の係数 - spline(_size, xy).y = spline(_size, xy).w = 0.0; - for(size_t i = 0; i < _size; i++) - { - spline(i, xy).w = ( spline(i+1, xy).z - spline(i, xy).z) / 3.0; - spline(i, xy).y = spline(i+1, xy).x - spline(i, xy).x - spline(i, xy).z - spline(i, xy).w; - } - } - // 最後のセグメントは最後の制御点一点ようなので消しておく - _spline.erase(_spline.begin()+_spline.size()-1); - - _all_length = 0; - for(auto& seg : _spline) - { - auto c = seg.coeff; - seg.coeff = spline::cubic_function_to_bezier( - Vector2d(c.xb.w, c.yb.w), - Vector2d(c.xb.z, c.yb.z), - Vector2d(c.xb.y, c.yb.y), - Vector2d(c.xb.x, c.yb.x) - ); - seg.length = spline::length(seg.coeff, error); - seg.split_lengths.resize(100); - for(size_t i = 0; i < 100; i++) - { - seg.split_lengths[i] = spline::length(seg.coeff, 0.0, static_cast(i) / 100.0, error); - } - _all_length += seg.length; - } - } - -private: - Vector4d& spline(size_t i, size_t xy) - { - if (xy == 0) - return _spline[i].coeff.xb; - else - return _spline[i].coeff.yb; - } - - double& p(std::vector& points, size_t i, size_t xy) - { - if (xy == 0) - return points[i].x; - else - return points[i].y; - } -}; - -} \ No newline at end of file diff --git a/include/cpp_robotics/path_planning/spline_path.hpp b/include/cpp_robotics/path_planning/spline_path.hpp new file mode 100644 index 000000000..e1cd7a0d3 --- /dev/null +++ b/include/cpp_robotics/path_planning/spline_path.hpp @@ -0,0 +1,547 @@ +#pragma once + +#include +#include "cpp_robotics/optimize/golden_serach.hpp" +#include "cpp_robotics/utility/math_utils.hpp" + +namespace cpp_robotics +{ + +template +struct eigen_spline_api +{ + using Vector = VectorType; + static constexpr int Dim = Vector::RowsAtCompileTime; + using Scalar = typename Vector::Scalar; + using SplineCoeff = Eigen::Matrix; + using WeightVector = Eigen::Matrix; + + /////////////////////////////////////////// base spline functions /////////////////////////////////////////// + static SplineCoeff bezier_spline(const Vector &p0, const Vector &p1, const Vector &p2, const Vector &p3) + { + SplineCoeff coeff; + coeff << p0, p1, p2, p3; + return coeff; + } + + static SplineCoeff hermite_spline(const Vector &p0, const Vector &p1, const Vector &v0, const Vector &v1) + { + Vector bp1 = p0 + (1.0 / 3.0) * v0; + Vector bp2 = p1 - (1.0 / 3.0) * v1; + return bezier_spline(p0, bp1, bp2, p1); + } + + static SplineCoeff catumull_spline(const Vector &p0, const Vector &p1, const Vector &p2, const Vector &p3) + { + Vector bp1 = p1 + (1.0 / 6.0) * (p2 - p0); + Vector bp2 = p2 - (1.0 / 6.0) * (p3 - p1); + return bezier_spline(p1, bp1, bp2, p2); + } + + // f(t) = a t^3 + b t^2 + c t + d -> bezier + static SplineCoeff cubic_function_to_bezier(const Vector &a, const Vector &b, const Vector &c, const Vector &d) + { + Vector bp1 = d + (1.0 / 3.0) * c; + Vector bp2 = (1.0 / 3.0) * b + (2.0 / 3.0) * c + d; + Vector bp3 = a + b + c + d; + return bezier_spline(d, bp1, bp2, bp3); + } + + /////////////////////////////////////////// bezier weigth & evaluate functions /////////////////////////////////////////// + static WeightVector bezier_weight(const double t) + { + double s = 1.0 - t; + double t2 = t * t; + double t3 = t2 * t; + double s2 = s * s; + double s3 = s2 * s; + return WeightVector(s3, 3.0 * s2 * t, 3.0 * s * t2, t3); + } + + static WeightVector bezier_weight(const WeightVector t) + { + return WeightVector( + t(0) - 3.0 * t(1) + 3.0 * t(2) - t(3), + 3.0 * t(1) - 6.0 * t(2) + 3.0 * t(3), + 3.0 * t(2) - 3.0 * t(3), + t(3)); + } + + static inline Vector evaluate(const SplineCoeff &spline, const WeightVector &w) + { + return spline * w; + } + + /////////////////////////////////////////// position, velocity, acceleration /////////////////////////////////////////// + static Vector position(const SplineCoeff &spline, double t) + { + return evaluate(spline, bezier_weight(t)); + } + + static Vector velocity(const SplineCoeff &spline, double t) + { + WeightVector dt(0.0, 1.0, 2.0 * t, 3.0 * t * t); + return evaluate(spline, bezier_weight(dt)); + } + + static Vector acceleration(const SplineCoeff &spline, double t) + { + WeightVector ddt(0.0, 0.0, 2.0, 6.0 * t); + return evaluate(spline, bezier_weight(ddt)); + } + + /////////////////////////////////////////// split /////////////////////////////////////////// + static inline void split(const WeightVector &spline, WeightVector &spline0, WeightVector &spline1, double t) + { + // assumption: seg = (P0, P1, P2, P3) + double q0 = lerp(spline(0), spline(1), t); + double q1 = lerp(spline(1), spline(2), t); + double q2 = lerp(spline(2), spline(3), t); + double r0 = lerp(q0, q1, t); + double r1 = lerp(q1, q2, t); + double s0 = lerp(r0, r1, t); + double sx = spline(0); + double sw = spline(3); + + spline0 = WeightVector(sx, q0, r0, s0); + spline1 = WeightVector(s0, r1, q2, sw); + } + + // Optimised for t=0.5 + static inline void split(const WeightVector &spline, WeightVector &spline0, WeightVector &spline1) + { + double q0 = (spline(0) + spline(1)) * 0.5; // x + y / 2 + double q1 = (spline(1) + spline(2)) * 0.5; // y + z / 2 + double q2 = (spline(2) + spline(3)) * 0.5; // z + w / 2 + double r0 = (q0 + q1) * 0.5; // x + 2y + z / 4 + double r1 = (q1 + q2) * 0.5; // y + 2z + w / 4 + double s0 = (r0 + r1) * 0.5; // q0 + 2q1 + q2 / 4 = x+y + 2(y+z) + z+w / 8 = x + 3y + 3z + w + double sx = spline(0); + double sw = spline(3); + + spline0 = WeightVector(sx, q0, r0, s0); + spline1 = WeightVector(s0, r1, q2, sw); + } + + static void split(const SplineCoeff& spline, SplineCoeff& spline0, SplineCoeff& spline1) + { + WeightVector sp0, sp1; + for(int i = 0; i < Dim; i++) + { + split(spline.row(i), sp0, sp1); + spline0.row(i) = sp0; + spline1.row(i) = sp1; + } + } + + static void split(const SplineCoeff& spline, SplineCoeff& spline0, SplineCoeff& spline1, double t) + { + WeightVector sp0, sp1; + for(int i = 0; i < Dim; i++) + { + split(spline.row(i), sp0, sp1, t); + spline0.row(i) = sp0; + spline1.row(i) = sp1; + } + } + + /////////////////////////////////////////// length /////////////////////////////////////////// + static double length_estimate(const SplineCoeff &s, double &error) + { + double d03 = (s.col(0) - s.col(3)).squaredNorm(); + double d01 = (s.col(0) - s.col(1)).squaredNorm(); + double d12 = (s.col(1) - s.col(2)).squaredNorm(); + double d23 = (s.col(2) - s.col(3)).squaredNorm(); + + double min_length = std::sqrt(d03); + double max_length = std::sqrt(d01) + std::sqrt(d12) + std::sqrt(d23); + + min_length *= 0.5; + max_length *= 0.5; + + error = max_length - min_length; + return min_length + max_length; + } + + static double length(const SplineCoeff &s, double max_error) + { + double error; + double len = length_estimate(s, error); + + if (error > max_error) + { + SplineCoeff s0; + SplineCoeff s1; + split(s, s0, s1); + return length(s0, max_error) + length(s1, max_error); + } + + return len; + } + + static double length(const SplineCoeff &s, double t0, double t1, double max_error) + { + assert(0.0 <= t0 && t0 < 1.0); + assert(0.0 <= t1 && t1 <= 1.0); + assert(t0 <= t1); + + if(t0 == t1) + return 0.0; + + SplineCoeff s0, s1; + if (t0 == 0.0) + { + if (t1 == 1.0) + return length(s, max_error); + split(s, s0, s1, t1); + return length(s0, max_error); + } + else + { + split(s, s0, s1, t0); + if (t1 == 1.0) + return length(s1, max_error); + split(s1, s0, s1, (t1 - t0) / (1.0 - t0)); + return length(s0, max_error); + } + } +}; + +template +class SplinePathBase +{ +protected: + using spline_api = eigen_spline_api; +public: + using Vector = VectorType; + SplinePathBase() = default; + SplinePathBase(const std::vector &waypoints) + { + set_path(waypoints); + } + + virtual void set_path(const std::vector &waypoints) = 0; + + bool is_empty() const { return waypoints_.size() == 0; } + size_t waypoints_size() const { return waypoints_.size(); } + double length() const { return total_length_; } + + double nearest_position(const Vector pos, double now_length = -1, double search_range = -1) + { + double low, high; + if(now_length < 0) + { + low = 0; + high = total_length_; + } + else + { + if(search_range < 0) + search_range = total_length_ * 0.3; + low = now_length - search_range; + high = now_length + search_range; + low = std::clamp(low, 0.0, total_length_); + high = std::clamp(high, 0.0, total_length_); + } + + return golden_search([&, pos](double length) + { + return (position(length) - pos).squaredNorm(); + }, low, high, total_length_*5e-3); + } + + Vector position(double length) const + { + assert(is_empty() == false); + length = std::clamp(length, 0.0, total_length_); + segment_info_t segment = get_segmet_idx_length(length); + return spline_api::position(spline_segments_[segment.i].coeff, segment.t); + } + + Vector direction(double length) const + { + assert(is_empty() == false); + length = std::clamp(length, 0.0, total_length_); + segment_info_t segment = get_segmet_idx_length(length); + return spline_api::velocity(spline_segments_[segment.i].coeff, segment.t).normalized(); + } + +protected: + std::vector waypoints_; + std::vector length_; + double total_length_ = 0; + + struct segment_t + { + spline_api::SplineCoeff coeff; + double length; + std::vector split_lengths; + }; + + std::vector spline_segments_; + + struct segment_info_t + { + size_t i; + double t; + }; + + segment_info_t get_segmet_idx(const double t) const + { + segment_info_t result; + if(waypoints_size() < 1) + { + result.i = 0; + result.t = 0; + return result; + } + result.i = std::min(static_cast(std::floor(t)), waypoints_size()-1); + result.t = t - static_cast(result.i); + return result; + } + + segment_info_t get_segmet_idx_length(double length) const + { + segment_info_t result; + result.i = 0; + result.t = 1.0; + while (length > spline_segments_[result.i].length) + { + length -= spline_segments_[result.i].length; + result.i++; + } + + double dt = 1.0 / spline_segments_[result.i].split_lengths.size(); + for(size_t i = 1; i < spline_segments_[result.i].split_lengths.size(); i++) + { + if(length < spline_segments_[result.i].split_lengths[i]) + { + double diff = length - spline_segments_[result.i].split_lengths[i-1]; + double seg_diff = spline_segments_[result.i].split_lengths[i] - spline_segments_[result.i].split_lengths[i-1]; + result.t = static_cast(i) * dt + (diff/seg_diff) * dt; + return result; + } + } + + return result; + } +}; + +template +class CatumullRomSplinePath : public SplinePathBase +{ + using spline_api = eigen_spline_api; + using segment_t = typename SplinePathBase::segment_t; +public: + static constexpr double DEFAULT_LENGTH_ERROR = 0.05; + using Vector = VectorType; + CatumullRomSplinePath() = default; + CatumullRomSplinePath(std::vector& waypoints, bool trajectory_loop = false, const double error = DEFAULT_LENGTH_ERROR) + { + set_path_with_config(waypoints, trajectory_loop, error); + } + + void set_path_with_config(std::vector& waypoints, bool trajectory_loop = false, const double error = DEFAULT_LENGTH_ERROR) + { + trajectory_loop_ = trajectory_loop; + error_ = error; + set_path(waypoints); + } + + virtual void set_path(const std::vector &waypoints) override + { + auto &waypoints_ = this->waypoints_; + auto &spline_segments_ = this->spline_segments_; + auto &total_length_ = this->total_length_; + + waypoints_ = waypoints; + const size_t p_size = this->waypoints_.size(); + + switch (p_size) + { + case 0: + return; + case 1: + { + Vector& p0 = waypoints_[0]; + spline_segments_.push_back({spline_api::catumull_spline(p0, p0, p0, p0), 0, {}}); + return; + } + case 2: + spline_segments_.resize(1); + calcu_segment(spline_segments_[0], waypoints_, error_, 0, 0, 0, 1, 1); + total_length_ = spline_segments_[0].length; + return; + } + + const int segment_size = p_size - 1; + spline_segments_.resize(segment_size); + + if(trajectory_loop_) + calcu_segment(spline_segments_[0], waypoints_, error_, 0, p_size-2, 0, 1, 2); + else + calcu_segment(spline_segments_[0], waypoints_, error_, 0, 0, 0, 1, 2); + for (size_t i = 0; i < p_size - 3; i++) + { + calcu_segment(spline_segments_[i+1], waypoints_, error_, i, 0, 1, 2, 3); + } + if(trajectory_loop_) + calcu_segment(spline_segments_[segment_size-1], waypoints_, error_, 0, p_size-3, p_size-2, p_size-1, 1); + else + calcu_segment(spline_segments_[segment_size-1], waypoints_, error_, p_size-3, 0, 1, 2, 2); + + total_length_ = 0; + for(auto& seg : spline_segments_) + { + total_length_ += seg.length; + } + } + +private: + void calcu_segment(segment_t& seg, std::vector& waypoints_, const double error, const size_t start, const size_t i0 = 0, const size_t i1 = 1, const size_t i2 = 2, const size_t i3 = 3) + { + Vector &p0 = waypoints_[start + i0]; + Vector &p1 = waypoints_[start + i1]; + Vector &p2 = waypoints_[start + i2]; + Vector &p3 = waypoints_[start + i3]; + + seg.coeff = spline_api::catumull_spline(p0, p1, p2, p3); + seg.length = spline_api::length(seg.coeff, error_); + seg.split_lengths.resize(100); + for(size_t i = 0; i < 100; i++) + { + seg.split_lengths[i] = spline_api::length(seg.coeff, 0.0, static_cast(i) / 100.0, error); + } + } + + bool trajectory_loop_ = false; + double error_ = DEFAULT_LENGTH_ERROR; +}; + +// template +// class CubicSplinePath : public SplinePathBase +// { +// using spline_api = eigen_spline_api; +// using segment_t = typename SplinePathBase::segment_t; +// using Vector4d = typename spline_api::Vector4d; +// public: +// static constexpr double DEFAULT_LENGTH_ERROR = 0.05; +// using Vector = VectorType; + +// CubicSplinePath() = default; +// CubicSplinePath(std::vector& waypoints, const double error = DEFAULT_LENGTH_ERROR) +// { +// set_path_with_config(waypoints, error); +// } + +// void set_path_with_config(std::vector& waypoints, const double error = DEFAULT_LENGTH_ERROR) +// { +// error_ = error; +// set_path(waypoints); +// } + +// virtual void set_path(const std::vector &waypoints) override +// { +// auto &waypoints_ = this->waypoints_; +// auto &spline_segments_ = this->spline_segments_; +// auto &total_length_ = this->total_length_; + +// waypoints_ = waypoints; +// const size_t p_size = this->waypoints_.size(); + +// switch (p_size) +// { +// case 0: +// return; +// case 1: +// { +// Vector& p0 = waypoints_[0]; +// spline_segments_.push_back({spline_api::catumull_spline(p0, p0, p0, p0), 0, {}}); +// return; +// } +// } + +// const size_t p_size = points.size(); +// std::vector w; + +// const size_t segment_size = p_size - 1; +// spline_segments_.resize(p_size); +// w.resize(p_size); + +// // for(size_t dim = 0; xy < 2; xy++) +// { +// for(size_t i = 0; i <= segment_size; i++) +// { +// // spline(i, xy).x = p(points, i, xy); +// spline_segments_[i].coeff +// } +// // spline(0, xy).z = spline(segment_size, xy).z = 0.0; +// // for(size_t i = 1; i < segment_size; i++) +// // { +// // spline(i, xy).z = 3.0 * (spline(i-1, xy).x - 2.0 * spline(i, xy).x + spline(i+1, xy).x); +// // } +// // w[0] = 0.0; +// // for(size_t i = 1; i < segment_size; i++) +// // { +// // double temp = 4.0 - w[i-1]; +// // spline(i, xy).z = (spline(i, xy).z - spline(i-1, xy).z)/temp; +// // w[i] = 1.0 / temp; +// // } +// // for(size_t i = segment_size-1; i > 0; i--) { +// // spline(i, xy).z = spline(i, xy).z - spline(i+1, xy).z * w[i]; +// // } +// // spline(segment_size, xy).y = spline(segment_size, xy).w = 0.0; +// // for(size_t i = 0; i < segment_size; i++) +// // { +// // spline(i, xy).w = ( spline(i+1, xy).z - spline(i, xy).z) / 3.0; +// // spline(i, xy).y = spline(i+1, xy).x - spline(i, xy).x - spline(i, xy).z - spline(i, xy).w; +// // } +// } +// spline_segments_.erase(spline_segments_.begin()+spline_segments_.size()-1); + +// // _all_length = 0; +// // for(auto& seg : spline_segments_) +// // { +// // auto c = seg.coeff; +// // seg.coeff = spline::cubic_function_to_bezier( +// // Vector2d(c.xb.w, c.yb.w), +// // Vector2d(c.xb.z, c.yb.z), +// // Vector2d(c.xb.y, c.yb.y), +// // Vector2d(c.xb.x, c.yb.x) +// // ); +// // seg.length = spline::length(seg.coeff, error); +// // seg.split_lengths.resize(100); +// // for(size_t i = 0; i < 100; i++) +// // { +// // seg.split_lengths[i] = spline::length(seg.coeff, 0.0, static_cast(i) / 100.0, error); +// // } +// // _all_length += seg.length; +// // } +// } + +// private: +// // Vector4d& spline(size_t i, size_t dim_index) +// // { +// // if (xy == 0) +// // return spline_segments_[i].coeff.xb; +// // else +// // return spline_segments_[i].coeff.yb; +// // } + +// // double& p(std::vector& points, size_t i, size_t xy) +// // { +// // if (xy == 0) +// // return points[i].x; +// // else +// // return points[i].y; +// // } + +// double error_; +// }; + +using CatumullRomSplinePath2d = CatumullRomSplinePath; +using CatumullRomSplinePath3d = CatumullRomSplinePath; + + + +} \ No newline at end of file From 755b6169ce1a6fefff89e2f77c10b9f387c5de1c Mon Sep 17 00:00:00 2001 From: Kotakku Date: Sat, 27 Jan 2024 22:24:25 +0900 Subject: [PATCH 2/3] =?UTF-8?q?CubicSplinePath=E3=81=AE=E5=AE=9F=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- example/path_planning/spline.cpp | 36 ++- .../path_planning/spline_path.hpp | 241 +++++++++--------- 2 files changed, 147 insertions(+), 130 deletions(-) diff --git a/example/path_planning/spline.cpp b/example/path_planning/spline.cpp index 942dccad1..612e6dbc7 100644 --- a/example/path_planning/spline.cpp +++ b/example/path_planning/spline.cpp @@ -17,21 +17,36 @@ int main() {5, 0}, {4, -1}, {2, -2}, - {1, -1}, - {0, 0}, + {1, -1} }; - CatumullRomSplinePath spline(waypoints, true); + CatumullRomSplinePath2d cutmull_spline(waypoints); + CubicSplinePath2d cubic_spline(waypoints); std::vector x, y; - const double length = spline.length(); - for (double i = 0; i < length; i += 0.05) { - Eigen::Vector2d pos = spline.position(i); - x.push_back(pos[0]); - y.push_back(pos[1]); + const double length = cutmull_spline.length(); + for (double i = 0; i < length; i += 0.05) + { + Eigen::Vector2d pos = cutmull_spline.position(i); + x.push_back(pos[0]); + y.push_back(pos[1]); + } + plt::named_plot("CatmullRomCurve", x, y); + } + + x.clear(); + y.clear(); + { + const double length = cubic_spline.length(); + for (double i = 0; i < length; i += 0.05) + { + Eigen::Vector2d pos = cubic_spline.position(i); + x.push_back(pos[0]); + y.push_back(pos[1]); + } + plt::named_plot("CubicSplineCurve", x, y); } - plt::plot(x, y); x.clear(); y.clear(); @@ -41,7 +56,8 @@ int main() x.push_back(p[0]); y.push_back(p[1]); } - plt::plot(x, y, "o"); + plt::named_plot("Waypoints", x, y, "o"); + plt::legend(); plt::show(); return 0; diff --git a/include/cpp_robotics/path_planning/spline_path.hpp b/include/cpp_robotics/path_planning/spline_path.hpp index e1cd7a0d3..883bbd2de 100644 --- a/include/cpp_robotics/path_planning/spline_path.hpp +++ b/include/cpp_robotics/path_planning/spline_path.hpp @@ -418,130 +418,131 @@ class CatumullRomSplinePath : public SplinePathBase double error_ = DEFAULT_LENGTH_ERROR; }; -// template -// class CubicSplinePath : public SplinePathBase -// { -// using spline_api = eigen_spline_api; -// using segment_t = typename SplinePathBase::segment_t; -// using Vector4d = typename spline_api::Vector4d; -// public: -// static constexpr double DEFAULT_LENGTH_ERROR = 0.05; -// using Vector = VectorType; - -// CubicSplinePath() = default; -// CubicSplinePath(std::vector& waypoints, const double error = DEFAULT_LENGTH_ERROR) -// { -// set_path_with_config(waypoints, error); -// } +template +class CubicSplinePath : public SplinePathBase +{ + using spline_api = eigen_spline_api; + using segment_t = typename SplinePathBase::segment_t; +public: + static constexpr double DEFAULT_LENGTH_ERROR = 0.05; + using Vector = VectorType; + + CubicSplinePath() = default; + CubicSplinePath(std::vector& waypoints, const double error = DEFAULT_LENGTH_ERROR) + { + set_path_with_config(waypoints, error); + } -// void set_path_with_config(std::vector& waypoints, const double error = DEFAULT_LENGTH_ERROR) -// { -// error_ = error; -// set_path(waypoints); -// } - -// virtual void set_path(const std::vector &waypoints) override -// { -// auto &waypoints_ = this->waypoints_; -// auto &spline_segments_ = this->spline_segments_; -// auto &total_length_ = this->total_length_; - -// waypoints_ = waypoints; -// const size_t p_size = this->waypoints_.size(); - -// switch (p_size) -// { -// case 0: -// return; -// case 1: -// { -// Vector& p0 = waypoints_[0]; -// spline_segments_.push_back({spline_api::catumull_spline(p0, p0, p0, p0), 0, {}}); -// return; -// } -// } - -// const size_t p_size = points.size(); -// std::vector w; - -// const size_t segment_size = p_size - 1; -// spline_segments_.resize(p_size); -// w.resize(p_size); - -// // for(size_t dim = 0; xy < 2; xy++) -// { -// for(size_t i = 0; i <= segment_size; i++) -// { -// // spline(i, xy).x = p(points, i, xy); -// spline_segments_[i].coeff -// } -// // spline(0, xy).z = spline(segment_size, xy).z = 0.0; -// // for(size_t i = 1; i < segment_size; i++) -// // { -// // spline(i, xy).z = 3.0 * (spline(i-1, xy).x - 2.0 * spline(i, xy).x + spline(i+1, xy).x); -// // } -// // w[0] = 0.0; -// // for(size_t i = 1; i < segment_size; i++) -// // { -// // double temp = 4.0 - w[i-1]; -// // spline(i, xy).z = (spline(i, xy).z - spline(i-1, xy).z)/temp; -// // w[i] = 1.0 / temp; -// // } -// // for(size_t i = segment_size-1; i > 0; i--) { -// // spline(i, xy).z = spline(i, xy).z - spline(i+1, xy).z * w[i]; -// // } -// // spline(segment_size, xy).y = spline(segment_size, xy).w = 0.0; -// // for(size_t i = 0; i < segment_size; i++) -// // { -// // spline(i, xy).w = ( spline(i+1, xy).z - spline(i, xy).z) / 3.0; -// // spline(i, xy).y = spline(i+1, xy).x - spline(i, xy).x - spline(i, xy).z - spline(i, xy).w; -// // } -// } -// spline_segments_.erase(spline_segments_.begin()+spline_segments_.size()-1); - -// // _all_length = 0; -// // for(auto& seg : spline_segments_) -// // { -// // auto c = seg.coeff; -// // seg.coeff = spline::cubic_function_to_bezier( -// // Vector2d(c.xb.w, c.yb.w), -// // Vector2d(c.xb.z, c.yb.z), -// // Vector2d(c.xb.y, c.yb.y), -// // Vector2d(c.xb.x, c.yb.x) -// // ); -// // seg.length = spline::length(seg.coeff, error); -// // seg.split_lengths.resize(100); -// // for(size_t i = 0; i < 100; i++) -// // { -// // seg.split_lengths[i] = spline::length(seg.coeff, 0.0, static_cast(i) / 100.0, error); -// // } -// // _all_length += seg.length; -// // } -// } - -// private: -// // Vector4d& spline(size_t i, size_t dim_index) -// // { -// // if (xy == 0) -// // return spline_segments_[i].coeff.xb; -// // else -// // return spline_segments_[i].coeff.yb; -// // } - -// // double& p(std::vector& points, size_t i, size_t xy) -// // { -// // if (xy == 0) -// // return points[i].x; -// // else -// // return points[i].y; -// // } - -// double error_; -// }; + void set_path_with_config(std::vector& waypoints, const double error = DEFAULT_LENGTH_ERROR) + { + error_ = error; + set_path(waypoints); + } + + virtual void set_path(const std::vector &waypoints) override + { + auto &waypoints_ = this->waypoints_; + auto &spline_segments_ = this->spline_segments_; + auto &total_length_ = this->total_length_; + + waypoints_ = waypoints; + const size_t p_size = waypoints_.size(); + + switch (p_size) + { + case 0: + return; + case 1: + { + Vector& p0 = waypoints_[0]; + spline_segments_.push_back({spline_api::catumull_spline(p0, p0, p0, p0), 0, {}}); + return; + } + } + + std::vector w; + const size_t segment_size = p_size - 1; + spline_segments_.resize(p_size); + w.resize(p_size); + + Vector zero; + if constexpr (Vector::RowsAtCompileTime == Eigen::Dynamic) + { + zero = Vector::Zero(waypoints[0].size()); + } + else + { + zero = Vector::Zero(); + } + + // f(t) = a t^3 + b t^2 + c t + d + auto coeff_a = [&](size_t i) -> Vector { return spline_segments_[i].coeff.col(0); }; + auto coeff_b = [&](size_t i) -> Vector { return spline_segments_[i].coeff.col(1); }; + // auto coeff_c = [&](size_t i) -> Vector { return spline_segments_[i].coeff.col(2); }; + auto coeff_d = [&](size_t i) -> Vector { return spline_segments_[i].coeff.col(3); }; + auto set_coeff_a = [&](size_t i, const Vector& v) { spline_segments_[i].coeff.col(0) = v; }; + auto set_coeff_b = [&](size_t i, const Vector& v) { spline_segments_[i].coeff.col(1) = v; }; + auto set_coeff_c = [&](size_t i, const Vector& v) { spline_segments_[i].coeff.col(2) = v; }; + auto set_coeff_d = [&](size_t i, const Vector& v) { spline_segments_[i].coeff.col(3) = v; }; + + { + for(size_t i = 0; i < p_size; i++) + { + set_coeff_d(i, waypoints[i]); + } + + set_coeff_b(0, zero); + set_coeff_b(segment_size, zero); + for(size_t i = 1; i < segment_size; i++) + { + set_coeff_b(i, 3.0 * (coeff_d(i-1) - 2.0 * coeff_d(i) + coeff_d(i+1))); + } + + w[0] = 0.0; + for(size_t i = 1; i < segment_size; i++) + { + double tmp = 4.0 - w[i-1]; + w[i] = 1.0 / tmp; + set_coeff_b(i, (coeff_b(i) - coeff_b(i-1)) / tmp); + } + + for(size_t i = segment_size-1; i > 0; i--) { + set_coeff_b(i, coeff_b(i) - coeff_b(i+1) * w[i]); + } + + set_coeff_a(segment_size, zero); + set_coeff_c(segment_size, zero); + for(size_t i = 0; i < segment_size; i++) + { + set_coeff_a(i, (coeff_b(i+1) - coeff_b(i)) / 3.0); + set_coeff_c(i, coeff_d(i+1) - coeff_d(i) - coeff_b(i) - coeff_a(i)); + } + } + spline_segments_.erase(spline_segments_.begin()+spline_segments_.size()-1); + + total_length_ = 0; + for(auto& seg : spline_segments_) + { + auto c = seg.coeff; + seg.coeff = spline_api::cubic_function_to_bezier(c.col(0), c.col(1), c.col(2), c.col(3)); + seg.length = spline_api::length(seg.coeff, error_); + seg.split_lengths.resize(100); + for(size_t i = 0; i < 100; i++) + { + seg.split_lengths[i] = spline_api::length(seg.coeff, 0.0, static_cast(i) / 100.0, error_); + } + total_length_ += seg.length; + } + } + +private: + double error_; +}; using CatumullRomSplinePath2d = CatumullRomSplinePath; using CatumullRomSplinePath3d = CatumullRomSplinePath; - +using CubicSplinePath2d = CubicSplinePath; +using CubicSplinePath3d = CubicSplinePath; } \ No newline at end of file From 3402f61cffe5bbef30f853a2855cbb70e7d4add7 Mon Sep 17 00:00:00 2001 From: Kotakku Date: Sat, 27 Jan 2024 22:37:13 +0900 Subject: [PATCH 3/3] =?UTF-8?q?=E3=82=B9=E3=83=97=E3=83=A9=E3=82=A4?= =?UTF-8?q?=E3=83=B3=E6=9B=B2=E7=B7=9A=E3=81=AEexample=E3=83=89=E3=82=AD?= =?UTF-8?q?=E3=83=A5=E3=83=A1=E3=83=B3=E3=83=88=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/example/fig/spline.png | Bin 0 -> 41808 bytes docs/example/path_planning/spline.md | 8 ++++++++ example/path_planning/spline.cpp | 2 +- mkdocs.yml | 1 + 4 files changed, 10 insertions(+), 1 deletion(-) create mode 100644 docs/example/fig/spline.png create mode 100644 docs/example/path_planning/spline.md diff --git a/docs/example/fig/spline.png b/docs/example/fig/spline.png new file mode 100644 index 0000000000000000000000000000000000000000..4265f69e91d31f16e2e983fe9f05f593c6c37714 GIT binary patch literal 41808 zcmeFYWmHws*Dp*6NDI=bG}7Ingfw_Sx>KaPTUu%95GADJ(A`Lvba%IO-F5!Yz0ZBd zJKpc_mzNzxnw`%U`2a9gbc$*YGr^g3*ZgOa}FAxv{|Fz}*rg=s6hBpF_F_4^zDhw0pt+-Ta z@DCY$JgEb;=Q1x;Xt?onWHcDF>2fRv9;oBelsf2UzX)eJg)A}(ozIJSRbn3xHkH9A zUyJ>E9^LJe`M_~?e!nFpDJhxbeh@r3f9}D2blySX(C1pd7SbD-xLNCS*}E`uo<@j+ zgA@DHfm?xq+YYbXj(XZe{RgvLGV`Z5pVICx!#{`JE!WfF|at~KNI zQU(1J3O7n2!X#X@E(#O#t2e?C;6~llz<1`p`;dT%C#6>SH2+^4lyuP6rJg5=PZY>2 zJ>8wAzUOp5;MqqzJAcWFi;Ls?`8}oD+S=-~va*&+xu1jgq=NJ?6Ua+@dU`r~d*6>a zZ4YNCDk?6bG9z5Z6B9v|FA=t!(%+3xFgRtEvRXSLg*4RH>lx%LWvH;RmCdOaegFCu z#h5nbNa`d?JUII-CZXC8*yZhAT!^$ zTui#DeFu@1<*Ezjjv=yQE4!a33th0VvF)sC=O0udkaA6Cv^dy!n+w4`Vv*hNJUs=E zj*djz+uN&I2qF#Szf7}38MZeGGMJ%0LjhkGd+`2P$)*aAM9?#>UYcr3!^UURMXp7q+a6bqN1V_ znUIhW=felPZH=3lnE3kU25HWkmztKg8}Yi@U-2BH*B#vT!-{rjHt!Y$^=hg_r&do+ zF5;-40AyqRpe8dxp;`ehE+yr$ZMAkS{OPPZp}XF%l)*GdG{K<^5fxELt?eSOdeLNA z!cq`e&M^Lz$@Y(_A@1R{gVBzmDY&x1h-gVlumQFu zP&qLW{rvnE78W8Nw3U^W!Of_ss90Cu^=9|jF*;FxE145iS$GXDzOyu?B+oBjB}7!9 zQ@xy6hlR$hV|w~3u44iTeA@M++4(aBiPGs=MymVyYSt;zRoA{V>oXUUb}QB6Y_tCN7_COQ6=oBiVKTuh zYip2|#et-b$w_=d!X#t2O;t^DtEpnff7Ks8e8|-g?K54Q)G#o+*qijeT67bbT6ACk z=6pE!JwMYEEsbScCZhZI`cJKjA8C$`wU9Sz5i$ZMy{V}Rjy+FL;&ZByKm2uH!e9E- zW#8H+mNJ(ajkgsIPyX{+;W%on=Xf+nERu%e_jN8VHmhA~ExdB85IjTwrs6)s+$U(w zOA5A%@!#Jc>KYo(Hj@kxMt{A>&h$Q$UGhF35;m$Na@iOTBHXeu9f)C_(^^wuJk0%gw`m)9CSSGHupn^5l4-pf zNXtWBRIuU4Eyaq$%1Y}u+n;*?rTSZ8J2GU8e9MDq$yk45g($b4 z$HzP|+BpspdPDRjbDFsB>$mDXnihrdpgftaR}Zq%hCX{dw(sp58|u8vgQzb4m}BKT z9UoO)1bofmN|!zBns&^b;I*5JN|*WLvE6oPjF@or`ym9@f4g5q#7*FJj zsjsTzH&Xd3L#3#*-s`<*q}ohCACmU%v=efxblO&OJ-Yv{d|ak{bmXI0ZM(Xp^LgD^ zs6R=0K{8tZfvEcW)lJqtBf|W$x4R!9RhjMk(AVhvkO%XlR;Xy{ZYyWA9bv=r==aQ* zQ}LhFe#tNIqxN65Zt$-8CihiE^V-u~K+th;7+)gs2%Gy2j=f7+j>Myr%1Q6{!OZ>M zRn^2XmTUBqq=9&5*m|YW#iC+6JUz+x{woH$twzm{g3th?s+DlI+B3--mSUdIs-4g1 zSp}LS)NFIdItRh3{QMjHklKfy-vl*K-4!Pf-t0u^JsV~-j0j8MLei5UPP7tI-J;|W zLR$J^Qaf8d2j7W*wdRU#!D)s?9@~(Hk3Q|lufuWaq$5zu&gj?&>R$lgPu>2-$u?R{ zKTS>Yq{5v2ov~l+&jpsAzQwqFH%AJF8LI^36ub4O9Cxh3Lg^SRcOiv2?5E zDBIfL`#YHnU7xbt%vI+&%)&3@O&+hd7V{&T{03SU519QhdE^8iZ>TT%xn6$Bx2Vl}QXl@VcG$Bc6V}rb=NZ zKnLZHc8btUT3WiIajzZ(1lL-=0V{2&2Snu@^XszD75P9C+lZSU&i)@Z>@C!!Ja69N zVG-iphs_2A_;6S0BRq>!`Ge`Pb?J7fgPe)o&x0u^p3z2l1)C~94AWo2bIjSV)N zqXKVJn633V_85M*b673G6~=q)30-ZZ+C;#>!^gkBKpIdYxA1&661?C$CHB`LavQo% z&KGibG*tJq&1&J==1Hc499`eut^q(YDhh!>`28N6_882`QLxBnkcFR{nx-+==O0jS zn&v;LMgG}r|1BMTYH+T*kEfKS^sMz2f{)&757I2ZW*s9)4uXt=LYFD0-q|+}OG`@{ zI=aHBxB}|sh2Kg8QX6OHXLJ`cc4Wd4%-FfM@j~K5&)KS7UY$c=|x1y2?+^70kwB=86puKwXKbh zjeYCsc}u$}RU*hLJEghgK>$OM2^IvAxqmxT_ZZZo27g;Iz~FY*>siEHLC*ic>W+;x z!pV1xu)>PP$gEQM9RjQItPJd=g~9*%Cz)e~c!9bcsHQlK+%LDLD?D1_;^WELU;|0A z3Q1ul^hS|04GF@2%t@~wmA&oWA^Fec4q;$7!GMosz<-VOQ~bZ4^Wg^`Y`9Pc71#{J zB=G+Ctk4o*(^z|f^#9-f|54}&EeKVJXRLXnC`2O4%Gg4T_#4S!3y<($xmlnErK5|V zTS5CJu7wP)k^A)NH+0|y09x`Vpf-JIbhWWa`cTTKk_c~LVDRc3eRq3%d*d@KGZ=7j zai3#i4&9O`1%lrPpy$^=+~5Ca<=+tLmAX1Kyb_0{5ZV7TN&b^ej5w~1B& z8L6aYvmF-O|6Ye=WpR=&AaJn8OIF|v?N!oWMLJ*u?FCET*jQ=B{-^64~ zggVaK)S3;wmzF-eUXK>4pu&OWcELszrPwfawD3E3cYc6kzl4O0LoJlm%k=efrbp8F zqW%5-A*Gc&RMgZQ7>J$h;6;AL7e3b#~*f=dQ;~hX=T9ea%-(1J|ePvf$w0dnswdOP^2S0iywZi32P-yGZ{ zthrj)vO%y!{*6iyg|z1>rtK^?@?nzl=2%YTeI*l!`$o!#z-}|goyubsQd`THot-TV zE(J#?!i$$LI~U!S6}`Q^)r(dl*;<~9Yibg)wcc|EpxB7-EiS{H=#j}*+8Pe=X6mYK_erhZchV!ef=+AzNq9Yk$Yb3NddNX{dji_ zoyyP6J>B^gjf;o(B{UR8!@!59qM|};-^AD$z$g%y(ZY{R!XhHO1|>75yhMc%=VQDe89dC>q!vk?(82M32o6eSK$0N`X$i;*7oufo~R zb5)jKf`g4}=i$NPiq)O=ke;5NzUJpoej9}Bu=lsX*vA**9dS$L=-?_uwwOx%0{ii~ zgP(sj(d)4dglejlLZKghHY=pMp4D>ouxioAaEIe0tT!Zegm^xf!o_URk{mf>60jI|d_mQk_r6{Y#UNsb z>tyG5Js`>ua@$(+IyM6V&B@7OHy_xh|2!ZcvTXlZNi(9agvuylVhrBdDaoZ zDRxA;-nhQ0UX@kjb9V`r+H#@JA$#7ebv0MN*;65zJphH$JBFCcd<78A)>g3)qZhci zaog1(;?!5meqwNJO(zIo=2bTHiK0HYaIkk1blzzL>w4z%kedot@3{3z6jXcIfy0^N zzMbI)CnG0EB`BEcalU2tkT{ge6ZxGw)Iw z@v<^FN8p;huFS#3{~dfvu}s^=2D2e-GX9v_WnUpCm7~SRBo>{Ta>I6nSLb|9U(uO!se>2xIE5GNw`13wE>l<9xUu^jSZ<{8n26^ z3svsMAL*GBx`P|6Kf>R>6cKVa!ILaMtDY18&dwkg8L^19jb*a@z;a2RWpr!7MW&r3 z(5(xRkP;63OG2Dxh--1?oUyG zC;)U8AaKaa$@SOLvNmoFrCEqzV+~uU*i98{WtxEvOiRK5kli2Xh^YL#cL=I^3hg5! z(O_-h%l)2wIXO8i1F8IrikJ@Ps!gU#^@694H#fiI)3GtnS6RMP`8NXP2av;Y@M?cL z8nEgJaC(im_;?YpXiE+xvRIDipu*|YSQ~-33TTJ3&9wTpBA7A4qLV4&wUwO3??43> z*LtDuLjZ(T>^*Nsze5U3I4l_2SpLY1Z7fe8RDgiym!)XbD|ULoPckz63zbYXLqV?L zyISP;zZguGiR=fLv0oa~jem>~c$#@jh<7X(Ym0=$z73dhdO+@`?d9M!r}4rrDy>84WS^Xm6$ zZ#5+9XEYl;7kTE<@g#&h@+gY#8C{R)1!`+pM8sMrwpb3>6yE>F-fOY6#oP#79?Zrw zs^)FXYJe-yie<_2#kDYiibyM;7?Y472~O2?y(pPJ0WTu;Zv(>Bn1-%F1P?ztsb3^N zJ?+@=3e7(X4qOQ-mm6<8U}Y!`yEQIgX%=#=>bm9OcEg>^O{wcdk_*M`_hJemBdhW% zzJ9|km*2`n)X5&s9qv>b+qUKc91)m()pJpaCBFAoSOsA0|=I<`it9*cx6ZH zZLm`(wK=CFk&%%d@2^kER$1?cm|!BzcZ*Dq5P&1bTmAe%+WV)ak?v2IS+x1Xi<|$L zZ}NajJZKSYo?|?J{)(Obg}+H})EKBGKirQUR@wr;?sxAfFi&?KB z0UXzhIQl*RED77ePFDg!kWp0Z2Gwpv>`A!DLqrS^3xV4^IT;y-NKPqO_1({JZ?H5} zM8xIm`9>A*86&ANov2>yta|QU5qqc)SM{Q z6Y7bkR6gGts?cOE)~%1*+4&@TyF(3PX4iZ{AmVjt+8Kg5m@dH8E85Y~0qBq^P@}3o zjUfPqSe{}+n3FRuo?e;LWlzoX>S*X}b3m)cn%LLZ7pr*$9ufIukZXk9qz40NEZsra zqJLDCMWiMRKNhIwW2xpV(FzEpfP52jPLK4-Y;-;Bn3#yaJe-G}I^bFSE_-+*B3HVx z(GI=jZYL{2*k|Ay7aw1$M7z3j>KhS9pcd4846GG67a0pR%7$Bf?lha+IiTi49oAER z3i6Q%W;ooR|ArmT*E$_72qd!V^~b!H&;%$oAp7jQhW=`9lKz|ZH&PM1Gj=E46839X zgEyYj@2+HDj599})sl%0q+_CJRjl(9=xi0dF>oXNNrE!gZ23Hc6lrbU_f(AWF|+h< z$(EuT^>cyp$P1+HK4$uOWr7=E=(`!l{Wh5vo>ZbOr9YlkLM|p{`~dJ za<-CoB2UqzKY8#3{69F6xZVy7SIQ#{&O8BG=aRKBpfI6uT>pf;JBR<@`qdCpZgV$AE`M>(AhXLpp7{-g!SeaTlJt+J9!`rc(;r%WeU^N# z7QLspr{58ikjQy^H+4P@;5j4@q3!F3R;#v|!;ci&uNT>WEc3RjoIO<*F7s&(O%{2= zb~)@QY4LTF9b-~xGWNmUrK6G+nKV+c{sH*~60^uphc^Ei(Cm|u=_b41m z$VNv;7qU5!B-F?qyCzUYAxe%MPS4p0*KqS~(D#*7^=+=XW=g%Q7?<&7ylsUMk-b%9 zs<5w~L`Navl9Zf7ilTX@?G-n*iL|ld#`Etz04{iK;n^(coBugBsoVU7z=d41)lc#r znkx=u%5fwlLWi|3RIvJzK+)Wpt7dAwKSPHpI%8vnGWCQ)q+82z#B_*dU+#=KkJpCr z>82k%;*KW#Om7RtWNT;Y8s54jinwA<+*Q9hW%p=zaVay=-rx>wa;%9%_6lY%S$|eq zG@kHAnksUU9@m>LQIyMXYx^NzJ{K4x_1vGco5E(H7DA2BV&yHW+afB;cbCy`(=LKD zC)!yo_!Z=!?7NM6%(t{o-06m5Zket1sC+y{ALyo8^fj&of4=*5M#-3edUYmZn-g`_hCu{~l1~<7OQvpQr7!=G1wU0C@nu-ns* zQjErnNHA{bl#_+y6NVShjXhkX%#)j%Ke8l>IoRA_#1q!ly|CAbFgcSg18bpMMhEKG*~Nkp=d`-6@Fd$L2EE zD204qR+l9%kOg>e9Ph;bW!}7ezpjc}b~HptEjU(d^Qn7}ePM|7)(_+3zUq);AZ?4C zVZY2q=1tnowr&Kp=p{};0&Hne@~^fr;w4((TS>HvKw*zhI^3%0o|lEr?2Pk=u~8hU zIovt@D@AtmLDQc^9hm85&bs2yNbWfCsG!NyO4rL!;{=2Y(93!!q}sUu&4@XR-`MF5 z|9<-h8%#Ok=$~)$^7%6RECZLqEwV+4(oi|)T4%|iZX>0CDAhifmAzy)f45$8;VS!z z347zj*crM>Q;Yf4>A7%=yAZF=(g!z5G*JE>C_FZBIyyTY+}!8^1_rRM60k{Jp`Hx4 zrM{(RFTi#J5K(rPTg70yL1$+WfW=PxCA9=rlu=VEWb#R@w49u{z|4ZN)ukqZ_1@@h zz}<2kg&F-oDBe^q7&(3#X7>=}_y!NtNLsu0-u+2nn zL#&D3J!<&&u~F&8gPl7;eKL&MgM#6Ey^4~jl+h2+!!W3?;SmwWU12!1u4KSC3k*EN za!Abrf&}2s(g6Qv`CJlvT^_6gKDt#{R7xc(+M@88A<81lCsF9QDZa*Lz7lvIt_L&i z`(>@e02cKF{$k&Er>LyV*8V29icuvZ9=Ut#|MUW=sh#>+MES;xD43JR19IK>FXA!0 z^wO+ne1+^+z63HyQ&Su`ssdK z>>iMMt^G5h#d~0d0WB&3ka}Q9Zbu0n!L3A#Jp$;3IGD^41b~BTt*xG*%U)Mk_)E)q zUTE1Ax(kbbQ*so!(Aw(iTTpS&|0d`ik?4D$N@>?v_klNu2@n7R{{!g%e0#*g#-w&W z1*j;itE=vwp5s5I&_y5qO%z|G@!A-J!rKJuWYg_lk@LY!EP!<&2#)ohXeZ3b4I z7OvL86@mVxDTQ9sx4JKC78>dvvxhO6YM1fNJ4r(60#2}k!v!Rursi`j7VU+7%q#$s z^t}#wj7>~X(a_csb!=Z#Q&TV7v_4$w9M!F3**5Ibf%NFvn=As50S<<67L z1uPbD`?Ic>%J$QGZWXSF+HinceTk07#uoXf(sIXkWC7%PZ(Uv88L;ZdEqAPlC|Dan zpP&N!-zp8LH8A2(rVsp~UkR8l)~t}R9Qw}p533utCncysq9ZtTZuc)oRs_=c;}evE zUQrTYJz>9oqyR~Y0Xs;MPHZg#hbheykMA(}pV)tiK78|#m^W_FRXWw6A?{-@FrTHo zO8MmxLcsC|phtjXxNR5eB(X$n7x=;QBAX)r0{i_O^fye@+ARyuE2ZEF0KP%vNM<9CCcM8(D)smKbY^yO#LlzRUj>V4yAK9V^E zB%4Sz=#aU|Qj-U8DG=B{jlKfH-w?=IKyrwiDDqA;1eWu!u?P zr(KZ5jzs$Iv#E~>` z0)_+E>n)C%MhsnH&O2ia+qh~$b7eAY=7o2kL%z9Q6mojWJZr&!P&!>tv1_ezq*y zN0byjV`w?ZG@#)8+u&QzYy7InoQVEOCTR62aWBiy|I4d0_wQKQQ#j}K4rY~d37tuV z#E^nnl$A#!Tc)O`lXPtBgJIuE2EHi(9iN{Kuvt_G5?m$U^et9?B0>f|Gq3hf=j~c^ z)l1T+4H_F85l737B*20y1LrNfB!MoTvDf1HjbGG_o+rEginyTLkphSblSV15%20@S z;{Eqa`nnU>usrL0*1Y|0@u^m!c*Y~$i_n^mr&C((mpB22mBvlS1`r+x4Dr=YLZkk_jS%frgYY!+1}p zn7VRwG#pF){?q7>w^5AT+~YRZ+cOsndc(9&A0Qmc-(*4 z$uH=>Ieo&%M6~rlkFuqvS+&zYz4z~pS{^83x;(ibQe=JfEKoBaOnwWFD9hv1x2&8!vAT5E4AIf3UvoSLQs%$5| zhHg8cmwlvmP3-^+6Z2YYhI{k2w6uIB;fbKnbVmSYD=21)pk9kT{Cg=XDvCkO`2|+& zU=ILJf1KarO(@8n44{Qo+AWKk52yEm8-`4xMWmBh_1rHeHMGhN;mHM^4OcsY4`wQY z)JwGWs<1lQiP#O{xGaWELA5Q`g63)dS9M`L#(Kgq5KFzePyH~u_2l=;Z^kDul)$Fl z7&-x!Qe4`ZM<8^f#fm3%{}?Q^6y(9@ROEwMq@bA6OwpiEEZKWInW2}FKn<9`$^KM{ z0yqR=lLYIDLqsG4iep$*6!QN&tvo-l0&GnEy9HKxs2rO&4eN#lsQv2tROd_)u z5QASP3wv+^#X#5Vuo~`fsa_gb9DUE-+~T7E)k$bybuR-^VmwUsH{ohG()qQt6BvZ&S+~e1(4yr=`v*y6DWcn zOd4j{(0{n$ZXOg^YRmplH2$nyx>wYvaS( z)3+FJ+W)XjWp8g`U*Co(`H0nn*U6Jeamo_$I@wpk*H@{f$s!IgfW50u3V2Ps-(9s@ z^59`;_{q8&L~%(si4!NO(1_XZURzjqXt?yCGVXtp+3y>4?DRs;J1=07p0BZ~vYs8E z-BkM&?BjQFhZ$mIjYxsOEG}Ks7w0ox7A@oPJr$1fasVTqN4vv@3)->KAg1iNjFMt> z7LxZ`K_hw4V4LH`90j%xuAS%L+R3iulIBN2iIMOyHS=2}T*hc?bxlDsr6Yu8lF=t> z`++8}i+rE_I*Uj1aTeYN9@MR&#}fOG$%9$5NVZw1rAt*c$EIO7UP}gUG2~CWw^8J&ifh-+pqKP$szq!Bs^gG4l<}7o4 zn^=%9al@HOgYWn7*G4?L$l@XP-_yi=UL+HX%O+P9PED;xKcGdpIQ7;$(=s{20;ee6 zH}+fSMOprNwTSi`&YVH!Pw%|tkW2QSogDq(t!v!riKF65S($Qc8%jz%Fsi!|$}5{2 zSe@{=qrk(7T~qym$H)d5Ztq3p>J`U|{=zzfXixR}djOOV*G=@BbbHGn`5b|r+r{-q zYxwMxUpsKh^{m-77i>L#525jqU>?H2+D<^>CG~b?17z*d<;OG`dx}6(`tZtZ+V#Ey znoE(#Y5R%mGxyEqvJAYpMQ=xcklLEEPr49@9f%GWiYf6ozQKt_Qx1US2x|&$E~nWG zuMA_iHIft%tA`9lnvH~?Kf9}k+F$!UKU1aj!{Wf$5g=di{$w&YAb$Q?|K2k{#0(F| zAqH5V&_kW?#0>DZzKEY|x;{hCXVz@2MI5pGlr=<&9x2}yc?#4H4+Q(!?A#*2ndlLk z2;A`jAwsC00Vg&>F#yt)f*aLgaRu-C$%(Iu46bcp`LR7BgWjGiEt!WuFr>X@xa8b% z;*#R`B7q%*=vXj1#p=poC&6|!Ho|0QG#e@ECF>dnF!qOytJ}uCINx`m+9LBQARAuZ z1`RISKlh)$tNEeVT&To+#%abWF~V1ordz+%6wF#?f zy8AOz^8gMw4#R;vO%O?IrDbeP1zcHOry(AJ8ogRu@(`nPH@h1m%iY$e2R0yUGEG@w zbi&L=hK#_R@rjb3RKu07oMcOIwxXjJ6m_2E2UjLd_crF`#Ph#6M0EZ-+kQ)euFw%_ zh+&Df^t(@c=+-te?muFzr!paA=xOCv@14A%vr8Qe-*{SPz(2qroH)$fYG|1Letr7$ z=TBJa0}rjTTc-Gf5%v1@>#m-jxb*a|Ng7+ZbO6%+E!K($4qT!;c~Zyn^71Tg*v1b6 zIUI|bC&c?}?XC07r{F!z8OLjkXrCtXMsj4g-vd6=Awso1{VS;-R1VPszd8x;o}se& zLD3_nM;#(QhH3zNz%+^-^+gg{@1Nw@V!@`uuISm{AE-Z170W6pC`?r*ClkTkRB!^0 zD2qKLn0TnE;Q_&)hKdRSnDp=7y@S;ZV5Gp!9J&J4yA38suJa4I9<-TqR`~gCRW|@r zi=13BnnL8{-@hE7JThxl()3#SK0UZKIPV4mUmQTf^_IIMY8YtuIZ|DUjg1w)-T4Kp zW&m;k=f49iKytMxGc$%iC~M@M?3G(mauEo*GFtNd$nwXu=M8!F}}iN1bWm_$(WXx(7$^O zDhe=#;(1!lhe&{RzPnKGG<623vG3!J1#E4Ul`TNIxxd{n^FHk+BolUr8T1G#Zyf;l z091^xmjl#_PM&F92~cOCi6T$;=l}L5BMtl>zBe8B zn&3>yAQ$S5kB0KtT7^>z0TCWJ@P-wdfJ_O4y0JcIRhA3*mljJBDW5IqBMScgn*rpV zG+4vu*x3011vTAlr2+4ox#;5LsPPyu6A|mVDpY^a@J+y~`v=&7j%2mUS)vO7B5?y_ z81*F`fbi`)c4k=h4FJ7?zA8raa5p4x@+Wz2Ko zMeZEqlzma0#x%APg}a^9+vWzGHzq(~Puy|s#sNM61cD1zpjf9i3cMTe`y~NQ3jg+v z77zuH^uU0Z0KOC z;I{=1&Do&Ar|gAlD?*qKF+VS4b8OJ9&WX{4bhFN^2Jr&670auG5k7UEUNRwiFUN`yU!(+5-^{dJf#5M*RWT1m*<^$YKfbD)7%yQM18zvz{pr0OHeVt~?v`U>^v?VUJIA6K4tAo`^1nQcxTi5|2o z78ui<@l$e`-Pl@as99EFeO4^jU`Iy!!hrFv>D?#tzYS_plwds+6fnREpv4je2S~p; zFdbkj56C>whB5>!9~jP?)^)-J9_w%|(n$}gUDy148Zev^S#%P&o%u{NrAI`T0Re$a z<+khqdXB8zt>5x*QeJCt5(zjb)dL$>y+}GZZaiC@5i$p;-yV*PFxDh#2d1j~aWA5G z`6+t{{8KZO7RPAMYJ7K)m59%5>Bq#f8RpadBQ+3*9YDl`W=*$tU6m0xP`?vy^;VGv z&8i6|1xZRtRro%5!dgBi-=U@iID$3E$;*SeO&NkN0OhZj)Gn<7TJH_8H!xscAK!7L z5eqsq01%uDS}Q!1>=0o`fq>Iix!bXR&o|Ou(5Fyo+d#~3{|dMiFn|x*G|Iv1!URVc zd$6}Zp=})jU>8RJPuF_|fNn>7{GLV(RLfkO0o|E%mSo-R={xV{eX=s<0qY0P%O0lP zvEq<;^aN&?If;P)I0gHRfvz$)!`kiUb4|IW|CZ_l9htE&(ag0ssvx_eXMtb|z<&J? zNo$*`cd{7H5>p0894aOzEjxP*;7ApweYmigZ}i2VJL5RFpR9I*UZIg%7u*$Kkr|(^ zb#vP-rNf#N!PW;CBKRjI5i+S4uK?Zy>udlzDNq44mVEDQLYKB-%3}X_CNO+1I2o}H z*N>EpKr#3`i!WyzPG8YHiL9zE%w~#_Ezw$m_COf>KjAY4*5SZsXeEy= z6F4MrckpHQe@x|0acZT4))sLf$N7PZm8mY($$ou*KEmT{Ebw9GN~3kzkf?`u<#ihZ ztTS|$z8WAm0(lZ<@=Rj7&R;Vce(YDQGYSop(pP!y!fvWBEi@QzPEos@7t!IqI%;)z z4e%8}&pAzbQ!Gix6G=Pma%^Pg(Fo}##>Z!B?I>X;FF0*ocJQiYq(EyMMB6tRuGbH7 zq%eJtH{-QaZA{GP?2j36kMqhtiiuV>M?EBeF~q1-^iR*VDy1?q2w2b1m65ybt@ zvAypeu5xnrk2LG+K*ckz)&^7-9WdU0H`2z%(uqaKSh&joK32R7pE2N~h0W>0E; z1=2sMBJ`*BJg1K_&pQge47@2Jw#j8#{*xO@DCd+1dUvl+`$*zGNd70yX?Lf)%kN0p z>(68}2sO(ESfOQoz9paZ@`0BSzlQcZXl(Zbob(*Q$x&*C66m+>>CNUvShW#QKc$EPe_mDaNg70 zJ8&}FykK`5xI1urYTe2@Tw18hkv8<~*)vm2iJi^Ok2}c|RGR^dw)DWyuB+n*y-w_D zF3=om*epBpu2xo}XLN;k{Tvwz)L+6#;S7a_AW+rdrJwT;@CPD^93dWJYVk|b%;0F~I6=~DS!&*zJiwUmp zeamwyXLEUuXrp)lOZVT+JY8_jak`g?;hrc|Q05JgSbc@NsUE zKqq{zU<2me+~Am6PaY=iQnes7v?^DLhmD?)F1Lp-xeALKe#gg^KI$g$AuzdpEh$6)N09YEwdW#e9ZtOjy`CFgoz2+g)b- zd{O+zh_4(iwh1uyUTiI-<|y3M9(5bEl$>ynR}_WmJ!rE0snHb4Gp&? zgGJdWK8o`csWw*!Nn^u-CDgu!xmvZ5>2NN~X&NoKfj}Y~s(=*hB)Z^7if9 z1=o2S03dp2&18uy=ZXQF*=$5yt~iRkW!qIs(U?tLF<&#=e)(4uM^q+Z2*dqV9 z4Iz{rDHp7#!ok`8AeKM~lhCEV61@ee1~hC4-yJ}8LGRu@(A3<$yo4Hs8O@5Y%8bwQ z`qE`YW2>3@tLWZ_^WlhWXfNpfUDh{KuN^UsTh^B4nx~lZd7zkHIQ;>Y$Hj3#t5(tP z0_GB|q5+2{S1OVKh9ZD>QEoOsV39hkJwZ{m ze9k!)T4^=DucbsZSWbumEYe*w4~Om7Fk z0ep7C!POEXw}uNzVLv9-7j_}@Uuae!d157=u*EQsZ@`RG0H4>KTsMOpmj0Dr=r3T& z5@@8siRJty?tugP*SdOpUlJ3`f~Kv?Jm9F!%;w56$_$k|KHVF*)7c2}<)vG^dHu{| zlkAt!3{$dY>_i>t*i$Nm^cU5zq(KME+_naS22q$Hm$g$2Fs-kQ+3dk%TGVdEZo)@jNic6^`Rm)b9K4BCXp$72XSC-;G2mjcxkII(eg z;*8&W=+mCopf*o0jML51ZK%vESRAj zm|?J9FRuIQK!$%tv;@IwTqYckPUj?bhF+=~u3L54fEPt)`HW)nlP)sKlpt(d;QkV4 zOlt1pMyDpQp&c?d(H$E4D{}WTd2#hJ#n9Ek)dM2yUYA;5qJu}N*$8i_CrV(gPOm{r z1>wYF*sL_Ko%h&+E(_Sd37_&k#SnSY--;+@6E~#ZCZPYWbj(|Q)A7VQJlo9Cd-%nM zlK(#s1bil`8k>)Nwa6fP&A5`QwO{z_SDN1wICEk<2je-ojvQ%q+qch`sutk=Z7v34 zQdc$#!N(o|Do82;3G1=DF9xSMYRc3??+dSCS7(EqJg8Kv^rk2+b<)sjg`BKB*icRs zbw3}r>v2LERr<|#y+0$vUT@LZMBM!iIaYIg{m;7rmFi5V$auLSQ%?7_kfLN$@JZS5 zw*S)$@ZA4lo7ny`#t8_VFlA9UQUBVJK2(YR<=%F```tWIqS`g3*A+RrRDGSpy-?$< zwWkbdYRtZRlO4qU7D)<+^yhEQ@>kPY#)ubl&0+fRq%$gNlI@W{5eUR_cAKvVp2xgH z*tK!nzUqAy7))mh$s7@I@=uXHGaYF-=il_%|L3n>J4>$k#TN6Y0Hg#3y-AV135osA zvXMGG?4J5f0%Xg(S}?Xu{kYetZCf#M#myf7*%ygrzIOJ5DEdGnBH{6(*KF>#Jys;; zmVz|QHA0DDf3#Gy6FAN6v{|1rTX6ih=Tp@5?i#AIub5t_clVf5(SO!xq~f^H=IEXu z(mw5cesW_yL__6RqsEXs$Z&RB_jEpKO6b0$LY+*CeUIWTHCm3%-MVz-PUUy@Cl`}w zy*ZPa>jhSE$c5@*6u3_vb{~hyShm%)hvn zc9NLS2Dg=N@q@j0Nrj5a;%zXb|4X;f1QDMdJ)E7}IZnfxqLoAz#|vB=6VBLpuj_GS z@1v%4pO#Nwi-XEfTwno7=fIa^yujMVJ6lQMNv6a28l5+BOW*$Q?fK*AwOr$Rc)FT` zCS0wXO_$6F9Nb`V+1no9G*xgPF}QD1VZ!t}X;T^Yp(-csmQ71}=vGdJPb z3C=nv8}(3Uvy?LU5PIw_Q42S)X}EsAW6Yhi7`Qy`>@n3<3PRO8w=dxr)Notod&S6MJR9 z!>rrzfV~|@Xy?kZ56QXX2R}yzJ{r~_p7?_(S}I3vumozJcS#w=OI}}8YANliWZN|# zPz~EYuD6qa2CoMl&oiefN#!yBx~&a2IIH|tDRrnoQc_7V7$TQ(5l;Ly14{0n#9Acx zcOkR{ll&+sHN#qg?rh}S&SKce*Cp%AkG+3{FrZnLLUCb#7z(CQ;7CP8jhO*^tP^wF=Ekn5BGG1j05=x~q_!=t78aPZT3BHI{9##( zeJVj(*>auSt-C?`BOf|rKDvpMHW8lgB|_ReT4-dW&)k3m=d{GYpKj;`Op4Y#@W2k$ zW$P6~Y0>JRfgYT;Jk}XVN`5<6Sl2i6CP7hN*U~0{C?ipJIF{MaY6P zEU5PZAgG#omS;OvE~Xp!V@=xpSD_CGjavZ*KU@mtSF416{_)s8-ZtEr9P${Lk3MWE zjEVoOIKV&!34}e7$LvOQ%TB)gaC_yOe24w8PtTnFsgp`YRLQJlglZ89&ObYYr1ud= zJ&}l0iUBraGvxA;8$=+#V=pQD)`L^h;N3UY8vb5i7%}t*YwP_O%HYj^$>H-``49ULpd)`r6ZBE4;_tGTJBsIQHFE zhD5IK@hbE*{UbHX?AjTQ;c9LQ-zlMa-}#Z8=t^4Wf;)RQ%)GS9*I&T1qLjJ7_m(|X z^Xfjn`O-5-s+9B0(g5siXWKuBQde4tnRY4gMPe1;*iJA3Q>8kPAK4TGxlJ(!CHUy) z3#Y)wT~Cs#Z{j2v8@HV;zqcM~+2a>kir98<%b#;tCGb5gLE}w=u~62?Mup}6AI5*< zHRd$68_IzaIETwEcNk6nk`rU#gWClwCCJkxF;u)JOP+#?U7=%_Yh&~j&vCcspy zxP}`3E^=U-oTg2MFfIH4kO2`*FvK+Jbk8oK43J=<=TrI>nIZpv94wl zNs5IQ%P%VbD^}XfqI{;XAAW?~e1J(0LDQWmBw3vAK9?C=oQmkS>&PoR#H6L#i~^7A z+2-=t@U!Y;0TWfHL4?w^t%|&$KuLyv6pcen#Me5}TUaUq>mfb@J*oA(tD0k4F(va1^25Fi4yrN67UxNY4xbaE4$EjWR$R0$|Y` zah?*BOcYga2~|O}aLCt{`DiX&JB17d!|lhKg%gop)HVi}9BV~fHvB;fYr}gU&UMdm z40NjH3s?2jie&YA<5C_oPMx%=!y2PsIwsWC_xtGbuh-YthXHobs{VkjxK?ZoCbfuQ z51q-x9+@t;+b;U-GkLe4K0cLO8Yhyyi`k6S+bv2`&D<&VUM`&8vEh1WYf!CW=^Uy+%bWYt zq-o|wb0%@ypR!{TbK$A3bUPHnsGw-=M-Ox{wEb3;w6N@vNAd=IV}s3{R@(=vckF)p zXa08+K-|JK6bV(j9j{ZYa3Zuldu@-E0 zhs?8e$@r!>RM8SJ*x2yCpZml{c#FAZ1IAL*w?4~y2Pl8XD0g$_D!3%ke|SV?I(_^$ zXy4|A>`m7iL8tFc=X0Op(m*Np8Qn^baWF>jPhLAF zv3+1{*l~K#($mwue0+Qip9cH;D;UEbJQfpMRST}x7?vB2lWaaFqsUf+qav?5z5AwW zbNH)WKWYK(!*lGYtogSp*@eGlERqaloeyzb8Vcv_<8S6LN?lZ66JDAuU!=Hn4sWoS z;uDIEHFq8xnbIc$>IvO=g+60CoPg?Eb_EksYb9xSiIs2Dlzs7W@n62ZaTw_Yd9K*~`6pQaAVzHEHc;A+i z*9-`8{6%=_>F?M_W|pE%B-gp((~OLXX+`Vd4ap{pAtHN-c2&fKAlQp(=xWw-Dw!eK z9;6$aya?i}#}jNfritEa3B-DX&$fRhRhy8I(B9E86W`58?+NjPC%93Hu(3aiRLua? z0?t1!uV#s5vgQNPxf*}H#KCt=jY{TLzY+a7o6vc0nRj1C8`IwAN@Wp&Y&({%^D1Rm zY^2!f?F5BS^Ohfcf&N|Y@q!jd4X&G~?GATE8SAY`wwQbdf>NL_@&tLq>zcO5OKEF| zQM>DvOGRV3Go?36k>Xxdc$c)5u}cW)CF9S&I(l<06!B|&Q#~0|y!(N?;kxWpZ(Dyc zeplGBHx~5h8LaW7Qf@AnWn4*KSNr8t^kTGjL=YB^Y3r0FhKn^CeiAJy8 zXs4o>K>eOvu4Ru|JXZahhL|9# z1E55f(kgZvc&-F|mc%f^NjHBQFCmluQo3H<8c-wgOh%__n?pC5ZqUca_oU%QI8Hkk z;ZYy5yPNYhQ>aIPpPO3#Mafu`e2J!-?80%S=B@Sj0W;x-iqBj715G^L#qVte{>015 z4=?vG;nOhCi@k5k^}8&@wP3rRnhtID_H`RWilq8+@}CX}x|kiyre-*9qX$3W@rufC z#Y$`ZYTh4^?R=}MFKs9FTc~)o*l7hdP=d_iNobmnkO^D=ta{RbqsVS%|C^}`!8Kx9 zR6oU5Fsz6nPTv~dlx+wp!ywB*tdm~_sLwmwUTnNen_T*~2gRtJMC+y{BuIc1FsQBt zb=oNbKi&=XEm`Z&H>4<`pR7%7#~pLmAn%@gd5;aw1XKqmCqQ14GP1po?V`{aH9QHx zUlh7b`g~t+iphVo8iejR25}^6OzIe|v71io)Z~ftsK>CoNp*-l;_N@=-2oe4>R&lE zFH)$j*6OgN?1^IGxjN)eHnjacqbm4u5N{|GZ_^N;qvBoY9OTG0y2E7+K}=qEw7Q!{ z5=MAuvdIkGOFqr2IA4Mrm}fOaTG+Z_+*}h`RVGxxpVl-MI<;jcuJy@S*}(KHR}Q{N z__i3`wRS~H;9vS0AI`<=_ZsfdNeHqL<9loCZnQh*?Yl{#2EM47qPWl2ZEAa)7vxf8 zg~8B6jkRQ`FF#s2YcuC1@hcg(Y!~wvN$>?LA#=;eRxq0cmM#?5WOt>9kWlc=*{P14 zwW5{C?V<=P4$0b))W<7S0!sv`t5rfCFO3^n8Zl$w=)U_nOBg1@<0) zG%hlrzP$RleTdGJAF(piCl8xgCJNs(Q1rzA@Ei{-wf7@dAb88yh1(ywjtFcIP4=Vp){z z{pz-ud`Ye{#oDZI+EbdR0Fp@}rId5-+2Xx+|H3aj3->eNag0@>Hzu~PGI%|F(7$-A z_T+bw{>E#JG*QQ#_gJmAmKP#Gpp+WE>0J3b+_%A)A8w*W)QO1J5X)lY|6~C z5C1^5Qx+<27FkAM%!i7E0>hBqvz;%hu*X(y<;bL zM}OpF$u>67YMEJ+7E2x^GrGF2WIS_L!mr5Q_tuRUl}?CA;*I^VTx#9m?avV75J zFYyROI`p2fk9t0Kmq~tJMK0huRDa>8PGy|0c60@&-uR{lE_kc*??}cz9G^}_00!Ox z`QEm$1S<^!W0ofIZDpZ9WND-|z6LHf)HiIbg@+5l#No|04{D-T5$yPof-9#-S}3v+ zk^;%%PrfvXCuh8Jw;_G2g2-M)Y7B#TzM4mDQ@3oeyw=mi)lgBoTB}GoR)Ti6{UI2Y z*mL8rg&8?^k4nViDPMijxPr+!@Ty3eOq19nckBzg5`=pure-qt)!B~=Pw>mZ*#$s| zCESR*m2r3@p*WG&PgO{|z+5>Lv|SXfy|5wXEl-Mk(YNS_L#k22Dj*_Lb1b;W*LQX= zprEcu626rCTS(m}I)HsW>m3pgr#pP)BT1|nU&vm!tmz_YvBwl}uI!4x6~)-Lm_)eS6_PsMN1wnVmf9o9M*@smv&p)^#9_;4EcLR6uxUA`Hurou zIK3~k3L)fC%x60{yYydhT;kP_G!yslP=yMbe~L~LAYf5{w;LnZk7V6)JpHyebgrFy zRVnyczVvV;JDe5Gd_o2D^>+{EIPx1&ohG$}?zuCkvZp{+DS6<sC^w%99c>R*e&LZ7hso?nHI;eX^AXcTzY|yY5FT8N z?q7~j#XRgbbX_02HlY$Km|G4mb^3&jMa>SJcxj!wIIy;D-p|!_mFZ+-`fF>YNhP&w zjLG{bU*k2xhAciC8o3!F)HTBCL$+nhrmyWi1s|1Sl`%d*Z8uql!ozb>$@;ymOBqtt z`^E3IP0tw{6=+)*PD|Db@)sAvGn(>=&QbszQFDrAwO3=x=kDg@AxADw=y`HMz%G{GZmr;_X$xJHpW9jE@|;SV=OCyhVe?%8HUsL`AZupVvRs zlzi9raNnmmJo+4==Sj79Wn#1TU8>f7)vk()lsK5W5c%94g~3E2E`n1ScC@3gL^^5*Exg2i$vK1ms^4%_5Utc$@i>f4Eua7g^_^iMHzuZs7Bm z(+n_cMU2T=PD{JJQ}hKKp#~y;HLk_hiXLu%QR3KwT1;9&$a3T&#x*n*)Wg6L1I=XJ@lA+&V`z zxAXmMnmg1Ll;i*-@dn9^6T}w7Ov8^GmI5Yw2QkzQyMqG7gC;>dQg?AK(w4nLG1CAu zPyIR1eTPOd2_&%cE*zaoiIwCq86^&a{7C-S87uVY*$2MA#@sJ&P{1I}N{_Ag>=1U- zpPoATMcCpIgF}7iG!WeNMb=pe{9B9$XwKGrwec70$y**}C-=F9ma1`2yYb8=259~o zkDA$2&DUdo)p{Yy$*?9E?|}A&y^8#4K)m`sbU@ePLJ4z=UWH~Nf)Bd-2t&I&g7_}By!T_%T0kww&4>G z5Dp0J=6tEk>$4-wb#lr&5QKbUqWTpbeo?bkSaIrCs-o#Ytw)!|_TyWQ`$E`}r2_=S zsTpEzNfCQ_RP7peyiUHx_lafirF^=x2!=a)W zJbpx+y=g*#n;c9!7QmywH1DkV&OOS&7Ts1W%5w{GoIgH%>6G49)`;WcjQ)P+k3R{zrMujn3b$iBf}d&+@< zqGi&r0}s2yb7G*@+y-FI7r-?0P5XtsR{7m+etJ89pmnne$41oeGuT#0)go*_?#SFQ zMO^iu=WIc3{1xFe4!EQ%W0Q(=^L=f_PwF4Ohy6gFI6$MMpr{HAK{aSCDsnyRj1@t+ zD}P5FQn09Dno?9aIr0s;!Pw+|zyFYx*Sj@GT_aAAZSdR~%SN)8b`C6kv|juDV)x1? z79JW`reI@2_2nBVI0lxzy}*JSF~p@gb5-PL(eKbUpeuM^q<6b~x598ne*A4eoVEkR zpdQ~+ze$UOEcHU}bH=8pEeDRi{TI3fWA>7arhT9$Y1P$GK#Iag%YrXnVu>A(>2hn~ zl{Om!ohvMZUdOX|x0KF;kBGp6}ucRP6qp5edyI;4!Kh=NY%B4^kWm@Hu^oJ%g^V;|Kq@o0juk_)jMsCxTH0 z89S^XWCr>}!!TDa!?#(;#XFCU&98^hqozWuHjq=&9U zacXc;I2IVXm9DLaS8j!$j*69nAQ4`tPZq>qeD|gz&6$6e1gQj#2-8FhMNW0C=@y&R z*fX&bQBU3%LPMdkfaB1>@~hbC!jpO%3XZEx>YYCOlL_CC+>?prv*vM4{WVpmu4sD^ zKLwYgDQ%J5kL$(|&dJ~=YaxO7#tcABH;#f~$9S0IlDaqdj6a}-eNVC0?umAtr3OKo z{{F~m`Z#-maf3)c!*cbRqTdgq$9bu3%`57dLuC%=}+)R1KI0LS+$7PxVvXPuz)C ziJ`L*GkgB3u5-R`H+Q(xH{}ah8>HaXT}?XnUd<_hu>4F_y3N~m!sQsF?BBP4j%kBW zuyaN%HdjWA*hzP8g_-ozda($1eNQ5JRW-{e%g0~m->U3W`LAsw=QqY;^4+L zDnF4b1m45y-<)%fm0G>houBjmd;G$x-#s2)$)~MezgOV+)hw~xpck?cXO!-`QGbOi z&(SR=%%^H-L=u|65wD$KU<;)WaCl3R^O_xxM+jH&uQOvm5kd~gq;bm7L~r!hq&YLZ z-D7PH)8dKIZ1ro~2WeM|4Xur1ft>jX@8$e30*!0>h8&Pb@XkJ(CYF(n0&gpO^Dd;~ z=4f=F7Pq906jHDRo13;!3QBncUtDgDanD7nW^zBB%)juuX~X_&oiNa|4s|85`OfLCBKf^?tZ|%Y%>cIJtFJ$zyZT|{>0uX!XxOtL-Agpjx&S`q@84i{K|-2oM*4* ztz{9KZFfR2)%!L@K<}vVaFK>8 zvrjQZAh~^E?W+&Xet~~I5r)(S0{XQ%NnEw&>bDn~PM>~3z$G%uGu$w zQD9&XAa0S&Q@cK~g?YUAUUdbZN@?{=27=ZjfGPcvxl`D#ctpBKqo-D;rzG?6T8ISm zGpKP#m_{+8>>DgbsY!*=wRSN>ED@e5q>kX~PDx0`FDTr_SC)+PhefY}&M^caTsCcL zk!ruPz%-shwmj|eW|PIkbHURC@i(lhTOP1pS7jq0ss67kp2b2@sc%Icn~hv->0SKV z+iF3_GeKYCfB5FH=rR)J>X+q8lG0Svbsma?k=>HcZlZ^=$!1KWF5jF@7V`kz@V|DW zB;!7* zd?q@bEDlC+zF>Gw5ayN(_xNO|bskkCw%W z56V=jD0vMD=QGp{rkrD-mea-odUxAvY961s)%>6Ph!&pS7*)d3@~9xfm}?T2>eThY z*}?ymMLL1W50S*h3sl4U05*p)fJ%q*M=ds9hK}=7>7%!8qTMx0lf>+wuXX&0h>v3E ziq!&x+@UYqO5{)Lk@#gRXsbiPP7$BYIzHT&O+JGME%Q^vCLo1a9^^jC`kW~SWmW=U zTu|JryRY8BM^8`t9SQqvh%h2toA2w)`;<#O;1KQnSX(CD+`m4U{SAV;=#?>0?#;KqWCmQg zlE}n}Qj|EoiX5PPQ>P>jP+4G4cD1(y{(|#3U1kuh2P7jAw4lyv>!aLPt6k0oIdJr_ zjM)DDqL9HO(@DD7$|iFrx90&RZ2+`&;k1Brk=umxx38@A>yEMieR@=JF&9t__>Jw{ zkIi%g9RvQ9%KNangU$n|v^buVnAISpVDfkD8}vt+EDV9*_a=pV*P9zKcuu3>WupYW zipN)1xd53D=-%#rT!32d98kp2k=nIm!_;tK(|jAT=>mDUE3JxHao`ByO#<%1jnlpD zafAa=MFjGO89ti@K9sYN)a@7+aC2_9Ad*7MjiHr7RqH$InN*qeVL1x&+2He^buiS; znTA1tQ)V=4-6iNLn{1%gC3v^}>&gsDsRF3ac&_H{p`}=$T7t=Z@*`jd(AUgKW&Jh^ zk~{?pDv^mvk`D4L;Il3~zkmaat|I7Aan!~kjl(z~FCFD`IwxGqHiSIH%ZuPe!}%)XrZ&zWl|E@`n_=wX3@N9Sc8F&LVOSUgA{%LgPU= zb=OHbw-+FU?zRTk8^+@Yqqjzz&?|4$q3Nd|5Ijn>AM?VPbEvnWmE1W{EC+Zr`UOa? z+4Ldyi4Rd{S!H8VE{udr3Zlo`VH0V=dq)mGDMJ0E{QvXxwAJF|D}PxX87J?6&570o z7fnqHc+_hHOp7P1d$^3Jw}F53ot2Br9KEfNME1Q zO*6H+h0@Q`MJsJ| z>NDQ4zL{ruf-qQTvBnsvvDVxY|8dP22->jt?=76wOe+i+GTLpa6RQZ!Q3CvWx>RKLO7}C-cN~kC5sFNBu z+de!9uVj?6<;2)a{4?gqNZ?QN@EbCZsi@twR7=x6j&ugZ(9?(wiEld6WMUO!rLZgKVM|*8PfcMg6F`|(3=rDPto~Q z7KMR#nA5!W#z$J`PlK0E?b7Oen*Ko+1$FK}^T{IWp+AM1XXitMFIYXLStnoCwZ7p! zdYv7QUvV4WCAgRUrc9tW<+0>YE=7Nqa>_m#b=FiXKYCFVTI5D_OslQ-{L}Zh7vEd_+S0#qYuZchfKU}^ zxU*22qknrwQ@!F%%wagI+e)egp<#LDh>aS?jMg<-J6k#XpJ;-A%gU&|uKf)(tSYLB zF+5fKbLKDsgdj8tiRo(eU>v@|&`g$sXF2~pn?1kjn*e<_JITp5)1EGKehJ%14I1nx9~5dpyyqU83`6gzghJiS4NZ+qJ?Sf6KZ9MV z+KeufgPM0%^o;xafT>Lj!@at9s7`}=h2I(J+RQCOt)8VE2INfnq(K@5v*-UEjMVSI z_1?w)(COFYW4B%xyTgg@(_Lp`O2*oQ{E-3L!L1_;9jLdTd|9@`Ok(5li4`IYbDa#!p|RjI;_*Uc zvWzo35V}0XgUWe1LohtG#A1p_&uZ-1aPIA3-Xs4BEO=Ppn4L#z!LgL3KIMk4@W_`2 zQVNucO;16$+*GF(hOk!r=HUlEe-mrCmm>Cj2wQMW?Ik1m|9K*%GaxzJpL~NeaOKet zA^u>?Ap>Lgq4MpYIfi*Odc=US&1eeGe}mLcN%#`utgo7M>~i@niLHfiW(s40n4A#( zx4`sW;`8>ZXk;)+xlqhgOj+>wywY#q-$5m>A{0%KMb_WSUeuO)$h|BuZfNtPPP0v0 zBjZT#wuziu{@VJTN5s?to^UKAnG-n1E}-Z>fzzH62pqL6Ev13lqG@7@;F07XoDCKG z?|#`wMHGb~1?#)at(+H*i~;bqf?$MD9VMI#lcs*w#YB6v{VX#j;V#!?3X-)5TrwRQdXpj|sW??d^dmm$f7G|{=o`{hG$47RHx z-+jePPj4GQKePFXU%`MnXH+p-LeZ<5!@7G~isl}Eb{JG?oe<<4QOUQkDWq@f+QOC{ zaKKcY3HzP$ynp{5po*5KALqY;M<}ag7%8zMf9*!6 z5&8@MN+Xr*J;ZZ{>aU;dpk8Xwq3-#F^+xO`;DEN60R(28()1nVT<>p_52+}O z;^_I4!gRx1t!LYEDG`?-D~ZM>W>o|C=21AWtlR4Azs;Apm*vQj`ncl#$&_w(Ef{-=9pz>~{ebQJmjLZ7Vtuxm`(9diAjyp;|l@RrWbZkuG^#of0fy` zuK^QKA%Z^)N&0q8hy6Xs_M!p+gdraUsj8o>T)}*HpH$2n&&o>Ick7m|WjMK>!pL_@ zcyZS3%hNf{$c!fJaXh*)H@D)r)%!TDabZ=;URYv?97KNiS9N#7E-$jvqqJHta!%D; zk8;S~anG-30Dpd?8u8637dz7PH$z;55vk(!p7{?vA~MWw)`fmM?NHAx&yfN}&HIPm z4H&S(z&>r4(^?>S)wR+^Wm>wUET+Q~w!%+&w!3c$qDi#?)me`n+?~)XL*=|O_xGIG zhv(CaXQTb{O}1C$x4$#ACUO`(WN{=SA;1%|V*`%SUXH*hmzDVUkj4cPLjd66d7SuW z&<4Lfnf_}5Np#(TXh!OgzGSWX65Ge^24??E^i1ZMb-eDH&Z;)O>M#PA6YX}1HnM!> z-1oy0OdAp*WIZymzJ{{tH6J#(4-8CZeS~(X=dJbIt26H7@J6rniKCUHK9kH61o{A; z-q^iO+oL!1w(4%gF%pquS&%LB9K$De1URoXw<6zFCqHaF&;)? z_lUD$SR60D7_Y)`VeF$YC9hW(zrND~Dex4TXh}O;9oF1dqmmR=SSKBQXC3L8@1$zV ziK!yzV||!=$O~ce(EDI@qen0{MU*b<5a^Hnl3MnL;5t?&-E@Rw{wD7k9Dy-rlv~`- z`C}D?ON+MVc=>w8;Am=aj%>Ad47F8}t_-VOUnp}qdM0n(u|e}+P}>*}M$eHuqjAc8 zMLoxav8>I5zI2LV<7#IEkJv9xc}v$db!u#NFd16Yov!FXh{b6SZBI!4t=1)z8{-QP zVzjW5)lR1{;PedqcwX;nx~lFSqg8wNcnK@FCKwvTU|KeYpN+N|mqj zqJle5eCDq;f#}(%wb{poQr{VXrRF=U5B{cItnbpL@#FcuYpNv{=heADN0%Ar#E_gSFakKk)&(zr!J3Ru1DsA z>+e66RbIW)d42N!N?S?SNFfU!+>4T>^p(@EK1?och57jI0mKrczC7V?;ROqcUW4uR z3E(|{w-1hHOBGUMPWa6}*Jkoyd!Q?svGXN3m#`W!;!XWhSHs0L@BC;6B`jU;f|+P9 zF^*)UebLPYs9(Ac*1K(7rcVFrCqOwrn+Oxx14yQB( zPxV%%tY2X9@_gkm7&{3682yOf;f-pSYFl<~m8eDL_xGZMtZiO=hWbYa;)^hlP#+RG zP;&9S^U}JfF|Bw#>K)Q}DDq}Fiup{6O4gTN*%qx^^wf7pt2Djs&$HrjuA9d~v{{Qy zoR$(9gX4kvO<#6|_@zQQmTfDl+NI!9$pRB}^De#|50g!)i4O(|Io5xGk zM%;sVl^puiZ~1aZx1^Yt-^tUZ(a!Zfq!n$3eCOJRp|;aw6+P=(omKxVA%Z+*Pk~m*P&_zV32KmIMkNBR1|ZE_PU7uhZHSLBbYUs9x}xQ=;1S5pr!jbb z$Mu<0s|`tl(=ZW01Cc!m!k(I*-(N~!4lUxkZF-{7)@gOxADGLNcP~xGQ`mj2^HpDW zLk{&aUxaUiHRtzrRD=bo*9J#Lo(mekpULXY#Mr7o6_-mn3&j~va9eJlJ|C-4#XGP* z-r+7Y61(j*{^W+&eX|oR(ZJ^hwIexTN72CNH0A)VR!%=JXxfjv=KV1%3*Sw{f0*M)=2Kws-c{u?d}JtlVnW7i2w$pF?;@#~0qb1k z%<>1vfxSEyCmt}QU@1S2no~6M_ZfbMmgXkyE4P=FVc(e?sTvuEc_)MS)Ru8~{v_8SmSVKZdM*TI^eh zk(&Yj?$jyoHv+y-l z<~(bxnPK7y;4Y^ypG;R{MbR+!9Nn^V^RDR;n*kN1F?T*B+Pts#Qs0ZJIm^Uxv-_l} zF%??DUDLf;7DL8to-TsCS9etH+n`TH7ma2`840N8kbBUE}*LI1AAzN}_T$l^$7?~O= zpA&7EXTgnAd@~XREeWD<`sz+dr1rdk&z~Nl#j+fv8)4ucQrsB+pusQc_9nW2!oPlG z%2`}UoqcUA8tiK()JlPj8`f26i7VT{;Vr@(EZ2w^HJQ%;P_NaD#!9g-OGEL_3*7A% z=)U8iQ#BvzE|2Xk4|2+9aKVsU@$=Oc(XuyG{}@1IuHAXamTs(vw~Z8J3y5VZc#mPf zpmd5GPW9fM8G?3LK0Oz2R@|32TTEJARU&NR(kuZ93yh{mzzkFV7@@1xMeEVbF~*`f zi&Iw7*WdpZh>`&Uuhv$F9)(9C4+OViLIpM z^b`^3{oJ=}*|~W?mrIJq_2}Jl@Wz3iFz@{Q&eAh|oHd%_@NE_+st#%hsgG|#b2TME zztjEycyim(A zK7^%A6W%f`)X%qlaQ0u$9+{<35JCoR=Zx?&EbAV3to=2obGZ>X+KH7{Z-XF*kRCOs zZ9a9Uj;XqyuaU+-`$U_|`&4svk3$oFy|D(+)T1e)RFwy0Ayxv6{Gay_lM$EsjbF+F6dvday>H z_~o>EH@6#=gW1Gz{0cIxGxN)_ZaT;w@cixs0nZ9MB5iCNzrta^#?enc}dGgS6VPy3XoxRQc0vq zF-2WubQjg)n!Dy?Z4P%G7YXJya+yDr?LqA9kC2NG)C4XH5PKToP8(_|3$_I{&N9vq z@CTy*ex;*T6rn)`Dh8T@bNb5%C?C=Q_M7ZMVbJtApHsg@kKuT$SmuxJLY#@}{#pS% zUTt)pZ$v~SSqb_Nh<$?{w0A|-8xv`^T1<%WCvDE%x^_g z^eG-Q@)Qsm*cF&EV!9QyovzpzNDZbqlBmN@>}s;P{&jR0^2tAKr>dW}zoo(GpYPn) zA7Q~zrk|ia*d?&q*ATfp6n&<}FL&rU(3WG2L+>+H3GXSb(=! zwrDaII_NBn`5Y5BE^4PXvh+p*;<9o^E_XahZz;1Gzpl{9zB7@ZnDd=_9RBraxr=8f zU@N6g@P+giLlvScJ|Okf2G0zWXUq@&)}0U-ux;b)T*p%6n_C=*Lydj=FqvXEAKdhF z)d7;~lPyfPBR?1|No)1)-5TXu+(UacQXDM)2%`^V$vNP#V}DRute>jtSHlA~ zoLpf?m&3y8&tsJr(n1zhM{ z{hV=Q&<1DE*)SO@!UZ_B@g6)r$k*YxlV|tGOaHx~oxVrb^XD4STju#=Z<0E|M$**& zR5aZ;5t{Vu+U2Cz>ph7@k>WDl#UAzO)kmtm+>LSRrw5N&)q%&t%r|j_m$@=si|uSk zlhl@2cq9LJ$LC9B{+>$I#6Ht$;>1qx3aya#LAN2_;9868JX`D0h1s4fBT*y`9esa% z8f_Q8ccT0AWDNPM_JWCeirJ+>c`a=a`4tuxx7Xq4mDky{s|e?JMFKl2=YG+4k*UuL zQu~qOV9@FXAf7@4{;@J;VU)&Jvg;2rLHIJpKJacX4;i=)5axDKQB^pzxSA&CcRSx> z74PDBV<*JQy@_*-_lp+8UNib`wy|1@}Gm!SAaS6Jho#wW^o&AaDx zG9pLZ>DIX;cLU~Ig?Zkdy6Ako2L$K+0x%SX!(S0CiICyhYa4g)$82?I8q-s9KGUx?L&ZFVzwthfiKPHRF}*$)A_OePret$>Q+l=k0q>d0wLK1fA_MQrVNuDWt50 zZiMtXV#n+6Y1}ouZ--+h>f_8+yCtWaC)~}EIL?SYI$!l^f_?eXB$y)OmiM}H|I^l_ z^Qbs)Btj81U!j zIK9&~lrhX|iPG^yM=Mi}=8TkXGDYLQvD}jagWBd*iJG=-S=^O#^Kx;qVJ{||Uq>Pf zLza-EtweRjZ9n8zwQcU$*zH+=K*e+=oxgY`;(*}my%sm->b`*0#OFe7f&zaYl;4+D z&qki_@w|!t+ImW8QT#r1w>g4n{vB-Ta!S1UF0M7{ zHdIVWxX`<6tnpp6yn73j*{Rwdvh$T>RhO$fqtMdt(c>RXvK;k*J-iDlYMvQBQ=4xMW|2#b`|x z|AU6#C{?~r-nXMB&I`1XhwImIOmym#e%F4-)t8=`pL|KO@6PbcS5Eau!=D5+R?+jd z?t>Buy3YEMkT`wKJeZaSBlC-TkReIm2Dq@svgj`IZ*cAOu<}0s+gV-#igrW5K8Q-p z6%FJAY?hm00VnMMpqmAHr2Y$CU@x}f?CH{5vg4?DO-I^um#WK<^Y@adXf)LG>E+<> zu?dNR%c=%#s^I`x8K2sE35L=z^Uoz36BO{9?72<)t<~5JnrKe23_r_<=CqgSxReq@ zZ7x`5;yfug)^(VMNSpV1^}l{0>Y8&aZ{FM*V4X4l+VA>g?DC^So<5&>BuqGu_)iN9 zl-C?=C4K#X7`6xS=yQ17*DTzDWnW#PE7v+ixV$}OV}OFseAZyuDd4Z`?!gvtvR27O z#5%dwP>YKK@FL=_Ka5lT0=;<3D$KLZiN(r+ z8?^hPaM#kF6f08e_MwVje)o6!OGycd3?Kk1Y83_I&c>TZ{B?le-bg!Ed?pVI_Tbaj z$C(5hefs1ZSlJ^lW`2X3*dLyq>KgX3lOzI7wp@8L9$LH`a<|m{qsJSn<@9)e;P5$! zZK9+c>G5VO2$=Dh=}JwUzRxanXZ_`Cmo8_|?RVr5nU_agsrZm_$|S#5ipS+ z74eu4s!pVCHLIQhy0p%_Bgl}Zb))jS6O{QM2zc0WD!)?*Z*U`Pa337C)f%ij@8bjn zwxr%!ZO46#;*BtQTjU?Y$b=CD{ZW$wH1}2zUwM8?MIl*CW=6DWA?l~sY00`9Uo+Nw zzxg^W@8xK;Mn;&aLiOuL-~82#IWZs*UoT1Uo=PrN%VK%1)}9Jb(#B404W(^C6=s1@ z1<&L4n&{!(733)euo|oB}5zxFLxR#`%`e#u?^^Y73UCmoB8 zj19#(pF=#9-3T#0zDpPUIHKIhq8$+!NSGh(Z3j?sfZjE$+Y%rzJ`N25a#f(?0f>qF z6PRlYk(m^An_bwL*S4#n*7}ITlHBk=1VNB z+`MRSCZHg^yu2z`4x7yF91apbKr%3a-;wT03 zJZTD)uil41)ujOs;mR(P2+eaj47u3}%PsIbKu05(D$w&Zom%E-I}7h|A`f%)py`nX}=(K%-vhWp@#aGL$i@Rk-|@2Ndo z(m+;rHrDIcyW?vrBFNDjQ1z+5fB!OBzIp#%+*3aW{{Qhj_~#L2x5*RCW~6AnpacI8 zC3_xNPdlINi&e8q7Z`V_2@WZXN{&F)rhyKhj!vCV#>X9dI?~(Zz5u?h(~O~7w{3em zZm=j$MVBF7d4D=!#woWnB=)Y%Mpsr1N6MbbOo&mK(!g@y;Mj)5)l&ifyXv> z%V-+FPaNNtX67NDwOs|8ObhjT*FnCnqYf0Qg;Rdu~3Cq9rp`mJQWpN0x$|i8lvEFZEZe&aRow_T`#!MILSb=a=aY0|c zJf&P}KU6;}BEs1b2+!xB2(4$Kr{Dcj3G=UT+RBEg%g4EejA|^H` z8uDDb*6w-c2w+(gPrU$Q1K{`{fwnzp!N#oL`V2^+i6|*WgC=dD8qpROAK9(u$;imO z0QPgFP@P#dPI(2&ObJLy0{~%nU0pOXR88LPw1ZlI8or(6(Q z&{yE_>bx! z{uNEiHrNeAfh6_C-jrhHS7p^W#f*!cv98kwfUgBS=A(f28%jJ6y0P<>Y9jQo7C>&X>+!zfgGLwJ&*gvtMB(9vVlpzsC3De%*{?~p>7OIB| zxa<#BduhTK>Yby2c$kAjg;C0g=@%g93}kGh`unB9a@1DS@7Opxk^{O@*MLDCOnf#F zX|xsu&5?6)B?1#%|G#QG^Khv5w~vd`uf0>Yl%-B~A!Nx`igPU0Bz5fj*q0&+A(c`p zWtWU44mw#ITPVw!6EjNaaF{V9V@VW>WP9Gv#RSF8wimoB4h}bAR6V z;&b0G%NLGKymzzC*q&8w(HOykg7QUzD*9YgWOUL~bB9W!=?RtGMH zg~3Q^Q&mDmc$JKDC z0a&A#K9zljn4*u+@n_7x2aC7n8P(V*a$4ZP#m^1^9S|TLK?NIe{OTT*ua3;aJjCYx z*jpY0x5OA{jr(8UBKG>qnhiGQr)hCoUrz(EJ@X_3&gy5w+tMpnWb+9tLVLJ^P1PZM zwmZnT2mv3twNZ)`j^&uYrsy!|qj%(Jb-7zCEk;lUcxWy{6F|R94_SQjM82!Y zmVtT0n0YVXU5sC=h+hW1Po zb$M}~;C}>nA?Wj8UJO?AW1C*(-6W(J{$onJaoXf?Ur;|8Aae<};@-X2)cY^R!*?Vc zK8;5t7dK{1-@Sbs_{yJ;8X77))N@t%_F4Ttfk`X32?W)S9WC)Ebcuw4PSpt=+7e$4 z99aX7g$#Y3$}>%E#E!HTHysZ8@Ky$=leyaNdcsj^^9oWP`AB-e8 zCmVuo18A6OhHm=8WKD7XLS5*bE3j$A3+SETx(pJY|IF|)B7>UikFaM;#6(AH7(`8G z-Ny9L-Mh2uoh?(<{X}u3NA?XHcO|6aYMNSZ&G)p&d+ZLCQ#Z$U{`UDD$k4gwFEg;y zGs3?YaLOATGT~tf@(Exa&;ZMFPkvy^;>l^rb|LF7!|hf5IRTbI>o%L9iC)^x?NZaV zFQC2fbRQ3%LDU8Mi$qbDG_Zu)U)~(a$;l}}WTE{D=d)~y7m_9jzD57@Fk-?poZ5Tv zv0bp@@B#saI$(tlnbz##$1zRt`w~x1q3PnR%uaD zE%;Wq2?z{RFFd)kOEV56KBjl~>8uV1rIL`Zp1@_KU;)sjz?+F42ZCfhFfd?tqlM^S zIFSMb_Q0c9-9ZA}`Lqw4p$F-L^Ix}j@7uSjd0jTd^Jfi>s-r&7zjj#zWzp`>T7+E_ zSU1P+(u3zm>p?*?%bsQEgNn;FwVqgVY=ULc2 z)|4AjQBzo@t=qOa1u(Ig`~BG35Z>d4hP(ju4MFXl;f5tV1K?K9e`1R??{H7FcXyYD zN2naP2!y?zWRhhf@@=a9CD6xnF3=L@d_m^vArDpIVD$IM>sZ9<%nD&6kr7GN&h~nN z2)Hr6W$4QV+Z~btKEQb3I90X0`6NOGIKXXNx014#(4|5&yCA=!`Gyjk$*0#P805?< z{U(1i2*#?7JgZ#2irf#Gg9XrOsl=G3!6M8fTBL=HWz|@KB@pVLkxdR-9 zxQax_znq8a)Xs>IoBZ1!e~z!K7{gU?hqr)N%%Oo>^7upd9f9du8hNVgH^fVXQM+M~jBJCLUA~}42H)PL zS@t%<*K0nH*Xr4M4~my$q_(3c_# zr#X9KwVeOjXhn5*oTvcb@7eLT@dr~tNSW|oTSW) z3G3&xNPhUR74gg}f&>jPuG7t4!JmFA$hp-TwK-#2E!C80djm0a$D{UUgvP;xH>HI3 zI~~0rIreSp{SQduRBXi1O({yfx7rN+)C{7!`1Ew2pauJ|uZgD`ZH<19ooCoD4<;yS zB7&=7${T}08GY-PKfUzH0mSTDlT}T_p;%QKlezOU5*0|TbN7C5j9h|61qA*Ar^zn+ zK}QE>SaELI#uQqMKQNVKETB*%;Yp5B5u-I>mqjeT7sGBfyt~Vzd+GKm zGczIL;ej;qtDWLito_UlHqQ@(M(|egN7$qG(S)GdcOSKi}(?jGDXCA54jZ6TLtA1 z0A^+~Rud?1dtLCssM1l9)&iN@)!Qg<3y0dEwhjz2(wY z$X{lrxPZKc!@5Ka)t5cn_7VQlf}8B_?jAW8i>dwH_7Oho;=A(7eEr;{NCUoog7-t@)E^>b3tk|PRpWnvpe^ib^4wGLeuea^N`KVZ@pR^z^&ub z(`{YyTbr9pS{W%TrPQEXG9NFlTD6Kml;U|4Oj24Ccgos>!)X?lgklz`YiXq+%W)bhI)5!ALk#LB z`}Dn_L{LdPl*#6!#}b&^+XV&l?)PDOhFr_cBM#Y_ygagbg${XYR@NIJz`86)nq_Tm z?X69T2ozs-cXyBZjHr9Ql?G4Ey2w%aHxg;$wT(pdZ?_;hiCUL5d@%2;=Q8i3Dt9_z>rHq1uQ^lJg9?dlqepgNr?15%v) zc6&EBw`8Nt)eHI@(5mub9;wtVMVsP_lmRyT&{fEIDHdUqDb-;G)y2w^vCUFUe>aPw zUtPP!pF`(ojKg%FhR*pSe%d#Vzcl@a{sm5hf#@F)QnistBFjiy|LS_w8sn#bzuy-q z`kF$rJd>ng`yt5@RQjohx?v9h>9gqa=_!j^#luS~w$ouptb`$V74dXN6?5Y*jeE$s zamIT+6wd^a0PF;d{RVtr)C;FT>T_(m4udH$&zc6Bt+{h&4=NyO%zc(+IsXKUgEC6W zZeN1ZCnf}nQd2k7NehY2d^&5g##s8Gxq|)kheV)f+6%^PJ~PVbGH7!ARWwIbE#P3F zyLsiP_>8iuRZuxl@ULODk91{C7vxa};h^_by!;Gb-KG*!ho^<+<-Yu-aXGn|s11Tv!*m;@^9>CR29wEJVeQ!E@cO%vLw1gZz=EYJ${Z=J?&HGr#tZUi zNFV@`6d= zLv{AWrk2FS*N{1)722nJ*As)O>FI2=G9Q0zj2zUF=6ZDe6x(3JyArrNf%$Oc(+}A@ zEfZMmN~8mstlri1kJJzk&UHEfcX5e{G$Jn-mJIkh!AwTFj!k5&6Be%}6B8js_)?2= zsY-1!em6p;b3alM-o2Sa%;q|G4oBHn54`DfGVZW5AiZ@{M9#R5FKzqG9I01C)|Ouu z*(6O9f(uhGDw&8}V3n!naB*?_7#%%q9zHw3F2|c|MC$as@#ga6ro7l^cP6%r!9o}{ z>Mv&tA9K`3L0Q>fU1@#u>q?DZYVB9@N+mE$b8 zU10QL>qHin-Qrz-Wb!p}nVF{L4Wn^VD#PZj%%?>yqOSZa#|c+j)Hd%F-!z}Z0-Bb% zEcKR!eB`!UlewyyKgtOA>CNA_H9L)DQTg{23I@2i48@))mKoMmqcB5=cHlkars1&4 z869=O8SRzKU1{$+xK`g&(Rg|Fp{DeVUWmX<@Av&d`z>^=6o*3Z{(X>0&Sf;M8`{q( zQ4f6XJ^I=vZeHvTt+paumGdd9bt1i0q{l~Yk)zl(Q~7eR(Z#VYrNRz!6CEA4e*7lY zZ6I)&^Gst*kCN#jc)St253W5GomvF~VyU+LWV@VJ*?o0jWOP0tfo5zRoOW zb#w)!<<(B?NQi$?zsO{MNNp99l^Hxqtn0(3GnuVn^4^}!jfQ7Ow#3mk(<85}D$CdR zL4`hCkL)WVT{@+$7iEImzTZ)LVCRXGJU*ss6u|=rNcT_iItJ3UBAP0$kWU^i+uMCi z_D8*5FUWe}fxJ!ODC8@dK zbB~_;M9~Mk_;*NWIpnvdvseesg`1g2JiL~Ty|srU6o_?x9j|9P93lTWuj5QH*>QaR zMtV!X#!%fvHu*|G_?Ei_@% literal 0 HcmV?d00001 diff --git a/docs/example/path_planning/spline.md b/docs/example/path_planning/spline.md new file mode 100644 index 000000000..75c5323db --- /dev/null +++ b/docs/example/path_planning/spline.md @@ -0,0 +1,8 @@ +# スプライン曲線 + +スプライン曲線による補間 +CatmullRomとCubicSplineを比較する + +![](../fig/spline.png) + +{{ include_example("example/path_planning/spline.cpp") }} \ No newline at end of file diff --git a/example/path_planning/spline.cpp b/example/path_planning/spline.cpp index 612e6dbc7..3859ec08d 100644 --- a/example/path_planning/spline.cpp +++ b/example/path_planning/spline.cpp @@ -56,7 +56,7 @@ int main() x.push_back(p[0]); y.push_back(p[1]); } - plt::named_plot("Waypoints", x, y, "o"); + plt::named_plot("Waypoint", x, y, "o"); plt::legend(); plt::show(); diff --git a/mkdocs.yml b/mkdocs.yml index 704e90454..adf1d690f 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -89,6 +89,7 @@ nav: - 状態空間表現: example/system/state_space_system.md - パスプランニング: - Dubins曲線: example/path_planning/dubins_path.md + - スプライン曲線: example/path_planning/spline.md - FMT* (Fast Marching Tree): example/path_planning/fmt_star.md - 差動二輪ロボットのナビゲーション(FMT* + DWA): example/path_planning/navigation_diffbot.md - 単位系: