(* * LieGroups.m - a Mathematica Package for Mechanical control * systems on Lie Groups * * Francesco Bullo * * Division of Engineering and Applied Science * California Institute of Technology * Pasadena, CA 91125 * * Copyright (c) 1996-2000, Francesco Bullo. * Permission is granted to copy, modify and redistribute this file, * provided that this header message is retained. * * Revision history: * * Version 0.8 (Jan 00) * Version 0.07 (August 97) * Version 0.06 (August 97) * Version 0.05 (June 97) * Version 0.03 (July 96) * Version 0.02 (June 96) * Version 0.01 (May 96) * * Comments: * - rotation variables always first. * - for numerics, rotations are represented as quaternions *) BeginPackage["LieGroups`"]; adjoint::usage="adjoint[G,xi] computes the adjoint map ad_xi on the group G."; kindrift::usage="kindrift[G,g,xi] computes the kinematic drift. quaternions for rotations."; LogG::usage="LogG[G,g] computes the logarithm map (numerical, quaternions for rotation). "; ExpG::usage="ExpG[G,X] computes the exponential map (numerical, quaternions for rotation). "; ComposeG::usage= "ComposeG[G,g1,g2] computes the group operation between g1, g2. As usual, rotations are represented as unit quaternions. "; InverseG::usage= "InverseG[G,g] computes the group inverse of g. As usual, rotations are represented as unit quaternions. "; Lie::usage= "Lie[xi,eta,G] computes the Lie bracket between the vectors xi and eta on the group G"; Sym::usage= "Sym[xi,eta,G,M] computes the symmetric product between the vectors xi and eta on the group G and with metric M."; LieDual::usage= "LieDual[alpha,beta,G,M] computes the dual Lie bracket between the one forms alpha and beta on the group G"; SymDual::usage= "SymDual[alpha,beta,G,M] computes the dual symmetric product between the one forms alpha and beta on the group G and with metric M."; (* ************** *) Groups::usage= "Implemented groups are SE2, SO3, SE3, SO3xT, SO3xTxT, SE2xT, SE3xT, SE3xTxT."; SO3::usage= "SO3 is the Special Orthogonal group. Vectors in so3 are parametrized as (om1, om2, om3)."; SO3xT::usage= "SO3xT is the direct product of the Special Orthogonal group and a 1 dimensional torus. Vectors in so3xR are parametrized as (om1, om2, om3, s)."; SO3xTxT::usage= "SO3xTxT is the direct product of the Special Orthogonal group SO3 and two 1 dimensional tori. Vectors in so3xRxR are parametrized as (om1, om2, om3, s1, s2)."; SE2::usage= "SE2 is the Special Euclidean group of the plane. Vectors in se2 are parametrized as (om, v1, v2)."; SE2xT::usage= "SE2xT is the direct product of the SE2 and a 1 dimensional torus. Vectors in se2xR are parametrized as (om, v1, v2, s)."; SE3::usage= "SE3 is the Special Euclidean group of R^3. Vectors in se3 are (Omega, V)."; SE3xT::usage= "SE3xT is the direct product of the SE3 and a 1 dimensional torus."; SE3xTxT::usage= "SE3xTxT is the direct product of the SE3 and two 1 dimensional tori."; (* *********** *) EPdrift::usage= "EPdrift[xi,G,M] is Euler-Poincare' vector field on the Lie algebra."; LPdrift::usage= "LPdrift[mu,G,M] is Lie-Poisson one form on the dual of the Lie algebra."; Nabla::usage= "Nabla[xi,eta,G,M] is the covariant derivative of the vectors xi and eta on the group G with metric M."; InProd::usage="InProd[xi,eta,M] computes the M inner product of xi and eta."; InProdDual::usage= "InProd[alpha,beta,M] computes the M inner product of alpha and beta on the dual of the Lie algebra."; bar::usage="bar[f] computes the definite integral from 0 to t. We assume f contains t as time variable."; (* matrix utilities *) zeroMatrix::usage="zeroMatrix[nr, nc] or [n] ."; StackRows::usage="stacks rows together"; StackCols::usage="stacks columns together"; extractSubMatrix::usage="extractSubMatrix[A, rows, cols] "; (* graphics *) MyRotateShape::usage="MyRotateShape[shape, R] rotates a shape with R in SO(3)"; MyTranslateShape::usage="MyTranslateShape[shape, vect] translates a shape a distance vect"; RotateShapeSE2::usage="RotateShapeSE2[shape, theta] rotates a shape of an angle theta"; TranslateShapeSE2::usage="TranslateShapeSE2[shape, vect] translates a shape a distance vect"; (* elementary on quaternions *) Hat::usage="Hat[xi] computes hat isomorphism"; HatInv::usage="HatInv[xi] computes the inversehat isomorphism"; MatToQuat::usage"MatToQuat[R] computes the quaternion associated to R in SO(3). NEED FIX for when R is Pi rotation"; QuatToMat::usage="QuatToMat[Q] computes the R in SO(3) associated to q"; (* Old: exponential coordinates on SO(3) *) LogSO3::usage="LogSO3[R] computes the exponential coordinates of R in SO(3). Numerical."; ExpSO3::usage="ExpSO3[X] computes the rotation matrix associated with X in R^3. Numerical."; (* Craig, Introduction to Robotics, Mechanics and Control * Notation from Chapter 1: {^A}point = {^A}{_B}rotation {^B}point, where {A} is * fixed frame and {B} is body. Hence the R i always use is: {^A}{_B}rotation * From page 42 and on. *) ZYXEuler::usage= "ZYXEuler[phi, theta, psi] computes the rotation matrix \ associated with ZYX Euler angles. The singularity is at Cos[theta]=0."; ArcZYXEuler::usage= "ArcZYXEuler[R] compute the inverse kinematic map for the ZYX Euler angles and it sets -Pi/210^-10, A = { {Sin[th], Cos[th]-1}, {1-Cos[th], Sin[th]}}/th, A = IdentityMatrix[2] ]; Join[ {th}, A. x[[{2,3}]] ] ] SE2[LogG,g_] := Module[{th,Ainv}, th = N[ g[[1]] ]; If[ Abs[th]>10^-10, Ainv = .5 th (1+Cos[th])/Sin[th] IdentityMatrix[2] - .5 th {{0,-1},{1,0}} , Ainv = IdentityMatrix[2] ]; Join[ {th}, Ainv. g[[{2,3}]] ] ] SE2[ComposeG, g1_, g2_] := Module[{th=g1[[1]] }, Join[ {g1[[1]]+g2[[1]]}, g1[[{2,3}]]+ {{Cos[th],-Sin[th]},{Sin[th],Cos[th]}}.g2[[{2,3}]] ] ] SE2[InverseG, g_] := Module[{th=g[[1]]}, Join[{-th}, - {{Cos[th],Sin[th]},{-Sin[th],Cos[th]}}. g[[{2,3}]] ] ] (* necessary definitions: SO3[Dimension] := SO3[adjoint, xi_] := SO3[adjoint, xi1_, xi2_] := SO3[kindrift, q_, xi_] := SO3[ExpG,X_]:= SO3[LogG,RR_] := SO3[ComposeG, q1_, q2_] := SO3[InverseG, q_] := *) (* ************************** *) (* S0(3) descriptor functions *) (* g = quaternion *) (* xi = {om1, om2, om3} *) SO3[Dimension] := 3; SO3[adjoint, xi_] := {{0, -xi[[3]], xi[[2]]}, {xi[[3]], 0, -xi[[1]]}, {-xi[[2]], xi[[1]], 0}}; SO3[adjoint, xi1_, xi2_] := SO3[adjoint, xi1] . xi2; SO3[kindrift, q_, xi_] := Module[{E}, E = q[[1]] IdentityMatrix[3] + Hat[ q[[{2,3,4}]] ]; 1/2 Join[{-q[[{2,3,4}]].xi}, E.xi] ] (* Exponential coordinates *) SO3[ExpG,X_]:= Module[{NormX, VersX}, NormX = N[Sqrt[ X. X]]; If[ NormX < 10^-10, IdentityMatrix[3] , VersX = N[X/NormX]; MatToQuat[ IdentityMatrix[3] + Sin[NormX]*SO3[adjoint, VersX] + (1.0 - Cos[NormX])*(SO3[adjoint, VersX]. SO3[adjoint, VersX]) ] ] ]; SO3[LogG,RR_] := Module[{a,A, R}, If[ Dimensions[RR]=={4}, R=QuatToMat[RR], R=RR ]; a = N[ArcCos[ (Sum[ R[[i,i]], {i,3}]-1)*.5]]; If[ Abs[a]>10^-10, A = N[(R-Transpose[R])*0.5*a/Sin[a]], A=zeroMatrix[3]]; {A[[3,2]], A[[1,3]], A[[2,1]]} ]; SO3[ComposeG, q1_, q2_] := Module[{}, Join[ {q1[[1]] q2[[1]] - q1[[{2,3,4}]].q2[[{2,3,4}]]}, q1[[1]] q2[[{2,3,4}]] + q2[[1]] q1[[{2,3,4}]] + Hat[q1[[{2,3,4}]]].q2[[{2,3,4}]] ] ] SO3[InverseG, q_] := {q[[1]], -q[[2]],-q[[3]], -q[[4]]}; (* ************************** *) (* S0(3)xT descriptor functions *) (* xi = {om1, om2, om3, s1} *) SO3xT[Dimension] := 4; SO3xT[adjoint, xi_] := {{0, -xi[[3]], xi[[2]], 0}, {xi[[3]], 0, -xi[[1]], 0}, {-xi[[2]], xi[[1]], 0, 0}, {0, 0, 0, 0}}; SO3xT[adjoint, xi1_, xi2_] := SO3xT[adjoint, xi1] . xi2; SO3xT[kindrift, g_, xi_] := Module[{}, Join[ SO3[kindrift,g[[{1,2,3,4}]] , xi[[{1,2,3}]] ] , {xi[[4]]} ]] SO3xT[ExpG, X_] := Module[{}, Join[ SO3[ExpG,X[[{1,2,3}]]] ,{X[[4]]} ] ]; SO3xT[LogG, g_] := Module[{}, Join[ SO3[LogG, g[[{1,2,3,4}]] ] , {g[[5]]}]]; SO3xT[ComposeG, g1_, g2_] := Module[{}, Join[SO3[ComposeG, g1[[{1,2,3,4}]], g2[[{1,2,3,4}]] ], {g1[[5]] + g2[[5]]} ] ]; SO3xT[InverseG, g_] := {g[[1]], -g[[2]],-g[[3]], -g[[4]], -g[[5]]}; (* ************************** *) (* S0(3)xTxT descriptor functions *) (* xi = {om1, om2, om3, s1, s2} *) SO3xTxT[Dimension] := 4; SO3xTxT[adjoint, xi_] := {{0, -xi[[3]], xi[[2]], 0, 0}, {xi[[3]], 0, -xi[[1]], 0, 0}, {-xi[[2]], xi[[1]], 0, 0, 0}, {0, 0, 0, 0, 0}, {0, 0, 0, 0, 0}}; SO3xTxT[adjoint, xi1_, xi2_] := SO3xTxT[adjoint, xi1] . xi2; SO3xTxT[kindrift, g_, xi_] := Module[{}, Join[ SO3[kindrift,g[[{1,2,3,4}]] , xi[[{1,2,3}]] ] , {xi[[{4,5}]]} ]] SO3xTxT[ExpG, X_] := Module[{}, Join[ SO3[ExpG,X[[{1,2,3}]]] ,{X[[{4,5}]]} ] ]; SO3xTxT[LogG, g_] := Module[{}, Join[ SO3[LogG, g[[{1,2,3,4}]] ] , {g[[5]]}]]; SO3xTxT[ComposeG, g1_, g2_] := Module[{}, Join[SO3[ComposeG, g1[[{1,2,3,4}]], g2[[{1,2,3,4}]] ], {g1[[5]] + g2[[5]], g1[[6]]+g2[[6]]} ] ]; SO3xTxT[InverseG, g_] := {g[[1]], -g[[2]],-g[[3]], -g[[4]], -g[[5]], -g[[6]]}; (* ************************** *) (* SE(3) descriptor functions *) (* g = [quat, p ] *) (* xi = {om1, om2, om3, v1, v2, v3} *) SE3[Dimension] := 6; SE3[adjoint, xi_] := Block[ {omega=xi[[{1,2,3}]], vel=xi[[{4,5,6}]] }, StackRows[ StackCols[ adjoint[SO3,omega], zeroMatrix[3] ] , StackCols[ adjoint[SO3,vel], adjoint[SO3,omega]] ] ]; SE3[adjoint, xi1_, xi2_] := SE3[adjoint, xi1] . xi2; SE3[kindrift, g_, xi_] := Module[{quat,primo, secondo}, quat = g[[{1,2,3,4}]]; primo = Evaluate[ SO3[kindrift, quat, xi[[{1,2,3}]] ] ]; secondo = Evaluate[QuatToMat[quat].xi[[{4,5,6}]]]; Join[primo, secondo] ] SE3[LogG, g_] := Module[ {psi,Npsi,alpha,Ainv}, psi = SO3[LogG,g[[{1,2,3,4}]]]; Npsi = N[Sqrt[psi.psi]]; If[ Npsi < 10^-10, Ainv = IdentityMatrix[3], alpha = .5 Npsi Cot[.5 Npsi]; Ainv = IdentityMatrix[3] - .5 Hat[psi] + ((1-alpha)/Npsi^2) Hat[psi].Hat[psi];]; Join[psi, Ainv . g[[{5,6,7}]] ] ] SE3[ExpG,X_] := Module[{psi,Npsi,A}, psi = X[[{1,2,3}]]; Npsi = N[ Sqrt[psi.psi]]; If[ Npsi < 10^-10, A=IdentityMatrix[3], A= IdentityMatrix[3]+((1-Cos[Npsi])/Npsi^2) Hat[psi] + ((1-(Sin[Npsi]/Npsi))/Npsi^2) Hat[psi]. Hat[psi]; ]; Join[ SO3[ExpG,psi] , A.X[[{4,5,6}]] ] ] SE3[ComposeG, g1_, g2_] := Module[{q1=g1[[{1,2,3,4}]], q2=g2[[{1,2,3,4}]]}, Join[ SO3[ComposeG, q1,q2], QuatToMat[q1].g2[[{5,6,7}]]+g1[[{5,6,7}]] ] ] SE3[InverseG, g_] := Module[{q={ g[[1]], -g[[2]],-g[[3]], -g[[4]]}}, Join[q, - QuatToMat[q].g[[{5,6,7}]] ] ] (* ************************** *) (* SE(3)xTxT descriptor functions *) (* g = [quat, p ] *) (* xi = {om1, om2, om3, v1, v2, v3} *) SE3xTxT[Dimension] := 8; SE3xTxT[adjoint, xi_] := Block[ {omega=xi[[{1,2,3}]], vel=xi[[{4,5,6}]], tmp }, tmp = StackRows[ StackCols[ adjoint[SO3,omega], zeroMatrix[3] ] , StackCols[ adjoint[SO3,vel], adjoint[SO3,omega]] ]; StackRows[ StackCols[ tmp, zeroMatrix[6,2] ] , zeroMatrix[2,8] ] ]; SE3xTxT[adjoint, xi1_, xi2_] := SE3[adjoint, xi1] . xi2; SE3xTxT[kindrift, g_, xi_] := Module[{quat,primo, secondo}, quat = g[[{1,2,3,4}]]; primo = Evaluate[ SO3[kindrift, quat, xi[[{1,2,3}]] ] ]; secondo = Evaluate[QuatToMat[quat].xi[[{4,5,6}]]]; Join[primo, secondo, xi[[{7,8}]] ] ]; SE3xTxT[ExpG, X_] := Module[{}, Join[ SE3[ExpG,X[[{1,2,3,4,5,6}]]] ,X[[{7,8}]] ] ]; SE3xTxT[LogG, g_] := Module[{}, Join[ SE3[LogG, g[[{1,2,3,4,5,6,7}]] ] , g[[{8,9}]] ]]; SE3xTxT[ComposeG, g1_, g2_] := Module[{}, Join[SE3[ComposeG, g1[[{1,2,3,4,5,6,7}]], g2[[{1,2,3,4,5,6,7}]] ], g1[[{8,9}]] + g2[[{8,9}]] ] ]; SE3xTxT[InverseG, g_] := Module[{}, Join[ SE3[InverseG, g[[{1,2,3,4,5,6,7}]]], -g[[{8,9}]] ] ]; (*********************************************************) (* define the two brackets *) InProd[xi_,eta_,M_] := Expand[xi.M.eta]; InProdDual[alpha_,beta_,M_] := Expand[alpha.Inverse[M].beta]; Lie[xi_,eta_,G_] := Simplify[ G[adjoint,xi,eta] ]; Sym[xi_,eta_,G_,M_] := - Simplify[ Inverse[M] . ( Transpose[ adjoint[G,xi] ].M.eta + Transpose[ adjoint[G,eta]].M.xi )]; SymDual[alpha_,beta_,G_,M_] := - Simplify[ ( Transpose[adjoint[G,Inverse[M].alpha]].beta + Transpose[adjoint[G,Inverse[M].beta]].alpha )]; LieDual[alpha_,beta_,G_,M_] := Simplify[ M.Lie[ Inverse[M].alpha, Inverse[M].beta ,G] ]; (* drift for simple mechanical systems on Lie groups *) (* See Marsden Ratiu "Mechanics and Symmetry" *) EPdrift[xi_,G_,M_] := -Sym[xi,xi,G,M]/2; LPdrift[mu_,G_,M_] := -SymDual[mu,mu,G,M]/2; (***********************************************) (***********************************************) (* * Utility for time functions manipulation *) sinus = Table[Sin[j t], {j,1,9}]; cosinus = Table[Cos[j t], {j,1,9}]; various = Join[{t},sinus, cosinus]; bar[g_] := Collect[ Expand[ Integrate[ Expand[ (g /. t->temp) , Trig -> True] , {temp,0,t}] , Trig -> True], various ]; (***********************************************) (***********************************************) (* * Utility functions for stacking rows, cols, + other matrix operations * * StackRows stack rows of matrices together * StackCols stack cols of matrices together * zeroMatrix create a matrix of zeros * extractSubMatrix pick out pieces of a matrix [private] *) (* Stack matrix columns together *) StackCols[mats__] := Block[ {i,j}, Table[ Join[ Flatten[Table[{mats}[[j]][[i]], {j,Length[{mats}]}], 1] ], {i, Length[ {mats}[[1]] ]}] ]; (* Stack matrix rows together *) StackRows[mats__] := Join[Flatten[{mats}, 1]]; (* Create matrices of zeros *) zeroMatrix[nr_, nc_] := Table[0, {nr}, {nc}]; zeroMatrix[nr_] := zeroMatrix[nr, nr]; (* Extract a submatrix from a bigger matrix *) extractSubMatrix[A_, rows_, cols_] := Map[Take[#, cols]&, Take[A, rows]]; (***********************************************) (***********************************************) (* Maurer-Cartan forms in SE2 g = {{Cos[t], -Sin[t], x},{Sin[t], Cos[t], y}, {0,0,1}}; dg = {{-Sin[t] dt, -Cos[t] dt, dx},{Cos[t] dt, -Sin[t] dt, dy}, {0,0,0}}; left = Simplify[Inverse[g] . dg]; right = Simplify[dg . Inverse[g]]; *) (***********************************************) (***********************************************) (* Covariant derivatives and curvatures *) (* to double check *) Nabla[xi_,eta_,G_,M_] := Expand[ Lie[xi,eta,G]/2 + Sym[xi,eta, G,M]/2 ]; R[xi_,eta_,zeta_,G_,M_] := Expand[ Nabla[xi,Nabla[eta,zeta,G,M],G,M] - Nabla[eta,Nabla[xi,zeta,G,M],G,M] - Nabla[ Lie[xi,eta,G],zeta,G,M] ]; K[xi_,eta_,G_,M_] := Expand[ xi.M.R[xi,eta,eta, G,M] ]; (***********************************************) (***********************************************) (* following code was Extension package *) (***********************************************) (***********************************************) (* note: RotationMatrix3D uses ZXZ Euler angle convention with - signs!*) (* in order to be consistent, we redefine "RotateShape" from Graphics`Shapes *) MyRotateShape[ shape_, R_ ] := Block[{}, shape /. { poly:Polygon[_] :> Map[(R . #)&, poly, {2}], line:Line[_] :> Map[(R . #)&, line, {2}], point:Point[_] :> Map[(R . #)&, point,{1}] } ]; RotateShapeSE2[ shape_, th_ ] := Block[{R={{Cos[th],-Sin[th]},{Sin[th],Cos[th]}} }, shape /. { poly:Polygon[_] :> Map[(R . #)&, poly, {2}], line:Line[_] :> Map[(R . #)&, line, {2}], point:Point[_] :> Map[(R . #)&, point,{1}] } ]; MyTranslateShape[shape_, vec_] := Block[{tvec = N[vec]}, shape /. { poly:Polygon[_] :> Map[(tvec + #)&, poly, {2}], line:Line[_] :> Map[(tvec + #)&, line, {2}], point:Point[_] :> Map[(tvec + #)&, point,{1}] } ]; TranslateShapeSE2[shape_, vec_] := Block[{tvec = N[vec]}, shape /. { poly:Polygon[_] :> Map[(tvec + #)&, poly, {2}], line:Line[_] :> Map[(tvec + #)&, line, {2}], point:Point[_] :> Map[(tvec + #)&, point,{1}] , Circle[x_,r_] :> Circle[x+tvec,r] , Disk[x_,r_] :> Disk[x+tvec,r]} ]; Hat[xi_] := {{0, -xi[[3]], xi[[2]]}, {xi[[3]], 0, -xi[[1]]}, {-xi[[2]], xi[[1]], 0}}; HatInv[xi_]:= {xi[[3,2]], xi[[1,3]], xi[[2,1]]}; MatToQuat[R_]:= Block[ {qv}, qv = HatInv[ 1/(2 Sqrt[ 1+ R[[1,1]]+R[[2,2]]+R[[3,3]] ]) *(R - Transpose[R]) ]; Join[ { Sqrt[1 - qv.qv]}, qv]]; QuatToMat[Q_] := Block[ {q0,qv}, q0 = Q[[1]]; qv = Q[[{2,3,4}]]; IdentityMatrix[3] + 2 q0 Hat[qv] + 2 Hat[qv].Hat[qv] ]; (*********************) (* ZYX Euler angles *) ZYXEuler[alpha_,beta_,gamma_] := RotZ[alpha].RotY[beta].RotX[gamma]; ArcZYXEuler[R_] := Module[{alpha, beta, gamma}, alpha =ATan2[ R[[2,1]], R[[1,1]]]; beta =ATan2[ -R[[3,1]], Sqrt[ R[[1,1]]^2 + R[[2,1]]^2 ] ]; gamma =ATan2[ R[[3,2]], R[[3,3]] ]; {alpha, beta, gamma} ]; (* ZYZ Euler angles *) ZYZEuler[alpha_,beta_,gamma_] := RotZ[alpha].RotY[beta].RotZ[gamma]; ArcZYZEuler[R_] := Module[{alpha, beta, gamma}, alpha =ATan2[ R[[2,3]], R[[1,3]]]; beta =ATan2[ Sqrt[ R[[3,1]]^2 + R[[3,2]]^2 ], R[[3,3]] ]; gamma =ATan2[ R[[3,2]], -R[[3,1]] ]; {alpha, beta, gamma} ]; (* Exponential coordinates *) ExpSO3[X_]:= Module[{NormX, VersX}, NormX = N[Sqrt[ X. X]]; If[ NormX < 10^-10, IdentityMatrix[3] , VersX = N[X/NormX]; IdentityMatrix[3] + Sin[NormX]*SO3[adjoint, VersX] + (1.0 - Cos[NormX])*(SO3[adjoint, VersX]. SO3[adjoint, VersX]) ] ]; LogGSO3[RR_] := Module[{a,A, R}, If[ Dimensions[RR]=={4}, R=QuatToMat[RR], R=RR ]; a = N[ArcCos[ (Sum[ R[[i,i]], {i,3}]-1)*.5]]; A = N[(R-Transpose[R])*0.5*a/Sin[a]]; {A[[3,2]], A[[1,3]], A[[2,1]]} ]; (* This is RotationMatrix3D from "Geometry`Rotations" *) (* Note the different convention on what a rotation about a single axis is. From that we have different results *) (*** RotationMatrix3D[phi_,theta_,psi_] := ***) ZXZmath[phi_,theta_,psi_] := { { Cos[psi] Cos[phi] - Cos[theta] Sin[phi] Sin[psi], Cos[psi] Sin[phi] + Cos[theta] Cos[phi] Sin[psi], Sin[psi] Sin[theta] } , { -Sin[psi] Cos[phi] - Cos[theta] Sin[phi] Cos[psi], -Sin[psi] Sin[phi] + Cos[theta] Cos[phi] Cos[psi], Cos[psi] Sin[theta] } , { Sin[theta] Sin[phi], -Sin[theta] Cos[phi], Cos[theta] } }; (* ZXZmath[] := RotZ[-psi] . RotX[-theta] . RotZ[-phi]; *) ArcZXZmath[R_] := Module[{phi,theta,psi}, phi = ATan2[ R[[3,1]], -R[[3,2]] ]; theta = ATan2[ Sqrt[ R[[1,3]]^2 + R[[2,3]]^2 ], R[[3,3]] ]; psi = ATan2[ R[[1,3]], R[[2,3]] ]; {phi, theta, psi} ]; (*****************************) (******************************) (* Utilities *) (* Rotations about a single axis*) RotX[alpha_] := {{1,0,0},{0,Cos[alpha], -Sin[alpha]}, {0,Sin[alpha], Cos[alpha]}}; RotY[alpha_] := {{Cos[alpha],0,Sin[alpha]},{0,1,0}, {-Sin[alpha],0, Cos[alpha]}}; RotZ[alpha_] := {{Cos[alpha], -Sin[alpha],0}, {Sin[alpha], Cos[alpha],0},{0,0,1}}; ATan2[y_,x_]:=ArcTan[x,y]; Norm[X_] := Sqrt[X.X]; SetEqual[v_, w_] := Module[{N,i}, N=Evaluate[Dimensions[v]][[1]]; Table[ v[[i]]==w[[i]], {i,1, N}] ]; SetRule[v_, w_] := Module[{N,i}, N=Evaluate[Dimensions[v]][[1]]; Table[ v[[i]]->w[[i]], {i,1, N}] ]; (* Close up the open environments *) End[]; EndPackage[];