diff --git a/new_main.py b/new_main.py index 52f0d15..d86f4c7 100644 --- a/new_main.py +++ b/new_main.py @@ -18,6 +18,8 @@ import target_poses as tps import tuck_arms +import target_angles as ta + brickstuff = tps.brick_directions_notf class PickAndPlace(object): @@ -38,55 +40,6 @@ class PickAndPlace(object): print("Enabling robot... ") self._rs.enable() - def move_to_start(self, start_angles=None): - print("Moving the {0} arm to start pose...".format(self._limb_name)) - if not start_angles: - start_angles = dict(zip(self._joint_names, [0]*7)) - self._guarded_move_to_joint_position(start_angles) - self.gripper_open() - rospy.sleep(0.2) - print("Running. Ctrl-c to quit") - - 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 - def _guarded_move_to_joint_position(self, joint_angles): if joint_angles: self._limb.set_joint_position_speed(0.1) @@ -102,72 +55,6 @@ class PickAndPlace(object): self._gripper.close() rospy.sleep(0.2) - def _approach(self, pose): - approach = copy.deepcopy(pose) - # approach with a pose the hover-distance above the requested pose - approach.position.z = approach.position.z + 0.05# self._hover_distance - joint_angles = self.ik_request(approach) - self._guarded_move_to_joint_position(joint_angles) - - def _retract(self): - # retrieve current pose from endpoint - current_pose = self._limb.endpoint_pose() - ik_pose = Pose() - ik_pose.position.x = current_pose['position'].x - ik_pose.position.y = current_pose['position'].y - ik_pose.position.z = current_pose['position'].z + self._hover_distance - ik_pose.orientation.x = current_pose['orientation'].x - ik_pose.orientation.y = current_pose['orientation'].y - ik_pose.orientation.z = current_pose['orientation'].z - ik_pose.orientation.w = current_pose['orientation'].w - joint_angles = self.ik_request(ik_pose) - # servo up from current pose - self._guarded_move_to_joint_position(joint_angles) - - def _servo_to_pose(self, pose): - # servo down to release - joint_angles = self.ik_request(pose) - self._guarded_move_to_joint_position(joint_angles) - - def pick(self, pose): - # open the gripper - self.gripper_open() - # servo above pose - print('Approaching') - self._approach(pose) - # servo to pose - self._servo_to_pose(pose) - print('Ready to grip') - # close gripper - self.gripper_close() - print('grip') - # retract to clear object - self._retract() - - def place(self, pose): - # servo above pose - self._approach(pose) - # servo to pose - self._servo_to_pose(pose) - # open the gripper - self.gripper_open() - # retract to clear object - self._retract() - - def cust_place(self, pose): - # servo above pose - approach = copy.deepcopy(pose) - # approach with a pose the hover-distance above the requested pose - approach.position.z = approach.position.z + 0.1 - joint_angles = self.ik_request(approach) - self._guarded_move_to_joint_position(joint_angles) - # servo to pose - self._servo_to_pose(pose) - # open the gripper - self.gripper_open() - # retract to clear object - self._retract() - def send(self, angles): self._guarded_move_to_joint_position(angles) @@ -234,61 +121,85 @@ left_pnp = PickAndPlace('left', hover_distance) left_pnp.gripper_open() -## TODO: MAKE hover_distance higher for place part or solve IK collision error +def V_Routine(): + + spawn_v_brick() + + left_pnp.send(ta.V_approach) + x = raw_input('Ready?') + left_pnp.send(ta.V_pickup) + left_pnp.gripper_close() + left_pnp.send(ta.V_approach) + + + +def H_Routine(): + + spawn_h_brick() + + left_pnp.send(ta.H_approach) + x = raw_input('Ready?') + left_pnp.send(ta.H_pickup) + left_pnp.gripper_close() + left_pnp.send(ta.H_approach) + + + +V_Routine() +left_pnp.send(ta.B_1_A) +left_pnp.send(ta.B_1_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_1_A) + +V_Routine() +left_pnp.send(ta.B_2_A) +left_pnp.send(ta.B_2_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_2_A) + +V_Routine() +left_pnp.send(ta.B_3_A) +left_pnp.send(ta.B_3_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_3_A) + + +H_Routine() +left_pnp.send(ta.B_4_A) +left_pnp.send(ta.B_4_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_4_A) + +H_Routine() +left_pnp.send(ta.B_5_A) +left_pnp.send(ta.B_5_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_5_A) + + +V_Routine() +left_pnp.send(ta.B_6_A) +left_pnp.send(ta.B_6_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_6_A) + +V_Routine() +left_pnp.send(ta.B_7_A) +left_pnp.send(ta.B_7_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_7_A) + + +H_Routine() +left_pnp.send(ta.B_8_A) +left_pnp.send(ta.B_8_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_8_A) + + +V_Routine() +left_pnp.send(ta.B_9_A) +left_pnp.send(ta.B_9_P) +left_pnp.gripper_open() +left_pnp.send(ta.B_9_A) -# spawn_v_brick() -# left_pnp.pick(brickstuff[0]['pose']) -# left_pnp.place(brickstuff[2]['pose']) -# exit(0) -# tuck_arms.init_arms() -# paused = raw_input('Continue?') -# spawn_v_brick() -# left_pnp.pick(brickstuff[0]['pose']) -# print('\n \n-----------------------\n \n') -# left_pnp.place(brickstuff[3]['pose']) -# exit(1) -# tuck_arms.init_arms() -# paused = raw_input('Continue?') -# spawn_v_brick() -# # left_pnp.pick(brickstuff[0]['pose']) -spawn_v_brick() -left_pnp.send({'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}) -left_pnp.send({'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}) -left_pnp.gripper_close() -left_pnp.send({'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}) -# left_pnp.place(brickstuff[4]['pose']) -# tuck_arms.init_arms() -# exit(0) -# paused = raw_input('Continue?') -# spawn_h_brick() -# left_pnp.send({'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}) -# left_pnp.send({'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}) -# left_pnp.gripper_close() -# left_pnp.send({'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}) -# left_pnp.place(brickstuff[5]['pose']) -# exit(0) -# tuck_arms.init_arms() -# paused = raw_input('Continue?') -# spawn_h_brick() -# left_pnp.pick(brickstuff[1]['pose']) -# left_pnp.place(brickstuff[6]['pose']) -# exit(0) -# paused = raw_input('Continue?') -# spawn_v_brick() -# left_pnp.pick(brickstuff[0]['pose']) -# left_pnp.place(brickstuff[7]['pose']) -# exit(0) -# paused = raw_input('Continue?') -# spawn_v_brick() -# left_pnp.pick(brickstuff[0]['pose']) -# left_pnp.place(brickstuff[8]['pose']) -# exit(0) -# paused = raw_input('Continue?') -# spawn_h_brick() -# left_pnp.pick(brickstuff[1]['pose']) -# left_pnp.place(brickstuff[9]['pose']) -# exit(0) -# paused = raw_input('Continue?') -# spawn_v_brick() -# left_pnp.pick(brickstuff[0]['pose']) -left_pnp.place(brickstuff[10]['pose']) diff --git a/target_angles.py b/target_angles.py index d28264a..1fe928e 100644 --- a/target_angles.py +++ b/target_angles.py @@ -1,6 +1,6 @@ ''' -Pickup Vertical Offset : {'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} -Pickup Vertical Pick : {'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} +Pickup Vertical Offset :: {'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} +Pickup Vertical Pick :: {'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} Pickup Horizontal Offset : {'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} Pickup Horizontal Pick : {'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} Place 1 Offset : {'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} @@ -21,4 +21,27 @@ 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} -''' \ No newline at end of file +''' + +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} +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} +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} +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} +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} +B_5_P = {'left_w0': 0.9437250579116295, 'left_w1': 0.9167077216413124, 'left_w2': 1.222379447910923, 'left_e0': -0.879490332019234, 'left_e1': 1.7294606327190576, 'left_s0': 0.15746537974463826, 'left_s1': -0.5907485424149368} +B_6_A = {'left_w0': 0.13795791158290516, 'left_w1': 1.4976647271381562, 'left_w2': 1.3716861361389447, 'left_e0': -0.7668644025351493, 'left_e1': 1.5004594439971868, 'left_s0': -0.21720089382963984, 'left_s1': -1.3717982666530235} +B_6_P = {'left_w0': 0.30567556734697016, 'left_w1': 1.0253204952492578, 'left_w2': 1.2509852844361058, 'left_e0': -0.6998909977260979, 'left_e1': 1.8157485803507927, 'left_s0': -0.2856819912424196, 'left_s1': -1.1598950455287043} +B_7_A = {'left_w0': 0.2880633344490283, 'left_w1': 1.586029171376809, 'left_w2': 1.618882284053745, 'left_e0': -0.9484107003286485, 'left_e1': 1.3454477156124263, 'left_s0': 0.1821213719091443, 'left_s1': -1.2157353154435486} +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.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_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}