def run_pick(self, object_name, grasps):
pickup_goal = PickupGoal()
pickup_goal.target_name = object_name
pickup_goal.group_name = "left_arm"
pickup_goal.possible_grasps = grasps
# Augh. I *think* that this only handles if invalidated during execution, not during planning.
pickup_goal.planning_options.replan = True
pickup_goal.planning_options.replan_attempts = 5
pickup_goal.planning_options.replan_delay = 3.0
pickup_goal.planning_options.planning_scene_diff.is_diff = True
pickup_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# commenting these out b/c not allowed by the moveit_commander interface
#pickup_goal.support_surface_name = "table"
#pickup_goal.allow_gripper_support_collision = True
pickup_goal.allowed_planning_time = 5.0
# TODO: this shouldn't be required, I dont' think!
#pickup_goal.allowed_touch_objects.append(userdata.object_name)
# THIS IS THE ACTIONLIB APPROACH
# Fill in the goal here
self.pickup_client.send_goal(pickup_goal)
self.pickup_client.wait_for_result()
pickup_result = self.pickup_client.get_result()
print 'Pickup result: %d' % (pickup_result.error_code.val,)
if pickup_result.error_code.val == 1:
return True
else:
return False
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
Create a goal for picking up the grasping object创建一个捡起抓取物体的目标
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group #规划的组
goal.target_name = target #要拾取的对象的名称（
#可能使用的抓握列表。 必须至少填写一个掌握
goal.possible_grasps.extend(grasps)
#障碍物的可选清单，我们掌握有关语义的信息，可以在抓取过程中触摸/推动/移动这些障碍物； 注意：如果使用对象名称“ all”，则在进近和提升过程中禁止与所有对象发生碰撞。
goal.allowed_touch_objects.append(target)
#如果没有可用的名称，则在碰撞图中支撑表面（例如桌子）的名称可以留空
goal.support_surface_name = self._table_object_name
# 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
"""
print("----goal-------test-----")
print(goal)
print("----goal---end------test---")
"""
rospy.sleep(0.5)
print("-goal---sleep----end-----")
return goal
def run_pick(self, object_name, grasps):
pickup_goal = PickupGoal()
pickup_goal.target_name = object_name
pickup_goal.group_name = "left_arm"
pickup_goal.possible_grasps = grasps
# Augh. I *think* that this only handles if invalidated during execution, not during planning.
pickup_goal.planning_options.replan = True
pickup_goal.planning_options.replan_attempts = 5
pickup_goal.planning_options.replan_delay = 3.0
pickup_goal.planning_options.planning_scene_diff.is_diff = True
pickup_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# commenting these out b/c not allowed by the moveit_commander interface
pickup_goal.support_surface_name = "table"
pickup_goal.allow_gripper_support_collision = True
pickup_goal.allowed_planning_time = 5.0
# TODO: this shouldn't be required, I dont' think!
#pickup_goal.allowed_touch_objects.append(userdata.object_name)
# THIS IS THE ACTIONLIB APPROACH
# Fill in the goal here
self.pickup_client.send_goal(pickup_goal)
self.pickup_client.wait_for_result()
pickup_result = self.pickup_client.get_result()
print 'Pickup result: %d' % (pickup_result.error_code.val, )
if pickup_result.error_code.val == 1:
return True
else:
return False
def _create_pickup_goal(self, group, target, grasps):
"""
创建一个MoveIt！ 接送目标
创建一个捡起抓取物体的目标
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# 配置目标计划选项:
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
print "-------------test9------------------"
return goal
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# 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_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# 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_pickup_goal(group=PLANNING_GROUP, target=PICK_OBJECT, grasp_pose=PoseStamped(), possible_grasps=[]):
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
# configure
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
return pug
def pick(self, object_name, grasp, support_name = '', planner = 'BKPIECEkConfigDefault'):
goal = PickupGoal()
goal.target_name = object_name
goal.group_name = self.name
goal.end_effector = '{0}_gripper'.format(self.side)
goal.allowed_planning_time = 50
goal.support_surface_name = support_name # @TODO Anadir este parametro con info del shelfdetector
goal.planner_id = planner
goal.allow_gripper_support_collision = True
goal.attached_object_touch_links = ['bender/l_int_finger_link','bender/l_ext_finger_link','bender/l_wrist_pitch_link']
goal.allowed_touch_objects = ['pringles']
#goal.path_constraints = path_constraints
goal.possible_grasps = grasp
self.pick_client.send_goal(goal)
def createPickupGoal(target, possible_grasps, group="right_arm_torso"):
"""Create a PickupGoal with the provided data."""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
#pug.attached_object_touch_links = ['arm_right_5_link', "hand_right_grasping_frame"]
#pug.allowed_touch_objects.append(target)
#pug.attached_object_touch_links.append('all')
return pug # Hehe, we return a dog
def createPickupGoal(group="right_arm",
target="part",
grasp_pose=PoseStamped()):
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
# Create grasp to append
grsp = createGrasp(grasp_pose)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.allowed_touch_objects.append("all")
pug.allowed_touch_objects.append("part")
return pug
def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"]
pug.allowed_touch_objects.append(target)
#pug.attached_object_touch_links.append('all')
return pug
def createPickupGoal(group="right_arm_torso", target="part", grasp_pose=PoseStamped()):
pug = PickupGoal() # haha goal is a dog
pug.target_name = target
pug.group_name = group
#pug.end_effector = 'right_eef' # should check if i can put other links here... i dont think so :S
# Create grasp to append
grsp = createGrasp(grasp_pose)
#pug.possible_grasps.append(grsp)
pug.allowed_planning_time = 5.0 # Compare this to the... minute we had before!!
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
#pug.allowed_touch_objects.append("part") # doesnt seem to be used on the robotcommander
pug.allowed_touch_objects.append("all")
#pug.attached_object_touch_links.append("all") #looks bad XD
return pug
def create_pickup_goal(group=PLANNING_GROUP,
target=PICK_OBJECT,
grasp_pose=PoseStamped(),
possible_grasps=[]):
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
# configure
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
return pug
def createPickupGoal(group="right_arm_torso", target="part", grasp_pose=PoseStamped(), possible_grasps=[]):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal() # haha goal is a dog
pug.target_name = target
pug.group_name = group
#pug.end_effector = "right_eef"
#pug.end_effector = 'right_eef' # should check if i can put other links here... i dont think so :S
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0 # Compare this to the... minute we had before!!
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
#pug.allowed_touch_objects.append("all")
#pug.attached_object_touch_links.append("all") #looks bad XD
return pug
def createPickupGoal(group="right_arm_torso", target="part", grasp_pose=PoseStamped(), possible_grasps=[]):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal() # haha goal is a dog
pug.target_name = target
pug.group_name = group
#pug.end_effector = "right_eef"
#pug.end_effector = 'right_eef' # should check if i can put other links here... i dont think so :S
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0 # Compare this to the... minute we had before!!
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
#pug.allowed_touch_objects.append("all")
#pug.attached_object_touch_links.append("all") #looks bad XD
return pug
def createPickupGoal(group="arm_torso",
target="pringles",
grasp_pose=PoseStamped(),
possible_grasps=[],
links_to_allow_contact=None):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 35.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 1 # 10
pug.allowed_touch_objects = []
pug.attached_object_touch_links = ['<octomap>']
pug.attached_object_touch_links.extend(links_to_allow_contact)
def create_pickup_goal(self, group, target, grasps):
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
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 createPickupGoal(group="right_arm_torso",
target="part",
grasp_pose=PoseStamped()):
pug = PickupGoal() # haha goal is a dog
pug.target_name = target
pug.group_name = group
#pug.end_effector = 'right_eef' # should check if i can put other links here... i dont think so :S
# Create grasp to append
grsp = createGrasp(grasp_pose)
#pug.possible_grasps.append(grsp)
pug.allowed_planning_time = 5.0 # Compare this to the... minute we had before!!
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
#pug.allowed_touch_objects.append("part") # doesnt seem to be used on the robotcommander
pug.allowed_touch_objects.append("all")
#pug.attached_object_touch_links.append("all") #looks bad XD
return pug
def createPickupGoal(group="arm_torso", target="part", grasp_pose=PoseStamped(), possible_grasps=[], links_to_allow_contact=None): """ Create a PickupGoal with the provided data""" pug = PickupGoal() pug.target_name = target pug.group_name = group pug.possible_grasps.extend(possible_grasps) pug.allowed_planning_time = 35.0 pug.planning_options.planning_scene_diff.is_diff = True pug.planning_options.planning_scene_diff.robot_state.is_diff = True pug.planning_options.plan_only = False pug.planning_options.replan = True pug.planning_options.replan_attempts = 1 # 10 pug.allowed_touch_objects = [] pug.attached_object_touch_links = ['<octomap>'] pug.attached_object_touch_links.extend(links_to_allow_contact) return pug
def create_pickup_goal(group='arm_torso',
target='part',
grasp_pose=PoseStamped(),
possible_grasps=[],
links_to_allow_contact=None):
# create MoveIt pickup goal with the provided data
pickup_goal = PickupGoal()
pickup_goal.target_name = target
pickup_goal.group_name = group
pickup_goal.possible_grasps.extend(possible_grasps)
pickup_goal.allowed_planning_time = 35.0
pickup_goal.planning_options.planning_scene_diff.is_diff = True
pickup_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
pickup_goal.planning_options.plan_only = False
pickup_goal.planning_options.replan = True
pickup_goal.planning_options.replan_attempts = 50
pickup_goal.allowed_touch_objects = [
target
] # name of the collision object we intend to pick
pickup_goal.attached_object_touch_links = []
pickup_goal.attached_object_touch_links.extend(links_to_allow_contact)
return pickup_goal
def pickup(self, name, grasps, wait=True, **kwargs):
# Check arguments
supported_args = ("allow_gripper_support_collision",
"allowed_touch_objects", "plan_only", "planner_id",
"planning_scene_diff", "planning_time",
"support_name")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("pickup: unsupported argument: %s", arg)
# Create goal
g = PickupGoal()
# 1. Fill in target name
g.target_name = name
# 2. Fill in group name
g.group_name = self._group
# 3. Fill in end effector
g.end_effector = self._effector
# 4. List of grasps
g.possible_grasps = grasps
# 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
# 7. What links are part of the "gripper" (and can contact the object/surface)
try:
g.attached_object_touch_links = kwargs[
"attached_object_touch_links"]
except KeyError:
g.attached_object_touch_links = list(
) # empty list = use all links of end-effector
# 8. Fill in minimize_object_distance
# 9. Fill in path_constraints
# 10. Fill in planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 11. 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()
# 12. Planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.allowed_planning_time
# 13. 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._pick_action.send_goal(g)
if wait:
self._pick_action.wait_for_result()
return self._pick_action.get_result()
else:
return None
def pickup(self, name, grasps, wait=True, **kwargs):
# Check arguments
supported_args = ("allow_gripper_support_collision",
"allowed_touch_objects",
"plan_only",
"planner_id",
"planning_time",
"support_name")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("pickup: unsupported argument: %s", arg)
# Create goal
g = PickupGoal()
# 1. Fill in target name
g.target_name = name
# 2. Fill in group name
g.group_name = self._group
# 3. Fill in end effector
g.end_effector = self._effector
# 4. List of grasps
g.possible_grasps = grasps
# 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
# 7. What links are part of the "gripper" (and can contact the object/surface)
try:
g.attached_object_touch_links = kwargs["attached_object_touch_links"]
except KeyError:
g.attached_object_touch_links = list() # empty list = use all links of end-effector
# 8. Fill in minimize_object_distance
# 9. Fill in path_constraints
# 10. Fill in planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 11. 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()
# 12. Planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.allowed_planning_time
# 13. 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._pick_action.send_goal(g)
if wait:
self._pick_action.wait_for_result()
return self._pick_action.get_result()
else:
return None