Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Commit

Permalink
Revert "New Pick Cuboids Objectives (#73)"
Browse files Browse the repository at this point in the history
This reverts commit 85b5be9.
  • Loading branch information
sea-bass committed Jul 28, 2023
1 parent 85b5be9 commit 2822082
Show file tree
Hide file tree
Showing 6 changed files with 19 additions and 83 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,18 @@ FindSingularCuboids:
crop_box_size_y: 1.5
crop_box_size_z: 0.5

SetupMTCApproachGrasp:
SetupMTCPickCuboid:
# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

approach_distance: 0.15
lift_distance: 0.1

# Set to true to make the robot keep the object within view of its camera while reaching to grasp it
enforce_visibility_constraint: true
Expand All @@ -38,13 +42,6 @@ SetupMTCApproachGrasp:
sensor_z_offset: 0.02
target_diameter: 0.0 # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.

SetupMTCGenerateCuboidGrasps:
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

# Grasp candidate configs
grasp_candidate_config:
generate_x_axis_grasps: true
Expand Down Expand Up @@ -80,14 +77,3 @@ SetupMTCGenerateCuboidGrasps:
roll: 0.0
pitch: 0.0
yaw: 1.57

SetupMTCRetractFromGrasp:
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"

# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
approach_distance: 0.15
lift_distance: 0.1
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,18 @@ FindSingularCuboids:
crop_box_size_y: 0.75
crop_box_size_z: 0.5

SetupMTCPickApproachGrasp:
SetupMTCPickCuboid:
# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

approach_distance: 0.15
lift_distance: 0.1

# Set to true to make the robot keep the object within view of its camera while reaching to grasp it
enforce_visibility_constraint: true
Expand All @@ -38,13 +42,6 @@ SetupMTCPickApproachGrasp:
sensor_z_offset: 0.02
target_diameter: 0.0 # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.

SetupMTCGenerateCuboidGrasps:
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

# Grasp candidate configs
grasp_candidate_config:
generate_x_axis_grasps: true
Expand Down Expand Up @@ -81,17 +78,6 @@ SetupMTCGenerateCuboidGrasps:
pitch: 0.0
yaw: 1.57

SetupMTCRetractFromGrasp:
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"

# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
approach_distance: 0.15
lift_distance: 0.1

IsConstraintSatisfied:
camera_optical_frame_id: "wrist_mounted_camera_color_optical_frame"
camera_field_of_view_angle: 1.0472 # radians: approx. 60 degrees, which is narrower than the 70 degree diagonal FOV of the D415 camera
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,7 @@
<!-- Set up an MTC task to pick the cuboid and then plan the task. -->
<Action ID="InitializeMTCTask" task_id="looping_pick_and_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
<Action ID="SetupMTCCurrentState" task="{task}"/>
<Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
<Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
<Action ID="SetupMTCRetractFromGrasp" parameters="{parameters}" target_object="{cuboid_object}" task="{task}"/>
<Action ID="SetupMTCPickCuboid" cuboid_object="{cuboid_object}" task="{task}" parameters="{parameters}"/>
<Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>

<!-- Execute the planned motion. -->
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,18 @@ FindSingularCuboids:
crop_box_size_y: 0.75
crop_box_size_z: 0.5

SetupMTCApproachGrasp:
SetupMTCPickCuboid:
# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

approach_distance: 0.15
lift_distance: 0.1

# Set to true to make the robot keep the object within view of its camera while reaching to grasp it
enforce_visibility_constraint: true
Expand All @@ -39,14 +42,6 @@ SetupMTCApproachGrasp:
sensor_z_offset: 0.02
target_diameter: 0.0 # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration.


SetupMTCGenerateCuboidGrasps:
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

# Grasp candidate configs
grasp_candidate_config:
generate_x_axis_grasps: true
Expand Down Expand Up @@ -82,14 +77,3 @@ SetupMTCGenerateCuboidGrasps:
roll: 0.0
pitch: 0.0
yaw: 1.57

SetupMTCRetractFromGrasp:
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"

# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
approach_distance: 0.15
lift_distance: 0.1
4 changes: 1 addition & 3 deletions src/picknik_ur_site_config/objectives/pick_place_object.xml
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,7 @@
<!-- Set up an MTC task to pick the cuboid and then plan the task. -->
<Action ID="InitializeMTCTask" task_id="pick_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
<Action ID="SetupMTCCurrentState" task="{task}"/>
<Action ID="SetupMTCApproachGrasp" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
<Action ID="SetupMTCGenerateCuboidGrasps" parameters="{parameters}" target_object="{cuboid_object}" monitored_stage="{monitored_stage}" task="{task}"/>
<Action ID="SetupMTCRetractFromGrasp" parameters="{parameters}" target_object="{cuboid_object}" task="{task}"/>
<Action ID="SetupMTCPickCuboid" cuboid_object="{cuboid_object}" task="{task}" parameters="{parameters}"/>
<Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>

<!-- Send trajectory preview to user only if this is the first time trying to pick the object. -->
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,8 @@ FindSingularCuboids:
crop_box_size_y: 0.75
crop_box_size_z: 0.5

SetupMTCApproachGrasp:
SetupMTCPickCuboid:
# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
Expand All @@ -32,6 +33,7 @@ SetupMTCApproachGrasp:
ui_grasp_link: "manual_grasp_link"

approach_distance: 0.15
lift_distance: 0.1

# Set to true to make the robot keep the object within view of its camera while reaching to grasp it
enforce_visibility_constraint: true
Expand All @@ -49,13 +51,6 @@ SetupMTCApproachGrasp:
# Number of samples to generate for each quadrant of the cuboid.
samples_per_quadrant: 3

SetupMTCGenerateCuboidGrasps:
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"
ui_grasp_link: "manual_grasp_link"

# Grasp data configs
grasp_data:
# See https://ros-planning.github.io/moveit_tutorials/_images/finger_gripper_explanation.jpg for description of the following parameters
Expand Down Expand Up @@ -83,17 +78,6 @@ SetupMTCGenerateCuboidGrasps:
pitch: 0.0
yaw: 1.57

SetupMTCRetractFromGrasp:
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
hand_frame_name: "manual_grasp_link"
end_effector_closed_pose_name: "close"

# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
approach_distance: 0.15
lift_distance: 0.1

IsConstraintSatisfied:
camera_optical_frame_id: "wrist_mounted_camera_color_optical_frame"
camera_field_of_view_angle: 1.0472 # radians: approx. 60 degrees, which is narrower than the 70 degree diagonal FOV of the D415 camera
Expand Down

0 comments on commit 2822082

Please sign in to comment.