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('-p',
'--position',
dest='position',
nargs=3,
type=float,
help='the x, y, z position of the end-effector')
parser.add_argument(
'-o',
'--orientation',
dest='orientation',
nargs=4,
type=float,
help='the x, y, z, w quaternion orientation of the end-effector')
args = parser.parse_args(rospy.myargv()[1:])
print(args.position)
""" Create the moveit_interface """
moveit_interface = SawyerMoveitInterface()
pose = convert_data_to_pose(position=args.position,
orientation=args.orientation)
print(moveit_interface.get_IK_pose(pose))
def main():
joints = [
0.49774609375, 1.0437587890625, -2.2413759765625, 1.0047255859375,
2.8415927734375, 1.2034716796875, -2.499330078125
]
""" Create the moveit_interface """
moveit_interface = SawyerMoveitInterface()
robot_state = moveit_interface.create_robot_state(joints)
print(robot_state)
print(moveit_interface.check_point_validity(robot_state))
print(moveit_interface.get_FK_pose(joints))
def __init__(self, environment, constraints_topic='/lfd/applied_constraints', cost_service_name='custom_cost'):
self.service_name = cost_service_name
self.constraints_topic = constraints_topic
self.environment = environment
self.applied_constraints = []
self.interface = SawyerMoveitInterface()
self.analyzer = ConstraintAnalyzer(environment)
self.converter = SawyerSampleConverter(self.interface)
def handle(self, constraint_type):
# Get filepath to demo data
main_filepath = rospy.get_param("MAIN_FILEPATH")
config_filepath = main_filepath + "/config.json"
pos_directory = main_filepath + "/positive/labeled"
neg_directory = main_filepath + "/negative/labeled"
# Set data directory by constraint value
if constraint_type.constraint == 0:
directory = neg_directory
else:
directory = pos_directory
# Setup LfD parameters
bandwidth = 0.025
number_of_samples = 5
rospy.loginfo("CC-LfD: %s" % str(constraint_type.constraint))
# Import the data
importer = DataImporter()
configs = import_configuration(config_filepath)
labeled_demonstrations = importer.load_json_files(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 False
""" Create the moveit_interface """
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
cclfd = CC_LFD(configs, moveit_interface)
cclfd.build_environment()
cclfd.build_keyframe_graph(demonstrations, bandwidth)
cclfd.sample_keyframes(number_of_samples)
cclfd.perform_skill()
return True
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, 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:])
rospy.init_node("graph_traverse")
# Import the data
importer = DataImporter()
config_filepath = args.config
configs = import_configuration(config_filepath)
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(labeled_observations=observations))
if len(demonstrations) == 0:
rospy.logwarn("No demonstration data to model!!")
return 0
""" Create the moveit_interface """
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
moveit_interface.set_planner(str(configs["settings"]["planner"]))
lfd = LFD(configs, moveit_interface)
lfd.build_environment()
lfd.build_keyframe_graph(demonstrations, args.bandwidth)
if args.threshold is not None:
rospy.loginfo("Using user provided culling threshold of {}".format(args.threshold))
lfd.sample_keyframes(args.number_of_samples, automate_threshold=False, culling_threshold=args.threshold)
else:
lfd.sample_keyframes(args.number_of_samples, automate_threshold=True)
lfd.perform_skill()
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 the demonstration ')
required.add_argument(
'-i',
'--input_directory',
dest='input_directory',
required=False,
help='the directory from which to input prior demonstration .json files'
)
required.add_argument(
'-o',
'--output_directory',
dest='output_directory',
required=False,
help='the directory to save the given subjects .json files data')
required.add_argument('-t',
'--task',
dest='task',
required=False,
help='the name of the task being demonstrated')
required.add_argument('-s',
'--subject',
dest='subject',
required=False,
help='the ID of the subject')
args = parser.parse_args(rospy.myargv()[1:])
############################
# ROS Node Initialization #
############################
print("Initializing node... ")
rospy.init_node("ar4lfd")
print("Getting robot state... ")
robot_state = intera_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
robot_state.enable()
########################
# Import Configuration #
########################
config_filepath = args.config
configs = load_lfd_configuration(config_filepath)
#################################
# Configure the LFD class model #
#################################
model_settings = configs["settings"]["modeling_settings"]
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
moveit_interface.set_planner(str(model_settings["planner"]))
acclfd = ACC_LFD(configs, model_settings, moveit_interface)
acclfd.build_environment()
#####################################
# Raw Demonstration Data Processors #
#####################################
# Build processors and process demonstrations to generate derivative data e.g. relative position.
rk_processor = RelativeKinematicsProcessor(
acclfd.environment.get_item_ids(), acclfd.environment.get_robot_id())
ic_processor = InContactProcessor(acclfd.environment.get_item_ids(),
acclfd.environment.get_robot_id(), .06,
.5)
soi_processor = SphereOfInfluenceProcessor(
acclfd.environment.get_item_ids(), acclfd.environment.get_robot_id())
rp_processor = RelativePositionProcessor(acclfd.environment.get_item_ids(),
acclfd.environment.get_robot_id())
wp_processor = WithinPerimeterProcessor(acclfd.environment.get_item_ids(),
acclfd.environment.get_robot_id())
processing_pipeline = DataProcessingPipeline([
rk_processor, ic_processor, soi_processor, rp_processor, wp_processor
])
###############################################
# Configure the Sawyer Demonstration Recorder #
###############################################
rec_settings = configs["settings"]["recording_settings"]
recorder = SawyerDemonstrationRecorder(rec_settings,
acclfd.environment,
processing_pipeline,
publish_constraint_validity=True)
rospy.on_shutdown(recorder.stop)
##########################################################
# Configure the Sawyer Demonstration Aligner and Labeler #
##########################################################
label_settings = configs["settings"]["labeling_settings"]
# Demonstration vectorizor that converts observations into state vector in desired space for DTW alignment.
demo_vectorizor = partial(vectorize_demonstration,
vectorizors=[get_observation_joint_vector])
alignment = DemonstrationAlignment(demo_vectorizor)
demo_labeler = SawyerDemonstrationLabeler(label_settings, alignment)
#########################
# Import Initial Demos #
#########################
if args.input_directory is not None:
prior_poor_demonstrations = load_json_files(args.input_directory +
"/*.json")
# Convert imported data into Demonstrations and Observations
demonstrations = []
for datum in prior_poor_demonstrations["data"]:
observations = []
for entry in datum:
observations.append(Observation(entry))
demonstrations.append(Demonstration(observations))
if len(demonstrations) == 0:
rospy.logwarn(
"No prior demonstration data to model!! You sure you're using the right experiment script?"
)
return 0
labeled_initial_demos = demo_labeler.label(demonstrations)
acclfd.build_keyframe_graph(labeled_initial_demos,
model_settings.get("bandwidth", .025))
acclfd.generate_autoconstraints(labeled_initial_demos)
acclfd.sample_keyframes(model_settings.get("number_of_samples", 50),
automate_threshold=True)
else:
labeled_initial_demos = []
acclfd.build_keyframe_graph(labeled_initial_demos,
model_settings.get("bandwidth", .025))
#######################################
# Set defaults for command line args #
#######################################
if args.task is None:
output_directory = ""
else:
output_directory = args.output_directory
if args.task is None:
task_name = "****"
else:
task_name = args.task
if args.subject is None:
subject = "####"
else:
subject = args.subject
#########################
# Run Study Controller #
#########################
study = ACCLfDController(acclfd, recorder, demo_labeler,
labeled_initial_demos, output_directory,
task_name, subject)
study.run()
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 the demonstration ')
required.add_argument(
'-o',
'--output_directory',
dest='output_directory',
required=True,
help='the directory to save the given subjects .json files data')
args = parser.parse_args(rospy.myargv()[1:])
############################
# ROS Node Initialization #
############################
print("Initializing node... ")
rospy.init_node("recording_only")
print("Getting robot state... ")
robot_state = intera_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
robot_state.enable()
########################
# Import Configuration #
########################
config_filepath = args.config
configs = import_configuration(config_filepath)
#################################
# Configure the LFD class model #
#################################
model_settings = configs["settings"]["modeling_settings"]
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
moveit_interface.set_planner(str(model_settings["planner"]))
cclfd = CC_LFD(configs, model_settings, moveit_interface)
cclfd.build_environment()
#####################################
# Raw Demonstration Data Processors #
#####################################
# Build processors and process demonstrations to generate derivative data e.g. relative position.
rk_processor = RelativeKinematicsProcessor(
cclfd.environment.get_item_ids(), cclfd.environment.get_robot_id())
ic_processor = InContactProcessor(cclfd.environment.get_item_ids(),
cclfd.environment.get_robot_id(), .06,
.5)
soi_processor = SphereOfInfluenceProcessor(
cclfd.environment.get_item_ids(), cclfd.environment.get_robot_id())
rp_processor = RelativePositionProcessor(cclfd.environment.get_item_ids(),
cclfd.environment.get_robot_id())
wp_processor = WithinPerimeterProcessor(cclfd.environment.get_item_ids(),
cclfd.environment.get_robot_id())
processor_pipeline = ProcessorPipeline([
rk_processor, ic_processor, soi_processor, rp_processor, wp_processor
])
###############################################
# Configure the Sawyer Demonstration Recorder #
###############################################
rec_settings = configs["settings"]["recording_settings"]
recorder = SawyerDemonstrationRecorder(rec_settings,
cclfd.environment,
processor_pipeline,
publish_constraint_validity=True)
rospy.on_shutdown(recorder.stop)
recording_controller = RecordingOnlyController(cclfd, recorder,
args.output_directory)
recording_controller.run()
def _move_to_start_configuration(self):
""" Create the moveit_interface """
if self.start_configuration is not None:
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
moveit_interface.set_joint_target(self.start_configuration)
moveit_interface.execute(moveit_interface.plan())
else:
rospy.logwarn("No start configuration provided in your config.json file.")
self._clear_command()
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 handle(self, constraint_type):
""" Get params from ROS """
get_params = True
if get_params:
config = rospy.get_param("CONFIG") # the file path of configuration config.json file
pos_directory = rospy.get_param("POS_DIRECTORY") # the directory from which to input labeled demonstration .json files
neg_directory = rospy.get_param("NEG_DIRECTORY") # the directory from which to input labeled demonstration .json files
bandwidth = rospy.get_param("BANDWIDTH") # gaussian kernel density bandwidth
threshold = rospy.get_param("THRESHOLD") # log-liklihood threshold value
number_of_samples = rospy.get_param("NUMBER_OF_SAMPLES") # maximum allowed number of samples
else:
config = "/home/jgkawell/sawyer_ws/src/cairo-lfd/lfd_experiments/experimental_data/feedback_demo/config.json"
directory = "/home/jgkawell/sawyer_ws/src/cairo-lfd/lfd_experiments/experimental_data/feedback_demo/negative/labeled"
bandwidth = 0.025
threshold = -1200
number_of_samples = 50
rospy.logwarn(str(constraint_type.constraint))
if constraint_type.constraint == 0:
directory = neg_directory
else:
directory = pos_directory
""" Import and convert demonstrations """
rospy.loginfo("CC-LfD: Import and convert demonstrations")
# Import the data
importer = DataImporter()
labeled_demonstrations = importer.load_json_files(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("CC-LfD: No demonstration data to model!!")
return False
else:
rospy.loginfo("CC-LfD: Found demonstrations!!")
""" Create the Cairo LfD environment """
rospy.loginfo("CC-LfD: Create the Cairo LfD environment")
config_filepath = config
# NOTE: the config file can be used to parameterize any of the constraints we want to add to the learned skill
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)
""" Create the moveit_interface """
rospy.loginfo("CC-LfD: Create the moveit interface")
moveit_interface = SawyerMoveitInterface()
moveit_interface.set_velocity_scaling(.35)
moveit_interface.set_acceleration_scaling(.25)
""" Create KeyframeGraph object. """
rospy.loginfo("CC-LfD: 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
"""
rospy.loginfo("CC-LfD: 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=bandwidth)
graph.add_path(graph.nodes())
graph.fit_models(get_observation_joint_vector)
rospy.loginfo(graph.get_keyframe_sequence())
for node in graph.get_keyframe_sequence():
rospy.loginfo("CC-LfD: Keyframe type " + (graph.nodes[node]["keyframe_type"]))
# NOTE: Graph has been made so we can simply edit applied_constraints field to add our new constraints for sampling
""" Build a ConstraintAnalyzer and KeyframeGraphAnalyzer """
rospy.loginfo("CC-LfD: 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 """
rospy.loginfo("CC-LfD: Generate raw_samples from graph for each keyframe")
for node in graph.get_keyframe_sequence():
# Keep sampling
if graph.nodes[node]["keyframe_type"] == "constraint_transition":
rospy.loginfo("Sampling from a constraint transition keyframe.")
attempts, samples, matched_ids = sampler.generate_n_valid_samples(graph.nodes[node]["model"], graph.nodes[node]["applied_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)))
rospy.logwarn("CC-LfD: Constraints {} couldn't be met so attempting to find valid samples with constraints {}.".format(diff, matched_ids))
attempts, samples, matched_ids = sampler.generate_n_valid_samples(graph.nodes[node]["model"], matched_ids, n=n_samples)
else:
n_samples = number_of_samples
attempts, samples, matched_ids = sampler.generate_n_valid_samples(graph.nodes[node]["model"], graph.nodes[node]["applied_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)
# Order sampled points based on their intramodel log-liklihood
ranked_samples = sampler.rank_samples(graph.nodes[node]["model"], samples)
# User converter object to conver 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,.) """
rospy.loginfo("CC-LfD: 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-liklihood """
rospy.loginfo("CC-LfD: Cull/remove keyframes/nodes via change point estimation using log-liklihood")
graph_analyzer.cull_keyframes(threshold=threshold)
# """ Order sampled points based on their intramodel log-liklihood """
# for node in graph.get_keyframe_sequence():
# graph.rank_waypoint_samples(node)
rospy.loginfo(graph.get_keyframe_sequence())
""" Create a seequence of keyframe waypoints and excute motion plans to reconstruct skill """
rospy.loginfo("CC-LfD: Create a sequence of keyframe waypoints and execute motion plans to reconstruct skill")
joint_config_array = []
for node in graph.get_keyframe_sequence():
sample = graph.nodes[node]["free_samples"][0]
joints = sample.get_joint_angle()
joint_config_array.append(joints)
moveit_interface.move_to_joint_targets(joint_config_array)
return True
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