max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-15 02:03:10 +00:00
parent 82c06a1a96
commit e5f4a68f2b
1 changed files with 27 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
new_main.py
View File

@ -162,35 +162,36 @@ def cleanup():
delete_model(obj) delete_model(obj)
#LET THE SHITSTORM BEGIN LET THE SHITSTORM BEGIN
# import numpy as np import numpy as np
# import time 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): 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) 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) 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) 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) 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] 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)] 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 = Pose()
# brick_pose.position.x = 0.485 brick_pose.position.x = 0.485
# brick_pose.position.y = 0.709 brick_pose.position.y = 0.709
# brick_pose.position.z = 0.818 brick_pose.position.z = 0.818
# brick_reference_frame = 'world' brick_reference_frame = 'world'
# for x in POSSIBLES: for x in POSSIBLES:
# QUATS = etq(*x) print(x)
# brick_pose.orientation.x = QUATS[0] QUATS = etq(*x)
# brick_pose.orientation.y = QUATS[1] brick_pose.orientation.x = QUATS[0]
# brick_pose.orientation.z = QUATS[2] brick_pose.orientation.y = QUATS[1]
# brick_pose.orientation.w = QUATS[3] brick_pose.orientation.z = QUATS[2]
# brick_id = brick_ids.pop() brick_pose.orientation.w = QUATS[3]
# spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame) brick_id = brick_ids.pop()
# time.sleep(1) spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
# exit(0) time.sleep(3)
exit(0)
def spawn_v_brick(): def spawn_v_brick():
brick_pose = Pose() brick_pose = Pose()