Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

An more tests #127

Merged
merged 15 commits into from
Nov 7, 2023
Merged

An more tests #127

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
48ada82
r_abs for all visual elements
AndreaNeumayr Jun 5, 2023
9f4e478
check if gripping features are defined
AndreaNeumayr Jun 5, 2023
5d48a08
new transportation scenario
AndreaNeumayr Jun 5, 2023
5d0ff3e
little changes
AndreaNeumayr Jun 6, 2023
bbe7050
enable and dissable collision handling
AndreaNeumayr Jun 6, 2023
261226a
some test models
AndreaNeumayr Jun 6, 2023
173d7bc
modified files
AndreaNeumayr Jun 21, 2023
f833dae
use updateVisuElements again
AndreaNeumayr Nov 3, 2023
ceed597
finalize test models
AndreaNeumayr Nov 3, 2023
864ffeb
little changes
AndreaNeumayr Nov 3, 2023
b347d19
remove error message
AndreaNeumayr Nov 6, 2023
90fb1cf
update final results
AndreaNeumayr Nov 6, 2023
aa39469
update final states ubuntu
AndreaNeumayr Nov 6, 2023
6ae8da5
YouBotSphereTransport.jl no longer works
AndreaNeumayr Nov 6, 2023
57faff1
little changes
AndreaNeumayr Nov 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 20 additions & 5 deletions src/Composition/dynamics.jl
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading