I GIVE UP

new_main.py

@@ -148,6 +148,7 @@ class PickAndPlace(object):
         # retract to clear object
         self._retract()
 
+
 brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
 
 with open ("brick/model.sdf", "r") as brick_file:brick_sdf=brick_file.read().replace('\n', '')
@@ -160,6 +161,37 @@ def cleanup():
     for obj in brick_ids:
         delete_model(obj)
 
+
+#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:
+	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

redundant/brick_data.py

@@ -24,7 +24,8 @@ def etq(roll, pitch, yaw):
 def q_extrapolator(obj):
 	return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w
 
-print(etq(1.570733, 1.570733, 1.570733))
+print(etq(3.14, 1.57, 3.14))
+print(etq(0, 1.57, 1.57))
 
 exit(0)