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

Reverting accidental commit.

Browse Source
This commit is contained in:
Bastian Hofmann
2022-03-25 17:55:18 +01:00
parent df9512a2b2
commit 435332c054
41 changed files with 674 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/iisy_config/.setup_assistant Normal file
View File

@@ -0,0 +1,11 @@
moveit_setup_assistant_config:
URDF:
package: gaz_simulation
relative_path: urdf/iisy.urdf
xacro_args: ""
SRDF:
relative_path: config/iisy.srdf
CONFIG:
author_name: bastian
author_email: bastian.hofmann@xitaso.com
generated_timestamp: 1647313439

30
src/iisy_config/CMakeLists.txt Normal file
View File

@@ -0,0 +1,30 @@
cmake_minimum_required(VERSION 3.8)
project(iisy_config)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
install(
DIRECTORY config launch srdf stl urdf
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

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

4
src/iisy_config/config/kinematics.yml Normal file
View File

@@ -0,0 +1,4 @@
iisy:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005

157
src/iisy_config/config/ompl_planning.yml Normal file
View File

@@ -0,0 +1,157 @@
planner_configs:
AnytimePathShortening:
type: geometric::AnytimePathShortening
shortcut: true # Attempt to shortcut all new solution paths
hybridize: true # Compute hybrid solution trajectories
max_hybrid_paths: 24 # Number of hybrid paths generated per iteration
num_planners: 4 # The number of default planners to use for planning
planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
SBL:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
EST:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECE:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECE:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
KPIECE:
type: geometric::KPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
RRT:
type: geometric::RRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
RRTConnect:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstar:
type: geometric::RRTstar
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
TRRT:
type: geometric::TRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
PRM:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PRMstar:
type: geometric::PRMstar
FMT:
type: geometric::FMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
nearest_k: 1 # use Knearest strategy. default: 1
cache_cc: 1 # use collision checking cache. default: 1
heuristics: 0 # activate cost to go heuristics. default: 0
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
BFMT:
type: geometric::BFMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
nearest_k: 1 # use the Knearest strategy. default: 1
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
heuristics: 1 # activates cost to go heuristics. default: 1
cache_cc: 1 # use the collision checking cache. default: 1
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
PDST:
type: geometric::PDST
STRIDE:
type: geometric::STRIDE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
max_degree: 18 # max degree of a node in the GNAT. default: 12
min_degree: 12 # min degree of a node in the GNAT. default: 12
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
BiTRRT:
type: geometric::BiTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
init_temperature: 100 # initial temperature. default: 100
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
LBTRRT:
type: geometric::LBTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
epsilon: 0.4 # optimality approximation factor. default: 0.4
BiEST:
type: geometric::BiEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ProjEST:
type: geometric::ProjEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LazyPRM:
type: geometric::LazyPRM
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
LazyPRMstar:
type: geometric::LazyPRMstar
SPARS:
type: geometric::SPARS
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 1000 # maximum consecutive failure limit. default: 1000
SPARStwo:
type: geometric::SPARStwo
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
iisy:
planner_configs:
- AnytimePathShortening
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
projection_evaluator: joints(base_rot,base_link1_joint)
longest_valid_segment_fraction: 0.005

9
src/iisy_config/config/sensors_3d.yml Normal file
View File

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

57
src/iisy_config/launch/debug.launch.py Normal file
View File

@@ -0,0 +1,57 @@
import os
from launch.launch_description import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, SetEnvironmentVariable, UnsetEnvironmentVariable
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():
package_name = "gaz_simulation"
pkg_gazebo_ros = FindPackageShare('gazebo_ros').find('gazebo_ros')
path = FindPackageShare(package=package_name).find(package_name)
world_path = os.path.join(path, "world/test2.xml")
# 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", "iisy"
],
output="screen"
)
bringup = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("iisy_config"),
"launch",
"lbr_bringup.launch.py"
])
),
)
return LaunchDescription([
SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
launch_arguments={'world': world_path}.items()
),
UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),
# Start Gazebo client
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
),
spawn_entity,
bringup,
])

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

@@ -0,0 +1,78 @@
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):
# Load robot description
robot_description_content = Command(
[
FindExecutable(name="xacro"), " ",
PathJoinSubstitution(
[FindPackageShare("iisy_config"), "urdf/iisy.urdf"]
), " ",
"robot_name:=", "iisy", " ",
"sim:=", "true"
]
)
# Load controls
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("iisy_config"),
"launch",
"lbr_control.launch.py"
])
), launch_arguments=[
("robot_description", robot_description_content)
]
)
# Gazebo simulation
#simulation = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# PathJoinSubstitution([
# FindPackageShare("iisy_config"),
# "launch",
# "lbr_simulation.launch.py"
# ])
# ),
#)
# Move group
move_group = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("iisy_config"),
"launch",
"lbr_move_group.launch.py"
])
),
launch_arguments=[
("robot_description", robot_description_content)
]
)
return [
#simulation,
control,
move_group
]
def generate_launch_description():
# Launch arguments
launch_args = []
return LaunchDescription([
OpaqueFunction(function=launch_setup)
])

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

