示例#1
0
    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
示例#2
0
 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)