Remove force dependency on yarp

lib/wholebodycontrollib/facialexpression.py

@@ -0,0 +1,38 @@
+import yarp
+
+class FacialExpression(): 
+    def __init__(self)->None:
+        # rpc for face expression
+        yarp.Network.init()
+
+        rpc_client_name = '/ergoCubEmotions/rpcClient'
+        rpc_server_name = '/ergoCubEmotions/rpc'
+
+        self.rpc_command = yarp.Bottle()
+        self.rpc_command.addString('setEmotion')
+        self.rpc_response = yarp.Bottle()
+
+        self.rpc_client = yarp.RpcClient()
+        self.rpc_client.open(rpc_client_name)
+
+        yarp.Network.connect(rpc_client_name, rpc_server_name)
+
+        # Initialize face expression to angry
+        self.happy = False
+        self.rpc_command.addString('shy')
+        self.rpc_client.write(self.rpc_command, self.rpc_response)
+        self.rpc_command.pop() 
+
+    def update_face(self,ref):
+        if(ref<0.8):
+            if self.happy:
+                    self.rpc_command.addString('shy')
+                    self.rpc_client.write(self.rpc_command, self.rpc_response)
+                    self.rpc_command.pop()
+                    self.happy = False
+        else:
+            if not self.happy:
+                self.rpc_command.addString('happy')
+                self.rpc_client.write(self.rpc_command, self.rpc_response)
+                self.rpc_command.pop()
+                self.happy = True

lib/wholebodycontrollib/statemachine.py

@@ -1,5 +1,4 @@
 import numpy as np
-import yarp
 
 class Configuration():
 
@@ -10,35 +9,16 @@ def __init__(self, joint_position : np.array, com_position : np.array, duration)
 
 class StateMachine():
 
-    def __init__(self, repeat = False):
+    def __init__(self, repeat = False, set_emotion = False):
         self.configurations = []
         self.time = 0
         self.state_start_time = -1
         self.current_state = 0
         self.repeat = repeat
         self.phi = 0.0
-
-        # rpc for face expression
-        yarp.Network.init()
-
-        rpc_client_name = '/ergoCubEmotions/rpcClient'
-        rpc_server_name = '/ergoCubEmotions/rpc'
-
-        self.rpc_command = yarp.Bottle()
-        self.rpc_command.addString('setEmotion')
-        self.rpc_response = yarp.Bottle()
-
-        self.rpc_client = yarp.RpcClient()
-        self.rpc_client.open(rpc_client_name)
-
-        yarp.Network.connect(rpc_client_name, rpc_server_name)
-
-        # Initialize face expression to angry
-        self.happy = False
-        self.rpc_command.addString('shy')
-        self.rpc_client.write(self.rpc_command, self.rpc_response)
-        self.rpc_command.pop()
-
+        self.set_emotion = set_emotion
+        if self.set_emotion: from wholebodycontrollib.facialexpression import FacialExpression
+        if self.set_emotion: self.facial_expression = FacialExpression()
 
     def add_configuration(self, configuration : Configuration):
         self.configurations.append(configuration)
@@ -95,19 +75,8 @@ def get_state(self, use_parametrized=False, ref=0, pos=0, J=0, period=0.001, tra
                     tracking_gain =  tracking_gain * ((ref - 0.7)/0.1)
                 if ref<0.7:
                     tracking_gain = 0
-
-                if self.happy:
-                    self.rpc_command.addString('shy')
-                    self.rpc_client.write(self.rpc_command, self.rpc_response)
-                    self.rpc_command.pop()
-                    self.happy = False
-            else:
-                if not self.happy:
-                    self.rpc_command.addString('happy')
-                    self.rpc_client.write(self.rpc_command, self.rpc_response)
-                    self.rpc_command.pop()
-                    self.happy = True
-
+
+            if self.set_emotion: self.facial_expression.update_face(ref=ref)
 
             phi_dot =  -tracking_gain * (pos - ref )
 

lib/wholebodycontrollib/wholebodylib.py

@@ -1,7 +1,8 @@
 import numpy as np
 import idyntree.bindings as iDynTree
-
+import copy 
 from enum import Enum
+from typing import Union
 # from adam.casadi.computations import KinDynComputations
 
 class TransformFormat(Enum):
@@ -12,21 +13,28 @@ class TransformFormat(Enum):
 
 class robot():
 
-    def __init__(self, urdf_path, joints_list, base_link):
+    def __init__(self, urdf_path, joints_list, base_link, kinDyn:Union [iDynTree.KinDynComputations, None]=None):
+
+        if kinDyn is None:
+            self.joints_list = joints_list
+            self.base_link = base_link
+            self.urdf_path = urdf_path
+            self.ndof = len(joints_list)
 
-        self.joints_list = joints_list
-        self.base_link = base_link
-        self.urdf_path = urdf_path
-        self.ndof = len(joints_list)
+            # Load Model and Initialize KinDyn
+            self.kindyn = iDynTree.KinDynComputations()
+            mdlLoader = iDynTree.ModelLoader()
+            mdlLoader.loadReducedModelFromFile(urdf_path, joints_list)
+            if not self.kindyn.loadRobotModel(mdlLoader.model()):
+                raise ValueError('Failed to load the robot model in iDynTree.')
+            self.initialize_buffer_variables()
+        else: 
+            self.kindyn = kindyn
+            self.ndof = kindyn.getNrOfDegreesOfFreedom()
+            self.initialize_buffer_variables()
 
-        # Load Model and Initialize KinDyn
-        self.kindyn = iDynTree.KinDynComputations()
-        mdlLoader = iDynTree.ModelLoader()
-        mdlLoader.loadReducedModelFromFile(urdf_path, joints_list)
-        if not self.kindyn.loadRobotModel(mdlLoader.model()):
-            raise ValueError('Failed to load the robot model in iDynTree.')
 
-        # Initialize buffer variables
+    def initialize_buffer_variables(self): 
         self.s = iDynTree.VectorDynSize(self.ndof)
         self.ds = iDynTree.VectorDynSize(self.ndof)
         self.H_b = iDynTree.Transform()