def build_allowed_contact_specification(self, box_pose, box_dimensions): msg = AllowedContactSpecification() msg.name = "grasping_object_region" shape = Shape() shape.type = shape.BOX shape.dimensions = box_dimensions msg.shape = shape msg.pose_stamped = box_pose msg.link_names = [ "r_gripper_palm_link", "r_gripper_l_finger_link", "r_gripper_r_finger_link", "r_gripper_l_finger_tip_link", "r_gripper_r_finger_tip_link", "l_gripper_palm_link", "l_gripper_l_finger_link", "l_gripper_r_finger_link", "l_gripper_l_finger_tip_link", "l_gripper_r_finger_tip_link" ] return msg
if not move_arm_to_grasping_joint_pose(joint_names, joint_positions): exit(1) posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction) posture_controller.wait_for_server() rospy.loginfo('connected to gripper posture controller') pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE posture_controller.send_goal(pg) posture_controller.wait_for_result() # define allowed contacts table = segmentation_result.table table_contact = AllowedContactSpecification() table_contact.name = coll_map_res.collision_support_surface_name table_contact.shape = Shape(type=Shape.BOX, dimensions=[abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), 1e-6]) table_contact.pose_stamped = table.pose table_contact.link_names = ['L9_right_finger_link', 'L8_left_finger_link', 'L7_wrist_roll_link', 'L6_wrist_pitch_link'] table_contact.penetration_depth = 0.01 allowed_contacts = []#[table_contact,] # define temporary link paddings gripper_paddings = [LinkPadding(l,0.0) for l in ('L9_right_finger_link', 'L8_left_finger_link')] if not move_arm_to_grasping_joint_pose(ik_solution.joint_state.name, ik_solution.joint_state.position, allowed_contacts, gripper_paddings): exit(1) rospy.sleep(1)