Skip to content

Commit

Permalink
fix: handle last sample
Browse files Browse the repository at this point in the history
  • Loading branch information
@MarcoMagriDev
MarcoMagriDev committed Feb 10, 2025
1 parent 45119c0 commit c03a32a
Show file tree
Hide file tree
Showing 5 changed files with 80 additions and 55 deletions.
3 changes: 1 addition & 2 deletions ...industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,11 +119,10 @@ bool verifySampleJointLimits(const std::map<std::string, double>& position_last,
* @param trajectory The input KDL trajectory.
* @param interpolation_params Parameters for interpolation.
* @param time_samples Output vector of time samples.
* @param time_step The time step for sampling.
* @return true if time samples are successfully computed, false otherwise.
*/
bool computeTimeSamples(const KDL::Trajectory& trajectory, const interpolation::Params& interpolation_params,
std::vector<double>& time_samples, double time_step = 0.001);
std::vector<double>& time_samples);

/**
* @brief Generate joint trajectory from a KDL Cartesian trajectory
Expand Down
8 changes: 0 additions & 8 deletions moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,6 @@ interpolation:
gt: [ 0.0 ]
}
}
min_number_of_samples: {
type: int,
default_value: 0,
description: "Minimum number of samples for the generated trajectory",
validation: {
gt_eq: [ 0 ]
}
}
max_translation_interpolation_distance: {
type: double,
default_value: 10000000.0,
Expand Down
59 changes: 44 additions & 15 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,16 +204,17 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits(

bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& trajectory,
const interpolation::Params& interpolation_params,
std::vector<double>& time_samples, double time_step)
std::vector<double>& time_samples)
{
const double epsilon = 10e-6;
time_samples.clear();

// auto start_time = std::chrono::high_resolution_clock::now();
double t = 0;
double last_time = 0.0;
double prev_time = 0.0;
double traj_duration = trajectory.Duration();

if (traj_duration <= 1e-06)
if (traj_duration <= epsilon)
{
time_samples.push_back(0.0);
return true;
Expand All @@ -227,6 +228,15 @@ bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& t
double total_translation_distance = (last_frame.p - current_frame.p).Norm();
double total_rotation_distance = (last_frame.M * current_frame.M.Inverse()).GetRot().Norm();

bool has_translation = total_translation_distance > interpolation_params.min_translation_interpolation_distance;
bool has_rotation = total_rotation_distance > interpolation_params.min_rotation_interpolation_distance;

if (!has_translation && !has_rotation)
{
RCLCPP_ERROR(getLogger(), "Total translation and rotation distance are too small.");
return false;
}

// Compute the number of samples based on the maximum sample time, translation and rotation distance
int n_sample_duration = std::ceil(traj_duration / interpolation_params.max_sample_time);
int n_sample_translation =
Expand All @@ -242,20 +252,26 @@ bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& t
double target_rotation_distance = total_rotation_distance / n_samples;
double target_max_sample_time = traj_duration / n_samples;

if (target_translation_distance < interpolation_params.min_translation_interpolation_distance)
if (target_translation_distance < interpolation_params.min_translation_interpolation_distance &&
target_rotation_distance < interpolation_params.min_rotation_interpolation_distance)
{
if (total_translation_distance < interpolation_params.min_translation_interpolation_distance)
{
RCLCPP_DEBUG(getLogger(),
"Translation distance is too small, set to minimum value to ensure no numerical errors.");
target_translation_distance = 1e-06;
}
double min_feasible_max_sample_time = traj_duration / std::max(n_sample_translation, n_sample_rotation);
RCLCPP_ERROR_STREAM(getLogger(),
"Translation and rotation distance are too small. The provided sampling time is too "
"small min value is "
<< min_feasible_max_sample_time);
return false;
}

if (target_translation_distance < interpolation_params.min_translation_interpolation_distance && !has_translation)
{
RCLCPP_DEBUG(getLogger(), "Translation distance is too small, set to minimum value to ensure no numerical errors.");
target_translation_distance = epsilon;
}
if (target_rotation_distance < interpolation_params.min_rotation_interpolation_distance &&
total_rotation_distance < interpolation_params.min_rotation_interpolation_distance)
if (target_rotation_distance < interpolation_params.min_rotation_interpolation_distance && !has_rotation)
{
RCLCPP_DEBUG(getLogger(), "Rotation distance is too small, set to minimum value to ensure no numerical errors.");
target_rotation_distance = 1e-06;
target_rotation_distance = epsilon;
}

RCLCPP_DEBUG_STREAM(getLogger(), "target translation distance: "
Expand All @@ -265,6 +281,9 @@ bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& t

double translation_distance_from_previous, rotation_distance_from_previous;
double translation_distance_from_next, rotation_distance_from_next;
double translation_distance_from_last, rotation_distance_from_last;
// Set the time_step to a fraction of the interpolation adjusted max sample time
double time_step = std::max(std::min(0.001, target_max_sample_time / 10), epsilon);
time_samples.push_back(0.0);
while (t + time_step < traj_duration)
{
Expand All @@ -289,12 +308,22 @@ bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& t
rotation_distance_from_next = (prev_frame.M * next_frame.M.Inverse()).GetRot().Norm();

if (translation_distance_from_previous >= target_translation_distance ||
rotation_distance_from_previous >= target_rotation_distance || (t - last_time) >= target_max_sample_time ||
rotation_distance_from_previous >= target_rotation_distance || (t - prev_time) >= target_max_sample_time ||
translation_distance_from_next > interpolation_params.max_translation_interpolation_distance ||
rotation_distance_from_next > interpolation_params.max_rotation_interpolation_distance)
{
translation_distance_from_last = (current_frame.p - last_frame.p).Norm();
rotation_distance_from_last = (last_frame.M * current_frame.M.Inverse()).GetRot().Norm();
// Ensures last point is valid
if ((has_translation &&
translation_distance_from_last < interpolation_params.min_translation_interpolation_distance) &&
(has_rotation && rotation_distance_from_last < interpolation_params.min_rotation_interpolation_distance))
{
break;
}

time_samples.push_back(t); // Store the time
last_time = t;
prev_time = t;
prev_frame = current_frame;
}
}
Expand Down
17 changes: 1 addition & 16 deletions moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1300,13 +1300,6 @@ ::testing::AssertionResult testutils::hasTrapezoidVelocity(std::vector<double> a
// Check that acceleration is monotonously decreasing
if (!isMonotonouslyDecreasing(accelerations, acc_tol))
{
// Print the accelerations
std::stringstream debug_stream;
for (auto acc : accelerations)
{
debug_stream << acc << '\n';
}
std::cout << "Accelerations: " << debug_stream.str() << std::endl;
return ::testing::AssertionFailure() << "Cannot be a trapezoid since "
"acceleration is not monotonously "
"decreasing!";
Expand Down Expand Up @@ -1487,14 +1480,6 @@ testutils::checkInterpolationParameters(const robot_trajectory::RobotTrajectoryC
const std::string& link_name,
const interpolation::Params& interpolation_parameters, const double EPSILON)
{
if (trajectory->getWayPointCount() < static_cast<std::size_t>(interpolation_parameters.min_number_of_samples))
{
return ::testing::AssertionFailure()
<< "Number of samples in trajectory is less than the minimum required number of "
"samples of "
<< interpolation_parameters.min_number_of_samples;
}

// Iterate over waypoints and check that interpolation parameters are met
for (size_t i = 1; i < trajectory->getWayPointCount(); ++i)

Expand Down Expand Up @@ -1535,7 +1520,7 @@ testutils::checkInterpolationParameters(const robot_trajectory::RobotTrajectoryC
}

// This do not make sense as soon as the max_sample_time will be used as reference but increased
// by a small value to compensate the last waypoint
// if necessary to avoid inserting too close points
// auto time_between_waypoints = trajectory->getWayPointDurationFromPrevious(i);
// if (time_between_waypoints > interpolation_parameters.max_sample_time + EPSILON)
// {
Expand Down
48 changes: 34 additions & 14 deletions .../pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -307,11 +307,23 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile)
}
}

/**
* @brief Unit test for the TrajectoryGeneratorLIN class to verify interpolation parameters.
*
* This test constructs a motion plan request and sets various interpolation parameters to check
* the behavior of the linear trajectory generation. It verifies the following:
* - Successful trajectory generation with default parameters.
* - Failure of interpolation when the max translation interpolation distance is set to a very small value.
* - Failure of interpolation when the max rotation interpolation distance is set to a very small value.
* - Successful trajectory generation and interpolation with specific test parameters.
*/
TEST_F(TrajectoryGeneratorLINTest, interpolationParameters)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };

