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-19 00:29:07 +00:00
parent 53c00e1cc1
commit 92c656665f
2 changed files with 34 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
new_main.py
View File

@ -77,6 +77,14 @@ class PickAndPlace(object):
else: else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False return False
print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
print()
print()
print('Linb Joints:')
print(limb_joints)
print()
print()
print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
return limb_joints return limb_joints
def _guarded_move_to_joint_position(self, joint_angles): def _guarded_move_to_joint_position(self, joint_angles):
@ -99,10 +107,6 @@ class PickAndPlace(object):
# approach with a pose the hover-distance above the requested pose # approach with a pose the hover-distance above the requested pose
approach.position.z = approach.position.z + self._hover_distance approach.position.z = approach.position.z + self._hover_distance
joint_angles = self.ik_request(approach) joint_angles = self.ik_request(approach)
print('---------')
print('IK Solution:')
print(joint_angles)
print('---------')
self._guarded_move_to_joint_position(joint_angles) self._guarded_move_to_joint_position(joint_angles)
def _retract(self): def _retract(self):
@ -131,6 +135,7 @@ class PickAndPlace(object):
# servo above pose # servo above pose
print('Approaching') print('Approaching')
self._approach(pose) self._approach(pose)
return
# servo to pose # servo to pose
self._servo_to_pose(pose) self._servo_to_pose(pose)
print('Ready to grip') print('Ready to grip')
@ -143,6 +148,7 @@ class PickAndPlace(object):
def place(self, pose): def place(self, pose):
# servo above pose # servo above pose
self._approach(pose) self._approach(pose)
return
# servo to pose # servo to pose
self._servo_to_pose(pose) self._servo_to_pose(pose)
# open the gripper # open the gripper
@ -177,7 +183,6 @@ def cleanup():
for obj in brick_ids: for obj in brick_ids:
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
@ -189,7 +194,6 @@ def etq(roll, pitch, yaw):
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]
def spawn_v_brick(): def spawn_v_brick():
brick_pose = Pose() brick_pose = Pose()
brick_pose.position.x = 0.475 brick_pose.position.x = 0.475

24
target_angles.py Normal file
View File

@ -0,0 +1,24 @@
'''
Pickup Vertical Offset :
Pickup Vertical Pick :
Pickup Horizontal Offset :
Pickup Horizontal Pick :
Place 1 Offset :
Place 1 Down :
Place 2 Offset :
Place 2 Down :
Place 3 Offset :
Place 3 Down :
Place 4 Offset :
Place 4 Down :
Place 5 Offset :
Place 5 Down :
Place 6 Offset :
Place 6 Down :
Place 7 Offset :
Place 7 Down :
Place 8 Offset :
Place 8 Down :
Place 9 Offset :
Place 9 Down :
'''