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 home_pose_cb(ud):
#ud.pose_st = build_pose_stamped_msg('/wam_link0',0.5, 0, 0.13, 0.0, 1.0, 0.0, 0.0)
ud.pose_st = build_pose_stamped_msg('/wam_link0',0.5, 0, 0, 0.0, 1.0, 0.0, 0.0)
return 'finish'
