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 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(
'-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 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
