Skip to content

Commit 8f22b8f

Browse files
GerhardHippmannGerhardHippmann
authored
Merge pull request #127 from ModiaSim/an_moreTests
An more tests
2 parents 4657238 + 57faff1 commit 8f22b8f

File tree

11 files changed

+3006
-28
lines changed

11 files changed

+3006
-28
lines changed

‎src/Composition/dynamics.jl‎

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -146,11 +146,15 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode
146146
scene::Scene{F} = mbsBuild.mbs.scene
147147
#instantiatedModel = mbsBuild.mbs.instantiatedModel
148148
#gripPair = scene.gripPair
149-
if Modia3D.checkGrippingFeatures(scene, scene.gripPair)
150-
Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair)
151-
else
152-
@error("Andrea: print warning für gripping features")
153-
# printWarnGrip(robotOrDepot, movableObj, waitingPeriod)
149+
if isdefined(scene, :gripPair)
150+
if Modia3D.checkGrippingFeatures(scene, scene.gripPair)
151+
Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair)
152+
if isdefined(scene.gripPair, :enableContactDetection)
153+
scene.collide = scene.gripPair.enableContactDetection
154+
end
155+
else
156+
@error("Print warning für gripping features")
157+
end
154158
end
155159

156160
(worldDummy, revoluteObjects, prismaticObjects, freeMotionObjects, hiddenJointObjects, forceElements, resultElements) = checkMultibodySystemAndGetWorldAndJointsAndForceElementsAndResultElements(partiallyInstantiatedModel.modelName, parameters, modelPath, F)
@@ -166,6 +170,17 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode
166170
# rebuild MKS
167171
if scene.options.useOptimizedStructure
168172
rebuild_superObjs!(scene, world)
173+
174+
if isdefined(scene, :gripPair)
175+
if Modia3D.checkGrippingFeatures(scene, scene.gripPair)
176+
if isdefined(scene.gripPair, :enableContactDetection)
177+
if scene.options.enableContactDetection && scene.collide
178+
scene.collide = scene.gripPair.enableContactDetection
179+
end
180+
end
181+
end
182+
end
183+
169184
else
170185
error("Full restart is possible only if useOptimizedStructure is enabled in SceneOptions.")
171186
end

‎src/GrippingDetection/grippingPair.jl‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ mutable struct GrippingPair{F <: Modia3D.VarFloatType}
55
movableObj::Composition.Object3D{F} # must be part of a movable unit
66
# is set for Attach and ReleaseAndAttach
77
robotOrDepot::Composition.Object3D{F} # must be part of a gripper unit or part of a depot (like bottom or other movable unit)
8+
enableContactDetection::Bool
89
GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType} = new(gripStatus, movableObj)
910
GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}, robotOrDepot::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType} = new(gripStatus, movableObj, robotOrDepot)
1011
end

‎src/PathPlanning/referencePathInternal.jl‎

Lines changed: 29 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ struct ArbitraryMotion
44
inputArg1 # e.g. positions, waitingPeriod, robotOrDepot
55
inputArg2 # e.g. nothing, movablePart
66
inputArg3 # e.g. nothing, waitingPeriod
7+
inputArg4 # e.g. nothing, waitingPeriod
78
end
89

910

@@ -86,7 +87,7 @@ end
8687

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

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

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

110111

111112

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

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

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

131+
if !isnothing(enableContactDetection)
132+
ref.scene.gripPair.enableContactDetection = enableContactDetection
133+
end
134+
130135
setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
131136
return nothing
132137
end
133138

134139

135140
###------------------------- ActionAttach! ----------------------------------
136-
function ActionAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
141+
function ActionAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
137142
@assert(waitingPeriod >= 0.0) # waitingPeriod must be bigger than 0.1 sec
138143
ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Grip, getComponent(ref, movableObj), getComponent(ref, robotOrDepot))
144+
145+
if !isnothing(enableContactDetection)
146+
ref.scene.gripPair.enableContactDetection = enableContactDetection
147+
end
148+
139149
setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
140150
return nothing
141151
end
142152

143153

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

159+
if !isnothing(enableContactDetection)
160+
ref.scene.gripPair.enableContactDetection = enableContactDetection
161+
end
162+
149163
setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
150164
return nothing
151165
end
152166

153167

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

173+
if !isnothing(enableContactDetection)
174+
ref.scene.gripPair.enableContactDetection = enableContactDetection
175+
end
176+
159177
setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod)
160178
return nothing
161179
end

