diff --git a/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m b/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m
index d7df881..93af1bf 100644
--- a/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m
+++ b/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m
@@ -16,7 +16,7 @@
     % Pre-computed constants or internal states
     properties (Access = private)        
         robot_model
-        links
+        dof
 
         all_robot_types = ["BB Handler", "UR3e", "UR5e"];
         
@@ -31,7 +31,7 @@ function setupImpl(obj)
             % Perform one-time calculations, such as computing constants
             switch obj.RobotType
                 case obj.all_robot_types(1)
-                    obj.robot_model = obj.sdu_controllers.models.ParameterRobotModel("/home/madla/Documents/sdu_controllers/config/models/breeding_blanket_handling_robot.yaml");
+                    obj.robot_model = obj.sdu_controllers.models.BreedingBlanketHandlingRobotModel();
 
                 case obj.all_robot_types(2)
                     obj.robot_model = obj.sdu_controllers.models.URRobotModel(...
@@ -44,7 +44,7 @@ function setupImpl(obj)
                     );
             end
 
-            obj.links = double(obj.robot_model.get_dof());
+            obj.dof = double(obj.robot_model.get_dof());
 
             obj.f_contr = obj.sdu_controllers.controllers.ForceControlInnerVelocityLoop(obj.Kp, obj.Kd, obj.Md, obj.Kf, obj.robot_model);
         end
@@ -52,13 +52,13 @@ function setupImpl(obj)
         function [y] = stepImpl(obj, f_d, f_e, q, dq)
             f_d = reshape(f_d, 1, 6);
             f_e = reshape(f_e, 1, 6);
-            q = reshape(q, 1, obj.links);
-            dq = reshape(dq, 1, obj.links);
+            q = reshape(q, 1, obj.dof);
+            dq = reshape(dq, 1, obj.dof);
 
             obj.f_contr.step(f_d, f_e, q, dq);
 
             y = double(obj.f_contr.get_output());
-            y = reshape(y, obj.links, 1);
+            y = reshape(y, obj.dof, 1);
         end
 
         function [y] = isOutputFixedSizeImpl(~)
diff --git a/simulink/library/+cartesian_space_control/operational_space_controller.m b/simulink/library/+cartesian_space_control/operational_space_controller.m
index b1937f6..aa68e42 100644
--- a/simulink/library/+cartesian_space_control/operational_space_controller.m
+++ b/simulink/library/+cartesian_space_control/operational_space_controller.m
@@ -19,7 +19,7 @@
     % Pre-computed constants or internal states
     properties (Access = private)        
         robot_model
-        links
+        dof
 
         all_robot_types = ["BB Handler", "UR3e", "UR5e"];
         
@@ -34,7 +34,7 @@ function setupImpl(obj)
             % Perform one-time calculations, such as computing constants
             switch obj.RobotType
                 case obj.all_robot_types(1)
-                    obj.robot_model = obj.sdu_controllers.models.ParameterRobotModel("/home/madla/Documents/sdu_controllers/config/models/breeding_blanket_handling_robot.yaml");
+                    obj.robot_model = obj.sdu_controllers.models.BreedingBlanketHandlingRobotModel();
 
                 case obj.all_robot_types(2)
                     obj.robot_model = obj.sdu_controllers.models.URRobotModel(...
@@ -47,7 +47,7 @@ function setupImpl(obj)
                     );
             end
 
-            obj.links = double(obj.robot_model.get_dof());
+            obj.dof = double(obj.robot_model.get_dof());
 
             obj.os_contr = obj.sdu_controllers.controllers.OperationalSpaceController(obj.Kp, obj.Kd, obj.robot_model);
         end
@@ -56,13 +56,13 @@ function setupImpl(obj)
             x_d = reshape(x_d, 1, 6);
             dx_d = reshape(dx_d, 1, 6);
             ddx_d = reshape(ddx_d, 1, 6);
-            q = reshape(q, 1, obj.links);
-            dq = reshape(dq, 1, obj.links);
+            q = reshape(q, 1, obj.dof);
+            dq = reshape(dq, 1, obj.dof);
 
             obj.os_contr.step(x_d, dx_d, ddx_d, q, dq);
 
             y = double(obj.os_contr.get_output());
-            y = reshape(y, obj.links, 1);
+            y = reshape(y, obj.dof, 1);
         end
 
         function [y] = isOutputFixedSizeImpl(~)
