Skip to content

Commit

Permalink
Allow setting minimum delta_t between traj points in ISP solver
Browse files Browse the repository at this point in the history
  • Loading branch information
@schornakj
schornakj committed Jan 6, 2025
1 parent 82512cd commit b599c31
Show file tree
Hide file tree
Showing 9 changed files with 30 additions and 11 deletions.
2 changes: 2 additions & 0 deletions ...ude/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ struct IterativeSplineParameterizationProfile

/** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */
double max_acceleration_scaling_factor{ 1.0 };

double minimum_time_delta{ std::numeric_limits<double>::epsilon() };
};
} // namespace tesseract_planning

Expand Down
5 changes: 4 additions & 1 deletion tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,8 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context,
cur_composite_profile->max_acceleration_scaling_factor;
Eigen::VectorXd jerk_scaling_factors = Eigen::VectorXd::Ones(static_cast<Eigen::Index>(flattened.size()));

const auto minimum_time_delta = cur_composite_profile->minimum_time_delta;

// Loop over all MoveInstructions
for (Eigen::Index idx = 0; idx < static_cast<Eigen::Index>(flattened.size()); idx++)
{
Expand Down Expand Up @@ -219,7 +221,8 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context,
limits.jerk_limits,
velocity_scaling_factors,
acceleration_scaling_factors,
jerk_scaling_factors))
jerk_scaling_factors,
minimum_time_delta))
{
// If the output key is not the same as the input key the output data should be assigned the input data for error
// branching
Expand Down
4 changes: 3 additions & 1 deletion ...arameterization/core/include/tesseract_time_parameterization/core/time_parameterization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ class TimeParameterization
* @param velocity_scaling_factor The velocity scaling factor. Size should be trajectory.size()
* @param acceleration_scaling_factor The acceleration scaling factor. Size should be trajectory.size()
* @param jerk_scaling_factor The jerk scaling factor. Size should be trajectory.size()
* @param minimum_time_delta The smallest-allowable difference in seconds between timestamps of consecutive trajectory points.
* @return True if successful, otherwise false
*/
virtual bool compute(TrajectoryContainer& trajectory,
Expand All @@ -51,7 +52,8 @@ class TimeParameterization
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors) const = 0;
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors,
const double& minimum_time_delta) const = 0;
};
} // namespace tesseract_planning
#endif // TESSERACT_TIME_PARAMETERIZATION_TIME_PARAMETERIZATION_H
3 changes: 2 additions & 1 deletion ...ation/isp/include/tesseract_time_parameterization/isp/iterative_spline_parameterization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,8 @@ class IterativeSplineParameterization : public TimeParameterization
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors = Eigen::VectorXd::Ones(1),
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors = Eigen::VectorXd::Ones(1),
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override;
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1),
const double& minimum_time_delta = std::numeric_limits<double>::epsilon()) const override;

private:
/**
Expand Down
12 changes: 9 additions & 3 deletions tesseract_time_parameterization/isp/src/iterative_spline_parameterization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ static void adjust_two_positions(long n,
double x2_i, // NOLINT
double x2_f); // NOLINT
static void
init_times(long n, double dt[], const double x[], const double max_velocity[], double min_velocity[]); // NOLINT
init_times(long n, const double minimum_time_delta, double dt[], const double x[], const double max_velocity[], double min_velocity[]); // NOLINT
// static int fit_spline_and_adjust_times(const int n,
// double dt[],
// const double x[],
Expand Down Expand Up @@ -105,7 +105,8 @@ bool IterativeSplineParameterization::compute(TrajectoryContainer& trajectory,
const Eigen::Ref<const Eigen::MatrixX2d>& /*jerk_limits*/,
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/) const
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/,
const double& minimum_time_delta) const
{
if (trajectory.empty())
return true;
Expand Down Expand Up @@ -342,6 +343,7 @@ bool IterativeSplineParameterization::compute(TrajectoryContainer& trajectory,
std::vector<double> time_diff(static_cast<std::size_t>(num_points - 1), std::numeric_limits<double>::epsilon());
for (unsigned int j = 0; j < trajectory.dof(); j++)
init_times(static_cast<long>(num_points),
minimum_time_delta,
time_diff.data(),
t2[j].positions_.data(),
t2[j].max_velocity_.data(),
Expand Down Expand Up @@ -548,7 +550,7 @@ static void adjust_two_positions(const long n,
Increase a segment's time interval if the current time isn't long enough.
*/
// NOLINTNEXTLINE
static void init_times(long n, double dt[], const double x[], const double max_velocity[], double min_velocity[])
static void init_times(long n, const double minimum_time_delta, double dt[], const double x[], const double max_velocity[], double min_velocity[])
{
for (long i = 0; i < n - 1; i++)
{
Expand All @@ -560,6 +562,10 @@ static void init_times(long n, double dt[], const double x[], const double max_v
time = (dx / min_velocity[i]);
time += std::numeric_limits<double>::epsilon(); // prevent divide-by-zero

if (time < minimum_time_delta) {
time = minimum_time_delta;
}

if (dt[i] < time)
dt[i] = time;
}
Expand Down
3 changes: 2 additions & 1 deletion ...ation/ruckig/include/tesseract_time_parameterization/ruckig/ruckig_trajectory_smoothing.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ class RuckigTrajectorySmoothing : public TimeParameterization
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors = Eigen::VectorXd::Ones(1),
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors = Eigen::VectorXd::Ones(1),
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override;
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1),
const double& minimum_time_delta = std::numeric_limits<double>::epsilon()) const override;

protected:
double duration_extension_fraction_;
Expand Down
6 changes: 4 additions & 2 deletions tesseract_time_parameterization/ruckig/src/ruckig_trajectory_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory,
const Eigen::Ref<const Eigen::VectorXd>& max_jerk,
const Eigen::Ref<const Eigen::VectorXd>& max_velocity_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& max_acceleration_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& max_jerk_scaling_factors) const
const Eigen::Ref<const Eigen::VectorXd>& max_jerk_scaling_factors,
const double& /*minimum_time_delta*/) const
{
if (trajectory.size() < 2)
return true;
Expand Down Expand Up @@ -208,7 +209,8 @@ bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory,
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
const Eigen::Ref<const Eigen::VectorXd>& /*velocity_scaling_factors*/,
const Eigen::Ref<const Eigen::VectorXd>& /*acceleration_scaling_factors*/,
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/) const
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/,
const double& /*minimum_time_delta*/) const
{
if (trajectory.size() < 2)
return true;
Expand Down
3 changes: 2 additions & 1 deletion ...on/totg/include/tesseract_time_parameterization/totg/time_optimal_trajectory_generation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,8 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors = Eigen::VectorXd::Ones(1),
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors = Eigen::VectorXd::Ones(1),
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override;
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1),
const double& minimum_time_delta = std::numeric_limits<double>::epsilon()) const override;

private:
double path_tolerance_;
Expand Down
3 changes: 2 additions & 1 deletion tesseract_time_parameterization/totg/src/time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,8 @@ bool TimeOptimalTrajectoryGeneration::compute(TrajectoryContainer& trajectory,
const Eigen::Ref<const Eigen::MatrixX2d>& /*jerk_limits*/,
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors,
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/) const
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/,
const double& /*minimum_time_delta*/) const
{
if (trajectory.empty())
return true;
Expand Down

0 comments on commit b599c31

Please sign in to comment.