‎src/PathPlanning/referencePathUser.jl‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
function ptpJointSpace(ref::ModelActions{F,TimeType}, positions::Union{Matrix{Float64}, Vector{Float64}, Float64}, iargs...; kwargs...) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
55
checkErrorPtpJointSpace(iargs, kwargs)
6-
push!(ref.refMotion, ArbitraryMotion(ptpJointSpace!, positions, nothing, nothing))
6+
push!(ref.refMotion, ArbitraryMotion(ptpJointSpace!, positions, nothing, nothing, nothing))
77
return nothing
88
end
99
ptpJointSpace(;referencePath, positions) =
@@ -39,7 +39,7 @@ function executeActions(modelActions::ModelActions{F,TimeType}) where {F <: Modi
3939
# Execute next command in referenceMotion
4040
if !isempty(modelActions.refMotion)
4141
item = popfirst!(modelActions.refMotion)
42-
item.func(modelActions, item.inputArg1, item.inputArg2, item.inputArg3)
42+
item.func(modelActions, item.inputArg1, item.inputArg2, item.inputArg3, item.inputArg4)
4343
end; end; end
4444

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

6666

6767
### --------------------- ActionReleaseAndAttach -------------------------
68-
function ActionReleaseAndAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
68+
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}
6969
checkErrorExplicitRobotFunc("ActionReleaseAndAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
70-
push!(ref.refMotion, ArbitraryMotion(ActionReleaseAndAttach!, robotOrDepot, movablePart, waitingPeriod))
70+
push!(ref.refMotion, ArbitraryMotion(ActionReleaseAndAttach!, robotOrDepot, movablePart, waitingPeriod, enableContactDetection))
7171
return nothing
7272
end
7373
ActionReleaseAndAttach(iargs...; kwargs...)::Nothing =
7474
checkErrorExplicitRobotFunc("ActionReleaseAndAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=false)
7575

7676
function EventAfterPeriod(ref::ModelActions{F,TimeType}, waitingPeriod::Float64=0.0, iargs...; kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
7777
checkErrorExplicitRobotFunc("EventAfterPeriod", "waitingPeriod", iargs, kwargs, refPathExists=true)
78-
push!(ref.refMotion, ArbitraryMotion(robotEventAfterPeriod!, waitingPeriod, nothing, nothing))
78+
push!(ref.refMotion, ArbitraryMotion(robotEventAfterPeriod!, waitingPeriod, nothing, nothing, nothing))
7979
return nothing
8080
end
8181
EventAfterPeriod(iargs...; kwargs...)::Nothing =
8282
checkErrorExplicitRobotFunc("EventAfterPeriod", "waitingPeriod", iargs, kwargs, refPathExists=false)
8383

8484

8585
### --------------------- ActionAttach ------------------------------------
86-
function ActionAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
86+
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}
8787
checkErrorExplicitRobotFunc("ActionAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
88-
push!(ref.refMotion, ArbitraryMotion(ActionAttach!, robotOrDepot, movablePart, waitingPeriod))
88+
push!(ref.refMotion, ArbitraryMotion(ActionAttach!, robotOrDepot, movablePart, waitingPeriod, enableContactDetection))
8989
return nothing
9090
end
9191
ActionAttach(iargs...; kwargs...)::Nothing =
9292
checkErrorExplicitRobotFunc("ActionAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=false)
9393

9494

9595
### --------------------- ActionRelease -----------------------------------
96-
function ActionRelease(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
96+
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}
9797
checkErrorExplicitRobotFunc("ActionRelease", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
98-
push!(ref.refMotion, ArbitraryMotion(ActionRelease!, movablePart, waitingPeriod, nothing))
98+
push!(ref.refMotion, ArbitraryMotion(ActionRelease!, movablePart, waitingPeriod, enableContactDetection, nothing))
9999
return nothing
100100
end
101101
ActionRelease(iargs...; kwargs...)::Nothing =
@@ -106,7 +106,7 @@ ActionRelease(iargs...; kwargs...)::Nothing =
106106
### --------------------- ActionRelease -----------------------------------
107107
function ActionDelete(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
108108
checkErrorExplicitRobotFunc("ActionDelete", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
109-
push!(ref.refMotion, ArbitraryMotion(ActionDelete!, movablePart, waitingPeriod, nothing))
109+
push!(ref.refMotion, ArbitraryMotion(ActionDelete!, movablePart, waitingPeriod, nothing, nothing))
110110
return nothing
111111
end
112112
ActionDelete(iargs...; kwargs...)::Nothing =

0 commit comments

Comments
 (0)