From 82c4864849549707932efa9ae970277af39a379d Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Sat, 15 Feb 2020 02:05:45 +0000 Subject: [PATCH] Update --- new_main.py | 30 ++++++------------------------ 1 file changed, 6 insertions(+), 24 deletions(-) diff --git a/new_main.py b/new_main.py index 1d8e89f..0dbf0d1 100644 --- a/new_main.py +++ b/new_main.py @@ -161,47 +161,29 @@ def cleanup(): for obj in brick_ids: delete_model(obj) -cleanup() + # LET THE SHITSTORM BEGIN import numpy as np import time -rospy.init_node("THIS_IS_A_FUCKING_DISASTER") # Am I wrong?? - def etq(roll, pitch, yaw): 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] -POSSIBLES = [(0, 0, 0), (1.57, 0, 0), (0, 1.57, 0), (0, 0, 1.57), (1.57, 1.57, 0), (0, 1.57, 1.57), (1.57, 1.57, 1.57)] -brick_pose = Pose() -brick_pose.position.x = 0.485 -brick_pose.position.y = 0.709 -brick_pose.position.z = 0.818 -brick_reference_frame = 'world' -for x in POSSIBLES: - print(x) - QUATS = etq(*x) - brick_pose.orientation.x = QUATS[0] - brick_pose.orientation.y = QUATS[1] - brick_pose.orientation.z = QUATS[2] - brick_pose.orientation.w = QUATS[3] - brick_id = brick_ids.pop() - spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame) - time.sleep(3) -exit(0) def spawn_v_brick(): brick_pose = Pose() brick_pose.position.x = 0.485 brick_pose.position.y = 0.709 brick_pose.position.z = 0.818 - brick_pose.orientation.x = 0.5 - brick_pose.orientation.y = 0.5 - brick_pose.orientation.z = -0.5 - brick_pose.orientation.w = 0.5 + QUATS = etq(0, 1.57, 1.57) + brick_pose.orientation.x = QUATS[0] + brick_pose.orientation.y = QUATS[1] + brick_pose.orientation.z = QUATS[2] + brick_pose.orientation.w = QUATS[3] brick_reference_frame = 'world' brick_id = brick_ids.pop() spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)