u

PRM python version

M.Maris

2021 Oct 23

2021 Nov 20

"""

M.Maris

first version of order 0 Pointing Reconstruction Model (PRM)

Elemental Rotation Matrices according to Aereonautical convention

Those matrices projects Coordinate Axis of the Moving Reference Frame (example: camera) into the fixed reference frame (example: geographic topocentric coordinates).

alpha > 0 for an anticlockwise rotation

anticlockwise means:

Z rotation: 

     Y^

      |

   Z (.)->X 

Z exits from (X,Y) plane, X rotated toward Y 

equivalent to Z(X->Y)

Y rotation: Y exits from (Z,X) plane, Z rotated toward X

     X^

      |

   Y (.)->Z 

equivalent to Y(Z->X)

X rotation: X exits from (Y,Z) plane, Y rotated toward Z

     Z^

      |

   X (.)->Y 

equivalent to X(Y->Z)

RotX(alpha) : 

   1  0  0

   0  c -s

   0  s  c

RotY(alpha) : 

   c  0  s

   0  1  0

  -s  0  c

RotZ(alpha) : 

   c  s  0  

  -s  c  0

   0  0  1

with c=cos(alpha), s=sin(alpha)

Issue 

0.0.0 - 2021 Oct 23 : First implementation

0.0.1 - 2021 Nov 20 : Definition of matrices in agreement with STRIP PRM Doc

0.0.2 - 2021 Nov 22 : Improved defintion of matrices

"""

import numpy as np

