Icub dev

launch/ik_and_description.launch

@@ -16,6 +16,12 @@
     <param name="robot_description"
               textfile="$(find pycram)/resources/robots/boxy.urdf"/>
   </group>
+
+    <!-- iCub -->
+  <group if="$(eval robot == 'icub')">
+    <param name="robot_description"
+              textfile="$(find icub_model)/urdf/model.urdf"/>
+  </group>
   <!-- Tiago -->
   <group if="$(eval robot == 'tiago_dual')">
     <param name="robot_description"

src/pycram/external_interfaces/yarp_networking.py

@@ -0,0 +1,66 @@
+from typing_extensions import Tuple,Optional
+
+import yarp
+
+
+ACK_VOCAB = yarp.createVocab32('a','c','k')
+NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k')
+
+
+def init_yarp_network():
+    """
+        Initializes the YARP network.
+
+        **Returns:**
+        - `True` if the YARP network is successfully initialized.
+        - `False` if no YARP server is found.
+    """
+    if not yarp.Network.checkNetwork():
+        print("Unable to find a yarp server exiting ...")
+        return False
+
+    yarp.Network.init()
+    return True
+
+def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.RpcClient]]:
+    """
+        Opens a YARP RpcClient port with the specified name.
+
+        **Parameters:**
+        - `port_name` (str): The name of the RPC client port to be opened.
+
+        **Returns:**
+        - Tuple: (bool, yarp.RpcClient or None)
+            - `True` and the opened port if successful.
+            - `False` and `None` if the port fails to open.
+    """
+    handle_port: yarp.RpcClient = yarp.RpcClient()
+    if not handle_port.open(port_name):
+        print(f"Can't open the port %s correctly" % port_name)
+        return False , None
+    print(f"Port %s opened correctly" % port_name)
+    return True , handle_port
+
+def open_buffered_bottle_port(port_name:str)->Tuple[bool, Optional[yarp.BufferedPortBottle]]:
+    """
+        Opens a YARP BufferedPortBottle with the specified port name.
+
+        **Parameters:**
+        - `port_name` (str): The name of the port to be opened.
+
+        **Returns:**
+        - Tuple: (bool, yarp.BufferedPortBottle or None)
+            - `True` and the opened port if successful.
+            - `False` and `None` if the port fails to open.
+    """
+    opened_port: yarp.BufferedPortBottle = yarp.BufferedPortBottle()
+    if not opened_port.open(port_name):
+        print(f"Can't open the port %s correctly" % port_name)
+        return False , None
+    print(f"Port %s opened correctly" % port_name)
+    return True , opened_port
+
+
+def interrupt_and_close(m_module:yarp.RFModule):
+    m_module.interruptModule()
+    m_module.close()

src/pycram/failures.py

@@ -527,3 +527,14 @@ def __init__(self, link_name: str):
 class LinkGeometryHasNoMesh(Exception):
     def __init__(self, link_name: str, geometry_type: str):
         super().__init__(f"Link {link_name} geometry with type {geometry_type} has no mesh.")
+
+
+class YarpNetworkError(Exception):
+    def __init__(self):
+        super().__init__(f"Yarp Network Initialization Failed. Check if yarpserver is already up")
+
+
+class RobotNotInitialized(Exception):
+    def __init__(self,robot_name: str):
+        super().__init__(f"Robot ({robot_name} not correctly initialized")
+

src/pycram/process_module.py

@@ -425,3 +425,9 @@ def close(self) -> ProcessModule:
         """
         raise NotImplementedError(
             f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'")
+
+    def exit(self)->None:
+        """
+        Exit process module and disconnect from robot
+        """
+        pass

src/pycram/process_modules/__init__.py

@@ -6,12 +6,14 @@
 from .stretch_process_modules import StretchManager
 from .robotiq_gripper_process_module import RobotiqManager
 from .tiago_process_modules import TiagoManager
+from .icub_process_modules import ICubManager
 
 Pr2Manager()
 BoxyManager()
 DonbotManager()
 HSRBManager()
 DefaultManager()
 StretchManager()
+ICubManager()
 TiagoManager()
 RobotiqManager()