Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Const and preallocation based optimizations and cleanup #6

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Const and preallocation based optimizations and cleanup #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e854057
fixed rmader.yaml commented parameters
SJamieson Mar 29, 2023
3a2d085
Rmader: made check and utility member functions const
SJamieson Mar 29, 2023
7d1b7b8
rmader: const loop iterators and more const member functions
SJamieson Mar 29, 2023
7714bb9
rmader: replaced push_back Minkowski sums with emplace_back
SJamieson Mar 29, 2023
fd1c9d0
rmader: pre-allocated space for convex hulls of curves, convex hulls …
SJamieson Mar 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions rmader/.gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
cmake-*/
.idea/
*.so
*.o
*.lp
Expand Down
36 changes: 18 additions & 18 deletions rmader/include/rmader.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ using namespace termcolor;
class Rmader
{
public:
Rmader(mt::parameters par);
Rmader(mt::parameters const& par);
bool replan(mt::Edges& edges_obstacles_out, std::vector<mt::state>& X_safe_out, std::vector<Hyperplane3D>& planes,
int& num_of_LPs_run, int& num_of_QCQPs_run, mt::PieceWisePol& pwp_out);
bool replan_with_delaycheck(mt::Edges& edges_obstacles_out, std::vector<mt::state>& headsup_plan,
Expand All @@ -64,17 +64,17 @@ class Rmader
void setTerminalGoal(mt::state& term_goal);
void resetInitialization();

bool IsTranslating();
void updateTrajObstacles_with_delaycheck(mt::dynTraj traj);
void updateTrajObstacles(mt::dynTraj traj);
bool IsTranslating() const;
void updateTrajObstacles_with_delaycheck(mt::dynTraj const& traj);
void updateTrajObstacles(mt::dynTraj const& traj);

Eigen::Vector2d RotationMatrix(Eigen::Vector2d& vec, const double& angle);
void getID(int& id);
mt::state moveAoutOfBbox(const mt::state& A);
int getMissedMsgsCnt();
mt::state getGterm();
bool isGoalSeen();
bool isGoalReached();
bool isGoalSeen() const;
bool isGoalReached() const;
bool initializedAllExceptPlanner();

std::vector<mt::dynTrajCompiled> getTrajs();
Expand All @@ -92,29 +92,29 @@ class Rmader

bool initializedStateAndTermGoal();

bool safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& is_q0_fail);
bool safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized);
bool safetyCheckAfterOptForRmader(mt::PieceWisePol pwp_optimized, bool& is_q0_fail) const;
bool safetyCheckAfterOpt(mt::PieceWisePol pwp_optimized) const;

bool safetyCheck_for_A_star_failure(mt::PieceWisePol pwp_prev);
bool safetyCheck_for_A_star_failure_pwp_now(mt::PieceWisePol pwp_now);

bool trajsAndPwpAreInCollision_with_inflation(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized,
double t_start, double t_end, bool& is_q0_fail);
double t_start, double t_end, bool& is_q0_fail) const;

bool trajsAndPwpAreInCollision(mt::dynTrajCompiled traj, mt::PieceWisePol pwp_optimized, double t_start,
double t_end);
double t_end) const;

void removeTrajsThatWillNotAffectMe(const mt::state& A, double t_start, double t_end);

/* vec_E<Polyhedron<3>> cu::vectorGCALPol2vectorJPSPol(ConvexHullsOfCurves& convex_hulls_of_curves);
mt::ConvexHullsOfCurves_Std cu::vectorGCALPol2vectorStdEigen(ConvexHullsOfCurves& convexHulls);*/
ConvexHullsOfCurves convexHullsOfCurves(double t_start, double t_end);
ConvexHullsOfCurve convexHullsOfCurve(mt::dynTrajCompiled& traj, double t_start, double t_end);
CGAL_Polyhedron_3 convexHullOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end);
ConvexHullsOfCurve convexHullsOfCurve(mt::dynTrajCompiled const& traj, double t_start, double t_end);
CGAL_Polyhedron_3 convexHullOfInterval(mt::dynTrajCompiled const& traj, double t_start, double t_end);

std::vector<Eigen::Vector3d> vertexesOfInterval(mt::PieceWisePol& pwp, double t_start, double t_end,
const Eigen::Vector3d& delta_inflation);
std::vector<Eigen::Vector3d> vertexesOfInterval(mt::dynTrajCompiled& traj, double t_start, double t_end);
std::vector<Eigen::Vector3d> vertexesOfInterval(mt::PieceWisePol const& pwp, double t_start, double t_end,
const Eigen::Vector3d& delta_inflation) const;
std::vector<Eigen::Vector3d> vertexesOfInterval(mt::dynTrajCompiled const& traj, double t_start, double t_end) const;
void yaw(double diff, mt::state& next_goal);

void changeBBox(Eigen::Vector3d& drone_boundarybox);
Expand All @@ -134,7 +134,7 @@ class Rmader

mt::parameters par_;

double t_; // variable where the expressions of the trajs of the dyn obs are evaluated
mutable double t_; // variable where the expressions of the trajs of the dyn obs are evaluated

std::mutex mtx_trajs_;
std::vector<mt::dynTrajCompiled> trajs_;
Expand Down Expand Up @@ -168,7 +168,7 @@ class Rmader

std::mutex mtx_G;
std::mutex mtx_G_term;
std::mutex mtx_t_;
mutable std::mutex mtx_t_;

std::mutex mtx_hrtwch_;

Expand All @@ -190,7 +190,7 @@ class Rmader

bool exists_previous_pwp_ = false;

bool started_check_ = false;
mutable bool started_check_ = false;

bool have_received_trajectories_while_checking_ = false;

Expand Down
6 changes: 3 additions & 3 deletions rmader/param/rmader.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,8 +77,8 @@ setting:
tuning_param:
drone_bbox: [0.25, 0.25, 0.25] #(m) Used for collision checking replacing drone_radius. The bounding box dimension [x y z]
Ra: 30.0 # [m] Radius of my planning sphere (planning horizon). Should be < fov_depth
############# delay_check_sec: 0.05 # seconds
############### simulated_comm_delay_sec: 0.05 #seconds
delay_check_sec: 0.05 # seconds
simulated_comm_delay_sec: 0.05 #seconds
comm_delay_param: 1.0
v_max: [10.0, 10.0, 10.0] #[m/s] 7.0
a_max: [20.0, 20.0, 20.0] #[m/s2] Note that if a_max.z() > 9.81, the drone may flip
Expand Down Expand Up @@ -177,4 +177,4 @@ nlopt:
#Other possible solvers:
# LD_MMA, LD_CCSAQ, LN_NELDERMEAD, LD_LBFGS, LD_VAR2, LD_AUGLAG, LD_SLSQP, LN_SBPLX, LN_PRAXIS, ...
# LD_AUGLAG_EQ, LN_BOBYQA, LN_NEWUOA, LN_NEWUOA_BOUND, LN_COBYLA, LD_CCSAQ, LD_TNEWTON_PRECOND_RESTART, ...
# LD_TNEWTON_RESTART, LD_TNEWTON_PRECOND, LD_VAR1, LD_LBFGS_NOCEDAL, COBYLA
# LD_TNEWTON_RESTART, LD_TNEWTON_PRECOND, LD_VAR1, LD_LBFGS_NOCEDAL, COBYLA
Loading