yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

improved iisy_config module

Browse Source
This commit is contained in:
Bastian Hofmann
2022-03-18 09:16:59 +01:00
parent 16816db994
commit 0403aba97b
55 changed files with 1203 additions and 608 deletions
Download Patch File Download Diff File Expand all files Collapse all files

384
src/iisy_config/config/config.rviz Normal file
View File

@@ -0,0 +1,384 @@
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: <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
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: <Fixed 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

42
src/iisy_config/config/iisy_controllers.yml Normal file
View File

@@ -0,0 +1,42 @@
# 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"

15
src/iisy_config/config/iisy_moveit_controllers.yml Normal file
View File

@@ -0,0 +1,15 @@
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

0
src/iisy_config/config/kinematics.yaml → src/iisy_config/config/kinematics.yml
View File

0
src/iisy_config/config/ompl_planning.yaml → src/iisy_config/config/ompl_planning.yml
View File

23
src/iisy_config/launch/chomp_planning_pipeline.launch.xml
View File

@@ -1,23 +0,0 @@
<launch>
<arg name="start_state_max_bounds_error" default="0.1" />
<arg name="jiggle_fraction" default="0.05" />
<!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
<arg name="planning_adapters"
default="default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints"
/>
<param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
<param name="request_adapters" value="$(arg planning_adapters)" />
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->
<rosparam command="load" file="$(find iisy_config)/config/chomp_planning.yaml" />
</launch>

15
src/iisy_config/launch/default_warehouse_db.launch
View File

@@ -1,15 +0,0 @@
<launch>
<arg name="reset" default="false"/>
<!-- If not specified, we'll use a default database location -->
<arg name="moveit_warehouse_database_path" default="$(find iisy_config)/default_warehouse_mongo_db" />
<!-- Launch the warehouse with the configured database location -->
<include file="$(dirname)/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
</include>
<!-- If we want to reset the database, run this node -->
<node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
</launch>

68
src/iisy_config/launch/demo.launch
View File

@@ -1,68 +0,0 @@
<launch>
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find iisy_config)/default_warehouse_mongo_db" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!-- By default, we will load or override the robot_description -->
<arg name="load_robot_description" default="true"/>
<!-- Choose controller manager: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" default="fake" />
<!-- Set execution mode for fake execution controllers -->
<arg name="fake_execution_type" default="interpolate" />
<!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
<arg name="use_gui" default="false" />
<arg name="use_rviz" default="true" />
<!-- If needed, broadcast static tf for robot root -->
<group if="$(eval arg('moveit_controller_manager') == 'fake')">
<!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
<node name="joint_state_publisher" pkg="joint_state_publisher" exec="joint_state_publisher" unless="$(arg use_gui)">
<param from="source_list">[move_group/fake_controller_joint_states]</param>
</node>
<!-- If desired, a GUI version is available allowing to move the simulated robot around manually
This corresponds to moving around the real robot without the use of MoveIt. -->
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui" if="$(arg use_gui)">
<param from="source_list">[move_group/fake_controller_joint_states]</param>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher" respawn="true" output="screen" />
</group>
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(dirname)/move_group.launch">
<let name="allow_trajectory_execution" value="true"/>
<let name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
<let name="fake_execution_type" value="$(arg fake_execution_type)"/>
<let name="info" value="true"/>
<let name="debug" value="$(arg debug)"/>
<let name="pipeline" value="$(arg pipeline)"/>
<let name="load_robot_description" value="$(arg load_robot_description)"/>
</include>
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
<let name="rviz_config" value="$(dirname)/moveit.rviz"/>
<let name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
<let name="moveit_warehouse_database_path" value="$(arg db_path)"/>
</include>
</launch>

21
src/iisy_config/launch/demo_gazebo.launch
View File

@@ -1,21 +0,0 @@
<launch>
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!-- Gazebo specific options -->
<arg name="gazebo_gui" default="true"/>
<arg name="paused" default="false"/>
<!-- launch the gazebo simulator and spawn the robot -->
<include file="$(dirname)/gazebo.launch" >
<arg name="paused" value="$(arg paused)"/>
<arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
</include>
<include file="$(dirname)/demo.launch" pass_all_args="true">
<!-- robot description is loaded by gazebo.launch, to enable Gazebo features -->
<arg name="load_robot_description" value="false" />
<arg name="moveit_controller_manager" value="ros_control" />
</include>
</launch>

