Merge pull request #356 from jrgnicho/fix/exercise-3.0-moveit_config_pkg

Fix/exercise 3.3 moveit config pkg

exercises/3.3/ros2/src/myworkcell_moveit_config/config/controllers.yaml

@@ -1,7 +1,7 @@
 controller_names:
-  - fake_joint_trajectory_controller
+  - manipulator_joint_trajectory_controller
 
-fake_joint_trajectory_controller:
+manipulator_joint_trajectory_controller:
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true

exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_limits.yaml

@@ -9,9 +9,12 @@ joint_limits:
     max_acceleration: 0
   shoulder_lift_joint:
     has_velocity_limits: true
-    max_velocity: 3.15
-    has_acceleration_limits: false
-    max_acceleration: 0
+    max_velocity: 6.0
+    has_acceleration_limits: true
+    max_acceleration: 0.62831853
+    has_position_limits: true
+    max_position: 0.0
+    min_position: -2.0
   shoulder_pan_joint:
     has_velocity_limits: true
     max_velocity: 3.15

exercises/3.3/ros2/src/myworkcell_moveit_config/config/kinematics.yaml

@@ -1,5 +1,5 @@
 manipulator:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
-  kinematics_solver_attempts: 3
+  kinematics_solver_search_resolution: 0.01
+  kinematics_solver_timeout: 0.5
+  kinematics_solver_attempts: 40

exercises/3.3/ros2/src/myworkcell_moveit_config/config/myworkcell.srdf

@@ -12,6 +12,17 @@
     <group name="manipulator">
         <chain base_link="base_link" tip_link="tool0" />
     </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="manipulator">
+        <joint name="elbow_joint" value="1.0589" />
+        <joint name="shoulder_lift_joint" value="-0.9884" />
+        <joint name="shoulder_pan_joint" value="0.706" />
+        <joint name="wrist_1_joint" value="-1.5531" />
+        <joint name="wrist_2_joint" value="-1.5531" />
+        <joint name="wrist_3_joint" value="0" />
+    </group_state>
+
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
     <disable_collisions link1="base_link" link2="table" reason="Adjacent" />

exercises/3.3/ros2/src/myworkcell_moveit_config/config/ompl_planning.yaml

@@ -27,10 +27,10 @@ planner_configs:
   RRT:
     type: geometric::RRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    goal_bias: 0.01  # When close to goal select goal, with this probability? default: 0.05
   RRTConnect:
     type: geometric::RRTConnect
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    range: 0.01  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
   RRTstar:
     type: geometric::RRTstar
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -121,7 +121,7 @@ planner_configs:
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
 manipulator:
-  default_planner_config: RRT
+  default_planner_config: RRTConnect
   planner_configs:
     - SBL
     - EST
@@ -147,12 +147,12 @@ manipulator:
     - SPARS
     - SPARStwo
   projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
-  longest_valid_segment_fraction: "0.005"
-
+  longest_valid_segment_fraction: 0.01
 planning_plugin: 'ompl_interface/OMPLPlanner'
 request_adapters: >-
-  default_planner_request_adapters/AddTimeOptimalParameterization
-  default_planner_request_adapters/FixWorkspaceBounds
-  default_planner_request_adapters/FixStartStateBounds
-  default_planner_request_adapters/FixStartStateCollision
-  default_planner_request_adapters/FixStartStatePathConstraints
+    default_planner_request_adapters/AddTimeOptimalParameterization
+    default_planner_request_adapters/FixWorkspaceBounds
+    default_planner_request_adapters/FixStartStateBounds
+    default_planner_request_adapters/FixStartStateCollision
+    default_planner_request_adapters/FixStartStatePathConstraints
+start_state_max_bounds_error: 0.1

exercises/3.3/ros2/src/myworkcell_moveit_config/config/ros_controllers.yaml

@@ -1,35 +1,33 @@
-# MoveIt-specific simulation settings
-moveit_sim_hw_interface:
-  joint_model_group: controllers_initial_group_
-  joint_model_group_pose: controllers_initial_pose_
-# Settings for ros_control control loop
-generic_hw_control_loop:
-  loop_hz: 300
-  cycle_time_error_threshold: 0.01
-# Settings for ros_control hardware interface
-hardware_interface:
-  joints:
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-  sim_control_mode: 1  # 0: position, 1: velocity
-# Publish all joint states
-# Creates the /joint_states topic necessary in ROS
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-controller_list:
-  - name: manipulator_controller
-    action_ns: follow_joint_trajectory
-    default: True
-    type: FollowJointTrajectory
+controller_manager:
+  ros__parameters:
+    update_rate: 600  # Hz
+    manipulator_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    joint_state_controller:
+      type: joint_state_controller/JointStateController
+
+# parameters for each controller listed under controller manager
+manipulator_joint_trajectory_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
       - elbow_joint
+      - shoulder_lift_joint
+      - shoulder_pan_joint
       - wrist_1_joint
       - wrist_2_joint