interpolation::Params default_params;
interpolation::Params interpolation_params;
interpolation::Params test_params;
test_params.max_sample_time = 0.05;
test_params.max_translation_interpolation_distance = 0.001;
Expand All @@ -321,8 +333,6 @@ TEST_F(TrajectoryGeneratorLINTest, interpolationParameters)
/// + plan LIN trajectory +
/// +++++++++++++++++++++++
planning_interface::MotionPlanResponse res;
interpolation::Params default_params;
interpolation::Params interpolation_params;
interpolation_params.max_sample_time = test_params.max_sample_time;
lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params);
EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
Expand All @@ -331,10 +341,10 @@ TEST_F(TrajectoryGeneratorLINTest, interpolationParameters)
// and check that the interpolation fails
interpolation_params.max_translation_interpolation_distance = test_params.max_translation_interpolation_distance;
ASSERT_FALSE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params));
interpolation_params.max_translation_interpolation_distance = default_params.max_translation_interpolation_distance;

// set the max rotation interpolation distance to a very small value
// and check that the interpolation fails
interpolation_params.max_translation_interpolation_distance = default_params.max_translation_interpolation_distance;
interpolation_params.max_rotation_interpolation_distance = test_params.max_rotation_interpolation_distance;
ASSERT_FALSE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params));

Expand All @@ -344,32 +354,42 @@ TEST_F(TrajectoryGeneratorLINTest, interpolationParameters)
ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, test_params));
}

/**
* @brief Test case for verifying the interpolation parameters in the LIN trajectory generator.
*
* This test constructs a motion plan request for a linear joint motion and sets the interpolation parameters.
* It then generates a LIN trajectory and checks if the interpolation parameters are correctly applied.
* The test verifies the following:
* - The trajectory generation is successful with the initial interpolation parameters.
* - The interpolation parameters are correctly checked against the generated trajectory.
* - The trajectory generation is successful with modified interpolation parameters.
*/
TEST_F(TrajectoryGeneratorLINTest, interpolationParametersNumericalIK)
{
// construct motion plan request
moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() };

interpolation::Params test_params;
test_params.max_sample_time = 0.05;
test_params.max_translation_interpolation_distance = 0.005;
test_params.max_rotation_interpolation_distance = 0.01;
test_params.min_translation_interpolation_distance = 5e-4;
test_params.min_rotation_interpolation_distance = 5e-4;
interpolation::Params interpolation_params;
interpolation_params.max_sample_time = 0.01;
interpolation_params.max_translation_interpolation_distance = 0.001;
interpolation_params.max_rotation_interpolation_distance = 1.0;

/// +++++++++++++++++++++++
/// + plan LIN trajectory +
/// +++++++++++++++++++++++
planning_interface::MotionPlanResponse res;
interpolation::Params default_params;
interpolation::Params interpolation_params;
interpolation_params.max_sample_time = test_params.max_sample_time;
lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params);
EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params));

interpolation_params.min_translation_interpolation_distance = 5e-4;
interpolation_params.min_rotation_interpolation_distance = 5e-4;
ASSERT_FALSE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params));

// recompute the trajectory with the same interpolation parameters
// and check that the interpolation is successful
lin_->generate(planning_scene_, lin_joint_req, res, test_params);
ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, test_params));
lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params);
ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params));
}

/**
Expand Down

0 comments on commit c03a32a

Please sign in to comment.