start migrating to common package - first example worked, second not yet

examples/moveit_examples/CMakeLists.txt

@@ -12,14 +12,26 @@ find_package(moveit_visual_tools REQUIRED)
 
 include_directories(include)
 
-add_executable(moveit_basic_planners_example src/moveit_basic_planners_example.cpp)
-ament_target_dependencies(moveit_basic_planners_example
+add_executable(iiqka_moveit_basic_planners_example src/iiqka/moveit_basic_planners_example.cpp)
+ament_target_dependencies(iiqka_moveit_basic_planners_example
   moveit_ros_planning_interface
   moveit_visual_tools
 )
 
-add_executable(moveit_collision_avoidance_example src/moveit_collision_avoidance_example.cpp)
-ament_target_dependencies(moveit_collision_avoidance_example
+add_executable(kss_moveit_basic_planners_example src/kss/moveit_basic_planners_example.cpp)
+ament_target_dependencies(kss_moveit_basic_planners_example
+  moveit_ros_planning_interface
+  moveit_visual_tools
+)
+
+add_executable(iiqka_moveit_collision_avoidance_example src/iiqka/moveit_collision_avoidance_example.cpp)
+ament_target_dependencies(iiqka_moveit_collision_avoidance_example
+  moveit_ros_planning_interface
+  moveit_visual_tools
+)
+
+add_executable(kss_moveit_collision_avoidance_example src/kss/moveit_collision_avoidance_example.cpp)
+ament_target_dependencies(kss_moveit_collision_avoidance_example
   moveit_ros_planning_interface
   moveit_visual_tools
 )
@@ -37,8 +49,10 @@ ament_target_dependencies(moveit_depalletizing_example
 )
 
 install(TARGETS
-  moveit_basic_planners_example
-  moveit_collision_avoidance_example
+  iiqka_moveit_basic_planners_example
+  kss_moveit_basic_planners_example
+  iiqka_moveit_collision_avoidance_example
+  kss_moveit_collision_avoidance_example
   moveit_constrained_planning_example
   moveit_depalletizing_example
   EXPORT export_${PROJECT_NAME}

examples/moveit_examples/include/moveit_examples/moveit_basic_planners_example.hpp

@@ -0,0 +1,94 @@
+// Copyright 2024 Gergely Kovács
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOVEIT_EXAMPLES__MOVEIT_BASIC_PLANNERS_EXAMPLE_HPP_
+#define MOVEIT_EXAMPLES__MOVEIT_BASIC_PLANNERS_EXAMPLE_HPP_
+
+#include <math.h>
+
+#include <memory>
+
+#include "moveit_examples/moveit_example.hpp"
+
+void moveItBasicPlannersExample(std::shared_ptr<MoveitExample> example_node,
+    const geometry_msgs::msg::Vector3 & standing_pose_coords,
+    const geometry_msgs::msg::Vector3 & lin_goal_coords,
+    const geometry_msgs::msg::Vector3 & collision_box_pos,
+    const geometry_msgs::msg::Vector3 & collision_box_size)
+{
+  example_node->initialize();
+  example_node->addBreakPoint();
+
+  // Add robot platform
+  example_node->addRobotPlatform();
+
+  // Pilz PTP planner
+  auto standing_pose =
+    Eigen::Isometry3d(Eigen::Translation3d(standing_pose_coords.x, standing_pose_coords.y, standing_pose_coords.z) * Eigen::Quaterniond::Identity());
+
+  auto planned_trajectory =
+    example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP");
+  if (planned_trajectory != nullptr)
+  {
+    example_node->drawTrajectory(*planned_trajectory);
+    example_node->addBreakPoint();
+    example_node->moveGroupInterface()->execute(*planned_trajectory);
+  }
+  example_node->addBreakPoint();
+
+  // Pilz LIN planner
+  auto cart_goal =
+    Eigen::Isometry3d(Eigen::Translation3d(lin_goal_coords.x, lin_goal_coords.y, lin_goal_coords.z) * Eigen::Quaterniond::Identity());
+  planned_trajectory =
+    example_node->planToPoint(cart_goal, "pilz_industrial_motion_planner", "LIN");
+  if (planned_trajectory != nullptr)
+  {
+    example_node->drawTrajectory(*planned_trajectory);
+    example_node->addBreakPoint();
+    example_node->moveGroupInterface()->execute(*planned_trajectory);
+  }
+  example_node->addBreakPoint();
+
+  // Add collision object
+  example_node->addCollisionBox(collision_box_pos, collision_box_size);
+  example_node->addBreakPoint();
+
+  // Try moving back with Pilz LIN
+  planned_trajectory =
+    example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "LIN");
+  if (planned_trajectory != nullptr)
+  {
+    example_node->drawTrajectory(*planned_trajectory);
+  }
+  else
+  {
+    example_node->drawTitle("Failed planning with Pilz LIN");
+  }
+  example_node->addBreakPoint();
+
+  // Try moving back with Pilz PTP
+  planned_trajectory =
+    example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP");
+  if (planned_trajectory != nullptr)
+  {
+    example_node->drawTrajectory(*planned_trajectory);
+  }
+  else
+  {
+    example_node->drawTitle("Failed planning with Pilz PTP");
+  }
+  example_node->addBreakPoint();
+}
+
+#endif  // MOVEIT_EXAMPLES__MOVEIT_BASIC_PLANNERS_EXAMPLE_HPP_

examples/moveit_examples/src/moveit_collision_avoidance_example.cpp → examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp

@@ -1,4 +1,4 @@
-// Copyright 2022 Áron Svastits
+// Copyright 2024 Gergely Kovács
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,42 +12,38 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#ifndef MOVEIT_EXAMPLES__MOVEIT_COLLISION_AVOIDANCE_EXAMPLE_HPP_
+#define MOVEIT_EXAMPLES__MOVEIT_COLLISION_AVOIDANCE_EXAMPLE_HPP_
+
 #include <math.h>
 #include <memory>
 
 #include "moveit_examples/moveit_example.hpp"
 
-int main(int argc, char * argv[])
+void moveItCollisionAvoidanceExample(std::shared_ptr<MoveitExample> example_node,
+    const std::vector<double> & start_coords,
+    const geometry_msgs::msg::Vector3 & standing_pose_coords,
+    const geometry_msgs::msg::Vector3 & collision_box_pos,
+    const geometry_msgs::msg::Vector3 & collision_box_size)
 {
-  // Setup
-  // Initialize ROS and create the Node
-  rclcpp::init(argc, argv);
-  auto const example_node = std::make_shared<MoveitExample>();
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(example_node);
-  std::thread([&executor]() { executor.spin(); }).detach();
-
   example_node->initialize();
 
   // Add robot platform
   example_node->addRobotPlatform();
 
   // Go to correct position for the example
-  auto init_trajectory = example_node->planToPosition(
-    std::vector<double>{0.3587, 0.3055, -1.3867, 0.0, -0.4896, -0.3587});
+  auto init_trajectory = example_node->planToPosition(start_coords);
   if (init_trajectory != nullptr)
   {
     example_node->moveGroupInterface()->execute(*init_trajectory);
   }
 
   // Add collision object
-  example_node->addCollisionBox(
-    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.125).y(0.15).z(0.5),
-    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(1.0).z(0.1));
+  example_node->addCollisionBox(collision_box_pos, collision_box_size);
   example_node->addBreakPoint();
 
   auto standing_pose =
-    Eigen::Isometry3d(Eigen::Translation3d(0.1, 0, 0.8) * Eigen::Quaterniond::Identity());
+    Eigen::Isometry3d(Eigen::Translation3d(standing_pose_coords.x, standing_pose_coords.y, standing_pose_coords.z) * Eigen::Quaterniond::Identity());
 
   // Plan with collision avoidance
   auto planned_trajectory =
@@ -58,8 +54,6 @@ int main(int argc, char * argv[])
     example_node->addBreakPoint();
     example_node->moveGroupInterface()->execute(*planned_trajectory);
   }
-
-  // Shutdown ROS
-  rclcpp::shutdown();
-  return 0;
 }
