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

controller from ur_robotiq_control.launch.py starts up

Browse Source 
ur_robotiq_moveit.launch.py cant find gripper_gap joint
This commit is contained in:
NikoFeith 2024-05-13 11:34:34 +02:00
parent d2768af3c5
commit bcba4ef3d2
11 changed files with 157 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.idea/vcs.xml
View File

@ -4,6 +4,5 @@
<mapping directory="$PROJECT_DIR$" vcs="Git" />
<mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Description" vcs="Git" />
<mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Driver" vcs="Git" />
<mapping directory="$PROJECT_DIR$/src/cartesian_controllers" vcs="Git" />
</component>
</project>

122
src/ur_robotiq_moveit_config/config/joint_limits.yaml
View File

@ -1,50 +1,90 @@
# 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]
# Joints limits
#
# Sources:
#
# - Universal Robots e-Series, User Manual, UR3e, Version 5.8
# https://s3-eu-west-1.amazonaws.com/ur-support-site/69043/99403_UR3e_User_Manual_en_Global.pdf
# - Support > Articles > UR articles > Max. joint torques
# https://www.universal-robots.com/articles/ur-articles/max-joint-torques
# retrieved: 2020-06-16, last modified: 2020-06-09
joint_limits:
elbow_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
has_acceleration_limits: false
max_acceleration: 0
left_inner_knuckle_joint:
has_velocity_limits: true
max_velocity: 2
has_acceleration_limits: false
max_acceleration: 0
right_inner_knuckle_joint:
has_velocity_limits: true
max_velocity: 2
has_acceleration_limits: false
max_acceleration: 0
shoulder_lift_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
has_acceleration_limits: false
max_acceleration: 0
shoulder_pan_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
# acceleration limits are not publicly available
has_acceleration_limits: false
max_acceleration: 0
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 56.0
max_position: !degrees 360.0
max_velocity: !degrees 180.0
min_position: !degrees -360.0
shoulder_lift_joint:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 56.0
max_position: !degrees 360.0
max_velocity: !degrees 180.0
min_position: !degrees -360.0
elbow_joint:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 28.0
# we artificially limit this joint to half its actual joint position limit
# to avoid (MoveIt/OMPL) planning problems, as due to the physical
# construction of the robot, it's impossible to rotate the 'elbow_joint'
# over more than approx +- 1 pi (the shoulder lift joint gets in the way).
#
# This leads to planning problems as the search space will be divided into
# two sections, with no connections from one to the other.
#
# Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
# more information.
max_position: !degrees 180.0
max_velocity: !degrees 180.0
min_position: !degrees -180.0
wrist_1_joint:
has_velocity_limits: true
max_velocity: 6.2831853071795862
# acceleration limits are not publicly available
has_acceleration_limits: false
max_acceleration: 0
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 12.0
max_position: !degrees 360.0
max_velocity: !degrees 360.0
min_position: !degrees -360.0
wrist_2_joint:
has_velocity_limits: true
max_velocity: 6.2831853071795862
# acceleration limits are not publicly available
has_acceleration_limits: false
max_acceleration: 0
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 12.0
max_position: !degrees 360.0
max_velocity: !degrees 360.0
min_position: !degrees -360.0
wrist_3_joint:
has_velocity_limits: true
max_velocity: 6.2831853071795862
# acceleration limits are not publicly available
has_acceleration_limits: false
max_acceleration: 0
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 12.0
max_position: !degrees 360.0
max_velocity: !degrees 360.0
min_position: !degrees -360.0
gripper_gap:
# acceleration limits are not publicly available
has_acceleration_limits: false
has_effort_limits: true
has_position_limits: true
has_velocity_limits: true
max_effort: 235.0
max_position: 0.14
max_velocity: 0.15
min_position: 0.0

1
src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
View File

@ -1,4 +1,3 @@
controller_names:
- scaled_joint_trajectory_controller
- joint_trajectory_controller

2
src/ur_robotiq_moveit_config/config/ur_servo.yaml
View File

@ -2,7 +2,7 @@
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
use_intra_process_comms : true
use_intra_process_comms : True
## Properties of incoming commands
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:

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

