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:22:42 +00:00
parent 1e32d0124a
commit f915e07bbe
2 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
new_main.py
View File

@ -133,10 +133,10 @@ class PickAndPlace(object):
self._servo_to_pose(pose) self._servo_to_pose(pose)
print('Ready to grip') print('Ready to grip')
# close gripper # close gripper
# //self.gripper_close() self.gripper_close()
print('grip') print('grip')
# retract to clear object # retract to clear object
# //self._retract() self._retract()
def place(self, pose): def place(self, pose):
# servo above pose # servo above pose
@ -163,20 +163,20 @@ def cleanup():
def spawn_brick(vertical=True): def spawn_brick(vertical=True):
brick_pose = Pose() brick_pose = Pose()
if vertical: if vertical:
brick_pose.position.x = 0.4788 brick_pose.position.x = 0.485
brick_pose.position.y = 0.7106 brick_pose.position.y = 0.709
brick_pose.position.z = xx 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
else: else:
brick_pose.position.x = 0.4664 brick_pose.position.x = 0.4664
brick_pose.position.y = 0.8069 brick_pose.position.y = 0.8069
brick_pose.position.z = 0.7533 brick_pose.position.z = 0.7533
brick_pose.orientation.x = 0 brick_pose.orientation.x = 0
brick_pose.orientation.y = 0.707 brick_pose.orientation.y = 0
brick_pose.orientation.z = 0 brick_pose.orientation.z = 0.707
brick_pose.orientation.w = 0.707 brick_pose.orientation.w = 0.707
brick_reference_frame = 'world' brick_reference_frame = 'world'
@ -194,10 +194,10 @@ hover_distance = 0.2
left_pnp = PickAndPlace('left', hover_distance) left_pnp = PickAndPlace('left', hover_distance)
# spawn_brick(vertical=True) spawn_brick(vertical=True)
left_pnp.pick(brickstuff[0]['pose']) left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[2]['pose']) left_pnp.place(brickstuff[2]['pose'])
# spawn_brick(vertical=True) spawn_brick(vertical=True)
# left_pnp.pick(brickstuff[0]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[3]['pose']) # left_pnp.place(brickstuff[3]['pose'])
# spawn_brick(vertical=True) # spawn_brick(vertical=True)

2
redundant/brick_data.py
View File

@ -24,7 +24,7 @@ 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(0, 1.5707, 0)) print(etq(-3.125462, 1.570733, 1.570733))
exit(0) exit(0)