12
src/iisy_config/launch/fake_moveit_controller_manager.launch.xml
View File

@@ -1,12 +0,0 @@
<launch>
<!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
<arg name="fake_execution_type" default="interpolate" />
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
<!-- The rest of the params are specific to this plugin -->
<rosparam subst_value="true" file="$(find iisy_config)/config/fake_controllers.yaml"/>
</launch>

32
src/iisy_config/launch/gazebo.launch
View File

@@ -1,32 +0,0 @@
<?xml version="1.0"?>
<launch>
<arg name="paused" default="false"/>
<arg name="gazebo_gui" default="true"/>
<arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
default=" -J base_link1_joint 0 -J base_rot 0 -J link1_link2_joint 0 -J link2_link3_joint 0 -J link2_rot 0 -J link3_rot 0"/>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<let name="world_name" default="worlds/empty.world"/>
<let name="paused" value="true"/>
<let name="gui" value="$(arg gazebo_gui)"/>
</include>
<!-- send robot urdf to param server -->
<param name="robot_description" textfile="$(find gaz_simulation)/urdf/iisy.urdf" />
<!-- unpause only after loading robot model -->
<let name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
<!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
<arg name="world_pose" value="-x 0 -y 0 -z 0" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
respawn="false" output="screen" />
<!-- Load joint controller parameters for Gazebo -->
<param from="$(find iisy_config)/config/gazebo_controllers.yaml" />
<!-- Spawn Gazebo ROS controllers -->
<node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
<!-- Load ROS controllers -->
<include file="$(dirname)/ros_controllers.launch"/>
</launch>

3
src/iisy_config/launch/iisy_moveit_sensor_manager.launch.xml
View File

@@ -1,3 +0,0 @@
<launch>
</launch>

17
src/iisy_config/launch/joystick_control.launch
View File

@@ -1,17 +0,0 @@
<launch>
<!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
<arg name="dev" default="/dev/input/js0" />
<!-- Launch joy node -->
<node pkg="joy" type="joy_node" name="joy">
<param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
<param name="deadzone" value="0.2" />
<param name="autorepeat_rate" value="40" />
<param name="coalesce_interval" value="0.025" />
</node>
<!-- Launch python interface -->
<node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
</launch>

152
src/iisy_config/launch/lbr_bringup.launch.py Normal file
View File

@@ -0,0 +1,152 @@
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
def launch_setup(context, *args, **kwargs):
# Evaluate frequently used variables
model = LaunchConfiguration("model").perform(context)
# Load robot description
robot_description_content = Command(
[
FindExecutable(name="xacro"), " ",
PathJoinSubstitution(
[FindPackageShare("iisy_config"), "urdf/{}.urdf".format( model)]
), " ",
"robot_name:=", LaunchConfiguration("robot_name"), " ",
"sim:=", LaunchConfiguration("sim")
]
)
# Load controls
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("iisy_config"),
"launch",
"lbr_control.launch.py"
])
), launch_arguments=[
("robot_description", robot_description_content),
("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
("controller_configurations", LaunchConfiguration("controller_configurations")),
("controller", LaunchConfiguration("controller")),
("sim", LaunchConfiguration("sim"))
]
)
# Gazebo simulation
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("iisy_config"),
"launch",
"lbr_simulation.launch.py"
])
),
launch_arguments=[
("robot_name", LaunchConfiguration("robot_name"))
],
condition=IfCondition(LaunchConfiguration("sim"))
)
# Move group
move_group = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("iisy_config"),
"launch",
"lbr_move_group.launch.py"
])
),
launch_arguments=[
("robot_description", robot_description_content),
("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")),
("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")),
("model", LaunchConfiguration("model")),
("sim", LaunchConfiguration("sim"))
]
)
return [
simulation,
control,
move_group
]
def generate_launch_description():
# Launch arguments
launch_args = []
launch_args.append(DeclareLaunchArgument(
name="model",
default_value="iisy",
description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
))
launch_args.append(DeclareLaunchArgument(
name="robot_name",
default_value="iisy",
description="Set robot name."
))
launch_args.append(DeclareLaunchArgument(
name="sim",
default_value="true",
description="Launch robot in simulation or on real setup."
))
launch_args.append(
DeclareLaunchArgument(
name="controller_configurations_package",
default_value="iisy_config",
description="Package that contains controller configurations."
)
)
launch_args.append(
DeclareLaunchArgument(
name="controller_configurations",
default_value="config/iisy_controllers.yml",
description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
)
)
launch_args.append(
DeclareLaunchArgument(
name="controller",
default_value="position_trajectory_controller",
description="Robot controller."
)
)
launch_args.append(
DeclareLaunchArgument(
name="moveit_controller_configurations_package",
default_value="iisy_config",
description="Package that contains MoveIt! controller configurations."
)
)
launch_args.append(
DeclareLaunchArgument(
name="moveit_controller_configurations",
default_value="config/iisy_moveit_controllers.yml",
description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file."
)
)
return LaunchDescription(
launch_args + [
OpaqueFunction(function=launch_setup)
])