diff --git a/simulink/library/+dynamics/coriolis_matrix.m b/simulink/library/+dynamics/coriolis_matrix.m
index d61a224..690e7c2 100644
--- a/simulink/library/+dynamics/coriolis_matrix.m
+++ b/simulink/library/+dynamics/coriolis_matrix.m
@@ -1,10 +1,6 @@
 classdef coriolis_matrix < matlab.System
     % Coriolis matrix
-    %
-    % This template includes the minimum set of functions required
-    % to define a System object.
-
-    % Public, tunable properties
+ 
     properties
 
     end
@@ -13,30 +9,27 @@
         robot_model
     end
 
-    % Pre-computed constants or internal states
     properties (Access = private)
-        links
+        dof
 
         sdu_controllers
     end
 
     methods (Access = protected)
         function setupImpl(obj)
-            % Perform one-time calculations, such as computing constants
             obj.sdu_controllers = py.importlib.import_module('sdu_controllers');
 
-            obj.links = double(obj.robot_model.get_dof());
-            % disp(obj.links)
+            obj.dof = double(obj.robot_model.get_dof());
+
         end
 
         function [C] = stepImpl(obj, q, dq)
-            % Implement algorithm. Calculate y as a function of input u and
-            % internal states.
-            q = reshape(q, 1, obj.links);
-            dq = reshape(dq, 1, obj.links);
+
+            q = reshape(q, 1, obj.dof);
+            dq = reshape(dq, 1, obj.dof);
 
             C = obj.robot_model.get_coriolis(q, dq);
-            C = reshape(double(C), obj.links, obj.links);
+            C = reshape(double(C), obj.dof, obj.dof);
         end
 
         function C = isOutputFixedSizeImpl(~)
@@ -44,7 +37,7 @@ function setupImpl(obj)
         end
 
         function resetImpl(obj)
-            % Initialize / reset internal properties
+
         end
 
         function C = getOutputSizeImpl(obj)
@@ -53,18 +46,14 @@ function resetImpl(obj)
         end
 
         function C = getOutputDataTypeImpl(obj)
-            % Return data type for each output port
+
             C = "double";
 
-            % Example: inherit data type from first input port
-            % out = propagatedInputDataType(obj,1);
         end
 
         function C = isOutputComplexImpl(obj)
-            % Return true for each output port with complex data
             C = false;
-            % Example: inherit complexity from first input port
-            % out = propagatedInputComplexity(obj,1);
+
         end
 
         function icon = getIconImpl(obj)
diff --git a/simulink/library/+dynamics/forward_dynamics.m b/simulink/library/+dynamics/forward_dynamics.m
index f8431b3..f5d0f43 100644
--- a/simulink/library/+dynamics/forward_dynamics.m
+++ b/simulink/library/+dynamics/forward_dynamics.m
@@ -1,10 +1,6 @@
 classdef forward_dynamics < matlab.System
     % Forward Dynamics
-    %
-    % This template includes the minimum set of functions required
-    % to define a System object.
 
-    % Public, tunable properties
     properties
 
     end
@@ -13,9 +9,9 @@
         robot_model
     end
 
-    % Pre-computed constants or internal states
+
     properties (Access = private)
-        links
+        dof
 
         fwd_dyn
 
@@ -26,24 +22,20 @@
         function setupImpl(obj)
             obj.sdu_controllers = py.importlib.import_module('sdu_controllers');
 
-            obj.links = double(obj.robot_model.get_dof());
-            % disp(obj.links)
+            obj.dof = double(obj.robot_model.get_dof());
 
             obj.fwd_dyn = obj.sdu_controllers.math.ForwardDynamics(obj.robot_model);
         end
 
         function ddq = stepImpl(obj, q, dq, tau)
-            % Implement algorithm. Calculate y as a function of input u and
-            % internal states.
-            q = reshape(q, 1, obj.links);
-            dq = reshape(dq, 1, obj.links);
-            tau = reshape(tau, 1, obj.links);
+            q = reshape(q, 1, obj.dof);
+            dq = reshape(dq, 1, obj.dof);
+            tau = reshape(tau, 1, obj.dof);
 
             ddq = obj.fwd_dyn.forward_dynamics(q, dq, tau);
