Add minimal pick-and-place case with world embedded

This commit is contained in:
Igor Brylyov 2023-03-31 20:24:56 +00:00
parent 209e99a4b3
commit a87fb8a7ec
64 changed files with 2419 additions and 275 deletions

View file

@ -12,10 +12,12 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED)
install(
DIRECTORY launch
DIRECTORY launch worlds
DESTINATION share/${PROJECT_NAME}
)
add_subdirectory(models)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights

View file

@ -3,18 +3,15 @@ from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
RegisterEventHandler
ExecuteProcess
)
from ament_index_python.packages import get_package_share_directory, get_package_share_path
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition
from launch.event_handlers import OnProcessExit, OnExecutionComplete
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ur_moveit_config.launch_common import load_yaml
def generate_launch_description():
declared_arguments = []
# UR specific arguments
@ -26,6 +23,13 @@ def generate_launch_description():
default_value="ur5e",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"with_gripper",
default_value="true",
description="With gripper or not?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
@ -63,6 +67,13 @@ def generate_launch_description():
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_with_gripper_file",
default_value="ur_plus_gripper_controllers.yaml",
description="YAML file with the UR + gripper_controller configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
@ -101,6 +112,14 @@ def generate_launch_description():
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_gripper_controller",
default_value="gripper_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
@ -129,26 +148,31 @@ def generate_launch_description():
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)
# Initialize Arguments
# TODO тут всё переименовать и сделать боеле универсальным как под нашего робото так и под UR чтобы запускалось одинаково
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_with_gripper_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
initial_gripper_controller = LaunchConfiguration("initial_gripper_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
initial_joint_controllers = PathJoinSubstitution(
initial_joint_controllers_file_path = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file]
)
@ -157,7 +181,7 @@ def generate_launch_description():
)
world_config_file = PathJoinSubstitution(
[FindPackageShare("sdf_models"), "worlds", "empty_world.sdf"] # TODO тут пакет извне должен задаваться
[FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"]
)
robot_description_content = Command(
@ -180,7 +204,7 @@ def generate_launch_description():
"name:=",
"ur",
" ",
"ur_type:=", # TODO сделать более универсальным
"ur_type:=",
rbs_robot_type,
" ",
"prefix:=",
@ -189,7 +213,10 @@ def generate_launch_description():
"sim_ignition:=true",
" ",
"simulation_controllers:=",
initial_joint_controllers,
initial_joint_controllers_file_path,
" ",
"with_gripper:=",
with_gripper_condition
]
)
robot_description = {"robot_description": robot_description_content}
@ -220,13 +247,20 @@ def generate_launch_description():
arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"],
condition=UnlessCondition(start_joint_controller),
)
gripper_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
"gripper_controller"],
output='screen',
condition=IfCondition(with_gripper_condition)
)
# Gazebo nodes
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_path('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')),
launch_arguments={'ign_args': ['-r'," ", world_config_file]}.items(),
)
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_ign_gazebo'),
'launch', 'ign_gazebo.launch.py')]),
launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
# Spawn robot
gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
arguments=[
@ -252,6 +286,8 @@ def generate_launch_description():
"prefix:=",
prefix,
" ",
"with_gripper:=",
with_gripper_condition
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
@ -260,7 +296,6 @@ def generate_launch_description():
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
# Planning Configuration
ompl_planning_pipeline_config = {
"move_group": {
@ -269,16 +304,10 @@ def generate_launch_description():
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") # TODO сделать извне
ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
# the scaled_joint_trajectory_controller does not work on fake hardware
change_controllers = True
if change_controllers == "true":
controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
controllers_yaml["joint_trajectory_controller"]["default"] = True
moveit_controllers = {
"moveit_simple_controller_manager": controllers_yaml,
@ -287,7 +316,7 @@ def generate_launch_description():
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_execution_duration_scaling": 100.0,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
@ -299,12 +328,10 @@ def generate_launch_description():
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
name="move_group",
parameters=[
robot_description,
robot_description_semantic,
@ -326,24 +353,40 @@ def generate_launch_description():
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
robot_description_kinematics,
],
condition=IfCondition(launch_rviz),
)
# TODO: Launch skill servers in other launch file
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
name="move_to_pose_moveit",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time},
]
)
@ -351,19 +394,49 @@ def generate_launch_description():
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
name="move_cartesian_path_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time},
]
)
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml")
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader",
output="screen",
emulate_tty=True,
parameters=[
points_params
]
)
# add_planning_scene_object = Node(
# package="rbs_skill_servers",
# executable="add_planning_scene_object_service",
# output="screen",
# parameters=[
# robot_description,
# robot_description_semantic,
# robot_description_kinematics,
# {"use_sim_time": use_sim_time},
# ]
# )
nodes_to_start = [
robot_state_publisher_node,
joint_state_broadcaster_spawner,
@ -373,8 +446,13 @@ def generate_launch_description():
gazebo,
gazebo_spawn_robot,
move_group_node,
gripper_controller,
gripper_control_node,
move_topose_action_server,
move_cartesian_path_action_server,
move_joint_state_action_server,
grasp_pose_loader,
#add_planning_scene_object
]
return LaunchDescription(declared_arguments + nodes_to_start)

