diff --git a/doc/mkdocs/data/install.md b/doc/mkdocs/data/install.md index 8a7d01aa0..e097fbbbe 100644 --- a/doc/mkdocs/data/install.md +++ b/doc/mkdocs/data/install.md @@ -19,8 +19,9 @@ - [`iCub`](https://github.com/robotology/icub-main) - [Gazebo Simulator](http://gazebosim.org/) - [`gazebo_yarp_plugins`](https://github.com/robotology/gazebo_yarp_plugins) -- [`qpOASES`](https://github.com/robotology-dependencies/qpOASES/) +- [`qpOASES`](https://github.com/coin-or/qpOASES) - [`matio`](https://github.com/tbeu/matio) +- [`osqp-eigen`](https://github.com/robotology/osqp-eigen) ## Installation diff --git a/matlab/library/WBToolboxLibrary_repository.mdl b/matlab/library/WBToolboxLibrary_repository.mdl index d63bab36b..92a436b3b 100644 --- a/matlab/library/WBToolboxLibrary_repository.mdl +++ b/matlab/library/WBToolboxLibrary_repository.mdl @@ -1,13 +1,23 @@ Library { Name "WBToolboxLibrary_repository" - Version 8.4 + Version 10.3 SavedCharacterEncoding "UTF-8" + ModelUUID "ab3bc634-112e-4ce3-b3d3-90e66ca92abb" + LogicAnalyzerPlugin "on" + WebScopes_FoundationPlugin "on" + SLCCPlugin "on" + slcheck_filter_plugin "on" + DiagnosticSuppressor "on" + AnimationPlugin "on" + NotesPlugin "on" LibraryType "BlockLibrary" + EnableAccessToBaseWorkspace on ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off FPTRunName "Run 1" MaxMDLFileLineLength 120 + LastSavedArchitecture "win64" Object { $PropName "BdWindowsInfo" $ObjectID 1 @@ -17,7 +27,7 @@ Library { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [42.0, 23.0, 1750.0, 1097.0] + Location [-6.0, 0.0, 781.0, 830.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -27,6 +37,7 @@ Library { Width [50] Height [50] Filter [9] + Minimized "Unset" } Object { $PropName "ExplorerBarInfo" @@ -40,112 +51,185 @@ Library { Object { $ObjectID 5 IsActive [1] + IsTabbed [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1712.0, 868.0] - ZoomFactor [2.7153053132434577] - Offset [-71.812363004544125, -35.771685034737587] + Extents [913.0, 744.0] + ZoomFactor [2.0] + Offset [-51.647344237659695, -39.161038908920773] + SceneRectInView [-51.647344237659695, -39.161038908920773, 365.2, 297.6] } Object { $ObjectID 6 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "1296" - Extents [1712.0, 868.0] - ZoomFactor [5.2466166877616587] - Offset [-148.63713628266035, -114.21997476247031] + Extents [913.0, 744.0] + ZoomFactor [2.0] + Offset [-184.68563869352414, -164.64890379313098] + SceneRectInView [-184.68563869352414, -164.64890379313098, 365.2, 297.6] } Object { $ObjectID 7 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "192" - Extents [1712.0, 868.0] - ZoomFactor [1.5419454451878538] - Offset [-198.60003045066941, -99.1501929491047] + Extents [913.0, 744.0] + ZoomFactor [1.5] + Offset [371.69049024359208, -20.02193304701251] + SceneRectInView [371.69049024359208, -20.02193304701251, 486.93333333333334, 396.8] } Object { $ObjectID 8 IsActive [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "209" - Extents [2522.0, 1199.0] - ZoomFactor [2.5] - Offset [-258.01185578242905, -157.8236768561307] - } - Object { - $ObjectID 9 - IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "202" - Extents [2522.0, 1199.0] + Extents [1920.0, 1080.0] ZoomFactor [2.4606371641733422] - Offset [213.61276834817875, 99.508993480891093] + Offset [213.61276834817869, 99.508993480891093] + SceneRectInView [213.61276834817869, 99.508993480891093, 624.2285625707126, 351.12856644602584] } Object { - $ObjectID 10 + $ObjectID 9 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "176" - Extents [2522.0, 1199.0] + Extents [1920.0, 1080.0] ZoomFactor [4.0] Offset [235.1044081864668, 52.222402871604459] + SceneRectInView [235.1044081864668, 52.222402871604459, 384.0, 216.0] } Object { - $ObjectID 11 + $ObjectID 10 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "1633" - Extents [2010.0, 839.0] + Extents [1875.0, 754.0] ZoomFactor [6.2999999999999989] - Offset [-19.500372023809547, -8.00793650793652] + Offset [-19.500372023809547, -9.9920634920635081] + SceneRectInView [-19.500372023809547, -9.9920634920635081, 238.09523809523816, 95.746031746031761] } Object { - $ObjectID 12 + $ObjectID 11 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "1663" - Extents [2010.0, 839.0] + Extents [1875.0, 754.0] ZoomFactor [5.95] - Offset [-20.290375525210067, -11.920168067226882] + Offset [-20.290375525210067, -14.021008403361336] + SceneRectInView [-20.290375525210067, -14.021008403361336, 252.10084033613447, 101.3781512605042] } Object { - $ObjectID 13 + $ObjectID 12 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "369" - Extents [2010.0, 839.0] - ZoomFactor [2.3928943762659363] - Offset [129.66449864161302, 95.246736292428238] + Extents [1875.0, 754.0] + ZoomFactor [3.0] + Offset [201.78631633871396, 105.24564709323813] + SceneRectInView [201.78631633871396, 105.24564709323813, 500.0, 201.06666666666666] } Object { - $ObjectID 14 + $ObjectID 13 IsActive [0] + IsTabbed [1] ViewObjType "SimulinkSubsys" LoadSaveID "206" - Extents [1943.0, 884.0] + Extents [1875.0, 754.0] ZoomFactor [6.0] - Offset [252.44396551724137, 145.16666666666663] + Offset [252.44396551724139, 145.16666666666663] + SceneRectInView [252.44396551724139, 145.16666666666663, 250.0, 100.53333333333333] + } + Object { + $ObjectID 14 + IsActive [0] + IsTabbed [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "209" + Extents [1875.0, 754.0] + ZoomFactor [1.0] + Offset [-850.3318557824291, -279.74367685613072] + SceneRectInView [-850.3318557824291, -279.74367685613072, 1500.0, 603.2] } PropName "EditorsInfo" } + Array { + Type "Simulink.DockComponentInfo" + Dimension 2 + Object { + $ObjectID 15 + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [512] + Height [384] + Minimized "Unset" + } + Object { + $ObjectID 16 + Type "Simulink:Editor:ReferencedFiles" + ID "Referenced Files" + Visible [0] + CreateCallback "" + UserData "{\"filterShowRefModels\":\"true\",\"filterShowRefSubs\":\"true\",\"filterShowOnlyDirtyFiles\":\"false\"}\n" + Floating [0] + DockPosition "Left" + Width [512] + Height [384] + Minimized "Unset" + } + PropName "DockComponentsInfo" + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAABPsAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAB5AP////sAAABgAFMAaQBtAHUAb" + "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" + "QBzAAAAAAD/////AAAApwD///8AAAABAAAAAAAAAAD8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" + "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAhYA////AAADvgAAAy8AAAABAAAAAgAAAAEAAAAC/" + "AAAAAA=" + Array { + Type "Cell" + Dimension 0 + PropName "PersistedApps" + } + WindowUuid "2367939c-8f2c-4934-945a-7bf0db2130e7" } + BDUuid "b1b9725e-2637-4af0-b0bd-a6b38f60ac9a" } + HideAutomaticNames on + SequenceViewerTimePrecision 3 + SequenceViewerHistory 1000 Created "Thu Feb 06 02:21:39 2014" Creator "jorhabib" - UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "nunoguedelha" + LastModifiedBy "STraversaro" ModifiedDateFormat "%" - LastModifiedDate "Tue May 18 22:49:47 2021" - RTWModifiedTimeStamp 543278987 - ModelVersionFormat "1.%" + LastModifiedDate "Mon May 24 11:41:32 2021" + RTWModifiedTimeStamp 543755558 + ModelVersionFormat "%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "all" WideLines off ShowLineDimensions off ShowPortDataTypes off + ShowAllPropagatedSignalLabels off + PortDataTypeDisplayFormat "AliasTypeOnly" + ShowEditTimeErrors on + ShowEditTimeWarnings on + ShowEditTimeAdvisorChecks off + ShowPortUnits off ShowDesignRanges off ShowLoopsOnError on IgnoreBidirectionalLines off @@ -153,15 +237,21 @@ Library { ShowTestPointIcons on ShowSignalResolutionIcons on ShowViewerIcons on + VariantCondition off ShowLinearizationAnnotations on + ShowVisualizeInsertedRTB on ShowMarkup on BlockNameDataTip off BlockParametersDataTip off BlockDescriptionStringDataTip off + BlockVariantConditionDataTip off ToolBar on StatusBar on BrowserShowLibraryLinks off + FunctionConnectors off + ScheduleConnectors off BrowserLookUnderMasks off + MultithreadedSim "auto" SimulationMode "normal" PauseTimes "5" NumberOfSteps 1 @@ -171,33 +261,13 @@ Library { LinearizationMsg "none" Profile off ParamWorkspaceSource "MATLABWorkspace" - RecordCoverage off - CovPath "/" - CovSaveName "covdata" - CovMetricSettings "dw" - CovNameIncrementing off - CovHtmlReporting off - CovForceBlockReductionOff on - CovEnableCumulative on - CovSaveCumulativeToWorkspaceVar on - CovSaveSingleToWorkspaceVar on - CovCumulativeVarName "covCumulativeData" - CovCumulativeReport off - CovReportOnPause on - CovModelRefEnable "Off" - CovExternalEMLEnable off - CovSFcnEnable off - CovBoundaryAbsTol 0.000010 - CovBoundaryRelTol 0.010000 - CovUseTimeInterval off - CovStartTime 0 - CovStopTime 0 ExtModeBatchMode off ExtModeEnableFloating on ExtModeTrigType "manual" ExtModeTrigMode "normal" ExtModeTrigPort "1" ExtModeTrigElement "any" + ExtModeTrigSignalOutputPortIndex 0 ExtModeTrigDuration 1000 ExtModeTrigDurationFloating "auto" ExtModeTrigHoldOff 0 @@ -213,24 +283,31 @@ Library { ExtModeSkipDownloadWhenConnect off ExtModeLogAll on ExtModeAutoUpdateStatusClock on + VariantFading on ShowModelReferenceBlockVersion off ShowModelReferenceBlockIO off + OrderedModelArguments on Array { Type "Handle" Dimension 1 Simulink.ConfigSet { $ObjectID 17 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] + Description "" Array { Type "Handle" - Dimension 9 + Dimension 10 Simulink.SolverCC { $ObjectID 18 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] + Description "" Components [] StartTime "0.0" StopTime "10.0" AbsTol "auto" + AutoScaleAbsTol on FixedStep "auto" InitialStep "auto" MaxOrder 5 @@ -243,24 +320,32 @@ Library { MinStep "auto" MaxConsecutiveMinStep "1" RelTol "1e-3" - SolverMode "Auto" - EnableConcurrentExecution off + EnableMultiTasking on ConcurrentTasks off SolverName "ode45" SolverJacobianMethodControl "auto" + DaesscMode "auto" ShapePreserveControl "DisableAll" ZeroCrossControl "UseLocalSettings" ZeroCrossAlgorithm "Nonadaptive" AlgebraicLoopSolver "TrustRegion" + SolverInfoToggleStatus off + IsAutoAppliedInSIP off SolverResetMethod "Fast" PositivePriorityOrder off AutoInsertRateTranBlk off SampleTimeConstraint "Unconstrained" InsertRTBMode "Whenever possible" + SampleTimeProperty [] + DecoupledContinuousIntegration off + MinimalZcImpactIntegration off + ODENIntegrationMethod "ode3" } Simulink.DataIOCC { $ObjectID 19 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] + Description "" Components [] Decimation "1" ExternalInput "[t, u]" @@ -271,7 +356,7 @@ Library { LoadExternalInput off LoadInitialState off SaveFinalState off - SaveCompleteFinalSimState off + SaveOperatingPoint off SaveFormat "Array" SignalLoggingSaveFormat "ModelDataLogs" SaveOutput on @@ -280,6 +365,8 @@ Library { DSMLogging on InspectSignalLogs off VisualizeSimOutput on + StreamToWorkspace off + StreamVariableName "streamout" SaveTime on ReturnWorkspaceOutputs off StateSaveName "xout" @@ -291,10 +378,14 @@ Library { OutputTimes "[]" ReturnWorkspaceOutputsName "out" Refine "1" + LoggingToFile off + DatasetSignalFormat "timeseries" + LoggingFileName "out.mat" + LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { $ObjectID 20 - Version "1.14.3" + Version "21.0.0" Array { Type "Cell" Dimension 9 @@ -309,13 +400,16 @@ Library { Cell "EfficientTunableParamExpr" PropName "DisabledProps" } + Description "" Components [] BlockReduction on BooleanDataType on ConditionallyExecuteInputs on - InlineParams off - UseDivisionForNetSlopeComputation "Off" + DefaultParameterBehavior "Tunable" + UseDivisionForNetSlopeComputation "off" + GainParamInheritBuiltInType off UseFloatMulNetSlope off + InheritOutputTypeSmallerThanSingle off DefaultUnderspecifiedDataType "double" UseSpecifiedMinMax off InlineInvariantSignals off @@ -325,12 +419,15 @@ Library { CachingGlobalReferences off GlobalBufferReuse on StrengthReduction off + AdvancedOptControl "" ExpressionFolding on BooleansAsBitfields off BitfieldContainerType "uint_T" + BitwiseOrLogicalOp "Same as modeled" EnableMemcpy on MemcpyThreshold 64 PassReuseOutputArgsAs "Structure reference" + PassReuseOutputArgsThreshold 12 ExpressionDepthLimit 2147483647 LocalBlockOutputs on RollThreshold 5 @@ -343,21 +440,40 @@ Library { NoFixptDivByZeroProtection off EfficientFloat2IntCast off EfficientMapNaN2IntZero on - OptimizeModelRefInitCode off LifeSpan "inf" MaxStackSize "Inherit from target" BufferReusableBoundary on - SimCompilerOptimization "Off" + SimCompilerOptimization "off" AccelVerboseBuild off + OptimizeBlockOrder "off" + OptimizeDataStoreBuffers on + BusAssignmentInplaceUpdate on + DifferentSizesBufferReuse off + UseRowMajorAlgorithm off + OptimizationLevel "level2" + OptimizationPriority "Balanced" + OptimizationCustomize on + LabelGuidedReuse off + MultiThreadedLoops off + DenormalBehavior "GradualUnderflow" + EfficientTunableParamExpr off } Simulink.DebuggingCC { $ObjectID 21 - Version "1.14.3" + Version "21.0.0" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" Components [] RTPrefix "error" ConsistencyChecking "none" ArrayBoundsChecking "none" SignalInfNanChecking "none" + StringTruncationChecking "error" SignalRangeChecking "none" ReadBeforeWriteMsg "UseLocalSettings" WriteAfterWriteMsg "UseLocalSettings" @@ -370,7 +486,6 @@ Library { UnderspecifiedInitializationDetection "Classic" MergeDetectMultiDrivingBlocksExec "none" CheckExecutionContextPreStartOutputMsg off - CheckExecutionContextRuntimeOutputMsg off SignalResolutionControl "UseLocalSettings" BlockPriorityViolationMsg "warning" MinStepSizeMsg "warning" @@ -385,6 +500,7 @@ Library { MultiTaskRateTransMsg "error" SingleTaskRateTransMsg "none" TasksWithSamePriorityMsg "warning" + ExportedTasksRateTransMsg "none" SigSpecEnsureSampleTimeMsg "warning" CheckMatrixSingularityMsg "none" IntegerOverflowMsg "warning" @@ -400,47 +516,75 @@ Library { UnderSpecifiedDataTypeMsg "none" UnnecessaryDatatypeConvMsg "none" VectorMatrixConversionMsg "none" - FcnCallInpInsideContextMsg "EnableAllAsWarning" + FcnCallInpInsideContextMsg "warning" SignalLabelMismatchMsg "none" UnconnectedInputMsg "warning" UnconnectedOutputMsg "warning" UnconnectedLineMsg "warning" + UseOnlyExistingSharedCode "error" SFcnCompatibilityMsg "none" FrameProcessingCompatibilityMsg "error" UniqueDataStoreMsg "none" BusObjectLabelMismatch "warning" RootOutportRequireBusObject "warning" AssertControl "UseLocalSettings" + AllowSymbolicDim on + RowMajorDimensionSupport off ModelReferenceIOMsg "none" ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" ModelReferenceVersionMismatchMessage "none" ModelReferenceIOMismatchMessage "none" UnknownTsInhSupMsg "warning" ModelReferenceDataLoggingMessage "warning" + ModelReferenceNoExplicitFinalValueMsg "none" ModelReferenceSymbolNameMessage "warning" ModelReferenceExtraNoncontSigs "error" StateNameClashWarn "warning" - SimStateInterfaceChecksumMismatchMsg "warning" - SimStateOlderReleaseMsg "error" + OperatingPointInterfaceChecksumMismatchMsg "warning" + NonCurrentReleaseOperatingPointMsg "error" + ChecksumConsistencyForSSReuse "none" + PregeneratedLibrarySubsystemCodeDiagnostic "warning" + MatchCodeGenerationContextForUpdateDiagram "none" InitInArrayFormatMsg "warning" StrictBusMsg "ErrorLevel1" BusNameAdapt "WarnAndRepair" NonBusSignalsTreatedAsBus "none" + SymbolicDimMinMaxWarning "warning" + LossOfSymbolicDimsSimulationWarning "warning" + LossOfSymbolicDimsCodeGenerationWarning "error" + SymbolicDimsDataTypeCodeGenerationDiagnostic "error" BlockIODiagnostic "none" SFUnusedDataAndEventsDiag "warning" SFUnexpectedBacktrackingDiag "warning" SFInvalidInputDataAccessInChartInitDiag "warning" SFNoUnconditionalDefaultTransitionDiag "warning" SFTransitionOutsideNaturalParentDiag "warning" - SFUnconditionalTransitionShadowingDiag "warning" + SFUnreachableExecutionPathDiag "warning" SFUndirectedBroadcastEventsDiag "warning" SFTransitionActionBeforeConditionDiag "warning" + SFOutputUsedAsStateInMooreChartDiag "error" + SFTemporalDelaySmallerThanSampleTimeDiag "warning" + SFSelfTransitionDiag "warning" + SFExecutionAtInitializationDiag "none" + SFMachineParentedDataDiag "warning" IntegerSaturationMsg "warning" - ModelReferenceCSMismatchMessage "none" + AllowedUnitSystems "all" + UnitsInconsistencyMsg "warning" + AllowAutomaticUnitConversions on + RCSCRenamedMsg "warning" + RCSCObservableMsg "warning" + ForceCombineOutputUpdateInSim off + UnitDatabase "" + UnderSpecifiedDimensionMsg "none" + DebugExecutionForFMUViaOutOfProcess off + ArithmeticOperatorsInVariantConditions "warning" + VariantConditionMismatch "none" } Simulink.HardwareCC { $ObjectID 22 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] + Description "" Components [] ProdBitPerChar 8 ProdBitPerShort 16 @@ -450,6 +594,8 @@ Library { ProdBitPerFloat 32 ProdBitPerDouble 64 ProdBitPerPointer 32 + ProdBitPerSizeT 32 + ProdBitPerPtrDiffT 32 ProdLargestAtomicInteger "Char" ProdLargestAtomicFloat "None" ProdIntDivRoundTo "Undefined" @@ -466,6 +612,8 @@ Library { TargetBitPerFloat 32 TargetBitPerDouble 64 TargetBitPerPointer 32 + TargetBitPerSizeT 32 + TargetBitPerPtrDiffT 32 TargetLargestAtomicInteger "Char" TargetLargestAtomicFloat "None" TargetShiftRightIntArith on @@ -478,18 +626,25 @@ Library { TargetHWDeviceType "Specified" TargetUnknown off ProdEqTarget on + UseEmbeddedCoderFeatures on + UseSimulinkCoderFeatures on + HardwareBoardFeatureSet "EmbeddedCoderHSP" } Simulink.ModelReferenceCC { $ObjectID 23 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] + Description "" Components [] UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + EnableRefExpFcnMdlSchedulingChecks on CheckModelReferenceTargetMessage "error" EnableParallelModelReferenceBuilds off ParallelModelReferenceErrorOnInvalidPool on ParallelModelReferenceMATLABWorkerInit "None" ModelReferenceNumInstancesAllowed "Multi" PropagateVarSize "Infer from blocks in model" + ModelDependencies "" ModelReferencePassRootInputsByReference on ModelReferenceMinAlgLoopOccurrences off PropagateSignalLabelsOutOfModel off @@ -497,19 +652,56 @@ Library { } Simulink.SFSimCC { $ObjectID 24 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] + Description "" Components [] + SimCustomSourceCode "" + SimCustomHeaderCode "" + SimCustomInitializer "" + SimCustomTerminator "" + SimReservedNameArray [] + SimUserSources "" + SimUserIncludeDirs "" + SimUserLibraries "" + SimUserDefines "" + SimCustomCompilerFlags "" + SimCustomLinkerFlags "" SFSimEcho on SimCtrlC on - SimIntegrity on + SimIntegrity "on" SimUseLocalCustomCode off SimParseCustomCode on + SimAnalyzeCustomCode off + SimDebugExecutionForCustomCode off SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + LegacyBehaviorForPersistentVarInContinuousTime off + CustomCodeFunctionArrayLayout [] + DefaultCustomCodeFunctionArrayLayout "NotSpecified" + CustomCodeUndefinedFunction "UseInterfaceOnly" + CustomCodeGlobalsAsFunctionIO off + DefaultCustomCodeDeterministicFunctions "None" + CustomCodeDeterministicFunctions "" + SimHardwareAcceleration "generic" + SimTargetLang "C" + GPUAcceleration off + SimGPUMallocThreshold 200 + SimGPUStackLimitPerThread 1024 + SimGPUErrorChecks off + SimGPUCustomComputeCapability "" + SimGPUCompilerFlags "" + SimDLTargetLibrary "mkl-dnn" + SimDLAutoTuning on } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" $ObjectID 25 - Version "1.14.3" + Version "21.0.0" Array { Type "Cell" Dimension 16 @@ -531,25 +723,43 @@ Library { Cell "CodeProfilingInstrumentation" PropName "DisabledProps" } + Description "" SystemTargetFile "grt.tlc" + HardwareBoard "None" + ShowCustomHardwareApp off + ShowEmbeddedHardwareApp off TLCOptions "" GenCodeOnly off MakeCommand "make_rtw" GenerateMakefile on PackageGeneratedCodeAndArtifacts off + PackageName "" TemplateMakefile "grt_default_tmf" PostCodeGenCommand "" GenerateReport off RTWVerbose on RetainRTWFile off + RTWBuildHooks [] ProfileTLC off TLCDebug off TLCCoverage off TLCAssert off RTWUseLocalCustomCode off RTWUseSimCustomCode off + CustomSourceCode "" + CustomHeaderCode "" + CustomInclude "" + CustomSource "" + CustomLibrary "" + CustomDefine "" + CustomBLASCallback "" + CustomLAPACKCallback "" + CustomFFTCallback "" + CustomInitializer "" + CustomTerminator "" Toolchain "Automatically locate an installed toolchain" BuildConfiguration "Faster Builds" + CustomToolchainOptions [] IncludeHyperlinkInReport off LaunchReport off PortableWordSizes off @@ -557,9 +767,10 @@ Library { CodeExecutionProfiling off CodeExecutionProfileVariable "executionProfile" CodeProfilingSaveOptions "SummaryOnly" - CodeProfilingInstrumentation off + CodeProfilingInstrumentation "off" SILDebugging off TargetLang "C" + GenerateGPUCode "None" IncludeBusHierarchyInRTWFileBlockHierarchyMap off GenerateTraceInfo off GenerateTraceReport off @@ -569,16 +780,35 @@ Library { GenerateWebview off GenerateCodeMetricsReport off GenerateCodeReplacementReport off - RTWCompilerOptimization "Off" + GenerateMissedCodeReplacementReport off + RTWCompilerOptimization "off" + ObjectivePriorities [] RTWCustomCompilerOptimizations "" CheckMdlBeforeBuild "Off" SharedConstantsCachingThreshold 1024 + GPUKernelNamePrefix "" + GPUDeviceID -1 + GPUMallocMode "discrete" + GPUMallocThreshold 200 + GPUStackLimitPerThread 1024 + GPUcuBLAS on + GPUcuSOLVER on + GPUcuFFT on + GPUErrorChecks off + GPUComputeCapability "3.5" + GPUCustomComputeCapability "" + GPUCompilerFlags "" + GPUMaximumBlocksPerKernel 0 + DLTargetLibrary "none" + DLAutoTuning on + DLArmComputeVersion "19.05" + DLArmComputeArch "unspecified" Array { Type "Handle" Dimension 2 Simulink.CodeAppCC { $ObjectID 26 - Version "1.14.3" + Version "21.0.0" Array { Type "Cell" Dimension 28 @@ -612,7 +842,9 @@ Library { Cell "BlockCommentType" PropName "DisabledProps" } + Description "" Components [] + Comment "" ForceParamTrailComments off GenerateComments on CommentStyle "Auto" @@ -627,32 +859,46 @@ Library { SFDataObjDesc off MATLABFcnDesc off MangleLength 1 + SharedChecksumLength 8 CustomSymbolStrGlobalVar "$R$N$M" CustomSymbolStrType "$N$R$M_T" CustomSymbolStrField "$N$M" CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrModelFcn "$R$N" CustomSymbolStrFcnArg "rt$I$N$M" CustomSymbolStrBlkIO "rtb_$N$M" CustomSymbolStrTmpVar "$N$M" CustomSymbolStrMacro "$R$N$M" CustomSymbolStrUtil "$N$C" + CustomSymbolStrEmxType "emxArray_$M$N" + CustomSymbolStrEmxFcn "emx$M$N" + CustomUserTokenString "" + CustomCommentsFcn "" DefineNamingRule "None" + DefineNamingFcn "" ParamNamingRule "None" + ParamNamingFcn "" SignalNamingRule "None" + SignalNamingFcn "" InsertBlockDesc off InsertPolySpaceComments off SimulinkBlockComments on + BlockCommentType "BlockPathComment" + StateflowObjectComments on MATLABSourceComments off EnableCustomComments off + InternalIdentifierFile "" InternalIdentifier "Shortened" InlinedPrmAccess "Literals" ReqsInCode off UseSimReservedNames off + ReservedNameArray [] + EnumMemberNameClash "error" } Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" $ObjectID 27 - Version "1.14.3" + Version "21.0.0" Array { Type "Cell" Dimension 19 @@ -677,6 +923,7 @@ Library { Cell "PreserveStateflowLocalDataDimensions" PropName "DisabledProps" } + Description "" Components [] TargetFcnLib "ansi_tfl_table_tmw.mat" TargetLibSuffix "" @@ -685,9 +932,12 @@ Library { TargetLangStandard "C89/C90 (ANSI)" CodeReplacementLibrary "None" UtilityFuncGeneration "Auto" - ERTMultiwordTypeDef "System defined" + MultiwordTypeDef "System defined" MultiwordLength 2048 + DynamicStringBufferSize 256 GenerateFullHeader on + InferredTypesCompatibility off + ExistingSharedCode "" GenerateSampleERTMain off GenerateTestInterfaces off ModelReferenceCompliant on @@ -697,6 +947,7 @@ Library { IncludeMdlTerminateFcn on CombineOutputUpdateFcns off CombineSignalStateStructs off + GroupInternalDataByFunction off SuppressErrorStatus off IncludeFileDelimiter "Auto" ERTCustomFileBanners off @@ -704,11 +955,13 @@ Library { LogVarNameModifier "rt_" MatFileLogging on CodeInterfacePackaging "Nonreusable function" + PurelyIntegerCode off SupportNonFinite on SupportComplex on - PurelyIntegerCode off SupportContinuousTime on SupportNonInlinedSFcns on + RemoveDisableFunc off + RemoveResetFunc off SupportVariableSizeSignals off ParenthesesLevel "Nominal" CastingMode "Nominal" @@ -716,9 +969,21 @@ Library { ModelStepFunctionPrototypeControlCompliant off CPPClassGenCompliant on AutosarCompliant off + MDXCompliant off GRTInterface on GenerateAllocFcn off + UseToolchainInfoCompliant on GenerateSharedConstants on + CoderGroups [] + AccessMethods [] + LookupTableObjectStructAxisOrder "1,2,3,4,..." + LUTObjectStructOrderExplicitValues "Size,Breakpoints,Table" + LUTObjectStructOrderEvenSpacing "Size,Breakpoints,Table" + ArrayLayout "Column-major" + UnsupportedSFcnMsg "error" + ERTHeaderFileRootName "$R$E" + ERTSourceFileRootName "$R$E" + ERTDataFileRootName "$R_data" UseMalloc off ExtMode off ExtModeStaticAlloc off @@ -726,6 +991,7 @@ Library { ExtModeStaticAllocSize 1000000 ExtModeTransport 0 ExtModeMexFile "ext_comm" + ExtModeMexArgs "" ExtModeIntrfLevel "Level1" RTWCAPISignals off RTWCAPIParams off @@ -737,9 +1003,49 @@ Library { PropName "Components" } } + SlCovCC.ConfigComp { + $ObjectID 28 + Version "21.0.0" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + Components [] + Name "Simulink Coverage" + CovEnable off + CovScope "EntireSystem" + CovIncludeTopModel on + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovCompData "" + CovMetricSettings "dw" + CovFilter "" + CovHTMLOptions "" + CovNameIncrementing off + CovForceBlockReductionOff on + CovEnableCumulative on + CovSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovSaveOutputData on + CovOutputDir "slcov_output/$ModelName$" + CovDataFileName "$ModelName$_cvdata" + CovReportOnPause on + CovModelRefEnable "off" + CovModelRefExcluded "" + CovExternalEMLEnable off + CovSFcnEnable off + CovBoundaryAbsTol 1e-05 + CovBoundaryRelTol 0.01 + CovUseTimeInterval off + CovStartTime 0 + CovStopTime 0 + CovMcdcMode "Masking" + } hdlcoderui.hdlcc { $ObjectID 29 - Version "1.14.3" + Version "21.0.0" + DisabledProps [] Description "HDL Coder custom configuration component" Components [] Name "HDL Coder" @@ -756,6 +1062,7 @@ Library { Name "Configuration" CurrentDlgPage "Solver" ConfigPrmDlgPosition [ 963, 135, 1843, 765 ] + ExtraOptions "" } PropName "ConfigurationSets" } @@ -764,12 +1071,13 @@ Library { ForegroundColor "black" BackgroundColor "white" DropShadow off - NamePlacement "normal" + NameLocation "bottom" FontName "Helvetica" FontSize 10 FontWeight "normal" FontAngle "normal" ShowName on + HideAutomaticName on BlockRotation 0 BlockMirror off } @@ -783,7 +1091,12 @@ Library { FontSize 10 FontWeight "normal" FontAngle "normal" + MarkupType "model" UseDisplayTextAsClickCallback off + AnnotationType "note_annotation" + FixedHeight off + FixedWidth off + Interpreter "off" } LineDefaults { FontName "Helvetica" @@ -794,8 +1107,8 @@ Library { MaskDefaults { SelfModifiable "off" IconFrame "on" - IconOpaque "on" - RunInitForIconRedraw "off" + IconOpaque "opaque" + RunInitForIconRedraw "analyze" IconRotate "none" PortRotate "default" IconUnits "autoscale" @@ -871,6 +1184,7 @@ Library { OutDataTypeStr "Inherit: auto" LockScale off BusOutputAsStruct off + Unit "inherit" PortDimensions "-1" VarSizeSig "Inherit" SampleTime "-1" @@ -882,7 +1196,7 @@ Library { } Block { BlockType Memory - X0 "0" + InitialCondition "0" InheritSampleTime off LinearizeMemory off LinearizeAsDelay off @@ -892,20 +1206,26 @@ Library { BlockType Outport Port "1" IconDisplay "Port number" + OutputFunctionCall off OutMin "[]" OutMax "[]" OutDataTypeStr "Inherit: auto" LockScale off BusOutputAsStruct off + Unit "inherit" PortDimensions "-1" VarSizeSig "Inherit" SampleTime "-1" SignalType "auto" SamplingMode "auto" + EnsureOutportIsVirtual off SourceOfInitialOutputValue "Dialog" OutputWhenDisabled "held" InitialOutput "[]" + MustResolveToSignalObject off OutputWhenUnConnected off + OutputWhenUnconnectedValue "0" + VectorParamsAs1DForOutWhenUnconnected on } Block { BlockType Product @@ -927,6 +1247,7 @@ Library { FunctionName "system" SFunctionModules "''" PortCounts "[]" + MultithreadedSim "auto" } Block { BlockType SubSystem @@ -935,12 +1256,14 @@ Library { PermitHierarchicalResolution "All" TreatAsAtomicUnit off MinAlgLoopOccurrences off + ScheduleAs "Sample time" SystemSampleTime "-1" RTWSystemCode "Auto" RTWFcnNameOpts "Auto" RTWFileNameOpts "Auto" FunctionInterfaceSpec "void_void" FunctionWithSeparateData off + MatchGraphicalInterface off RTWMemSecFuncInitTerm "Inherit from model" RTWMemSecFuncExecute "Inherit from model" RTWMemSecDataConstants "Inherit from model" @@ -954,8 +1277,17 @@ Library { MaskHideContents off SFBlockType "NONE" Variant off - GeneratePreprocessorConditionals off + VariantControlMode "expression" + VariantActivationTime "update diagram" + AllowZeroVariantControls off + PropagateVariantConditions off + TreatAsGroupedWhenPropagatingVariantConditions on ContentPreviewEnabled off + IsWebBlock off + IsInjectorSS off + Latency "0" + AutoFrameSizeCalculation off + IsWebBlockPanel off } Block { BlockType Sum @@ -979,8 +1311,12 @@ Library { } System { Name "WBToolboxLibrary_repository" - Location [42, 23, 1792, 1120] - Open off + Location [-6, 0, 775, 830] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "black" @@ -991,9 +1327,10 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "272" + ZoomFactor "200" ReportName "simulink-default.rpt" - SIDHighWatermark "1859" + SIDHighWatermark "1861" + SimulinkSubDomain "Simulink" Block { BlockType SubSystem Name "Actuators" @@ -1013,7 +1350,11 @@ Library { System { Name "Actuators" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1025,6 +1366,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "562" + SimulinkSubDomain "Simulink" Block { BlockType S-Function Name "SetMotorParameters" @@ -1038,6 +1380,7 @@ Library { "tKTau,kTau,setBemf,bemf" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 31 @@ -1338,7 +1681,11 @@ Library { System { Name "SetMotorReferences" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1350,6 +1697,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "1000" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "References" @@ -1368,6 +1716,7 @@ Library { Parameters "'SetReferences','WBToolbox',WBTConfigParameters,configBlockAbsName,controlType,refSpeed" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Line { ZOrder 1 @@ -1513,7 +1862,11 @@ Library { System { Name "SetReferences" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1525,6 +1878,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "1000" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "References" @@ -1543,6 +1897,7 @@ Library { Parameters "'SetReferences','WBToolbox',WBTConfigParameters,configBlockAbsName,controlType,refTrajectory" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Line { ZOrder 1 @@ -1573,8 +1928,12 @@ Library { } System { Name "Model" - Location [67, 148, 2627, 1588] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1585,7 +1944,8 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "250" + ZoomFactor "100" + SimulinkSubDomain "Simulink" Block { BlockType SubSystem Name "Dynamics" @@ -1603,8 +1963,12 @@ Library { } System { Name "Dynamics" - Location [0, 0, 2048, 1080] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1615,7 +1979,8 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "239" + ZoomFactor "300" + SimulinkSubDomain "Simulink" Block { BlockType SubSystem Name "CentroidalMomentum" @@ -1678,7 +2043,11 @@ Library { System { Name "CentroidalMomentum" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1690,6 +2059,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "480" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -1732,6 +2102,7 @@ Library { Parameters "'CentroidalMomentum','WBToolbox',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -1837,7 +2208,11 @@ Library { System { Name "GetBiasForces" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1849,6 +2224,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "395" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -1909,6 +2285,7 @@ Library { Parameters "'InverseDynamics','WBToolbox',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -2036,7 +2413,11 @@ Library { System { Name "GetGravityForces" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2048,6 +2429,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "395" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -2092,6 +2474,7 @@ Library { Parameters "'InverseDynamics','WBToolbox',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -2178,6 +2561,7 @@ Library { Parameters "'InverseDynamics','WBToolbox',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 92 @@ -2290,8 +2674,12 @@ Library { } System { Name "MassMatrix" - Location [0, 0, 2048, 1080] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2303,6 +2691,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "630" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -2329,6 +2718,7 @@ Library { Parameters "'MassMatrix','WBToolbox',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -2381,8 +2771,12 @@ Library { } System { Name "Jacobians" - Location [67, 148, 2627, 1588] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2394,6 +2788,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "246" + SimulinkSubDomain "Simulink" Block { BlockType S-Function Name "CentroidalTotalMomentumMatrix" @@ -2405,6 +2800,7 @@ Library { Parameters "'CentroidalTotalMomentumMatrix', 'WBToolbox', WBTConfigParameters, configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 103 @@ -2526,7 +2922,11 @@ Library { System { Name "DotJNu" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2538,6 +2938,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "595" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -2580,6 +2981,7 @@ Library { Parameters "'DotJNu','WBToolbox',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -2711,8 +3113,12 @@ Library { } System { Name "Jacobian" - Location [0, 0, 2048, 1080] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2724,6 +3130,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "595" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -2750,6 +3157,7 @@ Library { Parameters "'Jacobian','WBToolbox',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -2802,8 +3210,12 @@ Library { } System { Name "Kinematics" - Location [67, 148, 2627, 1588] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2815,6 +3227,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "400" + SimulinkSubDomain "Simulink" Block { BlockType SubSystem Name "ForwardKinematics" @@ -2902,7 +3315,11 @@ Library { System { Name "ForwardKinematics" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2914,6 +3331,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "595" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Base Pose" @@ -2940,6 +3358,7 @@ Library { Parameters "'ForwardKinematics','WBToolbox',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -3163,7 +3582,11 @@ Library { System { Name "InverseKinematics" Location [0, 23, 1280, 744] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3175,6 +3598,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "312" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Desired frame pose" @@ -3209,6 +3633,7 @@ Library { Parameters "'InverseKinematics',robotName,localName,wbiFile,wbiList,baseFrame, endEffFrame,optOption" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -3362,7 +3787,11 @@ Library { System { Name "RelativeTransform" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3374,6 +3803,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "834" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Joint configuration" @@ -3392,6 +3822,7 @@ Library { Parameters "'RelativeTransform','WBToolbox',WBTConfigParameters,configBlockAbsName,frame1, frame2" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -3548,7 +3979,11 @@ Library { System { Name "RemoteInverseKinematics" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3560,6 +3995,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "469" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Desired frame pose" @@ -3586,6 +4022,7 @@ Library { Parameters "'RemoteInverseKinematics',solverName, dofs, optOption" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -3639,8 +4076,12 @@ Library { } System { Name "States" - Location [2568, 323, 4549, 1448] - Open off + Location [-7, -7, 1543, 831] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3652,6 +4093,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "600" + SimulinkSubDomain "Simulink" Block { BlockType SubSystem Name "GetLimits" @@ -3782,7 +4224,11 @@ Library { System { Name "GetLimits" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3795,6 +4241,7 @@ Library { ShowPageBoundaries off ZoomFactor "966" SIDHighWatermark "1773" + SimulinkSubDomain "Simulink" Block { BlockType S-Function Name "S-Function" @@ -3806,6 +4253,7 @@ Library { Parameters "'GetLimits','WBToolbox',WBTConfigParameters,configBlockAbsName,limitsType" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -3932,7 +4380,11 @@ Library { System { Name "GetMeasurement" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3944,6 +4396,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "1000" + SimulinkSubDomain "Simulink" Block { BlockType S-Function Name "S-Function" @@ -3955,6 +4408,7 @@ Library { Parameters "'GetMeasurement','WBToolbox',WBTConfigParameters,configBlockAbsName,measuredType" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -4071,7 +4525,11 @@ Library { System { Name "GetMotorMeasurement" Location [0, 27, 1920, 1080] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4083,6 +4541,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "1000" + SimulinkSubDomain "Simulink" Block { BlockType S-Function Name "S-Function" @@ -4094,6 +4553,7 @@ Library { Parameters "'GetMeasurement','WBToolbox',WBTConfigParameters,configBlockAbsName,measuredType" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off } Block { BlockType Outport @@ -4131,8 +4591,12 @@ Library { } System { Name "Utilities" - Location [42, 23, 1792, 1120] - Open off + Location [-6, 0, 775, 830] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4143,7 +4607,8 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "154" + ZoomFactor "150" + SimulinkSubDomain "Simulink" Block { BlockType SubSystem Name "Configuration" @@ -4345,7 +4810,11 @@ Library { System { Name "Configuration" Location [550, 86, 1677, 725] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4358,6 +4827,7 @@ Library { ShowPageBoundaries off ZoomFactor "600" SIDHighWatermark "70" + SimulinkSubDomain "Simulink" Block { BlockType Constant Name "ImConfig" @@ -4408,7 +4878,11 @@ Library { System { Name "DampPinv" Location [0, 29, 1280, 744] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4420,6 +4894,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "mat" @@ -4444,14 +4919,18 @@ Library { ZOrder -4 LibraryVersion "1.32" ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ParametersOnly" + PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" System { Name "Damped Pseudo Inverse" Location [12, 45, 1279, 3773] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4463,7 +4942,8 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1634" + SIDHighWatermark "1646" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "mat" @@ -4482,24 +4962,25 @@ Library { Block { BlockType Demux Name " Demux " - SID "107::1633" + SID "107::1645" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 113 + ZOrder 125 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "107::1632" - Tag "Stateflow S-Function WBToolboxLibrary_repository 6" + SID "107::1644" + Tag "Stateflow S-Function 6" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 112 + ZOrder 124 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Port { PortNumber 2 Name "DPinv" @@ -4508,9 +4989,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "107::1634" + SID "107::1646" Position [460, 241, 480, 259] - ZOrder 114 + ZOrder 126 } Block { BlockType Outport @@ -4520,7 +5001,7 @@ Library { ZOrder -6 } Line { - ZOrder 86 + ZOrder 106 SrcBlock "mat" SrcPort 1 Points [120, 0] @@ -4528,7 +5009,7 @@ Library { DstPort 1 } Line { - ZOrder 87 + ZOrder 107 SrcBlock "sigma" SrcPort 1 DstBlock " SFunction " @@ -4536,7 +5017,7 @@ Library { } Line { Name "DPinv" - ZOrder 88 + ZOrder 108 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -4544,14 +5025,14 @@ Library { DstPort 1 } Line { - ZOrder 89 + ZOrder 109 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 90 + ZOrder 110 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -4601,6 +5082,7 @@ Library { Parameters "'DiscreteFilter','WBToolbox',filterDataStruct" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 227 @@ -4824,7 +5306,11 @@ Library { System { Name "MatchSignalSizes" Location [548, 183, 3416, 1872] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4836,13 +5322,14 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "1000" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "Reference" SID "1819" Position [315, 178, 345, 192] ZOrder 240 - NamePlacement "alternate" + NameLocation "top" } Block { BlockType Inport @@ -4919,6 +5406,7 @@ Library { "ves,explicitInitialValue,externalSettlingTime,resetOnSettlingTime" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 252 @@ -5090,29 +5578,30 @@ Library { } Block { BlockType S-Function - Name "QP" - SID "1807" + Name "OSQP" + SID "1861" Ports [4, 2] - Position [405, -6, 595, 66] - ZOrder 104 + Position [650, -13, 835, 63] + ZOrder 255 FunctionName "BlockFactory" - Parameters "'QpOases','WBToolbox',lbA,ubA,lb,ub,computeObjVal,stopIfFails" + Parameters "'OSQP','WBToolbox',lbA,ubA,lb,ub,computeObjVal,stopIfFails" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 273 $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' *" - " x\n\ns.t.\n\nlbA <= Ax <= ubA\nlb <= x <= ub \n" - Display "disp('QP')\n\nport_label('output', 1, 'PrimalSolution')\nport_label('output', 2, 'Status')\n\nif (c" - "omputeObjVal)\n port_label('output', 3, 'Obj func value')\nend\n\nport_label('input', 1, 'H');\nport_label('in" - "put', 2, 'g');\n\ninputPortNumber = 2;\n\nif (lbA || ubA)\n inputPortNumber = 3;\n port_label('input', 3, '" - "A');\nend\n\nif (lbA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lbA'" - ");\nend\nif (ubA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ubA');\n" - "end\nif (lb)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lb');\nend\ni" - "f (ub)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ub');\nend" + Type "OSQP" + Description "This block solves a QP problem using the OSQP library:\n\nx = argmin 0.5 * x' * H * x + c' * x\n" + "\ns.t.\n\nlbA <= Ax <= ubA\nlb <= x <= ub \n" + Display "disp('OSQP')\n\nport_label('output', 1, 'PrimalSolution')\nport_label('output', 2, 'Status')\n\nif " + "(computeObjVal)\n port_label('output', 3, 'Obj func value')\nend\n\nport_label('input', 1, 'H');\nport_label('" + "input', 2, 'g');\n\ninputPortNumber = 2;\n\nif (lbA || ubA)\n inputPortNumber = 3;\n port_label('input', 3," + " 'A');\nend\n\nif (lbA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lb" + "A');\nend\nif (ubA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ubA');" + "\nend\nif (lb)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lb');\nend\n" + "if (ub)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ub');\nend" RunInitForIconRedraw "on" Array { Type "Simulink.MaskParameter" @@ -5244,6 +5733,163 @@ Library { } } } + Block { + BlockType S-Function + Name "QP" + SID "1807" + Ports [4, 2] + Position [405, -6, 595, 66] + ZOrder 104 + FunctionName "BlockFactory" + Parameters "'QpOases','WBToolbox',lbA,ubA,lb,ub,computeObjVal,stopIfFails" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 292 + $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' *" + " x\n\ns.t.\n\nlbA <= Ax <= ubA\nlb <= x <= ub \n" + Display "disp('QP')\n\nport_label('output', 1, 'PrimalSolution')\nport_label('output', 2, 'Status')\n\nif (c" + "omputeObjVal)\n port_label('output', 3, 'Obj func value')\nend\n\nport_label('input', 1, 'H');\nport_label('in" + "put', 2, 'g');\n\ninputPortNumber = 2;\n\nif (lbA || ubA)\n inputPortNumber = 3;\n port_label('input', 3, '" + "A');\nend\n\nif (lbA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lbA'" + ");\nend\nif (ubA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ubA');\n" + "end\nif (lb)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lb');\nend\ni" + "f (ub)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ub');\nend" + RunInitForIconRedraw "on" + Array { + Type "Simulink.MaskParameter" + Dimension 6 + Object { + $ObjectID 293 + Type "checkbox" + Name "lbA" + Prompt "Accept constraints lower bound input (lbA)" + Value "off" + } + Object { + $ObjectID 294 + Type "checkbox" + Name "ubA" + Prompt "Accept constraints upper bound input (ubA)" + Value "on" + } + Object { + $ObjectID 295 + Type "checkbox" + Name "lb" + Prompt "Accept lower bound input (lb)" + Value "off" + } + Object { + $ObjectID 296 + Type "checkbox" + Name "ub" + Prompt "Accept upper bound input (ub)" + Value "off" + } + Object { + $ObjectID 297 + Type "checkbox" + Name "computeObjVal" + Prompt "Output the value of the objective function" + Value "off" + } + Object { + $ObjectID 298 + Type "checkbox" + Name "stopIfFails" + Prompt "Stop the simulation if the solver fails" + Value "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 299 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 300 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 301 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 302 + Prompt "Constraints bounds" + Array { + Type "Simulink.dialog.parameter.CheckBox" + Dimension 2 + Object { + $ObjectID 303 + Name "lbA" + } + Object { + $ObjectID 304 + Name "ubA" + } + PropName "DialogControls" + } + Name "constraintsBoundsBox" + } + Object { + $ObjectID 305 + Prompt "Simple bounds" + Array { + Type "Simulink.dialog.parameter.CheckBox" + Dimension 2 + Object { + $ObjectID 306 + Name "lb" + } + Object { + $ObjectID 307 + Name "ub" + } + PropName "DialogControls" + } + Name "simpleBoundsBox" + } + Object { + $ObjectID 308 + Prompt "Other" + Array { + Type "Simulink.dialog.parameter.CheckBox" + Dimension 2 + Object { + $ObjectID 309 + Name "computeObjVal" + } + Object { + $ObjectID 310 + Name "stopIfFails" + } + PropName "DialogControls" + } + Name "otherBox" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } Block { BlockType S-Function Name "Real Time Synchronizer" @@ -5258,9 +5904,10 @@ Library { Parameters "'RealTimeSynchronizer','WBToolbox',period" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 292 + $ObjectID 311 $ClassName "Simulink.Mask" Type "Real Time Synchronizer" Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" @@ -5268,7 +5915,7 @@ Library { Display "disp('Real Time Synchronizer')" Object { $PropName "Parameters" - $ObjectID 293 + $ObjectID 312 $ClassName "Simulink.MaskParameter" Type "edit" Name "period" @@ -5290,9 +5937,10 @@ Library { Parameters "'SimulatorSynchronizer','WBToolbox',period, serverPortName, clientPortName" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 294 + $ObjectID 313 $ClassName "Simulink.Mask" Type "Simulator Synchronizer" Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" @@ -5302,21 +5950,21 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 295 + $ObjectID 314 Type "edit" Name "period" Prompt "Controller Period (in seconds)" Value "0.01" } Object { - $ObjectID 296 + $ObjectID 315 Type "edit" Name "serverPortName" Prompt "Server Port Name" Value "'/clock/rpc'" } Object { - $ObjectID 297 + $ObjectID 316 Type "edit" Name "clientPortName" Prompt "Client Port Name" @@ -5337,11 +5985,11 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 298 + $ObjectID 317 $ClassName "Simulink.Mask" Object { $PropName "Parameters" - $ObjectID 299 + $ObjectID 318 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -5352,7 +6000,11 @@ Library { System { Name "TruncPinv" Location [0, 29, 1280, 744] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5364,6 +6016,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "S" @@ -5388,14 +6041,18 @@ Library { ZOrder -4 LibraryVersion "1.32" ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ParametersOnly" + PermitHierarchicalResolution "ExplicitOnly" TreatAsAtomicUnit on RequestExecContextInheritance off SFBlockType "MATLAB Function" System { Name "Truncated PseudoInverse" Location [12, 45, 1279, 3773] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5407,7 +6064,8 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1625" + SIDHighWatermark "1637" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "mat" @@ -5426,24 +6084,25 @@ Library { Block { BlockType Demux Name " Demux " - SID "112::1624" + SID "112::1636" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 113 + ZOrder 125 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "112::1623" - Tag "Stateflow S-Function WBToolboxLibrary_repository 7" + SID "112::1635" + Tag "Stateflow S-Function 7" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 112 + ZOrder 124 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Port { PortNumber 2 Name "TPinv" @@ -5452,9 +6111,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "112::1625" + SID "112::1637" Position [460, 241, 480, 259] - ZOrder 114 + ZOrder 126 } Block { BlockType Outport @@ -5464,7 +6123,7 @@ Library { ZOrder -6 } Line { - ZOrder 86 + ZOrder 106 SrcBlock "mat" SrcPort 1 Points [120, 0] @@ -5472,7 +6131,7 @@ Library { DstPort 1 } Line { - ZOrder 87 + ZOrder 107 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -5480,7 +6139,7 @@ Library { } Line { Name "TPinv" - ZOrder 88 + ZOrder 108 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -5488,14 +6147,14 @@ Library { DstPort 1 } Line { - ZOrder 89 + ZOrder 109 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 90 + ZOrder 110 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -5545,9 +6204,10 @@ Library { Parameters "'YarpClock','WBToolbox'" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 300 + $ObjectID 319 $ClassName "Simulink.Mask" Type "YARP Clock" Description "This block outputs the current YARP Time.\nIn a nutshell, this block outputs the equivalent of " @@ -5568,9 +6228,10 @@ Library { Parameters "'YarpRead','WBToolbox',portName,signalSize,blocking,timestamp,autoconnect,errorOnConnection,timeout" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 301 + $ObjectID 320 $ClassName "Simulink.Mask" Type "YARP Read" Description "This block behaves as the command 'yarp read'. \n\nBy default it has the option 'Autoconnect' a" @@ -5590,21 +6251,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 302 + $ObjectID 321 Type "edit" Name "portName" Prompt "Source Port Name" Value "'/portname'" } Object { - $ObjectID 303 + $ObjectID 322 Type "edit" Name "signalSize" Prompt "Port Size" Value "1" } Object { - $ObjectID 304 + $ObjectID 323 Type "edit" Name "timeout" Prompt "Timeout" @@ -5612,7 +6273,7 @@ Library { Visible "off" } Object { - $ObjectID 305 + $ObjectID 324 Type "checkbox" Name "blocking" Prompt "Blocking Read" @@ -5623,14 +6284,14 @@ Library { "king_val mask_visibility" } Object { - $ObjectID 306 + $ObjectID 325 Type "checkbox" Name "timestamp" Prompt "Read Timestamp" Value "on" } Object { - $ObjectID 307 + $ObjectID 326 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -5643,7 +6304,7 @@ Library { "ibilities" } Object { - $ObjectID 308 + $ObjectID 327 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -5665,9 +6326,10 @@ Library { Parameters "'YarpWrite','WBToolbox',portName,autoconnect,errorOnConnection" SFunctionDeploymentMode off EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 309 + $ObjectID 328 $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" @@ -5680,14 +6342,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 310 + $ObjectID 329 Type "edit" Name "portName" Prompt "Opened Port Name" Value "'/portname'" } Object { - $ObjectID 311 + $ObjectID 330 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -5698,7 +6360,7 @@ Library { "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" } Object { - $ObjectID 312 + $ObjectID 331 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -5720,7 +6382,7 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 313 + $ObjectID 332 $ClassName "Simulink.Mask" Type "Errors" Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" @@ -5729,7 +6391,11 @@ Library { System { Name "errors" Location [0, 29, 1280, 744] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open off + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5741,6 +6407,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "x" @@ -5852,15 +6519,19 @@ Library { RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 314 + $ObjectID 333 $ClassName "Simulink.Mask" Type "Holder" Description "This block holds the first input value during the simulation." } System { Name "holder\n" - Location [42, 23, 1792, 1120] - Open off + Location [-6, 0, 775, 830] + SystemRect [0.000000, 0.000000, 0.000000, 0.000000] + Open on + PortBlocksUseCompactNotation off + SetExecutionDomain off + ExecutionDomainType "Deduce" ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5871,7 +6542,8 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "525" + ZoomFactor "200" + SimulinkSubDomain "Simulink" Block { BlockType Inport Name "s" @@ -5988,7 +6660,7 @@ Library { Annotation { SID "1213" Name "WHOLE BODY TOOLBOX" - Position [241, 157, 380, 174] + Position [172, 149, 304, 165] InternalMargins [0, 0, 0, 0] ForegroundColor "white" BackgroundColor "black" @@ -6011,7 +6683,7 @@ Library { "1) http://www.codyco.eu

\n

H2020 EU Project AnDy (No. 731540 H" "2020-ICT-2016-1) http://andy-project.eu

" - Position [245, 189, 421, 216] + Position [157, 176, 333, 208] InternalMargins [0, 0, 0, 0] ForegroundColor "white" BackgroundColor "black" @@ -6023,16 +6695,14 @@ Library { } #Finite State Machines # -# Stateflow 80000036 +# Stateflow 80000041 # # Stateflow { machine { id 1 name "WBToolboxLibrary_repository" - created "06-Feb-2014 02:21:39" - isLibrary 1 - sfVersion 80000001 + sfVersion 80000041 firstTarget 18 } chart { @@ -6092,6 +6762,9 @@ Stateflow { primitive SF_DOUBLE_TYPE } complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } } dataType "Inherit: Same as Simulink" linkNode [2 0 5] @@ -6112,6 +6785,9 @@ Stateflow { } complexity SF_COMPLEX_INHERITED frame SF_FRAME_NO + unit { + name "inherit" + } } dataType "Inherit: Same as Simulink" linkNode [2 4 6] @@ -6134,6 +6810,9 @@ Stateflow { } complexity SF_COMPLEX_INHERITED frame SF_FRAME_INHERITED + unit { + name "inherit" + } } dataType "Inherit: Same as Simulink" linkNode [2 5 0] @@ -6233,6 +6912,9 @@ Stateflow { primitive SF_DOUBLE_TYPE } complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } } dataType "Inherit: Same as Simulink" linkNode [10 0 13] @@ -6253,6 +6935,9 @@ Stateflow { } complexity SF_COMPLEX_INHERITED frame SF_FRAME_NO + unit { + name "inherit" + } } dataType "Inherit: Same as Simulink" linkNode [10 12 14] @@ -6275,6 +6960,9 @@ Stateflow { } complexity SF_COMPLEX_INHERITED frame SF_FRAME_INHERITED + unit { + name "inherit" + } } dataType "Inherit: Same as Simulink" linkNode [10 13 0] diff --git a/matlab/library/exported/WBToolboxLibrary.slx b/matlab/library/exported/WBToolboxLibrary.slx index 6ff0587e2..b489513c2 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 6869ca360..c2f365053 100644 --- a/toolbox/library/CMakeLists.txt +++ b/toolbox/library/CMakeLists.txt @@ -16,6 +16,11 @@ if(NOT WBT_USES_QPOASES) option(WBT_USES_QPOASES "Build blocks that depend on qpOASES library" ${qpOASES_FOUND}) endif() +find_package(OsqpEigen) +if(NOT WBT_USES_OSQPEIGEN) + option(WBT_USES_OSQPEIGEN "Build blocks that depend on osqp-eigen library" ${OsqpEigen_FOUND}) +endif() + # =================== # BLOCKFACTORY PLUGIN # =================== @@ -52,6 +57,10 @@ if(WBT_USES_QPOASES) list(APPEND WBT_BLOCKS "QpOases") endif() +if(WBT_USES_OSQPEIGEN) + list(APPEND WBT_BLOCKS "OSQP") +endif() + # Include CMake helpers provided by BlockFactory include(BlockFactoryPlugin) @@ -120,5 +129,10 @@ if(WBT_USES_QPOASES) target_include_directories(WBToolbox PRIVATE ${qpOASES_INCLUDE_DIRS}) endif() +if(WBT_USES_OSQPEIGEN) + target_compile_definitions(WBToolbox PRIVATE "WBT_USES_OSQPEIGEN") + target_link_libraries(WBToolbox PRIVATE OsqpEigen::OsqpEigen) +endif() + # Install the plugin install_blockfactory_plugin(WBToolbox) diff --git a/toolbox/library/include/WBToolbox/Block/OSQP.h b/toolbox/library/include/WBToolbox/Block/OSQP.h new file mode 100644 index 000000000..acf1cdfa2 --- /dev/null +++ b/toolbox/library/include/WBToolbox/Block/OSQP.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef WBT_OSQP_H +#define WBT_OSQP_H + +#include + +#include +#include + +namespace wbt { + namespace block { + class OSQP; + } // namespace block +} // namespace wbt + +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + +/** + * @brief The wbt::OSQP class + * + * @section Parameters + * + * In addition to @ref block_parameters, wbt::OSQP requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ParameterType::BOOL | 0 + Block::NumberOfParameters | 1 | 1 | "UseLba" | + * | ParameterType::BOOL | 1 + Block::NumberOfParameters | 1 | 1 | "UseUbA" | + * | ParameterType::BOOL | 2 + Block::NumberOfParameters | 1 | 1 | "UseLb" | + * | ParameterType::BOOL | 3 + Block::NumberOfParameters | 1 | 1 | "UseUb" | + * | ParameterType::BOOL | 4 + Block::NumberOfParameters | 1 | 1 | "ComputeObjVal" | + * | ParameterType::BOOL | 5 + Block::NumberOfParameters | 1 | 1 | "StopWhenFails" | + * + */ +class wbt::block::OSQP final : public blockfactory::core::Block +{ +private: + class impl; + std::unique_ptr pImpl; + bool solverInitialization(const blockfactory::core::BlockInformation* blockInfo); + +public: + OSQP(); + ~OSQP() override; + + unsigned numberOfParameters() override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool + initializeInitialConditions(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; +}; + +#endif // WBT_OSQP_H diff --git a/toolbox/library/src/Factory.cpp b/toolbox/library/src/Factory.cpp index 2fc47df7b..6f88be333 100644 --- a/toolbox/library/src/Factory.cpp +++ b/toolbox/library/src/Factory.cpp @@ -38,6 +38,9 @@ #ifdef WBT_USES_QPOASES #include "WBToolbox/Block/QpOases.h" #endif +#ifdef WBT_USES_OSQPEIGEN +#include "WBToolbox/Block/OSQP.h" +#endif #ifdef WBT_USES_IPOPT // #include "Block/InverseKinematics.h" #endif @@ -100,6 +103,9 @@ SHLIBPP_DEFINE_SHARED_SUBCLASS(MinimumJerkTrajectoryGenerator, #ifdef WBT_USES_QPOASES SHLIBPP_DEFINE_SHARED_SUBCLASS(QpOases, wbt::block::QpOases, blockfactory::core::Block) #endif +#ifdef WBT_USES_OSQPEIGEN +SHLIBPP_DEFINE_SHARED_SUBCLASS(OSQP, wbt::block::OSQP, blockfactory::core::Block) +#endif #ifdef WBT_USES_IPOPT SHLIBPP_DEFINE_SHARED_SUBCLASS(InverseKinematics, wbt::block::InverseKinematics, diff --git a/toolbox/library/src/OSQP.cpp b/toolbox/library/src/OSQP.cpp new file mode 100644 index 000000000..3af6064a5 --- /dev/null +++ b/toolbox/library/src/OSQP.cpp @@ -0,0 +1,588 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#include "WBToolbox/Block/OSQP.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include + +#ifdef ERROR +#undef ERROR +#endif + +using namespace blockfactory::core; + +const unsigned MaxIterations = 100; + +#define WBT_OSQP_INF 1e8 + +// INDICES: PARAMETERS, INPUTS, OUTPUT +// =================================== + +enum ParamIndex +{ + Bias = Block::NumberOfParameters - 1, + UseLbA, + UseUbA, + UseLb, + UseUb, + ComputeObjVal, + StopWhenFails, +}; + +enum InputIndex +{ + Hessian = 0, + Gradient, + // Other optional inputs +}; + +static size_t InputIndex_constraints = InputIndex::Gradient; +static size_t InputIndex_lbA = InputIndex::Gradient; +static size_t InputIndex_ubA = InputIndex::Gradient; +static size_t InputIndex_lb = InputIndex::Gradient; +static size_t InputIndex_ub = InputIndex::Gradient; + +enum OutputIndex +{ + PrimalSolution = 0, + Status, + // Other optional inputs +}; + +static size_t OutputIndex_objVal = OutputIndex::Status; + +// BLOCK PIMPL +// =========== + +using MatrixXdSimulink = Eigen::Matrix; + +class wbt::block::OSQP::impl +{ +public: + std::unique_ptr sqSolver; + bool useLbA; + bool useUbA; + bool useLb; + bool useUb; + bool computeObjVal; + bool stopWhenFails; + // Buffers + size_t numberOfVariables; + size_t numberOfTotalConstraints; + size_t numberOfProperConstraints; + Eigen::VectorXd properConstraintsLowerBounds; + Eigen::VectorXd properConstraintsUpperBounds; + Eigen::MatrixXd properConstraintMatrix; + Eigen::SparseMatrix properConstraintMatrixSparse; + Eigen::VectorXd variableConstraintsLowerBounds; + Eigen::VectorXd variableConstraintsUpperBounds; + Eigen::VectorXd totalConstraintsLowerBounds; + Eigen::VectorXd totalConstraintsUpperBounds; + Eigen::MatrixXd totalConstraintsMatrix; + Eigen::SparseMatrix totalConstraintsMatrixSparse; +}; + +// BLOCK CLASS +// =========== + +wbt::block::OSQP::OSQP() + : pImpl{new impl()} +{} + +wbt::block::OSQP::~OSQP() = default; + +unsigned wbt::block::OSQP::numberOfParameters() +{ + return Block::numberOfParameters() + 6; +} + +bool wbt::block::OSQP::parseParameters(BlockInformation* blockInfo) +{ + const std::vector metadata{ + {ParameterType::BOOL, ParamIndex::UseLbA, 1, 1, "UseLbA"}, + {ParameterType::BOOL, ParamIndex::UseUbA, 1, 1, "UseUbA"}, + {ParameterType::BOOL, ParamIndex::UseLb, 1, 1, "UseLb"}, + {ParameterType::BOOL, ParamIndex::UseUb, 1, 1, "UseUb"}, + {ParameterType::BOOL, ParamIndex::ComputeObjVal, 1, 1, "ComputeObjVal"}, + {ParameterType::BOOL, ParamIndex::StopWhenFails, 1, 1, "StopWhenFails"}, + }; + + for (const auto& md : metadata) { + if (!blockInfo->addParameterMetadata(md)) { + bfError << "Failed to store parameter metadata"; + return false; + } + } + + return blockInfo->parseParameters(m_parameters); +} + +bool wbt::block::OSQP::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // PARAMETERS + // ========== + + if (!OSQP::parseParameters(blockInfo)) { + bfError << "Failed to parse parameters."; + return false; + } + + bool useLbA = false; + bool useUbA = false; + bool useLb = false; + bool useUb = false; + bool computeObjVal = false; + + bool ok = true; + ok = ok && m_parameters.getParameter("UseLbA", useLbA); + ok = ok && m_parameters.getParameter("UseUbA", useUbA); + ok = ok && m_parameters.getParameter("UseLb", useLb); + ok = ok && m_parameters.getParameter("UseUb", useUb); + ok = ok && m_parameters.getParameter("ComputeObjVal", computeObjVal); + + if (!ok) { + bfError << "Failed to get parameters after their parsing."; + return false; + } + + // INPUTS + // ====== + // + // 1) Hessian Matrix (nV x nV) + // 2) Gradient vector (1 x nV) + // 3) Optional: Constraints matrix (nC x nV) + // 4) Optional: Constraints lower bounds (1 x nV) + // 5) Optional: Constraints upper bounds (1 x nV) + // 6) Optional: Variables lower bounds (1 x nC) + // 7) Optional: Variables upper bounds (1 x nC) + // + // OUTPUT + // ====== + // + // 1) Primal solution (1 x nV) + // 2) Status of the qp solver (1x1) + // 3) Optional: Value of the object function (1x1) + + InputPortsInfo inputPortsInfo; + OutputPortsInfo outputPortsInfo; + + // Inputs + inputPortsInfo.push_back({InputIndex::Hessian, + Port::Dimensions{Port::DynamicSize, Port::DynamicSize}, + Port::DataType::DOUBLE}); + inputPortsInfo.push_back( + {InputIndex::Gradient, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + outputPortsInfo.push_back( + {OutputIndex::PrimalSolution, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + outputPortsInfo.push_back({OutputIndex::Status, Port::Dimensions{1}, Port::DataType::DOUBLE}); + + // Optional inputs + size_t numberOfInputs = InputIndex::Gradient; + if (useLbA || useUbA) { + InputIndex_constraints = ++numberOfInputs; + inputPortsInfo.push_back({InputIndex_constraints, + Port::Dimensions{Port::DynamicSize, Port::DynamicSize}, + Port::DataType::DOUBLE}); + } + if (useLbA) { + InputIndex_lbA = ++numberOfInputs; + inputPortsInfo.push_back( + {InputIndex_lbA, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + } + if (useUbA) { + InputIndex_ubA = ++numberOfInputs; + inputPortsInfo.push_back( + {InputIndex_ubA, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + } + if (useLb) { + InputIndex_lb = ++numberOfInputs; + inputPortsInfo.push_back( + {InputIndex_lb, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + } + if (useUb) { + InputIndex_ub = ++numberOfInputs; + inputPortsInfo.push_back( + {InputIndex_ub, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + } + + // Optional outputs + size_t numberOfOutputs = OutputIndex::Status; + if (computeObjVal) { + OutputIndex_objVal = ++numberOfOutputs; + outputPortsInfo.push_back( + {OutputIndex_objVal, Port::Dimensions{1}, Port::DataType::DOUBLE}); + } + + if (!blockInfo->setPortsInfo(inputPortsInfo, outputPortsInfo)) { + bfError << "Failed to configure input / output ports."; + return false; + } + + return true; +} + +bool wbt::block::OSQP::solverInitialization(const BlockInformation*blockInfo) +{ + // Check the hessian size + const auto size_H = blockInfo->getInputPortMatrixSize(InputIndex::Hessian); + pImpl->numberOfVariables = size_H.rows; + if (size_H.rows != size_H.cols) { + bfError << "The Hessian matrix should be square."; + return false; + } + + // Check the gradient size + const auto size_g = blockInfo->getInputPortWidth(InputIndex::Gradient); + if (size_g != pImpl->numberOfVariables) { + bfError << "The gradient size does not match with the Hessian size."; + return false; + } + + // Check the simple bounds size + bool ok = true; + if (pImpl->useLb) { + ok = ok && (blockInfo->getInputPortWidth(InputIndex_lb) == pImpl->numberOfVariables); + } + if (pImpl->useUb) { + ok = ok && (blockInfo->getInputPortWidth(InputIndex_ub) == pImpl->numberOfVariables); + } + if (!ok) { + bfError << "Sizes of bounds do not match with the number of variables."; + return false; + } + + pImpl->numberOfTotalConstraints = 0; + pImpl->numberOfProperConstraints = 0; + if (pImpl->useLbA || pImpl->useUbA) { + // Check the constraints size + const auto size_c = blockInfo->getInputPortMatrixSize(InputIndex_constraints); + pImpl->numberOfProperConstraints = size_c.rows; + pImpl->numberOfTotalConstraints += pImpl->numberOfProperConstraints; + if (size_c.cols != pImpl->numberOfVariables) { + bfError << "The column size of the constraints matrix does not match with " + << "the Hessian size"; + return false; + } + + // Check the constraints' bound size + bool ok = true; + if (pImpl->useLbA) { + ok = ok && (blockInfo->getInputPortWidth(InputIndex_lbA) == pImpl->numberOfProperConstraints); + } + if (pImpl->useUbA) { + ok = ok && (blockInfo->getInputPortWidth(InputIndex_ubA) == pImpl->numberOfProperConstraints); + } + if (!ok) { + bfError << "Sizes of constraints' bounds do not match with the number of constraints."; + return false; + } + } + + if (pImpl->useLb || pImpl->useUb) { + pImpl->numberOfTotalConstraints += pImpl->numberOfVariables; + } + + // Allocate the solver. + pImpl->sqSolver = std::make_unique(); + + pImpl->sqSolver->data()->setNumberOfVariables(pImpl->numberOfVariables); + pImpl->sqSolver->data()->setNumberOfConstraints(pImpl->numberOfTotalConstraints); + + if (!pImpl->sqSolver) { + bfError << "Failed to allocate the OsqpEigen::Solver object."; + return false; + } + + // Resize buffers + pImpl->totalConstraintsLowerBounds = Eigen::VectorXd::Constant(pImpl->numberOfTotalConstraints, -WBT_OSQP_INF); + pImpl->totalConstraintsUpperBounds = Eigen::VectorXd::Constant(pImpl->numberOfTotalConstraints, WBT_OSQP_INF); + pImpl->totalConstraintsMatrix = Eigen::MatrixXd(pImpl->numberOfTotalConstraints, pImpl->numberOfVariables); + + // Setup options + pImpl->sqSolver->settings()->setVerbosity(true); + + // Set adaptive_rho to false to avoid converge problems + // See https://github.com/oxfordcontrol/osqp/issues/151 + pImpl->sqSolver->settings()->setAdaptiveRho(false); + + // Set warm start to true to permit to just update the values + // of hessians, gradient and constraints + pImpl->sqSolver->settings()->setWarmStart(true); + + +} + +bool wbt::block::OSQP::initialize(BlockInformation* blockInfo) +{ + if (!Block::initialize(blockInfo)) { + return false; + } + + // PARAMETERS + // ========== + + if (!OSQP::parseParameters(blockInfo)) { + bfError << "Failed to parse parameters."; + return false; + } + + bool ok = true; + ok = ok && m_parameters.getParameter("UseLbA", pImpl->useLbA); + ok = ok && m_parameters.getParameter("UseUbA", pImpl->useUbA); + ok = ok && m_parameters.getParameter("UseLb", pImpl->useLb); + ok = ok && m_parameters.getParameter("UseUb", pImpl->useUb); + ok = ok && m_parameters.getParameter("ComputeObjVal", pImpl->computeObjVal); + ok = ok && m_parameters.getParameter("StopWhenFails", pImpl->stopWhenFails); + + if (!ok) { + bfError << "Failed to get parameters after their parsing."; + return false; + } + + // CLASS INITIALIZATION + // ==================== + if (!this->solverInitialization(blockInfo)) { + return false; + } + + + return true; +} + +bool wbt::block::OSQP::initializeInitialConditions(const BlockInformation* /*blockInfo*/) +{ + pImpl->sqSolver->clearSolver(); + return true; +} + + +bool wbt::block::OSQP::output(const BlockInformation* blockInfo) +{ + // INPUTS + // ====== + + // Get the Signals. + // Note: the Hessian is symmetric, no need for conversion from column to row major. + InputSignalPtr hessianSignal = blockInfo->getInputPortSignal(InputIndex::Hessian); + InputSignalPtr gradientSignal = blockInfo->getInputPortSignal(InputIndex::Gradient); + + Eigen::Map hessian_colMajor( + const_cast(hessianSignal->getBuffer()), + blockInfo->getInputPortMatrixSize(InputIndex::Hessian).rows, + blockInfo->getInputPortMatrixSize(InputIndex::Hessian).cols); + + pImpl->sqSolver->data()->setNumberOfVariables(blockInfo->getInputPortMatrixSize(InputIndex::Hessian).rows); + + Eigen::Map gradient_colMajor( + const_cast(gradientSignal->getBuffer()), + blockInfo->getInputPortWidth(InputIndex::Gradient)); + + if (!hessianSignal || !gradientSignal) { + bfError << "Input signals not valid."; + return false; + } + + // OPTIONAL INPUTS + // =============== + if (pImpl->useLbA || pImpl->useUbA) { + InputSignalPtr constraintsSignal = blockInfo->getInputPortSignal(InputIndex_constraints); + if (!constraintsSignal) { + bfError << "Signal for lbA is not valid."; + return false; + } + + Eigen::Map constraints_colMajor( + const_cast(constraintsSignal->getBuffer()), + blockInfo->getInputPortMatrixSize(InputIndex_constraints).rows, + blockInfo->getInputPortMatrixSize(InputIndex_constraints).cols); + + pImpl->properConstraintMatrix = constraints_colMajor; + + if (pImpl->useLbA) { + InputSignalPtr lbASignal = blockInfo->getInputPortSignal(InputIndex_lbA); + if (!lbASignal) { + bfError << "Signal for lbA is not valid."; + return false; + } + + Eigen::Map lbA_colMajor( + const_cast(lbASignal->getBuffer()), + blockInfo->getInputPortWidth(InputIndex_lbA)); + pImpl->properConstraintsLowerBounds = lbA_colMajor; + } + + if (pImpl->useUbA) { + InputSignalPtr ubASignal = blockInfo->getInputPortSignal(InputIndex_ubA); + if (!ubASignal) { + bfError << "Signal for ubA is not valid."; + return false; + } + + Eigen::Map ubA_colMajor( + const_cast(ubASignal->getBuffer()), + blockInfo->getInputPortWidth(InputIndex_ubA)); + pImpl->properConstraintsUpperBounds = ubA_colMajor; + } + } + + if (pImpl->useLb || pImpl->useUb) { + if (pImpl->useLb) { + InputSignalPtr lbSignal = blockInfo->getInputPortSignal(InputIndex_lb); + if (!lbSignal) { + bfError << "Signal for lb is not valid."; + return false; + } + Eigen::Map variableConstraintsLowerBoundsMap(lbSignal->getBuffer(), + lbSignal->getWidth()); + pImpl->variableConstraintsLowerBounds = variableConstraintsLowerBoundsMap; + } + + if (pImpl->useUb) { + InputSignalPtr ubSignal = blockInfo->getInputPortSignal(InputIndex_ub); + if (!ubSignal) { + bfError << "Signal for ub is not valid."; + return false; + } + Eigen::Map variableConstraintsUpperBoundsMap(ubSignal->getBuffer(), + ubSignal->getWidth()); + pImpl->variableConstraintsUpperBounds = variableConstraintsUpperBoundsMap; + } + } + + + // Let's build the actual constraints passed to OSQP + if(pImpl->numberOfTotalConstraints != 0) { + if (pImpl->useLbA) { + pImpl->totalConstraintsLowerBounds.head(pImpl->numberOfProperConstraints) = pImpl->properConstraintsLowerBounds; + } + + if (pImpl->useUbA) { + pImpl->totalConstraintsUpperBounds.head(pImpl->numberOfProperConstraints) = pImpl->properConstraintsUpperBounds; + } + + + if (pImpl->useLb) { + pImpl->totalConstraintsLowerBounds.tail(pImpl->numberOfVariables) = pImpl->variableConstraintsLowerBounds; + } + + if (pImpl->useUb) { + pImpl->totalConstraintsUpperBounds.tail(pImpl->numberOfVariables) = pImpl->variableConstraintsUpperBounds; + } + + if (pImpl->useLbA || pImpl->useUbA) { + pImpl->totalConstraintsMatrix.topRows(pImpl->numberOfProperConstraints) = pImpl->properConstraintMatrix; + } + + if (pImpl->useLb || pImpl->useUb) { + pImpl->totalConstraintsMatrix.bottomRows(pImpl->numberOfVariables) = Eigen::MatrixXd::Identity(pImpl->numberOfVariables, pImpl->numberOfVariables); + } + + if (!pImpl->sqSolver->isInitialized()) { + pImpl->totalConstraintsMatrixSparse = pImpl->totalConstraintsMatrix.sparseView(); + pImpl->sqSolver->data()->setLinearConstraintsMatrix(pImpl->totalConstraintsMatrixSparse); + pImpl->sqSolver->data()->setBounds(pImpl->totalConstraintsLowerBounds, pImpl->totalConstraintsUpperBounds); + } else { + pImpl->totalConstraintsMatrixSparse = pImpl->totalConstraintsMatrix.sparseView(); + pImpl->sqSolver->updateLinearConstraintsMatrix(pImpl->totalConstraintsMatrixSparse); + pImpl->sqSolver->updateBounds(pImpl->totalConstraintsLowerBounds, pImpl->totalConstraintsUpperBounds); + } + } + // OUTPUTS + // ======= + + OutputSignalPtr solutionSignal = blockInfo->getOutputPortSignal(OutputIndex::PrimalSolution); + if (!solutionSignal) { + bfError << "Output signal not valid."; + return false; + } + Eigen::Map solution_colMajor( + const_cast(solutionSignal->getBuffer()), + blockInfo->getOutputPortWidth(OutputIndex::PrimalSolution)); + + + OutputSignalPtr statusSignal = blockInfo->getOutputPortSignal(OutputIndex::Status); + if (!statusSignal) { + bfError << "Status signal not valid."; + return false; + } + Eigen::Map status_colMajor( + const_cast(statusSignal->getBuffer()), + blockInfo->getInputPortWidth(OutputIndex::Status)); + + // Set inputs + Eigen::SparseMatrix hessian_sparse = hessian_colMajor.sparseView(); + + if (!pImpl->sqSolver->isInitialized()) { + pImpl->sqSolver->data()->setHessianMatrix(hessian_sparse); + pImpl->sqSolver->data()->setGradient(gradient_colMajor); + } else { + pImpl->sqSolver->updateHessianMatrix(hessian_sparse); + pImpl->sqSolver->updateGradient(gradient_colMajor); + } + + + // Solve + if(!pImpl->sqSolver->isInitialized()) { + if(!pImpl->sqSolver->initSolver()) { + bfError << "OSQP: initSolver() failed."; + return false; + } + } + + bool solveReturnVal = pImpl->sqSolver->solve(); + + if (pImpl->stopWhenFails && !solveReturnVal) { + bfError << "OSQP: solve() failed."; + return false; + } + + // Get outputs + if (solveReturnVal) { + solution_colMajor = pImpl->sqSolver->getSolution(); + } + + // Set status + double status = solveReturnVal ? 0 : 1; + if (!statusSignal->set(0, status)) { + bfError << "Failed to set status signal."; + return false; + } + + // OPTIONAL OUTPUTS + // ================ + + if (pImpl->computeObjVal) { + + double objVal = pImpl->sqSolver->workspace()->info->obj_val; + + OutputSignalPtr objValSignal = blockInfo->getOutputPortSignal(OutputIndex_objVal); + if (!objValSignal) { + bfError << "Object Value signal not valid."; + return false; + } + + if (!objValSignal->set(0, objVal)) { + bfError << "Failed to set object value signal."; + return false; + } + } + + return true; +}