diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt
index e8420aec32..18610f07ae 100644
--- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt
+++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt
@@ -127,6 +127,17 @@ ament_target_dependencies(planning_context_loader_circ
 target_link_libraries(planning_context_loader_circ planning_context_loader_base
                       joint_limits_common trajectory_generation_common)
 
+add_library(
+  planning_context_loader_polyline SHARED
+  src/planning_context_loader_polyline.cpp
+  src/trajectory_generator_polyline.cpp src/velocity_profile_atrap.cpp
+  src/path_polyline_generator.cpp)
+ament_target_dependencies(planning_context_loader_polyline
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(
+  planning_context_loader_polyline planning_context_loader_base
+  joint_limits_common trajectory_generation_common)
+
 add_library(sequence_capability SHARED src/move_group_sequence_action.cpp
                                        src/move_group_sequence_service.cpp)
 ament_target_dependencies(sequence_capability ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -153,6 +164,7 @@ install(
           planning_context_loader_ptp
           planning_context_loader_lin
           planning_context_loader_circ
+          planning_context_loader_polyline
           command_list_manager
           sequence_capability
           trajectory_generation_common
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.hpp
new file mode 100644
index 0000000000..ce94879891
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.hpp
@@ -0,0 +1,99 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <kdl/path.hpp>
+#include <kdl/path_roundedcomposite.hpp>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <kdl/utilities/error.h>
+
+#include <memory>
+
+namespace pilz_industrial_motion_planner
+{
+/**
+ * @brief Generator class for KDL::Path_RoundedComposite from different polyline path
+ * representations
+ */
+class PathPolylineGenerator
+{
+public:
+  /**
+   * @brief set the path polyline from waypoints
+   *
+   */
+  static std::unique_ptr<KDL::Path> polylineFromWaypoints(const KDL::Frame& start_pose,
+                                                          const std::vector<KDL::Frame>& waypoints,
+                                                          KDL::RotationalInterpolation* rot_interpo, double smoothness,
+                                                          double eqradius);
+
+  /**
+   * @brief compute the maximum rounding radius from KDL::Path_RoundedComosite
+   * @param waypoints_: waypoints defining the path
+   * @param smoothness: smoothness level [0..1] scaling the maximum blend radius
+   * @return maximum blend radius
+   */
+  static std::vector<KDL::Frame> filterWaypoints(const KDL::Frame& start_pose, const std::vector<KDL::Frame>& waypoints);
+  static double computeBlendRadius(const std::vector<KDL::Frame>& waypoints_, double smoothness);
+  static void checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3);
+
+private:
+  PathPolylineGenerator(){};  // no instantiation of this helper class!
+
+  static constexpr double MIN_SEGMENT_LENGTH{ 0.2e-3 };
+  static constexpr double MIN_SMOOTHNESS{ 0.01 };
+  static constexpr double MAX_SMOOTHNESS{ 0.99 };
+  static constexpr double MIN_COLINEAR_NORM{ 1e-9 };
+};
+
+class ErrorMotionPlanningColinearConsicutiveWaypoints : public KDL::Error_MotionPlanning
+{
+public:
+  const char* Description() const override
+  {
+    return "Three collinear consecutive waypoints."
+           " A Polyline Path cannot be created.";
+  }
+  int GetType() const override
+  {
+    return ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS;
+  }  // LCOV_EXCL_LINE
+
+private:
+  static constexpr int ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS{ 3104 };
+};
+
+}  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.hpp
new file mode 100644
index 0000000000..13b7c38ef9
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.hpp
@@ -0,0 +1,69 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <pilz_industrial_motion_planner/planning_context_loader.hpp>
+
+#include <moveit/planning_interface/planning_interface.hpp>
+
+namespace pilz_industrial_motion_planner
+{
+/**
+ * @brief Plugin that can generate instances of PlanningContextPolyline.
+ * Generates instances of PlanningContextPolyline.
+ */
+class PlanningContextLoaderPolyline : public PlanningContextLoader
+{
+public:
+  PlanningContextLoaderPolyline();
+  ~PlanningContextLoaderPolyline() override;
+
+  /**
+   * @brief return a instance of
+   * pilz_industrial_motion_planner::PlanningContextPolyline
+   * @param planning_context returned context
+   * @param name
+   * @param group
+   * @return true on success, false otherwise
+   */
+  bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name,
+                   const std::string& group) const override;
+};
+
+typedef std::shared_ptr<PlanningContextLoaderPolyline> PlanningContextLoaderPolylinePtr;
+typedef std::shared_ptr<const PlanningContextLoaderPolyline> PlanningContextLoaderPolylineConstPtr;
+
+}  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.hpp
new file mode 100644
index 0000000000..b9c668c0e6
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.hpp
@@ -0,0 +1,69 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <pilz_industrial_motion_planner/limits_container.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <moveit/planning_interface/planning_interface.hpp>
+#include <moveit/planning_interface/planning_response.hpp>
+
+#include <atomic>
+#include <thread>
+
+#include <pilz_industrial_motion_planner/planning_context_base.hpp>
+#include <pilz_industrial_motion_planner/trajectory_generator_polyline.hpp>
+
+namespace pilz_industrial_motion_planner
+{
+MOVEIT_CLASS_FORWARD(PlanningContext);
+
+/**
+ * @brief PlanningContext for obtaining Polyline trajectories
+ */
+class PlanningContextPolyline : public pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPolyline>
+{
+public:
+  PlanningContextPolyline(const std::string& name, const std::string& group,
+                          const moveit::core::RobotModelConstPtr& model,
+                          const pilz_industrial_motion_planner::LimitsContainer& limits)
+    : pilz_industrial_motion_planner::PlanningContextBase<TrajectoryGeneratorPolyline>(name, group, model, limits)
+  {
+  }
+};
+
+}  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp
index c17bcc8be3..71db4f3ea1 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp
@@ -122,6 +122,7 @@ class TrajectoryGenerator
     std::string link_name;
     Eigen::Isometry3d start_pose;
     Eigen::Isometry3d goal_pose;
+    std::vector<Eigen::Isometry3d> waypoints;
     std::map<std::string, double> start_joint_position;
     std::map<std::string, double> goal_joint_position;
     std::pair<std::string, Eigen::Vector3d> circ_path_point;
@@ -139,6 +140,11 @@ class TrajectoryGenerator
   std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(double max_velocity_scaling_factor,
                                                                      double max_acceleration_scaling_factor,
                                                                      const std::unique_ptr<KDL::Path>& path) const;
+  /**
+   * @brief Set the max cartesian speed from motion request
+   */
+
+  void setMaxCartesianSpeed(const moveit_msgs::msg::MotionPlanRequest& req);
 
 private:
   virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const;
@@ -270,6 +276,7 @@ class TrajectoryGenerator
 protected:
   const moveit::core::RobotModelConstPtr robot_model_;
   const pilz_industrial_motion_planner::LimitsContainer planner_limits_;
+  double max_cartesian_speed_;
   static constexpr double MIN_SCALING_FACTOR{ 0.0001 };
   static constexpr double MAX_SCALING_FACTOR{ 1. };
   static constexpr double VELOCITY_TOLERANCE{ 1e-8 };
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.hpp
new file mode 100644
index 0000000000..fce646b8fe
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.hpp
@@ -0,0 +1,97 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <eigen3/Eigen/Eigen>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+
+#include <pilz_industrial_motion_planner/trajectory_generator.hpp>
+#include <pilz_industrial_motion_planner/velocity_profile_atrap.hpp>
+
+namespace pilz_industrial_motion_planner
+{
+// TODO date type of units
+
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE);
+
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoWaypointsSpecified, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(ConsicutiveColinearWaypoints,
+                                   moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
+
+/**
+ * @brief This class implements a polyline trajectory generator in Cartesian
+ * space.
+ * The Cartesian trajetory are based on trapezoid velocity profile.
+ */
+class TrajectoryGeneratorPolyline : public TrajectoryGenerator
+{
+public:
+  /**
+   * @brief Constructor of Polyline Trajectory Generator
+   * @throw TrajectoryGeneratorInvalidLimitsException
+   * @param model: robot model
+   * @param planner_limits: limits in joint and Cartesian spaces
+   */
+  TrajectoryGeneratorPolyline(const moveit::core::RobotModelConstPtr& robot_model,
+                              const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                              const std::string& group_name);
+
+private:
+  void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;
+
+  void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                             const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
+
+  void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+            const MotionPlanInfo& plan_info, double sampling_time,
+            trajectory_msgs::msg::JointTrajectory& joint_trajectory) override;
+
+  /**
+   * @brief construct a KDL::Path object for a Cartesian polyline path
+   * @param start_pose: start pose of the path
+   * @param waypoints: waypoints defining the path
+   * @param smoothness_level: smoothness level for blending the waypoints
+   * @return a unique pointer of the path object. null_ptr in case of an error.
+   */
+  std::unique_ptr<KDL::Path> setPathPolyline(const Eigen::Affine3d& start_pose,
+                                             const std::vector<Eigen::Isometry3d>& waypoints,
+                                             double smoothness_level) const;
+};
+
+}  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml
index 4c95fcb49e..0a07d45f01 100644
--- a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml
+++ b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml
@@ -20,4 +20,11 @@
       <description>Loader for CIRC Context</description>
     </class>
   </library>
+  <library path="planning_context_loader_polyline">
+    <class name="pilz_industrial_motion_planner/PlanningContextLoaderPolyline"
+           type="pilz_industrial_motion_planner::PlanningContextLoaderPolyline"
+           base_class_type="pilz_industrial_motion_planner::PlanningContextLoader">
+      <description>Loader for Polyline Context</description>
+    </class>
+  </library>
 </class_libraries>
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp
new file mode 100644
index 0000000000..5ed0caaddd
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp
@@ -0,0 +1,145 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <pilz_industrial_motion_planner/path_polyline_generator.hpp>
+
+namespace pilz_industrial_motion_planner
+{
+std::unique_ptr<KDL::Path> PathPolylineGenerator::polylineFromWaypoints(const KDL::Frame& start_pose,
+                                                                        const std::vector<KDL::Frame>& waypoints,
+                                                                        KDL::RotationalInterpolation* rot_interpo,
+                                                                        double smoothness, double eqradius)
+{
+  std::vector<KDL::Frame> filtered_waypoints = filterWaypoints(start_pose, waypoints);
+  double blend_radius = computeBlendRadius(filtered_waypoints, smoothness);
+
+  KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(blend_radius, eqradius, rot_interpo);
+
+  for (const auto& waypoint : filtered_waypoints)
+  {
+    composite_path->Add(waypoint);
+  }
+
+  composite_path->Finish();
+  return std::unique_ptr<KDL::Path>(composite_path);
+}
+
+std::vector<KDL::Frame> PathPolylineGenerator::filterWaypoints(const KDL::Frame& start_pose,
+                                                               const std::vector<KDL::Frame>& waypoints)
+{
+  std::vector<KDL::Frame> filtered_waypoints = {};
+
+  // index for the last add point
+  filtered_waypoints.push_back(start_pose);
+  int last_added_point_indx = -1;  // -1 for the start_pose
+
+  // the following is to remove very close waypoints
+  // to avoid issues in KDL::Path_RoundedComposite like throwing Not_Fesible exceptions
+  // to get the last added point in the rounded composite path
+  auto last_point = [&]() { return last_added_point_indx != -1 ? waypoints[last_added_point_indx].p : start_pose.p; };
+  // distance between start pose and first waypoint
+  double dist;
+
+  // add points and skip the points which are too close to each other
+  for (const auto& waypoint : waypoints)
+  {
+    dist = (last_point() - waypoint.p).Norm();
+    if (dist > MIN_SEGMENT_LENGTH)
+    {
+      filtered_waypoints.push_back(waypoint);
+      ++last_added_point_indx;
+    }
+  }
+  return filtered_waypoints;
+}
+double PathPolylineGenerator::computeBlendRadius(const std::vector<KDL::Frame>& waypoints_, double smoothness)
+{
+  double max_allowed_radius = std::numeric_limits<double>::infinity();
+
+  auto pose_distance = [](const KDL::Frame& p1, const KDL::Frame& p2) { return (p1.p - p2.p).Norm(); };
+
+  // to calculate the angle between the two segments of p2
+  auto segment_angle = [](const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3) {
+    KDL::Vector v1 = p2.p - p1.p;
+    KDL::Vector v2 = p2.p - p3.p;
+
+    double norm_product = v1.Norm() * v2.Norm();
+    if (norm_product < MIN_SEGMENT_LENGTH * MIN_SEGMENT_LENGTH)
+      return 0.0;  // avoid division by zero
+
+    double cos_theta = KDL::dot(v1, v2) / norm_product;
+    cos_theta = std::clamp(cos_theta, -1.0, 1.0);
+
+    return std::acos(cos_theta);
+  };
+
+  for (size_t i = 1; i + 1 < waypoints_.size(); ++i)
+  {
+    double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]);
+    double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]);
+    checkConsecutiveColinearWaypoints(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]);
+    if (dist1 < MIN_SEGMENT_LENGTH || dist2 < MIN_SEGMENT_LENGTH)
+    {
+      continue;
+    }
+
+    // The maximum feasible radius for this junction
+    double theta = segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]);
+    double local_max_radius = std::abs(std::tan(theta / 2.0)) * std::min(dist1 / 2.0, dist2 / 2.0);
+
+    // track the tightest constraint
+    // due to KDL::Path_RoundedComposite don't support changing radius
+    if (local_max_radius < max_allowed_radius)
+      max_allowed_radius = local_max_radius;
+  }
+
+  max_allowed_radius *= std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS);
+
+  return max_allowed_radius;
+}
+void PathPolylineGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2,
+                                                              const KDL::Frame& p3)
+{
+  KDL::Vector v1 = p2.p - p1.p;
+  KDL::Vector v2 = p3.p - p2.p;
+
+  KDL::Vector cross_product = v1 * v2;
+  if (cross_product.Norm() < MIN_COLINEAR_NORM)
+  {
+    throw ErrorMotionPlanningColinearConsicutiveWaypoints();
+  }
+}
+
+}  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp
new file mode 100644
index 0000000000..67ef057275
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp
@@ -0,0 +1,85 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <pilz_industrial_motion_planner/planning_context_loader_polyline.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <pilz_industrial_motion_planner/planning_context_base.hpp>
+#include <pilz_industrial_motion_planner/planning_context_polyline.hpp>
+#include <moveit/utils/logger.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("moveit.planners.pilz.planning_context_loader.polyline");
+}
+}  // namespace
+
+pilz_industrial_motion_planner::PlanningContextLoaderPolyline::PlanningContextLoaderPolyline()
+{
+  alg_ = "POLYLINE";
+}
+
+pilz_industrial_motion_planner::PlanningContextLoaderPolyline::~PlanningContextLoaderPolyline()
+{
+}
+
+bool pilz_industrial_motion_planner::PlanningContextLoaderPolyline::loadContext(
+    planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const
+{
+  if (limits_set_ && model_set_)
+  {
+    planning_context = std::make_shared<PlanningContextPolyline>(name, group, model_, limits_);
+    return true;
+  }
+  else
+  {
+    if (!limits_set_)
+    {
+      RCLCPP_ERROR_STREAM(getLogger(),
+                          "Limits are not defined. Cannot load planning context. Call setLimits loadContext");
+    }
+    if (!model_set_)
+    {
+      RCLCPP_ERROR_STREAM(getLogger(), "Robot model was not set");
+    }
+    return false;
+  }
+}
+
+PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderPolyline,
+                       pilz_industrial_motion_planner::PlanningContextLoader)
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
index 6f17fbc3a2..4c5c40d747 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
@@ -283,7 +283,7 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_fa
                                                   const std::unique_ptr<KDL::Path>& path) const
 {
   std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Trap>(
-      max_velocity_scaling_factor * planner_limits_.getCartesianLimits().max_trans_vel,
+      max_velocity_scaling_factor * max_cartesian_speed_,
       max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().max_trans_acc);
 
   if (path->PathLength() > std::numeric_limits<double>::epsilon())  // avoid division by zero
@@ -380,4 +380,18 @@ TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::Planni
   }
 }
 
