def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
class planning:
def __init__(self):
# Retrieve params:
# if there is no param named 'table_object_name' then use the default
# set the scene in rzvi
# original initilization is for 2
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table') # table
self._grasp_object_name_1 = rospy.get_param('~grasp_object_name',
'Grasp_Object1') # box1
self._grasp_object_name_2 = rospy.get_param(
'~grasp_object_name2', 'Wall_behind') # wall_behind as a obstacle
self._grasp_object_name_3 = rospy.get_param(
'~grasp_object_name3', 'Wall_above') # wall_above as a obstacle
self._grasp_object_name_screw = rospy.get_param(
'~grasp_object_name5', 'screw') # screw
#print self._grasp_object_name_screw
self._grasp_object_name_capscrew4 = rospy.get_param(
'~grasp_object_name4', 'capscrew4') # capscrew
#print self._grasp_object_name_capscrew4
#### Not needed in the model spawn Section ####
# Create (debugging) publishers(display in rviz):
#self._grasps_pub = rospy.Publisher(
# 'grasps', PoseArray, queue_size=1, latch=True)
#self._places_pub = rospy.Publisher(
# 'places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name_1)
self._scene.remove_world_object(
self._grasp_object_name_2) #wall_behind
#self._scene.remove_world_object(self._grasp_object_name_3) #wall_above
self._scene.remove_world_object(
self._grasp_object_name_capscrew4) #capscrew4
self._scene.remove_world_object(self._grasp_object_name_screw) #screw
##############3# Add table and Coke can objects to the planning scene:#####################################
self._pose_table = self._add_table(self._table_object_name)
self._add_grasp_block_1(self._grasp_object_name_1)
# ##########################################3Adding obstacle_behind#############################
p_wall = PoseStamped()
robot = moveit_commander.RobotCommander()
p_wall.header.frame_id = robot.get_planning_frame()
p_wall.header.stamp = rospy.Time.now()
p_wall.pose.position.x = -0.25
p_wall.pose.position.y = 0.0
p_wall.pose.position.z = 1.5
q_wall_temp = quaternion_from_euler(0.0, 80.0, 0.0) #Q number
p_wall.pose.orientation = Quaternion(*q_wall_temp)
self._scene.add_box(self._grasp_object_name_2, p_wall, (3, 3, 0.05))
###################################### Adding obstacle wall_above######################################
p_wall2 = PoseStamped()
p_wall2.header.frame_id = robot.get_planning_frame()
p_wall2.header.stamp = rospy.Time.now()
p_wall2.pose.position.x = table_x
p_wall2.pose.position.y = table_y
p_wall2.pose.position.z = 1.25
q_wall2_temp = quaternion_from_euler(0.0, 0.0, 0.0) #Q number
p_wall2.pose.orientation = Quaternion(*q_wall2_temp)
#self._scene.add_box(self._grasp_object_name_3, p_wall2, (3, 3, 0.05))
###################################### Adding obstacle side-1#####################################
p_wall3 = PoseStamped()
p_wall3.header.frame_id = robot.get_planning_frame()
p_wall3.header.stamp = rospy.Time.now()
p_wall3.pose.position.x = table_x
p_wall3.pose.position.y = table_y - 0.8 / 2 - 0.2
p_wall3.pose.position.z = table_z
q_wall3_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number
p_wall3.pose.orientation = Quaternion(*q_wall3_temp)
self._scene.add_box("wall_side-1", p_wall3, (2, 2, 0.05))
###################################### Adding obstacle side -2######################################
p_wall4 = PoseStamped()
p_wall4.header.frame_id = robot.get_planning_frame()
p_wall4.header.stamp = rospy.Time.now()
p_wall4.pose.position.x = table_x
p_wall4.pose.position.y = table_y + 0.8 / 2 + 0.05
p_wall4.pose.position.z = table_z
q_wall4_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number
p_wall4.pose.orientation = Quaternion(*q_wall4_temp)
self._scene.add_box("wall_side-2", p_wall4, (2, 2, 0.05))
##############################################adding screw###########################
screw = PoseStamped()
screw.header.frame_id = robot.get_planning_frame()
screw.header.stamp = rospy.Time.now()
screw.pose.position.x = table_x
screw.pose.position.y = table_y - 0.2
screw.pose.position.z = box1_z + 0.02
screw_q = quaternion_from_euler(0.0, 0.0, 0.0) #Q number
screw.pose.orientation = Quaternion(*screw_q)
print self._scene.add_box(self._grasp_object_name_screw, screw,
(0.036, 0.046, 0.032))
##############################################adding capscrew 0.045###########################
capscrew4 = PoseStamped()
capscrew4.header.frame_id = robot.get_planning_frame()
capscrew4.header.stamp = rospy.Time.now()
capscrew4.pose.position.x = table_x
capscrew4.pose.position.y = table_y
capscrew4.pose.position.z = box1_z + 0.02
q4 = quaternion_from_euler(0.0, 0.0, 0.0) #Q number
capscrew4.pose.orientation = Quaternion(*q4)
print self._scene.add_box(self._grasp_object_name_capscrew4, capscrew4,
(0.05, 0.05, 0.040))
rospy.sleep(1.0)
# gripper graspping point setting.
# Define target place pose:
#########################################Section: Neccessay function for (displaying)Rviz###################################
######this section for adding stuffs in the planning scene.(table, box1, box2, etc...) in RViz############################3
def _add_table(self, name):
"""
Create and add table to the scene
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = table_x
p.pose.position.y = table_y
p.pose.position.z = table_z
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
# display in RZvi
print self._scene.add_box(name, p,
(1, 1, 0.05)) #arg(name, pose, size)
return p.pose
def _add_grasp_block_1(self, name):
"""
Create and add block to the scene
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = box1_x + 0.3
p.pose.position.y = box1_y + 0.3
p.pose.position.z = box1_z
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
#self._scene.add_box(name, p, (0.045, 0.045, 0.045))
return p.pose
class ObjectManipulationAS:
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_clusters = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
# TODO: Check if pose is not empty, if it is, reject
# elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id:
# goal.set_rejected() # "No objects to grasp were received on the objects topic."
# return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run the corresponding operation
if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK:
rospy.loginfo("ObjectManipulation: PICK!")
self.pick_operation()
elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE:
rospy.loginfo("ObjectManipulation: PLACE!")
self.place_operation()
else:
rospy.logerr("ObjectManipulation: Erroneous operation!")
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def pick_operation(self):
"""Execute pick operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking hand to use")
# Check which arm group was requested and if it's currently free of objects
if 'right' in goal_message_field.group:
if self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand already contains an object.")
return # necessary?
self.current_side = 'right'
elif 'left' in goal_message_field.group:
if self.left_hand_object:
self.update_aborted("Left hand already contains an object.")
return
self.current_side = 'left'
# Publish pose of goal position
if DEBUG_MODE:
self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose)
self.update_feedback("Detecting clusters")
if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published
self.update_aborted("Failed detecting clusters")
# Search closer cluster
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
object_to_grasp_pose = goal_message_field.target_pose.pose
self.update_feedback("Searching closer cluster while clustering")
(closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose)
rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
#TODO visualize bbox
#TODO publish filtered pointcloud?
rospy.logdebug("BBOX: " + str(obj_bbox_dims))
########
self.update_feedback("Check reachability")
########
self.update_feedback("Generating grasps")
rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
#transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2] / 2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("No grasps received")
return
if DEBUG_MODE: # TODO: change to dynamic param
publish_grasps_as_poses(grasp_list)
self.current_goal.publish_feedback(self.as_feedback)
########
self.update_feedback("Setup planning scene")
# Add object to grasp to planning scene
self.scene.add_box(self.current_side + "_hand_object", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.as_result.object_scene_name = self.current_side + "_hand_object"
########
self.update_feedback("Execute grasps")
pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
rospy.loginfo("Sending pickup goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result...")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.as_result.object_pose = object_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if self.current_side == 'right':
self.right_hand_object = self.current_side + "_hand_object"
elif self.current_side == 'left':
self.left_hand_object = self.current_side + "_hand_object"
self.current_goal.set_succeeded(result=self.as_result)
else:
# Remove object as it has not been picked
self.scene.remove_world_object(self.current_side + "_hand_object")
self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def place_operation(self):
"""Execute the place operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking arm to use")
# Check which arm group was requested and if it currently has an object
if 'right' in goal_message_field.group:
if not self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand does not contain an object.")
return # necessary?
self.current_side = 'right'
current_target = self.right_hand_object
elif 'left' in goal_message_field.group:
if not self.left_hand_object:
self.update_aborted("Left hand does not contain an object.")
return
self.current_side = 'left'
current_target = self.left_hand_object
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
placing_pose = goal_message_field.target_pose.pose
#### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE ####
self.update_feedback("Sending place order to MoveIt!")
placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose)
goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target)
rospy.loginfo("Sending place goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result...")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
self.as_result.object_pose = placing_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
# Emptying hand
self.update_feedback("Emptying hand")
if self.current_side == 'right':
self.as_result.object_scene_name = current_target
self.right_hand_object = None
elif self.current_side == 'left':
self.as_result.object_scene_name = current_target
self.left_hand_object = None
# Removing object from the collision world
self.scene.remove_world_object(current_target)
self.current_goal.set_succeeded(result=self.as_result)
else:
self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def message_fields_ok(self):
"""Check if the minimal fields for the message are fulfilled"""
if self.current_goal:
goal_message_field = self.current_goal.get_goal()
if len(goal_message_field.group) == 0:
rospy.logwarn("Group field empty.")
return False
if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE:
rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation))
return False
if len(goal_message_field.target_pose.header.frame_id) == 0:
rospy.logwarn("Target pose frame_id is empty")
return False
return True
def update_feedback(self, text):
"""Publish feedback with given text"""
self.as_feedback.last_state = text
self.current_goal.publish_feedback(self.as_feedback)
def update_aborted(self, text="", manipulation_result=None):
"""Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled"""
self.update_feedback("aborted." + text)
if manipulation_result == None:
aborted_result = ObjectManipulationResult()
aborted_result.error_message = text
self.current_goal.set_aborted(result=aborted_result)
else:
self.current_goal.set_aborted(result=manipulation_result)
def generate_grasps(self, pose, width):
"""Send request to block grasp generator service"""
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
grasp_options = GraspGeneratorOptions()
grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y
grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN
grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF
goal.options.append(grasp_options)
self.grasps_ac.send_goal(goal)
if DEBUG_MODE:
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
self.grasps_ac.wait_for_result()
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def get_id_of_closest_cluster_to_pose(self, input_pose):
"""Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped)
and all the associated clustering information"""
current_id = 0
closest_pose = None
closest_object_points = None
closest_id = 0
closest_bbox = None
closest_bbox_dims = None
closest_distance = 99999.9
for myobject in self.last_clusters.objects:
(object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0])
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose))
if closest_pose == None: # first loop iteration
closest_object_points = object_points
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_distance = dist_between_poses(closest_pose, input_pose)
else:
if dist_between_poses(object_pose, input_pose) < closest_distance:
closest_object_points = object_points
closest_distance = dist_between_poses(object_pose, input_pose)
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_id = current_id
current_id += 1
rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose))
return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose)
def wait_for_recognized_array(self, wait_time=6, timeout_time=10):
"""Ask for depth images until we get a recognized array
wait for wait_time between depth throtle calls
stop if timeout_time is achieved
If we dont find it in the correspondent time return false, true otherwise"""
initial_time = rospy.Time.now()
self.last_clusters = None
count = 0
num_calls = 1
self.depth_service.call(EmptyRequest())
rospy.loginfo("Depth throtle server call #" + str(num_calls))
rospy.loginfo("Waiting for a recognized array...")
while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None:
rospy.sleep(0.1)
count += 1
if count >= wait_time / 10:
self.depth_service.call(EmptyRequest())
num_calls += 1
rospy.loginfo("Depth throtle server call #" + str(num_calls))
if self.last_clusters == None:
return False
else:
return True
class GraspObjectServer:
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_objects = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
goal.set_rejected() # "No objects to grasp were received on the objects topic."
return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run grasping state machine
self.grasping_sm()
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def grasping_sm(self):
if self.current_goal:
self.update_feedback("running clustering")
(object_points, obj_bbox_dims,
object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
#TODO visualize bbox
#TODO publish filtered pointcloud?
print obj_bbox_dims
########
self.update_feedback("check reachability")
########
self.update_feedback("generate grasps")
#transform pose to base_link
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2]/2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("no grasps received")
return
self.publish_grasps_as_poses(grasp_list)
self.feedback.grasps = grasp_list
self.current_goal.publish_feedback(self.feedback)
self.result.grasps = grasp_list
########
self.update_feedback("setup planning scene")
#remove old objects
self.scene.remove_world_object("object_to_grasp")
# add object to grasp to planning scene
self.scene.add_box("object_to_grasp", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.result.object_scene_name = "object_to_grasp"
########
if self.current_goal.get_goal().execute_grasp:
self.update_feedback("execute grasps")
pug = self.createPickupGoal("object_to_grasp", grasp_list)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Result is:")
print result
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.result.object_pose = object_pose
#bounding box in a point message
self.result.bounding_box = Point()
self.result.bounding_box.x = obj_bbox_dims[0]
self.result.bounding_box.y = obj_bbox_dims[1]
self.result.bounding_box.z = obj_bbox_dims[2]
self.current_goal.set_succeeded(result = self.result)
#self.current_goal.set_aborted()
def update_feedback(self, text):
self.feedback.last_state = text
self.current_goal.publish_feedback(self.feedback)
def update_aborted(self, text = ""):
self.update_feedback("aborted." + text)
self.current_goal.set_aborted()
def generate_grasps(self, pose, width):
#send request to block grasp generator service
if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)):
return []
rospy.loginfo("Successfully connected.")
goal = GenerateBlockGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
t_start = rospy.Time.now()
self.grasps_ac.wait_for_result()
t_end = rospy.Time.now()
t_total = t_end - t_start
rospy.loginfo("Result received in " + str(t_total.to_sec()))
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def publish_grasps_as_poses(self, grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
self.grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"]
pug.allowed_touch_objects.append(target)
#pug.attached_object_touch_links.append('all')
return pug
scene = PlanningSceneInterface()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = REFERENCE_LINK
p.header.stamp = rospy.Time.now()
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat)
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# add table
scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
i = 1
while i <= 10:
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('MotionSequence')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying left wam finger poses
self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped)
self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped)
self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Define move group comander for each moveit group
left_wam = moveit_commander.MoveGroupCommander('left_wam')
right_wam = moveit_commander.MoveGroupCommander('right_wam')
left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1')
left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2')
left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3')
right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')
left_wam.set_planner_id("PRMstarkConfigDefault")
right_wam.set_planner_id("PRMstarkConfigDefault")
#left_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
# Get the name of the end-effector link
left_end_effector_link = left_wam.get_end_effector_link()
right_end_effector_link = right_wam.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link))
rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link))
# Allow some leeway in position (meters) and orientation (radians)
right_wam.set_goal_position_tolerance(0.01)
right_wam.set_goal_orientation_tolerance(0.05)
left_wam.set_goal_position_tolerance(0.01)
left_wam.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_wam.set_planning_time(15)
left_wam.set_planning_time(25)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Set the reference frame for wam arms
left_wam.set_pose_reference_frame(REFERENCE_FRAME)
right_wam.set_pose_reference_frame(REFERENCE_FRAME)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
spoon_id = 'spoon'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
scene.remove_world_object(spoon_id)
# Remove leftover objects from a previous run
scene.remove_attached_object(right_end_effector_link, 'spoon')
#right_wam.set_named_target('right_wam_start')
#right_wam.go()
#rospy.sleep(2)
# Closing the hand first
# Closing the hand
#right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
#right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
#right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
#right_wam_finger_1.execute(right_wam_finger_1.plan())
#rospy.sleep(5)
#right_wam_finger_2.execute(right_wam_finger_2.plan())
#rospy.sleep(5)
#right_wam_finger_3.execute(right_wam_finger_3.plan())
#rospy.sleep(5)
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = right_end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.02
# Align the object with the gripper (straight out)
p.pose.orientation.x = -0.5
p.pose.orientation.y = 0.5
p.pose.orientation.z = -0.5
p.pose.orientation.w = 0.5
# Attach the tool to the end-effector
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [0.90128, 0.381, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
scene.add_box(table_id, table_pose, table_size)
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = REFERENCE_FRAME
bowl_pose.pose.position.x = 0.015
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
# Set the height of the pitcher
#pitcher_ground = 0.57816875
#pitcher_pose = PoseStamped()
#pitcher_pose.header.frame_id = REFERENCE_FRAME
#pitcher_pose.pose.position.x = 0.25
#pitcher_pose.pose.position.y = 0.847725
#pitcher_pose.pose.position.z = pitcher_ground
#pitcher_pose.pose.orientation.w = -0.5
#pitcher_pose.pose.orientation.z = 0.707
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
#self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0)
self.sendColors()
rospy.sleep(2)
start = input("Start left_wam planning ? ")
# Set the support surface name to the table object
#left_wam.set_support_surface_name(table_id)
#right_wam.set_support_surface_name(table_id)
# Set the target pose.
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.40363476287
target_pose.pose.position.y = 0.847725
target_pose.pose.position.z = 0.721472317843
target_pose.pose.orientation.x = 0.707
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = -0.707
target_pose.pose.orientation.w = 0
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(target_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
left_wam.shift_pose_target(5, 0, left_end_effector_link)
start = input("Left wam starting position ")
# Pause for a second
# Closing the hand
left_wam_finger_1.set_named_target("left_wam_finger_1_grasp")
left_wam_finger_2.set_named_target("left_wam_finger_2_grasp")
left_wam_finger_3.set_named_target("left_wam_finger_3_grasp")
left_wam_finger_1.execute(left_wam_finger_1.plan())
#rospy.sleep(3)
left_wam_finger_2.execute(left_wam_finger_2.plan())
#rospy.sleep(3)
left_wam_finger_3.execute(left_wam_finger_3.plan())
#rospy.sleep(3)
start = input("Left wam hand closing ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose)
intermidiate_pose = deepcopy(end_pose)
intermidiate_pose.position.z = intermidiate_pose.position.z + 0.05
plan = self.StraightPath(end_pose, intermidiate_pose, left_wam)
left_wam.set_start_state_to_current_state()
left_wam.execute(plan)
start = input("Hold up the Pitcher ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose)
intermidiate_pose = deepcopy(end_pose)
intermidiate_pose.position.x = intermidiate_pose.position.x - 0.1
plan = self.StraightPath(end_pose, intermidiate_pose, left_wam)
left_wam.set_start_state_to_current_state()
left_wam.execute(plan)
start = input("left_wam into pouring position ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link))
back_pose = deepcopy(end_pose)
end_pose.pose.orientation.x = 0.97773401145
end_pose.pose.orientation.y = 0
end_pose.pose.orientation.z = -0.209726592658
end_pose.pose.orientation.w = 0
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(end_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
start = input("Pour the water ")
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(back_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link))
end_pose.pose.position.x = end_pose.pose.position.x + 0.1
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(end_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
start = input("Left_wam back to prep position")
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = -0.600350195463908
target_pose.pose.position.y = 0.80576308041
target_pose.pose.position.z = 0.794775212132
target_pose.pose.orientation.x = 3.01203251908e-05
target_pose.pose.orientation.y = 0.705562870053
target_pose.pose.orientation.z = 4.55236739937e-05
target_pose.pose.orientation.w = 0.708647326547
start = input("Start right_wam planning ? ")
right_wam.set_start_state_to_current_state()
right_wam.set_pose_target(target_pose, right_end_effector_link)
right_wam.execute(right_wam.plan())
start = input("right_wam into position. ")
start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6
plan = self.StraightPath(start_pose, intermidiate_pose, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
rospy.sleep(3)
# Closing the hand
right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
right_wam_finger_1.execute(right_wam_finger_1.plan())
#rospy.sleep(3)
right_wam_finger_2.execute(right_wam_finger_2.plan())
#rospy.sleep(3)
right_wam_finger_3.execute(right_wam_finger_3.plan())
rospy.sleep(1)
scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl')
#create a circle path
circles = input("How many circles you want the wam to mix ? ")
start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
plan = self.CircularPath(start_pose, circles, right_wam)
#execute the circle path
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("Mix the oatmeal ")
#put the right_wam back to preparation pose
end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose1 = deepcopy(end_pose)
intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1
plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("wait for the execution of straight path ")
end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose2 = deepcopy(end_pose)
intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25
plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("right_wam back into prep position ")
#left_wam.shift_pose_target(5, 0, left_end_effector_link)
#left_wam.go()
#rospy.sleep(2)
#left_wam.shift_pose_target(0, -0.05, left_end_effector_link)
#left_wam.go()
#rospy.sleep(2)
# Initialize the grasp pose to the target pose
#grasp_pose = target_pose
# Generate a list of grasps
#grasps = self.make_grasps(grasp_pose, [pitcher_id])
# Publish the grasp poses so they can be viewed in RViz
#for grasp in grasps:
# self.left_wam_finger_1_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
# self.left_wam_finger_2_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
# self.left_wam_finger_3_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
'''
#sim table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=0.75
table_pose.pose.position.y=0.0
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
'''
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.184
table_pose.pose.position.y = 0.62
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.m2j = rospy.Publisher("/two_finger/motoman_control/move_to_joint",
JointAnglesDuration,
queue_size=1,
latch=True)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
self.reference_frame = 'base_link'
arm.set_pose_reference_frame(self.reference_frame)
arm.set_planning_time(5)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target_id = 'target_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
target_size = [0.058, 0.058, 0.19]
'''
#target sim
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=0.6
target_pose.pose.position.y=0.05
target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
self.scene.add_box(self.target_id,target_pose,target_size)
'''
f_target_size = [0.2, 0.2, 0.04]
#final target
f_target_pose = PoseStamped()
f_target_pose.header.frame_id = self.reference_frame
f_target_pose.pose.position.x = -0.184 + 0.27
f_target_pose.pose.position.y = 0.62 + 0.1
f_target_pose.pose.position.z = self.table_ground + self.table_size[
2] + f_target_size[2] / 2.0
f_target_pose.pose.orientation.x = 0
f_target_pose.pose.orientation.y = 0
f_target_pose.pose.orientation.z = 0
f_target_pose.pose.orientation.w = 1
self.scene.add_box(self.f_target_id, f_target_pose, f_target_size)
#pouring pose
pour_pose = f_target_pose
pour_pose.pose.position.x -= 0.06
pour_pose.pose.position.y -= 0.12
pour_pose.pose.position.z += 0.15
#pour_pose.pose.position.y+=0.17
pour_pose.pose.orientation.x = -0.5
pour_pose.pose.orientation.y = -0.5
pour_pose.pose.orientation.z = -0.5
pour_pose.pose.orientation.w = 0.5
#set color
self.setColor(self.table_id, 0.8, 0, 0, 1.0)
self.setColor(self.f_target_id, 0.8, 0.4, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor(self.target_id, 0, 1, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(5)
#j_ori_state=[0.72316974401474, 0.4691084027290344, 0.41043856739997864, -2.3381359577178955, 0.5580500364303589, 1.1085972785949707, 3.14]
j_ori_state = [
-1.899937629699707, -0.5684762597084045, 0.46537330746650696,
2.3229329586029053, -0.057941947132349014, -1.2867668867111206,
0.2628822326660156
]
#j_trans_state=[1.708616018295288, 0.9996442198753357, -0.4782222807407379, -1.7541601657867432, 0.13480804860591888, 1.1835496425628662, 2.689549684524536]
signal = True
arm.set_joint_value_target(j_ori_state)
arm.go()
rospy.sleep(3)
#arm.set_joint_value_target(j_trans_state)
#arm.go()
#rospy.sleep(5)
waypoints_1 = []
waypoints_2 = []
start_pick_pose = arm.get_current_pose(self.arm_end_effector_link).pose
if cartesian:
msg = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.pose.position.x = -(msg.pose.position.y) - 0.28
target_pose.pose.position.y = -msg.pose.position.x + 0.81 - 0.072
target_pose.pose.position.z = 1.36 - msg.pose.position.z + 0.1
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = -0
target_pose.pose.orientation.w = 1
self.scene.add_box(self.target_id, target_pose, target_size)
self.setColor(self.target_id, 0, 0.8, 0)
self.sendColors()
#pre_g_pose
pre_grasp_pose = PoseStamped()
pre_grasp_pose.header.frame_id = self.reference_frame
pre_grasp_pose.pose.position = target_pose.pose.position
pre_grasp_pose.pose.position.y -= 0.2
pre_grasp_pose.pose.position.z += 0.01
pre_grasp_pose.pose.orientation.x = -0.5
pre_grasp_pose.pose.orientation.y = -0.5
pre_grasp_pose.pose.orientation.z = -0.5
pre_grasp_pose.pose.orientation.w = 0.5
#grasp_pose
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = self.reference_frame
grasp_pose.pose.position = target_pose.pose.position
grasp_pose.pose.position.y += 0.025
grasp_pose.pose.orientation.x = -0.5
grasp_pose.pose.orientation.y = -0.5
grasp_pose.pose.orientation.z = -0.5
grasp_pose.pose.orientation.w = 0.5
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
waypoints_1.append(start_pick_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_1.append(deepcopy(grasp_pose.pose))
fraction = 0.0
attempts = 0
maxtries = 300
while fraction != 1 and attempts < maxtries:
(plan_1, fraction) = arm.compute_cartesian_path(
waypoints_1, 0.01, 0.0, True)
attempts += 1
if (attempts % 300 == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
signal_pick = False
if fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " sttempts.")
rospy.loginfo("Arm moving.")
rospy.sleep(3)
#arm.execute(plan_1)
p_pick_1 = plan_1.joint_trajectory.points[-1]
#print p
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(0)
rospy.sleep(3)
self.scene.remove_world_object(self.target_id)
#p_pick_2=plan_1.joint_trajectory.points[1]
#print p
#arm.set_joint_value_target(p_pick_2.positions)
#arm.go()
#rospy.sleep(4)
signal_pick = True
#p=plan_1.joint_trajectory.points[-1]
#print p
#arm.set_joint_value_target(p.positions)
#arm.go()
if signal_pick:
start_trans_pose = arm.get_current_pose(
self.arm_end_effector_link).pose
waypoints_2.append(start_trans_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_2.append(deepcopy(pour_pose.pose))
fraction_2 = 0.0
attempts_2 = 0
maxtries_2 = 300
while fraction_2 != 1 and attempts_2 < maxtries_2:
(plan_2, fraction_2) = arm.compute_cartesian_path(
waypoints_2, 0.01, 0.0, True)
attempts_2 += 1
if (attempts_2 % 300 == 0 and fraction_2 != 1):
rospy.loginfo("path planning failed with " +
str(fraction_2 * 100) + "% success.")
if fraction_2 == 1:
#signal=False
rospy.loginfo("path compute successfully with " +
str(attempts_2) + " sttempts.")
rospy.loginfo("Arm moving.")
p_trans = plan_2.joint_trajectory.points[-1]
#print p
arm.set_joint_value_target(p_trans.positions)
arm.go()
rospy.sleep(3)
j_pre_pour_state = arm.get_current_joint_values()
j_pour_state = j_pre_pour_state
j_pour_state[6] -= (1.57 + 0.26)
arm.set_joint_value_target(j_pour_state)
arm.go()
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
'''
arm.set_joint_value_target(j_pre_pour_state)
arm.go()
rospy.sleep(6)
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(255)
arm.set_joint_value_target(j_ori_state)
arm.go()
rospy.sleep(5)
'''
'''
#rospy.sleep(2)
#arm.set_pose_target(pre_grasp_pose,self.arm_end_effector_link)
#arm.go()
#rospy.sleep(15)
start_pick_pose=arm.get_current_pose(self.arm_end_effector_link).pose
waypoints_1=[]
waypoints_2=[]
if cartesian:
waypoints_1.append(start_pick_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_1.append(deepcopy(grasp_pose.pose))
fraction=0.0
attempts=0
maxtries=300
while fraction!=1 and attempts<maxtries:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
signal_1=False
if fraction==1:
#signal=False
rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.")
rospy.loginfo("Arm moving.")
#print plan_1.joint_trajectory.points
arm.execute(plan_1)
#self.gripperCtrl(0)
rospy.sleep(7)
self.gripperCtrl(0)
self.scene.attach_box(self.arm_end_effector_link,self.target_id,g_p,target_size)
#signal=False
start_trans_pose=arm.get_current_pose(self.arm_end_effector_link).pose
waypoints_2.append(start_trans_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_2.append(deepcopy(pour_pose.pose))
fraction_2=0.0
attempts_2=0
maxtries_2=300
while fraction_2!=1 and attempts_2<maxtries_2:
(plan_2,fraction_2)=arm.compute_cartesian_path(waypoints_2,0.01,0.0,True)
attempts_2+=1
if (attempts_2 %300==0 and fraction_2!=1):
rospy.loginfo("path planning failed with " + str(fraction_2*100)+"% success.")
#signal_1=False
if fraction_2==1:
#signal=False
rospy.loginfo("path compute successfully with "+str(attempts_2)+" sttempts.")
rospy.loginfo("Arm moving.")
#print plan_1.joint_trajectory.points
arm.execute(plan_2)
#self.gripperCtrl(0)
rospy.sleep(7)
j_state=arm.get_current_joint_values()
j_state[6]+=(1.57+0.26)
arm.set_joint_value_target(j_state)
#if fraction==1.0:
#rospy.loginfo("path compute successfully. Move the arm.")
#self.m_pub_m2j.publish(place)
#arm.execute(plan_1)
#for i in range(len(plan_1.joint_trajectory.points)):
#p=plan_1.joint_trajectory.points[-1]
#place=JointAnglesDuration(JointAngles(p.positions[0],p.positions[1],p.positions[2],p.positions[3],p.positions[4],p.positions[5],p.positions[6]),rospy.Duration(5))
#self.m2j.publish(place)
#self.gripperCtrl(0)
#rospy.sleep(2)
#self.gripperCtrl(255)
#rospy.sleep(2)
'''
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
kuka = KukaRobot()
ur10e = Ur10eRobot()
# this amount of sleep is a MUST
rospy.sleep(2)
# add object and table to the planning scene
kuka_table_size = [1.0, 0.4, 1.0]
kuka_table_pose_list = [0.0, 0.4, kuka_table_size[2] / 2.0]
kuka_table_name = "kuka_table"
kuka_table_pose = PoseStamped()
kuka_table_pose.header.frame_id = "world"
kuka_table_pose.header.stamp = rospy.Time.now()
kuka_table_pose.pose.position.x = kuka_table_pose_list[0]
kuka_table_pose.pose.position.y = kuka_table_pose_list[1]
kuka_table_pose.pose.position.z = kuka_table_pose_list[2]
scene.add_box(name=kuka_table_name, pose=kuka_table_pose, size=kuka_table_size)
rospy.sleep(1)
kuka_object_size = [0.2, 0.2, 0.2]
kuka_object_pose_list = [
kuka_table_pose_list[0], kuka_table_pose_list[1],
kuka_table_size[2] + kuka_object_size[2] / 2
]
kuka_object_name = "kuka_object"
kuka_object_pose = PoseStamped()
kuka_object_pose.header.frame_id = "world"
kuka_object_pose.header.stamp = rospy.Time.now()
kuka_object_pose.pose.position.x = kuka_object_pose_list[0]
kuka_object_pose.pose.position.y = kuka_object_pose_list[1]
kuka_object_pose.pose.position.z = kuka_object_pose_list[2]
scene.add_box(name=kuka_object_name,
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 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)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 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)
# 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('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)
# Set the height of the table off the ground
table_ground = 0.30
# 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.30
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.34
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.25
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
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
success = False
n_attempts = 0
# 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 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)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
group.go()
# Add Object to Planning Scene
rospy.loginfo("Planning Scene Settings")
scene = PlanningSceneInterface()
rospy.sleep(2) # Waiting for PlanningSceneInterface
box_pose = PoseStamped()
box_pose.header.frame_id = group.get_planning_frame()
box_pose.pose.position.x = 0.3
box_pose.pose.position.y = -0.3
box_pose.pose.position.z = -0.25
box_pose.pose.orientation.w = 1.0
scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5))
rospy.sleep(2)
rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names()))
# Pose Target 2
rospy.loginfo("Start Pose Target 2")
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
pose_target_2.position.y = -0.5
pose_target_2.position.z = 0.15
pose_target_2.orientation.x = 0.0
pose_target_2.orientation.y = -0.707
pose_target_2.orientation.z = 0.0
pose_target_2.orientation.w = 0.707
class WorldManagerServer:
def __init__(self):
rospy.init_node('world_manager_node')
moveit_commander.roscpp_initialize(sys.argv)
# wait for moveit to come up
self.scene = PlanningSceneInterface()
self.tf_manager = TFManager()
self.clear_planning_scene_service = rospy.Service(
"/world_manager/clear_objects", std_srvs.srv.Empty,
self.clear_objects_cb)
self.add_box = rospy.Service("/world_manager/add_box",
world_manager_msgs.srv.AddBox,
self.add_box_cb)
self.add_mesh = rospy.Service("/world_manager/add_mesh",
world_manager_msgs.srv.AddMesh,
self.add_mesh_cb)
self.add_tf_service = rospy.Service("/world_manager/add_tf",
world_manager_msgs.srv.AddTF,
self.add_tf_cb)
self.add_walls_service = rospy.Service("/world_manager/add_walls",
std_srvs.srv.Empty,
self.add_walls_cb)
rospy.sleep(1.0)
self.clear_objects_cb(request=None)
self.add_walls_cb(request=None)
rospy.loginfo("World Manager Node is Up and Running")
def add_mesh_cb(self, request):
so = request.scene_object
# add the tf
self.tf_manager.add_tf(so.object_name, so.pose_stamped)
# remove the old completion if it is there
self.scene.remove_world_object(so.object_name)
# add the new object to the planning scene
self.scene.add_mesh(so.object_name, so.pose_stamped, so.mesh_filepath)
return []
def add_box_cb(self, request):
# type: (world_manager_msgs.srv.AddBoxRequest) -> []
box = request.scene_box
# type: box -> world_manager_msgs.msg.SceneBox
# add the tf
self.tf_manager.add_tf(box.object_name, box.pose_stamped)
# remove the old box if it is there
self.scene.remove_world_object(box.object_name)
# add the new box to the planning scene
self.scene.add_box(name=box.object_name,
pose=box.pose_stamped,
size=(box.edge_length_x, box.edge_length_y,
box.edge_length_z))
rospy.loginfo("Added box {}".format(box.object_name))
return []
def add_tf_cb(self, request):
self.tf_manager.add_tf(request.frame_name, request.pose_stamped)
return []
def clear_objects_cb(self, request):
body_names = self.scene.get_known_object_names()
walls = rospy.get_param('walls')
rospy.loginfo("Clearing objects: {}".format(body_names))
for body_name in body_names:
if not body_name in walls:
rospy.loginfo("Removing object: {}".format(body_name))
self.scene.remove_world_object(body_name)
self.tf_manager.clear_tfs()
return []
def add_walls_cb(self, request):
walls = rospy.get_param('walls')
for wall_params in walls:
rospy.loginfo("Adding wall " + str(wall_params))
self._add_wall(wall_params)
return []
def _add_wall(self, wall_params):
name = wall_params["name"]
x_thickness = wall_params["x_thickness"]
y_thickness = wall_params["y_thickness"]
z_thickness = wall_params["z_thickness"]
x = wall_params["x"]
y = wall_params["y"]
z = wall_params["z"]
qx = wall_params["qx"]
qy = wall_params["qy"]
qz = wall_params["qz"]
qw = wall_params["qw"]
frame_id = wall_params["frame_id"]
back_wall_pose = geometry_msgs.msg.PoseStamped()
back_wall_pose.header.frame_id = '/' + frame_id
wall_dimensions = [x_thickness, y_thickness, z_thickness]
back_wall_pose.pose.position = geometry_msgs.msg.Point(**{
'x': x,
'y': y,
'z': z
})
back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{
'x': qx,
'y': qy,
'z': qz,
'w': qw
})
self.scene.add_box(name, back_wall_pose, wall_dimensions)
class PickAndPlace:
def setColor(self,name,r,g,b,a=0.9):
color=ObjectColor()
color.id=name
color.color.r=r
color.color.g=g
color.color.b=b
color.color.a=a
self.colors[name]=color
def sendColors(self):
p=PlanningScene()
p.is_diff=True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def setupSence(self):
#box1_size=[0.1,0.05,0.03]
box2_size=[0.05,0.05,0.1]
r_tool_size=[0.03,0.01,0.06]
l_tool_size=[0.03,0.01,0.06]
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=0.75
table_pose.pose.position.y=0.0
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=self.arm_end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.04
l_p.pose.position.z=0.04
l_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=self.arm_end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.04
r_p.pose.position.z=0.04
r_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,'r_tool',r_p,r_tool_size)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
#self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
#self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
cartesian=rospy.get_param('~cartesian',True)
#gripper=MoveGroupCommander('gripper')
self.arm_end_effector_link=arm.get_end_effector_link()
#gripper_end_effector_link=gripper.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
#gripper.set_goal_position_tolerance(0.01)
#gripper.set_goal_orientation_tolerance(0.05)
#gripper.allow_replanning(True)
self.reference_frame='base_link'
arm.set_pose_reference_frame(self.reference_frame)
#gripper.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#gripper.set_planning_time(5)
#scene planning
self.table_id='table'
self.box2_id='box2'
self.target_id='target_object'
#scene.remove_world_object(box1_id)
self.scene.remove_world_object(self.box2_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
self.table_ground=0.5
self.table_size=[0.5,1,0.01]
self.setupSence()
target_size=[0.05,0.05,0.1]
#target
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=0.6
target_pose.pose.position.y=0.05
target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
self.scene.add_box(self.target_id,target_pose,target_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y=-0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
#set color
self.setColor(self.table_id,0.8,0,0,1.0)
#self.setColor(self.box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor(self.target_id,0,1,0)
self.sendColors()
#motion planning
arm.set_named_target("slight")
arm.go()
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
start_pose=arm.get_current_pose(self.arm_end_effector_link).pose
rospy.sleep(2)
grasp_pose=target_pose
grasp_pose.pose.position.x-=0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x=0
grasp_pose.pose.orientation.y=0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
waypoints_1=[]
waypoints_2=[]
if cartesian:
waypoints_1.append(start_pose)
waypoints_1.append(deepcopy(grasp_pose.pose))
#waypoints_1.append(deepcopy(grasp_pose.pose))
fraction=0.0
#maxtries=300
attempts=0
#arm.set_start_state__1to_current_state()
#plan the cartesian path connecting waypoints
#while fraction<1.0 and attempts<maxtries:
while fraction!=1:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1.0):
rospy.loginfo("path planning failed with " + str(attempts)+" attempts")
if KeyboardInterrupt:
break
rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.")
rospy.loginfo("Arm moving.")
arm.execute(plan_1)
rospy.sleep(6)
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class PickAndPlace:
def setColor(self,name,r,g,b,a=0.9):
color=ObjectColor()
color.id=name
color.color.r=r
color.color.g=g
color.color.b=b
color.color.a=a
self.colors[name]=color
def sendColors(self):
p=PlanningScene()
p.is_diff=True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self,traj, time, positions, velocities=None):
point= trajectory_msgs.msg.JointTrajectoryPoint()
point.positions= copy.deepcopy(positions)
if velocities is not None:
point.velocities= copy.deepcopy(velocities)
point.time_from_start= rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self,q_traj, t_traj):
assert(len(q_traj)==len(t_traj))
#Insert current position to beginning.
if t_traj[0]>1.0e-2:
t_traj.insert(0,0.0)
q_traj.insert(0,self.Q(arm=arm))
self.dq_traj=self.QTrajToDQTraj(q_traj, t_traj)
#self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj
#print traj
#self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj))
def QTrajToDQTraj(self,q_traj, t_traj):
dof= len(q_traj[0])
#Modeling the trajectory with spline.
splines= [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)]
splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj= []
for t in t_traj:
dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self,q,t,dq=None):
jp= trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions= q
jp.time_from_start= rospy.Duration(t)
if dq is not None: jp.velocities= dq
return jp
def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None):
assert(len(q_traj)==len(t_traj))
if dq_traj is not None: (len(dq_traj)==len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names= joint_names
if dq_traj is not None:
self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)]
else:
self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)]
self.traj.header.stamp= rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self,q_traj):
if len(q_traj)==0: return
q_prev= np.array(q_traj[0])
q_offset= np.array([0]*len(q_prev))
for q in q_traj:
q_diff= np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d]<-math.pi: q_offset[d]+=1
elif q_diff[d]>math.pi: q_offset[d]-=1
q_prev= copy.deepcopy(q)
q[:]= q+q_offset*2.0*math.pi
def add_target(self,f_target_pose,f_target_size,frame,x,y,o1,o2,o3,o4):
f_target_pose.header.frame_id=frame
f_target_pose.pose.position.x=x
f_target_pose.pose.position.y=y
f_target_pose.pose.position.z=self.table_ground+self.table_size[2]+f_target_size[2]/2.0
f_target_pose.pose.orientation.x=o1
f_target_pose.pose.orientation.y=o2
f_target_pose.pose.orientation.z=o3
f_target_pose.pose.orientation.w=o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self,start_pose,end_pose,maxtries,exe_signal=False):
waypoints=[]
fraction=0.0
attempts=0
#maxtries_z=300
waypoints.append(start_pose)
waypoints.append(end_pose)
while fraction!=1 and attempts<maxtries:
(plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True)
attempts+=1
if (attempts %maxtries==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
return 0,0,0
continue
elif fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.")
#print(plan.joint_trajectory.points[-5].velocities)
#for i in range(len(plan.joint_trajectory.points)):
#plan.joint_trajectory.points[i].velocities=[0,0,0,0,0,0,0]
#plan = self.arm.retime_trajectory(self.robot.get_current_state(), plan, 1.0)
rospy.sleep(3)
if exe_signal:
q_traj=[self.arm.get_current_joint_values()]
t_traj=[0.0]
for i in range(2,len(plan.joint_trajectory.points)):
q_traj.append(plan.joint_trajectory.points[i].positions)
t_traj.append(t_traj[-1]+0.03)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(5)
#self.sub_jpc.publish(plan)
#self.arm.execute(plan)
end_joint_state=plan.joint_trajectory.points[-1].positions
signal=1
return plan,end_joint_state, signal
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self,initial_state,start_joint_state,end_joint_state,freq,times):
q_traj=[initial_state]
t_traj=[0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1]+0.5/freq)
q_traj.append(start_joint_state)
t_traj.append(t_traj[-1]+0.5/freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1]+0.5/freq)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(6)
def setupSence(self):
r_tool_size=[0.03,0.02,0.18]
l_tool_size=[0.03,0.02,0.18]
#real scene table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=-0.0
table_pose.pose.position.y=0.65
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=self.arm_end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.057
l_p.pose.position.z=0.09
l_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=self.arm_end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.057
r_p.pose.position.z=0.09
r_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size)
def grasp_pose(self,x,y,z,r,theta):
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=x+r*math.sin(theta)
g_p.pose.position.y=y-r*math.cos(theta)
g_p.pose.position.z=z+0.05
g_p.pose.orientation.w=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta))
g_p.pose.orientation.x=-0.5*(math.cos(0.5*theta)+math.sin(0.5*theta))
g_p.pose.orientation.y=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta))
g_p.pose.orientation.z=0.5*(math.sin(0.5*theta)+math.cos(0.5*theta))
return g_p
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10)
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
#self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors=dict()
rospy.sleep(1)
self.robot=moveit_commander.robot.RobotCommander()
self.arm=MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian=rospy.get_param('~cartesian',True)
self.arm_end_effector_link=self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame='base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names= [[]]
self.joint_names[0]= rospy.get_param('controller_joint_names')
self.traj= trajectory_msgs.msg.JointTrajectory()
self.sub_jpc= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory,queue_size=10)
#scene planning
self.l_id='l_tool'
self.r_id='r_tool'
self.table_id='table'
self.target1_id='target1_object'
self.target2_id='target2_object'
self.target3_id='target3_object'
self.target4_id='target4_object'
self.f_target_id='receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
self.scene.remove_world_object(self.target3_id)
self.scene.remove_world_object(self.target4_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground=0.13
self.table_size=[0.9,0.6,0.018]
self.setupSence()
target1_size=[0.035,0.035,0.19]
target2_size=target1_size
target3_size=target1_size
target4_size=target1_size
self.f_target_size=[0.2,0.2,0.04]
f_target_pose=PoseStamped()
pre_pour_pose=PoseStamped()
target1_pose=PoseStamped()
target2_pose=PoseStamped()
target3_pose=PoseStamped()
target4_pose=PoseStamped()
joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
joint_names= rospy.get_param('controller_joint_names')
traj= trajectory_msgs.msg.JointTrajectory()
traj.joint_names= joint_names
#final target
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.3,0.6,0,0,0,1)
self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#pouring pose
pour_pose=f_target_pose
pour_pose.pose.position.x-=0.06
pour_pose.pose.position.y-=0.12
pour_pose.pose.position.z+=0.15
pour_pose.pose.orientation.x=-0.5
pour_pose.pose.orientation.y=-0.5
pour_pose.pose.orientation.z=-0.5
pour_pose.pose.orientation.w=0.5
#attach_pose
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y=-0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
#set color
self.setColor(self.target1_id,0.8,0,0,1.0)
self.setColor(self.target2_id,0.8,0,0,1.0)
self.setColor(self.target3_id,0.8,0,0,1.0)
self.setColor(self.target4_id,0.8,0,0,1.0)
self.setColor(self.f_target_id,0.8,0.3,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(5)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state=[-1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393]
signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#target localization
tar_num=4
maxtries=300
r=0.16
#msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
self.add_target(target1_pose,target1_size,self.reference_frame,-0.25,0.8,0,0,0,1)
self.scene.add_box(self.target1_id,target1_pose,target1_size)
self.add_target(target2_pose,target2_size,self.reference_frame,-0.12,0.87,0,0,0,1)
self.scene.add_box(self.target2_id,target2_pose,target2_size)
self.add_target(target3_pose,target3_size,self.reference_frame,0.02,0.88,0,0,0,1)
self.scene.add_box(self.target3_id,target3_pose,target3_size)
self.add_target(target4_pose,target4_size,self.reference_frame,0.12,0.81,0,0,0,1)
self.scene.add_box(self.target4_id,target4_pose,target4_size)
#path planning
#input : target pose
#output : policy and execute
#cts : output : plan, end_joint_state, signal
#m=0
for i in range(tar_num):
#grasp target
if i==0:
target_pose=target1_pose.pose
target_id=self.target1_id
target_size=target1_size
elif i==1:
target_pose=target2_pose.pose
target_id=self.target2_id
target_size=target2_size
elif i==2:
target_pose=target3_pose.pose
target_id=self.target3_id
target_size=target3_size
elif i==3:
target_pose=target4_pose.pose
target_id=self.target4_id
target_size=target4_size
#grasp :
#-- if x>0: theta range ()
initial_angle=int(np.arctan(abs(target_pose.position.x/target_pose.position.y))*100)
for theta in range(initial_angle,157,8):
#m+=1
if target_pose.position.x>0: theta*=-1
#if m%2==0: theta*=-1
theta/=100.0
start_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose
grasp_pose=self.grasp_pose(target_pose.position.x,target_pose.position.y,target_pose.position.z,r,theta)
#pick up
if cartesian:
plan_1, end_joint_state_1,signal_1=self.cts(start_pose,grasp_pose.pose,maxtries,True)
if signal_1==0: continue
break
rospy.sleep(2)
self.gripperCtrl(0)
rospy.sleep(2)
self.gripperCtrl(255)
rospy.sleep(2)
self.arm.set_start_state_to_current_state()
back_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose
#rospy.sleep(10)
if cartesian:
plan_2, end_joint_state_2,signal_2=self.cts(back_pose,start_pose,maxtries,True)
#shaking
"""
amp=
freq=
r_angle=
start_pick_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose
start_joint_state=self.arm.get_current_joint_values()
shift_x_pose=deepcopy(start_pick_pose)
shift_z_pose=deepcopy(start_pick_pose)
shift_x_pose.position.x+=0.04
shift_z_pose.position.z-=0.04
#exe_sig=False
if cartesian:
plan_z,end_joint_state_z,signal_z=self.cts(start_pick_pose,shift_z_pose,300)
plan_x,end_joint_state_x,signal_x=self.cts(start_pick_pose,shift_x_pose,300)
#shaking process
#signal_x=False
shake_times=0
#self.freq=3.0
signal=True
while signal:
if (shake_times%3!=0 and signal_x) or (shake_times%3==0 and not signal_z):
self.shaking(start_joint_state,end_joint_state_x,3,3)
elif shake_times%3==0 and signal_z:
self.shaking(start_joint_state,end_joint_state_z,3,1)
shake_times+=1
rospy.loginfo("shaking times : "+str(shake_times)+ " .")
if shake_times==10: signal=False
"""
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# p.pose.position.x = 0.8
# p.pose.position.y = -0.15
# p.pose.position.z = 0.85
# p.pose.orientation.w = 1.0
#scene.add_box("pole", p, (0.3, 0.05, 1.0))
# p.pose.position.x = 0.79
# p.pose.position.y = 0.15
# p.pose.position.z = 0.70
# add_result = scene.add_box("table", p, (0.6, 0.6, 0.15))
# print add_result
p.pose.position.x = 0.6
p.pose.position.y = 0.18
p.pose.position.z = 0.7
scene.add_box("part1", p, (0.05, 0.05, 0.1 ))
robot.right_arm.set_named_target("right_pregrasp")
robot.right_arm.go()
rospy.sleep(3)
gripper.gripper_open()
rospy.sleep(3)
# pick an object
robot.right_arm.pick("part1")
rospy.sleep(1)
gripper.gripper_close()
rospy.sleep(1)
robot.right_arm.set_named_target("right_normal")
class Pick_Place:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.06
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = 0.0
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (9, 9, 0.02))
return p.pose
def _add_grasp_block_(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0.05
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.1, 0.1, 0.1))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' %
self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' %
self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' %
self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def _add_objects(self):
"""
Here add all the objects that you want to add to the _scene
"""
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
class PigeonPickAndPlace:
#Class constructor (parecido com java, inicializa o que foi instanciado)
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
def __del__(self):
# Clean the scene
self._scene.remove_world_object(self._grasp_object_name)
def _add_object_grasp(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame() #pose
p.header.stamp = rospy.Time.now()
rospy.loginfo(self._robot.get_planning_frame())
p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
#p.pose.orientation.x = 0.0028925
#p.pose.orientation.y = -0.0019311
#p.pose.orientation.z = -0.71058
#p.pose.orientation.w = 0.70361
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/,
# using the measure tape tool from meshlab.
# The box is the bounding box of the lego cylinder.
# The values are taken from the cylinder base diameter and height.
# the size is length (x),thickness(y),height(z)
# I don't know if the detector return this values of object
self._scene.add_box(name, p, (0.032, 0.016, 0.020))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
#rospy.logerr('Generated: %f grasps:' % grasps.size)
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
#goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_object_grasp, 0.016)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
class PickAndPlaceServer(object):
def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param(
'~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn(
"Didn't find any links to allow contacts... at param ~links_to_allow_contact"
)
else:
rospy.loginfo("Found links to allow contacts: " +
str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer('/pickup_pose',
PickUpPoseAction,
execute_cb=self.pick_cb,
auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer('/place_pose',
PickUpPoseAction,
execute_cb=self.place_cb,
auto_start=False)
self.place_as.start()
def pick_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.grasp_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(p_res)
else:
self.pick_as.set_succeeded(p_res)
def place_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.place_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(p_res)
else:
self.place_as.set_succeeded(p_res)
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo("Waiting for object '" + object_name +
"'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
#Add object description in scene
self.scene.add_box(
"part", object_pose,
(self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_width / 2
table_width = 1.8
table_depth = 0.5
table_pose.pose.position.z += -(
2 * self.object_width) / 2 - table_height / 2
table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose,
(table_depth, table_width, table_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("table")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal("arm_torso", "part", object_pose,
possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo("Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val
def place_object(self, object_pose):
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
possible_placings = self.sg.create_placings_from_object_pose(
object_pose)
# Try only with arm
rospy.loginfo("Trying to place using only arm")
goal = createPlaceGoal(object_pose, possible_placings, "arm", "part",
self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo(str(moveit_error_dict[result.error_code.val]))
if str(moveit_error_dict[result.error_code.val]) != "SUCCESS":
rospy.loginfo("Trying to place with arm and torso")
# Try with arm and torso
goal = createPlaceGoal(object_pose, possible_placings, "arm_torso",
"part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.logerr(str(moveit_error_dict[result.error_code.val]))
# print result
rospy.loginfo("Result: " +
str(moveit_error_dict[result.error_code.val]))
rospy.loginfo("Removing previous 'part' object")
self.scene.remove_world_object("part")
return result.error_code.val
rospy.loginfo("Cleaning world objects")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.65
p.pose.orientation.w = 1.0
scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.4
p.pose.position.y = -0.3
p.pose.position.z = 1.15
angle = radians(80) # angles are expressed in radians
quat = quaternion_from_euler(0.0, 0.0, angle) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat.tolist())
#scene.add_box("part", p, (0.04, 0.04, 0.1))
scene.add_box("part", p, (0.03, 0.03, 0.1))
rospy.loginfo("Added object to world")
rospy.sleep(1)
rospy.loginfo("Cleaning world objects")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.65
p.pose.orientation.w = 1.0
scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.4
p.pose.position.y = -0.3
p.pose.position.z = 1.15
angle = radians(80) # angles are expressed in radians
quat = quaternion_from_euler(0.0, 0.0, angle) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat.tolist())
#scene.add_box("part", p, (0.04, 0.04, 0.1))
scene.add_box("part", p, (0.03, 0.03, 0.1))
rospy.loginfo("Added object to world")
rospy.sleep(1)
#move(right, pr)
move(left, pl)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("bowl")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.4
p.pose.position.y = -0.9
p.pose.position.z = -0.4
scene.add_box("table", p, (1.6, 0.8, 0.4))
# p.pose.position.x = 0.8
# p.pose.position.y = 0.0
# p.pose.position.z = -0.02
# scene.add_box("bowl", p, (0.8, 0.01, 0.05))
#scene.remove_world_object("part")
rospy.sleep(1)
print ("Standby at initial position")
# camera
# open camera
# rh_cam=CameraController("right_hand_camera")
# lh_cam=CameraController("left_hand_camera")
# h_cam=CameraController("head_camera")
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv) # initialise moveit
rospy.init_node('lm_move', anonymous=True) # create a node
arm = moveit_commander.MoveGroupCommander("right_manipulator")
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'rightbase_link'
arm.set_pose_reference_frame(reference_frame)
# 控制机械臂先回到初始化位置
# arm.set_named_target('right_up')
# arm.go()
# rospy.sleep(5)
wall_size = [2, 0.05, 2]
wall_pose = PoseStamped()
wall_pose.header.frame_id = 'world'
wall_pose.pose.position.x = 0
wall_pose.pose.position.y = -0.20
wall_pose.pose.position.z = 1
wall_pose.pose.orientation.w = 0
# 将桌子设置成红色，两个box设置成橙色
self.setColor('wall', 255, 255, 255, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
scene.add_box('wall', wall_pose, wall_size)
Pose_goal = PoseStamped()
Pose_goal.header.frame_id = 'rightbase_link'
Pose_goal.pose.position.x = -0.234666801624 # red line 0.2 0.2
Pose_goal.pose.position.y = 0.463673741637 # green line 0.15 0.15
Pose_goal.pose.position.z = 0.23632011805 # blue line # 0.35 0.6
Pose_goal.pose.orientation.x = -0.223798960073
Pose_goal.pose.orientation.y = -0.726641725375
Pose_goal.pose.orientation.z = -0.586249390912
Pose_goal.pose.orientation.w = 0.279673881636
arm.set_pose_target(Pose_goal)
arm.go(True)
rospy.sleep(3)
arm.set_named_target('right_up')
arm.go()
rospy.sleep(3)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# 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)
class MoveitMoveObstacle(object):
"""move obstacle
"""
def __init__(self):
# 设置参考坐标系
self.reference_frame = 'base_link'
# 初始化场景
self.scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 移除已存在障碍物
self.scene.remove_world_object()
# 添加障碍物
self.add_obstacles()
def add_obstacles(self):
"""
在场景中增加障碍物
"""
# 在场景中添加一个立方体
self.box1_id = 'box1'
self.box1_size = [0.1, 0.5, 0.2]
self.box1_pose = PoseStamped()
self.box1_pose.header.frame_id = self.reference_frame
self.box1_pose.pose.position.x = 0.5
self.box1_pose.pose.position.y = 0.6
self.box1_pose.pose.position.z = 0.3
self.box1_pose.pose.orientation.w = 1.0
self.scene.add_box(self.box1_id, self.box1_pose, self.box1_size)
self.setColor(self.box1_id, 0.8, 0.4, 0, 1.0)
# 在场景中添加一个桌子
self.desk1_id = 'desk1'
self.desk1_size = [3, 3, 0.1]
self.desk1_pose = PoseStamped()
self.desk1_pose.header.frame_id = self.reference_frame
self.desk1_pose.pose.position.x = 0.0
self.desk1_pose.pose.position.y = 0.0
self.desk1_pose.pose.position.z = -0.25
self.desk1_pose.pose.orientation.w = 1.0
self.scene.add_box(self.desk1_id, self.desk1_pose, self.desk1_size)
self.setColor(self.desk1_id, 0.1, 0.5, 0.1, 1.0)
# 在桌子上添加一个底座
self.base1_id = 'base1'
self.base1_size = [0.2, 0.2, 0.2]
self.base1_pose = PoseStamped()
self.base1_pose.header.frame_id = self.reference_frame
self.base1_pose.pose.position.x = 0.0
self.base1_pose.pose.position.y = 0.0
self.base1_pose.pose.position.z = -0.1
self.base1_pose.pose.orientation.w = 1.0
self.scene.add_box(self.base1_id, self.base1_pose, self.base1_size)
self.setColor(self.base1_id, 0.5, 0.5, 0, 1.0)
# 发送场景消息
self.sendColors()
rospy.sleep(1)
def setColor(self, name, r, g, b, a=0.9):
"""
设置场景物体的颜色
"""
# 初始化moveit颜色对象
color = ObjectColor()
# 设置颜色值
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# 更新颜色字典
self.colors[name] = color
def sendColors(self):
"""
将颜色设置发送并应用到moveit场景当中
"""
# 初始化规划场景对象
p = PlanningScene()
# 需要设置规划场景是否有差异
p.is_diff = True
# 从颜色字典中取出颜色设置
for color in self.colors.values():
p.object_colors.append(color)
# 发布场景物体颜色设置
self.scene_pub.publish(p)
def move_obstacle(self):
"""移动障碍物
"""
# 在场景中增加物体
for i in range(100):
self.scene.add_box(self.box1_id, self.box1_pose, self.box1_size)
self.scene.add_box(self.desk1_id, self.desk1_pose, self.desk1_size)
self.scene.add_box(self.base1_id, self.base1_pose, self.base1_size)
self.sendColors()
rospy.sleep(0.1)
# 连续移动障碍物
def move_box1_continuous(start_pose, end_pose, pose_number,
sleep_time):
rospy.loginfo('start move box1...')
poses = self.get_3d_linspace(start_pose, end_pose, pose_number)
for p in poses:
rospy.loginfo('new pose:\n {}'.format(p))
self.scene.remove_world_object(self.box1_id)
self.scene.add_box(self.box1_id, p, self.box1_size)
rospy.sleep(sleep_time)
# 把障碍物从pose_0移动到pose_1， 分5步走完，每步停顿0.2秒
#rospy.sleep(7)
pose_0 = copy.deepcopy(self.box1_pose)
pose_1 = copy.deepcopy(pose_0)
pose_1.pose.position.y -= 0.9
move_box1_continuous(pose_0, pose_1, 80, 0.6)
# 把障碍物从pose_2移动到pose_3， 分10步走完，每步停顿0.5秒
# pose_1 pose_2
pose_2 = copy.deepcopy(pose_1)
# 沿x轴移动
pose_2.pose.position.x += 0.25
pose_2.pose.position.y += 0.45
#pose_2.pose.position.z += 0.3
move_box1_continuous(pose_1, pose_2, 10, 0.6)
#rospy.sleep(10)
# 如果还需要增加路径，按照上面的方法继续写
# pose_2 pose_3
#pose_2.pose.orientation.x += 0.15
pose_3 = copy.deepcopy(pose_2)
#pose_3.pose.position.x += 0.4
pose_3.pose.position.y += 0.3
#pose_3.pose.position.z -= 0.2
move_box1_continuous(pose_2, pose_3, 10, 0.6)
#rospy.sleep(20)
#pose_3 --pose_4
#pose_3.pose.orientation.x -= 0.6
pose_4 = copy.deepcopy(pose_3)
pose_4.pose.position.x -= 0.1
pose_4.pose.position.y -= 0.6
#pose_4.pose.position.x -= 0.4
pose_4.pose.position.z += 0.3
#pose_4.pose.position.z -= 0.8
self.box1_size = [0.3, 0.5, 0.3]
self.setColor(self.box1_id, 0.8, 0.4, 0, 0.3)
# 发送场景消息
self.sendColors()
rospy.sleep(1)
move_box1_continuous(pose_3, pose_4, 10, 0.1)
#rospy.sleep(10)
pose_5 = copy.deepcopy(pose_4)
pose_5.pose.position.y += 0.9
#pose_4.pose.position.x -= 0.4
#pose_4.pose.position.z -= 0.8
move_box1_continuous(pose_4, pose_5, 10, 0.6)
pose_6 = copy.deepcopy(pose_5)
pose_6.pose.position.y -= 0.9
pose_6.pose.position.z -= 0.3
#pose_4.pose.position.x -= 0.4
#pose_4.pose.position.z -= 0.8
move_box1_continuous(pose_5, pose_6, 10, 0.6)
#rospy.sleep(10)
#pose_4 pose_5
#pose_5 = copy.deepcopy(pose_4)
#pose_5.pose.position.y -= 0.4
#pose_5.pose.position.z += 0.4
#move_box1_continuous(pose_4, pose_5, 7, 0.8)
#rospy.sleep(10)
def get_3d_linspace(self, start_p, end_p, p_number):
"""calculate 3d linspace between 2 points
:param start_p: PoseStamped()
:param end_p: PoseStamped()
:param p_number: points number
:return: points list
"""
x_new = np.linspace(start_p.pose.position.x, end_p.pose.position.x,
p_number)
y_new = np.linspace(start_p.pose.position.y, end_p.pose.position.y,
p_number)
z_new = np.linspace(start_p.pose.position.z, end_p.pose.position.z,
p_number)
points = []
for i in range(len(x_new)):
p = copy.deepcopy(start_p)
p.pose.position.x = float(x_new[i])
p.pose.position.y = float(y_new[i])
p.pose.position.z = float(z_new[i])
points.append(p)
return points
if __name__ == '__main__':
rospy.init_node('scripting_example')
while rospy.get_time() == 0.0: pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
### Move to Cartesian position
goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
mgc.go(goal_pose.pose)
### Move Cartesian linear
goal_pose.pose.position.z -= 0.1
(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
mgc.execute(traj)
class Pick_Place:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:创建（调试）发布者：
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:创建规划场景和机器人命令：
self._scene = PlanningSceneInterface() #未知
self._robot = RobotCommander() #未知
rospy.sleep(0.5)
# Clean the scene:清理现场：
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景：
self._pose_table = self._add_table(self._table_object_name) #增加桌子
self._pose_coke_can = self._add_grasp_block_(
self._grasp_object_name) #增加障碍物块
rospy.sleep(0.5)
# Define target place pose:定义目标位置姿势
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.20
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._arm.set_named_target('up')
self._arm.go(wait=True)
print("up pose")
print("第一部分恢复位置初始")
# Create grasp generator 'generate' action client:
# #创建抓取生成器“生成”动作客户端：
print("开始Action通信")
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
print("结束Action通信")
# Create move group 'pickup' action client:
# 创建移动组“抓取”操作客户端：
print("开始pickup通信")
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
print("结束pickup通信")
# Create move group 'place' action client:
# 创建移动组“放置”动作客户端：
print("开始place通信")
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
print("结束place通信")
# Pick Coke can object:抓取快
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(0.5)
print("抓取物体")
rospy.loginfo('Pick up successfully')
print("pose_place: ", self._pose_place)
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(0.5)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
"""
Create and add table to the scene创建桌子并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.85
p.pose.position.y = 0.0
p.pose.position.z = 0.70
q = quaternion_from_euler(
0.0, 0.0, numpy.deg2rad(90.0)) #numpy.deg2rad(x)：度转换为弧度。
#旋转变换，调了 tf.transformations.quaternion_from_euler函数，将（0,0，msg.theta）转换成了quaternion四元数。
p.pose.orientation = Quaternion(*q) #转换成了quaternion四元数
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (1, 1, 0.05))
return p.pose
def _add_grasp_block_(self, name):
"""
Create and add block to the scene创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.74
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.045, 0.045, 0.045))
return p.pose
def _generate_grasps(self, pose, width):
"""
使用抓握生成器生成动作来生成抓握； 基于moveit_simple_grasps pkg上的server_test.py示例。
生成抓取姿势阵列数据以进行可视化，
然后将抓取目标发送到抓取服务器.
"""
self._grasps_ac.wait_for_server()
rospy.loginfo("Successfully connected!")
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
# Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary.
#
rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal))
#self._grasps_ac.wait_for_result()
#发送目标并等待结果：
# ＃将目标发送到ActionServer，等待目标完成，然后抢占目标是必要的。
t_start = rospy.Time.now()
state = self._grasps_ac.send_goal_and_wait(goal)
t_end = rospy.Time.now()
t_toal = t_end - t_start
rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec()))
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' %
self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
"""
print("---------grasps--start------------")
print(grasps)
print("-----------test-grasps-----end-----------------")
rospy.sleep(2)
print("-grasps---sleep----end-----")
"""
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
为该位置的姿势创建姿势数组数据
"""
# Generate places:
places = []
now = rospy.Time.now()
print("---------------test3--------------------------")
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:生成前置位置方法：
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.1
#print("place1---test=====")
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.06
#print("place2---test=====")
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
"""
print(places)
print("------test4---------------------------------------")
"""
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
Create a goal for picking up the grasping object创建一个捡起抓取物体的目标
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group #规划的组
goal.target_name = target #要拾取的对象的名称（
#可能使用的抓握列表。 必须至少填写一个掌握
goal.possible_grasps.extend(grasps)
#障碍物的可选清单，我们掌握有关语义的信息，可以在抓取过程中触摸/推动/移动这些障碍物； 注意：如果使用对象名称“ all”，则在进近和提升过程中禁止与所有对象发生碰撞。
goal.allowed_touch_objects.append(target)
#如果没有可用的名称，则在碰撞图中支撑表面（例如桌子）的名称可以留空
goal.support_surface_name = self._table_object_name
# Configure goal planning options:配置目标计划选项：
#运动计划者可以规划的最长时间
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
"""
print("----goal-------test-----")
print(goal)
print("----goal---end------test---")
"""
rospy.sleep(0.5)
print("-goal---sleep----end-----")
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
Create a place goal for MoveIt!
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group使用规划小组选择目标
"""
# Obtain possible grasps from the grasp generator server:从掌握生成器服务器获取可能的抓握：
grasps = self._generate_grasps(self._pose_coke_can, width)
print("--goal--start----")
# Create and send Pickup goal:创建并发送zhua取目标：
goal = self._create_pickup_goal(group, target, grasps)
print("--goal--end----")
print("---pick---up---1----")
state = self._pickup_ac.send_goal_and_wait(goal)
print("-----------------state-------------------")
print(state)
print("---pick---up---2----")
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' %
self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
"""
print("-----test1--------")
print(result)
print("------test1--end------")
"""
# Check for error:
err = result.error_code.val
print(err)
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
print("pickup-----end-----")
rospy.sleep(0.5)
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:获取可能的位置：
places = self._generate_places(place)
# Create and send Place goal:创建并发送地方目标：
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' %
self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
"""
print("-----test2--------")
print(result)
print("------test2--end------")
"""
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message使用PoseArray消息将抓取发布为姿势
"""
print(self._grasps_pub.get_num_connections())
if self._grasps_pub.get_num_connections() != 1:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
print(msg)
rospy.loginfo('Publisher ' + str(len(msg.poses)) + ' poses')
self._grasps_pub.publish(msg)
"""
print("11-111")
rospy.loginfo('Publisher'+str(len(msg))+'poses')
print("11-111")
rospy.sleep(2)
"""
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("ball")
scene.remove_world_object("table")
scene.remove_world_object("ground_plane")
rospy.sleep(5)
print ">>>>> add scenes"
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = -1
p.pose.position.z = 0.2
p.pose.orientation.w = 1.0
scene.add_box("pole", p, (0.4, 0.1, 0.4))
p.pose.position.x = 0
p.pose.position.y = 0.8
p.pose.position.z = 0.1
p.pose.orientation.w = 1.0
scene.add_sphere("ball", p, 0.1)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.1
scene.add_box("table", p, (1.0, 2.6, 0.2))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.4
robot = RobotCommander()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.0
scene.add_box("pole", p, (0.3, 0.1, 1.0))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
scene.add_box("table", p, (0.5, 1.5, 0.35))
p.pose.position.x = 0.6
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("part", p, (0.15, 0.1, 0.3))
rospy.sleep(1)
# pick an object
robot.right_arm.pick("part")
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
#bring the arm near the object
(aim_x, aim_y, aim_z,
aim_yaw) = onine_arm.get_valid_pose(item_translation[0],
item_translation[1],
item_translation[2], -0.20)
# onine_arm.ready()
# onine_arm.open_gripper()
# onine_arm.go(aim_x, aim_y, aim_z, aim_yaw)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = item_translation[0]
p.pose.position.y = item_translation[1]
p.pose.position.z = item_translation[2]
scene.add_box("target", p, (0.01, 0.01, 0.08))
# rospy.sleep(20)
# (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080)
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "left_gripper_link"
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.position.x = aim_x
grasp_pose.pose.position.y = aim_y
grasp_pose.pose.position.z = aim_z
grasp_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0.0, 0, aim_yaw))
g = Grasp()
g.pre_grasp_posture = onine_arm.make_gripper_posture(0.09)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(20)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.1)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(3)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
rospy.sleep(5)
# 设置桌面的高度
table_ground = 0.25
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 1.1, 0.01]
box1_size = [0.1, 0.05, 0.8]
box2_size = [0.05, 0.05, 0.8]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.46
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.41
box1_pose.pose.position.y = -0.3
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.39
box2_pose.pose.position.y = 0.3
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)
# 将场景中的颜色设置发布
self.sendColors()
# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.5
target_pose.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(5)
# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.4
target_pose2.pose.position.y = 0.5
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.4
target_pose2.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose2, end_effector_link)
arm.go()
rospy.sleep(5)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
robot_arm.set_goal_orientation_tolerance(0.005)
robot_arm.set_planning_time(5)
robot_arm.set_num_planning_attempts(5)
###eef = robot.get_end_effector_link() #-----error 01
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
robot_arm.allow_replanning(True)
#scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
#scene.remove_world_object("table")
#scene.remove_world_object("part")
robot_arm.set_named_target("home") #go to goal state.
robot_arm.go(wait=True)
'''
right_gripper.set_named_target("open")
right_gripper.go(wait=True)
'''
rospy.sleep(1)
print("====== move plan go to home 1 ======")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p.pose.position.x = 0.170
p.pose.position.y = 0.04
p.pose.position.z = 0.3
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = -0.00756585784256
start_pose.pose.position.y = -0.225419849157
start_pose.pose.position.z = 0.117192693055
start_pose.pose.orientation.x = 0.95493721962
start_pose.pose.orientation.y = -0.0160209629685
start_pose.pose.orientation.z = -0.00497157918289
start_pose.pose.orientation.w = 0.296333402395
print("going to pick up pose")
robot_arm.set_pose_target(start_pose)
#right_gripper.set_named_target("close")
robot_arm.go(wait=True)
#right_gripper.go(wait=True)
rospy.sleep(1)
print("====== move plan go to up ======")
''' test code 04/20'''
pose_zero = PoseStamped()
pose_zero.header.frame_id = FIXED_FRAME
pose_zero.pose.position.x = 0.0
pose_zero.pose.position.y = 0.194525
pose_zero.pose.position.z = 0.694149
pose_zero.pose.orientation.x = 0.0
pose_zero.pose.orientation.y = 0.0
pose_zero.pose.orientation.x = 0.707062
pose_zero.pose.orientation.x = 0.707062
robot_arm.set_pose_target(pose_zero)
robot_arm.go(wait=True)
rospy.sleep(3)
print("===== move plan test code =====")
''' '''
#robot_arm.set_named_target("up")
#robot_arm.go(wait=True)
next_pose = PoseStamped()
next_pose.header.frame_id = FIXED_FRAME
next_pose.pose.position.x = -0.100732862949
next_pose.pose.position.y = -0.210876911879
next_pose.pose.position.z = 0.244678631425
next_pose.pose.orientation.x = 0.784905433655
next_pose.pose.orientation.y = -0.177844554186
next_pose.pose.orientation.z = -0.131161093712
next_pose.pose.orientation.w = 0.578870952129
robot_arm.set_pose_target(next_pose)
#right_gripper.set_named_target("open")
robot_arm.go(wait=True)
#right_gripper.go(wait=True)
print("going to pick up pose 2")
rospy.sleep(3)
print("====== move plan go to home 3 ======")
robot_arm.set_named_target("home")
robot_arm.go(wait=True)
rospy.spin()
roscpp_shutdown()
class MoveItDemo:
def __init__(self):
global obj_att
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # Initialize the grasp object
## g = Grasp()
## grasps = []
## # Set the first grasp pose to the input pose
## g.grasp_pose = pre_grasping
## g.allowed_touch_objects = [target_id]
## grasps.append(deepcopy(g))
## right_arm.pick(target_id, grasps)
# #Change the frame_id for the planning to take place!
# #target_pose.header.frame_id = 'gazebo_world'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
class ProbotSorting:
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.arm = moveit_commander.MoveGroupCommander('manipulator')
self.end_effector_link = self.arm.get_end_effector_link()
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.allow_replanning(True)
self.arm.set_goal_position_tolerance(0.001)
self.arm.set_goal_orientation_tolerance(0.01)
# 初始化场景对象
self.scene = PlanningSceneInterface()
rospy.sleep(1)
# Initialize IO
self.ioPub = rospy.Publisher('probot_set_output_io', SetOutputIO, queue_size=1)
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(1)
# 移除场景中之前运行残留的物体
#scene.remove_attached_object(end_effector_link, 'tool')
self.scene.remove_world_object('table')
self.scene.remove_world_object('target')
# 设置桌面的高度
self.table_ground = 0.05
# 设置table和tool的三维尺寸
self.table_size = [0.04, 0.04, 1.2]
# 将table加入场景当中
self.table_pose = PoseStamped()
self.table_pose.header.frame_id = 'base_link'
self.table_pose.pose.position.x = -0.2
self.table_pose.pose.position.y = 0.2
self.table_pose.pose.position.z = 0.2
self.table_pose.pose.orientation.w = 1.0
self.scene.add_box('table', self.table_pose, self.table_size)
rospy.sleep(2)
# 更新当前的位姿
self.arm.set_start_state_to_current_state()
#########
def moveToHome(self):
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(1)
def moveTo(self, x, y, z):
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = x
target_pose.pose.position.y = y
target_pose.pose.position.z = z
target_pose.pose.orientation.x = 0.70692
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 0.70729
self.arm.set_start_state_to_current_state()
self.arm.set_pose_target(target_pose, self.end_effector_link)
traj = self.arm.plan()
self.arm.execute(traj)
def moveSingleJoint(self, index, value):
group_variable_values = self.arm.get_current_joint_values()
group_variable_values[index] = value
self.arm.set_joint_value_target(group_variable_values)
traj = self.arm.plan()
self.arm.execute(traj)
def pick(self, x, y):
self.moveTo(x, y, 0.09)
ioOutput0 = SetOutputIO()
ioOutput0.ioNumber = 1
ioOutput0.status = SetOutputIO.IO_HIGH
self.ioPub.publish(ioOutput0)
rospy.sleep(1)
ioOutput1 = SetOutputIO()
ioOutput1.ioNumber = 2
ioOutput1.status = SetOutputIO.IO_HIGH
self.ioPub.publish(ioOutput1)
rospy.sleep(1)
def place(self, x, y):
self.moveTo(x,y,0.15)
rospy.sleep(0.3)
ioOutput0 = SetOutputIO()
ioOutput0.ioNumber = 1
ioOutput0.status = SetOutputIO.IO_LOW
self.ioPub.publish(ioOutput0)
rospy.sleep(1)
ioOutput1 = SetOutputIO()
ioOutput1.ioNumber = 2
ioOutput1.status = SetOutputIO.IO_LOW
self.ioPub.publish(ioOutput1)
rospy.sleep(1)
def shutdown(self):
print "The sorting is shutting down"
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def ro(self,angle):
group_variable_values = self.arm.get_current_joint_values()
group_variable_values[0] = angle
self.arm.set_joint_value_target(group_variable_values)
traj = self.arm.plan()
# 按照规划的运动路径控制机械臂运动
self.arm.execute(traj)
rospy.sleep(1)
def __init__(self):
"""
Setup ROS communication between the Kinect and Baxter
:return:
"""
# Initialise variables
self.rgb_img = None
self.depth_img = None
self.marker_box = None
self.marker_center_x = None
self.marker_center_y = None
self.marker_depth = None
self.bridge = CvBridge()
# First initialize moveit_commander and rospy.
print "============ Initialising Baxter"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('can_node', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander("left_arm")
self.right_arm = moveit_commander.MoveGroupCommander("right_arm")
self.right_arm_navigator = Navigator('right')
# Setup grippers
self.right_gripper = baxter_interface.Gripper('right')
# Setup the table in the scene
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene)
table_id = 'table'
scene.remove_world_object(table_id)
rospy.sleep(1)
table_ground = -0.3
table_size = [4.0, 4.6, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = self.robot.get_planning_frame()
table_pose.pose.position.x = 0.7
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)
# Create a dictionary to hold object colors
self.colors = dict()
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.sendColors()
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
# We can get a list of all the groups in the robot
print "============ Right Pose:"
print self.right_arm.get_current_pose()
print "============ Left Pose:"
print self.left_arm.get_current_pose()
# Setup the subscribers and publishers
self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb)
self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.callback_points)
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=5)
self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10)
self.right_hand_range_pub = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1)
self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)
class PickAndPlace:
def setColor(self,name,r,g,b,a=0.9):
color=ObjectColor()
color.id=name
color.color.r=r
color.color.g=g
color.color.b=b
color.color.a=a
self.colors[name]=color
def sendColors(self):
p=PlanningScene()
p.is_diff=True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self,traj, time, positions, velocities=None):
point= trajectory_msgs.msg.JointTrajectoryPoint()
point.positions= copy.deepcopy(positions)
if velocities is not None:
point.velocities= copy.deepcopy(velocities)
point.time_from_start= rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self,q_traj, t_traj):
assert(len(q_traj)==len(t_traj))
#Insert current position to beginning.
if t_traj[0]>1.0e-2:
t_traj.insert(0,0.0)
q_traj.insert(0,self.Q(arm=arm))
self.dq_traj=self.QTrajToDQTraj(q_traj, t_traj)
#self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj
#print traj
#self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj))
def QTrajToDQTraj(self,q_traj, t_traj):
dof= len(q_traj[0])
#Modeling the trajectory with spline.
splines= [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)]
splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj= []
for t in t_traj:
dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
print dq_traj
return dq_traj
def JointNames(self):
#arm= 0
return self.joint_names[0]
def ROSGetJTP(self,q,t,dq=None):
jp= trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions= q
jp.time_from_start= rospy.Duration(t)
if dq is not None: jp.velocities= dq
return jp
def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None):
assert(len(q_traj)==len(t_traj))
if dq_traj is not None: (len(dq_traj)==len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names= joint_names
if dq_traj is not None:
self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)]
else:
self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)]
self.traj.header.stamp= rospy.Time.now()
print self.traj
return self.traj
def SmoothQTraj(self,q_traj):
if len(q_traj)==0: return
q_prev= np.array(q_traj[0])
q_offset= np.array([0]*len(q_prev))
for q in q_traj:
q_diff= np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d]<-math.pi: q_offset[d]+=1
elif q_diff[d]>math.pi: q_offset[d]-=1
q_prev= copy.deepcopy(q)
q[:]= q+q_offset*2.0*math.pi
def setupSence(self):
r_tool_size=[0.03,0.02,0.18]
l_tool_size=[0.03,0.02,0.18]
'''
#sim table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=0.75
table_pose.pose.position.y=0.0
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
'''
#real scene table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=-0.184
table_pose.pose.position.y=0.62
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=self.arm_end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.057
l_p.pose.position.z=0.09
l_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=self.arm_end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.057
r_p.pose.position.z=0.09
r_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=1)
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
cartesian=rospy.get_param('~cartesian',True)
self.arm_end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
self.reference_frame='base_link'
arm.set_pose_reference_frame(self.reference_frame)
arm.set_planning_time(5)
#scene planning
self.l_id='l_tool'
self.r_id='r_tool'
self.table_id='table'
self.target_id='target_object'
self.f_target_id='receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground=0.13
self.table_size=[0.9,0.6,0.018]
self.setupSence()
joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
joint_names= rospy.get_param('controller_joint_names')
traj= trajectory_msgs.msg.JointTrajectory()
traj.joint_names= joint_names
target_size=[0.058,0.058,0.19]
f_target_size=[0.2,0.2,0.04]
#final target
f_target_pose=PoseStamped()
f_target_pose.header.frame_id=self.reference_frame
f_target_pose.pose.position.x=-0.184+0.27
f_target_pose.pose.position.y=0.62+0.1
f_target_pose.pose.position.z=self.table_ground+self.table_size[2]+f_target_size[2]/2.0
f_target_pose.pose.orientation.x=0
f_target_pose.pose.orientation.y=0
f_target_pose.pose.orientation.z=0
f_target_pose.pose.orientation.w=1
self.scene.add_box(self.f_target_id,f_target_pose,f_target_size)
#pouring pose
pour_pose=f_target_pose
pour_pose.pose.position.x-=0.06
pour_pose.pose.position.y-=0.12
pour_pose.pose.position.z+=0.15
#pour_pose.pose.position.y+=0.17
pour_pose.pose.orientation.x=-0.5
pour_pose.pose.orientation.y=-0.5
pour_pose.pose.orientation.z=-0.5
pour_pose.pose.orientation.w=0.5
#set color
self.setColor(self.table_id,0.8,0,0,1.0)
self.setColor(self.f_target_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor(self.target_id,0,1,0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(5)
#j_ori_state=[0.72316974401474, 0.4691084027290344, 0.41043856739997864, -2.3381359577178955, 0.5580500364303589, 1.1085972785949707, 3.14]
j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_trans_state=[1.708616018295288, 0.9996442198753357, -0.4782222807407379, -1.7541601657867432, 0.13480804860591888, 1.1835496425628662, 2.689549684524536]
signal= True
arm.set_joint_value_target(j_ori_state)
arm.go()
rospy.sleep(3)
#arm.set_joint_value_target(j_trans_state)
#arm.go()
#rospy.sleep(5)
waypoints_1=[]
waypoints_2=[]
start_pick_pose=arm.get_current_pose(self.arm_end_effector_link).pose
if cartesian:
msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=-(msg.pose.position.y)-0.28
target_pose.pose.position.y=-msg.pose.position.x+0.81-0.072
target_pose.pose.position.z=1.36-msg.pose.position.z+0.1
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=-0
target_pose.pose.orientation.w=1
self.scene.add_box(self.target_id,target_pose,target_size)
self.setColor(self.target_id,0,0.8,0)
self.sendColors()
#pre_g_pose
pre_grasp_pose=PoseStamped()
pre_grasp_pose.header.frame_id=self.reference_frame
pre_grasp_pose.pose.position=target_pose.pose.position
pre_grasp_pose.pose.position.y-=0.2
pre_grasp_pose.pose.position.z+=0.01
pre_grasp_pose.pose.orientation.x=-0.5
pre_grasp_pose.pose.orientation.y=-0.5
pre_grasp_pose.pose.orientation.z=-0.5
pre_grasp_pose.pose.orientation.w=0.5
#grasp_pose
grasp_pose=PoseStamped()
grasp_pose.header.frame_id=self.reference_frame
grasp_pose.pose.position=target_pose.pose.position
grasp_pose.pose.position.y+=0.025
grasp_pose.pose.orientation.x=-0.5
grasp_pose.pose.orientation.y=-0.5
grasp_pose.pose.orientation.z=-0.5
grasp_pose.pose.orientation.w=0.5
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
waypoints_1.append(start_pick_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_1.append(deepcopy(grasp_pose.pose))
fraction=0.0
attempts=0
maxtries=300
while fraction!=1 and attempts<maxtries:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
signal_pick=False
if fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.")
rospy.loginfo("Arm moving.")
rospy.sleep(3)
#arm.execute(plan_1)
p_pick_1=plan_1.joint_trajectory.points[-1]
#print p
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(0)
rospy.sleep(3)
self.scene.remove_world_object(self.target_id)
#p_pick_2=plan_1.joint_trajectory.points[1]
#print p
#arm.set_joint_value_target(p_pick_2.positions)
#arm.go()
#rospy.sleep(4)
signal_pick=True
#p=plan_1.joint_trajectory.points[-1]
#print p
#arm.set_joint_value_target(p.positions)
#arm.go()
if signal_pick:
start_trans_pose=arm.get_current_pose(self.arm_end_effector_link).pose
waypoints_2.append(start_trans_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_2.append(deepcopy(pour_pose.pose))
fraction_2=0.0
attempts_2=0
maxtries_2=300
while fraction_2!=1 and attempts_2<maxtries_2:
(plan_2,fraction_2)=arm.compute_cartesian_path(waypoints_2,0.01,0.0,True)
attempts_2+=1
if (attempts_2 %300==0 and fraction_2!=1):
rospy.loginfo("path planning failed with " + str(fraction_2*100)+"% success.")
if fraction_2==1:
#signal=False
rospy.loginfo("path compute successfully with "+str(attempts_2)+" sttempts.")
rospy.loginfo("Arm moving.")
p_trans_shake=plan_2.joint_trajectory.points[-1]
p_trans=plan_2.joint_trajectory.points[-2]
#print p
arm.set_joint_value_target(p_trans.positions)
arm.go()
rospy.sleep(3)
j_pre_pour_state=arm.get_current_joint_values()
j_pour_state=j_pre_pour_state
j_pour_state[6]-=(1.57+0.26)
arm.set_joint_value_target(j_pour_state)
arm.go()
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
#shake=True
for i in range(10):
self.add_point(traj, 0.3, p_trans_shake, [0.0]*7)
self.add_point(traj, 0.3, p_trans, [0.0]*7)
traj.header.stamp= rospy.Time.now()
pub_traj.publish(traj)
arm.set_joint_value_target(j_pre_pour_state)
arm.go()
rospy.sleep(6)
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(255)
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__=="__main__":
try:
PickAndPlace()
except KeyboardInterrupt:
raise
rospy.sleep(1)
# create a pose
p = PoseStamped()
p.header.frame_id = "world" #robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.05
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# add a box there
scene.add_box("ground", p, (3, 3, 0.02))
# access some meshes
rospack = rospkg.RosPack()
resourcepath = rospack.get_path('regrasping_app')+"/../resources/"
# modify the pose
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
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) #0.04 in original file
arm.set_goal_orientation_tolerance(0.1) #0.1 original value
# 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 = 1
# 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'
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 "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.39
# Set the dimensions of the scene objects [l, w, h]
table_size = [
0.7, 0.7, 0.01
] #set the table coordinates in order to contain the whole scene [first value = 0.2 before]
#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.018, 0.018,
0.018] #corresponds roughly to the size of the cube
# 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.3
table_pose.pose.position.y = -0.19
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 = 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.34 #table_pose.pose.position.x
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
#origin position of the target
# 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 = table_pose.pose.position.x - 0.03
place_pose.pose.position.y = 0.15
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 2.0
#final position of the target
# 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")
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)
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
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success:
rospy.loginfo("Place attempt")
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)
#break
success = 1
if success:
break
#n_attempts += 1
#rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
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)
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)
self.left_arm = MoveGroupCommander('left_arm')
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.left_gripper = MoveGroupCommander('left_gripper')
# eel = len(self.right_arm.get_end_effector_link())
# print eel
# Allow 5 seconds per planning attempt
# self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
### Attach / Remove Object Flag ###
self.aro = None
# Run and keep in the BG the scene generator with ctrl^c kill ###
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
## Give some time to ensure the thread starts!! ##
rospy.sleep(5)
### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ###
self.idx_list = self.bl()
### GIVE SCENE TIME TO CATCH UP ###
rospy.sleep(5)
################################## GRASP EXECUTION #####################################
print "==================== Executing ==========================="
start_time = time.time()
### PERSONAL REMINDER!!! WHAT IS WHAT!!! ###
# print obj_id[obj_id.index('target')]
# print obj_id.index('target')
### MOVE LEFT ARM OUT OF THE WAY ###
self.lasp()
success = False
while success == False and len(self.idx_list)>0:
success, pgr_target = self.grasp_attempt()
print ("GA Returns:", success)
if success is not False:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_target, obj_id.index('target'),'true')
self.place_object(obj_id.index('target'))
break
else:
idx = self.idx_list[0]
ds, pgr_col_obj = self.declutter_scene(idx)
print ("DS Returns:", ds)
if ds == True:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true')
self.place_object(obj_id.index(obj_id[idx]))
self.idx_list.pop(0)
print "==================== THE END! ======================"
print("--- %s seconds ---" % (time.time() - start_time))
rospy.sleep(5)
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def grasp_attempt(self):
# start_time = time.time()
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
# while obj_id[obj_id.index('target')] is not 'target':
# print '!!!!!'
# rospy.sleep(0.05)
pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg')
gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i = 1
for pg, gr in izip(pre_grasps, grasps):
self.gripper_pose_pub.publish(gr)
print ("G Attempt: ", i)
plp = self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go(wait=True)
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
success = True
retreat = gr
print "Grasping"
break
# print("--- %s seconds ---" % (time.time() - start_time))
return success , retreat
def declutter_scene(self,index):
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
pg = self.grasp_pose(obj_pose[index], axis, 'pg')
gp = self.grasp_pose(obj_pose[index], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i= 1
for pg, gr in izip(pre_grasps, grasps):
plp = self.right_arm.plan(pg.pose)
print (" DS Attempt: ", i)
self.gripper_pose_pub.publish(gr)
self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go()
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
print "Grasping"
success = True
retreat = gr
break
return success, retreat
def place_object(self, obj_idx):
self.aro = obj_idx
### GENERATE PLACE POSES ###
places = self.place_generator()
### TRY THESE POSES ###
i = 1
for place in places:
print (" Place Attempt: ", i)
plpl = self.right_arm.plan(place.pose)
print len(plpl.joint_trajectory.points)
if len(plpl.joint_trajectory.points) == 0:
i+=1
continue
self.right_arm.plan(plpl)
self.right_arm.go(wait=True)
### INFORM SCENE ###
# self.open_gripper()
# self.aro = None
### RETURN HAND TO STARTING POSITION ###
self.post_grasp(place,obj_idx, 'false')
self.rasp()
break
def post_grasp(self,new_pose, obj_idx, fl):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
if fl == 'true':
self.close_gripper()
self.aro = obj_idx
else:
self.open_gripper()
self.aro = None
rospy.sleep(2)
### POST GRASP RETREAT ###
M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w])
M1[0,3] = new_pose.pose.position.x
M1[1,3] = new_pose.pose.position.y
M1[2,3] = new_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T1 = np.dot(M2, M1)
npo = deepcopy(new_pose)
npo.pose.position.x = T1[0,3]
npo.pose.position.y = T1[1,3]
npo.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
npo.pose.orientation.x = quat[0]
npo.pose.orientation.y = quat[1]
npo.pose.orientation.z = quat[2]
npo.pose.orientation.w = quat[3]
npo.header.frame_id = REFERENCE_FRAME
self.right_arm.plan(npo.pose)
self.right_arm.go(wait=True)
def place_generator(self):
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.57
place_pose.pose.position.y = 0.16
place_pose.pose.position.z = 0.56
place_pose.pose.orientation.w = 1.0
P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w])
P[0,3] = place_pose.pose.position.x
P[1,3] = place_pose.pose.position.y
P[2,3] = place_pose.pose.position.z
places =[]
yaw_angles = [0, 1,57, -1,57 , 3,14]
x_vals = [0, 0.05 ,0.1 , 0.15]
z_vals = [0.05 ,0.1 , 0.15]
for y in yaw_angles:
G = transformations.euler_matrix(0, 0, y)
G[0,3] = 0.0 # offset about x
G[1,3] = 0.0 # about y
G[2,3] = 0.0 # about z
for z in z_vals:
for x in x_vals:
TM = np.dot(P, G)
pl = deepcopy(place_pose)
pl.pose.position.x = TM[0,3] +x
pl.pose.position.y = TM[1,3]
pl.pose.position.z = TM[2,3] +z
quat = transformations.quaternion_from_matrix(TM)
pl.pose.orientation.x = quat[0]
pl.pose.orientation.y = quat[1]
pl.pose.orientation.z = quat[2]
pl.pose.orientation.w = quat[3]
pl.header.frame_id = REFERENCE_FRAME
places.append(deepcopy(pl))
return places
def grasp_pose(self, target_pose, axis, stage):
############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE #########
if axis == 0:
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, 0, 0)
if stage == 'pg':
M2[0,3] = -0.25 # offset about x
elif stage == 'gp':
M2[0,3] = -0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 1:
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, 0, 1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = -0.25 # about y
elif stage == 'gp':
M2[1,3] = -0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 2:
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, 0, -1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = 0.25 # about y
elif stage == 'gp':
M2[1,3] = 0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 3:
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, 0, 3.14)
if stage == 'pg':
M2[0,3] = 0.25 # offset about x
elif stage == 'gp':
M2[0,3] = 0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 4:
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
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
else:
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(1.57, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
T1 = np.dot(M1, M2)
grasp_pose = deepcopy(target_pose)
grasp_pose.pose.position.x = T1[0,3]
grasp_pose.pose.position.y = T1[1,3]
grasp_pose.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
grasp_pose.pose.orientation.x = quat[0]
grasp_pose.pose.orientation.y = quat[1]
grasp_pose.pose.orientation.z = quat[2]
grasp_pose.pose.orientation.w = quat[3]
grasp_pose.header.frame_id = REFERENCE_FRAME
return grasp_pose
def grasp_generator(self, initial_poses):
# A list to hold the grasps
grasps = []
o = [] # Original Pose of the object (o)
O=[]
i= 0
while i < len(initial_poses):
o.append(initial_poses[i])
i+=1
G = transformations.euler_matrix(0, 0, 0)
# Generate a grasps for along z axis (x and y directions)
k = 0
while k <= 5:
O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w]))
O[k][0,3] = o[k].pose.position.x
O[k][1,3] = o[k].pose.position.y
O[k][2,3] = o[k].pose.position.z
if k in range(0,4):
for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ###
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3] +z
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
elif k == 4:
for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3] +x
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
else:
for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3] +y
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
k+=1
print len(grasps)
# Return the list
return grasps
def scene_generator(self):
while True:
# print "happening"
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
# obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
self.setColor(obj_id[i], 1, 0.623, 0, 1.0)
### Make the target purple and table green ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0)
self.scene.remove_attached_object(GRIPPER_FRAME)
# Send the colors to the planning scene
self.sendColors()
else:
if self.flag == 0:
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
self.flag +=1
time.sleep(0.5)
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def bl(self):
blist = ['target','custom_2','custom_3', 'custom_table']
self.blist = []
for name in obj_id:
if name not in blist:
self.blist.append(obj_id.index(name))
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)])
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
return self.blist
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 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.0899, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
# 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 lasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.3665
sp.pose.position.y = 0.74094
sp.pose.position.z = 1.1449
sp.pose.orientation.x = 0.80503
sp.pose.orientation.y = -0.18319
sp.pose.orientation.z = 0.31988
sp.pose.orientation.w = 0.46481
self.left_arm.plan(sp)
self.left_arm.go(wait=True)
def rasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.39571
sp.pose.position.y = -0.40201
sp.pose.position.z = 1.1128
sp.pose.orientation.x =0.00044829
sp.pose.orientation.y = 0.57956
sp.pose.orientation.z = 9.4878e-05
sp.pose.orientation.w = 0.81493
self.right_arm.plan(sp)
self.right_arm.go(wait=True)
def __init__(self):
rospy.init_node('moveit_demo')
#self.sub = rospy.Subscriber('/aruco_single/pose', Pose, self.update)
#self.sub = rospy.Subscriber('/aruco_single/pose', PoseArray, self.update, queue_size=1)
#msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
#grasp msg publisher
m_pub_joint_g_1= rospy.Publisher("/ik_solution_g_1", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_g_2= rospy.Publisher("/ik_solution_g_2", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_g_3= rospy.Publisher("/ik_solution_g_3", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_g_4= rospy.Publisher("/ik_solution_g_4", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_g_5= rospy.Publisher("/ik_solution_g_5", JointTrajectoryPoint, queue_size=1, latch=True)
#place msg publisher
m_pub_joint_p_1= rospy.Publisher("/ik_solution_p_1", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_p_2= rospy.Publisher("/ik_solution_p_2", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_p_3= rospy.Publisher("/ik_solution_p_3", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_p_4= rospy.Publisher("/ik_solution_p_4", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_p_5= rospy.Publisher("/ik_solution_p_5", JointTrajectoryPoint, queue_size=1, latch=True)
m_pub_joint_p_6= rospy.Publisher("/ik_solution_p_6", JointTrajectoryPoint, queue_size=1, latch=True)
moveit_commander.roscpp_initialize(sys.argv)
cartesian=rospy.get_param('~cartesian',True)
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
'''
"""set up ros publishers"""
self.m_pub_m2j = rospy.Publisher("/two_finger/motoman_control/move_to_joint", JointAnglesDuration, queue_size=1, latch=True);
self.m_pub_m2p = rospy.Publisher("/two_finger/motoman_control/move_to_pos", PoseDuration, queue_size=1, latch=True)
self.m_pub_m2p_star = rospy.Publisher("/two_finger/motoman_control/move_to_pos_straight", PoseDuration, queue_size=1, latch=True)
""" set up ros service """
print("wait gripper service")
rospy.wait_for_service("/two_finger/gripper/gotoPosition")
rospy.wait_for_service("/two_finger/gripper/gotoPositionUntilTouch")
self.m_srv_grpCtrl = rospy.ServiceProxy("/two_finger/gripper/gotoPosition", SetPosition)
self.m_srv_grpCtrlUntilTouch = rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.m_srv_grpSpeed = rospy.ServiceProxy("/two_finger/gripper/setSpeed", SetParameter)
self.m_srv_grpTorque = rospy.ServiceProxy("/two_finger/gripper/setTorque", SetParameter)
self.m_srv_grpTorque(20)
self.m_srv_grpSpeed(128)
rospy.wait_for_service("/two_finger/motoman_control/trajectory_status")
self.m_srv_trajectoryStatus = rospy.ServiceProxy("/two_finger/motoman_control/trajectory_status", Trigger)
'''
rospy.sleep(1)
arm=MoveGroupCommander('arm')
arm.allow_replanning(True)
end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.03)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame='base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id='table'
#cylinder_id='cylinder'
box_id='box'
#box2_id='box2'
target_id='target_object'
scene.remove_world_object(box_id)
#scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(target_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
#table_ground=0.62
table_size=[0.9,0.6,0.018]
box_size=[0.15,0.15,0.015]
#box2_size=[0.05,0.05,0.1]
r_tool_size=[0.03,0.02,0.18]
l_tool_size=[0.03,0.02,0.18]
target_size=[0.059,0.059,0.12]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=-0.05
table_pose.pose.position.y=0.61
table_pose.pose.position.z=0.194
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.7
box1_pose.pose.position.y=-0.2
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.6
box2_pose.pose.position.y=-0.05
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)
'''
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.057
l_p.pose.position.z=0.09
l_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.057
r_p.pose.position.z=0.09
r_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size)
#box
#place
box_pose=PoseStamped()
box_pose.header.frame_id=reference_frame
box_pose.pose.position.x=-0.30
box_pose.pose.position.y=0.69
box_pose.pose.position.z=0.21
box_pose.pose.orientation.w=1
scene.add_box(box_id,box_pose,box_size)
#place
p_pose=box_pose
p_pose.pose.position.x+=0.01
p_pose.pose.position.y+=0.01
p_pose.pose.position.z+=0.11
p_pose.pose.orientation.w=-0.5
p_pose.pose.orientation.x=-0.5
p_pose.pose.orientation.y=-0.5
p_pose.pose.orientation.z=0.5
#scene.add_box(box1_id,p_pose,box2_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=end_effector_link
g_p.pose.position.x=0
g_p.pose.position.y=0
g_p.pose.position.z=0.15
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=-0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0.0
#set color
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('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.sendColors()
#motion planning
dev=InputDevice('/dev/input/event4')
print "xxx"
while not rospy.is_shutdown():
duration=rospy.Duration(4)
duration_s = rospy.Duration(2.5)
select([dev],[],[])
for event in dev.read():
if (event.value==0) and (event.code==16):
print " Path planning"
scene.remove_world_object(target_id)
rospy.loginfo("Plz move the target")
msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
target_pose=PoseStamped()
target_pose.header.frame_id=reference_frame
target_pose.pose.position.x=-(msg.pose.position.y)-0.28
target_pose.pose.position.y=-msg.pose.position.x+0.81-0.072
target_pose.pose.position.z=1.36-msg.pose.position.z+0.06
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=-0
target_pose.pose.orientation.w=1
scene.add_box(target_id,target_pose,target_size)
self.setColor('target_object',0,1,0)
#pre-grasp pose
pre_grasp_pose=PoseStamped()
pre_grasp_pose.header.frame_id=reference_frame
pre_grasp_pose.pose.position=target_pose.pose.position
pre_grasp_pose.pose.position.y-=0.17
pre_grasp_pose.pose.position.z+=0.02
pre_grasp_pose.pose.orientation.x=-0.5
pre_grasp_pose.pose.orientation.y=-0.5
pre_grasp_pose.pose.orientation.z=-0.5
pre_grasp_pose.pose.orientation.w=0.5
#grasp pose
grasp_pose=PoseStamped()
grasp_pose.header.frame_id=reference_frame
grasp_pose.pose.position=target_pose.pose.position
grasp_pose.pose.position.y-=0.12
grasp_pose.pose.position.z+=0.02
'''
#orientation from left
grasp_pose.pose.orientation.x=-0.000149
grasp_pose.pose.orientation.y=-0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
'''
#orientation from forward
grasp_pose.pose.orientation.x=-0.5
grasp_pose.pose.orientation.y=-0.5
grasp_pose.pose.orientation.z=-0.5
grasp_pose.pose.orientation.w=0.5
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
start_pose=arm.get_current_pose(end_effector_link).pose
#initial waypoints list
waypoints_1=[]
waypoints_2=[]
waypoints_3=[]
waypoints_4=[]
if cartesian:
waypoints_1.append(start_pose)
waypoints_1.append(deepcopy(pre_grasp_pose.pose))
fraction=0.0
maxtries=300
attempts=0
#arm.set_start_state__1to_current_state()
#plan the cartesian path connecting waypoints
while fraction<1.0 and attempts<maxtries:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1.0):
rospy.loginfo("path planning failed with only " + str(fraction*100)+ "% success after "+ str(maxtries)+" attempts")
continue
if fraction==1.0:
rospy.loginfo("path compute successfully. Move the arm.")
#place = JointAnglesDuration(JointAngles(plan.joint_trajectory.points[len(plan.joint_trajectory)-1].positions), duration)
#self.m_pub_m2j.publish(place)
print
arm.execute(plan_1)
m_pub_joint_g_1.publish(plan_1.joint_trajectory.points[-1])
m_pub_joint_g_2.publish(plan_1.joint_trajectory.points[-2])
m_pub_joint_g_3.publish(plan_1.joint_trajectory.points[-3])
m_pub_joint_g_4.publish(plan_1.joint_trajectory.points[-5])
m_pub_joint_g_5.publish(plan_1.joint_trajectory.points[-8])
rospy.loginfo("path execution complete. ")
#scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(5)
'''
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose,end_effector_link)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
print arm.get_current_joint_values()
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0,0.11,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
saved_target_pose=arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(1,-0.2,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
#arm.set_pose_target(saved_target_pose,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
'''
#remove and shut down
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
scene.remove_attached_object(end_effector_link,'l_tool')
scene.remove_attached_object(end_effector_link,'r_tool')
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)
#move(right, pr)
move(left, pl)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("bowl")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.4
p.pose.position.y = -0.9
p.pose.position.z = -0.4
scene.add_box("table", p, (1.6, 0.8, 0.4))
# p.pose.position.x = 0.8
# p.pose.position.y = 0.0
# p.pose.position.z = -0.02
# scene.add_box("bowl", p, (0.8, 0.01, 0.05))
#scene.remove_world_object("part")
rospy.sleep(1)
print("Standby at initial position")
# camera
# open camera
# rh_cam=CameraController("right_hand_camera")
# lh_cam=CameraController("left_hand_camera")
# h_cam=CameraController("head_camera")
# rh_cam.open()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# 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 right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault");
left_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 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)
# 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_start')
#left_arm.go()
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 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.56
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.51
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.49
box2_pose.pose.position.y = 0.15
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)
# 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)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit 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#########################################################################################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('SceneSetup')
# 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)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Set the reference frame for pose targets
reference_frame = 'rack1'
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [1.0128, 0.481, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = reference_frame
bowl_pose.pose.position.x = 0
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
# Set the height of the pitcher
pitcher_ground = 0.57816875
pitcher_pose = PoseStamped()
pitcher_pose.header.frame_id = reference_frame
pitcher_pose.pose.position.x = 0.4
pitcher_pose.pose.position.y = 0.847725
pitcher_pose.pose.position.z = pitcher_ground
pitcher_pose.pose.orientation.w = -0.5
pitcher_pose.pose.orientation.z = 0.707
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
self.sendColors()
scene.add_box(table_id, table_pose, table_size)
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.45
p.pose.orientation.w = 1.0
#scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.45
p.pose.position.y = -0.4
p.pose.position.z = 0.95
scene.add_box("part", p, (0.04, 0.04, 0.1))
rospy.loginfo("Added object to world")
rospy.sleep(1)
# p = PoseStamped()
# p.header.frame_id = "/base_link"
# p.header.stamp = rospy.Time.now()
# # p.pose.position.x = 0.61654
# # p.pose.position.y = 0.00954
# # p.pose.position.z = 0.98733
# p.pose.position.x = 0.25
# p.pose.position.y = -0.2
# p.pose.position.z = 1.00
# p.pose.orientation.x = 0.028598
#scene.remove_world_object("pole")
#scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# p.pose.position.x = 0.7
# p.pose.position.y = -0.4
# p.pose.position.z = 1.15
p.pose.orientation.w = 1.0
#scene.add_box("pole", p, (0.3, 0.1, 1.0))
# p.pose.position.y = -0.2
# p.pose.position.z = 0.175
#scene.add_box("table", p, (0.5, 1.5, 0.8))
# p.pose.position.x = 0.6
# p.pose.position.y = -0.7
# p.pose.position.z = 0.8
p.pose.position.x = 0.15
p.pose.position.y = -0.3
p.pose.position.z = 0.55
scene.add_box("part", p, (0.05, 0.05, 0.05))
rospy.sleep(1)
# pick an object
robot.right_arm.pick("part")
roscpp_shutdown()
def main():
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
rospy.sleep(2)
# centre table
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 1. # position of the table in the world frame
p1.pose.position.y = 0.
p1.pose.position.z = 0.
# left table (from baxter's perspective)
p1_l = PoseStamped()
p1_l.header.frame_id = robot.get_planning_frame()
p1_l.pose.position.x = 0.5 # position of the table in the world frame
p1_l.pose.position.y = 1.
p1_l.pose.position.z = 0.
p1_l.pose.orientation.x = 0.707
p1_l.pose.orientation.y = 0.707
# right table (from baxter's perspective)
p1_r = PoseStamped()
p1_r.header.frame_id = robot.get_planning_frame()
p1_r.pose.position.x = 0.5 # position of the table in the world frame
p1_r.pose.position.y = -1.
p1_r.pose.position.z = 0.
p1_r.pose.orientation.x = 0.707
p1_r.pose.orientation.y = 0.707
# open back shelf
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 1.2 # position of the table in the world frame
p2.pose.position.y = 0.0
p2.pose.position.z = 0.75
p2.pose.orientation.x = 0.5
p2.pose.orientation.y = -0.5
p2.pose.orientation.z = -0.5
p2.pose.orientation.w = 0.5
pw = PoseStamped()
pw.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p_ob1 = PoseStamped()
p_ob1.header.frame_id = robot.get_planning_frame()
p_ob1.pose.position.x = .92
p_ob1.pose.position.y = 0.3
p_ob1.pose.position.z = 0.89
# the ole duck
p_ob2 = PoseStamped()
p_ob2.header.frame_id = robot.get_planning_frame()
p_ob2.pose.position.x = 0.87
p_ob2.pose.position.y = -0.4
p_ob2.pose.position.z = 0.24
# clean environment
scene.remove_world_object("table_center")
scene.remove_world_object("table_side_left")
scene.remove_world_object("table_side_right")
scene.remove_world_object("shelf")
scene.remove_world_object("wall")
scene.remove_world_object("part")
scene.remove_world_object("duck")
rospy.sleep(1)
scene.add_box("table_center", p1,
size=(.5, 1.5, 0.4)) # dimensions of the table
scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4))
scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4))
scene.add_mesh("shelf", p2, "./bookshelf_light.stl", size=(.025, .01, .01))
scene.add_plane("wall", pw, normal=(0, 1, 0))
part_size = (0.07, 0.05, 0.12)
scene.add_box("part", p_ob1, size=part_size)
scene.add_mesh("duck", p_ob2, "./duck.stl", size=(.001, .001, .001))
rospy.sleep(1)
print(scene.get_known_object_names())
## ---> SET COLOURS
table_color = color_norm([105, 105,
105]) # normalize colors to range [0, 1]
shelf_color = color_norm([139, 69, 19])
duck_color = color_norm([255, 255, 0])
_colors = {}
setColor('table_center', _colors, table_color, 1)
setColor('table_side_left', _colors, table_color, 1)
setColor('table_side_right', _colors, table_color, 1)
setColor('shelf', _colors, shelf_color, 1)
setColor('duck', _colors, duck_color, 1)
sendColors(_colors, scene)
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 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
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# 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)
# 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)
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)
print "==================== Generating Transformations ==========================="
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'
# # Initialize the grasp object
# g = Grasp()
# grasps = []
# # Set the first grasp pose to the input pose
# g.grasp_pose = pre_grasping
# g.allowed_touch_objects = [target_id]
# grasps.append(deepcopy(g))
# right_arm.pick(target_id, grasps)
#Change the frame_id for the planning to take place!
#target_pose.header.frame_id = 'gazebo_world'
self.p_pub.publish(pre_grasping)
right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
plan = right_arm.plan()
rospy.sleep(2)
right_arm.go(wait=True)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class add_box:
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object-2')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:创建（调试）发布者：
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:创建规划场景和机器人命令：
self._scene = PlanningSceneInterface()#未知
self._robot = RobotCommander()#未知
rospy.sleep(0.5)
# Clean the scene:清理现场：
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._grasp_object2_name)
# Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景：
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)#增加障碍物块
self._pose_coke_can = self._add_grasp_fanfkuai_(self._grasp_object2_name)#增加障碍物块
rospy.sleep(0.5)
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._grasp_object2_name)
#----------------------------------1hang---------------------------------
def _add_grasp_block_(self, name):
"""
Create and add block to the scene创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = -0.0
p.pose.position.z = 0.75
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.045, 0.045, 0.045))
return p.pose
def _add_grasp_fanfkuai_(self, name):
"""
Create and add block to the scene创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = 0.4
p.pose.position.z = 0.75
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.045, 0.045, 0.045))
return p.pose
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# 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=5)
# 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 right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 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.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 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)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 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.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.3
box1_pose.pose.position.y = 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 = 0.3
box2_pose.pose.position.y = 0.25
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)
# 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)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
q = quaternion_from_euler(0, 0, -1.57079633)
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]
# Set the target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class Place(object):
_feedback = moveit_msgs.msg.PlaceActionFeedback().feedback
_result = moveit_msgs.msg.PlaceActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PlaceAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Place action is ok. Awaiting for connections')
def get_target(self):
target = [0.17, 0.10, 0.028]
return target
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
if len(self.scene.get_attached_objects()) < 1:
rospy.loginfo("No object attached")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self.robot.set_start_state_to_current_state()
self._feedback.state = "Planing to place pose"
self._as.publish_feedback(self._feedback)
target = [
goal.place_locations[0].place_pose.pose.position.x,
goal.place_locations[0].place_pose.pose.position.y,
goal.place_locations[0].place_pose.pose.position.z
]
plan = self.robot.ef_pose(target)
if plan is None:
rospy.loginfo("Plan to place failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._feedback.state = "Going to place the object"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to place the object")
self._result.trajectory_stages.append(plan)
self.robot.arm_execute(plan)
rospy.sleep(7)
self._feedback.state = "Removing object to be placed from the planning scene"
self._as.publish_feedback(self._feedback)
obj = self.scene.get_attached_objects()
link = obj[obj.keys()[0]].link_name
obj = obj[obj.keys()[0]].object
self.scene.remove_attached_object(link, obj.id)
self._feedback.state = "Planning to open the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Openning gripper"
print self._feedback
self.robot.gripper_execute(plan)
rospy.sleep(1)
self._as.publish_feedback(self._feedback)
self._feedback.state = "Re-adding object"
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.add_box(obj.id, pose, obj.primitives[0].dimensions)
self._as.publish_feedback(self._feedback)
self._feedback.state = "Planing to retreat after place"
self._as.publish_feedback(self._feedback)
target[2] += 0.02
plan = self.robot.ef_pose(target)
if plan is None:
rospy.loginfo("Plan to retreat failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._feedback.state = "Retreating"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append("Retreat")
self._result.trajectory_stages.append(plan)
self.robot.arm_execute(plan)
rospy.sleep(7)
if sucess:
self._result.error_code.val = 1
rospy.loginfo(self._result)
self._as.set_succeeded(self._result)
class scene_generator:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('scene_generator')
# 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 dictionary to hold object colors
self.colors = dict()
# 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 THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# 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()
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('custom_1')
table_id = 'table'
self.tid = self.pwh.name.index('table')
obstacle1_id = 'obstacle1'
self.o1id = self.pwh.name.index('wood_cube_5cm')
obstacle2_id = 'obstacle2'
self.o2id = self.pwh.name.index('custom_2')
# Set the sizes [l, w, h]
table_size = [1.5, 0.8, 0.03]
target_size = [0.05, 0.05, 0.15]
obstacle1_size = [0.05, 0.05, 0.05]
obstacle2_size = [0.05, 0.05, 0.10]
## 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]
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]
obstacle1_pose.pose.position.z += 0.025
# Add the target object to the scene
self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
obstacle2_pose = PoseStamped()
obstacle2_pose.header.frame_id = REFERENCE_FRAME
obstacle2_pose.pose = self.pwh.pose[self.o2id]
# Add the target object to the scene
self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_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 __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)
# right_arm.go()
# right_gripper.set_named_target("open")
# right_gripper.go()
rospy.sleep(3)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 1.0
p.pose.position.y = 0.2
p.pose.position.z = 0.3
scene.add_box("table", p, (0.7, 1, 0.7))
# add an object to be grasped
p.pose.position.x = 0.7
p.pose.position.y = -0.2
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
# right_arm.set_named_target("r_start")
# right_arm.go()
# rospy.sleep(1)
# right_arm.set_random_target()
# right_arm.go()
# rospy.sleep(1)
arm.go()
gripper.set_named_target("open")
gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 0.42
p.pose.position.y = -0.2
p.pose.position.z = 0.3
scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.205
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "world"
if __name__=='__main__':
rospy.init_node("part2_attacher")
scene = PlanningSceneInterface()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.45
p.pose.orientation.w = 1.0
#scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.45
p.pose.position.y = -0.1
p.pose.position.z = 1.0
#scene.add_box("part", p, (0.04, 0.04, 0.1))
scene.add_box("part2", p, (0.03, 0.03, 0.1))
rospy.loginfo("Added object to world")
rospy.sleep(1)
p.header.frame_id = "base_link"
p.pose.position.x = 0.165
p.pose.position.y = 0.
p.pose.position.z = 0.066 - 0.115
p.pose.orientation.x = 0.707106
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 0.707106
# scene.add_box("base", p, [0.052, 0.136, 0.526])
p.header.frame_id = "base_link"
p.pose.position.x = 0.165
p.pose.position.y = 0.
p.pose.position.z = 0.150 - 0.115
p.pose.orientation.w = 1
p.pose.orientation.x = 0
scene.add_box("box", p, [0.03, 0.03, 0.03])
rospy.sleep(1)
# ------------------------------
# Configure the planner
# -----------------------------
robot = RobotCommander()
robot.pincher_arm.set_start_state(RobotState())
#robot.pincher_arm.set_planner_id('RRTConnectkConfigDefault')
robot.pincher_arm.set_num_planning_attempts(10000)
robot.pincher_arm.set_planning_time(5)
robot.pincher_arm.set_goal_position_tolerance(0.01)
robot.pincher_arm.set_goal_orientation_tolerance(0.5)
robot.pincher_gripper.set_goal_position_tolerance(0.01)
robot.pincher_gripper.set_goal_orientation_tolerance(0.5)
# clean the scene
#scene.remove_world_object("block1")
#scene.remove_world_object("block2")
#scene.remove_world_object("block3")
#scene.remove_world_object("block4")
#scene.remove_world_object("table")
#scene.remove_world_object("bowl")
#scene.remove_world_object("box")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.57
scene.add_box("block1", p, (0.044, 0.044, 0.044))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
scene.add_box("block2", p, (0.044, 0.044, 0.044))
p.pose.position.x = 0.6
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("block3", p, (0.044, 0.044, 0.044))
p.pose.position.x = 1
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("block4", p, (0.044, 0.044, 0.044))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = 0
scene.add_mesh("table", p, "table_scaled.stl")
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
arm.set_planning_time(10)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('target')
# 设置桌面的高度
table_ground = 0.6
# 设置table和tool的三维尺寸
table_size = [0.1, 0.7, 0.01]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = -0.015
p.pose.position.z = 0.0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
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', table_pose, table_size)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
0.827228546495185, 0.29496592875743577, 1.1185644936946095,
-0.7987583317769674, -0.18950024740190782, 0.11752152218233858
]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
