Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ur_robotiq_control.launch.py

Browse Source 
and ur_robotiq_moveit.launch.py
Working
This commit is contained in:
Niko Feith 2024-03-22 16:51:46 +01:00
parent 1e52a8f81f
commit 6449dccd60
12 changed files with 389 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Universal_Robots_ROS2_Description

@ -1 +1 @@
Subproject commit c1c8ac92cd09e1b550837ad3566b427a957caf7c
Subproject commit 2e178fad2651b08c94324a8f96152cd1d78816dc

108
src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py → src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
View File

@ -191,11 +191,7 @@ def launch_setup(context, *args, **kwargs):
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
robot_description,
update_rate_config_file,
ParameterFile(initial_joint_controllers, allow_substs=True),
],
parameters=[robot_description, update_rate_config_file, initial_joint_controllers],
output="screen",
condition=IfCondition(use_fake_hardware),
)
@ -203,11 +199,7 @@ def launch_setup(context, *args, **kwargs):
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),
],
parameters=[robot_description, update_rate_config_file, initial_joint_controllers],
output="screen",
condition=UnlessCondition(use_fake_hardware),
)
@ -216,12 +208,14 @@ def launch_setup(context, *args, **kwargs):
package="controller_manager",
executable="spawner",
arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
condition=UnlessCondition(use_fake_hardware),
)
robotiq_activation_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
condition=UnlessCondition(use_fake_hardware),
)
dashboard_client_node = Node(
@ -249,13 +243,6 @@ def launch_setup(context, *args, **kwargs):
],
)
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",
@ -272,6 +259,8 @@ def launch_setup(context, *args, **kwargs):
"force_torque_sensor_broadcaster",
"joint_state_broadcaster",
"speed_scaling_state_broadcaster",
"robotiq_gripper_controller",
"robotiq_activation_controller",
]
},
],
@ -293,58 +282,55 @@ def launch_setup(context, *args, **kwargs):
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,
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
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"]
io_and_status_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["io_and_status_controller", "-c", "/controller_manager"],
)
controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
controller_spawner(name, active=False) for name in controller_spawner_inactive_names
]
speed_scaling_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"speed_scaling_state_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
force_torque_sensor_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"force_torque_sensor_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
forward_position_controller_spawner_stopped = Node(
package="controller_manager",
executable="spawner",
arguments=["forward_position_controller", "-c", "/controller_manager", "--stopped"],
)
# 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,
],
arguments=[initial_joint_controller, "-c", "/controller_manager"],
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",
],
arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"],
condition=UnlessCondition(activate_joint_controller),
)
@ -356,12 +342,16 @@ def launch_setup(context, *args, **kwargs):
dashboard_client_node,
tool_communication_node,
controller_stopper_node,
urscript_interface,
robot_state_publisher_node,
rviz_node,
joint_state_broadcaster_spawner,
io_and_status_controller_spawner,
speed_scaling_state_broadcaster_spawner,
force_torque_sensor_broadcaster_spawner,
forward_position_controller_spawner_stopped,
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
] + controller_spawners
]
return nodes_to_start

10
src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro
View File

@ -1,5 +1,5 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_manipulator">
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
<!-- parameters -->
<xacro:arg name="name" default="ur_manipulator"/>
@ -99,18 +99,12 @@
<origin xyz="0 0 0" rpy="0 0 0" /> <!-- position robot in the world -->
</xacro:ur_robot>
<xacro:robotiq_gripper name="$(arg tf_prefix)gripper"
prefix="$(arg tf_prefix)"
parent="$(arg tf_prefix)ur_to_robotiq_link"
parent="$(arg tf_prefix)tool0"
use_fake_hardware="$(arg use_fake_hardware)"
>
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:robotiq_gripper>
<xacro:ur_to_robotiq prefix="$(arg tf_prefix)"
parent="$(arg tf_prefix)tool0"
child="$(arg tf_prefix)robotiq_base_link"
>
</xacro:ur_to_robotiq>
</robot>

