Merge pull request #64 from clearpathrobotics/lcamero/manipulation

Manipulation

clearpath_config/clearpath_config.py

@@ -30,6 +30,7 @@
 from clearpath_config.system.system import SystemConfig
 from clearpath_config.platform.platform import PlatformConfig
 from clearpath_config.links.links import LinksConfig
+from clearpath_config.manipulators.manipulators import ManipulatorConfig
 from clearpath_config.mounts.mounts import MountsConfig
 from clearpath_config.sensors.sensors import SensorConfig
 
@@ -44,6 +45,7 @@ class ClearpathConfig(BaseConfig):
     SYSTEM = "system"
     PLATFORM = "platform"
     LINKS = "links"
+    MANIPULATORS = "manipulators"
     MOUNTS = "mounts"
     SENSORS = "sensors"
 
@@ -53,6 +55,7 @@ class ClearpathConfig(BaseConfig):
         SYSTEM: SYSTEM,
         PLATFORM: PLATFORM,
         LINKS: LINKS,
+        MANIPULATORS: MANIPULATORS,
         MOUNTS: MOUNTS,
         SENSORS: SENSORS
     }
@@ -65,6 +68,7 @@ class ClearpathConfig(BaseConfig):
         SYSTEM: SystemConfig.DEFAULTS,
         PLATFORM: PlatformConfig.DEFAULTS,
         LINKS: LinksConfig.DEFAULTS,
+        MANIPULATORS: ManipulatorConfig.DEFAULTS,
         MOUNTS: MountsConfig.DEFAULTS,
         SENSORS: SensorConfig.DEFAULTS,
     }
@@ -78,6 +82,7 @@ def __init__(self, config: dict | str = None) -> None:
         self._system = SystemConfig(self.DEFAULTS[self.SYSTEM])
         self._platform = PlatformConfig(self.DEFAULTS[self.PLATFORM])
         self._links = LinksConfig(self.DEFAULTS[self.LINKS])
+        self._manipulators = ManipulatorConfig(self.DEFAULTS[self.MANIPULATORS])
         self._mounts = MountsConfig(self.DEFAULTS[self.MOUNTS])
         self._sensors = SensorConfig(self.DEFAULTS[self.SENSORS])
         # Initialization
@@ -90,6 +95,7 @@ def __init__(self, config: dict | str = None) -> None:
             self.SYSTEM: ClearpathConfig.system,
             self.PLATFORM: ClearpathConfig.platform,
             self.LINKS: ClearpathConfig.links,
+            self.MANIPULATORS: ClearpathConfig.manipulators,
             self.MOUNTS: ClearpathConfig.mounts,
             self.SENSORS: ClearpathConfig.sensors,
         }
@@ -169,6 +175,17 @@ def links(self) -> LinksConfig:
     def links(self, config: dict) -> None:
         self._links.config = config
 
