Skip to content

Commit

Permalink
Lin: change DCM_logmap calls to small angle calculations in lineari…
Browse files Browse the repository at this point in the history 
…zation perturbations of orientations

Change flag `UseLogMaps` to `UseSmlAngles` and change following routines to use a small angle perturbation instead of `DCM_logmap`:
 - PackMotionMesh
 - PackMotionMesh_dY
 - PerturbOrientationMatrix
  • Loading branch information
@andrew-platt @rafmudaf
andrew-platt authored and rafmudaf committed May 9, 2022
1 parent 572380b commit 8364b53
Show file tree
Hide file tree
Showing 8 changed files with 76 additions and 62 deletions.
Binary file added modules/beamdyn/src/3DRotations_Rev2.docx
View file
Binary file not shown.
2 changes: 1 addition & 1 deletion modules/beamdyn/src/BeamDyn.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6609,7 +6609,7 @@ SUBROUTINE BD_GetOP( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, u_op,
FieldMask(MASKID_RotationVel) = .true.
FieldMask(MASKID_TranslationAcc) = .true.
FieldMask(MASKID_RotationAcc) = .true.
call PackMotionMesh(y%BldMotion, y_op, index, FieldMask=FieldMask, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%BldMotion, y_op, index, FieldMask=FieldMask, UseSmlAngle=ReturnLogMap)

index = index - 1
do i=1,p%NumOuts + p%BldNd_TotNumOuts
Expand Down
16 changes: 8 additions & 8 deletions modules/elastodyn/src/ElastoDyn.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11643,8 +11643,8 @@ SUBROUTINE Compute_dY(p, y_p, y_m, delta, dY)
end do
end if

call PackMotionMesh_dY(y_p%PlatformPtMesh, y_m%PlatformPtMesh, dY, indx_first, UseLogMaps=.true.)
call PackMotionMesh_dY(y_p%TowerLn2Mesh, y_m%TowerLn2Mesh, dY, indx_first, UseLogMaps=.true.)
call PackMotionMesh_dY(y_p%PlatformPtMesh, y_m%PlatformPtMesh, dY, indx_first, UseSmlAngle=.true.)
call PackMotionMesh_dY(y_p%TowerLn2Mesh, y_m%TowerLn2Mesh, dY, indx_first, UseSmlAngle=.true.)
call PackMotionMesh_dY(y_p%HubPtMotion, y_m%HubPtMotion, dY, indx_first, FieldMask=Mask)
do k=1,p%NumBl
call PackMotionMesh_dY(y_p%BladeRootMotion(k), y_m%BladeRootMotion(k), dY, indx_first)
Expand Down Expand Up @@ -11781,16 +11781,16 @@ SUBROUTINE ED_GetOP( t, u, p, x, xd, z, OtherState, y, m, ErrStat, ErrMsg, u_op,
index = 1
if (allocated(y%BladeLn2Mesh)) then
do k=1,p%NumBl
call PackMotionMesh(y%BladeLn2Mesh(k), y_op, index, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%BladeLn2Mesh(k), y_op, index, UseSmlAngle=.false.)
end do
end if
call PackMotionMesh(y%PlatformPtMesh, y_op, index, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%TowerLn2Mesh, y_op, index, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%HubPtMotion, y_op, index, FieldMask=Mask, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%PlatformPtMesh, y_op, index, UseSmlAngle=ReturnLogMap)
call PackMotionMesh(y%TowerLn2Mesh, y_op, index, UseSmlAngle=ReturnLogMap)
call PackMotionMesh(y%HubPtMotion, y_op, index, FieldMask=Mask, UseSmlAngle=.false.)
do k=1,p%NumBl
call PackMotionMesh(y%BladeRootMotion(k), y_op, index, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%BladeRootMotion(k), y_op, index, UseSmlAngle=.false.)
end do
call PackMotionMesh(y%NacelleMotion, y_op, index, UseLogMaps=ReturnLogMap)
call PackMotionMesh(y%NacelleMotion, y_op, index, UseSmlAngle=.false.)

y_op(index) = y%Yaw ; index = index + 1
y_op(index) = y%YawRate ; index = index + 1
Expand Down
14 changes: 7 additions & 7 deletions modules/hydrodyn/src/HydroDyn.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3450,7 +3450,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE ( 1) !Module/Mesh/Field: u%Morison%Mesh%TranslationDisp = 1
u%Morison%Mesh%TranslationDisp (fieldIndx,node) = u%Morison%Mesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE ( 2) !Module/Mesh/Field: u%Morison%Mesh%Orientation = 2
CALL PerturbOrientationMatrix( u%Morison%Mesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%Morison%Mesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE ( 3) !Module/Mesh/Field: u%Morison%Mesh%TranslationVel = 3
u%Morison%Mesh%TranslationVel( fieldIndx,node) = u%Morison%Mesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE ( 4) !Module/Mesh/Field: u%Morison%Mesh%RotationVel = 4
Expand All @@ -3465,7 +3465,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE ( 7) !Module/Mesh/Field: u%WAMITMesh%TranslationDisp = 7
u%WAMITMesh%TranslationDisp (fieldIndx,node) = u%WAMITMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE ( 8) !Module/Mesh/Field: u%WAMITMesh%Orientation = 8
CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE ( 9) !Module/Mesh/Field: u%WAMITMesh%TranslationVel = 9
u%WAMITMesh%TranslationVel( fieldIndx,node) = u%WAMITMesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE (10) !Module/Mesh/Field: u%WAMITMesh%RotationVel = 10
Expand All @@ -3479,7 +3479,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE (13) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 13
u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE (14) !Module/Mesh/Field: u%PRPMesh%Orientation = 14
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE (15) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 15
u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE (16) !Module/Mesh/Field: u%PRPMesh%RotationVel = 16
Expand All @@ -3494,7 +3494,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE ( 7) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 7
u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE ( 8) !Module/Mesh/Field: u%PRPMesh%Orientation = 8
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE ( 9) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 9
u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE (10) !Module/Mesh/Field: u%PRPMesh%RotationVel = 10
Expand All @@ -3510,7 +3510,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE (1) !Module/Mesh/Field: u%WAMITMesh%TranslationDisp = 1
u%WAMITMesh%TranslationDisp (fieldIndx,node) = u%WAMITMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE (2) !Module/Mesh/Field: u%WAMITMesh%Orientation = 2
CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%WAMITMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE (3) !Module/Mesh/Field: u%WAMITMesh%TranslationVel = 3
u%WAMITMesh%TranslationVel( fieldIndx,node) = u%WAMITMesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE (4) !Module/Mesh/Field: u%WAMITMesh%RotationVel = 4
Expand All @@ -3524,7 +3524,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE ( 7) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 7
u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE ( 8) !Module/Mesh/Field: u%PRPMesh%Orientation = 8
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE ( 9) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 9
u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE (10) !Module/Mesh/Field: u%PRPMesh%RotationVel = 10
Expand All @@ -3539,7 +3539,7 @@ SUBROUTINE HD_Perturb_u( p, n, perturb_sign, u, du )
CASE ( 1) !Module/Mesh/Field: u%PRPMesh%TranslationDisp = 1
u%PRPMesh%TranslationDisp (fieldIndx,node) = u%PRPMesh%TranslationDisp (fieldIndx,node) + du * perturb_sign
CASE ( 2) !Module/Mesh/Field: u%PRPMesh%Orientation = 2
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseLogMaps=.true. )
CALL PerturbOrientationMatrix( u%PRPMesh%Orientation(:,:,node), du * perturb_sign, fieldIndx, UseSmlAngle=.true. )
CASE ( 3) !Module/Mesh/Field: u%PRPMesh%TranslationVel = 3
u%PRPMesh%TranslationVel( fieldIndx,node) = u%PRPMesh%TranslationVel( fieldIndx,node) + du * perturb_sign
CASE ( 4) !Module/Mesh/Field: u%PRPMesh%RotationVel = 4
Expand Down
52 changes: 30 additions & 22 deletions modules/nwtc-library/src/ModMesh.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3075,20 +3075,21 @@ END SUBROUTINE PackMotionMesh_Names
!> This subroutine returns the operating point values of the mesh fields. It assumes all fields marked
!! by FieldMask are allocated; Some fields may be allocated by the ModMesh module and not used in
!! the linearization procedure, thus I am not using the check if they are allocated to determine if they should be included.
SUBROUTINE PackMotionMesh(M, Ary, indx_first, FieldMask, UseLogMaps)
SUBROUTINE PackMotionMesh(M, Ary, indx_first, FieldMask, UseSmlAngle)

TYPE(MeshType) , INTENT(IN ) :: M !< Motion mesh
REAL(ReKi) , INTENT(INOUT) :: Ary(:) !< array to pack this mesh into
INTEGER(IntKi) , INTENT(INOUT) :: indx_first !< index into Ary; gives location of next array position to fill
LOGICAL, OPTIONAL , INTENT(IN ) :: FieldMask(FIELDMASK_SIZE) !< flags to determine if this field is part of the packing
LOGICAL, OPTIONAL , INTENT(IN ) :: UseLogMaps !< flag to determine if the orientation should be packed as a DCM or a log map
LOGICAL, OPTIONAL , INTENT(IN ) :: UseSmlAngle !< flag to determine if the orientation should be packed as a DCM or a log map


! local variables:
INTEGER(IntKi) :: i, j, k
LOGICAL :: Mask(FIELDMASK_SIZE) !< flags to determine if this field is part of the packing
LOGICAL :: OutputLogMap
REAL(R8Ki) :: logmap(3) !< array to pack logmaps into
LOGICAL :: OutputSmlAngle
!REAL(R8Ki) :: logmap(3) !< array to pack logmaps into
REAL(R8Ki) :: angles(3) !< array to pack logmaps into
INTEGER(IntKi) :: ErrStat2
CHARACTER(ErrMsgLen) :: ErrMsg2

Expand All @@ -3110,17 +3111,19 @@ SUBROUTINE PackMotionMesh(M, Ary, indx_first, FieldMask, UseLogMaps)
end if

if (Mask(MASKID_ORIENTATION)) then
if (present(UseLogMaps)) then
OutputLogMap = UseLogMaps
if (present(UseSmlAngle)) then
OutputSmlAngle = UseSmlAngle
else
OutputLogMap = .false.
OutputSmlAngle = .false.
end if

if (OutputLogMap) then
if (OutputSmlAngle) then
do i=1,M%NNodes
call DCM_logMap(M%Orientation(:,:,i), logmap, ErrStat2, ErrMsg2)
!call DCM_logMap(M%Orientation(:,:,i), logmap, ErrStat2, ErrMsg2)
angles = GetSmllRotAngs ( M%Orientation(:,:,i), ErrStat2, ErrMsg2 )
do k=1,3
Ary(indx_first) = logmap(k)
!Ary(indx_first) = logmap(k)
Ary(indx_first) = angles(k)
indx_first = indx_first + 1
end do
end do
Expand Down Expand Up @@ -3176,23 +3179,25 @@ SUBROUTINE PackMotionMesh(M, Ary, indx_first, FieldMask, UseLogMaps)
END SUBROUTINE PackMotionMesh
!...............................................................................................................................
!> This subroutine computes the differences of two meshes and packs that value into appropriate locations in the dY array.
SUBROUTINE PackMotionMesh_dY(M_p, M_m, dY, indx_first, FieldMask, UseLogMaps)
SUBROUTINE PackMotionMesh_dY(M_p, M_m, dY, indx_first, FieldMask, UseSmlAngle)

TYPE(MeshType) , INTENT(IN ) :: M_p !< ED outputs on given mesh at \f$ u + \Delta u \f$ (p=plus)
TYPE(MeshType) , INTENT(IN ) :: M_m !< ED outputs on given mesh at \f$ u - \Delta u \f$ (m=minus)
REAL(R8Ki) , INTENT(INOUT) :: dY(:) !< column of dYdu \f$ \frac{\partial Y}{\partial u_i} = \frac{y_p - y_m}{2 \, \Delta u}\f$
INTEGER(IntKi) , INTENT(INOUT) :: indx_first !< index into dY array; gives location of next array position to fill
LOGICAL, OPTIONAL , INTENT(IN ) :: FieldMask(FIELDMASK_SIZE) !< flags to determine if this field is part of the packing
LOGICAL, OPTIONAL , INTENT(IN ) :: UseLogMaps !< flag to determine if the orientation should be packed as a DCM or a log map
LOGICAL, OPTIONAL , INTENT(IN ) :: UseSmlAngle !< flag to determine if the orientation should be packed as a DCM or a log map

! local variables:
INTEGER(IntKi) :: ErrStat2 ! we're ignoring the errors about small angles
CHARACTER(ErrMsgLen) :: ErrMsg2

INTEGER(IntKi) :: i, indx_last
LOGICAL :: OutputLogMap
REAL(R8Ki) :: lambda_m(3)
REAL(R8Ki) :: lambda_p(3)
LOGICAL :: OutputSmlAngle
!REAL(R8Ki) :: lambda_m(3)
!REAL(R8Ki) :: lambda_p(3)
REAL(R8Ki) :: angles_m(3)
REAL(R8Ki) :: angles_p(3)
REAL(R8Ki) :: smallAngles(3)
REAL(R8Ki) :: orientation(3,3)
LOGICAL :: Mask(FIELDMASK_SIZE) !< flags to determine if this field is part of the packing
Expand All @@ -3213,18 +3218,21 @@ SUBROUTINE PackMotionMesh_dY(M_p, M_m, dY, indx_first, FieldMask, UseLogMaps)
end if

if (Mask(MASKID_ORIENTATION)) then
if (present(UseLogMaps)) then
OutputLogMap = UseLogMaps
if (present(UseSmlAngle)) then
OutputSmlAngle = UseSmlAngle
else
OutputLogMap = .false.
OutputSmlAngle = .false.
end if

if (OutputLogMap) then
if (OutputSmlAngle) then
do i=1,M_p%NNodes
call DCM_logMap( M_m%Orientation(:,:,i), lambda_m, ErrStat2, ErrMsg2 )
call DCM_logMap( M_p%Orientation(:,:,i), lambda_p, ErrStat2, ErrMsg2 )
!call DCM_logMap( M_m%Orientation(:,:,i), lambda_m, ErrStat2, ErrMsg2 )
!call DCM_logMap( M_p%Orientation(:,:,i), lambda_p, ErrStat2, ErrMsg2 )
angles_m = GetSmllRotAngs ( M_m%Orientation(:,:,i), ErrStat2, ErrMsg2 )
angles_p = GetSmllRotAngs ( M_p%Orientation(:,:,i), ErrStat2, ErrMsg2 )

smallAngles = lambda_p - lambda_m
!smallAngles = lambda_p - lambda_m
smallAngles = angles_p - angles_m

indx_last = indx_first + 2
dY(indx_first:indx_last) = smallAngles
Expand Down
42 changes: 24 additions & 18 deletions modules/nwtc-library/src/NWTC_Num.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4578,43 +4578,49 @@ FUNCTION OuterProductR16(u,v)

END FUNCTION OuterProductR16
!=======================================================================
!> This subroutine perturbs an orientation matrix by a small angle, using
!! a logarithmic map. For small angles, the change in angle is equivalent to
!! a change in log map parameters.
!! NOTE: all warnings from DCM_LogMap and SmllRotTrans are ignored.
!! NOTE2: notice no checks are made to verify correct set of inputs given
!! one of the follwing combinations must be provided (others truly optional):
!! Perturbations
!! Perturbation + AngleDim

SUBROUTINE PerturbOrientationMatrix( Orientation, Perturbation, AngleDim, Perturbations, UseLogMaps )
!> This subroutine perturbs an orientation matrix by a small angle. Two methods
!! are used:
!! small angle DCM: perturb small angles extracted from DCM
!! large angle DCM: multiply input DCM with DCM created with small angle
!! perturbations
!! NOTE1: this routine originally used logarithmic mapping for small angle
!! perturbations
!! NOTE2: all warnings from SmllRotTrans are ignored.
!! NOTE3: notice no checks are made to verify correct set of inputs given
!! one of the follwing combinations must be provided (others truly
!! optional):
!! Perturbations
!! Perturbation + AngleDim
SUBROUTINE PerturbOrientationMatrix( Orientation, Perturbation, AngleDim, Perturbations, UseSmlAngle )
REAL(R8Ki), INTENT(INOUT) :: Orientation(3,3)
REAL(R8Ki), OPTIONAL, INTENT(IN) :: Perturbation ! angle (radians) of the perturbation
INTEGER, OPTIONAL, INTENT(IN) :: AngleDim
REAL(R8Ki), OPTIONAL, INTENT(IN) :: Perturbations(3) ! angles (radians) of the perturbations
LOGICAL, OPTIONAL, INTENT(IN) :: UseLogMaps
LOGICAL, OPTIONAL, INTENT(IN) :: UseSmlAngle

! Local variables
REAL(R8Ki) :: angles(3)
REAL(R8Ki) :: OrientationTmp(3,3)
LOGICAL :: OutputLogMap
LOGICAL :: OutputSmlAngle
integer(intKi) :: ErrStat2
character(ErrMsgLen) :: ErrMsg2

if (present(UseLogMaps)) then
OutputLogMap = UseLogMaps
if (present(UseSmlAngle)) then
OutputSmlAngle = UseSmlAngle
else
OutputLogMap = .false.
OutputSmlAngle = .false.
end if

if (OutputLogMap) then
CALL DCM_LogMap( Orientation, angles, ErrStat2, ErrMsg2 )
if (OutputSmlAngle) then
! CALL DCM_LogMap( Orientation, angles, ErrStat2, ErrMsg2 )
angles = GetSmllRotAngs ( Orientation, ErrStat2, ErrMsg2 )
IF (PRESENT(Perturbations)) THEN
angles = angles + Perturbations
ELSE
angles(AngleDim) = angles(AngleDim) + Perturbation
END IF
Orientation = DCM_exp( angles )
! Orientation = DCM_exp( angles )
call SmllRotTrans( 'linearization perturbation', angles(1), angles(2), angles(3), OrientationTmp, ErrStat=ErrStat2, ErrMsg=ErrMsg2 )
else !Only works if AngleDim is specified
IF (PRESENT(Perturbations)) THEN
angles = Perturbations
Expand Down
Loading

0 comments on commit 8364b53

Please sign in to comment.