86
src/iisy_config/launch/lbr_control.launch.py Normal file
View File

@@ -0,0 +1,86 @@
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
def generate_launch_description():
# Launch arguments
launch_args = []
launch_args.append(
DeclareLaunchArgument(
name="controller_configurations_package",
default_value="iisy_config",
description="Package that contains controller configurations."
)
)
launch_args.append(
DeclareLaunchArgument(
name="controller_configurations",
default_value="config/iisy_controllers.yml",
description="Relative path to controller configurations YAML file."
)
)
launch_args.append(
DeclareLaunchArgument(
name="robot_description",
description="Robot description XML file."
)
)
launch_args.append(
DeclareLaunchArgument(
name="controller",
default_value="position_trajectory_controller",
description="Robot controller."
)
)
launch_args.append(
DeclareLaunchArgument(
name="sim",
default_value="true",
description="Launch robot in simulation or on real setup."
)
)
# Configure robot_description
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
# Load controllers from YAML configuration file
controller_configurations = PathJoinSubstitution([
FindPackageShare(LaunchConfiguration("controller_configurations_package")),
LaunchConfiguration("controller_configurations")
])
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description]
)
joint_state_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
controller = Node(
package="controller_manager",
executable="spawner",
arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"],
)
return LaunchDescription(
launch_args + [
robot_state_publisher,
joint_state_broadcaster,
controller
])

181
src/iisy_config/launch/lbr_move_group.launch.py Normal file
View File

@@ -0,0 +1,181 @@
import os
import yaml
import traceback
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
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(
LaunchConfiguration("moveit_controller_configurations_package").perform(context),
LaunchConfiguration("moveit_controller_configurations").perform(context)
)
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": LaunchConfiguration("sim")}
# 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
]
)
# 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
]
)
return [
move_group_node,
rviz
]
def generate_launch_description():
# Launch arguments
launch_args = []
launch_args.append(
DeclareLaunchArgument(
name="robot_description",
description="Robot description XML file."
)
)
launch_args.append(
DeclareLaunchArgument(
name="moveit_controller_configurations_package",
default_value="iisy_config",
description="Package that contains MoveIt! controller configurations."
)
)
launch_args.append(
DeclareLaunchArgument(
name="moveit_controller_configurations",
default_value="config/iisy_moveit_controllers.yml",
description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
)
)
launch_args.append(DeclareLaunchArgument(
name="sim",
default_value="true",
description="Launch robot in simulation or on real setup."
))
return LaunchDescription(
launch_args + [
OpaqueFunction(function=launch_setup)
])

49
src/iisy_config/launch/lbr_simulation.launch.py Normal file
View File

@@ -0,0 +1,49 @@
from launch.launch_description import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
# Launch arguments
launch_args = []
launch_args.append(DeclareLaunchArgument(
name="robot_name",
default_value="iisy",
description="Set robot name."
))
# Launch Gazebo
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("gazebo_ros"),
"launch",
"gazebo.launch.py"
])
)
)
# Note: Environment variable GAZEBO_MODEL_PATH is extended as in
# ROS2 control demos via environment hook https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description
# Also see https://colcon.readthedocs.io/en/released/developer/environment.html#dsv-files
# Gazebo launch scripts append GAZEBO_MODEL_PATH with known paths, see https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ab1ae5c05eda62674b36df74eb3be8c93cdc8761/gazebo_ros/launch/gzclient.launch.py#L26
spawn_entity = Node(
package="gazebo_ros",
executable="spawn_entity.py",
arguments=[
"-topic", "robot_description",
"-entity", LaunchConfiguration("robot_name")
],
output="screen"
)
return LaunchDescription(
launch_args + [
gazebo,
spawn_entity
])

