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
def build_allowed_contact_specification(self, shape, pose, arm_name, penetration_depth, collision_object_name = "collision_map", ): """ Creates a motion_planning_msgs/AllowedContactSpecification message. This can be passed to a move_[left|right]_arm call to allow for contacts. Parameters: name: the name of the regions shape: the shape of the region in the environment. A arm_navigation_msgs/Shape msg pose: the PoseStamped of the space defining the region arm_name: either "right_arm" or "left_arm", the arm which is allowed to collide penetration_depth: the maximum penetration depth allowed for every link """ if arm_name == "right_arm": link_names = ["r_gripper_palm_joint", "r_gripper_l_finger_joint", "r_gripper_l_finger_tip_joint", "r_gripper_led_joint", "r_gripper_motor_accelerometer_joint", "r_gripper_motor_slider_joint", "r_gripper_motor_screw_joint", "r_gripper_r_finger_joint", "r_gripper_r_finger_tip_joint", "r_gripper_joint", "r_gripper_tool_joint", ] else: link_names = ["l_gripper_palm_joint", "l_gripper_l_finger_joint", "l_gripper_l_finger_tip_joint", "l_gripper_led_joint", "l_gripper_motor_accelerometer_joint", "l_gripper_motor_slider_joint", "l_gripper_motor_screw_joint", "l_gripper_r_finger_joint", "l_gripper_r_finger_tip_joint", "l_gripper_joint", "l_gripper_tool_joint", ] collisions = [] for link in link_names: msg = AllowedContactSpecification() msg.name = "collision " + link + " vs " + collision_object_name msg.shape = shape msg.pose_stamped = pose msg.link_names = [link, collision_object_name] msg.penetration_depth = penetration_depth collisions.append(msg) return collisions
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)