def main(): rospy.init_node("custom_cost_service") arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-c', '--config', dest='config', required=True, help='the file path of configuration config.json file ') args = parser.parse_args(rospy.myargv()[1:]) config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() constraints = ConstraintFactory(configs).generate_constraints() # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=None) ccs = CustomCostService(environment, constraints_topic='/lfd/applied_constraints', cost_service_name='custom_cost') ccs.start_server()
def build_environment(self): items = ItemFactory(self.configs["robots"], self.configs['items']).generate_items() constraints = ConstraintFactory( self.configs["constraints"]).generate_constraints() # We only have just the one robot...for now....... self.environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=None)
def build_environment(self): robots = RobotFactory(self.configs['robots']).generate_robots() items = ItemFactory(self.configs['items']).generate_items() triggers = TriggerFactory(self.configs['triggers']).generate_triggers() constraints = ConstraintFactory( self.configs['constraints']).generate_constraints() # We only have just the one robot...for now....... self.environment = Environment(items=items, robot=robots[0], constraints=constraints, triggers=triggers)
def main(): """ Demonstration Recorder Record a series of demonstrations. """ rospy.init_node("trigger_test") arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-c', '--config', dest='config', required=True, help='the file path of the demonstration ' ) args = parser.parse_args(rospy.myargv()[1:]) config_filepath = args.config configs = import_configuration(config_filepath) triggers = TriggerFactory(configs).generate_triggers() for trigger in triggers: print(type(trigger)) config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() triggers = TriggerFactory(configs).generate_triggers() constraints = ConstraintFactory(configs).generate_constraints() # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=triggers) quit = GracefulQuit() while not quit.kill: rospy.sleep(2) print(environment.check_constraint_triggers())
def main(): """ Demonstration Recorder Record a series of demonstrations. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-c', '--config', dest='config', required=True, help='the file path of the demonstration ') required.add_argument( '-d', '--directory', dest='directory', required=True, help='the directory to save raw demonstration .json files') parser.add_argument('-r', '--record_rate', type=int, default=45, metavar='RECORDRATE', help='rate at which to record (default: 45)') args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("sdk_joint_recorder") print("Getting robot state... ") rs = intera_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() recorder = SawyerRecorder(args.record_rate) rospy.on_shutdown(recorder.stop) config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() constraints = ConstraintFactory( configs["constraints"]).generate_constraints() # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints) exp = DataExporter() print("Recording. Press Ctrl-C to stop.") demos = recorder.record_demonstrations(environment) # Build processors and process demonstrations to generate derivative data e.g. relative position. rk_processor = RelativeKinematicsProcessor(environment.get_item_ids(), environment.get_robot_id()) ic_processor = InContactProcessor(environment.get_item_ids(), environment.get_robot_id(), .06, .5) soi_processor = SphereOfInfluenceProcessor(environment.get_item_ids(), environment.get_robot_id()) rp_processor = RelativePositionProcessor(environment.get_item_ids(), environment.get_robot_id()) pipeline = ProcessorPipeline( [rk_processor, ic_processor, soi_processor, rp_processor]) pipeline.process(demonstrations) # Analyze for applied constraints. This will apply a set of constraints for all observations that for which the constraint set holds true. i.e. true until its not. constraint_analyzer = ConstraintAnalyzer(environment) for demo in demos: constraint_analyzer.applied_constraint_evaluator(demo.observations) exp = DataExporter() for idx, demo in enumerate(demos): raw_data = [obs.data for obs in demo.observations] print("'/raw_demonstration{}.json': {} observations".format( idx, len(raw_data))) exp.export_to_json( args.directory + "/raw_demonstration{}.json".format(idx), raw_data)
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-c', '--config', dest='config', required=True, help='the file path of configuration config.json file ') required.add_argument( '-d', '--directory', dest='directory', required=True, help= 'the directory from which to input labeled demonstration .json files') parser.add_argument('-b', '--bandwidth', type=float, default=.025, metavar='BANDWIDTH', help='gaussian kernel density bandwidth') parser.add_argument('-t', '--threshold', type=int, default=-1200, metavar='THRESHOLD', help='log-liklihood threshold value') parser.add_argument( '-n', '--number_of_samples', type=int, default=50, metavar='NUMBEROFSAMPLES', help='the number of samples to validate for each keyframe') args = parser.parse_args(rospy.myargv()[1:]) # Import the data importer = DataImporter() labeled_demonstrations = importer.load_json_files(args.directory + "/*.json") # Convert imported data into Demonstrations and Observations demonstrations = [] for datum in labeled_demonstrations["data"]: observations = [] for entry in datum: observations.append(Observation(entry)) demonstrations.append(Demonstration(observations)) if len(demonstrations) == 0: rospy.logwarn("No demonstration data to model!!") return 0 rospy.init_node("graph_traverse") """ Create the Cairo LfD environment """ config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() constraints = ConstraintFactory(configs).generate_constraints() # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=None) """ Create the moveit_interface """ moveit_interface = SawyerMoveitInterface() moveit_interface.set_velocity_scaling(.35) moveit_interface.set_acceleration_scaling(.25) """ Create KeyframeGraph object. """ graph = KeyframeGraph() cluster_generator = ObservationClusterer() """ Generate clusters using labeled observations, build the models, graphs, and atributes for each cluster in the KeyFrameGraph """ clusters = cluster_generator.generate_clusters(demonstrations) for cluster_id in clusters.keys(): graph.add_node(cluster_id) graph.nodes[cluster_id]["observations"] = clusters[cluster_id][ "observations"] graph.nodes[cluster_id]["keyframe_type"] = clusters[cluster_id][ "keyframe_type"] graph.nodes[cluster_id]["applied_constraints"] = clusters[cluster_id][ "applied_constraints"] graph.nodes[cluster_id]["meta_constraints"] = {} graph.nodes[cluster_id]["model"] = KDEModel(kernel='gaussian', bandwidth=args.bandwidth) graph.add_path(graph.nodes()) graph.fit_models(get_observation_joint_vector) graph._identify_primal_observations(get_observation_joint_vector) rospy.loginfo(graph.get_keyframe_sequence()) for node in graph.get_keyframe_sequence(): print("KEYFRAME: {}".format(node)) print(graph.nodes[node]["keyframe_type"]) print(graph.nodes[node]["applied_constraints"]) print # Create height segmentation and heuristic model position_vectorizor = partial(vectorize_demonstration, vectorizors=[vectorize_robot_position]) position_vectors = np.array(map(position_vectorizor, demonstrations)) # stack all observation vectors X = np.vstack(position_vectors) height_segment_model = BayesianGMMSegmentModel(X, n_components=10) height_heuristic = HeightHeuristicModel(height_segment_model) height_heuristic.fit() height_static_parameters = { "item_id": 1, "reference_height": 0, "direction": "positive" } height_metaconstraint_builder = HeightMetaconstraintBuilder( height_heuristic, height_static_parameters) metaconstraint_assigner = MetaconstraintAssigner( environment, graph, [height_metaconstraint_builder]) metaconstraint_assigner.assign_metaconstraints() """ Build a ConstraintAnalyzer and KeyframeGraphAnalyzer """ constraint_analyzer = ConstraintAnalyzer(environment) graph_analyzer = KeyframeGraphAnalyzer(graph, moveit_interface, get_observation_joint_vector) sample_to_obsv_converter = SawyerSampleConverter(moveit_interface) sampler = KeyframeSampler(constraint_analyzer, sample_to_obsv_converter) """ Generate raw_samples from graph for each keyframe """ for node in graph.get_keyframe_sequence(): n_samples = args.number_of_samples constraints = [ meta.constraints[4] for meta in graph.nodes[node]["metaconstraints"] ] for constraint in constraints: print constraint attempts, samples, matched_ids = sampler.generate_n_valid_samples( graph.nodes[node]["model"], graph.nodes[node]["primal_observation"], constraints, n=n_samples) rospy.loginfo("Keyframe %d: %s valid of %s attempts", node, len(samples), attempts) if len(samples) < n_samples: rospy.loginfo("Keyframe %d: only %s of %s waypoints provided", node, len(samples), n_samples) if len(samples) == 0: # TODO: DOWN SAMPLE METACONSTRAINTS AND KEEP TESTING rospy.loginfo("Keyframe %d has no valid sample observations", node) rospy.loginfo("Sampling with no meta constraints") attempts, samples, matched_ids = sampler.generate_n_valid_samples( graph.nodes[node]["model"], graph.nodes[node]["primal_observation"], [], n=n_samples) # Order sampled points based on their intra-model log-likelihood ranked_samples = sampler.rank_samples(graph.nodes[node]["model"], samples) # User converter object to convert raw sample vectors into LfD observations graph.nodes[node]["samples"] = [ sample_to_obsv_converter.convert(sample, run_fk=True) for sample in ranked_samples ] """ Clear occluded points (points in collision etc,.) """ for node in graph.get_keyframe_sequence(): samples = graph.nodes[node]["samples"] free_samples, trash = graph_analyzer.evaluate_keyframe_occlusion( samples) if free_samples == []: rospy.loginfo( "Keyframe {} has no free samples and will be culled.".format( node)) graph.cull_node(node) else: graph.nodes[node]["free_samples"] = free_samples """ Cull/remove keyframes/nodes that via change point estimation using log-likelihood """ graph_analyzer.cull_keyframes(threshold=args.threshold) # """ Order sampled points based on their intra-model log-likelihood """ # for node in graph.get_keyframe_sequence(): # graph.rank_waypoint_samples(node) output = [] """ Create a sequence of keyframe way points and execute motion plans to reconstruct skill """ joint_config_array = [] for node in graph.get_keyframe_sequence(): output.append((node, graph.nodes[node]["applied_constraints"])) sample = graph.nodes[node]["free_samples"][0] joints = sample.get_joint_angle() joint_config_array.append(joints) print output # moveit_interface.move_to_joint_targets(joint_config_array) return 0
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-c', '--config', dest='config', required=True, help='the file path of configuration config.json file ' ) required.add_argument( '-d', '--directory', dest='directory', required=True, help='the directory from which to input labeled demonstration .json files' ) parser.add_argument( '-b', '--bandwidth', type=float, default=.025, metavar='BANDWIDTH', help='gaussian kernel density bandwidth' ) parser.add_argument( '-t', '--threshold', type=int, default=, metavar='THRESHOLD', help='Kullbach-Leibler divergence threshold value - optional' ) parser.add_argument( '-n', '--number_of_samples', type=int, default=50, metavar='NUMBEROFSAMPLES', help='the number of samples to validate for each keyframe' ) args = parser.parse_args(rospy.myargv()[1:]) # Import the data importer = DataImporter() labeled_demonstrations = importer.load_json_files(args.directory + "/*.json") # Convert imported data into Demonstrations and Observations demonstrations = [] for datum in labeled_demonstrations["data"]: observations = [] for entry in datum: observations.append(Observation(entry)) demonstrations.append(Demonstration(observations)) if len(demonstrations) == 0: rospy.logwarn("No demonstration data to model!!") return 0 rospy.init_node("graph_traverse") """ Create the Cairo LfD environment """ config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() constraints = ConstraintFactory(configs).generate_constraints() # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=None) """ Create the moveit_interface """ moveit_interface = SawyerMoveitInterface() moveit_interface.set_velocity_scaling(.35) moveit_interface.set_acceleration_scaling(.25) """ Create KeyframeGraph object. """ graph = KeyframeGraph() cluster_generator = ObservationClusterer() """ Generate clusters using labeled observations, build the models, graphs, and atributes for each cluster in the KeyFrameGraph """ clusters = cluster_generator.generate_clusters(demonstrations) for cluster_id in clusters.keys(): graph.add_node(cluster_id) graph.nodes[cluster_id]["observations"] = clusters[cluster_id]["observations"] graph.nodes[cluster_id]["keyframe_type"] = clusters[cluster_id]["keyframe_type"] graph.nodes[cluster_id]["applied_constraints"] = clusters[cluster_id]["applied_constraints"] graph.nodes[cluster_id]["model"] = KDEModel(kernel='gaussian', bandwidth=args.bandwidth) graph.add_path(graph.nodes()) graph.fit_models(get_observation_joint_vector) graph._identify_primal_observations(get_observation_joint_vector) rospy.loginfo(graph.get_keyframe_sequence()) for node in graph.get_keyframe_sequence(): print("KEYFRAME: {}".format(node)) print(graph.nodes[node]["keyframe_type"]) print(graph.nodes[node]["applied_constraints"]) print """ Build a ConstraintAnalyzer and KeyframeGraphAnalyzer """ constraint_analyzer = ConstraintAnalyzer(environment) graph_analyzer = KeyframeGraphAnalyzer(graph, moveit_interface, get_observation_joint_vector) sample_to_obsv_converter = SawyerSampleConverter(moveit_interface) sampler = KeyframeSampler(constraint_analyzer, sample_to_obsv_converter) model_score_ranker = ModelScoreSampleRanker() configraution_ranker = ConfigurationSpaceSampleRanker() """ Generate raw_samples from graph for each keyframe """ prior_sample = None for node in graph.get_keyframe_sequence(): print "Keyframe {}".format(node) # Keep sampling if graph.nodes[node]["keyframe_type"] == "constraint_transition": rospy.loginfo("Sampling from a constraint transition keyframe.") constraints = [environment.get_constraint_by_id(constraint_id) for constraint_id in graph.nodes[node]["applied_constraints"]] attempts, samples, matched_ids = sampler.generate_n_valid_samples(graph.nodes[node]["model"], graph.nodes[node]["primal_observation"], constraints, n=n_samples) if len(samples) == 0: # Some constraints couldn't be sampled successfully, so using best available samples. diff = list(set(graph.nodes[node]["applied_constraints"]).difference(set(matched_ids))) if len(matched_ids) > 0: rospy.logwarn("Constraints {} couldn't be met so attempting to find valid samples with constraints {}.".format(diff, matched_ids)) constraints = [environment.get_constraint_by_id(constraint_id) for constraint_id in graph.nodes[node]["applied_constraints"]] attempts, samples, matched_ids = sampler.generate_n_valid_samples(graph.nodes[node]["model"], graph.nodes[node]["primal_observation"], constraints, n=n_samples) else: rospy.logwarn("Constraints {} couldn't be met so. Cannot meet any constraints.".format(diff)) else: n_samples = args.number_of_samples constraints = [environment.get_constraint_by_id(constraint_id) for constraint_id in graph.nodes[node]["applied_constraints"]] attempts, samples, matched_ids = sampler.generate_n_valid_samples(graph.nodes[node]["model"], graph.nodes[node]["primal_observation"], constraints, n=n_samples) rospy.loginfo("Keyframe %d: %s valid of %s attempts", node, len(samples), attempts) if len(samples) < n_samples: rospy.loginfo("Keyframe %d: only %s of %s waypoints provided", node, len(samples), n_samples) if len(samples) == 0: rospy.loginfo("Keyframe %d has no valid sample observations", node) graph.cull_node(node) else: # Order sampled points based on their intra-model log-likelihood if prior_sample is None: ranked_samples = model_score_ranker.rank(graph.nodes[node]["model"], samples) else: ranked_samples = configraution_ranker.rank(graph.nodes[node]["model"], samples, prior_sample) prior_sample = ranked_samples[0] # User converter object to convert raw sample vectors into LfD observations graph.nodes[node]["samples"] = [sample_to_obsv_converter.convert(sample, run_fk=True) for sample in ranked_samples] """ Clear occluded points (points in collision etc,.) """ for node in graph.get_keyframe_sequence(): samples = graph.nodes[node]["samples"] free_samples, trash = graph_analyzer.evaluate_keyframe_occlusion(samples) if free_samples == []: rospy.loginfo("Keyframe {} has no free samples and will be culled.".format(node)) graph.cull_node(node) else: graph.nodes[node]["free_samples"] = free_samples """ Cull/remove keyframes/nodes that via change point estimation using log-likelihood """ if "automated_culling_threshold" in config['settings']: automated_threshold = config['settings']['automated_culling_threshold'] else: automated_culling = True graph_analyzer.cull_keyframes(threshold=args.threshold, automated=automated_threshold) # """ Order sampled points based on their intra-model log-likelihood """ # for node in graph.get_keyframe_sequence(): # graph.rank_waypoint_samples(node) output = [] """ Create a sequence of keyframe way points and execute motion plans to reconstruct skill """ joint_config_array = [] for node in graph.get_keyframe_sequence(): output.append((node, graph.nodes[node]["applied_constraints"])) sample = graph.nodes[node]["free_samples"][0] joints = sample.get_joint_angle() joint_config_array.append(joints) print output moveit_interface.move_to_joint_targets(joint_config_array) return 0
def main(): """ Demonstration Recorder Record a series of demonstrations. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-c', '--config', dest='config', required=True, help='the file path of the demonstration ') required.add_argument( '-d', '--directory', dest='directory', required=True, help='the directory to save raw demonstration .json files') parser.add_argument('-r', '--record_rate', type=int, default=50, metavar='RECORDRATE', help='rate at which to record (default: 50)') args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("sdk_joint_recorder") print("Getting robot state... ") robot_state = intera_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") robot_state.enable() interaction_pub = InteractionPublisher() interaction_options = InteractionOptions() interaction_options.set_max_impedance([False]) interaction_options.set_rotations_for_constrained_zeroG(True) interaction_frame = Pose() interaction_frame.position.x = 0 interaction_frame.position.y = 0 interaction_frame.position.z = 0 interaction_frame.orientation.x = 0 interaction_frame.orientation.y = 0 interaction_frame.orientation.z = 0 interaction_frame.orientation.w = 1 interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0]) interaction_options.set_K_nullspace([0, 0, 0, 0, 0, 0, 0]) interaction_options.set_interaction_frame(interaction_frame) rospy.loginfo(interaction_options.to_msg()) rospy.on_shutdown(interaction_pub.send_position_mode_cmd) config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() triggers = TriggerFactory(configs).generate_triggers() constraints = ConstraintFactory(configs).generate_constraints() # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=triggers) exp = DataExporter() print("Recording. Press Ctrl-C to stop.") constraint_analyzer = ConstraintAnalyzer(environment) start_configuration = configs["settings"]["start_configuration"] recorder = SawyerRecorder(start_configuration, args.record_rate, interaction_pub, interaction_options) rospy.on_shutdown(recorder.stop) demos = recorder.run(environment, constraint_analyzer, auto_zeroG=True) # Build processors and process demonstrations to generate derivative data e.g. relative position. rk_processor = RelativeKinematicsProcessor(environment.get_item_ids(), environment.get_robot_id()) ic_processor = InContactProcessor(environment.get_item_ids(), environment.get_robot_id(), .06, .5) soi_processor = SphereOfInfluenceProcessor(environment.get_item_ids(), environment.get_robot_id()) rp_processor = RelativePositionProcessor(environment.get_item_ids(), environment.get_robot_id()) wp_processor = WithinPerimeterProcessor(environment.get_item_ids(), environment.get_robot_id()) pipeline = ProcessorPipeline([ rk_processor, ic_processor, soi_processor, rp_processor, wp_processor ]) pipeline.process(demos) # Analyze observations for constraints. If using web triggered constraints, we don't evaluate and # instead the applied constraints are those explicitly set by the user. for demo in demos: if configs['settings'][ 'constraint_trigger_mechanism'] == 'cuff_trigger': # Using the cuff trigger will cause a propagation forward. constraint_analyzer.applied_constraint_evaluator(demo.observations) elif configs['settings'][ 'constraint_trigger_mechanism'] == 'web_trigger': for observation in demo.observations: observation.data[ "applied_constraints"] = observation.get_triggered_constraint_data( ) else: rospy.logerr("No valid constraint trigger mechanism passed.") exp = DataExporter() for idx, demo in enumerate(demos): raw_data = [obs.data for obs in demo.observations] print("'/raw_demonstration{}.json': {} observations".format( idx, len(raw_data))) exp.export_to_json( args.directory + "/raw_demonstration{}.json".format(idx), raw_data)
def main(): """ Demonstration Recorder Record a series of demonstrations. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-c', '--config', dest='config', required=True, help='the file path of the demonstration ') parser.add_argument('-r', '--record_rate', type=int, default=45, metavar='RECORDRATE', help='rate at which to record (default: 45)') args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("live_constraint") print("Getting robot state... ") robot_state = intera_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") robot_state.enable() interaction_pub = InteractionPublisher() interaction_options = InteractionOptions() interaction_options.set_max_impedance([False]) interaction_options.set_rotations_for_constrained_zeroG(True) interaction_frame = Pose() interaction_frame.position.x = 0 interaction_frame.position.y = 0 interaction_frame.position.z = 0 interaction_frame.orientation.x = 0 interaction_frame.orientation.y = 0 interaction_frame.orientation.z = 0 interaction_frame.orientation.w = 1 interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0]) interaction_options.set_K_nullspace([5, 5, 5, 5, 5, 5, 5]) interaction_options.set_interaction_frame(interaction_frame) rospy.loginfo(interaction_options.to_msg()) rospy.on_shutdown(interaction_pub.send_position_mode_cmd) interaction_pub.external_rate_send_command(interaction_options) config_filepath = args.config configs = import_configuration(config_filepath) items = ItemFactory(configs).generate_items() triggers = TriggerFactory(configs).generate_triggers() constraints = ConstraintFactory(configs).generate_constraints() constraint_ids = [constraint.id for constraint in constraints] print("Constraint IDs: {}".format(constraint_ids)) # We only have just the one robot...for now....... environment = Environment(items=items['items'], robot=items['robots'][0], constraints=constraints, triggers=triggers) constraint_analyzer = ConstraintAnalyzer(environment) user_input = "" while environment.robot._navigator.get_button_state( "right_button_back") != 2 or user_input == "q": stdin, stdout, stderr = select.select([sys.stdin], [], [], .0001) for s in stdin: if s == sys.stdin: user_input = sys.stdin.readline().strip() data = { "robot": environment.get_robot_state(), "items": environment.get_item_state(), "triggered_constraints": environment.check_constraint_triggers() } observation = Observation(data) print "Position: " + str(data["robot"]["position"]) print "Orientation: " + str(data["robot"]["orientation"]) print "Config" + str(data["robot"]["joint_angle"]) print(constraint_analyzer.evaluate(constraints, observation)) print(data["triggered_constraints"]) valid_constraints = constraint_analyzer.evaluate( environment.constraints, observation)[1] pub = rospy.Publisher('cairo_lfd/valid_constraints', Int8MultiArray, queue_size=10) msg = Int8MultiArray(data=valid_constraints) pub.publish(msg) rospy.sleep(1) if rospy.is_shutdown(): return 1