def histogram_of_stop_point_elbow(self, subjects, labels):
fig1 = plt.figure(4)
fig2 = plt.figure(5)
labels = ['good']
stop_locations = []
arm_lengths = []
for num, label in enumerate(labels):
for subj_num, subject in enumerate(subjects):
subject_stop_locations = []
paramlist = rosparam.load_file(''.join([data_path, '/', subject, '/params.yaml']))
for params, ns in paramlist:
rosparam.upload_params(ns, params)
arm_length = rosparam.get_param('crook_to_fist')/100.
# fig1 = plt.figure(2*num+1)
ax1 = fig1.add_subplot(111)
ax1.set_xlim(0.2, .4)
# ax1.set_ylim(-10.0, 1.0)
ax1.set_xlabel('Stop Position (m)', fontsize=20)
ax1.set_ylabel('Number of trials', fontsize=20)
ax1.set_title(''.join(['Difference between arm length and stop position at the elbow']), fontsize=20)
ax1.tick_params(axis='x', labelsize=20)
ax1.tick_params(axis='y', labelsize=20)
ax2 = fig2.add_subplot(431+subj_num)
ax2.set_xlim(0.2, .4)
# ax1.set_ylim(-10.0, 1.0)
ax2.set_xlabel('Position (m)')
ax2.set_ylabel('Number of trials')
ax2.set_title(''.join(['Stop position for "Good" outcome']), fontsize=20)
vel = 0.1
directory = ''.join([data_path, '/', subject, '/formatted_three/', str(vel),'mps/', label, '/'])
force_file_list = os.listdir(directory)
for file_name in force_file_list:
# print directory+file_name
loaded_data = load_pickle(directory+file_name)
stop_locations.append(np.max(loaded_data[:,1])-arm_length)
subject_stop_locations.append(np.max(loaded_data[:,1])-arm_length)
arm_lengths.append(arm_length)
vel = 0.15
directory = ''.join([data_path, '/', subject, '/formatted_three/', str(vel),'mps/', label, '/'])
force_file_list = os.listdir(directory)
for file_name in force_file_list:
loaded_data = load_pickle(directory+file_name)
stop_locations.append(np.max(loaded_data[:,1]-arm_length))
subject_stop_locations.append(np.max(loaded_data[:,1])-arm_length)
ax2.hist(subject_stop_locations)
mu = np.mean(stop_locations)
sigma = np.std(stop_locations)
print 'The minimum arm length is: ', np.min(arm_lengths)
print 'The max arm length is: ', np.max(arm_lengths)
print 'The mean arm length is: ', np.mean(arm_lengths)
print 'The mean of the stop location is: ', mu
print arm_lengths
print 'The standard deviation of the stop location is: ', sigma
n, bins, patches = ax1.hist(stop_locations, 10, color="green", alpha=0.75)
points = np.arange(0, 10, 0.001)
y = mlab.normpdf(points, mu, sigma)
l = ax1.plot(points, y, 'r--', linewidth=2)
def main():
print("Starting HwBridge node.")
rospy.init_node('HwBridge')
actions_manifest_file = rospy.get_param(
rospy.get_name() + "/actions_manifest", "actions_manifest")
perceptions_manifest_file = rospy.get_param(
rospy.get_name() + "/perceptions_manifest", "perceptions_manifest")
agent_name = rospy.get_param(rospy.get_name() + "/agent_name", "")
args = arg_parser()
if args["param"] is not None:
import yaml
with open(args["param"][0], 'r') as stream:
try:
yaml_file = yaml.safe_load(stream)
rosparam.upload_params("/", yaml_file)
except yaml.YAMLError as exc:
print(exc)
if args["action"] is not None:
actions_manifest_file = args["action"][0]
if args["perception"] is not None:
perceptions_manifest_file = args["perception"][0]
action_controller = ActionController()
action_controller.read_manifest(actions_manifest_file)
jason_actions_status_pub = rospy.Publisher('jason/actions_status',
jason_ros_msgs.msg.ActionStatus,
queue_size=1,
latch=False)
jason_action_sub = rospy.Subscriber(
'jason/actions', jason_ros_msgs.msg.Action, act,
(action_controller, jason_actions_status_pub, agent_name))
perception_controller = PerceptionController(agent_name)
perception_controller.read_manifest(perceptions_manifest_file)
perception_controller.start_perceiving()
jason_percepts_pub = rospy.Publisher(
'jason/percepts',
jason_ros_msgs.msg.Perception,
queue_size=(2 * perception_controller.comm_len),
latch=False)
signal.signal(signal.SIGINT, signal.SIG_DFL)
rate = rospy.Rate(perception_controller.rate)
while not rospy.is_shutdown():
perception_controller.p_lock.acquire()
for p in perception_controller.perceptions.items():
if p[1] is not None:
jason_percepts_pub.publish(p[1])
perception_controller.perceptions[p[0]] = None
perception_controller.p_lock.release()
perception_controller.p_event.wait()
perception_controller.p_event.clear()
rate.sleep()
rospy.spin()
def init():
try:
data = rosparam.load_file("smach.yaml", default_namespace="smach")
print "load yaml file success"
#update to server
for params, ns in data:
rosparam.upload_params(ns, params)
time.sleep(0.5)
except:
print "load yaml file faile"
sys.exit(0)
try:
slam = int(rospy.get_param('~/smach/slam'))
navigation = int(rospy.get_param('~/smach/navigation'))
control = int(rospy.get_param('~/smach/control'))
print "load param success"
time.sleep(0.5)
print "**********************************************"
print "value:"
print("slam { %d }" % (slam)) #0:none, 1:open
print("navigation { %d }" % (navigation)
) #0:none, 1:location1, 2:location2 ,etc
print("control { %d }" % (control)
) #0:none, 1:front, 2:back, 3:left, 2:right
print "**********************************************"
time.sleep(0.1)
except:
print "load param faile"
sys.exit(0)
def __init__(self):
try:
self._map = rospy.get_param("~map")
except KeyError as e:
rospy.logfatal("map param not set!: %s " % e)
raise MapSetupException("map param not set")
# Clean/delete params:
try:
rospy.delete_param("mmap")
except:
pass
# Publish map_in_use (latched):
self._map_in_use_pub = rospy.Publisher("map_in_use",
String,
queue_size=1,
latch=True)
self._map_in_use_pub.publish("submap_0")
# Set params:
rospy.set_param("map_package_in_use", "deprecated")
rospy.set_param("nice_map_in_use", os.path.join(self._map, "map.bmp"))
rospy.set_param("map_transformation_in_use",
os.path.join(self._map, "transformation.xml"))
# Load
paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"),
"mmap")
for param, ns in paramlist:
try:
rosparam.upload_params(ns, param)
except:
pass # ignore empty params
def launch_platform_controls_spawner(self):
rospack = rospkg.RosPack()
srvss_bobcat_pkg_path = rospack.get_path('srvss_bobcat')
srvss_bobcat_controllers_yaml = srvss_bobcat_pkg_path + "/controllers/srvss_bobcat_controllers.yaml"
paramlist = rosparam.load_file(srvss_bobcat_controllers_yaml,
default_namespace='',
verbose=True)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
# if the controllers do not load it possible to increase the time of waiting for the server in the spawner
# it located in /home/userws3/gz_ros_cws/src/ros_control/controller_manager/scripts
node = ROSNode("controller_manager",
"spawner",
name="platform_controllers_spawner",
namespace="/srvss_bobcat",
output="screen",
respawn="false",
args="joint_state_controller \
front_right_wheel_velocity_controller \
front_left_wheel_velocity_controller \
back_right_wheel_velocity_controller \
back_left_wheel_velocity_controller")
self.launcher.launch(node)
time.sleep(10)
node = ROSNode("srvss_bobcat",
"srvss_bobcat_rate2effort_node",
name="srvss_bobcat_RateToEffort_node",
cwd="node",
output="screen")
self.launcher.launch(node)
time.sleep(3)
def load_params_from_yaml(complete_file_path):
from rosparam import upload_params
from yaml import load
f = open(complete_file_path, 'r')
yamlfile = load(f)
f.close()
upload_params('/', yamlfile)
def load_params(self, req):
''' EXAMPLE OF LOADING PARAMETERS FROM A YAML FILE:
paramlist=rosparam.load_file("/path/to/myfile",default_namespace="my_namespace")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
'''
resp = LoadResponse()
if len(req.type) == 0:
resp.status.result = Status.FAILURE
resp.status.error = Status.TYPE_MISSING
resp.status.info = "No type provided!"
else:
path = join(self._root, req.type)
filename = join(path, req.id)
if not os.path.exists(path):
resp.status.result = Status.FAILURE
resp.status.error = Status.NO_SUCH_TYPE
resp.status.info = "Type %s does not exist!"%(req.type)
elif not os.path.exists(filename):
resp.status.result = Status.FAILURE
resp.status.error = Status.FILE_MISSING
resp.status.info = "File %s does not exist as a member of type %s!"%(req.id, req.type)
else:
paramlist=rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns,params)
resp.status.result = Status.SUCCESS
return resp
def carefree_switch(self,
arm,
new_controller,
param_file=None,
possible_controllers=None):
if '%s' in new_controller:
new_controller = new_controller % arm
if param_file is not None:
params = rosparam.load_file(
roslib.substitution_args.resolve_args(param_file))
rosparam.upload_params("", params[0][0])
if possible_controllers is None:
possible_controllers = POSSIBLE_ARM_CONTROLLERS
check_arm_controllers = [
controller % arm for controller in possible_controllers
]
resp = self.list_controllers_srv()
start_controllers, stop_controllers = [new_controller], []
for i, controller in enumerate(resp.controllers):
if controller in check_arm_controllers and resp.state[
i] == 'running':
stop_controllers.append(controller)
self.load_controller(new_controller)
rospy.loginfo("[pr2_controller_switcher] Starting controller %s" %
(start_controllers[0]) + " and stopping controllers: [" +
",".join(stop_controllers) + "]")
resp = self.switch_controller_srv(start_controllers, stop_controllers,
1)
return resp.ok
def __init__(self):
try:
self._map = rospy.get_param("~map")
except KeyError as e:
rospy.logfatal("map param not set!: %s " % e)
raise MapSetupException("map param not set")
# Clean/delete params:
try:
rospy.delete_param("mmap")
except:
pass
# Publish map_in_use (latched):
self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True)
self._map_in_use_pub.publish("submap_0")
# Set params:
rospy.set_param("map_package_in_use" , "deprecated")
rospy.set_param("nice_map_in_use" , os.path.join(self._map, "map.bmp"))
rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml"))
# Load
paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap")
for param, ns in paramlist:
try:
rosparam.upload_params(ns, param)
except:
pass # ignore empty params
def __init__(self):
''' connect to midi device and set up ros
'''
pygame.midi.init()
devices = pygame.midi.get_count()
if devices < 1:
rospy.logerr("No MIDI devices detected")
exit(-1)
rospy.loginfo("Found %d MIDI devices" % devices)
input_dev = int(rospy.get_param("~input_dev",
pygame.midi.get_default_input_id))
rospy.loginfo("Using input device %d" % input_dev)
self.controller = pygame.midi.Input(input_dev)
# load in default parameters if not set
if not rospy.has_param('~modes'):
rospack = rospkg.RosPack()
paramlist = rosparam.load_file(rospack.get_path('korg_nanokontrol') +
'/config/nanokontrol_config.yaml')
for params, ns in paramlist:
rosparam.upload_params(ns, params)
self.modes = rospy.get_param('~modes')
# determine how axis should be interpreted
self.center_axis = rospy.get_param('~center_axis', True)
self.msg = Joy()
self.mode = None
self.pub = rospy.Publisher('joy', Joy, queue_size=10, latch=True)
def load_device_map(self, fname):
try:
params = rosparam.load_file(fname)[0][0]
if 'lights' in params:
lights = params
for k, v in params['lights'].items():
try:
light_id = int(k, 16)
alias = v['alias_id']
except ValueError:
rospy.logerr(
f'Failed to load device map config: device ID should be hexadecimal, but \'{k}\' value was found instead'
)
return False
except KeyError:
rospy.logerr(
f'Failed to load device map config: missing alias_id key'
)
return False
rosparam.upload_params(rospy.get_name(), lights)
else:
rospy.logerr(
f'Failed to load device map config: unable to find \'lights\' parameter'
)
return False
except Exception as e:
rospy.logerr(f'Failed to load device map config: {e}')
return False
return True
def srv_callback(self, request):
label = request.label
response = SaveSpotServiceMessageResponse()
"""
---
bool navigation_successfull
string message # Direction
"""
os.chdir("/home/zulhafiz/catkin_ws/src/kasimx_navigation/spots")
paramlist = rosparam.load_file("spots.yaml", default_namespace=None)
for params, ns in paramlist: #ns,param
for key, value in params.items():
if key == request.label:
rosparam.upload_params(ns, params) #ns,param
response.message = "Correctly uploaded parameters"
send_coordinates = SendCoordinates(request.label)
response.navigation_successfull = True
return response
def load_params_from_yaml(complete_file_path):
from rosparam import upload_params
from yaml import load
f = open(complete_file_path, 'r')
yamlfile = load(f)
f.close()
upload_params('/', yamlfile)
def load_params(self, req):
''' EXAMPLE OF LOADING PARAMETERS FROM A YAML FILE:
paramlist=rosparam.load_file("/path/to/myfile",default_namespace="my_namespace")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
'''
resp = LoadResponse()
if len(req.type) == 0:
resp.status.result = Status.FAILURE
resp.status.error = Status.TYPE_MISSING
resp.status.info = "No type provided!"
else:
path = join(self._root, req.type)
filename = join(path, req.id)
if not os.path.exists(path):
resp.status.result = Status.FAILURE
resp.status.error = Status.NO_SUCH_TYPE
resp.status.info = "Type %s does not exist!" % (req.type)
elif not os.path.exists(filename):
resp.status.result = Status.FAILURE
resp.status.error = Status.FILE_MISSING
resp.status.info = "File %s does not exist as a member of type %s!" % (
req.id, req.type)
else:
paramlist = rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
resp.status.result = Status.SUCCESS
return resp
def loadYaml(self, file, namespace):
'''
Load all the parameters on the parameter server
@return: True if OK, False otherwise
'''
parameters = rosparam.load_file("file")
rosparam.upload_params(namespace, parameters[0][0][namespace])
def load_parameters(self, file_name):
try:
param_file = open(rospkg.RosPack().get_path('borealis_dashboard')+"/config/" + file_name + ".yaml")
yaml_file = yaml.load(param_file)
param_file.close()
rosparam.upload_params(rospy.get_name()+'/', yaml_file)
except Exception as ex:
rospy.logerr(str(ex))
def load_params_from_yaml(full_path):
f = open(full_path, 'r')
yamlfile = load(f)
f.close()
upload_params('/', yamlfile)
for key in yamlfile:
print key,':', yamlfile[key]
return yamlfile
def load_params_from_yaml(full_path):
f = open(full_path, "r")
yamlfile = load(f)
f.close()
upload_params("/", yamlfile)
for key in yamlfile:
print key, ":", yamlfile[key]
return yamlfile
def __init__(self, name, configFilePath):
params = rosparam.load_file(configFilePath)
for param, ns in params:
try:
rosparam.upload_params(ns, param)
except ROSParamException, e:
print >> sys.stderr, "ERROR: " + str(e)
pass # ignore empty params
def rosmodel2owl(configFilePath):
# the file path should be given as argument, alternatively a ns can be added
# params = rosparam.load_file(rospack.get_path('mros1_reasoner')+'/config/nav_config.yaml', '')
# loads individuals to specify a single function and QalityAttributeTypes from the domain_ontology_file
params = rosparam.load_file(configFilePath)
for param, ns in params:
try:
rosparam.upload_params(ns, param)
except:
pass # ignore empty params
ontology_pkg = rospy.get_param('ontology_pkg')
ontology_path = rospy.get_param('ontology_path')
ontology_file = os.path.join(
rospack.get_path(ontology_pkg) + '/' + ontology_path)
domain_ontology_pkg = rospy.get_param('domain_ontology_pkg')
domain_ontology_path = rospy.get_param('domain_ontology_path')
domain_ontology_file = os.path.join(
rospack.get_path(domain_ontology_pkg) + '/' + domain_ontology_path)
function = rospy.get_param('function')
configs = rospy.get_param('configs')
result_file = rospy.get_param('result_file')
tomasys = get_ontology(ontology_file).load()
onto = get_ontology(domain_ontology_file).load()
rospy.loginfo('Loaded domain ontology: ' + domain_ontology_file)
function_ = onto.search_one(iri='*' + function + '*')
if function_ == None:
print('The domain ontology provided does not contain a Function ',
function)
sys.exit(0)
for config in configs:
config_name = config
file_name = config_name + '.rossystem'
file_path = os.path.join(rospack.get_path(config_name), file_name)
parser = RosSystemModelParser(file_path)
model = parser.parse()
sys_name = model.system_name[0]
# create a FunctionDesign
fd = tomasys.FunctionDesign(sys_name,
namespace=onto,
solvesF=function_)
# create a QualityAttribute expected value for the FunctionDesign with the type indicated by the name of the param in the RosSystem and the value of the param (e.g. 0.5)
for qa_param in model.global_parameters:
qa_string = qa_param.param_name[0].replace('qa_', '')
value = qa_param.param_value[0]
qa = tomasys.QAvalue('{0}_{1}'.format(qa_param.param_name[0],
sys_name),
namespace=onto,
isQAtype=onto.search_one(iri='*' + qa_string),
hasValue=value)
fd.hasQAestimation.append(qa)
# save the ontology to a file
onto.save(file=result_file, format='rdfxml')
def file_open(self):
os.chdir("/home/user/catkin_ws/src/navigation_exam/params")
paramlist = rosparam.load_file("task2.yaml", default_namespace=None)
for params, ns in paramlist: #ns,param
for key, value in params.items():
rosparam.upload_params(ns, params)
return self.point(key, value)
def save_params(pos, rot, filename):
rot_homo = np.eye(4)
rot_homo[:3, :3] = rot
rot_quat = tf_trans.quaternion_from_matrix(rot_homo)
optitrak_params = {"position": pos.T.A[0].tolist(), "orientation": rot_quat.tolist()}
print optitrak_params
rosparam.upload_params("optitrak_calibration", optitrak_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, "optitrak_calibration")
def __init__(self):
########### load yaml file ###########
try:
self.data = rosparam.load_file("location.yaml",
default_namespace="artag")
print "load yaml file success"
#update to server
for params, ns in self.data:
rosparam.upload_params(ns, params)
time.sleep(1)
except:
print "load yaml file faile"
sys.exit(0)
########### load param ###########
try:
self.local1_con = int(
rospy.get_param('~/artag/location1/Confidence'))
self.local1_x = float(rospy.get_param('~/artag/location1/x'))
self.local1_y = float(rospy.get_param('~/artag/location1/y'))
self.local2_con = int(
rospy.get_param('~/artag/location2/Confidence'))
self.local2_x = float(rospy.get_param('~/artag/location2/x'))
self.local2_y = float(rospy.get_param('~/artag/location2/y'))
self.local3_con = int(
rospy.get_param('~/artag/location3/Confidence'))
self.local3_x = float(rospy.get_param('~/artag/location3/x'))
self.local3_y = float(rospy.get_param('~/artag/location3/y'))
self.local4_con = int(
rospy.get_param('~/artag/location4/Confidence'))
self.local4_x = float(rospy.get_param('~/artag/location4/x'))
self.local4_y = float(rospy.get_param('~/artag/location4/y'))
print "load param success"
time.sleep(1)
print "**********************************************"
print "value:"
print("local1 { con %d , x %.4f , y %.4f }" %
(self.local1_con, self.local1_x, self.local1_y))
print("local2 { con %d , x %.4f , y %.4f }" %
(self.local2_con, self.local2_x, self.local2_y))
print("local3 { con %d , x %.4f , y %.4f }" %
(self.local3_con, self.local3_x, self.local3_y))
print("local4 { con %d , x %.4f , y %.4f }" %
(self.local4_con, self.local4_x, self.local4_y))
print "**********************************************"
time.sleep(1)
except:
print "load param faile"
sys.exit(0)
self.listener = tf.TransformListener()
self.listener2 = tf.TransformListener()
self.listener3 = tf.TransformListener()
self.listener4 = tf.TransformListener()
def _get_map_params(self):
paramlist=rosparam.load_file(self.map_path + ".yaml",default_namespace="map_params")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
origin = rospy.get_param('/map_params/origin')
resolution = rospy.get_param('/map_params/resolution')
return (origin, resolution)
def main():
rospy.init_node("netft_gravity_zeroing")
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--file', dest="filename", default="",
help="YAML file to save parameters in.")
p.add_option('-l', '--load', dest="is_load",
action="store_true", default=False,
help="Load parameters from file.")
p.add_option('-r', '--run', dest="is_run",
action="store_true", default=False,
help="Publish a zeroed wrench.")
p.add_option('-t', '--train', dest="is_train",
action="store_true", default=False,
help="Train by moving the gripper to different poses and measuring the wrenches.")
p.add_option('-p', '--pr2', dest="is_pr2",
action="store_true", default=False,
help="Will run automated data collection if this is the PR2.")
p.add_option('-z', '--start_zero', dest="start_zero",
action="store_true", default=False,
help="Use the first value in to zero the sensor.")
p.add_option('-n', '--ntrials', dest="n_trials",
default="6,6,4",
help="Number of trials for each of the last 3 joint angles to move through. (default: 6,6,4)")
(opts, args) = p.parse_args()
try:
n_4, n_5, n_6 = [int(n_str) for n_str in opts.n_trials.split(',')]
except:
print "Bad --ntrials parameter (format: --ntrials 6,6,4)"
p.print_help()
return
if opts.is_load:
params = rosparam.load_file(opts.filename, rospy.get_name())[0][0]
rosparam.upload_params(rospy.get_name(), params)
if opts.is_run:
rospy.sleep(0.1)
nft_z = NetFTZeroer(start_zero=opts.start_zero, is_pr2=opts.is_pr2)
rospy.spin()
return
if opts.is_train:
if opts.is_pr2:
data = collect_data_pr2(n_4, n_5, n_6)
param_vector = process_data(data, True)
save_params(param_vector, opts.filename)
return
else:
data = collect_data_tool()
param_vector = process_data(data, False)
save_params(param_vector, opts.filename)
return
def main():
rospy.init_node("optitrak_odom_remap")
from optparse import OptionParser
p = OptionParser()
p.add_option('-f',
'--file',
dest="filename",
default="",
help="YAML file to save parameters in.")
p.add_option('-l',
'--load',
dest="is_load",
action="store_true",
default=False,
help="Load parameters from file.")
p.add_option(
'-t',
'--train',
dest="is_train",
action="store_true",
default=False,
help=
"Train by moving the head to different poses and capturing the relative positions."
)
p.add_option(
'-p',
'--publish',
dest="is_pub",
action="store_true",
default=False,
help="Publish the transformation from optitrak to base_footprint.")
(opts, args) = p.parse_args()
if opts.filename == "":
print "Bad command line parameters"
p.print_help()
return
if opts.is_load:
print "hi"
params = rosparam.load_file(opts.filename,
"optitrak_calibration")[0][0]
rosparam.upload_params("optitrak_calibration", params)
rospy.sleep(0.1)
if opts.is_pub:
publish_transform()
return
if opts.is_train:
opti_mat, robot_mat = capture_data()
opti_mat = np.mat(opti_mat).T
robot_mat = np.mat(robot_mat).T
pos, rot = umeyama_method(opti_mat, robot_mat)
print np.linalg.norm(robot_mat - (rot * opti_mat + pos))
save_params(pos, rot, opts.filename)
return
def upload_output_params(upload_str, output_path=None, upload=True, ns_=None):
if upload:
paramlist = rosparam.load_str(upload_str,
"generated",
default_namespace=ns_)
for params, namespace in paramlist:
rosparam.upload_params(namespace, params)
if output_path is not None:
file_writer = open(output_path, "wb")
file_writer.write(upload_str)
file_writer.close()
def LoadYamlFileParamsTest(rospackage_name, rel_path_from_package_to_file, yaml_file_name):
rospack = rospkg.RosPack()
pkg_path = rospack.get_path(rospackage_name)
config_dir = os.path.join(pkg_path, rel_path_from_package_to_file)
path_config_file = os.path.join(config_dir, yaml_file_name)
paramlist=rosparam.load_file(path_config_file)
for params, ns in paramlist:
rosparam.upload_params(ns,params)
def write_and_start(filename, buffer_string, task):
filename += task + ".yaml"
print "filename: %s" % filename
f = open(filename, "w+")
f.write(buffer_string)
f.close()
pub.publish(Empty())
paramlist = rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
def save_params(pos, rot, filename):
rot_homo = np.eye(4)
rot_homo[:3, :3] = rot
rot_quat = tf_trans.quaternion_from_matrix(rot_homo)
optitrak_params = {
"position": pos.T.A[0].tolist(),
"orientation": rot_quat.tolist()
}
print optitrak_params
rosparam.upload_params("optitrak_calibration", optitrak_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, "optitrak_calibration")
def __init__(self):
f = rospy.get_param("pololu_controller/sub_yaml/")
paramlist = rosparam.load_file(f,default_namespace="motors")
for params, ns in paramlist:
rosparam.upload_params(ns,params)
port = rospy.get_param("pololu_controller/port_name/")
baud = rospy.get_param("pololu_controller/baud_rate/")
self.port = serial.Serial(port, baud, timeout=0.5)
self.port.flush()
self.kill_all()
self.sub = rospy.Subscriber('motor_controllers/pololu_control/command', Float64MultiArray, self.command_callback, queue_size=40)
def spawn_inserter(launch, name, xyz):
# Spawn a namespaced inserter bot at the xyz position
base_path = 'src/inserter_detector/'
# Resolve the xacro file
xml = subprocess.check_output([
'xacro', '--inorder',
base_path + 'simulation/inserter/inserter2.xacro',
'robot_name:={name}'.format(name=name)
])
# Load the URDF into the ROS Parameter Server - this must be done for each robot
# in order for the gazebo_ros_control plugin to have a different namespace for each robot.
rospy.set_param('/' + name + '/robot_description', xml)
# Spawn the model
model_args = '-urdf -model {name} -param /{name}/robot_description -x {xyz[0]:.2f} -y {xyz[1]:.2f} -z {xyz[2]:.2f}'.format(
name=name, xyz=xyz)
model_node = roslaunch.core.Node('gazebo_ros',
'spawn_model',
namespace=name,
respawn=False,
output='screen',
args=model_args)
launch.launch(model_node)
# Load the joints yaml file
paramlist = rosparam.load_file(base_path + 'config/joints.yaml',
default_namespace=name,
verbose=True)
for params, ns in paramlist:
rosparam.upload_params(ns, params, verbose=True)
# Start the joint state controller within the namespace
joint_node = roslaunch.core.Node(
'controller_manager',
'spawner',
namespace=name,
args='joint_state_controller arm_controller')
launch.launch(joint_node)
# Publish the static transform from the world to the robot
tf_args = '{xyz[0]:.2f} {xyz[1]:.2f} {xyz[2]:.2f} 0 0 0 world /{name}/base_link 100'.format(
name=name, xyz=xyz)
tf_node = roslaunch.core.Node('tf',
'static_transform_publisher',
namespace=name,
respawn=False,
output='screen',
args=tf_args)
launch.launch(tf_node)
def save_params(p, filename):
ft_params = { "mass" : p[0],
"force_zero_x" : p[1],
"force_zero_y" : p[2],
"force_zero_z" : p[3],
"com_pos_x" : p[4],
"com_pos_y" : p[5],
"com_pos_z" : p[6],
"torque_zero_x" : p[7],
"torque_zero_y" : p[8],
"torque_zero_z" : p[9] }
rosparam.upload_params(rospy.get_name(), ft_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, rospy.get_name())
def __init__(self):
f = rospy.get_param("/controller/arm_yaml/")
paramlist = rosparam.load_file(f, default_namespace="motors")
for params, ns in paramlist:
rosparam.upload_params(ns, params)
port = rospy.get_param("/controller/port_name/")
baud = rospy.get_param("/controller/baud_rate/")
self.port = serial.Serial(port, baud, timeout=0.5)
self.port.flush()
self.sub = rospy.Subscriber('pololu/command',
MotorCommand,
self.command_callback,
queue_size=40)
def save_params(p, filename):
ft_params = {
"mass": p[0],
"force_zero_x": p[1],
"force_zero_y": p[2],
"force_zero_z": p[3],
"com_pos_x": p[4],
"com_pos_y": p[5],
"com_pos_z": p[6],
"torque_zero_x": p[7],
"torque_zero_y": p[8],
"torque_zero_z": p[9]
}
rosparam.upload_params(rospy.get_name(), ft_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, rospy.get_name())
def __init__(self, *args):
super(TestAddAnalyzer, self).__init__(*args)
rospy.init_node('test_add_analyzer')
self.namespace = rospy.get_name()
paramlist = rosparam.load_file(rospy.myargv()[1])
# expect to receive these paths in the added analyzers
self.expected = [paramlist[0][1] + analyzer['path'] for name, analyzer in paramlist[0][0]['analyzers'].iteritems()]
self._mutex = threading.Lock()
self.agg_msgs = {}
# put parameters in the node namespace so they can be read by the aggregator
for params, ns in paramlist:
rosparam.upload_params(rospy.get_name() + '/' + ns, params)
rospy.Subscriber('/diagnostics_agg', DiagnosticArray, self.agg_cb)
self.pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
def main():
rospy.init_node("optitrak_odom_remap")
from optparse import OptionParser
p = OptionParser()
p.add_option("-f", "--file", dest="filename", default="", help="YAML file to save parameters in.")
p.add_option("-l", "--load", dest="is_load", action="store_true", default=False, help="Load parameters from file.")
p.add_option(
"-t",
"--train",
dest="is_train",
action="store_true",
default=False,
help="Train by moving the head to different poses and capturing the relative positions.",
)
p.add_option(
"-p",
"--publish",
dest="is_pub",
action="store_true",
default=False,
help="Publish the transformation from optitrak to base_footprint.",
)
(opts, args) = p.parse_args()
if opts.filename == "":
print "Bad command line parameters"
p.print_help()
return
if opts.is_load:
print "hi"
params = rosparam.load_file(opts.filename, "optitrak_calibration")[0][0]
rosparam.upload_params("optitrak_calibration", params)
rospy.sleep(0.1)
if opts.is_pub:
publish_transform()
return
if opts.is_train:
opti_mat, robot_mat = capture_data()
opti_mat = np.mat(opti_mat).T
robot_mat = np.mat(robot_mat).T
pos, rot = umeyama_method(opti_mat, robot_mat)
print np.linalg.norm(robot_mat - (rot * opti_mat + pos))
save_params(pos, rot, opts.filename)
return
def write_and_start(filename, buffer_string):
f = open(filename, "w+")
f.write(buffer_string)
f.close()
paramlist = rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
if ON:
# send start command to smach!
start = rospy.ServiceProxy("/start_SMACH", Empty)
try:
rospy.wait_for_service('/start_SMACH')
start()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def __init__(self, *args):
super(TestAddAnalyzer, self).__init__(*args)
rospy.init_node('test_add_analyzer')
self.namespace = rospy.get_name()
paramlist = rosparam.load_file(rospy.myargv()[1])
# expect to receive these paths in the added analyzers
self.expected = [paramlist[0][1] + analyzer['path'] for name, analyzer in paramlist[0][0]['analyzers'].iteritems()]
self._mutex = threading.Lock()
self.agg_msgs = {}
# put parameters in the node namespace so they can be read by the aggregator
for params, ns in paramlist:
rosparam.upload_params(rospy.get_name() + '/' + ns, params)
rospy.Subscriber('/diagnostics_agg', DiagnosticArray, self.agg_cb)
self.pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
def write_and_start(filename, buffer_string):
f = open(filename, "w+")
f.write(buffer_string)
f.close()
paramlist = rosparam.load_file(filename)
for params, ns in paramlist:
rosparam.upload_params(ns, params)
if ON:
# send start command to smach!
start = rospy.ServiceProxy("/start_SMACH", Empty)
try:
rospy.wait_for_service('/start_SMACH')
start()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def switch(self, old_controller, new_controller, param_file=None):
if param_file is None:
self.load_controller(new_controller)
resp = self.switch_controller_srv([new_controller], [old_controller], 1)
self.unload_controller(old_controller)
return resp.ok
else:
params = rosparam.load_file(roslib.substitution_args.resolve_args(param_file))
rosparam.upload_params("", params[0][0])
self.switch_controller_srv([], [old_controller], 1)
self.unload_controller(old_controller)
if old_controller != new_controller:
self.unload_controller(new_controller)
self.load_controller(old_controller)
if old_controller != new_controller:
self.load_controller(new_controller)
resp = self.switch_controller_srv([new_controller], [], 1)
return resp.ok
def __loadparam_main(self, casedir, vehicleid_param):
os.environ["ROS_MASTER_URI"] = "http://127.0.0.1:" + self.ros_port
os.environ["ROS_IP"] = "127.0.0.1"
self.__clean_paramserver()
params = self.__init_nodeyaml(casedir)
try:
if "vehicle_id" in params["aw_simulation_vehicle"]:
vehicle_id = params["aw_simulation_vehicle"]["vehicle_id"]
print("vhicle id: %s " % vehicle_id)
else:
vehicle_id = params["vehicle_pose_collector"]["vehicle_id"]
###制定了仿真的车辆
if vehicleid_param is not None:
vehicle_id = vehicleid_param
if "park_id" in params["aw_simulation_vehicle"]:
park_id = params["aw_simulation_vehicle"]["park_id"]
print("park id: %s " % park_id)
else:
park_id = params["aw"]["planning"]["aw_planning"]["itinerary"][
"park_id"]
aw_config = AW_Config()
setup_filename = aw_config.GenSetup(vehicle_id, park_id, None)
parse_shell(
os.path.join(os.environ["HOME"], ".autowise", "simu",
setup_filename))
except KeyError as err:
rospy.logerr(
err.message +
" Something missed in the planning.yaml; Should create the test case again."
)
sys.exit(-1)
os.environ["ROS_MASTER_URI"] = "http://127.0.0.1:" + self.ros_port
os.environ["ROS_IP"] = "127.0.0.1"
node_yaml_file = os.path.join(casedir, self.param_file)
with open(node_yaml_file) as f:
node = load(f)
upload_params("/", node)
with open(casedir + '/goal.yaml', 'r') as f:
yamlfile = load(f)
upload_params('/', yamlfile)
#Pload = subprocess.Popen("rosparam load %s" % node_yaml_file, shell=True)
return True
def carefree_switch(self, arm, new_controller, param_file=None, possible_controllers=None):
if '%s' in new_controller:
new_controller = new_controller % arm
if param_file is not None:
params = rosparam.load_file(roslib.substitution_args.resolve_args(param_file))
rosparam.upload_params("", params[0][0])
if possible_controllers is None:
possible_controllers = POSSIBLE_ARM_CONTROLLERS
check_arm_controllers = [controller % arm for controller in possible_controllers]
resp = self.list_controllers_srv()
start_controllers, stop_controllers = [new_controller], []
for i, controller in enumerate(resp.controllers):
if controller in check_arm_controllers and resp.state[i] == 'running':
stop_controllers.append(controller)
self.load_controller(new_controller)
rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) +
" and stopping controllers: [" + ",".join(stop_controllers) + "]")
resp = self.switch_controller_srv(start_controllers, stop_controllers, 1)
return resp.ok
def carefree_switch(self, arm, new_controller, param_file=None, reset=True):
if '%s' in new_controller:
new_ctrl = new_controller % arm
else:
new_ctrl = new_controller
if param_file is not None:
params = rosparam.load_file(roslaunch.substitution_args.resolve_args(param_file))
if new_ctrl not in params[0][0]:
rospy.logwarn("[pr2_controller_switcher] Controller not in parameter file.")
return
else:
rosparam.upload_params("", {new_ctrl : params[0][0][new_ctrl]})
possible_controllers = rosparam.get_param(LOADED_CTRLS_PARAMS[arm])
if new_ctrl not in possible_controllers:
possible_controllers.append(new_ctrl)
rosparam.set_param_raw(LOADED_CTRLS_PARAMS[arm], possible_controllers)
check_arm_controllers = []
for controller in possible_controllers:
if '%s' in controller:
controller = controller % arm
if controller[0] == arm:
check_arm_controllers.append(controller)
resp = self.list_controllers_srv()
start_controllers, stop_controllers = [new_ctrl], []
for i, controller in enumerate(resp.controllers):
if controller in check_arm_controllers and resp.state[i] == 'running':
stop_controllers.append(controller)
if controller == new_ctrl:
if resp.state[i] == 'running':
if not reset:
rospy.loginfo("[pr2_controller_switcher] Specified controller is already running.")
return True
self.switch_controller_srv([], [new_ctrl], 1)
self.unload_controller(new_ctrl)
self.load_controller(new_ctrl)
rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) +
" and stopping controllers: [" + ",".join(stop_controllers) + "]")
resp = self.switch_controller_srv(start_controllers, stop_controllers, 1)
return resp.ok
import os
import shutil
import subprocess
from geometry_msgs.msg._PoseWithCovarianceStamped import PoseWithCovarianceStamped
# Loading the parameters depending on which robot we are using.
# Reduce the size of image file
robot = os.environ.get('PAL_ROBOT')
if robot == 'rh2c' or robot == 'rh2m':
IMAGE_PATH = roslib.packages.get_pkg_dir('reemh2_maps') + '/config/'
RH2_PATH = roslib.packages.get_pkg_dir('reemh2_maps')
paramlist = rosparam.load_file(RH2_PATH+"/config/map.yaml", default_namespace="emergency_situation")
for params, ns in paramlist:
rosparam.upload_params(ns, params)
elif robot == 'reemh3c' or robot == 'reemh3m':
IMAGE_PATH = roslib.packages.get_pkg_dir('reem_maps') + '/config/'
REEMH3_PATH = roslib.packages.get_pkg_dir('reem_maps')
paramlist = rosparam.load_file(REEMH3_PATH+'/config/map.yaml', default_namespace="emergency_situation")
for params, ns in paramlist:
rosparam.upload_params(ns, params)
else:
#IMAGE_PATH = roslib.packages.get_pkg_dir('robocup_iworlds') + '/navigation/'
IMAGE_PATH = roslib.packages.get_pkg_dir('emergency_situation') + '/config/'
#ROBOCUP_PATH = roslib.packages.get_pkg_dir('robocup_worlds')
#paramlist = rosparam.load_file(ROBOCUP_PATH+"/navigation/subMap1.yaml", default_namespace="emergency_situation")
paramlist = rosparam.load_file(IMAGE_PATH+"/subMap1.yaml",default_namespace="emergency_situation")
for params, ns in paramlist:
def format_data_three_categories(self, subject, result):
# print subject
# print result
print 'Now editing files to insert position data and storing them in labeled folders.'
paramlist = rosparam.load_file(''.join([self.data_path, '/', subject, '/params.yaml']))
for params, ns in paramlist:
rosparam.upload_params(ns, params)
arm_length = rosparam.get_param('crook_to_fist')
fist_length = arm_length - rosparam.get_param('crook_to_wrist')
print 'Fist length: ', fist_length, ' cm'
position_of_initial_contact = (44.0 - arm_length)/100.
position_profile = None
for ind_i in xrange(len(result)):
for ind_j in xrange(len(result[0])):
if ind_j == 2:
continue
else:
if result[ind_i][ind_j] is not None:
if ind_i < len(result)/2:
load_num = ind_i
vel = 0.1
else:
load_num = ind_i - len(result)/2
vel = 0.15
if vel == 0.1:
# print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
# print 'Position profile loaded!'
elif vel == 0.15:
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl']))
# print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])
# print 'Position profile loaded!'
else:
print 'There is no saved position profile for this velocity! Something has gone wrong!'
return None
ft_threshold_was_exceeded = False
# current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(load_num), '.log']))])
# while os.path.isfile(''.join([self.pkg_path, '/data/', subject, '/',str(vel),'mps/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])):
ft_threshold_was_exceeded = False
# print ''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])
current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height', str(ind_j), '/ft_sleeve_', str(load_num), '.log']))])
del_index = []
for k in xrange(len(current_data)):
if current_data[k][0] < 5.0:
del_index.append(k)
current_data = np.delete(current_data, del_index, 0)
position_of_stop = 0.
del_index = []
time_of_initial_contact = None
# current_data = load_pickle(''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl']))
# if np.max(current_data[:, 2]) >= 10. or np.max(current_data[:, 3]) >= 10. \
# or np.max(current_data[:, 4]) >= 10.:
# ft_threshold_was_exceeded = True
position_of_stop = 0.
del_index = []
for num, j in enumerate(current_data):
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
j[0] = j[0] - 2.0
# if j[0] < 0.5:
# j[1] = 0
if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
elif ft_threshold_was_exceeded:
del_index.append(num)
# j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
if abs(new_position - position_of_stop) < 0.0005 and new_position > 0.8:
del_index.append(num)
elif new_position < position_of_initial_contact:
del_index.append(num)
else:
if time_of_initial_contact is None:
time_of_initial_contact = j[0]
position_of_stop = new_position
# if result[ind_i][ind_j] == 'good'
current_data = np.delete(current_data, del_index, 0)
output_data = []
for item in current_data:
output_data.append([item[0]-time_of_initial_contact, item[1]-position_of_initial_contact, item[2], item[3], item[4]])
output_data = np.array(output_data)
save_number = 0
if result[ind_i][ind_j] == 'caught_fist' or result[ind_i][ind_j] == 'caught_other':
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', 'caught', '/force_profile_', str(save_number), '.pkl'])
else:
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', result[ind_i][ind_j], '/force_profile_', str(save_number), '.pkl'])
while os.path.isfile(save_file):
save_number += 1
if result[ind_i][ind_j] == 'caught_fist' or result[ind_i][ind_j] == 'caught_other':
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', 'caught', '/force_profile_', str(save_number), '.pkl'])
else:
save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', result[ind_i][ind_j], '/force_profile_', str(save_number), '.pkl'])
print 'Saving with file name', save_file
save_pickle(output_data, save_file)
print 'Done editing files!'
def autolabel_and_set_position(self, subject, result):
print 'Now inserting position data and labeling results automatically according to where they stopped.'
paramlist = rosparam.load_file(''.join([self.data_path, '/', subject, '/params.yaml']))
for params, ns in paramlist:
rosparam.upload_params(ns, params)
arm_length = rosparam.get_param('crook_to_fist')
fist_length = arm_length - rosparam.get_param('crook_to_wrist')
print 'Fist length: ', fist_length, ' cm'
position_of_initial_contact = (44.0 - arm_length)/100.
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
vel = 0.1
ft_threshold_was_exceeded = False
reference_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height3/ft_sleeve_0.log']))])
del_index = []
for k in xrange(len(reference_data)):
if reference_data[k][0] < 5.0:
del_index.append(k)
reference_data = np.delete(reference_data, del_index, 0)
del_index = []
for num, j in enumerate(reference_data):
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
j[0] = j[0] - 2.0
# if j[0] < 0.5:
# j[1] = 0
if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
success_position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
elif ft_threshold_was_exceeded:
del_index.append(num)
# j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
print 'The stop position when the sleeve reaches the elbow is: ', success_position_of_stop
position_profile = None
for ind_i in xrange(len(result)):
for ind_j in xrange(len(result[0])):
if result[ind_i][ind_j] is not None:
if ind_i < len(result)/2:
load_num = ind_i
vel = 0.1
else:
load_num = ind_i - len(result)/2
vel = 0.15
if vel == 0.1:
# print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']))
# print 'Position profile loaded!'
elif vel == 0.15:
position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl']))
# print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])
# print 'Position profile loaded!'
else:
print 'There is no saved position profile for this velocity! Something has gone wrong!'
return None
ft_threshold_was_exceeded = False
current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height', str(ind_j), '/ft_sleeve_', str(load_num), '.log']))])
del_index = []
for k in xrange(len(current_data)):
if current_data[k][0] < 5.0:
del_index.append(k)
current_data = np.delete(current_data, del_index, 0)
position_of_stop = 0.
del_index = []
time_of_initial_contact = None
for num, j in enumerate(current_data):
j[2] = -j[2]
j[3] = -j[3]
j[4] = -j[4]
j[0] = j[0] - 2.0
# if j[0] < 0.5:
# j[1] = 0
if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded:
time_of_stop = j[0]
ft_threshold_was_exceeded = True
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
position_of_stop = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
elif ft_threshold_was_exceeded:
del_index.append(num)
# j[1] = position_of_stop
else:
for k in xrange(len(position_profile)-1):
if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]:
new_position = position_profile[k, 1] + \
(position_profile[k+1, 1] - position_profile[k, 1]) * \
(j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0])
j[1] = new_position
if abs(new_position - position_of_stop) < 0.0005 and new_position > 0.8:
del_index.append(num)
elif new_position < position_of_initial_contact:
del_index.append(num)
else:
if time_of_initial_contact is None:
time_of_initial_contact = j[0]
position_of_stop = new_position
# if num > 8 and 0.05 < new_position < 0.4 and position_of_initial_contact is None:
# prev_f_x_average = np.mean(current_data[num-7:num,2])
# curr_f_x_average = np.mean(current_data[num-6:num+1,2])
# prev_f_z_average = np.mean(current_data[num-7:num,4])
# curr_f_z_average = np.mean(current_data[num-6:num+1,4])
# if curr_f_x_average < -0.08 and curr_f_x_average < prev_f_x_average and curr_f_z_average < prev_f_z_average and curr_f_z_average < -0.03:
# position_of_initial_contact = copy.copy(new_position)
# if position_of_initial_contact is None:
# print 'Position of initial contact was not detected for ', subject, ' and trial ', ind_j, 'at height ', ind_i
# position_of_initial_contact = 0.
# # return
# else:
# print 'Position of initial contact is: ', position_of_initial_contact
# current_data = np.delete(current_data, del_index, 0)
# del_index = []
# for i in xrange(len(current_data)):
# if current_data[i, 1] < position_of_initial_contact:
# del_index.append(i)
current_data = np.delete(current_data, del_index, 0)
output_data = []
for item in current_data:
output_data.append([item[0]-time_of_initial_contact, item[1]-position_of_initial_contact, item[2], item[3], item[4]])
output_data = np.array(output_data)
print 'This trial failed at: ', position_of_stop
this_label = None
if not ft_threshold_was_exceeded:
this_label = 'missed'
elif abs(position_of_stop - success_position_of_stop) < 0.015 or position_of_stop >= success_position_of_stop:
this_label = 'good'
else:
if abs(position_of_stop - success_position_of_stop) + fist_length/100. + 0.05 >= arm_length/100.:
this_label = 'caught_fist'
else:
this_label = 'caught_other'
save_number = 0
save_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', str(vel), 'mps/', this_label, '/force_profile_', str(save_number), '.pkl'])
while os.path.isfile(save_file):
save_number += 1
save_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', str(vel), 'mps/', this_label, '/force_profile_', str(save_number), '.pkl'])
print 'Saving with file name', save_file
save_pickle(output_data, save_file)
print 'Done editing files!'