добавлена поддержка moveit
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,65 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<robot name="iiwa7">
|
||||
<!--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="arm">
|
||||
<joint name="joint1"/>
|
||||
<joint name="joint2"/>
|
||||
<joint name="joint3"/>
|
||||
<joint name="joint4"/>
|
||||
<joint name="joint5"/>
|
||||
<joint name="joint6"/>
|
||||
<joint name="joint7"/>
|
||||
</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="arm">
|
||||
<joint name="joint1" value="0"/>
|
||||
<joint name="joint2" value="0"/>
|
||||
<joint name="joint3" value="0"/>
|
||||
<joint name="joint4" value="0"/>
|
||||
<joint name="joint5" value="0"/>
|
||||
<joint name="joint6" value="0"/>
|
||||
<joint name="joint7" value="0"/>
|
||||
</group_state>
|
||||
|
||||
<group_state name="work" group="arm">
|
||||
<joint name="joint1" value="0"/>
|
||||
<joint name="joint2" value="0"/>
|
||||
<joint name="joint3" value="0"/>
|
||||
<joint name="joint4" value="-1.57"/>
|
||||
<joint name="joint5" value="0"/>
|
||||
<joint name="joint6" value="1.57"/>
|
||||
<joint name="joint7" 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_link" link2="link1" reason="Adjacent"/>
|
||||
<disable_collisions link1="base_link" link2="link2" reason="Never"/>
|
||||
<disable_collisions link1="base_link" link2="link3" reason="Never"/>
|
||||
<disable_collisions link1="base_link" link2="link4" reason="Never"/>
|
||||
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
|
||||
<disable_collisions link1="link1" link2="link3" reason="Never"/>
|
||||
<disable_collisions link1="link1" link2="link4" reason="Never"/>
|
||||
<disable_collisions link1="link1" link2="link5" reason="Never"/>
|
||||
<disable_collisions link1="link1" link2="link6" reason="Never"/>
|
||||
<disable_collisions link1="link1" link2="link7" reason="Never"/>
|
||||
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
|
||||
<disable_collisions link1="link2" link2="link4" reason="Never"/>
|
||||
<disable_collisions link1="link2" link2="link5" reason="Never"/>
|
||||
<disable_collisions link1="link2" link2="link6" reason="Never"/>
|
||||
<disable_collisions link1="link2" link2="link7" reason="Never"/>
|
||||
<disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
|
||||
<disable_collisions link1="link3" link2="link5" reason="Never"/>
|
||||
<disable_collisions link1="link3" link2="link6" reason="Never"/>
|
||||
<disable_collisions link1="link3" link2="link7" reason="Never"/>
|
||||
<disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
|
||||
<disable_collisions link1="link4" link2="link6" reason="Never"/>
|
||||
<disable_collisions link1="link4" link2="link7" reason="Never"/>
|
||||
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
|
||||
<disable_collisions link1="link5" link2="link7" reason="Never"/>
|
||||
<disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
|
||||
</robot>
|
||||
@@ -8,7 +8,6 @@ controller_manager:
|
||||
iiwa_arm_controller:
|
||||
type: "joint_trajectory_controller/JointTrajectoryController"
|
||||
|
||||
|
||||
iiwa_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
@@ -28,3 +27,4 @@ iiwa_arm_controller:
|
||||
|
||||
allow_partial_joints_goal: false
|
||||
interpolate_from_desired_state: true
|
||||
allow_nonzero_velocity_at_trajectory_end: true
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
# Default initial positions for iiwa7's ros2_control fake system
|
||||
|
||||
initial_positions:
|
||||
joint1: 0
|
||||
joint2: 0
|
||||
joint3: 0
|
||||
joint4: 0
|
||||
joint5: 0
|
||||
joint6: 0
|
||||
joint7: 0
|
||||
@@ -0,0 +1,45 @@
|
||||
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
||||
|
||||
# For beginners, we downscale velocity and acceleration limits.
|
||||
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
|
||||
default_velocity_scaling_factor: 0.1
|
||||
default_acceleration_scaling_factor: 0.1
|
||||
|
||||
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
||||
joint_limits:
|
||||
joint1:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 1.71
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
joint2:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 1.71
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
joint3:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 1.75
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
joint4:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2.27
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
joint5:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2.4399999999999999
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
joint6:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.1400000000000001
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
joint7:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.1400000000000001
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 3.0
|
||||
@@ -0,0 +1,4 @@
|
||||
arm:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
@@ -0,0 +1,20 @@
|
||||
# MoveIt uses this configuration for controller management
|
||||
|
||||
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
|
||||
|
||||
moveit_simple_controller_manager:
|
||||
controller_names:
|
||||
- iiwa_arm_controller
|
||||
|
||||
iiwa_arm_controller:
|
||||
type: FollowJointTrajectory
|
||||
joints:
|
||||
- joint1
|
||||
- joint2
|
||||
- joint3
|
||||
- joint4
|
||||
- joint5
|
||||
- joint6
|
||||
- joint7
|
||||
action_ns: follow_joint_trajectory
|
||||
default: true
|
||||
@@ -0,0 +1,6 @@
|
||||
# Limits for the Pilz planner
|
||||
cartesian_limits:
|
||||
max_trans_vel: 1.0
|
||||
max_trans_acc: 2.25
|
||||
max_trans_dec: -5.0
|
||||
max_rot_vel: 1.57
|
||||
@@ -1,13 +1,19 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.events import Shutdown
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
|
||||
from iiwa_bringup.utils import converter
|
||||
|
||||
@@ -15,20 +21,38 @@ PACKAGE = "iiwa_bringup"
|
||||
DESCRIPTION_PKG = "iiwa_description"
|
||||
|
||||
|
||||
def _arg(name: str, default, desc: str):
|
||||
"""Short helper to declare a launch argument."""
|
||||
return DeclareLaunchArgument(name=name, default_value=default, description=desc)
|
||||
|
||||
|
||||
def _share_file(pkg_arg_name: str, *relpath: str):
|
||||
"""Join <share>/<relpath...> where <share> is FindPackageShare(LaunchConfiguration(pkg_arg_name))."""
|
||||
return PathJoinSubstitution([FindPackageShare(LaunchConfiguration(pkg_arg_name)), *relpath])
|
||||
|
||||
|
||||
def _runtime_setup(context, *args, **kwatgs):
|
||||
setup = []
|
||||
|
||||
model_path = LaunchConfiguration("model").perform(context)
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
world_path = LaunchConfiguration("world").perform(context)
|
||||
rviz_status = LaunchConfiguration("rviz").perform(context).lower() in ["true", "1", "yes",]
|
||||
|
||||
transform = LaunchConfiguration("transform").perform(context)
|
||||
rotation = LaunchConfiguration("rotation").perform(context)
|
||||
timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
|
||||
xacro_file = LaunchConfiguration("xacro_file").perform(context)
|
||||
srdf_file = LaunchConfiguration("srdf_file").perform(context)
|
||||
|
||||
# ros2_controllers_file = LaunchConfiguration("controller").perform(context)
|
||||
initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
|
||||
|
||||
kinematics_yaml = LaunchConfiguration("kinematics_yaml").perform(context)
|
||||
joint_limits_yaml = LaunchConfiguration("joint_limits_yaml").perform(context)
|
||||
pilz_limits_yaml = LaunchConfiguration("pilz_limits_yaml").perform(context)
|
||||
moveit_controllers_yaml = LaunchConfiguration("moveit_controllers_yaml").perform(context)
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=model_path, robot_name=robot_name
|
||||
model_path=xacro_file, robot_name=robot_name
|
||||
)
|
||||
|
||||
rsp_node = Node(
|
||||
@@ -36,7 +60,8 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[{"robot_description": robot_description, "use_sim_time": True}],
|
||||
parameters=[{"robot_description": robot_description,
|
||||
"use_sim_time": False}],
|
||||
)
|
||||
|
||||
webots_launch = IncludeLaunchDescription(
|
||||
@@ -52,7 +77,7 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_name": robot_name,
|
||||
"model": model_path,
|
||||
"xacro_file": xacro_file,
|
||||
"world": world_path,
|
||||
"transform": transform,
|
||||
"rotation": rotation,
|
||||
@@ -60,91 +85,176 @@ def _runtime_setup(context, *args, **kwatgs):
|
||||
}.items(),
|
||||
)
|
||||
|
||||
setup += [rsp_node, webots_launch]
|
||||
moveit_configs = (
|
||||
MoveItConfigsBuilder("iiwa7", package_name=LaunchConfiguration("config_pkg").perform(context))
|
||||
.robot_description(file_path=xacro_file, mappings={
|
||||
"initial_positions_file": initial_positions_file,
|
||||
})
|
||||
.robot_description_semantic(file_path=srdf_file)
|
||||
.robot_description_kinematics(file_path=kinematics_yaml)
|
||||
.joint_limits(file_path=joint_limits_yaml)
|
||||
.pilz_cartesian_limits(file_path=pilz_limits_yaml)
|
||||
.trajectory_execution(file_path=moveit_controllers_yaml)
|
||||
.to_moveit_configs()
|
||||
)
|
||||
|
||||
if rviz_status:
|
||||
rviz_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[FindPackageShare(PACKAGE), "launch", "supported", "rviz.launch.py"]
|
||||
)
|
||||
),
|
||||
launch_arguments={"package_name": PACKAGE}.items(),
|
||||
)
|
||||
move_group = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
parameters=[
|
||||
moveit_configs.to_dict(),
|
||||
robot_description,
|
||||
],
|
||||
)
|
||||
|
||||
setup.append(rviz_launch)
|
||||
rviz_config = PathJoinSubstitution(
|
||||
[FindPackageShare(PACKAGE), "config", "rviz_iiwa.rviz"]
|
||||
)
|
||||
|
||||
rviz_launch = Node(
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=["-d", rviz_config],
|
||||
output="log",
|
||||
parameters=[
|
||||
moveit_configs.robot_description,
|
||||
moveit_configs.robot_description_semantic,
|
||||
moveit_configs.robot_description_kinematics,
|
||||
moveit_configs.planning_pipelines,
|
||||
moveit_configs.joint_limits,
|
||||
],
|
||||
)
|
||||
|
||||
shutdown_on_rviz_exit = RegisterEventHandler(
|
||||
OnProcessExit(target_action=rviz_launch,
|
||||
on_exit=[EmitEvent(event=Shutdown())])
|
||||
)
|
||||
|
||||
setup += [rsp_node,
|
||||
webots_launch,
|
||||
move_group,
|
||||
rviz_launch,
|
||||
shutdown_on_rviz_exit
|
||||
]
|
||||
|
||||
return setup
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Объявление аргументов командной строки
|
||||
declare_model_arg = DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value=PathJoinSubstitution(
|
||||
[FindPackageShare(DESCRIPTION_PKG), "urdf", "iiwa7.urdf.xacro"]
|
||||
),
|
||||
description="Path to robot xacro or urdf file (used to build robot_description).",
|
||||
declare_description_pkg = _arg(
|
||||
"description_pkg",
|
||||
DESCRIPTION_PKG,
|
||||
"Package containing URDF/Xacro (and optionally SRDF/worlds).",
|
||||
)
|
||||
declare_config_pkg = _arg(
|
||||
"config_pkg",
|
||||
PACKAGE,
|
||||
"Package containing MoveIt/ros2_control config YAML files (config/*).",
|
||||
)
|
||||
|
||||
declare_robot_name_arg = DeclareLaunchArgument(
|
||||
name="robot_name",
|
||||
default_value="iiwa7",
|
||||
description="Robot name (used for TF and naming spawned robot).",
|
||||
declare_robot_name = _arg(
|
||||
"robot_name",
|
||||
"iiwa7",
|
||||
"Robot name (used for TF and naming).",
|
||||
)
|
||||
declare_world = _arg(
|
||||
"world",
|
||||
_share_file("description_pkg", "worlds", "iiwa.wbt"),
|
||||
"Path to Webots world (.wbt).",
|
||||
)
|
||||
declare_controller = _arg(
|
||||
"controller",
|
||||
_share_file("config_pkg", "config", "iiwa_controller.yaml"),
|
||||
"Controllers YAML (spawner/controller_manager).",
|
||||
)
|
||||
declare_transform = _arg(
|
||||
"transform",
|
||||
"-0.25 0 0.79",
|
||||
"Spawn translation in Webots (x y z).",
|
||||
)
|
||||
declare_rotation = _arg(
|
||||
"rotation",
|
||||
"0 0 1 0",
|
||||
"Spawn rotation axis-angle in Webots.",
|
||||
)
|
||||
declare_rviz = _arg(
|
||||
"rviz",
|
||||
"0",
|
||||
"If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch).",
|
||||
)
|
||||
declare_controller_timer = _arg(
|
||||
"controller_timer",
|
||||
"50",
|
||||
"Timeout (seconds) for controller_manager spawners.",
|
||||
)
|
||||
|
||||
declare_world_arg = DeclareLaunchArgument(
|
||||
name="world",
|
||||
default_value=PathJoinSubstitution(
|
||||
[FindPackageShare(DESCRIPTION_PKG), "worlds", "iiwa.wbt"]
|
||||
),
|
||||
description="Path to the Webots world (.wbt) to launch.",
|
||||
# URDF/SRDF
|
||||
declare_xacro_file = _arg(
|
||||
"xacro_file",
|
||||
_share_file("description_pkg", "urdf", "iiwa7.urdf.xacro"),
|
||||
"Xacro used by MoveIt robot_description.",
|
||||
)
|
||||
declare_srdf_file = _arg(
|
||||
"srdf_file",
|
||||
_share_file("config_pkg", "config", "iiwa7.srdf"),
|
||||
"SRDF path.",
|
||||
)
|
||||
|
||||
declare_controller_arg = DeclareLaunchArgument(
|
||||
name="controller",
|
||||
default_value=PathJoinSubstitution(
|
||||
[FindPackageShare(PACKAGE), "config", "iiwa_controller.yaml"]
|
||||
),
|
||||
description="Path to controllers YAML file (used by spawner and controller_manager).",
|
||||
# YAML конфиги
|
||||
declare_initial_positions = _arg(
|
||||
"initial_positions_file",
|
||||
_share_file("config_pkg", "config", "initial_positions.yaml"),
|
||||
"initial_positions.yaml passed into xacro arg initial_positions_file.",
|
||||
)
|
||||
declare_kinematics_yaml = _arg(
|
||||
"kinematics_yaml",
|
||||
_share_file("config_pkg", "config", "kinematics.yaml"),
|
||||
"MoveIt kinematics.yaml",
|
||||
)
|
||||
declare_joint_limits_yaml = _arg(
|
||||
"joint_limits_yaml",
|
||||
_share_file("config_pkg", "config", "joint_limits.yaml"),
|
||||
"MoveIt joint_limits.yaml",
|
||||
)
|
||||
declare_pilz_limits_yaml = _arg(
|
||||
"pilz_limits_yaml",
|
||||
_share_file("config_pkg", "config", "pilz_cartesian_limits.yaml"),
|
||||
"Pilz cartesian limits yaml.",
|
||||
)
|
||||
declare_moveit_controllers_yaml = _arg(
|
||||
"moveit_controllers_yaml",
|
||||
_share_file("config_pkg", "config", "moveit_controllers.yaml"),
|
||||
"MoveIt controllers (trajectory_execution / simple_controller_manager).",
|
||||
)
|
||||
|
||||
declare_transform_arg = DeclareLaunchArgument(
|
||||
name="transform",
|
||||
default_value="-0.25 0 0.79",
|
||||
description="Translation applied when spawning the robot in the Webots world (x y z).",
|
||||
)
|
||||
|
||||
declare_rotation_arg = DeclareLaunchArgument(
|
||||
name="rotation",
|
||||
default_value="0 0 1 0",
|
||||
description="Rotation (axis-angle) applied when spawning the robot in the Webots world.",
|
||||
)
|
||||
|
||||
declare_rviz_arg = DeclareLaunchArgument(
|
||||
name="rviz",
|
||||
default_value="0",
|
||||
description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers.",
|
||||
)
|
||||
|
||||
declare_controller_manager_arg = DeclareLaunchArgument(
|
||||
name="controller_timer",
|
||||
default_value="50",
|
||||
description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout).",
|
||||
)
|
||||
|
||||
runtime_setup = OpaqueFunction(function=_runtime_setup)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
declare_model_arg,
|
||||
declare_robot_name_arg,
|
||||
declare_world_arg,
|
||||
declare_controller_arg,
|
||||
declare_transform_arg,
|
||||
declare_rotation_arg,
|
||||
declare_rviz_arg,
|
||||
declare_controller_manager_arg,
|
||||
declare_description_pkg,
|
||||
declare_config_pkg,
|
||||
|
||||
declare_robot_name,
|
||||
declare_world,
|
||||
declare_controller,
|
||||
declare_transform,
|
||||
declare_rotation,
|
||||
declare_rviz,
|
||||
declare_controller_timer,
|
||||
|
||||
declare_xacro_file,
|
||||
declare_srdf_file,
|
||||
declare_initial_positions,
|
||||
declare_kinematics_yaml,
|
||||
declare_joint_limits_yaml,
|
||||
declare_pilz_limits_yaml,
|
||||
declare_moveit_controllers_yaml,
|
||||
|
||||
runtime_setup,
|
||||
]
|
||||
)
|
||||
|
||||
@@ -1,82 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.events import Shutdown
|
||||
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
package_name = "iiwa_bringup"
|
||||
description_package_name = "iiwa_description"
|
||||
|
||||
declare_model_arg = DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value=PathJoinSubstitution(
|
||||
[FindPackageShare(description_package_name), "urdf", "iiwa7.urdf.xacro"]
|
||||
),
|
||||
description="Path to the robot URDF/Xacro file",
|
||||
)
|
||||
|
||||
declare_robot_name_arg = DeclareLaunchArgument(
|
||||
name="robot_name",
|
||||
default_value="iiwa7",
|
||||
description="Robot name fro the TF tree",
|
||||
)
|
||||
|
||||
robot_description = Command(
|
||||
[
|
||||
"xacro ",
|
||||
LaunchConfiguration("model"),
|
||||
" robot_name:=",
|
||||
LaunchConfiguration("robot_name"),
|
||||
]
|
||||
)
|
||||
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[
|
||||
{
|
||||
"robot_description": robot_description,
|
||||
"use_sim_time": False,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
joint_state_publisher_gui = Node(
|
||||
package="joint_state_publisher_gui",
|
||||
executable="joint_state_publisher_gui",
|
||||
name="joint_state_publisher_gui",
|
||||
parameters=[{"use_sim_time": False}],
|
||||
)
|
||||
|
||||
rviz_config = PathJoinSubstitution(
|
||||
[FindPackageShare(package_name), "config", "rviz_iiwa.rviz"]
|
||||
)
|
||||
|
||||
rviz = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=["-d", rviz_config],
|
||||
output="log",
|
||||
)
|
||||
|
||||
shutdown_on_rviz_exit = RegisterEventHandler(
|
||||
OnProcessExit(target_action=rviz, on_exit=[EmitEvent(event=Shutdown())])
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
declare_model_arg,
|
||||
declare_robot_name_arg,
|
||||
robot_state_publisher,
|
||||
joint_state_publisher_gui,
|
||||
rviz,
|
||||
shutdown_on_rviz_exit,
|
||||
]
|
||||
)
|
||||
@@ -1,36 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.events import Shutdown
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declare_package_arg = DeclareLaunchArgument(
|
||||
"package_name",
|
||||
default_value="iiwa_bringup",
|
||||
description="Package name where RViz config is stored",
|
||||
)
|
||||
|
||||
package_name = LaunchConfiguration("package_name")
|
||||
|
||||
rviz_config = PathJoinSubstitution(
|
||||
[FindPackageShare(package_name), "config", "rviz_iiwa.rviz"]
|
||||
)
|
||||
|
||||
rviz = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=["-d", rviz_config],
|
||||
output="log",
|
||||
)
|
||||
|
||||
# При закрытии rviz закрываем все
|
||||
shutdown_on_rviz_exit = RegisterEventHandler(
|
||||
OnProcessExit(target_action=rviz, on_exit=[EmitEvent(event=Shutdown())])
|
||||
)
|
||||
|
||||
return LaunchDescription([declare_package_arg, rviz, shutdown_on_rviz_exit])
|
||||
@@ -4,22 +4,24 @@ from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from webots_ros2_driver.urdf_spawner import URDFSpawner
|
||||
|
||||
|
||||
from iiwa_bringup.utils import converter
|
||||
|
||||
|
||||
def _setup_controllers(context, *args, **kwargs):
|
||||
model_path = LaunchConfiguration("model").perform(context)
|
||||
xacro_file = LaunchConfiguration("xacro_file").perform(context)
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
transform = LaunchConfiguration("transform").perform(context)
|
||||
rotation = LaunchConfiguration("rotation").perform(context)
|
||||
timer = LaunchConfiguration("controller_timer").perform(context)
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=model_path, robot_name=robot_name
|
||||
model_path=xacro_file, robot_name=robot_name
|
||||
)
|
||||
|
||||
tmo = ["--controller-manager-timeout", str(timer)]
|
||||
|
||||
|
||||
jsb = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
|
||||
@@ -16,7 +16,7 @@ from webots_ros2_driver.webots_launcher import WebotsLauncher
|
||||
|
||||
|
||||
def _spawn_setup(context, *args, **kwargs):
|
||||
model_path = LaunchConfiguration("model").perform(context)
|
||||
xacro_file = LaunchConfiguration("xacro_file").perform(context)
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
world_path = LaunchConfiguration("world").perform(context)
|
||||
transform = LaunchConfiguration("transform").perform(context)
|
||||
@@ -29,13 +29,13 @@ def _spawn_setup(context, *args, **kwargs):
|
||||
robot_name=robot_name,
|
||||
parameters=[
|
||||
{
|
||||
"robot_description": model_path,
|
||||
"use_sim_time": True,
|
||||
"robot_description": xacro_file,
|
||||
"use_sim_time": False,
|
||||
"set_robot_state_publisher": False,
|
||||
},
|
||||
LaunchConfiguration("controller").perform(context),
|
||||
],
|
||||
respawn=False,
|
||||
respawn=True,
|
||||
)
|
||||
|
||||
controllers_launch = IncludeLaunchDescription(
|
||||
@@ -51,7 +51,7 @@ def _spawn_setup(context, *args, **kwargs):
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_name": robot_name,
|
||||
"model": model_path,
|
||||
"xacro_file": xacro_file,
|
||||
"transform": transform,
|
||||
"rotation": rotation,
|
||||
"controller_timer": timer,
|
||||
|
||||
@@ -1,280 +0,0 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ./src/iiwa_description/urdf/iiwa7.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="iiwa7">
|
||||
<link name="world"/>
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.012856764088336 7.05371084939836E-07 0.0679655592591469"/>
|
||||
<mass value="30.2763517543289"/>
|
||||
<inertia ixx="0.111239565538359" ixy="-3.81097437253422e-07" ixz="0.00486659750522703" iyy="0.139017104291389" iyz="7.87871531503135e-07" izz="0.133028621926226"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/base_link.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/base_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-4.78790441849064E-07 -0.0348192058752328 -0.0692021315991948"/>
|
||||
<mass value="21.1627406257303"/>
|
||||
<inertia ixx="0.133321610552378" ixy="-5.3830316300325e-07" ixz="9.16989471201082e-07" iyy="0.127213841326705" iyz="0.0236704924620788" izz="0.047155320135412"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link1.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link1.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-5.2906167189587E-07 -0.0869460684741495 0.0287906957294968"/>
|
||||
<mass value="25.1403147192251"/>
|
||||
<inertia ixx="0.197631241693207" ixy="9.50708464039702e-08" ixz="-3.51963818841194e-07" iyy="0.0602720613821397" iyz="-0.0367196702125505" izz="0.187557269753206"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link2.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link2.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="8.94264666317923E-07 0.0343188244798595 -0.0692020521907963"/>
|
||||
<mass value="21.1627874292289"/>
|
||||
<inertia ixx="0.133322417523041" ixy="-2.72387851582384e-07" ixz="-2.31579646685682e-07" iyy="0.127213838333813" iyz="-0.0236710393468367" izz="0.0471557500559207"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link3.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link3.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="9.9343785475714E-08 -0.0869462342636022 -0.0292907044657978"/>
|
||||
<mass value="25.140328983289"/>
|
||||
<inertia ixx="0.197630839812683" ixy="-3.9082887807781e-07" ixz="-3.24564289064112e-07" iyy="0.0602723071447973" iyz="0.0367195588674331" izz="0.187556898167347"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link4.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link4.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-1.5059645121912E-07 -0.0298223596156005 -0.116233672352317"/>
|
||||
<mass value="9.53466992761605"/>
|
||||
<inertia ixx="0.0529038960536523" ixy="6.86274869084724e-08" ixz="3.57472026918582e-07" iyy="0.0444942713885967" iyz="0.0174914068345989" izz="0.0239916848880951"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link5.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link5.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="3.09820422451923E-06 -0.000588422963829549 0.000348569863276505"/>
|
||||
<mass value="14.2007599322815"/>
|
||||
<inertia ixx="0.0384515762635731" ixy="-6.70279497724119e-08" ixz="2.19173188415509e-06" iyy="0.0280010940276735" iyz="-0.00192249677468735" izz="0.0368124806983828"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link6.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link6.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link7">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="5.28878934317328E-07 3.37199922245852E-07 -0.0274928177649973"/>
|
||||
<mass value="2.47156969383589"/>
|
||||
<inertia ixx="0.00177365937026673" ixy="-1.2410521430667e-07" ixz="-2.95409038849194e-08" iyy="0.00177328722902027" iyz="-1.57567641084958e-08" izz="0.00272748710623445"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/visual/link7.DAE"/>
|
||||
</geometry>
|
||||
<material name="iiwa_material">
|
||||
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://iiwa_description/resource/meshes/collision/link7.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link_ee"/>
|
||||
<joint name="world_base_joint" type="fixed">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="world"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<joint name="joint1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.3375"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="link1"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="176" lower="-2.97" upper="2.97" velocity="1.71"/>
|
||||
</joint>
|
||||
<joint name="joint2" type="revolute">
|
||||
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||
<parent link="link1"/>
|
||||
<child link="link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="176" lower="-2.09" upper="2.09" velocity="1.71"/>
|
||||
</joint>
|
||||
<joint name="joint3" type="revolute">
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.3993 0"/>
|
||||
<parent link="link2"/>
|
||||
<child link="link3"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="110" lower="-2.97" upper="2.97" velocity="1.75"/>
|
||||
</joint>
|
||||
<joint name="joint4" type="revolute">
|
||||
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||
<parent link="link3"/>
|
||||
<child link="link4"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="110" lower="-2.09" upper="2.09" velocity="2.27"/>
|
||||
</joint>
|
||||
<joint name="joint5" type="revolute">
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.3993 0"/>
|
||||
<parent link="link4"/>
|
||||
<child link="link5"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="110" lower="-2.97" upper="2.97" velocity="2.44"/>
|
||||
</joint>
|
||||
<joint name="joint6" type="revolute">
|
||||
<origin rpy="-1.5708 0 0" xyz="0 0 0"/>
|
||||
<parent link="link5"/>
|
||||
<child link="link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" lower="-2.09" upper="2.09" velocity="3.14"/>
|
||||
</joint>
|
||||
<joint name="joint7" type="revolute">
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.126 0"/>
|
||||
<parent link="link6"/>
|
||||
<child link="link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" lower="-3.05" upper="3.05" velocity="3.14"/>
|
||||
</joint>
|
||||
<joint name="tools_joint" type="fixed">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="link7"/>
|
||||
<child link="link_ee"/>
|
||||
</joint>
|
||||
<webots>
|
||||
<plugin type="webots_ros2_control::Ros2Control"/>
|
||||
</webots>
|
||||
<ros2_control name="iiwaWebotsControl" type="system">
|
||||
<hardware>
|
||||
<plugin>webots_ros2_control::Ros2ControlSystem</plugin>
|
||||
</hardware>
|
||||
<joint name="joint1">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint2">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint3">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint4">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint5">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint6">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint7">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
</robot>
|
||||
@@ -1,6 +1,14 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="iiwa7">
|
||||
|
||||
<!-- Аргукменты -->
|
||||
<xacro:arg name="initial_positions_file"
|
||||
default="$(find iiwa_bringup)/config/initial_positions.yaml"/>
|
||||
|
||||
<xacro:property name="initial_positions"
|
||||
value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
|
||||
|
||||
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/params.xacro"/>
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/macros.xacro"/>
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
|
||||
@@ -13,41 +21,50 @@
|
||||
<ros2_control name="iiwaWebotsControl" type="system">
|
||||
<hardware>
|
||||
<plugin>webots_ros2_control::Ros2ControlSystem</plugin>
|
||||
<!-- <plugin>mock_components/GenericSystem</plugin> -->
|
||||
</hardware>
|
||||
|
||||
<joint name="joint1">
|
||||
<state_interface name="position"/>
|
||||
<joint name="joint1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint1']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="joint2">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint2']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="joint3">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint3']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="joint4">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint4']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="joint5">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint5']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="joint6">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint6']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="joint7">
|
||||
<state_interface name="position"/>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint7']}</param>
|
||||
</state_interface>
|
||||
</joint>
|
||||
|
||||
</ros2_control>
|
||||
|
||||
Reference in New Issue
Block a user