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

Ensure limits passed to TOTP are finite #11

Merged
merged 1 commit into from
Jun 24, 2021
Merged

Ensure limits passed to TOTP are finite #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
14 changes: 6 additions & 8 deletions include/common_robotics_utilities/time_optimal_trajectory_parametrization.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,15 +121,13 @@ class Trajectory
Trajectory(const std::list<Eigen::VectorXd>& waypoints,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration,
const double max_deviation,
const double timestep);
const double max_deviation = 0.0,
const double timestep = 0.001);

Trajectory(const Path &path_,
const Eigen::VectorXd& max_velocity_,
const Eigen::VectorXd& max_acceleration_,
double timestep = 0.001);

~Trajectory(void);
Trajectory(const Path& path,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration,
const double timestep = 0.001);
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

BTW Consider sharing default constants with an explicitly declared constant, e.g. kDefautlTimestamp?

Copy link
Owner Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will deal with them more broadly in the future #12


// Returns the optimal duration of the trajectory
double Duration() const;
Expand Down
70 changes: 25 additions & 45 deletions src/common_robotics_utilities/time_optimal_trajectory_parametrization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -196,9 +196,14 @@ Path::Path(const std::list<Eigen::VectorXd>& path, const double max_deviation)
{
if (path.size() < 2)
{
throw std::runtime_error(
throw std::invalid_argument(
"TOTP for paths with fewer than two waypoints is ill-defined");
}
if (!std::isfinite(max_deviation))
{
throw std::invalid_argument("max_deviation must be finite");
}

std::list<Eigen::VectorXd>::const_iterator config1 = path.begin();
std::list<Eigen::VectorXd>::const_iterator config2 = config1;
config2++;
Expand Down Expand Up @@ -344,59 +349,36 @@ Trajectory::Trajectory(const std::list<Eigen::VectorXd>& waypoints,
const Eigen::VectorXd& max_acceleration,
const double max_deviation,
const double timestep)
: path_(Path(waypoints, max_deviation)),
max_velocity_(max_velocity),
max_acceleration_(max_acceleration),
n_(static_cast<uint32_t>(max_velocity.size())),
time_step_(timestep),
cached_time_(std::numeric_limits<double>::max())
{
trajectory_.push_back(TrajectoryStep(0.0, 0.0));
double after_acceleration = GetMinMaxPathAcceleration(0.0, 0.0, true);
while (!IntegrateForward(trajectory_, after_acceleration))
{
double before_acceleration = 0.0;
TrajectoryStep switching_point;
if (GetNextSwitchingPoint(trajectory_.back().path_pos_,
switching_point,
before_acceleration,
after_acceleration))
{
break;
}
IntegrateBackward(trajectory_,
switching_point.path_pos_,
switching_point.path_vel_,
before_acceleration);
}
const double before_acceleration
= GetMinMaxPathAcceleration(path_.Length(), 0.0, false);
IntegrateBackward(trajectory_, path_.Length(), 0.0, before_acceleration);
// calculate timing
std::list<TrajectoryStep>::iterator previous = trajectory_.begin();
std::list<TrajectoryStep>::iterator it = previous;
it->time_ = 0.0;
it++;
while (it != trajectory_.end())
{
it->time_ = previous->time_ + (it->path_pos_ - previous->path_pos_)
/ ((it->path_vel_ + previous->path_vel_) / 2.0);
previous = it;
it++;
}
}
: Trajectory(Path(waypoints, max_deviation),
max_velocity,
max_acceleration,
timestep) {}


Trajectory::Trajectory(const Path& path,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration,
double timestep)
const double timestep)
: path_(path),
max_velocity_(max_velocity),
max_acceleration_(max_acceleration),
n_(static_cast<uint32_t>(max_velocity.size())),
time_step_(timestep),
cached_time_(std::numeric_limits<double>::max())
{
if (!max_velocity_.array().isFinite().all())
{
throw std::invalid_argument("max_velocity must be finite");
}
if (!max_acceleration_.array().isFinite().all())
{
throw std::invalid_argument("max_acceleration must be finite");
}
if (!std::isfinite(time_step_))
{
throw std::invalid_argument("timestep must be finite");
}

trajectory_.push_back(TrajectoryStep(0.0, 0.0));
double after_acceleration = GetMinMaxPathAcceleration(0.0, 0.0, true);
while (!IntegrateForward(trajectory_, after_acceleration))
Expand Down Expand Up @@ -432,8 +414,6 @@ Trajectory::Trajectory(const Path& path,
}
}

Trajectory::~Trajectory(void) {}

void Trajectory::OutputPhasePlaneTrajectory() const
{
std::ofstream velocity_file("max_velocity.txt");
Expand Down