101
src/iisy_config/launch/move_group.launch
View File

@@ -1,101 +0,0 @@
<launch>
<!-- GDB Debug Option -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix"
value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
<!-- Verbose Mode Option -->
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />
<!-- move_group settings -->
<arg name="pipeline" default="ompl" />
<arg name="allow_trajectory_execution" default="true"/>
<arg name="moveit_controller_manager" default="simple" />
<arg name="fake_execution_type" default="interpolate"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="publish_monitored_planning_scene" default="true"/>
<arg name="capabilities" default=""/>
<arg name="disable_capabilities" default=""/>
<!-- load these non-default MoveGroup capabilities (space seperated) -->
<!--
<arg name="capabilities" value="
a_package/AwsomeMotionPlanningCapability
another_package/GraspPlanningPipeline
" />
-->
<!-- inhibit these default MoveGroup capabilities (space seperated) -->
<!--
<arg name="disable_capabilities" value="
move_group/MoveGroupKinematicsService
move_group/ClearOctomapService
" />
-->
<arg name="load_robot_description" default="true" />
<!-- load URDF, SRDF and joint_limits configuration -->
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="$(arg load_robot_description)" />
</include>
<!-- Planning Pipelines -->
<group ns="move_group/planning_pipelines">
<!-- OMPL -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
</include>
<!-- CHOMP -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="chomp" />
</include>
<!-- Pilz Industrial Motion -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="pilz_industrial_motion_planner" />
</include>
<!-- Support custom planning pipeline -->
<include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg pipeline)" />
</include>
</group>
<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
<arg name="fake_execution_type" value="$(arg fake_execution_type)" />
</include>
<!-- Sensors Functionality -->
<include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_sensor_manager" value="iisy" />
</include>
<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<!-- Set the display variable, in case OpenGL code is used internally -->
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
<param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="default_planning_pipeline" value="$(arg pipeline)" />
<param name="capabilities" value="$(arg capabilities)" />
<param name="disable_capabilities" value="$(arg disable_capabilities)" />
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
</node>
</launch>

15
src/iisy_config/launch/moveit_rviz.launch
View File

@@ -1,15 +0,0 @@
<launch>
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="rviz_config" default="" />
<arg if="$(eval rviz_config=='')" name="command_args" value="" />
<arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
<node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
args="$(arg command_args)" output="screen">
</node>
</launch>

19
src/iisy_config/launch/ompl-chomp_planning_pipeline.launch.xml
View File

@@ -1,19 +0,0 @@
<launch>
<!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
<include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
<arg name="planning_adapters" value="
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
chomp/OptimizerAdapter"
/>
</include>
<!-- load chomp config -->
<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
<!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
<param name="trajectory_initialization_method" value="fillTrajectory"/>
</launch>

26
src/iisy_config/launch/ompl_planning_pipeline.launch.xml
View File

@@ -1,26 +0,0 @@
<launch>
<!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->
<arg name="planning_adapters"
default="default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints"
/>
<arg name="start_state_max_bounds_error" default="0.1" />
<arg name="jiggle_fraction" default="0.05" />
<param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
<param name="request_adapters" value="$(arg planning_adapters)" />
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->
<rosparam command="load" file="$(find iisy_config)/config/ompl_planning.yaml"/>
</launch>

15
src/iisy_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
View File

@@ -1,15 +0,0 @@
<launch>
<!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
<arg name="planning_adapters" default="" />
<param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
<param name="request_adapters" value="$(arg planning_adapters)" />
<!-- Define default planner (for all groups) -->
<param name="default_planner_config" value="PTP" />
<!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
<param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
pilz_industrial_motion_planner/MoveGroupSequenceService" />
</launch>

26
src/iisy_config/launch/planning_context.launch
View File

@@ -1,26 +0,0 @@
<launch>
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_robot_description" default="false"/>
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
<param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find gaz_simulation)/urdf/iisy.urdf"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find iisy_config)/config/iisy.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find iisy_config)/config/joint_limits.yaml"/>
<rosparam command="load" file="$(find iisy_config)/config/cartesian_limits.yaml"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find iisy_config)/config/kinematics.yaml"/>
</group>
</launch>

