From faa86604744181c5dd8b61943cd7c444c53414dd Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Sun, 23 Feb 2020 23:31:40 +0000 Subject: [PATCH] Update --- Vanilla | 1 + new_main.py | 245 ++++++++++++++++++++++++++++++++++------------- target_angles.py | 9 +- target_poses.py | 4 +- 4 files changed, 189 insertions(+), 70 deletions(-) create mode 160000 Vanilla diff --git a/Vanilla b/Vanilla new file mode 160000 index 0000000..14d2871 --- /dev/null +++ b/Vanilla @@ -0,0 +1 @@ +Subproject commit 14d2871975996001d10457b6bdc752f296e1415f diff --git a/new_main.py b/new_main.py index 469655c..befd2e8 100644 --- a/new_main.py +++ b/new_main.py @@ -58,6 +58,46 @@ class PickAndPlace(object): def send(self, angles): self._guarded_move_to_joint_position(angles) + def ik_request(self, pose): + hdr = Header(stamp=rospy.Time.now(), frame_id='base') + ikreq = SolvePositionIKRequest() + ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) + try: + resp = self._iksvc(ikreq) + except (rospy.ServiceException, rospy.ROSException), e: + rospy.logerr("Service call failed: %s" % (e,)) + return False + # Check if result valid, and type of seed ultimately used to get solution + # convert rospy's string representation of uint8[]'s to int's + resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type) + limb_joints = {} + if (resp_seeds[0] != resp.RESULT_INVALID): + seed_str = { + ikreq.SEED_USER: 'User Provided Seed', + ikreq.SEED_CURRENT: 'Current Joint Angles', + ikreq.SEED_NS_MAP: 'Nullspace Setpoints', + }.get(resp_seeds[0], 'None') + if self._verbose: + print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format( + (seed_str))) + # Format solution into Limb API-compatible dictionary + limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) + if self._verbose: + # print("IK Joint Solution:\n{0}".format(limb_joints)) + print("------------------") + else: + rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") + return False + print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') + print() + print() + print('Linb Joints:') + print(limb_joints) + print() + print() + print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') + return limb_joints + brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9'] @@ -169,82 +209,155 @@ def H_Routine(): -V_Routine() -left_pnp.send(ta.B_1_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_1_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_1_A) - -V_Routine() -left_pnp.send(ta.B_2_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_2_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_2_A) - -V_Routine() -left_pnp.send(ta.B_3_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_3_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_3_A) +startv = Pose() +startv.position.x = 0.474 +startv.position.y = 0.759 +startv.position.z = 0.234 +startv.orientation.x = -0.7071067811865476 +startv.orientation.y = -0.7071067811865475 +startv.orientation.z = 4.329780281177466e-17 +startv.orientation.w = 4.329780281177467e-17 -H_Routine() -left_pnp.send(ta.B_4_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_4_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_4_A) +print('-----START-V-----') +print(left_pnp.ik_request(startv)) +print() +print() -H_Routine() -left_pnp.send(ta.B_5_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_5_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_5_A) +starth = Pose() +starth.position.x = 0.464 +starth.position.y = 0.804 +starth.position.z = 0.126 +starth.orientation.x = 00.7071067811865476 +starth.orientation.y = 0.7071067811865475 +starth.orientation.z = 4.329780281177466e-17 +starth.orientation.w = 4.329780281177467e-17 + +print('-----START-H-----') +print(left_pnp.ik_request(starth)) +print() +print() + +brick1 = Pose() +brick1.position.x = 0.615 +brick1.position.y = 0.164 +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 + +print('-----BRICK-1-----') +print(left_pnp.ik_request(brick1)) +print() +print() -V_Routine() -left_pnp.send(ta.B_6_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_6_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_6_A) +brick3 = Pose() +brick3.position.x = 0.615 +brick3.position.y = 0.552 +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 -V_Routine() -left_pnp.send(ta.B_7_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_7_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_7_A) +print('-----BRICK-3-----') +print(left_pnp.ik_request(brick3)) +print() +print() -H_Routine() -left_pnp.send(ta.B_8_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_8_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_8_A) +brick8 = Pose() +brick8.position.x = 0.635 +brick8.position.y = 0.349 +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 + +print('-----BRICK-8-----') +print(left_pnp.ik_request(brick8)) +print() +print() + +# V_Routine() +# left_pnp.send(ta.B_1_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_1_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_1_A) + +# V_Routine() +# left_pnp.send(ta.B_2_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_2_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_2_A) + +# V_Routine() +# left_pnp.send(ta.B_3_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_3_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_3_A) -V_Routine() -left_pnp.send(ta.B_9_A) -x = raw_input('Continue?: ') -left_pnp.send(ta.B_9_P) -x = raw_input('Continue?: ') -left_pnp.gripper_open() -left_pnp.send(ta.B_9_A) +# H_Routine() +# left_pnp.send(ta.B_4_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_4_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_4_A) + +# H_Routine() +# left_pnp.send(ta.B_5_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_5_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_5_A) -left_pnp.send(ta.H_pickup) +# V_Routine() +# left_pnp.send(ta.B_6_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_6_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_6_A) + +# V_Routine() +# left_pnp.send(ta.B_7_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_7_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_7_A) + + +# H_Routine() +# left_pnp.send(ta.B_8_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_8_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_8_A) + + +# V_Routine() +# left_pnp.send(ta.B_9_A) +# x = raw_input('Continue?: ') +# left_pnp.send(ta.B_9_P) +# x = raw_input('Continue?: ') +# left_pnp.gripper_open() +# left_pnp.send(ta.B_9_A) + + +# left_pnp.send(ta.H_pickup) diff --git a/target_angles.py b/target_angles.py index 2e5ab5c..a994974 100644 --- a/target_angles.py +++ b/target_angles.py @@ -21,19 +21,23 @@ Place 8 Offset : {'left_w0': 0.47397216515126994, 'left_w1': 1.8191693493757988, Place 8 Down : {'left_w0': 0.47397216515126994, 'left_w1': 1.8191693493757988, 'left_w2': 1.3505030133400355, 'left_e0': -1.2629306563238563, 'left_e1': 1.135600354775232, 'left_s0': 0.09497817386676323, 'left_s1': -1.0891358446984596} Place 9 Offset : {'left_w0': 0.35959165932558207, 'left_w1': 1.9280499585601125, 'left_w2': 1.4753075963656614, 'left_e0': -1.014625396487737, 'left_e1': 0.9725959751301161, 'left_s0': -0.03418343328074433, 'left_s1': -1.1722171342836722} Place 9 Down : {'left_w0': 0.31592446450227823, 'left_w1': 1.7694573442805632, 'left_w2': 1.4526326487219683, 'left_e0': -1.0428825978748242, 'left_e1': 1.1732291351006587, 'left_s0': 0.05048765124424395, 'left_s1': -1.2037991885581427} -''' - +''' +# 1 cm up V_approach = {'left_w0': 0.309347832698974, 'left_w1': 1.881132793719985, 'left_w2': 2.3018260606950194, 'left_e0': -0.6620952326288202, 'left_e1': 0.8506124094054622, 'left_s0': 0.4467327362087081, 'left_s1': -1.0924293648582206} V_pickup = {'left_w0': 0.32998286773274293, 'left_w1': 1.3930185907441222, 'left_w2': 2.2171019702140753, 'left_e0': -0.6821342836475758, 'left_e1': 1.3311146610544962, 'left_s0': 0.5323550070725158, 'left_s1': -1.0404847669613069} +# 1 cm down H_approach = {'left_w0': 0.8139517749899786, 'left_w1': 1.7913332347783784, 'left_w2': 2.140253332470379, 'left_e0': -1.2881506550429311, 'left_e1': 0.9559160919794364, 'left_s0': 0.718405344706577, 'left_s1': -0.7389057972202023} H_pickup = {'left_w0': 0.9011295809085222, 'left_w1': 1.4021253667832847, 'left_w2': 2.0105076391011076, 'left_e0': -1.171667748823461, 'left_e1': 1.2991409722196496, 'left_s0': 0.7750446847107565, 'left_s1': -0.5757716624754514} +# 1 cm down B_1_A = {'left_w0': -0.24575885307317663, 'left_w1': 0.8749420307826046, 'left_w2': 1.6948949798814181, 'left_e0': 0.4583130030868797, 'left_e1': 1.8748037298799984, 'left_s0': -1.240546397484759, 'left_s1': -1.1229907414393936} B_1_P = {'left_w0': -0.44706325183614026, 'left_w1': 0.5308931382072065, 'left_w2': 1.8176589482381398, 'left_e0': 0.2909093427758618, 'left_e1': 1.8092679400066711, 'left_s0': -1.1225317078625037, 'left_s1': -0.7034153618455765} +# 3 cm to the left #^^Problematic B_2_A = {'left_w0': 1.0334701994763147, 'left_w1': 1.2668145176284678, 'left_w2': 0.9572318887458758, 'left_e0': -1.3409238316370244, 'left_e1': 1.7778524701894391, 'left_s0': 0.2271848446398137, 'left_s1': -0.573105088634528} B_2_P = {'left_w0': 1.2915396241568844, 'left_w1': 0.964866770561432, 'left_w2': 0.8596545556176473, 'left_e0': -0.991322423912265, 'left_e1': 1.7633745271360939, 'left_s0': 0.07709549283393327, 'left_s1': -0.33774783820639565} B_3_A = {'left_w0': 0.803725300735474, 'left_w1': 1.194163188977739, 'left_w2': 1.4667013915694584, 'left_e0': -1.0771478530987262, 'left_e1': 1.58481527249928, 'left_s0': 0.3549391390860414, 'left_s1': -0.7083016536375286} B_3_P = {'left_w0': 1.0094440753014968, 'left_w1': 0.8974942275422987, 'left_w2': 1.3801020782459945, 'left_e0': -0.8128594434709138, 'left_e1': 1.56853086901738, 'left_s0': 0.2186545601611162, 'left_s1': -0.4267459808731298} +# 2 cm to the right B_4_A = {'left_w0': 0.1671234856733194, 'left_w1': 0.8261944766180188, 'left_w2': 1.379487069256468, 'left_e0': -0.28247213805949767, 'left_e1': 1.8837252545953784, 'left_s0': -0.607713162458475, 'left_s1': -1.1163951963030918} B_4_P = {'left_w0': 0.24419474317602866, 'left_w1': 0.6290231193065527, 'left_w2': 1.326407292573995, 'left_e0': -0.23303382594081024, 'left_e1': 1.8771117428607849, 'left_s0': -0.6460371665357855, 'left_s1': -0.9078139028116797} B_5_A = {'left_w0': 0.6984403275545308, 'left_w1': 1.2544926841081905, 'left_w2': 1.3544614580142273, 'left_e0': -1.1745832201258934, 'left_e1': 1.6458210205020332, 'left_s0': 0.31526732584172107, 'left_s1': -0.8464672073407838} @@ -44,6 +48,7 @@ B_7_A = {'left_w0': 0.2880633344490283, 'left_w1': 1.586029171376809, 'left_w2': B_7_P = {'left_w0': 0.42723819133965163, 'left_w1': 1.1318459412158908, 'left_w2': 1.5190079397734162, 'left_e0': -0.8526717542593604, 'left_e1': 1.6855962489232676, 'left_s0': 0.13537792888171907, 'left_s1': -1.049425917711375} B_8_A = {'left_w0': 0.47397216515126994, 'left_w1': 1.8191693493757988, 'left_w2': 1.3505030133400355, 'left_e0': -1.2629306563238563, 'left_e1': 1.135600354775232, 'left_s0': 0.09497817386676323, 'left_s1': -1.0891358446984596} B_8_P = {'left_w0': 0.5436725589260862, 'left_w1': 1.3428680926888281, 'left_w2': 1.2439396903866538, 'left_e0': -1.172890274584358, 'left_e1': 1.5875808001304175, 'left_s0': 0.1289507547348684, 'left_s1': -0.992605606172027} +# 1 cm to the right #^^Problematic B_9_A = {'left_w0': 0.35959165932558207, 'left_w1': 1.9280499585601125, 'left_w2': 1.4753075963656614, 'left_e0': -1.014625396487737, 'left_e1': 0.9725959751301161, 'left_s0': -0.03418343328074433, 'left_s1': -1.1722171342836722} B_9_P = {'left_w0': 0.31592446450227823, 'left_w1': 1.7694573442805632, 'left_w2': 1.4526326487219683, 'left_e0': -1.0428825978748242, 'left_e1': 1.1732291351006587, 'left_s0': 0.05048765124424395, 'left_s1': -1.2037991885581427} diff --git a/target_poses.py b/target_poses.py index 5d121d1..bf5242e 100644 --- a/target_poses.py +++ b/target_poses.py @@ -28,7 +28,7 @@ from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion) startv = Pose() startv.position.x = 0.474 startv.position.y = 0.759 -startv.position.z = 0.224 +startv.position.z = 0.234 startv.orientation.x = -0.7071067811865476 startv.orientation.y = -0.7071067811865475 startv.orientation.z = 4.329780281177466e-17 @@ -37,7 +37,7 @@ startv.orientation.w = 4.329780281177467e-17 starth = Pose() starth.position.x = 0.464 starth.position.y = 0.804 -starth.position.z = 0.116 +starth.position.z = 0.126 starth.orientation.x = 00.7071067811865476 starth.orientation.y = 0.7071067811865475 starth.orientation.z = 4.329780281177466e-17