+void TrajectoryGenerator::setMaxCartesianSpeed(const moveit_msgs::msg::MotionPlanRequest& req)
+{
+  if (req.max_cartesian_speed > 0.0 && !req.cartesian_speed_limited_link.empty())
+  {
+    max_cartesian_speed_ = req.max_cartesian_speed;
+    RCLCPP_INFO(getLogger(), "received max_cartesian_speed: %f", max_cartesian_speed_);
+  }
+  else
+  {
+    max_cartesian_speed_ = planner_limits_.getCartesianLimits().max_trans_vel;
+    RCLCPP_INFO(getLogger(), "using default max_cartesian_speed: %f", max_cartesian_speed_);
+  }
+}
+
 }  // namespace pilz_industrial_motion_planner
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 388542daf3..744374132d 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
@@ -202,7 +202,10 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr&
                                    const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                    double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
 {
+  // set pilz cartesian limits for each item
+  setMaxCartesianSpeed(req);
   std::unique_ptr<KDL::Path> cart_path(setPathCIRC(plan_info));
+  // create velocity profile
   std::unique_ptr<KDL::VelocityProfile> vel_profile(
       cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path));
 
@@ -241,8 +244,7 @@ std::unique_ptr<KDL::Path> TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlan
   // The KDL::Path implementation chooses the motion with the longer duration
   // (translation vs. rotation)
   // and uses eqradius as scaling factor between the distances.
