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

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 (внимательно проверяй)

src/iiwa_bringup/config/iiwa7.srdf

@@ -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"/>

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

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

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: <Fixed Frame>
       Value: true
-    - Alpha: 1
+    - Acceleration_Scaling_Factor: 0.1
-      Class: rviz_default_plugins/RobotModel
+      Class: moveit_rviz_plugin/MotionPlanning
-      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
       Enabled: true
-      Links:
+      Move Group Namespace: ""
-        All Links Enabled: true
+      MoveIt_Allow_Approximate_IK: false
-        Expand Joint Details: false
+      MoveIt_Allow_External_Program: false
-        Expand Link Details: false
+      MoveIt_Allow_Replanning: false
-        Expand Tree: false
+      MoveIt_Allow_Sensor_Positioning: false
-        Link Tree Style: Links in Alphabetic Order
+      MoveIt_Planning_Attempts: 10
-        base_link:
+      MoveIt_Planning_Time: 5
-          Alpha: 1
+      MoveIt_Use_Cartesian_Path: false
-          Show Axes: false
+      MoveIt_Use_Constraint_Aware_IK: false
-          Show Trail: false
+      MoveIt_Workspace:
-          Value: true
+        Center:
-        link1:
+          X: 0
-          Alpha: 1
+          Y: 0
-          Show Axes: false
+          Z: 0
-          Show Trail: false
+        Size:
-          Value: true
+          X: 2
-        link2:
+          Y: 2
-          Alpha: 1
+          Z: 2
-          Show Axes: false
+      Name: MotionPlanning
-          Show Trail: false
+      Planned Path:
-          Value: true
+        Color Enabled: false
-        link3:
+        Interrupt Display: false
-          Alpha: 1
+        Links:
-          Show Axes: false
+          All Links Enabled: true
-          Show Trail: false
+          Expand Joint Details: false
-          Value: true
+          Expand Link Details: false
-        link4:
+          Expand Tree: false
-          Alpha: 1
+          Link Tree Style: Links in Alphabetic Order
-          Show Axes: false
+          base_link:
-          Show Trail: false
+            Alpha: 1
-          Value: true
+            Show Axes: false
-        link5:
+            Show Trail: false
-          Alpha: 1
+            Value: true
-          Show Axes: false
+          link1:
-          Show Trail: false
+            Alpha: 1
-          Value: true
+            Show Axes: false
-        link6:
+            Show Trail: false
-          Alpha: 1
+            Value: true
-          Show Axes: false
+          link2:
-          Show Trail: false
+            Alpha: 1
-          Value: true
+            Show Axes: false
-        link7:
+            Show Trail: false
-          Alpha: 1
+            Value: true
-          Show Axes: false
+          link3:
-          Show Trail: false
+            Alpha: 1
-          Value: true
+            Show Axes: false
-        link7_ee:
+            Show Trail: false
-          Alpha: 1
+            Value: true
-          Show Axes: false
+          link4:
-          Show Trail: false
+            Alpha: 1
-        world:
+            Show Axes: false
-          Alpha: 1
+            Show Trail: false
-          Show Axes: false
+            Value: true
-          Show Trail: false
+          link5:
-      Mass Properties:
+            Alpha: 1
-        Inertia: false
+            Show Axes: false
-        Mass: false
+            Show Trail: false
-      Name: RobotModel
+            Value: true
-      TF Prefix: ""
+          link6:
-      Update Interval: 0
+            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
+  Width: 1200
-  X: 0
+  X: 414
-  Y: 0
+  Y: 160

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(
+        .moveit_cpp(file_path="/home/daniel/dev/ros2_iiwa7/src/iiwa_bringup/config/motion_planing.yaml") # TODO: сделать подстановочным значением
-            pipelines=["ompl", "stomp", "pilz_industrial_motion_planner"],
-            default_planning_pipeline="pilz_industrial_motion_planner",
-        )
         .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
             ]

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()

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"
         ],
     },
 )