def __init__(self):
self.config = PomdpGraspConfig()
self.config.place_point = build_pose_stamped_msg('/wam_link0',0.5, 0, 0.33, 0.0, 1.0, 0.0, 0.0)
self.config.grabbing_zone = self.config.BOTH_ZONES
# pre-grasp is relative to real_grasp and do not change
self.config.approach_config.pre_grasp_modifier = build_transform_msg(0, 0, -0.1, 0, 0, 0, 1)
self.config.approach_config.grasp_modifier_used = 0
# always use GSO at the begging of grasp
self.config.fingers_grasp_configs.append("GSTO")
def get_instance(self, magic_number):
if (magic_number > 999) or (magic_number < 0):
rospy.logfatal("Pomdp magic number is out of ranges")
alg_id = magic_number / 100
approach_id = (magic_number - (100 * alg_id)) / 10
grasp_id = (magic_number - (100 * alg_id) - (10 * approach_id))
if (alg_id == 0):
self.config.best_pose_algorithm_id = self.config.MAX_HEIGHT_ALG
elif (alg_id == 1):
self.config.best_pose_algorithm_id = self.config.MAX_WRINKLE_ALG
elif (alg_id == 2):
self.config.best_pose_algorithm_id = self.config.FUSION_ALG
else:
rospy.logfatal("Magic number alg_is it out of range")
if (approach_id == self.config.APPROACH_TOP_DEEP):
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0, 0, -0.03, 1, 0, 0, 0))
elif (approach_id == self.config.APPROACH_TOP_SURFACE):
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0, 0, 0.05, 1, 0, 0, 0))
elif (approach_id == self.config.APPROACH_SIDE_DEEP):
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0.03, 0, 0, 0, 0.7071, 0, 0.7071))
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0, 0.03, 0, -0.5, 0.5, 0.5, 0.5))
self.config.approach_config.grasp_modifiers.append(build_transform_msg(-0.03, 0, 0, -0.7071, 0, 0.7071,0))
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0, -0.03, 0, 0.5, 0.5, -0.5, 0.5))
elif (approach_id == self.config.APPROACH_SIDE_SURFACE):
self.config.approach_config.grasp_modifiers.append(build_transform_msg(-0.05, 0, 0, 0, 0.7071, 0, 0.7071))
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0, -0.05, 0, -0.5, 0.5, 0.5, 0.5))
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0.05, 0, 0, 0, -0.7071, 0, 0.7071))
self.config.approach_config.grasp_modifiers.append(build_transform_msg(0, 0.05, 0, 0.5, 0.5, -0.5, 0.5))
else:
rospy.logfatal("Magic number approach is out of range")
if (grasp_id == 0):
pass
elif (grasp_id == 1):
self.config.fingers_grasp_configs.append("GM 7000")
self.config.fingers_grasp_configs.append("GTC")
elif (grasp_id == 2):
self.config.fingers_grasp_configs.append("GM 10000")
self.config.fingers_grasp_configs.append("GTC")
elif (grasp_id == 3):
self.config.fingers_grasp_configs.append("SM 500")
self.config.fingers_grasp_configs.append("GTC")
elif (grasp_id == 4):
self.config.fingers_grasp_configs.append("SM 500")
self.config.fingers_grasp_configs.append("GM 7000")
self.config.fingers_grasp_configs.append("GTC")
elif (grasp_id == 5):
self.config.fingers_grasp_configs.append("STC")
self.config.fingers_grasp_configs.append("GTC")
elif (grasp_id == 6):
self.config.fingers_grasp_configs.append("SM 1500")
self.config.fingers_grasp_configs.append("3TC")
self.config.fingers_grasp_configs.append("GTC")
else:
rospy.logfatal("Magic number grasp is out of range")
#closing grasp config
#self.config.fingers_grasp_configs.append("GTC")
return self.config
