diff --git a/matlab/library/WBToolboxLibrary_repository.mdl b/matlab/library/WBToolboxLibrary_repository.mdl index fe33f20e6..bbf6b44e8 100644 --- a/matlab/library/WBToolboxLibrary_repository.mdl +++ b/matlab/library/WBToolboxLibrary_repository.mdl @@ -25,7 +25,7 @@ Library { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [75.0, 35.0, 1831.0, 1037.0] + Location [0.0, 164.0, 2560.0, 1440.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -52,32 +52,32 @@ Library { IsTabbed [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1793.0, 796.0] + Extents [2522.0, 1199.0] ZoomFactor [5.0] - Offset [-1.4290296712109409, -6.061988773055333] - SceneRectInView [-1.4290296712109409, -6.061988773055333, 358.6, 159.2] + Offset [-1.4290296712109409, -9.7619887730553359] + SceneRectInView [-1.4290296712109409, -9.7619887730553359, 504.4, 239.8] } Object { $ObjectID 6 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "202" + LoadSaveID "209" Extents [1793.0, 796.0] - ZoomFactor [1.7493744787322769] - Offset [213.61276834817875, 115.63518360375741] - SceneRectInView [213.61276834817875, 115.63518360375741, 1024.9377830750893, 455.0197854588796] + ZoomFactor [4.4014962593516209] + Offset [66.819263456090653, -7.9237960339943356] + SceneRectInView [66.819263456090653, -7.9237960339943356, 407.36147308781869, 180.84759206798867] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" - LoadSaveID "209" + LoadSaveID "202" Extents [1793.0, 796.0] - ZoomFactor [4.4014962593516209] - Offset [66.819263456090653, -7.9237960339943356] - SceneRectInView [66.819263456090653, -7.9237960339943356, 407.36147308781869, 180.84759206798867] + ZoomFactor [1.7493744787322769] + Offset [213.61276834817875, 115.63518360375741] + SceneRectInView [213.61276834817875, 115.63518360375741, 1024.9377830750893, 455.0197854588796] } Object { $ObjectID 8 @@ -166,7 +166,7 @@ Library { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAANQAAAooAAABiAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAcnAAADWQAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" + "QBjAHQAbwByAAAAAAD/////AAAAOQD///8AAAoAAAAE7AAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAA" Array { Type "Cell" Dimension 0 @@ -183,9 +183,9 @@ Library { ModifiedByFormat "%" LastModifiedBy "csartore" ModifiedDateFormat "%" - LastModifiedDate "Fri Jul 17 15:08:44 2020" - RTWModifiedTimeStamp 516296661 - ModelVersionFormat "1.%" + LastModifiedDate "Wed Jul 22 14:16:21 2020" + RTWModifiedTimeStamp 517328117 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "all" @@ -1239,7 +1239,7 @@ Library { } System { Name "WBToolboxLibrary_repository" - Location [75, 35, 1906, 1072] + Location [0, 164, 2560, 1604] Open on PortBlocksUseCompactNotation off SetExecutionDomain off @@ -1856,7 +1856,7 @@ Library { } System { Name "Model" - Location [75, 35, 1906, 1072] + Location [271, 377, 2102, 1414] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -2724,7 +2724,7 @@ Library { } System { Name "Jacobians" - Location [75, 35, 1906, 1072] + Location [271, 377, 2102, 1414] Open off PortBlocksUseCompactNotation off SetExecutionDomain off @@ -2743,7 +2743,7 @@ Library { SimulinkSubDomain "Simulink" Block { BlockType S-Function - Name "Centroidal Momentum Matrix" + Name "CentroidalTotalMomentumMatrix" SID "1852" Ports [2, 1] Position [805, 171, 1075, 264] @@ -2757,17 +2757,31 @@ Library { $PropName "MaskObject" $ObjectID 101 $ClassName "Simulink.Mask" - Type "Centroidal Momentum Matrix" - Description "This block retrieves the Centroidal Momentum Matrix.\n\nAssuming DoFs is the number of internal degree" - "s of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformation between t" - "he the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the configuration" - " of the joints.\n\nOutput:\n - CMM: a 6x6+dofs matrix representing the Centroidal Momentum Matrix written in the wo" - "rld frame." + Type "CentroidalTotalMomentumMatrix" + Description "This block retrieves the Centroidal Total Momentum Matrix.\n\nAssuming DoFs is the number of internal " + "degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformation bet" + "ween the the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the configu" + "ration of the joints.\n\nOutput:\n - CMM: a 6x6+dofs matrix representing the Centroidal Momentum Matrix written in " + "the world frame." Initialization "try\n [configBlockAbsName, WBTConfig] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTC" "onfigParameters = WBTConfig.getSimulinkParameters;\n clear WBTConfig;\ncatch\nend" - Display "\nport_label('output', 1, 'CMM', 'texmode','on')\nport_label('input', 1, '{}^{world} H_{base}', 'texmode'" - ",'on')\nport_label('input', 2, 'Joint configuration')\n\n" + Display "\nport_label('output', 1, 'CentroidalTotalMomentumMatrix', 'texmode','on')\nport_label('input', 1, '{}^{w" + "orld} H_{base}', 'texmode','on')\nport_label('input', 2, 'Joint configuration')\n\n" RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 102 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 103 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } } } Block { @@ -2780,7 +2794,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 102 + $ObjectID 104 $ClassName "Simulink.Mask" Type "DotJNu" Description "This block computes the product between the time derivative of the Jacobian of the specified frame and" @@ -2801,7 +2815,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 103 + $ObjectID 105 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -2812,11 +2826,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 104 + $ObjectID 106 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 105 + $ObjectID 107 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2824,22 +2838,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 106 + $ObjectID 108 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 107 + $ObjectID 109 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 108 + $ObjectID 110 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 109 + $ObjectID 111 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -2980,7 +2994,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 110 + $ObjectID 112 $ClassName "Simulink.Mask" Type "Jacobian" Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of internal d" @@ -2998,7 +3012,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 111 + $ObjectID 113 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -3009,11 +3023,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 112 + $ObjectID 114 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 113 + $ObjectID 115 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3021,22 +3035,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 114 + $ObjectID 116 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 115 + $ObjectID 117 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 116 + $ObjectID 118 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 117 + $ObjectID 119 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3150,7 +3164,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 118 + $ObjectID 120 $ClassName "Simulink.Mask" Display "image(imread('forwardKinematics.png'))" } @@ -3183,7 +3197,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 119 + $ObjectID 121 $ClassName "Simulink.Mask" Type "ForwardKinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -3201,7 +3215,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 120 + $ObjectID 122 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -3212,11 +3226,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 121 + $ObjectID 123 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 122 + $ObjectID 124 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3224,22 +3238,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 123 + $ObjectID 125 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 124 + $ObjectID 126 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 125 + $ObjectID 127 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 126 + $ObjectID 128 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3355,7 +3369,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 127 + $ObjectID 129 $ClassName "Simulink.Mask" Type "ForwardKinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -3380,21 +3394,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 128 + $ObjectID 130 Type "edit" Name "baseFrame" Prompt "Base Frame" Value "'root_link'" } Object { - $ObjectID 129 + $ObjectID 131 Type "edit" Name "endEffFrame" Prompt "End Effector frame" Value "'l_sole'" } Object { - $ObjectID 130 + $ObjectID 132 Type "popup" Array { Type "Cell" @@ -3408,7 +3422,7 @@ Library { Value "Full Constraint (Position and Orientation)" } Object { - $ObjectID 131 + $ObjectID 133 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -3416,7 +3430,7 @@ Library { Tunable "off" } Object { - $ObjectID 132 + $ObjectID 134 Type "edit" Name "localName" Prompt "Model Name" @@ -3424,7 +3438,7 @@ Library { Tunable "off" } Object { - $ObjectID 133 + $ObjectID 135 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -3432,7 +3446,7 @@ Library { Tunable "off" } Object { - $ObjectID 134 + $ObjectID 136 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -3445,11 +3459,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 135 + $ObjectID 137 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 136 + $ObjectID 138 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3457,34 +3471,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 137 + $ObjectID 139 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 138 + $ObjectID 140 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 139 + $ObjectID 141 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 140 + $ObjectID 142 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "baseFrame" } Object { - $ObjectID 141 + $ObjectID 143 $ClassName "Simulink.dialog.parameter.Edit" Name "endEffFrame" } Object { - $ObjectID 142 + $ObjectID 144 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -3493,25 +3507,25 @@ Library { Name "Container8" } Object { - $ObjectID 143 + $ObjectID 145 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 144 + $ObjectID 146 Name "robotName" } Object { - $ObjectID 145 + $ObjectID 147 Name "localName" } Object { - $ObjectID 146 + $ObjectID 148 Name "wbiFile" } Object { - $ObjectID 147 + $ObjectID 149 Name "wbiList" } PropName "DialogControls" @@ -3651,7 +3665,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 148 + $ObjectID 150 $ClassName "Simulink.Mask" Type "RelativeTransform" Description "This block calculates the relative transform between two frames.\n\nAssuming DoFs is the number of int" @@ -3669,14 +3683,14 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 149 + $ObjectID 151 Type "edit" Name "frame1" Prompt "Frame1 name" Value "'frame1'" } Object { - $ObjectID 150 + $ObjectID 152 Type "edit" Name "frame2" Prompt "Frame2 name" @@ -3688,11 +3702,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 151 + $ObjectID 153 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 152 + $ObjectID 154 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3700,27 +3714,27 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 153 + $ObjectID 155 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 3 Object { - $ObjectID 154 + $ObjectID 156 $ClassName "Simulink.dialog.parameter.Edit" Name "frame1" } Object { - $ObjectID 155 + $ObjectID 157 $ClassName "Simulink.dialog.parameter.Edit" Name "frame2" } Object { - $ObjectID 156 + $ObjectID 158 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 157 + $ObjectID 159 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3818,7 +3832,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 158 + $ObjectID 160 $ClassName "Simulink.Mask" Type "ForwardKinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -3845,14 +3859,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 159 + $ObjectID 161 Type "edit" Name "solverName" Prompt "Solver Name" Value "'/cartesianSolver'" } Object { - $ObjectID 160 + $ObjectID 162 Type "edit" Name "dofs" Prompt "#Dofs" @@ -3860,7 +3874,7 @@ Library { Tunable "off" } Object { - $ObjectID 161 + $ObjectID 163 Type "popup" Array { Type "Cell" @@ -3879,11 +3893,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 162 + $ObjectID 164 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 163 + $ObjectID 165 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3891,33 +3905,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 164 + $ObjectID 166 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 165 + $ObjectID 167 $ClassName "Simulink.dialog.TabContainer" Object { $PropName "DialogControls" - $ObjectID 166 + $ObjectID 168 $ClassName "Simulink.dialog.Tab" Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 167 + $ObjectID 169 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "solverName" } Object { - $ObjectID 168 + $ObjectID 170 $ClassName "Simulink.dialog.parameter.Edit" Name "dofs" } Object { - $ObjectID 169 + $ObjectID 171 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -4029,7 +4043,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 170 + $ObjectID 172 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyStates.png'),'center');" } @@ -4063,7 +4077,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 171 + $ObjectID 173 $ClassName "Simulink.Mask" Type "GetLimits" Description "This block provides the joint limits gathering data from either the Robot's Control Board or UR" @@ -4087,7 +4101,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 172 + $ObjectID 174 Type "popup" Array { Type "Cell" @@ -4108,7 +4122,7 @@ Library { "imit blockParameters limitsTypeBlockParam" } Object { - $ObjectID 173 + $ObjectID 175 Type "popup" Array { Type "Cell" @@ -4133,11 +4147,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 174 + $ObjectID 176 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 175 + $ObjectID 177 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4145,17 +4159,17 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 176 + $ObjectID 178 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Popup" Dimension 2 Object { - $ObjectID 177 + $ObjectID 179 Name "limitsSource" } Object { - $ObjectID 178 + $ObjectID 180 Name "limitsType" } PropName "DialogControls" @@ -4163,10 +4177,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 179 + $ObjectID 181 Object { $PropName "DialogControls" - $ObjectID 180 + $ObjectID 182 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -4259,7 +4273,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 181 + $ObjectID 183 $ClassName "Simulink.Mask" Type "GetMeasurement" Description "This block gathers the measurement of the specified type.\n\nAssuming DoFs is the number of deg" @@ -4273,7 +4287,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 182 + $ObjectID 184 $ClassName "Simulink.MaskParameter" Type "popup" Array { @@ -4300,11 +4314,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 183 + $ObjectID 185 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 184 + $ObjectID 186 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4312,21 +4326,21 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 185 + $ObjectID 187 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 186 + $ObjectID 188 $ClassName "Simulink.dialog.parameter.Popup" Name "measuredType" } Name "ParameterGroupVar" } Object { - $ObjectID 187 + $ObjectID 189 Object { $PropName "DialogControls" - $ObjectID 188 + $ObjectID 190 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -4401,7 +4415,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 189 + $ObjectID 191 $ClassName "Simulink.Mask" Type "GetMeasurement" Description "This block gathers the measurement of the specified type.\n\nAssuming DoFs is the number of deg" @@ -4415,7 +4429,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 190 + $ObjectID 192 $ClassName "Simulink.MaskParameter" Type "popup" Array { @@ -4447,11 +4461,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 191 + $ObjectID 193 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 192 + $ObjectID 194 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4459,21 +4473,21 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 193 + $ObjectID 195 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 194 + $ObjectID 196 $ClassName "Simulink.dialog.parameter.Popup" Name "measuredType" } Name "ParameterGroupVar" } Object { - $ObjectID 195 + $ObjectID 197 Object { $PropName "DialogControls" - $ObjectID 196 + $ObjectID 198 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -4551,7 +4565,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 197 + $ObjectID 199 $ClassName "Simulink.Mask" Display "image(imread('utilities.png'))" } @@ -4587,7 +4601,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 198 + $ObjectID 200 $ClassName "Simulink.Mask" SelfModifiable "on" Display "disp('Config')" @@ -4596,7 +4610,7 @@ Library { Type "Simulink.MaskParameter" Dimension 8 Object { - $ObjectID 199 + $ObjectID 201 Type "popup" Array { Type "Cell" @@ -4623,7 +4637,7 @@ Library { "h currentConfigSource" } Object { - $ObjectID 200 + $ObjectID 202 Type "edit" Name "ConfigObject" Prompt "Name of the object" @@ -4635,42 +4649,42 @@ Library { "oolbox.ConfigurationToMask(gcb,currentConfigObject);\n\nclear currentConfigObject;" } Object { - $ObjectID 201 + $ObjectID 203 Type "edit" Name "RobotName" Prompt "Robot Name" Value "'icubSim'" } Object { - $ObjectID 202 + $ObjectID 204 Type "edit" Name "UrdfFile" Prompt "Urdf File" Value "'model.urdf'" } Object { - $ObjectID 203 + $ObjectID 205 Type "edit" Name "ControlledJoints" Prompt "Controlled Joints" Value "{'l_elbow','l_shoulder_pitch','torso_roll'}" } Object { - $ObjectID 204 + $ObjectID 206 Type "edit" Name "ControlBoardsNames" Prompt "Control Boards Names" Value "{'left_arm','right_arm','torso'}" } Object { - $ObjectID 205 + $ObjectID 207 Type "edit" Name "LocalName" Prompt "Local Name" Value "'WBT'" } Object { - $ObjectID 206 + $ObjectID 208 Type "edit" Name "GravityVector" Prompt "Gravity Vector" @@ -4682,11 +4696,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 207 + $ObjectID 209 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 208 + $ObjectID 210 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4694,28 +4708,28 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 209 + $ObjectID 211 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 210 + $ObjectID 212 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 211 + $ObjectID 213 Prompt "From" Array { Type "Simulink.dialog.parameter.Control" Dimension 2 Object { - $ObjectID 212 + $ObjectID 214 $ClassName "Simulink.dialog.parameter.Popup" Name "ConfigSource" } Object { - $ObjectID 213 + $ObjectID 215 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "ConfigObject" @@ -4725,38 +4739,38 @@ Library { Name "TabFrom" } Object { - $ObjectID 214 + $ObjectID 216 Prompt "Data" Array { Type "Simulink.dialog.parameter.Edit" Dimension 6 Object { - $ObjectID 215 + $ObjectID 217 PromptLocation "left" Name "RobotName" } Object { - $ObjectID 216 + $ObjectID 218 PromptLocation "left" Name "UrdfFile" } Object { - $ObjectID 217 + $ObjectID 219 PromptLocation "left" Name "ControlledJoints" } Object { - $ObjectID 218 + $ObjectID 220 PromptLocation "left" Name "ControlBoardsNames" } Object { - $ObjectID 219 + $ObjectID 221 PromptLocation "left" Name "LocalName" } Object { - $ObjectID 220 + $ObjectID 222 PromptLocation "left" Name "GravityVector" } @@ -4828,12 +4842,12 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 221 + $ObjectID 223 $ClassName "Simulink.Mask" RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 222 + $ObjectID 224 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -5057,7 +5071,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 223 + $ObjectID 225 $ClassName "Simulink.Mask" Type "DiscreteFilter" Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." @@ -5071,7 +5085,7 @@ Library { Type "Simulink.MaskParameter" Dimension 9 Object { - $ObjectID 224 + $ObjectID 226 Type "popup" Array { Type "Cell" @@ -5096,21 +5110,21 @@ Library { "lterType initStatus p howToCoeffs vis_init;" } Object { - $ObjectID 225 + $ObjectID 227 Type "edit" Name "numCoeffs" Prompt "Numerator Coefficients*" Value "[0]" } Object { - $ObjectID 226 + $ObjectID 228 Type "edit" Name "denCoeffs" Prompt "Denominator Coefficients*" Value "[0]" } Object { - $ObjectID 227 + $ObjectID 229 Type "edit" Name "Fc" Prompt "Cut Frequency (Hz)" @@ -5118,7 +5132,7 @@ Library { Visible "off" } Object { - $ObjectID 228 + $ObjectID 230 Type "edit" Name "Ts" Prompt "Sampling time (s)" @@ -5126,7 +5140,7 @@ Library { Visible "off" } Object { - $ObjectID 229 + $ObjectID 231 Type "edit" Name "orderMedianFilter" Prompt "Order" @@ -5134,7 +5148,7 @@ Library { Visible "off" } Object { - $ObjectID 230 + $ObjectID 232 Type "checkbox" Name "initStatus" Prompt "Define initial conditions" @@ -5146,7 +5160,7 @@ Library { "atus visibilities filterType;" } Object { - $ObjectID 231 + $ObjectID 233 Type "edit" Name "y0" Prompt "Output y0" @@ -5154,7 +5168,7 @@ Library { Visible "off" } Object { - $ObjectID 232 + $ObjectID 234 Type "edit" Name "u0" Prompt "Input u0" @@ -5167,11 +5181,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 233 + $ObjectID 235 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 234 + $ObjectID 236 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5179,68 +5193,68 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 235 + $ObjectID 237 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 8 Object { - $ObjectID 236 + $ObjectID 238 $ClassName "Simulink.dialog.parameter.Popup" Name "filterType" } Object { - $ObjectID 237 + $ObjectID 239 $ClassName "Simulink.dialog.parameter.Edit" Name "numCoeffs" } Object { - $ObjectID 238 + $ObjectID 240 $ClassName "Simulink.dialog.parameter.Edit" Name "denCoeffs" } Object { - $ObjectID 239 + $ObjectID 241 $ClassName "Simulink.dialog.Text" Prompt "* The coefficients are ordered in increasing power of z^-1" Name "howToCoeffs" } Object { - $ObjectID 240 + $ObjectID 242 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "Fc" } Object { - $ObjectID 241 + $ObjectID 243 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "Ts" } Object { - $ObjectID 242 + $ObjectID 244 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "orderMedianFilter" } Object { - $ObjectID 243 + $ObjectID 245 $ClassName "Simulink.dialog.Group" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 244 + $ObjectID 246 $ClassName "Simulink.dialog.parameter.CheckBox" Name "initStatus" } Object { - $ObjectID 245 + $ObjectID 247 $ClassName "Simulink.dialog.parameter.Edit" Name "y0" } Object { - $ObjectID 246 + $ObjectID 248 $ClassName "Simulink.dialog.parameter.Edit" Name "u0" } @@ -5266,7 +5280,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 247 + $ObjectID 249 $ClassName "Simulink.Mask" Type "MatchSignalSizes" Description "It can happen on particular cases that the Simulink engine cannot propagate the right sizes fro" @@ -5386,7 +5400,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 248 + $ObjectID 250 $ClassName "Simulink.Mask" Type "MinimumJerkTrajectoryGenerator" Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " @@ -5410,7 +5424,7 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 249 + $ObjectID 251 Type "checkbox" Name "externalSettlingTime" Prompt "External Settling Time" @@ -5421,21 +5435,21 @@ Library { "SettlingTime" } Object { - $ObjectID 250 + $ObjectID 252 Type "edit" Name "settlingTime" Prompt "Settling Time" Value "3" } Object { - $ObjectID 251 + $ObjectID 253 Type "edit" Name "sampleTime" Prompt "Sample Time" Value "0.01" } Object { - $ObjectID 252 + $ObjectID 254 Type "checkbox" Name "resetOnSettlingTime" Prompt "Reset on Settling Time Changes" @@ -5443,21 +5457,21 @@ Library { Visible "off" } Object { - $ObjectID 253 + $ObjectID 255 Type "checkbox" Name "firstDerivatives" Prompt "Output First Derivative" Value "on" } Object { - $ObjectID 254 + $ObjectID 256 Type "checkbox" Name "secondDerivatives" Prompt "Output Second Derivative" Value "on" } Object { - $ObjectID 255 + $ObjectID 257 Type "checkbox" Name "explicitInitialValue" Prompt "Explicit Initial Value" @@ -5470,11 +5484,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 256 + $ObjectID 258 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 257 + $ObjectID 259 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5482,38 +5496,38 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 258 + $ObjectID 260 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 259 + $ObjectID 261 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 260 + $ObjectID 262 Prompt "Trajectory Parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 4 Object { - $ObjectID 261 + $ObjectID 263 $ClassName "Simulink.dialog.parameter.CheckBox" Name "externalSettlingTime" } Object { - $ObjectID 262 + $ObjectID 264 $ClassName "Simulink.dialog.parameter.Edit" Name "settlingTime" } Object { - $ObjectID 263 + $ObjectID 265 $ClassName "Simulink.dialog.parameter.Edit" Name "sampleTime" } Object { - $ObjectID 264 + $ObjectID 266 $ClassName "Simulink.dialog.parameter.CheckBox" Name "resetOnSettlingTime" } @@ -5522,21 +5536,21 @@ Library { Name "Tab1" } Object { - $ObjectID 265 + $ObjectID 267 Prompt "Input/Output" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 3 Object { - $ObjectID 266 + $ObjectID 268 Name "firstDerivatives" } Object { - $ObjectID 267 + $ObjectID 269 Name "secondDerivatives" } Object { - $ObjectID 268 + $ObjectID 270 Name "explicitInitialValue" } PropName "DialogControls" @@ -5567,7 +5581,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 269 + $ObjectID 271 $ClassName "Simulink.Mask" Type "QP" Description "This block solves a QP problem using the qpOASES library:\n\nx = argmin 0.5 * x' * H * x + c' *" @@ -5584,42 +5598,42 @@ Library { Type "Simulink.MaskParameter" Dimension 6 Object { - $ObjectID 270 + $ObjectID 272 Type "checkbox" Name "lbA" Prompt "Accept constraints lower bound input (lbA)" Value "off" } Object { - $ObjectID 271 + $ObjectID 273 Type "checkbox" Name "ubA" Prompt "Accept constraints upper bound input (ubA)" Value "on" } Object { - $ObjectID 272 + $ObjectID 274 Type "checkbox" Name "lb" Prompt "Accept lower bound input (lb)" Value "off" } Object { - $ObjectID 273 + $ObjectID 275 Type "checkbox" Name "ub" Prompt "Accept upper bound input (ub)" Value "off" } Object { - $ObjectID 274 + $ObjectID 276 Type "checkbox" Name "computeObjVal" Prompt "Output the value of the objective function" Value "off" } Object { - $ObjectID 275 + $ObjectID 277 Type "checkbox" Name "stopIfFails" Prompt "Stop the simulation if the solver fails" @@ -5631,11 +5645,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 276 + $ObjectID 278 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 277 + $ObjectID 279 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5643,23 +5657,23 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 278 + $ObjectID 280 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 279 + $ObjectID 281 Prompt "Constraints bounds" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 2 Object { - $ObjectID 280 + $ObjectID 282 Name "lbA" } Object { - $ObjectID 281 + $ObjectID 283 Name "ubA" } PropName "DialogControls" @@ -5667,17 +5681,17 @@ Library { Name "constraintsBoundsBox" } Object { - $ObjectID 282 + $ObjectID 284 Prompt "Simple bounds" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 2 Object { - $ObjectID 283 + $ObjectID 285 Name "lb" } Object { - $ObjectID 284 + $ObjectID 286 Name "ub" } PropName "DialogControls" @@ -5685,17 +5699,17 @@ Library { Name "simpleBoundsBox" } Object { - $ObjectID 285 + $ObjectID 287 Prompt "Other" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 2 Object { - $ObjectID 286 + $ObjectID 288 Name "computeObjVal" } Object { - $ObjectID 287 + $ObjectID 289 Name "stopIfFails" } PropName "DialogControls" @@ -5727,7 +5741,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 288 + $ObjectID 290 $ClassName "Simulink.Mask" Type "Real Time Synchronizer" Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" @@ -5736,7 +5750,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 289 + $ObjectID 291 $ClassName "Simulink.MaskParameter" Type "edit" Name "period" @@ -5761,7 +5775,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 290 + $ObjectID 292 $ClassName "Simulink.Mask" Type "Simulator Synchronizer" Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" @@ -5772,21 +5786,21 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 291 + $ObjectID 293 Type "edit" Name "period" Prompt "Controller Period (in seconds)" Value "0.01" } Object { - $ObjectID 292 + $ObjectID 294 Type "edit" Name "serverPortName" Prompt "Server Port Name" Value "'/clock/rpc'" } Object { - $ObjectID 293 + $ObjectID 295 Type "edit" Name "clientPortName" Prompt "Client Port Name" @@ -5807,12 +5821,12 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 294 + $ObjectID 296 $ClassName "Simulink.Mask" RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 295 + $ObjectID 297 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -6036,7 +6050,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 296 + $ObjectID 298 $ClassName "Simulink.Mask" Type "YARP Clock" Description "This block outputs the current YARP Time.\nIn a nutshell, this block outputs the equivalent of " @@ -6061,7 +6075,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 297 + $ObjectID 299 $ClassName "Simulink.Mask" Type "YARP Read" Description "This block behaves as the command 'yarp read'. \n\nBy default it has the option 'Autoconnect' a" @@ -6081,21 +6095,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 298 + $ObjectID 300 Type "edit" Name "portName" Prompt "Source Port Name" Value "'/portname'" } Object { - $ObjectID 299 + $ObjectID 301 Type "edit" Name "signalSize" Prompt "Port Size" Value "1" } Object { - $ObjectID 300 + $ObjectID 302 Type "edit" Name "timeout" Prompt "Timeout" @@ -6103,7 +6117,7 @@ Library { Visible "off" } Object { - $ObjectID 301 + $ObjectID 303 Type "checkbox" Name "blocking" Prompt "Blocking Read" @@ -6114,14 +6128,14 @@ Library { "king_val mask_visibility" } Object { - $ObjectID 302 + $ObjectID 304 Type "checkbox" Name "timestamp" Prompt "Read Timestamp" Value "on" } Object { - $ObjectID 303 + $ObjectID 305 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -6134,7 +6148,7 @@ Library { "ibilities" } Object { - $ObjectID 304 + $ObjectID 306 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -6159,7 +6173,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 305 + $ObjectID 307 $ClassName "Simulink.Mask" Type "YARP Write" Description "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as" @@ -6173,14 +6187,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 306 + $ObjectID 308 Type "edit" Name "portName" Prompt "Opened Port Name" Value "'/portname'" } Object { - $ObjectID 307 + $ObjectID 309 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -6191,7 +6205,7 @@ Library { "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" } Object { - $ObjectID 308 + $ObjectID 310 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -6213,7 +6227,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 309 + $ObjectID 311 $ClassName "Simulink.Mask" Type "Errors" Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" @@ -6362,7 +6376,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 310 + $ObjectID 312 $ClassName "Simulink.Mask" Type "Holder" Description "This block holds the first input value during the simulation." diff --git a/matlab/library/exported/WBToolboxLibrary.slx b/matlab/library/exported/WBToolboxLibrary.slx index b93229585..d1e983841 100644 Binary files a/matlab/library/exported/WBToolboxLibrary.slx and b/matlab/library/exported/WBToolboxLibrary.slx differ diff --git a/toolbox/library/CMakeLists.txt b/toolbox/library/CMakeLists.txt index 82d52d1c5..6869ca360 100644 --- a/toolbox/library/CMakeLists.txt +++ b/toolbox/library/CMakeLists.txt @@ -38,7 +38,7 @@ list(APPEND WBT_BLOCKS "Jacobian") list(APPEND WBT_BLOCKS "ForwardKinematics") list(APPEND WBT_BLOCKS "RelativeTransform") list(APPEND WBT_BLOCKS "CentroidalMomentum") -list(APPEND WBT_BLOCKS "CMM") +list(APPEND WBT_BLOCKS "CentroidalTotalMomentumMatrix") # Other list(APPEND WBT_BLOCKS "RealTimeSynchronizer") diff --git a/toolbox/library/include/WBToolbox/Block/CMM.h b/toolbox/library/include/WBToolbox/Block/CentroidalTotalMomentumMatrix.h similarity index 70% rename from toolbox/library/include/WBToolbox/Block/CMM.h rename to toolbox/library/include/WBToolbox/Block/CentroidalTotalMomentumMatrix.h index 731274115..03f9f2e86 100644 --- a/toolbox/library/include/WBToolbox/Block/CMM.h +++ b/toolbox/library/include/WBToolbox/Block/CentroidalTotalMomentumMatrix.h @@ -6,8 +6,8 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#ifndef WBT_CMM_H -#define WBT_CMM_H +#ifndef WBT_CentroidalTotalMomentumMatrix_H +#define WBT_CentroidalTotalMomentumMatrix_H #include "WBToolbox/Base/WBBlock.h" @@ -16,7 +16,7 @@ namespace wbt { namespace block { - class CMM; + class CentroidalTotalMomentumMatrix; } // namespace block } // namespace wbt @@ -27,22 +27,22 @@ namespace blockfactory { } // namespace blockfactory /** - * @brief The wbt::CMM class + * @brief The wbt::CentroidalTotalMomentumMatrix class */ -class wbt::block::CMM final : public wbt::base::WBBlock +class wbt::block::CentroidalTotalMomentumMatrix final : public wbt::base::WBBlock { private: class impl; std::unique_ptr pImpl; public: - CMM(); - ~CMM() override; + CentroidalTotalMomentumMatrix(); + ~CentroidalTotalMomentumMatrix() override; bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; bool initialize(blockfactory::core::BlockInformation* blockInfo) override; bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; -#endif // WBT_CMM_H +#endif // WBT_CentroidalTotalMomentumMatrix_H diff --git a/toolbox/library/src/CMM.cpp b/toolbox/library/src/CentroidalTotalMomentumMatrix.cpp similarity index 83% rename from toolbox/library/src/CMM.cpp rename to toolbox/library/src/CentroidalTotalMomentumMatrix.cpp index 1e9d1bba7..6770c9542 100644 --- a/toolbox/library/src/CMM.cpp +++ b/toolbox/library/src/CentroidalTotalMomentumMatrix.cpp @@ -6,7 +6,7 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "WBToolbox/Block/CMM.h" +#include "WBToolbox/Block/CentroidalTotalMomentumMatrix.h" #include "WBToolbox/Base/Configuration.h" #include "WBToolbox/Base/RobotInterface.h" @@ -40,13 +40,13 @@ enum InputIndex enum OutputIndex { - CMM = 0, + CentroidalTotalMomentumMatrix = 0, }; // BLOCK PIMPL // =========== -class CMM::impl +class CentroidalTotalMomentumMatrix::impl { public: iDynTree::MatrixDynSize CentroidalTotalMomentumMatrix; @@ -55,15 +55,13 @@ class CMM::impl // BLOCK CLASS // =========== -CMM::CMM() +CentroidalTotalMomentumMatrix::CentroidalTotalMomentumMatrix() : pImpl{new impl()} {} -CMM::~CMM() = default; +CentroidalTotalMomentumMatrix::~CentroidalTotalMomentumMatrix() = default; - - -bool CMM::configureSizeAndPorts(BlockInformation* blockInfo) +bool CentroidalTotalMomentumMatrix::configureSizeAndPorts(BlockInformation* blockInfo) { if (!WBBlock::configureSizeAndPorts(blockInfo)) { return false; @@ -92,7 +90,7 @@ bool CMM::configureSizeAndPorts(BlockInformation* blockInfo) }, { // Outputs - {OutputIndex::CMM, Port::Dimensions{6, 6 + dofs}, Port::DataType::DOUBLE}, + {OutputIndex::CentroidalTotalMomentumMatrix, Port::Dimensions{6, 6 + dofs}, Port::DataType::DOUBLE}, }); if (!ok) { @@ -103,22 +101,18 @@ bool CMM::configureSizeAndPorts(BlockInformation* blockInfo) return true; } - -bool CMM::initialize(BlockInformation* blockInfo) +bool CentroidalTotalMomentumMatrix::initialize(BlockInformation* blockInfo) { if (!WBBlock::initialize(blockInfo)) { return false; } - - auto kinDyn = getKinDynComputations(); if (!kinDyn) { bfError << "Cannot retrieve handle to KinDynComputations."; return false; } - // Initialize buffers // ------------------ @@ -132,12 +126,12 @@ bool CMM::initialize(BlockInformation* blockInfo) return true; } -bool CMM::terminate(const BlockInformation* blockInfo) +bool CentroidalTotalMomentumMatrix::terminate(const BlockInformation* blockInfo) { return WBBlock::terminate(blockInfo); } -bool CMM::output(const BlockInformation* blockInfo) +bool CentroidalTotalMomentumMatrix::output(const BlockInformation* blockInfo) { using namespace Eigen; using MatrixXdSimulink = Matrix; @@ -170,7 +164,6 @@ bool CMM::output(const BlockInformation* blockInfo) // OUTPUT // ====== - // Compute the CentroidalTotalMomentumMatrix ok = kinDyn->getCentroidalTotalMomentumJacobian(pImpl->CentroidalTotalMomentumMatrix); @@ -182,7 +175,7 @@ bool CMM::output(const BlockInformation* blockInfo) // Get the output signal memory location - OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::CMM); + OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::CentroidalTotalMomentumMatrix); if (!output) { bfError << "Output signal not valid."; @@ -193,8 +186,8 @@ bool CMM::output(const BlockInformation* blockInfo) Map CentroidalTotalMomentumMatrixRowMajor = toEigen(pImpl->CentroidalTotalMomentumMatrix); Map CentroidalTotalMomentumMatrixColMajor( output->getBuffer(), - blockInfo->getOutputPortMatrixSize(OutputIndex::CMM).rows, - blockInfo->getOutputPortMatrixSize(OutputIndex::CMM).cols); + blockInfo->getOutputPortMatrixSize(OutputIndex::CentroidalTotalMomentumMatrix).rows, + blockInfo->getOutputPortMatrixSize(OutputIndex::CentroidalTotalMomentumMatrix).cols); // Forward the buffer to Simulink transforming it to ColMajor CentroidalTotalMomentumMatrixColMajor = CentroidalTotalMomentumMatrixRowMajor; diff --git a/toolbox/library/src/Factory.cpp b/toolbox/library/src/Factory.cpp index 9b2bbd840..cc581bc06 100644 --- a/toolbox/library/src/Factory.cpp +++ b/toolbox/library/src/Factory.cpp @@ -26,7 +26,7 @@ #include "WBToolbox/Block/Jacobian.h" #include "WBToolbox/Block/MassMatrix.h" #include "WBToolbox/Block/RelativeTransform.h" -#include "WBToolbox/Block/CMM.h" +#include "WBToolbox/Block/CentroidalTotalMomentumMatrix.h" // iCub-dependent blocks #ifdef WBT_USES_ICUB @@ -82,7 +82,7 @@ SHLIBPP_DEFINE_SHARED_SUBCLASS(MassMatrix, wbt::block::MassMatrix, blockfactory: SHLIBPP_DEFINE_SHARED_SUBCLASS(RelativeTransform, wbt::block::RelativeTransform, blockfactory::core::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(CMM, wbt::block::CMM, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(CentroidalTotalMomentumMatrix, wbt::block::CentroidalTotalMomentumMatrix, blockfactory::core::Block) // iCub-dependent blocks #ifdef WBT_USES_ICUB