issue with Gripper controllers
This commit is contained in:
parent
b88d58c291
commit
8d1f4642dc
@ -5,9 +5,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
install(DIRECTORY urdf/
|
install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
|
||||||
DESTINATION share/${PROJECT_NAME}/urdf)
|
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
|
||||||
|
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
|
||||||
|
install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
|
||||||
|
|
||||||
# find dependencies
|
# find dependencies
|
||||||
find_package(ament_cmake REQUIRED)
|
find_package(ament_cmake REQUIRED)
|
||||||
|
144
src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
Normal file
144
src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
Normal file
@ -0,0 +1,144 @@
|
|||||||
|
controller_manager:
|
||||||
|
ros__parameters:
|
||||||
|
joint_state_broadcaster:
|
||||||
|
type: joint_state_broadcaster/JointStateBroadcaster
|
||||||
|
|
||||||
|
io_and_status_controller:
|
||||||
|
type: ur_controllers/GPIOController
|
||||||
|
|
||||||
|
speed_scaling_state_broadcaster:
|
||||||
|
type: ur_controllers/SpeedScalingStateBroadcaster
|
||||||
|
|
||||||
|
force_torque_sensor_broadcaster:
|
||||||
|
type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
|
||||||
|
|
||||||
|
joint_trajectory_controller:
|
||||||
|
type: joint_trajectory_controller/JointTrajectoryController
|
||||||
|
|
||||||
|
scaled_joint_trajectory_controller:
|
||||||
|
type: ur_controllers/ScaledJointTrajectoryController
|
||||||
|
|
||||||
|
forward_velocity_controller:
|
||||||
|
type: velocity_controllers/JointGroupVelocityController
|
||||||
|
|
||||||
|
forward_position_controller:
|
||||||
|
type: position_controllers/JointGroupPositionController
|
||||||
|
|
||||||
|
robotiq_gripper_controller:
|
||||||
|
type: position_controllers/GripperActionController
|
||||||
|
|
||||||
|
robotiq_activation_controller:
|
||||||
|
type: robotiq_controllers/RobotiqActivationController
|
||||||
|
|
||||||
|
|
||||||
|
speed_scaling_state_broadcaster:
|
||||||
|
ros__parameters:
|
||||||
|
state_publish_rate: 100.0
|
||||||
|
tf_prefix: "$(var tf_prefix)"
|
||||||
|
|
||||||
|
io_and_status_controller:
|
||||||
|
ros__parameters:
|
||||||
|
tf_prefix: "$(var tf_prefix)"
|
||||||
|
|
||||||
|
force_torque_sensor_broadcaster:
|
||||||
|
ros__parameters:
|
||||||
|
sensor_name: $(var tf_prefix)tcp_fts_sensor
|
||||||
|
state_interface_names:
|
||||||
|
- force.x
|
||||||
|
- force.y
|
||||||
|
- force.z
|
||||||
|
- torque.x
|
||||||
|
- torque.y
|
||||||
|
- torque.z
|
||||||
|
frame_id: $(var tf_prefix)tool0
|
||||||
|
topic_name: ft_data
|
||||||
|
|
||||||
|
|
||||||
|
joint_trajectory_controller:
|
||||||
|
ros__parameters:
|
||||||
|
joints:
|
||||||
|
- $(var tf_prefix)shoulder_pan_joint
|
||||||
|
- $(var tf_prefix)shoulder_lift_joint
|
||||||
|
- $(var tf_prefix)elbow_joint
|
||||||
|
- $(var tf_prefix)wrist_1_joint
|
||||||
|
- $(var tf_prefix)wrist_2_joint
|
||||||
|
- $(var tf_prefix)wrist_3_joint
|
||||||
|
command_interfaces:
|
||||||
|
- position
|
||||||
|
state_interfaces:
|
||||||
|
- position
|
||||||
|
- velocity
|
||||||
|
state_publish_rate: 100.0
|
||||||
|
action_monitor_rate: 20.0
|
||||||
|
allow_partial_joints_goal: false
|
||||||
|
constraints:
|
||||||
|
stopped_velocity_tolerance: 0.2
|
||||||
|
goal_time: 0.0
|
||||||
|
$(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
|
||||||
|
|
||||||
|
scaled_joint_trajectory_controller:
|
||||||
|
ros__parameters:
|
||||||
|
joints:
|
||||||
|
- $(var tf_prefix)shoulder_pan_joint
|
||||||
|
- $(var tf_prefix)shoulder_lift_joint
|
||||||
|
- $(var tf_prefix)elbow_joint
|
||||||
|
- $(var tf_prefix)wrist_1_joint
|
||||||
|
- $(var tf_prefix)wrist_2_joint
|
||||||
|
- $(var tf_prefix)wrist_3_joint
|
||||||
|
command_interfaces:
|
||||||
|
- position
|
||||||
|
state_interfaces:
|
||||||
|
- position
|
||||||
|
- velocity
|
||||||
|
state_publish_rate: 100.0
|
||||||
|
action_monitor_rate: 20.0
|
||||||
|
allow_partial_joints_goal: false
|
||||||
|
constraints:
|
||||||
|
stopped_velocity_tolerance: 0.2
|
||||||
|
goal_time: 0.0
|
||||||
|
$(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
$(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
|
||||||
|
speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
|
||||||
|
|
||||||
|
forward_velocity_controller:
|
||||||
|
ros__parameters:
|
||||||
|
joints:
|
||||||
|
- $(var tf_prefix)shoulder_pan_joint
|
||||||
|
- $(var tf_prefix)shoulder_lift_joint
|
||||||
|
- $(var tf_prefix)elbow_joint
|
||||||
|
- $(var tf_prefix)wrist_1_joint
|
||||||
|
- $(var tf_prefix)wrist_2_joint
|
||||||
|
- $(var tf_prefix)wrist_3_joint
|
||||||
|
interface_name: velocity
|
||||||
|
|
||||||
|
forward_position_controller:
|
||||||
|
ros__parameters:
|
||||||
|
joints:
|
||||||
|
- $(var tf_prefix)shoulder_pan_joint
|
||||||
|
- $(var tf_prefix)shoulder_lift_joint
|
||||||
|
- $(var tf_prefix)elbow_joint
|
||||||
|
- $(var tf_prefix)wrist_1_joint
|
||||||
|
- $(var tf_prefix)wrist_2_joint
|
||||||
|
- $(var tf_prefix)wrist_3_joint
|
||||||
|
|
||||||
|
robotiq_gripper_controller:
|
||||||
|
ros__parameters:
|
||||||
|
default: true
|
||||||
|
joint: $(var tf_prefix)robotiq_140_left_knuckle_joint
|
||||||
|
use_effort_interface: true
|
||||||
|
use_speed_interface: true
|
||||||
|
|
||||||
|
robotiq_activation_controller:
|
||||||
|
ros__parameters:
|
||||||
|
default: true
|
||||||
|
|
@ -0,0 +1,614 @@
|
|||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch_ros.parameter_descriptions import ParameterFile
|
||||||
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||||
|
from launch.conditions import IfCondition, UnlessCondition
|
||||||
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
|
||||||
|
|
||||||
|
def launch_setup(context, *args, **kwargs):
|
||||||
|
# Initialize Arguments
|
||||||
|
ur_type = LaunchConfiguration("ur_type")
|
||||||
|
robot_ip = LaunchConfiguration("robot_ip")
|
||||||
|
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")
|
||||||
|
description_package = LaunchConfiguration("description_package")
|
||||||
|
ur_description_package = LaunchConfiguration("ur_description_package")
|
||||||
|
description_file = LaunchConfiguration("description_file")
|
||||||
|
tf_prefix = LaunchConfiguration("tf_prefix")
|
||||||
|
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
|
||||||
|
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
|
||||||
|
controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout")
|
||||||
|
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
|
||||||
|
activate_joint_controller = LaunchConfiguration("activate_joint_controller")
|
||||||
|
launch_rviz = LaunchConfiguration("launch_rviz")
|
||||||
|
headless_mode = LaunchConfiguration("headless_mode")
|
||||||
|
launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
|
||||||
|
use_tool_communication = LaunchConfiguration("use_tool_communication")
|
||||||
|
tool_parity = LaunchConfiguration("tool_parity")
|
||||||
|
tool_baud_rate = LaunchConfiguration("tool_baud_rate")
|
||||||
|
tool_stop_bits = LaunchConfiguration("tool_stop_bits")
|
||||||
|
tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars")
|
||||||
|
tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars")
|
||||||
|
tool_device_name = LaunchConfiguration("tool_device_name")
|
||||||
|
tool_tcp_port = LaunchConfiguration("tool_tcp_port")
|
||||||
|
tool_voltage = LaunchConfiguration("tool_voltage")
|
||||||
|
reverse_ip = LaunchConfiguration("reverse_ip")
|
||||||
|
script_command_port = LaunchConfiguration("script_command_port")
|
||||||
|
reverse_port = LaunchConfiguration("reverse_port")
|
||||||
|
script_sender_port = LaunchConfiguration("script_sender_port")
|
||||||
|
trajectory_port = LaunchConfiguration("trajectory_port")
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# Config Paths UR
|
||||||
|
joint_limit_params = PathJoinSubstitution(
|
||||||
|
[FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
|
||||||
|
)
|
||||||
|
kinematics_params = PathJoinSubstitution(
|
||||||
|
[FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"]
|
||||||
|
)
|
||||||
|
physical_params = PathJoinSubstitution(
|
||||||
|
[FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
|
||||||
|
)
|
||||||
|
visual_params = PathJoinSubstitution(
|
||||||
|
[FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"]
|
||||||
|
)
|
||||||
|
script_filename = PathJoinSubstitution(
|
||||||
|
[FindPackageShare("ur_client_library"), "resources", "external_control.urscript"]
|
||||||
|
)
|
||||||
|
input_recipe_filename = PathJoinSubstitution(
|
||||||
|
[FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
|
||||||
|
)
|
||||||
|
output_recipe_filename = PathJoinSubstitution(
|
||||||
|
[FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
|
||||||
|
)
|
||||||
|
|
||||||
|
# URDF
|
||||||
|
robot_description_content = Command(
|
||||||
|
[
|
||||||
|
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||||
|
" ",
|
||||||
|
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
|
||||||
|
" ",
|
||||||
|
"robot_ip:=",
|
||||||
|
robot_ip,
|
||||||
|
" ",
|
||||||
|
"joint_limit_params:=",
|
||||||
|
joint_limit_params,
|
||||||
|
" ",
|
||||||
|
"kinematics_params:=",
|
||||||
|
kinematics_params,
|
||||||
|
" ",
|
||||||
|
"physical_params:=",
|
||||||
|
physical_params,
|
||||||
|
" ",
|
||||||
|
"visual_params:=",
|
||||||
|
visual_params,
|
||||||
|
" ",
|
||||||
|
"safety_limits:=",
|
||||||
|
safety_limits,
|
||||||
|
" ",
|
||||||
|
"safety_pos_margin:=",
|
||||||
|
safety_pos_margin,
|
||||||
|
" ",
|
||||||
|
"safety_k_position:=",
|
||||||
|
safety_k_position,
|
||||||
|
" ",
|
||||||
|
"name:=",
|
||||||
|
ur_type,
|
||||||
|
" ",
|
||||||
|
"script_filename:=",
|
||||||
|
script_filename,
|
||||||
|
" ",
|
||||||
|
"input_recipe_filename:=",
|
||||||
|
input_recipe_filename,
|
||||||
|
" ",
|
||||||
|
"output_recipe_filename:=",
|
||||||
|
output_recipe_filename,
|
||||||
|
" ",
|
||||||
|
"tf_prefix:=",
|
||||||
|
tf_prefix,
|
||||||
|
" ",
|
||||||
|
"use_fake_hardware:=",
|
||||||
|
use_fake_hardware,
|
||||||
|
" ",
|
||||||
|
"fake_sensor_commands:=",
|
||||||
|
fake_sensor_commands,
|
||||||
|
" ",
|
||||||
|
"headless_mode:=",
|
||||||
|
headless_mode,
|
||||||
|
" ",
|
||||||
|
"use_tool_communication:=",
|
||||||
|
use_tool_communication,
|
||||||
|
" ",
|
||||||
|
"tool_parity:=",
|
||||||
|
tool_parity,
|
||||||
|
" ",
|
||||||
|
"tool_baud_rate:=",
|
||||||
|
tool_baud_rate,
|
||||||
|
" ",
|
||||||
|
"tool_stop_bits:=",
|
||||||
|
tool_stop_bits,
|
||||||
|
" ",
|
||||||
|
"tool_rx_idle_chars:=",
|
||||||
|
tool_rx_idle_chars,
|
||||||
|
" ",
|
||||||
|
"tool_tx_idle_chars:=",
|
||||||
|
tool_tx_idle_chars,
|
||||||
|
" ",
|
||||||
|
"tool_device_name:=",
|
||||||
|
tool_device_name,
|
||||||
|
" ",
|
||||||
|
"tool_tcp_port:=",
|
||||||
|
tool_tcp_port,
|
||||||
|
" ",
|
||||||
|
"tool_voltage:=",
|
||||||
|
tool_voltage,
|
||||||
|
" ",
|
||||||
|
"reverse_ip:=",
|
||||||
|
reverse_ip,
|
||||||
|
" ",
|
||||||
|
"script_command_port:=",
|
||||||
|
script_command_port,
|
||||||
|
" ",
|
||||||
|
"reverse_port:=",
|
||||||
|
reverse_port,
|
||||||
|
" ",
|
||||||
|
"script_sender_port:=",
|
||||||
|
script_sender_port,
|
||||||
|
" ",
|
||||||
|
"trajectory_port:=",
|
||||||
|
trajectory_port,
|
||||||
|
" ",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
robot_description = {"robot_description": robot_description_content}
|
||||||
|
|
||||||
|
initial_joint_controllers = PathJoinSubstitution(
|
||||||
|
[FindPackageShare(description_package), "config", controllers_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
rviz_config_file = PathJoinSubstitution(
|
||||||
|
[FindPackageShare(description_package), "rviz", "view_robot.rviz"]
|
||||||
|
)
|
||||||
|
|
||||||
|
# define update rate
|
||||||
|
update_rate_config_file = PathJoinSubstitution(
|
||||||
|
[
|
||||||
|
FindPackageShare(runtime_config_package),
|
||||||
|
"config",
|
||||||
|
ur_type.perform(context) + "_update_rate.yaml",
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
control_node = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="ros2_control_node",
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
update_rate_config_file,
|
||||||
|
ParameterFile(initial_joint_controllers, allow_substs=True),
|
||||||
|
],
|
||||||
|
output="screen",
|
||||||
|
condition=IfCondition(use_fake_hardware),
|
||||||
|
)
|
||||||
|
|
||||||
|
ur_control_node = Node(
|
||||||
|
package="ur_robot_driver",
|
||||||
|
executable="ur_ros2_control_node",
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
update_rate_config_file,
|
||||||
|
ParameterFile(initial_joint_controllers, allow_substs=True),
|
||||||
|
],
|
||||||
|
output="screen",
|
||||||
|
condition=UnlessCondition(use_fake_hardware),
|
||||||
|
)
|
||||||
|
|
||||||
|
robotiq_gripper_controller_spawner = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="spawner",
|
||||||
|
arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
|
||||||
|
)
|
||||||
|
|
||||||
|
robotiq_activation_controller_spawner = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="spawner",
|
||||||
|
arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
|
||||||
|
)
|
||||||
|
|
||||||
|
dashboard_client_node = Node(
|
||||||
|
package="ur_robot_driver",
|
||||||
|
condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware),
|
||||||
|
executable="dashboard_client",
|
||||||
|
name="dashboard_client",
|
||||||
|
output="screen",
|
||||||
|
emulate_tty=True,
|
||||||
|
parameters=[{"robot_ip": robot_ip}],
|
||||||
|
)
|
||||||
|
|
||||||
|
tool_communication_node = Node(
|
||||||
|
package="ur_robot_driver",
|
||||||
|
condition=IfCondition(use_tool_communication),
|
||||||
|
executable="tool_communication.py",
|
||||||
|
name="ur_tool_comm",
|
||||||
|
output="screen",
|
||||||
|
parameters=[
|
||||||
|
{
|
||||||
|
"robot_ip": robot_ip,
|
||||||
|
"tcp_port": tool_tcp_port,
|
||||||
|
"device_name": tool_device_name,
|
||||||
|
}
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
urscript_interface = Node(
|
||||||
|
package="ur_robot_driver",
|
||||||
|
executable="urscript_interface",
|
||||||
|
parameters=[{"robot_ip": robot_ip}],
|
||||||
|
output="screen",
|
||||||
|
)
|
||||||
|
|
||||||
|
controller_stopper_node = Node(
|
||||||
|
package="ur_robot_driver",
|
||||||
|
executable="controller_stopper_node",
|
||||||
|
name="controller_stopper",
|
||||||
|
output="screen",
|
||||||
|
emulate_tty=True,
|
||||||
|
condition=UnlessCondition(use_fake_hardware),
|
||||||
|
parameters=[
|
||||||
|
{"headless_mode": headless_mode},
|
||||||
|
{"joint_controller_active": activate_joint_controller},
|
||||||
|
{
|
||||||
|
"consistent_controllers": [
|
||||||
|
"io_and_status_controller",
|
||||||
|
"force_torque_sensor_broadcaster",
|
||||||
|
"joint_state_broadcaster",
|
||||||
|
"speed_scaling_state_broadcaster",
|
||||||
|
]
|
||||||
|
},
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
robot_state_publisher_node = Node(
|
||||||
|
package="robot_state_publisher",
|
||||||
|
executable="robot_state_publisher",
|
||||||
|
output="both",
|
||||||
|
parameters=[robot_description],
|
||||||
|
)
|
||||||
|
|
||||||
|
rviz_node = Node(
|
||||||
|
package="rviz2",
|
||||||
|
condition=IfCondition(launch_rviz),
|
||||||
|
executable="rviz2",
|
||||||
|
name="rviz2",
|
||||||
|
output="log",
|
||||||
|
arguments=["-d", rviz_config_file],
|
||||||
|
)
|
||||||
|
|
||||||
|
# Spawn controllers
|
||||||
|
def controller_spawner(name, active=True):
|
||||||
|
inactive_flags = ["--inactive"] if not active else []
|
||||||
|
return Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="spawner",
|
||||||
|
arguments=[
|
||||||
|
name,
|
||||||
|
"--controller-manager",
|
||||||
|
"/controller_manager",
|
||||||
|
"--controller-manager-timeout",
|
||||||
|
controller_spawner_timeout,
|
||||||
|
]
|
||||||
|
+ inactive_flags,
|
||||||
|
)
|
||||||
|
|
||||||
|
controller_spawner_names = [
|
||||||
|
"joint_state_broadcaster",
|
||||||
|
"io_and_status_controller",
|
||||||
|
"speed_scaling_state_broadcaster",
|
||||||
|
"force_torque_sensor_broadcaster",
|
||||||
|
]
|
||||||
|
controller_spawner_inactive_names = ["forward_position_controller"]
|
||||||
|
|
||||||
|
controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
|
||||||
|
controller_spawner(name, active=False) for name in controller_spawner_inactive_names
|
||||||
|
]
|
||||||
|
|
||||||
|
# There may be other controllers of the joints, but this is the initially-started one
|
||||||
|
initial_joint_controller_spawner_started = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="spawner",
|
||||||
|
arguments=[
|
||||||
|
initial_joint_controller,
|
||||||
|
"-c",
|
||||||
|
"/controller_manager",
|
||||||
|
"--controller-manager-timeout",
|
||||||
|
controller_spawner_timeout,
|
||||||
|
],
|
||||||
|
condition=IfCondition(activate_joint_controller),
|
||||||
|
)
|
||||||
|
initial_joint_controller_spawner_stopped = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="spawner",
|
||||||
|
arguments=[
|
||||||
|
initial_joint_controller,
|
||||||
|
"-c",
|
||||||
|
"/controller_manager",
|
||||||
|
"--controller-manager-timeout",
|
||||||
|
controller_spawner_timeout,
|
||||||
|
"--inactive",
|
||||||
|
],
|
||||||
|
condition=UnlessCondition(activate_joint_controller),
|
||||||
|
)
|
||||||
|
|
||||||
|
nodes_to_start = [
|
||||||
|
control_node,
|
||||||
|
ur_control_node,
|
||||||
|
robotiq_gripper_controller_spawner,
|
||||||
|
robotiq_activation_controller_spawner,
|
||||||
|
dashboard_client_node,
|
||||||
|
tool_communication_node,
|
||||||
|
controller_stopper_node,
|
||||||
|
urscript_interface,
|
||||||
|
robot_state_publisher_node,
|
||||||
|
rviz_node,
|
||||||
|
initial_joint_controller_spawner_stopped,
|
||||||
|
initial_joint_controller_spawner_started,
|
||||||
|
] + controller_spawners
|
||||||
|
|
||||||
|
return nodes_to_start
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
# Declare Arguments
|
||||||
|
declared_arguments = []
|
||||||
|
# UR specific arguments
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"ur_type",
|
||||||
|
description="Type/series of used UR robot.",
|
||||||
|
default_value="ur3e",
|
||||||
|
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"robot_ip", description="IP address by which the robot can be reached."
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"safety_limits",
|
||||||
|
default_value="true",
|
||||||
|
description="Enables the safety limits controller if true.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"safety_pos_margin",
|
||||||
|
default_value="0.15",
|
||||||
|
description="The margin to lower and upper limits in the safety controller.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"safety_k_position",
|
||||||
|
default_value="20",
|
||||||
|
description="k-position factor in the safety controller.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
# General arguments
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"runtime_config_package",
|
||||||
|
default_value="ur_robot_driver",
|
||||||
|
description='Package with the controller\'s configuration in "config" folder. \
|
||||||
|
Usually the argument is not set, it enables use of a custom setup.',
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"controllers_file",
|
||||||
|
default_value="ur_robotiq_controllers.yaml",
|
||||||
|
description="YAML file with the controllers configuration.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"description_package",
|
||||||
|
default_value="ur_robotiq_description",
|
||||||
|
description="Description package with robot URDF/XACRO files. Usually the argument \
|
||||||
|
is not set, it enables use of a custom description.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"ur_description_package",
|
||||||
|
default_value="ur_description",
|
||||||
|
description="Description package with robot URDF/XACRO files. Usually the argument \
|
||||||
|
is not set, it enables use of a custom description.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"description_file",
|
||||||
|
default_value="ur_robotiq.urdf.xacro",
|
||||||
|
description="URDF/XACRO description file with the robot.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tf_prefix",
|
||||||
|
default_value="",
|
||||||
|
description="tf_prefix of the joint names, useful for \
|
||||||
|
multi-robot setup. If changed, also joint names in the controllers' configuration \
|
||||||
|
have to be updated.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"use_fake_hardware",
|
||||||
|
default_value="true",
|
||||||
|
description="Start robot with fake hardware mirroring command to its states.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"fake_sensor_commands",
|
||||||
|
default_value="false",
|
||||||
|
description="Enable fake command interfaces for sensors used for simple simulations. \
|
||||||
|
Used only if 'use_fake_hardware' parameter is true.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"headless_mode",
|
||||||
|
default_value="false",
|
||||||
|
description="Enable headless mode for robot control",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"controller_spawner_timeout",
|
||||||
|
default_value="10",
|
||||||
|
description="Timeout used when spawning controllers.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"initial_joint_controller",
|
||||||
|
default_value="scaled_joint_trajectory_controller",
|
||||||
|
description="Initially loaded robot controller.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"activate_joint_controller",
|
||||||
|
default_value="true",
|
||||||
|
description="Activate loaded joint controller.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"launch_dashboard_client", default_value="true", description="Launch Dashboard Client?"
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"use_tool_communication",
|
||||||
|
default_value="false",
|
||||||
|
description="Only available for e series!",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_parity",
|
||||||
|
default_value="0",
|
||||||
|
description="Parity configuration for serial communication. Only effective, if \
|
||||||
|
use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_baud_rate",
|
||||||
|
default_value="115200",
|
||||||
|
description="Baud rate configuration for serial communication. Only effective, if \
|
||||||
|
use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_stop_bits",
|
||||||
|
default_value="1",
|
||||||
|
description="Stop bits configuration for serial communication. Only effective, if \
|
||||||
|
use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_rx_idle_chars",
|
||||||
|
default_value="1.5",
|
||||||
|
description="RX idle chars configuration for serial communication. Only effective, \
|
||||||
|
if use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_tx_idle_chars",
|
||||||
|
default_value="3.5",
|
||||||
|
description="TX idle chars configuration for serial communication. Only effective, \
|
||||||
|
if use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_device_name",
|
||||||
|
default_value="/tmp/ttyUR",
|
||||||
|
description="File descriptor that will be generated for the tool communication device. \
|
||||||
|
The user has be be allowed to write to this location. \
|
||||||
|
Only effective, if use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_tcp_port",
|
||||||
|
default_value="54321",
|
||||||
|
description="Remote port that will be used for bridging the tool's serial device. \
|
||||||
|
Only effective, if use_tool_communication is set to True.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"tool_voltage",
|
||||||
|
default_value="0", # 0 being a conservative value that won't destroy anything
|
||||||
|
description="Tool voltage that will be setup.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"reverse_ip",
|
||||||
|
default_value="0.0.0.0",
|
||||||
|
description="IP that will be used for the robot controller to communicate back to the driver.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"script_command_port",
|
||||||
|
default_value="50004",
|
||||||
|
description="Port that will be opened to forward URScript commands to the robot.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"reverse_port",
|
||||||
|
default_value="50001",
|
||||||
|
description="Port that will be opened to send cyclic instructions from the driver to the robot controller.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"script_sender_port",
|
||||||
|
default_value="50002",
|
||||||
|
description="The driver will offer an interface to query the external_control URScript on this port.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"trajectory_port",
|
||||||
|
default_value="50003",
|
||||||
|
description="Port that will be opened for trajectory control.",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
|
@ -9,6 +9,26 @@
|
|||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||||
|
<exec_depend>launch</exec_depend>
|
||||||
|
<exec_depend>launch_ros</exec_depend>
|
||||||
|
<exec_depend>robot_state_publisher</exec_depend>
|
||||||
|
<exec_depend>rviz2</exec_depend>
|
||||||
|
<exec_depend>urdf</exec_depend>
|
||||||
|
<exec_depend>xacro</exec_depend>
|
||||||
|
|
||||||
|
<exec_depend>ros2_control</exec_depend>
|
||||||
|
<exec_depend>ros2_controllers</exec_depend>
|
||||||
|
|
||||||
|
<exec_depend>ur_description</exec_depend>
|
||||||
|
<exec_depend>ur_robot_driver</exec_depend>
|
||||||
|
<exec_depend>ur_controllers</exec_depend>
|
||||||
|
<exec_depend>robotiq_description</exec_depend>
|
||||||
|
<exec_depend>robotiq_driver</exec_depend>
|
||||||
|
<exec_depend>robotiq_controllers</exec_depend>
|
||||||
|
<exec_depend>gripper_controllers</exec_depend>
|
||||||
|
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
<test_depend>ament_lint_common</test_depend>
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
@ -1,16 +1,12 @@
|
|||||||
Panels:
|
Panels:
|
||||||
- Class: rviz_common/Displays
|
- Class: rviz_common/Displays
|
||||||
Help Height: 78
|
Help Height: 87
|
||||||
Name: Displays
|
Name: Displays
|
||||||
Property Tree Widget:
|
Property Tree Widget:
|
||||||
Expanded:
|
Expanded:
|
||||||
- /Global Options1
|
- /RobotModel1/Description Topic1
|
||||||
- /Status1
|
|
||||||
- /RobotModel1
|
|
||||||
- /RobotModel1/Status1
|
|
||||||
- /RobotModel1/Links1
|
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
Tree Height: 746
|
Tree Height: 1096
|
||||||
- Class: rviz_common/Selection
|
- Class: rviz_common/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz_common/Tool Properties
|
- Class: rviz_common/Tool Properties
|
||||||
@ -24,11 +20,6 @@ Panels:
|
|||||||
- /Current View1
|
- /Current View1
|
||||||
Name: Views
|
Name: Views
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
- Class: rviz_common/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: ""
|
|
||||||
Visualization Manager:
|
Visualization Manager:
|
||||||
Class: ""
|
Class: ""
|
||||||
Displays:
|
Displays:
|
||||||
@ -90,69 +81,6 @@ Visualization Manager:
|
|||||||
Show Axes: false
|
Show Axes: false
|
||||||
Show Trail: false
|
Show Trail: false
|
||||||
Value: true
|
Value: true
|
||||||
ft_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
left_inner_finger:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
left_inner_finger_pad:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
left_inner_knuckle:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
left_outer_finger:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
left_outer_knuckle:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
right_inner_finger:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
right_inner_finger_pad:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
right_inner_knuckle:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
right_outer_finger:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
right_outer_knuckle:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
robotiq_140_base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
robotiq_base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
shoulder_link:
|
shoulder_link:
|
||||||
Alpha: 1
|
Alpha: 1
|
||||||
Show Axes: false
|
Show Axes: false
|
||||||
@ -167,15 +95,6 @@ Visualization Manager:
|
|||||||
Show Axes: false
|
Show Axes: false
|
||||||
Show Trail: false
|
Show Trail: false
|
||||||
Value: true
|
Value: true
|
||||||
ur_to_robotiq_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
world:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
wrist_1_link:
|
wrist_1_link:
|
||||||
Alpha: 1
|
Alpha: 1
|
||||||
Show Axes: false
|
Show Axes: false
|
||||||
@ -191,9 +110,6 @@ Visualization Manager:
|
|||||||
Show Axes: false
|
Show Axes: false
|
||||||
Show Trail: false
|
Show Trail: false
|
||||||
Value: true
|
Value: true
|
||||||
Mass Properties:
|
|
||||||
Inertia: false
|
|
||||||
Mass: false
|
|
||||||
Name: RobotModel
|
Name: RobotModel
|
||||||
TF Prefix: ""
|
TF Prefix: ""
|
||||||
Update Interval: 0
|
Update Interval: 0
|
||||||
@ -202,7 +118,7 @@ Visualization Manager:
|
|||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 48; 48; 48
|
Background Color: 48; 48; 48
|
||||||
Fixed Frame: world
|
Fixed Frame: base_link
|
||||||
Frame Rate: 30
|
Frame Rate: 30
|
||||||
Name: root
|
Name: root
|
||||||
Tools:
|
Tools:
|
||||||
@ -214,9 +130,6 @@ Visualization Manager:
|
|||||||
- Class: rviz_default_plugins/Measure
|
- Class: rviz_default_plugins/Measure
|
||||||
Line color: 128; 128; 0
|
Line color: 128; 128; 0
|
||||||
- Class: rviz_default_plugins/SetInitialPose
|
- Class: rviz_default_plugins/SetInitialPose
|
||||||
Covariance x: 0.25
|
|
||||||
Covariance y: 0.25
|
|
||||||
Covariance yaw: 0.06853891909122467
|
|
||||||
Topic:
|
Topic:
|
||||||
Depth: 5
|
Depth: 5
|
||||||
Durability Policy: Volatile
|
Durability Policy: Volatile
|
||||||
@ -245,41 +158,39 @@ Visualization Manager:
|
|||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz_default_plugins/Orbit
|
Class: rviz_default_plugins/Orbit
|
||||||
Distance: 1.6701574325561523
|
Distance: 3.493516445159912
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
Swap Stereo Eyes: false
|
Swap Stereo Eyes: false
|
||||||
Value: false
|
Value: false
|
||||||
Focal Point:
|
Focal Point:
|
||||||
X: 0
|
X: -0.0457618348300457
|
||||||
Y: 0
|
Y: -0.07058511674404144
|
||||||
Z: 0
|
Z: 0.49734944105148315
|
||||||
Focal Shape Fixed Size: true
|
Focal Shape Fixed Size: true
|
||||||
Focal Shape Size: 0.05000000074505806
|
Focal Shape Size: 0.05000000074505806
|
||||||
Invert Z Axis: false
|
Invert Z Axis: false
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.009999999776482582
|
Near Clip Distance: 0.009999999776482582
|
||||||
Pitch: 0.785398006439209
|
Pitch: 0.15039828419685364
|
||||||
Target Frame: <Fixed Frame>
|
Target Frame: <Fixed Frame>
|
||||||
Value: Orbit (rviz)
|
Value: Orbit (rviz)
|
||||||
Yaw: 0.785398006439209
|
Yaw: 0.5353983640670776
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 1043
|
Height: 1379
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: false
|
||||||
QMainWindow State: 000000ff00000000fd0000000400000000000002a000000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c50000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000004f00000010101000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000048000004f0000000db01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
Tool Properties:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 1920
|
Width: 2560
|
||||||
X: 0
|
X: 0
|
||||||
Y: 0
|
Y: 30
|
@ -5,7 +5,7 @@
|
|||||||
<xacro:arg name="name" default="ur"/>
|
<xacro:arg name="name" default="ur"/>
|
||||||
<!-- import main macro -->
|
<!-- import main macro -->
|
||||||
<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
|
<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
|
||||||
<xacro:include filename="$(find robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
|
<xacro:include filename="$(find ur_robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
|
||||||
<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
|
<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
|
||||||
|
|
||||||
<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
|
<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
|
||||||
|
@ -0,0 +1,39 @@
|
|||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
||||||
|
<xacro:macro name="ur_to_robotiq" params="prefix parent child rotation:=^|${0.0}">
|
||||||
|
|
||||||
|
<joint name="${prefix}ur_to_robotiq_joint" type="fixed">
|
||||||
|
<!-- The parent link must be read from the robot model it is attached to. -->
|
||||||
|
<parent link="${parent}"/>
|
||||||
|
<child link="${prefix}ur_to_robotiq_link"/>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 ${rotation}"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${prefix}ur_to_robotiq_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://robotiq_description/meshes/visual/2f_85/ur_to_robotiq_adapter.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://robotiq_description/meshes/collision/2f_85/ur_to_robotiq_adapter.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01" />
|
||||||
|
<origin xyz="0 0 0" />
|
||||||
|
<inertia ixx="0.000044" ixy="0.0" ixz="0.0" iyy="0.000027" iyz="0.0" izz="0.000027" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="${prefix}gripper_side_joint" type="fixed">
|
||||||
|
<parent link="${prefix}ur_to_robotiq_link"/>
|
||||||
|
<child link="${child}"/>
|
||||||
|
<!-- <origin xyz="0 0 0.011" rpy="0 ${-pi/2} ${pi/2}"/> -->
|
||||||
|
<origin xyz="0 0 0.011" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
@ -5,8 +5,7 @@ find_package(ament_cmake REQUIRED)
|
|||||||
|
|
||||||
ament_package()
|
ament_package()
|
||||||
|
|
||||||
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
|
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
|
||||||
PATTERN "setup_assistant.launch" EXCLUDE)
|
|
||||||
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
|
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
|
||||||
install(DIRECTORY srdf DESTINATION share/${PROJECT_NAME})
|
install(DIRECTORY srdf DESTINATION share/${PROJECT_NAME})
|
||||||
install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
|
install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
|
||||||
|
@ -1,6 +1,8 @@
|
|||||||
controller_names:
|
controller_names:
|
||||||
- scaled_joint_trajectory_controller
|
- scaled_joint_trajectory_controller
|
||||||
- joint_trajectory_controller
|
- joint_trajectory_controller
|
||||||
|
- robotiq_gripper_controller
|
||||||
|
- robotiq_activation_controller
|
||||||
|
|
||||||
|
|
||||||
scaled_joint_trajectory_controller:
|
scaled_joint_trajectory_controller:
|
||||||
@ -27,3 +29,16 @@ joint_trajectory_controller:
|
|||||||
- wrist_1_joint
|
- wrist_1_joint
|
||||||
- wrist_2_joint
|
- wrist_2_joint
|
||||||
- wrist_3_joint
|
- wrist_3_joint
|
||||||
|
|
||||||
|
robotiq_gripper_controller:
|
||||||
|
action_ns: antipodal_gripper_action_controller
|
||||||
|
type: GripperActionController
|
||||||
|
default: true
|
||||||
|
joint: robotiq_85_left_knuckle_joint
|
||||||
|
use_effort_interface: true
|
||||||
|
use_speed_interface: true
|
||||||
|
|
||||||
|
robotiq_activation_controller:
|
||||||
|
action_ns: robotiq_controllers
|
||||||
|
type: RobotiqActivationController
|
||||||
|
default: true
|
||||||
|
@ -7,3 +7,4 @@ initial_positions:
|
|||||||
wrist_1_joint: 0
|
wrist_1_joint: 0
|
||||||
wrist_2_joint: 0
|
wrist_2_joint: 0
|
||||||
wrist_3_joint: 0
|
wrist_3_joint: 0
|
||||||
|
finger_joint: 0.695
|
@ -0,0 +1,3 @@
|
|||||||
|
controller_manager:
|
||||||
|
ros__parameters:
|
||||||
|
update_rate: 500 # Hz
|
Loading…
Reference in New Issue
Block a user