add Bezier library

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)

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 <dennis.mannhart@gmail.com>
+ *
+ */
+
+#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<typename Tp>
+void BezierQuad<Tp>::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<typename Tp>
+void BezierQuad<Tp>::getBezier(Data &pt0, Data &ctrl, Data &pt1)
+{
+	pt0 = _pt0;
+	ctrl = _ctrl;
+	pt1 = _pt1;
+}
+
+template<typename Tp>
+matrix::Vector<Tp, 3> BezierQuad<Tp>::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<typename Tp>
+matrix::Vector<Tp, 3> BezierQuad<Tp>::getVelocity(const Tp t)
+{
+	return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration));
+}
+
+template<typename Tp>
+matrix::Vector<Tp, 3> BezierQuad<Tp>::getAcceleration()
+{
+	return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration));
+}
+
+template<typename Tp>
+void BezierQuad<Tp>::getStates(Data &point, Data &vel, Data &acc, const Tp time)
+{
+	point = getPoint(time);
+	vel = getVelocity(time);
+	acc = getAcceleration();
+}
+
+template<typename Tp>
+void BezierQuad<Tp>::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<typename Tp>
+void BezierQuad<Tp>::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<typename Tp>
+Tp BezierQuad<Tp>::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<typename Tp>
+Tp BezierQuad<Tp>::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<typename Tp>
+Tp BezierQuad<Tp>::_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<typename Tp>
+Tp BezierQuad<Tp>::_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);
+}
+}

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 <px4_defines.h>
+#include <stdlib.h>
+
+#include <matrix/math.hpp>
+
+namespace bezier
+{
+template<typename Tp>
+class __EXPORT BezierQuad
+{
+public:
+
+	using Data = matrix::Vector<Tp, 3>;
+
+	/**
+	 * 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<float>;
+using BezierQuadd = BezierQuad<double>;
+}
+
+// include implementation
+#include "BezierQuad.cpp"

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
+)