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 01:53:14 +00:00
parent e8802da8f5
commit 08e49551cf
2 changed files with 29 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
new_main.py
View File

@ -163,44 +163,44 @@ def cleanup():
#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) # QUATS = etq(*x)
brick_pose.orientation.x = QUATS[0] # brick_pose.orientation.x = QUATS[0]
brick_pose.orientation.y = QUATS[1] # brick_pose.orientation.y = QUATS[1]
brick_pose.orientation.z = QUATS[2] # brick_pose.orientation.z = QUATS[2]
brick_pose.orientation.w = QUATS[3] # brick_pose.orientation.w = QUATS[3]
brick_id = brick_ids.pop() # brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame) # spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
time.sleep(1) # time.sleep(1)
exit(0) # exit(0)
def spawn_v_brick(): def spawn_v_brick():
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_pose.orientation.x = 0 brick_pose.orientation.x = -0.5
brick_pose.orientation.y = 0.707 brick_pose.orientation.y = 0.5
brick_pose.orientation.z = 0 brick_pose.orientation.z = 0.5
brick_pose.orientation.w = 0.707 brick_pose.orientation.w = 0.5
brick_reference_frame = 'world' brick_reference_frame = 'world'
brick_id = brick_ids.pop() brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame) spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)

1
redundant/brick_data.py
View File

@ -24,7 +24,6 @@ def etq(roll, pitch, yaw):
def q_extrapolator(obj): def q_extrapolator(obj):
return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w 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)) print(etq(0, 1.57, 1.57))
exit(0) exit(0)