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
+
+from pycram.ros.logging import logwarn
+
+try:
+    import yarp
+except ImportError:
+    logwarn("yarp wasn't found, please check the installation")
+
+if yarp is not None:
+
+    ACK_VOCAB = yarp.createVocab32('a','c','k')
+    NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k')
+
+
+    def init_yarp_network():
+        """
+         Initializes the YARP network.
+
+         :param func: Function that should be thread safe
+         :return bool -> true  if the YARP network is successfully initialized.
+         """
+        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.
+
+            :param `port_name` (str): The name of the RPC client port to be opened.
+            :return Tuple: (bool ( success/ fail ), yarp.RpcClient or None)
+        """
+        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.
+
+            :param `port_name` (str): The name of the port to be opened.
+            :return Tuple: (bool ( success/ fail ), yarp.BufferedPortBottle or None)
+        """
+        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):
+        """
+             interrupt and close the module
+             :param m_module: yarp module
+        """
+        m_module.interruptModule()
+        m_module.close()
+        print("iCub state updater closed")

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

@@ -439,3 +439,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()