Skip to content

Commit fcff676

Browse files
MarcoMagriDevMarcoMagriDev
committed
feat: adds first draft of interpolation_parameters
1 parent fbdd8c5 commit fcff676

25 files changed

+147
-35
lines changed

‎moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt

+8-1
Original file line numberDiff line numberDiff line change
@@ -63,8 +63,14 @@ generate_parameter_library(
6363
src/cartesian_limits_parameters.yaml # path to input yaml file
6464
)
6565

66+
generate_parameter_library(
67+
interpolation_parameters # cmake target name for the parameter library
68+
src/interpolation_parameters.yaml # path to input yaml file
69+
)
70+
6671
add_library(planning_context_loader_base SHARED src/planning_context_loader.cpp)
67-
target_link_libraries(planning_context_loader_base cartesian_limits_parameters)
72+
target_link_libraries(planning_context_loader_base cartesian_limits_parameters
73+
interpolation_parameters)
6874
ament_target_dependencies(planning_context_loader_base
6975
${THIS_PACKAGE_INCLUDE_DEPENDS})
7076

@@ -148,6 +154,7 @@ pluginlib_export_plugin_description_file(
148154
install(
149155
TARGETS pilz_industrial_motion_planner
150156
cartesian_limits_parameters
157+
interpolation_parameters
151158
joint_limits_common
152159
planning_context_loader_base
153160
planning_context_loader_ptp

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
#include <memory>
4747

4848
#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
49+
#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
4950

5051
namespace pilz_industrial_motion_planner
5152
{
@@ -136,6 +137,9 @@ class CommandPlanner : public planning_interface::PlannerManager
136137
/// cartesian limit
137138
std::shared_ptr<cartesian_limits::ParamListener> param_listener_;
138139
cartesian_limits::Params params_;
140+
141+
// interpolation params
142+
std::shared_ptr<interpolation::ParamListener> interpolation_param_listener_;
139143
};
140144

141145
MOVEIT_CLASS_FORWARD(CommandPlanner);

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -58,11 +58,13 @@ class PlanningContextBase : public planning_interface::PlanningContext
5858
public:
5959
PlanningContextBase<GeneratorT>(const std::string& name, const std::string& group,
6060
const moveit::core::RobotModelConstPtr& model,
61-
const pilz_industrial_motion_planner::LimitsContainer& limits)
61+
const pilz_industrial_motion_planner::LimitsContainer& limits,
62+
const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
6263
: planning_interface::PlanningContext(name, group)
6364
, terminated_(false)
6465
, model_(model)
6566
, limits_(limits)
67+
, interpolation_param_listener_(interpolation_param_listener)
6668
, generator_(model, limits_, group)
6769
{
6870
}
@@ -112,6 +114,9 @@ class PlanningContextBase : public planning_interface::PlanningContext
112114
/// Joint limits to be used during planning
113115
pilz_industrial_motion_planner::LimitsContainer limits_;
114116

117+
/// Listener for interpolation parameters
118+
std::shared_ptr<interpolation::ParamListener> interpolation_param_listener_;
119+
115120
protected:
116121
GeneratorT generator_;
117122
};
@@ -126,7 +131,7 @@ void pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::solve(plan
126131
res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
127132
return;
128133
}
129-
generator_.generate(getPlanningScene(), request_, res);
134+
generator_.generate(getPlanningScene(), request_, res, interpolation_param_listener_->get_params());
130135
}
131136

132137
template <typename GeneratorT>

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -58,8 +58,10 @@ class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningConte
5858
{
5959
public:
6060
PlanningContextCIRC(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
61-
const pilz_industrial_motion_planner::LimitsContainer& limits)
62-
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorCIRC>(name, group, model, limits)
61+
const pilz_industrial_motion_planner::LimitsContainer& limits,
62+
const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
63+
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorCIRC>(name, group, model, limits,
64+
interpolation_param_listener)
6365
{
6466
}
6567
};

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -58,8 +58,10 @@ class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContex
5858
{
5959
public:
6060
PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
61-
const pilz_industrial_motion_planner::LimitsContainer& limits)
62-
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorLIN>(name, group, model, limits)
61+
const pilz_industrial_motion_planner::LimitsContainer& limits,
62+
const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
63+
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorLIN>(name, group, model, limits,
64+
interpolation_param_listener)
6365
{
6466
}
6567
};

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp

+12-1
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
#include <moveit/planning_interface/planning_interface.hpp>
4343

4444
#include <pilz_industrial_motion_planner/limits_container.hpp>
45+
#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
4546

4647
namespace pilz_industrial_motion_planner
4748
{
@@ -75,6 +76,13 @@ class PlanningContextLoader
7576
*/
7677
virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits);
7778

