fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 02:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

BezierQuad: update description, replace Data with Vector3_t

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-25 08:50:10 +02:00
parent 6ddf214bbd
commit 7559610a7b
3 changed files with 111 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/bezier/BezierQuad.cpp
+26 -36
View File
@@ -35,7 +35,6 @@
* Bezier function
*
* @author Dennis Mannhart <dennis.mannhart@gmail.com>
*
*/
#include "BezierQuad.hpp"
@@ -47,7 +46,7 @@ namespace bezier
#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,
void BezierQuad<Tp>::setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1,
Tp duration)
{
_pt0 = pt0;
@@ -55,11 +54,10 @@ void BezierQuad<Tp>::setBezier(const Data &pt0, const Data &ctrl, const Data &pt
_pt1 = pt1;
_duration = duration;
_cached_resolution = (Tp)(-1);
}
template<typename Tp>
void BezierQuad<Tp>::getBezier(Data &pt0, Data &ctrl, Data &pt1)
void BezierQuad<Tp>::getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1)
{
pt0 = _pt0;
ctrl = _ctrl;
@@ -87,7 +85,7 @@ matrix::Vector<Tp, 3> BezierQuad<Tp>::getAcceleration()
}
template<typename Tp>
void BezierQuad<Tp>::getStates(Data &point, Data &vel, Data &acc, const Tp time)
void BezierQuad<Tp>::getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp time)
{
point = getPoint(time);
vel = getVelocity(time);
@@ -95,22 +93,22 @@ void BezierQuad<Tp>::getStates(Data &point, Data &vel, Data &acc, const Tp time)
}
template<typename Tp>
void BezierQuad<Tp>::getStatesClosest(Data &point, Data &vel, Data &acc,
const Data pose)
void BezierQuad<Tp>::getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc,
const Vector3_t pose)
{
/* get t that corresponds to point closest on bezier point */
// get t that corresponds to point closest on bezier point
Tp t = _goldenSectionSearch(pose);
/* get states corresponding to t */
// 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,
void BezierQuad<Tp>::setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1,
const Tp duration)
{
/* update bezier points */
// update bezier points
_ctrl = ctrl;
_duration = duration;
_pt0 = _ctrl - vel0 * _duration / (Tp)2;
@@ -129,8 +127,6 @@ Tp BezierQuad<Tp>::getArcLength(const Tp resolution)
// 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) {
@@ -139,9 +135,10 @@ Tp BezierQuad<Tp>::getArcLength(const Tp resolution)
// step size
Tp h = (_duration) / n;
// get integration
Data y;
_cached_arc_length = (Tp)0;
Vector3_t y;
for (int i = 1; i < n; i++) {
@@ -155,10 +152,9 @@ Tp BezierQuad<Tp>::getArcLength(const Tp resolution)
}
}
v0 = getVelocity((Tp)0);
vn = getVelocity(_duration);
y0 = v0.length();
yn = vn.length();
// velocity length at start and end points
Tp y0 = getVelocity((Tp)0).length();
Tp yn = getVelocity(_duration).length();
// 1/3 simpsons rule
_cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length);
@@ -170,27 +166,22 @@ Tp BezierQuad<Tp>::getArcLength(const Tp resolution)
}
template<typename Tp>
Tp BezierQuad<Tp>::getDistToClosestPoint(const Data &pose)
Tp BezierQuad<Tp>::getDistToClosestPoint(const Vector3_t &pose)
{
/* get t that corresponds to point closest on bezier point */
// get t that corresponds to point closest on bezier point
Tp t = _goldenSectionSearch(pose);
/* get closest point */
Data point = getPoint(t);
// get closest point
Vector3_t point = getPoint(t);
return (pose - point).length();
}
/*
* HELPER FUNCTIONS (private)
*/
template<typename Tp>
Tp BezierQuad<Tp>::_goldenSectionSearch(const Data &pose)
Tp BezierQuad<Tp>::_goldenSectionSearch(const Vector3_t &pose)
{
Tp a, b, c, d;
a = (Tp)0; //represents most left point
b = _duration * (Tp)1; //represents most right point
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;
@@ -205,22 +196,21 @@ Tp BezierQuad<Tp>::_goldenSectionSearch(const Data &pose)
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)
Tp BezierQuad<Tp>::_getDistanceSquared(const Tp t, const Vector3_t &pose)
{
/* get point on bezier */
Data vec = getPoint(t);
// get point on bezier
Vector3_t vec = getPoint(t);
/* get vector from point to pose */
// get vector from point to pose
vec = vec - pose;
/* norm squared */
// norm squared
return (vec * vec);
}
}
src/lib/bezier/BezierQuad.hpp
+82 -54
View File
@@ -35,147 +35,175 @@
* @file BerzierQuad.hpp
*
* Quadratic bezier lib
*
* A quadratic bezier function/spline is completely defined by three 3D points in space and a time scaling factor.
* pt0 and pt1 define the start and end points of the spline. ctrl point is a point in space that effects the curvature
* of the spline. The time scaling factor (= duration) defines the time it takes to travel along the spline from pt0 to
* pt1.
* A bezier spline is a continuous function from which position, velocity and acceleration can be extracted. For a given spline,
* acceleration stays constant.
*/
#pragma once
#include <px4_defines.h>
#include <stdlib.h>
#include <matrix/math.hpp>
namespace bezier
{
template<typename Tp>
class __EXPORT BezierQuad
class BezierQuad
{
public:
using Data = matrix::Vector<Tp, 3>;
using Vector3_t = matrix::Vector<Tp, 3>;
/**
* empty constructor
* Empty constructor
*/
BezierQuad(void) :
_pt0(Data()), _ctrl(Data()), _pt1(Data()), _duration(1.0f) {}
_pt0(Vector3_t()), _ctrl(Vector3_t()), _pt1(Vector3_t()), _duration(1.0f) {}
/**
* constructor from array
* 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) {}
_pt0(Vector3_t(pt0)), _ctrl(Vector3_t(ctrl)), _pt1(Vector3_t(pt1)), _duration(duration) {}
/**
* constructor from vector
* Constructor from vector
*/
BezierQuad(const Data &pt0, const Data &ctrl, const Data &pt1,
BezierQuad(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1,
Tp duration = 1.0f):
_pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {}
/*
* return bezier points
* Get bezier points
*/
void getBezier(Data &pt0, Data &ctrl, Data &pt1);
void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1);
/*
* get pt0
* Return pt0
*/
Data getPt0() {return _pt0;}
Vector3_t getPt0() {return _pt0;}
/*
* get ctrl
* Return ctrl
*/
Data getCtrl() {return _ctrl;}
Vector3_t getCtrl() {return _ctrl;}
/*
* get pt1
* Return pt1
*/
Data getPt1() {return _pt1;}
Vector3_t getPt1() {return _pt1;}
/**
* set new bezier points
* Set new bezier points and duration
*/
void setBezier(const Data &pt0, const Data &ctrl, const Data &pt1,
void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1,
Tp duration = 1.0f);
/*
* set duration
*/
* Set duration
*
* @param time is the total time it takes to travel along the bezier spline.
*/
void setDuration(const Tp time) {_duration = time;}
/**
* get point on bezier point corresponding to t
* Return point on bezier point corresponding to time t
*
* @param t is a time in seconds in between [0, duration]
* @return a point on bezier
*/
Data getPoint(const Tp t);
Vector3_t getPoint(const Tp t);
/*
* Distance to closest point given a position
*
* @param pose is a position in 3D space from which distance to bezier is computed.
* @return distance to closest point on bezier
*/
Tp getDistToClosestPoint(const Data &pose);
Tp getDistToClosestPoint(const Vector3_t &pose);
/*
* get velocity on bezier corresponding to t
* Return velocity on bezier corresponding to time t
*
* @param t is a time in seconds in between [0, duration]
* @return velocity vector at time t
*/
Data getVelocity(const Tp t);
Vector3_t getVelocity(const Tp t);
/*
* get acceleration on bezier corresponding to t
* Return acceleration on bezier corresponding to time t
*
* @return constant acceleration of bezier
*/
Data getAcceleration();
Vector3_t getAcceleration();
/*
* get states on bezier corresponding to t
* Get all states on bezier corresponding to time t
*/
void getStates(Data &point, Data &vel, Data &acc, const Tp t);
void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t);
/*
* get states on bezier which are closest to pose
* Get states on bezier which are closest to pose in space
*
* @param point is a posiiton on the spline that is closest to a given pose
* @param vel is the velocity at that given point
* @param acc is the acceleration for that spline
* @param pose represent a position in space from which closest point is computed
*/
void getStatesClosest(Data &point, Data &vel, Data &acc,
const Data pose);
void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc,
const Vector3_t pose);
/*
* compute bezier from velocity at bezier end points and ctrl point
* Compute bezier from velocity at bezier end points and ctrl point
*
* The bezier end points are fully defined by a given control point ctrl, the duration and
* the desired velocity vectors at the end points.
*/
void setBezFromVel(const Data &ctrl, const Data &vel0, const Data &vel1,
void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1,
const Tp duration = 1.0f);
/*
* simpsons inegrattion applied to velocity
* Return the arc length of a bezier spline
*
* The arc length is computed with simpsons integration.
* @param resolution in meters.
*/
Tp getArcLength(const Tp resolution);
private:
/* control points */
Data _pt0;
Data _ctrl;
Data _pt1;
Tp _duration;
Vector3_t _pt0; /**< Bezier starting point */
Vector3_t _ctrl; /**< Bezier control point */
Vector3_t _pt1; /**< bezier end point */
Tp _duration = (Tp)1; /**< Total time to travle along spline */
/* cache */
Tp _cached_arc_length;
Tp _cached_resolution = (Tp)(-1); // negative number means that cache is not up to date
Tp _cached_arc_length = (Tp)0; /**< The saved arc length of the spline */
Tp _cached_resolution = (Tp)(
-1); /**< The resolution used to compute the arc length. Negative number means that cache is not up to date. */
/*
* Helper functions
* Golden section search
*/
/* golden section search */
Tp _goldenSectionSearch(const Data &pose);
Tp _goldenSectionSearch(const Vector3_t &pose);
/*
* get distance to point on bezier
* Get squared distance from 3D pose in space and a point on bezier.
*
* @param t is the time in between [0, duration] that defines a point on the bezier.
* @param pose is a 3D pose in space.
*/
Tp _getDistanceSquared(const Tp t, const Data &pose);
Tp _getDistanceSquared(const Tp t, const Vector3_t &pose);
};
using BezierQuadf = BezierQuad<float>;
using BezierQuadd = BezierQuad<double>;
using BezierQuad_f = BezierQuad<float>;
using BezierQuad_d = BezierQuad<double>;
}
// include implementation
src/systemcmds/tests/test_bezierQuad.cpp
+3 -3
View File
@@ -38,7 +38,7 @@ bool BezierQuadTest::_get_states_from_time()
matrix::Vector3f pt1(0.5f, 0.0f, 0.0f);
// create bezier with default t = [0,1]
bezier::BezierQuadf bz(pt0, ctrl, pt1);
bezier::BezierQuad_f bz(pt0, ctrl, pt1);
matrix::Vector3f pos, vel, acc;
float precision = 0.00001;
@@ -165,7 +165,7 @@ bool BezierQuadTest::_get_arc_length()
duration = random(0.0f, T);
// create bezier
bezier::BezierQuadf bz(pt0, ctrl, pt1, duration);
bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration);
// compute arc length, triangle length and straigh length
arc_length = bz.getArcLength(resolution);
@@ -211,7 +211,7 @@ bool BezierQuadTest::_set_bez_from_vel()
matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max));
float duration = random(0.0f, 100.0f);
bezier::BezierQuadf bz;;
bezier::BezierQuad_f bz;;
bz.setBezFromVel(ctrl, vel0, vel1, duration);
// get velocity back