From 1a65f48c030584b962415982d99543ab9667dd6c 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: Wed, 24 Dec 2025 21:14:32 +1000 Subject: [PATCH] =?UTF-8?q?=D0=B8=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B0=20=D0=BD=D0=B0=D0=B7=D0=B2=D0=B0=D0=BD=D0=B8?= =?UTF-8?q?=D0=B5=20=D0=B3=D1=80=D1=83=D0=BF=D0=BF=D1=8B=20=D1=83=D0=BF?= =?UTF-8?q?=D1=80=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=D0=B8=D1=8F=20=D0=BE=D1=81?= =?UTF-8?q?=D1=8F=D0=BC=D0=B8,=20=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=20=D1=84=D0=B0=D0=B9=D0=BB=20=D0=BA=D0=BE=D0=BD=D1=84?= =?UTF-8?q?=D0=B8=D0=B3=D1=83=D0=B0=D1=86=D0=B8=D0=B8=20=D0=B4=D0=BB=D1=8F?= =?UTF-8?q?=20=D1=83=D0=BF=D1=80=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=D0=B8=D1=8F?= =?UTF-8?q?=20=D0=BE=D1=81=D1=8F=D0=BC=D0=B8=20=D1=87=D0=B5=D1=80=D0=B5?= =?UTF-8?q?=D0=B7=20api=20moveit,=20=D0=B0=20=D1=82=D0=B0=D0=BA=D0=B6?= =?UTF-8?q?=D0=B5=20=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=20?= =?UTF-8?q?=D0=BF=D1=80=D0=B8=D0=BC=D0=B5=D1=80=20=D0=BD=D0=B0=20=D1=8F?= =?UTF-8?q?=D0=B7=D1=8B=D0=BA=D0=B5=20python?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 2 + src/iiwa_bringup/config/iiwa7.srdf | 6 +- src/iiwa_bringup/config/kinematics.yaml | 2 +- src/iiwa_bringup/config/motion_planing.yaml | 43 +++ src/iiwa_bringup/config/rviz_iiwa.rviz | 274 ++++++++++++------ .../launch/digital_twin.launch.py | 17 +- .../motion_planing_test.py | 87 ++++++ src/iiwa_object_spawner/setup.py | 3 +- 8 files changed, 340 insertions(+), 94 deletions(-) create mode 100644 src/iiwa_bringup/config/motion_planing.yaml create mode 100644 src/iiwa_object_spawner/iiwa_object_spawner/motion_planing_test.py diff --git a/README.md b/README.md index 6320bca..1dcd090 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,8 @@ sudo apt install -y ros-${ROS_DISTRO}-webots-ros2 \ ros-${ROS_DISTRO}-ros2-control \ ros-${ROS_DISTRO}-ros2-controllers \ ros-${ROS_DISTRO}-moveit \ + ros-${ROS_DISTRO}-moveit-py + ``` Установка moveit2 (внимательно проверяй) diff --git a/src/iiwa_bringup/config/iiwa7.srdf b/src/iiwa_bringup/config/iiwa7.srdf index a640dd5..edd2e51 100644 --- a/src/iiwa_bringup/config/iiwa7.srdf +++ b/src/iiwa_bringup/config/iiwa7.srdf @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + @@ -26,7 +26,7 @@ - + diff --git a/src/iiwa_bringup/config/kinematics.yaml b/src/iiwa_bringup/config/kinematics.yaml index 2f6bb96..8490cd6 100644 --- a/src/iiwa_bringup/config/kinematics.yaml +++ b/src/iiwa_bringup/config/kinematics.yaml @@ -1,4 +1,4 @@ -arm: +iiwa_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/src/iiwa_bringup/config/motion_planing.yaml b/src/iiwa_bringup/config/motion_planing.yaml new file mode 100644 index 0000000..d9bf720 --- /dev/null +++ b/src/iiwa_bringup/config/motion_planing.yaml @@ -0,0 +1,43 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +ompl_rrtc: # Namespace for individual plan request + plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp + planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem + planning_pipeline: ompl # Name of the pipeline that is being used + planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline + max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned + +pilz_lin: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "PTP" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 0.8 + +chomp_planner: + plan_request_params: + planning_attempts: 1 + planning_pipeline: chomp + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 \ No newline at end of file diff --git a/src/iiwa_bringup/config/rviz_iiwa.rviz b/src/iiwa_bringup/config/rviz_iiwa.rviz index c6b18a5..110d571 100644 --- a/src/iiwa_bringup/config/rviz_iiwa.rviz +++ b/src/iiwa_bringup/config/rviz_iiwa.rviz @@ -6,10 +6,8 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /RobotModel1 - - /RobotModel1/Mass Properties1 Splitter Ratio: 0.5 - Tree Height: 834 + Tree Height: 166 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -49,84 +47,188 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7_ee: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: iiwa_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true Value: true - Visual Enabled: true + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -169,7 +271,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.324988842010498 + Distance: 3.753772020339966 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -184,18 +286,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7203982472419739 + Pitch: 0.6753981113433838 Target Frame: Value: Orbit (rviz) - Yaw: 5.598599910736084 + Yaw: 5.888577938079834 Saved: ~ Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false Displays: collapsed: false - Height: 1128 + Height: 846 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000003ccfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000003cc000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003ccfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000003cc000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000023200fffffffb0000000800540069006d006501000000000000045000000000000000000000050f000003cc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c1000002b2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c00000130000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004000fffffffbffffffff01000001720000017c0000017100ffffff000000010000010f000002b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000002b2000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023200fffffffb0000000800540069006d00650100000000000004500000000000000000000001d4000002b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -204,6 +310,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 0 + Width: 1200 + X: 414 + Y: 160 diff --git a/src/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py index 49692c4..bf155af 100644 --- a/src/iiwa_bringup/launch/digital_twin.launch.py +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -96,10 +96,7 @@ def _runtime_setup(context, *args, **kwatgs): .joint_limits(file_path=joint_limits_yaml) .pilz_cartesian_limits(file_path=pilz_limits_yaml) .trajectory_execution(file_path=moveit_controllers_yaml) - .planning_pipelines( - pipelines=["ompl", "stomp", "pilz_industrial_motion_planner"], - default_planning_pipeline="pilz_industrial_motion_planner", - ) + .moveit_cpp(file_path="/home/daniel/dev/ros2_iiwa7/src/iiwa_bringup/config/motion_planing.yaml") # TODO: сделать подстановочным значением .to_moveit_configs() ) @@ -113,6 +110,15 @@ def _runtime_setup(context, *args, **kwatgs): ], ) + # TODO: не забудь поменять правильное название и имя пакета + moveit_py_node = Node( + name="moveit_py", + package="iiwa_object_spawner", + executable="motion_planning_test", + output="both", + parameters=[moveit_configs.to_dict()], + ) + rviz_config = PathJoinSubstitution( [FindPackageShare(PACKAGE), "config", "rviz_iiwa.rviz"] ) @@ -140,7 +146,8 @@ def _runtime_setup(context, *args, **kwatgs): setup += [rsp_node, webots_launch, - move_group, + move_group, + moveit_py_node, rviz_launch, shutdown_on_rviz_exit ] diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/motion_planing_test.py b/src/iiwa_object_spawner/iiwa_object_spawner/motion_planing_test.py new file mode 100644 index 0000000..144845a --- /dev/null +++ b/src/iiwa_object_spawner/iiwa_object_spawner/motion_planing_test.py @@ -0,0 +1,87 @@ +import time + +# generic ros libraries +import rclpy +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy +from rclpy.impl.rcutils_logger import RcutilsLogger +from rclpy.logging import get_logger + + +def plan_and_execute(robot: MoveItPy, + planning_component, + logger: RcutilsLogger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0): + logger.info("Planning trajectory") + + if multi_plan_parameters is not None: + plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters) + elif single_plan_parameters is not None: + plan_result = planning_component.plan( + single_plan_parameters=single_plan_parameters + ) + else: + plan_result = planning_component.plan() + + # execute the plan + if plan_result: + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + + time.sleep(sleep_time) + + +def main(): + rclpy.init() + logger = get_logger("moveit_py.pose_goal") + + iiwa = MoveItPy(node_name="moveit_py") + iiwa_arm = iiwa.get_planning_component("iiwa_arm") + logger.info("MoveItPy instance created") + + iiwa_arm.set_start_state(configuration_name="ready") + iiwa_arm.set_goal_state(configuration_name="extended") + + plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0) + + robot_model = iiwa.get_robot_model() + robot_state = RobotState(robot_model) + + # randomize the robot state + robot_state.set_to_random_positions() + + # set plan start state to current state + iiwa_arm.set_start_state_to_current_state() + + # set goal state to the initialized robot state + logger.info("Set goal state to the initialized robot state") + iiwa_arm.set_goal_state(robot_state=robot_state) + + # plan to goal + plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0) + + # set plan start state to current state + iiwa_arm.set_start_state_to_current_state() + + # set pose goal with PoseStamped message + from geometry_msgs.msg import PoseStamped + + pose_goal = PoseStamped() + pose_goal.header.frame_id = "base_link" + pose_goal.pose.orientation.w = 1.0 + pose_goal.pose.position.x = 0.28 + pose_goal.pose.position.y = -0.2 + pose_goal.pose.position.z = 0.5 + iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="link7") + + # plan to goal + plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/iiwa_object_spawner/setup.py b/src/iiwa_object_spawner/setup.py index ec91923..2f18014 100644 --- a/src/iiwa_object_spawner/setup.py +++ b/src/iiwa_object_spawner/setup.py @@ -19,7 +19,8 @@ setup( license='Apache-2.0', entry_points={ 'console_scripts': [ - 'object_spawner = iiwa_object_spawner.object_spawner:main' + 'object_spawner = iiwa_object_spawner.object_spawner:main', + "motion_planning_test = iiwa_object_spawner.motion_planing_test:main" ], }, )