14
src/ur_robotiq_moveit_config/config/initial_positions.yaml
View File

@ -1,7 +1,7 @@
elbow_joint: 0
shoulder_lift_joint: 0
shoulder_pan_joint: 0
wrist_1_joint: 0
wrist_2_joint: 0
wrist_3_joint: 0
finger_joint: 0.695
shoulder_pan_joint: 0.0
shoulder_lift_joint: -1.57
elbow_joint: 0.0
wrist_1_joint: -1.57
wrist_2_joint: 0.0
wrist_3_joint: 0.0
finger_joint: 20.0

14
src/ur_robotiq_moveit_config/config/kinematics.yaml
View File

@ -1,12 +1,4 @@
ur_manipulator:
ros__parameters:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
gripper:
ros__parameters:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.05

8
src/ur_robotiq_moveit_config/config/controllers.yaml → src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
View File

@ -1,3 +1,4 @@
controller_names:
- scaled_joint_trajectory_controller
- joint_trajectory_controller
@ -30,11 +31,10 @@ joint_trajectory_controller:
- wrist_3_joint
robotiq_gripper_controller:
action_ns: position_controllers
type: GripperActionController
action_ns: gripper_cmd
type: GripperCommand
default: true
joints:
- finger_joint
use_effort_interface: true
use_speed_interface: true

116
src/ur_robotiq_moveit_config/config/ompl_planning.yaml
View File

@ -46,12 +46,83 @@ planner_configs:
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup()
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
PRMkConfigDefault:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PRMstarkConfigDefault:
type: geometric::PRMstar
FMTkConfigDefault:
type: geometric::FMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
nearest_k: 1 # use Knearest strategy. default: 1
cache_cc: 1 # use collision checking cache. default: 1
heuristics: 0 # activate cost to go heuristics. default: 0
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
BFMTkConfigDefault:
type: geometric::BFMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
nearest_k: 1 # use the Knearest strategy. default: 1
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
heuristics: 1 # activates cost to go heuristics. default: 1
cache_cc: 1 # use the collision checking cache. default: 1
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
PDSTkConfigDefault:
type: geometric::PDST
STRIDEkConfigDefault:
type: geometric::STRIDE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
max_degree: 18 # max degree of a node in the GNAT. default: 12
min_degree: 12 # min degree of a node in the GNAT. default: 12
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
BiTRRTkConfigDefault:
type: geometric::BiTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
init_temperature: 100 # initial temperature. default: 100
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
LBTRRTkConfigDefault:
type: geometric::LBTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
epsilon: 0.4 # optimality approximation factor. default: 0.4
BiESTkConfigDefault:
type: geometric::BiEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ProjESTkConfigDefault:
type: geometric::ProjEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LazyPRMkConfigDefault:
type: geometric::LazyPRM
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
LazyPRMstarkConfigDefault:
type: geometric::LazyPRMstar
SPARSkConfigDefault:
type: geometric::SPARS
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 1000 # maximum consecutive failure limit. default: 1000
SPARStwokConfigDefault:
type: geometric::SPARStwo
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
TrajOptDefault:
type: geometric::TrajOpt
ur_manipulator:
planner_configs:
- SBLkConfigDefault
@ -65,6 +136,43 @@ ur_manipulator:
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
#projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
longest_valid_segment_fraction: 0.01
- FMTkConfigDefault
- BFMTkConfigDefault
- PDSTkConfigDefault
- STRIDEkConfigDefault
- BiTRRTkConfigDefault
- LBTRRTkConfigDefault
- BiESTkConfigDefault
- ProjESTkConfigDefault
- LazyPRMkConfigDefault
- LazyPRMstarkConfigDefault
- SPARSkConfigDefault
- SPARStwokConfigDefault
- TrajOptDefault
gripper:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
- FMTkConfigDefault
- BFMTkConfigDefault
- PDSTkConfigDefault
- STRIDEkConfigDefault
- BiTRRTkConfigDefault
- LBTRRTkConfigDefault
- BiESTkConfigDefault
- ProjESTkConfigDefault
- LazyPRMkConfigDefault
- LazyPRMstarkConfigDefault
- SPARSkConfigDefault
- SPARStwokConfigDefault
- TrajOptDefault