View file

@ -0,0 +1,21 @@
macro(list_subdirectories retval curdir return_relative)
file(GLOB sub-dir RELATIVE ${curdir} *)
set(list_of_dirs "")
foreach(dir ${sub-dir})
if(IS_DIRECTORY ${curdir}/${dir})
if (${return_relative})
set(list_of_dirs ${list_of_dirs} ${dir})
else()
set(list_of_dirs ${list_of_dirs} ${curdir}/${dir})
endif()
endif()
endforeach()
set(${retval} ${list_of_dirs})
endmacro()
list_subdirectories(SUBDIR ${CMAKE_CURRENT_LIST_DIR} TRUE)
install(
DIRECTORY ${SUBDIR}
DESTINATION share/${PROJECT_NAME}
)

View file

@ -0,0 +1,40 @@
{
"label": "box1",
"type": "component",
"include": [],
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature057": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.149
},
"orientation": {
"x": 0.0,
"y": 1.0,
"z": 0.0,
"w": 0.0
}
},
"distance": 0.015
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<model>
<name>box1</name>
<sdf version="1.0">model.sdf</sdf>
<author>
<name>Splinter1984</name>
<email>rom.andrianov1984@gmail.com</email>
</author>
<description>
A box1.
</description>
</model>

View file

@ -0,0 +1,58 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box1">
<pose>0 0 0.015 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.00004167</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00004167</iyy>
<iyz>0.0</iyz>
<izz>0.00004167</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<!-- <gravity>0</gravity> -->
<collision name="collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode>
<slip>
0.3
</slip>
</ode>
</torsional>
<ode>
<mu>0.2</mu>
<mu2>0.2</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<!-- <material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material> -->
</visual>
</link>
</model>
</sdf>

View file

@ -0,0 +1,40 @@
{
"label": "box12",
"type": "assembly",
"include": ["box1", "box2"],
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature059": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.129
},
"orientation": {
"x": 0.0,
"y": 1.0,
"z": 0.0,
"w": 0.0
}
},
"distance": 0.015
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<model>
<name>box12</name>
<sdf version="1.0">model.sdf</sdf>
<author>
<name>Splinter1984</name>
<email>rom.andrianov1984@gmail.com</email>
</author>
<description>
A box12.
</description>
</model>

View file

@ -0,0 +1,19 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box12">
<pose>0 0 0.015 0 0 0</pose>
<link name="box1">
<pose>0 0 0 0 0 0</pose>
<gravity>0</gravity>
</link>
<link name="box2">
<pose>0 0 0.03 0 0 0</pose>
<gravity>0</gravity>
</link>
<joint name="joint" type="fixed">
<parent>box1</parent>
<child>box2</child>
<pose>0.0 0.0 0.06 0.0 0.0 0.0</pose>
</joint>
</model>
</sdf>

View file

@ -0,0 +1,40 @@
{
"label": "box2",
"type": "component",
"include": [],
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature058": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.129
},
"orientation": {
"x": 0.0,
"y": 1.0,
"z": 0.0,
"w": 0.0
}
},
"distance": 0.015
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<model>
<name>box2</name>
<sdf version="1.0">model.sdf</sdf>
<author>
<name>Splinter1984</name>
<email>rom.andrianov1984@gmail.com</email>
</author>
<description>
A box2.
</description>
</model>

View file

