本文整理汇总了Python中moveit_commander.PlanningSceneInterface.remove_attached_object方法的典型用法代码示例。如果您正苦于以下问题:Python PlanningSceneInterface.remove_attached_object方法的具体用法?Python PlanningSceneInterface.remove_attached_object怎么用?Python PlanningSceneInterface.remove_attached_object使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类moveit_commander.PlanningSceneInterface
的用法示例。
在下文中一共展示了PlanningSceneInterface.remove_attached_object方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: main
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
示例2: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
#.........这里部分代码省略.........
示例3: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
self.obj_att = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = target_pose
initial_pose.header.frame_id = 'gazebo_world'
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
# M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
# M1[0,3] = target_pose.pose.position.x
# M1[1,3] = target_pose.pose.position.y
# M1[2,3] = target_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 1.57, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.25 # about z
#.........这里部分代码省略.........
示例4: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('MotionSequence')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying left wam finger poses
self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped)
self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped)
self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Define move group comander for each moveit group
left_wam = moveit_commander.MoveGroupCommander('left_wam')
right_wam = moveit_commander.MoveGroupCommander('right_wam')
left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1')
left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2')
left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3')
right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')
left_wam.set_planner_id("PRMstarkConfigDefault")
right_wam.set_planner_id("PRMstarkConfigDefault")
#left_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
# Get the name of the end-effector link
left_end_effector_link = left_wam.get_end_effector_link()
right_end_effector_link = right_wam.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link))
rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link))
# Allow some leeway in position (meters) and orientation (radians)
right_wam.set_goal_position_tolerance(0.01)
right_wam.set_goal_orientation_tolerance(0.05)
left_wam.set_goal_position_tolerance(0.01)
left_wam.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_wam.set_planning_time(15)
left_wam.set_planning_time(25)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Set the reference frame for wam arms
left_wam.set_pose_reference_frame(REFERENCE_FRAME)
right_wam.set_pose_reference_frame(REFERENCE_FRAME)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
spoon_id = 'spoon'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
scene.remove_world_object(spoon_id)
# Remove leftover objects from a previous run
scene.remove_attached_object(right_end_effector_link, 'spoon')
#right_wam.set_named_target('right_wam_start')
#right_wam.go()
#.........这里部分代码省略.........
示例5:
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
group_left.set_pose_target(pose, "l_eef")
result = group_left.go(None,1)
if result == 0:
print "ERROR init pos left"
sys.exit()
####################### ADD RIGHT SHOE TO THE SCENE ###############################
scene.remove_attached_object('r_eef', 'right_shoe')
rospy.sleep(1)
pose.pose.position.x = 0.475
pose.pose.position.y = -0.1500
pose.pose.position.z = 1.035
pose.pose.orientation.w = 0.99144
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0.13053
pose.header.frame_id = robot.get_planning_frame()
pose.header.stamp = rospy.Time.now()
path = roslib.packages.get_pkg_dir('hermes_virtual_robot')+'/common/files/stl/right_shoe.stl'
scene.add_mesh("right_shoe", pose, path)
rospy.sleep(1)
示例6: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('box1')
scene.remove_world_object('box2')
scene.remove_world_object('target')
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table
table_size = [0.2, 0.7, 0.01]
# Set the length, width and height of the object to attach
tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
scene.attach_mesh
# Place the end of the object within the grasp of the gripper
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_footprint'
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
# Update the current state
right_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Return the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
示例7: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('AttachMesh')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the right wam and fingers
right_wam = moveit_commander.MoveGroupCommander('right_wam')
right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')
# Get the name of the end-effector link
end_effector_link = right_wam.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_wam.set_goal_position_tolerance(0.01)
right_wam.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_wam.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_effector_link, 'spoon')
right_wam.set_named_target('right_wam_start')
right_wam.go()
rospy.sleep(2)
# Closing the hand first
# Closing the hand
right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
right_wam_finger_1.execute(right_wam_finger_1.plan())
rospy.sleep(5)
right_wam_finger_2.execute(right_wam_finger_2.plan())
rospy.sleep(5)
right_wam_finger_3.execute(right_wam_finger_3.plan())
rospy.sleep(5)
# Set the length, width and height of the object to attach
#tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.02
# Align the object with the gripper (straight out)
p.pose.orientation.x = -0.5
p.pose.orientation.y = 0.5
p.pose.orientation.z = -0.5
p.pose.orientation.w = 0.5
scene.attach_mesh(end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
示例8: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
class MoveItDemo:
def __init__(self):
global obj_att
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
#.........这里部分代码省略.........
示例9: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
示例10: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
class BTMotion:
def __init__(self, name):
# create messages that are used to publish feedback/result
self._feedback = amazon_challenge_bt_actions.msg.BTFeedback()
self._result = amazon_challenge_bt_actions.msg.BTResult()
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, amazon_challenge_bt_actions.msg.BTAction, execute_cb=self.execute_cb, auto_start = False)
self.pub_posed = rospy.Publisher('arm_posed', String, queue_size=10)
self.pub_rate = rospy.Rate(30)
self._planning_scene = PlanningSceneInterface()
# get ROS parameters
rospy.loginfo('Getting parameters...')
while not rospy.is_shutdown():
try:
self._base_move_params = rospy.get_param('/base_move')
self._timeout = rospy.get_param(name + '/timeout')
self._sim = rospy.get_param(name + '/sim')
self._base_pos_dict = rospy.get_param('/base_pos_dict')
self._left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
self._right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
break
except:
rospy.sleep(random.uniform(0,2))
continue
self._exit = False
while not rospy.is_shutdown():
try:
self._robot = moveit_commander.RobotCommander()
self._left_arm = self._robot.get_group('left_arm')
self._right_arm = self._robot.get_group('right_arm')
self._arms = self._robot.get_group('arms')
self._torso = self._robot.get_group('torso')
self._head = self._robot.get_group('head')
self._arms_dict = {'left_arm': self._left_arm, 'right_arm': self._right_arm}
break
except:
rospy.sleep(random.uniform(0,2))
continue
self._tf_listener = tf.TransformListener()
self._next_task_sub = rospy.Subscriber("/amazon_next_task", String, self.get_task)
self._shelf_pose_sub = rospy.Subscriber("/pubShelfSep", PoseStamped, self.get_shelf_pose)
self._got_shelf_pose = False
self._l_gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)
while not rospy.is_shutdown():
try:
self._tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04])
self._contest = rospy.get_param('/contest', True)
break
except:
rospy.sleep(random.uniform(0,1))
continue
if self._contest:
self._length_tool = 0.18 + self._tool_size[0]
else:
self._length_tool = 0.216 + self._tool_size[0]
self._as.start()
rospy.loginfo('['+rospy.get_name()+']: ready!')
def get_shelf_pose(self, msg):
self._shelf_pose = msg
self._got_shelf_pose = True
def get_bm_srv(self):
while not rospy.is_shutdown():
try:
rospy.wait_for_service('/base_move_server/move', 5.0)
rospy.wait_for_service('/base_move_server/preempt', 5.0)
break
except:
rospy.loginfo('[' + rospy.get_name() + ']: waiting for base move server')
continue
self._bm_move_srv = rospy.ServiceProxy('/base_move_server/move', BaseMove)
self._bm_preempt_srv = rospy.ServiceProxy('/base_move_server/preempt', Empty)
def timer_callback(self, event):
self._timer_started = True
rospy.logerr('[' + rospy.get_name() + ']: TIMED OUT!')
self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object')
self._planning_scene.remove_world_object('grasped_object')
# pull the base back 60 cm
self._left_arm.stop()
self._right_arm.stop()
#.........这里部分代码省略.........
示例11: roscpp_initialize
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
GRIPPER_FRAME = 'right_gripper_link'
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
#rospy.logwarn("cleaning world")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("open")
#right_gripper.go()
rospy.sleep(3)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 1.0
示例12: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
eef = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
# right_arm.set_goal_position_tolerance(0.05)
# right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
#.........这里部分代码省略.........
示例13: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
#right_arm.set_planner_id("KPIECEkConfigDefault");
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("right_gripper_open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.7
p.pose.position.y = -0.2
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.47636
start_pose.pose.position.y = -0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
示例14: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
#.........这里部分代码省略.........
def scene_generator(self):
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
### REMOVE OBJECT FROM PREVIOUS RUNS ###
for i in range(0, len(obj_id)):
self.scene.remove_world_object(obj_id[i])
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')])
next_call = time.time()
while True:
next_call = next_call+1
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
### Make the target purple and obstacles orange ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
###### ATTACH ITEM TO GRIPPER ######
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
time.sleep(next_call - time.time())
示例15: place
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_attached_object [as 别名]
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
rospy.sleep(1)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
# remove part
scene.remove_world_object(PICK_OBJECT)
scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT)
rospy.sleep(2)
i += 1
# end
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)