roll pitch yaw to orientation altitude tool

I require a function to convert angles expressed in Roll, Pitch, Yaw to
O (orientation), a (altitude), t (tool). The oat notation is used by
the Puma and Kawasaki robots to express the orientation of the end
effector.

I have a tempted to write the code in python. Please can someone
validate my code. If any one can help, it does not matter what computer
language the code is written in please email me: andrew at vaporise dot
net.

  r = 50 # roll
  p = 60 # pitch
  y = 90 # yaw

  #generate empty 4x4 matrix
  inp = [[0,0,0,0], [0,0,0,0], [0,0,0,0],[0,0,0,0]]

  #Convert roll, pitch, roll to homogeneous transform matrix
  # Equation from
  # Robot Manipulators: Mathematics, Programming and Control
  # Richard P. Paul
  # page 47
  inp[0][0] = cos(r)*cos(p)
  inp[0][1] = cos(r)*sin(p)*sin(y) - sin(r)*cos(y)
  inp[0][2] = cos(r)*sin(p)*cos(y) + sin(r)*sin(y)
  inp[0][3] = 0

  inp[1][0] = sin(r)*cos(p)
  inp[1][1] = sin(r)*sin(p)*sin(y) + cos(r)*cos(y)
  inp[1][2] = sin(r)*sin(p)*cos(y) - cos(r)*sin(y)
  inp[1][3] = 0

  inp[2][0] = -sin(p)
  inp[2][1] = cos(p)*sin(y)
  inp[2][2] = cos(p)*cos(y)
  inp[2][3] = 0

  inp[3][0] = 0
  inp[3][1] = 0
  inp[3][2] = 0
  inp[3][3] = 1

  print inp

  #convert matrix to oat
  # Equations from
  # Robotics: Control, sensing, vision and intelligence.
  # K.S. Fu R.C. Gonzalez, C.S.G. Lee
  # page 60
  t=atan2(inp[2][1], -inp[2][0])
  ct=cos(t)
  st=sin(t)
  p = ((-inp[2][0] * ct) + (inp[1][2]*st))
  a = atan2(-inp[2][2],p)
  top = ((inp[1][0]*st) + (inp[1][1]*ct))
  bottom = ((inp[0][0]*st)+(inp[0][1]*ct))
  o=atan2(top,bottom)

  #convert radians to degrees
  #degrees = radians * 180 / pi

  o = o * 180 / pi
  a = a * 180 / pi
  t = t * 180 / pi

  print "o=",o
  print "a=",a
  print "t=",t

Andrew Seaford
andrew at vaporise dot net