79+
/**
80+
* @brief Set the listener for interpolation parameters
81+
* @param param_listener
82+
* @return true on success, false otherwise
83+
*/
84+
virtual bool setInterpolationParamListener(const std::shared_ptr<interpolation::ParamListener>& param_listener);
85+
7886
/**
7987
* @brief Return the planning context
8088
* @param planning_context
@@ -112,6 +120,9 @@ class PlanningContextLoader
112120

113121
/// The robot model
114122
moveit::core::RobotModelConstPtr model_;
123+
124+
/// Listener for interpolation parameters
125+
std::shared_ptr<interpolation::ParamListener> interpolation_param_listener_;
115126
};
116127

117128
typedef std::shared_ptr<PlanningContextLoader> PlanningContextLoaderPtr;
@@ -123,7 +134,7 @@ bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr&
123134
{
124135
if (limits_set_ && model_set_)
125136
{
126-
planning_context = std::make_shared<T>(name, group, model_, limits_);
137+
planning_context = std::make_shared<T>(name, group, model_, limits_, interpolation_param_listener_);
127138
return true;
128139
}
129140
else

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -58,8 +58,10 @@ class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContex
5858
{
5959
public:
6060
PlanningContextPTP(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model,
61-
const pilz_industrial_motion_planner::LimitsContainer& limits)
62-
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPTP>(name, group, model, limits)
61+
const pilz_industrial_motion_planner::LimitsContainer& limits,
62+
const std::shared_ptr<interpolation::ParamListener>& interpolation_param_listener)
63+
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPTP>(name, group, model, limits,
64+
interpolation_param_listener)
6365
{
6466
}
6567
};

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848
#include <pilz_industrial_motion_planner/limits_container.hpp>
4949
#include <pilz_industrial_motion_planner/trajectory_functions.hpp>
5050
#include <pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp>
51+
#include <pilz_industrial_motion_planner/interpolation_parameters.hpp>
5152

5253
namespace pilz_industrial_motion_planner
5354
{
@@ -107,7 +108,8 @@ class TrajectoryGenerator
107108
* @param sampling_time: sampling time of the generate trajectory
108109
*/
109110
void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
110-
planning_interface::MotionPlanResponse& res, double sampling_time = 0.1);
111+
planning_interface::MotionPlanResponse& res,
112+
const interpolation::Params& interpolation_params = interpolation::Params());
111113

