Skip to content

Commit 2b3eee9

Browse files
schornakjschornakj
committed
Allow setting minimum delta_t between traj points in ISP solver
1 parent e4973a6 commit 2b3eee9

File tree

9 files changed

+31
-11
lines changed

9 files changed

+31
-11
lines changed

‎tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,9 @@ struct IterativeSplineParameterizationProfile : public Profile
5555
/** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */
5656
double max_acceleration_scaling_factor{ 1.0 };
5757

58+
/** @brief minimum_time_delta The smallest-allowable difference between timestamps of consecutive trajectory points. Passed to the solver. */
59+
double minimum_time_delta{ std::numeric_limits<double>::epsilon() };
60+
5861
protected:
5962
friend class boost::serialization::access;
6063
template <class Archive>

‎tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,8 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context,
171171
cur_composite_profile->max_acceleration_scaling_factor;
172172
Eigen::VectorXd jerk_scaling_factors = Eigen::VectorXd::Ones(static_cast<Eigen::Index>(flattened.size()));
173173

174+
const auto minimum_time_delta = cur_composite_profile->minimum_time_delta;
175+
174176
// Loop over all MoveInstructions
175177
for (Eigen::Index idx = 0; idx < static_cast<Eigen::Index>(flattened.size()); idx++)
176178
{
@@ -196,7 +198,8 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context,
196198
limits.jerk_limits,
197199
velocity_scaling_factors,
198200
acceleration_scaling_factors,
199-
jerk_scaling_factors))
201+
jerk_scaling_factors,
202+
minimum_time_delta))
200203
{
201204
// If the output key is not the same as the input key the output data should be assigned the input data for error
202205
// branching

‎tesseract_time_parameterization/core/include/tesseract_time_parameterization/core/time_parameterization.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ class TimeParameterization
4343
* @param velocity_scaling_factor The velocity scaling factor. Size should be trajectory.size()
4444
* @param acceleration_scaling_factor The acceleration scaling factor. Size should be trajectory.size()
4545
* @param jerk_scaling_factor The jerk scaling factor. Size should be trajectory.size()
46+
* @param minimum_time_delta The smallest-allowable difference in seconds between timestamps of consecutive trajectory points.
4647
* @return True if successful, otherwise false
4748
*/
4849
virtual bool compute(TrajectoryContainer& trajectory,
@@ -51,7 +52,8 @@ class TimeParameterization
5152
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
5253
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors,
5354
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors,
54-
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors) const = 0;
55+
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors,
56+
const double& minimum_time_delta) const = 0;
5557
};
5658
} // namespace tesseract_planning
5759
#endif // TESSERACT_TIME_PARAMETERIZATION_TIME_PARAMETERIZATION_H

‎tesseract_time_parameterization/isp/include/tesseract_time_parameterization/isp/iterative_spline_parameterization.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,8 @@ class IterativeSplineParameterization : public TimeParameterization
9191
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
9292
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors = Eigen::VectorXd::Ones(1),
9393
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors = Eigen::VectorXd::Ones(1),
94-
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override;
94+
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1),
95+
const double& minimum_time_delta = std::numeric_limits<double>::epsilon()) const override;
9596