10
src/iisy_config/launch/planning_pipeline.launch.xml
View File

@@ -1,10 +0,0 @@
<launch>
<!-- This file makes it easy to include different planning pipelines;
It is assumed that all planning pipelines are named XXX_planning_pipeline.launch -->
<arg name="pipeline" default="ompl" />
<include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
</launch>

4
src/iisy_config/launch/ros_control_moveit_controller_manager.launch.xml
View File

@@ -1,4 +0,0 @@
<launch>
<!-- Define MoveIt controller manager plugin -->
<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
</launch>

11
src/iisy_config/launch/ros_controllers.launch
View File

@@ -1,11 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find iisy_config)/config/ros_controllers.yaml" command="load"/>
<!-- Load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="iisy_controller "/>
</launch>

21
src/iisy_config/launch/run_benchmark_ompl.launch
View File

@@ -1,21 +0,0 @@
<launch>
<!-- This argument must specify the list of .cfg files to process for benchmarking -->
<arg name="cfg" />
<!-- Load URDF -->
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- Start the database -->
<include file="$(dirname)/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
</include>
<!-- Start Benchmark Executable -->
<node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
<rosparam command="load" file="$(find iisy_config)/config/ompl_planning.yaml"/>
</node>
</launch>

17
src/iisy_config/launch/sensor_manager.launch.xml
View File

@@ -1,17 +0,0 @@
<launch>
<!-- This file makes it easy to include the settings for sensor managers -->
<!-- Params for 3D sensors config -->
<rosparam command="load" file="$(find iisy_config)/config/sensors_3d.yaml" />
<!-- Params for the octomap monitor -->
<!-- <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
<param name="octomap_resolution" type="double" value="0.025" />
<param name="max_range" type="double" value="5.0" />
<!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
<arg name="moveit_sensor_manager" default="iisy" />
<include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

16
src/iisy_config/launch/setup_assistant.launch
View File

@@ -1,16 +0,0 @@
<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
<launch>
<!-- Debug Info -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<!-- Run -->
<node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
args="--config_pkg=iisy_config"
launch-prefix="$(arg launch_prefix)"
required="true"
output="screen" />
</launch>

8
src/iisy_config/launch/simple_moveit_controller_manager.launch.xml
View File

@@ -1,8 +0,0 @@
<launch>
<!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<!-- Load controller list to the parameter server -->
<rosparam file="$(find iisy_config)/config/simple_moveit_controllers.yaml" />
<rosparam file="$(find iisy_config)/config/ros_controllers.yaml" />
</launch>

25
src/iisy_config/launch/stomp_planning_pipeline.launch.xml
View File

@@ -1,25 +0,0 @@
<launch>
<!-- Stomp Plugin for MoveIt -->
<arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
<arg name="start_state_max_bounds_error" value="0.1" />
<arg name="jiggle_fraction" value="0.05" />
<!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
<arg name="planning_adapters"
default="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" />
<param name="planning_plugin" value="$(arg planning_plugin)" />
<param name="request_adapters" value="$(arg planning_adapters)" />
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->
<rosparam command="load" file="$(find iisy_config)/config/stomp_planning.yaml"/>
</launch>

23
src/iisy_config/launch/trajectory_execution.launch.xml
View File

@@ -1,23 +0,0 @@
<launch>
<!-- This file summarizes all settings required for trajectory execution -->
<!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" />
<arg name="fake_execution_type" default="interpolate" />
<!-- Flag indicating whether MoveIt is allowed to load/unload or switch controllers -->
<arg name="moveit_manage_controllers" default="true"/>
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
<param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
<!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
<!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
<param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
<!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
<include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
</launch>

93
src/iisy_config/launch/view_robot.launch.py Normal file
View File