112114
protected:
113115
/**
@@ -157,7 +159,8 @@ class TrajectoryGenerator
157159

158160
virtual void plan(const planning_scene::PlanningSceneConstPtr& scene,
159161
const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
160-
double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0;
162+
const interpolation::Params& interpolation_params,
163+
trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0;
161164

162165
private:
163166
/**

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -89,7 +89,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator
8989
const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
9090

9191
void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
92-
const MotionPlanInfo& plan_info, double sampling_time,
92+
const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params,
9393
trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
9494

9595
/**

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator
7373
const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
7474

7575
void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
76-
const MotionPlanInfo& plan_info, double sampling_time,
76+
const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params,
7777
trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
7878

7979
/**

‎moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
8383
double acceleration_scaling_factor, double sampling_time);
8484

8585
void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
86-
const MotionPlanInfo& plan_info, double sampling_time,
86+
const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params,
8787
trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
8888

8989
private:

‎moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
interpolation:
2+
max_sample_time: {
3+
type: double,
4+
default_value: 0.1,
5+
description: "Max time interval between two consecutive samples",
6+
validation: {
7+
gt: [ 0.0 ]
8+
}
9+
}
10+
min_number_of_samples: {
11+
type: int,
12+
default_value: 0,
13+
description: "Minimum number of samples for the generated trajectory",
14+
validation: {
15+
gt_eq: [ 0.0 ]
16+
}
17+
}
18+
max_translation_interpolation_distance: {
19+
type: double,
20+
default_value: 10000000.0,
21+
description: "Max translation distance between two consecutive samples",
22+
validation: {
23+
gt: [ 0.0 ]
24+
}
25+
}
26+
max_rotation_interpolation_distance: {
27+
type: double,
28+
default_value: 10000000.0,
29+
description: "Max rotation distance between two consecutive samples",
30+
validation: {
31+
gt: [ 0.0 ]
32+
}
33+
}

‎moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,9 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
8080
std::make_shared<cartesian_limits::ParamListener>(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits");
8181
params_ = param_listener_->get_params();
8282

83+
interpolation_param_listener_ =
84+
std::make_shared<interpolation::ParamListener>(node, PARAM_NAMESPACE_LIMITS + ".interpolation");
85+
8386
// Load the planning context loader
8487
planner_context_loader_ = std::make_unique<pluginlib::ClassLoader<PlanningContextLoader>>(
8588
"pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader");
@@ -106,6 +109,7 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
106109

107110
loader_pointer->setLimits(limits);
108111
loader_pointer->setModel(model_);
112+
loader_pointer->setInterpolationParamListener(interpolation_param_listener_);
109113

110114
registerContextLoader(loader_pointer);
111115
}

‎moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp

+7
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,13 @@ bool pilz_industrial_motion_planner::PlanningContextLoader::setLimits(
5757
return true;
5858
}
5959

60+
bool pilz_industrial_motion_planner::PlanningContextLoader::setInterpolationParamListener(
61+
const std::shared_ptr<interpolation::ParamListener>& param_listener)
62+
{
63+
interpolation_param_listener_ = param_listener;
64+
return true;
65+
}
66+
6067
std::string pilz_industrial_motion_planner::PlanningContextLoader::getAlgorithm() const
6168
{
6269
return alg_;

‎moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext(
6565
{
6666
if (limits_set_ && model_set_)
6767
{
68-
planning_context = std::make_shared<PlanningContextCIRC>(name, group, model_, limits_);
68+
planning_context =
69+
std::make_shared<PlanningContextCIRC>(name, group, model_, limits_, interpolation_param_listener_);
6970
return true;
7071
}
7172
else

‎moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext(
6262
{
6363
if (limits_set_ && model_set_)
6464
{
65-
planning_context = std::make_shared<PlanningContextLIN>(name, group, model_, limits_);
65+
planning_context =
66+
std::make_shared<PlanningContextLIN>(name, group, model_, limits_, interpolation_param_listener_);
6667
return true;
6768
}
6869
else

‎moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext(
6161
{
6262
if (limits_set_ && model_set_)
6363
{
64-
planning_context = std::make_shared<PlanningContextPTP>(name, group, model_, limits_);
64+
planning_context =
65+
std::make_shared<PlanningContextPTP>(name, group, model_, limits_, interpolation_param_listener_);
6566
return true;
6667
}
6768
else

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -299,7 +299,8 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_fa
299299

300300
void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene,
301301
const planning_interface::MotionPlanRequest& req,
302-
planning_interface::MotionPlanResponse& res, double sampling_time)
302+
planning_interface::MotionPlanResponse& res,
303+
const interpolation::Params& interpolation_params)
303304
{
304305
RCLCPP_INFO_STREAM(getLogger(), "Generating " << req.planner_id << " trajectory...");
305306
rclcpp::Time planning_begin = clock_->now();
@@ -345,7 +346,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
345346
trajectory_msgs::msg::JointTrajectory joint_trajectory;
346347
try
347348
{
348-
plan(plan_info.start_scene, req, plan_info, sampling_time, joint_trajectory);
349+
plan(plan_info.start_scene, req, plan_info, interpolation_params, joint_trajectory);
349350
}
350351
catch (const MoveItErrorCodeException& ex)
351352
{

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

+10-3
Original file line numberDiff line numberDiff line change
@@ -200,7 +200,8 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
200200

201201
void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene,
202202
const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
203-
double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
203+
const interpolation::Params& interpolation_params,
204+
trajectory_msgs::msg::JointTrajectory& joint_trajectory)
204205
{
205206
std::unique_ptr<KDL::Path> cart_path(setPathCIRC(plan_info));
206207
std::unique_ptr<KDL::VelocityProfile> vel_profile(
@@ -215,9 +216,15 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr&
215216
moveit_msgs::msg::MoveItErrorCodes error_code;
216217
// sample the Cartesian trajectory and compute joint trajectory using inverse
217218
// kinematics
219+
auto cartesian_limits = planner_limits_.getCartesianLimits();
220+
auto st = std::min({ interpolation_params.max_sample_time,
221+
interpolation_params.max_translation_interpolation_distance /
222+
(cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor),
223+
interpolation_params.max_rotation_interpolation_distance /
224+
(cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) });
225+
RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", st);
218226
if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
219-
plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
220-
error_code))
227+
plan_info.link_name, plan_info.start_joint_position, st, joint_trajectory, error_code))
221228
{
222229
throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path",
223230
error_code.val);

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

+10-3
Original file line numberDiff line numberDiff line change
@@ -144,7 +144,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
144144

145145
void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,
146146
const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
147-
double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
147+
const interpolation::Params& interpolation_params,
148+
trajectory_msgs::msg::JointTrajectory& joint_trajectory)
148149
{
149150
// create Cartesian path for lin
150151
std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));
@@ -162,9 +163,15 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s
162163
moveit_msgs::msg::MoveItErrorCodes error_code;
163164
// sample the Cartesian trajectory and compute joint trajectory using inverse
164165
// kinematics
166+
auto cartesian_limits = planner_limits_.getCartesianLimits();
167+
auto st = std::min({ interpolation_params.max_sample_time,
168+
interpolation_params.max_translation_interpolation_distance /
169+
(cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor),
170+
interpolation_params.max_rotation_interpolation_distance /
171+
(cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) });
172+
RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", st);
165173
if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
166-
plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
167-
error_code))
174+
plan_info.link_name, plan_info.start_joint_position, st, joint_trajectory, error_code))
168175
{
169176
std::ostringstream os;
170177
os << "Failed to generate valid joint trajectory from the Cartesian path";

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -265,11 +265,12 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin
265265

266266
void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/,
267267
const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
268-
double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
268+
const interpolation::Params& interpolation_params,
269+
trajectory_msgs::msg::JointTrajectory& joint_trajectory)
269270
{
270271
// plan the ptp trajectory
271272
planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory,
272-
req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time);
273+
req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, interpolation_params.max_sample_time);
273274
}
274275

275276
} // namespace pilz_industrial_motion_planner

0 commit comments

Comments
 (0)