Skip to content

Corrected naming (DOF vs links) in simulink libary + Removed path to … #53

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ediget merged 1 commit into from
Aug 26, 2026
Merged

Corrected naming (DOF vs links) in simulink libary + Removed path to … #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
% Pre-computed constants or internal states
properties (Access = private)
robot_model
links
dof

all_robot_types = ["BB Handler", "UR3e", "UR5e"];

Expand All @@ -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(...
Expand All @@ -44,21 +44,21 @@ 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

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(~)
Expand Down
12 changes: 6 additions & 6 deletions simulink/library/+cartesian_space_control/operational_space_controller.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
% Pre-computed constants or internal states
properties (Access = private)
robot_model
links
dof

all_robot_types = ["BB Handler", "UR3e", "UR5e"];

Expand All @@ -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(...
Expand All @@ -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
Expand All @@ -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(~)
Expand Down
33 changes: 11 additions & 22 deletions simulink/library/+dynamics/coriolis_matrix.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -13,38 +9,35 @@
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(~)
C = true;
end

function resetImpl(obj)
% Initialize / reset internal properties

end

function C = getOutputSizeImpl(obj)
Expand All @@ -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)
Expand Down
37 changes: 13 additions & 24 deletions simulink/library/+dynamics/forward_dynamics.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -13,9 +9,9 @@
robot_model
end

% Pre-computed constants or internal states

properties (Access = private)
links
dof

fwd_dyn

Expand All @@ -26,51 +22,44 @@
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(~)
ddq = true;
end

function resetImpl(obj)
% Initialize / reset internal properties

end

function ddq = getOutputSizeImpl(obj)
ddq = propagatedInputSize(obj, 1);
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)
Expand Down
23 changes: 9 additions & 14 deletions simulink/library/+dynamics/gravity_torques.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@

% Pre-computed constants or internal states
properties (Access = private)
links
dof

sdu_controllers
end
Expand All @@ -24,44 +24,39 @@
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(~)
tau = true;
end

function resetImpl(obj)
% Initialize / reset internal properties

end

function tau = getOutputSizeImpl(obj)
tau = propagatedInputSize(obj, 1);
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)
Expand Down
8 changes: 4 additions & 4 deletions simulink/library/+dynamics/inertia_matrix.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@

% Pre-computed constants or internal states
properties (Access = private)
links
dof

sdu_controllers
end
Expand All @@ -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(~)
Expand Down
Loading
Loading