Merge pull request #131 from ModiaSim/an-gh_enableKinematicChainFlipping

Enable kinematic chain flipping

.github/workflows/test.yml

@@ -14,7 +14,7 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - '1.9.0'
+          - '1.9.4'
         os:
           - ubuntu-latest
           - windows-latest

Project.toml

@@ -8,7 +8,7 @@ Colors = "0.12, 0.11, 0.10"
 DataFrames = "1"
 DoubleFloats = "1"
 FileIO = "1"
-HTTP = "1.10"
+HTTP = "1"
 JSON = "0.21"
 Measurements = "2"
 MeshIO = "0.4.10"

docs/src/index.md

@@ -94,14 +94,19 @@ julia -JModia3D_sysimage.so  (otherwise)
 
 ## Release Notes
 
+### Forthcoming version
+
+- Enable kinematic chain flipping for structure variable systems
+
+
 ### Version 0.12.2
 
 - Add Force Element PolygonalContactModel and tests
 - Add Result Element infrastructure and Result Element ContactResult
+- Segmented Simulation: add flag `enableContactDetection` to action commands (`ActionAttach`, `ActionReleaseAndAttach`, `ActionRelease`, `ActionDelete`) to enable or disable collision handling for the whole model in the actual segment
 - Bugfix: Correct parent Object3D torque calculation for Fix Joint with non-zero rotation
 - Bugfix: Enable AnimationExport in case of no renderer available
 - Bugfix: Enable position/orientation result signals independent of visualization/animation configuration
-- Segmented Simulation: add flag `enableContactDetection` to action commands (`ActionAttach`, `ActionReleaseAndAttach`, `ActionRelease`, `ActionDelete`) to enable or disable collision handling for the whole model in the actual segment
 
 
 ### Version 0.12.1

src/Composition/handler.jl

@@ -6,6 +6,7 @@
 #
 
 
+
 function build_tree!(scene::Scene{F}, world::Object3D{F})::Nothing where F <: Modia3D.VarFloatType
     tree                 = scene.tree
     stack                = scene.stack
@@ -117,6 +118,88 @@ function changeParent(newParent::Object3D{F}, obj::Object3D{F})::Nothing where F
 end
 
 
+"""define new parent for an obj
+
+param:
+    newParent: the new parent of the object
+    obj: the object whos parent is to be changed
+"""
+function invertParentChild!(newParent::Object3D{F}, obj::Object3D{F}, ind = "") where F <: Modia3D.VarFloatType
+
+    oldParent = obj.parent
+
+    # add obj as children to new parent (blue line)
+    push!(newParent.children, obj)
+
+    # def new parent for obj (green line)
+    obj.parent = newParent
+
+    # delete obj from oldParent (del old blue line)
+    Basics.deleteItem(oldParent.children, obj)
+
+    # update r_rel and R_rel (red line)
+    obj.r_rel =  newParent.R_abs * (obj.r_abs - newParent.r_abs)
+    obj.R_rel =  obj.R_abs * newParent.R_abs'
+
+    # following formula might be more precise but does not work in all cases
+    # obj.r_rel =  -newParent.R_rel' * newParent.r_rel
+    # obj.R_rel =  newParent.R_rel'
+
+    isBeginOfChain = obj == oldParent
+    if isBeginOfChain
+        deleteJointInfo(obj)
+    else
+        # trigger recursion
+        invertParentChild!(obj, oldParent, ind*"  ")
+    end
+
+    revertJointInfo!(newParent, obj)
+
+    return nothing
+end
+
+"""revertJointInfo!
+Revert joint info between the old and the new child
+
+param:
+    oldChild: the old child i.e. it contains all joint info
+    newChild: the old child i.e. all joint info is to be mover here
+"""
+function revertJointInfo!(oldChild::Object3D{F}, newChild::Object3D{F}) where F <: Modia3D.VarFloatType
+    newChild.jointKind = oldChild.jointKind
+
+    newChild.joint = oldChild.joint
+
+    # joint specific treatment
+    jointSpecificTreatment!(newChild, oldChild)
+
+    oldChild.hasChildJoint = oldChild.hasChildJoint || newChild.hasChildJoint
+
+    newChild.fixedToParent = oldChild.fixedToParent
+
+    newChild.jointIndex = oldChild.jointIndex
+
+    newChild.ndof = oldChild.ndof
+
+    return nothing
+end
+
+"""deleteJointInfo
+Delete joint info of an object
+
+param:
+    obj: the object whos join info shall be deleted
+"""
+function deleteJointInfo(obj::Object3D{F}) where F <: Modia3D.VarFloatType
+    obj.joint     = FixedJoint{F}()
+    obj.jointKind = FixKind
+    obj.jointIndex = 0
+    obj.ndof       = 0
+    obj.fixedToParent = true
+
+    return nothing
+end
+
 function getRootObj(obj::Object3D{F}) where F <: Modia3D.VarFloatType
     if isRootObject(obj)
         return obj
@@ -540,3 +623,56 @@ function closeAnalysis!(scene::Scene{F})::Nothing where F <: Modia3D.VarFloatTyp
     emptyScene!(scene)
     return nothing
 end
+
+
+function dispTree(root, ind="")
+    if ind == ""
+        println("\nT R E E\n")
+
+        err_r_abs = 0
+        err_R_abs = 0
+    else
+        err_r_abs = norm(root.r_abs - (root.parent.r_abs + root.parent.R_abs' * root.r_rel))
+        err_R_abs = norm(root.R_abs - (root.R_rel * root.parent.R_abs))
+    end
+
+    # println(ind, Modia3D.fullName(root), " ", err_r_abs, " ", err_R_abs)#, " ", root.r_abs)
+
+    println(ind, Modia3D.fullName(root), " ", Modia3D.fullName(root.parent), " ", root.jointKind)
+
+    # migth be interesting too:
+    # root.r_abs, root.r_rel, root.fixedToParent
+    # root.jointIndex, root.ndof
+    # root.hasChildJoint,
+
+    for child in root.children
+        dispTree(child, ind * "  ")
+    end
+
+    if ind == ""
+        println("\n")
+    end
+end
+
+function dispSceneTree(root, ind="")
+    if ind == ""
+        println("\n\n\nxxxxxxxxxxxxxxxxxxxxxx\n\n\n")
+
+    #     err_r_abs = 0
+    #     err_R_abs = 0
+    # else
+    #     err_r_abs = norm(root.r_abs - (root.parent.r_abs + root.parent.R_abs' * root.r_rel))
+    #     err_R_abs = norm(root.R_abs - (root.R_rel * root.parent.R_abs))
+    end
+
+    # println(ind, Modia3D.fullName(root), " ", err_r_abs, " ", err_R_abs, " ", root.r_abs)
+    # println(ind, Modia3D.fullName(root), root.r_abs, " ", err_r_abs)
+
+    for child in root
+        dispTree(child, ind * "  ")
+    end
+
+    if ind == ""
+        println("\n\n\nxxxxxxxxxxxxxxxxxxxxxx\n\n\n")
+    end
+end

src/Composition/joints/Revolute.jl

@@ -77,3 +77,10 @@ mutable struct Revolute{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
     end
 end
 Revolute(; kwargs...) = Revolute{Float64}(; kwargs...)
+
+
+function revertRevoluteKind!(oldChild::Object3D{F}, newChild::Object3D{F}) where F <: Modia3D.VarFloatType
+    newChild.joint.obj1, newChild.joint.obj2 = newChild.joint.obj2, newChild.joint.obj1
+    newChild.joint.eAxis = -newChild.joint.eAxis
+    return nothing
+end

src/Composition/joints/joints.jl

@@ -859,3 +859,18 @@ function setDistance!(prismatic::Prismatic, s::F) where F <: Modia3D.VarFloatTyp
    obj.r_abs   = parent.r_abs + parent.R_abs'*obj.r_rel
    return prismatic
 end
+
+
+function jointSpecificTreatment!(oldChild::Object3D{F}, newChild::Object3D{F})::Nothing where F <: Modia3D.VarFloatType
+    if newChild.jointKind == Modia3D.Composition.RevoluteKind
+        revertRevoluteKind!(oldChild, newChild)
+        return nothing
+    elseif newChild.jointKind == Modia3D.Composition.FixKind
+        # nothing needs to be done here
+        return nothing
+    else
+        @error(newChild.jointKind, " is not implemented yet")
+        return nothing
+    end
+    return nothing
+end

src/GrippingDetection/_module.jl

@@ -9,9 +9,9 @@ using LinearAlgebra
 
 const NOTHING = Nothing
 
-export GripStatus, Grip, Release, ReleaseAndSetDown, Delete, NoAction
+export GripStatus, Grip, Release, ReleaseAndSetDown, Delete, FlipChain, NoAction
 
-@enum GripStatus Grip Release ReleaseAndSetDown Delete NoAction
+@enum GripStatus Grip Release ReleaseAndSetDown Delete FlipChain NoAction
 
 include("grippingPair.jl")
 

src/GrippingDetection/grippingPair.jl

@@ -6,8 +6,14 @@ mutable struct GrippingPair{F <: Modia3D.VarFloatType}
     # 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
+    movableObjOld::Composition.Object3D{F}  # must be part of a movable unit
     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)
+    function GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}, robotOrDepot::Composition.Object3D{F}, movableObjOld::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType}
+        obj = new(gripStatus, movableObj, robotOrDepot)
+        obj.movableObjOld = movableObjOld
+        return obj
+    end
 end
 
 
