def _move_robot_to_post_place_pose(self, robot_pose):
post_place_mat_in_suction_frame = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, -self._post_place_dist],
[0, 0, 0, 1],
])
pos = robot_pose.position
ori = robot_pose.orientation
robot_np_pose = np.array(
[pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w])
robot_mat = gk.quaternion_pose_to_mat(np, robot_np_pose)
post_place_mat_in_world_frame = np.matmul(
robot_mat, post_place_mat_in_suction_frame)
post_place_pose_in_world_frame = gk.mat_to_quaternion_pose(
np, post_place_mat_in_world_frame)
translation = post_place_pose_in_world_frame[0:3]
rotation = post_place_pose_in_world_frame[3:]
self._tf_broadcaster.sendTransform(
translation,
rotation,
self._object_ros_time,
"post_place",
"panda_link0",
)
position = Point(translation[0], translation[1], translation[2])
self.set_end_effector_position_linearly(position, 0.25, 0.25)
def _object_poses_callback(self, object_poses):
for object_pose in object_poses.poses:
object_pose_in_world_frame = object_pose.pose
pos = object_pose_in_world_frame.position
ori = object_pose_in_world_frame.orientation
object_np_pose_in_world_frame = np.array(
[pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w])
object_mat_in_world_frame = gk.quaternion_pose_to_mat(
np, object_np_pose_in_world_frame)
self._object_mats_in_world_frame[
object_pose.class_id] = object_mat_in_world_frame
self._object_pose_msgs_in_world_frame[
object_pose.class_id] = object_pose_in_world_frame
def _pick_poses_callback(self, pick_point_poses):
if len(pick_point_poses.poses) == 0:
return
self._object_ros_time = pick_point_poses.header.stamp
frame_id = pick_point_poses.header.frame_id
# FIXME: current code assumes there are no multiple instances of the same class
self._class_id_to_instance_id = {}
for object_pose in pick_point_poses.poses:
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame_id
pose_stamped.header.stamp = self._object_ros_time
pose_stamped.pose = object_pose.pose
pick_point_pose_in_world_frame = self._tf_listener.transformPose(
"panda_link0", pose_stamped).pose
pos = pick_point_pose_in_world_frame.position
ori = pick_point_pose_in_world_frame.orientation
pick_point_np_pose_in_world_frame = np.array(
[pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w])
self._pick_point_np_poses_in_world_frame[
object_pose.class_id] = pick_point_np_pose_in_world_frame
pick_point_mat_in_world_frame = gk.quaternion_pose_to_mat(
np, pick_point_np_pose_in_world_frame)
self._pick_point_mats_in_world_frame[
object_pose.class_id] = pick_point_mat_in_world_frame
self._class_id_to_instance_id[
object_pose.class_id] = object_pose.instance_id
self._ordered_object_ids_to_grasp = [
object_pose.class_id for object_pose in pick_point_poses.poses
]
ordered_class_names = (ycb_video_dataset.class_names[i]
for i in self._ordered_object_ids_to_grasp)
morefusion.ros.loginfo_blue(
f"Object tree: {' -> '.join(ordered_class_names)}")
def _move_robot_to_place_pose(self, pre_pose_reached, object_to_robot_mat):
place_position = pre_pose_reached.position
place_position.z -= self._pre_placement_z_dist
_, robot_pose = self.set_end_effector_position_linearly(
place_position, 0.1, 0.1)
pos = robot_pose.position
ori = robot_pose.orientation
robot_np_pose = np.array(
[pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w])
object_mat = np.matmul(
gk.quaternion_pose_to_mat(np, robot_np_pose),
np.linalg.inv(object_to_robot_mat),
)
object_np_pose = gk.mat_to_quaternion_pose(np, object_mat)
object_pose = Pose()
object_pose.position.x = object_np_pose[0]
object_pose.position.y = object_np_pose[1]
object_pose.position.z = object_np_pose[2]
object_pose.orientation.x = object_np_pose[3]
object_pose.orientation.y = object_np_pose[4]
object_pose.orientation.z = object_np_pose[5]
object_pose.orientation.w = object_np_pose[6]
return robot_pose, object_pose
def pick_and_place(self):
while not self._all_objects_removed:
# object selection #
# -----------------#
self._object_id_to_grasp = self._choose_next_object_to_grasp()
class_name = ycb_video_dataset.class_names[
self._object_id_to_grasp]
morefusion.ros.loginfo_blue(f"Picking up {class_name}")
self._broadcast_object_pose()
# pose calculation #
# -----------------#
pre_grasp_pose = self._get_pre_grasp_pose()
grasp_pose = self._get_grasp_pose()
post_grasp_pose = self._get_post_grasp_pose()
inv_object_mat = np.linalg.inv(
self._object_mats_in_world_frame[self._object_id_to_grasp])
# Distractors #
# ------------#
self._all_distractors_removed = (
self._check_if_all_distractors_removed())
# move to object #
# ---------------#
self._move_robot_over_table()
self._move_robot_to_pre_grasp_pose(pre_grasp_pose)
robot_pose = self._move_robot_to_grasp_pose(grasp_pose)
# update robot pose #
# ------------------#
pos = robot_pose.position
ori = robot_pose.orientation
robot_np_pose = np.array(
[pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w])
object_to_robot_mat = np.matmul(
inv_object_mat, gk.quaternion_pose_to_mat(np, robot_np_pose))
# move object #
# ------------#
self._suction_grip_object()
self._update_scene_with_grasp()
self._publish_moved()
self._move_robot_to_post_grasp_pose(post_grasp_pose)
self._move_robot_over_table()
if self._all_distractors_removed:
self._move_robot_over_target_box()
# get possible robot poses
robot_poses = self._object_pose_interface.get_robot_poses(
self._object_id_to_grasp,
object_to_robot_mat,
self._in_target_box_position,
)
# filter poses based on face down threshold
filtered_robot_poses = self._filter_robot_poses(robot_poses)
if len(filtered_robot_poses) == 0:
raise Exception(
"Orientation of object in gripper is too difficult for placement in box.\n"
"Consider increasing the angle_from_vertical_limit threshold"
)
# make motion
robot_pose = self._move_robot_to_pre_place_pose(
filtered_robot_poses)
robot_pose, obj_pose = self._move_robot_to_place_pose(
robot_pose, object_to_robot_mat)
self._update_scene_with_placement(obj_pose)
self._release_suction_grip()
self._move_robot_to_post_place_pose(robot_pose)
else:
self._move_robot_over_distractor_box()
self._move_robot_to_drop_pose()
self._update_scene_with_drop()
self._release_suction_grip()
# object logging #
# ---------------#
self._picked_objects.append(self._object_id_to_grasp)
self._all_objects_removed = self._check_if_all_objects_removed()
# reset robot #
# ------------#
self.move_to_overlook_pose()
morefusion.ros.loginfo_blue(f"Completed moving {class_name}")
morefusion.ros.loginfo_blue("Demo completed")
self.move_to_reset_pose()