@ -40,17 +40,17 @@ def launch_setup(context, *args, **kwargs):
launch_servo = LaunchConfiguration("launch_servo")
# File and package names
ur_description_package = "ur_description"
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.srdf.xacro"
ur_description_package = LaunchConfiguration("ur_description_package")
ur_robotiq_description_package = LaunchConfiguration("ur_robotiq_description_package")
ur_robotiq_description_file = LaunchConfiguration("ur_robotiq_description_file")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
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"]
[FindPackageShare(ur_robotiq_description_package), "config", "ur_robotiq_calibration.yaml"]
)
physical_params = PathJoinSubstitution(
[FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs):
[FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
@ -126,7 +128,7 @@ def launch_setup(context, *args, **kwargs):
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml")
robot_description_kinematics_params = load_yaml("ur_robotiq_moveit_config", "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")
@ -140,16 +142,15 @@ def launch_setup(context, *args, **kwargs):
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
ompl_planning_yaml = load_yaml("ur_robotiq_moveit_config", "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
# the scaled_joint_trajectory_controller does not work on fake hardware
use_fake_hw = use_fake_hardware.perform(context)
controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
controllers_yaml = load_yaml("ur_robotiq_moveit_config", "config/moveit_controllers.yaml")
if use_fake_hw == "true":
if use_fake_hw == "true" or use_fake_hw == "True":
controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
controllers_yaml["joint_trajectory_controller"]["default"] = True
@ -219,7 +220,7 @@ def launch_setup(context, *args, **kwargs):
# Servo node for realtime control
servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml")
servo_yaml = load_yaml("ur_robotiq_moveit_config", "config/ur_servo.yaml")
servo_params = {"moveit_servo": servo_yaml}
servo_node = Node(
package="moveit_servo",
@ -241,7 +242,46 @@ def launch_setup(context, *args, **kwargs):
def generate_launch_description():
# Declare the Launch arguments
declared_arguments = []
# UR specific arguments
declared_arguments.append(
DeclareLaunchArgument(
"ur_description_package",
description="Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
default_value="ur_description",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"ur_robotiq_description_package",
description="Description package with robot and gripper URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
default_value="ur_robotiq_description",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"ur_robotiq_description_file",
description="URDF/XACRO description file with the robot.",
default_value="ur_robotiq.urdf.xacro",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
description="MoveIt config package with robot and gripper SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
default_value="ur_robotiq_moveit_config",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
description="MoveIt SRDF/XACRO description file with the robot and gripper.",
default_value="ur_robotiq.srdf.xacro",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"ur_type",

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

@ -30,10 +30,10 @@
<joint name="${prefix}base_to_dummy"/>
</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}open" group="${prefix}gripper">
<group_state name="open" group="${prefix}gripper">
<joint name="${prefix}gripper_gap" value="0.14"/>
</group_state>
<group_state name="${prefix}closed" group="${prefix}gripper">
<group_state name="closed" group="${prefix}gripper">
<joint name="${prefix}gripper_gap" value="0.0"/>
</group_state>
<group_state name="${prefix}home" group="${prefix}${name}_manipulator">

4
src/ur_robotiq_servo/config/ps5-params.yaml
View File

@ -1,6 +1,6 @@
ps5_servo_node:
ros__parameters:
linear_speed_multiplier: 0.2
gripper_speed_multiplier: 0.02
gripper_position_multiplier: 0.02
gripper_lower_limit: 0.0
gripper_upper_limit: 0.70
gripper_upper_limit: 0.14

1
src/ur_robotiq_servo/package.xml
View File

@ -11,6 +11,7 @@
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>robotiq_2f_msgs</depend>
<depend>std_srvs</depend>
<depend>joy</depend>
<depend>moveit_servo</depend>

35
src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
View File

@ -4,7 +4,7 @@ from sensor_msgs.msg import Joy, JointState
from geometry_msgs.msg import TwistStamped
from control_msgs.msg import JointJog
from std_srvs.srv import Trigger
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout
from robotiq_2f_msgs.msg import ForwardCommand
class PS5ControllerNode(Node):
@ -24,17 +24,17 @@ class PS5ControllerNode(Node):
self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value
self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}")
self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0)
self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier')
self.gripper_position_multiplier = self.declare_parameter('gripper_position_multiplier', 1.0)
self.gripper_position_multiplier = (self.get_parameter('gripper_position_multiplier')
.get_parameter_value().double_value)
self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}")
self.get_logger().info(f"Gripper position multiplier: {self.gripper_position_multiplier}")
self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0)
self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 0.0)
self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit')
.get_parameter_value().double_value)
self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}")
self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0)
self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 0.14)
self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit')
.get_parameter_value().double_value)
self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}")
@ -62,15 +62,15 @@ class PS5ControllerNode(Node):
10)
self.gripper_cmd_pub = self.create_publisher(
Float64MultiArray,
'/forward_gripper_position_controller/commands',
ForwardCommand,
'/forward_gripper_position_controller/command',
10)
# Services
self.servo_client = self.create_client(Trigger, '/servo_node/start_servo')
srv_msg = Trigger.Request()
while not self.servo_client.wait_for_service(timeout_sec=1.0):
while not self.servo_client.wait_for_service(timeout_sec=5.0):
self.get_logger().info('/servo_node/start_servo service not available, waiting again...')
self.call_start_servo()
@ -89,7 +89,7 @@ class PS5ControllerNode(Node):
def joint_state_callback(self, msg):
try:
index = msg.name.index('finger_joint')
index = msg.name.index('gripper_gap')
self.last_finger_joint_angle = msg.position[index]
except ValueError:
self.get_logger().error('finger_joint not found in /joint_states msg')
@ -140,19 +140,14 @@ class PS5ControllerNode(Node):
# Gripper controller
new_finger_joint_angle = (self.last_finger_joint_angle +
(msg.buttons[1] - msg.buttons[0]) * self.gripper_speed_multiplier)
(msg.buttons[1] - msg.buttons[0]) * self.gripper_position_multiplier)
if self.gripper_lower_limit > new_finger_joint_angle or self.gripper_upper_limit < new_finger_joint_angle:
self.get_logger().debug(f"New finger joint angle out of bounds: {new_finger_joint_angle}")
new_finger_joint_angle = self.last_finger_joint_angle
gripper_msg = Float64MultiArray()
layout_msg = MultiArrayLayout()
layout_msg.dim = [MultiArrayDimension()]
layout_msg.dim[0].label = "finger_joint"
layout_msg.dim[0].size = 1
layout_msg.dim[0].stride = 1
layout_msg.data_offset = 0
gripper_msg.layout = layout_msg
gripper_msg.data = [new_finger_joint_angle]
gripper_msg = ForwardCommand()
gripper_msg.position = new_finger_joint_angle
gripper_msg.max_velocity = 0.15
gripper_msg.max_effort = 235.0
self.gripper_cmd_pub.publish(gripper_msg)