From 780639b70eeef7a81af65a6542624d034cdb181d Mon Sep 17 00:00:00 2001 From: Beatriz Leon Date: Wed, 15 Mar 2023 11:50:41 -0500 Subject: [PATCH] Enable autodetection params determine the pack position for different hands (#817) * Adding params * Adding params * Adding params * Adding params * Adding params * Adding params * Adding params * Adding params * Adding params * Adding params * Changing load params * Updating positions to fit hand lite * Fixing comments from review * Fixing lint * Fixing test * Fixing all f strings * Fixing all f strings --- .../config/shadowhand.srdf.xacro | 190 +++++++++++------- .../config/shadowhands_prefix.srdf.xacro | 57 +++--- .../generate_hand_srdf.py | 85 ++++---- .../test/test_end_effector.cpp | 8 +- .../test/test_end_effector.test | 5 +- .../launch/load_robot_description.launch | 3 + .../load_robot_description_bimanual.launch | 5 + 7 files changed, 206 insertions(+), 147 deletions(-) diff --git a/sr_moveit_hand_config/config/shadowhand.srdf.xacro b/sr_moveit_hand_config/config/shadowhand.srdf.xacro index 644ff1885..318d55b30 100644 --- a/sr_moveit_hand_config/config/shadowhand.srdf.xacro +++ b/sr_moveit_hand_config/config/shadowhand.srdf.xacro @@ -42,7 +42,7 @@ - + @@ -50,13 +50,15 @@ + + - + @@ -82,61 +84,118 @@ - + + - - - - - - - - + + + + + + + + + - - - - - - - + + + + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + + - - - - - - - - + + + + + + + + + + + + + + + + + + + + @@ -164,6 +223,7 @@ + @@ -173,8 +233,9 @@ + - + @@ -186,14 +247,15 @@ - - + + + @@ -244,10 +306,9 @@ - - + @@ -257,7 +318,6 @@ - @@ -301,8 +361,7 @@ - - + @@ -317,51 +376,41 @@ - - - - - - + + - - - - - + - - + - - + @@ -371,8 +420,8 @@ - - + + @@ -388,17 +437,14 @@ - - - diff --git a/sr_moveit_hand_config/config/shadowhands_prefix.srdf.xacro b/sr_moveit_hand_config/config/shadowhands_prefix.srdf.xacro index 0f0a8249a..e7434c7df 100644 --- a/sr_moveit_hand_config/config/shadowhands_prefix.srdf.xacro +++ b/sr_moveit_hand_config/config/shadowhands_prefix.srdf.xacro @@ -32,7 +32,8 @@ - + + @@ -153,20 +154,20 @@ - + - + - + - + - + @@ -178,17 +179,17 @@ - + - + - + - + @@ -225,29 +226,29 @@ - + - + - + - + - + @@ -273,20 +274,20 @@ - + - + - + - + - + @@ -295,36 +296,36 @@ - + - + - + - + - + - + - + - + diff --git a/sr_moveit_hand_config/scripts/sr_moveit_hand_config/generate_hand_srdf.py b/sr_moveit_hand_config/scripts/sr_moveit_hand_config/generate_hand_srdf.py index 66997eb8b..f7b07c967 100755 --- a/sr_moveit_hand_config/scripts/sr_moveit_hand_config/generate_hand_srdf.py +++ b/sr_moveit_hand_config/scripts/sr_moveit_hand_config/generate_hand_srdf.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # Software License Agreement (BSD License) # -# Copyright (c) 2015, CITEC, Bielefeld University +# Copyright (c) 2015, 2023 CITEC, Bielefeld University # All rights reserved. # # Redistribution and use in source and binary forms, with or without @@ -29,6 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE. # Author: Guillaume Walck +# Author: Shadow Software Team """ generate the srdf according to the urdf @@ -36,13 +37,11 @@ """ import sys -import os from xml.dom.minidom import parse import xacro import rospy import rospkg from rosgraph.names import load_mappings - from urdf_parser_py.urdf import URDF @@ -55,34 +54,23 @@ def __init__(self, urdf_str=None, load=True, save=True): # load the urdf from the parameter server urdf_str = rospy.get_param('robot_description') - robot = URDF.from_xml_string(urdf_str) extracted_prefix = False prefix = "" first_finger = middle_finger = ring_finger = little_finger = thumb = False is_lite = True - is_biotac = False - hand_name = "right_hand" - - # Check if hand has the old biotac sensors - for key in robot.link_map: - link = robot.link_map[key] - if link.visual: - if hasattr(link.visual.geometry, 'filename'): - filename = os.path.basename(link.visual.geometry.filename) - if filename == "biotac_decimated.dae": - is_biotac = True - break + tip_sensors = "pst" + side = "right" for key in robot.joint_map: # any joint is supposed to have the same prefix and a joint name with 4 chars if not extracted_prefix: prefix = key.split("_")[0] + "_" - rospy.loginfo("Found prefix:" + prefix) + rospy.loginfo(f"Found prefix: {prefix}") extracted_prefix = True if prefix == "lh_": - hand_name = "left_hand" + side = "left" if not first_finger and key.endswith("FFJ4"): first_finger = True @@ -96,32 +84,52 @@ def __init__(self, urdf_str=None, load=True, save=True): thumb = True if is_lite and key.endswith("WRJ2"): is_lite = False - - rospy.logdebug(f"Found fingers (ff mf rf lf th) {str(first_finger)} {str(middle_finger)} " + - f"{str(ring_finger)} {str(little_finger)} {str(thumb)}") - rospy.logdebug(f"is_lite: {str(is_lite)}") - rospy.logdebug(f"is_biotac: {str(is_biotac)}") - rospy.logdebug(f"Hand name: {str(hand_name)}") - - mappings = load_mappings([f'prefix:={str(prefix)}', + hand_name = f"{side}_hand" + + param = f"{side}_tip_sensors" + while not rospy.has_param(param): + rospy.sleep(0.5) + rospy.loginfo(f"waiting for {param}") + # load the tip_sensors from the parameter server after the hand has been auto-detected + tip_sensors_param = rospy.get_param(param) + + # Check if hand has biotac 2p sensors + if tip_sensors_param.find('bt_2p') > -1: + tip_sensors = "bt_2p" + elif tip_sensors_param.find('bt_sp') > -1: + tip_sensors = "bt_sp" + + param = f"{side}_hand_version" + while not rospy.has_param(param): + rospy.sleep(0.5) + rospy.loginfo(f"waiting for {param}") + # load the tip_sensors from the parameter server after the hand has been auto-detected + hand_version = rospy.get_param(param) + + rospy.logdebug(f"Found fingers (ff mf rf lf th): {first_finger} {middle_finger} " + + f"{ring_finger} {little_finger} {thumb}") + rospy.logdebug(f"is_lite: {is_lite}") + rospy.logdebug(f"tip_sensors: {tip_sensors}") + rospy.logdebug(f"hand_version: {hand_version}") + + mappings = load_mappings([f'prefix:={prefix}', f'robot_name:={robot.name}', - f'ff:={str(int(first_finger))}', - f'mf:={str(int(middle_finger))}', - f'rf:={str(int(ring_finger))}', - f'lf:={str(int(little_finger))}', - f'th:={str(int(thumb))}', - f'is_lite:={str(int(is_lite))}', - f'is_biotac:={str(int(is_biotac))}', - f'hand_name:={str(hand_name)}' + f'ff:={first_finger}', + f'mf:={middle_finger}', + f'rf:={ring_finger}', + f'lf:={little_finger}', + f'th:={thumb}', + f'is_lite:={is_lite}', + f'tip_sensors:={tip_sensors}', + f'hand_version:={hand_version}', + f'hand_name:={hand_name}' ]) # the prefix version of the srdf_xacro must be loaded package_path = rospkg.RosPack().get_path('sr_moveit_hand_config') - srdf_xacro_filename = f"{package_path}/config/shadowhands_prefix.srdf.xacro" - rospy.loginfo(f"File loaded {srdf_xacro_filename}") # open and parse the xacro.srdf file - with open(srdf_xacro_filename, 'r', encoding="utf-8") as srdf_xacro_file: + with open(f"{package_path}/config/shadowhands_prefix.srdf.xacro", 'r', encoding="utf-8") as srdf_xacro_file: self.srdf_xacro_xml = parse(srdf_xacro_file) # expand the xacro @@ -138,8 +146,7 @@ def __init__(self, urdf_str=None, load=True, save=True): if load: rospy.loginfo("Loading SRDF on parameter server") - robot_description_param = rospy.resolve_name('robot_description') + "_semantic" - rospy.set_param(robot_description_param, + rospy.set_param(rospy.resolve_name('robot_description') + "_semantic", self.srdf_xacro_xml.toprettyxml(indent=' ')) if save: output_path = f"{package_path}/config/generated_shadowhand.srdf" diff --git a/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.cpp b/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.cpp index af270dcc1..d73b0b78f 100644 --- a/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.cpp +++ b/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.cpp @@ -1,12 +1,8 @@ /** * @file test_end_effector.cpp * @author Guillaume Walck -*/ - - -/********************************************************************* -* Software License Agreement (BSD License) * +* Software License Agreement (BSD License) * Copyright (c) 2013, Willow Garage, Inc. * All rights reserved. * @@ -36,7 +32,7 @@ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ +*/ /* Author: Ioan Sucan */ diff --git a/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.test b/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.test index f89175dd9..dad1a2e73 100644 --- a/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.test +++ b/sr_multi_moveit/sr_multi_moveit_test/test/test_end_effector.test @@ -1,6 +1,7 @@ - - + + + diff --git a/sr_robot_launch/launch/load_robot_description.launch b/sr_robot_launch/launch/load_robot_description.launch index 629a59259..7dfa57e16 100644 --- a/sr_robot_launch/launch/load_robot_description.launch +++ b/sr_robot_launch/launch/load_robot_description.launch @@ -73,6 +73,9 @@ + + + diff --git a/sr_robot_launch/launch/load_robot_description_bimanual.launch b/sr_robot_launch/launch/load_robot_description_bimanual.launch index c841987cd..8d8a22ac6 100644 --- a/sr_robot_launch/launch/load_robot_description_bimanual.launch +++ b/sr_robot_launch/launch/load_robot_description_bimanual.launch @@ -99,6 +99,11 @@ + + + + +