Skip to content

Commit

Permalink
Merge pull request #127 from ModiaSim/an_moreTests
Browse files Browse the repository at this point in the history 
An more tests
  • Loading branch information
@GerhardHippmann
GerhardHippmann authored Nov 7, 2023
2 parents 4657238 + 57faff1 commit 8f22b8f
Show file tree
Hide file tree
Showing 11 changed files with 3,006 additions and 28 deletions.
25 changes: 20 additions & 5 deletions src/Composition/dynamics.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,11 +146,15 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode
scene::Scene{F} = mbsBuild.mbs.scene
#instantiatedModel = mbsBuild.mbs.instantiatedModel
#gripPair = scene.gripPair
if Modia3D.checkGrippingFeatures(scene, scene.gripPair)
Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair)
else
@error("Andrea: print warning für gripping features")
# printWarnGrip(robotOrDepot, movableObj, waitingPeriod)
if isdefined(scene, :gripPair)
if Modia3D.checkGrippingFeatures(scene, scene.gripPair)
Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair)
if isdefined(scene.gripPair, :enableContactDetection)
scene.collide = scene.gripPair.enableContactDetection
end
else
@error("Print warning für gripping features")
end
end

(worldDummy, revoluteObjects, prismaticObjects, freeMotionObjects, hiddenJointObjects, forceElements, resultElements) = checkMultibodySystemAndGetWorldAndJointsAndForceElementsAndResultElements(partiallyInstantiatedModel.modelName, parameters, modelPath, F)
Expand All @@ -166,6 +170,17 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode
# rebuild MKS
if scene.options.useOptimizedStructure
rebuild_superObjs!(scene, world)

if isdefined(scene, :gripPair)
if Modia3D.checkGrippingFeatures(scene, scene.gripPair)
if isdefined(scene.gripPair, :enableContactDetection)
if scene.options.enableContactDetection && scene.collide
scene.collide = scene.gripPair.enableContactDetection
end
end
end
end

else
error("Full restart is possible only if useOptimizedStructure is enabled in SceneOptions.")
end
Expand Down
1 change: 1 addition & 0 deletions src/GrippingDetection/grippingPair.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ mutable struct GrippingPair{F <: Modia3D.VarFloatType}
movableObj::Composition.Object3D{F} # must be part of a movable unit
# is set for Attach and ReleaseAndAttach
robotOrDepot::Composition.Object3D{F} # must be part of a gripper unit or part of a depot (like bottom or other movable unit)
enableContactDetection::Bool
GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType} = new(gripStatus, movableObj)
GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}, robotOrDepot::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType} = new(gripStatus, movableObj, robotOrDepot)
end
Expand Down
40 changes: 29 additions & 11 deletions src/PathPlanning/referencePathInternal.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ struct ArbitraryMotion
inputArg1 # e.g. positions, waitingPeriod, robotOrDepot
inputArg2 # e.g. nothing, movablePart
inputArg3 # e.g. nothing, waitingPeriod
inputArg4 # e.g. nothing, waitingPeriod
end


Expand Down Expand Up @@ -86,7 +87,7 @@ end

#-------------- Functions that can be executed at an event instant -------
### ------------------ ptpJointSpace! -------------------------------------
function ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Matrix{Float64}, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Matrix{Float64}, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
pos1 = size(transpose(ref.referencePath.position))
pos2 = size(positions)
if pos1[2] != pos2[2]
Expand All @@ -101,61 +102,78 @@ function ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Matrix{Float64
return nothing
end

ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Vector{Float64}, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} =
ptpJointSpace!(ref, reshape(positions, :, 1), dummy1, dummy2)
ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Vector{Float64}, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} =
ptpJointSpace!(ref, reshape(positions, :, 1), dummy1, dummy2, dummy3)

ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Float64, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} =
ptpJointSpace!(ref, reshape([positions], :, 1), dummy1, dummy2)
ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Float64, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} =
ptpJointSpace!(ref, reshape([positions], :, 1), dummy1, dummy2, dummy3)



###------------------------- ActionWait! ----------------------------------
function ActionWait!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionWait!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
@assert(waitingPeriod >= 0.0)
setAttributesReferencePath!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
return nothing
end

function robotEventAfterPeriod!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function robotEventAfterPeriod!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
@assert(waitingPeriod >= 0.0)
setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
return nothing
end

###--------------------- ActionReleaseAndAttach! -------------------------
function ActionReleaseAndAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionReleaseAndAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool, Nothing}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
@assert(waitingPeriod >= 0.0) # waitingPeriod must be positive
ref.scene.gripPair = Modia3D.GrippingPair{F}( Modia3D.ReleaseAndSetDown, getComponent(ref, movableObj), getComponent(ref, robotOrDepot))

if !isnothing(enableContactDetection)
ref.scene.gripPair.enableContactDetection = enableContactDetection
end

setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
return nothing
end


###------------------------- ActionAttach! ----------------------------------
function ActionAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
@assert(waitingPeriod >= 0.0) # waitingPeriod must be bigger than 0.1 sec
ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Grip, getComponent(ref, movableObj), getComponent(ref, robotOrDepot))

if !isnothing(enableContactDetection)
ref.scene.gripPair.enableContactDetection = enableContactDetection
end

setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
return nothing
end


###------------------------- ActionRelease! -------------------------------
function ActionRelease!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionRelease!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
@assert(waitingPeriod >= 0.0) # waitingPeriod must be positive
ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Release, getComponent(ref, movableObj))

if !isnothing(enableContactDetection)
ref.scene.gripPair.enableContactDetection = enableContactDetection
end

setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
return nothing
end


###------------------------- ActionDelete! -------------------------------
function ActionDelete!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionDelete!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
@assert(waitingPeriod >= 0.0) # waitingPeriod must be positive
ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Delete, getComponent(ref, movableObj))

if !isnothing(enableContactDetection)
ref.scene.gripPair.enableContactDetection = enableContactDetection
end

setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
return nothing
end
Expand Down
22 changes: 11 additions & 11 deletions src/PathPlanning/referencePathUser.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

function ptpJointSpace(ref::ModelActions{F,TimeType}, positions::Union{Matrix{Float64}, Vector{Float64}, Float64}, iargs...; kwargs...) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorPtpJointSpace(iargs, kwargs)
push!(ref.refMotion, ArbitraryMotion(ptpJointSpace!, positions, nothing, nothing))
push!(ref.refMotion, ArbitraryMotion(ptpJointSpace!, positions, nothing, nothing, nothing))
return nothing
end
ptpJointSpace(;referencePath, positions) =
Expand Down Expand Up @@ -39,7 +39,7 @@ function executeActions(modelActions::ModelActions{F,TimeType}) where {F <: Modi
# Execute next command in referenceMotion
if !isempty(modelActions.refMotion)
item = popfirst!(modelActions.refMotion)
item.func(modelActions, item.inputArg1, item.inputArg2, item.inputArg3)
item.func(modelActions, item.inputArg1, item.inputArg2, item.inputArg3, item.inputArg4)
end; end; end

# only if a ptp path is defined getPosition! can be executed
Expand All @@ -57,45 +57,45 @@ end
### ------------------------- ActionWait ------------------------------
function ActionWait(ref::ModelActions{F,TimeType}, waitingPeriod::Float64=0.0, iargs...; kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorExplicitRobotFunc("ActionWait", "waitingPeriod", iargs, kwargs, refPathExists=true)
push!(ref.refMotion, ArbitraryMotion(ActionWait!, waitingPeriod, nothing, nothing))
push!(ref.refMotion, ArbitraryMotion(ActionWait!, waitingPeriod, nothing, nothing, nothing))
return nothing
end
ActionWait(iargs...; kwargs...)::Nothing =
checkErrorExplicitRobotFunc("ActionWait", "waitingPeriod", iargs, kwargs, refPathExists=false)


### --------------------- ActionReleaseAndAttach -------------------------
function ActionReleaseAndAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionReleaseAndAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, enableContactDetection::Bool=true, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorExplicitRobotFunc("ActionReleaseAndAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
push!(ref.refMotion, ArbitraryMotion(ActionReleaseAndAttach!, robotOrDepot, movablePart, waitingPeriod))
push!(ref.refMotion, ArbitraryMotion(ActionReleaseAndAttach!, robotOrDepot, movablePart, waitingPeriod, enableContactDetection))
return nothing
end
ActionReleaseAndAttach(iargs...; kwargs...)::Nothing =
checkErrorExplicitRobotFunc("ActionReleaseAndAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=false)

function EventAfterPeriod(ref::ModelActions{F,TimeType}, waitingPeriod::Float64=0.0, iargs...; kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorExplicitRobotFunc("EventAfterPeriod", "waitingPeriod", iargs, kwargs, refPathExists=true)
push!(ref.refMotion, ArbitraryMotion(robotEventAfterPeriod!, waitingPeriod, nothing, nothing))
push!(ref.refMotion, ArbitraryMotion(robotEventAfterPeriod!, waitingPeriod, nothing, nothing, nothing))
return nothing
end
EventAfterPeriod(iargs...; kwargs...)::Nothing =
checkErrorExplicitRobotFunc("EventAfterPeriod", "waitingPeriod", iargs, kwargs, refPathExists=false)


### --------------------- ActionAttach ------------------------------------
function ActionAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, enableContactDetection::Bool=true, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorExplicitRobotFunc("ActionAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
push!(ref.refMotion, ArbitraryMotion(ActionAttach!, robotOrDepot, movablePart, waitingPeriod))
push!(ref.refMotion, ArbitraryMotion(ActionAttach!, robotOrDepot, movablePart, waitingPeriod, enableContactDetection))
return nothing
end
ActionAttach(iargs...; kwargs...)::Nothing =
checkErrorExplicitRobotFunc("ActionAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=false)


### --------------------- ActionRelease -----------------------------------
function ActionRelease(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
function ActionRelease(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, enableContactDetection::Bool=true, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorExplicitRobotFunc("ActionRelease", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
push!(ref.refMotion, ArbitraryMotion(ActionRelease!, movablePart, waitingPeriod, nothing))
push!(ref.refMotion, ArbitraryMotion(ActionRelease!, movablePart, waitingPeriod, enableContactDetection, nothing))
return nothing
end
ActionRelease(iargs...; kwargs...)::Nothing =
Expand All @@ -106,7 +106,7 @@ ActionRelease(iargs...; kwargs...)::Nothing =
### --------------------- ActionRelease -----------------------------------
function ActionDelete(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
checkErrorExplicitRobotFunc("ActionDelete", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
push!(ref.refMotion, ArbitraryMotion(ActionDelete!, movablePart, waitingPeriod, nothing))
push!(ref.refMotion, ArbitraryMotion(ActionDelete!, movablePart, waitingPeriod, nothing, nothing))
return nothing
end
ActionDelete(iargs...; kwargs...)::Nothing =
Expand Down
Loading

0 comments on commit 8f22b8f

Please sign in to comment.