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
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
snipped-for-privacy@hotmail.com wrote:

looks reasonable to me. haven't checked the exact equations, but it looks like you are loading the 4X4 matrix correctly. does python represent matrices as [row][column]? what values of o,a,t do you get?
Mike Ross
--



Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
snipped-for-privacy@hotmail.com wrote:

But in what order are roll, pitch, and yaw applied, and are they relative or absolute? There are 24 different representations possible. Here's C code to support all 24:
http://www.acm.org/pubs/tog/GraphicsGems/gemsiv/euler_angle
                    John Nagle
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Polytechforum.com is a website by engineers for engineers. It is not affiliated with any of manufacturers or vendors discussed here. All logos and trade names are the property of their respective owners.