@@ -0,0 +1,93 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
# for reference see
# https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description
def generate_launch_description():
# Launch arguments
launch_args = []
launch_args.append(DeclareLaunchArgument(
name='description_package',
default_value='iisy_config',
description='Description package.'
))
launch_args.append(DeclareLaunchArgument(
name='description_file',
default_value='urdf/iisy.urdf',
description='Path to URDF file, relative to description_package.'
))
launch_args.append(DeclareLaunchArgument(
name='rviz_config',
default_value='config/config.rviz',
description='Rviz configuration relative to description_package.'
))
launch_args.append(DeclareLaunchArgument(
name='robot_name',
default_value='iisy',
description='Set robot name.'
))
launch_args.append(DeclareLaunchArgument(
name='origin_xyz',
default_value="'0 0 0'",
description='Set position origin of robot.'
))
launch_args.append(DeclareLaunchArgument(
name='origin_rpy',
default_value="'0 0 0'",
description='Set orientation origin of robot.'
))
# Load robot description
robot_description_content = Command(
[
FindExecutable(name="xacro"), " ",
PathJoinSubstitution(
[FindPackageShare(LaunchConfiguration('description_package')), LaunchConfiguration('description_file')]
), " ",
"origin_xyz:=", LaunchConfiguration('origin_xyz'), " ",
"origin_rpy:=", LaunchConfiguration('origin_rpy'), " ",
"robot_name:=", LaunchConfiguration('robot_name')
]
)
robot_description = {'robot_description': robot_description_content}
# Create required nodes
joint_state_publisher_node = Node(
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz = Node(
package='rviz2',
executable='rviz2',
parameters=[{'config': PathJoinSubstitution(
[FindPackageShare(LaunchConfiguration('description_package')), LaunchConfiguration('rviz_config')]
)}]
)
return LaunchDescription(
launch_args +
[
joint_state_publisher_node,
robot_state_publisher_node,
rviz
]
)

15
src/iisy_config/launch/warehouse.launch
View File

@@ -1,15 +0,0 @@
<launch>
<!-- The path to the database must be specified -->
<arg name="moveit_warehouse_database_path" />
<!-- Load warehouse parameters -->
<include file="$(dirname)/warehouse_settings.launch.xml" />
<!-- Run the DB server -->
<node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
<param name="overwrite" value="false"/>
<param name="database_path" value="$(arg moveit_warehouse_database_path)" />
</node>
</launch>

16
src/iisy_config/launch/warehouse_settings.launch.xml
View File

@@ -1,16 +0,0 @@
<launch>
<!-- Set the parameters for the warehouse and run the mongodb server. -->
<!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
<arg name="moveit_warehouse_port" default="33829" />
<!-- The default DB host for moveit -->
<arg name="moveit_warehouse_host" default="localhost" />
<!-- Set parameters for the warehouse -->
<param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
<param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
<param name="warehouse_exec" value="mongod" />
<param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
</launch>

0
src/iisy_config/config/iisy.srdf → src/iisy_config/srdf/iisy.srdf
View File

BIN
src/iisy_config/stl/base_bottom_coll.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/base_bottom_low.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/base_top_low.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_1_full_coll.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_1_full_low.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_2_bottom_low.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_2_top_coll.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_2_top_low.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_3_bottom_coll.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_3_bottom_low.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_3_top_coll.stl Executable file
View File

Binary file not shown.

BIN
src/iisy_config/stl/link_3_top_low.stl Executable file
View File

Binary file not shown.

407
src/iisy_config/urdf/iisy.urdf Normal file
View File

@@ -0,0 +1,407 @@
<?xml version="1.0" ?>
<robot name="iisy" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="joint_damping" value="10.0"/>
<xacro:property name="joint_friction" value="0.1"/>
<link name="world"/>
<link name="base_link"/>
<link name="base">
<collision>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/base_bottom_coll.stl"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/base_bottom_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0.025 0 0.05" rpy="0 0 0"/>
<mass value="6"/>
<inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
</inertial>
</link>
<link name="base_top">
<collision>
<origin xyz="0 -0.005 0.08"/>
<geometry>
<box size="0.14 0.15 0.16"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/base_top_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.155" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
</inertial>
</link>
<link name="link1_full">
<collision>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_1_full_coll.stl"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_1_full_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.36" rpy="0 0 0"/>
<mass value="4"/>
<inertia ixx="0.0325" ixy="0.0" ixz="0.0" iyy="0.0325" iyz="0.0" izz="0.005"/>
</inertial>
</link>
<link name="link2_bottom">
<collision>
<origin xyz="0 0.07 0.025"/>
<geometry>
<box size="0.14 0.14 0.19"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_2_bottom_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.585" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
</inertial>
</link>
<link name="link2_top">
<collision>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_2_top_coll.stl"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_2_top_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.735" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
</inertial>
</link>
<link name="link3_bottom">
<collision>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_bottom_coll.stl"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_bottom_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.86" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.0029166666666667" ixy="0.0" ixz="0.0" iyy="0.0029166666666667" iyz="0.0" izz="0.0025"/>
</inertial>
</link>
<link name="link3_top">
<collision>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_top_coll.stl"/>
</geometry>
</collision>
<visual>
<geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_top_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.94025" rpy="0 0 0"/>
<mass value="0.8"/>
<inertia ixx="0.00074401666666667" ixy="0.0" ixz="0.0" iyy="0.00074401666666667" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint type="fixed" name="world_base_link_fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
<joint type="fixed" name="base_base_link_fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="base"/>
</joint>
<joint type="continuous" name="base_rot">
<origin xyz="0 0 0.1205" rpy="0 0 0"/>
<parent link="base"/>
<child link="base_top"/>
<axis xyz="0 0 1"/>
<limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="${joint_damping}" friction="${joint_friction}"/>
</joint>
<joint type="revolute" name="base_link1_joint">
<origin xyz="0 -0.080499 0.092997" rpy="0 0 0"/>
<parent link="base_top"/>
<child link="link1_full"/>
<axis xyz="0 1 0"/>
<limit effort="30" velocity="1.7453292519943" lower="-2.2689280275926" upper="2.4434609527921"/>
<dynamics damping="${joint_damping}" friction="${joint_friction}"/>
</joint>
<joint type="revolute" name="link1_link2_joint">
<origin xyz="0 0 0.3" rpy="0 0 0"/>
<parent link="link1_full"/>
<child link="link2_bottom"/>
<axis xyz="0 1 0"/>
<limit effort="30" velocity="1.7453292519943" lower="-2.6179938779915" upper="2.6179938779915"/>
<dynamics damping="${joint_damping}" friction="${joint_friction}"/>
</joint>
<joint type="continuous" name="link2_rot">
<origin xyz="0 0.080501 0.120502" rpy="0 0 0"/>
<parent link="link2_bottom"/>
<child link="link2_top"/>
<axis xyz="0 0 1"/>
<limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="${joint_damping}" friction="${joint_friction}"/>
</joint>
<joint type="continuous" name="link2_link3_joint">
<origin xyz="0 0.0565 0.178003" rpy="0 0 0"/>
<parent link="link2_top"/>
<child link="link3_bottom"/>
<axis xyz="0 1 0"/>
<limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="${joint_damping}" friction="${joint_friction}"/>
</joint>
<joint type="continuous" name="link3_rot">
<origin xyz="0 -0.055001 0.085501" rpy="0 0 0"/>
<parent link="link3_bottom"/>
<child link="link3_top"/>
<axis xyz="0 0 1"/>
<limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="${joint_damping}" friction="${joint_friction}"/>
</joint>
<!--<transmission name="trans_base_rot">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_rot">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="base_rot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_base_link1_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_link1_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="base_link1_joint_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link1_link2_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link1_link2_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link1_link2_joint_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link2_rot">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link2_rot">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link2_rot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link2_link3_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link2_link3_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link2_link3_joint_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link3_rot">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link3_rot">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link3_rot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>-->
<ros2_control name="lbr_hardware_interface" type="system">
<!-- define hardware including parameters, also gazebo -->
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<!-- define lbr specific state interfaces as sensor, see https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
<sensor name="iisy_state">
<!-- see KUKA::FRI::LBRState -->
<state_interface name="sample_time"/>
<state_interface name="time_stamp_sec"/>
<state_interface name="time_stamp_nano_sec"/>
</sensor>
<!-- define joints and command/state interfaces for each joint -->
<joint name="base_rot">
<command_interface name="position">
<!-- better to use radians as min max first -->
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="base_link1_joint">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="link1_link2_joint">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="link2_rot">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="link2_link3_joint">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="link3_rot">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
</ros2_control>
<gazebo>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters>
<robotNamespace>/iisy</robotNamespace>
</plugin>
</gazebo>
<gazebo reference="base">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="base_top">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link1_full">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link2_bottom">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link2_top">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link3_bottom">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link3_top">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Grey</material>
</gazebo>
</robot>