-            % disp(ddq)
-            % ddq = double(ddq).';
-            ddq = reshape(double(ddq), obj.links, 1);
-            % disp(ddq)
+
+            ddq = reshape(double(ddq), obj.dof, 1);
+
         end
 
         function ddq = isOutputFixedSizeImpl(~)
@@ -51,7 +43,7 @@ function setupImpl(obj)
         end
 
         function resetImpl(obj)
-            % Initialize / reset internal properties
+
         end
 
         function ddq = getOutputSizeImpl(obj)
@@ -59,18 +51,15 @@ function resetImpl(obj)
         end
 
         function ddq = getOutputDataTypeImpl(obj)
-            % Return data type for each output port
+
             ddq = "double";
 
-            % Example: inherit data type from first input port
-            % out = propagatedInputDataType(obj,1);
         end
 
         function ddq = isOutputComplexImpl(obj)
-            % Return true for each output port with complex data
+
             ddq = false;
-            % Example: inherit complexity from first input port
-            % out = propagatedInputComplexity(obj,1);
+
         end
 
         function icon = getIconImpl(obj)
diff --git a/simulink/library/+dynamics/gravity_torques.m b/simulink/library/+dynamics/gravity_torques.m
index 5c507b8..bd22951 100644
--- a/simulink/library/+dynamics/gravity_torques.m
+++ b/simulink/library/+dynamics/gravity_torques.m
@@ -15,7 +15,7 @@
 
     % Pre-computed constants or internal states
     properties (Access = private)
-        links
+        dof
 
         sdu_controllers
     end
@@ -24,17 +24,15 @@
         function setupImpl(obj)
             obj.sdu_controllers = py.importlib.import_module('sdu_controllers');
             
-            obj.links = double(obj.robot_model.get_dof());
-            % disp(obj.links)
+            obj.dof = double(obj.robot_model.get_dof());
         end
 
         function tau = stepImpl(obj, q)
-            % Implement algorithm. Calculate y as a function of input u and
-            % internal states.
-            q = reshape(q, 1, obj.links);
+
+            q = reshape(q, 1, obj.dof);
 
             tau = obj.robot_model.get_gravity(q);
-            tau = reshape(double(tau), obj.links, 1);
+            tau = reshape(double(tau), obj.dof, 1);
         end
 
         function tau = isOutputFixedSizeImpl(~)
@@ -42,7 +40,7 @@ function setupImpl(obj)
         end
 
         function resetImpl(obj)
-            % Initialize / reset internal properties
+
         end
 
         function tau = getOutputSizeImpl(obj)
@@ -50,18 +48,15 @@ function resetImpl(obj)
         end
 
         function tau = getOutputDataTypeImpl(obj)
-            % Return data type for each output port
+
             tau = "double";
 
-            % Example: inherit data type from first input port
-            % out = propagatedInputDataType(obj,1);
         end
 
         function tau = isOutputComplexImpl(obj)
-            % Return true for each output port with complex data
+
             tau = false;
-            % Example: inherit complexity from first input port
-            % out = propagatedInputComplexity(obj,1);
+
         end
 
         function icon = getIconImpl(obj)
diff --git a/simulink/library/+dynamics/inertia_matrix.m b/simulink/library/+dynamics/inertia_matrix.m
index a17eb2e..8e2faf7 100644
--- a/simulink/library/+dynamics/inertia_matrix.m
+++ b/simulink/library/+dynamics/inertia_matrix.m
@@ -15,7 +15,7 @@
 
     % Pre-computed constants or internal states
     properties (Access = private)
-        links
+        dof
 
         sdu_controllers
     end
@@ -24,15 +24,15 @@
         function setupImpl(obj)
             obj.sdu_controllers = py.importlib.import_module('sdu_controllers');
 
-            obj.links = double(obj.robot_model.get_dof());
+            obj.dof = double(obj.robot_model.get_dof());
         end
 
         function [B] = stepImpl(obj, q)
             % Implement algorithm. Calculate y as a function of input u and
             % internal states.
-            q = reshape(q, 1, obj.links);
+            q = reshape(q, 1, obj.dof);
             B = obj.robot_model.get_inertia_matrix(q);
-            B = reshape(double(B), obj.links, obj.links);
+            B = reshape(double(B), obj.dof, obj.dof);
         end
 
         function B = isOutputFixedSizeImpl(~)