+
+#endif  // MOVEIT_EXAMPLES__MOVEIT_COLLISION_AVOIDANCE_EXAMPLE_HPP_

examples/moveit_examples/launch/moveit_planning_example.launch.py → examples/moveit_examples/launch/iiqka_moveit_planning_example.launch.py

@@ -58,8 +58,10 @@ def launch_setup(context, *args, **kwargs):
             get_package_share_directory("kuka_lbr_iisy_moveit_config")
             + f"/urdf/{robot_model.perform(context)}.srdf"
         )
-        .robot_description_kinematics(file_path="config/kinematics.yaml")
-        .trajectory_execution(file_path="config/moveit_controllers.yaml")
+        .robot_description_kinematics(file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
+            + f"/config/kinematics.yaml")
+        .trajectory_execution(file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
+            + f"/config/moveit_controllers.yaml")
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )

examples/moveit_examples/launch/moveit_rsi_planning_example.launch.py → examples/moveit_examples/launch/kss_moveit_planning_example.launch.py

@@ -107,7 +107,7 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     launch_arguments = []
     launch_arguments.append(DeclareLaunchArgument(
-        "robot_model", default_value="kr6_r700_sixx"))
+        "robot_model", default_value="kr6_r900_sixx"))
     launch_arguments.append(DeclareLaunchArgument(
         "namespace", default_value=""))
     launch_arguments.append(DeclareLaunchArgument("x", default_value="0"))

