def execute(self, userdata):
rospy.loginfo('Executing state ScanObstacles')
userdata.sec_try_done = False
if userdata.sec_try:
current_region = self.classified_regions[self.next_cluster - 1][0]
userdata.sec_try_done = True
else:
#get regions
if len(self.classified_regions) == 0:
obstacle_cluster = utils.map.get_obstacle_regions()
rospy.logdebug(
str(len(self.classified_regions)) + " regions found.")
print '#####################################'
self.classified_regions = utils.map.undercover_classifier(
obstacle_cluster, userdata.yaml.objects)
print self.classified_regions
print '#####################################'
base = utils.manipulation.get_base_origin().point
self.classified_regions.sort(key=lambda x: euclidean_distance(
Point(*(utils.map.index_to_coordinates(*x[0].get_avg())) +
(0.0, )), base))
# x[0].get_number_of_cells())
if self.next_cluster >= len(self.classified_regions):
rospy.loginfo("searched all cluster")
return 'noRegionLeft'
current_region = self.classified_regions[self.next_cluster][0]
rospy.logdebug("current region: " + str(self.next_cluster) + "\n" +
str(current_region) + "\nclassified as " +
str(self.classified_regions[self.next_cluster][1]))
self.next_cluster += 1
region_centroid = Point(
*(utils.map.index_to_coordinates(*current_region.get_avg())) +
(-0.065, ))
dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0),
region_centroid)
#if userdata.yaml.task_type == Task.TASK_5:
# fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0))
# dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid)
# if dist_to_fixture < 0.35:
# rospy.logdebug("Region classified as puzzle fixture, skipping")
# return 'mapScanned'
# If the arm cannot move ignore distant regions
# TODO find the best max distance
if not userdata.enable_movement:
rospy.logdebug('Distance of the current region to the arm: %s' %
str(dist_to_region))
if dist_to_region > 1.1:
rospy.logwarn('Current region is out of reach. Ignoring it.')
return 'mapScanned'
angle = 1.2
distance = 0.6 + current_region.get_number_of_cells() * 0.008
poses = make_scan_pose(region_centroid, distance, angle, n=16)
if not userdata.enable_movement:
poses = utils.manipulation.filter_close_poses(poses)
poses = utils.map.filter_invalid_scan_poses2(region_centroid.x,
region_centroid.y, poses)
if userdata.sec_try:
current_pose = utils.manipulation.get_eef_position().pose.position
current_pose.z = 0
region_to_eef = subtract_point(region_centroid, current_pose)
poses.sort(key=lambda pose: abs(
get_angle(
region_to_eef,
subtract_point(
Point(pose.pose.position.x, pose.pose.position.y, 0),
region_centroid)) - pi / 2))
visualize_poses(poses)
if userdata.enable_movement:
# move = utils.manipulation.move_arm_and_base_to
plan = utils.manipulation.plan_arm_and_base_to
else:
# move = utils.manipulation.move_to
plan = utils.manipulation.plan_arm_to
utils.manipulation.blow_up_objects(do_not_blow_up_list=("map"))
for pose in poses:
# utils.manipulation.set_planning_time_arm(2)
if utils.manipulation.move_with_plan_to(plan(pose)):
# utils.manipulation.set_planning_time_arm(5)
userdata.focused_point = region_centroid
rospy.logdebug('Wait for clock')
time.sleep(0.5)
rospy.sleep(2.5)
return 'newImage'
utils.manipulation.blow_down_objects()
return 'mapScanned'
def execute(self, userdata):
rospy.loginfo('Executing state GraspObject')
collision_object_name = userdata.object_to_move.mpe_object.id
rospy.loginfo('Trying to grasp %s' % collision_object_name)
if userdata.enable_movement:
move_to_func = utils.manipulation.move_arm_and_base_to
plan_to_func = utils.manipulation.plan_arm_and_base_to
else:
move_to_func = utils.manipulation.move_to
plan_to_func = utils.manipulation.plan_arm_to
#get the collisionobject out of the planningscene
collision_object = utils.manipulation.get_planning_scene().get_collision_object(collision_object_name)
if collision_object is None:
rospy.logwarn("Collision Object " + collision_object_name + " is not in planningscene.")
return 'objectNotInPlanningscene'
rospy.logdebug("Grasping: " + str(collision_object))
grasp_positions = calculate_grasp_position(collision_object, utils.manipulation.transform_to)
#filter out some invalid grasps
grasp_positions = utils.manipulation.filter_low_poses(grasp_positions)
if not userdata.enable_movement:
grasp_positions = utils.manipulation.filter_close_poses(grasp_positions)
if len(grasp_positions) == 0:
rospy.logwarn("No grasppositions found for " + collision_object_name)
userdata.failed_object = userdata.object_to_move
return 'noGraspPosition'
#sort to try the best grasps first
grasp_positions.sort(cmp=lambda x, y: utils.manipulation.cmp_pose_stamped(collision_object, x, y))
visualize_poses(grasp_positions)
utils.manipulation.open_gripper()
grasp_positions = [utils.manipulation.transform_to(grasp) for grasp in grasp_positions]
utils.manipulation.blow_up_objects(do_not_blow_up_list=(collision_object_name))
for grasp in grasp_positions:
plan_pre_grasp = plan_to_func(get_pre_grasp(grasp))
if plan_pre_grasp is None:
continue
rospy.logdebug("Plan to pregraspposition found")
utils.manipulation.blow_up_objects(do_not_blow_up_list=("map", collision_object_name))
plan_to_grasp = plan_to_func(grasp, start_state=utils.manipulation.get_end_state(plan_pre_grasp))
if plan_to_grasp is None or not utils.manipulation.move_with_plan_to(plan_pre_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.sleep(0.5)
if not utils.manipulation.move_with_plan_to(plan_to_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.logdebug("Graspposition taken")
time.sleep(0.5)
rospy.sleep(1)
if not utils.manipulation.close_gripper(collision_object, get_fingertip(utils.manipulation.transform_to(grasp))):
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
time.sleep(0.5)
rospy.sleep(1.5)
#calculate the center of mass and weight of the object and call the load object service
com = utils.manipulation.get_center_of_mass(collision_object)
com = utils.manipulation.transform_to(com, "/tcp")
if com is None:
rospy.logwarn("TF failed")
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
density = 1
for obj in userdata.yaml.objects:
if obj.name == collision_object_name:
density = obj.primitive_densities[0]
if isinf(density):
rospy.logerr("Infinite density! WTF. setting density to 7850")
density = 7850
utils.manipulation.load_object(utils.manipulation.calc_object_weight(collision_object, density),
Vector3(com.point.x, com.point.y, com.point.z))
rospy.loginfo("grasped " + collision_object_name)
#save grasp data for placing
userdata.grasp = grasp
fingertip = get_fingertip(grasp)
fingertip_to_tcp = subtract_point(grasp.pose.position, fingertip.point)
userdata.dist_to_obj = magnitude(fingertip_to_tcp)
rospy.logdebug("lift object")
the_pre_grasp = get_pre_grasp(grasp)
the_move_to_func = move_to_func(the_pre_grasp, do_not_blow_up_list=collision_object_name)
if not the_move_to_func:
rospy.logwarn("couldnt lift object. continue anyway")
userdata.failed_object = None
return 'success'
rospy.logwarn("Grapsing failed.")
userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state ScanObstacles')
userdata.sec_try_done = False
if userdata.sec_try:
current_region = self.classified_regions[self.next_cluster-1][0]
userdata.sec_try_done = True
else:
#get regions
if len(self.classified_regions) == 0:
obstacle_cluster = utils.map.get_obstacle_regions()
rospy.logdebug(str(len(self.classified_regions)) + " regions found.")
print '#####################################'
self.classified_regions = utils.map.undercover_classifier(obstacle_cluster, userdata.yaml.objects)
print self.classified_regions
print '#####################################'
base = utils.manipulation.get_base_origin().point
self.classified_regions.sort(key=lambda x: euclidean_distance(Point(*(utils.map.index_to_coordinates(*x[0].get_avg()))+(0.0,)), base))
# x[0].get_number_of_cells())
if self.next_cluster >= len(self.classified_regions):
rospy.loginfo("searched all cluster")
return 'noRegionLeft'
current_region = self.classified_regions[self.next_cluster][0]
rospy.logdebug("current region: " + str(self.next_cluster) + "\n" + str(current_region) +
"\nclassified as " + str(self.classified_regions[self.next_cluster][1]))
self.next_cluster += 1
region_centroid = Point(*(utils.map.index_to_coordinates(*current_region.get_avg()))+(-0.065,))
dist_to_region = mathemagie.euclidean_distance(Point(0, 0, 0), region_centroid)
#if userdata.yaml.task_type == Task.TASK_5:
# fixture_position = mathemagie.add_point(userdata.yaml.puzzle_fixture.position, Point(0.115, 0.165, 0))
# dist_to_fixture = mathemagie.euclidean_distance(fixture_position, region_centroid)
# if dist_to_fixture < 0.35:
# rospy.logdebug("Region classified as puzzle fixture, skipping")
# return 'mapScanned'
# If the arm cannot move ignore distant regions
# TODO find the best max distance
if not userdata.enable_movement:
rospy.logdebug('Distance of the current region to the arm: %s' % str(dist_to_region))
if dist_to_region > 1.1:
rospy.logwarn('Current region is out of reach. Ignoring it.')
return 'mapScanned'
angle = 1.2
distance = 0.6 + current_region.get_number_of_cells()*0.008
poses = make_scan_pose(region_centroid, distance, angle, n=16)
if not userdata.enable_movement:
poses = utils.manipulation.filter_close_poses(poses)
poses = utils.map.filter_invalid_scan_poses2(region_centroid.x, region_centroid.y, poses)
if userdata.sec_try:
current_pose = utils.manipulation.get_eef_position().pose.position
current_pose.z = 0
region_to_eef = subtract_point(region_centroid, current_pose)
poses.sort(key=lambda pose: abs(get_angle(region_to_eef, subtract_point(Point(pose.pose.position.x, pose.pose.position.y, 0), region_centroid)) - pi/2))
visualize_poses(poses)
if userdata.enable_movement:
# move = utils.manipulation.move_arm_and_base_to
plan = utils.manipulation.plan_arm_and_base_to
else:
# move = utils.manipulation.move_to
plan = utils.manipulation.plan_arm_to
utils.manipulation.blow_up_objects(do_not_blow_up_list=("map"))
for pose in poses:
# utils.manipulation.set_planning_time_arm(2)
if utils.manipulation.move_with_plan_to(plan(pose)):
# utils.manipulation.set_planning_time_arm(5)
userdata.focused_point = region_centroid
rospy.logdebug('Wait for clock')
time.sleep(0.5)
rospy.sleep(2.5)
return 'newImage'
utils.manipulation.blow_down_objects()
return 'mapScanned'
def execute(self, userdata):
rospy.loginfo('Executing state GraspObject')
collision_object_name = userdata.object_to_move.mpe_object.id
rospy.loginfo('Trying to grasp %s' % collision_object_name)
if userdata.enable_movement:
move_to_func = utils.manipulation.move_arm_and_base_to
plan_to_func = utils.manipulation.plan_arm_and_base_to
else:
move_to_func = utils.manipulation.move_to
plan_to_func = utils.manipulation.plan_arm_to
#get the collisionobject out of the planningscene
collision_object = utils.manipulation.get_planning_scene(
).get_collision_object(collision_object_name)
if collision_object is None:
rospy.logwarn("Collision Object " + collision_object_name +
" is not in planningscene.")
return 'objectNotInPlanningscene'
rospy.logdebug("Grasping: " + str(collision_object))
grasp_positions = calculate_grasp_position(
collision_object, utils.manipulation.transform_to)
#filter out some invalid grasps
grasp_positions = utils.manipulation.filter_low_poses(grasp_positions)
if not userdata.enable_movement:
grasp_positions = utils.manipulation.filter_close_poses(
grasp_positions)
if len(grasp_positions) == 0:
rospy.logwarn("No grasppositions found for " +
collision_object_name)
userdata.failed_object = userdata.object_to_move
return 'noGraspPosition'
#sort to try the best grasps first
grasp_positions.sort(cmp=lambda x, y: utils.manipulation.
cmp_pose_stamped(collision_object, x, y))
visualize_poses(grasp_positions)
utils.manipulation.open_gripper()
grasp_positions = [
utils.manipulation.transform_to(grasp) for grasp in grasp_positions
]
utils.manipulation.blow_up_objects(
do_not_blow_up_list=(collision_object_name))
for grasp in grasp_positions:
plan_pre_grasp = plan_to_func(get_pre_grasp(grasp))
if plan_pre_grasp is None:
continue
rospy.logdebug("Plan to pregraspposition found")
utils.manipulation.blow_up_objects(
do_not_blow_up_list=("map", collision_object_name))
plan_to_grasp = plan_to_func(
grasp,
start_state=utils.manipulation.get_end_state(plan_pre_grasp))
if plan_to_grasp is None or not utils.manipulation.move_with_plan_to(
plan_pre_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.sleep(0.5)
if not utils.manipulation.move_with_plan_to(plan_to_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.logdebug("Graspposition taken")
time.sleep(0.5)
rospy.sleep(1)
if not utils.manipulation.close_gripper(
collision_object,
get_fingertip(utils.manipulation.transform_to(grasp))):
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
time.sleep(0.5)
rospy.sleep(1.5)
#calculate the center of mass and weight of the object and call the load object service
com = utils.manipulation.get_center_of_mass(collision_object)
com = utils.manipulation.transform_to(com, "/tcp")
if com is None:
rospy.logwarn("TF failed")
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
density = 1
for obj in userdata.yaml.objects:
if obj.name == collision_object_name:
density = obj.primitive_densities[0]
if isinf(density):
rospy.logerr(
"Infinite density! WTF. setting density to 7850")
density = 7850
utils.manipulation.load_object(
utils.manipulation.calc_object_weight(collision_object,
density),
Vector3(com.point.x, com.point.y, com.point.z))
rospy.loginfo("grasped " + collision_object_name)
#save grasp data for placing
userdata.grasp = grasp
fingertip = get_fingertip(grasp)
fingertip_to_tcp = subtract_point(grasp.pose.position,
fingertip.point)
userdata.dist_to_obj = magnitude(fingertip_to_tcp)
rospy.logdebug("lift object")
the_pre_grasp = get_pre_grasp(grasp)
the_move_to_func = move_to_func(
the_pre_grasp, do_not_blow_up_list=collision_object_name)
if not the_move_to_func:
rospy.logwarn("couldnt lift object. continue anyway")
userdata.failed_object = None
return 'success'
rospy.logwarn("Grapsing failed.")
userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'