diff --git a/simulink/library/+dynamics/inverse_dynamics.m b/simulink/library/+dynamics/inverse_dynamics.m
index ed9cd70..67c4e99 100644
--- a/simulink/library/+dynamics/inverse_dynamics.m
+++ b/simulink/library/+dynamics/inverse_dynamics.m
@@ -1,10 +1,7 @@
 classdef inverse_dynamics < matlab.System
     % Inverse Dynamics
     %
-    % This template includes the minimum set of functions required
-    % to define a System object.
 
-    % Public, tunable properties
     properties
     end
 
@@ -12,9 +9,8 @@
         robot_model
     end
 
-    % Pre-computed constants or internal states
     properties (Access = private)
-        links
+        dof
 
         inv_dyn
 
@@ -25,25 +21,20 @@
         function setupImpl(obj)
             obj.sdu_controllers = py.importlib.import_module('sdu_controllers');
 
-            obj.links = double(obj.robot_model.get_dof());
-            % disp(obj.links)
+            obj.dof = double(obj.robot_model.get_dof());
 
             obj.inv_dyn = obj.sdu_controllers.math.InverseDynamicsJointSpace(obj.robot_model);
         end
 
         function tau = stepImpl(obj, q, dq, y)
-            % Implement algorithm. Calculate y as a function of input u and
-            % internal states.
-            y = reshape(y, 1, obj.links);
-            q = reshape(q, 1, obj.links);
-            dq = reshape(dq, 1, obj.links);
+
+            y = reshape(y, 1, obj.dof);
+            q = reshape(q, 1, obj.dof);
+            dq = reshape(dq, 1, obj.dof);
 
             tau = obj.inv_dyn.inverse_dynamics(y, q, dq);
-            % disp(tau)
-            % tau = double(tau).';
-            % tau = reshape(double(tau), obj.links, 1);
             tau = reshape(double(tau), propagatedInputSize(obj, 1));
-            % disp(tau)
+
         end
 
         function tau = isOutputFixedSizeImpl(~)
@@ -57,20 +48,16 @@ function resetImpl(obj)
         function tau = getOutputSizeImpl(obj)
             tau = propagatedInputSize(obj, 1);
         end
-
+% ln
         function tau = getOutputDataTypeImpl(obj)
-            % Return data type for each output port
+
             tau = "double";
 
-            % Example: inherit data type from first input port
-            % out = propagatedInputDataType(obj,1);
         end
 
         function tau = isOutputComplexImpl(obj)
-            % Return true for each output port with complex data
             tau = false;
-            % Example: inherit complexity from first input port
-            % out = propagatedInputComplexity(obj,1);
+
         end
 
         function icon = getIconImpl(obj)
diff --git a/simulink/library/+kinematics/forward_kinematics.m b/simulink/library/+kinematics/forward_kinematics.m
index e6b1ada..f79919a 100644
--- a/simulink/library/+kinematics/forward_kinematics.m
+++ b/simulink/library/+kinematics/forward_kinematics.m
@@ -1,10 +1,5 @@
 classdef forward_kinematics < matlab.System
-    % Inertia matrix
-    %
-    % This template includes the minimum set of functions required
-    % to define a System object.
 
-    % Public, tunable properties
     properties
 
     end
@@ -13,10 +8,10 @@
         RobotType {mustBeMember(RobotType, ["BB Handler", "UR3e", "UR5e"])} = "UR5e"
     end
 
-    % Pre-computed constants or internal states
+
     properties (Access = private)
         robot_model
-        links
+        dof
         fk_solver
 
         all_robot_types = ["BB Handler", "UR3e", "UR5e"];
@@ -32,30 +27,26 @@ function setupImpl(obj)
             switch obj.RobotType
                 case obj.all_robot_types(1)
                     obj.robot_model = obj.sdu_controllers.models.ParameterRobotModel("/home/madla/Documents/sdu_controllers/config/models/breeding_blanket_handling_robot.yaml");
-                    obj.links = 6;
 
                 case obj.all_robot_types(2)
                     obj.robot_model = obj.sdu_controllers.models.URRobotModel(...
                         obj.sdu_controllers.models.RobotType(0) ...
                     );
-                    obj.links = 6;
+
 
                 case obj.all_robot_types(3)
                     obj.robot_model = obj.sdu_controllers.models.URRobotModel(...
                         obj.sdu_controllers.models.RobotType(1) ...
                     );
-                    obj.links = 6;
-            end
 
