Added g2p closed loop controller //Marcel

controllers/g2p-closed-loop/src/main.py

@@ -0,0 +1,50 @@
+import numpy as np
+from matplotlib import pyplot as plt
+from warnings import simplefilter
+from tensorflow.keras.models import save_model
+from functions import *
+from motor_babbling import *
+
+# Make sure we always get the same random data
+np.random.seed(0)
+
+# Generate first training data with motor babbling
+[babbling_kinematics, babbling_activations] = motor_babbling(babbling_seconds = 300, timestep = 0.01, pass_chance = 0.01, skip_rows = 200, max_in = 1, min_in = 0)
+
+# Create training database initially with babbling data
+cum_kinematics = babbling_kinematics
+cum_activations = babbling_activations
+
+# Create inverse mapping with babbling data and save MinMaxScaler from input and output
+model, scalerIn, scalerOut = inverse_mapping(kinematics = babbling_kinematics, activations = babbling_activations, early_stopping = True)
+
+# Constants for PID Controller
+# TODO: Might have to be tuned/adjusted
+# TODO: Should we use D as well? -> Make it more responsive to changes in error
+P = np.array([10])
+I = np.array([2])
+
+# Number of trials to get refined model
+trial_number = 1
+# Average error of predictions for each trial
+average_error = []
+# List of all models from each trial
+models = []
+#Make sure we always get the same random data
+np.random.seed(0)
+
+# Iterate over trials and refine model
+for i in range(trial_number):
+
+    # Refine model 10 times with optimized training data
+    model, errors, cum_kinematics, cum_activations = refine(model = model, scalerIn = scalerIn, scalerOut = scalerOut, babbling_kinematics = babbling_kinematics, babbling_activations = babbling_activations, P = P, I = I, num_refinements = 10, timestep = 0.005)
+
+    # Save model and average error for each trial
+    models.append(model)
+    average_error.append(np.mean(errors))
+
+# Find model with lowest average error
+best_model = models[np.argmin(average_error)]
+
+# Save best model as h5 file. It can be used for inverse mapping!
+save_model(best_model, 'model.h5')

controllers/g2p-closed-loop/src/motor_babbling.py

@@ -0,0 +1,75 @@
+from functions import *
+import numpy as np
+
+def motor_babbling(babbling_seconds = 300, timestep = 0.01, pass_chance = 0.01, skip_rows = 200, max_in = 1, min_in = 0):
+
+    """
+    Implementation of motor babbling to create an inverse kinematics model of the test bench
+
+    Parameters
+    ----------
+        * babbling_seconds: number of seconds to babble (default: 300)
+        * timestep: duration of each timestep in seconds (default: 0.01)
+        * pass_chance: probability of activation being passed to motor (default: 0.01)
+        * skip_rows: number of rows to skip in babbling data (setup phase) (default: 200)
+        * max_in: maximum activation value (default: 1)
+        * min_in: minimum activation value (default: 0)
+
+    Returns
+    -------
+        * babbling_kinematics: kinematics resulting from given activations
+        * babbling_activations: activations of babbling generated from generate_activation_values()
+    """
+
+    #number of samples to run
+    run_samples = int(np.round(babbling_seconds/timestep))
+
+    #generating random activations in range (min_in, max_in) -> These activations could fight against each other but that is okay
+    motor1_act = generate_activations(babbling_seconds, pass_chance, max_in, min_in, run_samples)
+    motor2_act = generate_activations(babbling_seconds, pass_chance, max_in, min_in, run_samples)
+
+    #concatenate motor activations in one vector with shape (run_samples, num_motors)
+    babbling_activations = np.transpose(np.concatenate([[motor1_act],[motor2_act]], axis=0))
+
+    #run babbling activations and map activations to resulting kinematics
+    [babbling_kinematics, babbling_activations, _] = run_activations(babbling_activations, timestep)
+
+    #return kinematics to activations mapping after skipping setup phase
+    return babbling_kinematics[skip_rows:,:], babbling_activations[skip_rows:, :]
+
+def generate_activations(signal_duration: int, pass_chance: float, max_in: float, min_in: float, run_samples: int):
+
+    """
+    Generating random activations for each motor
+    
+    Parameters
+    ----------
+        * signal_duration: duration of signal in seconds
+        * pass_chance: probability of activation being passed to motor
+        * max_in: maximum activation value
+        * min_in: minimum activation value
+        * run_samples: number of samples to run
+    
+    Returns
+    -------
+        * generated_activations: activations that can be run on robot
+    """
+
+    #generate evenly spaced samples
+    samples = np.linspace(0, signal_duration, run_samples)
+
+    #initialize activations vector
+    generated_activations = np.zeros(run_samples,)
+
+	#generating random activations
+    for i in range(1, run_samples):
+        pass_rand = np.random.uniform(0,1,1)
+
+        if pass_rand < pass_chance:
+            #generate random activation in range (min_in, max_in)
+            generated_activations[i] = ((max_in - min_in) * np.random.uniform(0,1,1)) + min_in
+        else:
+            #set activation to previous activation
+            generated_activations[i] = generated_activations[i-1]
+
+    return generated_activations

controllers/g2p-closed-loop/src/run.py

@@ -0,0 +1,55 @@
+import numpy as np
+import sklearn
+import rospy
+
+def run_activations(activations: np.ndarray, timestep: int):
+
+	"""
+	Running activations on test bench
+
+	Parameters
+	----------
+		* activations: activations to run on test bench
+		* timestep: duration of each timestep in seconds
+
+	Returns
+	-------
+		* actual_kinematics: kinematics resulting from given activations
+		* actual_activations: activations that were run on test bench
+		* actual_tendon_forces: tendon forces that were measured on test bench
+	"""
+
+	#number of activations
+	num_activations = activations.shape[0]
+
+	#actual angles of test bench joint
+	actual_angles = np.zeros((num_activations,1))
+
+	#actual tendon forces of test bench tendons (felx, extend)
+	actual_tendon_forces = np.zeros((num_activations,2))
+
+	#actual activations of test bench motors (felx, extend)
+	actual_activations = np.zeros((num_activations,2))
+
+	#iterate over activations and run test bench with it
+	for i in range(num_activations):
+
+		#TODO: Send activation to test bench
+		#Here we would run the test bench with the activation
+		#Check Nils code for how to do that
+		#Current problem: Motors are not backdrivable -> How can we avoid tendon slackness? Valero Lab had backdrivable motors
+
+		msg = BenchMotorControl()
+		msg.flex_myobrick_pwm = activations[i,0]
+		msg.extend_myobrick_pwm = activations[i,1]
+		publisher.publish(msg)
+
+		#appending positions and activations to arrays
+		actual_angles[i,:] = 0 # Todo get angle from test bench
+
+		pass
+
+	#calculate kinematics from angles
+	actual_kinematics = calculate_kinematics(angles = actual_angles, timestep = timestep)
+
+	return actual_kinematics, actual_activations, actual_tendon_forces