diff --git a/abc.py b/abc.py new file mode 100644 index 0000000..4e34ba7 --- /dev/null +++ b/abc.py @@ -0,0 +1,106 @@ +# pick up position +startv = Pose() +startv.position.x = 0.474 +startv.position.y = 0.708 +startv.position.z = 0.254 +startv.orientation.x = 0.646 +startv.orientation.y = 0.762 +startv.orientation.z = -0.003 +startv.orientation.w = 0.003 + +brick1 = Pose() +brick1.position.x = 0.615 +brick1.position.y = 0.194 +brick1.position.z = -0.131 +brick1.orientation.x: 0.656 +brick1.orientation.y: 0.753 +brick1.orientation.z: -0.013 +brick1.orientation.w: -0.015 + +brick2 = Pose() +brick2.position.x = 0.615 +brick2.position.y = 0.346 +brick2.position.z = -0.125 +brick2.orientation.x: 0.667 +brick2.orientation.y: 0.742 +brick2.orientation.z: -0.045 +brick2.orientation.w: 0.033 + +brick3 = Pose() +brick3.position.x = 0.615 +brick3.position.y = 0.532 +brick3.position.z = -0.122 +brick3.orientation.x: 0.689 +brick3.orientation.y: 0.724 +brick3.orientation.z: 0.008 +brick3.orientation.w: 0.014 + +##horizontal 1 +#pick up position +starth = Pose() +starth.position.x = 0.464 +starth.position.y = 0.804 +starth.position.z = 0.116 +starth.orientation.x: 0.661 +starth.orientation.y: 0.748 +starth.orientation.z: 0.043 +starth.orientation.w: -0.006 + +brick4 = Pose() +brick4.position.x = 0.615 +brick4.position.y = 0.240 +brick4.position.z = -0.0499 +brick4.orientation.x: 0.692 +brick4.orientation.y: 0.720 +brick4.orientation.z: -0.030 +brick4.orientation.w: -0.021 + + +brick5 = Pose() +brick5.position.x = 0.615 +brick5.position.y = 0.449 +brick5.position.z = -0.058 +brick5.orientation.x: 0.677 +brick5.orientation.y: 0.734 +brick5.orientation.z: -0.032 +brick5.orientation.w: -0.016 + +brick6 = Pose() +brick6.position.x = 0.615 +brick6.position.y = 0.247 +brick6.position.z = 0.123 +brick6.orientation.x: 0.656 +brick6.orientation.y: 0.753 +brick6.orientation.z: -0.040 +brick6.orientation.w: 0.000 + +brick7 = Pose() +brick7.position.x = 0.615 +brick7.position.y = 0.441 +brick7.position.z = 0.138 +brick7.orientation.x: 0.671 +brick7.orientation.y: 0.735 +brick7.orientation.z: -0.060 +brick7.orientation.w: 0.005 + +##horizontal 2 +brick8 = Pose() +brick8.position.x = 0.635 +brick8.position.y = 0.334 +brick8.position.z = 0.209 +brick8.orientation.x: 0.680 +brick8.orientation.y: 0.729 +brick8.orientation.z: -0.048 +brick8.orientation.w: -0.042 + +brick9 = Pose() +brick9.position.x = 0.635 +brick9.position.y = 0.349 +brick9.position.z = 0.406 +brick9.orientation.x: 0.674 +brick9.orientation.y: 0.737 +brick9.orientation.z: -0.008 +brick9.orientation.w: 0.016 + + + diff --git a/brick_data.py b/brick_data.py new file mode 100644 index 0000000..bc62940 --- /dev/null +++ b/brick_data.py @@ -0,0 +1,512 @@ +import math +import numpy as np + +def qte(x, y, z, w): + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(t0, t1) + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = math.asin(t2) + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(t3, t4) + return [math.degrees(roll), math.degrees(pitch), math.degrees(yaw)] + +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] + +def q_extrapolator(obj): + return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w + +##DELETE THIS +class Position(): + def __init__(self): + self.x = 0 + self.y = 0 + self.z = 0 + +class Orientation(): + def __init__(self): + self.x = 0 + self.y = 0 + self.z = 0 + self.w = 0 + +class EAngle(): + def __init__(self): + self.roll = 0 + self.pitch = 0 + self.yaw = 0 + +class Pose(): + def __init__(self): + self.position = Position() + self.orientation = Orientation() +##DELETE THIS + +# pick up position +startv = Pose() +startv.position.x = 0.474 +startv.position.y = 0.708 +startv.position.z = 0.254 +startv.orientation.x = 0.646 +startv.orientation.y = 0.762 +startv.orientation.z = -0.003 +startv.orientation.w = 0.003 + +starth = Pose() +starth.position.x = 0.464 +starth.position.y = 0.804 +starth.position.z = 0.116 +starth.orientation.x = 0.661 +starth.orientation.y = 0.748 +starth.orientation.z = 0.043 +starth.orientation.w = -0.006 + +brick1 = Pose() +brick1.position.x = 0.615 +brick1.position.y = 0.194 +brick1.position.z = -0.131 +brick1.orientation.x = 0.656 +brick1.orientation.y = 0.753 +brick1.orientation.z = -0.013 +brick1.orientation.w = -0.015 + +brick2 = Pose() +brick2.position.x = 0.615 +brick2.position.y = 0.346 +brick2.position.z = -0.125 +brick2.orientation.x = 0.667 +brick2.orientation.y = 0.742 +brick2.orientation.z = -0.045 +brick2.orientation.w = 0.033 + +brick3 = Pose() +brick3.position.x = 0.615 +brick3.position.y = 0.532 +brick3.position.z = -0.122 +brick3.orientation.x = 0.689 +brick3.orientation.y = 0.724 +brick3.orientation.z = 0.008 +brick3.orientation.w = 0.014 + +brick4 = Pose() +brick4.position.x = 0.615 +brick4.position.y = 0.240 +brick4.position.z = -0.0499 +brick4.orientation.x = 0.692 +brick4.orientation.y = 0.720 +brick4.orientation.z = -0.030 +brick4.orientation.w = -0.021 + +brick5 = Pose() +brick5.position.x = 0.615 +brick5.position.y = 0.449 +brick5.position.z = -0.058 +brick5.orientation.x = 0.677 +brick5.orientation.y = 0.734 +brick5.orientation.z = -0.032 +brick5.orientation.w = -0.016 + +brick6 = Pose() +brick6.position.x = 0.615 +brick6.position.y = 0.247 +brick6.position.z = 0.123 +brick6.orientation.x = 0.656 +brick6.orientation.y = 0.753 +brick6.orientation.z = -0.040 +brick6.orientation.w = 0.000 + +brick7 = Pose() +brick7.position.x = 0.615 +brick7.position.y = 0.441 +brick7.position.z = 0.138 +brick7.orientation.x = 0.671 +brick7.orientation.y = 0.735 +brick7.orientation.z = -0.060 +brick7.orientation.w = 0.005 + +brick8 = Pose() +brick8.position.x = 0.635 +brick8.position.y = 0.334 +brick8.position.z = 0.209 +brick8.orientation.x = 0.680 +brick8.orientation.y = 0.729 +brick8.orientation.z = -0.048 +brick8.orientation.w = -0.042 + +brick9 = Pose() +brick9.position.x = 0.635 +brick9.position.y = 0.349 +brick9.position.z = 0.406 +brick9.orientation.x = 0.674 +brick9.orientation.y = 0.737 +brick9.orientation.z = -0.008 +brick9.orientation.w = 0.016 + + + + +brick_directions_notf = [ + { + 'id':'tv', + 'pose':startv, + 'isVertical':True + }, + {'id':'th', + 'pose':starth, + 'isVertical':False + }, + {'id':'b1', + 'pose':brick1, + 'isVertical':True + }, + {'id':'b2', + 'pose':brick2, + 'isVertical':True + }, + {'id':'b3', + 'pose':brick3, + 'isVertical':True + }, + {'id':'b4', + 'pose':brick4, + 'isVertical':False + }, + {'id':'b5', + 'pose':brick5, + 'isVertical':False + }, + {'id':'b6', + 'pose':brick6, + 'isVertical':True + }, + {'id':'b7', + 'pose':brick7, + 'isVertical':True + }, + {'id':'b8', + 'pose':brick8, + 'isVertical':False + }, + {'id':'b9', + 'pose':brick9, + 'isVertical':True + }] + + +for item in brick_directions_notf: + angles = qte(*q_extrapolator(item['pose'])) + clean_angles = [] + for one in angles: + if one > -185 and one < -172: + clean_angles.append(-180) + elif one > 175 and one < 185: + clean_angles.append(180) + elif one < 7 and one > -5: + clean_angles.append(0) + elif one > 85 and one < 100: + clean_angles.append(90) + else: + clean_angles.append(one) + # print('Brick: ', item['id'], ' (Vertical:', item['isVertical'], ')') + # print('Initial: ', angles) + # print('Clean: ', clean_angles) + for ix in range (0, 3): + clean_angles[ix] = math.radians(clean_angles[ix]) + quats = etq(clean_angles[0], clean_angles[1], clean_angles[2]) + # print(q_extrapolator(item['pose'])) + # print(quats) + # print() + # print('Begin data') + print('---') + print(item['id']) + print('x_:_ ', quats[0]) + print('y_:_ ', quats[1]) + print('z_:_ ', quats[2]) + print('w_:_ ', quats[3]) + print('---') + print() + + +# CORRECTED FOR ERRORS +# --- +# tv +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# th +# x_:_ 0.7071067811865476 +# y_:_ 0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b1 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b2 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b3 +# x_:_ 0.7071067811865476 +# y_:_ 0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b4 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b5 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b6 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b7 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b8 +# x_:_ -0.7071067811865476 +# y_:_ -0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + +# --- +# b9 +# x_:_ 0.7071067811865476 +# y_:_ 0.7071067811865475 +# z_:_ 4.329780281177466e-17 +# w_:_ 4.329780281177467e-17 +# --- + + + + + +# Brick: tv (Vertical: True ) +# Initial: [-179.959958775756, 0.48404050300017853, 99.30511432490599] +# Clean: [-180, 0, 90] +# --- + +# Brick: th (Vertical: False ) +# Initial: [176.7489708415661, -3.774051353934406, 97.07726408520018] +# Clean: [-180, 0, 90] +# --- + +# Brick: b1 (Vertical: True ) +# Initial: [-177.73984770155016, -0.31707646225879454, 97.74150389024307] +# Clean: [-180, 0, 90] +# --- + +# Brick: b2 (Vertical: True ) +# Initial: [-178.68432711359827, 6.2577884502559975, 96.08354223678543] +# Clean: [-180, 0, 90] +# --- + +# Brick: b3 (Vertical: True ) +# Initial: [178.22758990527294, 0.5298789221469992, 92.78136914285481] +# Clean: [-180, 0, 90] +# --- + +# Brick: b4 (Vertical: False ) +# Initial: [-175.84410045162116, 0.6463100993122008, 92.21552176954216] +# Clean: [-180, 0, 90] +# --- + +# Brick: b5 (Vertical: False ) +# Initial: [-176.05042286692418, 1.1368228542886731, 94.57227316004405] +# Clean: [-180, 0, 90] +# --- + +# Brick: b6 (Vertical: True ) +# Initial: [-176.53430985789132, 3.0082644567182153, 97.90742822617871] +# Clean: [-180, 0, 90] +# --- + +# Brick: b7 (Vertical: True ) +# Initial: [-175.25111166494776, 5.0410815408391505, 95.08110720439343] +# Clean: [-180, 0, 90] +# --- + +# Brick: b8 (Vertical: False ) +# Initial: [-172.6669557315482, 0.23170476390131117, 93.87854586635889] +# Clean: [-180, 0, 90] +# --- + +# Brick: b9 (Vertical: True ) +# Initial: [179.43701764607906, 1.9695292014695087, 94.97541131205563] +# Clean: [-180, 0, 90] +# --- + +# startv = Pose() +# startv.position.x = 0.474118380271 +# startv.position.y = 0.708592081278 +# startv.position.z = 0.254463995665 +# startv.orientation.x = 0.646837059517 +# startv.orientation.y = 0.762613332443 +# startv.orientation.z = -0.00349921264719 +# startv.orientation.w = 0.00323714782189 + +# ## layer 1 +# # brick 1 +# brick1 = Pose() +# brick1.position.x = 0.614432692238 +# brick1.position.y = 0.194920200078 +# brick1.position.z = -0.131955624179 +# brick1.orientation.x = 0.656912926072 +# brick1.orientation.y = 0.753670267934 +# brick1.orientation.z = -0.0139518496021 +# brick1.orientation.w = -0.0158707493391 + +# # brick 2 +# brick2 = Pose() +# brick2.position.x = 0.618107604964 +# brick2.position.y = 0.346427058986 +# brick2.position.z = -0.12642347212 +# brick2.orientation.x = 0.667702789864 +# brick2.orientation.y = 0.742295147084 +# brick2.orientation.z = -0.0450738163396 +# brick2.orientation.w = 0.0337527792232 + +# # brick 3 +# brick3 = Pose() +# brick3.position.x = 0.615060266097 +# brick3.position.y = 0.532057264536 +# brick3.position.z = -0.122815510307 +# brick3.orientation.x = 0.689053473867 +# brick3.orientation.y = 0.724504622211 +# brick3.orientation.z = 0.00896708942344 +# brick3.orientation.w = 0.0147632602552 + +# ##horizontal 1 +# #pick up position +# starth = Pose() +# starth.position.x = 0.464710515625 +# starth.position.y = 0.804733475267 +# starth.position.z = 0.116929175434 +# starth.orientation.x = 0.661218573375 +# starth.orientation.y = 0.748928202611 +# starth.orientation.z = 0.0430656607147 +# starth.orientation.w = -0.00647259034557 + +# #brick 4 +# brick4 = Pose() +# brick4.position.x = 0.622954841097 +# brick4.position.y = 0.240541825386 +# brick4.position.z = -0.0499969605648 +# brick4.orientation.x = 0.692591952198 +# brick4.orientation.y = 0.720363275918 +# brick4.orientation.z = -0.0303026653131 +# brick4.orientation.w = -0.0217919006618 + + +# #brick 5 +# brick5 = Pose() +# brick5.position.x = 0.60878876974 +# brick5.position.y = 0.449420093325 +# brick5.position.z = -0.0582220466391 +# brick5.orientation.x = 0.677785576319 +# brick5.orientation.y = 0.734386540736 +# brick5.orientation.z = -0.0320204279271 +# brick5.orientation.w = -0.0160565723407 + +# ##layer 2 +# #brick 6 +# brick6 = Pose() +# brick6.position.x = 0.654521558648 +# brick6.position.y = 0.247991453631 +# brick6.position.z = 0.123275208559 +# brick6.orientation.x = 0.656490490121 +# brick6.orientation.y = 0.753242934575 +# brick6.orientation.z = -0.0405574649753 +# brick6.orientation.w = 0.00064025573316 + +# #brick 7 +# brick7 = Pose() +# brick7.position.x = 0.621248794776 +# brick7.position.y = 0.441610520059 +# brick7.position.z = 0.138989144559 +# brick7.orientation.x = 0.671416445783 +# brick7.orientation.y = 0.73860918343 +# brick7.orientation.z = -0.0602439761379 +# .orientation.w = 0.00520517280538 + +# ##horizontal 2 +# #brick 8 +# brick8 = Pose() +# brick8.position.x = 0.637564580857 +# brick8.position.y = 0.334499476763 +# brick8.position.z = 0.209965609695 +# brick8.orientation.x = 0.680984543038 +# brick8.orientation.y = 0.729464268389 +# brick8.orientation.z = -0.0481331171548 +# brick8.orientation.w = -0.0427216142147 + +# ##layer 3 +# #brick 9 +# brick9 = Pose() +# brick9.position.x = 0.635704106485 +# brick9.position.y = 0.349136096976 +# brick9.position.z = 0.406618422176 +# brick9.orientation.x = 0.674781286383 +# brick9.orientation.y = 0.737779159737 +# brick9.orientation.z = -0.00827498306978 +# brick9.orientation.w = 0.016841961284 + + + + + + + + + + + + + diff --git a/target_poses.py b/target_poses.py new file mode 100644 index 0000000..e948d85 --- /dev/null +++ b/target_poses.py @@ -0,0 +1,185 @@ + + +startv = Pose() +startv.position.x = 0.474 +startv.position.y = 0.708 +startv.position.z = 0.254 +startv.orientation.x = -0.7071067811865476 +startv.orientation.y = -0.7071067811865475 +startv.orientation.z = 4.329780281177466e-17 +startv.orientation.w = 4.329780281177467e-17 + +starth = Pose() +starth.position.x = 0.464 +starth.position.y = 0.804 +starth.position.z = 0.116 +starth.orientation.x = 0.7071067811865476 +starth.orientation.y = 0.7071067811865475 +starth.orientation.z = 4.329780281177466e-17 +starth.orientation.w = 4.329780281177467e-17 + +brick1 = Pose() +brick1.position.x = 0.615 +brick1.position.y = 0.194 +brick1.position.z = -0.131 +brick1.orientation.x = -0.7071067811865476 +brick1.orientation.y = -0.7071067811865475 +brick1.orientation.z = 4.329780281177466e-17 +brick1.orientation.w = 4.329780281177467e-17 + +brick2 = Pose() +brick2.position.x = 0.615 +brick2.position.y = 0.346 +brick2.position.z = -0.125 +brick2.orientation.x = -0.7071067811865476 +brick2.orientation.y = -0.7071067811865475 +brick2.orientation.z = 4.329780281177466e-17 +brick2.orientation.w = 4.329780281177467e-17 + +brick3 = Pose() +brick3.position.x = 0.615 +brick3.position.y = 0.532 +brick3.position.z = -0.122 +brick3.orientation.x = 0.7071067811865476 +brick3.orientation.y = 0.7071067811865475 +brick3.orientation.z = 4.329780281177466e-17 +brick3.orientation.w = 4.329780281177467e-17 + +brick4 = Pose() +brick4.position.x = 0.615 +brick4.position.y = 0.240 +brick4.position.z = -0.0499 +brick4.orientation.x = -0.7071067811865476 +brick4.orientation.y = -0.7071067811865475 +brick4.orientation.z = 4.329780281177466e-17 +brick4.orientation.w = 4.329780281177467e-17 + +brick5 = Pose() +brick5.position.x = 0.615 +brick5.position.y = 0.449 +brick5.position.z = -0.058 +brick5.orientation.x = -0.7071067811865476 +brick5.orientation.y = -0.7071067811865475 +brick5.orientation.z = 4.329780281177466e-17 +brick5.orientation.w = 4.329780281177467e-17 + +brick6 = Pose() +brick6.position.x = 0.615 +brick6.position.y = 0.247 +brick6.position.z = 0.123 +brick6.orientation.x = -0.7071067811865476 +brick6.orientation.y = -0.7071067811865475 +brick6.orientation.z = 4.329780281177466e-17 +brick6.orientation.w = 4.329780281177467e-17 + +brick7 = Pose() +brick7.position.x = 0.615 +brick7.position.y = 0.441 +brick7.position.z = 0.138 +brick7.orientation.x = -0.7071067811865476 +brick7.orientation.y = -0.7071067811865475 +brick7.orientation.z = 4.329780281177466e-17 +brick7.orientation.w = 4.329780281177467e-17 + +brick8 = Pose() +brick8.position.x = 0.635 +brick8.position.y = 0.334 +brick8.position.z = 0.209 +brick8.orientation.x = -0.7071067811865476 +brick8.orientation.y = -0.7071067811865475 +brick8.orientation.z = 4.329780281177466e-17 +brick8.orientation.w = 4.329780281177467e-17 + +brick9 = Pose() +brick9.position.x = 0.635 +brick9.position.y = 0.349 +brick9.position.z = 0.406 +brick9.orientation.x = 0.7071067811865476 +brick9.orientation.y = 0.7071067811865475 +brick9.orientation.z = 4.329780281177466e-17 +brick9.orientation.w = 4.329780281177467e-17 + + +brick_directions_notf = [ + { + 'id':'tv', + 'pose':startv, + 'isVertical':True + }, + {'id':'th', + 'pose':starth, + 'isVertical':False + }, + {'id':'b1', + 'pose':brick1, + 'isVertical':True + }, + {'id':'b2', + 'pose':brick2, + 'isVertical':True + }, + {'id':'b3', + 'pose':brick3, + 'isVertical':True + }, + {'id':'b4', + 'pose':brick4, + 'isVertical':False + }, + {'id':'b5', + 'pose':brick5, + 'isVertical':False + }, + {'id':'b6', + 'pose':brick6, + 'isVertical':True + }, + {'id':'b7', + 'pose':brick7, + 'isVertical':True + }, + {'id':'b8', + 'pose':brick8, + 'isVertical':False + }, + {'id':'b9', + 'pose':brick9, + 'isVertical':True + }] + +bdf = brick_directions_notf + +tower_instructions = [ bdf[0], + bdf[2], + bdf[0], + bdf[3], + bdf[0], + bdf[4], + bdf[1], + bdf[5], + bdf[1], + bdf[6], + bdf[0], + bdf[7], + bdf[0], + bdf[8], + bdf[1], + bdf[9], + bdf[0], + bdf[10], + ] + + + + + + + + + + + + + + +