def follow_constrained_path(self, target_pose):
add_padded_lab()
other_pose = self.arm.get_current_pose().pose
other_pose.orientation = target_pose.orientation
self.arm.set_pose_target(other_pose)
self.arm.set_goal_position_tolerance(0.7)
plan = self.arm.plan()
if not move(self.arm, plan.joint_trajectory):
return False
constraints = Constraints()
link_target_orientation = calibrateOrientation(target_pose.orientation)
orientation_constraint = OrientationConstraint(
header=Header(stamp=rospy.Time.now(), frame_id="/base_link"),
orientation=link_target_orientation,
link_name="traybody_hand_right",
absolute_x_axis_tolerance=0.15,
absolute_y_axis_tolerance=3.14,
absolute_z_axis_tolerance=0.15,
weight=10)
# position_constraint = PositionConstraint( header=Header(stamp=rospy.Time.now(), frame_id="/base_link"),
# link_name="arm_right_link_7_t",
# target_point_offset=make_vector(0.01, 0.01, 0.01),
# weight=9 )
# constraints.position_constraints.append(position_constraint)
constraints.orientation_constraints.append(orientation_constraint)
self.arm.set_path_constraints(constraints)
add_shelf(Shelf.PADDED)
# remove_shelf()
self.arm.set_goal_tolerance(0.01)
self.arm.set_planner_id("RRTstarkConfigDefault")
self.arm.set_pose_reference_frame("/base_link")
self.arm.set_pose_target(target_pose)
self.arm.set_planning_time(30)
rospy.loginfo("planning constrained path")
plan = self.arm.plan()
if not len(plan.joint_trajectory.points) > 0:
return 'Failure'
rospy.loginfo("moving constrained path")
remove_padded_lab()
result = move(self.arm, plan.joint_trajectory)
if not result:
return 'Failure'
rospy.loginfo("Success constrained path")
self.arm.clear_path_constraints()
return result
def Start_server():
rospy.init_node('trajectory_service');
s = rospy.Service('trajectory_display', GetTrajectory, runTrajectory);
#Load_Bin_model(X_pos, Y_pos, Z_pos);
add_shelf();
check_collision_server.wait_for_service(0.5);
# Might be a problem
pos_init();
print " >>>>>>>>>>>>>> Waiting For trajectory request... >>>>>>>>>>>>>>>>>>>";
print " Request format:";
print " Task: <Forward/Drop> + BinCode: < A ... L > + UsingTorso < y/n >";
print " Default: Forward + A + n";
rospy.spin()
def execute(self, userdata):
# COMMENT OUT THIS RETURN UNLESS 'PushWithScoop' IS CALLING EVERYTHING
# return 'Success'
self.targetBin = userdata.bin
jointConfigHor = [0, 0, 0, 0, 0, 0, 0, 0]
rospy.loginfo("Trying to scoop from bin '" + self.targetBin + "' ")
# SCOOP
self.arm.set_workspace([-3, -3, -3, 3, 3, 3])
self.arm.set_planning_time(10)
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.arm.set_pose_reference_frame("/shelf")
# horiztonal pose relative to bin
horizontalPose = bin_pose_tray(self.targetBin).pose
horizontalPose.position.x += -0.307581
horizontalPose.position.y += -0.011221
# horizontalPose.position.z += 0.05463
horizontalPose.position.z += 0.18463
horizontalPose.orientation.x = -0.293106
horizontalPose.orientation.y = -0.512959
horizontalPose.orientation.z = 0.403541
horizontalPose.orientation.w = 0.698654
# TODO: calibrate orientation?
leftOffsetRight = -0.110
leftOffsetLeft = -0.080
middleOffsetMiddle = -0.1275
middleOffsetRight = -0.175
middleOffsetLeft = -0.08
rightOffsetRight = 0.140
rightOffsetLeft = 0.170
if (self.targetBin == "A" or self.targetBin == "D"
or self.targetBin == "G" or self.targetBin == "J"):
horizontalPose.position.y += leftOffsetRight
elif (self.targetBin == "B" or self.targetBin == "E"
or self.targetBin == "H" or self.targetBin == "K"):
# horizontalPose.position.y += middleOffsetMiddle
horizontalPose.position.y += middleOffsetLeft
elif (self.targetBin == "C" or self.targetBin == "F"
or self.targetBin == "I" or self.targetBin == "L"):
horizontalPose.position.y += rightOffsetLeft
self.rightColumn = True
# # TEMPORARY ##################################################
# horizontalPose.position.z += 0.13
# if self.isLeftToRight: # THIS ISN"T A THING IN THIS FILE!!!!
# if self.middleColumn:
# horizontalPose.position.y += -0.15
# else:
# horizontalPose.position.y += -0.12
# else:
# horizontalPose.position.y += 0.12
# ##################################################################
jointConfigHor = [0, 0, 0, 0, 0, 0, 0, 0]
if self.targetBin == "A": # SCOOP SUCCESS
# horiztonal pose
jointConfigHor = [
2.787122819843751, 1.1426674464611235, -0.4407243014959283,
-2.938360463288102, -1.7825824022519545, 1.877092883918891,
-1.8864029108808542, -1.7389067722224323
]
# jointConfigHor = [2.782707824398671, 1.1213539604121765, -0.5105509676099902, -2.95, -1.8127950575104665, 1.8751692165531906, -1.8956069620580396, -1.843186568028629]
# default
# jointConfigHor = [2.608074188232422, -0.29658669233322144, 0.8934586644172668, 1.7289633750915527, 1.573803424835205, 1.2867212295532227, 1.4699939489364624, -2.8265552520751953]
# default
jointConfigHor = [
2.608074188232422, -0.29658669233322144, 0.8934586644172668,
1.7289633750915527, 1.573803424835205, 1.2867212295532227,
1.4699939489364624, -2.8265552520751953
]
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
# ^^ FINE POSITION TUNING FOR INDIVIDUAL BINS
elif self.targetBin == "B": #
# vertical pose
jointConfigHor = [
-2.691808686026977, -2.8018998306066116, 1.3848981009275314,
-2.282453315654881, 1.8152513141302793, -0.6202050989860174,
-1.624154936000525, -0.3587748187263247
]
# [-1.0030035405789968, -1.3676622009763029, 0.4576668194873724, -2.177249279887369, 0.450985796862375, -1.204311025594268, -1.5581180459959898, -2.75676652084731, 1.5618443734944463, -2.1295047284401276, 1.4317529318186657, -0.6433763681214768, -1.8426522259677303, -1.653964354060165, -2.8327725595827187, 0.0]
# horizontal pose DOESN"T FINISH CARTESIAN PATH INTO BIN (69%)
jointConfigHor = [
-2.8819607919477774, -2.5683663777443138, 1.9,
-2.241769564971686, 1.1787675479307382, -0.5392291309261772,
-1.8531964931997877, -1.8695438375092903
]
# jointConfigHor = [-2.831730496183794, -2.7264830881170523, 1.6366395998456265, -2.1613363852348173, 1.3968113538046523, -0.6176682002638111, -1.843012465236674, -1.7061506709410292]
# default
# jointConfigHor =
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "C": #
# vertical pose
# jointConfigHor = [0.1128913227811488, 0.17736465719817437, -1.0755894763756846, 1.734991297482921, 1.9132498375426665, 2.425141013887845, -1.0310688499779752, -2.4997632535514924]
# horizontal pose
# jointConfigHor = [0.29026194412817585, 0.461542044508441, -1.594581910530645, 1.7898741139752892, 1.8562945499829162, 2.900906794012061, -1.1051486713512129, -1.0617352188427895]
# 0.29029384157045307, 0.46193821390527073, -1.594604801319865, 1.7897073727041457, 1.8560188191669673, 2.9010215195414353, -1.1056236281812253, -1.0615710089607395
# [0.37818669922807013, 0.48325390802784585, -1.868336510770857, 1.5357536397230587, 1.8766570272027714, 3.0659668609349486, -1.124000502851055, -0.81050432002129] WORKED
jointConfigHor = [
2.813563143840949, 1.4247315556375264, -1.8998582348657576,
-0.828076527049852, 1.0315090081732738, 1.0233878539013184,
1.2436657002000155, 0.8659094216700685
]
# jointConfigHor = [0, 0, 0, 0, 0, 0, 0, 0]
# default
# jointConfigHor =
# default
# jointConfigHor =
self.isLeftToRight = False
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "D": #
# vertical pose
# jointConfigHor = [-2.223934986801738, 3.13, 1.2092354002259527, 0.9307218279859997, -1.8873873503542566, 2.2149979825293564, -1.2486240605659136, 0.28324722321298806]
# default
jointConfigHor = [
2.608074188232422, -0.29658669233322144, 0.8934586644172668,
1.7289633750915527, 1.573803424835205, 1.2867212295532227,
1.4699939489364624, -2.8265552520751953
]
# default
jointConfigHor = [
2.608074188232422, -0.29658669233322144, 0.8934586644172668,
1.7289633750915527, 1.573803424835205, 1.2867212295532227,
1.4699939489364624, -2.8265552520751953
]
self.shortRow = True
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "E": #
# vertical pose
# jointConfigHor = [2.9544418528587726, 1.3567262870651748, -1.3266391225690815, -0.22451889273765355, 2.064895928713241, 1.7098359105053893, 1.747522515305617, 2.125112210336924]
# default
jointConfigHor = [
2.9667019844055176, 1.4224945306777954, -0.7801656126976013,
-0.2995363175868988, 2.195582151412964, 1.864424467086792,
1.6602683067321777, 2.5383474826812744
]
# default
jointConfigHor = [
2.9667019844055176, 1.4224945306777954, -0.7801656126976013,
-0.2995363175868988, 2.195582151412964, 1.864424467086792,
1.6602683067321777, 2.5383474826812744
]
self.shortRow = True
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "F": #
# old
jointConfigHor = [
0.1128913227811488, 0.17736465719817437, -1.0755894763756846,
1.734991297482921, 1.9132498375426665, 2.425141013887845,
-1.0310688499779752, -2.4997632535514924
]
# default
# jointConfigHor = [1.5194422006607056,1.810523509979248, -1.2088792324066162, 1.3328773975372314, -1.8696491718292236, 1.8829082250595093, -1.2678426504135132, 1.606799840927124]
jointConfigHor = [
1.7185487089792388, 1.628701886968344, -1.9,
-1.289229684257566, 2.0208721134965324, 0.9377729352449973,
1.4027401168916545, 0.09704192362409228
]
self.shortRow = True
self.isLeftToRight = False
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "G": #
# vertical pose
jointConfigHor = [
2.4718291585873913, 1.1047984538173783, 1.5290256049994881,
-2.1169639224415793, -2.0890748066865283, -2.178313072949579,
1.57456751422334, -1.7351008864298179
]
# default
jointConfigHor = [
2.4718597530192796, 1.1048811085600885, 1.8289698505492917,
-2.1170583249526715, -2.089052808535928, -2.178255911290856,
1.5745535013303766, -1.735037580794114
]
self.shortRow = True
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "H": #
# old
jointConfigHor = [
2.4718291585873913, 1.1047984538173783, 1.5290256049994881,
-2.1169639224415793, -2.0890748066865283, -2.178313072949579,
1.57456751422334, -1.7351008864298179
]
# default
jointConfigHor = [
2.966156482696533, 1.8770301342010498, 1.2306787967681885,
-0.586269199848175, 2.2546935081481934, 1.669684886932373,
1.7160991430282593, 0.7149554491043091
]
self.shortRow = True
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "I": #
# old
# jointConfigHor = [1.3418513542538393, -1.9163393148721648, 1.8999111796476877, 1.9555683274308242, 2.085973354202339, 0.8327696820366999, 1.521983626079816, 0.9235781887349414]
# default
# jointConfigHor = [1.5194591283798218, 1.251114845275879, -1.8047455549240112, 2.224393606185913, -1.9810069799423218, 1.1204286813735962, -1.827457070350647, 0.8016403913497925]
jointConfigHor = [
2.2079043637321725, 0.8601312830660215, -1.9,
-0.9939683067758994, 1.9120743451785713, 1.4944922595683932,
1.4339132032870154, -0.5700904832953211
]
# default
jointConfigHor = [
1.5194591283798218, 1.251114845275879, -1.8047455549240112,
2.224393606185913, -1.9810069799423218, 1.1204286813735962,
-1.827457070350647, 0.8016403913497925
]
self.shortRow = True
self.isLeftToRight = False
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "J": #
# vertical pose
jointConfigHor = [
-2.814859477213427, 1.171284271024935, 1.2964470093710962,
-1.8496939730019695, 2.154119940035741, -2.417159189716691,
0.29654290371162795, -3.13
]
# default
jointConfigHor = [
1.7551809549331665, 0.04665006324648857, -1.8453619480133057,
1.8693605661392212, -1.189427375793457, 1.5698546171188354,
-1.871213436126709, 0.8811066150665283
]
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "K": #
# old
jointConfigHor = [
-2.814859477213427, 1.171284271024935, 1.2964470093710962,
-1.8496939730019695, 2.154119940035741, -2.417159189716691,
0.29654290371162795, -3.13
]
# default
jointConfigHor = [
2.9667019844055176, -0.873210072517395, -0.5380352735519409,
2.7276151180267334, -2.2068514823913574, 1.085071086883545,
1.8169622421264648, 1.6070705652236938
]
self.isLeftToRight = True
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
elif self.targetBin == "L": #
# vertical pose
# jointConfigHor = [1.2086075801715137, 0.23124532053402494, -1.7309804228879488, -1.2106734273580417, 1.8133929146598422, 1.1998904379674205, 1.7356579754157866, -3.13]
# default
# jointConfigHor = [1.7551809549331665, 0.04665006324648857, -1.8453619480133057, 1.8693605661392212, -1.189427375793457, 1.5698546171188354, -1.871213436126709, 0.8811066150665283]
jointConfigHor = [
-0.12513779447492768, -2.336026338817234, -0.9455866795141264,
-2.5098342621859016, -2.172419746644237, -0.8552426475281811,
1.7569769188312936, -2.2410209148173426
]
# default
jointConfigHor = [
1.7551809549331665, 0.04665006324648857, -1.8453619480133057,
1.8693605661392212, -1.189427375793457, 1.5698546171188354,
-1.871213436126709, 0.8811066150665283
]
self.isLeftToRight = False
horizontalPose.position.x += 0.000
horizontalPose.position.y += 0.000
horizontalPose.position.z += 0.000
add_shelf(Shelf.PADDED)
# remove_shelf() # SHELF SHOULD NOT ACTUALLY BE REMOVED
self.arm.set_joint_value_target(jointConfigHor)
rospy.loginfo("planning to jointConfigHor")
plan = self.arm.plan()
if not len(plan.joint_trajectory.points) > 0:
return 'Failure'
remove_shelf()
add_shelf(Shelf.FULL)
rospy.loginfo("Moving to jointConfigHor")
if not move(self.arm, plan.joint_trajectory):
rospy.logerr("Failed to get to jointConfigHor")
return 'Failure'
horizontalPose = self.convertFrameRobotToShelf(horizontalPose)
if not self.scoopBin(horizontalPose):
return 'Failure'
# rospy.sleep(100)
# add_shelf(Shelf.FULL)
rospy.loginfo("planning cartesian path out of bin")
if self.targetBin == "A":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
# poses[-1].position.x += -0.4586
poses[-1].position.x += -0.300
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
rospy.loginfo("failed half out")
return 'Failure'
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.10
poses[-1].position.z += 0.05
rospy.loginfo("planning cartesian path to final bin pose")
if not follow_path(self.arm, poses):
rospy.loginfo("failed out")
return 'Failure'
elif self.targetBin == "B":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OVER (FOR JOINT LIMITS)
poses.append(deepcopy(poses[-1]))
poses[-1].position.y += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
rospy.loginfo("moved over")
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
# poses[-1].position.x += -0.4586
poses[-1].position.x += -0.3586 # MAY BE IN COLLISION WITH SHELF
# poses[-1].position.y += 0.04
poses[-1].position.z += 0.19
# poses[-1].position.z += 0.04
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "C":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.x += -0.3586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
# poses[-1].position.z += 0.15
poses[-1].position.z += 0.10
if not follow_path(self.arm, poses):
return 'Failure'
# poses = [self.convertFrameRobotToShelf(self.arm.
# get_current_pose().pose)]
# poses.append(deepcopy(poses[-1]))
# poses[-1].position.z += 0.05
# rospy.loginfo("planning cartesian path to final bin pose")
# if not follow_path(self.arm, poses):
# return 'Failure'
# poses = [self.convertFrameRobotToShelf(self.arm.
# get_current_pose().pose)]
# poses.append(deepcopy(poses[-1]))
# # To right side of shelf
# poses[-1].position.y = -0.686495 # INCLUDES CURRENT SHELF CALIBRATION as of Saturday night
# poses[-1].position.z += -0.05
# poses[-1].orientation.x = -0.36667
# poses[-1].orientation.y = -0.648119
# poses[-1].orientation.z = 0.333549
# poses[-1].orientation.w = 0.578135
# rospy.loginfo("planning cartesian path to pre-dumping pose")
# if not follow_path(self.arm, poses):
# return 'Failure'
elif self.targetBin == "D":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "E":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "F":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "G":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "H":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "I":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
poses[-1].position.y -= 0.035
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "J":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "K":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
# poses[-1].position.x += -0.4586
poses[-1].position.x += -0.3586
# poses[-1].position.y += 0.05
# poses[-1].position.z += 0.05
if not follow_path(self.arm, poses):
return 'Failure'
elif self.targetBin == "L":
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# OUT + UP
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -0.4886
# poses[-1].position.y += 0.05
poses[-1].position.z += 0.03
if not follow_path(self.arm, poses):
return 'Failure'
#######################################################################
rospy.loginfo("made it out")
#######################################################################
# bins C, F, I, L
if self.targetBin == 'C' or self.targetBin == 'F' or self.targetBin == 'I' or self.targetBin == 'L' or self.targetBin == 'B' or self.targetBin == 'E' or self.targetBin == 'H' or self.targetBin == 'K':
target_pose = Pose()
target_pose.position.x = 0.49195
target_pose.position.y = -0.39594
target_pose.position.z = 0.64392
target_pose.orientation.x = 0.16997
target_pose.orientation.y = -0.63061
target_pose.orientation.z = 0.73307
target_pose.orientation.w = 0.18988
else:
#bins A, D, G, J,
target_pose = Pose()
target_pose.position.x = 0.24128
target_pose.position.y = 0.65743
target_pose.position.z = 0.72495
target_pose.orientation.x = -0.52171
target_pose.orientation.y = -0.28389
target_pose.orientation.z = -0.029079
target_pose.orientation.w = 0.80398
rospy.loginfo("Trying to follow constrained path")
if not self.follow_constrained_path(target_pose):
rospy.loginfo("FAILED to follow constrained path")
return 'Failure'
# poses = [self.convertFrameRobotToShelf(self.arm.
# get_current_pose().pose)]
# # To order bin
# poses.append(deepcopy(poses[-1]))
# poses[-1].position.x = 0.472985 # 0.482178
# poses[-1].position.y = -0.351667 # -0.335627
# poses[-1].position.z = 0.753171 # 0.706449
# poses[-1].orientation.x = -0.164656 # -0.198328
# poses[-1].orientation.y = 0.766477 # 0.759802
# poses[-1].orientation.z = -0.591483 # -0.598499
# poses[-1].orientation.w = -0.188543 # -0.158639
# poses[-1] = self.convertFrameRobotToShelf(poses[-1])
#
# rospy.loginfo("planning cartesian path to dumping pose")
# if not follow_path(self.arm, poses):
# return 'Failure'
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
if self.targetBin == 'C' or self.targetBin == 'F' or self.targetBin == 'I' or self.targetBin == 'L' or self.targetBin == 'B' or self.targetBin == 'E' or self.targetBin == 'H' or self.targetBin == 'K':
# TODO: FIX THIS STUFF
# Tilt little by little
poses.append(deepcopy(poses[-1]))
poses[-1].position.x = 0.472985 # 0.482178
poses[-1].position.y = -0.351667 # -0.335627
poses[-1].position.y += 0.02
poses[-1].position.z = 0.753171 # 0.706449
poses[-1].orientation.x = 0.143945
poses[-1].orientation.y = -0.605741
poses[-1].orientation.z = 0.757694
poses[-1].orientation.w = 0.195594
poses[-1] = self.convertFrameRobotToShelf(poses[-1])
poses.append(deepcopy(poses[-1]))
poses[-1].position.x = 0.472985 # 0.482178
poses[-1].position.y = -0.351667 # -0.335627
poses[-1].position.y += 0.02
poses[-1].position.z = 0.753171 # 0.706449
poses[-1].orientation.x = 0.113945
poses[-1].orientation.y = -0.525741
poses[-1].orientation.z = 0.827694
poses[-1].orientation.w = 0.215594
poses[-1] = self.convertFrameRobotToShelf(poses[-1])
poses.append(deepcopy(poses[-1]))
poses[-1].position.x = 0.472985 # 0.482178
poses[-1].position.y = -0.351667 # -0.335627
poses[-1].position.y += 0.02
poses[-1].position.z = 0.753171 # 0.706449
poses[-1].orientation.x = 0.112873
poses[-1].orientation.y = -0.520793
poses[-1].orientation.z = 0.819904
poses[-1].orientation.w = 0.209268
poses[-1] = self.convertFrameRobotToShelf(poses[-1])
print "Planning Cartesian Path Dump....."
if not follow_path(self.arm, poses):
rospy.loginfo("FAILED to dump")
return 'Failure'
# return right arm to home position
add_shelf(Shelf.PADDED)
jointConfigHome = [0, 0, 0, 0, 0, 0, 0, 0, 0]
jointConfigHome[0] = 0.0 # Torso
jointConfigHome[1] = 1.699523295294849
jointConfigHome[2] = -0.6448955832339801
jointConfigHome[3] = -0.06852598822491722
jointConfigHome[4] = -2.3331612363309975
jointConfigHome[5] = -0.3915515016420941
jointConfigHome[6] = 0.15148041914194765
jointConfigHome[7] = 0.4944912570006051
# this is currently done instead of goto_pose to use joint values
self.arm.set_planning_time(5)
self.arm.set_joint_value_target(jointConfigHome)
plan = self.arm.plan()
if not len(plan.joint_trajectory.points) > 0:
return 'Failure'
remove_shelf()
add_shelf(Shelf.FULL)
rospy.loginfo("moving right arm to home position")
if not self.move(plan.joint_trajectory):
rospy.loginfo("FAILED to move right arm home")
return 'Failure'
return 'Success'
#arm_right_group.set_planner_id("RRTstarkConfigDefault");
arm_left_group.set_planning_time(planning_time)
arm_right_group.set_planning_time(planning_time)
print ">>>> Import Bin model, Generate Testing Targets >>>>"
if len(sys.argv) > 1:
X_pos = float(sys.argv[1])
Y_pos = float(sys.argv[2])
Z_pos = float(sys.argv[3])
X_pos = default_Bin_X
Y_pos = default_Bin_Y
Z_pos = default_Bin_Z
print "Current Shelf Position: ", X_pos, Y_pos, Z_pos
#Load_Bin_model(X_pos, Y_pos, Z_pos);
add_shelf()
print ">>>> Waiting for service `compute_ik` >>>>"
rospy.wait_for_service('compute_ik')
ik = rospy.ServiceProxy("compute_ik", GetPositionIK)
print ">>>> Importing Init / Drop configurations..."
key_joint_state = generate_key_joint_state(arm_left_group.get_name())
# Generating SCANNING Configuration
print ">>>> Generating SCANNING goal Ponits..."
Scanning_points = generate_Scan_points(Bin_base_x=X_pos,
Bin_base_y=Y_pos,
Bin_base_z=Z_pos,
Extend_distance=0.65)
print "Total", len(Scanning_points), "SCAN points"
"B": Shelf.BIN_B,
"C": Shelf.BIN_C,
"D": Shelf.BIN_D,
"E": Shelf.BIN_E,
"F": Shelf.BIN_F,
"G": Shelf.BIN_G,
"H": Shelf.BIN_H,
"I": Shelf.BIN_I,
"J": Shelf.BIN_J,
"K": Shelf.BIN_K,
"L": Shelf.BIN_L
}
if __name__ == '__main__':
rospy.init_node("shelf_collsion")
parser = argparse.ArgumentParser(description="Publish shelf collision")
parser.add_argument("--remove",
help="Remove the shelf",
action="store_true")
parser.add_argument("--quality",
default="Full",
help="Shelf quality (None, Simple, Full, Padded)")
args = parser.parse_args()
if args.remove:
remove_shelf()
else:
add_shelf(qualities[args.quality.upper()])
def pushToSide(self, verticalPose, jointConfigVert):
remove_shelf()
# START
# rospy.loginfo("going to vertical pose")
# self.startPose = self.convertFrameRobotToShelf(self.arm.
# get_current_pose().pose)
# poses = [self.startPose]
# poses.append(self.startPose)
# # poses[-1].position.x = verticalPose.position.x
# # poses[-1].position.y = verticalPose.position.y
# # poses[-1].position.z = verticalPose.position.z
# poses[-1].orientation.x = verticalPose.orientation.x
# poses[-1].orientation.y = verticalPose.orientation.y
# poses[-1].orientation.z = verticalPose.orientation.z
# poses[-1].orientation.w = verticalPose.orientation.w
# if not follow_path(self.arm, poses):
# # rospy.loginfo("FAILED going to vertical position")
# rospy.loginfo("FAILED going to vertical orientation")
# # rospy.sleep(10)
# return False
# #
# poses = [self.convertFrameRobotToShelf(self.arm.
# get_current_pose().pose)]
# # poses.append(verticalPose)
# # poses[-1].position.x += -0.10
# poses.append(verticalPose)
# if not follow_path(self.arm, poses):
# rospy.loginfo("FAILED going to vertical pose")
# # rospy.sleep(10)
# return False
# THIS BLOCK OF CODE FOR OBTAINING JOINT CONFIGURATIONS FOR PLANNING ONLY, SHOULD BE COMMENTED OUT FOR ACTUAL RUNS
remove_shelf() # SHELF SHOULD NOT ACTUALLY BE REMOVED HERE
rospy.sleep(1.0)
self.arm.set_pose_target(verticalPose)
plan = self.arm.plan()
if not len(plan.joint_trajectory.points) > 0:
return False
rospy.loginfo("going to vertical pose")
add_shelf(Shelf.FULL)
if not self.move(plan.joint_trajectory):
rospy.loginfo("FAILED going to vertical pose")
return False
verticalPose = self.convertFrameShelfToRobot(verticalPose)
rospy.loginfo(verticalPose)
with open("vertical_joint_config.txt", "a+") as out_file:
joint_config = self.arm.get_current_joint_values()
out_file.write(
str(self.targetBin) + "\t" + str(joint_config) + "\n")
# raw_input("Hit enter to continue ")
# rospy.sleep(15.0)
####################################################################################################
# IN
rospy.loginfo("Going along inside wall")
remove_shelf()
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
poses.append(deepcopy(poses[-1]))
# xDist1 = 0.12
xDist1 = 0.04
poses[-1].position.x += xDist1
if self.isLeftToRight:
poses[-1].position.y += 0.05
else:
poses[-1].position.y += -0.05
if self.shortRow:
poses[-1].position.z += -xDist1 * 0.0875 # 5 degrees
poses.append(deepcopy(poses[-1]))
# xDist2 = 0.35
xDist2 = 0.25
poses[-1].position.x += xDist2
if self.isLeftToRight:
poses[-1].position.y += 0.03
else:
poses[-1].position.y += -0.03
if self.shortRow:
poses[-1].position.z += -xDist2 * 0.0875 # 5 degrees
if not follow_path(self.arm, poses):
rospy.loginfo("FAILED Going along inside wall")
# rospy.sleep(10)
return False
# add_shelf(Shelf.FULL)
# rospy.sleep(15)
# remove_shelf()
# PUSH
rospy.loginfo("Pushing items to side")
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# adjust orientation?
poses.append(deepcopy(poses[-1]))
if self.isLeftToRight:
poses[-1].position.y += -0.10
else:
poses[-1].position.y += 0.10
# if self.middleColumn:
# if self.isLeftToRight:
# poses[-1].position.y += -0.05
# else:
# poses[-1].position.y += 0.05
if not follow_path(self.arm, poses):
rospy.loginfo("FAILED pushing items to side")
# rospy.sleep(10)
return False
# TODO: REPLACE COMPUTE CARTESIAN CALLS WITH ACTUAL REVERSE TRAJECTORIES
# (reverse) PUSH
rospy.loginfo("(reverse) Pushing items to side")
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
# adjust orientation?
poses.append(deepcopy(poses[-1]))
if self.isLeftToRight:
poses[-1].position.y += 0.10
else:
poses[-1].position.y += -0.10
# if self.middleColumn:
# if self.isLeftToRight:
# poses[-1].position.y += -0.05
# else:
# poses[-1].position.y += 0.05
if not follow_path(self.arm, poses):
rospy.loginfo("FAILED (reverse) pushing items to side")
# rospy.sleep(10)
return False
# OUT
rospy.loginfo("(reverse) Going along inside wall")
poses = [
self.convertFrameRobotToShelf(self.arm.get_current_pose().pose)
]
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -xDist1
if self.isLeftToRight:
poses[-1].position.y += -0.05
else:
poses[-1].position.y += 0.05
if self.shortRow:
poses[-1].position.z += xDist1 * 0.0875 # 5 degrees
poses.append(deepcopy(poses[-1]))
poses[-1].position.x += -xDist2
if self.isLeftToRight:
poses[-1].position.y += -0.03
else:
poses[-1].position.y += 0.03
if self.shortRow:
poses[-1].position.z += xDist2 * 0.0875 # 5 degrees
if not follow_path(self.arm, poses):
rospy.loginfo("FAILED (reverse) Going along inside wall")
# rospy.sleep(10)
return False
# BACK TO START
# poses = [self.convertFrameRobotToShelf(self.arm.
# get_current_pose().pose)]
# rospy.loginfo("Removing tray from bin")
# # poses.append(deepcopy(poses[-1]))
# # # poses[-1].position.y += 0.05
# # # poses[-1].position.x += -0.50 # WORKS FOR BINS A,B,C?
# # poses[-1].position.x += -0.40
# # if self.shortRow:
# # poses[-1].position.z += xDist*0.0875
# # if self.isLeftToRight:
# # poses[-1].position.y += 0.05
# # else:
# # poses[-1].position.y += -0.05
# poses.append(self.startPose)
# if not follow_path(self.arm, poses):
# return False
# BACK TO VERTICAL CONFIG
rospy.loginfo("going back to vertical config")
# add_shelf(Shelf.FULL) # MAYBE TODO: GET PADDED SHELF TO PASS VALIDITY CHECK AND USE PADDED INSTEAD OF FULL, CURRENTLY TRAY COLLIDES WITH SHELF
add_shelf(Shelf.PADDED)
self.arm.set_joint_value_target(jointConfigVert)
plan = self.arm.plan()
if not len(plan.joint_trajectory.points) > 0:
return False
remove_shelf()
add_shelf(Shelf.FULL)
if not self.move(plan.joint_trajectory):
rospy.loginfo("FAILED going back to vertical config")
# rospy.sleep(10)
return False
rospy.loginfo("PushWithScoop Success!")
return True
def execute(self, userdata):
return 'Success'
self.targetBin = userdata.bin
# startBin = "C"
# add_shelf(Shelf.FULL)
add_shelf(Shelf.PADDED)
rospy.loginfo("Trying to push with scoop in bin '" + self.targetBin +
"' ")
verticalPose = bin_pose_tray(self.targetBin).pose
verticalPose.position.x += -0.364436
verticalPose.position.y += 0.12305766
verticalPose.position.z += 0.027
# verticalPose.position.z += 0.022
verticalPose.position.x += 0.08
jointConfigVert = [0, 0, 0, 0, 0, 0, 0, 0]
if self.targetBin == "A": # SUCCESS!
# calibrated pose
jointConfigVert = [
1.9681722954889078, -0.023200618121380513, 1.5089116451880789,
1.8039264439517484, 1.9849145300443676, 1.248039336029401,
1.620441408283454, -3.13
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [1.633413838847524, -0.02234441730095664, 1.6987069230021477, 1.731898728022606, 2.0131615638876266, 1.403746405677286, 1.6226511587959498, -3.13]
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
# ^^ FINE POSITION TUNING FOR INDIVIDUAL BINS
elif self.targetBin == "B": # SUCCESS!
# calibrated pose
jointConfigVert = [
-2.691808686026977, -2.8018998306066116, 1.3848981009275314,
-2.282453315654881, 1.8152513141302793, -0.6202050989860174,
-1.624154936000525, -0.3587748187263247
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [-2.8249161687471696, -2.7789494140209747, 1.5085386941026857, -2.436800483385195, 1.5706770697724681, -0.446029707443648, -1.5329404189508564, -0.4038196604364859]
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "C": # SUCCESS!
# calibrated pose
jointConfigVert = [
0.1128913227811488, 0.17736465719817437, -1.0755894763756846,
1.734991297482921, 1.9132498375426665, 2.425141013887845,
-1.0310688499779752, -2.4997632535514924
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [0.2146956177344928, 0.25739452246930106, -0.9936528543562603, 1.8907519420203824, 1.812992032991957, 2.271019899336707, -0.9276949282878929, -2.5856899046613986]
self.isLeftToRight = False
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "D": # SUCCESS!
# calibrated pose
jointConfigVert = [
-2.223934986801738, 3.13, 1.2092354002259527,
0.9307218279859997, -1.8873873503542566, 2.2149979825293564,
-1.2486240605659136, 0.28324722321298806
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [-2.302278940598966, 3.13, 1.5073420982925052, 0.8837162017522663, -1.7329108195800096, 2.5263578927542536, -1.3214070581899602, 0.24415863494339873]
self.shortRow = True
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "E": # SUCCESS!
# calibrated pose
jointConfigVert = [
2.9544418528587726, 1.3567262870651748, -1.3266391225690815,
-0.22451889273765355, 2.064895928713241, 1.7098359105053893,
1.747522515305617, 2.125112210336924
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [2.8370436083524377, 1.2072956998282003, -1.4402799541583589, -0.41370552630331814, 1.8375703830875532, 1.8587010454768464, 1.6856722070955155, 2.145829386630368]
self.shortRow = True
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "F": # NO VERTICAL POSE (collides with shelf????)
# old
jointConfigVert = [
0.1128913227811488, 0.17736465719817437, -1.0755894763756846,
1.734991297482921, 1.9132498375426665, 2.425141013887845,
-1.0310688499779752, -2.4997632535514924
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [0.4908599722393113, -0.0910288057446271, -1.4864957238544354, 1.6901968871490405, 2.3, 2.229444269049108, -1.1981784513615523, -2.5977552449716548]
self.shortRow = True
self.isLeftToRight = False
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "G": # SUCCESS!
jointConfigVert = [
2.4718291585873913, 1.1047984538173783, 1.5290256049994881,
-2.1169639224415793, -2.0890748066865283, -2.178313072949579,
1.57456751422334, -1.7351008864298179
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [-2.0552769954231422, 0.7351067304681358, -1.5887923919877596, 2.4504723077835133, -1.87645629773813, 0.21523952433394805, 0.7463567724752466, 1.3300852303752424]
self.shortRow = True
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "H": # NO VERTICAL POSE - no valid goal states, in collision with cylinder around torso
# pose FAILED
# jointConfigVert = [2.4718291585873913, 1.1047984538173783, 1.5290256049994881, -2.1169639224415793, -2.0890748066865283, -2.178313072949579, 1.57456751422334, -1.7351008864298179]
# calibrated pose (+10 cm x offset)
jointConfigVert = [
2.8645850077570496, -1.9284717090078833, 1.9,
2.4566897358850075, 2.087668969280465, -1.4017452375240989,
-1.4628372999888233, -1.6874292990338764
]
self.shortRow = True
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "I": # NO VERTICAL POSE - no valid goal states, in collision with cylinder around torso
# old
jointConfigVert = [
1.3418513542538393, -1.9163393148721648, 1.8999111796476877,
1.9555683274308242, 2.085973354202339, 0.8327696820366999,
1.521983626079816, 0.9235781887349414
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [2.1317464313897188, -2.2614131661311063, 1.9, 2.1982278009279286, 1.503858147401992, -1.4623520824160112, -1.5083072373951962, 1.0445921533565907]
self.shortRow = True
self.isLeftToRight = False
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "J": # FAILED - pushing items (completed 85%)
# old calibrated pose
jointConfigVert = [
-2.814859477213427, 1.171284271024935, 1.2964470093710962,
-1.8496939730019695, 2.154119940035741, -2.417159189716691,
0.29654290371162795, -3.13
]
# # calibrated pose (+10 cm x offset)
# jointConfigVert =
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "K": # NO VERTICAL POSE - no valid goal states, in collision with cylinder around torso
# old
jointConfigVert = [
2.7000695658667015, -0.8731849060533569, -0.5379493727737572,
2.7276986404944212, -2.206815270597627, 1.0851166746411938,
1.5169290144011378, 1.6070088457908016
]
# # calibrated pose (+10 cm x offset)
# jointConfigVert =
self.isLeftToRight = True
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
elif self.targetBin == "L": # SUCCESS!
# calibrated pose
jointConfigVert = [
1.2086075801715137, 0.23124532053402494, -1.7309804228879488,
-1.2106734273580417, 1.8133929146598422, 1.1998904379674205,
1.7356579754157866, -3.13
]
# calibrated pose (+10 cm x offset)
# jointConfigVert = [1.2106165946363499, 0.26898797477567826, -1.5493649328884376, -1.4180416174391741, 1.860903253764141, 1.426190340627987, 1.590612808438451, -3.0546921093913526]
self.isLeftToRight = False
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
# FIX
leftOffset = 0.000
middleOffset = 0.000
rightOffset = 0.000
# TEMPORARY ##################################################
verticalPose.position.z += 0.13
if self.isLeftToRight:
if self.middleColumn:
verticalPose.position.y += -0.15
else:
verticalPose.position.y += -0.12
else:
verticalPose.position.y += 0.12
##################################################################
if self.isLeftToRight:
if self.shortRow: # D, E, G, H
verticalPose.orientation.x = 0.336374
verticalPose.orientation.y = -0.590811
verticalPose.orientation.z = -0.250799
verticalPose.orientation.w = 0.689126
# TODO: calibrate orientation?
verticalPose.position.x += -0.050
verticalPose.position.y += -0.100
verticalPose.position.z += 0.123
verticalPose.position.y += leftOffset
if self.middleColumn:
verticalPose.position.y += middleOffset
else: # A, B, J, K
verticalPose.orientation.x = 0.19924
verticalPose.orientation.y = -0.69387
verticalPose.orientation.z = -0.14743
verticalPose.orientation.w = 0.6761
# TODO: calibrate orientation?
verticalPose.position.x += 0.000
verticalPose.position.y += 0.000
verticalPose.position.z += 0.000
verticalPose.position.y += leftOffset
if self.middleColumn:
verticalPose.position.y += middleOffset
elif not self.isLeftToRight:
if self.shortRow: # F, I
verticalPose.orientation.x = -0.659937
verticalPose.orientation.y = 0.0351767
verticalPose.orientation.z = 0.738262
verticalPose.orientation.w = 0.13496
# TODO: calibrate orientation?
verticalPose.position.x += -0.050
verticalPose.position.y += -0.100
verticalPose.position.z += 0.133
verticalPose.position.y += rightOffset
else: # C, L
verticalPose.orientation.x = 0.686353
verticalPose.orientation.y = 0.166894
verticalPose.orientation.z = -0.680383
verticalPose.orientation.w = -0.195307
# TODO: calibrate orientation?
verticalPose.position.x += -0.018493
verticalPose.position.y += -.261708
verticalPose.position.z += 0.025
verticalPose.position.y += rightOffset
rospy.loginfo("going to vertical config")
# TODO: FIX THIS ##################################################################
# currently calls get_known_trajectory directly to bypass trajectory validation
# plan, success = get_known_trajectory('Pick', startBin)
# if not self.move(plan.joint_trajectory):
# return 'Failure'
# plan, success = get_known_trajectory('Dump', startBin)
# if not self.move(plan.joint_trajectory):
# return 'Failure'
# plan, success = get_known_trajectory('Lift', startBin)
# if not self.move(plan.joint_trajectory):
# return 'Failure'
# plan, success = get_known_trajectory('Home', startBin)
# if not self.move(plan.joint_trajectory):
# return 'Failure'
# return 'Success'
##################################################################################
remove_shelf()
# TODO: USE FOLLOWING CODE BLOCK ONCE PATHS ARE PRE-COMPUTED #####################
# if not execute_known_trajectory(self.arm, 'Pick', self.targetBin):
# if not execute_known_trajectory(self.arm, 'Pick', startBin):
# return 'Failure'
# remove_shelf()
# if not execute_known_trajectory(self.arm, 'Dump', self.targetBin):
# return 'Failure'
# add_shelf()
# if not execute_known_trajectory(self.arm, 'Lift', self.targetBin):
# return 'Failure'
# if not execute_known_trajectory(self.arm, 'Home', self.targetBin):
# return 'Failure'
# return 'Success'
##################################################################################
self.arm.set_planning_time(5)
self.arm.set_planner_id("RRTConnectkConfigDefault")
# self.arm.set_planner_id("RRTstarkConfigDefault")
# remove_shelf() # SHELF SHOULD NOT ACTUALLY BE REMOVED HERE
# add_shelf(Shelf.FULL)
add_shelf(Shelf.PADDED)
self.arm.set_joint_value_target(jointConfigVert)
plan = self.arm.plan()
if not len(plan.joint_trajectory.points) > 0:
return 'Failure'
remove_shelf()
add_shelf(Shelf.FULL)
if not self.move(plan.joint_trajectory):
return 'Failure'
self.arm.set_pose_reference_frame("/shelf")
verticalPose = self.convertFrameRobotToShelf(verticalPose)
# rospy.loginfo(verticalPose)
if not self.pushToSide(verticalPose, jointConfigVert):
return 'Failure'
return 'Success'