47 const Eigen::Vector3f& pos,
48 const Eigen::Vector3f& vel,
49 const Eigen::Vector3f& goal,
51 const Eigen::Vector3f& vsamples,
52 std::vector<Eigen::Vector3f> additional_samples,
53 bool discretize_by_time) {
54 initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
61 const Eigen::Vector3f& pos,
62 const Eigen::Vector3f& vel,
63 const Eigen::Vector3f& goal,
65 const Eigen::Vector3f& vsamples,
66 bool discretize_by_time) {
71 double min_vel_th = -1.0 * max_vel_th;
86 if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
88 Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
89 Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
94 double dist =
hypot(goal[0] - pos[0], goal[1] - pos[1]);
95 max_vel_x = std::max(std::min(max_vel_x, dist /
sim_time_), min_vel_x);
96 max_vel_y = std::max(std::min(max_vel_y, dist /
sim_time_), min_vel_y);
99 max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] *
sim_time_);
100 max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
101 max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
103 min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
104 min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
105 min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
108 max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] *
sim_period_);
109 max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
110 max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
112 min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
113 min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
114 min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
117 Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
139 double sim_granularity,
140 double angular_sim_granularity,
183 Eigen::Vector3f sample_target_vel,
185 double vmag =
hypot(sample_target_vel[0], sample_target_vel[1]);
194 (
limits_->
min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
207 double sim_time_distance = vmag *
sim_time_;
208 double sim_time_angle = fabs(sample_target_vel[2]) *
sim_time_;
218 Eigen::Vector3f loop_vel;
222 traj.
xv_ = loop_vel[0];
223 traj.
yv_ = loop_vel[1];
227 loop_vel = sample_target_vel;
228 traj.
xv_ = sample_target_vel[0];
229 traj.
yv_ = sample_target_vel[1];
230 traj.
thetav_ = sample_target_vel[2];
234 for (
int i = 0; i < num_steps; ++i) {
237 traj.
addPoint(pos[0], pos[1], pos[2]);
250 return num_steps > 0;
254 const Eigen::Vector3f& vel,
double dt) {
255 Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
256 new_pos[0] = pos[0] + (vel[0] *
cos(pos[2]) + vel[1] *
cos(M_PI_2 + pos[2])) * dt;
257 new_pos[1] = pos[1] + (vel[0] *
sin(pos[2]) + vel[1] *
sin(M_PI_2 + pos[2])) * dt;
258 new_pos[2] = pos[2] + vel[2] * dt;
266 const Eigen::Vector3f& vel, Eigen::Vector3f acclimits,
double dt) {
267 Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
268 for (
int i = 0; i < 3; ++i) {
269 if (vel[i] < sample_target_vel[i]) {
270 new_vel[i] = std::min(
double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
272 new_vel[i] = std::max(
double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
void initialise(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, const Eigen::Vector3f &goal, base_local_planner::LocalPlannerLimits *limits, const Eigen::Vector3f &vsamples, std::vector< Eigen::Vector3f > additional_samples, bool discretize_by_time=false)
Eigen::Vector3f getAccLimits()
Get the acceleration limits of the robot.
bool generateTrajectory(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f sample_target_vel, base_local_planner::Trajectory &traj)
bool hasMoreTrajectories()
static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f &sample_target_vel, const Eigen::Vector3f &vel, Eigen::Vector3f acclimits, double dt)
double time_delta_
The time gap between points.
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
double angular_sim_granularity_
double cost_
The cost/score of the trajectory.
bool nextTrajectory(Trajectory &traj)
void resetPoints()
Clear the trajectory's points.
double thetav_
The x, y, and theta velocities of the trajectory.
static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
std::vector< Eigen::Vector3f > sample_params_
base_local_planner::LocalPlannerLimits * limits_
void setParameters(double sim_time, double sim_granularity, double angular_sim_granularity, bool use_dwa=false, double sim_period=0.0)
void addPoint(double x, double y, double th)
Add a point to the end of a trajectory.
unsigned int next_sample_index_
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
bool continued_acceleration_
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Holds a trajectory generated by considering an x, y, and theta velocity.