From 1b90a0da9497698abb38468b795ec9116cb58f24 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Mon, 24 Feb 2025 21:08:18 +0900 Subject: [PATCH 1/2] =?UTF-8?q?fix:=20=E9=A0=86=E9=81=8B=E5=8B=95=E5=AD=A6?= =?UTF-8?q?=E3=81=AE=E5=88=B6=E5=BE=A1=E9=87=8F=E3=81=AE=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E6=96=B9=E6=B3=95=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mofpy/mofpy/action/moveit_servo_joint.py | 36 ++++++++++++++++++------ mofpy/mofpy/joy_mapping.py | 11 ++++++++ mofpy_demo/config/presets/arm.yaml | 4 +-- mofpy_demo/config/ps4_wired.yaml | 12 +++----- 4 files changed, 43 insertions(+), 20 deletions(-) diff --git a/mofpy/mofpy/action/moveit_servo_joint.py b/mofpy/mofpy/action/moveit_servo_joint.py index 7892266..1953667 100644 --- a/mofpy/mofpy/action/moveit_servo_joint.py +++ b/mofpy/mofpy/action/moveit_servo_joint.py @@ -24,7 +24,7 @@ def __init__(self, definition, node: Node): self.__scale = self.get("scale", 0.1) self.__quiet_on_zero = self.get("quiet_on_zero", True) self.__joint_mapping = self.__mapping__("joints") - self.__axis_mapping = self.__mapping__("axes") + self.__value_mapping = self.__mapping__("value") self.__published_zero = False self.__pub = node.create_publisher( @@ -56,7 +56,7 @@ def execute(self, named_joy=None): ) jog_joints, is_quiet = self.__get_jog_joints__(named_joy["buttons"], named_joy["axes"]) - + if self.__quiet_on_zero: if is_quiet: # Publish the all-zero message just once @@ -103,11 +103,11 @@ def __get_jog_joints__(self, named_buttons, named_axes): msg.header.frame_id = self.__frame_id for joint in self.__joint_mapping: if joint in self.__joint_model_names: - v = self.__get_value__("plus", named_axes) - self.__get_value__("minus", named_axes) + v = self.__get_value__(named_axes) vel = self.__scale * self.__get_enable_joint__(joint, named_buttons) * v msg.joint_names.append(joint) msg.velocities.append(vel) - + is_quiet = all(val == 0 for val in msg.velocities) return msg, is_quiet @@ -115,9 +115,16 @@ def __mapping__(self, key=None): mapping_key = "mapping" + f"/{key}" if key else key params = self.get(mapping_key, {}) mapping = {} + + if type(params) is not dict: + return params + for key in params.keys(): val = params[key] - mapping[key] = val + if type(val) is tuple or type(val) is list: + mapping[key] = [val[0], val[1]] + else: + mapping[key] = val return mapping @@ -128,7 +135,7 @@ def __get_enable_joint__(self, joint_name, named_buttons): button = self.__joint_mapping[joint_name] return 1 if named_buttons[button].value else 0 - def __get_value__(self, axis, named_axes): + def __get_value__(self, named_axes): """ Extract the axis/buttons value from joy. @@ -136,10 +143,21 @@ def __get_value__(self, axis, named_axes): :param named_axes: the processed joy values to get the value from :return: the value """ - if axis not in self.__axis_mapping: - return 0 - return named_axes[self.__axis_mapping[axis]].value + # List of button names to be added in order to get the value. + # A name could start with '-', indicating to invert the value + names = self.__value_mapping + + if type(names) is not list: + return named_axes[names].value + + val = 0 + for name in names: + v = named_axes[name.lstrip("-")].value + if name.startswith("-"): + v = -v + val += v + return val Action.register_preset(MoveitServoJoint) diff --git a/mofpy/mofpy/joy_mapping.py b/mofpy/mofpy/joy_mapping.py index 76a7b5c..531ead9 100644 --- a/mofpy/mofpy/joy_mapping.py +++ b/mofpy/mofpy/joy_mapping.py @@ -34,6 +34,11 @@ def __init__(self, name, definition): if self._is_ps_shoulder: self._button_counterpart = definition["ps_shoulder_counterpart"] + self._range = definition["range"] if "range" in definition else None + r = self._range + if r and r[0] > r[1]: + r[0], r[1] = r[1], r[0] + def update_value(self, msg): # Normal case: treat axis as axis if self.real_type == JoyMapping.AXIS: @@ -46,6 +51,12 @@ def update_value(self, msg): v = (-arr[self.real_index] + 1) * 0.5 else: v = arr[self.real_index] + + if self._range: + if v < self._range[0]: + v = self._range[0] + elif v > self._range[1]: + v = self._range[1] self.value = v # Special case: treat button as axis else: diff --git a/mofpy_demo/config/presets/arm.yaml b/mofpy_demo/config/presets/arm.yaml index f13690b..fac8051 100644 --- a/mofpy_demo/config/presets/arm.yaml +++ b/mofpy_demo/config/presets/arm.yaml @@ -49,6 +49,4 @@ presets: panda_joint5: L1 panda_joint6: R1 panda_joint7: OP - axes: - plus: C_U - minus: C_L + value: [C_U, C_L] diff --git a/mofpy_demo/config/ps4_wired.yaml b/mofpy_demo/config/ps4_wired.yaml index f4a62c0..86d9938 100644 --- a/mofpy_demo/config/ps4_wired.yaml +++ b/mofpy_demo/config/ps4_wired.yaml @@ -43,29 +43,25 @@ virtual: real: type: axis index: 6 - low_range: [-0.1, 0.0] - high_range: [-1.0, -0.1] + range: [-1.0, 0.0] # Left cursor button C_L: real: type: axis index: 6 - low_range: [0.0, 0.1] - high_range: [0.1, 1.0] + range: [0.0, 0.1] # Up cursor button C_U: real: type: axis index: 7 - low_range: [0.0, 0.1] - high_range: [0.1, 1.0] + range: [0.0, 1.0] # Down cursor button C_D: real: type: axis index: 7 - low_range: [-0.1, 0.0] - high_range: [-1.0, -0.1] + range: [-1.0, 0.0] buttons: X: real: From 2d72cb0e6606f92f93bcfc00e7a8c8d9be443f68 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Mon, 24 Feb 2025 21:16:15 +0900 Subject: [PATCH 2/2] reformat --- mofpy/mofpy/action/moveit_servo_joint.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/mofpy/mofpy/action/moveit_servo_joint.py b/mofpy/mofpy/action/moveit_servo_joint.py index 1953667..cd3040f 100644 --- a/mofpy/mofpy/action/moveit_servo_joint.py +++ b/mofpy/mofpy/action/moveit_servo_joint.py @@ -56,7 +56,7 @@ def execute(self, named_joy=None): ) jog_joints, is_quiet = self.__get_jog_joints__(named_joy["buttons"], named_joy["axes"]) - + if self.__quiet_on_zero: if is_quiet: # Publish the all-zero message just once @@ -107,7 +107,7 @@ def __get_jog_joints__(self, named_buttons, named_axes): vel = self.__scale * self.__get_enable_joint__(joint, named_buttons) * v msg.joint_names.append(joint) msg.velocities.append(vel) - + is_quiet = all(val == 0 for val in msg.velocities) return msg, is_quiet @@ -115,10 +115,10 @@ def __mapping__(self, key=None): mapping_key = "mapping" + f"/{key}" if key else key params = self.get(mapping_key, {}) mapping = {} - + if type(params) is not dict: return params - + for key in params.keys(): val = params[key] if type(val) is tuple or type(val) is list: @@ -143,7 +143,6 @@ def __get_value__(self, named_axes): :param named_axes: the processed joy values to get the value from :return: the value """ - # List of button names to be added in order to get the value. # A name could start with '-', indicating to invert the value names = self.__value_mapping