+            end
+            obj.dof = double(obj.robot_model.get_dof());
             obj.fk_solver = obj.robot_model.get_fk_solver();
 
-            % obj.links = double(obj.robot_model.get_dof());
-            % disp(obj.links)
         end
 
         function [T] = stepImpl(obj, q)
-            % Implement algorithm. Calculate y as a function of input u and
-            % internal states.
+
             T = obj.fk_solver.forward_kinematics(q);
             T = double(T);
         end
@@ -65,7 +56,7 @@ function setupImpl(obj)
         end
 
         function resetImpl(obj)
-            % Initialize / reset internal properties
+
         end
 
         function T = getOutputSizeImpl(obj)
@@ -73,18 +64,15 @@ function resetImpl(obj)
         end
 
         function T = getOutputDataTypeImpl(obj)
-            % Return data type for each output port
+
             T = "double";
 
-            % Example: inherit data type from first input port
-            % out = propagatedInputDataType(obj,1);
         end
 
         function T = isOutputComplexImpl(obj)
-            % Return true for each output port with complex data
+
             T = false;
-            % Example: inherit complexity from first input port
-            % out = propagatedInputComplexity(obj,1);
+
         end
 
         function icon = getIconImpl(obj)
diff --git a/simulink/library/+kinematics/jacobian.m b/simulink/library/+kinematics/jacobian.m
new file mode 100644
index 0000000..33d05a2
--- /dev/null
+++ b/simulink/library/+kinematics/jacobian.m
@@ -0,0 +1,99 @@
+classdef jacobian < matlab.System
+    % Inertia matrix
+    %
+    % This template includes the minimum set of functions required
+    % to define a System object.
+
+    % Public, tunable properties
+    properties
+
+    end
+
+    properties(Nontunable)
+        RobotType {mustBeMember(RobotType, ["BB Handler", "UR3e", "UR5e"])} = "UR5e"
+    end
+
+    % Pre-computed constants or internal states
+    properties (Access = private)
+        robot_model
+        dof
+        fk_solver
+
+        all_robot_types = ["BB Handler", "UR3e", "UR5e"];
+
+        sdu_controllers
+    end
+
+    methods (Access = protected)
+        function setupImpl(obj)
+            obj.sdu_controllers = py.importlib.import_module('sdu_controllers');
+            
+            % Perform one-time calculations, such as computing constants
+            switch obj.RobotType
+                case obj.all_robot_types(1)
+                    obj.robot_model = obj.sdu_controllers.models.BreedingBlanketHandlingRobotModel();
+
+                case obj.all_robot_types(2)
+                    obj.robot_model = obj.sdu_controllers.models.URRobotModel(...
+                        obj.sdu_controllers.models.RobotType(0) ...
+                    );
+
+                case obj.all_robot_types(3)
+                    obj.robot_model = obj.sdu_controllers.models.URRobotModel(...
+                        obj.sdu_controllers.models.RobotType(1) ...
+                    );
+            end
+
+            obj.dof = double(obj.robot_model.get_dof());
+        end
+
+        function [J] = stepImpl(obj, q)
+            % Implement algorithm. Calculate y as a function of input u and
+            % internal states.
+            J = obj.robot_model.get_jacobian(q');
+            
+            J = double(J);
+        end
+
+        function J = isOutputFixedSizeImpl(~)
+            J = true;
+        end
+
+        function resetImpl(obj)
+            % Initialize / reset internal properties
+        end
+
+        function J = getOutputSizeImpl(obj)
+            switch obj.RobotType
+                case obj.all_robot_types(1)
+                    obj.links = 6;
+
+                case obj.all_robot_types(2)
+                    obj.links = 6;
+
+                case obj.all_robot_types(3)
+                    obj.links = 6;
+            end
+            J = [6,obj.links];
+        end
+
+        function J = getOutputDataTypeImpl(obj)
+            % Return data type for each output port
+            J = "double";
+
+            % Example: inherit data type from first input port
+            % out = propagatedInputDataType(obj,1);
+        end
+
+        function J = isOutputComplexImpl(obj)
+            % Return true for each output port with complex data
+            J = false;
+            % Example: inherit complexity from first input port
+            % out = propagatedInputComplexity(obj,1);
+        end
+
+        function icon = getIconImpl(obj)
+            icon = {'Jacobian', obj.RobotType};
+        end
+    end
+end