Merge pull request #315 from lukaslueken/develop

Develop
do-mpc · Oct 14, 2022 · e86a9cc · e86a9cc
2 parents 0d422ee + 0f701ec
commit e86a9cc
Showing 1 changed file with 40 additions and 19 deletions.
diff --git a/do_mpc/differentiator/differentiator.py b/do_mpc/differentiator/differentiator.py
@@ -97,9 +97,39 @@ def _transform_nlp_to_standard_full(self):
             all_equal_sym = nl_equal_sym
             all_inequal_sym = vertcat(nl_upper_sym, nl_lower_sym)
 
-        # Create transformation of lagrange multiplier    
+        # symbolically check, wether upper or lower bounds on inequality constraints are active
+        sign_lam_g_sym = sign(lam_g_sym)
+        sign_lam_x_sym = sign(lam_x_sym)        
+
+        # correction_lam_g_upper = (sign_lam_g_sym == 1)
+        # correction_lam_x_upper = (sign_lam_x_sym == 1)
+        # correction_lam_g_lower = -1*(sign_lam_g_sym == -1)
+        # correction_lam_x_lower = -1*(sign_lam_x_sym == -1)
+
+        lam_gx_trans = []
+        lam_gx_trans.append((sign_lam_g_sym == 1)[self.where_g_upper])
+        lam_gx_trans.append((sign_lam_x_sym == 1)[self.where_x_upper])
+        lam_gx_trans.append(-1*(sign_lam_g_sym == -1)[self.where_g_lower])
+        lam_gx_trans.append(-1*(sign_lam_x_sym == -1)[self.where_x_lower])
+        lam_gx_trans_sym = vertcat(*lam_gx_trans)
+
+        # Create transformation of lagrange multiplier
         nu_sym_transformed  = vertcat(lam_g_sym[self.where_g_equal], lam_x_sym[self.where_x_equal])
-        lam_sym_transformed = vertcat(lam_g_sym[self.where_g_upper], lam_x_sym[self.where_x_upper], lam_g_sym[self.where_g_lower], lam_x_sym[self.where_x_lower])
+
+        # lam_sym_transformed = vertcat((lam_g_sym*correction_lam_g_upper)[self.where_g_upper],
+        #     (lam_x_sym*correction_lam_x_upper)[self.where_x_upper], 
+        #     (lam_g_sym*correction_lam_g_lower)[self.where_g_lower], 
+            # (lam_x_sym*correction_lam_x_lower)[self.where_x_lower])
+
+        # lam_sym_transformed = vertcat((lam_g_sym*sign_lam_g_sym*(sign_lam_g_sym == 1))[self.where_g_upper],
+        #     (lam_x_sym*sign_lam_x_sym*(sign_lam_x_sym == 1))[self.where_x_upper], 
+        #     (lam_g_sym*sign_lam_g_sym*(sign_lam_g_sym == -1))[self.where_g_lower], 
+        #     (lam_x_sym*sign_lam_x_sym*(sign_lam_x_sym == -1))[self.where_x_lower])
+
+        lam_sym_transformed = vertcat(lam_g_sym[self.where_g_upper],
+            lam_x_sym[self.where_x_upper], 
+            lam_g_sym[self.where_g_lower], 
+            lam_x_sym[self.where_x_lower])*lam_gx_trans_sym
 
         self.nu_function =  Function('nu_function',  [lam_g_sym, lam_x_sym], [nu_sym_transformed], ["lam_g", "lam_x"], ["nu_sym"])
         self.lam_function = Function('lam_function', [lam_g_sym, lam_x_sym], [lam_sym_transformed], ["lam_g", "lam_x"], ["lam_sym"])
@@ -116,32 +146,23 @@ def _transform_nlp_to_standard_full(self):
 
         self.nlp_standard_full_bounds = {"lbg":lbg_full_standard, "ubg":ubg_full_standard}
 
-        print("NLP transformed: \n")
-        print("[g_nl,g_x] --> [g_nl_ubg,g_x_ubx,g_nl_lbg,g_x_lbx] + [h_nl, h_x]")
-
-        self.flags['transformed'] = 'full_standard'
-
-
-    def _get_primal_dual_sym(self):
-
-        if self.flags['transformed'] == 'full_standard':
-            nlp = self.nlp_standard_full_dict
-        else:
-            raise Exception('NLP not transformed yet.')
-
+        # 6. symbolic expressions for dual variables
         nu_sym  = SX.sym('nu',  self.n_h) # dual variables for equality constraints
         lam_sym = SX.sym('lam', self.n_g) # dual variables for inequality constraints
-
-        nlp.update({
+        self.nlp_standard_full_dict.update({
             "lam":lam_sym,
             "nu":nu_sym
         })
 
+        print("NLP transformed: \n")
+        print("[g_nl,g_x] --> [g_nl_ubg,g_x_ubx,g_nl_lbg,g_x_lbx] + [h_nl, h_x]")
+
+        self.flags['transformed'] = 'full_standard'
+
 
     def transform_nlp(self, variant='full_standard'):
         if variant is 'full_standard':
             self._transform_nlp_to_standard_full()
-            self._get_primal_dual_sym()
         else:
             raise NotImplementedError("Transformation variant {} is not implemented.".format(variant))
 
@@ -243,7 +264,7 @@ def setup_NLP_example_1():
     ## setup Bounds
     lbg = -np.inf*np.ones(g_sym.shape)
     ubg = np.zeros(g_sym.shape)
-    ubg[-1] = 1.0
+    ubg[-1] = 1.2
     lbg[-1] = 1.0
 
     # lbx = np.zeros(x_sym.shape)