From 37761f9f3d8bd8e00b0896c625f79f30e4976eb7 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 1/7] feat: adds checks to validate interpolation parameters in LIN trajectories --- kinematics.yaml | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 kinematics.yaml diff --git a/kinematics.yaml b/kinematics.yaml new file mode 100644 index 0000000000..c597dc1b75 --- /dev/null +++ b/kinematics.yaml @@ -0,0 +1,6 @@ +manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + From 7d0c520ffd5c131665eb394330cb4fb3f0c2c6aa Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 2/7] working min interpolations testNonLinearBlending forced --- .../trajectory_functions.hpp | 9 +- .../trajectory_blender_transition_window.cpp | 3 +- .../src/trajectory_functions.cpp | 142 ++++++++++++++---- .../src/trajectory_generator_circ.cpp | 13 +- .../src/trajectory_generator_lin.cpp | 11 +- .../test/test_utils.cpp | 7 + ...t_trajectory_blender_transition_window.cpp | 23 ++- .../src/unittest_trajectory_functions.cpp | 12 +- .../src/unittest_trajectory_generator_lin.cpp | 54 +++---- 9 files changed, 187 insertions(+), 87 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index 07b62df1eb..89057e9c9a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -51,6 +51,7 @@ #include #include #include +#include namespace pilz_industrial_motion_planner { @@ -112,6 +113,9 @@ bool verifySampleJointLimits(const std::map& position_last, const std::map& position_current, double duration_last, double duration_current, const JointLimitsContainer& joint_limits); +void compute_time_samples(const KDL::Trajectory& trajectory, const interpolation::Params& interpolation_params, + std::vector& time_samples, double time_step = 0.001); + /** * @brief Interpolates between two poses. * @@ -146,8 +150,8 @@ void interpolate(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& e bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, double sampling_time, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const std::map& initial_joint_position, + std::vector time_samples, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); /** @@ -166,6 +170,7 @@ bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, const std::map& initial_joint_velocity, + const double& last_sample_duration, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index cb5dfa668f..a3ba5c09a5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -94,12 +94,13 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( initial_joint_velocity[joint_name] = req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name); } + double duration_last_sample = req.first_trajectory->getWayPointDurationFromPrevious(first_intersection_index - 1); trajectory_msgs::msg::JointTrajectory blend_joint_trajectory; moveit_msgs::msg::MoveItErrorCodes error_code; if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian, req.group_name, req.link_name, initial_joint_position, initial_joint_velocity, - blend_joint_trajectory, error_code)) + duration_last_sample, blend_joint_trajectory, error_code)) { // LCOV_EXCL_START RCLCPP_INFO(getLogger(), "Failed to generate joint trajectory for blending trajectory."); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 7a4fb6def1..4090fcf090 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -215,11 +215,94 @@ void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d& start_ interpolated_pose.linear() = quat1.slerp(interpolation_factor, quat2).toRotationMatrix(); } +void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& trajectory, + const interpolation::Params& interpolation_params, + std::vector& time_samples, double time_step) +{ + // auto start_time = std::chrono::high_resolution_clock::now(); + double t = 0; + double last_time = 0.0; + double traj_duration = trajectory.Duration(); + + // Get the initial position + KDL::Frame prev_frame, current_frame = trajectory.Pos(t); + KDL::Frame last_frame = trajectory.Pos(traj_duration); + + double total_translation_distance = (last_frame.p - current_frame.p).Norm(); + double total_rotation_distance = (last_frame.M * current_frame.M.Inverse()).GetRot().Norm(); + + std::cout << traj_duration << " " << total_translation_distance << " " << total_rotation_distance << std::endl; + + int n_sample_duration = std::ceil(traj_duration / interpolation_params.max_sample_time); + int n_sample_translation = + std::ceil(total_translation_distance / (interpolation_params.max_translation_interpolation_distance * 0.95)); + int n_sample_rotation = + std::ceil(total_rotation_distance / (interpolation_params.max_rotation_interpolation_distance * 0.95)); + std::cout << traj_duration / interpolation_params.max_sample_time << std::endl; + std::cout << n_sample_duration << " " << n_sample_translation << " " << n_sample_rotation << std::endl; + int n_samples = std::max({ n_sample_duration, n_sample_translation, n_sample_rotation }); + + double translation_distance_from_previous, rotation_distance_from_previous; + double translation_distance_from_next, rotation_distance_from_next; + double target_translation_distance = total_translation_distance / n_samples; + double target_rotation_distance = total_rotation_distance / n_samples; + double target_max_sample_time = traj_duration / n_samples; + + std::cout << "target translation distance: " << target_translation_distance << std::endl; + std::cout << "target rotation distance: " << target_rotation_distance << std::endl; + std::cout << "max sample time: " << target_max_sample_time << std::endl; + std::cout << "max sampling time: " << interpolation_params.max_sample_time << std::endl; + + time_samples.clear(); + while (t + time_step < traj_duration) + { + // Increment the time by the time step + t += time_step; + + // Get the current position at time t + current_frame = trajectory.Pos(t); + + // Compute the distance from the previous position + translation_distance_from_previous = (current_frame.p - prev_frame.p).Norm(); + rotation_distance_from_previous = (prev_frame.M * current_frame.M.Inverse()).GetRot().Norm(); + + if (translation_distance_from_previous < interpolation_params.min_translation_interpolation_distance + 2e-6 && + rotation_distance_from_previous < interpolation_params.min_rotation_interpolation_distance + 2e-6) + { + continue; + } + + KDL::Frame next_frame = trajectory.Pos(t + time_step); + translation_distance_from_next = (next_frame.p - prev_frame.p).Norm(); + 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 || + translation_distance_from_next > interpolation_params.max_translation_interpolation_distance || + rotation_distance_from_next > interpolation_params.max_rotation_interpolation_distance) + { + time_samples.push_back(t); // Store the time + + last_time = t; + prev_frame = current_frame; + } + } + + if (time_samples.empty() || time_samples.back() < traj_duration) + { + time_samples.push_back(traj_duration); + } + std::cout << time_samples[time_samples.size() - 1] << std::endl; + std::cout << time_samples[time_samples.size() - 2] << std::endl; + + // std::chrono::duration execution_time = std::chrono::high_resolution_clock::now() - start_time; +} + bool pilz_industrial_motion_planner::generateJointTrajectory( const planning_scene::PlanningSceneConstPtr& scene, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, double sampling_time, + const std::map& initial_joint_position, std::vector time_samples, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { @@ -229,19 +312,9 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( rclcpp::Clock clock; rclcpp::Time generation_begin = clock.now(); - // generate the time samples - std::vector time_samples; - int num_samples = std::floor(trajectory.Duration() / sampling_time); - sampling_time = trajectory.Duration() / num_samples; - time_samples.reserve(num_samples); - for (int i = 0; i < num_samples; ++i) - { - time_samples.push_back(i * sampling_time); - } - time_samples.push_back(trajectory.Duration()); - // sample the trajectory and solve the inverse kinematics Eigen::Isometry3d pose_sample; + Eigen::Isometry3d pose_sample_last; std::map ik_solution_last, ik_solution, joint_velocity_last; ik_solution_last = initial_joint_position; for (const auto& item : ik_solution_last) @@ -249,6 +322,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( joint_velocity_last[item.first] = 0.0; } + double duration_current_sample, duration_last_sample; for (std::vector::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end(); ++time_iter) { @@ -262,14 +336,25 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( joint_trajectory.points.clear(); return false; } - - // check the joint limits - double duration_current_sample = sampling_time; - // last interval can be shorter than the sampling time - if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1) + // if (time_iter != time_samples.begin() && time_samples.size() > 1) + // { + // // Compute the forward kinematics of the IK solution to check the error + // double position_error = (pose_sample_last.translation() - pose_sample.translation()).norm(); + // double orientation_error = + // Eigen::AngleAxisd(pose_sample_last.rotation().transpose() * pose_sample.rotation()).angle(); + + // std::cout << "IK solution error - Position error: " << position_error + // << ", Orientation error: " << orientation_error << std::endl; + // } + // pose_sample_last = pose_sample; + if (time_iter != time_samples.begin()) { duration_current_sample = *time_iter - *(time_iter - 1); } + if (*time_iter == time_samples[1]) + { + duration_last_sample = duration_current_sample; + } if (time_samples.size() == 1) { duration_current_sample = *time_iter; @@ -277,7 +362,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // skip the first sample with zero time from start for limits checking if (time_iter != time_samples.begin() && - !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time, + !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last_sample, duration_current_sample, joint_limits)) { RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at " @@ -309,7 +394,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample; point.velocities.push_back(joint_velocity); point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) / - (duration_current_sample + sampling_time) * 2); + (duration_current_sample + duration_last_sample) * 2); joint_velocity_last[joint_name] = joint_velocity; } else @@ -323,6 +408,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // update joint trajectory joint_trajectory.points.push_back(point); ik_solution_last = ik_solution; + duration_last_sample = duration_current_sample; } error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; @@ -339,7 +425,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, - const std::map& initial_joint_velocity, + const std::map& initial_joint_velocity, const double& last_sample_duration, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { @@ -351,7 +437,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( std::map ik_solution_last = initial_joint_position; std::map joint_velocity_last = initial_joint_velocity; - double duration_last = 0; + double duration_last = last_sample_duration; double duration_current = 0; joint_trajectory.joint_names.clear(); for (const auto& joint_position : ik_solution_last) @@ -376,8 +462,6 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( if (i == 0) { duration_current = trajectory.points.front().time_from_start.seconds(); - // This still assumes all the points in first_trajectory have the same duration - duration_last = duration_current; } else { @@ -494,8 +578,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_position_1 - joint_position_2).norm() > epsilon) { - RCLCPP_DEBUG_STREAM(getLogger(), "Joint positions of the two states are different. state1: " - << joint_position_1 << " state2: " << joint_position_2); + RCLCPP_INFO_STREAM(getLogger(), "Joint positions of the two states are different. state1: " + << joint_position_1 << " state2: " << joint_position_2); return false; } @@ -506,8 +590,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon) { - RCLCPP_DEBUG_STREAM(getLogger(), "Joint velocities of the two states are different. state1: " - << joint_velocity_1 << " state2: " << joint_velocity_2); + RCLCPP_INFO_STREAM(getLogger(), "Joint velocities of the two states are different. state1: " + << joint_velocity_1 << " state2: " << joint_velocity_2); return false; } @@ -518,8 +602,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_acc_1 - joint_acc_2).norm() > epsilon) { - RCLCPP_DEBUG_STREAM(getLogger(), "Joint accelerations of the two states are different. state1: " - << joint_acc_1 << " state2: " << joint_acc_2); + RCLCPP_INFO_STREAM(getLogger(), "Joint accelerations of the two states are different. state1: " + << joint_acc_1 << " state2: " << joint_acc_2); return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 9ce64af29d..6e364ce680 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -214,17 +214,10 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false); moveit_msgs::msg::MoveItErrorCodes error_code; - // sample the Cartesian trajectory and compute joint trajectory using inverse - // kinematics - auto cartesian_limits = planner_limits_.getCartesianLimits(); - auto sampling_time = std::min({ interpolation_params.max_sample_time, - interpolation_params.max_translation_interpolation_distance / - (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), - interpolation_params.max_rotation_interpolation_distance / - (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); - RCLCPP_DEBUG(getLogger(), "Sampling time for CIR command: %f", sampling_time); + std::vector time_samples; + compute_time_samples(cart_trajectory, interpolation_params, time_samples); if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, - plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory, + plan_info.link_name, plan_info.start_joint_position, time_samples, joint_trajectory, error_code)) { throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path", diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index bbd1e715ef..305cb5843d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -163,15 +163,10 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s moveit_msgs::msg::MoveItErrorCodes error_code; // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics - auto cartesian_limits = planner_limits_.getCartesianLimits(); - auto sampling_time = std::min({ interpolation_params.max_sample_time, - interpolation_params.max_translation_interpolation_distance / - (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), - interpolation_params.max_rotation_interpolation_distance / - (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); - RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", sampling_time); + std::vector time_samples; + compute_time_samples(cart_trajectory, interpolation_params, time_samples); if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, - plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory, + plan_info.link_name, plan_info.start_joint_position, time_samples, joint_trajectory, error_code)) { std::ostringstream os; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 94d6233351..48b1f01058 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -1300,6 +1300,13 @@ ::testing::AssertionResult testutils::hasTrapezoidVelocity(std::vector 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!"; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 22b6eebd34..13aaf54858 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -648,7 +648,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) trajectory_msgs::msg::JointTrajectory joint_traj; const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) }; // time from start zero does not work - const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() }; + // const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() }; // generate modified cartesian trajectory for (size_t i = 0; i < lin_traj->getWayPointCount(); ++i) @@ -662,14 +662,15 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) geometry_msgs::msg::Pose waypoint_pose = tf2::toMsg(eigen_pose); // add scaled sine function - waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); - waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); - waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); + // This is causing the test to fail for unmatched positions + // waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); + // waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); + // waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); // add to trajectory waypoint.pose = waypoint_pose; - waypoint.time_from_start = rclcpp::Duration::from_seconds( - time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); + waypoint.time_from_start = + rclcpp::Duration::from_seconds(time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); cart_traj.points.push_back(waypoint); } @@ -692,15 +693,21 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) } } + double duration_last_sample = (traj_index == 0) ? lin_traj->getWayPointDurationFromPrevious(1) : + sine_trajs[traj_index - 1]->getWayPointDurationFromPrevious( + sine_trajs[traj_index - 1]->getWayPointCount() - 1); + moveit_msgs::msg::MoveItErrorCodes error_code; if (!generateJointTrajectory(planning_scene_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, - target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code, - true)) + target_link_, initial_joint_position, initial_joint_velocity, duration_last_sample, + joint_traj, error_code, true)) { std::runtime_error("Failed to generate trajectory."); } joint_traj.points.front().time_from_start = rclcpp::Duration::from_seconds(0.0); + joint_traj.points.front().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); + joint_traj.points.front().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 63f2783aeb..d4cc3cc69d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -901,8 +901,16 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI moveit_msgs::msg::MoveItErrorCodes error_code; bool check_self_collision{ false }; + // generate a vector of equally spaced time_samples + std::vector time_samples; + for (double t = 0; t < kdl_trajectory.Duration(); t += sampling_time) + { + time_samples.push_back(t); + } + time_samples.push_back(kdl_trajectory.Duration()); + EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( - planning_scene_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time, + planning_scene_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, time_samples, joint_trajectory, error_code, check_self_collision)); std::map initial_joint_velocity; @@ -915,7 +923,7 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory( planning_scene_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity, - joint_trajectory, error_code, check_self_collision)); + 0.1, joint_trajectory, error_code, check_self_collision)); } /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 08240785d6..e262e74bef 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -364,33 +364,33 @@ TEST_F(TrajectoryGeneratorLINTest, interpolationParameters) * - 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 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; -// 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, interpolation_params); -// ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params)); -// } +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; + + /// +++++++++++++++++++++++ + /// + 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); + + // 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)); +} /** * @brief Check that lin planner returns 'false' if From a4080b99fb393dc461f0bb590e49c16510f394f9 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 3/7] restore cheated test on blending sets limits to avoid numerical errors --- .../src/trajectory_functions.cpp | 60 +++++++++++++------ ...t_trajectory_blender_transition_window.cpp | 20 +++---- 2 files changed, 49 insertions(+), 31 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 4090fcf090..63ec0cd474 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -219,41 +219,65 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& const interpolation::Params& interpolation_params, std::vector& time_samples, double time_step) { + time_samples.clear(); + // auto start_time = std::chrono::high_resolution_clock::now(); double t = 0; double last_time = 0.0; double traj_duration = trajectory.Duration(); + if (traj_duration <= 1e-06) + { + time_samples.push_back(0.0); + return; + } + // Get the initial position KDL::Frame prev_frame, current_frame = trajectory.Pos(t); KDL::Frame last_frame = trajectory.Pos(traj_duration); + // Compute the total translation and rotation distance double total_translation_distance = (last_frame.p - current_frame.p).Norm(); double total_rotation_distance = (last_frame.M * current_frame.M.Inverse()).GetRot().Norm(); - std::cout << traj_duration << " " << total_translation_distance << " " << total_rotation_distance << std::endl; - + // 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 = - std::ceil(total_translation_distance / (interpolation_params.max_translation_interpolation_distance * 0.95)); + std::ceil(total_translation_distance / (interpolation_params.max_translation_interpolation_distance)); int n_sample_rotation = - std::ceil(total_rotation_distance / (interpolation_params.max_rotation_interpolation_distance * 0.95)); - std::cout << traj_duration / interpolation_params.max_sample_time << std::endl; - std::cout << n_sample_duration << " " << n_sample_translation << " " << n_sample_rotation << std::endl; + std::ceil(total_rotation_distance / (interpolation_params.max_rotation_interpolation_distance)); + + // Get the maximum number of samples int n_samples = std::max({ n_sample_duration, n_sample_translation, n_sample_rotation }); - double translation_distance_from_previous, rotation_distance_from_previous; - double translation_distance_from_next, rotation_distance_from_next; + // Compute the target translation and rotation distance based on the number of samples double target_translation_distance = total_translation_distance / n_samples; double target_rotation_distance = total_rotation_distance / n_samples; double target_max_sample_time = traj_duration / n_samples; - std::cout << "target translation distance: " << target_translation_distance << std::endl; - std::cout << "target rotation distance: " << target_rotation_distance << std::endl; - std::cout << "max sample time: " << target_max_sample_time << std::endl; - std::cout << "max sampling time: " << interpolation_params.max_sample_time << std::endl; + if (target_translation_distance < interpolation_params.min_translation_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; + } + } + if (target_rotation_distance < interpolation_params.min_rotation_interpolation_distance && + total_rotation_distance < interpolation_params.min_rotation_interpolation_distance) + { + RCLCPP_DEBUG(getLogger(), "Rotation distance is too small, set to minimum value to ensure no numerical errors."); + target_rotation_distance = 1e-06; + } - time_samples.clear(); + RCLCPP_DEBUG_STREAM(getLogger(), "target translation distance: " + << target_translation_distance << ", target rotation distance: " + << target_rotation_distance << ", max sample time: " << target_max_sample_time + << ", max sampling time: " << interpolation_params.max_sample_time); + + double translation_distance_from_previous, rotation_distance_from_previous; + double translation_distance_from_next, rotation_distance_from_next; + time_samples.push_back(0.0); while (t + time_step < traj_duration) { // Increment the time by the time step @@ -266,8 +290,8 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& translation_distance_from_previous = (current_frame.p - prev_frame.p).Norm(); rotation_distance_from_previous = (prev_frame.M * current_frame.M.Inverse()).GetRot().Norm(); - if (translation_distance_from_previous < interpolation_params.min_translation_interpolation_distance + 2e-6 && - rotation_distance_from_previous < interpolation_params.min_rotation_interpolation_distance + 2e-6) + if (translation_distance_from_previous < interpolation_params.min_translation_interpolation_distance && + rotation_distance_from_previous < interpolation_params.min_rotation_interpolation_distance) { continue; } @@ -282,7 +306,6 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& rotation_distance_from_next > interpolation_params.max_rotation_interpolation_distance) { time_samples.push_back(t); // Store the time - last_time = t; prev_frame = current_frame; } @@ -292,9 +315,6 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& { time_samples.push_back(traj_duration); } - std::cout << time_samples[time_samples.size() - 1] << std::endl; - std::cout << time_samples[time_samples.size() - 2] << std::endl; - // std::chrono::duration execution_time = std::chrono::high_resolution_clock::now() - start_time; } @@ -365,6 +385,8 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last_sample, duration_current_sample, joint_limits)) { + std::cout << "Duration current sample: " << duration_current_sample << std::endl; + std::cout << "Duration last sample: " << duration_last_sample << std::endl; RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at " << *time_iter << "s violates the joint velocity/acceleration/deceleration limits."); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 13aaf54858..f176d445ee 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -648,7 +648,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) trajectory_msgs::msg::JointTrajectory joint_traj; const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) }; // time from start zero does not work - // const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() }; + const double time_from_start_offset{ time_scaling_factor * lin_traj->getAverageSegmentDuration() }; // generate modified cartesian trajectory for (size_t i = 0; i < lin_traj->getWayPointCount(); ++i) @@ -662,15 +662,15 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) geometry_msgs::msg::Pose waypoint_pose = tf2::toMsg(eigen_pose); // add scaled sine function - // This is causing the test to fail for unmatched positions - // waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); - // waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); - // waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); + // This is causing the test to fail for high accelerations + waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); + waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); + waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); // add to trajectory waypoint.pose = waypoint_pose; - waypoint.time_from_start = - rclcpp::Duration::from_seconds(time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); + waypoint.time_from_start = rclcpp::Duration::from_seconds( + time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i)); cart_traj.points.push_back(waypoint); } @@ -693,9 +693,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) } } - double duration_last_sample = (traj_index == 0) ? lin_traj->getWayPointDurationFromPrevious(1) : - sine_trajs[traj_index - 1]->getWayPointDurationFromPrevious( - sine_trajs[traj_index - 1]->getWayPointCount() - 1); + double duration_last_sample = cart_traj.points[1].time_from_start.seconds(); moveit_msgs::msg::MoveItErrorCodes error_code; if (!generateJointTrajectory(planning_scene_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_, @@ -706,8 +704,6 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) } joint_traj.points.front().time_from_start = rclcpp::Duration::from_seconds(0.0); - joint_traj.points.front().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); - joint_traj.points.front().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); From dec1a135fc1e575404754111e6ed495edfa61721 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 4/7] refactor and cleanup --- .../trajectory_functions.hpp | 13 +++++++-- .../src/trajectory_functions.cpp | 27 ++++++++++--------- .../src/trajectory_generator_circ.cpp | 4 +-- .../src/trajectory_generator_lin.cpp | 6 ++--- 4 files changed, 29 insertions(+), 21 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index 89057e9c9a..7f096d790d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -113,8 +113,17 @@ bool verifySampleJointLimits(const std::map& position_last, const std::map& position_current, double duration_last, double duration_current, const JointLimitsContainer& joint_limits); -void compute_time_samples(const KDL::Trajectory& trajectory, const interpolation::Params& interpolation_params, - std::vector& time_samples, double time_step = 0.001); +/** + * @brief Compute time samples for a given trajectory based on interpolation parameters. + * + * @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& time_samples, double time_step = 0.001); /** * @brief Interpolates between two poses. diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 63ec0cd474..6ee776af6e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -215,9 +215,9 @@ void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d& start_ interpolated_pose.linear() = quat1.slerp(interpolation_factor, quat2).toRotationMatrix(); } -void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& trajectory, - const interpolation::Params& interpolation_params, - std::vector& time_samples, double time_step) +bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& trajectory, + const interpolation::Params& interpolation_params, + std::vector& time_samples, double time_step) { time_samples.clear(); @@ -229,7 +229,7 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& if (traj_duration <= 1e-06) { time_samples.push_back(0.0); - return; + return true; } // Get the initial position @@ -259,7 +259,8 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& { 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."); + RCLCPP_DEBUG(getLogger(), + "Translation distance is too small, set to minimum value to ensure no numerical errors."); target_translation_distance = 1e-06; } } @@ -316,6 +317,8 @@ void pilz_industrial_motion_planner::compute_time_samples(const KDL::Trajectory& time_samples.push_back(traj_duration); } // std::chrono::duration execution_time = std::chrono::high_resolution_clock::now() - start_time; + + return true; } bool pilz_industrial_motion_planner::generateJointTrajectory( @@ -385,8 +388,6 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last_sample, duration_current_sample, joint_limits)) { - std::cout << "Duration current sample: " << duration_current_sample << std::endl; - std::cout << "Duration last sample: " << duration_last_sample << std::endl; RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at " << *time_iter << "s violates the joint velocity/acceleration/deceleration limits."); @@ -600,8 +601,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_position_1 - joint_position_2).norm() > epsilon) { - RCLCPP_INFO_STREAM(getLogger(), "Joint positions of the two states are different. state1: " - << joint_position_1 << " state2: " << joint_position_2); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint positions of the two states are different. state1: " + << joint_position_1 << " state2: " << joint_position_2); return false; } @@ -612,8 +613,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon) { - RCLCPP_INFO_STREAM(getLogger(), "Joint velocities of the two states are different. state1: " - << joint_velocity_1 << " state2: " << joint_velocity_2); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint velocities of the two states are different. state1: " + << joint_velocity_1 << " state2: " << joint_velocity_2); return false; } @@ -624,8 +625,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_acc_1 - joint_acc_2).norm() > epsilon) { - RCLCPP_INFO_STREAM(getLogger(), "Joint accelerations of the two states are different. state1: " - << joint_acc_1 << " state2: " << joint_acc_2); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint accelerations of the two states are different. state1: " + << joint_acc_1 << " state2: " << joint_acc_2); return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 6e364ce680..58c977cc8e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -215,8 +215,8 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& moveit_msgs::msg::MoveItErrorCodes error_code; std::vector time_samples; - compute_time_samples(cart_trajectory, interpolation_params, time_samples); - if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, + if (!computeTimeSamples(cart_trajectory, interpolation_params, time_samples) || + !generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, time_samples, joint_trajectory, error_code)) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 305cb5843d..c083369fc0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -161,11 +161,9 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false); moveit_msgs::msg::MoveItErrorCodes error_code; - // sample the Cartesian trajectory and compute joint trajectory using inverse - // kinematics std::vector time_samples; - compute_time_samples(cart_trajectory, interpolation_params, time_samples); - if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, + if (!computeTimeSamples(cart_trajectory, interpolation_params, time_samples) || + !generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, time_samples, joint_trajectory, error_code)) { From 809f97121062532bbbf18d87bbe2ebf17bc62851 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 5/7] fix: handle last sample --- .../trajectory_functions.hpp | 3 +- .../src/trajectory_functions.cpp | 59 ++++++++++++++----- .../test/test_utils.cpp | 7 --- .../src/unittest_trajectory_generator_lin.cpp | 22 +++---- 4 files changed, 56 insertions(+), 35 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index 7f096d790d..ce5d0541c4 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -119,11 +119,10 @@ bool verifySampleJointLimits(const std::map& 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& time_samples, double time_step = 0.001); + std::vector& time_samples); /** * @brief Interpolates between two poses. diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 6ee776af6e..a053ba3a26 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -217,16 +217,17 @@ void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d& start_ bool pilz_industrial_motion_planner::computeTimeSamples(const KDL::Trajectory& trajectory, const interpolation::Params& interpolation_params, - std::vector& time_samples, double time_step) + std::vector& 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; @@ -240,6 +241,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 = @@ -255,20 +265,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: " @@ -278,6 +294,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) { @@ -302,12 +321,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; } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 48b1f01058..94d6233351 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -1300,13 +1300,6 @@ ::testing::AssertionResult testutils::hasTrapezoidVelocity(std::vector 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!"; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index e262e74bef..dfb792518b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -369,27 +369,27 @@ 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)); } /** From 4b52cbc4fb9e44fb5f766bf28c02727f2ba28121 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 6/7] fix: removes wrong changes --- .../trajectory_functions.hpp | 3 ++- .../src/trajectory_functions.cpp | 15 ++------------- ...ttest_trajectory_blender_transition_window.cpp | 1 - 3 files changed, 4 insertions(+), 15 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index ce5d0541c4..38c56d7d9a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -159,7 +159,8 @@ bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, const std::map& initial_joint_position, - std::vector time_samples, trajectory_msgs::msg::JointTrajectory& joint_trajectory, + const std::vector& time_samples, + trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); /** diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index a053ba3a26..acf2d527ba 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -354,7 +354,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( const planning_scene::PlanningSceneConstPtr& scene, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, std::vector time_samples, + const std::map& initial_joint_position, const std::vector& time_samples, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { @@ -366,7 +366,6 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // sample the trajectory and solve the inverse kinematics Eigen::Isometry3d pose_sample; - Eigen::Isometry3d pose_sample_last; std::map ik_solution_last, ik_solution, joint_velocity_last; ik_solution_last = initial_joint_position; for (const auto& item : ik_solution_last) @@ -388,17 +387,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( joint_trajectory.points.clear(); return false; } - // if (time_iter != time_samples.begin() && time_samples.size() > 1) - // { - // // Compute the forward kinematics of the IK solution to check the error - // double position_error = (pose_sample_last.translation() - pose_sample.translation()).norm(); - // double orientation_error = - // Eigen::AngleAxisd(pose_sample_last.rotation().transpose() * pose_sample.rotation()).angle(); - - // std::cout << "IK solution error - Position error: " << position_error - // << ", Orientation error: " << orientation_error << std::endl; - // } - // pose_sample_last = pose_sample; + if (time_iter != time_samples.begin()) { duration_current_sample = *time_iter - *(time_iter - 1); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index f176d445ee..55e7deffe0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -662,7 +662,6 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) geometry_msgs::msg::Pose waypoint_pose = tf2::toMsg(eigen_pose); // add scaled sine function - // This is causing the test to fail for high accelerations waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg); waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg); waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg); From 7c58746e8e70c214d3ac7a06989d54bb873c6d33 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:24 +0100 Subject: [PATCH 7/7] fix: removes wrong file --- kinematics.yaml | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 kinematics.yaml diff --git a/kinematics.yaml b/kinematics.yaml deleted file mode 100644 index c597dc1b75..0000000000 --- a/kinematics.yaml +++ /dev/null @@ -1,6 +0,0 @@ -manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 -