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 execute_cb(self, goal): r = rospy.Rate(1) sucess = True if len(self.scene.get_attached_objects()) < 1: rospy.loginfo("No object attached") self._result.error_code.val = -1 self._as.set_aborted(self._result) 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 ] quat = [ goal.place_locations[0].place_pose.pose.orientation.x, goal.place_locations[0].place_pose.pose.orientation.y, goal.place_locations[0].place_pose.pose.orientation.z, goal.place_locations[0].place_pose.pose.orientation.w ] if np.sum(quat) == 0: # not valid quaternion quat = [] rospy.loginfo('Place quaternion [%s]', quat) plan = self.robot.ef_pose(list(target), orientation=quat) if plan is None: rospy.loginfo("Plan to place failed") self._result.error_code.val = -1 self._as.set_aborted(self._result) 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) ex_status = self.robot.arm_execute(plan) if not ex_status: rospy.loginfo("Execution to place failed: [%s]", ex_status) self._result.error_code.val = -4 self._as.set_aborted(self._result) sucess = False return None rospy.sleep(1) 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._result.error_code.val = -1 self._as.set_aborted(self._result) sucess = False return None self._result.trajectory_descriptions.append('OpenGripper') self._result.trajectory_stages.append(plan) self._feedback.state = "Openning gripper" print self._feedback ex_status = self.robot.gripper_execute(plan) if not ex_status: rospy.loginfo("Execution to open gripper failed: [%s]", ex_status) self._result.error_code.val = -4 self._as.set_aborted(self._result) sucess = False return None rospy.sleep(1) self._as.publish_feedback(self._feedback) if sucess: self._result.error_code.val = 1 self._as.set_succeeded(self._result)
class CollisionEnv(object): NS = "/art/collision_env/" def __init__(self, setup, world_frame): assert setup != "" self.ready = False self.setup = setup self.world_frame = world_frame self.tf_listener = TransformListener() self.api = ArtApiHelper() rospy.loginfo("Waiting for DB API") self.api.wait_for_db_api() self.ignored_prefixes = array_from_param("~ignored_prefixes") rospy.loginfo("Will ignore following prefixes: " + str(self.ignored_prefixes)) self.lock = RLock() self.ps = PlanningSceneInterface() self._paused = False self.oh = ObjectHelper(self.object_cb) self.artificial_objects = {} self.collision_objects_pub = rospy.Publisher(self.NS + "artificial", CollisionObjects, latch=True, queue_size=1) self.pub_artificial() self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb) self.paused_pub = rospy.Publisher(self.NS + "paused", Bool, latch=True, queue_size=1) self.paused = False def start(self): self.ready = True rospy.loginfo("Ready") def load_from_db(self): for prim in self.api.get_collision_primitives(self.setup): rospy.loginfo("Loading object: " + prim.name) self.add_primitive(prim) def save_primitive(self, name): with self.lock: if name not in self.artificial_objects: raise CollisionEnvException("Unknown object name") p = self.artificial_objects[name] self.api.add_collision_primitive(p) def save_primitives(self): with self.lock: for name in self.artificial_objects.keys(): self.save_primitive(name) def set_primitive_pose(self, name, ps): with self.lock: p = self.artificial_objects[name] assert p.pose.header.frame_id == ps.header.frame_id p.pose.pose = ps.pose self.ps.add_box(p.name, p.pose, p.bbox.dimensions) self.pub_artificial() def add_primitive(self, p): with self.lock: self.artificial_objects[p.name] = p self.ps.add_box(p.name, p.pose, p.bbox.dimensions) self.pub_artificial() def pub_artificial(self): msg = CollisionObjects() for v in self.artificial_objects.values(): # transform all collision primitives into world frame (= marker) if v.pose.header.frame_id != self.world_frame: try: self.tf_listener.waitForTransform(v.pose.header.frame_id, self.world_frame, rospy.Time(0), rospy.Duration(1.0)) v.pose = self.tf_listener.transformPose( self.world_frame, v.pose) except tf.Exception as e: rospy.logwarn("Failed to transform artificial object: " + str(e)) continue msg.primitives.append(v) self.collision_objects_pub.publish(msg) @property def paused(self): return self._paused @paused.setter def paused(self, val): self._paused = val self.paused_pub.publish(val) def remove_name(self, name): with self.lock: if name not in self.artificial_objects: rospy.logwarn("Unknown artificial object: " + name) return False self.ps.remove_world_object(name) del self.artificial_objects[name] self.pub_artificial() if not self.api.clear_collision_primitives(self.setup, names=[name]): rospy.logwarn("Failed to remove from permanent storage") rospy.loginfo("Removed object: " + name) return True def clear_all(self, permanent=True): with self.lock: rospy.loginfo("Clearing " + str(len(self.artificial_objects)) + " artificial objects...") for k in self.artificial_objects.keys(): if self.is_ignored(k): continue self.ps.remove_world_object(k) self.artificial_objects = {} self.pub_artificial() try: if permanent and not self.api.clear_collision_primitives( self.setup): rospy.logwarn("Failed to remove from permanent storage") except ArtApiException as e: rospy.logerr(str(e)) def reload(self): self.clear_all(permanent=False) self.load_from_db() self.pub_artificial() def _generate_name(self): # as we use only part of uuid, there might be collisions... while True: name = str(uuid.uuid4())[:8] if name not in self.artificial_objects.keys(): break return name def set_det_pose(self, name, ps): object_type = self.api.get_object_type( self.oh.objects[name].object_type) if object_type is not None: self.add_detected(name, ps, object_type) def clear_det_on_table(self, inv=False, ignore=None): if ignore is None: ignore = [] ret = [] with self.lock: for v in self.oh.objects.values(): if v.object_id in ignore: continue if not inv and not v.on_table: continue if inv and v.on_table: continue ret.append(v.object_id) self.clear_detected(v.object_id) return ret def is_ignored(self, name): for ip in self.ignored_prefixes: if name.startswith(ip): return True return False def timer_cb(self, evt): if self.paused: return with self.lock: known_objects = self.ps.get_known_object_names() for name in known_objects: if name not in self.artificial_objects and name not in self.oh.objects and not self.is_ignored( name): rospy.loginfo("Removing outdated object: " + name) self.clear_detected(name) # restore artificial objects if they are lost somehow (e.g. by restart of MoveIt!) for k, v in self.artificial_objects.iteritems(): if k in known_objects: continue rospy.loginfo("Restoring artificial object: " + v.name) if v.bbox.type == SolidPrimitive.BOX: self.ps.add_box(v.name, v.pose, v.bbox.dimensions) else: # TODO other types pass def object_cb(self, evt, h, inst): if self.paused or not self.ready: return with self.lock: attached_objects = self.ps.get_attached_objects() if evt in (ObjectHelper.OBJECT_ADDED, ObjectHelper.OBJECT_UPDATED): if inst.object_id in attached_objects: return ps = PoseStamped() ps.header = h ps.pose = inst.pose object_type = self.api.get_object_type(inst.object_type) if object_type is not None: self.add_detected(inst.object_id, ps, object_type) elif evt == ObjectHelper.OBJECT_LOST: if inst.object_id not in attached_objects: self.clear_detected(inst.object_id) def add_detected(self, name, ps, object_type): with self.lock: self.ps.add_box(name, ps, object_type.bbox.dimensions) def clear_detected(self, name): with self.lock: self.ps.remove_world_object(name) def clear_all_det(self, ignore=None): if ignore is None: ignore = [] ret = [] with self.lock: for v in self.oh.objects.values(): name = v.object_id if name in ignore: continue self.clear_detected(name) ret.append(name) rospy.loginfo("Removed " + str(len(ret)) + " detected objects.") return ret def get_attached(self, transform_to_world=True): """ keep in mind - attached objects might not be detected """ ret = [] with self.lock: ao = self.ps.get_attached_objects() for k, v in ao.iteritems(): if len(v.object.primitives) != 1 or len( v.object.primitive_poses) != 1: rospy.logwarn("Unsupported type of attached object: " + k) continue if v.object.primitives[0].type != SolidPrimitive.BOX: rospy.logwarn( "Only box-like attached objects are supported so far.") continue ps = PoseStamped() ps.header = v.object.header ps.pose = v.object.primitive_poses[0] if transform_to_world and ps.header.frame_id != self.world_frame: try: self.tf_listener.waitForTransform(ps.header.frame_id, self.world_frame, ps.header.stamp, rospy.Duration(1.0)) ps = self.tf_listener.transformPose(self.world_frame, ps) except tf.Exception as e: rospy.logwarn("Failed to transform attached object: " + str(e)) continue ret.append((k, ps, v.object.primitives[0])) return ret
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [ rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01 ] self.gripper_closed = [ rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01 ] self.gripper_neutral = [ rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0]) / 2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.04) arm.set_goal_orientation_tolerance(0.01) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 3 # Set a limit on the number of place attempts max_place_attempts = 3 rospy.loginfo("Scaling for MoveIt timeout=" + str( rospy.get_param( '/move_group/trajectory_execution/allowed_execution_duration_scaling' ))) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run self.scene.remove_world_object(table_id) self.scene.remove_world_object(box1_id) self.scene.remove_world_object(box2_id) self.scene.remove_world_object(target_id) self.scene.remove_world_object(tool_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "arm_up" pose stored in the SRDF file rospy.loginfo("Set Arm: right_up") arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") # Move the gripper to the open position rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened)) gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") # Set the height of the table off the ground table_ground = 0.4 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.005, 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.36 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 self.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 self.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 self.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 = table_pose.pose.position.x - 0.03 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 # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) 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 = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation result = MoveItErrorCodes.FAILURE n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: rospy.loginfo("Pick attempt #" + str(n_attempts)) for grasp in grasps: # Publish the grasp poses so they can be viewed in RViz self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) result = arm.pick(target_id, grasps) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: rospy.loginfo(" Pick: Done!") # Generate valid place poses places = self.make_places(target_id, place_pose) success = False n_attempts = 0 # Repeat until we succeed or run out of attempts while not success and n_attempts < max_place_attempts: rospy.loginfo("Place attempt #" + str(n_attempts)) for place in places: # Publish the place poses so they can be viewed in RViz self.gripper_pose_pub.publish(place) rospy.sleep(0.2) success = arm.place(target_id, place) 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) # Get the gripper posture as a JointTrajectory def make_gripper_posture(self, joint_positions): # Initialize the joint trajectory for the gripper joints t = JointTrajectory() # Set the joint names to the gripper joint names t.header.stamp = rospy.get_rostime() t.joint_names = GRIPPER_JOINT_NAMES # Initialize a joint trajectory point to represent the goal tp = JointTrajectoryPoint() # Assign the trajectory joint positions to the input positions tp.positions = joint_positions # Set the gripper effort tp.effort = GRIPPER_EFFORT tp.time_from_start = rospy.Duration(0.0) # Append the goal point to the trajectory points t.points.append(tp) # Return the joint trajectory return t # Generate a gripper translation in the direction given by vector def make_gripper_translation(self, min_dist, desired, vector): # Initialize the gripper translation object g = GripperTranslation() # Set the direction vector components to the input g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # The vector is relative to the gripper frame g.direction.header.frame_id = GRIPPER_FRAME # Assign the min and desired distances from the input g.min_distance = min_dist g.desired_distance = desired return g # Generate a list of possible grasps def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately; # grasp_opening should be a bit smaller than target width g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened) g.grasp_posture = self.make_gripper_posture(grasp_opening) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation( 0.01, 0.1, [1.0, 0.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation( 0.1, 0.15, [0.0, -1.0, 1.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4] # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading # from arm base to the object to pick (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1, -0.1]] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for yaw in yaw_vals: for pitch in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps # Generate a list of possible place poses def make_places(self, target_id, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015] # A list of pitch angles to try pitch_vals = [0] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] # A list to hold the places places = [] # Generate a place pose for each angle and translation for pitch in pitch_vals: for dy in y_vals: for dx in x_vals: place.pose.position.x = init_pose.pose.position.x + dx place.pose.position.y = init_pose.pose.position.y + dy # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from # arm base to the place location (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform( place, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw = atan2(y, x) # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the # objects orientation!), so we cancel this transformation. It is applied here: # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 acobjs = self.scene.get_attached_objects([target_id]) aco_pose = self.get_pose(acobjs[target_id]) if aco_pose is None: rospy.logerr( "Attached collision object '%s' not found" % target_id) return None aco_tf = self.pose_to_mat(aco_pose) place_tf = self.pose_to_mat(place.pose) place.pose = self.mat_to_pose(place_tf, aco_tf) rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \ % (aco_pose, place.pose)) # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places # Set the color of an object def setColor(self, name, r, g, b, a=0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def get_pose(self, co): # We get object's pose from the mesh/primitive poses; try first with the meshes if isinstance(co, CollisionObject): if co.mesh_poses: return co.mesh_poses[0] elif co.primitive_poses: return co.primitive_poses[0] else: rospy.logerr( "Collision object '%s' has no mesh/primitive poses" % co.id) return None elif isinstance(co, AttachedCollisionObject): if co.object.mesh_poses: return co.object.mesh_poses[0] elif co.object.primitive_poses: return co.object.primitive_poses[0] else: rospy.logerr( "Attached collision object '%s' has no mesh/primitive poses" % co.id) return None else: rospy.logerr("Input parameter is not a collision object") return None def pose_to_mat(self, pose): '''Convert a pose message to a 4x4 numpy matrix. Args: pose (geometry_msgs.msg.Pose): Pose rospy message class. Returns: mat (numpy.matrix): 4x4 numpy matrix ''' quat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T mat = np.matrix(quaternion_matrix(quat)) mat[0:3, 3] = pos return mat def mat_to_pose(self, mat, transform=None): '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform. Args: mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform. transform (numpy.ndarray): Optional 4x4 array representing additional transform Returns: pose (geometry_msgs.msg.Pose): Pose message representing transform. ''' if transform != None: mat = np.dot(mat, transform) pose = Pose() pose.position.x = mat[0, 3] pose.position.y = mat[1, 3] pose.position.z = mat[2, 3] quat = quaternion_from_matrix(mat) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose
def init(): global marker_array_pub, marker_pub, tf_broadcaster, tf_listener global move_group, turntable global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir rospy.init_node('acquisition') camera_mesh = rospy.get_param('~camera_mesh', None) camera_orientation = rospy.get_param('~camera_orientation', None) camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0]) camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1]) min_distance = rospy.get_param('~min_distance', 0.2) max_distance = rospy.get_param('~max_distance', min_distance) max_velocity = rospy.get_param('~max_velocity', 0.1) num_positions = rospy.get_param('~num_positions', 15) num_spins = rospy.get_param('~num_spins', 8) object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2])) photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0]) photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0]) ptpip = rospy.get_param('~ptpip', None) reach = rospy.get_param('~reach', 0.85) simulation = '/gazebo' in get_node_names() test = rospy.get_param('~test', True) turntable_pos = rospy.get_param( '~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05]) turntable_radius = rospy.get_param('~turntable_radius', 0.2) wall_thickness = rospy.get_param('~wall_thickness', 0.04) marker_array_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1, latch=True) marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1, latch=True) tf_broadcaster = tf.TransformBroadcaster() tf_listener = tf.TransformListener() move_group = MoveGroupCommander('manipulator') move_group.set_max_acceleration_scaling_factor( 1.0 if simulation else max_velocity) move_group.set_max_velocity_scaling_factor( 1.0 if simulation else max_velocity) robot = RobotCommander() scene = PlanningSceneInterface(synchronous=True) try: st_control = load_source( 'st_control', os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts', 'iai_scanning_table', 'st_control.py')) turntable = st_control.ElmoUdp() if turntable.check_device(): turntable.configure() turntable.reset_encoder() turntable.start_controller() turntable.set_speed_deg(30) except Exception as e: print(e) sys.exit('Could not connect to turntable.') if simulation: move_home() elif not test: working_dir = rospy.get_param('~working_dir', None) if working_dir is None: sys.exit('Working directory not specified.') elif not camera.init( os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'out', working_dir, 'images'), ptpip): sys.exit('Could not initialize camera.') # add ground plane ps = PoseStamped() ps.header.frame_id = robot.get_planning_frame() scene.add_plane('ground', ps) # add photobox ps.pose.position.x = photobox_pos[ 0] + photobox_size[0] / 2 + wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_left', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[ 0] - photobox_size[0] / 2 - wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_right', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[ 1] - photobox_size[1] / 2 - wall_thickness / 2 ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_back', ps, (photobox_size[0], wall_thickness, photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] - wall_thickness / 2 scene.add_box('box_ground', ps, (photobox_size[0], photobox_size[1], wall_thickness)) # add turntable turntable_height = turntable_pos[2] - photobox_pos[2] ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = photobox_pos[2] + turntable_height / 2 scene.add_cylinder('turntable', ps, turntable_height, turntable_radius) # add object on turntable ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = turntable_pos[2] + object_size[2] / 2 scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2) # add cable mounts scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount') scene.remove_attached_object('forearm_link', 'forearm_cable_mount') scene.remove_world_object('upper_arm_cable_mount') scene.remove_world_object('forearm_cable_mount') if ptpip is None and not simulation: size = [0.08, 0.08, 0.08] ps.header.frame_id = 'upper_arm_link' ps.pose.position.x = -0.13 ps.pose.position.y = -0.095 ps.pose.position.z = 0.135 scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size) ps.header.frame_id = 'forearm_link' ps.pose.position.x = -0.275 ps.pose.position.y = -0.08 ps.pose.position.z = 0.02 scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size) # add camera eef_link = move_group.get_end_effector_link() ps = PoseStamped() ps.header.frame_id = eef_link scene.remove_attached_object(eef_link, 'camera_0') scene.remove_attached_object(eef_link, 'camera_1') scene.remove_world_object('camera_0') scene.remove_world_object('camera_1') ps.pose.position.y = camera_pos[1] ps.pose.position.z = camera_pos[2] if camera_mesh: ps.pose.position.x = camera_pos[0] quaternion = tf.transformations.quaternion_from_euler( math.radians(camera_orientation[0]), math.radians(camera_orientation[1]), math.radians(camera_orientation[2])) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] scene.attach_mesh(eef_link, 'camera_0', ps, os.path.expanduser(camera_mesh), camera_size) if not simulation: scene.attach_mesh(eef_link, 'camera_1', ps, os.path.expanduser(camera_mesh), numpy.array(camera_size) * 1.5) vertices = scene.get_attached_objects( ['camera_0'])['camera_0'].object.meshes[0].vertices camera_size[0] = max(vertice.x for vertice in vertices) - min( vertice.x for vertice in vertices) camera_size[1] = max(vertice.y for vertice in vertices) - min( vertice.y for vertice in vertices) camera_size[2] = max(vertice.z for vertice in vertices) - min( vertice.z for vertice in vertices) else: ps.pose.position.x = camera_pos[0] + camera_size[0] / 2 scene.attach_box(eef_link, 'camera_0', ps, camera_size)
class PickAndPlace: def __init__(self, robot_name="panda_arm", frame="panda_link0"): try: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node(name="pick_place_test") self.scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # region Robot initial self.robot = MoveGroupCommander(robot_name) self.robot.set_goal_joint_tolerance(0.00001) self.robot.set_goal_position_tolerance(0.00001) self.robot.set_goal_orientation_tolerance(0.01) self.robot.set_goal_tolerance(0.00001) self.robot.allow_replanning(True) self.robot.set_pose_reference_frame(frame) self.robot.set_planning_time(3) # endregion self.gripper = MoveGroupCommander("hand") self.gripper.set_joint_value_target(GRIPPER_CLOSED) self.gripper.go() self.gripper.set_joint_value_target(GRIPPER_OPEN) self.gripper.go() self.gripper.set_joint_value_target(GRIPPER_CLOSED) self.gripper.go() # Robot go home self.robot.set_named_target("home") self.robot.go() # clear all object in world self.clear_all_object() table_pose = un.Pose(0, 0, -10, 0, 0, 0) table_color = un.Color(255, 255, 0, 100) self.add_object_box("table", table_pose, table_color, frame, (2000, 2000, 10)) bearing_pose = un.Pose(250, 250, 500, -90, 45, -90) bearing_color = un.Color(255, 0, 255, 255) bearing_file_name = "../stl/bearing.stl" self.add_object_mesh("bearing", bearing_pose, bearing_color, frame, bearing_file_name) obpose = self.scene.get_object_poses(["bearing"]) # self.robot.set_support_surface_name("table") g = Grasp() # Create gripper position open or close g.pre_grasp_posture = self.open_gripper() g.grasp_posture = self.close_gripper() g.pre_grasp_approach = self.make_gripper_translation( 0.01, 0.1, [0, 1.0, 0]) g.post_grasp_retreat = self.make_gripper_translation( 0.01, 0.9, [0, 1.0, 0]) p = PoseStamped() p.header.frame_id = "panda_link0" p.pose.orientation = obpose["bearing"].orientation p.pose.position = obpose["bearing"].position g.grasp_pose = p g.allowed_touch_objects = ["bearing"] a = [] a.append(g) result = self.robot.pick(object_name="bearing", grasp=a) print(result) except Exception as ex: print(ex) moveit_commander.roscpp_shutdown() moveit_commander.roscpp_shutdown() def make_gripper_translation(self, min_dist, desired, vector): g = GripperTranslation() g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] g.direction.header.frame_id = "panda_link0" g.min_distance = min_dist g.desired_distance = desired return g def open_gripper(self): t = JointTrajectory() t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] tp = JointTrajectoryPoint() tp.positions = [0.04, 0.04] tp.time_from_start = rospy.Duration(secs=1) t.points.append(tp) return t def close_gripper(self): t = JointTrajectory() t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] tp = JointTrajectoryPoint() tp.positions = [0.0, 0.0] tp.time_from_start = rospy.Duration(secs=1) t.points.append(tp) return t def clear_all_object(self): for world_object in (self.scene.get_known_object_names()): self.scene.remove_world_object(world_object) for attached_object in (self.scene.get_attached_objects()): self.scene.remove_attached_object(attached_object) def add_object_box(self, object_name, pose, color, frame, size): """ add object in rviz :param object_name: name of object :param pose: pose of object. (xyz) in mm,(abc) in degree :param color: color of object.(RGBA) :param frame: reference_frame :param size: size of object(mm) :return: None """ # Add object object_pose = PoseStamped() object_pose.header.frame_id = frame object_pose.pose.position.x = pose.X / 1000.0 object_pose.pose.position.y = pose.Y / 1000.0 object_pose.pose.position.z = pose.Z / 1000.0 quaternion = quaternion_from_euler(np.deg2rad(pose.A), np.deg2rad(pose.B), np.deg2rad(pose.C)) object_pose.pose.orientation.x = quaternion[0] object_pose.pose.orientation.y = quaternion[1] object_pose.pose.orientation.z = quaternion[2] object_pose.pose.orientation.w = quaternion[3] self.scene.add_box(name=object_name, pose=object_pose, size=(size[0] / 1000.0, size[1] / 1000.0, size[2] / 1000.0)) # Add object color object_color = ObjectColor() object_color.id = object_name object_color.color.r = color.R / 255.00 object_color.color.g = color.G / 255.00 object_color.color.b = color.B / 255.00 object_color.color.a = color.A / 255.00 p = PlanningScene() p.is_diff = True p.object_colors.append(object_color) self.scene_pub.publish(p) def add_object_mesh(self, object_name, pose, color, frame, file_name): """ add object in rviz :param object_name: name of object :param pose: pose of object. (xyz) in mm,(abc) in degree :param color: color of object.(RGBA) :param frame: reference_frame :param file_name: mesh file name :return: None """ # Add object object_pose = PoseStamped() object_pose.header.frame_id = frame object_pose.pose.position.x = pose.X / 1000.0 object_pose.pose.position.y = pose.Y / 1000.0 object_pose.pose.position.z = pose.Z / 1000.0 quaternion = quaternion_from_euler(np.deg2rad(pose.A), np.deg2rad(pose.B), np.deg2rad(pose.C)) object_pose.pose.orientation.x = quaternion[0] object_pose.pose.orientation.y = quaternion[1] object_pose.pose.orientation.z = quaternion[2] object_pose.pose.orientation.w = quaternion[3] self.scene.add_mesh(name=object_name, pose=object_pose, filename=file_name, size=(0.001, 0.001, 0.001)) # Add object color object_color = ObjectColor() object_color.id = object_name object_color.color.r = color.R / 255.00 object_color.color.g = color.G / 255.00 object_color.color.b = color.B / 255.00 object_color.color.a = color.A / 255.00 p = PlanningScene() p.is_diff = True p.object_colors.append(object_color) self.scene_pub.publish(p)
class 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: interface = None # type: PlanningSceneInterface publisher = None # type: rospy.Publisher ref_frame = '' # type: str colors = dict() # type: dict default_color = (0.75, 0.75, 0.75, 1) # type: tuple def __init__(self, ref_frame='base_link'): self.interface = PlanningSceneInterface() self.publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) self.ref_frame = ref_frame rospy.sleep(1) def destroy(self): pass # ---- add & remove ---- # def add_box(self, name, size, transform, rgba=None): if rgba is None: rgba = self.default_color self.interface.remove_world_object(name) pose = PoseStamped() pose.header.frame_id = self.ref_frame pose.pose = frame_to_pose(transform) self.interface.add_box(name, pose, size) self.wait_state_update(name) self.set_color(name, rgba) self.send_colors() def remove_object(self, name): self.interface.remove_world_object(name) self.wait_state_update(name, is_known=False) # ---- utils ---- # def set_color(self, name, rgba): color = ObjectColor() color.id = name color.color.r = rgba[0] color.color.g = rgba[1] color.color.b = rgba[2] color.color.a = rgba[3] self.colors[name] = color def send_colors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.publisher.publish(p) def wait_state_update(self, name, is_known=True, is_attached=False, timeout=4): start = rospy.get_time() current = rospy.get_time() while (current - start < timeout) and not rospy.is_shutdown(): attached_objects = self.interface.get_attached_objects([name]) cur_is_attached = len(attached_objects.keys()) > 0 cur_is_known = name in self.interface.get_known_object_names() if (cur_is_attached == is_attached) and (cur_is_known == is_known): rospy.loginfo('Scene update succeeded.') return True rospy.sleep(0.1) current = rospy.get_time() # rospy.loginfo("Wait scene update [" + str(current - start) + "s]...") rospy.loginfo('Scene update failed') return False