Public Member Functions | Public Attributes
pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager Class Reference

Manager for pick and place actions. More...

Inheritance diagram for pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager:
Inheritance graph
[legend]

List of all members.

Public Member Functions

def __init__
def attach_object
 attach the object in the hand to the gripper (right = 0, left = 1)
def call_find_cluster_bounding_box
 call find_cluster_bounding_box to get the bounding box for a cluster
def call_reactive_place
 call the reactive place service for the appropriate arm
def call_tabletop_detection
 call tabletop object detection and collision_map_processing (detects table/objects and adds them to collision map)
def check_arms_to_use
def check_grasp_successful
 is an object in the hand? (is the gripper not closed all the way?)
def check_joint_controllers
 make sure the joint controllers are on
def check_preempted
 check for a preempt request (overload this for your application)
def choose_place_location
 choose a new location to try to place an object held by whicharm (right = 0, left = 1) if override_pose >=0, then pick an override pose from front to back
def close_gripper
 close the gripper (whicharm is 0 for right, 1 for left)
def combine_point_cloud_objects
 adds the point cloud of object2 to object1 and updates the bounding box
def count_objects
 how many relevant objects did we detect?
def create_gripper_link_padding
 return a LinkPadding list with the gripper touch links for whicharm (right = 0, left = 1) and padding pad
def detach_and_add_back_object
 detach and add the object back to the collision map
def detach_object
 detach the object in the hand from the gripper (right = 0, left = 1)
def detect_and_pick_up_object
 detect objects and try to pick up the nearest one
def draw_object_pose
 draw a visualization box (bounding or at-the-base) for an object at a PoseStamped
def draw_place_area
 draw the current place area as a set of boxes
def fill_in_taken_place_grid_spots
 fills in place grid spots that are already taken by objects seen by the last tabletop detection
def find_cross_direction
 return the sign of only the (x,y) components of (p1-p2)x(p3-p2) for convex hull inclusion testing p1, p2, and p3 are lists of coords
def find_height_above_table
 find the height of an object pose above a detected table plane
def find_IK_solution
 find an IK solution for a pose mat
def find_table
 point the head to find the front edge of the table and detect
def get_keep_object_level_constraint
 create an OrientationConstraint to keep the object level
def grasp_object
 tell the grasp action to grasp an object (whicharm: right = 0, left = 1)
def grasp_object_and_check_success
 grasp the object and check if there's something in the hand
def input_object_num
 get the number of an object to pick up from the user
def input_side
 get which arm/side to use from the user
def keyboard_extensions
 add extensions to the keyboard interface here, return 1 to continue and 0 to go on down the possibilities
def keyboard_interface
 keyboard interface for starting up the demo and overriding with manual commands
def keypause
 pause for input
def move_arm_to_side
 move whicharm off to the side
def move_arm_to_side_open_loop
 move whicharm off to the side open-loop
def move_cartesian_step
 move to a nearby Cartesian pose using the Cartesian controllers
def open_gripper
 open the gripper (whicharm is 0 for right, 1 for left)
def pick_side
def pick_up_nearest_object
 pick up the object_num-th nearest feasible object (tries each one in turn) arms_to_try indicates which arms to try in what order (0=right, 1=left)
def pick_up_object_near_point
 pick up the object nearest to a PointStamped with whicharm (0=right, 1=left)
def place_grid_location
 return the location of a point in the place grid
def place_grid_pose
 return the pose of the object at a place grid location
def place_object
 tell the place service to place an object held by whicharm at a particular location (right = 0, left = 1) expressed as a PoseStamped
def place_object_override
 place the object open-loop, shoving other objects aside
def point_head
 point the head (really the narrow-stereo camera frame) at a location
def point_head_at_place_rect
 point the head at the place rectangle
def point_to_box_dist
 distance from a point (2-list) to a bounding box (projected onto table plane) returns 0 if the point is inside; positive if it's outside
def pose_to_narrow_stereo_center_dist
 distance between the stereo camera's center ray and a pose_stamped's center
def print_keyboard_extensions
 print instructions for using extensions to the keyboard interface here
def print_object_list
 print the list of possible objects to grasp
def put_down_object
 put down an object held in whicharm (right = 0, left = 1)
def refine_if_not_centered
 refine the view of the object, if it's too far from the center of the narrow stereo center
