Enhance IIWA Robot Configuration and Launch Files
- Updated iiwa7 URDF Xacro to include command and state interfaces for position and effort with defined limits for all joints. - Modified setting_loader.py to include new fields for command_mode and description in the RobotCfg dataclass and settings loading process. - Created iiwa.launch.py to manage the launch of the IIWA robot, integrating MoveIt configurations and RViz support. - Added iiwa_controllers.launch.py to set up the controller manager and spawner for the IIWA robot. - Introduced iiwa_hardware_interface_plugin.xml to define the hardware interface for the KUKA IIWA 7 robot. - Added iiwa7_fri.urdf.xacro to support FRI (Fast Robot Interface) with appropriate command and state interfaces for each joint.
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,176 @@
|
||||
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_utils import converter, setting_loader
|
||||
|
||||
import yaml
|
||||
import tempfile
|
||||
|
||||
def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str:
|
||||
with open(yaml_path, "r") as f:
|
||||
data = yaml.safe_load(f)
|
||||
|
||||
wrapped = {namespace: {"ros__parameters": data}}
|
||||
|
||||
tmp = tempfile.NamedTemporaryFile(
|
||||
mode="w", suffix=".yaml", delete=False
|
||||
)
|
||||
yaml.dump(wrapped, tmp, default_flow_style=False)
|
||||
tmp.close()
|
||||
return tmp.name
|
||||
|
||||
|
||||
def _runtime_setup(context, *args, **kwargs):
|
||||
settings = setting_loader.build_settings(
|
||||
settings_path=LaunchConfiguration("setting").perform(context), check_files=True
|
||||
)
|
||||
|
||||
xacro_args = {
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions,
|
||||
"robot_ip": settings.robot.ip,
|
||||
"fri_port": str(settings.robot.port),
|
||||
"simulate": "false",
|
||||
"command_mode": settings.robot.command_mode,
|
||||
}
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=settings.robot.description,
|
||||
robot_name=settings.robot.name,
|
||||
xacro_args=xacro_args,
|
||||
)
|
||||
|
||||
rsp_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[{"robot_description": robot_description,
|
||||
"use_sim_time": False}],
|
||||
)
|
||||
|
||||
controllers_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("iiwa_bringup"),
|
||||
"launch",
|
||||
"supported",
|
||||
"iiwa_controllers.launch.py"
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_name": str(settings.robot.name),
|
||||
"description": str(settings.robot.description),
|
||||
"initial_positions_file": str(settings.controller.moveit.initial_positions),
|
||||
"controller_path": str(settings.controller.controller_path)
|
||||
}.items()
|
||||
)
|
||||
|
||||
# Moveit
|
||||
moveit_configs = (
|
||||
MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
|
||||
.robot_description(
|
||||
file_path=settings.robot.description,
|
||||
mappings={
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions
|
||||
},
|
||||
)
|
||||
.robot_description_semantic(file_path=settings.controller.moveit.srdf)
|
||||
.robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
|
||||
.joint_limits(file_path=settings.controller.moveit.joint_limits)
|
||||
.pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
|
||||
.trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
|
||||
.moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
|
||||
.to_moveit_configs()
|
||||
)
|
||||
|
||||
move_group = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
parameters=[
|
||||
moveit_configs.to_dict(),
|
||||
{"robot_description": robot_description},
|
||||
],
|
||||
)
|
||||
|
||||
|
||||
joint_limits_ros2 = wrap_for_ros2_params(
|
||||
settings.controller.moveit.joint_limits,
|
||||
"robot_description_planning"
|
||||
)
|
||||
kinematics_ros2 = wrap_for_ros2_params(
|
||||
settings.controller.moveit.kinematics,
|
||||
"robot_description_kinematics"
|
||||
)
|
||||
|
||||
rviz_launch = Node(
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=["-d", settings.digital_twin.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,
|
||||
joint_limits_ros2,
|
||||
kinematics_ros2,
|
||||
],
|
||||
)
|
||||
|
||||
shutdown_on_rviz_exit = RegisterEventHandler(
|
||||
OnProcessExit(target_action=rviz_launch, on_exit=[EmitEvent(event=Shutdown())])
|
||||
)
|
||||
|
||||
return [
|
||||
rsp_node,
|
||||
controllers_launch,
|
||||
move_group,
|
||||
rviz_launch,
|
||||
shutdown_on_rviz_exit
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declare_rviz = DeclareLaunchArgument(
|
||||
name="rviz",
|
||||
default_value="0",
|
||||
description="If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch)",
|
||||
)
|
||||
|
||||
declacre_setting = DeclareLaunchArgument(
|
||||
name="setting",
|
||||
default_value=PathJoinSubstitution(
|
||||
[FindPackageShare("iiwa_config"), "config", "setting.yaml"]
|
||||
),
|
||||
description="Absolute path to settings file",
|
||||
)
|
||||
|
||||
runtime_setup = OpaqueFunction(function=_runtime_setup)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
declare_rviz,
|
||||
declacre_setting,
|
||||
runtime_setup,
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,66 @@
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.actions import OpaqueFunction
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
from iiwa_utils import converter
|
||||
|
||||
|
||||
def _setup_controllers(context, *args, **kwargs):
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
description = LaunchConfiguration("description").perform(context)
|
||||
initial_positions_file = LaunchConfiguration("initial_positions_file").perform(
|
||||
context
|
||||
)
|
||||
controller_path = LaunchConfiguration("controller_path").perform(context)
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=description,
|
||||
robot_name=robot_name,
|
||||
xacro_args={"initial_positions_file": initial_positions_file},
|
||||
)
|
||||
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{"robot_description": robot_description},
|
||||
controller_path,
|
||||
],
|
||||
)
|
||||
|
||||
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["iiwa_arm_controller", "--controller-manager", "/controller_manager"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
arm_controller_after_jsb = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[arm_controller_spawner],
|
||||
)
|
||||
)
|
||||
|
||||
return [
|
||||
ros2_control_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
arm_controller_after_jsb,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([OpaqueFunction(function=_setup_controllers)])
|
||||
@@ -41,6 +41,15 @@ def _setup_controllers(context, *args, **kwargs):
|
||||
parameters=[{"use_sim_time": False}],
|
||||
)
|
||||
|
||||
torque_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
output="screen",
|
||||
arguments=["forward_torque_controller",
|
||||
"--inactive"] + tmo,
|
||||
parameters=[{"use_sim_time": False}]
|
||||
)
|
||||
|
||||
spawner_urdf = URDFSpawner(
|
||||
name=robot_name,
|
||||
robot_description=robot_description,
|
||||
@@ -48,7 +57,7 @@ def _setup_controllers(context, *args, **kwargs):
|
||||
rotation=rotation,
|
||||
)
|
||||
|
||||
return [jsb, jtc, spawner_urdf]
|
||||
return [jsb, jtc, torque_controller_spawner, spawner_urdf]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
Reference in New Issue
Block a user