def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_demo')
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
#cartesian = rospy.get_param('~cartesian', True)
print "===== It is OK ===="
rospy.sleep(3)
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=1)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Set the left arm reference frame
left_arm.set_pose_reference_frame('base')
# Allow 5 seconds per planning attempt
left_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 = 10
# Give the scene a chance to catch up
rospy.sleep(2)
#object1_id = 'object1'
table_id = 'table'
target_id = 'target'
#tool_id = 'tool'
#obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
#scene.remove_world_object(object1_id)
scene.remove_world_object(table_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('base', 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
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.0
#object1_size = [0.088, 0.04, 0.02]
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
# 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
#obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = left_reference_frame
#obstacle1_pose.pose.position.x = 0.96
#obstacle1_pose.pose.position.y = 0.24
#obstacle1_pose.pose.position.z = 0.04
#obstacle1_pose.pose.orientation.w = 1.0
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
#self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
#object1_pose = PoseStamped()
#object1_pose.header.frame_id = left_reference_frame
#object1_pose.pose.position.x = 0.80
#object1_pose.pose.position.y = 0.04
#object1_pose.pose.position.z = table_ground + table_size[2] + object1_size[2] / 2.0
#object1_pose.pose.orientation.w = 1.0
#scene.add_box(object1_id, object1_pose, object1_size)
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = left_reference_frame
table_pose.pose.position.x = 1
table_pose.pose.position.y = 0.7
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)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = left_reference_frame
target_pose.pose.position.x = 1
target_pose.pose.position.y = 0.7
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
#self.setColor(object1_id, 0.8, 0, 0, 1.0)
self.setColor(table_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
left_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = left_reference_frame
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = -0.18
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = left_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = left_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
# Open the gripper to the neutral position
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
# 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('target_pose',
PoseStamped,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.04)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 3
# Set a limit on the number of place attempts
max_place_attempts = 3
rospy.loginfo("Scaling for MoveIt timeout=" + str(
rospy.get_param(
'/move_group/trajectory_execution/allowed_execution_duration_scaling'
)))
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
#table_id = 'table' We also remove the table object in order to run a test
#box1_id = 'box1' These boxes are commented as we do not need them
#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) These boxes are commented as we do not need them
#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 "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the closed position
rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed))
gripper.set_joint_value_target(self.gripper_closed)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the neutral position
rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral))
gripper.set_joint_value_target(self.gripper_neutral)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Set the height of the table off the ground
#table_ground = 0.4
# 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] commented for the same reasons as previously
#box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [
0.018, 0.018, 0.018
] #[0.02, 0.005, 0.12] original object dimensions in meters
# 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.36
#table_pose.pose.position.y = 0.0
#table_pose.pose.position.z = table_ground + table_size[2] -0.08 / 2.0 #0.4+0.01/2 aka table_ground + table_size[2] + target_size[2] / 2.0
#table_pose.pose.orientation.w = 1.0
#scene.add_box(table_id, table_pose, table_size)
#box1_pose = PoseStamped() These two blocks of code are commented as they assign postion to unwanted boxes
#box1_pose.header.frame_id = REFERENCE_FRAME
#box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
#box1_pose.pose.position.y = 0.0
#box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
#box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
#box2_pose = PoseStamped()
#box2_pose.header.frame_id = REFERENCE_FRAME
#box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
#box2_pose.pose.position.y = 0.2
#box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
#box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] / 2.0 table_ground + table_size[2] + target_size[2] -0.08 / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
#self.setColor(table_id, 0.8, 0, 0, 1.0)
#self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
#self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
#arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03
place_pose.pose.position.y = -0.15
place_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] -0.08 / 2.0 0.4+0.01+0.018/2 aka table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id],
[target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
success = False #from here we are forcing the success cases by stting and making it always true it makes an error appear and the arm stays to the middle position
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts: #while not has been removed added = after operator <
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
break
if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " +
str(n_attempts) + " attempts.")
else: #end of forcing
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
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()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.55
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.54
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.60
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.25
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to 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)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
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,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# 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()
print(end_effector_link)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
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 = 2
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, "part")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('default')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(1)
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.240
place_pose.pose.position.y = 0.01
place_pose.pose.position.z = 0.3
scene.add_box("part", place_pose, (0.07, 0.01, 0.2))
# Specify a pose to place the target after being picked up
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
# start the gripper in a neutral pose part way to the target
target_pose.pose.position.x = 0.0733923763037
target_pose.pose.position.y = 0.0129100652412
target_pose.pose.position.z = 0.3097191751
target_pose.pose.orientation.x = -0.524236500263
target_pose.pose.orientation.y = 0.440069645643
target_pose.pose.orientation.z = -0.468739062548
target_pose.pose.orientation.w = 0.558389186859
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
print("going to start pose")
right_arm.set_pose_target(target_pose)
right_arm.go()
rospy.sleep(2)
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
#grasp_pose.pose.position.x = 0.12792118579
#grasp_pose.pose.position.y = -0.285290879999
#grasp_pose.pose.position.z = 0.120301181892
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, 'part')
# Publish the grasp poses so they can be viewed in RViz
print "Publishing grasps"
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick('part', grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place('part', place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_rest')
right_arm.go()
# Open the gripper to the open position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class Pick_and_Place:
def __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.arm = MoveGroupCommander("arm")
rospy.sleep(1)
#remove existing objects
self.scene.remove_world_object()
self.scene.remove_attached_object("endeff", "part")
rospy.sleep(5)
'''
# publish a demo scene
self.pos = PoseStamped()
self.pos.header.frame_id = "world"
# add wall in between
self.pos.pose.position.x = -0.14
self.pos.pose.position.y = 0.02
self.pos.pose.position.z = 0.09
self.scene.add_box("wall", pos, (0.06, 0.01, 0.18))
# add an object to be grasped
self.pos.pose.position.x = -0.14
self.pos.pose.position.y = -0.0434
self.pos.pose.position.z = 0.054
'''
self.g=Grasp()
self.g.pre_grasp_approach.direction.vector.z= 1
self.g.pre_grasp_approach.direction.header.frame_id = 'endeff'
self.g.pre_grasp_approach.min_distance = 0.04
self.g.pre_grasp_approach.desired_distance = 0.10
self.g.grasp_posture = self.make_gripper_posture(0)
self.g.post_grasp_retreat.direction.vector.z= -1
self.g.post_grasp_retreat.direction.header.frame_id = 'endeff'
self.g.post_grasp_retreat.min_distance = 0.04
self.g.post_grasp_retreat.desired_distance = 0.10
self.g.allowed_touch_objects = ["part"]
self.p=PlaceLocation()
self.p.place_pose.header.frame_id= 'world'
self.p.place_pose.pose.position.x= -0.13341
self.p.place_pose.pose.position.y= 0.12294
self.p.place_pose.pose.position.z= 0.099833
self.p.place_pose.pose.orientation.x= 0
self.p.place_pose.pose.orientation.y= 0
self.p.place_pose.pose.orientation.z= 0
self.p.place_pose.pose.orientation.w= 1
self.p.pre_place_approach.direction.vector.z= 1
self.p.pre_place_approach.direction.header.frame_id = 'endeff'
self.p.pre_place_approach.min_distance = 0.06
self.p.pre_place_approach.desired_distance = 0.12
self.p.post_place_posture= self.make_gripper_posture(-1.1158)
self.p.post_place_retreat.direction.vector.z= -1
self.p.post_place_retreat.direction.header.frame_id = 'endeff'
self.p.post_place_retreat.min_distance = 0.05
self.p.post_place_retreat.desired_distance = 0.06
self.p.allowed_touch_objects=["part"]
self._as = actionlib.SimpleActionServer("server_test", TwoIntsAction, execute_cb=self.execute_pick_place, auto_start = False)
self._as.start()
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self,pose):
t = JointTrajectory()
t.joint_names = ['joint6']
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
#tp.effort = GRIPPER_EFFORT
t.points.append(tp)
return t
def execute_pick_place(self,g):
res= TwoIntsResult()
obj_pos = PoseStamped()
obj_pos.header.frame_id = "world"
obj_pos.pose.position.x = -g.a*0.01
obj_pos.pose.position.y = -g.b*0.01
obj_pos.pose.position.z = 0.054 #msg.z-0.02
self.scene.add_box("part", obj_pos, (0.02, 0.02, 0.02))
rospy.sleep(2)
tar_pose = PoseStamped()
tar_pose.header.frame_id = 'world'
tar_pose.pose.position.x = -g.a*0.01 #-0.14
tar_pose.pose.position.y = -g.b*0.01 #-0.0434
tar_pose.pose.position.z = 0.074 #msg.z
tar_pose.pose.orientation.x = -0.076043
tar_pose.pose.orientation.y = 0.99627
tar_pose.pose.orientation.z = -0.00033696
tar_pose.pose.orientation.w = -0.028802
self.g.grasp_pose=tar_pose
grasps = []
grasps.append(copy.deepcopy(self.g))
result=-1
attempt=0
while result < 0 and attempt <= 150:
result=self.arm.pick("part", grasps)
print "Attempt:", attempt
print "pick result: ", result
attempt+=1
if result < 0:
print "Pick Failed"
else:
print "Pick Success"
result=False
attempt=0
while result == False and attempt <= 150:
result=self.arm.place("part", self.p)
print "Attempt:", attempt
attempt+=1
if result == False:
print "Place Failed"
#else:
# print "Place Success"
self.arm.set_named_target("Home")
self.arm.go()
res.sum=1
rospy.sleep(1)
self._as.set_succeeded(res)
class Jaco_rapper():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.jaco_arm = MoveGroupCommander("Arm")
self.hand = MoveGroupCommander("Hand")
#self.pose_pub = rospy.Publisher("hand_pose", PoseStamped,queue_size = 100)
self.pick_command = rospy.Publisher("pick_command",
Bool,
queue_size=100)
rospy.Subscriber("pick_pose", PoseStamped, self.pick)
self.jaco_arm.allow_replanning(True)
# Set the right arm reference frame
self.jaco_arm.set_pose_reference_frame(REFERENCE_FRAME)
self.jaco_arm.set_planning_time(5)
self.jaco_arm.set_goal_tolerance(0.02)
self.jaco_arm.set_goal_orientation_tolerance(0.1)
#self.pick_command.publish(True)
def test(self):
#self.hand.set_joint_value_target([0, 0, 0, 0])
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = 'arm_stand'
grasp_pose.pose.position.x = 0
grasp_pose.pose.position.y = 0.24
grasp_pose.pose.position.z = -0.4
grasp_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
# self.hand.set_joint_value_target([0, 0.012 ,0.012 ,0.012])
while (True):
self.jaco_arm.set_pose_target(
grasp_pose) # move to the top of the target
self.jaco_arm.go()
rospy.sleep(0.2)
#result = self.jaco_arm.go()
def pick(self, p):
target_id = 'target'
# 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
# Remove leftover objects from a previous run
self.scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Open the gripper to the neutral position
self.hand.set_joint_value_target([0.2, 0.2, 0.2, 0.2])
self.hand.go()
rospy.sleep(1)
target_size = [0.01, 0.01, 0.01]
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = p.pose.position.x - 0.015
#p.pose.position.x - 0.015
target_pose.pose.position.y = 0.05
target_pose.pose.position.z = p.pose.position.z
target_pose.pose.orientation.w = 0
print "Arm is catching {} object at ({}, {}, {})".format(
p.header.frame_id, p.pose.position.x, p.pose.position.y,
p.pose.position.z)
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
# Initialize the grasp pose to the target pose
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = REFERENCE_FRAME
grasp_pose.pose = target_pose.pose
grasp_pose.pose.position.y = 0.24
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
#self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = self.jaco_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(p.header.frame_id)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = self.jaco_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(0.2)
self.scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, target_id)
self.pick_command.publish(Bool(True))
return
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
# Generate a gripper translation in the direction given by vector
def make_gripper_translation(self, min_dist, desired, vector):
# Initialize the gripper translation object
g = GripperTranslation()
# Set the direction vector components to the input
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
# The vector is relative to the gripper frame
g.direction.header.frame_id = 'arm_stand'
# Assign the min and desired distances from the input
g.min_distance = min_dist
g.desired_distance = desired
return g
# Generate a list of possible grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(
0.05, 0.15, [0.0, -1.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(
0.05, 0.1, [0.0, 1.0, 0.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
# Yaw angles to try
yaw_vals = [0]
# A list to hold the grasps
g.grasp_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(yaw_vals))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
grasps = [g]
# Return the list
return grasps
# Generate a list of possible place poses
def make_places(self, color):
# Initialize the place location as a PoseStamped message
x = 0
z = 0
if (color == 'red'):
x = 0.4
z = -0.4
elif (color == 'blue'):
x = 0.4
z = -0.25
else:
x = -0.3
z = -0.4
place_pose = PoseStamped()
place_pose.pose.position.x = x
place_pose.pose.position.y = 0.2
place_pose.pose.position.z = z
# Start with the input place pose
place_pose.header.frame_id = REFERENCE_FRAME
# A list to hold the places
places = []
# Create a quaternion from the Euler angles
place_pose.pose.orientation = Quaternion(0, 0, 0, 0)
# Append this place pose to the list
places.append(deepcopy(place_pose))
# Return the list
return places
def PickAndPlace(self, goal):
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link(
) # get end effector link name
arm.allow_replanning(True)
arm.set_planning_time(5)
# setting object id
table_id = 'table'
side_id = 'side'
table2_id = 'table2'
table2_ground_id = 'table2_ground'
box2_id = 'box2'
target_id = 'target'
wall_id = 'wall'
ground_id = 'ground'
rospy.sleep(1)
# setting object size
table_ground = 0.55
table_size = [0.7, 0.2, 0.01]
table2_size = [0.6, 0.25, 0.01]
table2_ground_size = [0.9, 0.02, 0.6]
box2_size = [0.05, 0.05, 0.15]
wall_size = [0.9, 0.01, 0.6]
ground_size = [3, 3, 0.01]
side_size = [0.7, 0.01, table_ground]
# setting object pose and add to the world
'''
self.scene_manage(table2_ground_id, table2_ground_size, [ 0.0, 0.36, table2_ground_size[2]/2.0])
self.scene_manage(box2_id, box2_size, [-0.12, -0.63, table_ground + table_size[2] + box2_size[2]/2.0])
self.scene_manage(table_id, table_size, [0.0, -0.7, table_ground + table_size[2]/2.0])
self.scene_manage(table2_id, table2_size, [0.0, 0.325, table2_ground_size[2] + table2_size[2]/2.0])
self.scene_manage(wall_id, wall_size, [0.0, 0.405, table2_ground_size[2] + table2_size[2] + wall_size[2]/2.0])
self.scene_manage(ground_id, ground_size, [0.0, 0.0, -ground_size[2]/2.0])
self.scene_manage(side_id, side_size, [0.0, -0.605, side_size[2]/2.0])
'''
self.scene_manage(ground_id, ground_size,
[0.0, 0.0, -ground_size[2] / 2.0])
target_size = [
goal.object_size.x, goal.object_size.y, goal.object_size.z
]
target_position = [
goal.object_pose.pose.position.x, goal.object_pose.pose.position.y,
goal.object_pose.pose.position.z
]
self.scene_manage(target_id, target_size, target_position)
# setting object color
self.setColor(table_id, 0.8, 0.0, 0.0, 1.0)
self.setColor(table2_id, 0.8, 0.0, 0.0)
self.setColor(table2_ground_id, 0.9, 0.9, 0.9)
self.setColor(box2_id, 0.8, 0.4, 1.0)
self.setColor(target_id, 0.5, 0.4, 1.0)
self.setColor(wall_id, 0.9, 0.9, 0.9)
self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0)
self.setColor(side_id, 0.8, 0.0, 0.0, 1.0)
self.sendColors()
arm.set_support_surface_name(ground_id) # avoid collision detection
# setting placing position
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_orientation = Quaternion()
place_orientation = quaternion_from_euler(0.0, 0.0, 0.0)
# self.setPose(place_pose, [-0.22, 0.24, table2_ground_size[2] + table2_size[2] + target_size[2]/2.0], list(place_orientation))
self.setPose(place_pose, [0.0, -0.5, 0.25 + target_size[2] / 2.0])
# setting grasping position
grasp_pose = goal.object_pose
grasp_init_orientation = Quaternion()
grasp_init_orientation = quaternion_from_euler(1.57, -1.57, 0.0)
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.y += 0.02
rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation))
# generate grasp postures
grasps = self.make_grasps(grasp_pose, [table_id],
[0.05, 0.07, [0.0, -1.0, 0.0]],
[0.1, 0.15, [0.0, 1.0, 1.0]], 1)
result = None
n_attempts = 0
# execute pick action
while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
arm.set_support_surface_name(ground_id)
# generate place action
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table2_id],
[0.05, 0.07, [0.0, -1.0, -1.0]],
[0.1, 0.15, [0.0, 1.0, 1.0]])
# execute place action
while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
# return result and restore Home posture
if MoveItErrorCodes.SUCCESS:
arm.set_named_target('Home')
arm.go()
#rospy.Subscriber('/j2s7s300_driver/out/tool_pose', PoseStamped, callback=self.cb)
#self._result.arm_pose = arm.get_current_pose()
self._result.arm_pose = rospy.wait_for_message(
'/j2s7s300_driver/out/tool_pose', PoseStamped)
self._server.set_succeeded(self._result)
rospy.sleep(2)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
self.scene = scene
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
self.arm = arm
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
self.end_effector_link = end_effector_link
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 1
max_place_attempts = 1
# 控制机械臂先回到初始化位置
# arm.set_named_target('left_arm_zero')
arm.set_named_target(ARM_INIT_POSE)
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
# gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
############################## 获取目标物体位置 #########################
obj_position, obj_orientation = self.obj_listener()
# 调整四元数顺序以便调用
obj_orientation_wxyz = (obj_orientation[3], obj_orientation[0],
obj_orientation[1], obj_orientation[2])
# 获得旋转矩阵，旋转矩阵每一列对应一个轴的方向向量
rotation_matrix = quaternions.quat2mat(obj_orientation_wxyz)
direction_x = rotation_matrix[:, 0] # 旋转矩阵第一列为x轴方向向量
direction_y = rotation_matrix[:, 1] # 旋转矩阵第二列为y轴方向向量
direction_z = rotation_matrix[:, 2] # 旋转矩阵第三列为z轴方向向量
self.direction_x = direction_x
self.direction_y = direction_y
self.direction_z = direction_z
# 获得x轴方向向量
print "rotation_matrix:\n", rotation_matrix
print "direction vector:\n", direction_x, direction_y, direction_z, "\n"
# 手抓指向一定是朝下的，否则退出程序
if direction_z[2] > 0:
print "Invalid grasp pose !"
sys.exit()
# 设置桌面的高度
self.table_ground = 0.65
# 设置桌子的三维尺寸[长, 宽, 高]
self.table_size = [0.4, 0.8, 0.5]
# 添加场景物体
self.add_scene()
target_id = 'target'
# 设置目标物体的尺寸
target_size = [0.01, 0.01, 0.01]
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
scene.remove_world_object(target_id)
############################ 设置目标物体位置 ##########################
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = obj_position[0]
target_pose.pose.position.y = obj_position[1]
target_pose.pose.position.z = obj_position[2]
print "obj_position:\n", target_pose.pose.position
# 按方向向量平移一定距离
distance = 0.06 # 平移的距离
target_pose.pose.position.x += direction_z[0] * distance
target_pose.pose.position.y += direction_z[1] * distance
target_pose.pose.position.z += direction_z[2] * distance
print "obj_position_moved:\n", target_pose.pose.position
target_pose.pose.orientation.x = obj_orientation[0]
target_pose.pose.orientation.y = obj_orientation[1]
target_pose.pose.orientation.z = obj_orientation[2]
target_pose.pose.orientation.w = obj_orientation[3]
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
rospy.sleep(0.9)
############################ 设置目标物体放置位置 #######################
place_pose = PoseStamped()
place_pose.header.frame_id = 'base_link'
place_pose.pose.position.x = 0.5
place_pose.pose.position.y = -0.3
place_pose.pose.position.z = self.table_ground + target_size[
2] / 2.0 + 0.1
place_pose.pose.orientation.w = 1.0
# scene.add_box('place', place_pose, target_size)
########################### 设置机器人的抓取目标位置 ######################
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = REFERENCE_FRAME
grasp_pose.pose.position.x = obj_position[0]
grasp_pose.pose.position.y = obj_position[1]
grasp_pose.pose.position.z = obj_position[2]
grasp_pose.pose.orientation.x = obj_orientation[0]
grasp_pose.pose.orientation.y = obj_orientation[1]
grasp_pose.pose.orientation.z = obj_orientation[2]
grasp_pose.pose.orientation.w = obj_orientation[3]
########################## 生成抓取姿态、规划并执行抓取 ####################
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
######################### 生成放置姿态、规划并执行放置 #####################
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target(ARM_INIT_POSE)
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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("base_link")
# 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 5 seconds per planning attempt
right_arm.set_planning_time(15)
# 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 = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# 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('resting')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
the_object = None
the_object_dist = 1.0
count = -1
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
right_arm.set_support_surface_name(support_surface)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = target_pose.pose.position.x
place_pose.pose.position.y = 0.03
place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Set the start state to the current state
#right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
# Give the servos a rest
arbotix_relax_all_servos()
rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
#end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
#arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：20s
arm.set_planning_time(20)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table1_id = 'table1'
table2_id = 'table2'
target_id = 'cube_marker'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table1_id)
scene.remove_world_object(table2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先运动到准备位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置table的三维尺寸[长, 宽, 高]
table1_size = [0.8, 1.5, 0.03]
table2_size = [1.5, 0.8, 0.03]
# 将两张桌子加入场景当中
table1_pose = PoseStamped()
table1_pose.header.frame_id = 'base_link'
table1_pose.pose.position.x = 0
table1_pose.pose.position.y = 1.05
table1_pose.pose.position.z = -0.05
table1_pose.pose.orientation.w = 1.0
scene.add_box(table1_id, table1_pose, table1_size)
table2_pose = PoseStamped()
table2_pose.header.frame_id = 'base_link'
table2_pose.pose.position.x = -1.05
table2_pose.pose.position.y = -0.1
table2_pose.pose.position.z = -0.05
table2_pose.pose.orientation.w = 1.0
scene.add_box(table2_id, table2_pose, table2_size)
# 将桌子设置成红色
self.setColor(table1_id, 0.8, 0.4, 0, 1.0)
self.setColor(table2_id, 0.8, 0.4, 0, 1.0)
#监听目标到'base_link'的tf变换
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
(trans,
rot) = listener.lookupTransform('base_link', 'ar_marker_0',
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo(
"Waiting for transform between 'base_link' and 'ar_marker_0'"
)
rospy.sleep(1)
rospy.loginfo("Found transform between 'base_link' and 'ar_marker_0'")
# 设置目标物体的尺寸
target_size = [0.05, 0.05, 0.2]
# 设置目标物体的位置
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = trans[0]
target_pose.pose.position.y = trans[1]
target_pose.pose.position.z = 0.065
target_pose.pose.orientation.x = rot[0]
target_pose.pose.orientation.y = rot[1]
target_pose.pose.orientation.z = rot[2]
target_pose.pose.orientation.w = rot[3]
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table2_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = -0.5
place_pose.pose.position.y = 0
place_pose.pose.position.z = 0.065
while not rospy.is_shutdown():
try:
listener.waitForTransform('ar_marker_0', 'ee_link',
rospy.Time(0), rospy.Duration(4.0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo(
"Waiting for transform between 'ar_marker_0' and 'ee_link'"
)
rospy.sleep(1)
rospy.loginfo("Found transform between 'ar_marker_0' and 'ee_link'")
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = 'ar_marker_0'
grasp_pose.pose.position.x = 0
grasp_pose.pose.position.y = -0.15
grasp_pose.pose.position.z = -0.1
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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()
#move_base action
self.fridge = (Pose(Point(X_FRIDGE, Y_FRIDGE, 0.0),
Quaternion(0.0, 0.0, 0, 1))) #location of the beer
self.person = (Pose(Point(X_PERSON, Y_PERSON, 0.0),
Quaternion(0.0, 0.0, 0,
1))) #person requesting the beer
self.station = (Pose(Point(0.5, 0.0, 0.0),
Quaternion(0.0, 0.0, 0,
1))) #person requesting the beer
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
left_arm = MoveGroupCommander('left_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'
person1_id = 'person1'
# 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)
scene.remove_world_object(person1_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()
left_arm.set_named_target('left_start')
left_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]
person1_size = [0.3, 0.7, 0.01]
# 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 = X_FRIDGE + 0.55
table_pose.pose.position.y = Y_FRIDGE + 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()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = X_FRIDGE + 0.55
box1_pose.pose.position.y = Y_FRIDGE + -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = X_FRIDGE + 0.54
box2_pose.pose.position.y = Y_FRIDGE + 0.13
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
#add the person to the scene
person1_pose = PoseStamped()
person1_pose.header.frame_id = REFERENCE_FRAME
person1_pose.pose.position.x = X_PERSON + 0.54
person1_pose.pose.position.y = Y_PERSON + 0.13
person1_pose.pose.position.z = table_ground + table_size[
2] + person1_size[2] / 2.0
person1_pose.pose.orientation.w = 1.0
scene.add_box(person1_id, person1_pose, person1_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = X_FRIDGE + 0.50
target_pose.pose.position.y = Y_FRIDGE + 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
self.setColor(person1_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = X_PERSON + 0.50
place_pose.pose.position.y = Y_PERSON + -0.25
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
#move to target
self.move_to(self.fridge)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
right_arm.set_named_target('r_travel')
right_arm.go()
self.move_to(self.person)
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to 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)
#move to station
self.move_to(self.station)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def placeSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
rospy.wait_for_service('tms_db_reader')
try:
tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
res = tms_db_reader(temp_dbdata)
target = res.tmsdb[0]
except rospy.ServiceException as e:
print "Service call failed: %s" % e
self.shutdown()
print(target.name)
scene = PlanningSceneInterface()
# 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
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.1)
arm.set_goal_orientation_tolerance(0.3)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(0.05)
target_id = str(req.object_id)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = req.x
target_pose.pose.position.y = req.y
target_pose.pose.position.z = req.z
# q = quaternion_from_euler(target.rr, target.rp, target.ry)
q = quaternion_from_euler(req.roll, req.pitch, req.yaw)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
print(target_pose.pose.position.x)
print(target_pose.pose.position.y)
print(target_pose.pose.position.z)
print(target_pose.pose.orientation.x)
print(target_pose.pose.orientation.y)
print(target_pose.pose.orientation.z)
print(target_pose.pose.orientation.w)
# Initialize the grasp pose to the target pose
place_pose = target_pose
# Generate a list of grasps
places = self.make_places(place_pose)
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
print(result)
if result == MoveItErrorCodes.SUCCESS:
break
# rospy.sleep(0.2)
scene.remove_world_object(str(req.object_id))
ret = rp_placeResponse()
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success the place operation")
ret.result = True
else:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
ret.result = False
return ret
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)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
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, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # Initialize the grasp object
## g = Grasp()
## grasps = []
## # Set the first grasp pose to the input pose
## g.grasp_pose = pre_grasping
## g.allowed_touch_objects = [target_id]
## grasps.append(deepcopy(g))
## right_arm.pick(target_id, grasps)
# #Change the frame_id for the planning to take place!
# #target_pose.header.frame_id = 'gazebo_world'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
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')
# 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]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
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,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
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)
# 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 "grasp" pose stored in the SRDF file
arm.set_named_target('left_arm_up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.04
# 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.25
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()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.21
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.19
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.24
target_pose.pose.position.y = 0.275
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
grasp_pose.pose.position.x = 0.12792118579 + .1
grasp_pose.pose.position.y = 0.285290879999 + 0.05
grasp_pose.pose.position.z = 0.120301181892
#grasp_pose.pose.orientation =
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id, table_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
break
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('left_arm_rest')
arm.go()
# Open the gripper to the open position
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
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(20)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 2
# 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_home')
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()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.55
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.54
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.575868
target_pose.pose.position.y = -0.25149
# target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.position.z = 0.735643773079
target_pose.pose.orientation.w = 1.0
print("Initial target_pose", target_pose)
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.58
place_pose.pose.position.y = -0.35
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
#================For testing orientation constraint======================================
# pose_target_r = geometry_msgs.msg.Pose()
# pose_target_r.position.x = 0.50
# pose_target_r.position.y = -0.160
# pose_target_r.position.z = 0.72
# pose_target_r.position.x = 0.5232
# pose_target_r.position.y = -0.2743
# pose_target_r.position.z = 0.6846
# q = quaternion_from_euler(-1.57, 0, -1.57)
# pose_target_r.orientation.x = q[0]
# pose_target_r.orientation.y = q[1]
# pose_target_r.orientation.z = q[2]
# pose_target_r.orientation.w = q[3]
# print ("pose_target_r",pose_target_r )
# right_arm.set_pose_target(pose_target_r)
# right_arm.go()
# rospy.sleep(5)
# Open the gripper to the full position
# right_gripper.set_named_target("right_open")
# right_gripper.plan()
# right_gripper.go()
# rospy.sleep(5)
# # Create Cartesian Path to move forward mainting the end-effector pose
# waypoints = []
# wpose = right_arm.get_current_pose().pose
# wpose.position.x += 0.05 # move forward in (x)
# waypoints.append(copy.deepcopy(wpose))
# (plan, fraction) = right_arm.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.01, # eef_step
# 0.0) # jump_threshold
# if (plan.joint_trajectory.points) : # True if trajectory contains points
# move_success = right_arm.execute(plan, wait=True)
# if(move_success == True):
# rospy.loginfo("Move forward Successful")
# rospy.sleep(2)
# # right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# right_gripper.set_named_target("right_close")
# right_gripper.plan()
# right_gripper.go()
# rospy.sleep(2)
# waypoints = []
# wpose = right_arm.get_current_pose().pose
# wpose.position.z += 0.1 # move up in (z)
# waypoints.append(copy.deepcopy(wpose))
# (plan, fraction) = right_arm.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.01, # eef_step
# 0.0) # jump_threshold
# if (plan.joint_trajectory.points) : # True if trajectory contains points
# move_success = right_arm.execute(plan, wait=True)
# if(move_success == True):
# rospy.loginfo("Lift Successful")
# else:
# rospy.logwarn("Cartesian Paht Planning Failed for forward movement")
# # 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_home')
# right_arm.go()
#=======================================
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# grasp_pose.pose.position.x = 0.55
# grasp_pose.pose.position.y = -0.165
# grasp_pose.pose.position.z = 0.72
# Shift the grasp pose by half the width of the target to center it
# grasp_pose.pose.position.y += 0.020 # bring the target in between gripper
# grasp_pose.pose.position.x = -0.05
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
print("current grasp pose", grasps[n_attempts].grasp_pose.pose )
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_home')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 移除场景中之前运行残留的物体
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_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.2
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
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_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.31
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.29
box2_pose.pose.position.y = -0.4
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.04, 0.04, 0.05]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
rospy.sleep(2)
scene.add_box("part", pos, (0.02, 0.04, 0.02))
rospy.sleep(2)
result = -1
attempt = 0
while result < 0:
result = arm.pick("part", grasps)
print "Attempt:", attempt
print "Final Result: ", result
attempt += 1
print "pick completed"
result = False
attempt = 0
while result == False or result < 0:
result = arm.place("part", p)
print "Attempt:", attempt
print "Final Result: ", result
attempt += 1
print "Place Completed"
arm.set_named_target("Home")
arm.go()
rospy.sleep(2)
rospy.spin()
roscpp_shutdown()
