diff --git a/toolbox/WBToolboxLibrary.mdl b/toolbox/WBToolboxLibrary.mdl
index 9d0e733f1..4df715224 100644
--- a/toolbox/WBToolboxLibrary.mdl
+++ b/toolbox/WBToolboxLibrary.mdl
@@ -8,15 +8,15 @@ Library {
   DisableAllScopes	  off
   FPTRunName		  "Run 1"
   MaxMDLFileLineLength	  120
-  Created		  "Thu Feb 06 02:21:39 2014"
+  Created		  "Thu Feb 06 01:21:39 2014"
   Creator		  "jorhabib"
   UpdateHistory		  "UpdateHistoryNever"
   ModifiedByFormat	  "%<Auto>"
   LastModifiedBy	  "icub"
   ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Fri Aug 11 07:02:44 2017"
-  RTWModifiedTimeStamp	  424335764
-  ModelVersionFormat	  "1.%<AutoIncrement:383>"
+  LastModifiedDate	  "Fri Aug 11 12:52:09 2017"
+  RTWModifiedTimeStamp	  424356729
+  ModelVersionFormat	  "1.%<AutoIncrement:395>"
   ConfigurationManager	  "none"
   SampleTimeColors	  off
   SampleTimeAnnotations	  off
@@ -755,7 +755,7 @@ Library {
   }
   System {
     Name		    "WBToolboxLibrary"
-    Location		    [173, 232, 1453, 953]
+    Location		    [123, 132, 1403, 853]
     Open		    off
     ModelBrowserVisibility  off
     ModelBrowserWidth	    200
@@ -789,7 +789,7 @@ Library {
       MaskIconUnits	      "autoscale"
       System {
 	Name			"Utilities"
-	Location		[173, 232, 1453, 953]
+	Location		[447, 302, 1727, 1023]
 	Open			off
 	ModelBrowserVisibility	off
 	ModelBrowserWidth	200
@@ -801,7 +801,7 @@ Library {
 	TiledPaperMargins	[0.500000, 0.500000, 0.500000, 0.500000]
 	TiledPageScale		1
 	ShowPageBoundaries	off
-	ZoomFactor		"166"
+	ZoomFactor		"175"
 	Block {
 	  BlockType		  SubSystem
 	  Name			  "DampPinv"
@@ -917,20 +917,20 @@ Library {
 		Block {
 		  BlockType		  Demux
 		  Name			  " Demux "
-		  SID			  "107::1618"
+		  SID			  "107::1627"
 		  Ports			  [1, 1]
 		  Position		  [270, 230, 320, 270]
-		  ZOrder		  98
+		  ZOrder		  107
 		  Outputs		  "1"
 		}
 		Block {
 		  BlockType		  S-Function
 		  Name			  " SFunction "
-		  SID			  "107::1617"
+		  SID			  "107::1626"
 		  Tag			  "Stateflow S-Function WBToolboxLibrary 6"
 		  Ports			  [2, 2]
 		  Position		  [180, 100, 230, 160]
-		  ZOrder		  97
+		  ZOrder		  106
 		  FunctionName		  "sf_sfun"
 		  PortCounts		  "[2 2]"
 		  SFunctionDeploymentMode off
@@ -945,9 +945,9 @@ Library {
 		Block {
 		  BlockType		  Terminator
 		  Name			  " Terminator "
-		  SID			  "107::1619"
+		  SID			  "107::1628"
 		  Position		  [460, 241, 480, 259]
-		  ZOrder		  99
+		  ZOrder		  108
 		}
 		Block {
 		  BlockType		  Outport
@@ -1033,29 +1033,58 @@ Library {
 	  MaskHideContents	  off
 	}
 	Block {
-	  BlockType		  Reference
+	  BlockType		  S-Function
 	  Name			  "DiscreteFilter"
 	  SID			  "1772"
 	  Ports			  [1, 1]
-	  Position		  [440, 260, 500, 290]
-	  ZOrder		  83
+	  Position		  [400, 245, 460, 275]
+	  ZOrder		  82
 	  BackgroundColor	  "yellow"
-	  LibraryVersion	  "1.386"
-	  SourceBlock		  "WBToolboxLibrary/Utilities/DiscreteFilter"
-	  SourceType		  "Discrete Filter"
-	  filterType		  "Generic"
-	  numCoeffs		  "[0]"
-	  denCoeffs		  "[0]"
-	  Fc			  "0"
-	  Ts			  "0"
-	  orderMedianFilter	  "0"
+	  FunctionName		  "WBToolbox"
+	  Parameters		  "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0"
+	  SFunctionDeploymentMode off
+	  EnableBusSupport	  off
+	  MaskType		  "Discrete Filter"
+	  MaskDescription	  "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl."
+	  MaskPromptString	  "Type of the filter|Numerator Coefficients*|Denominator Coefficients*|Cut Frequency (Hz)|Samplin"
+	  "g time (s)|Order|Define initial conditions|Output y0|Input u0"
+	  MaskStyleString	  "popup(Generic|FirstOrderLowPassFilter|MedianFilter),edit,edit,edit,edit,edit,checkbox,edit,edit"
+	  MaskVariables		  "filterType=@1;numCoeffs=@2;denCoeffs=@3;Fc=@4;Ts=@5;orderMedianFilter=@6;initStatus=@7;y0=@8;u0=@"
+	  "9;"
+	  MaskTunableValueString  "on,on,on,on,on,on,on,on,on"
+	  MaskCallbackString	  "% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = g"
+	  "et_param(gcb, 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs"
+	  " = p.getDialogControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n    vi"
+	  "s_init = 'on';\nelse\n    vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n    set_param(gcb, 'MaskVisi"
+	  "bilities',{'on','on','on','off','off','off','on',vis_init,vis_init});\n    howToCoeffs.Visible = 'on';\nelseif(strc"
+	  "mp(filterType, 'FirstOrderLowPassFilter'))\n    set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off'"
+	  ",'on',vis_init,'off'});\n    howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n    set_para"
+	  "m(gcb, 'MaskVisibilities',{'on','off','off','off','off','on','on',vis_init,'off'});\n    howToCoeffs.Visible = 'off"
+	  "';\nend\n\nclear filterType initStatus p howToCoeffs vis_init;||||||initStatus   = get_param(gcb, 'initStatus');\nv"
+	  "isibilities = get_param(gcb, 'MaskVisibilities');\nfilterType   = get_param(gcb, 'filterType');\n\nif (strcmp(initS"
+	  "tatus,'off'))\n    visibilities{8} = 'off';\n    visibilities{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n    vi"
+	  "sibilities{8} = 'on';\n    if (strcmp(filterType,'Generic'))\n        visibilities{9} = 'on';\n    end\nend\n\nset_"
+	  "param(gcb, 'MaskVisibilities', visibilities);\n\nclear initStatus visibilities filterType;||"
+	  MaskEnableString	  "on,on,on,on,on,on,on,on,on"
+	  MaskVisibilityString	  "on,on,on,off,off,off,on,off,off"
+	  MaskToolTipString	  "on,on,on,on,on,on,on,on,on"
+	  MaskInitialization	  "filterType = get_param(gcb, 'filterType');\nnumCoeffs  = mat2str(numCoeffs);\ndenCoeffs  = ma"
+	  "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n    y0 = mat2str(y"
+	  "0);\n    u0 = mat2str(u0);\nelse\n    y0 = 'none';\n    u0 = 'none';\nend\n\nclear initStatus"
+	  MaskDisplay		  "disp('Filter')"
+	  MaskIconFrame		  on
+	  MaskIconOpaque	  on
+	  MaskIconRotate	  "none"
+	  MaskPortRotate	  "default"
+	  MaskIconUnits		  "autoscale"
+	  MaskValueString	  "Generic|[0]|[0]|0|0|0|off|[0]|[0]"
 	}
 	Block {
 	  BlockType		  S-Function
 	  Name			  "DoFs converter"
 	  SID			  "1771"
 	  Ports			  [1, 5]
-	  Position		  [240, 241, 385, 309]
+	  Position		  [160, 226, 305, 294]
 	  ZOrder		  81
 	  InitFcn		  "if ~exist('WBT_robotName','var')\n    WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n    W"
 	  "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n    WBT_wbiFilename = 'yarpWholeBodyInterf"
@@ -1124,7 +1153,7 @@ Library {
 	  "else\n    visibility{2} = 'on';\n    visibility{4} = 'off';\nend\nset_param(gcb, 'MaskVisibilities',visibility);\nc"
 	  "lear externalSettlingTime||||||"
 	  MaskEnableString	  "on,on,on,on,on,on,on"
-	  MaskVisibilityString	  "on,on,on,on,on,on,off"
+	  MaskVisibilityString	  "on,on,on,off,on,on,off"
 	  MaskToolTipString	  "on,on,on,on,on,on,on"
 	  MaskDisplay		  "firstDer = get_param(gcb, 'firstDerivatives');\nsecondDer = get_param(gcb, 'secondDerivatives');\ni"
 	  "nitialValues = get_param(gcb, 'explicitInitialValue');\nexternalSettlingTimeParam = get_param(gcb, 'externalSettlin"
@@ -1322,20 +1351,20 @@ Library {
 		Block {
 		  BlockType		  Demux
 		  Name			  " Demux "
-		  SID			  "112::1609"
+		  SID			  "112::1618"
 		  Ports			  [1, 1]
 		  Position		  [270, 230, 320, 270]
-		  ZOrder		  98
+		  ZOrder		  107
 		  Outputs		  "1"
 		}
 		Block {
 		  BlockType		  S-Function
 		  Name			  " SFunction "
-		  SID			  "112::1608"
+		  SID			  "112::1617"
 		  Tag			  "Stateflow S-Function WBToolboxLibrary 7"
 		  Ports			  [2, 2]
 		  Position		  [180, 100, 230, 160]
-		  ZOrder		  97
+		  ZOrder		  106
 		  FunctionName		  "sf_sfun"
 		  PortCounts		  "[2 2]"
 		  SFunctionDeploymentMode off
@@ -1350,9 +1379,9 @@ Library {
 		Block {
 		  BlockType		  Terminator
 		  Name			  " Terminator "
-		  SID			  "112::1610"
+		  SID			  "112::1619"
 		  Position		  [460, 241, 480, 259]
-		  ZOrder		  99
+		  ZOrder		  108
 		}
 		Block {
 		  BlockType		  Outport
@@ -1780,20 +1809,20 @@ Library {
 		Block {
 		  BlockType		  Demux
 		  Name			  " Demux "
-		  SID			  "1300::1609"
+		  SID			  "1300::1618"
 		  Ports			  [1, 1]
 		  Position		  [270, 230, 320, 270]
-		  ZOrder		  86
+		  ZOrder		  95
 		  Outputs		  "1"
 		}
 		Block {
 		  BlockType		  S-Function
 		  Name			  " SFunction "
-		  SID			  "1300::1608"
+		  SID			  "1300::1617"
 		  Tag			  "Stateflow S-Function WBToolboxLibrary 1"
 		  Ports			  [2, 2]
 		  Position		  [180, 100, 230, 160]
-		  ZOrder		  85
+		  ZOrder		  94
 		  FunctionName		  "sf_sfun"
 		  PortCounts		  "[2 2]"
 		  SFunctionDeploymentMode off
@@ -1808,9 +1837,9 @@ Library {
 		Block {
 		  BlockType		  Terminator
 		  Name			  " Terminator "
-		  SID			  "1300::1610"
+		  SID			  "1300::1619"
 		  Position		  [460, 241, 480, 259]
-		  ZOrder		  87
+		  ZOrder		  96
 		}
 		Block {
 		  BlockType		  Outport
diff --git a/toolbox/WBToolboxLibrary.slx b/toolbox/WBToolboxLibrary.slx
index 63ee86486..9264c236b 100644
Binary files a/toolbox/WBToolboxLibrary.slx and b/toolbox/WBToolboxLibrary.slx differ
diff --git a/toolbox/WBToolboxLibrary_repository.mdl b/toolbox/WBToolboxLibrary_repository.mdl
index 5bc9551fe..b5034389e 100644
--- a/toolbox/WBToolboxLibrary_repository.mdl
+++ b/toolbox/WBToolboxLibrary_repository.mdl
@@ -23,7 +23,7 @@ Library {
       $ObjectID		      2
       $ClassName	      "Simulink.WindowInfo"
       IsActive		      [1]
-      Location		      [173.0, 232.0, 1280.0, 721.0]
+      Location		      [447.0, 302.0, 1280.0, 721.0]
       Object {
 	$PropName		"ModelBrowserInfo"
 	$ObjectID		3
@@ -48,8 +48,8 @@ Library {
 	ViewObjType		"SimulinkSubsys"
 	LoadSaveID		"192"
 	Extents			[1226.0, 537.0]
-	ZoomFactor		[1.66]
-	Offset			[0.0, 0.0]
+	ZoomFactor		[1.75]
+	Offset			[10.634251290877842, 6.0]
       }
       Object {
 	$PropName		"DockComponentsInfo"
@@ -76,15 +76,15 @@ Library {
       "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA"
     }
   }
-  Created		  "Thu Feb 06 02:21:39 2014"
+  Created		  "Thu Feb 06 01:21:39 2014"
   Creator		  "jorhabib"
   UpdateHistory		  "UpdateHistoryNever"
   ModifiedByFormat	  "%<Auto>"
   LastModifiedBy	  "icub"
   ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Fri Aug 11 06:58:36 2017"
-  RTWModifiedTimeStamp	  424335513
-  ModelVersionFormat	  "1.%<AutoIncrement:382>"
+  LastModifiedDate	  "Fri Aug 11 12:50:36 2017"
+  RTWModifiedTimeStamp	  424356590
+  ModelVersionFormat	  "1.%<AutoIncrement:394>"
   ConfigurationManager	  "none"
   SampleTimeColors	  off
   SampleTimeAnnotations	  off
@@ -1031,7 +1031,7 @@ Library {
   }
   System {
     Name		    "WBToolboxLibrary_repository"
-    Location		    [173, 232, 1453, 953]
+    Location		    [123, 132, 1403, 853]
     Open		    off
     ModelBrowserVisibility  off
     ModelBrowserWidth	    200
@@ -1065,7 +1065,7 @@ Library {
       }
       System {
 	Name			"Utilities"
-	Location		[173, 232, 1453, 953]
+	Location		[447, 302, 1727, 1023]
 	Open			on
 	ModelBrowserVisibility	off
 	ModelBrowserWidth	200
@@ -1077,7 +1077,7 @@ Library {
 	TiledPaperMargins	[0.500000, 0.500000, 0.500000, 0.500000]
 	TiledPageScale		1
 	ShowPageBoundaries	off
-	ZoomFactor		"166"
+	ZoomFactor		"175"
 	Block {
 	  BlockType		  SubSystem
 	  Name			  "DampPinv"
@@ -1163,7 +1163,7 @@ Library {
 		TiledPageScale		1
 		ShowPageBoundaries	off
 		ZoomFactor		"100"
-		SIDHighWatermark	"1619"
+		SIDHighWatermark	"1628"
 		Block {
 		  BlockType		  Inport
 		  Name			  "mat"
@@ -1184,20 +1184,20 @@ Library {
 		Block {
 		  BlockType		  Demux
 		  Name			  " Demux "
-		  SID			  "107::1618"
+		  SID			  "107::1627"
 		  Ports			  [1, 1]
 		  Position		  [270, 230, 320, 270]
-		  ZOrder		  98
+		  ZOrder		  107
 		  Outputs		  "1"
 		}
 		Block {
 		  BlockType		  S-Function
 		  Name			  " SFunction "
-		  SID			  "107::1617"
+		  SID			  "107::1626"
 		  Tag			  "Stateflow S-Function WBToolboxLibrary_repository 6"
 		  Ports			  [2, 2]
 		  Position		  [180, 100, 230, 160]
-		  ZOrder		  97
+		  ZOrder		  106
 		  FunctionName		  "sf_sfun"
 		  PortCounts		  "[2 2]"
 		  SFunctionDeploymentMode off
@@ -1213,9 +1213,9 @@ Library {
 		Block {
 		  BlockType		  Terminator
 		  Name			  " Terminator "
-		  SID			  "107::1619"
+		  SID			  "107::1628"
 		  Position		  [460, 241, 480, 259]
-		  ZOrder		  99
+		  ZOrder		  108
 		}
 		Block {
 		  BlockType		  Outport
@@ -1226,7 +1226,7 @@ Library {
 		  IconDisplay		  "Port number"
 		}
 		Line {
-		  ZOrder		  61
+		  ZOrder		  76
 		  SrcBlock		  "mat"
 		  SrcPort		  1
 		  Points		  [120, 0]
@@ -1234,7 +1234,7 @@ Library {
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  62
+		  ZOrder		  77
 		  SrcBlock		  "sigma"
 		  SrcPort		  1
 		  DstBlock		  " SFunction "
@@ -1242,7 +1242,7 @@ Library {
 		}
 		Line {
 		  Name			  "DPinv"
-		  ZOrder		  63
+		  ZOrder		  78
 		  Labels		  [0, 0]
 		  SrcBlock		  " SFunction "
 		  SrcPort		  2
@@ -1250,14 +1250,14 @@ Library {
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  64
+		  ZOrder		  79
 		  SrcBlock		  " Demux "
 		  SrcPort		  1
 		  DstBlock		  " Terminator "
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  65
+		  ZOrder		  80
 		  SrcBlock		  " SFunction "
 		  SrcPort		  1
 		  DstBlock		  " Demux "
@@ -1297,29 +1297,222 @@ Library {
 	  }
 	}
 	Block {
-	  BlockType		  Reference
+	  BlockType		  S-Function
 	  Name			  "DiscreteFilter"
 	  SID			  "1772"
 	  Ports			  [1, 1]
-	  Position		  [440, 260, 500, 290]
-	  ZOrder		  83
+	  Position		  [400, 245, 460, 275]
+	  ZOrder		  82
 	  BackgroundColor	  "yellow"
-	  LibraryVersion	  "1.386"
-	  SourceBlock		  "WBToolboxLibrary/Utilities/DiscreteFilter"
-	  SourceType		  "Discrete Filter"
-	  filterType		  "Generic"
-	  numCoeffs		  "[0]"
-	  denCoeffs		  "[0]"
-	  Fc			  "0"
-	  Ts			  "0"
-	  orderMedianFilter	  "0"
+	  FunctionName		  "WBToolbox"
+	  Parameters		  "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0"
+	  SFunctionDeploymentMode off
+	  EnableBusSupport	  off
+	  SFcnIsStateOwnerBlock	  off
+	  Object {
+	    $PropName		    "MaskObject"
+	    $ObjectID		    23
+	    $ClassName		    "Simulink.Mask"
+	    Type		    "Discrete Filter"
+	    Description		    "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl."
+	    Initialization	    "filterType = get_param(gcb, 'filterType');\nnumCoeffs  = mat2str(numCoeffs);\ndenCoeffs  = ma"
+	    "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n    y0 = mat2str"
+	    "(y0);\n    u0 = mat2str(u0);\nelse\n    y0 = 'none';\n    u0 = 'none';\nend\n\nclear initStatus"
+	    Display		    "disp('Filter')"
+	    Array {
+	      Type		      "Simulink.MaskParameter"
+	      Dimension		      9
+	      Object {
+		$ObjectID		24
+		Type			"popup"
+		Array {
+		  Type			  "Cell"
+		  Dimension		  3
+		  Cell			  "Generic"
+		  Cell			  "FirstOrderLowPassFilter"
+		  Cell			  "MedianFilter"
+		  PropName		  "TypeOptions"
+		}
+		Name			"filterType"
+		Prompt			"Type of the filter"
+		Value			"Generic"
+		Callback		"% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = get_param(gcb"
+		", 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs = p.getDialo"
+		"gControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n    vis_init = 'on';"
+		"\nelse\n    vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n    set_param(gcb, 'MaskVisibilities',{'on'"
+		",'on','on','off','off','off','on',vis_init,vis_init});\n    howToCoeffs.Visible = 'on';\nelseif(strcmp(filterType, '"
+		"FirstOrderLowPassFilter'))\n    set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off','on',vis_init,'o"
+		"ff'});\n    howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n    set_param(gcb, 'MaskVisibi"
+		"lities',{'on','off','off','off','off','on','on',vis_init,'off'});\n    howToCoeffs.Visible = 'off';\nend\n\nclear fi"
+		"lterType initStatus p howToCoeffs vis_init;"
+	      }
+	      Object {
+		$ObjectID		25
+		Type			"edit"
+		Name			"numCoeffs"
+		Prompt			"Numerator Coefficients*"
+		Value			"[0]"
+	      }
+	      Object {
+		$ObjectID		26
+		Type			"edit"
+		Name			"denCoeffs"
+		Prompt			"Denominator Coefficients*"
+		Value			"[0]"
+	      }
+	      Object {
+		$ObjectID		27
+		Type			"edit"
+		Name			"Fc"
+		Prompt			"Cut Frequency (Hz)"
+		Value			"0"
+		Visible			"off"
+	      }
+	      Object {
+		$ObjectID		28
+		Type			"edit"
+		Name			"Ts"
+		Prompt			"Sampling time (s)"
+		Value			"0"
+		Visible			"off"
+	      }
+	      Object {
+		$ObjectID		29
+		Type			"edit"
+		Name			"orderMedianFilter"
+		Prompt			"Order"
+		Value			"0"
+		Visible			"off"
+	      }
+	      Object {
+		$ObjectID		30
+		Type			"checkbox"
+		Name			"initStatus"
+		Prompt			"Define initial conditions"
+		Value			"off"
+		Callback		"initStatus   = get_param(gcb, 'initStatus');\nvisibilities = get_param(gcb, 'MaskVisibilities');\nfilterT"
+		"ype   = get_param(gcb, 'filterType');\n\nif (strcmp(initStatus,'off'))\n    visibilities{8} = 'off';\n    visibiliti"
+		"es{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n    visibilities{8} = 'on';\n    if (strcmp(filterType,'Generic'))"
+		"\n        visibilities{9} = 'on';\n    end\nend\n\nset_param(gcb, 'MaskVisibilities', visibilities);\n\nclear initSt"
+		"atus visibilities filterType;"
+	      }
+	      Object {
+		$ObjectID		31
+		Type			"edit"
+		Name			"y0"
+		Prompt			"Output y0"
+		Value			"[0]"
+		Visible			"off"
+	      }
+	      Object {
+		$ObjectID		32
+		Type			"edit"
+		Name			"u0"
+		Prompt			"Input u0"
+		Value			"[0]"
+		Visible			"off"
+	      }
+	      PropName		      "Parameters"
+	    }
+	    Array {
+	      Type		      "Simulink.dialog.Group"
+	      Dimension		      2
+	      Object {
+		$ObjectID		33
+		Prompt			"%<MaskType>"
+		Object {
+		  $PropName		  "DialogControls"
+		  $ObjectID		  34
+		  $ClassName		  "Simulink.dialog.Text"
+		  Prompt		  "%<MaskDescription>"
+		  Name			  "DescTextVar"
+		}
+		Name			"DescGroupVar"
+	      }
+	      Object {
+		$ObjectID		35
+		Prompt			"Simulink:studio:ToolBarParametersMenu"
+		Array {
+		  Type			  "Simulink.dialog.Control"
+		  Dimension		  8
+		  Object {
+		    $ObjectID		    36
+		    $ClassName		    "Simulink.dialog.parameter.Popup"
+		    Name		    "filterType"
+		  }
+		  Object {
+		    $ObjectID		    37
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    Name		    "numCoeffs"
+		  }
+		  Object {
+		    $ObjectID		    38
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    Name		    "denCoeffs"
+		  }
+		  Object {
+		    $ObjectID		    39
+		    $ClassName		    "Simulink.dialog.Text"
+		    Prompt		    "* The coefficients are ordered in increasing power of z^-1"
+		    Name		    "howToCoeffs"
+		  }
+		  Object {
+		    $ObjectID		    40
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    PromptLocation	    "left"
+		    Name		    "Fc"
+		  }
+		  Object {
+		    $ObjectID		    41
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    PromptLocation	    "left"
+		    Name		    "Ts"
+		  }
+		  Object {
+		    $ObjectID		    42
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    PromptLocation	    "left"
+		    Name		    "orderMedianFilter"
+		  }
+		  Object {
+		    $ObjectID		    43
+		    $ClassName		    "Simulink.dialog.Group"
+		    Array {
+		    Type		    "Simulink.dialog.parameter.Control"
+		    Dimension		    3
+		    Object {
+		    $ObjectID		    44
+		    $ClassName		    "Simulink.dialog.parameter.CheckBox"
+		    Name		    "initStatus"
+		    }
+		    Object {
+		    $ObjectID		    45
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    Name		    "y0"
+		    }
+		    Object {
+		    $ObjectID		    46
+		    $ClassName		    "Simulink.dialog.parameter.Edit"
+		    Name		    "u0"
+		    }
+		    PropName		    "DialogControls"
+		    }
+		    Name		    "Container3"
+		  }
+		  PropName		  "DialogControls"
+		}
+		Name			"ParameterGroupVar"
+	      }
+	      PropName		      "DialogControls"
+	    }
+	  }
 	}
 	Block {
 	  BlockType		  S-Function
 	  Name			  "DoFs converter"
 	  SID			  "1771"
 	  Ports			  [1, 5]
-	  Position		  [240, 241, 385, 309]
+	  Position		  [160, 226, 305, 294]
 	  ZOrder		  81
 	  InitFcn		  "if ~exist('WBT_robotName','var')\n    WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n    W"
 	  "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n    WBT_wbiFilename = 'yarpWholeBodyInterf"
@@ -1331,7 +1524,7 @@ Library {
 	  SFcnIsStateOwnerBlock	  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    23
+	    $ObjectID		    47
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "DoFs converter"
 	    Description		    "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on"
@@ -1350,7 +1543,7 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      5
 	      Object {
-		$ObjectID		24
+		$ObjectID		48
 		Type			"popup"
 		Array {
 		  Type			  "Cell"
@@ -1364,28 +1557,28 @@ Library {
 		Value			"From YARP to WBI"
 	      }
 	      Object {
-		$ObjectID		25
+		$ObjectID		49
 		Type			"edit"
 		Name			"robotName"
 		Prompt			"Robot Port Name"
 		Value			"WBT_robotName"
 	      }
 	      Object {
-		$ObjectID		26
+		$ObjectID		50
 		Type			"edit"
 		Name			"localName"
 		Prompt			"Model Name"
 		Value			"WBT_modelName"
 	      }
 	      Object {
-		$ObjectID		27
+		$ObjectID		51
 		Type			"edit"
 		Name			"wbiFile"
 		Prompt			"WBI filename"
 		Value			"WBT_wbiFilename"
 	      }
 	      Object {
-		$ObjectID		28
+		$ObjectID		52
 		Type			"edit"
 		Name			"wbiList"
 		Prompt			"WBI list name"
@@ -1397,11 +1590,11 @@ Library {
 	      Type		      "Simulink.dialog.Group"
 	      Dimension		      2
 	      Object {
-		$ObjectID		29
+		$ObjectID		53
 		Prompt			"%<MaskType>"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  30
+		  $ObjectID		  54
 		  $ClassName		  "Simulink.dialog.Text"
 		  Prompt		  "%<MaskDescription>"
 		  Name			  "DescTextVar"
@@ -1409,46 +1602,46 @@ Library {
 		Name			"DescGroupVar"
 	      }
 	      Object {
-		$ObjectID		31
+		$ObjectID		55
 		Prompt			"Simulink:studio:ToolBarParametersMenu"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  32
+		  $ObjectID		  56
 		  $ClassName		  "Simulink.dialog.TabContainer"
 		  Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    33
+		    $ObjectID		    57
 		    Prompt		    "Block Parameters"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    34
+		    $ObjectID		    58
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "yarp2WBIOption"
 		    }
 		    Name		    "Container4"
 		    }
 		    Object {
-		    $ObjectID		    35
+		    $ObjectID		    59
 		    Prompt		    "WBI Parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    36
+		    $ObjectID		    60
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    37
+		    $ObjectID		    61
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    38
+		    $ObjectID		    62
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    39
+		    $ObjectID		    63
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -1480,7 +1673,7 @@ Library {
 	  SFcnIsStateOwnerBlock	  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    40
+	    $ObjectID		    64
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Minimum Jerk Trajectory Generator"
 	    Description		    "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk "
@@ -1503,7 +1696,7 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      7
 	      Object {
-		$ObjectID		41
+		$ObjectID		65
 		Type			"checkbox"
 		Name			"externalSettlingTime"
 		Prompt			"External Settling Time"
@@ -1514,42 +1707,43 @@ Library {
 		"SettlingTime"
 	      }
 	      Object {
-		$ObjectID		42
+		$ObjectID		66
 		Type			"edit"
 		Name			"settlingTime"
 		Prompt			"Settling Time"
 		Value			"3"
 	      }
 	      Object {
-		$ObjectID		43
+		$ObjectID		67
 		Type			"edit"
 		Name			"sampleTime"
 		Prompt			"Sample Time"
 		Value			"0.01"
 	      }
 	      Object {
-		$ObjectID		44
+		$ObjectID		68
 		Type			"checkbox"
 		Name			"resetOnSettlingTime"
 		Prompt			"Reset on Settling Time Changes"
 		Value			"off"
+		Visible			"off"
 	      }
 	      Object {
-		$ObjectID		45
+		$ObjectID		69
 		Type			"checkbox"
 		Name			"firstDerivatives"
 		Prompt			"Output First Derivative"
 		Value			"on"
 	      }
 	      Object {
-		$ObjectID		46
+		$ObjectID		70
 		Type			"checkbox"
 		Name			"secondDerivatives"
 		Prompt			"Output Second Derivative"
 		Value			"on"
 	      }
 	      Object {
-		$ObjectID		47
+		$ObjectID		71
 		Type			"checkbox"
 		Name			"explicitInitialValue"
 		Prompt			"Explicit Initial Value"
@@ -1562,11 +1756,11 @@ Library {
 	      Type		      "Simulink.dialog.Group"
 	      Dimension		      2
 	      Object {
-		$ObjectID		48
+		$ObjectID		72
 		Prompt			"%<MaskType>"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  49
+		  $ObjectID		  73
 		  $ClassName		  "Simulink.dialog.Text"
 		  Prompt		  "%<MaskDescription>"
 		  Name			  "DescTextVar"
@@ -1574,38 +1768,38 @@ Library {
 		Name			"DescGroupVar"
 	      }
 	      Object {
-		$ObjectID		50
+		$ObjectID		74
 		Prompt			"Simulink:studio:ToolBarParametersMenu"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  51
+		  $ObjectID		  75
 		  $ClassName		  "Simulink.dialog.TabContainer"
 		  Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    52
+		    $ObjectID		    76
 		    Prompt		    "Trajectory Parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Control"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    53
+		    $ObjectID		    77
 		    $ClassName		    "Simulink.dialog.parameter.CheckBox"
 		    Name		    "externalSettlingTime"
 		    }
 		    Object {
-		    $ObjectID		    54
+		    $ObjectID		    78
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "settlingTime"
 		    }
 		    Object {
-		    $ObjectID		    55
+		    $ObjectID		    79
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "sampleTime"
 		    }
 		    Object {
-		    $ObjectID		    56
+		    $ObjectID		    80
 		    $ClassName		    "Simulink.dialog.parameter.CheckBox"
 		    Name		    "resetOnSettlingTime"
 		    }
@@ -1614,21 +1808,21 @@ Library {
 		    Name		    "Tab1"
 		    }
 		    Object {
-		    $ObjectID		    57
+		    $ObjectID		    81
 		    Prompt		    "Input/Output"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.CheckBox"
 		    Dimension		    3
 		    Object {
-		    $ObjectID		    58
+		    $ObjectID		    82
 		    Name		    "firstDerivatives"
 		    }
 		    Object {
-		    $ObjectID		    59
+		    $ObjectID		    83
 		    Name		    "secondDerivatives"
 		    }
 		    Object {
-		    $ObjectID		    60
+		    $ObjectID		    84
 		    Name		    "explicitInitialValue"
 		    }
 		    PropName		    "DialogControls"
@@ -1662,7 +1856,7 @@ Library {
 	  SFcnIsStateOwnerBlock	  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    61
+	    $ObjectID		    85
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Real Time Synchronizer"
 	    Description		    "This block slows down the simulation trying to match the period specified \nas parameter (in se"
@@ -1670,7 +1864,7 @@ Library {
 	    Display		    "disp('Real Time Synchronizer')"
 	    Object {
 	      $PropName		      "Parameters"
-	      $ObjectID		      62
+	      $ObjectID		      86
 	      $ClassName	      "Simulink.MaskParameter"
 	      Type		      "edit"
 	      Name		      "period"
@@ -1695,7 +1889,7 @@ Library {
 	  SFcnIsStateOwnerBlock	  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    63
+	    $ObjectID		    87
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Simulator Synchronizer"
 	    Description		    "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported"
@@ -1705,21 +1899,21 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      3
 	      Object {
-		$ObjectID		64
+		$ObjectID		88
 		Type			"edit"
 		Name			"period"
 		Prompt			"Controller Period (in seconds)"
 		Value			"0.01"
 	      }
 	      Object {
-		$ObjectID		65
+		$ObjectID		89
 		Type			"edit"
 		Name			"serverPortName"
 		Prompt			"Server Port Name"
 		Value			"'/clock/rpc'"
 	      }
 	      Object {
-		$ObjectID		66
+		$ObjectID		90
 		Type			"edit"
 		Name			"clientPortName"
 		Prompt			"Client Port Name"
@@ -1741,11 +1935,11 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    67
+	    $ObjectID		    91
 	    $ClassName		    "Simulink.Mask"
 	    Object {
 	      $PropName		      "Parameters"
-	      $ObjectID		      68
+	      $ObjectID		      92
 	      $ClassName	      "Simulink.MaskParameter"
 	      Type		      "edit"
 	      Name		      "tol"
@@ -1814,7 +2008,7 @@ Library {
 		TiledPageScale		1
 		ShowPageBoundaries	off
 		ZoomFactor		"100"
-		SIDHighWatermark	"1610"
+		SIDHighWatermark	"1619"
 		Block {
 		  BlockType		  Inport
 		  Name			  "mat"
@@ -1835,20 +2029,20 @@ Library {
 		Block {
 		  BlockType		  Demux
 		  Name			  " Demux "
-		  SID			  "112::1609"
+		  SID			  "112::1618"
 		  Ports			  [1, 1]
 		  Position		  [270, 230, 320, 270]
-		  ZOrder		  98
+		  ZOrder		  107
 		  Outputs		  "1"
 		}
 		Block {
 		  BlockType		  S-Function
 		  Name			  " SFunction "
-		  SID			  "112::1608"
+		  SID			  "112::1617"
 		  Tag			  "Stateflow S-Function WBToolboxLibrary_repository 7"
 		  Ports			  [2, 2]
 		  Position		  [180, 100, 230, 160]
-		  ZOrder		  97
+		  ZOrder		  106
 		  FunctionName		  "sf_sfun"
 		  PortCounts		  "[2 2]"
 		  SFunctionDeploymentMode off
@@ -1864,9 +2058,9 @@ Library {
 		Block {
 		  BlockType		  Terminator
 		  Name			  " Terminator "
-		  SID			  "112::1610"
+		  SID			  "112::1619"
 		  Position		  [460, 241, 480, 259]
-		  ZOrder		  99
+		  ZOrder		  108
 		}
 		Block {
 		  BlockType		  Outport
@@ -1877,7 +2071,7 @@ Library {
 		  IconDisplay		  "Port number"
 		}
 		Line {
-		  ZOrder		  61
+		  ZOrder		  76
 		  SrcBlock		  "mat"
 		  SrcPort		  1
 		  Points		  [120, 0]
@@ -1885,7 +2079,7 @@ Library {
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  62
+		  ZOrder		  77
 		  SrcBlock		  "tol"
 		  SrcPort		  1
 		  DstBlock		  " SFunction "
@@ -1893,7 +2087,7 @@ Library {
 		}
 		Line {
 		  Name			  "TPinv"
-		  ZOrder		  63
+		  ZOrder		  78
 		  Labels		  [0, 0]
 		  SrcBlock		  " SFunction "
 		  SrcPort		  2
@@ -1901,14 +2095,14 @@ Library {
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  64
+		  ZOrder		  79
 		  SrcBlock		  " Demux "
 		  SrcPort		  1
 		  DstBlock		  " Terminator "
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  65
+		  ZOrder		  80
 		  SrcBlock		  " SFunction "
 		  SrcPort		  1
 		  DstBlock		  " Demux "
@@ -1962,7 +2156,7 @@ Library {
 	  SFcnIsStateOwnerBlock	  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    69
+	    $ObjectID		    93
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "YARP Read"
 	    Description		    "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' act"
@@ -1980,35 +2174,35 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      6
 	      Object {
-		$ObjectID		70
+		$ObjectID		94
 		Type			"edit"
 		Name			"portName"
 		Prompt			"Source Port Name"
 		Value			"'/portname'"
 	      }
 	      Object {
-		$ObjectID		71
+		$ObjectID		95
 		Type			"edit"
 		Name			"signalSize"
 		Prompt			"Port Size"
 		Value			"1"
 	      }
 	      Object {
-		$ObjectID		72
+		$ObjectID		96
 		Type			"checkbox"
 		Name			"blocking"
 		Prompt			"Blocking Read"
 		Value			"off"
 	      }
 	      Object {
-		$ObjectID		73
+		$ObjectID		97
 		Type			"checkbox"
 		Name			"timestamp"
 		Prompt			"Read Timestamp"
 		Value			"on"
 	      }
 	      Object {
-		$ObjectID		74
+		$ObjectID		98
 		Type			"checkbox"
 		Name			"autoconnect"
 		Prompt			"Autoconnect"
@@ -2020,7 +2214,7 @@ Library {
 		"t_string"
 	      }
 	      Object {
-		$ObjectID		75
+		$ObjectID		99
 		Type			"checkbox"
 		Name			"errorOnConnection"
 		Prompt			"Error on missing connection"
@@ -2045,7 +2239,7 @@ Library {
 	  SFcnIsStateOwnerBlock	  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    76
+	    $ObjectID		    100
 	    $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"
@@ -2058,14 +2252,14 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      3
 	      Object {
-		$ObjectID		77
+		$ObjectID		101
 		Type			"edit"
 		Name			"portName"
 		Prompt			"Opened Port Name"
 		Value			"'/portname'"
 	      }
 	      Object {
-		$ObjectID		78
+		$ObjectID		102
 		Type			"checkbox"
 		Name			"autoconnect"
 		Prompt			"Autoconnect"
@@ -2076,7 +2270,7 @@ Library {
 		"';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string"
 	      }
 	      Object {
-		$ObjectID		79
+		$ObjectID		103
 		Type			"checkbox"
 		Name			"errorOnConnection"
 		Prompt			"Error on missing connection"
@@ -2099,7 +2293,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    80
+	    $ObjectID		    104
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Errors"
 	    Description		    "Computes two kinds of errors. The first is just the difference between x\nand y while the secon"
@@ -2242,7 +2436,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    81
+	    $ObjectID		    105
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Holder"
 	    Description		    "This block holds the first input value during the simulation."
@@ -2322,7 +2516,7 @@ Library {
 		TiledPageScale		1
 		ShowPageBoundaries	off
 		ZoomFactor		"100"
-		SIDHighWatermark	"1610"
+		SIDHighWatermark	"1619"
 		Block {
 		  BlockType		  Inport
 		  Name			  "s"
@@ -2343,20 +2537,20 @@ Library {
 		Block {
 		  BlockType		  Demux
 		  Name			  " Demux "
-		  SID			  "1300::1609"
+		  SID			  "1300::1618"
 		  Ports			  [1, 1]
 		  Position		  [270, 230, 320, 270]
-		  ZOrder		  86
+		  ZOrder		  95
 		  Outputs		  "1"
 		}
 		Block {
 		  BlockType		  S-Function
 		  Name			  " SFunction "
-		  SID			  "1300::1608"
+		  SID			  "1300::1617"
 		  Tag			  "Stateflow S-Function WBToolboxLibrary_repository 1"
 		  Ports			  [2, 2]
 		  Position		  [180, 100, 230, 160]
-		  ZOrder		  85
+		  ZOrder		  94
 		  FunctionName		  "sf_sfun"
 		  PortCounts		  "[2 2]"
 		  SFunctionDeploymentMode off
@@ -2372,9 +2566,9 @@ Library {
 		Block {
 		  BlockType		  Terminator
 		  Name			  " Terminator "
-		  SID			  "1300::1610"
+		  SID			  "1300::1619"
 		  Position		  [460, 241, 480, 259]
-		  ZOrder		  87
+		  ZOrder		  96
 		}
 		Block {
 		  BlockType		  Outport
@@ -2385,7 +2579,7 @@ Library {
 		  IconDisplay		  "Port number"
 		}
 		Line {
-		  ZOrder		  61
+		  ZOrder		  76
 		  SrcBlock		  "s"
 		  SrcPort		  1
 		  Points		  [120, 0]
@@ -2393,7 +2587,7 @@ Library {
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  62
+		  ZOrder		  77
 		  SrcBlock		  "unused"
 		  SrcPort		  1
 		  DstBlock		  " SFunction "
@@ -2401,7 +2595,7 @@ Library {
 		}
 		Line {
 		  Name			  "s0"
-		  ZOrder		  63
+		  ZOrder		  78
 		  Labels		  [0, 0]
 		  SrcBlock		  " SFunction "
 		  SrcPort		  2
@@ -2409,14 +2603,14 @@ Library {
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  64
+		  ZOrder		  79
 		  SrcBlock		  " Demux "
 		  SrcPort		  1
 		  DstBlock		  " Terminator "
 		  DstPort		  1
 		}
 		Line {
-		  ZOrder		  65
+		  ZOrder		  80
 		  SrcBlock		  " SFunction "
 		  SrcPort		  1
 		  DstBlock		  " Demux "
@@ -2477,7 +2671,7 @@ Library {
       Variant		      off
       Object {
 	$PropName		"MaskObject"
-	$ObjectID		82
+	$ObjectID		106
 	$ClassName		"Simulink.Mask"
 	Display			"image(imread('wholeBodyActuators.png'),'center')"
       }
@@ -2511,7 +2705,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    83
+	    $ObjectID		    107
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Set Low Level PIDs"
 	    Description		    "This block sets the low level PID values for the robot actuators.\n\nIf only one gain file is s"
@@ -2530,7 +2724,7 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      6
 	      Object {
-		$ObjectID		84
+		$ObjectID		108
 		Type			"edit"
 		Name			"pidParameters"
 		Prompt			"PID parameter"
@@ -2538,7 +2732,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		85
+		$ObjectID		109
 		Type			"popup"
 		Array {
 		  Type			  "Cell"
@@ -2558,7 +2752,7 @@ Library {
 		"kStr, 'Torque')\n    set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear  maskStr"
 	      }
 	      Object {
-		$ObjectID		86
+		$ObjectID		110
 		Type			"edit"
 		Name			"robotName"
 		Prompt			"Robot Port Name"
@@ -2566,7 +2760,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		87
+		$ObjectID		111
 		Type			"edit"
 		Name			"localName"
 		Prompt			"Model Name"
@@ -2574,7 +2768,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		88
+		$ObjectID		112
 		Type			"edit"
 		Name			"wbiFile"
 		Prompt			"WBI filename"
@@ -2582,7 +2776,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		89
+		$ObjectID		113
 		Type			"edit"
 		Name			"wbiList"
 		Prompt			"WBI list name"
@@ -2595,11 +2789,11 @@ Library {
 	      Type		      "Simulink.dialog.Group"
 	      Dimension		      2
 	      Object {
-		$ObjectID		90
+		$ObjectID		114
 		Prompt			"%<MaskType>"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  91
+		  $ObjectID		  115
 		  $ClassName		  "Simulink.dialog.Text"
 		  Prompt		  "%<MaskDescription>"
 		  Name			  "DescTextVar"
@@ -2607,29 +2801,29 @@ Library {
 		Name			"DescGroupVar"
 	      }
 	      Object {
-		$ObjectID		92
+		$ObjectID		116
 		Prompt			"Simulink:studio:ToolBarParametersMenu"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  93
+		  $ObjectID		  117
 		  $ClassName		  "Simulink.dialog.TabContainer"
 		  Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    94
+		    $ObjectID		    118
 		    Prompt		    "Block parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Control"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    95
+		    $ObjectID		    119
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    PromptLocation	    "left"
 		    Name		    "pidParameters"
 		    }
 		    Object {
-		    $ObjectID		    96
+		    $ObjectID		    120
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "pidType"
 		    }
@@ -2638,25 +2832,25 @@ Library {
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    97
+		    $ObjectID		    121
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    98
+		    $ObjectID		    122
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    99
+		    $ObjectID		    123
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    100
+		    $ObjectID		    124
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    101
+		    $ObjectID		    125
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -2717,7 +2911,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    102
+	    $ObjectID		    126
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Set References"
 	    Description		    "This block sets the references for the robot actuators.\nThe type of control mode is specified "
@@ -2734,7 +2928,7 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      7
 	      Object {
-		$ObjectID		103
+		$ObjectID		127
 		Type			"popup"
 		Array {
 		  Type			  "Cell"
@@ -2758,7 +2952,7 @@ Library {
 		"'Open Loop')\n    set_param(gcb,'BackgroundColor', '[0.98, 0.98, 0.98, 1.0]');\nend\nclear  maskStr"
 	      }
 	      Object {
-		$ObjectID		104
+		$ObjectID		128
 		Type			"popup"
 		Array {
 		  Type			  "Cell"
@@ -2775,7 +2969,7 @@ Library {
 		";'on';'on';'on';'on'});\nend\nclear subOrFull "
 	      }
 	      Object {
-		$ObjectID		105
+		$ObjectID		129
 		Type			"edit"
 		Name			"controlledJoints"
 		Prompt			"Controlled Joint List"
@@ -2783,7 +2977,7 @@ Library {
 		Visible			"off"
 	      }
 	      Object {
-		$ObjectID		106
+		$ObjectID		130
 		Type			"edit"
 		Name			"robotName"
 		Prompt			"Robot Port Name"
@@ -2791,7 +2985,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		107
+		$ObjectID		131
 		Type			"edit"
 		Name			"localName"
 		Prompt			"Model Name"
@@ -2799,7 +2993,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		108
+		$ObjectID		132
 		Type			"edit"
 		Name			"wbiFile"
 		Prompt			"WBI filename"
@@ -2807,7 +3001,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		109
+		$ObjectID		133
 		Type			"edit"
 		Name			"wbiList"
 		Prompt			"WBI list name"
@@ -2820,11 +3014,11 @@ Library {
 	      Type		      "Simulink.dialog.Group"
 	      Dimension		      2
 	      Object {
-		$ObjectID		110
+		$ObjectID		134
 		Prompt			"%<MaskType>"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  111
+		  $ObjectID		  135
 		  $ClassName		  "Simulink.dialog.Text"
 		  Prompt		  "%<MaskDescription>"
 		  Name			  "DescTextVar"
@@ -2832,33 +3026,33 @@ Library {
 		Name			"DescGroupVar"
 	      }
 	      Object {
-		$ObjectID		112
+		$ObjectID		136
 		Prompt			"Simulink:studio:ToolBarParametersMenu"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  113
+		  $ObjectID		  137
 		  $ClassName		  "Simulink.dialog.TabContainer"
 		  Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    114
+		    $ObjectID		    138
 		    Prompt		    "Block parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Control"
 		    Dimension		    3
 		    Object {
-		    $ObjectID		    115
+		    $ObjectID		    139
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "controlType"
 		    }
 		    Object {
-		    $ObjectID		    116
+		    $ObjectID		    140
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "fullOrSubControl"
 		    }
 		    Object {
-		    $ObjectID		    117
+		    $ObjectID		    141
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "controlledJoints"
 		    }
@@ -2867,25 +3061,25 @@ Library {
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    118
+		    $ObjectID		    142
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    119
+		    $ObjectID		    143
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    120
+		    $ObjectID		    144
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    121
+		    $ObjectID		    145
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    122
+		    $ObjectID		    146
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -2962,7 +3156,7 @@ Library {
       Variant		      off
       Object {
 	$PropName		"MaskObject"
-	$ObjectID		123
+	$ObjectID		147
 	$ClassName		"Simulink.Mask"
 	Display			"image(imread('wholeBodyModel.png'),'center')"
       }
@@ -2993,7 +3187,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    124
+	    $ObjectID		    148
 	    $ClassName		    "Simulink.Mask"
 	    Display		    "image(imread('Dynamics.png'))"
 	  }
@@ -3027,7 +3221,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		125
+		$ObjectID		149
 		$ClassName		"Simulink.Mask"
 		Type			"Centroidal Momentum"
 		Description		"This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dynamics of a huma"
@@ -3046,7 +3240,7 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  4
 		  Object {
-		    $ObjectID		    126
+		    $ObjectID		    150
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -3054,7 +3248,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    127
+		    $ObjectID		    151
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -3062,7 +3256,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    128
+		    $ObjectID		    152
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -3070,7 +3264,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    129
+		    $ObjectID		    153
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -3203,7 +3397,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		130
+		$ObjectID		154
 		$ClassName		"Simulink.Mask"
 		Type			"Get Generalized Bias Forces"
 		Description		"This block retrieves the generalied bias forces of the robot.\n\nParameter:\n- Explicit Gravity. If Tr"
@@ -3228,7 +3422,7 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  5
 		  Object {
-		    $ObjectID		    131
+		    $ObjectID		    155
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -3236,7 +3430,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    132
+		    $ObjectID		    156
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -3244,7 +3438,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    133
+		    $ObjectID		    157
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -3252,7 +3446,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    134
+		    $ObjectID		    158
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -3260,7 +3454,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    135
+		    $ObjectID		    159
 		    Type		    "checkbox"
 		    Name		    "explicitGravity"
 		    Prompt		    "Explicit Gravity"
@@ -3438,7 +3632,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		136
+		$ObjectID		160
 		$ClassName		"Simulink.Mask"
 		Type			"Gravity bias"
 		Description		"This block compute the generalized bias forces due to the gravity\n\nParameter:\n- Explicit Gravity. I"
@@ -3461,7 +3655,7 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  5
 		  Object {
-		    $ObjectID		    137
+		    $ObjectID		    161
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -3469,7 +3663,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    138
+		    $ObjectID		    162
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -3477,7 +3671,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    139
+		    $ObjectID		    163
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -3485,7 +3679,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    140
+		    $ObjectID		    164
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -3493,7 +3687,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    141
+		    $ObjectID		    165
 		    Type		    "checkbox"
 		    Name		    "explicitGravity"
 		    Prompt		    "Explicit Gravity"
@@ -3658,7 +3852,7 @@ Library {
 	      SFcnIsStateOwnerBlock   off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		142
+		$ObjectID		166
 		$ClassName		"Simulink.Mask"
 		Type			"Inverse Dynamics"
 		Description		"This block compute the inverse dynamics of the robot.\n\nParameter:\n- Explicit Gravity. If True, an a"
@@ -3684,35 +3878,35 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  5
 		  Object {
-		    $ObjectID		    143
+		    $ObjectID		    167
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
 		    Value		    "WBT_robotName"
 		  }
 		  Object {
-		    $ObjectID		    144
+		    $ObjectID		    168
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
 		    Value		    "WBT_modelName"
 		  }
 		  Object {
-		    $ObjectID		    145
+		    $ObjectID		    169
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
 		    Value		    "WBT_wbiFilename"
 		  }
 		  Object {
-		    $ObjectID		    146
+		    $ObjectID		    170
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
 		    Value		    "WBT_wbiList"
 		  }
 		  Object {
-		    $ObjectID		    147
+		    $ObjectID		    171
 		    Type		    "checkbox"
 		    Name		    "explicitGravity"
 		    Prompt		    "Explicit Gravity"
@@ -3737,7 +3931,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		148
+		$ObjectID		172
 		$ClassName		"Simulink.Mask"
 		Type			"Mass Matrix"
 		Description		"This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of freedom of th"
@@ -3753,7 +3947,7 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  4
 		  Object {
-		    $ObjectID		    149
+		    $ObjectID		    173
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -3761,7 +3955,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    150
+		    $ObjectID		    174
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -3769,7 +3963,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    151
+		    $ObjectID		    175
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -3777,7 +3971,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    152
+		    $ObjectID		    176
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -3879,7 +4073,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    153
+	    $ObjectID		    177
 	    $ClassName		    "Simulink.Mask"
 	    Display		    "image(imread('jacobian.png'))"
 	  }
@@ -3913,7 +4107,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		154
+		$ObjectID		178
 		$ClassName		"Simulink.Mask"
 		Type			"DotJ Nu"
 		Description		"This block computes the product between the time derivative of the\n         Jacobian of the specified"
@@ -3937,14 +4131,14 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  5
 		  Object {
-		    $ObjectID		    155
+		    $ObjectID		    179
 		    Type		    "edit"
 		    Name		    "frameName"
 		    Prompt		    "Frame name"
 		    Value		    "'frame'"
 		  }
 		  Object {
-		    $ObjectID		    156
+		    $ObjectID		    180
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -3952,7 +4146,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    157
+		    $ObjectID		    181
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -3960,7 +4154,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    158
+		    $ObjectID		    182
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -3968,7 +4162,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    159
+		    $ObjectID		    183
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -3981,11 +4175,11 @@ Library {
 		  Type			  "Simulink.dialog.Group"
 		  Dimension		  2
 		  Object {
-		    $ObjectID		    160
+		    $ObjectID		    184
 		    Prompt		    "%<MaskType>"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    161
+		    $ObjectID		    185
 		    $ClassName		    "Simulink.dialog.Text"
 		    Prompt		    "%<MaskDescription>"
 		    Name		    "DescTextVar"
@@ -3993,46 +4187,46 @@ Library {
 		    Name		    "DescGroupVar"
 		  }
 		  Object {
-		    $ObjectID		    162
+		    $ObjectID		    186
 		    Prompt		    "Simulink:studio:ToolBarParametersMenu"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    163
+		    $ObjectID		    187
 		    $ClassName		    "Simulink.dialog.TabContainer"
 		    Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    164
+		    $ObjectID		    188
 		    Prompt		    "Block parameters"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    165
+		    $ObjectID		    189
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "frameName"
 		    }
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    166
+		    $ObjectID		    190
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    167
+		    $ObjectID		    191
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    168
+		    $ObjectID		    192
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    169
+		    $ObjectID		    193
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    170
+		    $ObjectID		    194
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -4172,7 +4366,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		171
+		$ObjectID		195
 		$ClassName		"Simulink.Mask"
 		Type			"Jacobian"
 		Description		"This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of degrees of"
@@ -4192,14 +4386,14 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  5
 		  Object {
-		    $ObjectID		    172
+		    $ObjectID		    196
 		    Type		    "edit"
 		    Name		    "frameName"
 		    Prompt		    "Frame name"
 		    Value		    "'frame'"
 		  }
 		  Object {
-		    $ObjectID		    173
+		    $ObjectID		    197
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -4207,7 +4401,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    174
+		    $ObjectID		    198
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -4215,7 +4409,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    175
+		    $ObjectID		    199
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -4223,7 +4417,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    176
+		    $ObjectID		    200
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -4236,11 +4430,11 @@ Library {
 		  Type			  "Simulink.dialog.Group"
 		  Dimension		  2
 		  Object {
-		    $ObjectID		    177
+		    $ObjectID		    201
 		    Prompt		    "%<MaskType>"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    178
+		    $ObjectID		    202
 		    $ClassName		    "Simulink.dialog.Text"
 		    Prompt		    "%<MaskDescription>"
 		    Name		    "DescTextVar"
@@ -4248,46 +4442,46 @@ Library {
 		    Name		    "DescGroupVar"
 		  }
 		  Object {
-		    $ObjectID		    179
+		    $ObjectID		    203
 		    Prompt		    "Simulink:studio:ToolBarParametersMenu"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    180
+		    $ObjectID		    204
 		    $ClassName		    "Simulink.dialog.TabContainer"
 		    Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    181
+		    $ObjectID		    205
 		    Prompt		    "Block parameters"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    182
+		    $ObjectID		    206
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "frameName"
 		    }
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    183
+		    $ObjectID		    207
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    184
+		    $ObjectID		    208
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    185
+		    $ObjectID		    209
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    186
+		    $ObjectID		    210
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    187
+		    $ObjectID		    211
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -4395,7 +4589,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    188
+	    $ObjectID		    212
 	    $ClassName		    "Simulink.Mask"
 	    Display		    "image(imread('forwardKinematics.png'))"
 	  }
@@ -4429,7 +4623,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		189
+		$ObjectID		213
 		$ClassName		"Simulink.Mask"
 		Type			"Forward Kinematics"
 		Description		"This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of "
@@ -4449,14 +4643,14 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  5
 		  Object {
-		    $ObjectID		    190
+		    $ObjectID		    214
 		    Type		    "edit"
 		    Name		    "frameName"
 		    Prompt		    "Frame name"
 		    Value		    "'frame'"
 		  }
 		  Object {
-		    $ObjectID		    191
+		    $ObjectID		    215
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -4464,7 +4658,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    192
+		    $ObjectID		    216
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -4472,7 +4666,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    193
+		    $ObjectID		    217
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -4480,7 +4674,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    194
+		    $ObjectID		    218
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -4493,11 +4687,11 @@ Library {
 		  Type			  "Simulink.dialog.Group"
 		  Dimension		  2
 		  Object {
-		    $ObjectID		    195
+		    $ObjectID		    219
 		    Prompt		    "%<MaskType>"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    196
+		    $ObjectID		    220
 		    $ClassName		    "Simulink.dialog.Text"
 		    Prompt		    "%<MaskDescription>"
 		    Name		    "DescTextVar"
@@ -4505,46 +4699,46 @@ Library {
 		    Name		    "DescGroupVar"
 		  }
 		  Object {
-		    $ObjectID		    197
+		    $ObjectID		    221
 		    Prompt		    "Simulink:studio:ToolBarParametersMenu"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    198
+		    $ObjectID		    222
 		    $ClassName		    "Simulink.dialog.TabContainer"
 		    Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    199
+		    $ObjectID		    223
 		    Prompt		    "Block parameters"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    200
+		    $ObjectID		    224
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "frameName"
 		    }
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    201
+		    $ObjectID		    225
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    202
+		    $ObjectID		    226
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    203
+		    $ObjectID		    227
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    204
+		    $ObjectID		    228
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    205
+		    $ObjectID		    229
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -4653,7 +4847,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		206
+		$ObjectID		230
 		$ClassName		"Simulink.Mask"
 		Type			"Forward Kinematics"
 		Description		"This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of "
@@ -4677,21 +4871,21 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  7
 		  Object {
-		    $ObjectID		    207
+		    $ObjectID		    231
 		    Type		    "edit"
 		    Name		    "baseFrame"
 		    Prompt		    "Base Frame"
 		    Value		    "'root_link'"
 		  }
 		  Object {
-		    $ObjectID		    208
+		    $ObjectID		    232
 		    Type		    "edit"
 		    Name		    "endEffFrame"
 		    Prompt		    "End Effector frame"
 		    Value		    "'l_sole'"
 		  }
 		  Object {
-		    $ObjectID		    209
+		    $ObjectID		    233
 		    Type		    "popup"
 		    Array {
 		    Type		    "Cell"
@@ -4705,7 +4899,7 @@ Library {
 		    Value		    "Full Constraint (Position and Orientation)"
 		  }
 		  Object {
-		    $ObjectID		    210
+		    $ObjectID		    234
 		    Type		    "edit"
 		    Name		    "robotName"
 		    Prompt		    "Robot Port Name"
@@ -4713,7 +4907,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    211
+		    $ObjectID		    235
 		    Type		    "edit"
 		    Name		    "localName"
 		    Prompt		    "Model Name"
@@ -4721,7 +4915,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    212
+		    $ObjectID		    236
 		    Type		    "edit"
 		    Name		    "wbiFile"
 		    Prompt		    "WBI filename"
@@ -4729,7 +4923,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    213
+		    $ObjectID		    237
 		    Type		    "edit"
 		    Name		    "wbiList"
 		    Prompt		    "WBI list name"
@@ -4742,11 +4936,11 @@ Library {
 		  Type			  "Simulink.dialog.Group"
 		  Dimension		  2
 		  Object {
-		    $ObjectID		    214
+		    $ObjectID		    238
 		    Prompt		    "%<MaskType>"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    215
+		    $ObjectID		    239
 		    $ClassName		    "Simulink.dialog.Text"
 		    Prompt		    "%<MaskDescription>"
 		    Name		    "DescTextVar"
@@ -4754,34 +4948,34 @@ Library {
 		    Name		    "DescGroupVar"
 		  }
 		  Object {
-		    $ObjectID		    216
+		    $ObjectID		    240
 		    Prompt		    "Simulink:studio:ToolBarParametersMenu"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    217
+		    $ObjectID		    241
 		    $ClassName		    "Simulink.dialog.TabContainer"
 		    Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    218
+		    $ObjectID		    242
 		    Prompt		    "Block parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Control"
 		    Dimension		    3
 		    Object {
-		    $ObjectID		    219
+		    $ObjectID		    243
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    PromptLocation	    "left"
 		    Name		    "baseFrame"
 		    }
 		    Object {
-		    $ObjectID		    220
+		    $ObjectID		    244
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "endEffFrame"
 		    }
 		    Object {
-		    $ObjectID		    221
+		    $ObjectID		    245
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "optOption"
 		    }
@@ -4790,25 +4984,25 @@ Library {
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    222
+		    $ObjectID		    246
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    223
+		    $ObjectID		    247
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    224
+		    $ObjectID		    248
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    225
+		    $ObjectID		    249
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    226
+		    $ObjectID		    250
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -4947,7 +5141,7 @@ Library {
 	      Variant		      off
 	      Object {
 		$PropName		"MaskObject"
-		$ObjectID		227
+		$ObjectID		251
 		$ClassName		"Simulink.Mask"
 		Type			"Forward Kinematics"
 		Description		"This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of "
@@ -4973,14 +5167,14 @@ Library {
 		  Type			  "Simulink.MaskParameter"
 		  Dimension		  3
 		  Object {
-		    $ObjectID		    228
+		    $ObjectID		    252
 		    Type		    "edit"
 		    Name		    "solverName"
 		    Prompt		    "Solver Name"
 		    Value		    "'/cartesianSolver'"
 		  }
 		  Object {
-		    $ObjectID		    229
+		    $ObjectID		    253
 		    Type		    "edit"
 		    Name		    "dofs"
 		    Prompt		    "#Dofs"
@@ -4988,7 +5182,7 @@ Library {
 		    Tunable		    "off"
 		  }
 		  Object {
-		    $ObjectID		    230
+		    $ObjectID		    254
 		    Type		    "popup"
 		    Array {
 		    Type		    "Cell"
@@ -5007,11 +5201,11 @@ Library {
 		  Type			  "Simulink.dialog.Group"
 		  Dimension		  2
 		  Object {
-		    $ObjectID		    231
+		    $ObjectID		    255
 		    Prompt		    "%<MaskType>"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    232
+		    $ObjectID		    256
 		    $ClassName		    "Simulink.dialog.Text"
 		    Prompt		    "%<MaskDescription>"
 		    Name		    "DescTextVar"
@@ -5019,33 +5213,33 @@ Library {
 		    Name		    "DescGroupVar"
 		  }
 		  Object {
-		    $ObjectID		    233
+		    $ObjectID		    257
 		    Prompt		    "Simulink:studio:ToolBarParametersMenu"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    234
+		    $ObjectID		    258
 		    $ClassName		    "Simulink.dialog.TabContainer"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    235
+		    $ObjectID		    259
 		    $ClassName		    "Simulink.dialog.Tab"
 		    Prompt		    "Block parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Control"
 		    Dimension		    3
 		    Object {
-		    $ObjectID		    236
+		    $ObjectID		    260
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    PromptLocation	    "left"
 		    Name		    "solverName"
 		    }
 		    Object {
-		    $ObjectID		    237
+		    $ObjectID		    261
 		    $ClassName		    "Simulink.dialog.parameter.Edit"
 		    Name		    "dofs"
 		    }
 		    Object {
-		    $ObjectID		    238
+		    $ObjectID		    262
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "optOption"
 		    }
@@ -5153,7 +5347,7 @@ Library {
       Variant		      off
       Object {
 	$PropName		"MaskObject"
-	$ObjectID		239
+	$ObjectID		263
 	$ClassName		"Simulink.Mask"
 	Display			"image(imread('wholeBodyStates.png'),'center');"
       }
@@ -5187,7 +5381,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    240
+	    $ObjectID		    264
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Get Estimate"
 	    Description		    "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of "
@@ -5202,7 +5396,7 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      5
 	      Object {
-		$ObjectID		241
+		$ObjectID		265
 		Type			"popup"
 		Array {
 		  Type			  "Cell"
@@ -5225,7 +5419,7 @@ Library {
 		"');\nend\nclear  maskStr"
 	      }
 	      Object {
-		$ObjectID		242
+		$ObjectID		266
 		Type			"edit"
 		Name			"robotName"
 		Prompt			"Robot Port Name"
@@ -5233,7 +5427,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		243
+		$ObjectID		267
 		Type			"edit"
 		Name			"localName"
 		Prompt			"Model Name"
@@ -5241,7 +5435,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		244
+		$ObjectID		268
 		Type			"edit"
 		Name			"wbiFile"
 		Prompt			"WBI filename"
@@ -5249,7 +5443,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		245
+		$ObjectID		269
 		Type			"edit"
 		Name			"wbiList"
 		Prompt			"WBI list name"
@@ -5262,11 +5456,11 @@ Library {
 	      Type		      "Simulink.dialog.Group"
 	      Dimension		      2
 	      Object {
-		$ObjectID		246
+		$ObjectID		270
 		Prompt			"%<MaskType>"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  247
+		  $ObjectID		  271
 		  $ClassName		  "Simulink.dialog.Text"
 		  Prompt		  "%<MaskDescription>"
 		  Name			  "DescTextVar"
@@ -5274,46 +5468,46 @@ Library {
 		Name			"DescGroupVar"
 	      }
 	      Object {
-		$ObjectID		248
+		$ObjectID		272
 		Prompt			"Simulink:studio:ToolBarParametersMenu"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  249
+		  $ObjectID		  273
 		  $ClassName		  "Simulink.dialog.TabContainer"
 		  Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    250
+		    $ObjectID		    274
 		    Prompt		    "Block parameters"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    251
+		    $ObjectID		    275
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "estimateType"
 		    }
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    252
+		    $ObjectID		    276
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    253
+		    $ObjectID		    277
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    254
+		    $ObjectID		    278
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    255
+		    $ObjectID		    279
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    256
+		    $ObjectID		    280
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"
@@ -5389,7 +5583,7 @@ Library {
 	  Variant		  off
 	  Object {
 	    $PropName		    "MaskObject"
-	    $ObjectID		    257
+	    $ObjectID		    281
 	    $ClassName		    "Simulink.Mask"
 	    Type		    "Get Estimate"
 	    Description		    "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of "
@@ -5404,7 +5598,7 @@ Library {
 	      Type		      "Simulink.MaskParameter"
 	      Dimension		      5
 	      Object {
-		$ObjectID		258
+		$ObjectID		282
 		Type			"popup"
 		Array {
 		  Type			  "Cell"
@@ -5424,7 +5618,7 @@ Library {
 		";\nend\nclear  maskStr"
 	      }
 	      Object {
-		$ObjectID		259
+		$ObjectID		283
 		Type			"edit"
 		Name			"robotName"
 		Prompt			"Robot Port Name"
@@ -5432,7 +5626,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		260
+		$ObjectID		284
 		Type			"edit"
 		Name			"localName"
 		Prompt			"Model Name"
@@ -5440,7 +5634,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		261
+		$ObjectID		285
 		Type			"edit"
 		Name			"wbiFile"
 		Prompt			"WBI filename"
@@ -5448,7 +5642,7 @@ Library {
 		Tunable			"off"
 	      }
 	      Object {
-		$ObjectID		262
+		$ObjectID		286
 		Type			"edit"
 		Name			"wbiList"
 		Prompt			"WBI list name"
@@ -5461,11 +5655,11 @@ Library {
 	      Type		      "Simulink.dialog.Group"
 	      Dimension		      2
 	      Object {
-		$ObjectID		263
+		$ObjectID		287
 		Prompt			"%<MaskType>"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  264
+		  $ObjectID		  288
 		  $ClassName		  "Simulink.dialog.Text"
 		  Prompt		  "%<MaskDescription>"
 		  Name			  "DescTextVar"
@@ -5473,46 +5667,46 @@ Library {
 		Name			"DescGroupVar"
 	      }
 	      Object {
-		$ObjectID		265
+		$ObjectID		289
 		Prompt			"Simulink:studio:ToolBarParametersMenu"
 		Object {
 		  $PropName		  "DialogControls"
-		  $ObjectID		  266
+		  $ObjectID		  290
 		  $ClassName		  "Simulink.dialog.TabContainer"
 		  Array {
 		    Type		    "Simulink.dialog.Tab"
 		    Dimension		    2
 		    Object {
-		    $ObjectID		    267
+		    $ObjectID		    291
 		    Prompt		    "Block parameters"
 		    Object {
 		    $PropName		    "DialogControls"
-		    $ObjectID		    268
+		    $ObjectID		    292
 		    $ClassName		    "Simulink.dialog.parameter.Popup"
 		    Name		    "limitsType"
 		    }
 		    Name		    "Container8"
 		    }
 		    Object {
-		    $ObjectID		    269
+		    $ObjectID		    293
 		    Prompt		    "WBI parameters"
 		    Array {
 		    Type		    "Simulink.dialog.parameter.Edit"
 		    Dimension		    4
 		    Object {
-		    $ObjectID		    270
+		    $ObjectID		    294
 		    Name		    "robotName"
 		    }
 		    Object {
-		    $ObjectID		    271
+		    $ObjectID		    295
 		    Name		    "localName"
 		    }
 		    Object {
-		    $ObjectID		    272
+		    $ObjectID		    296
 		    Name		    "wbiFile"
 		    }
 		    Object {
-		    $ObjectID		    273
+		    $ObjectID		    297
 		    Name		    "wbiList"
 		    }
 		    PropName		    "DialogControls"