class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.add_table()
# self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _set_gripper_width(self, width):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasp(self, x, y, z, roll, pitch, yaw):
grasp = Grasp()
# pre_grasp
grasp.pre_grasp_posture = self._open_gripper()
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# grasp
grasp.grasp_posture = self._close_gripper()
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# post_grasp
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
return [grasp]
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, roll, pitch, yaw, width):
self.add_object('object', [0.37, -0.24, 0.1, math.pi, 0., 0.],
[0.1, 0.1, 0.1])
grasps = self._create_grasp(x, y, z, roll, pitch, yaw)
result = self.arm.pick('object', grasps)
self.remove_object()
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def open_fingers(self):
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self,
x,
y,
z,
roll,
pitch,
yaw,
frame_id="m1n6s200_link_base"):
q = quaternion_from_euler(roll, pitch, yaw)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = True
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def remove_object(self, object='object'):
self.scene.remove_attached_object(self.end_effector_link, object)
self.scene.remove_world_object(object)
rospy.loginfo("Object removed")
def add_object(self, name, pose, size):
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = pose[0]
object_pose.pose.position.y = pose[1]
object_pose.pose.position.z = pose[2]
q = quaternion_from_euler(*pose[3:])
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box(name, object_pose, size)
def add_table(self):
self.add_object('table', [0, 0, -0.005, 0, 0, 0], (2, 2, 0.01))
class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasps(self, x, y, z, z_max, rotation):
grasps = []
# You can create multiple grasps and add them to the grasps list
grasp = Grasp() # create a new grasp
# Set the pre grasp posture (the fingers)
grasp.pre_grasp_posture = self._open_gripper()
# Set the grasp posture (the fingers)
grasp.grasp_posture = self._close_gripper()
# Set the position of where to grasp
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
# Set the orientation of the end effector
q = quaternion_from_euler(math.pi, 0.0, rotation)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# Set the pre_grasp_approach
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# # Set the post_grasp_approach
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm)
grasps.append(
grasp
) # add all your grasps in the grasps list, MoveIT will pick the best one
for z_offset in np.arange(z + 0.02, z_max, 0.01):
new_grasp = copy.deepcopy(grasp)
new_grasp.grasp_pose.pose.position.z = z_offset
grasps.append(new_grasp)
return grasps
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, z_max, rotation, size):
print '******************* grasp'
# Object distance:
obj_dist = np.linalg.norm(np.asarray((x, y, z)))
if obj_dist > 0.5:
rospy.loginfo(
"Object too far appart ({} m), skipping pick".format(obj_dist))
return False
# Add collision object, easiest to name the object, "object"
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = x
object_pose.pose.position.y = y
object_pose.pose.position.z = z
q = quaternion_from_euler(math.pi, 0.0, rotation)
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box("object", object_pose, size)
rospy.sleep(0.5)
self.clear_octomap()
rospy.sleep(1.0)
# Create and return grasps
# z += size[2]/2 # Focus on the top of the object only
# z += size[2]/2 + 0.02 # Focus on the top of the object only
grasps = self._create_grasps(x, y, z, z_max, rotation)
print '******************************************************************************'
result = self.arm.pick(
'object', grasps) # Perform pick on "object", returns result
print '******************************************************************************'
# self.move_to(x, y, z + 0.15, rotation)
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def clear_object(self, x, y, z, z_max, rotation, size):
print '******************* clear_object'
self.move_to_waypoint()
success = self.grasp(x, y, z, z_max, rotation, size)
if success:
self.move_to_waypoint()
success = self.move_to_drop_zone()
if success:
print 'success move to drop zone'
else:
print 'failed move to drop zone'
self.open_fingers()
self.remove_object()
rospy.sleep(1.0)
self.close_fingers()
return success
def open_fingers(self):
print '******************* open_fingers'
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
print '******************* close_fingers'
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"):
print '******************* move_to'
q = quaternion_from_euler(math.pi, 0.0, rotation)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def move_to_waypoint(self):
print '******************* move_to_waypoint'
return self.move_to(0.35, 0, 0.25, 1.57)
def rtb(self):
print '******************* rtb'
self.move_to_drop_zone()
# pose = PoseStamped()
# pose.header.frame_id = 'base_footprint'
# pose.pose.position.x = -0.191258927921
# pose.pose.position.y = 0.1849306168113
# pose.pose.position.z = 0.813729734732
# pose.pose.orientation.x = -0.934842026356
# pose.pose.orientation.y = 0.350652799078
# pose.pose.orientation.z = -0.00168532388516
# pose.pose.orientation.w = 0.0557688079539
#
# self.arm.set_pose_target(pose, self.end_effector_link)
# plan = self.arm.plan()
# self.arm.go(wait=True)
# self.arm.stop()
# self.arm.clear_pose_targets()
def move_to_drop_zone(self):
print '******************* move_to_drop_zone'
pose = PoseStamped()
pose.header.frame_id = "m1n6s200_link_base"
pose.pose.position.x = 0.2175546259709541
pose.pose.position.y = 0.18347985269448372
pose.pose.position.z = 0.16757751444136426
pose.pose.orientation.x = 0.6934210704552356
pose.pose.orientation.y = 0.6589390059796749
pose.pose.orientation.z = -0.23223137602833943
pose.pose.orientation.w = -0.17616808290725341
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def print_position(self):
pose = self.arm.get_current_pose()
self.transformer.waitForTransform("m1n6s200_link_base",
"base_footprint", rospy.Time.now(),
rospy.Duration(10))
eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose)
orientation = eef_pose.pose.orientation
orientation = [
orientation.x, orientation.y, orientation.z, orientation.w
]
euler = euler_from_quaternion(orientation)
print "z:", eef_pose.pose.position.x
print "y:", eef_pose.pose.position.y
print "z:", eef_pose.pose.position.z
print "yaw (degrees):", math.degrees(euler[2])
def remove_object(self):
self.scene.remove_attached_object(self.end_effector_link, "object")
self.scene.remove_world_object("object")
rospy.loginfo("Object removed")
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, pickPos, placePos):
# 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
pick_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the momitive group for the right gripper
pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = pick_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
pick_arm.set_goal_position_tolerance(0.05)
pick_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
pick_arm.allow_replanning(True)
# Set the right arm reference frame
pick_arm.set_pose_reference_frame(REFERENCE_FRAME)
#pick_arm.set_planner_id("RRTConnectkConfigDefault")
#pick_arm.set_planner_id("RRTstarkConfigDefault")
pick_arm.set_planner_id("RRTstarkConfigDefault")
# Allow 5 seconds per planning attempt
pick_arm.set_planning_time(3)
# 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
base_table_id = 'base_table'
target_id = 'target'
limit_table_id = 'limit_table'
#tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(base_table_id)
#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(limit_table_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
# table_ground = 0.6
table_ground = 0.0
# Set the dimensions of the scene objects [l, w, h]
base_table_size = [2, 2, 0.04]
#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.055, 0.055, 0.12]
target_radius = 0.03305
target_height = 0.12310
limit_table_size = [0.6, 1.8, 0.04]
# Add a base table to the scene
base_table_pose = PoseStamped()
base_table_pose.header.frame_id = REFERENCE_FRAME
base_table_pose.pose.position.x = 0.0
base_table_pose.pose.position.y = 0.0
base_table_pose.pose.position.z = -0.3
base_table_pose.pose.orientation.w = 1.0
scene.add_box(base_table_id, base_table_pose, base_table_size)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
rospy.sleep(1)
# 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.5
#table_pose.pose.position.y = -0.4
#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 = REFERENCE_FRAME
target_pose.pose.position.x = pickPos.pose.position.x
target_pose.pose.position.y = pickPos.pose.position.y
target_pose.pose.position.z = pickPos.pose.position.z
target_pose.pose.orientation.w = pickPos.pose.orientation.w
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
#scene.add_cylinder(target_id,target_pose,target_height,target_radius)
#Add the limit_table object to the scene
limit_table_pose = PoseStamped()
limit_table_pose.header.frame_id = REFERENCE_FRAME
limit_table_pose.pose.position.x = 0.58
limit_table_pose.pose.position.y = -0.4
limit_table_pose.pose.position.z = 0.18
limit_table_pose.pose.orientation.w = 1.0
#scene.add_box(limit_table_id, limit_table_pose, limit_table_size)
self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0)
# 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
#pick_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 = placePos.pose.position.x
place_pose.pose.position.y = placePos.pose.position.y
place_pose.pose.position.z = placePos.pose.position.z
place_pose.pose.orientation.w = placePos.pose.orientation.w
place_pose.pose.orientation.x = placePos.pose.orientation.x
place_pose.pose.orientation.y = placePos.pose.orientation.y
place_pose.pose.orientation.z = placePos.pose.orientation.z
#设置机器人运行最大速度位百分之～
pick_arm.set_max_velocity_scaling_factor(0.5)
# 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
# this way is just used by PR2 robot
#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 RV
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:
while result != 1 and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
#moveit中的pick接口，target_id为moveit添加场景的id，此处为目标物体，grasps为可尝试抓取的点位序列
result = pick_arm.pick(target_id, grasps)
#打印信息
rospy.logerr("pick_arm.pick： " + str(result))
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
#如果抓取成功，尝试放置操作
result_g = None
n_attempts = 2
#if result == MoveItErrorCodes.SUCCESS:
while result_g != True and n_attempts < max_pick_attempts: #and result == 1:
# Generate valid place poses
#places = self.make_places(place_pose)
n_attempts += 1
print("-------------------")
print(place_pose)
#更新当前的机械臂状态
pick_arm.set_start_state_to_current_state()
#设置moveit运动的目标点位
pick_arm.set_pose_target(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 = pick_arm.place(target_id, place)
# if result == MoveItErrorCodes.SUCCESS:
# break
# rospy.sleep(0.2)
#moveit的运动接口，wait=True表示等到执行完成才返回
result_g = pick_arm.go(wait=True)
rospy.logerr("pick_arm.go： " + str(result_g))
#打开夹爪
open_client(500)
rospy.sleep(0.2)
#if result == "False"
# rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
#else:
# rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "home" pose stored in the SRDF file
#将机械臂返回到SRDF中的“home”姿态
#scene.remove_world_object(target_id)
#scene.remove_attached_object(GRIPPER_FRAME, target_id)
#scene.remove_world_object(target_id)
rospy.sleep(1)
open_client(500)
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
rospy.logerr("pick_server over ")
class MoveItDemo:
def __init__(self):
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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()
self.server = InteractiveMarkerServer("obj_find")
# Initialize the move group for the right arm
#######arm = MoveGroupCommander(GROUP_NAME_ARM)
self.arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
#######gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
#######end_effector_link = arm.get_end_effector_link()
self.end_effector_link = self.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)
self.arm.set_goal_position_tolerance(0.05)
self.arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
#######arm.allow_replanning(True)
self.arm.allow_replanning(True)
# Set the right arm reference frame
#######arm.set_pose_reference_frame(REFERENCE_FRAME)
self.arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
#######arm.set_planning_time(15)
self.arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
#######max_pick_attempts = 1
self.max_pick_attempts = 1
# Set a limit on the number of place attempts
########max_place_attempts = 3
self.max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
'''
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")
rospy.sleep(1)
'''
self.arm.set_named_target('right_up')
if self.arm.go() != True:
rospy.logwarn(" Go failed")
self.gripper.set_joint_value_target(self.gripper_opened)
if self.gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
# Initialize the grasping goal
#goal = FindGraspableObjectsGoal()
#goal.plan_grasps = False
'''
find_objects.send_goal(goal)
find_objects.wait_for_result(rospy.Duration(5.0))
find_result = find_objects.get_result()
rospy.loginfo("Found %d objects" %len(find_result.objects))
'''
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
self.scene._colors = dict()
# Use the nearest object on the table as the target
self.target_pose = PoseStamped()
self.target_pose.header.frame_id = REFERENCE_FRAME
self.target_id = 'target'
self.target_size = None
self.the_object = None
self.the_object_dist = 0.30
self.the_object_dist_min = 0.10
self.count = -1
#for obj in find_result.objects:
# count +=1
#dx = obj.object.primitive_poses[0].position.x
#dy = obj.object.primitive_poses[0].position.y
dx = 0.25
dy = 0.0
d = math.sqrt((dx * dx) + (dy * dy))
m = Marker()
m.type = Marker.CUBE
m.scale.x = 0.02
m.scale.y = 0.02
m.scale.z = 0.05
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.5
m.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "block_1"
int_marker.pose.position.x = dx
int_marker.pose.position.y = dy
int_marker.pose.position.z = 0.04
int_marker.pose.orientation.x = 0.0
int_marker.pose.orientation.y = 0.0
int_marker.pose.orientation.z = 0.0
int_marker.pose.orientation.w = 1.0
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(control)
control.markers.append(m)
control.always_visible = True
int_marker.controls.append(control)
self.server.insert(int_marker,self.processFeedback)
self.server.applyChanges()
def processFeedback(self,feedback):
#p.pose.position = feedback.pose.position
#self.dist_xy.pose.position = feedback.pose.position
old_pose = Pose()
new_pose = Pose()
result = None
if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_DOWN:
old_pose.position.x = feedback.pose.position.x
old_pose.position.y = feedback.pose.position.y
result = self.pick_pose(old_pose)
print "old pose is " + str(old_pose.position.x) + " , " + str(old_pose.position.y)
if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_UP:
new_pose.position.x = feedback.pose.position.x
new_pose.position.y = feedback.pose.position.y
self.pick_and_place_pose(old_pose, new_pose)
print "new pose is " + str(new_pose.position.x) + " , " + str(new_pose.position.y)
def pick_and_place_pose(self,pick_pose,place_pose):
pass
def pick_pose(self, pick_pose):
moveit_commander.roscpp_initialize(sys.argv)
dx = pick_pose.position.x
dy = pick_pose.position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < self.the_object_dist and d > self.the_object_dist_min:
#if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
# if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
rospy.loginfo("object is in the working zone")
self.the_object_dist = d
self.the_object = self.count
self.target_size = [0.02, 0.02, 0.05]
#target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
#target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
self.target_pose.pose.position.x = dx + self.target_size[0] / 2.0
self.target_pose.pose.position.y = dy
self.target_pose.pose.position.z = 0.04
self.target_pose.pose.orientation.x = 0.0
self.target_pose.pose.orientation.y = 0.0
self.target_pose.pose.orientation.z = 0.0
self.target_pose.pose.orientation.w = 1.0
# wait for the scene to sync
self.scene.waitForSync()
self.scene.setColor(self.target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
self.scene.sendColors()
grasp_pose = self.target_pose
grasp_pose.pose.position.y -= self.target_size[1] / 2.0
grasp_pose.pose.position.x += self.target_size[0]
grasps = self.make_grasps(grasp_pose, [self.target_id], [self.target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
'''
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
self.arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_pick_attempts:
result = self.arm.pick(self.target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
result = self.arm.pick(self.target_id, grasps)
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
self.arm.set_named_target('right_up')
self.arm.go()
# Open the gripper to the neutral position
self.gripper.set_joint_value_target(self.gripper_neutral)
self.gripper.go()
rospy.sleep(1)
return result
# 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.header.stamp = rospy.get_rostime()
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(0.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 = GRIPPER_FRAME
# 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, grasp_opening=[0]):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately;
# grasp_opening should be a bit smaller than target width
g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
g.grasp_posture = self.make_gripper_posture(grasp_opening)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5])
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5])
# 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.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5]
# Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
# from arm base to the object to pick (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for yaw in yaw_vals:
for pitch in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the grasp pose orientation accordingly
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# 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
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
# Generate a list of possible place poses
def make_places(self, target_id, 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.005] #, 0.01, -0.01, 0.015, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]
# A list of pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for pitch in pitch_vals:
for dy in y_vals:
for dx in x_vals:
place.pose.position.x = init_pose.pose.position.x + dx
place.pose.position.y = init_pose.pose.position.y + dy
# Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
# arm base to the place location (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw = atan2(y, x)
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# 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]
# MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
# the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
# objects orientation!), so we cancel this transformation. It is applied here:
# https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
# More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
acobjs = self.scene.get_attached_objects([target_id])
aco_pose = self.get_pose(acobjs[target_id])
if aco_pose is None:
rospy.logerr("Attached collision object '%s' not found" % target_id)
return None
aco_tf = self.pose_to_mat(aco_pose)
place_tf = self.pose_to_mat(place.pose)
place.pose = self.mat_to_pose(place_tf, aco_tf)
rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
% (aco_pose, place.pose))
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
# 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 get_pose(self, co):
# We get object's pose from the mesh/primitive poses; try first with the meshes
if isinstance(co, CollisionObject):
if co.mesh_poses:
return co.mesh_poses[0]
elif co.primitive_poses:
return co.primitive_poses[0]
else:
rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id)
return None
elif isinstance(co, AttachedCollisionObject):
if co.object.mesh_poses:
return co.object.mesh_poses[0]
elif co.object.primitive_poses:
return co.object.primitive_poses[0]
else:
rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id)
return None
else:
rospy.logerr("Input parameter is not a collision object")
return None
def pose_to_mat(self, pose):
'''Convert a pose message to a 4x4 numpy matrix.
Args:
pose (geometry_msgs.msg.Pose): Pose rospy message class.
Returns:
mat (numpy.matrix): 4x4 numpy matrix
'''
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
mat = np.matrix(quaternion_matrix(quat))
mat[0:3, 3] = pos
return mat
def mat_to_pose(self, mat, transform=None):
'''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.
Args:
mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
transform (numpy.ndarray): Optional 4x4 array representing additional transform
Returns:
pose (geometry_msgs.msg.Pose): Pose message representing transform.
'''
if transform != None:
mat = np.dot(mat, transform)
pose = Pose()
pose.position.x = mat[0,3]
pose.position.y = mat[1,3]
pose.position.z = mat[2,3]
quat = quaternion_from_matrix(mat)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
ptu_action = FollowTrajectoryClient(
"ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"])
ptu_action.move_to([
0.0,
-0.75,
])
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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()
print("End Effector Link", end_effector_link)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.05)
# 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 15 seconds per planning attempt
right_arm.set_planning_time(15)
# 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 = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
find_topic = "basic_grasping_perception/find_objects"
sub_topic = "handles_position"
rospy.Subscriber(sub_topic, target_pose, getHandlesCb)
rospy.loginfo(
"Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient(
find_topic, 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('right_down')
# 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(10.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))
rospy.loginfo("Found %d Support Surfaces" %
len(find_result.support_surfaces))
# print("perception_results_primitives",find_result.objects.primitives[0])
# print("perception_results_primitives_poses",find_result.objects.primitives_poses)
# 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
# target_position = None
the_object = None
the_object_dist = 1.0
count = -1
# target_size
# 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
print("target_size %d ", count, target_size)
# target_position = obj.object.primitive_poses[0].position
# print("target_size %d ", %count, target_size )
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
print("target_pose_before ", target_pose)
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.519617092464
target_pose.pose.orientation.y = -0.510439243162
target_pose.pose.orientation.z = 0.479912175114
target_pose.pose.orientation.w = -0.489013456294
print("target_pose_after ", target_pose)
# 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] ]
# obj.primitive_poses[0].position.z += -height/1.5
# 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
print("table_size", table_size)
# 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]
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):
# 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 __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)
# Initialize the move group for the right arm
Arm = MoveGroupCommander(GROUP_NAME_ARM)
Arm.set_planner_id("RRTstarkConfigDefault")
# Initialize the move group for the right gripper
Hand = 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(10)
# 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
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
target_id = 'target'
# Remove leftover objects from a previous run
scene.remove_world_object(target_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)
# Open the gripper to the neutral position
#Hand.set_joint_value_target(GRIPPER_NEUTRAL)
#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 = 0.3
target_pose.pose.position.y = 0.03
target_pose.pose.position.z = -0.3
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
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.2
# 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 = Arm.pick(target_id, grasps)
rospy.sleep(0.2)
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')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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('target_pose', PoseStamped, queue_size=5)
# 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.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(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# 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 simple_grasping 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
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the simple_grasping 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 self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# Clear the virtual object colors
self.scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_id = 'target'
target_size = None
the_object = None
the_object_dist = 0.30
the_object_dist_min = 0.10
count = -1
for obj in find_result.objects:
count += 1
self.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
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist and d > the_object_dist_min:
rospy.loginfo("object is in the working zone")
the_object_dist = d
the_object = count
target_size = [0.02, 0.02, 0.05]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
target_pose.pose.position.z = 0.04
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
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
# 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
self.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
self.scene.waitForSync()
# Set colors of the table and the object we are grabbing
self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
self.scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
self.scene.sendColors()
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
arm.set_support_surface_name(support_surface)
# 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")
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])
# 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
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 = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
rospy.sleep(2)
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('right_up')
arm.go()
rospy.sleep(2)
if args.once:
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface('world')
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
self.colors = dict()
max_pick_attempts = 10
max_place_attempts = 10
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
# arm.set_goal_position_tolerance(0.02)
# arm.set_goal_orientation_tolerance(0.03)
arm.allow_replanning(True)
arm.set_planning_time(5)
table_id = 'table'
side_id = 'side'
table2_id = 'table2'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
wall_id = 'wall'
ground_id = 'ground'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(table2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(wall_id)
scene.remove_world_object(ground_id)
scene.remove_world_object(side_id)
rospy.sleep(1)
table_ground = 0.55
table_size = [0.2, 0.7, 0.01]
table2_size = [0.25, 0.6, 0.01]
box1_size = [0.02, 0.9, 0.6]
box2_size = [0.05, 0.05, 0.15]
target_size = [0.03, 0.06, 0.10]
wall_size = [0.01, 0.9, 0.6]
ground_size = [3, 3, 0.01]
side_size = [0.01, 0.7, table_ground]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table_pose,
[0.7, 0.0, table_ground + table_size[2] / 2.0])
scene.add_box(table_id, table_pose, table_size)
side_pose = PoseStamped()
side_pose.header.frame_id = REFERENCE_FRAME
self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0])
scene.add_box(side_id, side_pose, side_size)
table2_pose = PoseStamped()
table2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table2_pose,
[-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0])
# scene.add_box(table2_id, table2_pose, table2_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0])
# scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
box2_pose,
[0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0])
# scene.add_box(box2_id, box2_pose, box2_size)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
target_pose,
[0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0])
scene.add_box(target_id, target_pose, target_size)
wall_pose = PoseStamped()
wall_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
wall_pose,
[-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0])
# scene.add_box(wall_id, wall_pose, wall_size)
ground_pose = PoseStamped()
ground_pose.header.frame_id = REFERENCE_FRAME
self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0])
# scene.add_box(ground_id, ground_pose, ground_size)
self.setColor(table_id, 0.8, 0.0, 0.0, 1.0)
# self.setColor(table2_id, 0.8, 0.0, 0.0)
# self.setColor(box1_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()
constraints = Constraints()
constraints.name = 'gripper constraint'
orientation_constraint = OrientationConstraint()
arm.set_support_surface_name(table_id)
test_pose = PoseStamped()
test_pose.header.frame_id = REFERENCE_FRAME
test_orientation = Quaternion()
test_orientation = quaternion_from_euler(0.0, 1.57, -3.14)
self.setPose(test_pose, [
-0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0
], list(test_orientation))
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
place_pose,
[0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0])
grasp_pose = target_pose
grasp_init_orientation = Quaternion()
grasp_init_orientation = quaternion_from_euler(0.0, 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.x -= 0.02
rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation))
place_pose.pose.orientation.x = grasp_init_orientation[0]
place_pose.pose.orientation.y = grasp_init_orientation[1]
place_pose.pose.orientation.z = grasp_init_orientation[2]
place_pose.pose.orientation.w = grasp_init_orientation[3]
orientation_constraint.header = grasp_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = grasp_init_orientation[0]
orientation_constraint.orientation.y = grasp_init_orientation[1]
orientation_constraint.orientation.z = grasp_init_orientation[2]
orientation_constraint.orientation.w = grasp_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
grasps = self.make_grasps(grasp_pose, [table_id],
[0.05, 0.07, [1.0, 0.0, 0.0]],
[0.22, 0.26, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < 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_path_constraints(constraints)
arm.set_pose_target(place_pose)
arm.go()
'''
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < 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)
'''
'''
grasp_pose = place_pose
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.x -= 0.02
grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < 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(table2_id)
self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0])
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < 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)
'''
rospy.sleep(2)
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')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
eef = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
# right_arm.set_goal_position_tolerance(0.05)
# right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
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
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
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()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
#grasp_pose.header.frame_id = 'gazebo_wolrd'
# 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 = target_pose.pose.position.x / 2.0
# grasp_pose.pose.position.x = target_pose.pose.position.x -0.07
# grasp_pose.pose.position.z += 0.18
#Allowed touch object list
# ato = [target_id, 'r_forearm_link']
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id]
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
# print grasp.grasp_pose
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
#Allowed touch link list
atl = ['r_forearm_link']
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_pick_attempts:
success = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(0.2)
if success:
self.ac.send_goal(g_close)
rospy.sleep(3)
## If the pick was successful, attempt the place operation
#if success:
#success = False
#n_attempts = 0
## Generate valid place poses
#places = self.make_places(place_pose)
## Repeat until we succeed or run out of attempts
#while success == False and n_attempts < max_place_attempts:
#for place in places:
#success = right_arm.place(target_id, place)
#if success:
#break
#n_attempts += 1
#rospy.loginfo("Place attempt: " + str(n_attempts))
#rospy.sleep(0.2)
#if not 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('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)
#rospy.spin()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
g.grasp_posture.joint_names = ["joint6"]
pos = JointTrajectoryPoint()
pos.positions.append(0.2)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)
# set the post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "base_cylinder"
g.post_grasp_retreat.direction.vector.x = 0.0
g.post_grasp_retreat.direction.vector.y = 0.0
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.desired_distance = 0.25
g.post_grasp_retreat.min_distance = 0.01
g.allowed_touch_objects = ["table", "part"]
g.max_contact_force = 0
# append the grasp to the list of grasps
grasps.append(g)
rospy.sleep(2)
# pick the object
right_arm.pick("part", grasps)
rospy.spin()
roscpp_shutdown()
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()
# 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("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)
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault")
hand.set_planning_time(10.0)
# set the hand to a safe target
hand.set_named_target("open")
# plan and execute the motion
hand.go()
# load grasps
grasps = load_grasps(resourcepath)
two_finger_grasp = modify_grasp_reference_frame(
grasps['two_finger_precision_horizontal'], "pen")
four_finger_grasp = modify_grasp_reference_frame(
grasps['four_finger_precision_horizontal'], "pen")
# plan and pick the object with two fingers first
ret = arm.pick("pen", [two_finger_grasp])
rospy.sleep(1)
if ret != -1:
rospy.sleep(2)
# do some in-hand movements there
rospy.spin()
roscpp_shutdown()
rospy.loginfo("Stopping grasp app")
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)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
self.obj_att = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = target_pose
initial_pose.header.frame_id = 'gazebo_world'
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
# M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
# M1[0,3] = target_pose.pose.position.x
# M1[1,3] = target_pose.pose.position.y
# M1[2,3] = target_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 1.57, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.25 # about z
# 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)
################# GENERATE GRASPS ###################
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
grasps = self.grasp_generator(initial_pose)
possible_grasps = []
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
possible_grasps.append(grasp.pose)
rospy.sleep(0.2)
#print possible_grasps
self.right_arm.pick(target_id, grasps)
# target_name = target_id
# group_name = GROUP_NAME_ARM
# end_effector = GROUP_NAME_GRIPPER
# attached_object_touch_links = ['r_forearm_link']
# #print (target_name, group_name, end_effector)
# PickupGoal(target_name, group_name ,end_effector, possible_grasps )
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def grasp_generator(self, initial_pose):
# Pitch angles to try
pitch_vals = [1, 1.57,0, 1,2 ]
# Yaw angles to try
yaw_vals = [0]#, 1.57, -1.57]
# A list to hold the grasps
grasps = []
g = PoseStamped()
g.header.frame_id = REFERENCE_FRAME
g.pose = initial_pose.pose
#g.pose.position.z += 0.18
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = transformations.quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.pose.orientation.x = q[0]
g.pose.orientation.y = q[1]
g.pose.orientation.z = q[2]
g.pose.orientation.w = q[3]
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
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
next_call = time.time()
while True:
next_call = next_call+1
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
if self.obj_att is None:
# Add the target object to the scene
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)
### 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')
time.sleep(next_call - time.time())
#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 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)
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 PickAndPlace:
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def make_gripper_translation(self, min_dist, desired, vector):
g = GripperTranslation()
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
g.direction.header.frame_id = "panda_link0"
g.min_distance = min_dist
g.desired_distance = desired
return g
def open_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.04, 0.04]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def close_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.0, 0.0]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def clear_all_object(self):
for world_object in (self.scene.get_known_object_names()):
self.scene.remove_world_object(world_object)
for attached_object in (self.scene.get_attached_objects()):
self.scene.remove_attached_object(attached_object)
def add_object_box(self, object_name, pose, color, frame, size):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param size: size of object(mm)
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_box(name=object_name,
pose=object_pose,
size=(size[0] / 1000.0, size[1] / 1000.0,
size[2] / 1000.0))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
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 __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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.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(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
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")
rospy.sleep(1)
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
goal.plan_grasps = False
find_objects.send_goal(goal)
find_objects.wait_for_result(rospy.Duration(5.0))
find_result = find_objects.get_result()
rospy.loginfo("Found %d objects" %len(find_result.objects))
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
self.scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_id = 'target'
target_size = None
the_object = None
the_object_dist = 0.30
the_object_dist_min = 0.10
count = -1
for obj in find_result.objects:
count +=1
dx = obj.object.primitive_poses[0].position.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist and d > the_object_dist_min:
#if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
# if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
rospy.loginfo("object is in the working zone")
the_object_dist = d
the_object = count
target_size = [0.02, 0.02, 0.05]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
target_pose.pose.position.z = 0.04
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
# wait for the scene to sync
self.scene.waitForSync()
self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
self.scene.sendColors()
grasp_pose = target_pose
grasp_pose.pose.position.y -= target_size[1] / 2.0
grasp_pose.pose.position.x += target_size[0]
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
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
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 = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
arm.set_named_target('right_up')
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,
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)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " +
str(err))
raise err
self.gripper_opened = [
rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01
]
self.gripper_closed = [
rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01
]
self.gripper_neutral = [
rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0]) /
2.0)
]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# 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('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.01)
# 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 each of the scene objects a unique name
table_id = 'table'
target_id = 'target'
# Remove leftover objects from a previous run
self.scene.remove_world_object(table_id)
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)
# 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")
# 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")
# Set the height of the table off the ground
table_ground = 0.0
# Set the dimensions of the scene objects [l, w, h]
# Set the target size [l, w, h]
# target_size = [0.02, 0.005, 0.12]
target_size = [0.02, 0.02, 0.05]
#target pose array
target_pose_array = PoseArray()
target_pose_array.header.frame_id = REFERENCE_FRAME
pose1 = Pose()
pose1.position.x = 0.20
pose1.position.y = -0.10
pose1.position.z = 0.025
pose1.orientation.w = 1.0
target_pose_array.poses.append(pose1)
pose2 = Pose()
pose2.position.x = 0.20
pose2.position.y = 0.10
pose2.position.z = 0.025
pose2.orientation.w = 1.0
target_pose_array.poses.append(pose2)
# 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.29
target_pose.pose.position.y = -0.10
target_pose.pose.position.z = 0.025
target_pose.pose.orientation.w = 1.0
'''
target_pose.pose.position.x = target_pose_array.poses[1].position.x
target_pose.pose.position.y = target_pose_array.poses[1].position.y
target_pose.pose.position.z = target_pose_array.poses[1].position.z
target_pose.pose.orientation.w = target_pose_array.poses[
1].orientation.w
# Add the target object to the scene
self.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)
# 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
# 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!")
# 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, 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
my_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
my_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = my_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
my_arm.set_goal_position_tolerance(0.05)
my_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
my_arm.allow_replanning(True)
# Set the right arm reference frame
my_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
my_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)
#remove objects from a previous session
# Start the arm in the "resting" pose stored in the SRDF file
#right_arm.set_named_target('resting')
#right_arm.go()
# joint_positions = [-0.0253998950875,-0.709038560046,0.927768320774,1.40608266002,-1.56855335407,0.0245406559189]
# my_arm.set_joint_value_target(joint_positions)
# my_arm.go()
# rospy.sleep(1)
# Open the gripper to the neutral position
my_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
my_gripper.go()
rospy.sleep(1)
#find_objects
# rospy.Subscriber("",String,callback);
table_id='table'
target_id='target'
table_size=[0.6,0.32,0.4]
target_size=[0.18,0.18,0.2]
table_ground=0
# 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.2-rx
table_pose.pose.position.y = 0.0-ry
table_pose.pose.position.z = table_ground + table_size[2] / 2.0 -rz
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size) #so does box
# Set the target pose in between the boxes and on the table,the target is defined by photos shot by camera
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.2-rx
target_pose.pose.position.y = 0.0-ry
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 -rz
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Set the support surface name to the table object
my_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.18
# 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!!!!!there are other ways to shift
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= target_size[1] / 2.0
grasp_pose.pose.position.z += target_size[2] / 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 = my_arm.pick(target_id, grasps)
rospy.loginfo(result)
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 = my_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.")
# # Open the gripper to the neutral position
# my_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# my_gripper.go()
# rospy.sleep(2)
# # Return the arm to the "resting" pose stored in the SRDF file
# my_arm.set_named_target('resting')
# my_arm.go()
# rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
arm.go()
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault")
hand.set_planning_time(10.0)
# set the hand to a safe target
hand.set_named_target("open")
# plan and execute the motion
hand.go()
# load grasps
grasps = load_grasps(resourcepath)
two_finger_grasp = modify_grasp_reference_frame (grasps['two_finger_precision_horizontal'], "pen")
four_finger_grasp = modify_grasp_reference_frame (grasps['four_finger_precision_horizontal'], "pen")
# plan and pick the object with two fingers first
ret = arm.pick("pen",[two_finger_grasp])
rospy.sleep(1)
if ret != -1:
rospy.sleep(2)
# do some in-hand movements there
rospy.spin()
roscpp_shutdown()
rospy.loginfo("Stopping grasp app")
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 pickSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
temp_dbdata.state = 1
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)
# 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)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, 10)
# Create a publisher for displaying gripper poses
#self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped,
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.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
print("***** set pose 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
# Give the scene a chance to catch up
rospy.sleep(0.05)
target_id = str(req.object_id)
scene.remove_world_object(target_id)
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(0.05)
arm.set_named_target('l_arm_init')
arm.go()
print("***** Set initial pose defined SRDF")
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(0.05)
if target.offset_x < 0.01 and target.offset_y < 0.01 and target.offset_x < 0.01:
target.offset_x = 0.033
target.offset_y = 0.033
target.offset_z = 0.083
target_size = [(target.offset_x * 2), (target.offset_y * 2),
(target.offset_z * 2)]
# target_size = [0.03, 0.03, 0.12]
# target_size = [0.07, 0.07, 0.14]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target.x
target_pose.pose.position.y = target.y
target_pose.pose.position.z = target.z + target.offset_z + 0.01
# q = quaternion_from_euler(target.rr, target.rp, target.ry)
q = quaternion_from_euler(0, 0, 0)
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]
scene.add_box(target_id, target_pose, target_size)
rospy.sleep(0.05)
print(target_pose.pose.position.x)
print(target_pose.pose.position.y)
print(target_pose.pose.position.z)
# 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.x -= target_size[0] / 2.0 + 0.01
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.05)
# 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)
print(result)
rospy.sleep(0.02)
if result != MoveItErrorCodes.SUCCESS:
scene.remove_attached_object(GRIPPER_FRAME, target_id)
ret = rp_pickResponse()
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success the pick operation")
ret.result = True
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
ret.result = False
return ret
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(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)
p.post_place_retreat.desired_distance = 0.06
p.allowed_touch_objects = ["part"]
# append the grasp to the list of grasps
for i in range(3):
scene.remove_attached_object("endeff", "part")
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"
