Skip to content

Commit

Permalink
add fig
Browse files Browse the repository at this point in the history
  • Loading branch information
@ZbyLGsc
ZbyLGsc committed Apr 12, 2020
1 parent 68d79c2 commit c03bad8
Show file tree
Hide file tree
Showing 8 changed files with 60 additions and 624 deletions.
12 changes: 6 additions & 6 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,9 @@ This package is under active maintenance. New features will be listed here.

- The implementation of the ICRA2020 paper: Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths is integrated into this project. - April 12, 2020

- The online mapping algorithm is now available. It can take in depth image and camera pose pairs as input, do raycasting to update a probabilistic volumetric map, and build a Euclidean signed distance field (ESDF) for the planning system.
- The online mapping algorithm is now available. It can take in depth image and camera pose pairs as input, do raycasting to update a probabilistic volumetric map, and build a Euclidean signed distance field (ESDF) for the planning system. - Jan 30, 2020

- The heading (yaw angle) planner which enables smoother change of heading direction is available.
- The heading (yaw angle) planner which enables smoother change of heading direction is available. - Jan 11, 2020

## Overview

Expand All @@ -26,8 +26,8 @@ Demonstrations about the planner have been reported on the [IEEE Spectrum](https

- __B-spline trajectory optimization guided by topological paths__:
<p align="center">
<img src="https://github.com/HKUST-Aerial-Robotics/TopoTraj/blob/master/files/indoor.gif" width = "420" height = "237"/>
<img src="https://github.com/HKUST-Aerial-Robotics/TopoTraj/blob/master/files/outdoor.gif" width = "420" height = "237"/>
<img src="files/indoor.gif" width = "420" height = "237"/>
<img src="files/outdoor.gif" width = "420" height = "237"/>
</p>

__Authors__: [Boyu Zhou](http://boyuzhou.net), [Fei Gao](https://ustfei.com/) and [Shaojie Shen](http://uav.ust.hk/group/) from the [HUKST Aerial Robotics Group](http://uav.ust.hk/).
Expand Down Expand Up @@ -95,7 +95,7 @@ Run [Rviz](http://wiki.ros.org/rviz) with our configuration firstly:
```

Then run the quadrotor simulator and __Fast-Planner__.
Currently there are two different planning algorithms available, as list in the following:
Currently there are two different planning algorithms available, as listed in the following:

### 3.1 Kinodynamic Search & B-spline Optimization

Expand Down Expand Up @@ -140,7 +140,7 @@ Similarly , you will find the random map generated and can use the ```2D Nav Goa

<!-- add some gif here -->
<p align="center">
<img src="https://github.com/HKUST-Aerial-Robotics/TopoTraj/blob/master/files/sim.gif" width = "500" height = "360"/>
<img src="files/sim.gif" width = "500" height = "360"/>
</p>

If you use this algorithm for your application or research, please cite our related paper:
Expand Down
38 changes: 18 additions & 20 deletions fast_planner/bspline/include/bspline/non_uniform_bspline.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
using namespace std;

namespace fast_planner {

// An implementation of non-uniform B-spline with different dimensions
// It also represents uniform B-spline which is a special case of non-uniform
class NonUniformBspline {
Expand All @@ -19,17 +18,16 @@ class NonUniformBspline {
// e.g. B-spline with N points in 3D space -> Nx3 matrix
Eigen::MatrixXd control_points_;

int p_, n_, m_; // p degree, n+1 control points, m = n+p+1
Eigen::VectorXd u_; // knots vector
double interval_; // knot span \delta t
int p_, n_, m_; // p degree, n+1 control points, m = n+p+1
Eigen::VectorXd u_; // knots vector
double interval_; // knot span \delta t

Eigen::MatrixXd getDerivativeControlPoints();

double limit_vel_, limit_acc_, limit_ratio_; // physical limits and time adjustment ratio

public:
NonUniformBspline() {
}
NonUniformBspline() {}
NonUniformBspline(const Eigen::MatrixXd& points, const int& order, const double& interval);
~NonUniformBspline();

Expand All @@ -38,17 +36,17 @@ class NonUniformBspline {

// get / set basic bspline info

void setKnot(const Eigen::VectorXd& knot);
Eigen::VectorXd getKnot();
Eigen::MatrixXd getControlPoint();
double getInterval();
void getTimeSpan(double& um, double& um_p);
void setKnot(const Eigen::VectorXd& knot);
Eigen::VectorXd getKnot();
Eigen::MatrixXd getControlPoint();
double getInterval();
void getTimeSpan(double& um, double& um_p);
pair<Eigen::VectorXd, Eigen::VectorXd> getHeadTailPts();

// compute position / derivative

Eigen::VectorXd evaluateDeBoor(const double& u); // use u \in [up, u_mp]
Eigen::VectorXd evaluateDeBoorT(const double& t); // use t \in [0, duration]
Eigen::VectorXd evaluateDeBoor(const double& u); // use u \in [up, u_mp]
Eigen::VectorXd evaluateDeBoorT(const double& t); // use t \in [0, duration]
NonUniformBspline getDerivative();

// 3D B-spline interpolation of points in point_set, with boundary vel&acc
Expand All @@ -57,23 +55,23 @@ class NonUniformBspline {
// output: (K+6) control_pts
static void parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
const vector<Eigen::Vector3d>& start_end_derivative,
Eigen::MatrixXd& ctrl_pts);
Eigen::MatrixXd& ctrl_pts);

/* check feasibility, adjust time */

void setPhysicalLimits(const double& vel, const double& acc);
bool checkFeasibility(bool show = false);
void setPhysicalLimits(const double& vel, const double& acc);
bool checkFeasibility(bool show = false);
double checkRatio();
void lengthenTime(const double& ratio);
bool reallocateTime(bool show = false);
void lengthenTime(const double& ratio);
bool reallocateTime(bool show = false);

/* for performance evaluation */

double getTimeSum();
double getLength(const double& res = 0.01);
double getJerk();
void getMeanAndMaxVel(double& mean_v, double& max_v);
void getMeanAndMaxAcc(double& mean_a, double& max_a);
void getMeanAndMaxVel(double& mean_v, double& max_v);
void getMeanAndMaxAcc(double& mean_a, double& max_a);

void recomputeInit();

Expand Down
72 changes: 36 additions & 36 deletions fast_planner/bspline/src/non_uniform_bspline.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@ NonUniformBspline::~NonUniformBspline() {}
void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,
const double& interval) {
control_points_ = points;
p_ = order;
interval_ = interval;
p_ = order;
interval_ = interval;

n_ = points.rows() - 1;
m_ = n_ + p_ + 1;
Expand All @@ -37,7 +37,7 @@ void NonUniformBspline::setKnot(const Eigen::VectorXd& knot) { this->u_ = knot;
Eigen::VectorXd NonUniformBspline::getKnot() { return this->u_; }

void NonUniformBspline::getTimeSpan(double& um, double& um_p) {
um = u_(p_);
um = u_(p_);
um_p = u_(m_ - p_);
}

Expand Down Expand Up @@ -94,7 +94,7 @@ Eigen::MatrixXd NonUniformBspline::getDerivativeControlPoints() {
}

NonUniformBspline NonUniformBspline::getDerivative() {
Eigen::MatrixXd ctp = getDerivativeControlPoints();
Eigen::MatrixXd ctp = getDerivativeControlPoints();
NonUniformBspline derivative(ctp, p_ - 1, interval_);

/* cut the first and last knot */
Expand All @@ -108,17 +108,17 @@ NonUniformBspline NonUniformBspline::getDerivative() {
double NonUniformBspline::getInterval() { return interval_; }

void NonUniformBspline::setPhysicalLimits(const double& vel, const double& acc) {
limit_vel_ = vel;
limit_acc_ = acc;
limit_vel_ = vel;
limit_acc_ = acc;
limit_ratio_ = 1.1;
}

bool NonUniformBspline::checkFeasibility(bool show) {
bool fea = true;
// SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;

Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();

/* check vel feasibility and insert points */
double max_vel = -1.0;
Expand Down Expand Up @@ -164,8 +164,8 @@ bool NonUniformBspline::checkFeasibility(bool show) {
}

double NonUniformBspline::checkRatio() {
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();

// find max vel
double max_vel = -1.0;
Expand Down Expand Up @@ -197,8 +197,8 @@ bool NonUniformBspline::reallocateTime(bool show) {
// cout << "origin knots:\n" << u_.transpose() << endl;
bool fea = true;

Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();

double max_vel, max_acc;

Expand All @@ -222,8 +222,8 @@ bool NonUniformBspline::reallocateTime(bool show) {

double time_ori = u_(i + p_ + 1) - u_(i + 1);
double time_new = ratio * time_ori;
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_);
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_);

for (int j = i + 2; j <= i + p_ + 1; ++j) {
u_(j) += double(j - i - 1) * t_inc;
Expand Down Expand Up @@ -263,8 +263,8 @@ bool NonUniformBspline::reallocateTime(bool show) {

double time_ori = u_(i + p_ + 1) - u_(i + 2);
double time_new = ratio * time_ori;
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_ - 1);
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_ - 1);

if (i == 1 || i == 2) {
// cout << "acc i: " << i << endl;
Expand Down Expand Up @@ -300,7 +300,7 @@ void NonUniformBspline::lengthenTime(const double& ratio) {
int num2 = getKnot().rows() - 1 - 5;

double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1));
double t_inc = delta_t / double(num2 - num1);
double t_inc = delta_t / double(num2 - num1);
for (int i = num1 + 1; i <= num2; ++i) u_(i) += double(i - num1) * t_inc;
for (int i = num2 + 1; i < u_.rows(); ++i) u_(i) += delta_t;
}
Expand All @@ -309,7 +309,7 @@ void NonUniformBspline::recomputeInit() {}

void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
const vector<Eigen::Vector3d>& start_end_derivative,
Eigen::MatrixXd& ctrl_pts) {
Eigen::MatrixXd& ctrl_pts) {
if (ts <= 0) {
cout << "[B-spline]:time step error." << endl;
return;
Expand All @@ -336,10 +336,10 @@ void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eig

for (int i = 0; i < K; ++i) A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();

A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();

A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
// cout << "A:\n" << A << endl;

Expand Down Expand Up @@ -383,9 +383,9 @@ double NonUniformBspline::getTimeSum() {
}

double NonUniformBspline::getLength(const double& res) {
double length = 0.0;
double dur = getTimeSum();
Eigen::VectorXd p_l = evaluateDeBoorT(0.0), p_n;
double length = 0.0;
double dur = getTimeSum();
Eigen::VectorXd p_l = evaluateDeBoorT(0.0), p_n;
for (double t = res; t <= dur + 1e-4; t += res) {
p_n = evaluateDeBoorT(t);
length += (p_n - p_l).norm();
Expand All @@ -397,9 +397,9 @@ double NonUniformBspline::getLength(const double& res) {
double NonUniformBspline::getJerk() {
NonUniformBspline jerk_traj = getDerivative().getDerivative().getDerivative();

Eigen::VectorXd times = jerk_traj.getKnot();
Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint();
int dimension = ctrl_pts.cols();
Eigen::VectorXd times = jerk_traj.getKnot();
Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint();
int dimension = ctrl_pts.cols();

double jerk = 0.0;
for (int i = 0; i < ctrl_pts.rows(); ++i) {
Expand All @@ -413,14 +413,14 @@ double NonUniformBspline::getJerk() {

void NonUniformBspline::getMeanAndMaxVel(double& mean_v, double& max_v) {
NonUniformBspline vel = getDerivative();
double tm, tmp;
double tm, tmp;
vel.getTimeSpan(tm, tmp);

double max_vel = -1.0, mean_vel = 0.0;
int num = 0;
int num = 0;
for (double t = tm; t <= tmp; t += 0.01) {
Eigen::VectorXd vxd = vel.evaluateDeBoor(t);
double vn = vxd.norm();
double vn = vxd.norm();

mean_vel += vn;
++num;
Expand All @@ -430,20 +430,20 @@ void NonUniformBspline::getMeanAndMaxVel(double& mean_v, double& max_v) {
}

mean_vel = mean_vel / double(num);
mean_v = mean_vel;
max_v = max_vel;
mean_v = mean_vel;
max_v = max_vel;
}

void NonUniformBspline::getMeanAndMaxAcc(double& mean_a, double& max_a) {
NonUniformBspline acc = getDerivative().getDerivative();
double tm, tmp;
double tm, tmp;
acc.getTimeSpan(tm, tmp);

double max_acc = -1.0, mean_acc = 0.0;
int num = 0;
int num = 0;
for (double t = tm; t <= tmp; t += 0.01) {
Eigen::VectorXd axd = acc.evaluateDeBoor(t);
double an = axd.norm();
double an = axd.norm();

mean_acc += an;
++num;
Expand All @@ -453,7 +453,7 @@ void NonUniformBspline::getMeanAndMaxAcc(double& mean_a, double& max_a) {
}

mean_acc = mean_acc / double(num);
mean_a = mean_acc;
max_a = max_acc;
mean_a = mean_acc;
max_a = max_acc;
}
} // namespace fast_planner
Loading

0 comments on commit c03bad8

Please sign in to comment.