@@ -29,6 +29,8 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
2929 nz:: Int # Number of used zero crossing functions
3030 time:: TimeType # Current time
3131
32+ freeMotionResiduals:: Vector{F}
33+
3234 # for multibodyAccelerations
3335 leq:: Vector{ModiaBase.LinearEquations{F}}
3436
@@ -39,7 +41,9 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
3941 revoluteObjects, prismaticObjects, freeMotionObjects,
4042 zeros (F, length (revoluteObjects)), zeros (F, length (prismaticObjects)), zeros (SVector{3 ,F}, 2 * length (freeMotionObjects)),
4143 zeros (F, length (revoluteObjects)), zeros (F, length (prismaticObjects)), zeros (SVector{3 ,F}, 2 * length (freeMotionObjects)),
42- zStartIndex, nz, Modia3D. convertAndStripUnit (TimeType, u "" . LinearEquations{F}[])
44+ zStartIndex, nz, Modia3D. convertAndStripUnit (TimeType, u ""
45+ zeros (F, 2 * 3 * length (freeMotionObjects)),
46+ ModiaBase. LinearEquations{F}[])
4347end
4448
4549mutable struct MultibodyBuild{F <: Modia3D.VarFloatType , TimeType}
466470
467471Copy states of the revolute joints into the corresponding Object3Ds.
468472"""
469- @inline function setStatesRevolute! (mbs:: MultibodyData{F} , args:: Vararg{F,NJOINTS_TIMES_2 } ):: MultibodyData{F} where {F,NJOINTS_TIMES_2 }
470- @assert (NJOINTS_TIMES_2 == 2 * length (mbs. revoluteObjects))
473+ function setStatesRevolute! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{F} ):: MultibodyData{F,TimeType } where {F,TimeType }
474+ @assert (length (args) == 2 * length (mbs. revoluteObjects))
471475 scene = mbs. scene
472476 j = 1
473477 @inbounds for obj in mbs. revoluteObjects
485489
486490Copy states of the prismatic joints into the corresponding Object3Ds.
487491"""
488- @inline function setStatesPrismatic! (mbs:: MultibodyData{F} , args:: Vararg{F,NJOINTS_TIMES_2 } ):: MultibodyData{F} where {F,NJOINTS_TIMES_2 }
489- @assert (NJOINTS_TIMES_2 == 2 * length (mbs. prismaticObjects))
492+ function setStatesPrismatic! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{F} ):: MultibodyData{F,TimeType } where {F,TimeType }
493+ @assert (length (args) == 2 * length (mbs. prismaticObjects))
490494 scene = mbs. scene
491495 j = 1
492496 @inbounds for obj in mbs. prismaticObjects
504508
505509Copy states of the free motion joints into the corresponding Object3Ds.
506510"""
507- @inline function setStatesFreeMotion! (mbs:: MultibodyData{F} , args:: Vararg{SVector{3,F},NJOINTS_TIMES_4 } ):: MultibodyData{F} where {F,NJOINTS_TIMES_4 }
508- @assert (NJOINTS_TIMES_4 == 4 * length (mbs. freeMotionObjects))
511+ function setStatesFreeMotion! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{SVector{3,F}} ):: MultibodyData{F,TimeType } where {F,TimeType }
512+ @assert (length (args) == 4 * length (mbs. freeMotionObjects))
509513 scene = mbs. scene
510514 j = 1
511515 @inbounds for obj in mbs. freeMotionObjects
525529
526530Copy isrot123 of the free motion joints into the corresponding Object3Ds.
527531"""
528- @inline function setStatesFreeMotion_isrot123! (mbs:: MultibodyData{F} , args:: Vararg{Bool,NJOINTS } ):: MultibodyData{F} where {F,NJOINTS }
529- @assert (NJOINTS == length (mbs. freeMotionObjects))
532+ function setStatesFreeMotion_isrot123! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{Bool} ):: MultibodyData{F,TimeType } where {F,TimeType }
533+ @assert (length (args) == length (mbs. freeMotionObjects))
530534 scene = mbs. scene
531535 @inbounds for (i,obj) in enumerate (mbs. freeMotionObjects)
532536 scene. freeMotion[obj. jointIndex]. isrot123 = args[i]
540544
541545Copy accelerations of revolute joints into mbs.
542546"""
543- @inline function setAccelerationsRevolute! (mbs:: MultibodyData{F} , args:: Vararg{F,NJOINTS } ) where {F,NJOINTS }
544- @assert (NJOINTS == length (mbs. revoluteObjects))
547+ function setAccelerationsRevolute! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{F} ) where {F,TimeType }
548+ @assert (length (args) == length (mbs. revoluteObjects))
545549 scene = mbs. scene
546550 @inbounds for (i,obj) in enumerate (mbs. revoluteObjects)
547551 scene. revolute[obj. jointIndex]. a = args[i]
555559
556560Copy accelerations of prismatic joints into mbs.
557561"""
558- @inline function setAccelerationsPrismatic! (mbs:: MultibodyData{F} , args:: Vararg{F,NJOINTS } ) where {F,NJOINTS }
559- @assert (NJOINTS == length (mbs. prismaticObjects))
562+ function setAccelerationsPrismatic! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{F} ) where {F,TimeType }
563+ @assert (length (args) == length (mbs. prismaticObjects))
560564 scene = mbs. scene
561565 @inbounds for (i,obj) in enumerate (mbs. prismaticObjects)
562566 scene. prismatic[obj. jointIndex]. a = args[i]
570574
571575Copy accelerations of free motion joints into mbs
572576"""
573- @inline function setAccelerationsFreeMotion! (mbs:: MultibodyData{F} , args:: Vararg{SVector{3,F},NJOINTS_TIMES_2 } ) where {F,NJOINTS_TIMES_2 }
574- @assert (NJOINTS_TIMES_2 == 2 * length (mbs. freeMotionObjects))
577+ function setAccelerationsFreeMotion! (mbs:: MultibodyData{F,TimeType } , args:: Vararg{SVector{3,F}} ) where {F,TimeType }
578+ @assert (length (args) == 2 * length (mbs. freeMotionObjects))
575579 scene = mbs. scene
576580 j = 1
577581 @inbounds for obj in mbs. freeMotionObjects
@@ -583,32 +587,41 @@ Copy accelerations of free motion joints into mbs
583587 return mbs
584588end
585589
586-
587590"""
588591 (tau1,tau2,<...>) = getGenForcesRevolute(mbs::MultibodyData{F}, ::Val{N}) where {F <: Modia3D.VarFloatType,N}
589592
590593Return generalized forces of revolute joints as NTuple.
591594"""
592- @inline getGenForcesRevolute (mbs:: MultibodyData{F} , :: Val{N} ) where {F <: Modia3D.VarFloatType ,N} = ntuple (i-> mbs. revoluteGenForces[i], Val (N))
595+ # getGenForcesRevolute(mbs::MultibodyData{F,TimeType}) where {F,TimeType} = mbs.revoluteGenForces
596+ getGenForcesRevolute (mbs:: MultibodyData{F,TimeType} , :: Val{N} ) where {F,TimeType,N} = ntuple (i-> mbs. revoluteGenForces[i], Val (N))
593597
594598
595599"""
596600 (f1,f2,<...>) = getGenForcesPrismatic(mbs::MultibodyData{F}, ::Val{N}) where {F <: Modia3D.VarFloatType,N}
597601
598602Return generalized forces of prismatic joints as NTuple.
599603"""
600- @inline getGenForcesPrismatic (mbs:: MultibodyData{F} , :: Val{N} ) where {F <: Modia3D.VarFloatType ,N} = ntuple (i-> mbs. prismaticGenForces[i], Val (N))
604+ # getGenForcesPrismatic(mbs::MultibodyData{F,TimeType}) where {F,TimeType} = mbs.prismaticGenForces
605+ getGenForcesPrismatic (mbs:: MultibodyData{F,TimeType} , :: Val{N} ) where {F,TimeType,N} = ntuple (i-> mbs. prismaticGenForces[i], Val (N))
601606
602607
603608"""
604- (genf1,gent1,genf2,gent2,<...>) = getGenForcesFreeMotion(mbs::MultibodyData{F}, ::Val{N}) where {F <: Modia3D.VarFloatType,N}
609+ residuals = getGenForcesFreeMotion(mbs::MultibodyData{F}, ::Val{N}) where {F <: Modia3D.VarFloatType,N}
605610
606- Return generalized forces of free motion joints as NTuple .
611+ Return generalized forces of free motion joints as Vector to be used for residuals vector .
607612"""
608- @inline getGenForcesFreeMotion (mbs:: MultibodyData{F} , :: Val{N} ) where {F <: Modia3D.VarFloatType ,N} = ntuple (i-> mbs. freeMotionGenForces[i], Val (N))
609-
610-
611-
613+ function getGenForcesFreeMotion (mbs:: MultibodyData{F,TimeType} ):: Vector{F} where {F,TimeType}
614+ res = mbs. freeMotionResiduals
615+ j = 1
616+ @assert (length (res) == 3 * length (mbs. freeMotionGenForces))
617+ @inbounds for vec in mbs. freeMotionGenForces
618+ res[j] = vec[1 ]
619+ res[j+ 1 ] = vec[2 ]
620+ res[j+ 2 ] = vec[3 ]
621+ j += 3
622+ end
623+ return res
624+ end
612625
613626
614627# For backwards compatibility (do not use for new models)
0 commit comments