@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box2">
<pose>0 0 0.015 0 0 0</pose>
<link name="link">
<gravity>0</gravity>
<collision name="collision">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
<material>
<ambient>0.1 0.75 0 5</ambient>
<diffuse>0.9 0.7 0 1</diffuse>
<specular>0.4 0.4 0.4 64</specular>
<emissive>0.2 0 0.2 2</emissive>
</material>
</visual>
</link>
</model>
</sdf>

View file

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>rack</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name>brothermechanic</name>
<email>brothermechanic@gmail.com</email>
</author>
<description>
rack
</description>
</model>

View file

@ -0,0 +1,25 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="rack">
<pose>0 0 0 0 0 0</pose>
<static>true</static>
<link name="body">
<visual name="visual">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://rack/model/rack.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="collision">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://rack/model/rack.stl</uri>
</mesh>
</geometry>
</collision>
</link>
</model>
</sdf>

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.6 MiB

File diff suppressed because one or more lines are too long

Binary file not shown.

View file

@ -0,0 +1,35 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rack">
<xacro:macro name="rack" params="*origin parent">
<link name="rack_body">
<static>true</static>
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<mesh filename="package://rbs_simulation/rack/model/rack.dae" scale="0.35 0.35 0.35"/>
</geometry>
<material name="asd">
<color rgba="1.0 0.0 0.0 1.0"/>
<texture filename="package://rbs_simulation/rack/model/Metal.jpg"/>
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<mesh filename="package://rbs_simulation/rack/model/rack.stl" scale="0.35 0.35 0.35"/>
</geometry>
</collision>
</link>
<joint name="rack_base_joint" type="fixed">
<parent link="${parent}" />
<child link="rack_body" />
<xacro:insert_block name="origin" />
</joint>
</xacro:macro>
</robot>

View file

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>table</name>
<version>1.0</version>
<sdf version="1.6">table.sdf</sdf>
<author>
<name>brothermechanic</name>
<email>brothermechanic@gmail.com</email>
</author>
<description>
table
</description>
</model>

View file

@ -0,0 +1,25 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="table">
<pose>0 0 0 0 0 0</pose>
<static>true</static>
<link name="body">
<visual name="visual">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://table/model/table.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="collision">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://table/model/table.stl</uri>
</mesh>
</geometry>
</collision>
</link>
</model>
</sdf>

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.6 MiB

File diff suppressed because one or more lines are too long

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 74 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 599 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.4 MiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 878 KiB

View file

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="table">
<xacro:macro name="table" params="*origin parent">
<link name="table_body">
<static>true</static>
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- <mesh filename="package://rbs_simulation/table/model/table.dae" scale="1 1 1"/> -->
<box size="0.8 0.8 0.4"/>
</geometry>
<material name="asd">
<color rgba="1.0 1.0 0.0 1.0"/>
<!-- <texture filename="package://rbs_simulation/table/model/Metal.jpg"/> -->
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- <mesh filename="package://rbs_simulation/table/model/table.stl" scale="1 1 1"/> -->
<box size="0.8 0.8 0.4"/>
</geometry>
</collision>
</link>
<joint name="table_base_joint" type="fixed">
<parent link="${parent}" />
<child link="table_body" />
<xacro:insert_block name="origin" />
</joint>
</xacro:macro>
</robot>

View file

@ -0,0 +1,119 @@
<sdf version='1.9'>
<world name='empty'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<model name='ground_plane'>
<static>true</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<bounce/>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<enable_wind>false</enable_wind>
</link>
<pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide>
</model>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>0.3 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>0.2 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>0.1 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>0 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box5</name>
<pose>-0.1 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box6</name>
<pose>-0.2 0.4 0 0 0 0</pose>
</include>
<light name='sun' type='directional'>
<pose>0 0 10 0 -0 0</pose>
<cast_shadows>true</cast_shadows>
<intensity>1</intensity>
<direction>-0.5 0.1 -0.9</direction>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<linear>0.01</linear>
<constant>0.90000000000000002</constant>
<quadratic>0.001</quadratic>
</attenuation>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
</world>
</sdf>

View file

