diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/controllers.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/controllers.yaml
index ce3f29601..465580df4 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/controllers.yaml
+++ b/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
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/fake_controllers.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/fake_controllers.yaml
deleted file mode 100644
index 358d0ab5f..000000000
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/fake_controllers.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-fake_joint_trajectory_controller:
- ros__parameters:
- joints:
- - shoulder_pan_joint
- - shoulder_lift_joint
- - elbow_joint
- - wrist_1_joint
- - wrist_2_joint
- - wrist_3_joint
- write_op_modes:
- - shoulder_pan_joint
- - shoulder_lift_joint
- - elbow_joint
- - wrist_1_joint
- - wrist_2_joint
- - wrist_3_joint
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_limits.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_limits.yaml
index 2ba4dd6d3..ce7d93ea8 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_limits.yaml
+++ b/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
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_names.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_names.yaml
deleted file mode 100644
index dce3623f7..000000000
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/joint_names.yaml
+++ /dev/null
@@ -1 +0,0 @@
-controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
\ No newline at end of file
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/kinematics.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/kinematics.yaml
index 5d492ac1f..0479f5a3a 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/kinematics.yaml
+++ b/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
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/myworkcell.srdf b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/myworkcell.srdf
index e877c60d1..0dae15137 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/myworkcell.srdf
+++ b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/myworkcell.srdf
@@ -12,6 +12,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ompl_planning.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ompl_planning.yaml
index 9713587f8..8af330bda 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ompl_planning.yaml
+++ b/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
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ros_controllers.yaml b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ros_controllers.yaml
index 9d3719861..aca485c83 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ros_controllers.yaml
+++ b/exercises/3.3/ros2/src/myworkcell_moveit_config/config/ros_controllers.yaml
@@ -1,35 +1,32 @@
-# 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
\ No newline at end of file
+ - 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
\ No newline at end of file
diff --git a/exercises/3.3/ros2/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch.py b/exercises/3.3/ros2/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch.py
index c1912f662..25df97e2e 100644
--- a/exercises/3.3/ros2/src/myworkcell_moveit_config/launch/myworkcell_planning_execution.launch.py
+++ b/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,7 +42,8 @@ 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)
@@ -49,7 +51,7 @@ def generate_launch_description():
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
+ )
diff --git a/exercises/3.3/ros2/src/myworkcell_support/urdf/workcell.urdf.xacro b/exercises/3.3/ros2/src/myworkcell_support/urdf/workcell.urdf.xacro
index f0a126dc7..9fe6dcfce 100644
--- a/exercises/3.3/ros2/src/myworkcell_support/urdf/workcell.urdf.xacro
+++ b/exercises/3.3/ros2/src/myworkcell_support/urdf/workcell.urdf.xacro
@@ -1,7 +1,25 @@
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+