-      - wrist_3_joint
+      - wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+
+joint_state_controller:
+  ros__parameters:
+    type: joint_state_controller/JointStateController

exercises/3.3/ros2/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch.py

@@ -1,7 +1,8 @@
 import os
 import yaml
-import launch
-import launch_ros
+import xacro
+from launch import LaunchDescription
+from launch_ros.actions import Node
 from ament_index_python import get_package_share_directory
 
 def get_package_file(package, file_path):
@@ -41,15 +42,16 @@ def generate_launch_description():
     srdf_file = get_package_file('myworkcell_moveit_config', 'config/myworkcell.srdf')
     kinematics_file = get_package_file('myworkcell_moveit_config', 'config/kinematics.yaml')
     ompl_config_file = get_package_file('myworkcell_moveit_config', 'config/ompl_planning.yaml')
-    controllers_file = get_package_file('myworkcell_moveit_config', 'config/controllers.yaml')
+    moveit_controllers_file = get_package_file('myworkcell_moveit_config', 'config/controllers.yaml')
+    ros_controllers_file = get_package_file('myworkcell_moveit_config', 'config/ros_controllers.yaml')
 
     robot_description = load_file(urdf_file)
     robot_description_semantic = load_file(srdf_file)
     kinematics_config = load_yaml(kinematics_file)
     ompl_config = load_yaml(ompl_config_file)
 
     moveit_controllers = {
-        'moveit_simple_controller_manager' : load_yaml(controllers_file),
+        'moveit_simple_controller_manager' : load_yaml(moveit_controllers_file),
         'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
     }
     trajectory_execution = {
@@ -65,57 +67,73 @@ def generate_launch_description():
         'publish_transforms_updates': True
     }
 
-    return launch.LaunchDescription([
-        launch_ros.actions.Node(
-            #name='move_group_node',
-            package='moveit_ros_move_group',
-            executable='move_group',
-            output='screen',
-            parameters=[
-                {
-                    'robot_description': robot_description,
-                    'robot_description_semantic': robot_description_semantic,
-                    'robot_description_kinematics': kinematics_config,
-                    'ompl': ompl_config,
-                },
-                moveit_controllers,
-                trajectory_execution,
-                planning_scene_monitor_config,
-            ],
-        ),
-        launch_ros.actions.Node(
-            name='robot_state_publisher',
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            output='screen',
-            parameters=[
-                {'robot_description': robot_description}
-            ]
-        ),
-        launch_ros.actions.Node(
-            package='fake_joint_driver',
-            executable='fake_joint_driver_node',
-            output='screen',
-            parameters=[
-                {
-                    'robot_description': robot_description,
-                    'controller_name': 'fake_joint_trajectory_controller'
-                },
-                get_package_file("myworkcell_moveit_config", "config/fake_controllers.yaml"),
-            ],
-        ),
-        launch_ros.actions.Node(
-            name='rviz',
-            package='rviz2',
-            executable='rviz2',
-            output='screen',
-            parameters=[
-                {
-                    'robot_description': robot_description,
-                    'robot_description_semantic': robot_description_semantic,
-                    'robot_description_kinematics': kinematics_config,
-                    'ompl': ompl_config,
-                }
-            ],
-        )
-    ])
+    # MoveIt node
+    move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            {
+                'robot_description': robot_description,
+                'robot_description_semantic': robot_description_semantic,
+                'robot_description_kinematics': kinematics_config,
+                'ompl': ompl_config,
+                'planning_pipelines': ['ompl'],
+            },
+            moveit_controllers,
+            trajectory_execution,
+            planning_scene_monitor_config,
+        ],
+    )
+    # TF information
+    robot_state_publisher = Node(
+        name='robot_state_publisher',
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[
+            {'robot_description': robot_description}
+        ]
+    )
+    # Visualization (parameters needed for MoveIt display plugin)
+    rviz = Node(
+        name='rviz',
+        package='rviz2',
+        executable='rviz2',
+        output='screen',
+        parameters=[
+            {
+                'robot_description': robot_description,
+                'robot_description_semantic': robot_description_semantic,
+                'robot_description_kinematics': kinematics_config,
+            }
+        ],
+    )
+    # Controller manager for realtime interactions
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters= [
+            {'robot_description': robot_description},
+            ros_controllers_file
+        ],
+        output="screen",
+    )
+    # Startup up ROS2 controllers (will exit immediately)
+    controller_names = ['manipulator_joint_trajectory_controller', 'joint_state_controller']
+    spawn_controllers = [
+        Node(
+            package="controller_manager",
+            executable="spawner.py",
+            arguments=[controller],
+            output="screen")
+        for controller in controller_names
+    ]
+
+    return LaunchDescription([
+        move_group_node,
+        robot_state_publisher,
+        ros2_control_node,
+        rviz,
+        ] + spawn_controllers
+    )