@ -0,0 +1,243 @@
<?xml version="1.0"?>
<sdf version='1.9'>
<world name='mir'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>false</shadows>
</scene>
<gui fullscreen="0">
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>1.0 1.0 1.0</ambient_light>
<background_color>0.4 0.6 1.0</background_color>
<camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
</plugin>
<plugin filename="WorldStats" name="World stats">
<ignition-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>
<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
</plugin>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground'>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='rack'>
<pose>0 0.7 0 0 0 0</pose>
<static>1</static>
<link name='body'>
<visual name='visual'>
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>model://rack/model/rack.dae</uri>
</mesh>
</geometry>
</visual>
<collision name='collision'>
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>model://rack/model/rack.stl</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='table'>
<pose>-0.75 0 0.2 0 0 0</pose>
<static>1</static>
<link name='telo'>
<visual name='visual'>
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>model://table/model/table.dae</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>model://table/model/table.stl</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>0 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>0.25 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box5</name>
<pose>0 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box6</name>
<pose>-0.25 0.65 0.3015 0 0 0</pose>
</include>
<!-- <include>
<uri>model://box1</uri>
<name>pyramid1</name>
<pose>-0.9000 -0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid2</name>
<pose>-0.9000 0.0 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid3</name>
<pose>-0.9000 0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid4</name>
<pose>-0.9 -0.0243 0.5750 0 -0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid5</name>
<pose>-0.9 0.0260 0.5750 0 -0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid6</name>
<pose>-0.9 0 0.6250 0 0 0</pose>
</include> -->
</world>
</sdf>

View file

