From df9512a2b250f7a01193f6de9e48a1fbf3cf1105 Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Fri, 25 Mar 2022 16:20:10 +0100 Subject: [PATCH] Migration to official movit parser --- src/iisy_config/config/config.rviz | 384 ------------------ src/iisy_config/config/iisy_controllers.yml | 42 -- .../config/iisy_moveit_controllers.yml | 15 - src/iisy_config/config/sensors_3d.yml | 9 - .../launch/lbr_move_group.launch.py | 181 --------- .../.setup_assistant | 0 .../CMakeLists.txt | 2 +- .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 13 + .../config/gazebo_controllers.yaml | 4 + src/iisy_moveit_config/config/iisy.srdf | 39 ++ .../config/joint_limits.yaml | 40 ++ .../config/kinematics.yaml} | 0 .../config/ompl_planning.yaml} | 0 .../config/ros_controllers.yaml | 40 ++ src/iisy_moveit_config/config/sensors_3d.yaml | 2 + .../config/simple_moveit_controllers.yaml | 12 + .../config/stomp_planning.yaml | 39 ++ .../launch/debug.launch.py | 0 .../launch/lbr_bringup.launch.py | 16 +- .../launch/lbr_control.launch.py | 36 +- .../launch/lbr_move_group.launch.py | 90 ++++ .../launch/lbr_simulation.launch.py | 0 .../launch/moveit.rviz | 0 .../launch/view_robot.launch.py | 0 .../package.xml | 2 +- .../srdf/iisy.srdf | 0 .../stl/base_bottom_coll.stl | Bin .../stl/base_bottom_low.stl | Bin .../stl/base_top_low.stl | Bin .../stl/link_1_full_coll.stl | Bin .../stl/link_1_full_low.stl | Bin .../stl/link_2_bottom_low.stl | Bin .../stl/link_2_top_coll.stl | Bin .../stl/link_2_top_low.stl | Bin .../stl/link_3_bottom_coll.stl | Bin .../stl/link_3_bottom_low.stl | Bin .../stl/link_3_top_coll.stl | Bin .../stl/link_3_top_low.stl | Bin .../urdf/iisy.urdf | 0 41 files changed, 315 insertions(+), 674 deletions(-) delete mode 100644 src/iisy_config/config/config.rviz delete mode 100644 src/iisy_config/config/iisy_controllers.yml delete mode 100644 src/iisy_config/config/iisy_moveit_controllers.yml delete mode 100644 src/iisy_config/config/sensors_3d.yml delete mode 100644 src/iisy_config/launch/lbr_move_group.launch.py rename src/{iisy_config => iisy_moveit_config}/.setup_assistant (100%) rename src/{iisy_config => iisy_moveit_config}/CMakeLists.txt (97%) create mode 100644 src/iisy_moveit_config/config/cartesian_limits.yaml create mode 100644 src/iisy_moveit_config/config/chomp_planning.yaml create mode 100644 src/iisy_moveit_config/config/fake_controllers.yaml create mode 100644 src/iisy_moveit_config/config/gazebo_controllers.yaml create mode 100644 src/iisy_moveit_config/config/iisy.srdf create mode 100644 src/iisy_moveit_config/config/joint_limits.yaml rename src/{iisy_config/config/kinematics.yml => iisy_moveit_config/config/kinematics.yaml} (100%) rename src/{iisy_config/config/ompl_planning.yml => iisy_moveit_config/config/ompl_planning.yaml} (100%) create mode 100644 src/iisy_moveit_config/config/ros_controllers.yaml create mode 100644 src/iisy_moveit_config/config/sensors_3d.yaml create mode 100644 src/iisy_moveit_config/config/simple_moveit_controllers.yaml create mode 100644 src/iisy_moveit_config/config/stomp_planning.yaml rename src/{iisy_config => iisy_moveit_config}/launch/debug.launch.py (100%) rename src/{iisy_config => iisy_moveit_config}/launch/lbr_bringup.launch.py (76%) rename src/{iisy_config => iisy_moveit_config}/launch/lbr_control.launch.py (57%) create mode 100644 src/iisy_moveit_config/launch/lbr_move_group.launch.py rename src/{iisy_config => iisy_moveit_config}/launch/lbr_simulation.launch.py (100%) rename src/{iisy_config => iisy_moveit_config}/launch/moveit.rviz (100%) rename src/{iisy_config => iisy_moveit_config}/launch/view_robot.launch.py (100%) rename src/{iisy_config => iisy_moveit_config}/package.xml (98%) rename src/{iisy_config => iisy_moveit_config}/srdf/iisy.srdf (100%) rename src/{iisy_config => iisy_moveit_config}/stl/base_bottom_coll.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/base_bottom_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/base_top_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_1_full_coll.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_1_full_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_2_bottom_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_2_top_coll.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_2_top_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_3_bottom_coll.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_3_bottom_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_3_top_coll.stl (100%) rename src/{iisy_config => iisy_moveit_config}/stl/link_3_top_low.stl (100%) rename src/{iisy_config => iisy_moveit_config}/urdf/iisy.urdf (100%) diff --git a/src/iisy_config/config/config.rviz b/src/iisy_config/config/config.rviz deleted file mode 100644 index 1be2d1c..0000000 --- a/src/iisy_config/config/config.rviz +++ /dev/null @@ -1,384 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 212 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - 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 - 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 - lbr_link_0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_ee: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - 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_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - 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 - lbr_link_0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_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: 0.05 s - Trail Step Size: 1 - Trajectory Topic: /display_planned_path - 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: med7_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 - lbr_link_0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_link_7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lbr_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 - Velocity_Scaling_Factor: 0.01 - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.398728847503662 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.056836556643247604 - Y: -0.0032705869525671005 - Z: 0.7173376083374023 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5103980898857117 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.248579978942871 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f30000032afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000111000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000154000002130000017d00ffffff000000010000010000000280fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000280000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000006efc0100000002fb0000000800540069006d00650100000000000007380000005800fffffffb0000000800540069006d006501000000000000045000000000000000000000053f0000032a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1848 - X: 0 - Y: 27 diff --git a/src/iisy_config/config/iisy_controllers.yml b/src/iisy_config/config/iisy_controllers.yml deleted file mode 100644 index 84195fa..0000000 --- a/src/iisy_config/config/iisy_controllers.yml +++ /dev/null @@ -1,42 +0,0 @@ -# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html -controller_manager: - ros__parameters: - update_rate: 500 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41 - - position_trajectory_controller: - # find declaration here https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_trajectory_controller/joint_trajectory_plugin.xml#L2 - type: joint_trajectory_controller/JointTrajectoryController - - forward_position_controller: - type: position_controllers/JointGroupPositionController - -position_trajectory_controller: - ros__parameters: - # find required parameters here https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot - command_interfaces: - - position - state_interfaces: - - position - state_publish_rate: 50.0 - action_monitor_rate: 20.0 - -forward_position_controller: - ros__parameters: - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot - interface_name: "position" diff --git a/src/iisy_config/config/iisy_moveit_controllers.yml b/src/iisy_config/config/iisy_moveit_controllers.yml deleted file mode 100644 index 7e8cc98..0000000 --- a/src/iisy_config/config/iisy_moveit_controllers.yml +++ /dev/null @@ -1,15 +0,0 @@ -controller_names: - - position_trajectory_controller - -# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller -position_trajectory_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot diff --git a/src/iisy_config/config/sensors_3d.yml b/src/iisy_config/config/sensors_3d.yml deleted file mode 100644 index a8343dd..0000000 --- a/src/iisy_config/config/sensors_3d.yml +++ /dev/null @@ -1,9 +0,0 @@ -sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /camera/depth_registered/points - max_range: 5.0 - point_subsample: 1 - padding_offset: 0.1 - padding_scale: 1.0 - max_update_rate: 1.0 - filtered_cloud_topic: filtered_cloud \ No newline at end of file diff --git a/src/iisy_config/launch/lbr_move_group.launch.py b/src/iisy_config/launch/lbr_move_group.launch.py deleted file mode 100644 index 95f2470..0000000 --- a/src/iisy_config/launch/lbr_move_group.launch.py +++ /dev/null @@ -1,181 +0,0 @@ -import os -import yaml - -import traceback - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, TimerAction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from ament_index_python import get_package_share_directory - - -def load_yaml(package_name: str, file_path: str): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as f: - return yaml.safe_load(f) - except EnvironmentError as error: - print("Error loading yaml "+package_name+" | "+file_path) - traceback.print_exc() - return None - - -def load_file(package_name: str, file_path: str) -> str: - package_path = get_package_share_directory(package_name) - absolut_file_path = os.path.join(package_path, file_path) - - try: - with open(absolut_file_path, "r") as f: - return f.read() - except EnvironmentError: - print("Error loading file "+package_name+" | "+file_path) - traceback.print_exc() - return None - - -def launch_setup(context, *args, **kwargs): - - # Configure robot_description - robot_description = {"robot_description": LaunchConfiguration("robot_description")} - - # Robot semantics SRDF - robot_description_semantic = { - "robot_description_semantic": load_file("iisy_config", "srdf/iisy.srdf") - } - - # Kinematics - kinematics_yaml = load_yaml("iisy_config", "config/kinematics.yml") - - # Update group name - #kinematics_yaml["{}_arm".format(model)] = kinematics_yaml["group_name"] - #del kinematics_yaml["group_name"] - - # Joint limits - robot_description_planning = { - "robot_description_planning": PathJoinSubstitution( - [ - FindPackageShare("iisy_config"), - "config/joint_limits.yml" - ] - ) - } - - # Planning - ompl_yaml = load_yaml("iisy_config", "config/ompl_planning.yml") - - planning = { - "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": "default_planner_request_adapters/AddTimeParameterization 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 - } - } - - # Trajectory execution - trajectory_execution = {"allow_trajectory_execution": True, - "moveit_manage_controllers": True} - - # Controllers - controllers_yaml = load_yaml( - "iisy_config", - "config/iisy_moveit_controllers.yml" - ) - - moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"} - - # Planning scene - planning_scene_monitor_parameters = {"publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True} - - # Time configuration - use_sim_time = {"use_sim_time": True} - - perception_pipeline = load_yaml("iisy_config","config/sensors_3d.yml") - - # Prepare move group node - move_group_node = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - arguments=["--ros-args"], - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - robot_description_planning, - ompl_yaml, - planning, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - use_sim_time, - perception_pipeline - ] - ) - - # RViz - rviz_config = PathJoinSubstitution([FindPackageShare("iisy_config"), "config/config.rviz"]) - - rviz = Node( - package="rviz2", - executable="rviz2", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - planning, - use_sim_time - ], - arguments=[ - '-d', rviz_config - ] - ) - - # MoveGroupInterface demo executable - move_group_demo = Node( - name="btree_robot", - package="btree_robot", - executable="robot_test", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ], - ) - - return [ - move_group_node, - rviz, - TimerAction( - period=10.0, - actions=[ - move_group_demo - ] - ) - ] - -def generate_launch_description(): - - # Launch arguments - launch_args = [] - - launch_args.append( - DeclareLaunchArgument( - name="robot_description", - description="Robot description XML file." - ) - ) - - return LaunchDescription( - launch_args + [ - OpaqueFunction(function=launch_setup) - ]) diff --git a/src/iisy_config/.setup_assistant b/src/iisy_moveit_config/.setup_assistant similarity index 100% rename from src/iisy_config/.setup_assistant rename to src/iisy_moveit_config/.setup_assistant diff --git a/src/iisy_config/CMakeLists.txt b/src/iisy_moveit_config/CMakeLists.txt similarity index 97% rename from src/iisy_config/CMakeLists.txt rename to src/iisy_moveit_config/CMakeLists.txt index 17b2138..64b134b 100644 --- a/src/iisy_config/CMakeLists.txt +++ b/src/iisy_moveit_config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(iisy_config) +project(iisy_moveit_config) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/src/iisy_moveit_config/config/cartesian_limits.yaml b/src/iisy_moveit_config/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/src/iisy_moveit_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/src/iisy_moveit_config/config/chomp_planning.yaml b/src/iisy_moveit_config/config/chomp_planning.yaml new file mode 100644 index 0000000..eb9c912 --- /dev/null +++ b/src/iisy_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/src/iisy_moveit_config/config/fake_controllers.yaml b/src/iisy_moveit_config/config/fake_controllers.yaml new file mode 100644 index 0000000..a97a8f3 --- /dev/null +++ b/src/iisy_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,13 @@ +controller_list: + - name: fake_iisy_controller + type: $(arg fake_execution_type) + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot +initial: # Define initial robot poses per group + - group: iisy + pose: home \ No newline at end of file diff --git a/src/iisy_moveit_config/config/gazebo_controllers.yaml b/src/iisy_moveit_config/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/src/iisy_moveit_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/src/iisy_moveit_config/config/iisy.srdf b/src/iisy_moveit_config/config/iisy.srdf new file mode 100644 index 0000000..9296ecc --- /dev/null +++ b/src/iisy_moveit_config/config/iisy.srdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/iisy_moveit_config/config/joint_limits.yaml b/src/iisy_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..64ec7c1 --- /dev/null +++ b/src/iisy_moveit_config/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + base_link1_joint: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + base_rot: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link1_link2_joint: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link2_link3_joint: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link2_rot: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link3_rot: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/src/iisy_config/config/kinematics.yml b/src/iisy_moveit_config/config/kinematics.yaml similarity index 100% rename from src/iisy_config/config/kinematics.yml rename to src/iisy_moveit_config/config/kinematics.yaml diff --git a/src/iisy_config/config/ompl_planning.yml b/src/iisy_moveit_config/config/ompl_planning.yaml similarity index 100% rename from src/iisy_config/config/ompl_planning.yml rename to src/iisy_moveit_config/config/ompl_planning.yaml diff --git a/src/iisy_moveit_config/config/ros_controllers.yaml b/src/iisy_moveit_config/config/ros_controllers.yaml new file mode 100644 index 0000000..1a50f50 --- /dev/null +++ b/src/iisy_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,40 @@ +iisy_controller: + type: effort_controllers/JointTrajectoryController + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot + gains: + base_rot: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + base_link1_joint: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + link1_link2_joint: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + link2_rot: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + link2_link3_joint: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + link3_rot: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/src/iisy_moveit_config/config/sensors_3d.yaml b/src/iisy_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..51010a3 --- /dev/null +++ b/src/iisy_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/src/iisy_moveit_config/config/simple_moveit_controllers.yaml b/src/iisy_moveit_config/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..d2307c1 --- /dev/null +++ b/src/iisy_moveit_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,12 @@ +controller_list: + - name: iisy_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot \ No newline at end of file diff --git a/src/iisy_moveit_config/config/stomp_planning.yaml b/src/iisy_moveit_config/config/stomp_planning.yaml new file mode 100644 index 0000000..2a3d04a --- /dev/null +++ b/src/iisy_moveit_config/config/stomp_planning.yaml @@ -0,0 +1,39 @@ +stomp/iisy: + group_name: iisy + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/src/iisy_config/launch/debug.launch.py b/src/iisy_moveit_config/launch/debug.launch.py similarity index 100% rename from src/iisy_config/launch/debug.launch.py rename to src/iisy_moveit_config/launch/debug.launch.py diff --git a/src/iisy_config/launch/lbr_bringup.launch.py b/src/iisy_moveit_config/launch/lbr_bringup.launch.py similarity index 76% rename from src/iisy_config/launch/lbr_bringup.launch.py rename to src/iisy_moveit_config/launch/lbr_bringup.launch.py index 41b3846..949e9c4 100644 --- a/src/iisy_config/launch/lbr_bringup.launch.py +++ b/src/iisy_moveit_config/launch/lbr_bringup.launch.py @@ -1,10 +1,7 @@ -from launch.actions.declare_launch_argument import DeclareLaunchArgument + from launch.launch_description import LaunchDescription from launch.actions import IncludeLaunchDescription, OpaqueFunction -from launch.conditions import IfCondition from launch.substitutions import Command, FindExecutable, PathJoinSubstitution -from launch.substitutions.launch_configuration import LaunchConfiguration -from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -36,17 +33,6 @@ def launch_setup(context, *args, **kwargs): ] ) - # Gazebo simulation - #simulation = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # PathJoinSubstitution([ - # FindPackageShare("iisy_config"), - # "launch", - # "lbr_simulation.launch.py" - # ]) - # ), - #) - # Move group move_group = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/src/iisy_config/launch/lbr_control.launch.py b/src/iisy_moveit_config/launch/lbr_control.launch.py similarity index 57% rename from src/iisy_config/launch/lbr_control.launch.py rename to src/iisy_moveit_config/launch/lbr_control.launch.py index 52f5d37..8356b2a 100644 --- a/src/iisy_config/launch/lbr_control.launch.py +++ b/src/iisy_moveit_config/launch/lbr_control.launch.py @@ -1,38 +1,22 @@ from launch.launch_description import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import UnlessCondition from launch.substitutions import PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + def generate_launch_description(): - - # Launch arguments - launch_args = [] - - launch_args.append( - DeclareLaunchArgument( - name="robot_description", - description="Robot description XML file." - ) - ) - - - # Configure robot_description - robot_description = {"robot_description": LaunchConfiguration("robot_description")} - - # Load controllers from YAML configuration file - controller_configurations = PathJoinSubstitution([ - FindPackageShare("iisy_config"), - "config/iisy_controllers.yml" - ]) + moveit_config = MoveItConfigsBuilder("iisy").to_moveit_configs() robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", output="screen", - parameters=[robot_description] + parameters=[ + moveit_config.robot_description, + ] ) joint_state_broadcaster = Node( @@ -47,9 +31,9 @@ def generate_launch_description(): arguments=["position_trajectory_controller", "--controller-manager", "/controller_manager"], ) - return LaunchDescription( - launch_args + [ + return LaunchDescription([ robot_state_publisher, joint_state_broadcaster, - controller - ]) + controller, + ] + ) diff --git a/src/iisy_moveit_config/launch/lbr_move_group.launch.py b/src/iisy_moveit_config/launch/lbr_move_group.launch.py new file mode 100644 index 0000000..c2399c5 --- /dev/null +++ b/src/iisy_moveit_config/launch/lbr_move_group.launch.py @@ -0,0 +1,90 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, TimerAction +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def launch_setup(context, *args, **kwargs): + + moveit_config = MoveItConfigsBuilder("iisy").to_moveit_configs() + + # Time configuration + use_sim_time = {"use_sim_time": True} + + # Prepare move group node + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + arguments=["--ros-args"], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.planning_scene_monitor, + moveit_config.trajectory_execution, + use_sim_time, + ] + ) + + # RViz + rviz_config = PathJoinSubstitution([FindPackageShare("iisy_moveit_config"), "config/config.rviz"]) + + rviz = Node( + package="rviz2", + executable="rviz2", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.planning_scene_monitor, + use_sim_time, + ], + arguments=[ + '-d', rviz_config + ] + ) + + # MoveGroupInterface demo executable + move_group_demo = Node( + name="btree_robot", + package="btree_robot", + executable="robot_test", + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + ) + + return [ + move_group_node, + rviz, + TimerAction( + period=10.0, + actions=[ + move_group_demo, + ] + ) + ] + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append( + DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file." + ) + ) + + return LaunchDescription( + launch_args + [ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/iisy_config/launch/lbr_simulation.launch.py b/src/iisy_moveit_config/launch/lbr_simulation.launch.py similarity index 100% rename from src/iisy_config/launch/lbr_simulation.launch.py rename to src/iisy_moveit_config/launch/lbr_simulation.launch.py diff --git a/src/iisy_config/launch/moveit.rviz b/src/iisy_moveit_config/launch/moveit.rviz similarity index 100% rename from src/iisy_config/launch/moveit.rviz rename to src/iisy_moveit_config/launch/moveit.rviz diff --git a/src/iisy_config/launch/view_robot.launch.py b/src/iisy_moveit_config/launch/view_robot.launch.py similarity index 100% rename from src/iisy_config/launch/view_robot.launch.py rename to src/iisy_moveit_config/launch/view_robot.launch.py diff --git a/src/iisy_config/package.xml b/src/iisy_moveit_config/package.xml similarity index 98% rename from src/iisy_config/package.xml rename to src/iisy_moveit_config/package.xml index c29adac..e5f1f1e 100644 --- a/src/iisy_config/package.xml +++ b/src/iisy_moveit_config/package.xml @@ -1,7 +1,7 @@ - iisy_config + iisy_moveit_config 0.3.0 An automatically generated package with all the configuration and launch files for using the iisy with the MoveIt Motion Planning Framework diff --git a/src/iisy_config/srdf/iisy.srdf b/src/iisy_moveit_config/srdf/iisy.srdf similarity index 100% rename from src/iisy_config/srdf/iisy.srdf rename to src/iisy_moveit_config/srdf/iisy.srdf diff --git a/src/iisy_config/stl/base_bottom_coll.stl b/src/iisy_moveit_config/stl/base_bottom_coll.stl similarity index 100% rename from src/iisy_config/stl/base_bottom_coll.stl rename to src/iisy_moveit_config/stl/base_bottom_coll.stl diff --git a/src/iisy_config/stl/base_bottom_low.stl b/src/iisy_moveit_config/stl/base_bottom_low.stl similarity index 100% rename from src/iisy_config/stl/base_bottom_low.stl rename to src/iisy_moveit_config/stl/base_bottom_low.stl diff --git a/src/iisy_config/stl/base_top_low.stl b/src/iisy_moveit_config/stl/base_top_low.stl similarity index 100% rename from src/iisy_config/stl/base_top_low.stl rename to src/iisy_moveit_config/stl/base_top_low.stl diff --git a/src/iisy_config/stl/link_1_full_coll.stl b/src/iisy_moveit_config/stl/link_1_full_coll.stl similarity index 100% rename from src/iisy_config/stl/link_1_full_coll.stl rename to src/iisy_moveit_config/stl/link_1_full_coll.stl diff --git a/src/iisy_config/stl/link_1_full_low.stl b/src/iisy_moveit_config/stl/link_1_full_low.stl similarity index 100% rename from src/iisy_config/stl/link_1_full_low.stl rename to src/iisy_moveit_config/stl/link_1_full_low.stl diff --git a/src/iisy_config/stl/link_2_bottom_low.stl b/src/iisy_moveit_config/stl/link_2_bottom_low.stl similarity index 100% rename from src/iisy_config/stl/link_2_bottom_low.stl rename to src/iisy_moveit_config/stl/link_2_bottom_low.stl diff --git a/src/iisy_config/stl/link_2_top_coll.stl b/src/iisy_moveit_config/stl/link_2_top_coll.stl similarity index 100% rename from src/iisy_config/stl/link_2_top_coll.stl rename to src/iisy_moveit_config/stl/link_2_top_coll.stl diff --git a/src/iisy_config/stl/link_2_top_low.stl b/src/iisy_moveit_config/stl/link_2_top_low.stl similarity index 100% rename from src/iisy_config/stl/link_2_top_low.stl rename to src/iisy_moveit_config/stl/link_2_top_low.stl diff --git a/src/iisy_config/stl/link_3_bottom_coll.stl b/src/iisy_moveit_config/stl/link_3_bottom_coll.stl similarity index 100% rename from src/iisy_config/stl/link_3_bottom_coll.stl rename to src/iisy_moveit_config/stl/link_3_bottom_coll.stl diff --git a/src/iisy_config/stl/link_3_bottom_low.stl b/src/iisy_moveit_config/stl/link_3_bottom_low.stl similarity index 100% rename from src/iisy_config/stl/link_3_bottom_low.stl rename to src/iisy_moveit_config/stl/link_3_bottom_low.stl diff --git a/src/iisy_config/stl/link_3_top_coll.stl b/src/iisy_moveit_config/stl/link_3_top_coll.stl similarity index 100% rename from src/iisy_config/stl/link_3_top_coll.stl rename to src/iisy_moveit_config/stl/link_3_top_coll.stl diff --git a/src/iisy_config/stl/link_3_top_low.stl b/src/iisy_moveit_config/stl/link_3_top_low.stl similarity index 100% rename from src/iisy_config/stl/link_3_top_low.stl rename to src/iisy_moveit_config/stl/link_3_top_low.stl diff --git a/src/iisy_config/urdf/iisy.urdf b/src/iisy_moveit_config/urdf/iisy.urdf similarity index 100% rename from src/iisy_config/urdf/iisy.urdf rename to src/iisy_moveit_config/urdf/iisy.urdf