From a7fd9f02142bf84dcdfd157367f26887737a3b1c Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Mon, 9 Nov 2020 21:55:58 +0100 Subject: [PATCH] Fixed `test_matlab_system_19.mdl` test model, removed unused motor coupling parameter `config.MOTOR_REFLECTED_INERTIA_COUPLING`. --- app/robots/iCubGenova04/configRobot.m | 3 ++ init.m | 1 + lib/+wbs/@Robot/Robot.m | 4 +-- test_matlab_system_19.mdl | 47 ++++++++++++++++++--------- 4 files changed, 36 insertions(+), 19 deletions(-) diff --git a/app/robots/iCubGenova04/configRobot.m b/app/robots/iCubGenova04/configRobot.m index 07a9a59..ae06349 100644 --- a/app/robots/iCubGenova04/configRobot.m +++ b/app/robots/iCubGenova04/configRobot.m @@ -68,6 +68,9 @@ % friction coefficient for the feet Config.friction_coefficient = 0.1; +% Reflected inertia +Config.SIMULATE_MOTOR_REFLECTED_INERTIA = true; + % Robot frames list Frames.BASE = 'root_link'; Frames.COM = 'com'; diff --git a/init.m b/init.m index 3412a5d..02a6684 100644 --- a/init.m +++ b/init.m @@ -47,3 +47,4 @@ %% Init simulator core physics paramaters physics_config.GRAVITY_ACC = Config.GRAVITY_ACC; physics_config.TIME_STEP = Config.tStep; +robot_config.SIMULATE_MOTOR_REFLECTED_INERTIA = Config.SIMULATE_MOTOR_REFLECTED_INERTIA; diff --git a/lib/+wbs/@Robot/Robot.m b/lib/+wbs/@Robot/Robot.m index 3a8fc06..b4e4d54 100644 --- a/lib/+wbs/@Robot/Robot.m +++ b/lib/+wbs/@Robot/Robot.m @@ -20,7 +20,6 @@ g; % gravity vector M_iDyn; % mass matrix iDynTree useMotorReflectedInertias; % Adds the reflected inetias to the mass matrix - T; % motor coupling matrix J_LFoot_iDyntree; % Jacobian relative to left foot J_RFoot_iDyntree; % Jacobian relative to right foot JDot_nu_LFoot_iDyntree; % \dot{J} \nu relative to left foot @@ -54,7 +53,6 @@ obj.h_iDyn = iDynTree.FreeFloatingGeneralizedTorques(obj.KinDynModel.kinDynComp.model); obj.M_iDyn = iDynTree.MatrixDynSize(); obj.useMotorReflectedInertias = config.SIMULATE_MOTOR_REFLECTED_INERTIA; - obj.T = config.MOTOR_REFLECTED_INERTIA_COUPLING; obj.NDOF = obj.KinDynModel.NDOF; obj.S = [zeros(6, obj.KinDynModel.NDOF); eye(obj.KinDynModel.NDOF)]; end @@ -78,7 +76,7 @@ function set_robot_state(obj, w_H_b, s, base_pose_dot, s_dot) % Add the reflected inertia if the feature is activated if obj.useMotorReflectedInertias - M = obj.M_iDyn.toMatlab + obj.T*diag([zeros(6,1);motorInertias]); + M = obj.M_iDyn.toMatlab + diag([zeros(6,1);motorInertias]); else M = obj.M_iDyn.toMatlab; end diff --git a/test_matlab_system_19.mdl b/test_matlab_system_19.mdl index 1dc8e30..a992f9f 100644 --- a/test_matlab_system_19.mdl +++ b/test_matlab_system_19.mdl @@ -6,7 +6,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.89" + ComputedModelVersion "1.91" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -68,7 +68,7 @@ Model { $ObjectID 3 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [40.0, 23.0, 1752.0, 1097.0] + Location [39.0, 23.0, 1753.0, 1097.0] Object { $PropName "ModelBrowserInfo" $ObjectID 4 @@ -95,10 +95,10 @@ Model { IsTabbed [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1638.0, 868.0] + Extents [1639.0, 868.0] ZoomFactor [3.116883116883117] - Offset [-5.2624999999999886, 73.508333333333354] - SceneRectInView [-5.2624999999999886, 73.508333333333354, 525.525, 278.48333333333329] + Offset [-5.4229166666666515, 73.508333333333354] + SceneRectInView [-5.4229166666666515, 73.508333333333354, 525.8458333333333, 278.48333333333329] } Object { $ObjectID 7 @@ -220,7 +220,7 @@ Model { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABrAAf//wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAaMAAADoQAAAAEAAAACAAAAAQAAAAL8AAAAAwAAAAAAAAABAAAANgBjAG8AbABsAGEAcABzAGkAY" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAaNAAADoQAAAAEAAAACAAAAAQAAAAL8AAAAAwAAAAAAAAABAAAANgBjAG8AbABsAGEAcABzAGkAY" "gBsAGUAUABhAG4AZQBsAFQAbwBvAGwAQgBhAHIATABlAGYAdAMAAAAA/////wAAAAAAAAAAAAAAAQAAAAEAAAA4AGMAbwBsAGwAYQBwAHMAaQBiA" "GwAZQBQAGEAbgBlAGwAVABvAG8AbABCAGEAcgBSAGkAZwBoAHQDAAAAAP////8AAAAAAAAAAAAAAAMAAAABAAAAOgBjAG8AbABsAGEAcABzAGkAY" "gBsAGUAUABhAG4AZQBsAFQAbwBvAGwAQgBhAHIAQgBvAHQAdABvAG0AAAAAAP////8AAAAAAAAAAA==" @@ -240,9 +240,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Wed Nov 04 02:54:33 2020" - RTWModifiedTimeStamp 526351566 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Nov 09 21:55:27 2020" + RTWModifiedTimeStamp 526859590 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1300,7 +1300,7 @@ Model { } System { Name "test_matlab_system_19" - Location [40, 23, 1792, 1120] + Location [39, 23, 1792, 1120] Open on PortBlocksUseCompactNotation off SetExecutionDomain off @@ -1317,7 +1317,7 @@ Model { ShowPageBoundaries off ZoomFactor "312" ReportName "simulink-default.rpt" - SIDHighWatermark "204" + SIDHighWatermark "205" SimulinkSubDomain "Simulink" Block { BlockType BusSelector @@ -1372,16 +1372,24 @@ Model { BlockType Constant Name "Constant1" SID "140" - Position [0, 300, 30, 330] + Position [0, 275, 30, 305] ZOrder 1236 Value "zeros(Config.N_DOF + 6,1)" } + Block { + BlockType Constant + Name "Constant2" + SID "205" + Position [0, 310, 30, 340] + ZOrder 1261 + Value "zeros(Config.N_DOF,1)" + } Block { BlockType Delay Name "Delay1" SID "187" Ports [1, 1] - Position [55, 249, 80, 271] + Position [55, 244, 80, 266] ZOrder 1250 InputPortMap "u0" DelayLength "1" @@ -1712,10 +1720,10 @@ Model { BlockType Reference Name "RobotDynWithContacts" SID "197" - Ports [2, 4] + Ports [3, 4] Position [105, 232, 300, 343] ZOrder 1252 - LibraryVersion "1.14" + LibraryVersion "1.18" SourceBlock "robotDynamicsWithContacts_lib/RobotDynWithContacts" SourceType "SubSystem" SourceProductName "RobotDynamicsWithContacts" @@ -1811,7 +1819,7 @@ Model { Labels [0, 0] SrcBlock "MATLAB Function" SrcPort 1 - Points [-10, 0] + Points [-8, 0; 0, 125] DstBlock "Delay1" DstPort 1 } @@ -1847,6 +1855,13 @@ Model { DstBlock "Terminator" DstPort 1 } + Line { + ZOrder 169 + SrcBlock "Constant2" + SrcPort 1 + DstBlock "RobotDynWithContacts" + DstPort 3 + } } } #Finite State Machines