def refine_object_detection
 refine an object detection by pointing the head at it (input the object's position on the table)
def remove_object
 remove an object from the collision map
def reset_collision_map
 reset the collision map and take a new one (without tabletop or objects)
def reset_place_grid_temporary_collisions
 reset all the -1s (temporary collision entries) in the place grid
def return_current_pose_as_list
 return the current pos and rot of the wrist for whicharm as a 7-list (pos, quaternion rot)
def rotate_pose
 rotate the pose_stamped about the base-link z-axis by rotation rad but keep the position the same
def set_place_area
 set a rectangle centered at rect_pose_stamped of dims (x, y) to be the desired place area
def shift_place_pose_height
 shift a place pose to be above the current table plane
def throw_exception
 saw a serious error (problem with service call), overload this for your application
def transform_place_rect_pose
 convert the place rectangle pose to base_link frame if it isn't already (returns pose, not pose_stamped)
def try_hard_to_move_joint
 try to move to a set of joint angles using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop
def try_hard_to_move_joint_open_loop
 move open-loop through trajectory
def try_hard_to_move_pose
 try to move to a pose using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop (either to the IK solution if there is one, or using the Cartesian controllers if there isn't)
def try_to_move_constrained
 try to move to a location while constraining the gripper to remain approximately level
def update_table_info
 use the table detection to update the table information if adjust_place_rectangle is 1, adjusts a place rectangle on the table (assumed base_link frame) with the new table params

Public Attributes

 additional_tables
 arm_above_and_to_side_angles
 arm_dict
 arm_to_side_angles
 arms_to_use
 arms_to_use_list
 bounding_box_srv
 cms
 collision_map_interface
 collision_map_processing_name
 collision_map_processing_srv
 collision_support_surface_name
 detected_objects
 detected_table
 draw_functions
 find_bounding_box_name
 grasper_detect_name
 grasper_detect_srv
 grasper_grasp_action_client
 grasper_grasp_name
 grasper_place_action_client
 grasper_place_name
 head_action_client
 held_objects
 original_poses
 place_grid
 place_grid_resolution
 place_override_approach_offset
 place_rect_dims
 place_rect_pose_stamped
 reactive_place_clients
 result_code_dict
 stereo_camera_frame
 table_front_edge_x
 table_height
 tabletop_detection_result_dict
 tf_listener
 use_slip_controller
 use_slip_detection

Detailed Description

Manager for pick and place actions.

Definition at line 97 of file pick_and_place_manager.py.


Constructor & Destructor Documentation

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.__init__ (   self,
  use_slip_controller = 0,
  use_slip_detection = 0 
)

Member Function Documentation

attach the object in the hand to the gripper (right = 0, left = 1)

Definition at line 810 of file pick_and_place_manager.py.

call find_cluster_bounding_box to get the bounding box for a cluster

Definition at line 315 of file pick_and_place_manager.py.

call the reactive place service for the appropriate arm

Definition at line 797 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.call_tabletop_detection (   self,
  update_table = 0,
  clear_attached_objects = 1,
  replace_table_in_collision_map = 1,
  update_place_rectangle = 0,
  num_models = 0 
)

call tabletop object detection and collision_map_processing (detects table/objects and adds them to collision map)

Reimplemented in pr2_pick_and_place_demos.pick_and_place_demo.PickAndPlaceDemo.

Definition at line 330 of file pick_and_place_manager.py.

Definition at line 682 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.check_grasp_successful (   self,
  whicharm,
  min_gripper_opening = .0021,
  max_gripper_opening = .1 
)

is an object in the hand? (is the gripper not closed all the way?)

Definition at line 1602 of file pick_and_place_manager.py.

make sure the joint controllers are on

Definition at line 439 of file pick_and_place_manager.py.

check for a preempt request (overload this for your application)

Definition at line 1804 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.choose_place_location (   self,
  whicharm,
  override_pose = -1,
  z_offset = 0 
)

choose a new location to try to place an object held by whicharm (right = 0, left = 1) if override_pose >=0, then pick an override pose from front to back

Definition at line 1009 of file pick_and_place_manager.py.

close the gripper (whicharm is 0 for right, 1 for left)

Definition at line 1479 of file pick_and_place_manager.py.

adds the point cloud of object2 to object1 and updates the bounding box

Definition at line 426 of file pick_and_place_manager.py.

how many relevant objects did we detect?

Definition at line 1640 of file pick_and_place_manager.py.

return a LinkPadding list with the gripper touch links for whicharm (right = 0, left = 1) and padding pad

Definition at line 580 of file pick_and_place_manager.py.

detach and add the object back to the collision map

Definition at line 822 of file pick_and_place_manager.py.

detach the object in the hand from the gripper (right = 0, left = 1)

Definition at line 816 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.detect_and_pick_up_object (   self,
  point_head_loc,
  frame = 'base_link',
  arms_to_try = [0,
  constrained = 0 
)

detect objects and try to pick up the nearest one

Definition at line 1612 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.draw_object_pose (   self,
  object_info,
  pose_stamped,
  color,
  id,
  duration = 300. 
)

draw a visualization box (bounding or at-the-base) for an object at a PoseStamped

Definition at line 997 of file pick_and_place_manager.py.

draw the current place area as a set of boxes

Definition at line 923 of file pick_and_place_manager.py.

fills in place grid spots that are already taken by objects seen by the last tabletop detection

Definition at line 542 of file pick_and_place_manager.py.

return the sign of only the (x,y) components of (p1-p2)x(p3-p2) for convex hull inclusion testing p1, p2, and p3 are lists of coords

Definition at line 536 of file pick_and_place_manager.py.

find the height of an object pose above a detected table plane

Definition at line 291 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.find_IK_solution (   self,
  whicharm,
  pose_mat,
  start_angles,
  collision_aware = 1 
)

find an IK solution for a pose mat

Definition at line 673 of file pick_and_place_manager.py.

point the head to find the front edge of the table and detect

Definition at line 307 of file pick_and_place_manager.py.

create an OrientationConstraint to keep the object level

Definition at line 1700 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.grasp_object (   self,
  object,
  whicharm = None,
  use_reactive_grasp = 1,
  use_slip_detection = 0 
)

tell the grasp action to grasp an object (whicharm: right = 0, left = 1)

Definition at line 449 of file pick_and_place_manager.py.

grasp the object and check if there's something in the hand

Definition at line 1305 of file pick_and_place_manager.py.

get the number of an object to pick up from the user

Definition at line 1821 of file pick_and_place_manager.py.

get which arm/side to use from the user

Definition at line 1843 of file pick_and_place_manager.py.

add extensions to the keyboard interface here, return 1 to continue and 0 to go on down the possibilities

Reimplemented in pr2_pick_and_place_demos.pick_and_place_demo.PickAndPlaceDemo.

Definition at line 1869 of file pick_and_place_manager.py.

keyboard interface for starting up the demo and overriding with manual commands

Definition at line 1900 of file pick_and_place_manager.py.

pause for input

Definition at line 1814 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.move_arm_to_side (   self,
  whicharm,
  try_constrained = 0 
)

move whicharm off to the side

Definition at line 1658 of file pick_and_place_manager.py.

move whicharm off to the side open-loop

Definition at line 1719 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.move_cartesian_step (   self,
  whicharm,
  pose,
  timeout = 10.0,
  settling_time = 3.0,
  blocking = 0 
)

move to a nearby Cartesian pose using the Cartesian controllers

Definition at line 1592 of file pick_and_place_manager.py.

open the gripper (whicharm is 0 for right, 1 for left)

Definition at line 1474 of file pick_and_place_manager.py.

Definition at line 1888 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.pick_up_nearest_object (   self,
  object_num = 0,
  arms_to_try = [0 
)

pick up the object_num-th nearest feasible object (tries each one in turn) arms_to_try indicates which arms to try in what order (0=right, 1=left)

Definition at line 1443 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.pick_up_object_near_point (   self,
  point_stamped,
  whicharm,
  refine_view = True 
)

pick up the object nearest to a PointStamped with whicharm (0=right, 1=left)

Definition at line 1727 of file pick_and_place_manager.py.

return the location of a point in the place grid

Definition at line 942 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.place_grid_pose (   self,
  xind,
  yind,
  whicharm,
  add_noise = 0 
)

return the pose of the object at a place grid location

Definition at line 959 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.place_object (   self,
  whicharm,
  pose,
  padding = .05,
  constrained = False 
)

tell the place service to place an object held by whicharm at a particular location (right = 0, left = 1) expressed as a PoseStamped

Definition at line 597 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.place_object_override (   self,
  whicharm,
  pose,
  z_dist = .1,
  use_joint_open_loop = 0 
)

place the object open-loop, shoving other objects aside

Definition at line 693 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.point_head (   self,
  point,
  frame,
  pause = 1 
)

point the head (really the narrow-stereo camera frame) at a location

Definition at line 851 of file pick_and_place_manager.py.

point the head at the place rectangle

Definition at line 1089 of file pick_and_place_manager.py.

distance from a point (2-list) to a bounding box (projected onto table plane) returns 0 if the point is inside; positive if it's outside

Definition at line 514 of file pick_and_place_manager.py.

distance between the stereo camera's center ray and a pose_stamped's center

Definition at line 1412 of file pick_and_place_manager.py.

print instructions for using extensions to the keyboard interface here

Reimplemented in pr2_pick_and_place_demos.pick_and_place_demo.PickAndPlaceDemo.

Definition at line 1864 of file pick_and_place_manager.py.

print the list of possible objects to grasp

Definition at line 1773 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.put_down_object (   self,
  whicharm,
  max_place_tries = None,
  use_place_override = 0,
  move_to_side = True,
  constrained = False,
  update_table = True,
  update_place_rectangle = True,
  point_head = True 
)

put down an object held in whicharm (right = 0, left = 1)

Definition at line 1107 of file pick_and_place_manager.py.

refine the view of the object, if it's too far from the center of the narrow stereo center

Definition at line 1424 of file pick_and_place_manager.py.

refine an object detection by pointing the head at it (input the object's position on the table)

Definition at line 1393 of file pick_and_place_manager.py.

remove an object from the collision map

Definition at line 837 of file pick_and_place_manager.py.

reset the collision map and take a new one (without tabletop or objects)

Definition at line 1484 of file pick_and_place_manager.py.

reset all the -1s (temporary collision entries) in the place grid

Definition at line 1068 of file pick_and_place_manager.py.

return the current pos and rot of the wrist for whicharm as a 7-list (pos, quaternion rot)

Definition at line 1858 of file pick_and_place_manager.py.

rotate the pose_stamped about the base-link z-axis by rotation rad but keep the position the same

Definition at line 1077 of file pick_and_place_manager.py.

set a rectangle centered at rect_pose_stamped of dims (x, y) to be the desired place area

Definition at line 915 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.shift_place_pose_height (   self,
  object,
  place_pose,
  table,
  z_offset 
)

shift a place pose to be above the current table plane

Definition at line 271 of file pick_and_place_manager.py.

saw a serious error (problem with service call), overload this for your application

Reimplemented in pr2_pick_and_place_demos.pick_and_place_demo.PickAndPlaceDemo.

Definition at line 1809 of file pick_and_place_manager.py.

convert the place rectangle pose to base_link frame if it isn't already (returns pose, not pose_stamped)

Definition at line 905 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.try_hard_to_move_joint (   self,
  whicharm,
  trajectory,
  max_tries = 5,
  use_open_loop = 1 
)

try to move to a set of joint angles using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop

Definition at line 1490 of file pick_and_place_manager.py.

move open-loop through trajectory

Definition at line 1521 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.try_hard_to_move_pose (   self,
  whicharm,
  pose_stamped,
  max_tries = 3,
  use_joint_open_loop = 0,
  use_cartesian = 0,
  try_constrained = 0 
)

try to move to a pose using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop (either to the IK solution if there is one, or using the Cartesian controllers if there isn't)

Definition at line 1532 of file pick_and_place_manager.py.

def pr2_pick_and_place_demos.pick_and_place_manager.PickAndPlaceManager.try_to_move_constrained (   self,
  whicharm,
  constraint,
  max_tries = 3,
  start_angles = None,
  location = None,
  frame_id = 'torso_lift_link' 
)

try to move to a location while constraining the gripper to remain approximately level

Definition at line 1645 of file pick_and_place_manager.py.

use the table detection to update the table information if adjust_place_rectangle is 1, adjusts a place rectangle on the table (assumed base_link frame) with the new table params

Reimplemented in pr2_pick_and_place_demos.pick_and_place_demo.PickAndPlaceDemo.

Definition at line 873 of file pick_and_place_manager.py.


Member Data Documentation

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.

Definition at line 100 of file pick_and_place_manager.py.


The documentation for this class was generated from the following file:


pr2_pick_and_place_demos
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Mon Oct 6 2014 12:36:51