class ResetArm(smach.State): def __init__(self): smach.State.__init__(self, outcomes=outcomes, input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'move_robot' timeout = 30.0 self.max_attempts = 3 self.communication = Communication(topic, timeout=timeout) def execute(self, userdata): for attempt_remaining in reversed(range(self.max_attempts)): rospy.loginfo("[PIPELINE] Opening end effector") result = self.communication.wait_for_result('open') if result == FlexGraspErrorCodes.SUCCESS: rospy.loginfo("[PIPELINE] Homeing manipulator") result = self.communication.wait_for_result('home') if result == FlexGraspErrorCodes.SUCCESS: userdata.command = None userdata.prev_command = 'reset' return 'success' flex_grasp_error_log(result, "PIPELINE") rospy.logwarn("[PIPELINE] retry resetarm: %s attempts remaining", attempt_remaining) return error_handling(result)
class DetectObject(smach.State): def __init__(self): smach.State.__init__( self, outcomes=['success', 'failure', 'complete_failure'], input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'object_detection' timeout = 25.0 self.communication = Communication(topic, timeout=timeout) self.counter = 3 def execute(self, userdata): rospy.logdebug('Executing state Detect') # This ensures the robot is not in the way of the camera, even with delay. rospy.sleep(1.0) # command node result = self.communication.wait_for_result(userdata.command) if result == FlexGraspErrorCodes.SUCCESS: userdata.prev_command = userdata.command userdata.command = None return 'success' else: self.counter = self.counter - 1 if self.counter <= 0: return 'complete_failure' return 'failure'
class PickPlace(smach.State): def __init__(self): smach.State.__init__(self, outcomes=outcomes, input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'pick_place' timeout = 30.0 self.communication = Communication(topic, timeout=timeout) def execute(self, userdata): rospy.logdebug('Executing state Pick Place') rospy.logdebug('Statemachine publishing command %s', userdata.command) if userdata.command == "pick_place": commands = ["pick", "place"] else: commands = [userdata.command] for command in commands: result = self.communication.wait_for_result(command) flex_grasp_error_log(result) outcome = error_handling(result) if outcome != 'success': return outcome # determine success userdata.prev_command = userdata.command userdata.command = None return outcome
class ResetDynamixel(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['success', 'failure'], input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) timeout = 30.0 self.counter = 1 self.move_robot_communication = Communication('move_robot', timeout=timeout) self.monitor_robot_communication = Communication('monitor_robot', timeout=timeout) def execute(self, userdata): rospy.loginfo("[PIPELINE] Forceing robot") result = self.move_robot_communication.wait_for_result('force_robot') rospy.loginfo("[PIPELINE] Opening end effector") result = self.move_robot_communication.wait_for_result('open') rospy.loginfo("[PIPELINE] Sleep manipulator") result = self.move_robot_communication.wait_for_result('sleep') result = self.monitor_robot_communication.wait_for_result('reboot') result = self.move_robot_communication.wait_for_result( 'do_not_force_robot') # check if error is fixed result = self.monitor_robot_communication.wait_for_result( 'monitor_robot') if result != FlexGraspErrorCodes.SUCCESS: flex_grasp_error_log(result, "PIPELINE") return 'failure' result = self.move_robot_communication.wait_for_result('sleep') result = self.move_robot_communication.wait_for_result('open') result = self.move_robot_communication.wait_for_result('home') userdata.command = None userdata.prev_command = 'reset' return 'success'
class MoveRobot(smach.State): def __init__(self): smach.State.__init__(self, outcomes=outcomes, input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'move_robot' timeout = 30.0 self.communication = Communication(topic, timeout=timeout) def execute(self, userdata): rospy.logdebug('Executing state Move Robot') result = self.communication.wait_for_result(userdata.command) userdata.prev_command = userdata.command userdata.command = None return error_handling(result)
class CalibrateRobot(smach.State): def __init__(self): smach.State.__init__(self, outcomes=outcomes, input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'calibration_eye_on_base/calibrate' timeout = 60.0 self.communication = Communication(topic, timeout=timeout) def execute(self, userdata): rospy.logdebug('Executing state Calibrate') # command node result = self.communication.wait_for_result(userdata.command) userdata.prev_command = userdata.command userdata.command = None return error_handling(result)
class SpawnObject(smach.State): def __init__(self): smach.State.__init__( self, outcomes=['success', 'failure', 'complete_failure'], input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'gazebo_interface' timeout = 2.0 self.communication = Communication(topic, timeout=timeout, msg_type=GazeboInstruction) self.counter = 3 def execute(self, userdata): rospy.logdebug('Executing state SpawnObject') # command node if userdata.command == 'spawn_truss': msg = GazeboInstruction(command=GazeboInstruction.SPAWN, model_type='3d') elif userdata.command == 'set_pose_truss': msg = GazeboInstruction(command=GazeboInstruction.SETPOSE) else: rospy.logerr("Unknown command %s", userdata.command) result = self.communication.wait_for_result(msg) if result == FlexGraspErrorCodes.SUCCESS: userdata.prev_command = userdata.command userdata.command = None return 'success' else: self.counter = self.counter - 1 if self.counter <= 0: return 'complete_failure' return 'failure'
class PoseTransform(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['success', 'failure'], input_keys=['mode', 'command', 'prev_command'], output_keys=['mode', 'command', 'prev_command']) topic = 'transform_pose' timeout = 40.0 self.communication = Communication(topic, timeout=timeout) def execute(self, userdata): rospy.logdebug('Executing state Transform') # get response result = self.communication.wait_for_result(userdata.command) # determine success if result == FlexGraspErrorCodes.SUCCESS: userdata.prev_command = userdata.command userdata.command = None return 'success' else: return 'failure'
class PickPlace(object): node_name = 'PICK PLACE' frequency = 10 def __init__(self, node_name="pick_place", playback=False): self.node_name = node_name self.playback = playback if self.playback: rospy.loginfo("[{0}] Transform pose launched in playback mode!".format(self.node_name)) self.state = "init" self.prev_state = None self.command = None # state machine input rospy.Subscriber("~e_in", String, self.e_in_cb) self.pub_e_out = rospy.Publisher("~e_out", FlexGraspErrorCodes, queue_size=10, latch=True) # init move robot communication self.move_robot_communication = Communication("move_robot", timeout=15) self.pub_move_robot_pose = rospy.Publisher("robot_pose", PoseStamped, queue_size=10, latch=False) # create dict which stores all poses keys = ['pre_grasp', 'grasp', 'pre_place', 'place'] self.pose = dict.fromkeys(keys) # subscribe to corresponding poses values = [key + '_pose' for key in keys] pose_topic = dict(zip(keys, values)) for key in pose_topic: rospy.Subscriber(pose_topic[key], PoseStamped, self.pose_in_cb, key) def e_in_cb(self, msg): if self.command is None: self.command = msg.data rospy.logdebug("[{0}] Received command in message: {1}".format(self.node_name, self.command)) # reset outputting message msg = FlexGraspErrorCodes() msg.val = FlexGraspErrorCodes.NONE self.pub_e_out.publish(msg) def pose_in_cb(self, msg, key): rospy.logdebug("[%s] Received %s pose", self.node_name, key) self.pose[key] = msg def command_to_pose(self, pose): rospy.logdebug("[{0}] Commanding move robot to pose".format(self.node_name)) if pose is None: rospy.logwarn("[{0}] Cannot command to pose, since the pose is None!".format(self.node_name)) return FlexGraspErrorCodes.REQUIRED_DATA_MISSING self.pub_move_robot_pose.publish(pose) return self.move_robot_communication.wait_for_result("move_manipulator") def man_pre_grasp(self): rospy.logdebug("[{0}] Commanding move robot to pre grasp".format(self.node_name)) return self.command_to_pose(self.pose['pre_grasp']) def man_grasp(self): rospy.logdebug("[{0}] Commanding move robot to grasp".format(self.node_name)) return self.command_to_pose(self.pose['grasp']) def man_pre_place(self): rospy.logdebug("[{0}] Commanding move robot to pre place".format(self.node_name)) return self.command_to_pose(self.pose['pre_place']) def man_place(self): rospy.logdebug("[{0}] Commanding move robot to place".format(self.node_name)) return self.command_to_pose(self.pose['place']) def command_to_home(self): rospy.logdebug("[{0}] Commanding move robot to home".format(self.node_name)) result = self.move_robot_communication.wait_for_result("home") return result def apply_pre_grasp_ee(self): rospy.logdebug("[{0}] Applying pre-grasp with end effector".format(self.node_name)) result = self.move_robot_communication.wait_for_result("open") return result def apply_grasp_ee(self): rospy.logdebug("[{0}] Applying grasp with end effector".format(self.node_name)) result = self.move_robot_communication.wait_for_result("grasp") return result def apply_release_ee(self): rospy.logdebug("[{0}] Applying release with end effector".format(self.node_name)) result = self.move_robot_communication.wait_for_result("release") return result def pick(self): rospy.logdebug("[{0}] Picking object".format(self.node_name)) result = self.man_pre_grasp() if result == FlexGraspErrorCodes.SUCCESS: result = self.apply_pre_grasp_ee() if result == FlexGraspErrorCodes.SUCCESS: result = self.man_grasp() if result == FlexGraspErrorCodes.SUCCESS: result = self.apply_grasp_ee() if result == FlexGraspErrorCodes.SUCCESS: result = self.man_pre_grasp() return result def place(self): rospy.logdebug("[{0}] Placing object".format(self.node_name)) result = self.man_pre_place() if result == FlexGraspErrorCodes.SUCCESS: result = self.man_place() if result == FlexGraspErrorCodes.SUCCESS: result = self.apply_release_ee() if result == FlexGraspErrorCodes.SUCCESS: result = self.man_pre_place() if result == FlexGraspErrorCodes.SUCCESS: result = self.command_to_home() if result == FlexGraspErrorCodes.SUCCESS: result = self.reset_msg() return result def reset_msg(self): rospy.logdebug("[{0}] Resetting for next grasp".format(self.node_name)) for key in self.pose: self.pose[key] = None return FlexGraspErrorCodes.SUCCESS def received_all_data(self): success = True for key in self.pose: if self.pose[key] is None: success = False return success def log_state_update(self): rospy.loginfo("[{0}] updated pick place state, from {1} to {2}".format(self.node_name, self.prev_state, self.state)) def update_state(self, success): if (self.state == "idle") and not self.received_all_data(): self.prev_state = self.state self.state = "init" self.log_state_update() elif self.state == "init" and self.received_all_data(): self.prev_state = self.state self.state = "idle" self.log_state_update() elif (self.state == "idle") and (self.command == "pick") and success: self.prev_state = self.state self.state = "picked" self.log_state_update() elif (self.state == "picked") and (success is True or success is False): self.prev_state = self.state self.state = "init" self.log_state_update() def take_action(self): msg = FlexGraspErrorCodes() result = None # State dependent actions if self.state == "init" and not self.received_all_data(): if self.command == "pick": rospy.logwarn("[{0}] Can not pick object, it still needs to be detected!".format(self.node_name)) result = FlexGraspErrorCodes.STATE_ERROR if self.command == "e_init": rospy.logdebug("[{0}] executing e_init command".format(self.node_name)) result = FlexGraspErrorCodes.SUCCESS if self.state == "idle": if self.command == "pick": result = self.pick() elif self.command == "place": rospy.logwarn("[{0}] Can not place object, it is not picked!".format(self.node_name)) result = FlexGraspErrorCodes.STATE_ERROR elif self.state == "picked": if self.command == "place": result = self.place() elif self.command == "pick": rospy.logwarn("[{0}] Can not pick object, it still needs to be placed!".format(self.node_name)) result = FlexGraspErrorCodes.STATE_ERROR elif self.command == "reset": result = self.reset_msg() success = (result == FlexGraspErrorCodes.SUCCESS) if result is None: success = None self.update_state(success) # publish success if result is not None: msg.val = result flex_grasp_error_log(result, self.node_name) self.pub_e_out.publish(msg) self.command = None
class Calibration(object): """Calibration""" node_name = "CALIBRATION" def __init__(self, node_name, playback=False): # TODO: whe shouldn't have to run this node in a seperate calibration namespace, it would make things much better self.robot_ns = 'px150' self.node_name = node_name self.playback = playback self.command = None if self.playback: rospy.loginfo( "[{0}] Calibration launched in playback mode!".format( self.node_name)) rospy.sleep(5) rospy.logdebug("[CALIBRATE] initializing hand eye client") self.client = HandeyeClient() self.experiment_info = ExperimentInfo(self.node_name, namespace=self.robot_ns, id="initial_calibration") # Listen rospy.logdebug("[CALIBRATE] initializing tf2_ros buffer") self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.calibration_frame = rospy.get_param('/' + self.robot_ns + '/' + 'robot_base_frame') self.planning_frame = rospy.get_param('/' + self.robot_ns + '/' + 'planning_frame') self.pose_array = None self.pub_e_out = rospy.Publisher("~e_out", FlexGraspErrorCodes, queue_size=10, latch=True) move_robot_topic = '/' + self.robot_ns + '/' + 'move_robot' robot_pose_topic = '/' + self.robot_ns + '/' + 'robot_pose' pose_array_topic = '/' + self.robot_ns + '/' + 'pose_array' self.move_robot_communication = Communication(move_robot_topic, timeout=15, node_name=self.node_name) self.robot_pose_publisher = rospy.Publisher(robot_pose_topic, PoseStamped, queue_size=10, latch=False) self.pose_array_publisher = rospy.Publisher(pose_array_topic, PoseArray, queue_size=5, latch=True) self.output_logger = DataLogger( self.node_name, {"calibration": "calibration_transform"}, {"calibration": TransformStamped}, bag_name=self.node_name) # Subscribe rospy.Subscriber("~e_in", String, self.e_in_cb) # Subscribe to aruco tracker for it to publish the tf rospy.Subscriber('/' + self.robot_ns + '/' + 'aruco_tracker/pose', PoseStamped, self.aruco_tracker_cb) def e_in_cb(self, msg): if self.command is None: self.command = msg.data rospy.logdebug("[{0}] Received event message: {1}".format( self.node_name, self.command)) # reset outputting message msg = FlexGraspErrorCodes() msg.val = FlexGraspErrorCodes.NONE self.pub_e_out.publish(msg) def aruco_tracker_cb(self, msg): pass def init_poses_1(self): r_amplitude = 0.00 z_amplitude = 0.00 r_min = 0.24 z_min = 0.28 # 0.05 pos_intervals = 1 if pos_intervals == 1: r_vec = [r_min + r_amplitude ] # np.linspace(x_min, x_min + 2*x_amplitude, 2) # z_vec = [z_min + z_amplitude] else: r_vec = np.linspace(r_min, r_min + 2 * r_amplitude, pos_intervals) z_vec = np.linspace(z_min, z_min + 2 * z_amplitude, pos_intervals) ai_amplitude = np.deg2rad(38.0) aj_amplitude = np.deg2rad(38.0) ak_amplitude = np.deg2rad(15.0) rot_intervals = 2 ai_vec = np.linspace(-ai_amplitude, ai_amplitude, rot_intervals) aj_vec = np.linspace(-aj_amplitude, aj_amplitude, rot_intervals) ak_vec = [-ak_amplitude, ak_amplitude] return self.generate_poses(r_vec, z_vec, ai_vec, aj_vec, ak_vec) def init_poses_2(self): surface_height = 0.019 height_finger = 0.040 # [m] finger_link2ee_link = 0.023 # [m] grasp_height = height_finger + finger_link2ee_link - surface_height sag_angle = np.deg2rad(6.0) # [deg] r_amplitude = 0.08 z_amplitude = 0.00 r_min = 0.10 z_min = grasp_height # 0.05 pos_intervals = 3 if pos_intervals == 1: r_vec = [r_min + r_amplitude ] # np.linspace(x_min, x_min + 2*x_amplitude, 2) # z_vec = [z_min + z_amplitude] else: r_vec = np.linspace(r_min, r_min + 2 * r_amplitude, pos_intervals) z_vec = np.linspace(z_min, z_min + 2 * z_amplitude, pos_intervals) + np.tan(sag_angle) * r_vec ak_amplitude = np.deg2rad(15.0) rot_intervals = 2 ai_vec = [np.deg2rad(0)] aj_vec = [np.deg2rad(90)] ak_vec = [-ak_amplitude, ak_amplitude] return self.generate_poses_2(r_vec, z_vec, ai_vec, aj_vec, ak_vec) def generate_poses(self, r_vec, z_vec, ai_vec, aj_vec, ak_vec): pose_array = PoseArray() pose_array.header.frame_id = self.calibration_frame pose_array.header.stamp = rospy.Time.now() poses = [] for ak in ak_vec: for r in r_vec: for z in z_vec: for aj in aj_vec: for ai in ai_vec: pose = Pose() x = r * np.cos(ak) y = r * np.sin(ak) pose.position = list_to_position([x, y, z]) pose.orientation = list_to_orientation( [ai, aj, ak]) poses.append(pose) pose_array.poses = poses self.pose_array_publisher.publish(pose_array) self.pose_array = pose_array return FlexGraspErrorCodes.SUCCESS def generate_poses_2(self, r_vec, z_vec, ai_vec, aj_vec, ak_vec): pose_array = PoseArray() pose_array.header.frame_id = self.calibration_frame pose_array.header.stamp = rospy.Time.now() poses = [] for ak in ak_vec: for r, z in zip(r_vec, z_vec): for aj in aj_vec: for ai in ai_vec: pose = Pose() x = r * np.cos(ak) y = r * np.sin(ak) pose.position = list_to_position([x, y, z]) pose.orientation = list_to_orientation([ai, aj, ak]) poses.append(pose) pose_array.poses = poses self.pose_array_publisher.publish(pose_array) self.pose_array = pose_array return FlexGraspErrorCodes.SUCCESS def calibrate(self, track_marker=True): sample_list = self.client.get_sample_list().camera_marker_samples n_samples = len(sample_list) if n_samples > 0: rospy.loginfo( "[{0}] Found {1} old calibration samples, deleting them before recalibrating!" .format(self.node_name, n_samples)) for i in reversed(range(n_samples)): rospy.loginfo("[{0}] Deleting sample {1}".format( self.node_name, i)) self.client.remove_sample(i) sample_list = self.client.get_sample_list().camera_marker_samples n_samples = len(sample_list) if n_samples > 0: rospy.logwarn( "[{0}] Failed to remove all old samples, cannot calibrate". format(self.node_name)) print sample_list return FlexGraspErrorCodes.FAILURE else: rospy.loginfo("[{0}] All old samples have been removed".format( self.node_name)) if self.playback: rospy.loginfo( "[{0}] Playback is active: publishing messages from bag!". format(self.node_name)) messages = self.output_logger.load_messages_from_bag( self.experiment_info.path, self.experiment_info.id) if messages is not None: self.broadcast(messages['calibration']) return FlexGraspErrorCodes.SUCCESS else: return FlexGraspErrorCodes.FAILURE if self.pose_array is None: rospy.logwarn("[CALIBRATE] pose_array is still empty") return FlexGraspErrorCodes.REQUIRED_DATA_MISSING try: trans = self.tfBuffer.lookup_transform( self.planning_frame, self.pose_array.header.frame_id, rospy.Time(0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn("[CALIBRATE] failed to get transform from %s to %s", self.pose_array.header.frame_id, self.planning_frame) return FlexGraspErrorCodes.TRANSFORM_POSE_FAILED result = self.move_robot_communication.wait_for_result("reset") for pose in self.pose_array.poses: if rospy.is_shutdown(): return FlexGraspErrorCodes.SHUTDOWN pose_stamped = PoseStamped() pose_stamped.header = self.pose_array.header pose_stamped.pose = pose # transform to planning frame pose_trans = tf2_geometry_msgs.do_transform_pose( pose_stamped, trans) self.robot_pose_publisher.publish(pose_trans) result = self.move_robot_communication.wait_for_result( "move_manipulator") # timout = 5? if result == FlexGraspErrorCodes.SUCCESS: if track_marker: # camera delay + wait a small amount of time for vibrations to stop rospy.sleep(1.5) try: self.client.take_sample() except: rospy.logwarn( "[CALIBRATE] Failed to take sample, marker might not be visible." ) elif result == FlexGraspErrorCodes.DYNAMIXEL_ERROR: rospy.logwarn( "[{0}] Dynamixel error triggered, returning error".format( self.node_name)) return result elif result == FlexGraspErrorCodes.DYNAMIXEL_SEVERE_ERROR: rospy.logwarn( "[{0}] Dynamixel error triggered, returning error".format( self.node_name)) return result # reset result = self.move_robot_communication.wait_for_result("home") # compute calibration transform if not track_marker: return FlexGraspErrorCodes.SUCCESS else: calibration_transform = self.client.compute_calibration() if calibration_transform.valid: rospy.loginfo("[CALIBRATE] Found valid transfrom") self.broadcast(calibration_transform.calibration.transform) self.client.save() return FlexGraspErrorCodes.SUCCESS else: rospy.logwarn("[CALIBRATE] Computed calibration is invalid") return FlexGraspErrorCodes.FAILURE def broadcast(self, transform): rospy.loginfo("[{0}] Broadcasting result".format(self.node_name)) broadcaster = tf2_ros.StaticTransformBroadcaster() broadcaster.sendTransform(transform) if not self.playback: self.output_logger.write_messages_to_bag( {"calibration": transform}, self.experiment_info.path, self.experiment_info.id) def take_action(self): msg = FlexGraspErrorCodes() result = None if self.command == 'e_init': result = FlexGraspErrorCodes.SUCCESS elif self.command == 'calibrate': result = self.init_poses_1() if result == FlexGraspErrorCodes.SUCCESS: result = self.calibrate() elif self.command == 'calibrate_height': result = self.init_poses_2() if result == FlexGraspErrorCodes.SUCCESS: result = self.calibrate(track_marker=False) elif self.command is not None: rospy.logwarn( "[CALIBRATE] Can not take an action: received unknown command %s!", self.command) result = FlexGraspErrorCodes.UNKNOWN_COMMAND # publish success if result is not None: msg.val = result flex_grasp_error_log(result, self.node_name) self.pub_e_out.publish(msg) self.command = None
class MoveRobot(object): """MoveRobot""" node_name = "MOVE_ROBOT" def __init__(self): super(MoveRobot, self).__init__() self.debug_mode = rospy.get_param("move_robot/debug") if self.debug_mode: log_level = rospy.DEBUG rospy.loginfo("[MOVE ROBOT] Launching move robot in debug mode") else: log_level = rospy.INFO rospy.init_node("move_robot", anonymous=True, log_level=log_level) # wait until clock is initialized while rospy.get_time() < 0.1: pass self.force_robot = False self.state = "idle" self.prev_state = None self.command = None self.robot_pose = None self.rate = rospy.Rate(10) self.simulation = self.debug_mode = rospy.get_param("robot_sim") # tolerance self.position_tol = 0.03 # [m] self.orientation_tol = np.deg2rad(10.0) # [rad] self.man_joint_tolerance = np.deg2rad(5.0) # [rad] self.ee_joint_tolerance = 0.002 # [m] self.initialise_robot() self.initialise_enviroment() self.tfBuffer = tf2_ros.Buffer() tf2_ros.TransformListener(self.tfBuffer) self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) # Subscribers rospy.Subscriber("robot_pose", PoseStamped, self.robot_pose_cb) # rospy.Subscriber("endEffectorDistance", Float64, self.ee_distance_cb) rospy.Subscriber("~e_in", String, self.e_in_cb) # Publishers self.pub_e_out = rospy.Publisher("~e_out", FlexGraspErrorCodes, queue_size=10, latch=True) # init monitor robot communication monitor_robot_topic = "monitor_robot" self.monitor_robot_communication = Communication(monitor_robot_topic, timeout=15) def robot_pose_cb(self, msg): if self.robot_pose is None: if msg.header.frame_id != self.man_planning_frame: msg = self.transform_pose(msg, self.man_planning_frame) self.robot_pose = msg rospy.logdebug("[MOVE ROBOT] Received new robot pose massage") def e_in_cb(self, msg): if self.command is None: self.command = msg.data rospy.logdebug("[MOVE ROBOT] Received new move robot event in message: %s", self.command) # reset outputting message msg = FlexGraspErrorCodes() msg.val = FlexGraspErrorCodes.NONE self.pub_e_out.publish(msg) ### @brief ROS Subscriber Callback function to update the latest arm joint states ### @param msg - latest JointState message ### @details - the JointState message is mainly used to determine current gripper position def joint_state_cb(self, msg): self.joint_states = msg def initialise_robot(self): rospy.logdebug("===INITIALIZING ROBOT====") moveit_commander.roscpp_initialize(sys.argv) ## This object is the outer-level interface to the robot: robot = moveit_commander.RobotCommander() ## This object is an interface to one group of joints. man_group_name = rospy.get_param('manipulator_group_name') ee_group_name = rospy.get_param('ee_group_name') rospy.logdebug("[MOVE ROBOT] Manipulator group name: %s", man_group_name) rospy.logdebug("[MOVE ROBOT] End effector group name: %s", ee_group_name) man_group = moveit_commander.MoveGroupCommander(man_group_name) ee_group = moveit_commander.MoveGroupCommander(ee_group_name) man_planning_frame = man_group.get_planning_frame() ee_planning_frame = ee_group.get_planning_frame() ee_link = man_group.get_end_effector_link() rospy.logdebug("[MOVE ROBOT] Manipulator planning frame: %s", man_planning_frame) rospy.loginfo("[MOVE ROBOT] Manipulator end effector link: %s", ee_link) # px150/ee_arm_link rospy.logdebug("[MOVE ROBOT] End effector planning frame: %s", ee_planning_frame) manipulator_joint_names = man_group.get_joints() ee_joint_names = ee_group.get_named_target_values("Closed").keys() # the ee_group contains joints we cannot actually control? EE_CLOSED = ee_group.get_named_target_values("Closed").values() EE_OPEN = ee_group.get_named_target_values("Open").values() # Allow replanning to increase the odds of a solution man_group.allow_replanning(True) # Allow 5 seconds per planning attempt man_group.set_planning_time(5) man_group.set_max_velocity_scaling_factor(0.8) man_group.set_max_acceleration_scaling_factor(0.7) # Allow some leeway in position (meters) and orientation (radians) affects PLANNING! man_group.set_goal_position_tolerance(0.005) man_group.set_goal_orientation_tolerance(np.deg2rad(0.5)) man_group.set_goal_joint_tolerance(np.deg2rad(0.5)) # # ee_group.set_goal_position_tolerance(self.position_tol) # ee_group.set_goal_orientation_tolerance(self.orientation_tol) # ee_group.set_goal_joint_tolerance(self.ee_joint_tol) if not self.simulation: rospy.wait_for_service("get_robot_info") srv_robot_info = rospy.ServiceProxy("get_robot_info", RobotInfo) self.resp = srv_robot_info() self.num_joints = self.resp.num_joints self.gripper_index = self.num_joints + 1 self.set_operating_mode_srv = rospy.ServiceProxy('set_operating_modes', OperatingModes) self.pub_gripper_command = rospy.Publisher("single_joint/command", SingleCommand, queue_size=10, latch=True) self.sub_joint_states = rospy.Subscriber("joint_states", JointState, self.joint_state_cb) self.gripper_command = SingleCommand() self.gripper_command.joint_name = 'gripper' self.gripper_command.cmd = 0 self.joint_states = JointState() self.close_pwm_cmd = -250 self.open_pwm_cmd = 250 self.man_group_name = man_group_name self.ee_group_name = ee_group_name self.robot = robot self.man_group = man_group self.ee_group = ee_group self.man_planning_frame = man_planning_frame self.ee_planning_frame = ee_planning_frame self.ee_link = ee_link self.ee_joint_names = ee_joint_names self.EE_OPEN = EE_OPEN self.EE_CLOSED = EE_CLOSED def initialise_enviroment(self): """" Checks wether the RViz enviroment is correctly set """ rospy.logdebug("===INITIALIZING ENVIROMENT====") # interface to the world surrounding the robot. self.scene = moveit_commander.PlanningSceneInterface() rospy.sleep(1) self.target_object_name = 'peduncle' known_objects_prev = None required_object = 'table' found_required_object = False start_time = rospy.get_time() curr_time = rospy.get_time() timeout = 10 while (curr_time - start_time < timeout) and not found_required_object: known_objects = self.scene.get_known_object_names() if not known_objects == known_objects_prev: known_objects_prev = known_objects rospy.logdebug("[MOVE ROBOT] Known objects: %s", known_objects) if (required_object in known_objects): rospy.logdebug("[MOVE ROBOT] Continueing initialization, required object is present") found_required_object = True else: rospy.logwarn("[MOVE ROBOT] Refusing to continue until %s is present.", required_object) found_required_object = True # False self.rate.sleep() def check_ik(self, pose_stamped, timeout=rospy.Duration(5)): """Checks inverse kinematics for the given pose. Args: pose_stamped: geometry_msgs/PoseStamped. timeout: rospy.Duration. How long to wait before giving up on the IK solution. Returns: True if the inverse kinematics were found, False otherwise. """ request = GetPositionIKRequest() request.ik_request.pose_stamped = pose_stamped request.ik_request.group_name = self.man_group_name request.ik_request.timeout = timeout response = self._compute_ik(request) if not response.error_code.val == MoveItErrorCodes.SUCCESS: rospy.logwarn("[MOVE ROBOT] Goal pose is not reachable: inverse kinematics can not be found") return False else: return True def check_frames(self, goal_frame): # remove pre appended is present "/" if self.man_planning_frame[1:] == '/': planning_frame = self.man_planning_frame[1:] else: planning_frame = self.man_planning_frame if goal_frame == planning_frame: return True else: rospy.logwarn("[MOVE ROBOT] Goal pose specified with respect to wrong frame: should be specified with respect to %s, but is specified with respect to %s", planning_frame, goal_frame) return False def remove_attached_target_object(self): attached_objects = self.scene.get_attached_objects(object_ids=[self.target_object_name]) if self.target_object_name in attached_objects.keys(): if attached_objects[self.target_object_name].link_name == self.ee_link: rospy.logdebug("[MOVE ROBOT] Removing attached objects from ee_link") self.scene.remove_attached_object(self.ee_link, self.target_object_name) self.rate.sleep() self.scene.remove_world_object(self.target_object_name) return True else: rospy.logdebug( "[MOVE ROBOT] Cannot remove attached object: target object is not attached to the end effector link!") return True else: rospy.logdebug("[MOVE ROBOT] Cannot remove attached object: target object is not attached to anything!") return True def attach_object(self): grasping_group = self.ee_group_name touch_links = self.robot.get_link_names(group=grasping_group) self.scene.attach_box(self.ee_link, self.target_object_name, touch_links=touch_links) self.rate.sleep() return FlexGraspErrorCodes.SUCCESS def go_to_random_pose(self): rospy.logdebug("[MOVE ROBOT] Going to random pose") goal_pose = self.man_group.get_random_pose() success = self.go_to_pose(goal_pose) # self.grasp_pose = None return success def sleep_man(self): rospy.logdebug("[MOVE ROBOT] Sleeping manipulator") return self.move_to_joint_target(self.man_group, 'Sleep') def home_man(self): rospy.logdebug("[MOVE ROBOT] Homeing manipulator") return self.move_to_joint_target(self.man_group, 'Upright') def ready_man(self): rospy.logdebug("[MOVE ROBOT] Moving manipulator to ready joint target") return self.move_to_joint_target(self.man_group, 'Home') def open_ee(self): rospy.logdebug("[MOVE ROBOT] Opening end effector") if self.simulation: return self.move_to_joint_target(self.ee_group, "Open") else: self.set_operating_mode() return self.move_to_joint_target_pwm(self.open_pwm_cmd) def close_ee(self): rospy.logdebug("[MOVE ROBOT] Closing end effector") if self.simulation: return self.move_to_joint_target(self.ee_group, "Closed") else: self.set_operating_mode() return self.move_to_joint_target_pwm(self.close_pwm_cmd) def apply_release_ee(self): rospy.logdebug("[MOVE ROBOT] Aplying release with end effector") result = self.open_ee() if result == FlexGraspErrorCodes.SUCCESS: self.remove_attached_target_object() return result def apply_grasp_ee(self): rospy.logdebug("[MOVE ROBOT] Aplying grasping with end effector") result = self.attach_object() if result == FlexGraspErrorCodes.SUCCESS: result = self.close_ee() return result def monitor_group(self, group): """Ask permission from robot monitor""" group_name = group.get_name() if group_name == self.man_group_name: command = "monitor_arm" elif group_name == self.ee_group_name: command = "monitor_gripper" return self.monitor_robot_communication.wait_for_result(command) def go_to_pose(self, goal_pose, robot_force=False): rospy.logdebug("[MOVE ROBOT] Go to pose") if goal_pose is None: return FlexGraspErrorCodes.NO_GOAL_POSE if not self.check_frames(goal_pose.header.frame_id): return FlexGraspErrorCodes.INVALID_FRAME if not self.check_ik(goal_pose): return FlexGraspErrorCodes.NO_IK_SOLUTION result = self.monitor_group(self.man_group) if (result is not FlexGraspErrorCodes.SUCCESS) and (not self.force_robot): return result self.man_group.set_pose_target(goal_pose.pose) plan = self.man_group.plan() # if no plan found... very ugly, what is a better way to do this? if len(plan.joint_trajectory.joint_names) == 0: rospy.logwarn('[MOVE ROBOT] not attempting to go to pose goal, no plan found!') self.man_group.clear_pose_targets() return FlexGraspErrorCodes.PLANNING_FAILED success = self.man_group.execute(plan, wait=True) if success is not True: rospy.logwarn('[MOVE ROBOT] Controller failed during execution, trying to continue...') # return FlexGraspErrorCodes.CONTROL_FAILED # Ensures that there is no residual movement and clear the target rospy.logdebug("[MOVE ROBOT] executed plan") self.man_group.stop() self.man_group.clear_pose_targets() result = self.wait_for_pose_close(self.man_group, goal_pose, self.position_tol, self.orientation_tol, 3) return result def set_operating_mode(self): request = OperatingModesRequest() request.cmd = 3 # gripper request.mode = 'pwm' request.use_custom_profiles = False self.set_operating_mode_srv(request) def move_to_joint_target_pwm(self, command, timeout=2.0): rospy.logdebug("[MOVE ROBOT] Set PWM target") if self.force_robot: rospy.loginfo("[MOVE ROBOT] Forcing to execute command!") else: result = self.monitor_group(self.ee_group) if result != FlexGraspErrorCodes.SUCCESS: return result self.gripper_command.cmd = command self.pub_gripper_command.publish(self.gripper_command) start_time = rospy.get_time() curr_time = rospy.get_time() while (curr_time - start_time < timeout) and not rospy.is_shutdown(): js_msg = list(self.joint_states.position) if len(js_msg) != 0: if ((self.gripper_command.cmd > 0 and js_msg[self.gripper_index] >= self.EE_OPEN[0]) or (self.gripper_command.cmd < 0 and js_msg[self.gripper_index] <= self.EE_CLOSED[0])): self.gripper_command.cmd = 0 self.pub_gripper_command.publish(self.gripper_command) return FlexGraspErrorCodes.SUCCESS if rospy.is_shutdown(): return FlexGraspErrorCodes.SHUTDOWN curr_time = rospy.get_time() self.rate.sleep() if self.gripper_command.cmd > 0: js_target = self.EE_OPEN[0] elif self.gripper_command.cmd < 0: js_target = self.EE_CLOSED[0] else: js_target = None rospy.logwarn("[{0}] Control failed: gripper did not reach target within allocated time".format(self.node_name)) rospy.loginfo("[{0}] joint state target is {1}".format(self.node_name, js_target)) rospy.loginfo("[{0}] joint state actual is {1}".format(self.node_name, js_msg[self.gripper_index])) return FlexGraspErrorCodes.CONTROL_FAILED def move_to_joint_target(self, group, target): rospy.logdebug("[{0}] Go to joint target".format(self.node_name)) result = FlexGraspErrorCodes.SUCCESS if self.force_robot: rospy.loginfo("[{0}] Forcing to execute command!".format(self.node_name)) else: result = self.monitor_group(group) if (result != FlexGraspErrorCodes.SUCCESS): return result to_check = True # if the target is a named target, get the corresponding joint values if type(target) is str: if target == "Sleep": to_check = False target = group.get_named_target_values(target) group.set_joint_value_target(target) plan = group.plan() success = group.execute(plan, wait=True) if success is not True: rospy.logwarn('[MOVE ROBOT] Controller failed during excution!') return FlexGraspErrorCodes.CONTROL_FAILED group.stop() # group.get_joint_value_target() returns zeros on ee_group_name # target.values() returns values in the wrong order on man_group if (group.get_name() == self.ee_group_name): target_val = target.values() else: target_val = group.get_joint_value_target() if (group.get_name() == self.ee_group_name): tol = self.ee_joint_tolerance else: tol = self.man_joint_tolerance # oscilations can cause the robot to be at and different pose than desired, therefore we check several times if to_check: result = self.wait_for_joint_close(group, target_val, tol, 2.0) group.clear_pose_targets() return result def wait_for_pose_close(self, group, goal_pose, pos_tol, or_tol, timeout): start_time = rospy.get_time() curr_time = rospy.get_time() while curr_time - start_time < timeout: current_pose = group.get_current_pose() position_close, orientation_close = pose_close(goal_pose, current_pose, pos_tol, or_tol) if position_close and orientation_close: return FlexGraspErrorCodes.SUCCESS self.rate.sleep() curr_time = rospy.get_time() if orientation_close == False: rospy.logwarn( "[MOVE ROBOT] Failed to move to pose target, obtained orientation is not sufficiently close to goal orientation (tolerance: %s)", or_tol) if position_close == False: rospy.logwarn( "[MOVE ROBOT] Failed to move to pose target, obtained position is not sufficiently close to goal position (tolerance: %s)", pos_tol) rospy.logdebug("[MOVE ROBOT] Goal pose: %s", pose_to_list(goal_pose.pose)) rospy.logdebug("[MOVE ROBOT] Curr pose: %s", pose_to_list(current_pose.pose)) return FlexGraspErrorCodes.CONTROL_FAILED def wait_for_joint_close(self, group, target, tol, timeout): start_time = rospy.get_time() curr_time = rospy.get_time() while curr_time - start_time < timeout: current = group.get_current_joint_values() if len(current) == 0: rospy.logwarn("[MOVE ROBOT] Failed to get current robot state") group.clear_pose_targets() return FlexGraspErrorCodes.CONTROL_FAILED if joint_close(target, current, tol): return FlexGraspErrorCodes.SUCCESS self.rate.sleep() curr_time = rospy.get_time() rospy.logwarn( "[MOVE ROBOT] Failed to move to joint target: obtained joint values are not sufficiently close to target joint value (tolerance: %s)", tol) rospy.loginfo("[MOVE ROBOT] Target joint values: %s", target) rospy.loginfo("[MOVE ROBOT] Actual joint values: %s", current) return FlexGraspErrorCodes.CONTROL_FAILED def wait_for_robot_pose(self, timeout=1): start_time = rospy.get_time() while (rospy.get_time() - start_time < timeout): if self.robot_pose is not None: return FlexGraspErrorCodes.SUCCESS rospy.sleep(0.1) return FlexGraspErrorCodes.NO_GOAL_POSE def transform_pose(self, pose_stamped, to_frame): original_frame = pose_stamped.header.frame_id try: transform = self.tfBuffer.lookup_transform(to_frame, original_frame, time=rospy.Time.now()) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn("[%s] Cannot transform pose, failed to lookup transform from %s to %s!", self.node_name, original_frame, to_frame) return FlexGraspErrorCodes.TRANSFORM_POSE_FAILED return tf2_geometry_msgs.do_transform_pose(pose_stamped, transform) def reset(self): rospy.logdebug("[MOVE ROBOT] Resetting robot pose") self.robot_pose = None return FlexGraspErrorCodes.SUCCESS def take_action(self): msg = FlexGraspErrorCodes() result = None # General actions, non state dependent if self.command == "move_manipulator": # rospy.loginfo("[MOVE ROBOT] Going to pose...") result = self.wait_for_robot_pose() if result == FlexGraspErrorCodes.SUCCESS: result = self.go_to_pose(self.robot_pose) # rospy.loginfo("[MOVE ROBOT] Result: %s", move_robot_result_string(result)) self.robot_pose = None elif self.command == "sleep": result = self.sleep_man() elif self.command == "force_robot": self.force_robot = True result = FlexGraspErrorCodes.SUCCESS elif self.command == "do_not_force_robot": self.force_robot = False result = FlexGraspErrorCodes.SUCCESS elif self.command == "home": result = self.home_man() elif self.command == "ready": result = self.ready_man() elif self.command == "grasp": result = self.apply_grasp_ee() elif self.command == "release": result = self.apply_release_ee() elif self.command == "open": result = self.apply_release_ee() elif self.command == "close": result = self.close_ee() elif self.command == "reset": result = self.reset() elif self.command == "e_init": result = FlexGraspErrorCodes.SUCCESS elif self.command == None: pass else: rospy.logwarn("Received unknown command: %s", self.command) result = FlexGraspErrorCodes.UNKNOWN_COMMAND # publish result if result is not None: flex_grasp_error_log(result, self.node_name) msg.val = result self.pub_e_out.publish(msg) self.command = None