Merge pull request #6 from TohokuQuix/fix-joint-jog-mapping

fix: 順運動学アクションの制御量のマッピング方法を修正

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(
@@ -103,7 +103,7 @@ 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)
@@ -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,18 +135,28 @@ 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.
 
         :param axis: one of +, - to get the value of
         :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)

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:

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]

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: