CODEWITHSUNDEEP
pythonmatheuler-anglesvpython
zidik
zidik

Reputation: 136

Display box in VPython knowing roll pitch and yaw

I am trying to visualise a box in VPython The problem is: I know it's roll pitch and yaw, but Vpython's box has attributes "axis" and "up". How could I convert my angles to those two needed vectors?

Here is a short code that shows 3 axes and one box. Function setOrientation should change the boxes attributes "up" and "axis" provided roll, pitch and yaw.

import vis

def setOrientation(element, roll, pitch, yaw):    
    element.axis = ???
    element.up = ???


vis.display(
    title='Board orientation',
    x=0, y=200,
    width=600, height=600,
    center=(0, 0, 0),
    forward=(1, 0.4, 1),
    up = (0,0,-1),
    lights =[
        vis.distant_light(direction=(0.22, 0.44, -0.88), color=vis.color.gray(0.8)),
        vis.distant_light(direction=(-0.88, -0.22, 0.44), color=vis.color.gray(0.3))], 
range = 5
)

# Draw all axes
startingpoint = vis.sphere(pos=vis.vector(0, 0, 0), radius=0.2, color=vis.color.yellow)
vis.arrow(pos=startingpoint.pos, axis=vis.vector(3, 0, 0), shaftwidth=0.1, color=vis.color.red)
vis.arrow(pos=startingpoint.pos, axis=vis.vector(0, 3, 0), shaftwidth=0.1, color=vis.color.green)
vis.arrow(pos=startingpoint.pos, axis=vis.vector(0, 0, 3), shaftwidth=0.1, color=vis.color.blue)

#Make a box
mybox = vis.box(pos=(0,0,0), length=6, height=2, width=0.1, color=vis.color.red)
#Orient it by proviging roll, pitch and yaw
setOrientation(mybox, 0, 0, 0)

The axes and directions should match the ones when describing orientation of an aircraft

X - points forward

Y - points to the right

Z - points down

roll - positive direction is clockwise

pitch - positive is up

yaw - positive is clockwise

the closest thing i have found is code from Mike Smorto

axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) 
up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw),-cos(roll)*cos(pitch))

The problem with this solution is that it's axes don't match with my problem and I am unable to modify it to suit my needs.

Upvotes: 2

Views: 2424

Answers (2)

PenGoFox Liang
PenGoFox Liang

Reputation: 1

now we import vpython rather than import vis library.

And now the axis is same as the x axis, up axis is same as y axis.

Euler angle (that is, the roll, pitch, and yaw you are using now) can be convert to a rotate matrix, and my method is use rotate matrix to assign the vector to up and axis.

So first you need to do the convertion of euler angle to rotate matrix. You can google this and find the convert method.

Now, all you need to do is split a rotate matrix to x, y and z vector, and assign y to up, and x to axis.

Say, you have a rotate matrix (I don't know how to use a latex matrix in stackoverflow, so you see this):

    r11 r12 r13
R = r21 r22 r23
    r31 r32 r33

and x vector would be a column vector of the first column:

    r11
x = r21
    r31

and y vector would be the second column, z vector would be the third column:

    r12      r13
y = r22, z = r23
    r32      r33

After that, you just neet to do this in your python code:

element.axis = x
element.up = y

(sorry about my bad english)

Upvotes: 0

dloman
dloman

Reputation: 142

You are doing a rotation from a body quardinate system to a global coordinatye system. You can use a quaternion.

You can create a quaternion from roll pitch and yaw with this. 

def QuaternionFromEuler(Roll, Pitch, Yaw):
    cRoll = math.cos(Roll/2)
    cPitch = math.cos(Pitch/2)
    cYaw = math.cos(Yaw/2)
    sRoll = math.sin(Roll/2)
    sPitch = math.sin(Pitch/2)
    sYaw = math.sin(Yaw/2)
    Quaternion = numpy.empty(4)
    Quaternion[0] = cRoll*cPitch*cYaw + sYaw*sPitch*sYaw
    Quaternion[1] = sRoll*cPitch*cYaw - cRoll*sPitch*sYaw
    Quaternion[2] = cRoll*sPitch*cYaw + cYaw*cPitch*sYaw
    Quaternion[3] = cRoll*cPitch*sYaw - sRoll*sPitch*cYaw
    return Quaternion

Upvotes: 0

Related Questions