save parameter indices of solver generation and add example

Author: LuziaKn

base_mpc/planner/mpcPlanner.py

@@ -0,0 +1,25 @@
+import sys
+sys.path.append('/home/luzia/code/forces_pro_client/')  # todo remove
+import pickle
+import forcespro.nlp
+
+class MPCPlanner(object):
+
+    def __init__(self, robot_type, solver_dir):
+
+
+        self.solver = forcespro.nlp.Solver.from_directory(solver_dir + "/AlbertFORCESNLPsolver") #todo add robot_type
+
+        with open(solver_dir + "/Albertparams.pkl", 'rb') as file:
+            self.parameters = pickle.load(file)
+
+
+
+    def updateDynamicObstacles(self, obstArray):
+        print('not implemented yet')
+    def setx0(self, xinit):
+        print('not implemented yet')
+    def solve(self, current_state):
+        #self._xinit = current_state[0: self._nx]
+        self.setX0(self._xinit)
+        print('not implemented yet')

base_mpc/python_solver_generation/base_mpc_solver.py

@@ -17,6 +17,7 @@
 
 import numpy as np
 import copy
+import pickle
 import forcespro.nlp
 
 import dynamics
@@ -158,8 +159,16 @@ def objective_with_stage_index(stage_idx):
         #output1 = ("sol", [], [])
         generated_solver = solver.generate_solver(options) #, outputs
 
+        #save settings
+        params_dict = settings.params.save()
+        file_name = dir_path + "/Albertparams.pkl"
+
+        with open(file_name, 'wb') as outp:
+            pickle.dump(params_dict, outp, pickle.HIGHEST_PROTOCOL)
+
         # Move the solver up a directory for convenience
         if os.path.isdir(solver_path):
             shutil.move(solver_path, new_solver_path)
 
 
+

base_mpc/python_solver_generation/helpers.py

@@ -50,20 +50,28 @@ class ParameterStructure:
 
     def __init__(self):
         self.parameters = dict()
-        self.organization = dict() # Lists parameter grouping and indices
+        self.organization = dict()
+        self.indices = dict() # Lists parameter grouping and indices
         self.param_idx = 0
+        self.set_params = []
+
+    def __reduce__(self):
+        return (ParameterStructure, (self.progress_int,))
+
 
     def add_parameter(self, name):
         self.organization[self.param_idx] = 1
         self.parameters[self.param_idx] = name
-        setattr(self, name+ "_index", self.param_idx)
+        self.indices[name+ "_index"] = self.param_idx
+       # setattr(self, name+ "_index", self.param_idx)
         self.param_idx += 1
 
     def add_multiple_parameters(self, name, amount):
         self.organization[self.param_idx] = amount
         for i in range(amount):
             self.parameters[self.param_idx] = name + "_" + str(i)
-            setattr(self, name + "_" + str(i) + "_index", self.param_idx)
+            self.indices[name + "_" + str(i) + "_index"] = self.param_idx
+            #setattr(self, name + "_" + str(i) + "_index", self.param_idx)
             self.param_idx += 1
 
     def has_parameter(self, name):
@@ -88,7 +96,18 @@ def __str__(self):
     # When operating, retrieve the weights from param
     def load_params(self, params):
         for key, name in self.parameters.items(): # This is a parameter name
-            setattr(self, name, params[getattr(self, name+ "_index")]) # this is an index
+            #setattr(self, name, params[getattr(self, name+ "_index")]) # this is an index
+            setattr(self, name, params[self.indices[name + "_index"]])
+
+    def save(self):
+        return self.__dict__['indices']
+
+    def set_params(self,name, value):
+        if self.set_params.len == 0:
+            self.set_params = np.zeros(self.param_idx,1)
+        index = getattr(self, 'x_goal'+ "_index")
+        self.set_params[index] = value
+
 
 class WeightStructure: