def _init_example_info_queue(self):
"""
Read index file and put example info into SAMPLE_INFO_QUEUE
:return:
"""
log.info('Start filling {:s} dataset sample information queue...'.format(self._dataset_flag))
t_start = time.time()
for annotation_info in tqdm.tqdm(self._annotation_infos):
image_path = annotation_info[0]
lexicon_index = annotation_info[1]
try:
lexicon_label = [self._lexicon_infos[lexicon_index]]
encoded_label, _ = self.encode_labels(lexicon_label)
_SAMPLE_INFO_QUEUE.put((image_path, encoded_label[0]))
except IndexError:
log.error('Lexicon doesn\'t contain lexicon index {:d}'.format(lexicon_index))
continue
for i in range(self._writer_process_nums):
_SAMPLE_INFO_QUEUE.put(_SENTINEL)
log.debug('Complete filling dataset sample information queue[current size: {:d}], cost time: {:.5f}s'.format(
_SAMPLE_INFO_QUEUE.qsize(),
time.time() - t_start
))
def removeDeadSuccessors(self):
# Prune my successor's list for "alive" only
# Track changes to successor for debugging
origSuccessor = self.getSuccessor() # Solely for debugging when successors change
subFromI = 0
for i in range(len(self.theList)):
newIndex = i - subFromI
if newIndex < 0 or newIndex >= len(self.theList):
continue
currentSuccLocation = self.theList[newIndex]
# We are always alive
if currentSuccLocation.id == self.nodeLocation.id:
continue
alive = yield Utils.isAlive(currentSuccLocation)
if not alive:
# Successor wasn't alive, remove from my list of successors
glog.debug("Removing dead successor location: ID %s" % currentSuccLocation.id, system=self.nodeLocation)
subFromI = self.removeLocationFromList(currentSuccLocation)
self.checkFinalSuccessor(origSuccessor, "removeDeadSuccessors") #DEBUG
return
def move_output_files(self, pipeline_type, dataset):
""" Moves all output files for a particular pipeline and dataset
from their temporary logging location during runtime to the evaluation location.
Args:
pipeline_type: a pipeline representing a set of parameters to use, as
defined in the experiments yaml file for the dataset in question.
dataset: a dataset to run as defined in the experiments yaml file.
"""
dataset_name = dataset["name"]
dataset_results_dir = os.path.join(self.results_dir, dataset_name)
dataset_pipeline_result_dir = os.path.join(dataset_results_dir,
pipeline_type)
log.debug(
"\033[1mMoving output dir:\033[0m \n %s \n \033[1m to destination:\033[0m \n %s"
% (self.pipeline_output_dir, dataset_pipeline_result_dir))
try:
evt.move_output_from_to(self.pipeline_output_dir,
dataset_pipeline_result_dir)
except:
log.fatal(
"\033[1mFailed copying output dir: \033[0m\n %s \n \033[1m to destination: %s \033[0m\n"
% (self.pipeline_output_dir, dataset_pipeline_result_dir))
def __init__(self,
video_cap,
video_fps,
sample_rate=100.0,
start_tstamp=0.0,
end_tstamp=sys.maxsize):
"""Frames iterator constructor.
Args:
video_cap (cv2.VideoCapture): video capture obj
fps (float): The video fps.
sample_rate (float): rate to sample video
start_tstamp (float): start time for iterating
end_tstamp (float): end time condition for iterating
"""
self.cap = video_cap
self.period = max(1.0 / sample_rate, 1.0 / video_fps)
log.debug("Period: {}".format(self.period))
self.start_tstamp = start_tstamp
self.end_tstamp = end_tstamp
self.cur_tstamp = self.start_tstamp
self.first_frame = True
self._move_cursor_to_tstamp(self.start_tstamp)
log.debug(
"Setting start_tstamp of iterator to {}".format(start_tstamp))
def __init__(self, index, cursor, typ):
super().__init__()
self.index = index
self.kind = None
self.cursor = cursor
self.internal_typ = typ
self.desc = OpaType.get_key(self.cursor, self.internal_typ)
self.name = self.desc
if self.name is not None:
glog.debug(self.name)
assert isinstance(self.name, str)
if self.internal_typ:
self.kind = self.internal_typ.kind
self.deps = []
self.children = []
self.forward_decl = False
self.decl = False
self.node = None
self.parent = None
self.mark = 0
self.templates = []
self.template_base = None
self.template_ref = None
self.args = [] # for function proto
self.hash = None
def process(self, job, data):
log.debug(job)
hps = job.params
inputs1 = []
with open(data['data11']) as f:
for line in f:
if line[:-1]:
inputs1.append(float(line[:-1]))
inputs2 = []
with open(data['data12']) as f:
for line in f:
if line[:-1]:
inputs2.append(float(line[:-1]))
inputs3 = []
with open(data['data21']) as f:
for line in f:
if line[:-1]:
inputs3.append(float(line[:-1]))
inputs4 = []
with open(data['data22']) as f:
for line in f:
if line[:-1]:
inputs4.append(float(line[:-1]))
ret = []
for x in inputs1 + inputs2 + inputs3 + inputs4:
x = x * hps['mult'] + hps['add']
ret.append(x)
log.debug(ret)
return ret
def decideHyperparameters(graphs: List[S2VGraph]) -> int:
if cmd_args.hp_path == 'none':
log.info(f'Using previous CV results to decide hyperparameters')
optHp = parseHpTuning(cmd_args.data)
log.info(f'Optimal hyperparameter setting: {optHp}')
for (key, val) in optHp.items():
if key not in gHP:
log.debug(f'Add {key} = {val} to global HP')
elif gHP[key] != val:
log.debug(f'Replace {key} from {gHP[key]} to {val}')
gHP[key] = val
return gHP['optNumEpochs']
else:
log.info(f'Using 1st hyperparameter setting from {cmd_args.hp_path}')
hpIter = HyperParameterIterator(cmd_args.hp_path)
hp = next(hpIter)
for (key, val) in hp.items():
gHP[key] = val
numNodesList = sorted([g.num_nodes for g in graphs])
idx = int(math.ceil(hp['poolingRatio'] * len(graphs))) - 1
gHP['poolingK'] = numNodesList[idx]
return gHP['numEpochs']
def append_regions(self, t, regions):
"""Append a list of regions given a timestamp
If tstamp exists in frame_anns, appends regions, otherwise, creates new ImageAnns
Args:
t (float): tstamp
region (Region): region
"""
assert isinstance(t, float)
assert isinstance(regions, list)
for region in regions:
assert isinstance(region, type(Region()))
if t in self._tstamp2frameannsidx:
log.debug(f"t: {t} in frame_anns, extending Regions")
frame_anns_idx = self._tstamp2frameannsidx[t]
self._response_internal["media_annotation"]["frames_annotation"][
frame_anns_idx].extend(regions)
else:
log.debug(f"t: {t} NOT in frame_anns, appending ImageAnn")
ia = ImageAnn(t=t, regions=regions)
self._response_internal["media_annotation"][
"frames_annotation"].append(ia)
self._tstamp2frameannsidx[t] = len(self.frame_anns) - 1
def execute_cmd(cmd):
if FLAGS.debug == True:
log.info('Execute ' + ' '.join(cmd))
return None
else:
log.debug('Execute ' + ' '.join(cmd))
return subprocess.Popen(cmd)
def push(self, responses, dst_url=None):
"""Pushes responses to a destination URL via the requests lib
Args:
responses (List[Response]): list of Response objects
dst_url (str): url for POST endpoint
"""
if not isinstance(responses, list):
responses = [responses]
log.debug(f"Pushing {len(responses)} items")
for res in responses:
assert isinstance(res, Response)
if dst_url is None:
dst_url = res.request.dst_url
if dst_url:
log.info(f"Pushing to {dst_url}")
data = res.data
log.info(f"data is of type {type(data)}")
try:
ret = sender.post(dst_url,
headers=self.auth_header,
data=data)
log.info(f"requests.post(...) response: {ret}")
except Exception as e:
log.error(e)
log.error(traceback.print_exc())
else:
log.info("No dst_url field in request. Not pushing response.")
def convert_props_to_pandas_query(query_props):
"""Convert a list of query dicts to a boolean expression to be used in pandas.DataFrame.query
E.g.
[
{"property_type":"object", "value":"face"},
{"value":"car"}
]
is converted to
'(property_type == "object") & (value == "face") | (value == "car")'
Args:
query (list[dict]): list of dictionaries containing key/values of interest
Returns:
str: boolean expression
"""
assert isinstance(query_props, list)
log.debug(query_props)
bool_exp = ""
for i, q in enumerate(query_props):
assert isinstance(q, dict)
for j, (k, v) in enumerate(q.items()):
bool_exp += f'({k} == "{v}")'
if j != len(q) - 1:
bool_exp += " & "
if i != len(query_props) - 1:
bool_exp += " | "
log.debug(f"query: {query_props} transformed into boolean expression: {bool_exp}")
return bool_exp
def mouseDragEvent(self, ev):
glog.debug('drag >> %s', ev.isAccepted())
self.region.notify_mouse_drag(ev)
super().mouseDragEvent(ev)
if ev.isAccepted():
return
glog.debug('drag la')
def branch(self, inst) -> None:
"""Conditional jump to another address or fall throught"""
branchToAddr = inst.findAddrInInst()
self.addr2Inst[inst.address].branchTo = branchToAddr
log.debug(f'[Branch] From {inst.address:x} to {branchToAddr:x}')
self.enter(inst, branchToAddr)
self.enter(inst, inst.address + inst.size)
def recv_until(self, matcher, timeout=None):
timeout = self.normalize_timeout(timeout)
matcher = PatternMatcher.Normalize(matcher)
cur = self.buf
while True:
pos = matcher(cur)
if pos is not None and pos != -1:
self.buf = cur[pos:]
return cur[:pos]
if self.eof:
raise EOFError
try:
tmp = self.recv_base(self.recv_size, timeout)
self.buf = cur
glog.debug('recv_until: %d >> got=%s, cur=%s', len(tmp), tmp,
cur)
if len(tmp) == 0:
continue
except:
self.buf = cur
raise
if tmp is None:
self.buf = cur
self.eof = 1
raise EOFError
cur += tmp
def notify_mouse_drag(self, ev):
glog.debug('notify drag')
if ev.isAccepted():
return
if self.isVisible():
return
self.show()
def _process_request(self, request):
"""Send request_message through all the modules
Args:
request (Request): incoming request
Returns:
Response: outgoing response message
"""
if not isinstance(request, Request):
raise ValueError(f"{request} is of type {type(request)},"
f" not {Request}")
log.debug(f"Processing: {request}")
response = Response(request, self.schema_registry_url)
for module in self.modules:
log.info(f"Processing {request} in module: {module}")
try:
response = module.process(response)
except Exception as e:
log.error(traceback.format_exc())
log.error(f"Processing {request} in {module} FAILED. "
f"Setting error code to {Codes.ERROR_PROCESSING}.")
module.code = Codes.ERROR_PROCESSING
response = module.update_and_return_response()
log.info(f"Processing {request} in module: {module}"
f" ...Status: {module.code}")
if response is not None:
log.debug("response.to_dict():"
f" {json.dumps(response.to_dict(), indent=2)}")
return response
def _start(self):
"""Start server. While loop that pulls requests from the input_comm,
calls _process_request(request), and posts responses to output_comms
"""
while True:
try:
log.debug("Pulling requests")
requests = self.input_comm.pull()
for request in requests:
try:
if request.kill_flag is True:
log.info("Incoming request with kill_flag == True,"
" killing server")
return
response = self._process_request(request)
log.info(
f"Pushing reponse for {request} to output_comms")
for output_comm in self.output_comms:
try:
ret = output_comm.push(response)
except Exception as e:
log.error(e)
log.error(traceback.format_exc())
log.error(
f"Error pushing response for {request}"
f" to output_comm: {output_comm}")
except Exception:
log.error(traceback.format_exc())
log.error(f"Error processing request: {request}")
except Exception as e:
log.error(e)
log.error(traceback.format_exc())
log.error("Error processing requests")
def __init__(self,
job_id,
input_uri,
output_base,
output_path,
params=None,
producer=None,
consumer=None,
processing_time=None):
## The id of this job
self.id = job_id
## The uri to the data file.
self.input_uri = input_uri
## The base uri/db to save a new data file.
self.output_base = output_base
## The path/key to save a new data file.
self.output_path = output_path
## The params may be needed.
self.params = params
## The timastampe when the object is created.
self.timestamp = datetime.utcnow()
## The producer of the data obejct.
self.producer = producer
## The consumer of the data obejct.
self.consumer = consumer
## The time to process the data object.
self.processing_time = processing_time
log.debug('Creating data object: ' + str(self))
def gets(self, indent=0, seen=set()):
if len(seen) == 0:
seen = set()
prefix = '\t' * indent
s = []
s.append(
'Struct: name={}, kind={}, size={}, align={}, ptr_count={} array_size={}, choices={}'.format(
self.name, self.kind, self.size, self.align, self.ptr_count, self.array_size,
self.choices))
if self in seen:
s.append('already seen')
else:
seen.add(self)
s.append(' loc={}'.format(self.get_loc()))
if self.get_base_typ() != self.typ:
s.append('BaseType:\n{}'.format(self.get_base_typ().gets(indent + 1, seen)))
if self.fields:
s.append('Fields: (#{})'.format(len(self.ordered_fields)))
for f in self.ordered_fields:
glog.debug('got field %s', f)
s.append('name={}, off={}, typ:\n{}'.format(f.name, f.off, f.typ.gets(indent + 1, seen)))
s.append('=== DONE name={} ===='.format(self.name))
return '\n'.join([prefix + x for x in s])
def process(self, job, data):
log.debug(job)
hps = job.params
log.debug(data)
return [(x, hps['num']) for x in data]
def speaker_batch_generator(h5ref,
nb_speaker,
speaker_fn,
keys,
frame_length,
frame_stride,
batch_size,
one_hotify=True,
mfcc=False):
glog.debug('Total speaker:: %s' % nb_speaker)
lengths = {key: h5ref[key].shape[0] for key in keys}
key_frames = _generate_all_frame_by_key(keys, lengths, frame_length,
frame_stride)
counter = 0
while True:
batch_inputs = []
batch_outputs = []
for (key, start, end) in key_frames[counter:counter + batch_size]:
batch_inputs.append(h5ref[key][start:end])
batch_outputs.append(speaker_fn(key))
counter += batch_size
if counter + batch_size >= len(key_frames):
np.random.shuffle(key_frames)
counter = 0
yield np.array(batch_inputs), \
one_hot(np.array(batch_outputs, dtype='uint8'), count=nb_speaker),
def __init__(
self,
server_name,
version,
prop_type=None,
prop_id_map=None,
module_id_map=None,
):
"""Module that processes properties, not images
Args:
server_name (str): server_name
version (str): version
prop_type (str, optional): Defaults to None. property_type
prop_id_map (str, optional): Defaults to None. Map: property_type -> id
module_id_map (str, optional): Defaults to None. Map: server_name -> id
"""
super().__init__(
server_name=server_name,
version=version,
prop_type=prop_type,
prop_id_map=prop_id_map,
module_id_map=module_id_map,
)
log.debug("Creating PropertiesModule")
def build(self, extra_args=[], want_sysincludes=True, cpp_mode=True):
self.setup_includes()
args = []
if want_sysincludes: args.append('-isystem/usr/include')
if g_data.is_m32: args.append('-m32')
args.extend(extra_args)
code_content = '\n'.join(self.content)
assert len(self.content) > 0
glog.debug('Extracting >> %s', code_content)
self.res = OpaIndex.create_index(file_content=code_content,
cpp_mode=cpp_mode,
args=args,
filter=self.filter)
for macro in self.res.macros:
self.consts[macro.name] = macro.val
for typedef in self.res.typedefs:
glog.debug('GOT TYP %s', typedef.name)
self.typs[typedef.name] = typedef.typedef_typ
for struct in self.res.structs:
glog.debug('GOT struct %s', struct.name)
self.typs[struct.name] = struct
for fname, function in self.res.functions.items():
glog.debug('GOT function %s', function.name)
self.functions[fname] = function
for var in self.res.vars:
glog.debug('GOT var %s', var.name)
self.vars[var.name] = var
for extractor in self.extractors:
extractor.finish(self)
def __init__(self, binaryId: str, g: nx.Graph, label: int,
node_tags: int = None, node_features=None):
"""
g: a networkx graph
label: an integer graph label
node_tags: a list of integer node tags
node_features: a numpy array of continuous node features
"""
self.bId = binaryId
self.num_nodes = len(node_tags)
self.node_tags = node_tags
self.label = None if label == '?' else label
self.node_features = node_features # nparray (node_num * feature_dim)
self.degs = list(dict(g.degree).values())
if g.number_of_edges() != 0:
x, y = zip(*g.edges())
self.num_edges = len(x)
self.edge_pairs = np.ndarray(shape=(self.num_edges, 2), dtype=np.int32)
self.edge_pairs[:, 0] = x
self.edge_pairs[:, 1] = y
self.edge_pairs = self.edge_pairs.flatten()
else:
self.num_edges = 0
self.edge_pairs = np.array([])
log.warning(f'{binaryId} has no edge')
log.debug(f'{binaryId} #nodes: {self.num_nodes}, label: {label}')
def new_open(fullurl, *args, **kwargs):
# "fullurl" to match the parent function's args
log_url = fullurl
if isinstance(fullurl, urllib2.Request):
log_url = fullurl.get_full_url()
log.debug("http request: %s", log_url)
return orig_open(fullurl, *args, **kwargs)
def process(self, job, data):
log.debug(job)
# datalist = data
# Functions to read in the corpus
w2i = defaultdict(lambda: len(w2i))
t2i = defaultdict(lambda: len(t2i))
UNK = w2i["<unk>"]
training_data = data["train"]
validation_data = data["valid"]
def read_dataset(dataset):
for datapoint in dataset:
tag, words, numtag = datapoint["tag"], datapoint["words"], len(
datapoint["tag"])
yield ([w2i[x] for x in words.split(" ")], t2i[tag], numtag)
# Read in the data
basket = {}
basket['train'] = list(read_dataset(training_data))
basket['w2i'] = defaultdict(lambda: UNK, w2i)
basket['t2i'] = t2i
basket['dev'] = list(read_dataset(validation_data))
basket['nwords'] = len(w2i)
basket['ntags'] = len(t2i)
return basket
def aggregate_all_results(results_dir, use_pgo=False):
""" Aggregate APE results and draw APE boxplot as well as write latex table
with results:
Args:
- result_dir: path to the directory with results ordered as follows:
\* dataset_name:
|___\* pipeline_type:
| |___results.yaml
|___\* pipeline_type:
| |___results.yaml
\* dataset_name:
|___\* pipeline_type:
| |___results.yaml
Basically all subfolders with a results.yaml will be examined.
- use_pgo: whether to aggregate all results for VIO or for PGO trajectory.
set to True for PGO and False (default) for VIO
Returns:
- stats: a nested dictionary with the statistics and results of all pipelines:
* First level ordered with dataset_name as keys:
* Second level ordered with pipeline_type as keys:
* Each stats[dataset_name][pipeline_type] value has:
* absolute_errors: an evo Result type with trajectory and APE stats.
* relative_errors: RPE stats.
"""
import fnmatch
# Load results.
log.info("Aggregate dataset results.")
# Aggregate all stats for each pipeline and dataset
yaml_filename = 'results_vio.yaml'
if use_pgo:
yaml_filename = 'results_pgo.yaml'
stats = dict()
for root, dirnames, filenames in os.walk(results_dir):
for results_filename in fnmatch.filter(filenames, yaml_filename):
results_filepath = os.path.join(root, results_filename)
# Get pipeline name
pipeline_name = os.path.basename(root)
# Get dataset name
dataset_name = os.path.basename(os.path.split(root)[0])
# Collect stats
if stats.get(dataset_name) is None:
stats[dataset_name] = dict()
try:
stats[dataset_name][pipeline_name] = yaml.load(
open(results_filepath, 'r'), Loader=yaml.Loader)
except yaml.YAMLError as e:
raise Exception("Error in results file: ", e)
except:
log.fatal("\033[1mFailed opening file: \033[0m\n %s" %
results_filepath)
log.debug("Check stats from: " + results_filepath)
try:
evt.check_stats(stats[dataset_name][pipeline_name])
except Exception as e:
log.warning(e)
return stats
def jump(self, inst) -> None:
"""Unconditional jump to another address"""
jumpAddr = inst.findAddrInInst()
self.addr2Inst[inst.address].fallThrough = False
self.addr2Inst[inst.address].branchTo = jumpAddr
log.debug(f'[Jump] from {inst.address:x} to {jumpAddr:x}')
self.enter(inst, jumpAddr)
self.enter(inst, inst.address + inst.size)
def __init__(self, port=None, **kwargs):
super(Serial, self).__init__()
glog.debug('Creating serial port with params %s', kwargs)
#not setting port here because don't want to open write away
self.serial = serial.Serial(**kwargs)
self.serial.port = port
self.closed = True
def __init__(self):
"""Private implementation class for Avro IO of local files"""
local_schema_file = pkg_resources.resource_filename(
"multivitamin.data.response", config.SCHEMA_FILE)
log.debug("Using local schema file {}".format(local_schema_file))
if not os.path.exists(local_schema_file):
raise FileNotFoundError("Schema file not found")
self.schema = avro.schema.Parse(open(local_schema_file).read())
def is_run_valid(self, run_id):
is_valid = workflow.google_cloud_file_exists(
self.filter_bucket, f"{run_id}/mlbf/filter"
) and workflow.google_cloud_file_exists(
self.filter_bucket, f"{run_id}/mlbf/filter.stash"
)
log.debug(f"{run_id} {'Is Valid' if is_valid else 'Is Not Valid'}")
return is_valid
def run_test(self, elevator, goal,
iterations=200, controller_elevator=None,
observer_elevator=None):
"""Runs the Elevator plant with an initial condition and goal.
Args:
Elevator: elevator object to use.
initial_X: starting state.
goal: goal state.
iterations: Number of timesteps to run the model for.
controller_Elevator: elevator object to get K from, or None if we should
use Elevator.
observer_Elevator: elevator object to use for the observer, or None if we should
use the actual state.
"""
if controller_elevator is None:
controller_elevator = elevator
vbat = 10.0
if self.t:
initial_t = self.t[-1] + elevator.dt
else:
initial_t = 0
for i in xrange(iterations):
X_hat = elevator.X
if observer_elevator is not None:
X_hat = observer_elevator.X_hat
self.x_hat.append(observer_elevator.X_hat[0, 0])
gravity_compensation = 9.8 * elevator.mass * elevator.r / elevator.G / elevator.Kt * elevator.resistance
U = controller_elevator.K * (goal - X_hat)
U[0, 0] = numpy.clip(U[0, 0], -vbat , vbat )
self.x.append(elevator.X[0, 0])
if self.v:
last_v = self.v[-1]
else:
last_v = 0
self.v.append(elevator.X[1, 0])
self.a.append((self.v[-1] - last_v) / elevator.dt)
if observer_elevator is not None:
observer_elevator.Y = elevator.Y
observer_elevator.CorrectObserver(U)
elevator.Update(U - gravity_compensation)
if observer_elevator is not None:
observer_elevator.PredictObserver(U)
self.t.append(initial_t + i * elevator.dt)
self.u.append(U[0, 0])
# if numpy.abs((goal - X_hat)[0:2, 0]).sum() < .025:
# print "Time: ", self.t[-1]
# break
glog.debug('Time: %f', self.t[-1])
def getRemoteConnectionFromList(nodeLocationList, exclude=None, enclaveIDToFind='ANY', metricsMessageCounter=None):
'''Given a list of nodeLocations, see if we can connect to any one of them!
Do it randomly though to avoid burdening any one location.
nodeLocationList is a list of node locations --> [nodeLoc1, nodeLoc2, etc...]
exclude is a NodeLocation which will be skipped (so you don't connect to yourself).
returns (theNode, factory, conn, nodeLocation) if successful or
(False, False, False, False ) if not.
'''
if not nodeLocationList:
defer.returnValue( (False, False, False, False ))
orderedList = range(len(nodeLocationList))
random.shuffle(orderedList) # Get a random order
for index in orderedList:
nodeLocation = nodeLocationList[index]
# Skip?
if exclude is not None and exclude.ip == nodeLocation.ip and exclude.port == nodeLocation.port:
continue
try:
#log.msg("DEBUG: getRemoteConnectionFromList: %s" % nodeLocation)
(factory, conn) = getRemoteConnection(nodeLocation, metricsMessageCounter)
#log.msg("DEBUG: getting Root Node: %s" % factory._root)
d = factory.getRootObject()
#log.msg("DEBUG: getRemoteConnectionFromList: D is:%s " % d)
theNode = yield d
#log.msg("DEBUG: getRemoteConnectionFromList: GOT ROOT")
# Check the enclave
validEnclave = True
if enclaveIDToFind is not 'ANY':
validEnclave = yield theNode.callRemote("hasEnclaveId",enclaveIDToFind)
if validEnclave:
# We succeeded, return it!
defer.returnValue( (theNode, factory, conn, nodeLocation) )
else:
yield disconnect(None, conn)
except (error.ConnectionRefusedError, error.TimeoutError, error.ConnectError) as e:
glog.debug("getRemoteConnectionFromList failed to connect to : %s. Trying another location." % nodeLocation)
pass
except Exception:
log.err()
# We couldn't get there.
defer.returnValue( (False, False, False, False) )
def classMessageReceived(self, msg, inMyClass):
testNum = msg["testNum"]
if testNum not in self.classMsgs:
# Initialize it
self.classMsgs[testNum] = [0, 0] # Good, Wasted
if inMyClass:
self.classMsgs[testNum][0] += 1
glog.debug("%s got msg IN CLASS" % self.node.nodeLocation, system="MetricsMessageCounter")
else:
self.classMsgs[testNum][1] += 1
glog.debug("%s got msg NOT IN CLASS" % self.node.nodeLocation, system="MetricsMessageCounter")
def findEnclaveNames(nodeLoc):
'''Find the enclave names that the given nodeLoc knows about.'''
names = None
try:
(factory, conn) = getRemoteConnection(nodeLoc)
nodeRef = yield factory.getRootObject()
names = yield nodeRef.callRemote("getEnclaveNames")
yield disconnect(None, conn)
except Exception, e:
# Connection failed in some way
glog.debug("findEnclaveNames failed to get names. %s" % e)
def remote_call(sub_svr_name, sub_svr_id, func, args, kwds):
global call_data
if not gnet.is_sub_server:
call_id = _get_next_id()
async_result = gevent.event.AsyncResult()
call_data[call_id] = async_result
gnet.sends(sub_svr_name, sub_svr_id, [MSGID_REMOTE_CALL, call_id, func, args, kwds])
# 等待返回
is_seccess, return_data = async_result.get(True, REMOTE_CALL_TIMEOUT)
if is_seccess:
glog.debug("remote_call>remote_call return:%s" % repr(return_data))
return return_data
else:
glog.error("remote_call>remote_call return Exception:\n%s" % return_data)
def spawn(conn):
while True:
try:
buff = _recv_package(conn.socket)
except Exception, e:
raise e
break
if len(buff):
_on_data(conn, buff)
else:
glog.debug("trans>recv EMPTY buff, main server disconected")
break
gevent.sleep(0)
def __init__(self, name='Column', disable_indexer=False):
super(Column, self).__init__(name)
A_continuous = numpy.matrix(numpy.zeros((4, 4)))
B_continuous = numpy.matrix(numpy.zeros((4, 2)))
A_continuous[0, 1] = 1
A_continuous[1:, 1:] = self.A_continuous
B_continuous[1:, :] = self.B_continuous
self.A_continuous = A_continuous
self.B_continuous = B_continuous
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
orig_K = self.K
self.K = numpy.matrix(numpy.zeros((2, 4)))
self.K[:, 1:] = orig_K
glog.debug('K is ' + repr(self.K))
# TODO(austin): Do we want to damp velocity out or not when disabled?
#if disable_indexer:
# self.K[0, 1] = 0.0
# self.K[1, 1] = 0.0
orig_Kff = self.Kff
self.Kff = numpy.matrix(numpy.zeros((2, 4)))
self.Kff[:, 1:] = orig_Kff
q_pos = 0.12
q_vel = 2.00
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0, 0.0],
[0.0, 0.0, (q_pos ** 2.0), 0.0],
[0.0, 0.0, 0.0, (q_vel ** 2.0)]])
r_pos = 0.05
self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
[0.0, (r_pos ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
self.InitializeState()
def spawn(socket, address):
conn = _on_connect(socket, address)
while True:
try:
buff = _recv_package(socket)
except Exception, e:
raise e
break
if len(buff):
_on_data(conn, buff)
else:
glog.debug("trans>recv EMPTY buff, client disconected")
break
gevent.sleep(0)
def __init__(self, name='IntegralShooter'):
super(IntegralShooter, self).__init__(name=name)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
self.A_continuous[0:3, 0:3] = self.A_continuous_unaugmented
self.A_continuous[0:3, 3] = self.B_continuous_unaugmented
self.B_continuous = numpy.matrix(numpy.zeros((4, 1)))
self.B_continuous[0:3, 0] = self.B_continuous_unaugmented
self.C_unaugmented = self.C
self.C = numpy.matrix(numpy.zeros((1, 4)))
self.C[0:1, 0:3] = self.C_unaugmented
# The states are [position, unfiltered_velocity, velocity, torque_error]
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
glog.debug('A: \n%s', repr(self.A_continuous))
glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
glog.debug('schur(A): \n%s', repr(scipy.linalg.schur(self.A_continuous)))
glog.debug('A_dt(A): \n%s', repr(self.A))
q_pos = 0.01
q_vel = 5.0
q_velfilt = 1.5
q_voltage = 2.0
self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0, 0.0],
[0.0, 0.0, (q_velfilt ** 2.0), 0.0],
[0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
r_pos = 0.0003
self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
_, _, self.Q, self.R = controls.kalmd(
A_continuous=self.A_continuous, B_continuous=self.B_continuous,
Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
dt=self.dt)
self.KalmanGain, self.P_steady_state = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((1, 4)))
self.K[0, 0:3] = self.K_unaugmented
self.K[0, 3] = 1
self.Kff_unaugmented = self.Kff
self.Kff = numpy.matrix(numpy.zeros((1, 4)))
self.Kff[0, 0:3] = self.Kff_unaugmented
self.InitializeState()
def reBootstrap(self, bootstrapNodeLocList, enclaveId):
'''Try to re-bootstrap into the network being managed by bootstrapNodeLoc'''
try:
glog.debug("SuccessorsList: all successors are dead... attempting to re-bootstrap", system=str(self.nodeLocation))
# If I am my own successor but I'm not a bootstrap node, try to bootstrap
# back into the network.
# Try to get a connection to any of the bootstrap nodes.
# if bootstrapNodeLoc is not None and \
# bootstrapNodeLoc.id != self.nodeLocation.id:
# We have no successors, so we'd better bootstrap back in.
(bootstrapNode, factory, conn, nodeLocation) = yield Utils.getRemoteConnectionFromList(bootstrapNodeLocList, metricsMessageCounter=self.metricsMessageCounter)
if bootstrapNode is False:
# Could not get a connection
# DPF - This is a change on Oct, 2014 -- even if I am NOT a bootstrapNode,
# - I will add myself as a successor. Need to see if this breaks stuff.
# - old version only did this for bootstrap nodes.
self.addValues(self.nodeLocation)
else:
succLoc = yield bootstrapNode.callRemote("findSuccessorLocation", self.nodeLocation.id, enclaveId)
if succLoc is not None:
(factory, conn2) = Utils.getRemoteConnection(succLoc, self.metricsMessageCounter)
succNode = yield factory.getRootObject()
successorsSuccessorList = yield succNode.callRemote("getSuccessorList", enclaveId)
# Add the successor to my list
self.addValues(succLoc)
# Add the successorsList to my list also (for efficiency)
self.addValues(successorsSuccessorList)
# Close connections
Utils.disconnect(None, conn2)
# Close the connections
Utils.disconnect(None, conn)
except Exception, e:
log.msg("bootstrap node is dead. Try again later. [%s]" % e, system="SuccessorsList")
log.err(e, "bootstrap node is dead. Try again later.", system="SuccessorsList")
def __init__(self, name='IntegralArm', mass=None):
super(IntegralArm, self).__init__(name=name, mass=mass)
self.A_continuous_unaugmented = self.A_continuous
self.A_continuous = numpy.matrix(numpy.zeros((5, 5)))
self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
self.A_continuous[1, 4] = self.C2
# Start with the unmodified input
self.B_continuous_unaugmented = self.B_continuous
self.B_continuous = numpy.matrix(numpy.zeros((5, 2)))
self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
self.C_unaugmented = self.C
self.C = numpy.matrix(numpy.zeros((2, 5)))
self.C[0:2, 0:4] = self.C_unaugmented
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
glog.debug('A cont: %s', repr(self.A_continuous))
glog.debug('B cont %s', repr(self.B_continuous))
glog.debug('A discrete %s', repr(self.A))
q_pos = 0.08
q_vel = 0.40
q_pos_diff = 0.08
q_vel_diff = 0.40
q_voltage = 6.0
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
[0.0, 0.0, (q_pos_diff ** 2.0), 0.0, 0.0],
[0.0, 0.0, 0.0, (q_vel_diff ** 2.0), 0.0],
[0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
r_volts = 0.05
self.R = numpy.matrix([[(r_volts ** 2.0), 0.0],
[0.0, (r_volts ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((2, 5)))
self.K[0:2, 0:4] = self.K_unaugmented
self.K[0, 4] = 1
self.K[1, 4] = 1
glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
self.InitializeState()
def rebuildSuccessorList(self, enclaveId, bootstrapNodeLocList = None):
# Track changes to successor for debugging
origSuccessor = self.getSuccessor() # Solely for debugging when successors change
# Prune my successor's list for "alive" only
yield self.removeDeadSuccessors()
subFromI = 0
for i in range(len(self.theList)):
currentSuccLocation = self.theList[i-subFromI]
try:
# Check if the succesor's pred is currentPred
(factory, conn) = Utils.getRemoteConnection(currentSuccLocation, self.metricsMessageCounter)
succ = yield factory.getRootObject()
successorsSuccessorList = yield succ.callRemote("getSuccessorList", enclaveId)
# Add them to my successorsList
self.addValues(successorsSuccessorList)
# Close the connection
Utils.disconnect(None, conn)
# Once we find one live successor list, it's good enough. We can go.
#glog.debug("rebuildSuccessorList: connected successor is: %s" % currentSuccLocation)
defer.returnValue(True)
except Exception :
# Successor wasn't alive, remove from my list of successors
glog.debug("Removing dead successor during rebuild location: ID %s" % currentSuccLocation.id, system='rebuildSuccessorList')
subFromI = self.removeLocationFromList(currentSuccLocation)
# Attempt to re-bootstrap in
yield self.reBootstrap(bootstrapNodeLocList, enclaveId)
self.checkFinalSuccessor(origSuccessor, "rebuildSuccessorList") #DEBUG
defer.returnValue(True)
def disconnect(aDeferredResult, connection):
'''Starts the connection disconnect and returns a defered
which fires once the disconnect is complete.
Relies on the connection being opened using Utils.getRemoteConnection
'''
connection.disconnect()
if Config.USE_CONNECTION_CACHE:
# There is really no callback to call since the connection
# truly isn't closed right away... it's cached.
d= defer.Deferred()
d.callback(True)
return d
else:
try:
return connection.clientLostDefered
except AttributeError:
glog.debug("Connection was not made using GmuClientFactory... continuing.")
d= defer.Deferred()
d.callback(True)
return d
def __init__(self, name='SecondOrderVelocityShooter'):
super(SecondOrderVelocityShooter, self).__init__(name)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
self.A_continuous[0:1, 0:1] = self.A_continuous_unaugmented
self.A_continuous[1, 0] = 175.0
self.A_continuous[1, 1] = -self.A_continuous[1, 0]
self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
self.C = numpy.matrix([[0, 1]])
self.D = numpy.matrix([[0]])
# The states are [unfiltered_velocity, velocity]
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
self.PlaceControllerPoles([.70, 0.60])
q_vel = 40.0
q_filteredvel = 30.0
self.Q = numpy.matrix([[(1.0 / (q_vel ** 2.0)), 0.0],
[0.0, (1.0 / (q_filteredvel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (3.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('K %s', repr(self.K))
glog.debug('System poles are %s',
repr(numpy.linalg.eig(self.A_continuous)[0]))
glog.debug('Poles are %s',
repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.PlaceObserverPoles([0.3, 0.3])
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
qff_vel = 8.0
self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0), 0.0],
[0.0, 1.0 / (qff_vel ** 2.0)]])
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
self.InitializeState()
def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
self.CurrentDrivetrain().r)
low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
self.CurrentDrivetrain().r)
#print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
high_power = high_torque * high_omega
low_power = low_torque * low_omega
#if should_print:
# print gear_name, "High omega", high_omega, "Low omega", low_omega
# print gear_name, "High torque", high_torque, "Low torque", low_torque
# print gear_name, "High power", high_power, "Low power", low_power
# Shift algorithm improvements.
# TODO(aschuh):
# It takes time to shift. Shifting down for 1 cycle doesn't make sense
# because you will end up slower than without shifting. Figure out how
# to include that info.
# If the driver is still in high gear, but isn't asking for the extra power
# from low gear, don't shift until he asks for it.
goal_gear_is_high = high_power > low_power
#goal_gear_is_high = True
if not self.IsInGear(current_gear):
glog.debug('%s Not in gear.', gear_name)
return current_gear
else:
is_high = current_gear is VelocityDrivetrain.HIGH
if is_high != goal_gear_is_high:
if goal_gear_is_high:
glog.debug('%s Shifting up.', gear_name)
return VelocityDrivetrain.SHIFTING_UP
else:
glog.debug('%s Shifting down.', gear_name)
return VelocityDrivetrain.SHIFTING_DOWN
else:
return current_gear
def __init__(self, name='Shooter'):
super(Shooter, self).__init__(name)
self.A_continuous_unaugmented = self.A_continuous
self.B_continuous_unaugmented = self.B_continuous
self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
self.A_continuous[1:3, 1:3] = self.A_continuous_unaugmented
self.A_continuous[0, 2] = 1
self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
self.B_continuous[1:3, 0] = self.B_continuous_unaugmented
# State feedback matrices
# [position, unfiltered_velocity, angular velocity]
self.C = numpy.matrix([[1, 0, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
glog.debug(repr(self.A_continuous))
glog.debug(repr(self.B_continuous))
observeability = controls.ctrb(self.A.T, self.C.T)
glog.debug('Rank of augmented observability matrix. %d', numpy.linalg.matrix_rank(
observeability))
self.PlaceObserverPoles([0.9, 0.8, 0.7])
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((1, 3)))
self.K[0, 1:3] = self.K_unaugmented
self.Kff_unaugmented = self.Kff
self.Kff = numpy.matrix(numpy.zeros((1, 3)))
self.Kff[0, 1:3] = self.Kff_unaugmented
self.InitializeState()
def run_test(arm, initial_X, goal, max_separation_error=0.01,
show_graph=True, iterations=200, controller_arm=None,
observer_arm=None):
"""Runs the arm plant with an initial condition and goal.
The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
False if tests fail.
Args:
arm: arm object to use.
initial_X: starting state.
goal: goal state.
show_graph: Whether or not to display a graph showing the changing
states and voltages.
iterations: Number of timesteps to run the model for.
controller_arm: arm object to get K from, or None if we should
use arm.
observer_arm: arm object to use for the observer, or None if we should
use the actual state.
"""
arm.X = initial_X
if controller_arm is None:
controller_arm = arm
if observer_arm is not None:
observer_arm.X_hat = initial_X + 0.01
observer_arm.X_hat = initial_X
# Various lists for graphing things.
t = []
x_avg = []
x_sep = []
x_hat_avg = []
x_hat_sep = []
v_avg = []
v_sep = []
u_left = []
u_right = []
sep_plot_gain = 100.0
for i in xrange(iterations):
X_hat = arm.X
if observer_arm is not None:
X_hat = observer_arm.X_hat
x_hat_avg.append(observer_arm.X_hat[0, 0])
x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
U = controller_arm.K * (goal - X_hat)
U = CapU(U)
x_avg.append(arm.X[0, 0])
v_avg.append(arm.X[1, 0])
x_sep.append(arm.X[2, 0] * sep_plot_gain)
v_sep.append(arm.X[3, 0])
if observer_arm is not None:
observer_arm.PredictObserver(U)
arm.Update(U)
if observer_arm is not None:
observer_arm.Y = arm.Y
observer_arm.CorrectObserver(U)
t.append(i * arm.dt)
u_left.append(U[0, 0])
u_right.append(U[1, 0])
glog.debug(repr(numpy.linalg.inv(arm.A)))
glog.debug('delta time is %f', arm.dt)
glog.debug('Velocity at t=0 is %f %f %f %f', x_avg[0], v_avg[0], x_sep[0], v_sep[0])
glog.debug('Velocity at t=1+dt is %f %f %f %f', x_avg[1], v_avg[1], x_sep[1], v_sep[1])
if show_graph:
pylab.subplot(2, 1, 1)
pylab.plot(t, x_avg, label='x avg')
pylab.plot(t, x_sep, label='x sep')
if observer_arm is not None:
pylab.plot(t, x_hat_avg, label='x_hat avg')
pylab.plot(t, x_hat_sep, label='x_hat sep')
pylab.legend()
pylab.subplot(2, 1, 2)
pylab.plot(t, u_left, label='u left')
pylab.plot(t, u_right, label='u right')
pylab.legend()
pylab.show()
def run_test(self, intake, end_goal,
controller_intake,
observer_intake=None,
iterations=200):
"""Runs the intake plant with an initial condition and goal.
Args:
intake: intake object to use.
end_goal: end_goal state.
controller_intake: Intake object to get K from, or None if we should
use intake.
observer_intake: Intake object to use for the observer, or None if we should
use the actual state.
iterations: Number of timesteps to run the model for.
"""
if controller_intake is None:
controller_intake = intake
vbat = 12.0
if self.t:
initial_t = self.t[-1] + intake.dt
else:
initial_t = 0
goal = numpy.concatenate((intake.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
profile = TrapezoidProfile(intake.dt)
profile.set_maximum_acceleration(70.0)
profile.set_maximum_velocity(10.0)
profile.SetGoal(goal[0, 0])
U_last = numpy.matrix(numpy.zeros((1, 1)))
for i in xrange(iterations):
observer_intake.Y = intake.Y
observer_intake.CorrectObserver(U_last)
self.offset.append(observer_intake.X_hat[2, 0])
self.x_hat.append(observer_intake.X_hat[0, 0])
next_goal = numpy.concatenate(
(profile.Update(end_goal[0, 0], end_goal[1, 0]),
numpy.matrix(numpy.zeros((1, 1)))),
axis=0)
ff_U = controller_intake.Kff * (next_goal - observer_intake.A * goal)
U_uncapped = controller_intake.K * (goal - observer_intake.X_hat) + ff_U
U = U_uncapped.copy()
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(intake.X[0, 0])
if self.v:
last_v = self.v[-1]
else:
last_v = 0
self.v.append(intake.X[1, 0])
self.a.append((self.v[-1] - last_v) / intake.dt)
offset = 0.0
if i > 100:
offset = 2.0
intake.Update(U + offset)
observer_intake.PredictObserver(U)
self.t.append(initial_t + i * intake.dt)
self.u.append(U[0, 0])
ff_U -= U_uncapped - U
goal = controller_intake.A * goal + controller_intake.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
profile.MoveCurrentState(
numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', self.t[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
glog.debug('error %s', repr(observer_intake.X_hat - end_goal))
def __init__(self, name='Claw', mass=None):
super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
# Stall Current in Amps
self.stall_current = 80.730
# Free Speed in RPM
self.free_speed = 13906.0
# Free Current in Amps
self.free_current = 5.820
# Mass of the claw
if mass is None:
self.mass = 5.0
else:
self.mass = mass
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
# Claw length
self.r = 18 * 0.0254
self.J = self.r * self.mass
# Control loop time step
self.dt = 0.005
# State is [position, velocity]
# Input is [Voltage]
C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
C2 = self.Kt * self.G / (self.J * self.R)
self.A_continuous = numpy.matrix(
[[0, 1],
[0, -C1]])
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0],
[C2]])
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
glog.debug('Free speed is %f', self.free_speed * numpy.pi * 2.0 / 60.0 / self.G)
q_pos = 0.15
q_vel = 2.5
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('K: %s', repr(self.K))
glog.debug('Poles are: %s', repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
glog.debug('L is: %s', repr(self.L))
q_pos = 0.05
q_vel = 2.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
[0.0, (q_vel ** 2.0)]])
r_volts = 0.025
self.R = numpy.matrix([[(r_volts ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
glog.debug('Kal: %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
glog.debug('KalL is: %s', repr(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
self.InitializeState()
def __init__(self, name="Drivetrain", left_low=True, right_low=True):
super(Drivetrain, self).__init__(name)
# Number of motors per side
self.num_motors = 2
# Stall Torque in N m
self.stall_torque = 2.42 * self.num_motors * 0.60
# Stall Current in Amps
self.stall_current = 133.0 * self.num_motors
# Free Speed in RPM. Used number from last year.
self.free_speed = 5500.0
# Free Current in Amps
self.free_current = 4.7 * self.num_motors
# Moment of inertia of the drivetrain in kg m^2
self.J = 8.0
# Mass of the robot, in kg.
self.m = 45
# Radius of the robot, in meters (requires tuning by hand)
self.rb = 0.601 / 2.0
# Radius of the wheels, in meters.
self.r = 0.097155 * 0.9811158901447808 / 118.0 * 115.75
# Resistance of the motor, divided by the number of motors.
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
self.G_low = 14.0 / 48.0 * 18.0 / 60.0 * 18.0 / 36.0
self.G_high = 14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0
if left_low:
self.Gl = self.G_low
else:
self.Gl = self.G_high
if right_low:
self.Gr = self.G_low
else:
self.Gr = self.G_high
# Control loop time step
self.dt = 0.005
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
self.msp = 1.0 / self.m + self.rb * self.rb / self.J
self.msn = 1.0 / self.m - self.rb * self.rb / self.J
# The calculations which we will need for A and B.
self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
# State feedback matrices
# X will be of the format
# [[positionl], [velocityl], [positionr], velocityr]]
self.A_continuous = numpy.matrix(
[[0, 1, 0, 0],
[0, self.msp * self.tcl, 0, self.msn * self.tcr],
[0, 0, 0, 1],
[0, self.msn * self.tcl, 0, self.msp * self.tcr]])
self.B_continuous = numpy.matrix(
[[0, 0],
[self.msp * self.mpl, self.msn * self.mpr],
[0, 0],
[self.msn * self.mpl, self.msp * self.mpr]])
self.C = numpy.matrix([[1, 0, 0, 0],
[0, 0, 1, 0]])
self.D = numpy.matrix([[0, 0],
[0, 0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
if left_low or right_low:
q_pos = 0.12
q_vel = 1.0
else:
q_pos = 0.14
q_vel = 0.95
# Tune the LQR controller
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
[0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
[0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, (1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
glog.debug('K %s', repr(self.K))
self.hlp = 0.3
self.llp = 0.4
self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()
def __init__(self, name="Elevator", mass=None):
super(Elevator, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
# Stall Current in Amps
self.stall_current = 80.730
# Free Speed in RPM
self.free_speed = 13906.0
# Free Current in Amps
self.free_current = 5.820
# Mass of the elevator
if mass is None:
self.mass = 13.0
else:
self.mass = mass
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
self.G = (56.0 / 12.0) * (84.0 / 14.0)
# Pulley diameter
self.r = 32 * 0.005 / numpy.pi / 2.0
# Control loop time step
self.dt = 0.005
# Elevator left/right spring constant (N/m)
self.spring = 800.0
# State is [average position, average velocity,
# position difference/2, velocity difference/2]
# Input is [V_left, V_right]
C1 = self.spring / (self.mass * 0.5)
C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
C3 = self.G * self.G * self.Kt / (
self.R * self.r * self.r * self.mass * 0.5 * self.Kv)
self.A_continuous = numpy.matrix(
[[0, 1, 0, 0],
[0, -C3, 0, 0],
[0, 0, 0, 1],
[0, 0, -C1 * 2.0, -C3]])
glog.debug('Full speed is', C2 / C3 * 12.0)
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0, 0],
[C2 / 2.0, C2 / 2.0],
[0, 0],
[C2 / 2.0, -C2 / 2.0]])
self.C = numpy.matrix([[1, 0, 1, 0],
[1, 0, -1, 0]])
self.D = numpy.matrix([[0, 0],
[0, 0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
glog.debug(repr(self.A))
controllability = controls.ctrb(self.A, self.B)
glog.debug('Rank of augmented controllability matrix: %d',
numpy.linalg.matrix_rank(controllability))
q_pos = 0.02
q_vel = 0.400
q_pos_diff = 0.01
q_vel_diff = 0.45
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
[0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
[0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug(repr(self.K))
glog.debug(repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.20
self.ipl = 0.05
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
self.rpl - 1j * self.ipl])
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()
def __init__(self, name="Wrist"):
super(Wrist, self).__init__(name)
# TODO(constants): Update all of these & retune poles.
# Stall Torque in N m
self.stall_torque = 0.71
# Stall Current in Amps
self.stall_current = 134
# Free Speed in RPM
self.free_speed = 18730
# Free Current in Amps
self.free_current = 0.7
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 18.0) * (48.0 / 16.0)
self.J = 0.35
# Control loop time step
self.dt = 0.005
# State is [position, velocity]
# Input is [Voltage]
C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
C2 = self.Kt * self.G / (self.J * self.R)
self.A_continuous = numpy.matrix(
[[0, 1],
[0, -C1]])
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0],
[C2]])
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
q_pos = 0.20
q_vel = 8.0
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('Poles are %s for %s',
repr(numpy.linalg.eig(self.A - self.B * self.K)[0]), self._name)
q_pos = 0.05
q_vel = 2.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
[0.0, (q_vel ** 2.0)]])
r_volts = 0.025
self.R = numpy.matrix([[(r_volts ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
self.InitializeState()
def __init__(self, name="Intake"):
super(Intake, self).__init__(name)
# TODO(constants): Update all of these & retune poles.
# Stall Torque in N m
self.stall_torque = 0.71
# Stall Current in Amps
self.stall_current = 134
# Free Speed in RPM
self.free_speed = 18730
# Free Current in Amps
self.free_current = 0.7
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
self.G = (56.0 / 12.0) * (64.0 / 14.0) * (72.0 / 18.0) * (48.0 / 15.0)
# Moment of inertia, measured in CAD.
# Extra mass to compensate for friction is added on.
self.J = 0.34 + 0.40
# Control loop time step
self.dt = 0.005
# State is [position, velocity]
# Input is [Voltage]
C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
C2 = self.Kt * self.G / (self.J * self.R)
self.A_continuous = numpy.matrix(
[[0, 1],
[0, -C1]])
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0],
[C2]])
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
glog.debug("Free speed is %f", self.free_speed * numpy.pi * 2.0 / 60.0 / self.G)
q_pos = 0.20
q_vel = 5.0
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
q_pos_ff = 0.005
q_vel_ff = 1.0
self.Qff = numpy.matrix([[(1.0 / (q_pos_ff ** 2.0)), 0.0],
[0.0, (1.0 / (q_vel_ff ** 2.0))]])
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
glog.debug('K %s', repr(self.K))
glog.debug('Poles are %s',
repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
self.rpl = 0.30
self.ipl = 0.10
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
glog.debug('L is %s', repr(self.L))
q_pos = 0.10
q_vel = 1.65
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
[0.0, (q_vel ** 2.0)]])
r_volts = 0.025
self.R = numpy.matrix([[(r_volts ** 2.0)]])
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
glog.debug('Kal %s', repr(self.KalmanGain))
self.L = self.A * self.KalmanGain
glog.debug('KalL is %s', repr(self.L))
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
self.InitializeState()
def Update(self, throttle, steering):
# Shift into the gear which sends the most power to the floor.
# This is the same as sending the most torque down to the floor at the
# wheel.
self.left_gear = self.right_gear = True
if True:
self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
current_gear=self.left_gear,
gear_name="left")
self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
current_gear=self.right_gear,
gear_name="right")
if self.IsInGear(self.left_gear):
self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
if self.IsInGear(self.right_gear):
self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
# Filter the throttle to provide a nicer response.
fvel = self.FilterVelocity(throttle)
# Constant radius means that angualar_velocity / linear_velocity = constant.
# Compute the left and right velocities.
steering_velocity = numpy.abs(fvel) * steering
left_velocity = fvel - steering_velocity
right_velocity = fvel + steering_velocity
# Write this constraint in the form of K * R = w
# angular velocity / linear velocity = constant
# (left - right) / (left + right) = constant
# left - right = constant * left + constant * right
# (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
# (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
# constant
# (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
# (-steering * sign(fvel)) = constant
# (-steering * sign(fvel)) * (left + right) = left - right
# (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
equality_k = numpy.matrix(
[[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
equality_w = 0.0
self.R[0, 0] = left_velocity
self.R[1, 0] = right_velocity
# Construct a constraint on R by manipulating the constraint on U
# Start out with H * U <= k
# U = FF * R + K * (R - X)
# H * (FF * R + K * R - K * X) <= k
# H * (FF + K) * R <= k + H * K * X
R_poly = polytope.HPolytope(
self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
# Limit R back inside the box.
self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
else:
glog.debug('Not all in gear')
if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
# TODO(austin): Use battery volts here.
R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
self.U_ideal[0, 0] = numpy.clip(
self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
self.left_cim.U_min, self.left_cim.U_max)
self.left_cim.Update(self.U_ideal[0, 0])
R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
self.U_ideal[1, 0] = numpy.clip(
self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
self.right_cim.U_min, self.right_cim.U_max)
self.right_cim.Update(self.U_ideal[1, 0])
else:
assert False
self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
# TODO(austin): Model the robot as not accelerating when you shift...
# This hack only works when you shift at the same time.
if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
self.left_gear, self.left_shifter_position = self.SimShifter(
self.left_gear, self.left_shifter_position)
self.right_gear, self.right_shifter_position = self.SimShifter(
self.right_gear, self.right_shifter_position)
glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
glog.debug('Left shifter %s %d Right shifter %s %d',
self.left_gear, self.left_shifter_position,
self.right_gear, self.right_shifter_position)
def __init__(self, name='Arm', mass=None):
super(Arm, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.476
# Stall Current in Amps
self.stall_current = 80.730
# Free Speed in RPM
self.free_speed = 13906.0
# Free Current in Amps
self.free_current = 5.820
# Mass of the arm
if mass is None:
self.mass = 13.0
else:
self.mass = mass
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
self.G = (44.0 / 12.0) * (54.0 / 14.0) * (54.0 / 14.0) * (44.0 / 20.0) * (72.0 / 16.0)
# Fridge arm length
self.r = 32 * 0.0254
# Control loop time step
self.dt = 0.005
# Arm moment of inertia
self.J = self.r * self.mass
# Arm left/right spring constant (N*m / radian)
self.spring = 100.0
# State is [average position, average velocity,
# position difference/2, velocity difference/2]
# Position difference is 1 - 2
# Input is [Voltage 1, Voltage 2]
self.C1 = self.spring / (self.J * 0.5)
self.C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
self.C3 = self.G * self.G * self.Kt / (self.R * self.J * 0.5 * self.Kv)
self.A_continuous = numpy.matrix(
[[0, 1, 0, 0],
[0, -self.C3, 0, 0],
[0, 0, 0, 1],
[0, 0, -self.C1 * 2.0, -self.C3]])
glog.debug('Full speed is %f', self.C2 / self.C3 * 12.0)
glog.debug('Stall arm difference is %f', 12.0 * self.C2 / self.C1)
glog.debug('Stall arm difference first principles is %f', self.stall_torque * self.G / self.spring)
glog.debug('5 degrees of arm error is %f', self.spring / self.r * (math.pi * 5.0 / 180.0))
# Start with the unmodified input
self.B_continuous = numpy.matrix(
[[0, 0],
[self.C2 / 2.0, self.C2 / 2.0],
[0, 0],
[self.C2 / 2.0, -self.C2 / 2.0]])
self.C = numpy.matrix([[1, 0, 1, 0],
[1, 0, -1, 0]])
self.D = numpy.matrix([[0, 0],
[0, 0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
controllability = controls.ctrb(self.A, self.B)
glog.debug('Rank of augmented controllability matrix. %d', numpy.linalg.matrix_rank(
controllability))
q_pos = 0.02
q_vel = 0.300
q_pos_diff = 0.005
q_vel_diff = 0.13
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
[0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
[0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
[0.0, 1.0 / (12.0 ** 2.0)]])
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('Controller\n %s', repr(self.K))
glog.debug('Controller Poles\n %s',
numpy.linalg.eig(self.A - self.B * self.K)[0])
self.rpl = 0.20
self.ipl = 0.05
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
self.rpl - 1j * self.ipl])
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
glog.debug('Observer (Converted to a KF):\n%s',
repr(numpy.linalg.inv(self.A) * self.L))
self.InitializeState()
def run_integral_test(arm, initial_X, goal, observer_arm, disturbance,
max_separation_error=0.01, show_graph=True,
iterations=400):
"""Runs the integral control arm plant with an initial condition and goal.
The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
False if tests fail.
Args:
arm: arm object to use.
initial_X: starting state.
goal: goal state.
observer_arm: arm object to use for the observer.
show_graph: Whether or not to display a graph showing the changing
states and voltages.
iterations: Number of timesteps to run the model for.
disturbance: Voltage missmatch between controller and model.
"""
arm.X = initial_X
# Various lists for graphing things.
t = []
x_avg = []
x_sep = []
x_hat_avg = []
x_hat_sep = []
v_avg = []
v_sep = []
u_left = []
u_right = []
u_error = []
sep_plot_gain = 100.0
unaugmented_goal = goal
goal = numpy.matrix(numpy.zeros((5, 1)))
goal[0:4, 0] = unaugmented_goal
for i in xrange(iterations):
X_hat = observer_arm.X_hat[0:4]
x_hat_avg.append(observer_arm.X_hat[0, 0])
x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
U = observer_arm.K * (goal - observer_arm.X_hat)
u_error.append(observer_arm.X_hat[4,0])
U = CapU(U)
x_avg.append(arm.X[0, 0])
v_avg.append(arm.X[1, 0])
x_sep.append(arm.X[2, 0] * sep_plot_gain)
v_sep.append(arm.X[3, 0])
observer_arm.PredictObserver(U)
arm.Update(U + disturbance)
observer_arm.Y = arm.Y
observer_arm.CorrectObserver(U)
t.append(i * arm.dt)
u_left.append(U[0, 0])
u_right.append(U[1, 0])
glog.debug('End is %f', observer_arm.X_hat[4, 0])
if show_graph:
pylab.subplot(2, 1, 1)
pylab.plot(t, x_avg, label='x avg')
pylab.plot(t, x_sep, label='x sep')
if observer_arm is not None:
pylab.plot(t, x_hat_avg, label='x_hat avg')
pylab.plot(t, x_hat_sep, label='x_hat sep')
pylab.legend()
pylab.subplot(2, 1, 2)
pylab.plot(t, u_left, label='u left')
pylab.plot(t, u_right, label='u right')
pylab.plot(t, u_error, label='u error')
pylab.legend()
pylab.show()
def __init__(self, name='VelocityIndexer'):
super(VelocityIndexer, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.71
# Stall Current in Amps
self.stall_current = 134
self.free_speed_rpm = 18730.0
# Free Speed in rotations/second.
self.free_speed = self.free_speed_rpm / 60.0
# Free Current in Amps
self.free_current = 0.7
# Moment of inertia of the indexer halves in kg m^2
# This is measured as Iyy in CAD (the moment of inertia around the Y axis).
# Inner part of indexer -> Iyy = 59500 lb * mm * mm
# Inner spins with 12 / 48 * 18 / 48 * 24 / 36 * 16 / 72
# Outer part of indexer -> Iyy = 210000 lb * mm * mm
# 1 775 pro -> 12 / 48 * 18 / 48 * 30 / 422
self.J_inner = 0.0269
self.J_outer = 0.0952
# Gear ratios for the inner and outer parts.
self.G_inner = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (12.0 / 84.0)
self.G_outer = (12.0 / 48.0) * (20.0 / 34.0) * (18.0 / 36.0) * (24.0 / 420.0)
# Motor inertia in kg m^2
self.motor_inertia = 0.00001187
# The output coordinate system is in radians for the inner part of the
# indexer.
# Compute the effective moment of inertia assuming all the mass is in that
# coordinate system.
self.J = (
self.J_inner * self.G_inner * self.G_inner +
self.J_outer * self.G_outer * self.G_outer) / (self.G_inner * self.G_inner) + \
self.motor_inertia * ((1.0 / self.G_inner) ** 2.0)
glog.debug('Indexer J is %f', self.J)
self.G = self.G_inner
# Resistance of the motor, divided by 2 to account for the 2 motors
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
(12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Control loop time step
self.dt = 0.005
# State feedback matrices
# [angular velocity]
self.A_continuous = numpy.matrix(
[[-self.Kt / self.Kv / (self.J * self.G * self.G * self.resistance)]])
self.B_continuous = numpy.matrix(
[[self.Kt / (self.J * self.G * self.resistance)]])
self.C = numpy.matrix([[1]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
self.PlaceControllerPoles([.75])
self.PlaceObserverPoles([0.3])
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
qff_vel = 8.0
self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
self.InitializeState()
def main(argv):
vdrivetrain = VelocityDrivetrain()
if not FLAGS.plot:
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
namespaces = ['y2014_bot3', 'control_loops', 'drivetrain']
dog_loop_writer = control_loop.ControlLoopWriter(
"VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
namespaces=namespaces)
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
cim_writer.Write(argv[3], argv[4])
return
vl_plot = []
vr_plot = []
ul_plot = []
ur_plot = []
radius_plot = []
t_plot = []
left_gear_plot = []
right_gear_plot = []
vdrivetrain.left_shifter_position = 0.0
vdrivetrain.right_shifter_position = 0.0
vdrivetrain.left_gear = VelocityDrivetrain.LOW
vdrivetrain.right_gear = VelocityDrivetrain.LOW
glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
glog.debug('Left is high')
else:
glog.debug('Left is low')
if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
glog.debug('Right is high')
else:
glog.debug('Right is low')
for t in numpy.arange(0, 1.7, vdrivetrain.dt):
if t < 0.5:
vdrivetrain.Update(throttle=0.00, steering=1.0)
elif t < 1.2:
vdrivetrain.Update(throttle=0.5, steering=1.0)
else:
vdrivetrain.Update(throttle=0.00, steering=1.0)
t_plot.append(t)
vl_plot.append(vdrivetrain.X[0, 0])
vr_plot.append(vdrivetrain.X[1, 0])
ul_plot.append(vdrivetrain.U[0, 0])
ur_plot.append(vdrivetrain.U[1, 0])
left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
if abs(fwd_velocity) < 0.0000001:
radius_plot.append(turn_velocity)
else:
radius_plot.append(turn_velocity / fwd_velocity)
# TODO(austin):
# Shifting compensation.
# Tighten the turn.
# Closed loop drive.
pylab.plot(t_plot, vl_plot, label='left velocity')
pylab.plot(t_plot, vr_plot, label='right velocity')
pylab.plot(t_plot, ul_plot, label='left voltage')
pylab.plot(t_plot, ur_plot, label='right voltage')
pylab.plot(t_plot, radius_plot, label='radius')
pylab.plot(t_plot, left_gear_plot, label='left gear high')
pylab.plot(t_plot, right_gear_plot, label='right gear high')
pylab.legend()
pylab.show()
return 0
def test_debug(self):
log.debug('test')
