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)
