Skip to content

Commit 2002fb7

Browse files
AimanHaidarAimanHaidar
authored
Feature: New POLYLINE command in Pilz planner for space trajectory generation (#3610)
1 parent 760277f commit 2002fb7

17 files changed

+944
-8
lines changed

‎moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -127,6 +127,17 @@ ament_target_dependencies(planning_context_loader_circ
127127
target_link_libraries(planning_context_loader_circ planning_context_loader_base
128128
joint_limits_common trajectory_generation_common)
129129

130+
add_library(
131+
planning_context_loader_polyline SHARED
132+
src/planning_context_loader_polyline.cpp
133+
src/trajectory_generator_polyline.cpp src/velocity_profile_atrap.cpp
134+
src/path_polyline_generator.cpp)
135+
ament_target_dependencies(planning_context_loader_polyline
136+
${THIS_PACKAGE_INCLUDE_DEPENDS})
137+
target_link_libraries(
138+
planning_context_loader_polyline planning_context_loader_base
139+
joint_limits_common trajectory_generation_common)
140+
130141
add_library(sequence_capability SHARED src/move_group_sequence_action.cpp
131142
src/move_group_sequence_service.cpp)
132143
ament_target_dependencies(sequence_capability ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -153,6 +164,7 @@ install(
153164
planning_context_loader_ptp
154165
planning_context_loader_lin
155166
planning_context_loader_circ
167+
planning_context_loader_polyline
156168
command_list_manager
157169
sequence_capability
158170
trajectory_generation_common

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

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2018 Pilz GmbH & Co. KG
5+
* Copyright (c) 2025 Aiman Haidar
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of Pilz GmbH & Co. KG nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*********************************************************************/
35+
36+
#pragma once
37+
38+
#include <kdl/path.hpp>
39+
#include <kdl/path_roundedcomposite.hpp>
40+
#include <kdl/rotational_interpolation_sa.hpp>
41+
#include <kdl/utilities/error.h>
42+
43+
#include <memory>
44+
45+
namespace pilz_industrial_motion_planner
46+
{
47+
/**
48+
* @brief Generator class for KDL::Path_RoundedComposite from different polyline path
49+
* representations
50+
*/
51+
class PathPolylineGenerator
52+
{
53+
public:
54+
/**
55+
* @brief set the path polyline from waypoints
56+
*
57+
*/
58+
static std::unique_ptr<KDL::Path> polylineFromWaypoints(const KDL::Frame& start_pose,
59+
const std::vector<KDL::Frame>& waypoints,
60+
KDL::RotationalInterpolation* rot_interpo, double smoothness,
61+
double eqradius);
62+
63+
/**
64+
* @brief compute the maximum rounding radius from KDL::Path_RoundedComosite
65+
* @param waypoints_: waypoints defining the path
66+
* @param smoothness: smoothness level [0..1] scaling the maximum blend radius
67+
* @return maximum blend radius
68+
*/
69+
static std::vector<KDL::Frame> filterWaypoints(const KDL::Frame& start_pose, const std::vector<KDL::Frame>& waypoints);
70+
static double computeBlendRadius(const std::vector<KDL::Frame>& waypoints_, double smoothness);
71+
static void checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3);
72+
73+
private:
74+
PathPolylineGenerator(){}; // no instantiation of this helper class!
75+
76+
static constexpr double MIN_SEGMENT_LENGTH{ 0.2e-3 };
77+
static constexpr double MIN_SMOOTHNESS{ 0.01 };
78+
static constexpr double MAX_SMOOTHNESS{ 0.99 };
79+
static constexpr double MIN_COLINEAR_NORM{ 1e-9 };
80+
};
81+
82+
class ErrorMotionPlanningColinearConsicutiveWaypoints : public KDL::Error_MotionPlanning
83+
{
84+
public:
85+
const char* Description() const override
86+
{
87+
return "Three collinear consecutive waypoints."
88+
" A Polyline Path cannot be created.";
89+
}
90+
int GetType() const override
91+
{
92+
return ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS;
93+
} // LCOV_EXCL_LINE
94+
95+
private:
96+
static constexpr int ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS{ 3104 };
97+
};
98+
99+
} // namespace pilz_industrial_motion_planner

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

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2018 Pilz GmbH & Co. KG
5+
* Copyright (c) 2025 Aiman Haidar
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of Pilz GmbH & Co. KG nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*********************************************************************/
35+
36+
#pragma once
37+
38+
#include <pilz_industrial_motion_planner/planning_context_loader.hpp>
39+
40+
#include <moveit/planning_interface/planning_interface.hpp>
41+
42+
namespace pilz_industrial_motion_planner
43+
{
44+
/**
45+
* @brief Plugin that can generate instances of PlanningContextPolyline.
46+
* Generates instances of PlanningContextPolyline.
47+
*/
48+
class PlanningContextLoaderPolyline : public PlanningContextLoader
49+
{
50+
public:
51+
PlanningContextLoaderPolyline();
52+
~PlanningContextLoaderPolyline() override;
53+
54+
/**
55+
* @brief return a instance of
56+
* pilz_industrial_motion_planner::PlanningContextPolyline
57+
* @param planning_context returned context
58+
* @param name
59+
* @param group
60+
* @return true on success, false otherwise
61+
*/
62+
bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name,
63+
const std::string& group) const override;
64+
};
65+
66+
typedef std::shared_ptr<PlanningContextLoaderPolyline> PlanningContextLoaderPolylinePtr;
67+
typedef std::shared_ptr<const PlanningContextLoaderPolyline> PlanningContextLoaderPolylineConstPtr;
68+
69+
} // namespace pilz_industrial_motion_planner

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

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2018 Pilz GmbH & Co. KG
5+
* Copyright (c) 2025 Aiman Haidar
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of Pilz GmbH & Co. KG nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*********************************************************************/
35+
36+
#pragma once
37+
38+
#include <pilz_industrial_motion_planner/limits_container.hpp>
39+
40+
#include <rclcpp/rclcpp.hpp>
41+
42+
#include <moveit/planning_interface/planning_interface.hpp>
43+
#include <moveit/planning_interface/planning_response.hpp>
44+
45+
#include <atomic>
46+
#include <thread>
47+
48+
#include <pilz_industrial_motion_planner/planning_context_base.hpp>
49+
#include <pilz_industrial_motion_planner/trajectory_generator_polyline.hpp>
50+
51+
namespace pilz_industrial_motion_planner
52+
{
53+
MOVEIT_CLASS_FORWARD(PlanningContext);
54+
55+
/**
56+
* @brief PlanningContext for obtaining Polyline trajectories
57+
*/
58+
class PlanningContextPolyline : public pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPolyline>
59+
{
60+
public:
61+
PlanningContextPolyline(const std::string& name, const std::string& group,
62+
const moveit::core::RobotModelConstPtr& model,
63+
const pilz_industrial_motion_planner::LimitsContainer& limits)
64+
: pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPolyline>(name, group, model, limits)
65+
{
66+
}
67+
};
68+
69+
} // namespace pilz_industrial_motion_planner

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

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,7 @@ class TrajectoryGenerator
122122
std::string link_name;
123123
Eigen::Isometry3d start_pose;
124124
Eigen::Isometry3d goal_pose;
125+
std::vector<Eigen::Isometry3d> waypoints;
125126
std::map<std::string, double> start_joint_position;
126127
std::map<std::string, double> goal_joint_position;
127128
std::pair<std::string, Eigen::Vector3d> circ_path_point;
@@ -139,6 +140,11 @@ class TrajectoryGenerator
139140
std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(double max_velocity_scaling_factor,
140141
double max_acceleration_scaling_factor,
141142
const std::unique_ptr<KDL::Path>& path) const;
143+
/**
144+
* @brief Set the max cartesian speed from motion request
145+
*/
146+
147+
void setMaxCartesianSpeed(const moveit_msgs::msg::MotionPlanRequest& req);
142148

143149
private:
144150
virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const;
@@ -270,6 +276,7 @@ class TrajectoryGenerator
270276
protected:
271277
const moveit::core::RobotModelConstPtr robot_model_;
272278
const pilz_industrial_motion_planner::LimitsContainer planner_limits_;
279+
double max_cartesian_speed_;
273280
static constexpr double MIN_SCALING_FACTOR{ 0.0001 };
274281
static constexpr double MAX_SCALING_FACTOR{ 1. };
275282
static constexpr double VELOCITY_TOLERANCE{ 1e-8 };

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

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2018 Pilz GmbH & Co. KG
5+
* Copyright (c) 2025 Aiman Haidar
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of Pilz GmbH & Co. KG nor the names of its
19+
* contributors may be used to endorse or promote products derived
20+
* from this software without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*********************************************************************/
35+
36+
#pragma once
37+
38+
#include <eigen3/Eigen/Eigen>
39+
#include <kdl/rotational_interpolation_sa.hpp>
40+
#include <moveit/planning_scene/planning_scene.hpp>
41+
42+
#include <pilz_industrial_motion_planner/trajectory_generator.hpp>
43+
#include <pilz_industrial_motion_planner/velocity_profile_atrap.hpp>
44+
45+
namespace pilz_industrial_motion_planner
46+
{
47+
// TODO date type of units
48+
49+
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE);
50+
51+
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
52+
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
53+
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoWaypointsSpecified, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
54+
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(ConsicutiveColinearWaypoints,
55+
moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
56+
57+
/**
58+
* @brief This class implements a polyline trajectory generator in Cartesian
59+
* space.
60+
* The Cartesian trajetory are based on trapezoid velocity profile.
61+
*/
62+
class TrajectoryGeneratorPolyline : public TrajectoryGenerator
63+
{
64+
public:
65+
/**
66+
* @brief Constructor of Polyline Trajectory Generator
67+
* @throw TrajectoryGeneratorInvalidLimitsException
68+
* @param model: robot model
69+
* @param planner_limits: limits in joint and Cartesian spaces
70+
*/
71+
TrajectoryGeneratorPolyline(const moveit::core::RobotModelConstPtr& robot_model,
72+
const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
73+
const std::string& group_name);
74+
75+
private:
76+
void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;
77+
78+
void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
79+
const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
80+
81+
void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
82+
const MotionPlanInfo& plan_info, double sampling_time,
83+
trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
84+
85+
/**
86+
* @brief construct a KDL::Path object for a Cartesian polyline path
87+
* @param start_pose: start pose of the path
88+
* @param waypoints: waypoints defining the path
89+
* @param smoothness_level: smoothness level for blending the waypoints
90+
* @return a unique pointer of the path object. null_ptr in case of an error.
91+
*/
92+
std::unique_ptr<KDL::Path> setPathPolyline(const Eigen::Affine3d& start_pose,
93+
const std::vector<Eigen::Isometry3d>& waypoints,
94+
double smoothness_level) const;
95+
};
96+
97+
} // namespace pilz_industrial_motion_planner

0 commit comments

Comments
 (0)