@@ -47,7 +53,7 @@ function checkGrippingFeatures(scene::Composition.Scene{F}, gripPair::GrippingPa
     # the so-called movableObj must belong to a movable unit
     movableObjIsMovable = Composition.objectHasMovablePos(movableObj)
     if !movableObjIsMovable
-        error("das movable obj gehört nicht zu movable")
+        error("movable obj gehört nicht zu movable")
         printWarnMovableUnit(movableObj)
         allowedToChange = false
         return false
@@ -78,6 +84,27 @@ function checkGrippingFeatures(scene::Composition.Scene{F}, gripPair::GrippingPa
             return false
         end
     end
+
+    if gripPair.gripStatus == FlipChain
+        movableObjOld::Composition.Object3D{F} = gripPair.movableObjOld
+        # movableObj must be lockable
+        if !movableObjOld.interactionManner.lockable
+            error("movableObjOld is not lockable")
+            #printWarnLockable(robotOrDepot, movableObj)
+            allowedToChange = false
+            return false
+        end
+
+        # the so-called movableObj must belong to a movable unit
+        movableObjIsMovableOld = Composition.objectHasMovablePos(movableObjOld)
+        if !movableObjIsMovableOld
+            error("$(Modia3D.fullName(movableObjOld )) does not belong to a movable unit.")
+            printWarnMovableUnit(movableObjIsMovableOld)
+            allowedToChange = false
+            return false
+        end
+        return true
+    end
     return true
 end
 
@@ -125,7 +152,7 @@ function changeParentOfMovableUnit!(scene::Composition.Scene{F}, world::Composit
     end
 
     # new parent is robotOrDepot
-    if gripPair.gripStatus == ReleaseAndSetDown ||  gripPair.gripStatus == Grip
+    if gripPair.gripStatus == ReleaseAndSetDown ||  gripPair.gripStatus == Grip || gripPair.gripStatus == FlipChain
         if isdefined(gripPair, :robotOrDepot)
             robotOrDepot::Composition.Object3D{F} = gripPair.robotOrDepot
         else
@@ -152,6 +179,24 @@ function changeParentOfMovableUnit!(scene::Composition.Scene{F}, world::Composit
         end
         return nothing
     end
+
+    if gripPair.gripStatus == FlipChain
+        # 1. like == Release with movableObjOld
+        movableObjsOld = getSuperObjsMovable(scene,
+        gripPair.movableObjOld.interactionManner.movablePos)
+        movableObjRootOld::Composition.Object3D{F} = movableObjsOld[1]
+        # new parent is obj
+        Basics.deleteItem(movableObjRootOld.parent.children, movableObjRootOld)
+        movableObjRootOld.parent = movableObjRootOld
+
+        # 2. like == Grip
+        robotOrDepotPos = robotOrDepot.interactionManner.originPos
+        newParent = Composition.getRootObj(scene.buffer[robotOrDepotPos])
+
+
+        Composition.invertParentChild!(newParent, movableObjRoot)
+        return nothing
+    end
     return nothing
 end
 

src/ModiaInterface/_module.jl

@@ -22,7 +22,7 @@ export WorldForce, WorldTorque, Bushing, SpringDamperPtP, PolygonalContactModel
 export ContactResult
 export FreeMotion2
 
-export ModelActions, ActionAttach, ActionRelease, ActionReleaseAndAttach, ActionDelete, EventAfterPeriod, ActionWait
+export ModelActions, ActionAttach, ActionRelease, ActionReleaseAndAttach, ActionDelete, EventAfterPeriod, ActionWait, ActionFlipChain
 export addReferencePath
 
 export build_Model3D!, get_animationHistory

src/ModiaInterface/model3D.jl

@@ -41,6 +41,7 @@ ModelActions(; kwargs...) = Par(; _constructor = :(Modia3D.PathPlanning.ModelAct
 ActionAttach(args...; kwargs...) = Modia3D.PathPlanning.ActionAttach(args...; kwargs...)
 ActionRelease(args...; kwargs...) = Modia3D.PathPlanning.ActionRelease(args...; kwargs...)
 ActionReleaseAndAttach(args...; kwargs...) = Modia3D.PathPlanning.ActionReleaseAndAttach(args...; kwargs...)
+ActionFlipChain(args...; kwargs...) = Modia3D.PathPlanning.ActionFlipChain(args...; kwargs...)
 ActionDelete(args...; kwargs...) = Modia3D.PathPlanning.ActionDelete(args...; kwargs...)
 EventAfterPeriod(args...; kwargs...) = Modia3D.PathPlanning.EventAfterPeriod(args...; kwargs...)
 ActionWait(args...; kwargs...) = Modia3D.PathPlanning.ActionWait(args...; kwargs...)

src/PathPlanning/referencePathInternal.jl

@@ -165,6 +165,16 @@ function ActionRelease!(ref::ModelActions{F,TimeType}, movableObj::String, waiti
 end
 
 
+###------------------------- ActionFlipChain! -------------------------------
+function ActionFlipChain!(ref::ModelActions{F,TimeType}, movablePartNew::String, robotOrDepot::String, movablePartOld::String, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
+    # @assert(waitingPeriod >= 0.0) # waitingPeriod must be positive
+    ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.FlipChain,  getComponent(ref, movablePartNew), getComponent(ref, robotOrDepot), getComponent(ref, movablePartOld))
+
+    setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel))
+    return nothing
+end
+
+
 ###------------------------- ActionDelete! -------------------------------
 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

src/PathPlanning/referencePathUser.jl

@@ -102,6 +102,15 @@ ActionRelease(iargs...; kwargs...)::Nothing =
     checkErrorExplicitRobotFunc("ActionRelease", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=false)
 
 
+### --------------------- ActionFlipChain -----------------------------------
+function ActionFlipChain(ref::ModelActions{F,TimeType}, movablePartNew::String="", robotOrDepot::String="", movablePartOld::String="", iargs...; kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
+    checkErrorExplicitRobotFunc("ActionFlipChain", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true)
+    push!(ref.refMotion, ArbitraryMotion(ActionFlipChain!, movablePartNew, robotOrDepot, movablePartOld, nothing))
+    return nothing
+end
+ActionFlipChain(iargs...; kwargs...)::Nothing =
+    checkErrorExplicitRobotFunc("ActionFlipChain", "movablePartNew, movablePartOld, and robotOrDepot", iargs, kwargs, refPathExists=false)
+
 
 ### --------------------- ActionRelease -----------------------------------
 function ActionDelete(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}

test/includeTests.jl

@@ -61,9 +61,9 @@ Test.@testset "Robot" begin
         Test.@test_broken include(joinpath("Robot", "YouBotWithSphere.jl"))  # LinearAlgebra.SingularException
         include(joinpath("Robot", "YouBotGripping.jl"))
         include(joinpath("Robot", "YouBotSphereTransport.jl"))
-        include(joinpath("Robot", "ScenarioCollisionOnly.jl"))
     end
     if testsExtend == completeTests
+        include(joinpath("Robot", "ScenarioCollisionOnly.jl"))  # long computation time
         include(joinpath("Robot", "YouBotPingPong.jl"))  # long computation time
         include(joinpath("Robot", "YouBotsGripping.jl"))  # long computation time
     end