исправлена название группы управления осями, добавле файл конфигуации для управления осями через api moveit, а также добавлен пример на языке python

2025-12-24 21:14:32 +10:00
parent 7637a215d3
commit 1a65f48c03
8 changed files with 340 additions and 94 deletions
@@ -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 (внимательно проверяй)
@@ -5,7 +5,7 @@
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="arm">
+    <group name="iiwa_arm">
        <joint name="joint1"/>
        <joint name="joint2"/>
        <joint name="joint3"/>
@@ -16,7 +16,7 @@
    </group>

    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="home" group="arm">
+    <group_state name="home" group="iiwa_arm">
        <joint name="joint1" value="0"/>
        <joint name="joint2" value="0"/>
        <joint name="joint3" value="0"/>
@@ -26,7 +26,7 @@
        <joint name="joint7" value="0"/>
    </group_state>

-    <group_state name="work" group="arm">
+    <group_state name="work" group="iiwa_arm">
        <joint name="joint1" value="0"/>
        <joint name="joint2" value="0"/>
        <joint name="joint3" value="0"/>
@@ -1,4 +1,4 @@
-arm:
+iiwa_arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.0050000000000000001
  kinematics_solver_timeout: 0.0050000000000000001
@@ -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
@@ -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: <Fixed 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: <Fixed 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
@@ -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"]
    )
@@ -141,6 +147,7 @@ def _runtime_setup(context, *args, **kwatgs):
    setup += [rsp_node, 
              webots_launch, 
              move_group,
+              moveit_py_node, 
              rviz_launch, 
              shutdown_on_rviz_exit
            ]
@@ -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()
@@ -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"
        ],
    },
 )