9697
private:
9798
/**

‎tesseract_time_parameterization/isp/src/iterative_spline_parameterization.cpp‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ static void adjust_two_positions(long n,
5757
double x2_i, // NOLINT
5858
double x2_f); // NOLINT
5959
static void
60-
init_times(long n, double dt[], const double x[], const double max_velocity[], double min_velocity[]); // NOLINT
60+
init_times(long n, const double minimum_time_delta, double dt[], const double x[], const double max_velocity[], double min_velocity[]); // NOLINT
6161
// static int fit_spline_and_adjust_times(const int n,
6262
// double dt[],
6363
// const double x[],
@@ -105,7 +105,8 @@ bool IterativeSplineParameterization::compute(TrajectoryContainer& trajectory,
105105
const Eigen::Ref<const Eigen::MatrixX2d>& /*jerk_limits*/,
106106
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors,
107107
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors,
108-
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/) const
108+
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/,
109+
const double& minimum_time_delta) const
109110
{
110111
if (trajectory.empty())
111112
return true;
@@ -342,6 +343,7 @@ bool IterativeSplineParameterization::compute(TrajectoryContainer& trajectory,
342343
std::vector<double> time_diff(static_cast<std::size_t>(num_points - 1), std::numeric_limits<double>::epsilon());
343344
for (unsigned int j = 0; j < trajectory.dof(); j++)
344345
init_times(static_cast<long>(num_points),
346+
minimum_time_delta,
345347
time_diff.data(),
346348
t2[j].positions_.data(),
347349
t2[j].max_velocity_.data(),
@@ -548,7 +550,7 @@ static void adjust_two_positions(const long n,
548550
Increase a segment's time interval if the current time isn't long enough.
549551
*/
550552
// NOLINTNEXTLINE
551-
static void init_times(long n, double dt[], const double x[], const double max_velocity[], double min_velocity[])
553+
static void init_times(long n, const double minimum_time_delta, double dt[], const double x[], const double max_velocity[], double min_velocity[])
552554
{
553555
for (long i = 0; i < n - 1; i++)
554556
{
@@ -560,6 +562,10 @@ static void init_times(long n, double dt[], const double x[], const double max_v
560562
time = (dx / min_velocity[i]);
561563
time += std::numeric_limits<double>::epsilon(); // prevent divide-by-zero
562564

565+
if (time < minimum_time_delta) {
566+
time = minimum_time_delta;
567+
}
568+
563569
if (dt[i] < time)
564570
dt[i] = time;
565571
}

‎tesseract_time_parameterization/ruckig/include/tesseract_time_parameterization/ruckig/ruckig_trajectory_smoothing.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,8 @@ class RuckigTrajectorySmoothing : public TimeParameterization
5757
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
5858
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors = Eigen::VectorXd::Ones(1),
5959
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors = Eigen::VectorXd::Ones(1),
60-
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override;
60+
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1),
61+
const double& minimum_time_delta = std::numeric_limits<double>::epsilon()) const override;
6162

6263
protected:
6364
double duration_extension_fraction_;

‎tesseract_time_parameterization/ruckig/src/ruckig_trajectory_smoothing.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,8 @@ bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory,
7373
const Eigen::Ref<const Eigen::VectorXd>& max_jerk,
7474
const Eigen::Ref<const Eigen::VectorXd>& max_velocity_scaling_factors,
7575
const Eigen::Ref<const Eigen::VectorXd>& max_acceleration_scaling_factors,
76-
const Eigen::Ref<const Eigen::VectorXd>& max_jerk_scaling_factors) const
76+
const Eigen::Ref<const Eigen::VectorXd>& max_jerk_scaling_factors,
77+
const double& /*minimum_time_delta*/) const
7778
{
7879
if (trajectory.size() < 2)
7980
return true;
@@ -208,7 +209,8 @@ bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory,
208209
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
209210
const Eigen::Ref<const Eigen::VectorXd>& /*velocity_scaling_factors*/,
210211
const Eigen::Ref<const Eigen::VectorXd>& /*acceleration_scaling_factors*/,
211-
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/) const
212+
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/,
213+
const double& /*minimum_time_delta*/) const
212214
{
213215
if (trajectory.size() < 2)
214216
return true;

‎tesseract_time_parameterization/totg/include/tesseract_time_parameterization/totg/time_optimal_trajectory_generation.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,8 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
6767
const Eigen::Ref<const Eigen::MatrixX2d>& jerk_limits,
6868
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors = Eigen::VectorXd::Ones(1),
6969
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors = Eigen::VectorXd::Ones(1),
70-
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override;
70+
const Eigen::Ref<const Eigen::VectorXd>& jerk_scaling_factors = Eigen::VectorXd::Ones(1),
71+
const double& minimum_time_delta = std::numeric_limits<double>::epsilon()) const override;
7172

7273
private:
7374
double path_tolerance_;

‎tesseract_time_parameterization/totg/src/time_optimal_trajectory_generation.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,8 @@ bool TimeOptimalTrajectoryGeneration::compute(TrajectoryContainer& trajectory,
6767
const Eigen::Ref<const Eigen::MatrixX2d>& /*jerk_limits*/,
6868
const Eigen::Ref<const Eigen::VectorXd>& velocity_scaling_factors,
6969
const Eigen::Ref<const Eigen::VectorXd>& acceleration_scaling_factors,
70-
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/) const
70+
const Eigen::Ref<const Eigen::VectorXd>& /*jerk_scaling_factors*/,
71+
const double& /*minimum_time_delta*/) const
7172
{
7273
if (trajectory.empty())
7374
return true;

0 commit comments

Comments
 (0)