class ElemRot :

   @property

   def axis(self) :

      return self._axis

   @property

   def angle(self) :

      return self._angle

   @angle.setter

   def angle(self,this) :

      self._angle=angle

      self._c=np.cos(angle)

      self._s=np.sin(angle)

      self._m=np.zeros([3,3,len(self)])

      self._m[0,0]=np.ones(len(self))

      self._m[1,1]=np.ones(len(self))

      self._m[2,2]=np.ones(len(self))

      #

      if self.axis=='x' :

         self._m[1,1]=self._c

         self._m[1,2]=-self._s

         self._m[2,1]=self._s

         self._m[2,2]=self._c

      elif self.axis=='y' :

         self._m[0,0]=self._c

         self._m[0,2]=self._s

         self._m[2,0]=-self._s

         self._m[2,2]=self._c

      elif self.axis=='z' :

         self._m[0,0]=self._c

         self._m[0,1]=-self._s

         self._m[1,0]=self._s

         self._m[1,1]=self._c

   @property

   def M(self) :

      return self._m

   def __init__(self,axis,angle) :

      self._axis=None

      self._angle=None

      if axis==0 or str(axis).lower()=='x' :

         self._axis='x'

      elif axis==1 or str(axis).lower()=='y' :

         self._axis='y'

      elif axis==2 or str(axis).lower()=='z' :

         self._axis='z'

      else :

         self._axis=str(axis)

      if not angle is None :

         self.angle=angle

   def __len__(self) :

      return self._m.shape[2]

   def __getitem__(self,i,j) :

      return self._m[i,j]

   def __setitem__(self,i,j,this) :

      return self._m[i,j]=this

   def copy(self) :

      import copy

      return copy.deepcopy(self)

   def __rmul__(self,this) :

      if this.__class__==self.__class__ :

         out=np.zeros([3,3,max(len(self),len(this))]

         for i in range(3) :

            for j in range(3) :

               for l in range(3) :

                  out[i,j]+=self[i,l]*this[l,j]

         oo=self.copy()

         oo._axis='undef'

         oo._m=out

         return oo

class PRM0 :

   @property

   def roll(self) :

      return self._d['roll']

   @roll.setter

   def roll(self,this) :

      self._d['roll']=this

      self._map['roll']=1

      self._matr['roll']=self.MatrZ(np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def pan(self) :

      return self._d['pan']

   @pan.setter

   def pan(self,this) :

      self._d['pan']=this

      self._map['pan']=1

      self._matr['pan']=self.MatrY(np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def tilt(self) :

      return self._d['tilt']

   @tilt.setter

   def tilt(self,this) :

      self._d['tilt']=this

      self._map['tilt']=1

      self._matr['tilt']=self.MatrX(np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def ctheta0(self) :

      return self._d['ctheta0']

   @ctheta0.setter

   def ctheta0(self,this) :

      self._d['ctheta0']=this

      self._map['ctheta0']=1

      self._matr['ctheta0']=self.MatrY(-np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def cphi0(self) :

      return self._d['cphi0']

   @cphi0.setter

   def cphi0(self,this) :

      self._d['cphi0']=this

      self._map['cphi0']=1

      self._matr['cphi0']=self.MatrZ(-np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def tiltFork(self) :

      return self._d['tiltFork']

   @tiltFork.setter

   def tiltFork(self,this) :

      self._d['tiltFork']=this

      self._map['tiltFork']=1

      self._matr['tiltFork']=self.MatrX(np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def zVax(self) :

      return self._d['zVax']

   @zVax.setter

   def zVax(self,this) :

      self._d['zVax']=this

      self._map['zVax']=1

      self._matr['zVax']=self.MatrX(np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def omegaVax(self) :

      return self._d['omegaVax']

   @omegaVax.setter

   def omegaVax(self,this) :

      self._d['omegaVax']=this

      self._map['omegaVax']=1

      self._matr['omegaVax']=self.MatrZ(np.deg2rad(np.array([this])))

      self._isComplete=False

   #

   @property

   def isComplete(self) :

      return self._isComplete

   #

   def keys(self) :

      return ['roll','pan','tilt','ctheta0','cphi0','tiltFork','zVax','omegaVax']

   #

   def __getitem__(self,this) :

      return self._d[this]

   #

   def __setitem__(self,this,that) :

      self._d[this]=that

   #

   def copy(self) :

      import copy

      return copy.deepcopy(self)

   #

   def __init__(self) :

      self._isComplete=False

      self._d={}

      for k in self.keys() :

         self._d[k]=None

      self._mark={}

      for k in self.keys() :

         self._mark[k]=0

      self._matr={}

      for k in self.keys() :

         self._matr[k]=None

   #

   def isAllSet(self) :

      for k in self.keys() :

         if self._mark[k]==0 :

            return False

      return True

   #

   def commit(self) :

      if self.isAllSet :

         out=self.rmult(self._matr['pan'],self._matr['roll'])

         out=self.rmult(self._matr['tilt'],out)

         self._matr['M1']=self.rmult(self._matr['ctheta0'],out)

         self._matr['M2']=self.rmult(self._matr['cphi0'],self._matr['tiltFork'])

         self._matr['M3']=self.rmult(self._matr['omegaVax'],self._matr['zVax'])

         self._isComplete=True

      else :

         self._isComplete=False

         raise Error('Not configuration not completed','')

   #

   def __call__(self,CTHETA_PHI,axis0isNPOS=False) :

      """ 

Apply rotations for a list of NPOS control angles: (CTR_THETA, CTR_PHI)

CTHETA_PHI.shape = (NPOS,2)

CTHETA_PHI is an array of NPOS positions, each position being defined by the couple of angles (CTR_THETA,CTR_PHI) expressed in deg.

The output is a (3,3,NPOS) array, representing the corresponding NPOS matrices.

Example:

CTHETA_PHI=np.array([

   [45.,0]

  ,[45.,90]

  ,[45.,180]

  ,[45.,270]

])

PMatr=prm(CTHETA_PHI)

print(PMatr.shape) 

will result in (3,3,4)

while PMatr[:,:,0] is the rotation matrix for the case (45.,0.) and PMatr[:,2,0] is the corresponding pointing vector.

A different ordering could be to have (NPOS,3,3) matrices, this can be obtained by using the np.moveaxis function

PMatrReorder=np.moveaxis(PMatr,2,-1)

Now the first matrix is PMatrReorder[0] and the corresponding pointing vector is PMatrReorder[0][2].

the option axis0isNPOS=True could be used 

In this case 

PMatrB=prm(CTHETA_PHI,axis0isNPOS=True)

print(PMatrB.shape) 

will result in (4,3,3)

while PMatrB[0] is the rotation matrix for the case (45.,0.) and PMatrB[:,2] is the corresponding pointing vector.

      """ 

      if not self.isComplete :

         raise Error('Configuration not completed or not committed','')

      #

      self._matr['CTRL_THETA']=self.MatrY(np.deg2rad(CTHETA_PHI[:,0]))

      self._matr['CTRL_PHI']=self.MatrZ((np.deg2rad(CTHETA_PHI[:,1]))

      #

      out=self.rmult(self._matr['ALT_ENC'],self.['M1'])

      out=self.rmult(self._matr['M2'],out)

      out=self.rmult(self._matr['AZ_ENC'],out)

      out=self.rmult(self._matr['M3'],out)

      #

      #stores Rout 

      self._matr['Rout']=out

      #

      #set output has NPOS,3,3 shape if required

      if axis0isNPOS==True :

         out=np.moveaxis(out,0,2)

      #

      return out

   #

   def MatrX(self,angle) :

      out=np.zeros([3,3,len(angle)])

      out[0,0]+=1

      c=np.cos(angle)

      s=np.sin(angle)

      out[1,1]=c

      out[1,2]=-s

      out[2,1]=s

      out[2,2]=c

   #

   def MatrY(self,angle) :

      out=np.zeros([3,3,len(angle)])

      c=np.cos(angle)

      s=np.sin(angle)

      out[1,1]+=1

      out[0,0]=c

      out[0,2]=s

      out[2,0]=-s

      out[2,2]=c

   #

   def MatrZ(self,angle) :

      out=np.zeros([3,3,len(angle)])

      c=np.cos(angle)

      s=np.sin(angle)

      out[2,2]+=1

      out[0,0]=c

      out[0,1]=-s

      out[1,0]=s

      out[1,1]=c

   #

   def rmult(self,this,that) :

      out=np.zeros([3,3,max(this.shape[2],that.shape[2])])

      for i in range(3) :

         for j in range(3) :

            for l in range(3) :

               out[i,j]+=this[i,l]*that[l,j]

      return out