/**
* @param pos current position of robot
* @param vel desired velocity for sampling
*/
bool SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj) {
double vmag = ::hypot(sample_target_vel[0], sample_target_vel[1]);
double eps = 1e-4;
traj.cost_ = -1.0; // placed here in case we return early
//trajectory might be reused so we'll make sure to reset it
traj.resetPoints();
// make sure that the robot would at least be moving with one of
// the required minimum velocities for translation and rotation (if set)
if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
(limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
return false;
}
// make sure we do not exceed max diagonal (x+y) translational velocity (if set)
if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {
return false;
}
int num_steps;
if (discretize_by_time_) {
num_steps = ceil(sim_time_ / sim_granularity_);
} else {
//compute the number of steps we must take along this trajectory to be "safe"
double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
num_steps =
ceil(std::max(sim_time_distance / sim_granularity_,
sim_time_angle / angular_sim_granularity_));
}
//compute a timestep
double dt = sim_time_ / num_steps;
traj.time_delta_ = dt;
Eigen::Vector3f loop_vel;
if (continued_acceleration_) {
// assuming the velocity of the first cycle is the one we want to store in the trajectory object
loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
traj.xv_ = loop_vel[0];
traj.yv_ = loop_vel[1];
traj.thetav_ = loop_vel[2];
} else {
// assuming sample_vel is our target velocity within acc limits for one timestep
loop_vel = sample_target_vel;
traj.xv_ = sample_target_vel[0];
traj.yv_ = sample_target_vel[1];
traj.thetav_ = sample_target_vel[2];
}
//simulate the trajectory and check for collisions, updating costs along the way
for (int i = 0; i < num_steps; ++i) {
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);
if (continued_acceleration_) {
//calculate velocities
loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
//ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
}
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, loop_vel, dt);
} // end for simulation steps
return num_steps > 0; // true if trajectory has at least one point
}
void DWAPlanner::generateTrajectory(Eigen::Vector3f pos, const Eigen::Vector3f& vel, base_local_planner::Trajectory& traj, bool two_point_scoring, double sq_dist){
//ROS_ERROR("%.2f, %.2f, %.2f - %.2f %.2f", vel[0], vel[1], vel[2], sim_time_, sim_granularity_);
double impossible_cost = map_.map_.size();
double vmag = sqrt(vel[0] * vel[0] + vel[1] * vel[1]);
double eps = 1e-4;
//make sure that the robot is at least moving with one of the required velocities
if((vmag + eps < min_vel_trans_ && fabs(vel[2]) + eps < min_rot_vel_) ||
vmag - eps > max_vel_trans_ ||
oscillationCheck(vel)){
traj.cost_ = -1.0;
return;
}
//compute the number of steps we must take along this trajectory to be "safe"
int num_steps = ceil(std::max((vmag * sim_time_) / sim_granularity_, fabs(vel[2]) / sim_granularity_));
//compute a timestep
double dt = sim_time_ / num_steps;
double time = 0.0;
//initialize the costs for the trajectory
double path_dist = 0.0;
double goal_dist = 0.0;
double occ_cost = 0.0;
//we'll also be scoring a point infront of the robot
double front_path_dist = 0.0;
double front_goal_dist = 0.0;
//create a potential trajectory... it might be reused so we'll make sure to reset it
traj.resetPoints();
traj.xv_ = vel[0];
traj.yv_ = vel[1];
traj.thetav_ = vel[2];
traj.cost_ = -1.0;
//if we're not actualy going to simulate... we may as well just return now
if(num_steps == 0){
traj.cost_ = -1.0;
return;
}
//we want to check against the absolute value of the velocities for collisions later
//Eigen::Vector3f abs_vel = vel.array().abs();
//simulate the trajectory and check for collisions, updating costs along the way
for(int i = 0; i < num_steps; ++i){
//get the mapp coordinates of a point
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if(!costmap_.worldToMap(pos[0], pos[1], cell_x, cell_y)){
//we're off the map
traj.cost_ = -1.0;
return;
}
double front_x = pos[0] + forward_point_distance_ * cos(pos[2]);
double front_y = pos[1] + forward_point_distance_ * sin(pos[2]);
unsigned int front_cell_x, front_cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if(!costmap_.worldToMap(front_x, front_y, front_cell_x, front_cell_y)){
//we're off the map
traj.cost_ = -1.0;
return;
}
//if we're over a certain speed threshold, we'll scale the robot's
//footprint to make it either slow down or stay further from walls
double scale = 1.0;
if(vmag > scaling_speed_){
//scale up to the max scaling factor linearly... this could be changed later
double ratio = (vmag - scaling_speed_) / (max_vel_trans_ - scaling_speed_);
scale = max_scaling_factor_ * ratio + 1.0;
}
//we want to find the cost of the footprint
double footprint_cost = footprintCost(pos, scale);
//if the footprint hits an obstacle... we'll check if we can stop before we hit it... given the time to get there
if(footprint_cost < 0){
traj.cost_ = -1.0;
return;
/* TODO: I'm not convinced this code is working properly
//we want to compute the max allowable speeds to be able to stop
//to be safe... we'll make sure we can stop some time before we actually hit
Eigen::Vector3f max_vel = getMaxSpeedToStopInTime(time - stop_time_buffer_);
//check if we can stop in time
if(abs_vel[0] < max_vel[0] && abs_vel[1] < max_vel[1] && abs_vel[2] < max_vel[2]){
//if we can, then we'll just break out of the loop here.. no point in checking future points
break;
}
else{
//if we can't... then this trajectory is invalid
traj.cost_ = -1.0;
return;
}
*/
}
//compute the costs for this point on the trajectory
occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
path_dist = map_(cell_x, cell_y).path_dist;
goal_dist = map_(cell_x, cell_y).goal_dist;
front_path_dist = front_map_(front_cell_x, front_cell_y).path_dist;
front_goal_dist = front_map_(front_cell_x, front_cell_y).goal_dist;
//if a point on this trajectory has no clear path to the goal... it is invalid
if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
traj.cost_ = -2.0; //-2.0 means that we were blocked because propagation failed
return;
}
//add the point to the trajectory so we can draw it later if we want
traj.addPoint(pos[0], pos[1], pos[2]);
//update the position of the robot using the velocities passed in
pos = computeNewPositions(pos, vel, dt);
time += dt;
}
double goal_heading = tf::getYaw(global_plan_.back().pose.orientation);
double heading_diff = fabs(angles::shortest_angular_distance(pos[2],goal_heading));
//ROS_ERROR("%.2f, %.2f", goal_heading, heading_diff);
double resolution = costmap_.getResolution();
//double sq_dist = squareDist(robot_pose, global_plan_.back());
double heading_scale_ = 0;
if (sq_dist < squared_dist_for_rotating_) {
heading_scale_ = heading_bias_;
}
//if we're not at the last point in the plan, then we can just score
if(two_point_scoring)
traj.cost_ = pdist_scale_ * resolution * ((front_path_dist + path_dist) / 2.0) + gdist_scale_ * resolution * ((front_goal_dist + goal_dist) / 2.0) + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
else
traj.cost_ = pdist_scale_ * resolution * path_dist + gdist_scale_ * resolution * goal_dist + occdist_scale_ * occ_cost + heading_scale_ * heading_diff;
//ROS_ERROR("%.2f, %.2f, %.2f, %.2f", vel[0], vel[1], vel[2], traj.cost_);
}
/**
* Runs of the SMPSO algorithm.
* @return a <code>SolutionSet</code> that is a set of non dominated solutions
* as a result of the algorithm execution
*/
SolutionSet * PSO::execute() {
initParams();
success_ = false;
globalBest_ = NULL;
//->Step 1 (and 3) Create the initial population and evaluate
for (int i = 0; i < particlesSize_; i++) {
Solution * particle = new Solution(problem_);
problem_->evaluate(particle);
evaluations_ ++;
particles_->add(particle);
if ((globalBest_ == NULL) || (particle->getObjective(0) < globalBest_->getObjective(0))) {
if (globalBest_!= NULL) {
delete globalBest_;
}
globalBest_ = new Solution(particle);
}
}
//-> Step2. Initialize the speed_ of each particle to 0
for (int i = 0; i < particlesSize_; i++) {
speed_[i] = new double[problem_->getNumberOfVariables()];
for (int j = 0; j < problem_->getNumberOfVariables(); j++) {
speed_[i][j] = 0.0;
}
}
//-> Step 6. Initialize the memory of each particle
for (int i = 0; i < particles_->size(); i++) {
Solution * particle = new Solution(particles_->get(i));
localBest_[i] = particle;
}
//-> Step 7. Iterations ..
while (iteration_ < maxIterations_) {
int * bestIndividualPtr = (int*)findBestSolution_->execute(particles_);
int bestIndividual = *bestIndividualPtr;
delete bestIndividualPtr;
computeSpeed(iteration_, maxIterations_);
//Compute the new positions for the particles_
computeNewPositions();
//Mutate the particles_
//mopsoMutation(iteration_, maxIterations_);
//Evaluate the new particles_ in new positions
for (int i = 0; i < particles_->size(); i++) {
Solution * particle = particles_->get(i);
problem_->evaluate(particle);
evaluations_ ++;
}
//Actualize the memory of this particle
for (int i = 0; i < particles_->size(); i++) {
//int flag = comparator_.compare(particles_.get(i), localBest_[i]);
//if (flag < 0) { // the new particle is best_ than the older remember
if ((particles_->get(i)->getObjective(0) < localBest_[i]->getObjective(0))) {
Solution * particle = new Solution(particles_->get(i));
delete localBest_[i];
localBest_[i] = particle;
} // if
if ((particles_->get(i)->getObjective(0) < globalBest_->getObjective(0))) {
Solution * particle = new Solution(particles_->get(i));
delete globalBest_;
globalBest_ = particle;
} // if
}
iteration_++;
}
// Return a population with the best individual
SolutionSet * resultPopulation = new SolutionSet(1);
int * bestIndexPtr = (int *)findBestSolution_->execute(particles_);
int bestIndex = *bestIndexPtr;
delete bestIndexPtr;
cout << "Best index = " << bestIndex << endl;
Solution * s = particles_->get(bestIndex);
resultPopulation->add(new Solution(s));
// Free memory
deleteParams();
return resultPopulation;
} // execute
/**
* Runs of the SMPSO algorithm.
* @return a <code>SolutionSet</code> that is a set of non dominated solutions
* as a result of the algorithm execution
*/
SolutionSet *SMPSOhv::execute() {
initParams();
success = false;
//->Step 1 (and 3) Create the initial population and evaluate
for (int i = 0; i < swarmSize; i++){
Solution *particle = new Solution(problem_);
problem_->evaluate(particle);
problem_->evaluateConstraints(particle);
particles->add(particle);
}
//-> Step2. Initialize the speed of each particle to 0
for (int i = 0; i < swarmSize; i++) {
speed[i] = new double[problem_->getNumberOfVariables()];
for (int j = 0; j < problem_->getNumberOfVariables(); j++) {
speed[i][j] = 0.0;
}
}
// Step4 and 5
for (int i = 0; i < particles->size(); i++){
Solution *particle = new Solution(particles->get(i));
if (leaders->add(particle) == false){
delete particle;
}
}
//-> Step 6. Initialize the memory of each particle
for (int i = 0; i < particles->size(); i++){
Solution *particle = new Solution(particles->get(i));
best[i] = particle;
}
//Crowding the leaders_
//distance->crowdingDistanceAssignment(leaders, problem_->getNumberOfObjectives());
leaders->computeHVContribution();
//-> Step 7. Iterations ..
while (iteration < maxIterations) {
//Compute the speed_
computeSpeed(iteration, maxIterations);
//Compute the new positions for the particles_
computeNewPositions();
//Mutate the particles_
mopsoMutation(iteration,maxIterations);
//Evaluate the new particles in new positions
for (int i = 0; i < particles->size(); i++){
Solution *particle = particles->get(i);
problem_->evaluate(particle);
problem_->evaluateConstraints(particle);
}
//Update the archive
for (int i = 0; i < particles->size(); i++) {
Solution *particle = new Solution(particles->get(i));
if (leaders->add(particle) == false) {
delete particle;
}
}
//Update the memory of this particle
for (int i = 0; i < particles->size(); i++) {
int flag = dominance->compare(particles->get(i), best[i]);
if (flag != 1) { // the new particle is best_ than the older remembered
Solution *particle = new Solution(particles->get(i));
delete best[i];
best[i] = particle;
}
}
iteration++;
}
// Build the solution set result
SolutionSet * result = new SolutionSet(leaders->size());
for (int i=0;i<leaders->size();i++) {
result->add(new Solution(leaders->get(i)));
}
// Free memory
deleteParams();
return result;
} // execute