70
src/ur_robotiq_moveit_config/config/ompl_planning_old.yaml Normal file
View File

@ -0,0 +1,70 @@
planner_configs:
SBLkConfigDefault:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ESTkConfigDefault:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECEkConfigDefault:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECEkConfigDefault:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
KPIECEkConfigDefault:
type: geometric::KPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
RRTkConfigDefault:
type: geometric::RRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
RRTConnectkConfigDefault:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstarkConfigDefault:
type: geometric::RRTstar
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
TRRTkConfigDefault:
type: geometric::TRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup()
PRMkConfigDefault:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PRMstarkConfigDefault:
type: geometric::PRMstar
ur_manipulator:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
#projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
longest_valid_segment_fraction: 0.01

132
src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
View File

@ -27,22 +27,24 @@ def load_yaml(package_name, file_path):
def generate_launch_description():
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
robot_name = LaunchConfiguration("robot_name")
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
prefix = LaunchConfiguration("prefix")
use_sim_time = LaunchConfiguration("use_sim_time")
launch_rviz = LaunchConfiguration("launch_rviz")
launch_servo = LaunchConfiguration("launch_servo")
ur_type = "ur3e"
# File and package names
ur_description_package = "ur_description"
ur_moveit_config_package = "ur_moveit_config"
ur_robotiq_description_package = "ur_robotiq_description"
ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
moveit_config_package = "ur_robotiq_moveit_config"
moveit_config_file = "ur_robotiq_macro.srdf.xacro"
moveit_config_file = "ur_robotiq.srdf.xacro"
joint_limit_params = PathJoinSubstitution(
[FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
@ -65,9 +67,6 @@ def generate_launch_description():
" ",
"robot_ip:=xxx.yyy.zzz.www",
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"joint_limit_params:=",
joint_limit_params,
" ",
@ -80,14 +79,17 @@ def generate_launch_description():
"visual_params:=",
visual_params,
" ",
"safety_limits:=true",
"safety_limits:=",
safety_limits,
" ",
"safety_pos_margin:=0.15",
"safety_pos_margin:=",
safety_pos_margin,
" ",
"safety_k_position:=20",
"safety_k_position:=",
safety_k_position,
" ",
"name:=",
robot_name,
"ur",
" ",
"ur_type:=",
ur_type,
@ -114,7 +116,7 @@ def generate_launch_description():
PathJoinSubstitution([FindPackageShare(moveit_config_package), "srdf", moveit_config_file]),
" ",
"name:=",
robot_name,
"ur",
" ",
"prefix:=",
prefix,
@ -124,8 +126,8 @@ def generate_launch_description():
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_kinematics = load_yaml(moveit_config_package, "config/kinematics.yaml")
robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml")
robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics_params}
# robot_description_planning = {
# "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml")
# }
@ -142,10 +144,8 @@ def generate_launch_description():
ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml(moveit_config_package, "config/controllers.yaml")
controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
# the scaled_joint_trajectory_controller does not work on fake hardware
change_controllers = use_fake_hardware
if change_controllers == "true":
@ -170,6 +170,12 @@ def generate_launch_description():
"publish_state_updates": True,
"publish_transforms_updates": True,
}
warehouse_ros_config = {
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
"warehouse_host": warehouse_sqlite_path,
}
# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
@ -185,6 +191,7 @@ def generate_launch_description():
moveit_controllers,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time},
warehouse_ros_config,
],
)
@ -205,6 +212,7 @@ def generate_launch_description():
ompl_planning_pipeline_config,
robot_description_kinematics,
# robot_description_planning,
warehouse_ros_config,
],
)
@ -225,44 +233,76 @@ def generate_launch_description():
)
# Declare the Launch arguments
declared_arguments = []
# UR specific arguments
robot_name_arg = DeclareLaunchArgument(
"robot_name",
default_value="ur",
description="Name of the Universal Robot"
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"],
)
)
use_fake_hardware_arg = DeclareLaunchArgument(
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="true",
default_value="false",
description="Indicate whether robot is running with fake hardware mirroring command to its states.",
)
)
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
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
declared_arguments.append(
DeclareLaunchArgument(
"warehouse_sqlite_path",
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
description="Path where the warehouse database should be stored",
)
)
prefix_arg = DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
declared_arguments.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
)
launch_rviz_arg = DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
launch_servo_arg = DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
launch_args = [
robot_name_arg,
prefix_arg,
use_fake_hardware_arg,
use_sim_time_arg,
launch_rviz_arg,
launch_servo_arg
]
# servo_node,
nodes_to_start = [move_group_node, rviz_node, servo_node]
return LaunchDescription(launch_args + nodes_to_start)
return LaunchDescription(declared_arguments + nodes_to_start)

