// checks if a path is clear from current position for given speed, goal speed, time and accel bool CalibrateAction::checkPath(double vx, double vy, double vtheta, double sim_time_, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta) { Trajectory traj; tf::StampedTransform transform; try { // shift lookup in the past by 100ms to ensure cache is not empty listener->lookupTransform(cost_map->getGlobalFrameID(),robotFrame,ros::Time(0.0), transform); } catch (tf::TransformException ex) { ROS_ERROR("Nope! %s", ex.what()); return false; } //ROS_INFO("gen. Traj with pos x: %f, y: %f, th: %f, vel: x: %f, y:%f, th: %f, dur: %f", transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()), vx, vy, vtheta, sim_time_); // generate a trajectory for the given goal speed, rotation and time. No acceleration needed here generateTrajectory(transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()), vx, vy, vtheta, sim_time_, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, traj); visualize_trajectory(traj); return checkTrajectory(traj); }
// checks if a path is clear from a given position for given speed, goal speed, time and accel bool CalibrateAction::checkPath(double xPos, double yPos, double thetaPos, double vx, double vy, double vtheta, double sim_time_, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta) { Trajectory traj; // generate a trajectory for the given goal speed, rotation and time. No acceleration needed here generateTrajectory(xPos, yPos, thetaPos, vx, vy, vtheta, sim_time_, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, traj); visualize_trajectory(traj); return checkTrajectory(traj); }
bool UpperBodyPlanner::previewPathPlan(const std::vector<geometry_msgs::Pose>& pose_sequence, move_group_interface::MoveGroup::Plan &plan) { std::cout << "Adding " << pose_sequence.size() << " points." << std::endl; moveit_msgs::RobotTrajectory trajectory; moveit::core::robotStateToRobotStateMsg(*kinematic_state, plan.start_state_); double fraction = this->computeCartesianPath(pose_sequence, path_step, 0.0, trajectory); std::cout << "Fraction is: " << fraction << std::endl; std::cout << "Current position tolerance is: " << this->getGoalPositionTolerance() << std::endl; std::cout << "Current orientation tolerance is: " << this->getGoalOrientationTolerance() << std::endl; if (fraction != 1.0) ROS_WARN("The plan is not perfect. Current fraction isn't equal to 1.0"); checkTrajectory(trajectory, plan.trajectory_); if (singularity_check == false) ROS_WARN("The plan didn't pass singularity check. Suggest to replan."); ROS_INFO("Preview planning..."); if (fraction != 1.0 || singularity_check == false) return false; return true; }
// checks path when starting from zero speed with unknown speed up time bool CalibrateAction::checkPath(double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, Trajectory *resultTraj) { Trajectory traj; tf::StampedTransform transform; try { // shift lookup in the past by 100ms to ensure cache is not empty listener->lookupTransform(cost_map->getGlobalFrameID(),robotFrame,ros::Time(0.0), transform); } catch (tf::TransformException ex) { ROS_ERROR("Nope! %s", ex.what()); return false; } // getting maximum needed sim_time_, absolute value, as time can't be negative, but speed can be double sim_time_x = fabs((vx_samp - vx)/acc_x); double sim_time_y = fabs((vy_samp - vy)/acc_y); double sim_time_theta = fabs((vtheta_samp - vtheta)/acc_theta); double sim_time_ = std::max(sim_time_x,sim_time_y); sim_time_ = std::max(sim_time_,sim_time_theta); sim_time_ = std::max(sim_time_, min_time_clear); // always check ahead at least 'min_time_clear' seconds //ROS_INFO("gen. Traj with pos x: %f, y: %f, th: %f, vel: x: %f, y:%f, th: %f, dur: %f", transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()), vx, vy, vtheta, sim_time_); // generate a trajectory for the given goal speed, rotation and time. No acceleration needed here generateTrajectory(transform.getOrigin().getX(), transform.getOrigin().getY(), tf::getYaw(transform.getRotation()), vx, vy, vtheta, sim_time_, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, traj); visualize_trajectory(traj); if(resultTraj != NULL) { *resultTraj = traj; } return checkTrajectory(traj); }