def make_places(self,
init_pose,
allowed_touch_objects,
pre,
post,
set_rpy=0,
roll_vals=[0],
pitch_vals=[0],
yaw_vals=[0]):
place = PlaceLocation()
place.post_place_posture = self.make_gripper_posture(GRIPPER_OPEN)
place.pre_place_approach = self.make_gripper_translation(
pre[0], pre[1], pre[2])
place.post_place_retreat = self.make_gripper_translation(
post[0], post[1], post[2])
place.place_pose = init_pose
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
places = []
for yaw in yaw_vals:
for pitch in pitch_vals:
for y in y_vals:
for x in x_vals:
place.place_pose.pose.position.x = init_pose.pose.position.x + x
place.place_pose.pose.position.y = init_pose.pose.position.y + y
q = quaternion_from_euler(0, pitch, yaw)
if set_rpy:
place.place_pose.pose.orientation.x = q[0]
place.place_pose.pose.orientation.y = q[1]
place.place_pose.pose.orientation.z = q[2]
place.place_pose.pose.orientation.w = q[3]
place.id = str(len(places))
place.allowed_touch_objects = allowed_touch_objects
places.append(deepcopy(place))
return places
p.place_pose.pose.orientation.x = 0
p.place_pose.pose.orientation.y = 0
p.place_pose.pose.orientation.z = 0
p.place_pose.pose.orientation.w = 1
p.pre_place_approach.direction.vector.z = 1
p.pre_place_approach.direction.header.frame_id = 'endeff'
p.pre_place_approach.min_distance = 0.06
p.pre_place_approach.desired_distance = 0.12
p.post_place_retreat.direction.vector.z = -1
p.post_place_retreat.direction.header.frame_id = 'endeff'
p.post_place_retreat.min_distance = 0.05
p.post_place_retreat.desired_distance = 0.06
p.allowed_touch_objects = ["part"]
# append the grasp to the list of grasps
for i in range(3):
scene.remove_attached_object("endeff", "part")
rospy.sleep(2)
scene.add_box("part", pos, (0.02, 0.04, 0.02))
rospy.sleep(2)
result = -1
attempt = 0
while result < 0:
result = arm.pick("part", grasps)
print "Attempt:", attempt
print "Final Result: ", result
