change from axis positions to points, restructure constrained_plannin…

…g examples

examples/moveit_examples/CMakeLists.txt

@@ -36,14 +36,20 @@ ament_target_dependencies(kss_moveit_collision_avoidance_example
   moveit_visual_tools
 )
 
-add_executable(moveit_constrained_planning_example src/moveit_constrained_planning_example.cpp)
-ament_target_dependencies(moveit_constrained_planning_example
+add_executable(iiqka_moveit_constrained_planning_example src/iiqka/moveit_constrained_planning_example.cpp)
+ament_target_dependencies(iiqka_moveit_constrained_planning_example
   moveit_ros_planning_interface
   moveit_visual_tools
 )
 
-add_executable(moveit_depalletizing_example src/moveit_depalletizing_example.cpp)
-ament_target_dependencies(moveit_depalletizing_example
+add_executable(kss_moveit_constrained_planning_example src/kss/moveit_constrained_planning_example.cpp)
+ament_target_dependencies(kss_moveit_constrained_planning_example
+  moveit_ros_planning_interface
+  moveit_visual_tools
+)
+
+add_executable(iiqka_moveit_depalletizing_example src/iiqka/moveit_depalletizing_example.cpp)
+ament_target_dependencies(iiqka_moveit_depalletizing_example
   moveit_ros_planning_interface
   moveit_visual_tools
 )
@@ -53,8 +59,9 @@ install(TARGETS
   kss_moveit_basic_planners_example
   iiqka_moveit_collision_avoidance_example
   kss_moveit_collision_avoidance_example
-  moveit_constrained_planning_example
-  moveit_depalletizing_example
+  iiqka_moveit_constrained_planning_example
+  kss_moveit_constrained_planning_example
+  iiqka_moveit_depalletizing_example
   EXPORT export_${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )

examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp

@@ -21,7 +21,7 @@
 #include "moveit_examples/moveit_example.hpp"
 
 void moveItCollisionAvoidanceExample(std::shared_ptr<MoveitExample> example_node,
-    const std::vector<double> & start_coords,
+    const geometry_msgs::msg::Vector3 & 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)
@@ -32,7 +32,9 @@ void moveItCollisionAvoidanceExample(std::shared_ptr<MoveitExample> example_node
   example_node->addRobotPlatform();
 
   // Go to correct position for the example
-  auto init_trajectory = example_node->planToPosition(start_coords);
+  auto start_pos =
+    Eigen::Isometry3d(Eigen::Translation3d(start_coords.x, start_coords.y, start_coords.z) * Eigen::Quaterniond::Identity());
+  auto init_trajectory = example_node->planToPoint(start_pos);
   if (init_trajectory != nullptr)
   {
     example_node->moveGroupInterface()->execute(*init_trajectory);

examples/moveit_examples/src/moveit_constrained_planning_example.cpp → examples/moveit_examples/include/moveit_examples/moveit_constrained_planning_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,41 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#ifndef MOVEIT_EXAMPLES__MOVEIT_CONSTRAINED_PLANNING_EXAMPLE_HPP_
+#define MOVEIT_EXAMPLES__MOVEIT_CONSTRAINED_PLANNING_EXAMPLE_HPP_
+
 #include <math.h>
 #include <memory>
 
 #include "moveit_examples/moveit_example.hpp"
 
-int main(int argc, char * argv[])
+void moveItConstrainedPlanningExample(std::shared_ptr<MoveitExample> example_node,
+    const geometry_msgs::msg::Vector3 & start_coords,
+    const geometry_msgs::msg::Vector3 & end_goal,
+    const geometry_msgs::msg::Vector3 & collision_box_pos,
+    const geometry_msgs::msg::Vector3 & collision_box_size,
+    double constraint_tolerance)
 {
-  // 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.0017, -2.096, 1.514, 0.0012, -0.9888, -0.0029});
+  auto start_pos =
+    Eigen::Isometry3d(Eigen::Translation3d(start_coords.x, start_coords.y, start_coords.z) * Eigen::Quaterniond::Identity());
+  auto init_trajectory = example_node->planToPoint(start_pos);
   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 cart_goal =
-    Eigen::Isometry3d(Eigen::Translation3d(0.4, -0.15, 0.55) * Eigen::Quaterniond::Identity());
+    Eigen::Isometry3d(Eigen::Translation3d(end_goal.x, end_goal.y, end_goal.z) * Eigen::Quaterniond::Identity());
 
   geometry_msgs::msg::Quaternion q;
   q.x = 0;
@@ -57,18 +56,16 @@ int main(int argc, char * argv[])
 
   example_node->moveGroupInterface()->setPlanningTime(30.0);
 
-  example_node->setOrientationConstraint(q);
+  example_node->setOrientationConstraint(q, constraint_tolerance);
   // Plan with collision avoidance
   auto planned_trajectory =
-    example_node->planToPointUntilSuccess(cart_goal, "ompl", "RRTkConfigDefault");
+    example_node->planToPointUntilSuccess(cart_goal, "ompl", "RRTConnectkConfigDefault");
   if (planned_trajectory != nullptr)
   {
     example_node->drawTrajectory(*planned_trajectory);
     example_node->addBreakPoint();
     example_node->moveGroupInterface()->execute(*planned_trajectory);
   }
-
-  // Shutdown ROS
-  rclcpp::shutdown();
-  return 0;
 }
+
+#endif  // MOVEIT_EXAMPLES__MOVEIT_CONSTRAINED_PLANNING_EXAMPLE_HPP_

examples/moveit_examples/include/moveit_examples/moveit_example.hpp

@@ -255,16 +255,16 @@ class MoveitExample : public rclcpp::Node
     planning_scene_diff_publisher_->publish(planning_scene);
   }
 
-  void setOrientationConstraint(const geometry_msgs::msg::Quaternion & orientation)
+  void setOrientationConstraint(const geometry_msgs::msg::Quaternion & orientation, double tolerance)
   {
     moveit_msgs::msg::OrientationConstraint orientation_constraint;
     moveit_msgs::msg::Constraints constraints;
     orientation_constraint.header.frame_id = move_group_interface_->getPlanningFrame();
     orientation_constraint.link_name = move_group_interface_->getEndEffectorLink();
     orientation_constraint.orientation = orientation;
-    orientation_constraint.absolute_x_axis_tolerance = 0.2;
-    orientation_constraint.absolute_y_axis_tolerance = 0.2;
-    orientation_constraint.absolute_z_axis_tolerance = 0.2;
+    orientation_constraint.absolute_x_axis_tolerance = tolerance;
+    orientation_constraint.absolute_y_axis_tolerance = tolerance;
+    orientation_constraint.absolute_z_axis_tolerance = tolerance;
     orientation_constraint.weight = 1.0;
 
     constraints.orientation_constraints.emplace_back(orientation_constraint);

examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp

@@ -25,10 +25,10 @@ int main(int argc, char * argv[])
   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.4).y(-0.15).z(0.55),
       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));
+      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.8).z(0.1));
 
   // Shutdown ROS
   rclcpp::shutdown();

examples/moveit_examples/src/iiqka/moveit_constrained_planning_example.cpp

@@ -0,0 +1,37 @@
+// 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_constrained_planning_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();
+
+  moveItConstrainedPlanningExample(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.8).z(0.1),
+      0.2);
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}

examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp

@@ -25,10 +25,10 @@ int main(int argc, char * argv[])
   std::thread([&executor]() { executor.spin(); }).detach();
 
   moveItCollisionAvoidanceExample(example_node,
-      std::vector<double>{0.2111, -0.9106, 1.2075, 0.0, -1.8677,  -0.2111},
+      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.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));
+      geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.1).y(0.8).z(0.1));
 
   // Shutdown ROS
   rclcpp::shutdown();

examples/moveit_examples/src/kss/moveit_constrained_planning_example.cpp

@@ -0,0 +1,37 @@
+// 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_constrained_planning_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();
+
+  moveItConstrainedPlanningExample(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.8).z(0.1),
+      0.2);
+
+  // Shutdown ROS
+  rclcpp::shutdown();
+  return 0;
+}