From 0d0fb8fee845913807f88b24ac3a9af70872fd95 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Mon, 9 Mar 2020 15:53:43 +0000 Subject: [PATCH] Update --- new_main.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/new_main.py b/new_main.py index d7da738..81c9473 100755 --- a/new_main.py +++ b/new_main.py @@ -1,3 +1,4 @@ +clear #!/usr/bin/python import argparse import struct @@ -131,11 +132,11 @@ def cleanup(): def etq(roll, pitch, yaw): #Euler To Quaternian '''calculates Quaternian from euler angles''' - qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) - qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) - qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) - return [qx, qy, qz, qw] + qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + return [qx, qy, qz, qw] def spawn_v_brick(): '''Spawns Vertical brick in Gazebo simulation'''