diff --git a/new_main.py b/new_main.py index 914c084..afc3ed6 100644 --- a/new_main.py +++ b/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 diff --git a/redundant/brick_data.py b/redundant/brick_data.py index b2116aa..cac62ae 100644 --- a/redundant/brick_data.py +++ b/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)