def create_place_goal(group, target, places):
# create goal
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# configure
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
goal.allow_gripper_support_collision = False
return goal
def create_place_goal(self, group, target, places):
goal = PlaceGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(places)
goal.allowed_planning_time = 3.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def createPlaceGoal(place_pose, group="right_arm_torso", target="_undefined_target_"):
"""Create PlaceGoal with the provided data"""
placeg = PlaceGoal()
placeg.group_name = group
placeg.attached_object_name = target
if type(place_pose) == type(PoseStamped()):
placeg.place_locations = createPlaceLocations(place_pose)
elif type(place_pose) == type(Pose()):
placeg.place_locations = createPlaceLocations(PoseStamped(header=Header(frame_id="base_link"), pose=place_pose))
placeg.allowed_planning_time = 5.0
placeg.planning_options.planning_scene_diff.is_diff = True
placeg.planning_options.planning_scene_diff.robot_state.is_diff = True
placeg.planning_options.plan_only = False
placeg.planning_options.replan = True
placeg.planning_options.replan_attempts = 10
placeg.allow_gripper_support_collision = False
placeg.allowed_touch_objects = ['table'] # TODO: Sometimes refuses to do the movement if this is not set, research about table names returned
#placeg.planner_id
return placeg
def createPlaceGoal(place_pose,
group="right_arm_torso",
target="_undefined_target_"):
"""Create PlaceGoal with the provided data"""
placeg = PlaceGoal()
placeg.group_name = group
placeg.attached_object_name = target
if type(place_pose) == type(PoseStamped()):
placeg.place_locations = createPlaceLocations(place_pose)
elif type(place_pose) == type(Pose()):
placeg.place_locations = createPlaceLocations(
PoseStamped(header=Header(frame_id="base_link"), pose=place_pose))
placeg.allowed_planning_time = 5.0
placeg.planning_options.planning_scene_diff.is_diff = True
placeg.planning_options.planning_scene_diff.robot_state.is_diff = True
placeg.planning_options.plan_only = False
placeg.planning_options.replan = True
placeg.planning_options.replan_attempts = 10
placeg.allow_gripper_support_collision = False
placeg.allowed_touch_objects = [
'table'
] # TODO: Sometimes refuses to do the movement if this is not set, research about table names returned
#placeg.planner_id
return placeg
def createPlaceGoal(place_pose, group="right_arm_torso", target="part"):
""" Create PlaceGoal with the provided data"""
placeg = PlaceGoal()
placeg.group_name = group
placeg.attached_object_name = target
placeg.place_locations = createPlaceLocations(place_pose)
placeg.allowed_planning_time = 5.0
placeg.planning_options.planning_scene_diff.is_diff = True
placeg.planning_options.planning_scene_diff.robot_state.is_diff = True
placeg.planning_options.plan_only = False
placeg.planning_options.replan = True
placeg.planning_options.replan_attempts = 10
placeg.allow_gripper_support_collision = False
placeg.allowed_touch_objects = [
'table'
] # Sometimes refuses to do the movement if this is not set
#placeg.planner_id
return placeg
def createPlaceGoal(place_pose, group="right_arm_torso", target="part"):
""" Create PlaceGoal with the provided data"""
placeg = PlaceGoal()
placeg.group_name = group
placeg.attached_object_name = target
placeg.place_locations = createPlaceLocations(place_pose)
placeg.allowed_planning_time = 5.0
placeg.planning_options.planning_scene_diff.is_diff = True
placeg.planning_options.planning_scene_diff.robot_state.is_diff = True
placeg.planning_options.plan_only = False
placeg.planning_options.replan = True
placeg.planning_options.replan_attempts = 10
placeg.allow_gripper_support_collision = False
placeg.allowed_touch_objects = ['table'] # Sometimes refuses to do the movement if this is not set
#placeg.planner_id
return placeg
def run_drop(self, object_name):
place_goal = PlaceGoal()
place_goal.group_name = "left_arm"
place_goal.attached_object_name = object_name
place_goal.place_eef = True
place_goal.place_locations = self.get_place_locations()
place_goal.allowed_planning_time = 5.0
place_goal.planning_options.planning_scene_diff.is_diff = True
place_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
self.place_client.send_goal(place_goal)
self.place_client.wait_for_result()
place_result = self.place_client.get_result()
print 'Place result: %d' % (place_result.error_code.val, )
if place_result.error_code.val == 1:
return True
else:
return False
def create_place_goal(group, target, places):
# create goal
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# configure
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
goal.allow_gripper_support_collision = False
return goal
def create_place_goal(place_pose,
place_locations,
group='arm_torso',
target='part',
links_to_allow_contact=None):
# create MoveIt place goal with the provided data
place_goal = PlaceGoal()
place_goal.group_name = group
place_goal.attached_object_name = target
place_goal.place_locations = place_locations
place_goal.allowed_planning_time = 15.0
place_goal.planning_options.planning_scene_diff.is_diff = True
place_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
place_goal.planning_options.plan_only = False
place_goal.planning_options.replan = True
place_goal.planning_options.replan_attempts = 50
place_goal.allowed_touch_objects = ['<octomap>']
place_goal.allowed_touch_objects.extend(links_to_allow_contact)
return place_goal
def createPlaceGoal(place_pose, place_locations, group="arm", target="part", links_to_allow_contact=None): """Create PlaceGoal with the provided data""" placeg = PlaceGoal() placeg.group_name = group placeg.attached_object_name = target placeg.place_locations = place_locations placeg.allowed_planning_time = 15.0 placeg.planning_options.planning_scene_diff.is_diff = True placeg.planning_options.planning_scene_diff.robot_state.is_diff = True placeg.planning_options.plan_only = False placeg.planning_options.replan = True placeg.planning_options.replan_attempts = 1 placeg.allowed_touch_objects = ['<octomap>'] placeg.allowed_touch_objects.extend(links_to_allow_contact) return placeg
def run_drop(self, object_name):
place_goal = PlaceGoal()
place_goal.group_name = "left_arm"
place_goal.attached_object_name = object_name
place_goal.place_eef = True
place_goal.place_locations = self.get_place_locations()
place_goal.allowed_planning_time = 5.0
place_goal.planning_options.planning_scene_diff.is_diff = True
place_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
self.place_client.send_goal(place_goal)
self.place_client.wait_for_result()
place_result = self.place_client.get_result()
print 'Place result: %d' % (place_result.error_code.val,)
if place_result.error_code.val == 1:
return True
else:
return False
def createPlaceGoal(place_pose, place_locations, group="arm_torso", target="part", links_to_allow_contact=None): """Create PlaceGoal with the provided data""" placeg = PlaceGoal() placeg.group_name = group placeg.attached_object_name = target placeg.place_locations = place_locations placeg.allowed_planning_time = 15.0 placeg.planning_options.planning_scene_diff.is_diff = True placeg.planning_options.planning_scene_diff.robot_state.is_diff = True placeg.planning_options.plan_only = False placeg.planning_options.replan = True placeg.planning_options.replan_attempts = 1 placeg.allowed_touch_objects = ['<octomap>'] placeg.allowed_touch_objects.extend(links_to_allow_contact) return placeg
def place(self, name, locations, wait=True, **kwargs):
# Check arguments
supported_args = ("allow_gripper_support_collision",
"allowed_touch_objects", "goal_is_eef", "plan_only",
"planner_id", "planning_scene_diff", "planning_time",
"support_name")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("place: unsupported argument: %s", arg)
# Create goal
g = PlaceGoal()
# 1. Name of ARM planning group
g.group_name = self._group
# 2. Name of attached object to place
g.attached_object_name = name
# 3. Possible locations to place
g.place_locations = locations
# 4. If true, using eef pose (same as pick)
try:
g.place_eef = kwargs["goal_is_eef"]
except KeyError:
g.place_eef = False
# 5. Fill in support surface
try:
g.support_surface_name = kwargs["support_name"]
except KeyError:
g.support_surface_name = ""
# 6. Can gripper contact the support surface (named above)
try:
g.allow_gripper_support_collision = kwargs[
"allow_gripper_support_collision"]
except KeyError:
g.allow_gripper_support_collision = True
# 8. Fill in path_constraints
# 9. Fill in planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 10. List of obstacles that we are allowed to touch
try:
g.allowed_touch_objects = kwargs["allowed_touch_objects"]
except KeyError:
g.allowed_touch_objects = list()
# 11. Planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.allowed_planning_time
# 12. Planning options
try:
g.planning_options.planning_scene_diff = kwargs[
"planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
g.planning_options.plan_only = self._plan_only
self._place_action.send_goal(g)
if wait:
self._place_action.wait_for_result()
return self._place_action.get_result()
else:
return None
def place(self, name, locations, wait=True, **kwargs):
# Check arguments
supported_args = ("allow_gripper_support_collision",
"allowed_touch_objects",
"goal_is_eef",
"plan_only",
"planner_id",
"planning_time",
"support_name")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("place: unsupported argument: %s", arg)
# Create goal
g = PlaceGoal()
# 1. Name of ARM planning group
g.group_name = self._group
# 2. Name of attached object to place
g.attached_object_name = name
# 3. Possible locations to place
g.place_locations = locations
# 4. If true, using eef pose (same as pick)
try:
g.place_eef = kwargs["goal_is_eef"]
except KeyError:
g.place_eef = False
# 5. Fill in support surface
try:
g.support_surface_name = kwargs["support_name"]
except KeyError:
g.support_surface_name = ""
# 6. Can gripper contact the support surface (named above)
try:
g.allow_gripper_support_collision = kwargs["allow_gripper_support_collision"]
except KeyError:
g.allow_gripper_support_collision = True
# 8. Fill in path_constraints
# 9. Fill in planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 10. List of obstacles that we are allowed to touch
try:
g.allowed_touch_objects = kwargs["allowed_touch_objects"]
except KeyError:
g.allowed_touch_objects = list()
# 11. Planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.allowed_planning_time
# 12. Planning options
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
g.planning_options.plan_only = self._plan_only
self._place_action.send_goal(g)
if wait:
self._place_action.wait_for_result()
return self._place_action.get_result()
else:
return None