@ -0,0 +1,493 @@
<sdf version='1.9'>
<world name='empty'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<model name='ground_plane'>
<static>true</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<bounce/>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<enable_wind>false</enable_wind>
</link>
<pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide>
</model>
<model name='ur5e'>
<joint name='base_joint' type='fixed'>
<pose>0 0 0 0 -0 0</pose>
<parent>world</parent>
<child>base_link</child>
</joint>
<link name='base_link'>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.0044333300000000001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0044333300000000001</iyy>
<iyz>0</iyz>
<izz>0.0071999999999999998</izz>
</inertia>
</inertial>
<collision name='base_link_inertia_collision'>
<pose>0 0 0 0 0 -2.449293598294706e-16</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/base.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='base_link_inertia_visual'>
<pose>0 0 0 0 0 -2.449293598294706e-16</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/base.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='shoulder_pan_joint' type='revolute'>
<pose>0 0 0 0 -0 0</pose>
<parent>base_link</parent>
<child>shoulder_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>150</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='shoulder_link'>
<pose>0 0 0.1625 0 -0 -3.14159</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>3.7000000000000002</mass>
<inertia>
<ixx>0.010267500000000001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.010267500000000001</iyy>
<iyz>0</iyz>
<izz>0.0066600000000000001</izz>
</inertia>
</inertial>
<collision name='shoulder_link_collision'>
<pose>0 0 0 0 0 3.141592653589793</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/shoulder.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='shoulder_link_visual'>
<pose>0 0 0 0 0 3.141592653589793</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/shoulder.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='shoulder_lift_joint' type='revolute'>
<pose>-0 0 0 0 -0 0</pose>
<parent>shoulder_link</parent>
<child>upper_arm_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>150</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='upper_arm_link'>
<pose>0 -0 0.1625 1.5708 1.57 -3.14159</pose>
<inertial>
<pose>-0.2125 0 0.138 0 1.5708 0</pose>
<mass>8.3930000000000007</mass>
<inertia>
<ixx>0.13388600000000001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.13388600000000001</iyy>
<iyz>0</iyz>
<izz>0.0151074</izz>
</inertia>
</inertial>
<collision name='upper_arm_link_collision'>
<pose>0 0 0.138 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/upperarm.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='upper_arm_link_visual'>
<pose>0 0 0.138 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/upperarm.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='elbow_joint' type='revolute'>
<pose>0 -0 0 0 -0 0</pose>
<parent>upper_arm_link</parent>
<child>forearm_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.1415899999999999</lower>
<upper>3.1415899999999999</upper>
<effort>150</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='forearm_link'>
<pose>0.000338 0 0.5875 1.5708 1.57 -3.14159</pose>
<inertial>
<pose>-0.1961 0 0.007 0 1.5708 0</pose>
<mass>2.2749999999999999</mass>
<inertia>
<ixx>0.031209400000000002</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.031209400000000002</iyy>
<iyz>0</iyz>
<izz>0.0040949999999999997</izz>
</inertia>
</inertial>
<collision name='forearm_link_collision'>
<pose>0 0 0.007 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/forearm.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='forearm_link_visual'>
<pose>0 0 0.007 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/forearm.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='wrist_1_joint' type='revolute'>
<pose>0 0 0 0 -0 0</pose>
<parent>forearm_link</parent>
<child>wrist_1_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>28</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='wrist_1_link'>
<pose>0.000651 0.1333 0.9797 -1.5708 0.001593 -0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1.2190000000000001</mass>
<inertia>
<ixx>0.0025598999999999999</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0025598999999999999</iyy>
<iyz>0</iyz>
<izz>0.0021941999999999999</izz>
</inertia>
</inertial>
<collision name='wrist_1_link_collision'>
<pose>0 0 -0.127 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/wrist1.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='wrist_1_link_visual'>
<pose>0 0 -0.127 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/wrist1.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='wrist_2_joint' type='revolute'>
<pose>0 0 0 0 0 -0</pose>
<parent>wrist_1_link</parent>
<child>wrist_2_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>28</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='wrist_2_link'>
<pose>0.00081 0.1333 1.0794 -0 0.001593 -0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1.2190000000000001</mass>
<inertia>
<ixx>0.0025598999999999999</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0025598999999999999</iyy>
<iyz>0</iyz>
<izz>0.0021941999999999999</izz>
</inertia>
</inertial>
<collision name='wrist_2_link_collision'>
<pose>0 0 -0.0997 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/wrist2.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='wrist_2_link_visual'>
<pose>0 0 -0.0997 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/wrist2.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='wrist_3_joint' type='revolute'>
<pose>0 -0 -0 0 -0 0</pose>
<parent>wrist_2_link</parent>
<child>wrist_3_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>28</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='wrist_3_link'>
<pose>0.00081 0.2329 1.0794 -1.5708 0.001593 -0</pose>
<inertial>
<pose>0 0 -0.0229 0 -0 0</pose>
<mass>0.18790000000000001</mass>
<inertia>
<ixx>9.8904099999999994e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.8904099999999994e-05</iyy>
<iyz>0</iyz>
<izz>0.00013211700000000001</izz>
</inertia>
</inertial>
<collision name='wrist_3_link_collision'>
<pose>0 0 -0.0989 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/wrist3.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='wrist_3_link_visual'>
<pose>0 0 -0.0989 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/wrist3.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<plugin name='ign_ros2_control::IgnitionROS2ControlPlugin' filename='libign_ros2_control-system.so'>
<parameters>/home/bill-finger/rasms_ws/install/share/ur_moveit_config/config/ur_controllers.yaml</parameters>
<controller_manager_node_name>controller_manager</controller_manager_node_name>
</plugin>
<frame name='base_link-base_fixed_joint' attached_to='base_link'>
<pose>0 0 0 0 -0 3.14159</pose>
</frame>
<frame name='base' attached_to='base_link-base_fixed_joint'/>
<frame name='base_link-base_link_inertia' attached_to='base_link'>
<pose>0 0 0 0 -0 3.14159</pose>
</frame>
<frame name='base_link_inertia' attached_to='base_link-base_link_inertia'/>
<frame name='flange-tool0' attached_to='flange'>
<pose>0 0 0 1.5708 -0 1.5708</pose>
</frame>
<frame name='tool0' attached_to='flange-tool0'/>
<frame name='wrist_3-flange' attached_to='wrist_3_link'>
<pose>0 0 0 -1.5708 -1.5708 0</pose>
</frame>
<frame name='flange' attached_to='wrist_3-flange'/>
<frame name='wrist_3_link-ft_frame' attached_to='wrist_3_link'>
<pose>0 0 0 3.14159 -0 0</pose>
</frame>
<frame name='ft_frame' attached_to='wrist_3_link-ft_frame'/>
<pose>0 0 0 0 -0 0</pose>
<static>false</static>
<self_collide>false</self_collide>
</model>
<light name='sun' type='directional'>
<pose>0 0 10 0 -0 0</pose>
<cast_shadows>true</cast_shadows>
<intensity>1</intensity>
<direction>-0.5 0.1 -0.9</direction>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<linear>0.01</linear>
<constant>0.90000000000000002</constant>
<quadratic>0.001</quadratic>
</attenuation>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
</world>
</sdf>