Merge pull request #1 from YanzhenXiangRobotics/submissionHEADmaster

Submission

1 files changed, 38 insertions, 0 deletions

diff --git a/Submission files/MPC_TE_forces.m b/Submission files/MPC_TE_forces.m
new file mode 100644
index 0000000..5625f32
--- /dev/null
+++ b/Submission files/MPC_TE_forces.m
@@ -0,0 +1,38 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
+%
+% All rights reserved.
+%
+% Please see the LICENSE file that has been included as part of this package.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+classdef MPC_TE_forces
+    properties
+        forces_optimizer
+    end
+
+    methods
+        function obj = MPC_TE_forces(Q,R,N,params)
+            % YOUR CODE HERE
+            opts = getOptions('forcesSolver');
+            opts.printlevel = 0;
+            obj.forces_optimizer = % YOUR CODE HERE
+        end
+
+        function [u, ctrl_info] = eval(obj,x)
+            %% evaluate control action by solving MPC problem, e.g.
+            [optimizer_out,errorcode,info] = obj.forces_optimizer(x);
+            u = optimizer_out;
+            objective = info.pobj;
+            solvetime = info.solvetime;
+
+            feasible = true;
+            if any(errorcode ~= 1)
+                feasible = false;
+                warning('MPC infeasible');
+            end
+
+            ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime);
+        end
+    end
+end
\ No newline at end of file