examples/moveit_examples/src/iiqka/moveit_basic_planners_example.cpp

@@ -0,0 +1,36 @@
+// Copyright 2024 Gergely Kovács
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "moveit_examples/moveit_basic_planners_example.hpp"
+
+int main(int argc, char * argv[])
+{
+  // Setup
+  // Initialize ROS and create the Node
+  rclcpp::init(argc, argv);
+  auto const example_node = std::make_shared<MoveitExample>();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(example_node);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  moveItBasicPlannersExample(example_node,
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0).z(0.8),
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.4).y(-0.15).z(0.55),
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.25).y(-0.075).z(0.675),
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0.4).z(0.1));
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}

examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp

@@ -0,0 +1,36 @@
+// Copyright 2022 Áron Svastits
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "moveit_examples/moveit_collision_avoidance_example.hpp"
+
+int main(int argc, char * argv[])
+{
+  // Setup
+  // Initialize ROS and create the Node
+  rclcpp::init(argc, argv);
+  auto const example_node = std::make_shared<MoveitExample>();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(example_node);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  moveItCollisionAvoidanceExample(example_node,
+      std::vector<double>{0.3587, 0.3055, -1.3867, 0.0, -0.4896, -0.3587},
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0).z(0.8),
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.125).y(0.15).z(0.5),
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(1.0).z(0.1));
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}

examples/moveit_examples/src/kss/moveit_basic_planners_example.cpp

@@ -0,0 +1,36 @@
+// Copyright 2024 Gergely Kovács
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "moveit_examples/moveit_basic_planners_example.hpp"
+
+int main(int argc, char * argv[])
+{
+  // Setup
+  // Initialize ROS and create the Node
+  rclcpp::init(argc, argv);
+  auto const example_node = std::make_shared<MoveitExample>();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(example_node);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  moveItBasicPlannersExample(example_node,
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0).z(1.2),
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.7).y(-0.15).z(0.75),
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.3).y(-0.075).z(1),
+    geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0.4).z(0.1));
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}

examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp

@@ -0,0 +1,36 @@
+// Copyright 2022 Áron Svastits
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "moveit_examples/moveit_collision_avoidance_example.hpp"
+
+int main(int argc, char * argv[])
+{
+  // Setup
+  // Initialize ROS and create the Node
+  rclcpp::init(argc, argv);
+  auto const example_node = std::make_shared<MoveitExample>();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(example_node);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  moveItCollisionAvoidanceExample(example_node,
+      std::vector<double>{0.2111, -0.9106, 0.0, -1.8677, 1.2075, -0.2111},
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0).z(1.2),
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.3).y(-0.075).z(1),
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(1.0).z(0.1));
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}