@@ -0,0 +1,55 @@
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="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"
])
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=["position_trajectory_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, 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)
])

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

@@ -0,0 +1,38 @@
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 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", "iisy"
],
output="screen"
)
return LaunchDescription([
gazebo,
spawn_entity
])

131
src/iisy_config/launch/moveit.rviz Normal file
View File

@@ -0,0 +1,131 @@
Panels:
- Class: rviz/Displays
Help Height: 84
Name: Displays
Property Tree Widget:
Expanded:
- /MotionPlanning1
Splitter Ratio: 0.5
Tree Height: 226
- Class: rviz/Help
Name: Help
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
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
- Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Name: MotionPlanning
Planned Path:
Links: ~
Loop Animation: true
Robot Alpha: 0.5
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
Trajectory Topic: move_group/display_planned_path
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
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
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 1
Start State Color: 0; 255; 0
Planning Scene Topic: move_group/monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 1
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links: ~
Robot Alpha: 0.5
Show Scene Robot: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.0
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.75
Focal Point:
X: -0.1
Y: 0.25
Z: 0.30
Focal Shape Fixed Size: true
Focal Shape Size: 0.05
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.5
Target Frame: world
Yaw: -0.6232355833053589
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 848
Help:
collapsed: false
Hide Left Dock: false
Hide Right Dock: false
MotionPlanning:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
Width: 1291
X: 454
Y: 25

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

46
src/iisy_config/package.xml Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>iisy_config</name>
<version>0.3.0</version>
<description>
An automatically generated package with all the configuration and launch files for using the iisy with the MoveIt Motion Planning Framework
</description>
<author email="bastian.hofmann@xitaso.com">bastian</author>
<maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
<license>BSD</license>
<url type="website">http://moveit.ros.org/</url>
<url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
<url type="repository">https://github.com/ros-planning/moveit</url>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>moveit_ros_move_group</depend>
<!--<depend>moveit_fake_controller_manager</depend>-->
<depend>moveit_kinematics</depend>
<depend>moveit_planners_ompl</depend>
<depend>moveit_ros_visualization</depend>
<depend>moveit_setup_assistant</depend>
<depend>moveit_simple_controller_manager</depend>
<depend>joint_state_publisher</depend>
<depend>joint_state_publisher_gui</depend>
<depend>robot_state_publisher</depend>
<depend>rviz2</depend>
<depend>tf2_ros</depend>
<depend>xacro</depend>
<!-- The next 2 packages are required for the gazebo simulation.
We don't include them by default to prevent installing gazebo and all its dependencies. -->
<!-- <depend>joint_trajectory_controller</depend> -->
<!-- <depend>gazebo_ros_control</depend> -->
<!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
<!-- <depend>warehouse_ros_mongo</depend> -->
<depend>gaz_simulation</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

39
src/iisy_config/srdf/iisy.srdf Normal file
View File

@@ -0,0 +1,39 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="iisy">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="iisy">
<joint name="base_rot"/>
<joint name="base_link1_joint"/>
<joint name="link1_link2_joint"/>
<joint name="link2_rot"/>
<joint name="link2_link3_joint"/>
<joint name="link3_rot"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="home" group="iisy">
<joint name="base_link1_joint" value="0"/>
<joint name="base_rot" value="0"/>
<joint name="link1_link2_joint" value="0"/>
<joint name="link2_link3_joint" value="0"/>
<joint name="link2_rot" value="0"/>
<joint name="link3_rot" value="0"/>
</group_state>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="base_bottom" link2="base_top" reason="Adjacent"/>
<disable_collisions link1="base_top" link2="link1_full" reason="Adjacent"/>
<disable_collisions link1="base_top" link2="link2_bottom" reason="Never"/>
<disable_collisions link1="link1_full" link2="link2_bottom" reason="Adjacent"/>
<disable_collisions link1="link1_full" link2="link3_bottom" reason="Never"/>
<disable_collisions link1="link2_bottom" link2="link2_top" reason="Adjacent"/>
<disable_collisions link1="link2_bottom" link2="link3_bottom" reason="Never"/>
<disable_collisions link1="link2_top" link2="link3_bottom" reason="Adjacent"/>
<disable_collisions link1="link3_bottom" link2="link3_top" reason="Adjacent"/>
</robot>

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_bottom">
<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.043517 0.000000 0.054914" rpy="0 0 0"/>
<mass value="6"/>
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
</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.005 0.08" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
</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.050000 0.150000" rpy="0 0 0"/>
<mass value="4"/>
<inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
</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.07 0.025" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
</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.037363 0.086674" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
</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.055000 0.021413" rpy="0 0 0"/>
<mass value="2"/>
<inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
</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.015462 0.040000" rpy="0 0 0"/>
<mass value="0.8"/>
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
</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_bottom"/>
</joint>
<joint type="continuous" name="base_rot">
<origin xyz="0 0 0.1205" rpy="0 0 0"/>
<parent link="base_bottom"/>
<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_bottom">
<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>