+    @property
+    def manipulators(self) -> ManipulatorConfig:
+        self.set_config_param(
+            self.MANIPULATORS,
+            self._manipulators.config[self.MANIPULATORS])
+        return self._manipulators
+
+    @manipulators.setter
+    def manipulators(self, config: dict) -> None:
+        self._manipulators.config = config
+
     @property
     def mounts(self) -> MountsConfig:
         self.set_config_param(

clearpath_config/common/utils/dictionary.py

@@ -43,7 +43,7 @@ def flatten_dict(d: MutableMapping, parent_key: str = '', dlim: str = '.'):
     return dict(_flatten_dict_gen(d, parent_key, dlim))
 
 
-def merge_dict(a, b, path=None):
+def merge_dict(a, b, path=None, priority=0):
     """Merge dict b into dict a."""
     if path is None:
         path = []
@@ -52,9 +52,16 @@ def merge_dict(a, b, path=None):
             if isinstance(a[key], dict) and isinstance(b[key], dict):
                 merge_dict(a[key], b[key], path + [str(key)])
             elif a[key] == b[key]:
-                pass  # same leaf value
+                # same leaf value
+                pass
             else:
-                raise Exception('Conflict at %s' % '.'.join(path + [str(key)]))
+                # assign leaf value of priority
+                if isinstance(a[key], list) and isinstance(b[key], list):
+                    val = a[key]
+                    val.extend(b[key])
+                    a[key] = val
+                elif priority:
+                    a[key] = b[key]
         else:
             a[key] = b[key]
     return a
@@ -123,3 +130,37 @@ def extend_flat_dict(a: dict, b: dict):
     for key, value in flatten_dict(b).items():
         a[key] = value
     return a
+
+
+def replace_dict_keys(d: dict, replacements: dict):
+    new_d = dict()
+    for key, value in flatten_dict(d).items():
+        new_key = key
+        for r in replacements:
+            if r in key:
+                new_key = key.replace(r, replacements[r])
+        new_d[new_key] = value
+    return unflatten_dict(new_d)
+
+
+def replace_dict_values(d: dict, replacements: dict):
+    new_d = dict()
+    for key, value in flatten_dict(d).items():
+        for r in replacements:
+            new_value = value
+            if isinstance(value, str) and r in value:
+                new_value = value.replace(r, replacements[r])
+            if isinstance(value, list):
+                for i, v in enumerate(new_value):
+                    new_v = v
+                    if isinstance(v, str) and r in v:
+                        new_v = v.replace(r, replacements[r])
+                    new_value[i] = new_v
+            new_d[key] = new_value
+    return unflatten_dict(new_d)
+
+
+def replace_dict_items(d: dict, replacements: dict):
+    new_d = replace_dict_keys(d, replacements)
+    new_d = replace_dict_values(new_d, replacements)
+    return unflatten_dict(new_d)

clearpath_config/common/utils/yaml.py

@@ -47,22 +47,27 @@ def find_valid_path(path, cwd=None):
 
 
 def read_yaml(path: str) -> dict:
+    orig = path
     # Check YAML Path
-    path = find_valid_path(path, os.getcwd())
-    assert path, "YAML file '%s' could not be found" % path
+    try:
+        path = find_valid_path(path, os.getcwd())
+        assert path, "YAML file '%s' could not be found" % orig
+    except FileNotFoundError:
+        raise AssertionError(
+            "YAML file '%s' could not be found" % orig)
     # Check YAML can be Opened
     try:
         config = yaml.load(open(path), Loader=yaml.SafeLoader)
     except yaml.scanner.ScannerError:
         raise AssertionError(
-            "YAML file '%s' is not well formed" % path)
+            "YAML file '%s' is not well formed" % orig)
     except yaml.constructor.ConstructorError:
         raise AssertionError(
             "YAML file '%s' is attempting to create unsafe objects" % (
-                path))
+                orig))
     # Check contents are a Dictionary
     assert isinstance(config, dict), (
-        "YAML file '%s' is not a dictionary" % path)
+        "YAML file '%s' is not a dictionary" % orig)
     return config
 
 

clearpath_config/manipulators/manipulators.py

@@ -0,0 +1,107 @@
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <lcamero@clearpathrobotics.com>
+# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT 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.
+from typing import List
+
+from clearpath_config.common.types.config import BaseConfig
+from clearpath_config.common.types.list import OrderedListConfig
+from clearpath_config.common.utils.dictionary import flip_dict
+from clearpath_config.manipulators.types.arms import (
+    Arm,
+    BaseArm,
+)
+from clearpath_config.manipulators.types.manipulator import BaseManipulator
+
+
+class ArmListConfig(OrderedListConfig[BaseArm]):
+
+    def __init__(self) -> None:
+        super().__init__(obj_type=BaseArm)
+
+    def to_dict(self) -> List[dict]:
+        d = []
+        for arm in self.get_all():
+            d.append(arm.to_dict())
+        return d
+
+
+class ManipulatorConfig(BaseConfig):
+    MANIPULATORS = "manipulators"
+    ARMS = "arms"
+    LIFTS = "lifts"
+    TEMPLATE = {
+        MANIPULATORS: {
+            ARMS: ARMS,
+        }
+    }
+
+    KEYS = flip_dict(TEMPLATE)
+
+    DEFAULTS = {
+        ARMS: [],
+    }
+
+    def __init__(
+            self,
+            config: dict = {},
+            ) -> None:
+        # List Initialization
+        self._arms = ArmListConfig()
+        template = {
+            self.KEYS[self.ARMS]: ManipulatorConfig.arms,
+        }
+        super().__init__(template, config, self.MANIPULATORS)
+
+    @property
+    def arms(self) -> OrderedListConfig:
+        self.set_config_param(
+            key=self.KEYS[self.ARMS],
+            value=self._arms.to_dict()
+        )
+        return self._arms
+
+    @arms.setter
+    def arms(self, value: List[dict]) -> None:
+        assert isinstance(value, list), (
+            "Manipulators must be list of 'dict'")
+        assert all([isinstance(i, dict) for i in value]), (
+            "Manipulators must be list of 'dict'")
+        arms_list = []
+        for d in value:
+            arm = Arm(d['model'])
+            arm.from_dict(d)
+            arms_list.append(arm)
+        self._arms.set_all(arms_list)
+
+    def get_all_manipulators(self) -> List[BaseManipulator]:
+        manipulators = []
+        # Arms
+        manipulators.extend(self.get_all_arms())
+        return manipulators
+
+    def get_all_arms(self) -> List[BaseArm]:
+        return self._arms.get_all()