From 90f95bca0e4013a70010ca2be418d6d12f672e40 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Mon, 23 Jul 2018 17:11:15 +0200 Subject: [PATCH] add Bezier library --- src/lib/CMakeLists.txt | 1 + src/lib/bezier/BezierQuad.cpp | 226 ++++++++++++++++++++++++++++++++++ src/lib/bezier/BezierQuad.hpp | 182 +++++++++++++++++++++++++++ src/lib/bezier/CMakeLists.txt | 36 ++++++ 4 files changed, 445 insertions(+) create mode 100644 src/lib/bezier/BezierQuad.cpp create mode 100644 src/lib/bezier/BezierQuad.hpp create mode 100644 src/lib/bezier/CMakeLists.txt diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1d18e09ccf..c8ce5ca7e3 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_git_submodule(TARGET git_matrix PATH "matrix") add_subdirectory(airspeed) add_subdirectory(battery) +add_subdirectory(bezier) add_subdirectory(circuit_breaker) add_subdirectory(controllib) add_subdirectory(conversion) diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp new file mode 100644 index 0000000000..4660ab062b --- /dev/null +++ b/src/lib/bezier/BezierQuad.cpp @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file BezierQuad.cpp + * Bezier function + * + * @author Dennis Mannhart + * + */ + +#include "BezierQuad.hpp" + +namespace bezier +{ + +#define GOLDEN_RATIO 1.61803398 //(sqrt(5)+1)/2 +#define RESOLUTION 0.0001 //represents resolution; end criterion for golden section search + +template +void BezierQuad::setBezier(const Data &pt0, const Data &ctrl, const Data &pt1, + Tp duration) +{ + _pt0 = pt0; + _ctrl = ctrl; + _pt1 = pt1; + _duration = duration; + _cached_resolution = (Tp)(-1); + +} + +template +void BezierQuad::getBezier(Data &pt0, Data &ctrl, Data &pt1) +{ + pt0 = _pt0; + ctrl = _ctrl; + pt1 = _pt1; +} + +template +matrix::Vector BezierQuad::getPoint(const Tp t) +{ + return (_pt0 * ((Tp)1 - t / _duration) * ((Tp)1 - t / _duration) + _ctrl * (Tp)2 * (( + Tp)1 - t / _duration) * t / _duration + _pt1 * + t / _duration * t / _duration); +} + +template +matrix::Vector BezierQuad::getVelocity(const Tp t) +{ + return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration)); +} + +template +matrix::Vector BezierQuad::getAcceleration() +{ + return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration)); +} + +template +void BezierQuad::getStates(Data &point, Data &vel, Data &acc, const Tp time) +{ + point = getPoint(time); + vel = getVelocity(time); + acc = getAcceleration(); +} + +template +void BezierQuad::getStatesClosest(Data &point, Data &vel, Data &acc, + const Data pose) +{ + /* get t that corresponds to point closest on bezier point */ + Tp t = _goldenSectionSearch(pose); + + /* get states corresponding to t */ + getStates(point, vel, acc, t); + +} + +template +void BezierQuad::setBezFromVel(const Data &ctrl, const Data &vel0, const Data &vel1, + const Tp duration) +{ + /* update bezier points */ + _ctrl = ctrl; + _duration = duration; + _pt0 = _ctrl - vel0 * _duration / (Tp)2; + _pt1 = _ctrl + vel1 * _duration / (Tp)2; + _cached_resolution = (Tp)(-1); +} + +template +Tp BezierQuad::getArcLength(const Tp resolution) +{ + // we don't need to recompute arc length if: + // 1. _cached_resolution is up to date; 2. _cached_resolution is smaller than desired resolution (= more accurate) + if ((_cached_resolution > (Tp)0) && (_cached_resolution <= resolution)) { + return _cached_arc_length; + } + + // get number of elements + int n = (int)(roundf(_duration / resolution)); + Data v0, vn; + Tp y0, yn; + + // check if n is even + if (n % 2 == 1) { + n += 1; + } + + // step size + Tp h = (_duration) / n; + // get integration + Tp area = (Tp)0; + Data y; + + for (int i = 1; i < n; i++) { + + y = getVelocity(h * i); + + if (i % 2 == 1) { + area += (Tp)4 * y.length(); + + } else { + area += (Tp)2 * y.length(); + } + } + + v0 = getVelocity((Tp)0); + vn = getVelocity(_duration); + y0 = v0.length(); + yn = vn.length(); + + // 1/3 simpsons rule + area = h / (Tp)3 * (y0 + yn + area); + + // update cached resolution + _cached_resolution = resolution; + + return area; +} + +template +Tp BezierQuad::getDistToClosestPoint(const Data &pose) +{ + + /* get t that corresponds to point closest on bezier point */ + Tp t = _goldenSectionSearch(pose); + + /* get closest point */ + Data point = getPoint(t); + return (pose - point).length(); +} + +/* + * HELPER FUNCTIONS (private) + */ + +template +Tp BezierQuad::_goldenSectionSearch(const Data &pose) +{ + Tp a, b, c, d; + a = (Tp)0; //represents most left point + b = _duration * (Tp)1; //represents most right point + + c = b - (b - a) / GOLDEN_RATIO; + d = a + (b - a) / GOLDEN_RATIO; + + while (fabsf(c - d) > RESOLUTION) { + if (_getDistanceSquared(c, pose) < _getDistanceSquared(d, pose)) { + b = d; + + } else { + a = c; + } + + c = b - (b - a) / GOLDEN_RATIO; + d = a + (b - a) / GOLDEN_RATIO; + + } + + return (b + a) / (Tp)2; +} + +template +Tp BezierQuad::_getDistanceSquared(const Tp t, const Data &pose) +{ + /* get point on bezier */ + Data vec = getPoint(t); + + /* get vector from point to pose */ + vec = vec - pose; + + /* norm squared */ + return (vec * vec); +} +} diff --git a/src/lib/bezier/BezierQuad.hpp b/src/lib/bezier/BezierQuad.hpp new file mode 100644 index 0000000000..0d03e99017 --- /dev/null +++ b/src/lib/bezier/BezierQuad.hpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (C) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BerzierQuad.hpp + * + * Quadratic bezier lib + */ + + +#pragma once + +#include +#include + +#include + +namespace bezier +{ +template +class __EXPORT BezierQuad +{ +public: + + using Data = matrix::Vector; + + /** + * empty constructor + */ + BezierQuad(void) : + _pt0(Data()), _ctrl(Data()), _pt1(Data()), _duration(1.0f) {} + + /** + * constructor from array + */ + BezierQuad(const Tp pt0[3], const Tp ctrl[3], const Tp pt1[3], Tp duration = 1.0f) : + _pt0(Data(pt0)), _ctrl(Data(ctrl)), _pt1(Data(pt1)), _duration(duration) {} + + /** + * constructor from vector + */ + BezierQuad(const Data &pt0, const Data &ctrl, const Data &pt1, + Tp duration = 1.0f): + _pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {} + + + /* + * return bezier points + */ + void getBezier(Data &pt0, Data &ctrl, Data &pt1); + + /* + * get pt0 + */ + Data getPt0() {return _pt0;} + + /* + * get ctrl + */ + Data getCtrl() {return _ctrl;} + + /* + * get pt1 + */ + Data getPt1() {return _pt1;} + + /** + * set new bezier points + */ + void setBezier(const Data &pt0, const Data &ctrl, const Data &pt1, + Tp duration = 1.0f); + + /* + * set duration + */ + void setDuration(const Tp time) {_duration = time;} + + /** + * get point on bezier point corresponding to t + */ + Data getPoint(const Tp t); + + /* + * Distance to closest point given a position + */ + Tp getDistToClosestPoint(const Data &pose); + + /* + * get velocity on bezier corresponding to t + */ + Data getVelocity(const Tp t); + + /* + * get acceleration on bezier corresponding to t + */ + Data getAcceleration(); + + /* + * get states on bezier corresponding to t + */ + void getStates(Data &point, Data &vel, Data &acc, const Tp t); + + /* + * get states on bezier which are closest to pose + */ + void getStatesClosest(Data &point, Data &vel, Data &acc, + const Data pose); + + /* + * compute bezier from velocity at bezier end points and ctrl point + */ + void setBezFromVel(const Data &ctrl, const Data &vel0, const Data &vel1, + const Tp duration = 1.0f); + + /* + * simpsons inegrattion applied to velocity + */ + Tp getArcLength(const Tp resolution); + +private: + + /* control points */ + Data _pt0; + Data _ctrl; + Data _pt1; + Tp _duration; + + /* cache */ + Tp _cached_arc_length; + Tp _cached_resolution = (Tp)(-1); // negative number means that cache is not up to date + + /* + * Helper functions + */ + + /* golden section search */ + Tp _goldenSectionSearch(const Data &pose); + + /* + * get distance to point on bezier + */ + Tp _getDistanceSquared(const Tp t, const Data &pose); + + +}; + +using BezierQuadf = BezierQuad; +using BezierQuadd = BezierQuad; +} + +// include implementation +#include "BezierQuad.cpp" diff --git a/src/lib/bezier/CMakeLists.txt b/src/lib/bezier/CMakeLists.txt new file mode 100644 index 0000000000..44c8ff6183 --- /dev/null +++ b/src/lib/bezier/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(bezier + BezierQuad.cpp +)