From 032935dec98f5bea414bc1e6a1abf0699649ae12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Sun, 5 Apr 2026 12:22:30 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D1=8B=20=D0=BF=D0=B0=D1=80=D0=B0=D0=BC=D0=B5=D1=82=D1=80?= =?UTF-8?q?=D1=8B=20=D0=BF=D0=BB=D0=B0=D0=BD=D0=B8=D1=80=D0=BE=D0=B2=D0=B0?= =?UTF-8?q?=D0=BD=D0=B8=D1=8F=20=D0=B8=20=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=D1=8B=20=D0=BD=D0=BE=D0=B2=D1=8B=D0=B5=20?= =?UTF-8?q?=D1=84=D0=B0=D0=B9=D0=BB=D1=8B=20=D0=B4=D0=BB=D1=8F=20=D0=BC?= =?UTF-8?q?=D0=BE=D0=B4=D1=83=D0=BB=D1=8F=20=D0=BF=D0=BB=D0=B0=D0=BD=D0=B8?= =?UTF-8?q?=D1=80=D0=BE=D0=B2=D0=B0=D0=BD=D0=B8=D1=8F=20=D0=B4=D0=B2=D0=B8?= =?UTF-8?q?=D0=B6=D0=B5=D0=BD=D0=B8=D1=8F,=20=D0=B2=D0=BA=D0=BB=D1=8E?= =?UTF-8?q?=D1=87=D0=B0=D1=8F=20C++=20=D0=B8=20Python=20=D1=80=D0=B5=D0=B0?= =?UTF-8?q?=D0=BB=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D0=B8.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/iiwa_config/config/moveit/moveit_cpp.yaml | 2 +- src/iiwa_description/urdf/iiwa7.urdf | 295 ++++++++++++++++++ src/iiwa_description/urdf/joints.xacro | 21 +- src/iiwa_description/urdf/macros.xacro | 5 +- src/iiwa_description/urdf/params.xacro | 18 +- src/iiwa_planning/CMakeLists.txt | 23 +- src/iiwa_planning/scripts/motion_planning.py | 3 +- src/iiwa_planning/src/motion_planning_cpp.cpp | 58 ++++ 8 files changed, 404 insertions(+), 21 deletions(-) create mode 100644 src/iiwa_description/urdf/iiwa7.urdf create mode 100644 src/iiwa_planning/src/motion_planning_cpp.cpp diff --git a/src/iiwa_config/config/moveit/moveit_cpp.yaml b/src/iiwa_config/config/moveit/moveit_cpp.yaml index d9bf720..13c34c4 100644 --- a/src/iiwa_config/config/moveit/moveit_cpp.yaml +++ b/src/iiwa_config/config/moveit/moveit_cpp.yaml @@ -13,7 +13,7 @@ planning_pipelines: plan_request_params: planning_attempts: 1 planning_pipeline: ompl - max_velocity_scaling_factor: 1.0 + max_velocity_scaling_factor: 0.5 max_acceleration_scaling_factor: 1.0 ompl_rrtc: # Namespace for individual plan request diff --git a/src/iiwa_description/urdf/iiwa7.urdf b/src/iiwa_description/urdf/iiwa7.urdf new file mode 100644 index 0000000..289d447 --- /dev/null +++ b/src/iiwa_description/urdf/iiwa7.urdf @@ -0,0 +1,295 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + webots_ros2_control::Ros2ControlSystem + + + + + + 0 + + + + + + 0 + + + + + + 0 + + + + + + 0 + + + + + + 0 + + + + + + 0 + + + + + + 0 + + + + diff --git a/src/iiwa_description/urdf/joints.xacro b/src/iiwa_description/urdf/joints.xacro index c2440cc..978b2f1 100644 --- a/src/iiwa_description/urdf/joints.xacro +++ b/src/iiwa_description/urdf/joints.xacro @@ -12,43 +12,50 @@ + lower="-2.97" upper="2.97" effort="500" + velocity="1.71" damping="0.5"/> + lower="-2.09" upper="2.09" effort="500" + velocity="1.71" damping="0.5"/> + lower="-2.97" upper="2.97" effort="500" + velocity="1.75" damping="0.5"/> + lower="-2.09" upper="2.09" effort="500" + velocity="2.27" damping="0.5"/> + lower="-2.97" upper="2.97" effort="500" + velocity="2.44" damping="0.5"/> + lower="-2.09" upper="2.09" effort="500" + velocity="3.14" damping="0.5"/> + lower="-3.05" upper="3.05" effort="500" + velocity="3.14" damping="0.5"/> diff --git a/src/iiwa_description/urdf/macros.xacro b/src/iiwa_description/urdf/macros.xacro index ad86164..ec8f866 100644 --- a/src/iiwa_description/urdf/macros.xacro +++ b/src/iiwa_description/urdf/macros.xacro @@ -30,14 +30,15 @@ - + + - \ No newline at end of file + diff --git a/src/iiwa_description/urdf/params.xacro b/src/iiwa_description/urdf/params.xacro index 5a5e7db..483b3ce 100644 --- a/src/iiwa_description/urdf/params.xacro +++ b/src/iiwa_description/urdf/params.xacro @@ -2,14 +2,14 @@ - - - - - - - - + + + + + + + + @@ -20,4 +20,4 @@ - \ No newline at end of file + diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt index 03c64ec..4c65cc5 100644 --- a/src/iiwa_planning/CMakeLists.txt +++ b/src/iiwa_planning/CMakeLists.txt @@ -10,11 +10,32 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(motion_planning_cpp src/motion_planning_cpp.cpp) +target_include_directories(motion_planning_cpp PUBLIC include) + +ament_target_dependencies(motion_planning_cpp + rclcpp + moveit_ros_planning_interface + moveit_core + moveit_ros_planning + moveit_msgs + geometry_msgs +) -include_directories(include) ament_python_install_package(${PROJECT_NAME}) +install(TARGETS + motion_planning_cpp + DESTINATION lib/${PROJECT_NAME} +) + install(PROGRAMS # scripts/motion_planning_test.py scripts/motion_planning.py diff --git a/src/iiwa_planning/scripts/motion_planning.py b/src/iiwa_planning/scripts/motion_planning.py index bd4cf0b..4a4ec04 100644 --- a/src/iiwa_planning/scripts/motion_planning.py +++ b/src/iiwa_planning/scripts/motion_planning.py @@ -14,9 +14,10 @@ class MotionPlaning(Node): super().__init__("motion_planning_node") self.moveit = MoveItPy(node_name="motion_planning_node") - self.robot_arm = self.moveit.get_planning_component("iiwa_arm") + self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm") self.planning_scene_monitor = self.moveit.get_planning_scene_monitor() + self.get_logger().warn("***********************************************************") self.get_logger().info("MotionPlanning node is ready...") self.get_logger().warn("***********************************************************") diff --git a/src/iiwa_planning/src/motion_planning_cpp.cpp b/src/iiwa_planning/src/motion_planning_cpp.cpp new file mode 100644 index 0000000..330763c --- /dev/null +++ b/src/iiwa_planning/src/motion_planning_cpp.cpp @@ -0,0 +1,58 @@ +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +class MotionPlanning: public rclcpp::Node +{ + public: + MotionPlanning() : Node("motion_planning_node", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + { + RCLCPP_INFO(get_logger(), "***************"); + RCLCPP_INFO(get_logger(), "MotionPlaning node created"); + RCLCPP_INFO(get_logger(), "***************"); + + timer_ = create_wall_timer( + std::chrono::milliseconds(200), + std::bind(&MotionPlanning::runOnce, this) + ); + + } + + private: + void runOnce() + { + timer_->cancel(); + + } + + private: + rclcpp::TimerBase::SharedPtr timer_; + + const std::string planning_group_ = "iiwa_arm"; + const std::string base_frame_ = "base_link"; + const std::string ee_link = "link_ee"; + + +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}