diff --git a/new_main.py b/new_main.py index afc3ed6..787aec8 100644 --- a/new_main.py +++ b/new_main.py @@ -163,44 +163,44 @@ def cleanup(): #LET THE SHITSTORM BEGIN -import numpy as np -import time +# import numpy as np +# import time -rospy.init_node("THIS_IS_A_FUCKING_DISASTER") # Am I wrong?? +# 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' +# 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: - 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(1) -exit(0) +# for x in POSSIBLES: +# 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(1) +# 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 - brick_pose.orientation.y = 0.707 - brick_pose.orientation.z = 0 - brick_pose.orientation.w = 0.707 + brick_pose.orientation.x = -0.5 + brick_pose.orientation.y = 0.5 + brick_pose.orientation.z = 0.5 + brick_pose.orientation.w = 0.5 brick_reference_frame = 'world' brick_id = brick_ids.pop() spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame) diff --git a/redundant/brick_data.py b/redundant/brick_data.py index cac62ae..017b76d 100644 --- a/redundant/brick_data.py +++ b/redundant/brick_data.py @@ -24,7 +24,6 @@ def etq(roll, pitch, yaw): def q_extrapolator(obj): return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w -print(etq(3.14, 1.57, 3.14)) print(etq(0, 1.57, 1.57)) exit(0)