From b4957079f5d8c3ef930658743a220d2a9d1e93c0 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Thu, 6 Feb 2020 15:39:45 +0100 Subject: [PATCH] Add stiffness to flexible joints --- identification.org | 13 +++++++++ src/initializeJointDynamics.m | 51 ++++++++++++++++++++++++----------- stewart-architecture.org | 20 +++++++------- 3 files changed, 58 insertions(+), 26 deletions(-) diff --git a/identification.org b/identification.org index f929e33..5fe90b3 100644 --- a/identification.org +++ b/identification.org @@ -105,6 +105,19 @@ An important difference from basic Simulink models is that the states in a physi #+end_src * States as the motion of the mobile platform +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab :results none :exports none + simulinkproject('./'); +#+end_src + ** Initialize the Stewart Platform #+begin_src matlab stewart = initializeFramesPositions(); diff --git a/src/initializeJointDynamics.m b/src/initializeJointDynamics.m index a55aa17..831296d 100644 --- a/src/initializeJointDynamics.m +++ b/src/initializeJointDynamics.m @@ -5,27 +5,46 @@ function [stewart] = initializeJointDynamics(stewart, args) % % Inputs: % - args - Structure with the following fields: -% - Kbi [6x1] - Rotational Stiffness for each top spherical joints [N/rad] -% - Cbi [6x1] - Damping of each top spherical joint [N/(rad/s)] -% - Kti [6x1] - Rotational Stiffness for each bottom universal joints [N/rad] -% - Cti [6x1] - Damping of each bottom universal joint [N/(rad/s)] +% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [N/rad] +% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [N/(rad/s)] +% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [N/rad] +% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [N/(rad/s)] +% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [N/rad] +% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [N/(rad/s)] +% - disable [true/false] - Sets all the stiffness/damping to zero % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Kbi [6x1] - Rotational Stiffness for each top spherical joints [N/rad] -% - Cbi [6x1] - Damping of each top spherical joint [N/(rad/s)] -% - Kti [6x1] - Rotational Stiffness for each bottom universal joints [N/rad] -% - Cti [6x1] - Damping of each bottom universal joint [N/(rad/s)] +% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [N/rad] +% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [N/(rad/s)] +% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [N/rad] +% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [N/(rad/s)] +% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [N/rad] +% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [N/(rad/s)] arguments stewart - args.Kti (6,1) double {mustBeNumeric, mustBeNonnegative} = zeros(6,1) - args.Cti (6,1) double {mustBeNumeric, mustBeNonnegative} = zeros(6,1) - args.Kbi (6,1) double {mustBeNumeric, mustBeNonnegative} = zeros(6,1) - args.Cbi (6,1) double {mustBeNumeric, mustBeNonnegative} = zeros(6,1) + args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e+1*ones(6,1) + args.Csbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e+0*ones(6,1) + args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) + args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e+1*ones(6,1) + args.Cubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.disable logical {mustBeNumericOrLogical} = false end -stewart.Kti = args.Kti; -stewart.Cti = args.Cti; -stewart.Kbi = args.Kbi; -stewart.Cbi = args.Cbi; +if args.disable + stewart.Ksbi = zeros(6,1); + stewart.Csbi = zeros(6,1); + stewart.Ksti = zeros(6,1); + stewart.Csti = zeros(6,1); + stewart.Kubi = zeros(6,1); + stewart.Cubi = zeros(6,1); +else + stewart.Ksbi = args.Ksbi; + stewart.Csbi = args.Csbi; + stewart.Ksti = args.Ksti; + stewart.Csti = args.Csti; + stewart.Kubi = args.Kubi; + stewart.Cubi = args.Cubi; +end diff --git a/stewart-architecture.org b/stewart-architecture.org index a940ac2..553b2d5 100644 --- a/stewart-architecture.org +++ b/stewart-architecture.org @@ -874,11 +874,11 @@ This Matlab function is accessible [[file:src/initializeJointDynamics.m][here]]. #+begin_src matlab arguments stewart - args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-1*ones(6,1) + args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e+1*ones(6,1) args.Csbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) - args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2*ones(6,1) - args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) - args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-1*ones(6,1) + args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e+0*ones(6,1) + args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) + args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e+1*ones(6,1) args.Cubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) args.disable logical {mustBeNumericOrLogical} = false end @@ -890,12 +890,12 @@ This Matlab function is accessible [[file:src/initializeJointDynamics.m][here]]. :END: #+begin_src matlab if args.disable - stewart.Ksbi = 0; - stewart.Csbi = 0; - stewart.Ksti = 0; - stewart.Csti = 0; - stewart.Kubi = 0; - stewart.Cubi = 0; + stewart.Ksbi = zeros(6,1); + stewart.Csbi = zeros(6,1); + stewart.Ksti = zeros(6,1); + stewart.Csti = zeros(6,1); + stewart.Kubi = zeros(6,1); + stewart.Cubi = zeros(6,1); else stewart.Ksbi = args.Ksbi; stewart.Csbi = args.Csbi;