-  double eqradius =
-      planner_limits_.getCartesianLimits().max_trans_vel / planner_limits_.getCartesianLimits().max_rot_vel;
+  double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel;
 
   try
   {
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 77ed726c0e..c0c542fa76 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
@@ -146,9 +146,10 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s
                                   const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                   double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
 {
+  // set pilz cartesian limits for each item
+  setMaxCartesianSpeed(req);
   // create Cartesian path for lin
   std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));
-
   // create velocity profile
   std::unique_ptr<KDL::VelocityProfile> vp(
       cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));
@@ -180,8 +181,7 @@ std::unique_ptr<KDL::Path> TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affin
   KDL::Frame kdl_start_pose, kdl_goal_pose;
   tf2::transformEigenToKDL(start_pose, kdl_start_pose);
   tf2::transformEigenToKDL(goal_pose, kdl_goal_pose);
-  double eqradius =
-      planner_limits_.getCartesianLimits().max_trans_vel / planner_limits_.getCartesianLimits().max_rot_vel;
+  double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel;
   KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();
 
   return std::unique_ptr<KDL::Path>(
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp
new file mode 100644
index 0000000000..9a14a6020a
--- /dev/null
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp
@@ -0,0 +1,223 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Pilz GmbH & Co. KG
+ *  Copyright (c) 2025 Aiman Haidar
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <pilz_industrial_motion_planner/trajectory_generator_polyline.hpp>
+#include <pilz_industrial_motion_planner/path_polyline_generator.hpp>
+
+#include <pilz_industrial_motion_planner/tip_frame_getter.hpp>
+
+#include <cassert>
+#include <sstream>
+#include <time.h>
+#include <moveit/robot_state/conversions.hpp>
+#include <kdl/path_roundedcomposite.hpp>
+#include <kdl/trajectory_segment.hpp>
+#include <kdl/utilities/error.h>
+// TODO: Remove conditional include when released to all active distros.
+#if __has_include(<tf2/convert.hpp>)
+#include <tf2/convert.hpp>
+#else
+#include <tf2/convert.h>
+#endif
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <moveit/utils/logger.hpp>
+
+namespace pilz_industrial_motion_planner
+{
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("moveit.planners.pilz.trajectory_generator.polyline");
+}
+}  // namespace
+TrajectoryGeneratorPolyline::TrajectoryGeneratorPolyline(const moveit::core::RobotModelConstPtr& robot_model,
+                                                         const LimitsContainer& planner_limits,
+                                                         const std::string& /*group_name*/)
+  : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
+{
+  planner_limits_.printCartesianLimits();
+}
+
+void TrajectoryGeneratorPolyline::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                                                        const planning_interface::MotionPlanRequest& req,
+                                                        TrajectoryGenerator::MotionPlanInfo& info) const
+{
+  RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");
+
+  info.group_name = req.group_name;
+  moveit::core::RobotState robot_state = scene->getCurrentState();
+
+  std::string frame_id;
+
+  info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
+  if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
+      req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
+  {
+    RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of "
+                             "goal. Use model frame as default");
+    frame_id = robot_model_->getModelFrame();
+  }
+  else
+  {
+    frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
+  }
+
+  // Add the path waypoints
+  for (const auto& pc : req.path_constraints.position_constraints)
+  {
+    Eigen::Isometry3d waypoint;
+    tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint);
+    waypoint = scene->getFrameTransform(frame_id) * waypoint;
+    info.waypoints.push_back(waypoint);
+  }
+  // goal constraint is just the final pose
+  info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
+  frame_id = robot_model_->getModelFrame();
+
+  // check goal pose ik before Cartesian motion plan starts
+  std::map<std::string, double> ik_solution;
+  if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
+                     ik_solution))
+  {
+    std::ostringstream os;
+    os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
+    throw LinInverseForGoalIncalculable(os.str());
+  }
+
+  // Ignored return value because at this point the function should always
+  // return 'true'.
+  computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
+}
+
+void TrajectoryGeneratorPolyline::plan(const planning_scene::PlanningSceneConstPtr& scene,
+                                       const planning_interface::MotionPlanRequest& req,
+                                       const MotionPlanInfo& plan_info, double sampling_time,
+                                       trajectory_msgs::msg::JointTrajectory& joint_trajectory)
+{
+  // set pilz cartesian limits for each item
+  setMaxCartesianSpeed(req);
+  // create Cartesian POLYLINE path
+  std::unique_ptr<KDL::Path> path;
+  try
+  {
+    path = setPathPolyline(plan_info.start_pose, plan_info.waypoints, req.smoothness_level);
+  }
+  catch (const KDL::Error_MotionPlanning& e)
+  {
+    RCLCPP_ERROR(getLogger(), "Motion planning error: %s", e.Description());
+    int code = e.GetType();
+    std::ostringstream os;
+    if (code == 3102 || code == 3103)
+    {
+      os << "zero distance between two points";
+    }
+    else if (code == 3104)
+    {
+      os << "waypoints specified in path constraints have three consicutive colinear points";
+    }
+    else if (code == 3105 || code == 3106)
+    {
+      os << "rounding circle of a point is bigger than the distance with one of the neighbor points";
+    }
+    else if (code == 3001 || code == 3002)
+    {
+      os << "the rounding radius is lower than KDL::epsilon. use bigger smoothness or resample your waypoints";
+    }
+    throw ConsicutiveColinearWaypoints(os.str());
+  }
+  // create velocity profile
+  std::unique_ptr<KDL::VelocityProfile> vp(
+      cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));
+
+  // combine path and velocity profile into Cartesian trajectory
+  // with the third parameter set to false, KDL::Trajectory_Segment does not
+  // take
+  // the ownship of Path and Velocity Profile
+  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
+  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,
+                               error_code))
+  {
+    std::ostringstream os;
+    os << "Failed to generate valid joint trajectory from the Cartesian path";
+    throw LinTrajectoryConversionFailure(os.str(), error_code.val);
+  }
+}
+
+std::unique_ptr<KDL::Path> TrajectoryGeneratorPolyline::setPathPolyline(const Eigen::Affine3d& start_pose,
+                                                                        const std::vector<Eigen::Isometry3d>& waypoints,
+                                                                        double smoothness_level) const
+{
+  RCLCPP_DEBUG(getLogger(), "Set Cartesian path for POLYLINE command.");
+
+  KDL::Frame kdl_start_pose;
+  tf2::transformEigenToKDL(start_pose, kdl_start_pose);
+  // transform waypoints to KDL frames
+  std::vector<KDL::Frame> kdl_waypoints;
+  for (const auto& waypoint : waypoints)
+  {
+    KDL::Frame kdl_waypoint;
+    tf2::transformEigenToKDL(waypoint, kdl_waypoint);
+    kdl_waypoints.push_back(kdl_waypoint);
+  }
+
+  RCLCPP_INFO_STREAM(getLogger(), "Transformed waypoints number: " << kdl_waypoints.size());
+
+  double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel;
+  KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();
+
+  return PathPolylineGenerator::polylineFromWaypoints(kdl_start_pose, kdl_waypoints, rot_interpo, smoothness_level,
+                                                      eqradius);
+}
+
+void TrajectoryGeneratorPolyline::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const
+{
+  if (req.path_constraints.position_constraints.size() < 2)
+  {
+    std::ostringstream os;
+    os << "waypoints specified in path constraints is less than 2 for POLYLINE motion.";
+    throw NoWaypointsSpecified(os.str());
+  }
+}
+}  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
index 867c7f6587..41dffd6de2 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
+++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
@@ -44,7 +44,7 @@ to be moved out of singularities.
   2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner`
   3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image)
 ![moveit_1](img/acceptance_test_lin_img1.png)
-  4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute".
+  4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity polyline position. Click on "plan and execute".
 ![moveit_2](img/acceptance_test_lin_img2.png)
   5. The motion planning widget (lower left part of moveit) choose LIN in the dropdown below "Trapezoidal Command Planner" (see image)
 ![moveit_1](img/acceptance_test_lin_img3.png)
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp
index 33f9e2dc31..f8c0886a78 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp
@@ -128,7 +128,7 @@ TEST_F(CommandPlannerTest, ObtainLoadedPlanningAlgorithms)
   // Check for the algorithms
   std::vector<std::string> algs;
   planner_instance_->getPlanningAlgorithms(algs);
-  ASSERT_EQ(3u, algs.size()) << "Found more or less planning algorithms as expected! Found:"
+  ASSERT_EQ(4u, algs.size()) << "Found more or less planning algorithms as expected! Found:"
                              << ::testing::PrintToString(algs);
 
   // Collect the algorithms, check for uniqueness
@@ -141,6 +141,7 @@ TEST_F(CommandPlannerTest, ObtainLoadedPlanningAlgorithms)
   ASSERT_TRUE(algs_set.find("LIN") != algs_set.end());
   ASSERT_TRUE(algs_set.find("PTP") != algs_set.end());
   ASSERT_TRUE(algs_set.find("CIRC") != algs_set.end());
+  ASSERT_TRUE(algs_set.find("POLYLINE") != algs_set.end());
 }
 
 /**
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
index 33fdb0af0e..31a8645c1a 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp
@@ -734,6 +734,59 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
   checkCircResult(req, res);
 }
 
+/**
+ * @brief Set a proper max cartesian speed and check that trajectory is generated
+ */
+TEST_F(TrajectoryGeneratorCIRCTest, cartesianSpeedLimitProper)
+{
+  // set max cartesian speed to a positive value
+  moveit_msgs::msg::MotionPlanRequest circ_req{ tdp_->getCircJointInterimCart("circ3_interim").toRequest() };
+  circ_req.max_cartesian_speed = 0.5;
+  circ_req.cartesian_speed_limited_link = "test_link";
+
+  // generate circ trajectory
+  planning_interface::MotionPlanResponse res;
+  circ_->generate(planning_scene_, circ_req, res);
+  EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  // checkCircResult(circ_req, res);
+}
+
+/**
+ * @brief Set a cartesian speed limit to less than or equal zero or send request
+ * without setting any speed limit
+ *
+ * Expected Results:
+ *   1. Use the default max_trans_vel from limits container
+ */
+TEST_F(TrajectoryGeneratorCIRCTest, cartesianSpeedLimitLessEqualZero)
+{
+  // construct motion plan request
+  moveit_msgs::msg::MotionPlanRequest circ_req{ tdp_->getCircJointInterimCart("circ3_interim").toRequest() };
+  // Case 1: don't set any max cartesian speed (set to zero)
+  // generate circ trajectory
+  planning_interface::MotionPlanResponse res1;
+  circ_->generate(this->planning_scene_, circ_req, res1);
+  EXPECT_TRUE(res1.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  // checkCircResult(circ_req, res1);
+
+  // Case 2: set max cartesian speed to negative value
+  circ_req.max_cartesian_speed = -1.0;
+
+  // generate circ trajectory
+  planning_interface::MotionPlanResponse res2;
+  circ_->generate(this->planning_scene_, circ_req, res2);
+  EXPECT_TRUE(res2.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  // checkCircResult(circ_req, res2);
+  //  Case 3: set max cartesian speed to zero
+  circ_req.max_cartesian_speed = 0.0;
+
+  // generate circ trajectory
+  planning_interface::MotionPlanResponse res3;
+  circ_->generate(this->planning_scene_, circ_req, res3);
+  EXPECT_TRUE(res3.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  // checkCircResult(circ_req, res3);
+}
+
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);
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 6f049ed9a4..2117c21b09 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
@@ -431,6 +431,59 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints)
   EXPECT_TRUE(checkLinResponse(lin_cart_req, res));
 }
 
+/**
+ * @brief Set a proper max cartesian speed and check that trajectory is generated
+ */
+TEST_F(TrajectoryGeneratorLINTest, cartesianSpeedLimitProper)
+{
+  // set max cartesian speed to a positive value
+  moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() };
+  lin_cart_req.max_cartesian_speed = 0.5;
+  lin_cart_req.cartesian_speed_limited_link = "test_link";
+
+  // generate lin trajectory
+  planning_interface::MotionPlanResponse res;
+  lin_->generate(planning_scene_, lin_cart_req, res);
+  EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  EXPECT_TRUE(checkLinResponse(lin_cart_req, res));
+}
+
+/**
+ * @brief Set a cartesian speed limit to less than or equal zero or send request
+ * without setting any speed limit
+ *
+ * Expected Results:
+ *   1. Use the default max_trans_vel from limits container
+ */
+TEST_F(TrajectoryGeneratorLINTest, cartesianSpeedLimitLessEqualZero)
+{
+  // construct motion plan request
+  moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() };
+  // Case 1: don't set any max cartesian speed (set to zero)
+  // generate lin trajectory
+  planning_interface::MotionPlanResponse res1;
+  lin_->generate(this->planning_scene_, lin_cart_req, res1);
+  EXPECT_TRUE(res1.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  EXPECT_TRUE(checkLinResponse(lin_cart_req, res1));
+
+  // Case 2: set max cartesian speed to negative value
+  lin_cart_req.max_cartesian_speed = -1.0;
+
+  // generate lin trajectory
+  planning_interface::MotionPlanResponse res2;
+  lin_->generate(this->planning_scene_, lin_cart_req, res2);
+  EXPECT_TRUE(res2.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  EXPECT_TRUE(checkLinResponse(lin_cart_req, res2));
+  // Case 3: set max cartesian speed to zero
+  lin_cart_req.max_cartesian_speed = 0.0;
+
+  // generate lin trajectory
+  planning_interface::MotionPlanResponse res3;
+  lin_->generate(this->planning_scene_, lin_cart_req, res3);
+  EXPECT_TRUE(res3.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  EXPECT_TRUE(checkLinResponse(lin_cart_req, res3));
+}
+
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);