From c660f29c85314c799cb5f2a22064c6961fa619a6 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 16 Feb 2021 21:31:02 +0100 Subject: [PATCH 01/28] changed docstring --- robot_skills/src/robot_skills/arm/arms.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index cd2b703f24..c32d44c305 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -272,13 +272,8 @@ def __repr__(self): class Arm(RobotPart): """ - A single arm can be either left or right, extends Arms: - Use left or right to get arm while running from the python console - - Examples: - >>> left.send_goal(0.265, 1, 0.816, 0, 0, 0, 60) # doctest: +SKIP - or Equivalently: - >>> left.send_goal(px=0.265, py=1, pz=0.816, yaw=0, pitch=0, roll=0, time_out=60, pre_grasp=False, frame_id='/amigo/base_link') # doctest: +SKIP + A kinematic chain ending in an end_effector. Can be controlled using either joint goals or a goal to reach with + the end_effector described in carthesian coordinates. """ def __init__(self, robot_name, tf_listener, get_joint_states, side): """ From 4c553aaf7f38c238b1896d9e660fea574dd77779 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 16 Feb 2021 22:25:51 +0100 Subject: [PATCH 02/28] seperated gripper from arm naming conventions --- robot_skills/src/robot_skills/arm/arms.py | 2 +- robot_skills/src/robot_skills/arm/gripper.py | 2 +- robot_skills/src/robot_skills/hero.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index c32d44c305..03cb6a847e 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -290,7 +290,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): self._operational = True # In simulation, there will be no hardware cb # Get stuff from the parameter server - self.marker_to_grippoint_offset = self.load_param('skills/arm/' + self.side + '/marker_to_grippoint') + self.marker_to_grippoint_offset = self.load_param('skills/gripper/marker_to_grippoint') # Grasp offsets go = self.load_param('skills/arm/' + self.side + '/base_offset') diff --git a/robot_skills/src/robot_skills/arm/gripper.py b/robot_skills/src/robot_skills/arm/gripper.py index 9e21079776..f0512c9a2b 100644 --- a/robot_skills/src/robot_skills/arm/gripper.py +++ b/robot_skills/src/robot_skills/arm/gripper.py @@ -68,7 +68,7 @@ def __init__(self, robot_name, tf_listener, gripper_name): """ super(ParrallelGripper, self).__init__(robot_name=robot_name, tf_listener=tf_listener) self.gripper_name = gripper_name - offset = self.load_param('skills/arm/' + self.gripper_name + '/grasp_offset/') + offset = self.load_param('skills/' + self.gripper_name + '/grasp_offset/') self.offset = kdl.Frame(kdl.Rotation.RPY(offset["roll"], offset["pitch"], offset["yaw"]), kdl.Vector(offset["x"], offset["y"], offset["z"])) diff --git a/robot_skills/src/robot_skills/hero.py b/robot_skills/src/robot_skills/hero.py index 48be46d824..950d2f45b3 100644 --- a/robot_skills/src/robot_skills/hero.py +++ b/robot_skills/src/robot_skills/hero.py @@ -24,7 +24,7 @@ def __init__(self, wait_services=False): # add hero's arm hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left") hero_arm.add_part('force_sensor', force_sensor.ForceSensor(self.robot_name, self.tf_listener, "/" + self.robot_name + "/wrist_wrench/raw")) - hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'left')) + hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper')) hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'left')) self.add_arm_part('leftArm', hero_arm) From 06aa3ffe27a380d3f56e63c421bb159a16a6a24f Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 16 Feb 2021 22:30:46 +0100 Subject: [PATCH 03/28] change loading from parameter server --- robot_skills/src/robot_skills/arm/arms.py | 10 +++++----- robot_skills/src/robot_skills/hero.py | 3 --- robot_skills/src/robot_skills/torso.py | 2 +- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 03cb6a847e..c2154bf005 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -286,6 +286,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): """ super(Arm, self).__init__(robot_name=robot_name, tf_listener=tf_listener) self.side = side + self.arm_name = "arm_" + side self._operational = True # In simulation, there will be no hardware cb @@ -293,15 +294,14 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): self.marker_to_grippoint_offset = self.load_param('skills/gripper/marker_to_grippoint') # Grasp offsets - go = self.load_param('skills/arm/' + self.side + '/base_offset') + go = self.load_param('skills/' + self.arm_name + '/base_offset') self._base_offset = kdl.Vector(go["x"], go["y"], go["z"]) - self.joint_names = self.load_param('skills/arm/joint_names') - self.joint_names = [name + "_" + self.side for name in self.joint_names] + self.joint_names = self.load_param('skills/' + self.arm_name + '/joint_names') self.torso_joint_names = self.load_param('skills/torso/joint_names') - self.default_configurations = self.load_param('skills/arm/default_configurations') - self.default_trajectories = self.load_param('skills/arm/default_trajectories') + self.default_configurations = self.load_param('skills/' + self.arm_name + '/default_configurations') + self.default_trajectories = self.load_param('skills/' + self.arm_name + '/default_trajectories') # listen to the hardware status to determine if the arm is available self.subscribe_hardware_status(self.side + '_arm') diff --git a/robot_skills/src/robot_skills/hero.py b/robot_skills/src/robot_skills/hero.py index 950d2f45b3..03db21a721 100644 --- a/robot_skills/src/robot_skills/hero.py +++ b/robot_skills/src/robot_skills/hero.py @@ -54,9 +54,6 @@ def __init__(self, wait_services=False): # Reasoning/world modeling self.add_body_part('ed', world_model_ed.ED(self.robot_name, self.tf_listener)) - #rename joint names - self.parts['leftArm'].joint_names = self.parts['leftArm'].load_param('skills/arm/joint_names') - # These don't work for HSR because (then) Toyota's diagnostics aggregator makes the robot go into error somehow for part in self.parts.itervalues(): part.unsubscribe_hardware_status() diff --git a/robot_skills/src/robot_skills/torso.py b/robot_skills/src/robot_skills/torso.py index 838a1609a4..4dc24ea14f 100644 --- a/robot_skills/src/robot_skills/torso.py +++ b/robot_skills/src/robot_skills/torso.py @@ -22,7 +22,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states): super(Torso, self).__init__(robot_name=robot_name, tf_listener=tf_listener) self.joint_names = self.load_param('skills/torso/joint_names') - self._arm_joint_names = self.load_param('skills/arm/joint_names') + self._arm_joint_names = self.load_param('skills/arm_left/joint_names') #TODO make sure this is no longer needed self.default_configurations = self.load_param('skills/torso/default_configurations') self.default_tolerance = self.load_param('/skills/torso/default_tolerance') self.lower_limit = self.default_configurations['lower_limit'] From b6749c994259124dd95125e08d3596f33e7b95ce Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 16 Feb 2021 22:37:06 +0100 Subject: [PATCH 04/28] change hardware status (currently unused) --- robot_skills/src/robot_skills/arm/arms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index c2154bf005..1d9d10d98e 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -304,7 +304,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): self.default_trajectories = self.load_param('skills/' + self.arm_name + '/default_trajectories') # listen to the hardware status to determine if the arm is available - self.subscribe_hardware_status(self.side + '_arm') + self.subscribe_hardware_status(self.arm_name) # Init grasp precompute actionlib self._ac_grasp_precompute = self.create_simple_action_client( From 1cb1bb33940ffd1e3a9ea5403d6d7dec477543ab Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 16 Feb 2021 22:46:33 +0100 Subject: [PATCH 05/28] object_repr + marker topic --- robot_skills/src/robot_skills/arm/arms.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 1d9d10d98e..b1e7c47e11 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -265,7 +265,7 @@ def _test_die(self, cond, feature, hint=''): """ if not cond: msg = "get_arm for '{}' arm did not request '{}' access. Hint: {}" - raise AssertionError(msg.format(self._arm.side, feature, hint)) + raise AssertionError(msg.format(self._arm.arm_name, feature, hint)) def __repr__(self): return "PublicArm(arm={arm})".format(arm=self._arm) @@ -316,7 +316,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): # Init marker publisher self._marker_publisher = rospy.Publisher( - "/" + robot_name + "/" + self.side + "_arm/grasp_target", + "/" + robot_name + "/" + self.arm_name + "/grasp_target", # TODO: update rviz config visualization_msgs.msg.Marker, queue_size=10) self.get_joint_states = get_joint_states @@ -684,4 +684,4 @@ def _publish_marker(self, goal, color, ns=""): self._marker_publisher.publish(marker) def __repr__(self): - return "Arm(side='{side}')".format(side=self.side) + return "Arm(name='{}')".format(side=self.arm_name) From 54292da0e6dba8fa6baeb92bd3fc8b81c4fc56d7 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 16 Feb 2021 23:35:35 +0100 Subject: [PATCH 06/28] changed action server --- robot_skills/src/robot_skills/arm/arms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index a3153d8ee1..c3ee1a05f0 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -312,7 +312,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): # Init grasp precompute actionlib self._ac_grasp_precompute = self.create_simple_action_client( - "/" + robot_name + "/" + self.side + "_arm/grasp_precompute", GraspPrecomputeAction) + "/" + robot_name + "/" + self.arm_name + "/grasp_precompute", GraspPrecomputeAction) # Init joint trajectory action server self._ac_joint_traj = self.create_simple_action_client( From 527fafff776247c628c331c8767c8eec356760eb Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 20:03:11 +0100 Subject: [PATCH 07/28] represent arm with name, not side --- robot_skills/src/robot_skills/arm/arms.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index c3ee1a05f0..27401a9ede 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -270,6 +270,7 @@ def _test_die(self, cond, feature, hint=''): def __repr__(self): return "PublicArm(arm={arm})".format(arm=self._arm) + class Arm(RobotPart): """ A kinematic chain ending in an end_effector. Can be controlled using either joint goals or a goal to reach with @@ -395,10 +396,10 @@ def close(self): :return: no return """ try: - rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.side)) + rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.arm_name)) except AttributeError: print("{0} arm cancelling all goals on all arm-related ACs on close. rospy is already deleted.". - format(self.side)) + format(self.arm_name)) self._ac_grasp_precompute.cancel_all_goals() self._ac_joint_traj.cancel_all_goals() @@ -601,7 +602,7 @@ def _send_joint_trajectory(self, joints_references, max_joint_vel=0.7, timeout=r points=ps) goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=timeout) - rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.side, ps)) + rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.arm_name, ps)) import time; time.sleep(0.001) # This is necessary: the rtt_actionlib in the hardware seems # to only have a queue size of 1 and runs at 1000 hz. This @@ -688,4 +689,4 @@ def _publish_marker(self, goal, color, ns=""): self._marker_publisher.publish(marker) def __repr__(self): - return "Arm(name='{}')".format(side=self.arm_name) + return "Arm(name='{}')".format(self.arm_name) From c6a51b8bab183fd77f8f3297bd4091e6da16fb1f Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 20:20:59 +0100 Subject: [PATCH 08/28] made gripper frame configurable --- robot_skills/src/robot_skills/arm/arms.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 27401a9ede..9a69e9c020 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -308,6 +308,8 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): self.default_configurations = self.load_param('skills/' + self.arm_name + '/default_configurations') self.default_trajectories = self.load_param('skills/' + self.arm_name + '/default_trajectories') + self.grasp_frame = self.load_param('skills/gripper/grasp_frame') #TODO remove gripper specific parameters + # listen to the hardware status to determine if the arm is available self.subscribe_hardware_status(self.arm_name) @@ -493,7 +495,7 @@ def send_goal(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_o if result == GoalStatus.SUCCEEDED: result_pose = self.tf_listener.lookupTransform(self.robot_name + "/base_link", - self.robot_name + "/grippoint_{}".format(self.side), + self.robot_name + "/" + self.grasp_frame, rospy.Time(0)) dx = grasp_precompute_goal.goal.x - result_pose[0][0] dy = grasp_precompute_goal.goal.y - result_pose[0][1] From 4798967381a81f72efc146197f5fa47e1fa4a8db Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 20:31:27 +0100 Subject: [PATCH 09/28] rename side to name --- .../src/robot_skills/arm/handover_detector.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/handover_detector.py b/robot_skills/src/robot_skills/arm/handover_detector.py index 41e01eb4ca..06eed412fb 100644 --- a/robot_skills/src/robot_skills/arm/handover_detector.py +++ b/robot_skills/src/robot_skills/arm/handover_detector.py @@ -4,20 +4,21 @@ from robot_skills.robot_part import RobotPart from tue_manipulation_msgs.msg import GripperCommandGoal, GripperCommandAction + class HandoverDetector(RobotPart): """ Sensor functionality to detect when a handover is taking place """ - def __init__(self, robot_name, tf_listener, side): + def __init__(self, robot_name, tf_listener, name): """ constructor :param robot_name: robot_name :param tf_listener: tf_server.TFClient() - :param side: string used to identify the sensor + :param name: string used to identify the sensor """ super(HandoverDetector, self).__init__(robot_name=robot_name, tf_listener=tf_listener) - self.side = side + self.name = name def handover_to_human(self, timeout=10): """ @@ -27,12 +28,12 @@ def handover_to_human(self, timeout=10): :param timeout: timeout in seconds :return: True or False """ - pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.side + '/toggle_robot2human', + pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.name + '/toggle_robot2human', std_msgs.msg.Bool, queue_size=1, latch=True) pub.publish(std_msgs.msg.Bool(True)) try: - rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.side + '/result', std_msgs.msg.Bool, + rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', std_msgs.msg.Bool, timeout) # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') return True @@ -49,12 +50,12 @@ def handover_to_robot(self, timeout=10): :param timeout: timeout in seconds :return: True or False """ - pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.side + '/toggle_human2robot', + pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.name + '/toggle_human2robot', std_msgs.msg.Bool, queue_size=1, latch=True) pub.publish(std_msgs.msg.Bool(True)) try: - rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.side + '/result', std_msgs.msg.Bool, + rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', std_msgs.msg.Bool, timeout) # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') return True From e476b202b9565e2339c79acf6ff7390c59fd48df Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 20:36:58 +0100 Subject: [PATCH 10/28] fix imports --- .../src/robot_skills/arm/handover_detector.py | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/handover_detector.py b/robot_skills/src/robot_skills/arm/handover_detector.py index 06eed412fb..0e20947784 100644 --- a/robot_skills/src/robot_skills/arm/handover_detector.py +++ b/robot_skills/src/robot_skills/arm/handover_detector.py @@ -1,8 +1,7 @@ import rospy -import PyKDL as kdl +from std_msgs.msg import Bool from robot_skills.robot_part import RobotPart -from tue_manipulation_msgs.msg import GripperCommandGoal, GripperCommandAction class HandoverDetector(RobotPart): @@ -29,19 +28,17 @@ def handover_to_human(self, timeout=10): :return: True or False """ pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.name + '/toggle_robot2human', - std_msgs.msg.Bool, queue_size=1, latch=True) - pub.publish(std_msgs.msg.Bool(True)) + Bool, queue_size=1, latch=True) + pub.publish(Bool(True)) try: - rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', std_msgs.msg.Bool, - timeout) + rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', Bool, timeout) # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') return True except rospy.ROSException as e: rospy.logerr(e) return False - def handover_to_robot(self, timeout=10): """ Handover an item from a human to the robot. @@ -51,12 +48,11 @@ def handover_to_robot(self, timeout=10): :return: True or False """ pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.name + '/toggle_human2robot', - std_msgs.msg.Bool, queue_size=1, latch=True) - pub.publish(std_msgs.msg.Bool(True)) + Bool, queue_size=1, latch=True) + pub.publish(Bool(True)) try: - rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', std_msgs.msg.Bool, - timeout) + rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', Bool, timeout) # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') return True except rospy.ROSException as e: From 1073872dc8f77bf73ece0023c70a10128a3c7b33 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 20:54:40 +0100 Subject: [PATCH 11/28] removed side argument from arm --- robot_skills/src/robot_skills/arm/arms.py | 7 +++---- robot_skills/src/robot_skills/hero.py | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 9a69e9c020..1a6cef2a86 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -276,18 +276,17 @@ class Arm(RobotPart): A kinematic chain ending in an end_effector. Can be controlled using either joint goals or a goal to reach with the end_effector described in carthesian coordinates. """ - def __init__(self, robot_name, tf_listener, get_joint_states, side): + def __init__(self, robot_name, tf_listener, get_joint_states, name): """ constructor :param robot_name: robot_name :param tf_listener: tf_server.TFClient() :param get_joint_states: get_joint_states function for getting the last joint states - :param side: left or right + :param name: string used to identify the arm """ super(Arm, self).__init__(robot_name=robot_name, tf_listener=tf_listener) - self.side = side - self.arm_name = "arm_" + side + self.arm_name = name self._operational = True # In simulation, there will be no hardware cb diff --git a/robot_skills/src/robot_skills/hero.py b/robot_skills/src/robot_skills/hero.py index 03db21a721..2841f730c0 100644 --- a/robot_skills/src/robot_skills/hero.py +++ b/robot_skills/src/robot_skills/hero.py @@ -22,7 +22,7 @@ def __init__(self, wait_services=False): self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states)) # add hero's arm - hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left") + hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_center") hero_arm.add_part('force_sensor', force_sensor.ForceSensor(self.robot_name, self.tf_listener, "/" + self.robot_name + "/wrist_wrench/raw")) hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper')) hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'left')) From 188911b6b94868bf37e88990f4382bb2fa5ede9a Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 20:55:27 +0100 Subject: [PATCH 12/28] changed handover detector interface --- .../src/robot_skills/arm/handover_detector.py | 16 ++++++++-------- robot_skills/src/robot_skills/hero.py | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/handover_detector.py b/robot_skills/src/robot_skills/arm/handover_detector.py index 0e20947784..8b752c8e83 100644 --- a/robot_skills/src/robot_skills/arm/handover_detector.py +++ b/robot_skills/src/robot_skills/arm/handover_detector.py @@ -27,13 +27,13 @@ def handover_to_human(self, timeout=10): :param timeout: timeout in seconds :return: True or False """ - pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.name + '/toggle_robot2human', - Bool, queue_size=1, latch=True) + pub = rospy.Publisher('/' + self.robot_name + '/' + self.name + '/toggle_robot2human', Bool, queue_size=1, + latch=True) pub.publish(Bool(True)) try: - rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', Bool, timeout) - # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') + rospy.wait_for_message('/' + self.robot_name + '/' + self.name + '/result', Bool, timeout) + # print('/'+self.robot_name+'/'+self.name+'/result') return True except rospy.ROSException as e: rospy.logerr(e) @@ -47,13 +47,13 @@ def handover_to_robot(self, timeout=10): :param timeout: timeout in seconds :return: True or False """ - pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.name + '/toggle_human2robot', - Bool, queue_size=1, latch=True) + pub = rospy.Publisher('/' + self.robot_name + '/' + self.name + '/toggle_human2robot', Bool, queue_size=1, + latch=True) pub.publish(Bool(True)) try: - rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.name + '/result', Bool, timeout) - # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') + rospy.wait_for_message('/' + self.robot_name + '/' + self.name + '/result', Bool, timeout) + # print('/'+self.robot_name+'/'+self.name+'/result') return True except rospy.ROSException as e: rospy.logerr(e) diff --git a/robot_skills/src/robot_skills/hero.py b/robot_skills/src/robot_skills/hero.py index 2841f730c0..4fcbcba5dd 100644 --- a/robot_skills/src/robot_skills/hero.py +++ b/robot_skills/src/robot_skills/hero.py @@ -25,7 +25,7 @@ def __init__(self, wait_services=False): hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_center") hero_arm.add_part('force_sensor', force_sensor.ForceSensor(self.robot_name, self.tf_listener, "/" + self.robot_name + "/wrist_wrench/raw")) hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper')) - hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'left')) + hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'handover_detector')) self.add_arm_part('leftArm', hero_arm) From d9c5268fd090e2c21e05549ea272a2491a5f0565 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Fri, 19 Feb 2021 21:05:10 +0100 Subject: [PATCH 13/28] update mockbot --- robot_skills/src/robot_skills/mockbot.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/robot_skills/src/robot_skills/mockbot.py b/robot_skills/src/robot_skills/mockbot.py index b70a1c9859..adae94b9cc 100755 --- a/robot_skills/src/robot_skills/mockbot.py +++ b/robot_skills/src/robot_skills/mockbot.py @@ -126,10 +126,10 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs): class Arm(MockedRobotPart): - def __init__(self, robot_name, tf_listener, get_joint_states, side): + def __init__(self, robot_name, tf_listener, get_joint_states, name): super(Arm, self).__init__(robot_name, tf_listener) - self.side = side + self.arm_name = name self.get_joint_states = get_joint_states self._operational = True @@ -416,8 +416,8 @@ def __init__(self, *args, **kwargs): self.add_body_part('base', Base(self.robot_name, self.tf_listener)) self.add_body_part('torso', Torso(self.robot_name, self.tf_listener, self.get_joint_states)) - self.add_arm_part('leftArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left")) - self.add_arm_part('rightArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "right")) + self.add_arm_part('leftArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_left")) + self.add_arm_part('rightArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_right")) self.add_body_part('head', Head(self.robot_name, self.tf_listener)) From 73d9b22106c0ddada6b30f6ae99e3c4faf6113a1 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 23 Feb 2021 20:05:17 +0100 Subject: [PATCH 14/28] clean up grasp_sensor --- robot_skills/src/robot_skills/arm/grasp_sensor.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/grasp_sensor.py b/robot_skills/src/robot_skills/arm/grasp_sensor.py index c496de6fe8..a81d782a0c 100644 --- a/robot_skills/src/robot_skills/arm/grasp_sensor.py +++ b/robot_skills/src/robot_skills/arm/grasp_sensor.py @@ -1,7 +1,12 @@ +import rospy + +from std_msgs.msg import Float32MultiArray + from robot_skills.robot_part import RobotPart from tue_manipulation_msgs.msg import GripperCommandGoal, GripperCommandAction from tue_msgs.msg import GripperCommand + class GripperMeasurement(object): """ Class holding measurements from the distance sensor on the grippers @@ -81,23 +86,23 @@ def __repr__(self): return "Distance: {}, is_holding: {}, is_unknown: {}, " \ "is_empty: {}".format(self.distance, self.is_holding, self.is_unknown, self.is_empty) + class GraspSensor(RobotPart): """ Sensor to detect whether or not the robot is holding an object. """ - def __init__(self, robot_name, tf_listener, side): + def __init__(self, robot_name, tf_listener, sensor_topic): """ constructor :param robot_name: robot_name :param tf_listener: tf_server.TFClient() - :param side: left or right arm of the robot. + :param sensor_topic: name of the topic where measurements are published """ super(GraspSensor, self).__init__(robot_name=robot_name, tf_listener=tf_listener) # Init grasp sensor subscriber self._grasp_sensor_state = GripperMeasurement(0.0) - rospy.Subscriber("/" + robot_name + "/" + side + "_arm/proximity_sensor", - std_msgs.msg.Float32MultiArray, self._grasp_sensor_callback) + rospy.Subscriber("/" + robot_name + "/" + sensor_topic, Float32MultiArray, self._grasp_sensor_callback) @property def object_in_gripper_measurement(self): From 59a4aef07de083979916afad9b0ef479816f4988 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Sat, 6 Mar 2021 09:43:45 +0100 Subject: [PATCH 15/28] added test for the construction of hero --- robot_skills/CMakeLists.txt | 1 + robot_skills/test/rostest/test_hero.py | 23 +++++++++++++++++++++++ robot_skills/test/rostest/test_hero.test | 5 +++++ 3 files changed, 29 insertions(+) create mode 100755 robot_skills/test/rostest/test_hero.py create mode 100644 robot_skills/test/rostest/test_hero.test diff --git a/robot_skills/CMakeLists.txt b/robot_skills/CMakeLists.txt index 8eb55c1d77..6d0446bb78 100644 --- a/robot_skills/CMakeLists.txt +++ b/robot_skills/CMakeLists.txt @@ -37,4 +37,5 @@ if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) catkin_add_nosetests(test/nosetest) add_rostest(test/rostest/test_compose_robot.test) + add_rostest(test/rostest/test_hero.test) endif() diff --git a/robot_skills/test/rostest/test_hero.py b/robot_skills/test/rostest/test_hero.py new file mode 100755 index 0000000000..16bf2bfffc --- /dev/null +++ b/robot_skills/test/rostest/test_hero.py @@ -0,0 +1,23 @@ +#! /usr/bin/env python + +import rospy +import unittest + +# robot skills +from robot_skills.hero import Hero + + +class TestHeroInterface(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rospy.init_node('test_hero') + + def test_construct_interface(self): + robot = Hero() + self.assertIsNotNone(robot) + + +if __name__ == '__main__': + import rostest + rostest.rosrun('robot_skills', 'test_hero_interface', TestHeroInterface) diff --git a/robot_skills/test/rostest/test_hero.test b/robot_skills/test/rostest/test_hero.test new file mode 100644 index 0000000000..f46f57f55d --- /dev/null +++ b/robot_skills/test/rostest/test_hero.test @@ -0,0 +1,5 @@ + + + + + From 387e0648cbe5fa67492e79f280a1117d82cb6d09 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Sat, 6 Mar 2021 10:07:37 +0100 Subject: [PATCH 16/28] hacked torso arm_gathering --- robot_skills/src/robot_skills/torso.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/torso.py b/robot_skills/src/robot_skills/torso.py index 4dc24ea14f..f0353ebc4c 100644 --- a/robot_skills/src/robot_skills/torso.py +++ b/robot_skills/src/robot_skills/torso.py @@ -22,12 +22,16 @@ def __init__(self, robot_name, tf_listener, get_joint_states): super(Torso, self).__init__(robot_name=robot_name, tf_listener=tf_listener) self.joint_names = self.load_param('skills/torso/joint_names') - self._arm_joint_names = self.load_param('skills/arm_left/joint_names') #TODO make sure this is no longer needed self.default_configurations = self.load_param('skills/torso/default_configurations') self.default_tolerance = self.load_param('/skills/torso/default_tolerance') self.lower_limit = self.default_configurations['lower_limit'] self.upper_limit = self.default_configurations['upper_limit'] + try: # TODO make sure this is no longer needed + self._arm_joint_names = self.load_param('skills/arm_center/joint_names') + except KeyError: + self._arm_joint_names = [] + # Init action client self.ac_move_torso = self.create_simple_action_client('/' + self.robot_name + '/body/joint_trajectory_action', control_msgs.msg.FollowJointTrajectoryAction) From 64047afd880a2243a3618aef7bc9582ea63b25ee Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Thu, 11 Mar 2021 10:01:27 +0100 Subject: [PATCH 17/28] added test_depend --- robot_skills/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_skills/package.xml b/robot_skills/package.xml index 5ea9ab7726..47e7facaa1 100644 --- a/robot_skills/package.xml +++ b/robot_skills/package.xml @@ -41,6 +41,7 @@ test_tools python-six + hero_description python-sphinx python-sphinx-autoapi-pip From 3d41cc9ea1927a0844fb6ba73cc06ec2df4a95ef Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Sat, 20 Mar 2021 18:52:43 +0100 Subject: [PATCH 18/28] renamed atrribute arm_name to name --- robot_skills/src/robot_skills/arm/arms.py | 26 +++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 1a6cef2a86..f4c2c7fba8 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -265,7 +265,7 @@ def _test_die(self, cond, feature, hint=''): """ if not cond: msg = "get_arm for '{}' arm did not request '{}' access. Hint: {}" - raise AssertionError(msg.format(self._arm.arm_name, feature, hint)) + raise AssertionError(msg.format(self._arm.name, feature, hint)) def __repr__(self): return "PublicArm(arm={arm})".format(arm=self._arm) @@ -286,7 +286,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, name): :param name: string used to identify the arm """ super(Arm, self).__init__(robot_name=robot_name, tf_listener=tf_listener) - self.arm_name = name + self.name = name self._operational = True # In simulation, there will be no hardware cb @@ -298,23 +298,23 @@ def __init__(self, robot_name, tf_listener, get_joint_states, name): self.marker_to_grippoint_offset = self.load_param('skills/gripper/marker_to_grippoint') # Grasp offsets - go = self.load_param('skills/' + self.arm_name + '/base_offset') + go = self.load_param('skills/' + self.name + '/base_offset') self._base_offset = kdl.Vector(go["x"], go["y"], go["z"]) - self.joint_names = self.load_param('skills/' + self.arm_name + '/joint_names') + self.joint_names = self.load_param('skills/' + self.name + '/joint_names') self.torso_joint_names = self.load_param('skills/torso/joint_names') - self.default_configurations = self.load_param('skills/' + self.arm_name + '/default_configurations') - self.default_trajectories = self.load_param('skills/' + self.arm_name + '/default_trajectories') + self.default_configurations = self.load_param('skills/' + self.name + '/default_configurations') + self.default_trajectories = self.load_param('skills/' + self.name + '/default_trajectories') self.grasp_frame = self.load_param('skills/gripper/grasp_frame') #TODO remove gripper specific parameters # listen to the hardware status to determine if the arm is available - self.subscribe_hardware_status(self.arm_name) + self.subscribe_hardware_status(self.name) # Init grasp precompute actionlib self._ac_grasp_precompute = self.create_simple_action_client( - "/" + robot_name + "/" + self.arm_name + "/grasp_precompute", GraspPrecomputeAction) + "/" + robot_name + "/" + self.name + "/grasp_precompute", GraspPrecomputeAction) # Init joint trajectory action server self._ac_joint_traj = self.create_simple_action_client( @@ -322,7 +322,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, name): # Init marker publisher self._marker_publisher = rospy.Publisher( - "/" + robot_name + "/" + self.arm_name + "/grasp_target", # TODO: update rviz config + "/" + robot_name + "/" + self.name + "/grasp_target", # TODO: update rviz config visualization_msgs.msg.Marker, queue_size=10) self.get_joint_states = get_joint_states @@ -397,10 +397,10 @@ def close(self): :return: no return """ try: - rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.arm_name)) + rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.name)) except AttributeError: print("{0} arm cancelling all goals on all arm-related ACs on close. rospy is already deleted.". - format(self.arm_name)) + format(self.name)) self._ac_grasp_precompute.cancel_all_goals() self._ac_joint_traj.cancel_all_goals() @@ -603,7 +603,7 @@ def _send_joint_trajectory(self, joints_references, max_joint_vel=0.7, timeout=r points=ps) goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=timeout) - rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.arm_name, ps)) + rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.name, ps)) import time; time.sleep(0.001) # This is necessary: the rtt_actionlib in the hardware seems # to only have a queue size of 1 and runs at 1000 hz. This @@ -690,4 +690,4 @@ def _publish_marker(self, goal, color, ns=""): self._marker_publisher.publish(marker) def __repr__(self): - return "Arm(name='{}')".format(self.arm_name) + return "Arm(name='{}')".format(self.name) From d018927eeadd0972c8209ba0990b31172f0a1404 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Sat, 20 Mar 2021 19:05:15 +0100 Subject: [PATCH 19/28] moved init node to main --- robot_skills/test/rostest/test_compose_robot.py | 7 ++----- robot_skills/test/rostest/test_hero.py | 6 +----- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/robot_skills/test/rostest/test_compose_robot.py b/robot_skills/test/rostest/test_compose_robot.py index 46b5e3c217..d4e83d8923 100755 --- a/robot_skills/test/rostest/test_compose_robot.py +++ b/robot_skills/test/rostest/test_compose_robot.py @@ -39,11 +39,6 @@ def testfunction2(self): class TestComposeRobot(unittest.TestCase): - - @classmethod - def setUpClass(cls): - rospy.init_node('test_composable_robot_interface2') - def test_compose_robot(self): robot = Robot("testbot") part = RobotPart("testbot", robot.tf_listener) @@ -67,6 +62,8 @@ def test_compose_functionality(self): self.assertFalse(hasattr(robot.special_part, 'testfunction2')) + if __name__ == '__main__': import rostest + rospy.init_node('test_composable_robot_interface') rostest.rosrun('robot_skills', 'test_composable_robot_interface', TestComposeRobot) diff --git a/robot_skills/test/rostest/test_hero.py b/robot_skills/test/rostest/test_hero.py index 16bf2bfffc..5edb299395 100755 --- a/robot_skills/test/rostest/test_hero.py +++ b/robot_skills/test/rostest/test_hero.py @@ -8,11 +8,6 @@ class TestHeroInterface(unittest.TestCase): - - @classmethod - def setUpClass(cls): - rospy.init_node('test_hero') - def test_construct_interface(self): robot = Hero() self.assertIsNotNone(robot) @@ -20,4 +15,5 @@ def test_construct_interface(self): if __name__ == '__main__': import rostest + rospy.init_node('test_hero') rostest.rosrun('robot_skills', 'test_hero_interface', TestHeroInterface) From 9990790e45e1c17c1cf05df8328fcbdd2584b0e4 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 23 Mar 2021 18:59:40 +0100 Subject: [PATCH 20/28] changed rviz config --- robot_skills/src/robot_skills/arm/arms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index f4c2c7fba8..31a6512043 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -322,7 +322,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, name): # Init marker publisher self._marker_publisher = rospy.Publisher( - "/" + robot_name + "/" + self.name + "/grasp_target", # TODO: update rviz config + "/" + robot_name + "/" + self.name + "/grasp_target", visualization_msgs.msg.Marker, queue_size=10) self.get_joint_states = get_joint_states From ae671e9f895036c40d6a410e02863634bcf82cae Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 6 Apr 2021 09:40:25 +0200 Subject: [PATCH 21/28] dont prefix sensor_topic --- robot_skills/src/robot_skills/arm/grasp_sensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/grasp_sensor.py b/robot_skills/src/robot_skills/arm/grasp_sensor.py index 2b046ba2f1..8e428f0250 100644 --- a/robot_skills/src/robot_skills/arm/grasp_sensor.py +++ b/robot_skills/src/robot_skills/arm/grasp_sensor.py @@ -100,7 +100,7 @@ def __init__(self, robot_name, tf_listener, sensor_topic): super(GraspSensor, self).__init__(robot_name=robot_name, tf_listener=tf_listener) # Init grasp sensor subscriber self._grasp_sensor_state = GripperMeasurement(0.0) - rospy.Subscriber("/" + robot_name + "/" + sensor_topic, Float32MultiArray, self._grasp_sensor_callback) + rospy.Subscriber(sensor_topic, Float32MultiArray, self._grasp_sensor_callback) @property def object_in_gripper_measurement(self): From 407b74ba76971db54bddae5fbd76149f4dccd64c Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 6 Apr 2021 10:45:33 +0200 Subject: [PATCH 22/28] remove hero_test from robot_skills --- robot_skills/CMakeLists.txt | 1 - robot_skills/test/rostest/test_hero.py | 19 ------------------- robot_skills/test/rostest/test_hero.test | 5 ----- 3 files changed, 25 deletions(-) delete mode 100755 robot_skills/test/rostest/test_hero.py delete mode 100644 robot_skills/test/rostest/test_hero.test diff --git a/robot_skills/CMakeLists.txt b/robot_skills/CMakeLists.txt index 69d3c67eab..b2da624d3c 100644 --- a/robot_skills/CMakeLists.txt +++ b/robot_skills/CMakeLists.txt @@ -37,5 +37,4 @@ if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) catkin_add_nosetests(test/nosetest) add_rostest(test/rostest/test_compose_robot.test) - add_rostest(test/rostest/test_hero.test) endif() diff --git a/robot_skills/test/rostest/test_hero.py b/robot_skills/test/rostest/test_hero.py deleted file mode 100755 index 5edb299395..0000000000 --- a/robot_skills/test/rostest/test_hero.py +++ /dev/null @@ -1,19 +0,0 @@ -#! /usr/bin/env python - -import rospy -import unittest - -# robot skills -from robot_skills.hero import Hero - - -class TestHeroInterface(unittest.TestCase): - def test_construct_interface(self): - robot = Hero() - self.assertIsNotNone(robot) - - -if __name__ == '__main__': - import rostest - rospy.init_node('test_hero') - rostest.rosrun('robot_skills', 'test_hero_interface', TestHeroInterface) diff --git a/robot_skills/test/rostest/test_hero.test b/robot_skills/test/rostest/test_hero.test deleted file mode 100644 index f46f57f55d..0000000000 --- a/robot_skills/test/rostest/test_hero.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - From 59332f7e57bd39a90270c59036ea842fb981ab23 Mon Sep 17 00:00:00 2001 From: PetervDooren <19806646+PetervDooren@users.noreply.github.com> Date: Tue, 6 Apr 2021 11:13:42 +0200 Subject: [PATCH 23/28] remove test depend --- robot_skills/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/robot_skills/package.xml b/robot_skills/package.xml index 1d7847a598..6bfce906c1 100644 --- a/robot_skills/package.xml +++ b/robot_skills/package.xml @@ -47,7 +47,6 @@ python3-mock test_tools - hero_description python-six python3-six From bf05f7a7b5b47436adb7baf828b140eb9a826acb Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Tue, 6 Apr 2021 20:45:50 +0200 Subject: [PATCH 24/28] make gripper action_client parameterised --- amigo_skills/src/amigo_skills/amigo.py | 12 ++++++------ robot_skills/src/robot_skills/arm/gripper.py | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/amigo_skills/src/amigo_skills/amigo.py b/amigo_skills/src/amigo_skills/amigo.py index d8f7a9dda4..14f3cc9f86 100644 --- a/amigo_skills/src/amigo_skills/amigo.py +++ b/amigo_skills/src/amigo_skills/amigo.py @@ -22,17 +22,17 @@ def __init__(self, wait_services=False): self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states)) # construct left arm - left_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left") - left_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'left')) + left_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_left") + left_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper_left')) left_arm.add_part('handover_detector', - handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'left')) + handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'handoverdetector_left')) self.add_arm_part('leftArm', left_arm) # construct right arm - right_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "right") - right_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'right')) + right_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_right") + right_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper_right')) right_arm.add_part('handover_detector', - handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'right')) + handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'handoverdetector_right')) self.add_arm_part('rightArm', right_arm) self.add_body_part('head', head.Head(self.robot_name, self.tf_listener)) diff --git a/robot_skills/src/robot_skills/arm/gripper.py b/robot_skills/src/robot_skills/arm/gripper.py index 81469bbd3d..8d740ee753 100644 --- a/robot_skills/src/robot_skills/arm/gripper.py +++ b/robot_skills/src/robot_skills/arm/gripper.py @@ -74,8 +74,8 @@ def __init__(self, robot_name, tf_listener, gripper_name): kdl.Vector(offset["x"], offset["y"], offset["z"])) # Init gripper actionlib - self._ac_gripper = self.create_simple_action_client( - "/" + robot_name + "/" + self.gripper_name + "_arm/gripper/action", GripperCommandAction) + ac_name = self.load_param('skills/' + self.gripper_name + '/ac_gripper') + self._ac_gripper = self.create_simple_action_client(ac_name, GripperCommandAction) def send_goal(self, state, timeout=5.0, max_torque=0.1): #Todo: should send goal be universal for all grippers? """ From b09df1ebff606cd4e3462f804498e36fae928084 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Sun, 11 Apr 2021 15:57:09 +0200 Subject: [PATCH 25/28] move arm_names out of torso.py --- hero_skills/src/hero_skills/hero.py | 4 +++- robot_skills/src/robot_skills/torso.py | 7 ++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/hero_skills/src/hero_skills/hero.py b/hero_skills/src/hero_skills/hero.py index ed0b44eb99..f24b89005d 100644 --- a/hero_skills/src/hero_skills/hero.py +++ b/hero_skills/src/hero_skills/hero.py @@ -23,7 +23,9 @@ def __init__(self, wait_services=False): self._ignored_parts = ["leftArm", "torso", "spindle", "head"] self.add_body_part('base', base.Base(self.robot_name, self.tf_listener)) - self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states)) + + arm_joint_names = rospy.get_param('/' + self.robot_name + '/skills/arm_center/joint_names') + self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states, arm_joint_names)) # add hero's arm hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_center") diff --git a/robot_skills/src/robot_skills/torso.py b/robot_skills/src/robot_skills/torso.py index 81eaf19c46..6ae54b8f22 100644 --- a/robot_skills/src/robot_skills/torso.py +++ b/robot_skills/src/robot_skills/torso.py @@ -14,7 +14,7 @@ class Torso(RobotPart): - def __init__(self, robot_name, tf_listener, get_joint_states): + def __init__(self, robot_name, tf_listener, get_joint_states, arm_joint_names=[]): """ constructor @@ -29,10 +29,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states): self.lower_limit = self.default_configurations['lower_limit'] self.upper_limit = self.default_configurations['upper_limit'] - try: # TODO make sure this is no longer needed - self._arm_joint_names = self.load_param('skills/arm_center/joint_names') - except KeyError: - self._arm_joint_names = [] + self._arm_joint_names = arm_joint_names #TODO make sure this is no longer needed # Init action client self.ac_move_torso = self.create_simple_action_client('/' + self.robot_name + '/body/joint_trajectory_action', From 9df8d9852d485b5f582a23e4235af65663e6a358 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 21 Apr 2021 18:28:05 +0200 Subject: [PATCH 26/28] empty commit --- robot_skills/src/robot_skills/arm/arms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 79ba92ddd7..9737dc28d9 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -542,7 +542,7 @@ def send_joint_trajectory(self, configuration, timeout=5.0, max_joint_vel=0.7): def selfreset(self): """ Put the arm into the 'reset' pose - + :return: True or False """ return self.send_joint_goal('reset', timeout=0.0) From 690375136c5ac39e87cf14ffb0a57e109e16911c Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Wed, 21 Apr 2021 18:29:05 +0200 Subject: [PATCH 27/28] Revert "empty commit" This reverts commit 9df8d9852d485b5f582a23e4235af65663e6a358. --- robot_skills/src/robot_skills/arm/arms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_skills/src/robot_skills/arm/arms.py b/robot_skills/src/robot_skills/arm/arms.py index 9737dc28d9..79ba92ddd7 100644 --- a/robot_skills/src/robot_skills/arm/arms.py +++ b/robot_skills/src/robot_skills/arm/arms.py @@ -542,7 +542,7 @@ def send_joint_trajectory(self, configuration, timeout=5.0, max_joint_vel=0.7): def selfreset(self): """ Put the arm into the 'reset' pose - + :return: True or False """ return self.send_joint_goal('reset', timeout=0.0) From c1230ff274dc74b47e4efe327f3fc0973f9471f1 Mon Sep 17 00:00:00 2001 From: PetervDooren Date: Thu, 22 Apr 2021 14:43:39 +0200 Subject: [PATCH 28/28] dont use a mutable default argument --- robot_skills/src/robot_skills/torso.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_skills/src/robot_skills/torso.py b/robot_skills/src/robot_skills/torso.py index 6ae54b8f22..874d48a30e 100644 --- a/robot_skills/src/robot_skills/torso.py +++ b/robot_skills/src/robot_skills/torso.py @@ -14,7 +14,7 @@ class Torso(RobotPart): - def __init__(self, robot_name, tf_listener, get_joint_states, arm_joint_names=[]): + def __init__(self, robot_name, tf_listener, get_joint_states, arm_joint_names=None): """ constructor @@ -29,7 +29,7 @@ def __init__(self, robot_name, tf_listener, get_joint_states, arm_joint_names=[] self.lower_limit = self.default_configurations['lower_limit'] self.upper_limit = self.default_configurations['upper_limit'] - self._arm_joint_names = arm_joint_names #TODO make sure this is no longer needed + self._arm_joint_names = [] if arm_joint_names is None else arm_joint_names #TODO make sure this is no longer needed # Init action client self.ac_move_torso = self.create_simple_action_client('/' + self.robot_name + '/body/joint_trajectory_action',