2
src/ur_robotiq_moveit_config/rviz/view_robot.rviz
View File

@ -308,4 +308,4 @@ Window Geometry:
collapsed: false
Width: 2560
X: 0
Y: 30
Y: 30

4
src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf → src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf.xacro
View File

@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_robotiq_srdf">
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
<!-- robot name parameter -->
<xacro:arg name="name" default="ur_manipulator"/>
<xacro:arg name="name" default="ur"/>
<!-- parameters -->
<xacro:arg name="prefix" default="" />

60
src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
View File

@ -1,5 +1,5 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
<xacro:macro name="ur_robotiq_srdf" params="name prefix">
<!--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-->
@ -11,6 +11,13 @@
</group>
<group name="${prefix}gripper">
<joint name="${prefix}finger_joint"/>
<joint name="${prefix}left_outer_finger_joint" />
<joint name="${prefix}left_inner_knuckle_joint" />
<joint name="${prefix}left_inner_finger_joint" />
<joint name="${prefix}right_outer_knuckle_joint" />
<joint name="${prefix}right_outer_finger_joint" />
<joint name="${prefix}right_inner_knuckle_joint" />
<joint name="${prefix}right_inner_finger_joint" />
</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="${prefix}home" group="${prefix}${name}_manipulator">
@ -36,9 +43,25 @@
<joint name="${prefix}wrist_1_joint" value="-1.2" />
<joint name="${prefix}wrist_2_joint" value="-1.6" />
<joint name="${prefix}wrist_3_joint" value="-0.11" />
</group_state>
<group_state name="open" group="${prefix}gripper">
<joint name="${prefix}finger_joint" value="0" />
</group_state>
<group_state name="closed" group="${prefix}gripper">
<joint name="${prefix}finger_joint" value="0.698132" />
</group_state>
<!--END EFFECTOR - Purpose - Represent information about an end effector.-->
<end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator" />
<!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link" />
<!--PASSIVE JOINT - Purpose - this element is used to mark joints that are not actuated-->
<passive_joint name="${prefix}left_outer_finger_joint" />
<passive_joint name="${prefix}left_inner_knuckle_joint" />
<passive_joint name="${prefix}left_inner_finger_joint" />
<passive_joint name="${prefix}right_outer_knuckle_joint" />
<passive_joint name="${prefix}right_outer_finger_joint" />
<passive_joint name="${prefix}right_inner_knuckle_joint" />
<passive_joint name="${prefix}right_inner_finger_joint" />
<!--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. -->
<!--UR-Robot-->
<disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
@ -53,24 +76,45 @@
<disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never" />
<disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
<!--Robotiq-Gripper-->
<disable_collisions link1="${prefix}tool0" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="left_outer_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="right_outer_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
<!-- Weird Bug-->
<disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
</xacro:macro>
</robot>