From bce4dd606767764e9e01dc38fd49433859c9a635 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: Fri, 26 Dec 2025 23:03:41 +1000 Subject: [PATCH] =?UTF-8?q?=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=20=D0=BF=D0=BE=D0=BB=D0=BD=D0=BE=D1=86=D0=B5=D0=BD=D0=BD?= =?UTF-8?q?=D1=8B=D0=B9=20=D0=BC=D0=BE=D0=B4=D1=83=D0=BB=D1=8C=20=D0=BF?= =?UTF-8?q?=D0=BB=D0=B0=D0=BD=D0=B8=D1=80=D0=BE=D0=B2=D0=B0=D0=BD=D0=B8?= =?UTF-8?q?=D1=8F=20=D1=82=D1=80=D0=B0=D0=B5=D0=BA=D1=82=D0=BE=D1=80=D0=BD?= =?UTF-8?q?=D1=8B=D1=85=20=D0=BF=D0=B5=D1=80=D0=B5=D0=BC=D0=B5=D1=89=D0=B5?= =?UTF-8?q?=D0=BD=D0=B8=D0=B9.=20=D0=9D=D0=B5=D0=BE=D0=B1=D1=85=D0=BE?= =?UTF-8?q?=D0=B4=D0=B8=D0=BC=D0=BE=20=D0=B5=D0=B3=D0=BE=20=D0=BE=D1=84?= =?UTF-8?q?=D0=BE=D1=80=D0=BC=D0=B8=D1=82=D1=8C=20=D0=BF=D1=80=D0=B0=D0=B2?= =?UTF-8?q?=D0=B8=D0=BB=D1=8C=D0=BD=D1=8B=D0=BC=20=D0=BE=D0=B1=D1=80=D0=B0?= =?UTF-8?q?=D0=B7=D0=BE=D0=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/digital_twin.launch.py | 4 +- src/iiwa_planning/CMakeLists.txt | 5 +- src/iiwa_planning/scripts/motion_planning.py | 128 ++++++++++++++++++ src/iiwa_utils/iiwa_utils/object_spawner.py | 3 + 4 files changed, 136 insertions(+), 4 deletions(-) create mode 100644 src/iiwa_planning/scripts/motion_planning.py diff --git a/src/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py index 02493b9..52b46b9 100644 --- a/src/iiwa_bringup/launch/digital_twin.launch.py +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -93,9 +93,9 @@ def _runtime_setup(context, *args, **kwatgs): # TODO: не забудь поменять правильное название и имя пакета moveit_py_node = Node( - name="moveit_py", + # name="motion_planning_node", package="iiwa_planning", - executable="motion_planning_test", + executable="motion_planning", output="both", parameters=[moveit_configs.to_dict()], ) diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt index 63725dc..03c64ec 100644 --- a/src/iiwa_planning/CMakeLists.txt +++ b/src/iiwa_planning/CMakeLists.txt @@ -16,9 +16,10 @@ include_directories(include) ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS - scripts/motion_planning_test.py + # scripts/motion_planning_test.py + scripts/motion_planning.py DESTINATION lib/${PROJECT_NAME} - RENAME motion_planning_test + RENAME motion_planning ) diff --git a/src/iiwa_planning/scripts/motion_planning.py b/src/iiwa_planning/scripts/motion_planning.py new file mode 100644 index 0000000..bd4cf0b --- /dev/null +++ b/src/iiwa_planning/scripts/motion_planning.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanningComponent +from geometry_msgs.msg import PoseStamped +from rclpy.executors import ExternalShutdownException +from tf_transformations import quaternion_from_euler + + +class MotionPlaning(Node): + def __init__(self): + super().__init__("motion_planning_node") + + self.moveit = MoveItPy(node_name="motion_planning_node") + self.robot_arm = 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("***********************************************************") + # self.robot_arm.set_start_state(configuration_name="home") + # self.robot_arm.set_goal_state(configuration_name="work") + # self.plan_end_execute() + + self.timer = self.create_timer(5.0, self._run_once) + + def _run_once(self): + self.timer.cancel() + self.handle_plan_execute_pose(0.677, 0.25, 0.21, 3.14, 0, 3.14) + + + def handle_plan_execute_pose(self, x, y, z, a, b, c): + + pose_goal = PoseStamped() + pose_goal.header.frame_id="base_link" + pose_goal.header.stamp = self.get_clock().now().to_msg() + pose_goal.pose.position.x=x + pose_goal.pose.position.y=y + pose_goal.pose.position.z=z + + xx, yy, zz, w = quaternion_from_euler(a, b, c) + + pose_goal.pose.orientation.x=xx + pose_goal.pose.orientation.y=yy + pose_goal.pose.orientation.z=zz + pose_goal.pose.orientation.w=w + + # Check collisions + with self.planning_scene_monitor.read_only() as scene: + + robot_state = scene.current_state + original_joint_positions = robot_state.get_joint_group_positions("iiwa_arm") + + ok_ik = robot_state.set_from_ik("iiwa_arm", pose_goal.pose, "link_ee") + robot_state.update() + + if not ok_ik: + self.get_logger().warn("***********************************************************") + self.get_logger().error("IK failed -> abort") + self.get_logger().warn("***********************************************************") + return + + robot_collision_status = scene.is_state_colliding( + robot_state=robot_state, + joint_model_group_name="iiwa_arm", + verbose=True + ) + if robot_collision_status: + self.get_logger().warn("***********************************************************") + self.get_logger().error("Goal in collision -> abort") + self.get_logger().warn("***********************************************************") + return + + self.get_logger().warn("***********************************************************") + self.get_logger().info(f"\nRobot is in collision: {robot_collision_status}\n") + self.get_logger().warn("***********************************************************") + + robot_state.set_joint_group_positions( + "iiwa_arm", + original_joint_positions, + ) + robot_state.update() + + # получить текущее состояние как изначальное для перемещения + self.robot_arm.set_start_state_to_current_state() + # self.robot_arm.set_start_state(configuration_name="home") + # self.robot_arm.set_goal_state(configuration_name="work") + + self.robot_arm.set_goal_state( + pose_stamped_msg=pose_goal, + pose_link="link_ee" + ) + self.plan_end_execute() + + + def plan_end_execute(self): + self.get_logger().warn("***********************************************************") + self.get_logger().info("Planning trajectory") + self.get_logger().warn("***********************************************************") + + plan_result = self.robot_arm.plan() + if plan_result: + self.get_logger().warn("***********************************************************") + self.get_logger().info("Executing plan") + self.get_logger().warn("***********************************************************") + robot_trajectory = plan_result.trajectory + self.moveit.execute(robot_trajectory, controllers=[]) + else: + self.get_logger().warn("***********************************************************") + self.get_logger().info("Planning failed") + self.get_logger().warn("***********************************************************") + + +def main(args=None): + try: + rclpy.init(args=args) + node = MotionPlaning() + rclpy.spin(node=node) + node.destroy_node() + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/iiwa_utils/iiwa_utils/object_spawner.py b/src/iiwa_utils/iiwa_utils/object_spawner.py index f7b0e42..17b841d 100644 --- a/src/iiwa_utils/iiwa_utils/object_spawner.py +++ b/src/iiwa_utils/iiwa_utils/object_spawner.py @@ -79,6 +79,9 @@ def main(args=None): rclpy.spin(node=node) except (KeyboardInterrupt, ExternalShutdownException): pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == "__main__":