Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/aos/seasocks/gen_embedded.py b/aos/seasocks/gen_embedded.py
index fdcdfd9..b6f43aa 100755
--- a/aos/seasocks/gen_embedded.py
+++ b/aos/seasocks/gen_embedded.py
@@ -13,18 +13,18 @@
output = sys.argv[1].replace('"', '')
if not os.path.exists(os.path.dirname(output)):
- os.makedirs(os.path.dirname(output))
+ os.makedirs(os.path.dirname(output))
if len(sys.argv) >= 3:
- web = sys.argv[2:]
+ web = sys.argv[2:]
else:
- web = []
- for root, dirs, files in os.walk("./www_defaults", topdown=False):
- for name in files + dirs:
- web.append(os.path.join(root, name))
+ web = []
+ for root, dirs, files in os.walk("./www_defaults", topdown=False):
+ for name in files + dirs:
+ web.append(os.path.join(root, name))
with open(output, 'w') as o:
- o.write("""
+ o.write("""
#include "internal/Embedded.h"
#include <string>
@@ -35,14 +35,14 @@
std::unordered_map<std::string, EmbeddedContent> embedded = {
""")
- for f in web:
- with open(f, 'rb') as file:
- bytes = file.read()
- o.write('{"/%s", {' % os.path.basename(f))
- o.write('"' + "".join(['\\x%02x' % x for x in bytes]) + '"')
- o.write(',%d }},' % len(bytes))
+ for f in web:
+ with open(f, 'rb') as file:
+ bytes = file.read()
+ o.write('{"/%s", {' % os.path.basename(f))
+ o.write('"' + "".join(['\\x%02x' % x for x in bytes]) + '"')
+ o.write(',%d }},' % len(bytes))
- o.write("""
+ o.write("""
};
} // namespace
diff --git a/aos/util/log_to_mcap_test.py b/aos/util/log_to_mcap_test.py
index 72c9078..7bdffe4 100644
--- a/aos/util/log_to_mcap_test.py
+++ b/aos/util/log_to_mcap_test.py
@@ -12,25 +12,32 @@
def main(argv: Sequence[Text]):
parser = argparse.ArgumentParser()
- parser.add_argument("--log_to_mcap", required=True, help="Path to log_to_mcap binary.")
+ parser.add_argument("--log_to_mcap",
+ required=True,
+ help="Path to log_to_mcap binary.")
parser.add_argument("--mcap", required=True, help="Path to mcap binary.")
- parser.add_argument("--generate_log", required=True, help="Path to logfile generator.")
+ parser.add_argument("--generate_log",
+ required=True,
+ help="Path to logfile generator.")
args = parser.parse_args(argv)
with tempfile.TemporaryDirectory() as tmpdir:
log_name = tmpdir + "/test_log/"
mcap_name = tmpdir + "/log.mcap"
- subprocess.run([args.generate_log, "--output_folder", log_name]).check_returncode()
+ subprocess.run([args.generate_log, "--output_folder",
+ log_name]).check_returncode()
# Run with a really small chunk size, to force a multi-chunk file.
- subprocess.run(
- [args.log_to_mcap, "--output_path", mcap_name, "--mcap_chunk_size", "1000", "--mode", "json",
- log_name]).check_returncode()
+ subprocess.run([
+ args.log_to_mcap, "--output_path", mcap_name, "--mcap_chunk_size",
+ "1000", "--mode", "json", log_name
+ ]).check_returncode()
# MCAP attempts to find $HOME/.mcap.yaml, and dies on $HOME not existing. So
# give it an arbitrary config location (it seems to be fine with a non-existent config).
- doctor_result = subprocess.run(
- [args.mcap, "doctor", mcap_name, "--config", tmpdir + "/.mcap.yaml"],
- stdout=subprocess.PIPE,
- stderr=subprocess.PIPE,
- encoding='utf-8')
+ doctor_result = subprocess.run([
+ args.mcap, "doctor", mcap_name, "--config", tmpdir + "/.mcap.yaml"
+ ],
+ stdout=subprocess.PIPE,
+ stderr=subprocess.PIPE,
+ encoding='utf-8')
print(doctor_result.stdout)
print(doctor_result.stderr)
# mcap doctor doesn't actually return a non-zero exit code on certain failures...
diff --git a/aos/util/trapezoid_profile.py b/aos/util/trapezoid_profile.py
index af85339..76c9758 100644
--- a/aos/util/trapezoid_profile.py
+++ b/aos/util/trapezoid_profile.py
@@ -2,8 +2,9 @@
import numpy
+
class TrapezoidProfile(object):
- """Computes a trapezoidal motion profile
+ """Computes a trapezoidal motion profile
Attributes:
_acceleration_time: the amount of time the robot will travel at the
@@ -21,136 +22,140 @@
_timestep: time between calls to Update (delta_time)
_output: output array containing distance to goal and velocity
"""
- def __init__(self, delta_time):
- """Constructs a TrapezoidProfile.
+
+ def __init__(self, delta_time):
+ """Constructs a TrapezoidProfile.
Args:
delta_time: time between calls to Update (seconds)
"""
- self._acceleration_time = 0
- self._acceleration = 0
- self._constant_time = 0
- self._deceleration_time = 0
- self._deceleration = 0
+ self._acceleration_time = 0
+ self._acceleration = 0
+ self._constant_time = 0
+ self._deceleration_time = 0
+ self._deceleration = 0
- self._maximum_acceleration = 0
- self._maximum_velocity = 0
- self._timestep = delta_time
+ self._maximum_acceleration = 0
+ self._maximum_velocity = 0
+ self._timestep = delta_time
- self._output = numpy.array(numpy.zeros((2,1)))
+ self._output = numpy.array(numpy.zeros((2, 1)))
- # Updates the state
- def Update(self, goal_position, goal_velocity):
- self._CalculateTimes(goal_position - self._output[0], goal_velocity)
+ # Updates the state
+ def Update(self, goal_position, goal_velocity):
+ self._CalculateTimes(goal_position - self._output[0], goal_velocity)
- next_timestep = self._timestep
+ next_timestep = self._timestep
- # We now have the amount of time we need to accelerate to follow the
- # profile, the amount of time we need to move at constant velocity
- # to follow the profile, and the amount of time we need to decelerate to
- # follow the profile. Do as much of that as we have time left in dt.
- if self._acceleration_time > next_timestep:
- self._UpdateVals(self._acceleration, next_timestep)
- else:
- self._UpdateVals(self._acceleration, self._acceleration_time)
-
- next_timestep -= self._acceleration_time
- if self._constant_time > next_timestep:
- self._UpdateVals(0, next_timestep)
- else:
- self._UpdateVals(0, self._constant_time)
- next_timestep -= self._constant_time;
- if self._deceleration_time > next_timestep:
- self._UpdateVals(self._deceleration, next_timestep)
+ # We now have the amount of time we need to accelerate to follow the
+ # profile, the amount of time we need to move at constant velocity
+ # to follow the profile, and the amount of time we need to decelerate to
+ # follow the profile. Do as much of that as we have time left in dt.
+ if self._acceleration_time > next_timestep:
+ self._UpdateVals(self._acceleration, next_timestep)
else:
- self._UpdateVals(self._deceleration, self._deceleration_time)
- next_timestep -= self._deceleration_time
- self._UpdateVals(0, next_timestep)
+ self._UpdateVals(self._acceleration, self._acceleration_time)
- return self._output
+ next_timestep -= self._acceleration_time
+ if self._constant_time > next_timestep:
+ self._UpdateVals(0, next_timestep)
+ else:
+ self._UpdateVals(0, self._constant_time)
+ next_timestep -= self._constant_time
+ if self._deceleration_time > next_timestep:
+ self._UpdateVals(self._deceleration, next_timestep)
+ else:
+ self._UpdateVals(self._deceleration,
+ self._deceleration_time)
+ next_timestep -= self._deceleration_time
+ self._UpdateVals(0, next_timestep)
- # Useful for preventing windup etc.
- def MoveCurrentState(self, current):
- self._output = current
+ return self._output
- # Useful for preventing windup etc.
- def MoveGoal(self, dx):
- self._output[0] += dx
+ # Useful for preventing windup etc.
+ def MoveCurrentState(self, current):
+ self._output = current
- def SetGoal(self, x):
- self._output[0] = x
+ # Useful for preventing windup etc.
+ def MoveGoal(self, dx):
+ self._output[0] += dx
- def set_maximum_acceleration(self, maximum_acceleration):
- self._maximum_acceleration = maximum_acceleration
+ def SetGoal(self, x):
+ self._output[0] = x
- def set_maximum_velocity(self, maximum_velocity):
- self._maximum_velocity = maximum_velocity
+ def set_maximum_acceleration(self, maximum_acceleration):
+ self._maximum_acceleration = maximum_acceleration
- def _UpdateVals(self, acceleration, delta_time):
- self._output[0, 0] += (self._output[1, 0] * delta_time
- + 0.5 * acceleration * delta_time * delta_time)
- self._output[1, 0] += acceleration * delta_time
+ def set_maximum_velocity(self, maximum_velocity):
+ self._maximum_velocity = maximum_velocity
- def _CalculateTimes(self, distance_to_target, goal_velocity):
- if distance_to_target == 0:
- self._acceleration_time = 0
- self._acceleration = 0
- self._constant_time = 0
- self._deceleration_time = 0
- self._deceleration = 0
- return
- elif distance_to_target < 0:
- # Recurse with everything inverted.
- self._output[1] *= -1
- self._CalculateTimes(-distance_to_target, -goal_velocity)
- self._output[1] *= -1
- self._acceleration *= -1
- self._deceleration *= -1
- return
+ def _UpdateVals(self, acceleration, delta_time):
+ self._output[0, 0] += (self._output[1, 0] * delta_time +
+ 0.5 * acceleration * delta_time * delta_time)
+ self._output[1, 0] += acceleration * delta_time
- self._constant_time = 0
- self._acceleration = self._maximum_acceleration
- maximum_acceleration_velocity = (
- distance_to_target * 2 * numpy.abs(self._acceleration)
- + self._output[1] * self._output[1])
- if maximum_acceleration_velocity > 0:
- maximum_acceleration_velocity = numpy.sqrt(maximum_acceleration_velocity)
- else:
- maximum_acceleration_velocity = -numpy.sqrt(-maximum_acceleration_velocity)
+ def _CalculateTimes(self, distance_to_target, goal_velocity):
+ if distance_to_target == 0:
+ self._acceleration_time = 0
+ self._acceleration = 0
+ self._constant_time = 0
+ self._deceleration_time = 0
+ self._deceleration = 0
+ return
+ elif distance_to_target < 0:
+ # Recurse with everything inverted.
+ self._output[1] *= -1
+ self._CalculateTimes(-distance_to_target, -goal_velocity)
+ self._output[1] *= -1
+ self._acceleration *= -1
+ self._deceleration *= -1
+ return
- # Since we know what we'd have to do if we kept after it to decelerate, we
- # know the sign of the acceleration.
- if maximum_acceleration_velocity > goal_velocity:
- self._deceleration = -self._maximum_acceleration
- else:
- self._deceleration = self._maximum_acceleration
+ self._constant_time = 0
+ self._acceleration = self._maximum_acceleration
+ maximum_acceleration_velocity = (
+ distance_to_target * 2 * numpy.abs(self._acceleration) +
+ self._output[1] * self._output[1])
+ if maximum_acceleration_velocity > 0:
+ maximum_acceleration_velocity = numpy.sqrt(
+ maximum_acceleration_velocity)
+ else:
+ maximum_acceleration_velocity = -numpy.sqrt(
+ -maximum_acceleration_velocity)
- # We now know the top velocity we can get to.
- top_velocity = numpy.sqrt((distance_to_target +
- (self._output[1] * self._output[1]) /
- (2.0 * self._acceleration) +
- (goal_velocity * goal_velocity) /
- (2.0 * self._deceleration)) /
- (-1.0 / (2.0 * self._deceleration) +
- 1.0 / (2.0 * self._acceleration)))
+ # Since we know what we'd have to do if we kept after it to decelerate, we
+ # know the sign of the acceleration.
+ if maximum_acceleration_velocity > goal_velocity:
+ self._deceleration = -self._maximum_acceleration
+ else:
+ self._deceleration = self._maximum_acceleration
- # If it can go too fast, we now know how long we get to accelerate for and
- # how long to go at constant velocity.
- if top_velocity > self._maximum_velocity:
- self._acceleration_time = ((self._maximum_velocity - self._output[1]) /
- self._maximum_acceleration)
- self._constant_time = (distance_to_target +
- (goal_velocity * goal_velocity -
- self._maximum_velocity * self._maximum_velocity) /
- (2.0 * self._maximum_acceleration)) / self._maximum_velocity
- else:
- self._acceleration_time = (
- (top_velocity - self._output[1]) / self._acceleration)
+ # We now know the top velocity we can get to.
+ top_velocity = numpy.sqrt(
+ (distance_to_target + (self._output[1] * self._output[1]) /
+ (2.0 * self._acceleration) + (goal_velocity * goal_velocity) /
+ (2.0 * self._deceleration)) / (-1.0 /
+ (2.0 * self._deceleration) + 1.0 /
+ (2.0 * self._acceleration)))
- if self._output[1] > self._maximum_velocity:
- self._constant_time = 0
- self._acceleration_time = 0
+ # If it can go too fast, we now know how long we get to accelerate for and
+ # how long to go at constant velocity.
+ if top_velocity > self._maximum_velocity:
+ self._acceleration_time = (
+ (self._maximum_velocity - self._output[1]) /
+ self._maximum_acceleration)
+ self._constant_time = (
+ distance_to_target +
+ (goal_velocity * goal_velocity -
+ self._maximum_velocity * self._maximum_velocity) /
+ (2.0 * self._maximum_acceleration)) / self._maximum_velocity
+ else:
+ self._acceleration_time = ((top_velocity - self._output[1]) /
+ self._acceleration)
- self._deceleration_time = (
- (goal_velocity - top_velocity) / self._deceleration)
+ if self._output[1] > self._maximum_velocity:
+ self._constant_time = 0
+ self._acceleration_time = 0
+ self._deceleration_time = ((goal_velocity - top_velocity) /
+ self._deceleration)
diff --git a/aos/vision/download/downloader.py b/aos/vision/download/downloader.py
index 3dafb48..0d15b87 100644
--- a/aos/vision/download/downloader.py
+++ b/aos/vision/download/downloader.py
@@ -12,39 +12,51 @@
import os
import os.path
+
def RunAndSplitShas(shcmd):
- out = subprocess.check_output(shcmd)
- return [line.split(' ')[0] for line in filter(lambda x: x, out.split('\n'))]
+ out = subprocess.check_output(shcmd)
+ return [
+ line.split(' ')[0] for line in filter(lambda x: x, out.split('\n'))
+ ]
+
def GetChecksums(fnames):
- return RunAndSplitShas(["sha256sum"] + fnames)
+ return RunAndSplitShas(["sha256sum"] + fnames)
+
def GetJetsonChecksums(ssh_target, fnames):
- target_files = ["/root/%s" % fname for fname in fnames]
- subprocess.check_call(["ssh", ssh_target, "touch " + " ".join(target_files)])
- cmds = ["ssh", ssh_target, "sha256sum " + " ".join(target_files)]
- return RunAndSplitShas(cmds)
+ target_files = ["/root/%s" % fname for fname in fnames]
+ subprocess.check_call(
+ ["ssh", ssh_target, "touch " + " ".join(target_files)])
+ cmds = ["ssh", ssh_target, "sha256sum " + " ".join(target_files)]
+ return RunAndSplitShas(cmds)
+
def ToJetsonFname(fname):
- if (fname[-9:] == ".stripped"):
- fname = fname[:-9]
- return os.path.basename(fname)
+ if (fname[-9:] == ".stripped"):
+ fname = fname[:-9]
+ return os.path.basename(fname)
+
def VerifyCheckSumsAndUpload(fnames, ssh_target):
- jetson_fnames = [ToJetsonFname(fname) for fname in fnames]
- checksums = GetChecksums(fnames)
- jetson_checksums = GetJetsonChecksums(ssh_target, jetson_fnames)
- for i in range(len(fnames)):
- if (checksums[i] != jetson_checksums[i]):
- # if empty, unlink
- subprocess.check_call(["ssh", ssh_target, "unlink " + jetson_fnames[i]])
- subprocess.check_call(["scp", fnames[i], ssh_target + ":" + jetson_fnames[i]])
+ jetson_fnames = [ToJetsonFname(fname) for fname in fnames]
+ checksums = GetChecksums(fnames)
+ jetson_checksums = GetJetsonChecksums(ssh_target, jetson_fnames)
+ for i in range(len(fnames)):
+ if (checksums[i] != jetson_checksums[i]):
+ # if empty, unlink
+ subprocess.check_call(
+ ["ssh", ssh_target, "unlink " + jetson_fnames[i]])
+ subprocess.check_call(
+ ["scp", fnames[i], ssh_target + ":" + jetson_fnames[i]])
+
def main(argv):
- args = argv[argv.index('--') + 1:]
- files = argv[1:argv.index('--')]
+ args = argv[argv.index('--') + 1:]
+ files = argv[1:argv.index('--')]
- VerifyCheckSumsAndUpload(files, args[-1])
+ VerifyCheckSumsAndUpload(files, args[-1])
+
if __name__ == '__main__':
- main(sys.argv)
+ main(sys.argv)
diff --git a/build_tests/dummy_http_server.py b/build_tests/dummy_http_server.py
index f65122a..0661a8a 100644
--- a/build_tests/dummy_http_server.py
+++ b/build_tests/dummy_http_server.py
@@ -11,6 +11,7 @@
from http.server import BaseHTTPRequestHandler, HTTPServer
import os
+
def parse_auth(authorization):
auth_type, auth_info = authorization.split(" ", maxsplit=1)
if auth_type != "Basic":
@@ -18,7 +19,9 @@
username, _ = base64.b64decode(auth_info).decode().split(":", 1)
return username
+
class Server(BaseHTTPRequestHandler):
+
def _set_response(self):
self.send_response(200)
self.send_header("Content-type", "text/html")
@@ -34,6 +37,7 @@
username = parse_auth(self.headers["Authorization"])
self._write(f"Hello, {username}")
+
def main():
port = int(os.environ["APACHE_WRAPPED_PORT"])
server_address = ("", port)
@@ -44,5 +48,6 @@
pass
httpd.server_close()
+
if __name__ == "__main__":
main()
diff --git a/build_tests/python_opencv.py b/build_tests/python_opencv.py
index c4d8591..5f5f1aa 100644
--- a/build_tests/python_opencv.py
+++ b/build_tests/python_opencv.py
@@ -3,4 +3,4 @@
import cv2
if __name__ == '__main__':
- cv2.SIFT_create()
+ cv2.SIFT_create()
diff --git a/debian/download_packages.py b/debian/download_packages.py
index be43120..6d548b5 100755
--- a/debian/download_packages.py
+++ b/debian/download_packages.py
@@ -10,18 +10,19 @@
import argparse
import hashlib
+
def initialize_apt(apt_dir, apt_args, args):
- os.mkdir(os.path.join(apt_dir, 'etc'))
- os.mkdir(os.path.join(apt_dir, 'etc', 'apt'))
- os.mkdir(os.path.join(apt_dir, 'etc', 'apt', 'trusted.gpg.d'))
- os.mkdir(os.path.join(apt_dir, 'etc', 'apt', 'preferences.d'))
- os.mkdir(os.path.join(apt_dir, 'var'))
- os.mkdir(os.path.join(apt_dir, 'var', 'lib'))
- os.mkdir(os.path.join(apt_dir, 'var', 'lib', 'dpkg'))
- with open(os.path.join(apt_dir, 'var', 'lib', 'dpkg', 'status'), 'w'):
- pass
- with open(os.path.join(apt_dir, 'etc', 'apt', 'sources.list'), 'w') as f:
- f.write("""
+ os.mkdir(os.path.join(apt_dir, 'etc'))
+ os.mkdir(os.path.join(apt_dir, 'etc', 'apt'))
+ os.mkdir(os.path.join(apt_dir, 'etc', 'apt', 'trusted.gpg.d'))
+ os.mkdir(os.path.join(apt_dir, 'etc', 'apt', 'preferences.d'))
+ os.mkdir(os.path.join(apt_dir, 'var'))
+ os.mkdir(os.path.join(apt_dir, 'var', 'lib'))
+ os.mkdir(os.path.join(apt_dir, 'var', 'lib', 'dpkg'))
+ with open(os.path.join(apt_dir, 'var', 'lib', 'dpkg', 'status'), 'w'):
+ pass
+ with open(os.path.join(apt_dir, 'etc', 'apt', 'sources.list'), 'w') as f:
+ f.write("""
deb http://deb.debian.org/debian/ {release} main contrib non-free
deb-src http://deb.debian.org/debian/ {release} main contrib non-free
@@ -34,103 +35,117 @@
deb http://deb.debian.org/debian {release}-backports main contrib non-free
deb-src http://deb.debian.org/debian {release}-backports main contrib non-free
""".format(release=args.release))
- for key in args.apt_key:
- basename = os.path.basename(key)
- urllib.request.urlretrieve(key, os.path.join(apt_dir, 'etc', 'apt', 'trusted.gpg.d', basename))
- subprocess.check_call(["apt-get"] + apt_args + ["update"])
+ for key in args.apt_key:
+ basename = os.path.basename(key)
+ urllib.request.urlretrieve(
+ key, os.path.join(apt_dir, 'etc', 'apt', 'trusted.gpg.d',
+ basename))
+ subprocess.check_call(["apt-get"] + apt_args + ["update"])
+
def get_deps(apt_args, package):
- env = dict(os.environ)
- del env['LD_LIBRARY_PATH']
- out = subprocess.check_output(["apt-rdepends"] + apt_args + [package], env=env)
- deps = out.splitlines()
- return set([dep for dep in deps if not dep.startswith(b" ")])
+ env = dict(os.environ)
+ del env['LD_LIBRARY_PATH']
+ out = subprocess.check_output(["apt-rdepends"] + apt_args + [package],
+ env=env)
+ deps = out.splitlines()
+ return set([dep for dep in deps if not dep.startswith(b" ")])
+
def get_all_deps(apt_args, packages):
- deps = set()
- for package in packages or ():
- deps.update(get_deps(apt_args, package))
- return deps
+ deps = set()
+ for package in packages or ():
+ deps.update(get_deps(apt_args, package))
+ return deps
+
def map_virtual_packages(packages):
- '''Maps known virtual packages to the preferred concrete packages which
+ '''Maps known virtual packages to the preferred concrete packages which
provide them.'''
- for package in packages:
- if package == b'python-numpy-abi9':
- yield b'python-numpy'
- continue
- if package == b'python3-numpy-abi9':
- yield b'python3-numpy'
- continue
- if package == b'libjack-0.125':
- yield b'libjack-jackd2-0'
- continue
- if package == b'fonts-freefont':
- yield b'fonts-freefont-ttf'
- continue
- if package == b'gsettings-backend':
- yield b'dconf-gsettings-backend'
- continue
- if package == b'gdal-abi-2-4-0':
- yield b'libgdal20'
- continue
- if package == b'libglu1':
- yield b'libglu1-mesa'
- continue
- if package == b'liblapack.so.3':
- yield b'liblapack3'
- continue
- if package == b'libopencl1':
- yield b'ocl-icd-libopencl1'
- continue
- if package == b'libgcc1':
- yield b'libgcc-s1'
- continue
- if package == b'libopencl-1.2-1':
- yield b'ocl-icd-libopencl1'
- continue
- if package == b'libblas.so.3':
- yield b'libblas3'
- continue
- if package == b'debconf-2.0':
- yield b'debconf'
- continue
- yield package
+ for package in packages:
+ if package == b'python-numpy-abi9':
+ yield b'python-numpy'
+ continue
+ if package == b'python3-numpy-abi9':
+ yield b'python3-numpy'
+ continue
+ if package == b'libjack-0.125':
+ yield b'libjack-jackd2-0'
+ continue
+ if package == b'fonts-freefont':
+ yield b'fonts-freefont-ttf'
+ continue
+ if package == b'gsettings-backend':
+ yield b'dconf-gsettings-backend'
+ continue
+ if package == b'gdal-abi-2-4-0':
+ yield b'libgdal20'
+ continue
+ if package == b'libglu1':
+ yield b'libglu1-mesa'
+ continue
+ if package == b'liblapack.so.3':
+ yield b'liblapack3'
+ continue
+ if package == b'libopencl1':
+ yield b'ocl-icd-libopencl1'
+ continue
+ if package == b'libgcc1':
+ yield b'libgcc-s1'
+ continue
+ if package == b'libopencl-1.2-1':
+ yield b'ocl-icd-libopencl1'
+ continue
+ if package == b'libblas.so.3':
+ yield b'libblas3'
+ continue
+ if package == b'debconf-2.0':
+ yield b'debconf'
+ continue
+ yield package
+
def download_deps(apt_args, packages, excludes, force_includes):
- deps = get_all_deps(apt_args, packages)
- exclude_deps = get_all_deps(apt_args, excludes)
- deps -= exclude_deps
- force_include_deps = get_all_deps(apt_args, force_includes)
- deps |= force_include_deps
- env = dict(os.environ)
- del env['LD_LIBRARY_PATH']
- subprocess.check_call([b"apt-get"] + [a.encode('utf-8') for a in apt_args] + [b"download"] + list(map_virtual_packages(deps)), env=env)
+ deps = get_all_deps(apt_args, packages)
+ exclude_deps = get_all_deps(apt_args, excludes)
+ deps -= exclude_deps
+ force_include_deps = get_all_deps(apt_args, force_includes)
+ deps |= force_include_deps
+ env = dict(os.environ)
+ del env['LD_LIBRARY_PATH']
+ subprocess.check_call([b"apt-get"] + [a.encode('utf-8')
+ for a in apt_args] + [b"download"] +
+ list(map_virtual_packages(deps)),
+ env=env)
+
def fixup_files():
- # Gotta remove those pesky epoch numbers in the file names. Bazel doesn't
- # like them.
- regex = re.compile(".%3a")
- contents = os.listdir(os.getcwd())
- for deb in contents:
- new_name = regex.sub("", deb)
- if new_name != deb:
- os.rename(deb, new_name)
+ # Gotta remove those pesky epoch numbers in the file names. Bazel doesn't
+ # like them.
+ regex = re.compile(".%3a")
+ contents = os.listdir(os.getcwd())
+ for deb in contents:
+ new_name = regex.sub("", deb)
+ if new_name != deb:
+ os.rename(deb, new_name)
+
def sha256_checksum(filename, block_size=65536):
- sha256 = hashlib.sha256()
- with open(filename, 'rb') as f:
- for block in iter(lambda: f.read(block_size), b''):
- sha256.update(block)
- return sha256.hexdigest()
+ sha256 = hashlib.sha256()
+ with open(filename, 'rb') as f:
+ for block in iter(lambda: f.read(block_size), b''):
+ sha256.update(block)
+ return sha256.hexdigest()
+
def print_file_list():
- contents = os.listdir(os.getcwd())
- contents.sort()
- print("_files = {")
- for deb in contents:
- print(' "%s": "%s",' % (deb, sha256_checksum(deb)))
- print("}")
+ contents = os.listdir(os.getcwd())
+ contents.sort()
+ print("_files = {")
+ for deb in contents:
+ print(' "%s": "%s",' % (deb, sha256_checksum(deb)))
+ print("}")
+
_ALWAYS_EXCLUDE = [
"dbus-session-bus",
@@ -144,41 +159,66 @@
"libc6-dev",
]
+
def main(argv):
- parser = argparse.ArgumentParser()
- parser.add_argument("--exclude", "-e", type=str, action="append", help="A package to exclude from the list")
- parser.add_argument("--force-include", type=str, action="append", help="Force include this and its dependencies. Even if listed in excludes.")
- parser.add_argument("--arch", type=str, default="amd64", help="Architecture to download files for.")
- parser.add_argument("--apt-dir", type=str, help=" ".join([
- "File to generate and store apt files in.",
- "Helpful for saving time when downloading multiple groups of packages.",
- "Some flags will be ignored in favor of the values used to create this folder, so be careful.",
- ]))
- parser.add_argument("--release", type=str, default="bullseye", help="Debian release to use.")
- parser.add_argument("--apt-key", type=str, action="append", default=[
- "https://ftp-master.debian.org/keys/archive-key-11.asc",
- "https://ftp-master.debian.org/keys/archive-key-11-security.asc",
- ], help="URL of an additional apt archive key to trust.")
- parser.add_argument("package", nargs="+", help="The packages to download.")
- args = parser.parse_args(argv[1:])
- if args.apt_dir:
- apt_dir = args.apt_dir
- else:
- apt_dir = tempfile.mkdtemp()
- apt_args = ["-o", "Dir=" + apt_dir, "-o", "APT::Architecture=" + args.arch]
- if not args.apt_dir:
- print("Creating apt files in %s" % apt_dir)
- initialize_apt(apt_dir, apt_args, args)
- folder = tempfile.mkdtemp()
- os.chdir(folder)
- excludes = args.exclude or []
- # Exclude common packages that don't make sense to include in everything all
- # the time.
- excludes += _ALWAYS_EXCLUDE
- download_deps(apt_args, args.package, excludes, args.force_include)
- fixup_files()
- print_file_list()
- print("Your packages are all in %s" % folder)
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--exclude",
+ "-e",
+ type=str,
+ action="append",
+ help="A package to exclude from the list")
+ parser.add_argument(
+ "--force-include",
+ type=str,
+ action="append",
+ help=
+ "Force include this and its dependencies. Even if listed in excludes.")
+ parser.add_argument("--arch",
+ type=str,
+ default="amd64",
+ help="Architecture to download files for.")
+ parser.add_argument(
+ "--apt-dir",
+ type=str,
+ help=" ".join([
+ "File to generate and store apt files in.",
+ "Helpful for saving time when downloading multiple groups of packages.",
+ "Some flags will be ignored in favor of the values used to create this folder, so be careful.",
+ ]))
+ parser.add_argument("--release",
+ type=str,
+ default="bullseye",
+ help="Debian release to use.")
+ parser.add_argument(
+ "--apt-key",
+ type=str,
+ action="append",
+ default=[
+ "https://ftp-master.debian.org/keys/archive-key-11.asc",
+ "https://ftp-master.debian.org/keys/archive-key-11-security.asc",
+ ],
+ help="URL of an additional apt archive key to trust.")
+ parser.add_argument("package", nargs="+", help="The packages to download.")
+ args = parser.parse_args(argv[1:])
+ if args.apt_dir:
+ apt_dir = args.apt_dir
+ else:
+ apt_dir = tempfile.mkdtemp()
+ apt_args = ["-o", "Dir=" + apt_dir, "-o", "APT::Architecture=" + args.arch]
+ if not args.apt_dir:
+ print("Creating apt files in %s" % apt_dir)
+ initialize_apt(apt_dir, apt_args, args)
+ folder = tempfile.mkdtemp()
+ os.chdir(folder)
+ excludes = args.exclude or []
+ # Exclude common packages that don't make sense to include in everything all
+ # the time.
+ excludes += _ALWAYS_EXCLUDE
+ download_deps(apt_args, args.package, excludes, args.force_include)
+ fixup_files()
+ print_file_list()
+ print("Your packages are all in %s" % folder)
+
if __name__ == "__main__":
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/frc971/analysis/log_reader_test.py b/frc971/analysis/log_reader_test.py
index b41aa38..7af08e6 100644
--- a/frc971/analysis/log_reader_test.py
+++ b/frc971/analysis/log_reader_test.py
@@ -6,6 +6,7 @@
class LogReaderTest(unittest.TestCase):
+
def setUp(self):
self.reader = LogReader("external/sample_logfile/file/log.fbs")
# A list of all the channels in the logfile--this is used to confirm that
@@ -13,16 +14,14 @@
self.all_channels = [
("/aos", "aos.JoystickState"), ("/aos", "aos.RobotState"),
("/aos", "aos.timing.Report"), ("/aos", "frc971.PDPValues"),
- ("/aos",
- "frc971.wpilib.PneumaticsToLog"), ("/autonomous",
- "aos.common.actions.Status"),
+ ("/aos", "frc971.wpilib.PneumaticsToLog"),
+ ("/autonomous", "aos.common.actions.Status"),
("/autonomous", "frc971.autonomous.AutonomousMode"),
- ("/autonomous", "frc971.autonomous.Goal"), ("/camera",
- "y2019.CameraLog"),
+ ("/autonomous", "frc971.autonomous.Goal"),
+ ("/camera", "y2019.CameraLog"),
("/camera", "y2019.control_loops.drivetrain.CameraFrame"),
- ("/drivetrain",
- "frc971.IMUValues"), ("/drivetrain",
- "frc971.control_loops.drivetrain.Goal"),
+ ("/drivetrain", "frc971.IMUValues"),
+ ("/drivetrain", "frc971.control_loops.drivetrain.Goal"),
("/drivetrain",
"frc971.control_loops.drivetrain.LocalizerControl"),
("/drivetrain", "frc971.control_loops.drivetrain.Output"),
@@ -31,9 +30,8 @@
("/drivetrain", "frc971.sensors.GyroReading"),
("/drivetrain",
"y2019.control_loops.drivetrain.TargetSelectorHint"),
- ("/superstructure",
- "y2019.StatusLight"), ("/superstructure",
- "y2019.control_loops.superstructure.Goal"),
+ ("/superstructure", "y2019.StatusLight"),
+ ("/superstructure", "y2019.control_loops.superstructure.Goal"),
("/superstructure", "y2019.control_loops.superstructure.Output"),
("/superstructure", "y2019.control_loops.superstructure.Position"),
("/superstructure", "y2019.control_loops.superstructure.Status")
diff --git a/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py b/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
index 9caa9dc..0c21957 100755
--- a/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
+++ b/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
@@ -7,14 +7,14 @@
import numpy
if __name__ == '__main__':
- x = numpy.arange(-1, 1, 0.01)
+ x = numpy.arange(-1, 1, 0.01)
- for nonlin in numpy.arange(0.2, 1.0, 0.1):
- angular_range = numpy.pi * nonlin / 2.0
- newx1 = numpy.sin(x * angular_range) / numpy.sin(angular_range)
- newx2 = numpy.sin(newx1 * angular_range) / numpy.sin(angular_range)
+ for nonlin in numpy.arange(0.2, 1.0, 0.1):
+ angular_range = numpy.pi * nonlin / 2.0
+ newx1 = numpy.sin(x * angular_range) / numpy.sin(angular_range)
+ newx2 = numpy.sin(newx1 * angular_range) / numpy.sin(angular_range)
- pylab.plot(x, newx2, label="nonlin %f" % nonlin)
+ pylab.plot(x, newx2, label="nonlin %f" % nonlin)
- pylab.legend()
- pylab.show()
+ pylab.legend()
+ pylab.show()
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index df6c0f6..6772e71 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -9,6 +9,7 @@
class AngularSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -20,7 +21,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
- radius = None,
+ radius=None,
dt=0.00505):
"""Constructs an AngularSystemParams object.
@@ -44,6 +45,7 @@
class AngularSystem(control_loop.ControlLoop):
+
def __init__(self, params, name="AngularSystem"):
super(AngularSystem, self).__init__(name)
self.params = params
@@ -61,8 +63,8 @@
# State is [position, velocity]
# Input is [Voltage]
- C1 = self.motor.Kt / (
- self.G * self.G * self.motor.resistance * self.J * self.motor.Kv)
+ C1 = self.motor.Kt / (self.G * self.G * self.motor.resistance *
+ self.J * self.motor.Kv)
C2 = self.motor.Kt / (self.G * self.J * self.motor.resistance)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -86,8 +88,9 @@
if self.params.radius is not None:
glog.debug('Stall force: %f (N)',
self.motor.stall_torque / self.G / self.params.radius)
- glog.debug('Stall force: %f (lbf)',
- self.motor.stall_torque / self.G / self.params.radius * 0.224809)
+ glog.debug(
+ 'Stall force: %f (lbf)', self.motor.stall_torque / self.G /
+ self.params.radius * 0.224809)
glog.debug('Stall acceleration: %f (rad/s^2)',
self.motor.stall_torque / self.G / self.J)
@@ -117,8 +120,11 @@
self.R = numpy.matrix([[(self.params.kalman_r_position**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.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))
@@ -131,6 +137,7 @@
class IntegralAngularSystem(AngularSystem):
+
def __init__(self, params, name="IntegralAngularSystem"):
super(IntegralAngularSystem, self).__init__(params, name=name)
@@ -153,13 +160,16 @@
self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
[0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage
- **2.0)]])
+ [0.0, 0.0,
+ (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((1, 3)))
@@ -232,10 +242,10 @@
offset_plot.append(observer.X_hat[2, 0])
x_hat_plot.append(observer.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)
+ 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.Kff * (next_goal - observer.A * goal)
@@ -252,8 +262,8 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- motor_current = (U[0, 0] - plant.X[1, 0] / plant.G / plant.motor.Kv
- ) / plant.motor.resistance
+ motor_current = (U[0, 0] - plant.X[1, 0] / plant.G /
+ plant.motor.Kv) / plant.motor.resistance
motor_current_plot.append(motor_current)
battery_current = U[0, 0] * motor_current / 12.0
battery_current_plot.append(battery_current)
@@ -281,8 +291,8 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1,
+ 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -330,15 +340,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=0.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=0.0)
def PlotKick(params, R, plant_params=None):
@@ -357,15 +366,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=2.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=2.0)
def PlotMotion(params,
@@ -391,15 +399,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=True,
- max_velocity=max_velocity,
- max_acceleration=max_acceleration)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteAngularSystem(params, plant_files, controller_files, year_namespaces):
@@ -431,8 +438,9 @@
integral_angular_systems.append(
IntegralAngularSystem(params, 'Integral' + params.name))
- loop_writer = control_loop.ControlLoopWriter(
- name, angular_systems, namespaces=year_namespaces)
+ loop_writer = control_loop.ControlLoopWriter(name,
+ angular_systems,
+ namespaces=year_namespaces)
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', angular_systems[0].G))
loop_writer.AddConstant(
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index b8d49a9..44e4f49 100755
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -1,5 +1,6 @@
#!/usr/bin/python3
import gi
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
from gi.repository import GLib
@@ -13,6 +14,7 @@
# Override the matrix of a cairo context.
class OverrideMatrix(object):
+
def __init__(self, cr, matrix):
self.cr = cr
self.matrix = matrix
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index ac527de..f8a6e9a 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -5,6 +5,7 @@
class CIM(control_loop.ControlLoop):
+
def __init__(self):
super(CIM, self).__init__("CIM")
# Stall Torque in N m
diff --git a/frc971/control_loops/python/color.py b/frc971/control_loops/python/color.py
index 5634042..a66040c 100644
--- a/frc971/control_loops/python/color.py
+++ b/frc971/control_loops/python/color.py
@@ -1,4 +1,5 @@
class Color:
+
def __init__(self, r, g, b, a=1.0):
self.r = r
self.g = g
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index e038cb2..b2d9d57 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -17,8 +17,7 @@
FieldType = namedtuple(
'Field', ['name', 'tags', 'year', 'width', 'length', 'robot', 'field_id'])
-RobotType = namedtuple(
- "Robot", ['width', 'length'])
+RobotType = namedtuple("Robot", ['width', 'length'])
GALACTIC_SEARCH = "Galactic Search"
ARED = "A Red"
@@ -31,101 +30,91 @@
BARREL = "Barrel"
Robot2019 = RobotType(width=0.65, length=0.8)
-Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
+Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
Robot2021 = Robot2020
Robot2022 = RobotType(width=0.8763, length=0.96647)
FIELDS = {
"2019 Field":
- FieldType(
- "2019 Field",
- tags=[],
- year=2019,
- width=8.258302,
- length=8.258302,
- robot=Robot2019,
- field_id="2019"),
+ FieldType("2019 Field",
+ tags=[],
+ year=2019,
+ width=8.258302,
+ length=8.258302,
+ robot=Robot2019,
+ field_id="2019"),
"2020 Field":
- FieldType(
- "2020 Field",
- tags=[],
- year=2020,
- width=15.98295,
- length=8.21055,
- robot=Robot2020,
- field_id="2020"),
+ FieldType("2020 Field",
+ tags=[],
+ year=2020,
+ width=15.98295,
+ length=8.21055,
+ robot=Robot2020,
+ field_id="2020"),
"2021 Galactic Search BRed":
- FieldType(
- "2021 Galactic Search BRed",
- tags=[GALACTIC_SEARCH, BRED],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="red_b"),
+ FieldType("2021 Galactic Search BRed",
+ tags=[GALACTIC_SEARCH, BRED],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="red_b"),
"2021 Galactic Search ARed":
- FieldType(
- "2021 Galactic Search ARed",
- tags=[GALACTIC_SEARCH, ARED],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="red_a"),
+ FieldType("2021 Galactic Search ARed",
+ tags=[GALACTIC_SEARCH, ARED],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="red_a"),
"2021 Galactic Search BBlue":
- FieldType(
- "2021 Galactic Search BBlue",
- tags=[GALACTIC_SEARCH, BBLUE],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="blue_b"),
+ FieldType("2021 Galactic Search BBlue",
+ tags=[GALACTIC_SEARCH, BBLUE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="blue_b"),
"2021 Galactic Search ABlue":
- FieldType(
- "2021 Galactic Search ABlue",
- tags=[GALACTIC_SEARCH, ABLUE],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="blue_a"),
+ FieldType("2021 Galactic Search ABlue",
+ tags=[GALACTIC_SEARCH, ABLUE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="blue_a"),
"2021 AutoNav Barrel":
- FieldType(
- "2021 AutoNav Barrel",
- tags=[AUTONAV, BARREL],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="autonav_barrel"),
+ FieldType("2021 AutoNav Barrel",
+ tags=[AUTONAV, BARREL],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="autonav_barrel"),
"2021 AutoNav Slalom":
- FieldType(
- "2021 AutoNav Slalom",
- tags=[AUTONAV, SLALOM],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="autonav_slalom"),
+ FieldType("2021 AutoNav Slalom",
+ tags=[AUTONAV, SLALOM],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="autonav_slalom"),
"2021 AutoNav Bounce":
- FieldType(
- "2021 AutoNav Bounce",
- tags=[AUTONAV, BOUNCE],
- year=2021,
- width=9.144,
- length=4.572,
- robot=Robot2021,
- field_id="autonav_bounce"),
+ FieldType("2021 AutoNav Bounce",
+ tags=[AUTONAV, BOUNCE],
+ year=2021,
+ width=9.144,
+ length=4.572,
+ robot=Robot2021,
+ field_id="autonav_bounce"),
"2022 Field":
- FieldType(
- "2022 Field",
- tags=[],
- year=2022,
- width=16.4592,
- length=8.2296,
- robot=Robot2022,
- field_id="2022"),
+ FieldType("2022 Field",
+ tags=[],
+ year=2022,
+ width=16.4592,
+ length=8.2296,
+ robot=Robot2022,
+ field_id="2022"),
}
FIELD = FIELDS["2022 Field"]
@@ -139,5 +128,6 @@
else:
return "frc971/control_loops/python/spline_jsons"
+
def inToM(i):
return (i * 0.0254)
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 1649dd2..e836d1a 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -4,6 +4,7 @@
class Constant(object):
+
def __init__(self, name, formatt, value, comment=None):
self.name = name
self.formatt = formatt
@@ -25,6 +26,7 @@
class ControlLoopWriter(object):
+
def __init__(self,
gain_schedule_name,
loops,
@@ -76,8 +78,9 @@
return self._namespaces[0]
def _HeaderGuard(self, header_file):
- return ('_'.join([namespace.upper() for namespace in self._namespaces])
- + '_' + os.path.basename(header_file).upper().replace(
+ return ('_'.join([namespace.upper()
+ for namespace in self._namespaces]) + '_' +
+ os.path.basename(header_file).upper().replace(
'.', '_').replace('/', '_') + '_')
def Write(self, header_file, cc_file):
@@ -163,17 +166,17 @@
fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
fd.write('\n')
- fd.write('%s Make%sPlant();\n\n' % (self._PlantType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sPlant();\n\n' %
+ (self._PlantType(), self._gain_schedule_name))
- fd.write('%s Make%sController();\n\n' % (self._ControllerType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' %
+ (self._ControllerType(), self._gain_schedule_name))
- fd.write('%s Make%sObserver();\n\n' % (self._ObserverType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sObserver();\n\n' %
+ (self._ObserverType(), self._gain_schedule_name))
- fd.write('%s Make%sLoop();\n\n' % (self._LoopType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sLoop();\n\n' %
+ (self._LoopType(), self._gain_schedule_name))
fd.write(self._namespace_end)
fd.write('\n\n')
@@ -182,8 +185,8 @@
def WriteCC(self, header_file_name, cc_file):
"""Writes the cc file to the file named cc_file."""
with open(cc_file, 'w') as fd:
- fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
- header_file_name))
+ fd.write('#include \"%s/%s\"\n' %
+ (os.path.join(*self._namespaces), header_file_name))
fd.write('\n')
fd.write('#include <chrono>\n')
fd.write('#include <vector>\n')
@@ -208,19 +211,20 @@
self._scalar_type))
fd.write('\n')
- fd.write('%s Make%sPlant() {\n' % (self._PlantType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sPlant() {\n' %
+ (self._PlantType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' %
(self._PlantCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
- fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
- % (index, self._PlantCoeffType(),
- self._PlantCoeffType(), loop.PlantFunction()))
+ fd.write(
+ ' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
+ loop.PlantFunction()))
fd.write(' return %s(std::move(plants));\n' % self._PlantType())
fd.write('}\n\n')
- fd.write('%s Make%sController() {\n' % (self._ControllerType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sController() {\n' %
+ (self._ControllerType(), self._gain_schedule_name))
fd.write(
' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' %
(self._ControllerCoeffType(), len(self._loops)))
@@ -233,21 +237,22 @@
self._ControllerType())
fd.write('}\n\n')
- fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
- self._gain_schedule_name))
- fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n'
- % (self._ObserverCoeffType(), len(self._loops)))
+ fd.write('%s Make%sObserver() {\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+ fd.write(
+ ' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' %
+ (self._ObserverCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(
- ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
- % (index, self._ObserverCoeffType(),
- self._ObserverCoeffType(), loop.ObserverFunction()))
- fd.write(
- ' return %s(std::move(observers));\n' % self._ObserverType())
+ ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ObserverCoeffType(),
+ self._ObserverCoeffType(), loop.ObserverFunction()))
+ fd.write(' return %s(std::move(observers));\n' %
+ self._ObserverType())
fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
- self._gain_schedule_name))
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
fd.write(
' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n'
% (self._LoopType(), self._gain_schedule_name,
@@ -259,6 +264,7 @@
class ControlLoop(object):
+
def __init__(self, name):
"""Constructs a control loop object.
@@ -291,7 +297,8 @@
self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
- self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
+ self.last_U = numpy.matrix(
+ numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -368,8 +375,8 @@
if '.' not in write_type and 'e' not in write_type:
write_type += '.0'
write_type += 'f'
- ans.append(
- ' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
+ ans.append(' %s(%d, %d) = %s;\n' %
+ (matrix_name, x, y, write_type))
return ''.join(ans)
@@ -389,8 +396,8 @@
string, The function which will create the object.
"""
ans = [
- '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
- self._name)
+ '%s Make%sPlantCoefficients() {\n' %
+ (plant_coefficient_type, self._name)
]
ans.append(self._DumpMatrix('C', self.C, scalar_type))
@@ -402,11 +409,11 @@
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
- ans.append(
- ' const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
- ans.append(
- ' return %s'
- '(A, B, C, D, U_max, U_min, dt, %s);\n' % (plant_coefficient_type, delayed_u_string))
+ ans.append(' const std::chrono::nanoseconds dt(%d);\n' %
+ (self.dt * 1e9))
+ ans.append(' return %s'
+ '(A, B, C, D, U_max, U_min, dt, %s);\n' %
+ (plant_coefficient_type, delayed_u_string))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(
self._DumpMatrix('A_continuous', self.A_continuous,
@@ -414,9 +421,10 @@
ans.append(
self._DumpMatrix('B_continuous', self.B_continuous,
scalar_type))
- ans.append(' return %s'
- '(A_continuous, B_continuous, C, D, U_max, U_min, %s);\n' %
- (plant_coefficient_type, delayed_u_string))
+ ans.append(
+ ' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min, %s);\n' %
+ (plant_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported plant type %s', plant_coefficient_type)
@@ -521,8 +529,8 @@
self._DumpMatrix('P_steady_state', self.P_steady_state,
scalar_type))
ans.append(
- ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n' %
- (observer_coefficient_type, delayed_u_string))
+ ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n'
+ % (observer_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported observer type %s',
observer_coefficient_type)
@@ -532,6 +540,7 @@
class HybridControlLoop(ControlLoop):
+
def __init__(self, name):
super(HybridControlLoop, self).__init__(name=name)
@@ -569,6 +578,7 @@
class CIM(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 2.42
@@ -581,13 +591,14 @@
# Resistance of the motor
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = (
- self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
class MiniCIM(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 1.41
@@ -600,8 +611,8 @@
# Resistance of the motor
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = (
- self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -610,6 +621,7 @@
class NMotor(object):
+
def __init__(self, motor, n):
"""Gangs together n motors."""
self.motor = motor
@@ -625,6 +637,7 @@
class Vex775Pro(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 0.71
@@ -637,8 +650,8 @@
# Resistance of the motor
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = (
- self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Motor inertia in kg m^2
@@ -659,8 +672,8 @@
# Resistance of the motor (Ohms)
self.resistance = 12.0 / self.stall_current
# Motor velocity constant (radians / (sec * volt))
- self.Kv = (
- self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
# Torque constant (N * m / A)
self.Kt = self.stall_torque / self.stall_current
# Motor inertia in kg m^2
@@ -668,6 +681,7 @@
class MN3510(object):
+
def __init__(self):
# http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
# Free Current in Amps
@@ -702,8 +716,8 @@
# 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 / (12.0 - self.resistance * self.free_current))
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Motor inertia in kg m^2
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 1dfd061..4cb10a2 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -33,8 +33,8 @@
Returns:
numpy.matrix(m x n), K
"""
- return scipy.signal.place_poles(
- A=A, B=B, poles=numpy.array(poles)).gain_matrix
+ return scipy.signal.place_poles(A=A, B=B,
+ poles=numpy.array(poles)).gain_matrix
def c2d(A, B, dt):
@@ -48,8 +48,8 @@
em_upper = numpy.hstack((a, b))
# Need to stack zeros under the a and b matrices
- em_lower = numpy.hstack((numpy.zeros((b.shape[1], a.shape[0])),
- numpy.zeros((b.shape[1], b.shape[1]))))
+ em_lower = numpy.hstack((numpy.zeros(
+ (b.shape[1], a.shape[0])), numpy.zeros((b.shape[1], b.shape[1]))))
em = numpy.vstack((em_upper, em_lower))
ms = scipy.linalg.expm(dt * em)
@@ -164,8 +164,8 @@
axis=0)
phi = numpy.matrix(scipy.linalg.expm(M * dt))
phi12 = phi[0:number_of_states, number_of_states:(2 * number_of_states)]
- phi22 = phi[number_of_states:2 * number_of_states, number_of_states:2 *
- number_of_states]
+ phi22 = phi[number_of_states:2 * number_of_states,
+ number_of_states:2 * number_of_states]
Q_discrete = phi22.T * phi12
Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
R_discrete = R_continuous / dt
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 84cdfa9..956a2b3 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -18,6 +18,7 @@
class DownEstimator(control_loop.ControlLoop):
+
def __init__(self, name='DownEstimator'):
super(DownEstimator, self).__init__(name)
self.dt = 0.005
@@ -47,8 +48,11 @@
self.U_min = numpy.matrix([[-numpy.pi]])
self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ 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
@@ -93,8 +97,8 @@
angle = math.pi / 2
velocity = 1
for i in range(100):
- measured_velocity = velocity + (
- random.random() - 0.5) * 0.01 + 0.05
+ measured_velocity = velocity + (random.random() -
+ 0.5) * 0.01 + 0.05
estimator.Predict(measured_velocity)
estimator.Update(
math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
@@ -115,8 +119,9 @@
glog.error("Expected .h file name and .cc file name")
else:
namespaces = ['frc971', 'control_loops', 'drivetrain']
- kf_loop_writer = control_loop.ControlLoopWriter(
- "DownEstimator", [estimator], namespaces=namespaces)
+ kf_loop_writer = control_loop.ControlLoopWriter("DownEstimator",
+ [estimator],
+ namespaces=namespaces)
kf_loop_writer.Write(argv[1], argv[2])
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index 644e449..431abd7 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -58,7 +58,8 @@
cr.show_text(text)
cr.scale(widthb, -heightb)
+
def draw_points(cr, p, size):
for i in range(0, len(p)):
- draw_px_cross(cr, p[i][0], p[i][1], size, Color(
- 0, np.sqrt(0.2 * i), 0))
+ draw_px_cross(cr, p[i][0], p[i][1], size,
+ Color(0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 264b4a6..80a4a53 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -9,6 +9,7 @@
class DrivetrainParams(object):
+
def __init__(self,
J,
mass,
@@ -109,6 +110,7 @@
class Drivetrain(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
name="Drivetrain",
@@ -192,8 +194,8 @@
# Resistance of the motor, divided by the number of motors.
# Motor velocity constant
- self.Kv = (
- self.free_speed / (12.0 - self.resistance * self.free_current))
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -206,10 +208,10 @@
self.msnr = self.robot_radius_l / ( self.robot_radius_r * self.mass ) - \
self.robot_radius_l * self.robot_radius_r / 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.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)
@@ -268,6 +270,7 @@
class KFDrivetrain(Drivetrain):
+
def __init__(self,
drivetrain_params,
name="KFDrivetrain",
@@ -323,31 +326,36 @@
self.B_continuous, self.dt)
if self.has_imu:
- self.C = numpy.matrix(
- [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
- [
- 0, -0.5 / drivetrain_params.robot_radius, 0,
- 0.5 / drivetrain_params.robot_radius, 0, 0, 0
- ], [0, 0, 0, 0, 0, 0, 0]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [
+ 0,
+ -0.5 / drivetrain_params.robot_radius,
+ 0, 0.5 / drivetrain_params.robot_radius,
+ 0, 0, 0
+ ], [0, 0, 0, 0, 0, 0, 0]])
gravity = 9.8
self.C[3, 0:6] = 0.5 * (self.A_continuous[1, 0:6] +
self.A_continuous[3, 0:6]) / gravity
- self.D = numpy.matrix(
- [[0, 0], [0, 0], [0, 0],
- [
- 0.5 * (self.B_continuous[1, 0] + self.B_continuous[3, 0])
- / gravity,
- 0.5 * (self.B_continuous[1, 1] + self.B_continuous[3, 1])
- / gravity
- ]])
+ self.D = numpy.matrix([
+ [0, 0], [0, 0], [0, 0],
+ [
+ 0.5 * (self.B_continuous[1, 0] + self.B_continuous[3, 0]) /
+ gravity,
+ 0.5 * (self.B_continuous[1, 1] + self.B_continuous[3, 1]) /
+ gravity
+ ]
+ ])
else:
- self.C = numpy.matrix(
- [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
- [
- 0, -0.5 / drivetrain_params.robot_radius, 0,
- 0.5 / drivetrain_params.robot_radius, 0, 0, 0
- ]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [
+ 0,
+ -0.5 / drivetrain_params.robot_radius,
+ 0, 0.5 / drivetrain_params.robot_radius,
+ 0, 0, 0
+ ]])
self.D = numpy.matrix([[0, 0], [0, 0], [0, 0]])
@@ -378,14 +386,17 @@
[0.0, 0.0, (r_gyro**2.0)]])
# Solving for kf gains.
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
# If we don't have an IMU, pad various matrices with zeros so that
# we can still have 4 measurement outputs.
if not self.has_imu:
- self.KalmanGain = numpy.hstack((self.KalmanGain,
- numpy.matrix(numpy.zeros((7, 1)))))
+ self.KalmanGain = numpy.hstack(
+ (self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
Rtmp = numpy.zeros((4, 4))
@@ -415,8 +426,8 @@
self.Qff[2, 2] = 1.0 / qff_pos**2.0
self.Qff[3, 3] = 1.0 / qff_vel**2.0
self.Kff = numpy.matrix(numpy.zeros((2, 7)))
- self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(
- self.B[0:4, :], self.Qff)
+ self.Kff[0:2,
+ 0:4] = controls.TwoStateFeedForwards(self.B[0:4, :], self.Qff)
self.InitializeState()
@@ -428,59 +439,50 @@
scalar_type='double'):
# Write the generated constants out to a file.
- drivetrain_low_low = Drivetrain(
- name="DrivetrainLowLow",
- left_low=True,
- right_low=True,
- drivetrain_params=drivetrain_params)
- drivetrain_low_high = Drivetrain(
- name="DrivetrainLowHigh",
- left_low=True,
- right_low=False,
- drivetrain_params=drivetrain_params)
- drivetrain_high_low = Drivetrain(
- name="DrivetrainHighLow",
- left_low=False,
- right_low=True,
- drivetrain_params=drivetrain_params)
- drivetrain_high_high = Drivetrain(
- name="DrivetrainHighHigh",
- left_low=False,
- right_low=False,
- drivetrain_params=drivetrain_params)
+ drivetrain_low_low = Drivetrain(name="DrivetrainLowLow",
+ left_low=True,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh",
+ left_low=True,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ drivetrain_high_low = Drivetrain(name="DrivetrainHighLow",
+ left_low=False,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh",
+ left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
- kf_drivetrain_low_low = KFDrivetrain(
- name="KFDrivetrainLowLow",
- left_low=True,
- right_low=True,
- drivetrain_params=drivetrain_params)
- kf_drivetrain_low_high = KFDrivetrain(
- name="KFDrivetrainLowHigh",
- left_low=True,
- right_low=False,
- drivetrain_params=drivetrain_params)
- kf_drivetrain_high_low = KFDrivetrain(
- name="KFDrivetrainHighLow",
- left_low=False,
- right_low=True,
- drivetrain_params=drivetrain_params)
- kf_drivetrain_high_high = KFDrivetrain(
- name="KFDrivetrainHighHigh",
- left_low=False,
- right_low=False,
- drivetrain_params=drivetrain_params)
+ kf_drivetrain_low_low = KFDrivetrain(name="KFDrivetrainLowLow",
+ left_low=True,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_low_high = KFDrivetrain(name="KFDrivetrainLowHigh",
+ left_low=True,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_high_low = KFDrivetrain(name="KFDrivetrainHighLow",
+ left_low=False,
+ right_low=True,
+ drivetrain_params=drivetrain_params)
+ kf_drivetrain_high_high = KFDrivetrain(name="KFDrivetrainHighHigh",
+ left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
if isinstance(year_namespace, list):
namespaces = year_namespace
else:
namespaces = [year_namespace, 'control_loops', 'drivetrain']
- dog_loop_writer = control_loop.ControlLoopWriter(
- "Drivetrain", [
- drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
- drivetrain_high_high
- ],
- namespaces=namespaces,
- scalar_type=scalar_type)
+ dog_loop_writer = control_loop.ControlLoopWriter("Drivetrain", [
+ drivetrain_low_low, drivetrain_low_high, drivetrain_high_low,
+ drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
dog_loop_writer.AddConstant(
control_loop.Constant("kDt", "%f", drivetrain_low_low.dt))
dog_loop_writer.AddConstant(
@@ -522,20 +524,20 @@
dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- kf_loop_writer = control_loop.ControlLoopWriter(
- "KFDrivetrain", [
- kf_drivetrain_low_low, kf_drivetrain_low_high,
- kf_drivetrain_high_low, kf_drivetrain_high_high
- ],
- namespaces=namespaces,
- scalar_type=scalar_type)
+ kf_loop_writer = control_loop.ControlLoopWriter("KFDrivetrain", [
+ kf_drivetrain_low_low, kf_drivetrain_low_high, kf_drivetrain_high_low,
+ kf_drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
def PlotDrivetrainMotions(drivetrain_params):
# Test out the voltage error.
- drivetrain = KFDrivetrain(
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
left_power = []
@@ -562,8 +564,9 @@
pylab.show()
# Simulate the response of the system to a step input.
- drivetrain = KFDrivetrain(
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
simulated_left = []
simulated_right = []
for _ in range(100):
@@ -579,8 +582,9 @@
pylab.show()
# Simulate forwards motion.
- drivetrain = KFDrivetrain(
- left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
left_power = []
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index b85b996..178b63d 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -1,4 +1,5 @@
import gi
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
import numpy as np
@@ -14,6 +15,7 @@
class Graph(Gtk.Bin):
+
def __init__(self):
super(Graph, self).__init__()
fig = Figure(figsize=(5, 4), dpi=100)
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 7221870..99a2d60 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -22,6 +22,7 @@
class SystemParams(object):
+
def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
q_torque, current_limit):
self.J = J
@@ -45,29 +46,28 @@
#[0.25, 0.025],
-kWheel = SystemParams(
- J=0.0008,
- G=(1.25 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.20,
- q_torque=0.005,
- kP=7.0,
- kD=0.0,
- kCompensationTimeconstant=0.95,
- current_limit=4.5)
-kTrigger = SystemParams(
- J=0.00025,
- G=(0.925 * 2.0 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.1,
- q_torque=0.005,
- kP=120.0,
- kD=1.8,
- kCompensationTimeconstant=0.95,
- current_limit=3.0)
+kWheel = SystemParams(J=0.0008,
+ G=(1.25 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.20,
+ q_torque=0.005,
+ kP=7.0,
+ kD=0.0,
+ kCompensationTimeconstant=0.95,
+ current_limit=4.5)
+kTrigger = SystemParams(J=0.00025,
+ G=(0.925 * 2.0 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.1,
+ q_torque=0.005,
+ kP=120.0,
+ kD=1.8,
+ kCompensationTimeconstant=0.95,
+ current_limit=3.0)
class HapticInput(control_loop.ControlLoop):
+
def __init__(self, params=None, name='HapticInput'):
# The defaults are for the steering wheel.
super(HapticInput, self).__init__(name)
@@ -107,6 +107,7 @@
class IntegralHapticInput(HapticInput):
+
def __init__(self, params=None, name="IntegralHapticInput"):
super(IntegralHapticInput, self).__init__(name=name, params=params)
@@ -133,8 +134,11 @@
self.R = numpy.matrix([[(params.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.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.K_unaugmented = self.K
@@ -238,9 +242,8 @@
return p
haptic_observers = [
- IntegralHapticInput(params=DupParamsWithJ(params, j))
- for j in numpy.logspace(
- numpy.log10(min_J), numpy.log10(max_J), num=num_kf)
+ IntegralHapticInput(params=DupParamsWithJ(params, j)) for j in
+ numpy.logspace(numpy.log10(min_J), numpy.log10(max_J), num=num_kf)
]
# Initialize all the KF's.
haptic_observer.X_hat[1, 0] = initial_velocity
@@ -292,10 +295,9 @@
ax2.plot(data_time, data_request_current, label='request_current')
for i, kf_torque in enumerate(kf_torques):
- ax2.plot(
- data_time,
- kf_torque,
- label='-kf_torque[%f]' % haptic_observers[i].J)
+ ax2.plot(data_time,
+ kf_torque,
+ label='-kf_torque[%f]' % haptic_observers[i].J)
fig.tight_layout()
ax1.legend(loc=3)
ax2.legend(loc=4)
@@ -374,20 +376,18 @@
kind="zero")(trigger_data_time)
if FLAGS.rerun_kf:
- rerun_and_plot_kf(
- trigger_data_time,
- trigger_radians,
- trigger_current,
- resampled_trigger_request_current,
- kTrigger,
- run_correct=True)
- rerun_and_plot_kf(
- wheel_data_time,
- wheel_radians,
- wheel_current,
- resampled_wheel_request_current,
- kWheel,
- run_correct=True)
+ rerun_and_plot_kf(trigger_data_time,
+ trigger_radians,
+ trigger_current,
+ resampled_trigger_request_current,
+ kTrigger,
+ run_correct=True)
+ rerun_and_plot_kf(wheel_data_time,
+ wheel_radians,
+ wheel_current,
+ resampled_wheel_request_current,
+ kWheel,
+ run_correct=True)
else:
plot_input(trigger_data_time, trigger_radians,
trigger_velocity, trigger_torque, trigger_current,
@@ -403,17 +403,16 @@
else:
namespaces = ['frc971', 'control_loops', 'drivetrain']
for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
- ('HapticTrigger', kTrigger,
- argv[5:9])]:
+ ('HapticTrigger', kTrigger, argv[5:9])
+ ]:
haptic_input = HapticInput(params=params, name=name)
- loop_writer = control_loop.ControlLoopWriter(
- name, [haptic_input],
- namespaces=namespaces,
- scalar_type='float')
+ loop_writer = control_loop.ControlLoopWriter(name, [haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
loop_writer.Write(filenames[0], filenames[1])
- integral_haptic_input = IntegralHapticInput(
- params=params, name='Integral' + name)
+ integral_haptic_input = IntegralHapticInput(params=params,
+ name='Integral' + name)
integral_loop_writer = control_loop.ControlLoopWriter(
'Integral' + name, [integral_haptic_input],
namespaces=namespaces,
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index de44d72..7413896 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -9,6 +9,7 @@
class TestSpline(unittest.TestCase):
+
def testSimpleSpline(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
@@ -18,6 +19,7 @@
class TestDistanceSpline(unittest.TestCase):
+
def testDistanceSpline(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
@@ -27,6 +29,7 @@
class TestTrajectory(unittest.TestCase):
+
def testTrajectory(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 5.0],
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 9ba6f4f..5aded23 100755
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -46,9 +46,8 @@
assert points.shape == (2, 6)
self.__points = points
self.__spline = ct.c_void_p(
- libSpline.NewSpline(
- np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1])))
+ libSpline.NewSpline(np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1])))
def __del__(self):
libSpline.deleteSpline(self.__spline)
@@ -58,9 +57,8 @@
self.__points[1, index] = y
libSpline.deleteSpline(self.__spline)
self.__spline = ct.c_void_p(
- libSpline.newSpline(
- np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1])))
+ libSpline.newSpline(np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1])))
def Point(self, alpha):
result = np.zeros(2)
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 0391d93..3bda4e1 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -9,6 +9,7 @@
class LinearSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -37,6 +38,7 @@
class LinearSystem(control_loop.ControlLoop):
+
def __init__(self, params, name='LinearSystem'):
super(LinearSystem, self).__init__(name)
self.params = params
@@ -56,11 +58,11 @@
# State is [position, velocity]
# Input is [Voltage]
- C1 = self.motor.Kt / (
- self.G * self.G * self.radius * self.radius * self.motor.resistance
- * self.mass * self.motor.Kv)
- C2 = self.motor.Kt / (
- self.G * self.radius * self.motor.resistance * self.mass)
+ C1 = self.motor.Kt / (self.G * self.G * self.radius * self.radius *
+ self.motor.resistance * self.mass *
+ self.motor.Kv)
+ C2 = self.motor.Kt / (self.G * self.radius * self.motor.resistance *
+ self.mass)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -109,8 +111,11 @@
self.R = numpy.matrix([[(self.params.kalman_r_position**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.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))
@@ -123,6 +128,7 @@
class IntegralLinearSystem(LinearSystem):
+
def __init__(self, params, name='IntegralLinearSystem'):
super(IntegralLinearSystem, self).__init__(params, name=name)
@@ -145,13 +151,16 @@
self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
[0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage
- **2.0)]])
+ [0.0, 0.0,
+ (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.K_unaugmented = self.K
self.K = numpy.matrix(numpy.zeros((1, 3)))
@@ -222,10 +231,10 @@
offset_plot.append(observer.X_hat[2, 0])
x_hat_plot.append(observer.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)
+ 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.Kff * (next_goal - observer.A * goal)
@@ -264,8 +273,8 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1,
+ 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -305,15 +314,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=0.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=0.0)
def PlotKick(params, R, plant_params=None):
@@ -332,15 +340,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=False,
- kick_time=1.0,
- kick_magnitude=2.0)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=False,
+ kick_time=1.0,
+ kick_magnitude=2.0)
def PlotMotion(params,
@@ -366,15 +373,14 @@
initial_X = numpy.matrix([[0.0], [0.0]])
augmented_R = numpy.matrix(numpy.zeros((3, 1)))
augmented_R[0:2, :] = R
- RunTest(
- plant,
- end_goal=augmented_R,
- controller=controller,
- observer=observer,
- duration=2.0,
- use_profile=True,
- max_velocity=max_velocity,
- max_acceleration=max_acceleration)
+ RunTest(plant,
+ end_goal=augmented_R,
+ controller=controller,
+ observer=observer,
+ duration=2.0,
+ use_profile=True,
+ max_velocity=max_velocity,
+ max_acceleration=max_acceleration)
def WriteLinearSystem(params, plant_files, controller_files, year_namespaces):
@@ -405,8 +411,9 @@
integral_linear_systems.append(
IntegralLinearSystem(params, 'Integral' + params.name))
- loop_writer = control_loop.ControlLoopWriter(
- name, linear_systems, namespaces=year_namespaces)
+ loop_writer = control_loop.ControlLoopWriter(name,
+ linear_systems,
+ namespaces=year_namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f',
linear_systems[0].motor.free_speed))
diff --git a/frc971/control_loops/python/multispline.py b/frc971/control_loops/python/multispline.py
index 1441793..5551587 100644
--- a/frc971/control_loops/python/multispline.py
+++ b/frc971/control_loops/python/multispline.py
@@ -21,6 +21,7 @@
class Multispline():
+
def __init__(self):
self.staged_points = [] # Holds all points not yet in spline
self.libsplines = [] # Formatted for libspline library usage
@@ -97,8 +98,8 @@
# the three points on either side are entirely constrained by this spline
if next_spline is not None:
- f = spline[5] # the end of the spline
- e = spline[4] # determines the heading
+ f = spline[5] # the end of the spline
+ e = spline[4] # determines the heading
d = spline[3]
if next_multispline is None:
next_spline[0] = f
@@ -106,7 +107,9 @@
next_spline[2] = d + f * 4 + e * -4
else:
if snap:
- Multispline.snapSplines(spline, next_spline, match_first_to_second=False)
+ Multispline.snapSplines(spline,
+ next_spline,
+ match_first_to_second=False)
next_spline[0] = f
next_multispline.update_lib_spline()
@@ -121,7 +124,9 @@
prev_spline[3] = c + a * 4 + b * -4
else:
if snap:
- Multispline.snapSplines(prev_spline, spline, match_first_to_second=True)
+ Multispline.snapSplines(prev_spline,
+ spline,
+ match_first_to_second=True)
prev_spline[5] = a
prev_multispline.update_lib_spline()
@@ -189,7 +194,6 @@
spline[index_of_edit] = mouse
-
# all three points move together with the endpoint
if index_of_edit == 5:
spline[3] += diffs
@@ -246,6 +250,7 @@
Returns the closest multispline and the distance along that multispline
"""
+
def distance(t, distance_spline, point):
return np.sum((distance_spline.XY(t) - point)**2)
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 2210f9d..b66a668 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -6,6 +6,7 @@
from graph import Graph
import gi
import numpy as np
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
@@ -29,6 +30,7 @@
class FieldWidget(Gtk.DrawingArea):
"""Create a GTK+ widget on which we will draw using Cairo"""
+
def __init__(self):
super(FieldWidget, self).__init__()
self.set_field(FIELD)
@@ -192,7 +194,8 @@
):
self.draw_splines(cr)
- for i, points in enumerate(self.active_multispline.getSplines()):
+ for i, points in enumerate(
+ self.active_multispline.getSplines()):
points = [np.array([x, y]) for (x, y) in points]
draw_control_points(cr,
points,
@@ -432,7 +435,6 @@
# Move nearest to clicked
cur_p = [self.mousex, self.mousey]
-
# Get the distance between each for x and y
# Save the index of the point closest
nearest = 0.4 # Max distance away a the selected point can be in meters
@@ -452,9 +454,11 @@
index_multisplines, index_splines,
index_points)
- multispline, result = Multispline.nearest_distance(self.multisplines, cur_p)
+ multispline, result = Multispline.nearest_distance(
+ self.multisplines, cur_p)
if result and result.fun < 0.1:
- self.active_multispline_index = self.multisplines.index(multispline)
+ self.active_multispline_index = self.multisplines.index(
+ multispline)
self.queue_draw()
@@ -468,8 +472,7 @@
self.control_point_index.multispline_index]
multispline.updates_for_mouse_move(self.multisplines,
- self.control_point_index,
- mouse)
+ self.control_point_index, mouse)
multispline.update_lib_spline()
self.graph.schedule_recalculate(self.multisplines)
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 39fa33d..a45d738 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -98,6 +98,7 @@
class VelocityDrivetrainModel(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
left_low=True,
@@ -109,25 +110,27 @@
right_low=right_low,
drivetrain_params=drivetrain_params)
self.dt = drivetrain_params.dt
- self.A_continuous = numpy.matrix(
- [[
- self._drivetrain.A_continuous[1, 1],
- self._drivetrain.A_continuous[1, 3]
- ],
- [
- self._drivetrain.A_continuous[3, 1],
- self._drivetrain.A_continuous[3, 3]
- ]])
+ self.A_continuous = numpy.matrix([[
+ self._drivetrain.A_continuous[1, 1],
+ self._drivetrain.A_continuous[1, 3]
+ ],
+ [
+ self._drivetrain.A_continuous[3,
+ 1],
+ self._drivetrain.A_continuous[3,
+ 3]
+ ]])
- self.B_continuous = numpy.matrix(
- [[
- self._drivetrain.B_continuous[1, 0],
- self._drivetrain.B_continuous[1, 1]
- ],
- [
- self._drivetrain.B_continuous[3, 0],
- self._drivetrain.B_continuous[3, 1]
- ]])
+ self.B_continuous = numpy.matrix([[
+ self._drivetrain.B_continuous[1, 0],
+ self._drivetrain.B_continuous[1, 1]
+ ],
+ [
+ self._drivetrain.B_continuous[3,
+ 0],
+ self._drivetrain.B_continuous[3,
+ 1]
+ ]])
self.C = numpy.matrix(numpy.eye(2))
self.D = numpy.matrix(numpy.zeros((2, 2)))
@@ -141,20 +144,22 @@
# Build a kalman filter for the velocity. We don't care what the gains
# are, but the hybrid kalman filter that we want to write to disk to get
# access to A_continuous and B_continuous needs this for completeness.
- self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0, (0.5
- **2.0)]])
- self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0, (0.1
- **2.0)]])
+ self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0,
+ (0.5**2.0)]])
+ self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0,
+ (0.1**2.0)]])
self.PlaceObserverPoles(drivetrain_params.observer_poles)
- _, _, 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.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.KalmanGain, self.P_steady_state = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
@@ -277,12 +282,12 @@
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_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:
@@ -357,19 +362,18 @@
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")
+ 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])
+ 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(
@@ -422,23 +426,23 @@
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
+ 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])
+ 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])
+ 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,
@@ -473,19 +477,18 @@
drivetrain_params,
scalar_type='double'):
vdrivetrain = VelocityDrivetrain(drivetrain_params)
- hybrid_vdrivetrain = VelocityDrivetrain(
- drivetrain_params, name="HybridVelocityDrivetrain")
+ hybrid_vdrivetrain = VelocityDrivetrain(drivetrain_params,
+ name="HybridVelocityDrivetrain")
if isinstance(year_namespace, list):
namespaces = year_namespace
else:
namespaces = [year_namespace, '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,
- scalar_type=scalar_type)
+ 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,
+ scalar_type=scalar_type)
dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
@@ -503,8 +506,8 @@
hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
- cim_writer = control_loop.ControlLoopWriter(
- "CIM", [CIM()], scalar_type=scalar_type)
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()],
+ scalar_type=scalar_type)
cim_writer.Write(motor_files[0], motor_files[1])
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
index e5b8d5f..e0a8f61 100755
--- a/frc971/control_loops/python/polytope_test.py
+++ b/frc971/control_loops/python/polytope_test.py
@@ -15,6 +15,7 @@
class TestHPolytope(unittest.TestCase):
+
def setUp(self):
"""Builds a simple box polytope."""
self.H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
@@ -43,14 +44,12 @@
]
for inside_point in inside_points:
- self.assertTrue(
- self.p.IsInside(inside_point),
- msg='Point is' + str(inside_point))
+ self.assertTrue(self.p.IsInside(inside_point),
+ msg='Point is' + str(inside_point))
for outside_point in outside_points:
- self.assertFalse(
- self.p.IsInside(outside_point),
- msg='Point is' + str(outside_point))
+ self.assertFalse(self.p.IsInside(outside_point),
+ msg='Point is' + str(outside_point))
def AreVertices(self, p, vertices):
"""Checks that all the vertices are on corners of the set."""
@@ -80,10 +79,10 @@
found_points.add(actual_index)
break
- self.assertEqual(
- len(found_points),
- actual.shape[0],
- msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
+ self.assertEqual(len(found_points),
+ actual.shape[0],
+ msg="Expected:\n" + str(expected) + "\nActual:\n" +
+ str(actual))
def test_Skewed_Nonsym_Vertices(self):
"""Tests the vertices of a severely skewed space."""
diff --git a/frc971/control_loops/python/spline_drawing.py b/frc971/control_loops/python/spline_drawing.py
index 4e8b04b..7bfae29 100644
--- a/frc971/control_loops/python/spline_drawing.py
+++ b/frc971/control_loops/python/spline_drawing.py
@@ -3,6 +3,7 @@
import numpy as np
from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+
WIDTH_OF_FIELD_IN_METERS = 8.258302
PIXELS_ON_SCREEN = 800
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index fad635f..b7bc5ab 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -6,6 +6,7 @@
from path_edit import FieldWidget
from basic_window import RunApp
from constants import FIELDS, FIELD, SCREEN_SIZE
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
@@ -14,7 +15,9 @@
class GridWindow(Gtk.Window):
+
def method_connect(self, event, cb):
+
def handler(obj, *args):
cb(*args)
diff --git a/frc971/control_loops/python/spline_writer.py b/frc971/control_loops/python/spline_writer.py
index df622f3..a3d27cc 100644
--- a/frc971/control_loops/python/spline_writer.py
+++ b/frc971/control_loops/python/spline_writer.py
@@ -3,6 +3,7 @@
class SplineWriter(object):
+
def __init__(self, namespaces=None, filename="auto_splines.cc"):
if namespaces:
self._namespaces = namespaces
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index 4314845..bed72b9 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -31,19 +31,19 @@
def main(argv):
parser = argparse.ArgumentParser()
- parser.add_argument(
- "--target",
- type=str,
- default="roborio-971-frc.local",
- help="Target to deploy code to.")
- parser.add_argument(
- "--type",
- type=str,
- choices=["roborio", "pi"],
- required=True,
- help="Target type for deployment")
- parser.add_argument(
- "srcs", type=str, nargs='+', help="List of files to copy over")
+ parser.add_argument("--target",
+ type=str,
+ default="roborio-971-frc.local",
+ help="Target to deploy code to.")
+ parser.add_argument("--type",
+ type=str,
+ choices=["roborio", "pi"],
+ required=True,
+ help="Target type for deployment")
+ parser.add_argument("srcs",
+ type=str,
+ nargs='+',
+ help="List of files to copy over")
args = parser.parse_args(argv[1:])
srcs = args.srcs
@@ -52,9 +52,8 @@
result = re.match("(?:([^:@]+)@)?([^:@]+)(?::([^:@]+))?", destination)
if not result:
- print(
- "Not sure how to parse destination \"%s\"!" % destination,
- file=sys.stderr)
+ print("Not sure how to parse destination \"%s\"!" % destination,
+ file=sys.stderr)
return 1
user = None
if result.group(1):
diff --git a/frc971/vision_codelab/codelab_test.py b/frc971/vision_codelab/codelab_test.py
index e14d1cd..2ccf943 100755
--- a/frc971/vision_codelab/codelab_test.py
+++ b/frc971/vision_codelab/codelab_test.py
@@ -8,6 +8,7 @@
# TODO(milind): this should be integrated with bazel
class TestCodelab(unittest.TestCase):
+
def codelab_test(self, alliance, letter=None, img_path=None):
if img_path is None:
img_path = "%s_%s.png" % (alliance.name.lower(),
diff --git a/motors/decode_dump.py b/motors/decode_dump.py
index 27e1757..5d44694 100755
--- a/motors/decode_dump.py
+++ b/motors/decode_dump.py
@@ -12,51 +12,56 @@
data = bytes()
while len(data) < TOTAL_SIZE:
- read_now = sys.stdin.buffer.read(TOTAL_SIZE - len(data))
- if not read_now:
- print('EOF before data finished', file=sys.stderr)
- sys.exit(1)
- data += read_now
+ read_now = sys.stdin.buffer.read(TOTAL_SIZE - len(data))
+ if not read_now:
+ print('EOF before data finished', file=sys.stderr)
+ sys.exit(1)
+ data += read_now
print('%s' % len(data))
readable, _, _ = select.select([sys.stdin.buffer], [], [], 1)
if readable:
- print('Extra bytes', file=sys.stderr)
- sys.exit(1)
+ print('Extra bytes', file=sys.stderr)
+ sys.exit(1)
decoded = []
for i in range(DATAPOINTS):
- datapoint = DatapointStruct.unpack_from(data, i * DatapointStruct.size)
- decoded.append(datapoint)
+ datapoint = DatapointStruct.unpack_from(data, i * DatapointStruct.size)
+ decoded.append(datapoint)
+
def current(reading, ref):
- reading_voltage = reading / 4096 * 3.3 / 1.47 * (0.768 + 1.47)
- reading_voltage = reading / 4096 * 3.3 / 18.0 * (18.0 + 10.0)
- #reading_ref = ref / 4096 * 3.3
- reading_ref = 2.5
- #reading_ref = 0
- #return (reading_voltage - reading_ref) / 50 / 0.0003
- return (reading_voltage - reading_ref) / 0.195
+ reading_voltage = reading / 4096 * 3.3 / 1.47 * (0.768 + 1.47)
+ reading_voltage = reading / 4096 * 3.3 / 18.0 * (18.0 + 10.0)
+ #reading_ref = ref / 4096 * 3.3
+ reading_ref = 2.5
+ #reading_ref = 0
+ #return (reading_voltage - reading_ref) / 50 / 0.0003
+ return (reading_voltage - reading_ref) / 0.195
+
with open(sys.argv[1], 'w') as out:
- out.write('balanced0,balanced1,balanced2,current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n')
- #for point in decoded[2000:7200]:
- for point in decoded:
- out.write(','.join(str(d) for d in (
- current(point[0], point[6]),
- current(point[1], point[6]),
- current(point[2], point[6]),
- current(point[3], point[6]),
- current(point[4], point[6]),
- current(point[5], point[6]),
- current(point[6], point[6]),
- current(point[7], point[6]),
- current(point[8], point[6]),
- #point[6] / 100.0,
- #point[7] / 100.0,
- #point[8] / 100.0,
- point[9] / 100.0,
- point[10] / 100.0,
- )) + '\n')
+ out.write(
+ 'balanced0,balanced1,balanced2,current0.0,current1.0,current2.0,current0.1,current1.1,current2.1,count\n'
+ )
+ #for point in decoded[2000:7200]:
+ for point in decoded:
+ out.write(','.join(
+ str(d) for d in (
+ current(point[0], point[6]),
+ current(point[1], point[6]),
+ current(point[2], point[6]),
+ current(point[3], point[6]),
+ current(point[4], point[6]),
+ current(point[5], point[6]),
+ current(point[6], point[6]),
+ current(point[7], point[6]),
+ current(point[8], point[6]),
+ #point[6] / 100.0,
+ #point[7] / 100.0,
+ #point[8] / 100.0,
+ point[9] / 100.0,
+ point[10] / 100.0,
+ )) + '\n')
print('all done')
diff --git a/motors/fet12/calib_sensors.py b/motors/fet12/calib_sensors.py
index 7d882de..16c0ef7 100755
--- a/motors/fet12/calib_sensors.py
+++ b/motors/fet12/calib_sensors.py
@@ -6,8 +6,9 @@
# calib_data_60*.csv has each output channel set at a constant value of 60.
# calib_data_6030*.csv actuates two channels.
+
def calibrate(fnames):
- """Do fitting to calibrate ADC data given csv files.
+ """Do fitting to calibrate ADC data given csv files.
CSVs should be of format:
command_a, command_b, command_c, reading0, reading1, reading2
@@ -17,24 +18,25 @@
only care about the averaged samples because otherwise the solution matrix
can't be solved for in a stable manner.
"""
- data = np.zeros((1, 6))
- for fname in fnames:
- data = np.vstack((data, np.genfromtxt(fname, delimiter=',')))
- data = data[1:, :]
+ data = np.zeros((1, 6))
+ for fname in fnames:
+ data = np.vstack((data, np.genfromtxt(fname, delimiter=',')))
+ data = data[1:, :]
- data = data[:, :6]
+ data = data[:, :6]
- b = data[:, 0:3]
- b = b - np.tile(np.mean(b, axis=1), (3, 1)).T
- # Vcc / 3000 / R
- # 3000 converts duty cycle in FTM ticks to fraction of full.
- b *= 20.9 / 3000.0 / 0.0079
- A = data[:, 3:]
+ b = data[:, 0:3]
+ b = b - np.tile(np.mean(b, axis=1), (3, 1)).T
+ # Vcc / 3000 / R
+ # 3000 converts duty cycle in FTM ticks to fraction of full.
+ b *= 20.9 / 3000.0 / 0.0079
+ A = data[:, 3:]
- return np.linalg.lstsq(A, b[:])[0].T
+ return np.linalg.lstsq(A, b[:])[0].T
+
if __name__ == "__main__":
- if len(sys.argv) < 2:
- print("Need filenames for data")
- sys.exit(1)
- print(calibrate(sys.argv[1:]))
+ if len(sys.argv) < 2:
+ print("Need filenames for data")
+ sys.exit(1)
+ print(calibrate(sys.argv[1:]))
diff --git a/motors/fet12/current_equalize.py b/motors/fet12/current_equalize.py
index ea44916..114d53f 100755
--- a/motors/fet12/current_equalize.py
+++ b/motors/fet12/current_equalize.py
@@ -4,6 +4,7 @@
import sys
import calib_sensors
+
def manual_calibrate():
# 38 27 -84
# 36 -64 39
@@ -20,11 +21,12 @@
transform = I * numpy.linalg.inv(Is)
return transform
+
def main():
transform = manual_calibrate()
if len(sys.argv) > 1:
- transform = calib_sensors.calibrate(sys.argv[1:])
+ transform = calib_sensors.calibrate(sys.argv[1:])
print("#ifndef MOTORS_FET12_CURRENT_MATRIX_")
print("#define MOTORS_FET12_CURRENT_MATRIX_")
@@ -35,7 +37,8 @@
print("namespace motors {")
print("")
print(
- "inline ::std::array<float, 3> DecoupleCurrents(int16_t currents[3]) {")
+ "inline ::std::array<float, 3> DecoupleCurrents(int16_t currents[3]) {"
+ )
print(" ::std::array<float, 3> ans;")
for i in range(3):
@@ -54,5 +57,6 @@
return 0
+
if __name__ == '__main__':
sys.exit(main())
diff --git a/motors/pistol_grip/generate_cogging.py b/motors/pistol_grip/generate_cogging.py
index d889064..5b0a6e6 100644
--- a/motors/pistol_grip/generate_cogging.py
+++ b/motors/pistol_grip/generate_cogging.py
@@ -5,71 +5,73 @@
# TODO(austin): Plot flag.
+
def main(argv):
- if len(argv) < 4:
- print 'Args: input output.cc struct_name'
- return 1
- data_sum = [0.0] * 4096
- data_count = [0] * 4096
- data_list_absolute = []
- data_list_current = []
+ if len(argv) < 4:
+ print 'Args: input output.cc struct_name'
+ return 1
+ data_sum = [0.0] * 4096
+ data_count = [0] * 4096
+ data_list_absolute = []
+ data_list_current = []
- with open(argv[1], 'r') as fd:
- for line in fd:
- if line.startswith('reading'):
- split_line = line.split()
- data_absolute = int(split_line[1])
- data_index = int(split_line[3][2:])
- data_current = int(split_line[2]) / 10000.0
- data_sum[data_index] += data_current
- data_count[data_index] += 1
- data_list_absolute.append(data_absolute)
- data_list_current.append(data_current)
- data = [0.0] * 4096
- min_zero = 4096
- max_zero = 0
- for i in range(0, 4096):
- if data_count[i] == 0:
- min_zero = min(i, min_zero)
- max_zero = max(i, min_zero)
-
- for i in range(0, 4096):
- if data_count[i] != 0:
- data[i] = data_sum[i] / data_count[i]
- if min_zero == 0 and max_zero == 4095:
+ with open(argv[1], 'r') as fd:
+ for line in fd:
+ if line.startswith('reading'):
+ split_line = line.split()
+ data_absolute = int(split_line[1])
+ data_index = int(split_line[3][2:])
+ data_current = int(split_line[2]) / 10000.0
+ data_sum[data_index] += data_current
+ data_count[data_index] += 1
+ data_list_absolute.append(data_absolute)
+ data_list_current.append(data_current)
+ data = [0.0] * 4096
+ min_zero = 4096
+ max_zero = 0
for i in range(0, 4096):
- if data_count[i] != 0:
- while i > 0:
- data[i - 1] = data[i]
- i -= 1
- break;
+ if data_count[i] == 0:
+ min_zero = min(i, min_zero)
+ max_zero = max(i, min_zero)
- for i in reversed(range(0, 4096)):
- if data_count[i] != 0:
- while i < 4095:
- data[i + 1] = data[i]
- i += 1
- break;
- else:
for i in range(0, 4096):
- if data_count[i] == 0:
- if i < (min_zero + max_zero) / 2:
- data[i] = data[min_zero - 1]
- else:
- data[i] = data[max_zero + 1]
+ if data_count[i] != 0:
+ data[i] = data_sum[i] / data_count[i]
+ if min_zero == 0 and max_zero == 4095:
+ for i in range(0, 4096):
+ if data_count[i] != 0:
+ while i > 0:
+ data[i - 1] = data[i]
+ i -= 1
+ break
- pylab.plot(range(0, 4096), data)
- pylab.figure()
- pylab.plot(data_list_absolute, data_list_current)
- pylab.show()
- with open(argv[2], 'w') as out_fd:
- out_fd.write('extern const float %s[4096];\n' % argv[3])
- out_fd.write('const float %s[4096] = {\n' % argv[3])
- for datapoint in data:
- out_fd.write(' %ff,\n' % datapoint)
- out_fd.write('};')
+ for i in reversed(range(0, 4096)):
+ if data_count[i] != 0:
+ while i < 4095:
+ data[i + 1] = data[i]
+ i += 1
+ break
+ else:
+ for i in range(0, 4096):
+ if data_count[i] == 0:
+ if i < (min_zero + max_zero) / 2:
+ data[i] = data[min_zero - 1]
+ else:
+ data[i] = data[max_zero + 1]
- return 0
+ pylab.plot(range(0, 4096), data)
+ pylab.figure()
+ pylab.plot(data_list_absolute, data_list_current)
+ pylab.show()
+ with open(argv[2], 'w') as out_fd:
+ out_fd.write('extern const float %s[4096];\n' % argv[3])
+ out_fd.write('const float %s[4096] = {\n' % argv[3])
+ for datapoint in data:
+ out_fd.write(' %ff,\n' % datapoint)
+ out_fd.write('};')
+
+ return 0
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/motors/plot.py b/motors/plot.py
index 3d43080..c5b02a6 100755
--- a/motors/plot.py
+++ b/motors/plot.py
@@ -3,9 +3,7 @@
import numpy
from matplotlib import pylab
-data = numpy.loadtxt('/tmp/jkalsdjflsd.csv',
- delimiter=',',
- skiprows=1)
+data = numpy.loadtxt('/tmp/jkalsdjflsd.csv', delimiter=',', skiprows=1)
x = range(len(data))
pylab.subplot(1, 1, 1)
diff --git a/motors/print/itm_read.py b/motors/print/itm_read.py
index d616da4..ba12b90 100755
--- a/motors/print/itm_read.py
+++ b/motors/print/itm_read.py
@@ -11,97 +11,105 @@
import os
import sys
+
def open_file_for_bytes(path):
- '''Returns a file-like object which reads bytes without buffering.'''
- # Not using `open` because it's unclear from the docs how (if it's possible at
- # all) to get something that will only do one read call and return what that
- # gets on a fifo.
- try:
- return io.FileIO(path, 'r')
- except FileNotFoundError:
- # If it wasn't found, try (once) to create it and then open again.
+ '''Returns a file-like object which reads bytes without buffering.'''
+ # Not using `open` because it's unclear from the docs how (if it's possible at
+ # all) to get something that will only do one read call and return what that
+ # gets on a fifo.
try:
- os.mkfifo(path)
- except FileExistsError:
- pass
- return io.FileIO(path, 'r')
+ return io.FileIO(path, 'r')
+ except FileNotFoundError:
+ # If it wasn't found, try (once) to create it and then open again.
+ try:
+ os.mkfifo(path)
+ except FileExistsError:
+ pass
+ return io.FileIO(path, 'r')
+
def read_bytes(path):
- '''Reads bytes from a file. This is appropriate both for regular files and
+ '''Reads bytes from a file. This is appropriate both for regular files and
fifos.
Args:
path: A path-like object to open.
Yields:
Individual bytes from the file, until hitting EOF.
'''
- with open_file_for_bytes(path) as f:
- while True:
- buf = f.read(1024)
- if not buf:
- return
- for byte in buf:
- yield byte
+ with open_file_for_bytes(path) as f:
+ while True:
+ buf = f.read(1024)
+ if not buf:
+ return
+ for byte in buf:
+ yield byte
+
def parse_packets(source):
- '''Parses a stream of bytes into packets.
+ '''Parses a stream of bytes into packets.
Args:
source: A generator of individual bytes.
Generates:
Packets as bytes objects.
'''
- try:
- while True:
- header = next(source)
- if header == 0:
- # Synchronization packets consist of a bunch of 0 bits (not necessarily
- # a whole number of bytes), followed by a 128 byte. This is for hardware
- # to synchronize on, but we're not in a position to do that, so
- # presumably those should get filtered out before getting here?
- raise 'Not sure how to handle synchronization packets'
- packet = bytearray()
- packet.append(header)
- header_size = header & 3
- if header_size == 0:
- while packet[-1] & 128 and len(packet) < 7:
- packet.append(next(source))
- else:
- if header_size == 3:
- header_size = 4
- for _ in range(header_size):
- packet.append(next(source))
- yield bytes(packet)
- except StopIteration:
- return
+ try:
+ while True:
+ header = next(source)
+ if header == 0:
+ # Synchronization packets consist of a bunch of 0 bits (not necessarily
+ # a whole number of bytes), followed by a 128 byte. This is for hardware
+ # to synchronize on, but we're not in a position to do that, so
+ # presumably those should get filtered out before getting here?
+ raise 'Not sure how to handle synchronization packets'
+ packet = bytearray()
+ packet.append(header)
+ header_size = header & 3
+ if header_size == 0:
+ while packet[-1] & 128 and len(packet) < 7:
+ packet.append(next(source))
+ else:
+ if header_size == 3:
+ header_size = 4
+ for _ in range(header_size):
+ packet.append(next(source))
+ yield bytes(packet)
+ except StopIteration:
+ return
+
class PacketParser(object):
- def __init__(self):
- self.stimulus_handlers = {}
- def register_stimulus_handler(self, port_number, handler):
- '''Registers a function to call on packets to the specified port.'''
- self.stimulus_handlers[port_number] = handler
+ def __init__(self):
+ self.stimulus_handlers = {}
- def process(self, path):
- for packet in parse_packets(read_bytes(path)):
- header = packet[0]
- header_size = header & 3
- if header_size == 0:
- # TODO(Brian): At least handle overflow packets here.
- pass
- else:
- port_number = header >> 3
- if port_number in self.stimulus_handlers:
- self.stimulus_handlers[port_number](packet[1:])
- else:
- print('Warning: unhandled stimulus port %d' % port_number,
- file=sys.stderr)
- self.stimulus_handlers[port_number] = lambda _: None
+ def register_stimulus_handler(self, port_number, handler):
+ '''Registers a function to call on packets to the specified port.'''
+ self.stimulus_handlers[port_number] = handler
+
+ def process(self, path):
+ for packet in parse_packets(read_bytes(path)):
+ header = packet[0]
+ header_size = header & 3
+ if header_size == 0:
+ # TODO(Brian): At least handle overflow packets here.
+ pass
+ else:
+ port_number = header >> 3
+ if port_number in self.stimulus_handlers:
+ self.stimulus_handlers[port_number](packet[1:])
+ else:
+ print('Warning: unhandled stimulus port %d' % port_number,
+ file=sys.stderr)
+ self.stimulus_handlers[port_number] = lambda _: None
+
if __name__ == '__main__':
- parser = PacketParser()
- def print_byte(payload):
- sys.stdout.write(payload.decode('ascii'))
- parser.register_stimulus_handler(0, print_byte)
+ parser = PacketParser()
- for path in sys.argv[1:]:
- parser.process(path)
+ def print_byte(payload):
+ sys.stdout.write(payload.decode('ascii'))
+
+ parser.register_stimulus_handler(0, print_byte)
+
+ for path in sys.argv[1:]:
+ parser.process(path)
diff --git a/motors/python/big_phase_current.py b/motors/python/big_phase_current.py
index 8cbce3f..d31fc72 100755
--- a/motors/python/big_phase_current.py
+++ b/motors/python/big_phase_current.py
@@ -57,51 +57,62 @@
#switching_pattern = 'centered front shifted'
#switching_pattern = 'anticentered'
-Vconv = numpy.matrix([[2.0, -1.0, -1.0],
- [-1.0, 2.0, -1.0],
- [-1.0, -1.0, 2.0]]) / 3.0
+Vconv = numpy.matrix([[2.0, -1.0, -1.0], [-1.0, 2.0, -1.0], [-1.0, -1.0, 2.0]
+ ]) / 3.0
+
def f_single(theta):
- return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+
def g_single(theta):
- return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+
def gdot_single(theta):
- """Derivitive of the current.
+ """Derivitive of the current.
Must be multiplied by omega externally.
"""
- return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
+ return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
-f = numpy.vectorize(f_single, otypes=(numpy.float,))
-g = numpy.vectorize(g_single, otypes=(numpy.float,))
-gdot = numpy.vectorize(gdot_single, otypes=(numpy.float,))
+
+f = numpy.vectorize(f_single, otypes=(numpy.float, ))
+g = numpy.vectorize(g_single, otypes=(numpy.float, ))
+gdot = numpy.vectorize(gdot_single, otypes=(numpy.float, ))
+
def torque(theta):
- return f(theta) * g(theta)
+ return f(theta) * g(theta)
+
def phase_a(function, theta):
- return function(theta)
+ return function(theta)
+
def phase_b(function, theta):
- return function(theta + 2 * numpy.pi / 3)
+ return function(theta + 2 * numpy.pi / 3)
+
def phase_c(function, theta):
- return function(theta + 4 * numpy.pi / 3)
+ return function(theta + 4 * numpy.pi / 3)
+
def phases(function, theta):
- return numpy.matrix([[phase_a(function, theta)],
- [phase_b(function, theta)],
- [phase_c(function, theta)]])
+ return numpy.matrix([[phase_a(function,
+ theta)], [phase_b(function, theta)],
+ [phase_c(function, theta)]])
+
def all_phases(function, theta_range):
- return (phase_a(function, theta_range) +
- phase_b(function, theta_range) +
- phase_c(function, theta_range))
+ return (phase_a(function, theta_range) + phase_b(function, theta_range) +
+ phase_c(function, theta_range))
+
theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
-one_amp_driving_voltage = R * g(theta_range) + (L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) + M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
+one_amp_driving_voltage = R * g(theta_range) + (
+ L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) +
+ M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
max_one_amp_driving_voltage = max(one_amp_driving_voltage)
@@ -111,7 +122,8 @@
print('Max BEMF', max(f(theta_range)))
print('Max current', max(g(theta_range)))
-print('Max drive voltage (one_amp_driving_voltage)', max(one_amp_driving_voltage))
+print('Max drive voltage (one_amp_driving_voltage)',
+ max(one_amp_driving_voltage))
print('one_amp_scalar', one_amp_scalar)
pylab.figure()
@@ -119,12 +131,14 @@
pylab.plot(theta_range, f(theta_range), label='bemf')
pylab.plot(theta_range, g(theta_range), label='phase_current')
pylab.plot(theta_range, torque(theta_range), label='phase_torque')
-pylab.plot(theta_range, all_phases(torque, theta_range), label='sum_torque/current')
+pylab.plot(theta_range,
+ all_phases(torque, theta_range),
+ label='sum_torque/current')
pylab.legend()
def full_sample_times(Ton, Toff, dt, n, start_time):
- """Returns n + 4 samples for the provided switching times.
+ """Returns n + 4 samples for the provided switching times.
We need the timesteps and Us to integrate.
@@ -139,235 +153,262 @@
array of [t, U matrix]
"""
- assert((Toff <= 1.0).all())
- assert((Ton <= 1.0).all())
- assert((Toff >= 0.0).all())
- assert((Ton >= 0.0).all())
+ assert ((Toff <= 1.0).all())
+ assert ((Ton <= 1.0).all())
+ assert ((Toff >= 0.0).all())
+ assert ((Ton >= 0.0).all())
- if (Ton <= Toff).all():
- on_before_off = True
- else:
- # Verify that they are all ordered correctly.
- assert(not (Ton <= Toff).any())
- on_before_off = False
-
- Toff = Toff.copy() * dt
- Toff[Toff < 100e-9] = -1.0
- Toff[Toff > dt] = dt
-
- Ton = Ton.copy() * dt
- Ton[Ton < 100e-9] = -1.0
- Ton[Ton > dt - 100e-9] = dt + 1.0
-
- result = []
- t = 0
-
- result_times = numpy.concatenate(
- (numpy.linspace(0, dt, num=n),
- numpy.reshape(numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]), (-1,)),
- numpy.reshape(numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]), (-1,))
- ))
- result_times.sort()
- assert((result_times >= 0).all())
- assert((result_times <= dt).all())
-
- for t in result_times:
- if on_before_off:
- U = numpy.matrix([[vcc], [vcc], [vcc]])
- U[t <= Ton] = 0.0
- U[Toff < t] = 0.0
+ if (Ton <= Toff).all():
+ on_before_off = True
else:
- U = numpy.matrix([[0.0], [0.0], [0.0]])
- U[t > Ton] = vcc
- U[t <= Toff] = vcc
- result.append((float(t + start_time), U.copy()))
+ # Verify that they are all ordered correctly.
+ assert (not (Ton <= Toff).any())
+ on_before_off = False
- return result
+ Toff = Toff.copy() * dt
+ Toff[Toff < 100e-9] = -1.0
+ Toff[Toff > dt] = dt
+
+ Ton = Ton.copy() * dt
+ Ton[Ton < 100e-9] = -1.0
+ Ton[Ton > dt - 100e-9] = dt + 1.0
+
+ result = []
+ t = 0
+
+ result_times = numpy.concatenate(
+ (numpy.linspace(0, dt, num=n),
+ numpy.reshape(
+ numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]),
+ (-1, )),
+ numpy.reshape(
+ numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]),
+ (-1, ))))
+ result_times.sort()
+ assert ((result_times >= 0).all())
+ assert ((result_times <= dt).all())
+
+ for t in result_times:
+ if on_before_off:
+ U = numpy.matrix([[vcc], [vcc], [vcc]])
+ U[t <= Ton] = 0.0
+ U[Toff < t] = 0.0
+ else:
+ U = numpy.matrix([[0.0], [0.0], [0.0]])
+ U[t > Ton] = vcc
+ U[t <= Toff] = vcc
+ result.append((float(t + start_time), U.copy()))
+
+ return result
+
def sample_times(T, dt, n, start_time):
- if switching_pattern == 'rear':
- T = 1.0 - T
- ans = full_sample_times(T, numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n, start_time)
- elif switching_pattern == 'centered front shifted':
- # Centered, but shifted to the beginning of the cycle.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ if switching_pattern == 'rear':
+ T = 1.0 - T
+ ans = full_sample_times(T,
+ numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n,
+ start_time)
+ elif switching_pattern == 'centered front shifted':
+ # Centered, but shifted to the beginning of the cycle.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- tn = min(Ton)[0, 0]
- Ton -= tn
- Toff -= tn
+ tn = min(Ton)[0, 0]
+ Ton -= tn
+ Toff -= tn
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'centered':
- # Centered, looks waaay better.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'centered':
+ # Centered, looks waaay better.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'anticentered':
- # Centered, looks waaay better.
- Toff = T / 2.0
- Ton = 1.0 - T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'anticentered':
+ # Centered, looks waaay better.
+ Toff = T / 2.0
+ Ton = 1.0 - T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'front':
- ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n, start_time)
- else:
- assert(False)
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'front':
+ ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n,
+ start_time)
+ else:
+ assert (False)
- return ans
+ return ans
+
class DataLogger(object):
- def __init__(self, title=None):
- self.title = title
- self.ia = []
- self.ib = []
- self.ic = []
- self.ia_goal = []
- self.ib_goal = []
- self.ic_goal = []
- self.ia_controls = []
- self.ib_controls = []
- self.ic_controls = []
- self.isensea = []
- self.isenseb = []
- self.isensec = []
- self.va = []
- self.vb = []
- self.vc = []
- self.van = []
- self.vbn = []
- self.vcn = []
+ def __init__(self, title=None):
+ self.title = title
+ self.ia = []
+ self.ib = []
+ self.ic = []
+ self.ia_goal = []
+ self.ib_goal = []
+ self.ic_goal = []
+ self.ia_controls = []
+ self.ib_controls = []
+ self.ic_controls = []
+ self.isensea = []
+ self.isenseb = []
+ self.isensec = []
- self.ea = []
- self.eb = []
- self.ec = []
+ self.va = []
+ self.vb = []
+ self.vc = []
+ self.van = []
+ self.vbn = []
+ self.vcn = []
- self.theta = []
- self.omega = []
+ self.ea = []
+ self.eb = []
+ self.ec = []
- self.i_goal = []
+ self.theta = []
+ self.omega = []
- self.time = []
- self.controls_time = []
- self.predicted_time = []
+ self.i_goal = []
- self.ia_pred = []
- self.ib_pred = []
- self.ic_pred = []
+ self.time = []
+ self.controls_time = []
+ self.predicted_time = []
- self.voltage_time = []
- self.estimated_velocity = []
- self.U_last = numpy.matrix(numpy.zeros((3, 1)))
+ self.ia_pred = []
+ self.ib_pred = []
+ self.ic_pred = []
- def log_predicted(self, current_time, p):
- self.predicted_time.append(current_time)
- self.ia_pred.append(p[0, 0])
- self.ib_pred.append(p[1, 0])
- self.ic_pred.append(p[2, 0])
+ self.voltage_time = []
+ self.estimated_velocity = []
+ self.U_last = numpy.matrix(numpy.zeros((3, 1)))
- def log_controls(self, current_time, measured_current, In, E, estimated_velocity):
- self.controls_time.append(current_time)
- self.ia_controls.append(measured_current[0, 0])
- self.ib_controls.append(measured_current[1, 0])
- self.ic_controls.append(measured_current[2, 0])
+ def log_predicted(self, current_time, p):
+ self.predicted_time.append(current_time)
+ self.ia_pred.append(p[0, 0])
+ self.ib_pred.append(p[1, 0])
+ self.ic_pred.append(p[2, 0])
- self.ea.append(E[0, 0])
- self.eb.append(E[1, 0])
- self.ec.append(E[2, 0])
+ def log_controls(self, current_time, measured_current, In, E,
+ estimated_velocity):
+ self.controls_time.append(current_time)
+ self.ia_controls.append(measured_current[0, 0])
+ self.ib_controls.append(measured_current[1, 0])
+ self.ic_controls.append(measured_current[2, 0])
- self.ia_goal.append(In[0, 0])
- self.ib_goal.append(In[1, 0])
- self.ic_goal.append(In[2, 0])
- self.estimated_velocity.append(estimated_velocity)
+ self.ea.append(E[0, 0])
+ self.eb.append(E[1, 0])
+ self.ec.append(E[2, 0])
- def log_data(self, X, U, current_time, Vn, i_goal):
- self.ia.append(X[0, 0])
- self.ib.append(X[1, 0])
- self.ic.append(X[2, 0])
+ self.ia_goal.append(In[0, 0])
+ self.ib_goal.append(In[1, 0])
+ self.ic_goal.append(In[2, 0])
+ self.estimated_velocity.append(estimated_velocity)
- self.i_goal.append(i_goal)
+ def log_data(self, X, U, current_time, Vn, i_goal):
+ self.ia.append(X[0, 0])
+ self.ib.append(X[1, 0])
+ self.ic.append(X[2, 0])
- self.isensea.append(X[5, 0])
- self.isenseb.append(X[6, 0])
- self.isensec.append(X[7, 0])
+ self.i_goal.append(i_goal)
- self.theta.append(X[3, 0])
- self.omega.append(X[4, 0])
+ self.isensea.append(X[5, 0])
+ self.isenseb.append(X[6, 0])
+ self.isensec.append(X[7, 0])
- self.time.append(current_time)
+ self.theta.append(X[3, 0])
+ self.omega.append(X[4, 0])
- self.van.append(Vn[0, 0])
- self.vbn.append(Vn[1, 0])
- self.vcn.append(Vn[2, 0])
+ self.time.append(current_time)
- if (self.U_last != U).any():
- self.va.append(self.U_last[0, 0])
- self.vb.append(self.U_last[1, 0])
- self.vc.append(self.U_last[2, 0])
- self.voltage_time.append(current_time)
+ self.van.append(Vn[0, 0])
+ self.vbn.append(Vn[1, 0])
+ self.vcn.append(Vn[2, 0])
- self.va.append(U[0, 0])
- self.vb.append(U[1, 0])
- self.vc.append(U[2, 0])
- self.voltage_time.append(current_time)
- self.U_last = U.copy()
+ if (self.U_last != U).any():
+ self.va.append(self.U_last[0, 0])
+ self.vb.append(self.U_last[1, 0])
+ self.vc.append(self.U_last[2, 0])
+ self.voltage_time.append(current_time)
- def plot(self):
- fig = pylab.figure()
- pylab.subplot(3, 1, 1)
- pylab.plot(self.controls_time, self.ia_controls, 'ro', label='ia_controls')
- pylab.plot(self.controls_time, self.ib_controls, 'go', label='ib_controls')
- pylab.plot(self.controls_time, self.ic_controls, 'bo', label='ic_controls')
- pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
- pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
- pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
+ self.va.append(U[0, 0])
+ self.vb.append(U[1, 0])
+ self.vc.append(U[2, 0])
+ self.voltage_time.append(current_time)
+ self.U_last = U.copy()
- #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
- #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
- #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
- pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
- pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
- pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
- pylab.plot(self.time, self.ia, 'r', label='ia')
- pylab.plot(self.time, self.ib, 'g', label='ib')
- pylab.plot(self.time, self.ic, 'b', label='ic')
- pylab.plot(self.time, self.i_goal, label='i_goal')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
+ def plot(self):
+ fig = pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.controls_time,
+ self.ia_controls,
+ 'ro',
+ label='ia_controls')
+ pylab.plot(self.controls_time,
+ self.ib_controls,
+ 'go',
+ label='ib_controls')
+ pylab.plot(self.controls_time,
+ self.ic_controls,
+ 'bo',
+ label='ic_controls')
+ pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
+ pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
+ pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
- pylab.subplot(3, 1, 2)
- pylab.plot(self.voltage_time, self.va, label='va')
- pylab.plot(self.voltage_time, self.vb, label='vb')
- pylab.plot(self.voltage_time, self.vc, label='vc')
- pylab.plot(self.time, self.van, label='van')
- pylab.plot(self.time, self.vbn, label='vbn')
- pylab.plot(self.time, self.vcn, label='vcn')
- pylab.plot(self.controls_time, self.ea, label='ea')
- pylab.plot(self.controls_time, self.eb, label='eb')
- pylab.plot(self.controls_time, self.ec, label='ec')
- pylab.legend()
+ #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
+ #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
+ #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
+ pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
+ pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
+ pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
+ pylab.plot(self.time, self.ia, 'r', label='ia')
+ pylab.plot(self.time, self.ib, 'g', label='ib')
+ pylab.plot(self.time, self.ic, 'b', label='ic')
+ pylab.plot(self.time, self.i_goal, label='i_goal')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.time, self.theta, label='theta')
- pylab.plot(self.time, self.omega, label='omega')
- pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.voltage_time, self.va, label='va')
+ pylab.plot(self.voltage_time, self.vb, label='vb')
+ pylab.plot(self.voltage_time, self.vc, label='vc')
+ pylab.plot(self.time, self.van, label='van')
+ pylab.plot(self.time, self.vbn, label='vbn')
+ pylab.plot(self.time, self.vcn, label='vcn')
+ pylab.plot(self.controls_time, self.ea, label='ea')
+ pylab.plot(self.controls_time, self.eb, label='eb')
+ pylab.plot(self.controls_time, self.ec, label='ec')
+ pylab.legend()
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.time, self.theta, label='theta')
+ pylab.plot(self.time, self.omega, label='omega')
+ pylab.plot(self.controls_time,
+ self.estimated_velocity,
+ label='estimated omega')
- fig = pylab.figure()
- pylab.plot(self.controls_time,
- map(operator.sub, self.ia_goal, self.ia_controls), 'r', label='ia_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ib_goal, self.ib_controls), 'g', label='ib_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ic_goal, self.ic_controls), 'b', label='ic_error')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
- pylab.show()
+ pylab.legend()
+
+ fig = pylab.figure()
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ia_goal, self.ia_controls),
+ 'r',
+ label='ia_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ib_goal, self.ib_controls),
+ 'g',
+ label='ib_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ic_goal, self.ic_controls),
+ 'b',
+ label='ic_error')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+ pylab.show()
# So, from running a bunch of math, we know the following:
@@ -400,180 +441,204 @@
# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
# B * V - inv(L_matrix) * E - A * I = I_dot
class Simulation(object):
- def __init__(self):
- self.R_matrix = numpy.matrix(numpy.eye(3)) * R
- self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
- self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
- self.A = self.L_matrix_inv * self.R_matrix
- self.B = self.L_matrix_inv * Vconv
- self.A_discrete, self.B_discrete = controls.c2d(-self.A, self.B, 1.0 / hz)
- self.B_discrete_inverse = numpy.matrix(numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- self.R_model = R * 1.0
- self.L_model = L * 1.0
- self.M_model = M * 1.0
- self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
- self.L_matrix_model = numpy.matrix([[self.L_model, self.M_model, self.M_model],
- [self.M_model, self.L_model, self.M_model],
- [self.M_model, self.M_model, self.L_model]])
- self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
- self.A_model = self.L_matrix_inv_model * self.R_matrix_model
- self.B_model = self.L_matrix_inv_model * Vconv
- self.A_discrete_model, self.B_discrete_model = \
- controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
- self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
+ def __init__(self):
+ self.R_matrix = numpy.matrix(numpy.eye(3)) * R
+ self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
+ self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
+ self.A = self.L_matrix_inv * self.R_matrix
+ self.B = self.L_matrix_inv * Vconv
+ self.A_discrete, self.B_discrete = controls.c2d(
+ -self.A, self.B, 1.0 / hz)
+ self.B_discrete_inverse = numpy.matrix(
+ numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- print('constexpr double kL = %g;' % self.L_model)
- print('constexpr double kM = %g;' % self.M_model)
- print('constexpr double kR = %g;' % self.R_model)
- print('constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[0, 0])
- print('constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[1, 0])
- print('constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[0, 0])
- print('constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[1, 0])
- print('constexpr double kOneAmpScalar = %g;' % one_amp_scalar)
- print('constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage)
- print('A_discrete', self.A_discrete)
- print('B_discrete', self.B_discrete)
- print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
- print('B_discrete_inv', self.B_discrete_inverse)
+ self.R_model = R * 1.0
+ self.L_model = L * 1.0
+ self.M_model = M * 1.0
+ self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
+ self.L_matrix_model = numpy.matrix(
+ [[self.L_model, self.M_model, self.M_model],
+ [self.M_model, self.L_model, self.M_model],
+ [self.M_model, self.M_model, self.L_model]])
+ self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
+ self.A_model = self.L_matrix_inv_model * self.R_matrix_model
+ self.B_model = self.L_matrix_inv_model * Vconv
+ self.A_discrete_model, self.B_discrete_model = \
+ controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
+ self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (
+ self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
- # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
- # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
- self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 / (R_sense1 * C_sense))
- self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0)
+ print('constexpr double kL = %g;' % self.L_model)
+ print('constexpr double kM = %g;' % self.M_model)
+ print('constexpr double kR = %g;' % self.R_model)
+ print('constexpr float kAdiscrete_diagonal = %gf;' %
+ self.A_discrete_model[0, 0])
+ print('constexpr float kAdiscrete_offdiagonal = %gf;' %
+ self.A_discrete_model[1, 0])
+ print('constexpr float kBdiscrete_inv_diagonal = %gf;' %
+ self.B_discrete_inverse_model[0, 0])
+ print('constexpr float kBdiscrete_inv_offdiagonal = %gf;' %
+ self.B_discrete_inverse_model[1, 0])
+ print('constexpr double kOneAmpScalar = %g;' % one_amp_scalar)
+ print('constexpr double kMaxOneAmpDrivingVoltage = %g;' %
+ max_one_amp_driving_voltage)
+ print('A_discrete', self.A_discrete)
+ print('B_discrete', self.B_discrete)
+ print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
+ print('B_discrete_inv', self.B_discrete_inverse)
- # ia, ib, ic, theta, omega, isensea, isenseb, isensec
- self.X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
+ # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
+ # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
+ self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 /
+ (R_sense1 * C_sense))
+ self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (
+ R_sense1 / R_sense2 + 1.0)
- self.K = 0.05 * Vconv
- print('A %s' % repr(self.A))
- print('B %s' % repr(self.B))
- print('K %s' % repr(self.K))
+ # ia, ib, ic, theta, omega, isensea, isenseb, isensec
+ self.X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0],
+ [0.0]])
- print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
- print('Poles are %s' % repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.K = 0.05 * Vconv
+ print('A %s' % repr(self.A))
+ print('B %s' % repr(self.B))
+ print('K %s' % repr(self.K))
- controllability = controls.ctrb(self.A, self.B)
- print('Rank of augmented controlability matrix. %d' % numpy.linalg.matrix_rank(
- controllability))
+ print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
+ print('Poles are %s' %
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.data_logger = DataLogger(switching_pattern)
- self.current_time = 0.0
+ controllability = controls.ctrb(self.A, self.B)
+ print('Rank of augmented controlability matrix. %d' %
+ numpy.linalg.matrix_rank(controllability))
- self.estimated_velocity = self.X[4, 0]
+ self.data_logger = DataLogger(switching_pattern)
+ self.current_time = 0.0
- def motor_diffeq(self, x, t, U):
- I = numpy.matrix(x[0:3]).T
- theta = x[3]
- omega = x[4]
- Isense = numpy.matrix(x[5:]).T
+ self.estimated_velocity = self.X[4, 0]
- dflux = phases(f_single, theta) / Kv
+ def motor_diffeq(self, x, t, U):
+ I = numpy.matrix(x[0:3]).T
+ theta = x[3]
+ omega = x[4]
+ Isense = numpy.matrix(x[5:]).T
- Xdot = numpy.matrix(numpy.zeros((8, 1)))
- di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
- torque = I.T * dflux
- Xdot[0:3, :] = di_dt
- Xdot[3, :] = omega
- Xdot[4, :] = torque / J
+ dflux = phases(f_single, theta) / Kv
- Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
- return numpy.squeeze(numpy.asarray(Xdot))
+ Xdot = numpy.matrix(numpy.zeros((8, 1)))
+ di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
+ torque = I.T * dflux
+ Xdot[0:3, :] = di_dt
+ Xdot[3, :] = omega
+ Xdot[4, :] = torque / J
- def DoControls(self, goal_current):
- theta = self.X[3, 0]
- # Use the actual angular velocity.
- omega = self.X[4, 0]
+ Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
+ return numpy.squeeze(numpy.asarray(Xdot))
- measured_current = self.X[5:, :].copy()
+ def DoControls(self, goal_current):
+ theta = self.X[3, 0]
+ # Use the actual angular velocity.
+ omega = self.X[4, 0]
- # Ok, lets now fake it.
- E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
+ measured_current = self.X[5:, :].copy()
+
+ # Ok, lets now fake it.
+ E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
- E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
+ E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])
- overall_measured_current = ((E_imag1 + E_imag2).real.T * measured_current / one_amp_scalar)[0, 0]
+ overall_measured_current = ((E_imag1 + E_imag2).real.T *
+ measured_current / one_amp_scalar)[0, 0]
- current_error = goal_current - overall_measured_current
- #print(current_error)
- self.estimated_velocity += current_error * 1.0
- omega = self.estimated_velocity
+ current_error = goal_current - overall_measured_current
+ #print(current_error)
+ self.estimated_velocity += current_error * 1.0
+ omega = self.estimated_velocity
- # Now, apply the transfer function of the inductor.
- # Use that to difference the current across the cycle.
- Icurrent = self.Ilast
- # No history:
- #Icurrent = phases(g_single, theta) * goal_current
- Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
+ # Now, apply the transfer function of the inductor.
+ # Use that to difference the current across the cycle.
+ Icurrent = self.Ilast
+ # No history:
+ #Icurrent = phases(g_single, theta) * goal_current
+ Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
- deltaI = Inext - Icurrent
+ deltaI = Inext - Icurrent
- H1 = -numpy.linalg.inv(1j * omega * self.L_matrix + self.R_matrix) * omega / Kv
- H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix + self.R_matrix) * omega / Kv
- p_imag = H1 * E_imag1 + H2 * E_imag2
- p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
- numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
- p = p_imag.real
+ H1 = -numpy.linalg.inv(1j * omega * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ p_imag = H1 * E_imag1 + H2 * E_imag2
+ p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
+ numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
+ p = p_imag.real
- # So, we now know how much the change in current is due to changes in BEMF.
- # Subtract that, and then run the stock statespace equation.
- Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete * (Icurrent - p) - p_next_imag.real)
- print('Vn_ff', Vn_ff)
- print('Inext', Inext)
- Vn = Vn_ff + self.K * (Icurrent - measured_current)
+ # So, we now know how much the change in current is due to changes in BEMF.
+ # Subtract that, and then run the stock statespace equation.
+ Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete *
+ (Icurrent - p) - p_next_imag.real)
+ print('Vn_ff', Vn_ff)
+ print('Inext', Inext)
+ Vn = Vn_ff + self.K * (Icurrent - measured_current)
- E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
- self.data_logger.log_controls(self.current_time, measured_current, Icurrent, E, self.estimated_velocity)
+ E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
+ self.data_logger.log_controls(self.current_time, measured_current,
+ Icurrent, E, self.estimated_velocity)
- self.Ilast = Inext
+ self.Ilast = Inext
- return Vn
+ return Vn
- def Simulate(self):
- start_wall_time = time.time()
- self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
- for n in range(200):
- goal_current = 10.0
- max_current = (vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- min_current = (-vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- goal_current = max(min_current, min(max_current, goal_current))
+ def Simulate(self):
+ start_wall_time = time.time()
+ self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
+ for n in range(200):
+ goal_current = 10.0
+ max_current = (
+ vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ min_current = (
+ -vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ goal_current = max(min_current, min(max_current, goal_current))
- Vn = self.DoControls(goal_current)
+ Vn = self.DoControls(goal_current)
- #Vn = numpy.matrix([[0.20], [0.0], [0.0]])
- #Vn = numpy.matrix([[0.00], [0.20], [0.0]])
- #Vn = numpy.matrix([[0.00], [0.0], [0.20]])
+ #Vn = numpy.matrix([[0.20], [0.0], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.20], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.0], [0.20]])
- # T is the fractional rate.
- T = Vn / vcc
- tn = -numpy.min(T)
- T += tn
- if (T > 1.0).any():
- T = T / numpy.max(T)
+ # T is the fractional rate.
+ T = Vn / vcc
+ tn = -numpy.min(T)
+ T += tn
+ if (T > 1.0).any():
+ T = T / numpy.max(T)
- for t, U in sample_times(T = T,
- dt = 1.0 / hz, n = 10,
- start_time = self.current_time):
- # Analog amplifier mode!
- #U = Vn
+ for t, U in sample_times(T=T,
+ dt=1.0 / hz,
+ n=10,
+ start_time=self.current_time):
+ # Analog amplifier mode!
+ #U = Vn
- self.data_logger.log_data(self.X, (U - min(U)), self.current_time, Vn, goal_current)
- t_array = numpy.array([self.current_time, t])
- self.X = numpy.matrix(scipy.integrate.odeint(
- self.motor_diffeq,
- numpy.squeeze(numpy.asarray(self.X)),
- t_array, args=(U,)))[1, :].T
+ self.data_logger.log_data(self.X, (U - min(U)),
+ self.current_time, Vn, goal_current)
+ t_array = numpy.array([self.current_time, t])
+ self.X = numpy.matrix(
+ scipy.integrate.odeint(self.motor_diffeq,
+ numpy.squeeze(numpy.asarray(
+ self.X)),
+ t_array,
+ args=(U, )))[1, :].T
- self.current_time = t
+ self.current_time = t
- print('Took %f to simulate' % (time.time() - start_wall_time))
+ print('Took %f to simulate' % (time.time() - start_wall_time))
- self.data_logger.plot()
+ self.data_logger.plot()
+
simulation = Simulation()
simulation.Simulate()
diff --git a/motors/python/haptic_phase_current.py b/motors/python/haptic_phase_current.py
index b17c514..fec909d 100755
--- a/motors/python/haptic_phase_current.py
+++ b/motors/python/haptic_phase_current.py
@@ -54,51 +54,62 @@
#switching_pattern = 'centered front shifted'
#switching_pattern = 'anticentered'
-Vconv = numpy.matrix([[2.0, -1.0, -1.0],
- [-1.0, 2.0, -1.0],
- [-1.0, -1.0, 2.0]]) / 3.0
+Vconv = numpy.matrix([[2.0, -1.0, -1.0], [-1.0, 2.0, -1.0], [-1.0, -1.0, 2.0]
+ ]) / 3.0
+
def f_single(theta):
- return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+
def g_single(theta):
- return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+ return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+
def gdot_single(theta):
- """Derivitive of the current.
+ """Derivitive of the current.
Must be multiplied by omega externally.
"""
- return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
+ return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
-f = numpy.vectorize(f_single, otypes=(numpy.float,))
-g = numpy.vectorize(g_single, otypes=(numpy.float,))
-gdot = numpy.vectorize(gdot_single, otypes=(numpy.float,))
+
+f = numpy.vectorize(f_single, otypes=(numpy.float, ))
+g = numpy.vectorize(g_single, otypes=(numpy.float, ))
+gdot = numpy.vectorize(gdot_single, otypes=(numpy.float, ))
+
def torque(theta):
- return f(theta) * g(theta)
+ return f(theta) * g(theta)
+
def phase_a(function, theta):
- return function(theta)
+ return function(theta)
+
def phase_b(function, theta):
- return function(theta + 2 * numpy.pi / 3)
+ return function(theta + 2 * numpy.pi / 3)
+
def phase_c(function, theta):
- return function(theta + 4 * numpy.pi / 3)
+ return function(theta + 4 * numpy.pi / 3)
+
def phases(function, theta):
- return numpy.matrix([[phase_a(function, theta)],
- [phase_b(function, theta)],
- [phase_c(function, theta)]])
+ return numpy.matrix([[phase_a(function,
+ theta)], [phase_b(function, theta)],
+ [phase_c(function, theta)]])
+
def all_phases(function, theta_range):
- return (phase_a(function, theta_range) +
- phase_b(function, theta_range) +
- phase_c(function, theta_range))
+ return (phase_a(function, theta_range) + phase_b(function, theta_range) +
+ phase_c(function, theta_range))
+
theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
-one_amp_driving_voltage = R * g(theta_range) + (L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) + M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
+one_amp_driving_voltage = R * g(theta_range) + (
+ L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) +
+ M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
max_one_amp_driving_voltage = max(one_amp_driving_voltage)
@@ -108,7 +119,8 @@
print 'Max BEMF', max(f(theta_range))
print 'Max current', max(g(theta_range))
-print 'Max drive voltage (one_amp_driving_voltage)', max(one_amp_driving_voltage)
+print 'Max drive voltage (one_amp_driving_voltage)', max(
+ one_amp_driving_voltage)
print 'one_amp_scalar', one_amp_scalar
pylab.figure()
@@ -116,12 +128,14 @@
pylab.plot(theta_range, f(theta_range), label='bemf')
pylab.plot(theta_range, g(theta_range), label='phase_current')
pylab.plot(theta_range, torque(theta_range), label='phase_torque')
-pylab.plot(theta_range, all_phases(torque, theta_range), label='sum_torque/current')
+pylab.plot(theta_range,
+ all_phases(torque, theta_range),
+ label='sum_torque/current')
pylab.legend()
def full_sample_times(Ton, Toff, dt, n, start_time):
- """Returns n + 4 samples for the provided switching times.
+ """Returns n + 4 samples for the provided switching times.
We need the timesteps and Us to integrate.
@@ -136,235 +150,260 @@
array of [t, U matrix]
"""
- assert((Toff <= 1.0).all())
- assert((Ton <= 1.0).all())
- assert((Toff >= 0.0).all())
- assert((Ton >= 0.0).all())
+ assert ((Toff <= 1.0).all())
+ assert ((Ton <= 1.0).all())
+ assert ((Toff >= 0.0).all())
+ assert ((Ton >= 0.0).all())
- if (Ton <= Toff).all():
- on_before_off = True
- else:
- # Verify that they are all ordered correctly.
- assert(not (Ton <= Toff).any())
- on_before_off = False
-
- Toff = Toff.copy() * dt
- Toff[Toff < 100e-9] = -1.0
- Toff[Toff > dt] = dt
-
- Ton = Ton.copy() * dt
- Ton[Ton < 100e-9] = -1.0
- Ton[Ton > dt - 100e-9] = dt + 1.0
-
- result = []
- t = 0
-
- result_times = numpy.concatenate(
- (numpy.linspace(0, dt, num=n),
- numpy.reshape(numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]), (-1,)),
- numpy.reshape(numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]), (-1,))
- ))
- result_times.sort()
- assert((result_times >= 0).all())
- assert((result_times <= dt).all())
-
- for t in result_times:
- if on_before_off:
- U = numpy.matrix([[vcc], [vcc], [vcc]])
- U[t <= Ton] = 0.0
- U[Toff < t] = 0.0
+ if (Ton <= Toff).all():
+ on_before_off = True
else:
- U = numpy.matrix([[0.0], [0.0], [0.0]])
- U[t > Ton] = vcc
- U[t <= Toff] = vcc
- result.append((float(t + start_time), U.copy()))
+ # Verify that they are all ordered correctly.
+ assert (not (Ton <= Toff).any())
+ on_before_off = False
- return result
+ Toff = Toff.copy() * dt
+ Toff[Toff < 100e-9] = -1.0
+ Toff[Toff > dt] = dt
+
+ Ton = Ton.copy() * dt
+ Ton[Ton < 100e-9] = -1.0
+ Ton[Ton > dt - 100e-9] = dt + 1.0
+
+ result = []
+ t = 0
+
+ result_times = numpy.concatenate(
+ (numpy.linspace(0, dt, num=n),
+ numpy.reshape(
+ numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]),
+ (-1, )),
+ numpy.reshape(
+ numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]),
+ (-1, ))))
+ result_times.sort()
+ assert ((result_times >= 0).all())
+ assert ((result_times <= dt).all())
+
+ for t in result_times:
+ if on_before_off:
+ U = numpy.matrix([[vcc], [vcc], [vcc]])
+ U[t <= Ton] = 0.0
+ U[Toff < t] = 0.0
+ else:
+ U = numpy.matrix([[0.0], [0.0], [0.0]])
+ U[t > Ton] = vcc
+ U[t <= Toff] = vcc
+ result.append((float(t + start_time), U.copy()))
+
+ return result
+
def sample_times(T, dt, n, start_time):
- if switching_pattern == 'rear':
- T = 1.0 - T
- ans = full_sample_times(T, numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n, start_time)
- elif switching_pattern == 'centered front shifted':
- # Centered, but shifted to the beginning of the cycle.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ if switching_pattern == 'rear':
+ T = 1.0 - T
+ ans = full_sample_times(T,
+ numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n,
+ start_time)
+ elif switching_pattern == 'centered front shifted':
+ # Centered, but shifted to the beginning of the cycle.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- tn = min(Ton)[0, 0]
- Ton -= tn
- Toff -= tn
+ tn = min(Ton)[0, 0]
+ Ton -= tn
+ Toff -= tn
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'centered':
- # Centered, looks waaay better.
- Ton = 0.5 - T / 2.0
- Toff = 0.5 + T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'centered':
+ # Centered, looks waaay better.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'anticentered':
- # Centered, looks waaay better.
- Toff = T / 2.0
- Ton = 1.0 - T / 2.0
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'anticentered':
+ # Centered, looks waaay better.
+ Toff = T / 2.0
+ Ton = 1.0 - T / 2.0
- ans = full_sample_times(Ton, Toff, dt, n, start_time)
- elif switching_pattern == 'front':
- ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n, start_time)
- else:
- assert(False)
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'front':
+ ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n,
+ start_time)
+ else:
+ assert (False)
- return ans
+ return ans
+
class DataLogger(object):
- def __init__(self, title=None):
- self.title = title
- self.ia = []
- self.ib = []
- self.ic = []
- self.ia_goal = []
- self.ib_goal = []
- self.ic_goal = []
- self.ia_controls = []
- self.ib_controls = []
- self.ic_controls = []
- self.isensea = []
- self.isenseb = []
- self.isensec = []
- self.va = []
- self.vb = []
- self.vc = []
- self.van = []
- self.vbn = []
- self.vcn = []
+ def __init__(self, title=None):
+ self.title = title
+ self.ia = []
+ self.ib = []
+ self.ic = []
+ self.ia_goal = []
+ self.ib_goal = []
+ self.ic_goal = []
+ self.ia_controls = []
+ self.ib_controls = []
+ self.ic_controls = []
+ self.isensea = []
+ self.isenseb = []
+ self.isensec = []
- self.ea = []
- self.eb = []
- self.ec = []
+ self.va = []
+ self.vb = []
+ self.vc = []
+ self.van = []
+ self.vbn = []
+ self.vcn = []
- self.theta = []
- self.omega = []
+ self.ea = []
+ self.eb = []
+ self.ec = []
- self.i_goal = []
+ self.theta = []
+ self.omega = []
- self.time = []
- self.controls_time = []
- self.predicted_time = []
+ self.i_goal = []
- self.ia_pred = []
- self.ib_pred = []
- self.ic_pred = []
+ self.time = []
+ self.controls_time = []
+ self.predicted_time = []
- self.voltage_time = []
- self.estimated_velocity = []
- self.U_last = numpy.matrix(numpy.zeros((3, 1)))
+ self.ia_pred = []
+ self.ib_pred = []
+ self.ic_pred = []
- def log_predicted(self, current_time, p):
- self.predicted_time.append(current_time)
- self.ia_pred.append(p[0, 0])
- self.ib_pred.append(p[1, 0])
- self.ic_pred.append(p[2, 0])
+ self.voltage_time = []
+ self.estimated_velocity = []
+ self.U_last = numpy.matrix(numpy.zeros((3, 1)))
- def log_controls(self, current_time, measured_current, In, E, estimated_velocity):
- self.controls_time.append(current_time)
- self.ia_controls.append(measured_current[0, 0])
- self.ib_controls.append(measured_current[1, 0])
- self.ic_controls.append(measured_current[2, 0])
+ def log_predicted(self, current_time, p):
+ self.predicted_time.append(current_time)
+ self.ia_pred.append(p[0, 0])
+ self.ib_pred.append(p[1, 0])
+ self.ic_pred.append(p[2, 0])
- self.ea.append(E[0, 0])
- self.eb.append(E[1, 0])
- self.ec.append(E[2, 0])
+ def log_controls(self, current_time, measured_current, In, E,
+ estimated_velocity):
+ self.controls_time.append(current_time)
+ self.ia_controls.append(measured_current[0, 0])
+ self.ib_controls.append(measured_current[1, 0])
+ self.ic_controls.append(measured_current[2, 0])
- self.ia_goal.append(In[0, 0])
- self.ib_goal.append(In[1, 0])
- self.ic_goal.append(In[2, 0])
- self.estimated_velocity.append(estimated_velocity)
+ self.ea.append(E[0, 0])
+ self.eb.append(E[1, 0])
+ self.ec.append(E[2, 0])
- def log_data(self, X, U, current_time, Vn, i_goal):
- self.ia.append(X[0, 0])
- self.ib.append(X[1, 0])
- self.ic.append(X[2, 0])
+ self.ia_goal.append(In[0, 0])
+ self.ib_goal.append(In[1, 0])
+ self.ic_goal.append(In[2, 0])
+ self.estimated_velocity.append(estimated_velocity)
- self.i_goal.append(i_goal)
+ def log_data(self, X, U, current_time, Vn, i_goal):
+ self.ia.append(X[0, 0])
+ self.ib.append(X[1, 0])
+ self.ic.append(X[2, 0])
- self.isensea.append(X[5, 0])
- self.isenseb.append(X[6, 0])
- self.isensec.append(X[7, 0])
+ self.i_goal.append(i_goal)
- self.theta.append(X[3, 0])
- self.omega.append(X[4, 0])
+ self.isensea.append(X[5, 0])
+ self.isenseb.append(X[6, 0])
+ self.isensec.append(X[7, 0])
- self.time.append(current_time)
+ self.theta.append(X[3, 0])
+ self.omega.append(X[4, 0])
- self.van.append(Vn[0, 0])
- self.vbn.append(Vn[1, 0])
- self.vcn.append(Vn[2, 0])
+ self.time.append(current_time)
- if (self.U_last != U).any():
- self.va.append(self.U_last[0, 0])
- self.vb.append(self.U_last[1, 0])
- self.vc.append(self.U_last[2, 0])
- self.voltage_time.append(current_time)
+ self.van.append(Vn[0, 0])
+ self.vbn.append(Vn[1, 0])
+ self.vcn.append(Vn[2, 0])
- self.va.append(U[0, 0])
- self.vb.append(U[1, 0])
- self.vc.append(U[2, 0])
- self.voltage_time.append(current_time)
- self.U_last = U.copy()
+ if (self.U_last != U).any():
+ self.va.append(self.U_last[0, 0])
+ self.vb.append(self.U_last[1, 0])
+ self.vc.append(self.U_last[2, 0])
+ self.voltage_time.append(current_time)
- def plot(self):
- fig = pylab.figure()
- pylab.subplot(3, 1, 1)
- pylab.plot(self.controls_time, self.ia_controls, 'ro', label='ia_controls')
- pylab.plot(self.controls_time, self.ib_controls, 'go', label='ib_controls')
- pylab.plot(self.controls_time, self.ic_controls, 'bo', label='ic_controls')
- pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
- pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
- pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
+ self.va.append(U[0, 0])
+ self.vb.append(U[1, 0])
+ self.vc.append(U[2, 0])
+ self.voltage_time.append(current_time)
+ self.U_last = U.copy()
- #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
- #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
- #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
- pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
- pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
- pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
- pylab.plot(self.time, self.ia, 'r', label='ia')
- pylab.plot(self.time, self.ib, 'g', label='ib')
- pylab.plot(self.time, self.ic, 'b', label='ic')
- pylab.plot(self.time, self.i_goal, label='i_goal')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
+ def plot(self):
+ fig = pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.controls_time,
+ self.ia_controls,
+ 'ro',
+ label='ia_controls')
+ pylab.plot(self.controls_time,
+ self.ib_controls,
+ 'go',
+ label='ib_controls')
+ pylab.plot(self.controls_time,
+ self.ic_controls,
+ 'bo',
+ label='ic_controls')
+ pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
+ pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
+ pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
- pylab.subplot(3, 1, 2)
- pylab.plot(self.voltage_time, self.va, label='va')
- pylab.plot(self.voltage_time, self.vb, label='vb')
- pylab.plot(self.voltage_time, self.vc, label='vc')
- pylab.plot(self.time, self.van, label='van')
- pylab.plot(self.time, self.vbn, label='vbn')
- pylab.plot(self.time, self.vcn, label='vcn')
- pylab.plot(self.controls_time, self.ea, label='ea')
- pylab.plot(self.controls_time, self.eb, label='eb')
- pylab.plot(self.controls_time, self.ec, label='ec')
- pylab.legend()
+ #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
+ #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
+ #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
+ pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
+ pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
+ pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
+ pylab.plot(self.time, self.ia, 'r', label='ia')
+ pylab.plot(self.time, self.ib, 'g', label='ib')
+ pylab.plot(self.time, self.ic, 'b', label='ic')
+ pylab.plot(self.time, self.i_goal, label='i_goal')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.time, self.theta, label='theta')
- pylab.plot(self.time, self.omega, label='omega')
- #pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.voltage_time, self.va, label='va')
+ pylab.plot(self.voltage_time, self.vb, label='vb')
+ pylab.plot(self.voltage_time, self.vc, label='vc')
+ pylab.plot(self.time, self.van, label='van')
+ pylab.plot(self.time, self.vbn, label='vbn')
+ pylab.plot(self.time, self.vcn, label='vcn')
+ pylab.plot(self.controls_time, self.ea, label='ea')
+ pylab.plot(self.controls_time, self.eb, label='eb')
+ pylab.plot(self.controls_time, self.ec, label='ec')
+ pylab.legend()
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.time, self.theta, label='theta')
+ pylab.plot(self.time, self.omega, label='omega')
+ #pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
- fig = pylab.figure()
- pylab.plot(self.controls_time,
- map(operator.sub, self.ia_goal, self.ia_controls), 'r', label='ia_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ib_goal, self.ib_controls), 'g', label='ib_error')
- pylab.plot(self.controls_time,
- map(operator.sub, self.ic_goal, self.ic_controls), 'b', label='ic_error')
- if self.title is not None:
- fig.canvas.set_window_title(self.title)
- pylab.legend()
- pylab.show()
+ pylab.legend()
+
+ fig = pylab.figure()
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ia_goal, self.ia_controls),
+ 'r',
+ label='ia_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ib_goal, self.ib_controls),
+ 'g',
+ label='ib_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ic_goal, self.ic_controls),
+ 'b',
+ label='ic_error')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+ pylab.show()
# So, from running a bunch of math, we know the following:
@@ -397,180 +436,203 @@
# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
# B * V - inv(L_matrix) * E - A * I = I_dot
class Simulation(object):
- def __init__(self):
- self.R_matrix = numpy.matrix(numpy.eye(3)) * R
- self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
- self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
- self.A = self.L_matrix_inv * self.R_matrix
- self.B = self.L_matrix_inv * Vconv
- self.A_discrete, self.B_discrete = controls.c2d(-self.A, self.B, 1.0 / hz)
- self.B_discrete_inverse = numpy.matrix(numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- self.R_model = R * 1.0
- self.L_model = L * 1.0
- self.M_model = M * 1.0
- self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
- self.L_matrix_model = numpy.matrix([[self.L_model, self.M_model, self.M_model],
- [self.M_model, self.L_model, self.M_model],
- [self.M_model, self.M_model, self.L_model]])
- self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
- self.A_model = self.L_matrix_inv_model * self.R_matrix_model
- self.B_model = self.L_matrix_inv_model * Vconv
- self.A_discrete_model, self.B_discrete_model = \
- controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
- self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
+ def __init__(self):
+ self.R_matrix = numpy.matrix(numpy.eye(3)) * R
+ self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
+ self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
+ self.A = self.L_matrix_inv * self.R_matrix
+ self.B = self.L_matrix_inv * Vconv
+ self.A_discrete, self.B_discrete = controls.c2d(
+ -self.A, self.B, 1.0 / hz)
+ self.B_discrete_inverse = numpy.matrix(
+ numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
- print 'constexpr double kL = %g;' % self.L_model
- print 'constexpr double kM = %g;' % self.M_model
- print 'constexpr double kR = %g;' % self.R_model
- print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[0, 0]
- print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[1, 0]
- print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[0, 0]
- print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[1, 0]
- print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
- print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
- print('A_discrete', self.A_discrete)
- print('B_discrete', self.B_discrete)
- print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
- print('B_discrete_inv', self.B_discrete_inverse)
+ self.R_model = R * 1.0
+ self.L_model = L * 1.0
+ self.M_model = M * 1.0
+ self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
+ self.L_matrix_model = numpy.matrix(
+ [[self.L_model, self.M_model, self.M_model],
+ [self.M_model, self.L_model, self.M_model],
+ [self.M_model, self.M_model, self.L_model]])
+ self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
+ self.A_model = self.L_matrix_inv_model * self.R_matrix_model
+ self.B_model = self.L_matrix_inv_model * Vconv
+ self.A_discrete_model, self.B_discrete_model = \
+ controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
+ self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (
+ self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
- # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
- # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
- self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 / (R_sense1 * C_sense))
- self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0)
+ print 'constexpr double kL = %g;' % self.L_model
+ print 'constexpr double kM = %g;' % self.M_model
+ print 'constexpr double kR = %g;' % self.R_model
+ print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[
+ 0, 0]
+ print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[
+ 1, 0]
+ print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[
+ 0, 0]
+ print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[
+ 1, 0]
+ print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
+ print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
+ print('A_discrete', self.A_discrete)
+ print('B_discrete', self.B_discrete)
+ print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
+ print('B_discrete_inv', self.B_discrete_inverse)
- # ia, ib, ic, theta, omega, isensea, isenseb, isensec
- self.X = numpy.matrix([[0.0], [0.0], [0.0], [-2.0 * numpy.pi / 3.0], [0.0], [0.0], [0.0], [0.0]])
+ # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
+ # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
+ self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 /
+ (R_sense1 * C_sense))
+ self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (
+ R_sense1 / R_sense2 + 1.0)
- self.K = 0.05 * Vconv
- print('A %s' % repr(self.A))
- print('B %s' % repr(self.B))
- print('K %s' % repr(self.K))
+ # ia, ib, ic, theta, omega, isensea, isenseb, isensec
+ self.X = numpy.matrix([[0.0], [0.0], [0.0], [-2.0 * numpy.pi / 3.0],
+ [0.0], [0.0], [0.0], [0.0]])
- print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
- print('Poles are %s' % repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.K = 0.05 * Vconv
+ print('A %s' % repr(self.A))
+ print('B %s' % repr(self.B))
+ print('K %s' % repr(self.K))
- controllability = controls.ctrb(self.A, self.B)
- print('Rank of augmented controlability matrix. %d' % numpy.linalg.matrix_rank(
- controllability))
+ print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
+ print('Poles are %s' %
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.data_logger = DataLogger(switching_pattern)
- self.current_time = 0.0
+ controllability = controls.ctrb(self.A, self.B)
+ print('Rank of augmented controlability matrix. %d' %
+ numpy.linalg.matrix_rank(controllability))
- self.estimated_velocity = self.X[4, 0]
+ self.data_logger = DataLogger(switching_pattern)
+ self.current_time = 0.0
- def motor_diffeq(self, x, t, U):
- I = numpy.matrix(x[0:3]).T
- theta = x[3]
- omega = x[4]
- Isense = numpy.matrix(x[5:]).T
+ self.estimated_velocity = self.X[4, 0]
- dflux = phases(f_single, theta) / Kv
+ def motor_diffeq(self, x, t, U):
+ I = numpy.matrix(x[0:3]).T
+ theta = x[3]
+ omega = x[4]
+ Isense = numpy.matrix(x[5:]).T
- Xdot = numpy.matrix(numpy.zeros((8, 1)))
- di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
- torque = I.T * dflux
- Xdot[0:3, :] = di_dt
- Xdot[3, :] = omega
- Xdot[4, :] = torque / J
+ dflux = phases(f_single, theta) / Kv
- Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
- return numpy.squeeze(numpy.asarray(Xdot))
+ Xdot = numpy.matrix(numpy.zeros((8, 1)))
+ di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
+ torque = I.T * dflux
+ Xdot[0:3, :] = di_dt
+ Xdot[3, :] = omega
+ Xdot[4, :] = torque / J
- def DoControls(self, goal_current):
- theta = self.X[3, 0]
- # Use the actual angular velocity.
- omega = self.X[4, 0]
+ Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
+ return numpy.squeeze(numpy.asarray(Xdot))
- measured_current = self.X[5:, :].copy()
+ def DoControls(self, goal_current):
+ theta = self.X[3, 0]
+ # Use the actual angular velocity.
+ omega = self.X[4, 0]
- # Ok, lets now fake it.
- E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
+ measured_current = self.X[5:, :].copy()
+
+ # Ok, lets now fake it.
+ E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
- E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
- [[-1j],
- [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
+ E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
+ [[-1j], [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
[-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])
- overall_measured_current = ((E_imag1 + E_imag2).real.T * measured_current / one_amp_scalar)[0, 0]
+ overall_measured_current = ((E_imag1 + E_imag2).real.T *
+ measured_current / one_amp_scalar)[0, 0]
- current_error = goal_current - overall_measured_current
- #print(current_error)
- self.estimated_velocity += current_error * 1.0
- omega = self.estimated_velocity
+ current_error = goal_current - overall_measured_current
+ #print(current_error)
+ self.estimated_velocity += current_error * 1.0
+ omega = self.estimated_velocity
- # Now, apply the transfer function of the inductor.
- # Use that to difference the current across the cycle.
- Icurrent = self.Ilast
- # No history:
- #Icurrent = phases(g_single, theta) * goal_current
- Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
+ # Now, apply the transfer function of the inductor.
+ # Use that to difference the current across the cycle.
+ Icurrent = self.Ilast
+ # No history:
+ #Icurrent = phases(g_single, theta) * goal_current
+ Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
- deltaI = Inext - Icurrent
+ deltaI = Inext - Icurrent
- H1 = -numpy.linalg.inv(1j * omega * self.L_matrix + self.R_matrix) * omega / Kv
- H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix + self.R_matrix) * omega / Kv
- p_imag = H1 * E_imag1 + H2 * E_imag2
- p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
- numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
- p = p_imag.real
+ H1 = -numpy.linalg.inv(1j * omega * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix +
+ self.R_matrix) * omega / Kv
+ p_imag = H1 * E_imag1 + H2 * E_imag2
+ p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
+ numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
+ p = p_imag.real
- # So, we now know how much the change in current is due to changes in BEMF.
- # Subtract that, and then run the stock statespace equation.
- Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete * (Icurrent - p) - p_next_imag.real)
- print 'Vn_ff', Vn_ff
- print 'Inext', Inext
- Vn = Vn_ff + self.K * (Icurrent - measured_current)
+ # So, we now know how much the change in current is due to changes in BEMF.
+ # Subtract that, and then run the stock statespace equation.
+ Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete *
+ (Icurrent - p) - p_next_imag.real)
+ print 'Vn_ff', Vn_ff
+ print 'Inext', Inext
+ Vn = Vn_ff + self.K * (Icurrent - measured_current)
- E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
- self.data_logger.log_controls(self.current_time, measured_current, Icurrent, E, self.estimated_velocity)
+ E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
+ self.data_logger.log_controls(self.current_time, measured_current,
+ Icurrent, E, self.estimated_velocity)
- self.Ilast = Inext
+ self.Ilast = Inext
- return Vn
+ return Vn
- def Simulate(self):
- start_wall_time = time.time()
- self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
- for n in range(200):
- goal_current = 1.0
- max_current = (vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- min_current = (-vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
- goal_current = max(min_current, min(max_current, goal_current))
+ def Simulate(self):
+ start_wall_time = time.time()
+ self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
+ for n in range(200):
+ goal_current = 1.0
+ max_current = (
+ vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ min_current = (
+ -vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ goal_current = max(min_current, min(max_current, goal_current))
- Vn = self.DoControls(goal_current)
+ Vn = self.DoControls(goal_current)
- #Vn = numpy.matrix([[1.00], [0.0], [0.0]])
- Vn = numpy.matrix([[0.00], [1.00], [0.0]])
- #Vn = numpy.matrix([[0.00], [0.0], [1.00]])
+ #Vn = numpy.matrix([[1.00], [0.0], [0.0]])
+ Vn = numpy.matrix([[0.00], [1.00], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.0], [1.00]])
- # T is the fractional rate.
- T = Vn / vcc
- tn = -numpy.min(T)
- T += tn
- if (T > 1.0).any():
- T = T / numpy.max(T)
+ # T is the fractional rate.
+ T = Vn / vcc
+ tn = -numpy.min(T)
+ T += tn
+ if (T > 1.0).any():
+ T = T / numpy.max(T)
- for t, U in sample_times(T = T,
- dt = 1.0 / hz, n = 10,
- start_time = self.current_time):
- # Analog amplifier mode!
- #U = Vn
+ for t, U in sample_times(T=T,
+ dt=1.0 / hz,
+ n=10,
+ start_time=self.current_time):
+ # Analog amplifier mode!
+ #U = Vn
- self.data_logger.log_data(self.X, (U - min(U)), self.current_time, Vn, goal_current)
- t_array = numpy.array([self.current_time, t])
- self.X = numpy.matrix(scipy.integrate.odeint(
- self.motor_diffeq,
- numpy.squeeze(numpy.asarray(self.X)),
- t_array, args=(U,)))[1, :].T
+ self.data_logger.log_data(self.X, (U - min(U)),
+ self.current_time, Vn, goal_current)
+ t_array = numpy.array([self.current_time, t])
+ self.X = numpy.matrix(
+ scipy.integrate.odeint(self.motor_diffeq,
+ numpy.squeeze(numpy.asarray(
+ self.X)),
+ t_array,
+ args=(U, )))[1, :].T
- self.current_time = t
+ self.current_time = t
- print 'Took %f to simulate' % (time.time() - start_wall_time)
+ print 'Took %f to simulate' % (time.time() - start_wall_time)
- self.data_logger.plot()
+ self.data_logger.plot()
+
simulation = Simulation()
simulation.Simulate()
diff --git a/motors/seems_reasonable/drivetrain.py b/motors/seems_reasonable/drivetrain.py
index 52b3920..ad3d92a 100644
--- a/motors/seems_reasonable/drivetrain.py
+++ b/motors/seems_reasonable/drivetrain.py
@@ -30,11 +30,10 @@
glog.error("Expected .h file name and .cc file name")
else:
# Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(
- argv[1:3],
- argv[3:5], ['motors', 'seems_reasonable'],
- kDrivetrain,
- scalar_type='float')
+ drivetrain.WriteDrivetrain(argv[1:3],
+ argv[3:5], ['motors', 'seems_reasonable'],
+ kDrivetrain,
+ scalar_type='float')
if __name__ == '__main__':
diff --git a/motors/seems_reasonable/polydrivetrain.py b/motors/seems_reasonable/polydrivetrain.py
index 452b3fb..665739f 100644
--- a/motors/seems_reasonable/polydrivetrain.py
+++ b/motors/seems_reasonable/polydrivetrain.py
@@ -16,19 +16,19 @@
except gflags.DuplicateFlagError:
pass
+
def main(argv):
if FLAGS.plot:
polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
elif len(argv) != 7:
glog.fatal('Expected .h file name and .cc file name')
else:
- polydrivetrain.WritePolyDrivetrain(
- argv[1:3],
- argv[3:5],
- argv[5:7],
- ['motors', 'seems_reasonable'],
- drivetrain.kDrivetrain,
- scalar_type='float')
+ polydrivetrain.WritePolyDrivetrain(argv[1:3],
+ argv[3:5],
+ argv[5:7],
+ ['motors', 'seems_reasonable'],
+ drivetrain.kDrivetrain,
+ scalar_type='float')
if __name__ == '__main__':
diff --git a/scouting/deploy/deploy.py b/scouting/deploy/deploy.py
index c9886fb..5967956 100644
--- a/scouting/deploy/deploy.py
+++ b/scouting/deploy/deploy.py
@@ -3,6 +3,7 @@
import subprocess
import sys
+
def main(argv):
"""Installs the scouting application on the scouting server."""
parser = argparse.ArgumentParser()
@@ -23,11 +24,16 @@
# Copy the .deb to the scouting server, install it, and delete it again.
subprocess.run(["rsync", "-L", args.deb, f"{args.host}:/tmp/{deb.name}"],
- check=True, stdin=sys.stdin)
+ check=True,
+ stdin=sys.stdin)
subprocess.run(f"ssh -tt {args.host} sudo dpkg -i /tmp/{deb.name}",
- shell=True, check=True, stdin=sys.stdin)
+ shell=True,
+ check=True,
+ stdin=sys.stdin)
subprocess.run(f"ssh {args.host} rm -f /tmp/{deb.name}",
- shell=True, check=True, stdin=sys.stdin)
+ shell=True,
+ check=True,
+ stdin=sys.stdin)
if __name__ == "__main__":
diff --git a/scouting/testing/scouting_test_servers.py b/scouting/testing/scouting_test_servers.py
index b6e5c7a..9df23c1 100644
--- a/scouting/testing/scouting_test_servers.py
+++ b/scouting/testing/scouting_test_servers.py
@@ -18,6 +18,7 @@
import time
from typing import List
+
def wait_for_server(port: int):
"""Waits for the server at the specified port to respond to TCP connections."""
while True:
@@ -30,31 +31,37 @@
connection.close()
time.sleep(0.01)
+
def create_db_config(tmpdir: Path) -> Path:
config = tmpdir / "db_config.json"
- config.write_text(json.dumps({
- "username": "test",
- "password": "password",
- "port": 5432,
- }))
+ config.write_text(
+ json.dumps({
+ "username": "test",
+ "password": "password",
+ "port": 5432,
+ }))
return config
+
def create_tba_config(tmpdir: Path) -> Path:
# Configure the scouting webserver to scrape data from our fake TBA
# server.
config = tmpdir / "scouting_config.json"
- config.write_text(json.dumps({
- "api_key": "dummy_key_that_is_not_actually_used_in_this_test",
- "base_url": "http://localhost:7000",
- }))
+ config.write_text(
+ json.dumps({
+ "api_key": "dummy_key_that_is_not_actually_used_in_this_test",
+ "base_url": "http://localhost:7000",
+ }))
return config
+
def set_up_tba_api_dir(tmpdir: Path, year: int, event_code: str):
tba_api_dir = tmpdir / "api" / "v3" / "event" / f"{year}{event_code}"
tba_api_dir.mkdir(parents=True, exist_ok=True)
(tba_api_dir / "matches").write_text(
- Path(f"scouting/scraping/test_data/{year}_{event_code}.json").read_text()
- )
+ Path(f"scouting/scraping/test_data/{year}_{event_code}.json").
+ read_text())
+
class Runner:
"""Helps manage the services we need for testing the scouting app."""
@@ -105,6 +112,7 @@
except FileNotFoundError:
pass
+
def discard_signal(signum, frame):
"""A NOP handler to ignore certain signals.
@@ -113,9 +121,12 @@
"""
pass
+
def main(argv: List[str]):
parser = argparse.ArgumentParser()
- parser.add_argument("--port", type=int, help="The port for the actual web server.")
+ parser.add_argument("--port",
+ type=int,
+ help="The port for the actual web server.")
args = parser.parse_args(argv[1:])
runner = Runner()
diff --git a/scouting/webserver/requests/debug/cli/cli_test.py b/scouting/webserver/requests/debug/cli/cli_test.py
index 2bcd75b..d4310e9 100644
--- a/scouting/webserver/requests/debug/cli/cli_test.py
+++ b/scouting/webserver/requests/debug/cli/cli_test.py
@@ -22,6 +22,7 @@
json_path.write_text(json.dumps(content))
return json_path
+
def run_debug_cli(args: List[str]):
run_result = subprocess.run(
["scouting/webserver/requests/debug/cli/cli_/cli"] + args,
@@ -51,46 +52,71 @@
"year": year,
"event_code": event_code,
})
- exit_code, stdout, stderr = run_debug_cli(["-refreshMatchList", json_path])
+ exit_code, stdout, stderr = run_debug_cli(
+ ["-refreshMatchList", json_path])
self.assertEqual(exit_code, 0, f"{year}{event_code}: {stderr}")
- self.assertIn("(refresh_match_list_response.RefreshMatchListResponseT)", stdout)
+ self.assertIn(
+ "(refresh_match_list_response.RefreshMatchListResponseT)", stdout)
def test_submit_and_request_data_scouting(self):
self.refresh_match_list(year=2020, event_code="fake")
# First submit some data to be added to the database.
json_path = write_json_request({
- "team": 100,
- "match": 1,
- "set_number": 2,
- "comp_level": "quals",
- "starting_quadrant": 3,
- "auto_ball_1": True,
- "auto_ball_2": False,
- "auto_ball_3": False,
- "auto_ball_4": False,
- "auto_ball_5": True,
- "missed_shots_auto": 10,
- "upper_goal_auto": 11,
- "lower_goal_auto": 12,
- "missed_shots_tele": 13,
- "upper_goal_tele": 14,
- "lower_goal_tele": 15,
- "defense_rating": 3,
- "defense_received_rating": 4,
- "climb_level": "Medium",
- "comment": "A very inspiring and useful comment",
+ "team":
+ 100,
+ "match":
+ 1,
+ "set_number":
+ 2,
+ "comp_level":
+ "quals",
+ "starting_quadrant":
+ 3,
+ "auto_ball_1":
+ True,
+ "auto_ball_2":
+ False,
+ "auto_ball_3":
+ False,
+ "auto_ball_4":
+ False,
+ "auto_ball_5":
+ True,
+ "missed_shots_auto":
+ 10,
+ "upper_goal_auto":
+ 11,
+ "lower_goal_auto":
+ 12,
+ "missed_shots_tele":
+ 13,
+ "upper_goal_tele":
+ 14,
+ "lower_goal_tele":
+ 15,
+ "defense_rating":
+ 3,
+ "defense_received_rating":
+ 4,
+ "climb_level":
+ "Medium",
+ "comment":
+ "A very inspiring and useful comment",
})
- exit_code, _, stderr = run_debug_cli(["-submitDataScouting", json_path])
+ exit_code, _, stderr = run_debug_cli(
+ ["-submitDataScouting", json_path])
self.assertEqual(exit_code, 0, stderr)
# Now request the data back with zero indentation. That let's us
# validate the data easily.
json_path = write_json_request({})
- exit_code, stdout, stderr = run_debug_cli(["-requestDataScouting", json_path, "-indent="])
+ exit_code, stdout, stderr = run_debug_cli(
+ ["-requestDataScouting", json_path, "-indent="])
self.assertEqual(exit_code, 0, stderr)
- self.assertIn(textwrap.dedent("""\
+ self.assertIn(
+ textwrap.dedent("""\
{
Team: (int32) 100,
Match: (int32) 1,
@@ -120,10 +146,13 @@
# RequestAllMatches has no fields.
json_path = write_json_request({})
- exit_code, stdout, stderr = run_debug_cli(["-requestAllMatches", json_path])
+ exit_code, stdout, stderr = run_debug_cli(
+ ["-requestAllMatches", json_path])
self.assertEqual(exit_code, 0, stderr)
- self.assertIn("MatchList: ([]*request_all_matches_response.MatchT) (len=90 cap=90) {", stdout)
+ self.assertIn(
+ "MatchList: ([]*request_all_matches_response.MatchT) (len=90 cap=90) {",
+ stdout)
self.assertEqual(stdout.count("MatchNumber:"), 90)
def test_request_matches_for_team(self):
@@ -132,11 +161,14 @@
json_path = write_json_request({
"team": 4856,
})
- exit_code, stdout, stderr = run_debug_cli(["-requestMatchesForTeam", json_path])
+ exit_code, stdout, stderr = run_debug_cli(
+ ["-requestMatchesForTeam", json_path])
# Team 4856 has 12 matches.
self.assertEqual(exit_code, 0, stderr)
- self.assertIn("MatchList: ([]*request_matches_for_team_response.MatchT) (len=12 cap=12) {", stdout)
+ self.assertIn(
+ "MatchList: ([]*request_matches_for_team_response.MatchT) (len=12 cap=12) {",
+ stdout)
self.assertEqual(stdout.count("MatchNumber:"), 12)
self.assertEqual(len(re.findall(r": \(int32\) 4856[,\n]", stdout)), 12)
@@ -148,13 +180,16 @@
# RequestAllMatches has no fields.
json_path = write_json_request({})
- exit_code, stdout, stderr = run_debug_cli(["-requestAllMatches", json_path])
+ exit_code, stdout, stderr = run_debug_cli(
+ ["-requestAllMatches", json_path])
self.assertEqual(exit_code, 0, stderr)
request_all_matches_outputs.append(stdout)
self.maxDiff = None
- self.assertEqual(request_all_matches_outputs[0], request_all_matches_outputs[1])
+ self.assertEqual(request_all_matches_outputs[0],
+ request_all_matches_outputs[1])
+
if __name__ == "__main__":
unittest.main()
diff --git a/scouting/www/index_html_generator.py b/scouting/www/index_html_generator.py
index 3b057fd..bc0e63d 100644
--- a/scouting/www/index_html_generator.py
+++ b/scouting/www/index_html_generator.py
@@ -5,9 +5,11 @@
import sys
from pathlib import Path
+
def compute_sha256(filepath):
return hashlib.sha256(filepath.read_bytes()).hexdigest()
+
def main(argv):
parser = argparse.ArgumentParser()
parser.add_argument("--template", type=str)
@@ -20,9 +22,9 @@
bundle_sha256 = compute_sha256(bundle_path)
output = template.format(
- MAIN_BUNDLE_FILE = f"/sha256/{bundle_sha256}/{bundle_path.name}",
- )
+ MAIN_BUNDLE_FILE=f"/sha256/{bundle_sha256}/{bundle_path.name}", )
Path(args.output).write_text(output)
+
if __name__ == "__main__":
sys.exit(main(sys.argv))
diff --git a/tools/actions/generate_compile_command.py b/tools/actions/generate_compile_command.py
index b41f2fc..6f10e09 100755
--- a/tools/actions/generate_compile_command.py
+++ b/tools/actions/generate_compile_command.py
@@ -5,23 +5,26 @@
import third_party.bazel.protos.extra_actions_base_pb2 as extra_actions_base_pb2
+
def _get_cpp_command(cpp_compile_info):
- compiler = cpp_compile_info.tool
- options = ' '.join(cpp_compile_info.compiler_option)
- source = cpp_compile_info.source_file
- output = cpp_compile_info.output_file
- return '%s %s -c %s -o %s' % (compiler, options, source, output), source
+ compiler = cpp_compile_info.tool
+ options = ' '.join(cpp_compile_info.compiler_option)
+ source = cpp_compile_info.source_file
+ output = cpp_compile_info.output_file
+ return '%s %s -c %s -o %s' % (compiler, options, source, output), source
+
def main(argv):
- action = extra_actions_base_pb2.ExtraActionInfo()
- with open(argv[1], 'rb') as f:
- action.MergeFromString(f.read())
- command, source_file = _get_cpp_command(
- action.Extensions[extra_actions_base_pb2.CppCompileInfo.cpp_compile_info])
- with open(argv[2], 'w') as f:
- f.write(command)
- f.write('\0')
- f.write(source_file)
+ action = extra_actions_base_pb2.ExtraActionInfo()
+ with open(argv[1], 'rb') as f:
+ action.MergeFromString(f.read())
+ command, source_file = _get_cpp_command(action.Extensions[
+ extra_actions_base_pb2.CppCompileInfo.cpp_compile_info])
+ with open(argv[2], 'w') as f:
+ f.write(command)
+ f.write('\0')
+ f.write(source_file)
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/tools/actions/generate_compile_commands_json.py b/tools/actions/generate_compile_commands_json.py
index d867698..04df592 100755
--- a/tools/actions/generate_compile_commands_json.py
+++ b/tools/actions/generate_compile_commands_json.py
@@ -13,7 +13,6 @@
import pathlib
import os.path
import subprocess
-
'''
Args:
path: The pathlib.Path to _compile_command file.
@@ -21,18 +20,21 @@
Returns a string to stick in compile_commands.json.
'''
+
+
def _get_command(path, command_directory):
- with path.open('r') as f:
- contents = f.read().split('\0')
- if len(contents) != 2:
- # Old/incomplete file or something; silently ignore it.
- return None
- return '''{
+ with path.open('r') as f:
+ contents = f.read().split('\0')
+ if len(contents) != 2:
+ # Old/incomplete file or something; silently ignore it.
+ return None
+ return '''{
"directory": "%s",
"command": "%s",
"file": "%s",
},''' % (command_directory, contents[0].replace('"', '\\"'), contents[1])
+
'''
Args:
path: A directory pathlib.Path to look for _compile_command files under.
@@ -40,29 +42,34 @@
Yields strings to stick in compile_commands.json.
'''
+
+
def _get_compile_commands(path, command_directory):
- for f in path.iterdir():
- if f.is_dir():
- yield from _get_compile_commands(f, command_directory)
- elif f.name.endswith('_compile_command'):
- command = _get_command(f, command_directory)
- if command:
- yield command
+ for f in path.iterdir():
+ if f.is_dir():
+ yield from _get_compile_commands(f, command_directory)
+ elif f.name.endswith('_compile_command'):
+ command = _get_command(f, command_directory)
+ if command:
+ yield command
+
def main(argv):
- source_path = os.path.join(os.path.dirname(__file__), '../..')
- action_outs = os.path.join(source_path,
- 'bazel-bin/../extra_actions',
- 'tools/actions/generate_compile_commands_action')
- command_directory = subprocess.check_output(
- ('bazel', 'info', 'execution_root'),
- cwd=source_path).decode('utf-8').rstrip()
- commands = _get_compile_commands(pathlib.Path(action_outs), command_directory)
- with open(os.path.join(source_path, 'compile_commands.json'), 'w') as f:
- f.write('[')
- for command in commands:
- f.write(command)
- f.write(']')
+ source_path = os.path.join(os.path.dirname(__file__), '../..')
+ action_outs = os.path.join(
+ source_path, 'bazel-bin/../extra_actions',
+ 'tools/actions/generate_compile_commands_action')
+ command_directory = subprocess.check_output(
+ ('bazel', 'info', 'execution_root'),
+ cwd=source_path).decode('utf-8').rstrip()
+ commands = _get_compile_commands(pathlib.Path(action_outs),
+ command_directory)
+ with open(os.path.join(source_path, 'compile_commands.json'), 'w') as f:
+ f.write('[')
+ for command in commands:
+ f.write(command)
+ f.write(']')
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/tools/build_rules/apache_runner.py b/tools/build_rules/apache_runner.py
index 3839cb0..87d7224 100644
--- a/tools/build_rules/apache_runner.py
+++ b/tools/build_rules/apache_runner.py
@@ -34,6 +34,7 @@
dummy@frc971.org
"""
+
def wait_for_server(port: int):
"""Waits for the server at the specified port to respond to TCP connections."""
while True:
@@ -46,102 +47,110 @@
connection.close()
time.sleep(0.01)
+
def main(argv):
- parser = argparse.ArgumentParser()
- parser.add_argument("--binary", type=str, required=True)
- parser.add_argument("--https_port", type=int, default=7000)
- parser.add_argument("--wrapped_port", type=int, default=7500)
- parser.add_argument(
- "--ldap_info",
- type=str,
- help="JSON file containing 'ldap_bind_dn', 'ldap_url', and 'ldap_password' entries.",
- default="",
- )
- args, unknown_args = parser.parse_known_args(argv[1:])
-
- if not args.ldap_info:
- args.ldap_info = os.path.join(os.environ["BUILD_WORKSPACE_DIRECTORY"], "ldap.json")
-
- with open("tools/build_rules/apache_template.conf", "r") as file:
- template = jinja2.Template(file.read())
-
- with open(args.ldap_info, "r") as file:
- substitutions = json.load(file)
-
- for key in ("ldap_bind_dn", "ldap_url", "ldap_password"):
- if key not in substitutions:
- raise KeyError(f"The ldap_info JSON file must contain key '{key}'.")
-
- substitutions.update({
- "https_port": args.https_port,
- "wrapped_port": args.wrapped_port,
- })
-
- config_text = template.render(substitutions)
-
- with tempfile.TemporaryDirectory() as temp_dir:
- temp_dir = Path(temp_dir)
- with open(temp_dir / "apache2.conf", "w") as file:
- file.write(config_text)
-
- # Create a directory for error logs and such.
- logs_dir = temp_dir / "logs"
- os.mkdir(logs_dir)
-
- print("-" * 60)
- print(f"Logs are in {logs_dir}/")
- print("-" * 60)
-
- # Make modules available.
- modules_path = Path("external/apache2/usr/lib/apache2/modules")
- os.symlink(modules_path.resolve(), temp_dir / "modules")
-
- # Generate a testing cert.
- subprocess.run([
- "openssl",
- "req",
- "-x509",
- "-nodes",
- "-days=365",
- "-newkey=rsa:2048",
- "-keyout=" + str(temp_dir / "apache-selfsigned.key"),
- "-out=" + str(temp_dir / "apache-selfsigned.crt"),
- ],
- check=True,
- input=DUMMY_CERT_ANSWERS,
- text=True,
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--binary", type=str, required=True)
+ parser.add_argument("--https_port", type=int, default=7000)
+ parser.add_argument("--wrapped_port", type=int, default=7500)
+ parser.add_argument(
+ "--ldap_info",
+ type=str,
+ help=
+ "JSON file containing 'ldap_bind_dn', 'ldap_url', and 'ldap_password' entries.",
+ default="",
)
+ args, unknown_args = parser.parse_known_args(argv[1:])
- # Start the wrapped binary in the background.
- # Tell it via the environment what port to listen on.
- env = os.environ.copy()
- env["APACHE_WRAPPED_PORT"] = str(args.wrapped_port)
- wrapped_binary = subprocess.Popen([args.binary] + unknown_args, env=env)
+ if not args.ldap_info:
+ args.ldap_info = os.path.join(os.environ["BUILD_WORKSPACE_DIRECTORY"],
+ "ldap.json")
- # Start the apache server.
- env = os.environ.copy()
- env["LD_LIBRARY_PATH"] = "external/apache2/usr/lib/x86_64-linux-gnu"
- apache = subprocess.Popen(
- ["external/apache2/usr/sbin/apache2", "-X", "-d", str(temp_dir)],
- env=env,
- )
+ with open("tools/build_rules/apache_template.conf", "r") as file:
+ template = jinja2.Template(file.read())
- wait_for_server(args.https_port)
- wait_for_server(args.wrapped_port)
- # Sleep to attempt to get the HTTPS message after the webserver message.
- time.sleep(1)
- print(f"Serving HTTPS on port {args.https_port}")
+ with open(args.ldap_info, "r") as file:
+ substitutions = json.load(file)
- # Wait until we see a request to shut down.
- signal.signal(signal.SIGINT, lambda signum, frame: None)
- signal.signal(signal.SIGTERM, lambda signum, frame: None)
- signal.pause()
+ for key in ("ldap_bind_dn", "ldap_url", "ldap_password"):
+ if key not in substitutions:
+ raise KeyError(
+ f"The ldap_info JSON file must contain key '{key}'.")
- print("\nShutting down apache and wrapped binary.")
- apache.terminate()
- wrapped_binary.terminate()
- apache.wait()
- wrapped_binary.wait()
+ substitutions.update({
+ "https_port": args.https_port,
+ "wrapped_port": args.wrapped_port,
+ })
+
+ config_text = template.render(substitutions)
+
+ with tempfile.TemporaryDirectory() as temp_dir:
+ temp_dir = Path(temp_dir)
+ with open(temp_dir / "apache2.conf", "w") as file:
+ file.write(config_text)
+
+ # Create a directory for error logs and such.
+ logs_dir = temp_dir / "logs"
+ os.mkdir(logs_dir)
+
+ print("-" * 60)
+ print(f"Logs are in {logs_dir}/")
+ print("-" * 60)
+
+ # Make modules available.
+ modules_path = Path("external/apache2/usr/lib/apache2/modules")
+ os.symlink(modules_path.resolve(), temp_dir / "modules")
+
+ # Generate a testing cert.
+ subprocess.run(
+ [
+ "openssl",
+ "req",
+ "-x509",
+ "-nodes",
+ "-days=365",
+ "-newkey=rsa:2048",
+ "-keyout=" + str(temp_dir / "apache-selfsigned.key"),
+ "-out=" + str(temp_dir / "apache-selfsigned.crt"),
+ ],
+ check=True,
+ input=DUMMY_CERT_ANSWERS,
+ text=True,
+ )
+
+ # Start the wrapped binary in the background.
+ # Tell it via the environment what port to listen on.
+ env = os.environ.copy()
+ env["APACHE_WRAPPED_PORT"] = str(args.wrapped_port)
+ wrapped_binary = subprocess.Popen([args.binary] + unknown_args,
+ env=env)
+
+ # Start the apache server.
+ env = os.environ.copy()
+ env["LD_LIBRARY_PATH"] = "external/apache2/usr/lib/x86_64-linux-gnu"
+ apache = subprocess.Popen(
+ ["external/apache2/usr/sbin/apache2", "-X", "-d",
+ str(temp_dir)],
+ env=env,
+ )
+
+ wait_for_server(args.https_port)
+ wait_for_server(args.wrapped_port)
+ # Sleep to attempt to get the HTTPS message after the webserver message.
+ time.sleep(1)
+ print(f"Serving HTTPS on port {args.https_port}")
+
+ # Wait until we see a request to shut down.
+ signal.signal(signal.SIGINT, lambda signum, frame: None)
+ signal.signal(signal.SIGTERM, lambda signum, frame: None)
+ signal.pause()
+
+ print("\nShutting down apache and wrapped binary.")
+ apache.terminate()
+ wrapped_binary.terminate()
+ apache.wait()
+ wrapped_binary.wait()
+
if __name__ == "__main__":
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/tools/build_rules/jinja2_generator.py b/tools/build_rules/jinja2_generator.py
index 34f3354..e5f6fa3 100644
--- a/tools/build_rules/jinja2_generator.py
+++ b/tools/build_rules/jinja2_generator.py
@@ -8,22 +8,26 @@
def main():
- # Note: this is a pretty transparent interface to jinja2--there's no reason
- # this script couldn't be renamed and then used to generate any config from
- # a template.
- parser = argparse.ArgumentParser(
- description="Generates the raspberry pi configs from a template.")
- parser.add_argument("template", type=str, help="File to use for template.")
- parser.add_argument("replacements", type=json.loads, help="Dictionary of parameters to replace in the template.")
- parser.add_argument("output", type=str, help="Output file to create.")
- args = parser.parse_args(sys.argv[1:])
+ # Note: this is a pretty transparent interface to jinja2--there's no reason
+ # this script couldn't be renamed and then used to generate any config from
+ # a template.
+ parser = argparse.ArgumentParser(
+ description="Generates the raspberry pi configs from a template.")
+ parser.add_argument("template", type=str, help="File to use for template.")
+ parser.add_argument(
+ "replacements",
+ type=json.loads,
+ help="Dictionary of parameters to replace in the template.")
+ parser.add_argument("output", type=str, help="Output file to create.")
+ args = parser.parse_args(sys.argv[1:])
- with open(args.template, 'r') as input_file:
- template = jinja2.Template(input_file.read())
+ with open(args.template, 'r') as input_file:
+ template = jinja2.Template(input_file.read())
- output = template.render(args.replacements)
- with open(args.output, 'w') as config_file:
- config_file.write(output)
+ output = template.render(args.replacements)
+ with open(args.output, 'w') as config_file:
+ config_file.write(output)
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/tools/go/mirror_go_repos.py b/tools/go/mirror_go_repos.py
index ae8f722..52d611f 100644
--- a/tools/go/mirror_go_repos.py
+++ b/tools/go/mirror_go_repos.py
@@ -20,15 +20,20 @@
GO_DEPS_WWWW_DIR = "/var/www/html/files/frc971/Build-Dependencies/go_deps"
+
def compute_sha256(filepath: str) -> str:
"""Computes the SHA256 of a file at the specified location."""
with open(filepath, "rb") as file:
contents = file.read()
return hashlib.sha256(contents).hexdigest()
+
def get_existing_mirrored_repos(ssh_host: str) -> Dict[str, str]:
"""Gathers information about the libraries that are currently mirrored."""
- run_result = subprocess.run(["ssh", ssh_host, f"bash -c 'sha256sum {GO_DEPS_WWWW_DIR}/*'"], check=True, stdout=subprocess.PIPE)
+ run_result = subprocess.run(
+ ["ssh", ssh_host, f"bash -c 'sha256sum {GO_DEPS_WWWW_DIR}/*'"],
+ check=True,
+ stdout=subprocess.PIPE)
existing_mirrored_repos = {}
for line in run_result.stdout.decode("utf-8").splitlines():
@@ -37,10 +42,10 @@
return existing_mirrored_repos
-def download_repos(
- repos: Dict[str, str],
- existing_mirrored_repos: Dict[str, str],
- tar: tarfile.TarFile) -> Dict[str, str]:
+
+def download_repos(repos: Dict[str, str], existing_mirrored_repos: Dict[str,
+ str],
+ tar: tarfile.TarFile) -> Dict[str, str]:
"""Downloads the not-yet-mirrored repos into a tarball."""
cached_info = {}
@@ -52,7 +57,8 @@
download_result = subprocess.run(
["external/go_sdk/bin/go", "mod", "download", "-json", module],
- check=True, stdout=subprocess.PIPE)
+ check=True,
+ stdout=subprocess.PIPE)
if download_result.returncode != 0:
print("Failed to download file.")
return 1
@@ -81,6 +87,7 @@
return cached_info
+
def copy_to_host_and_unpack(filename: str, ssh_host: str) -> None:
subprocess.run(["scp", filename, f"{ssh_host}:"], check=True)
@@ -94,7 +101,10 @@
])
print("You might be asked for your sudo password shortly.")
- subprocess.run(["ssh", "-t", ssh_host, f"sudo -u www-data bash -c '{command}'"], check=True)
+ subprocess.run(
+ ["ssh", "-t", ssh_host, f"sudo -u www-data bash -c '{command}'"],
+ check=True)
+
def main(argv):
parser = argparse.ArgumentParser()
@@ -113,12 +123,15 @@
"Build-Dependencies files live. Only specify this if you have "
"access to the server."))
parser.add_argument("--go_deps_bzl", type=str, default="go_deps.bzl")
- parser.add_argument("--go_mirrors_bzl", type=str, default="tools/go/go_mirrors.bzl")
+ parser.add_argument("--go_mirrors_bzl",
+ type=str,
+ default="tools/go/go_mirrors.bzl")
args = parser.parse_args(argv[1:])
os.chdir(os.environ["BUILD_WORKSPACE_DIRECTORY"])
- repos = org_frc971.tools.go.mirror_lib.parse_go_repositories(args.go_deps_bzl)
+ repos = org_frc971.tools.go.mirror_lib.parse_go_repositories(
+ args.go_deps_bzl)
if args.ssh_host:
existing_mirrored_repos = get_existing_mirrored_repos(args.ssh_host)
@@ -129,7 +142,8 @@
if args.prune:
# Delete all mirror info that is not needed anymore.
- existing_cache_info = org_frc971.tools.go.mirror_lib.parse_go_mirror_info(args.go_mirrors_bzl)
+ existing_cache_info = org_frc971.tools.go.mirror_lib.parse_go_mirror_info(
+ args.go_mirrors_bzl)
cached_info = {}
for repo in repos:
try:
@@ -151,10 +165,12 @@
if args.ssh_host and num_not_already_mirrored:
copy_to_host_and_unpack("go_deps.tar", args.ssh_host)
else:
- print("Skipping mirroring because of lack of --ssh_host or there's "
- "nothing to actually mirror.")
+ print(
+ "Skipping mirroring because of lack of --ssh_host or there's "
+ "nothing to actually mirror.")
- org_frc971.tools.go.mirror_lib.write_go_mirror_info(args.go_mirrors_bzl, cached_info)
+ org_frc971.tools.go.mirror_lib.write_go_mirror_info(
+ args.go_mirrors_bzl, cached_info)
return exit_code
diff --git a/tools/go/mirror_lib.py b/tools/go/mirror_lib.py
index ea23abc..b613ca8 100644
--- a/tools/go/mirror_lib.py
+++ b/tools/go/mirror_lib.py
@@ -13,6 +13,7 @@
with open(filepath, "r") as file:
return file.read()
+
def parse_go_repositories(filepath: str) -> List[Dict[str, str]]:
"""Parses the top-level go_deps.bzl file.
@@ -53,7 +54,8 @@
return global_data["GO_MIRROR_INFO"]
-def write_go_mirror_info(filepath: str, mirror_info: Dict[str, Dict[str, str]]):
+def write_go_mirror_info(filepath: str, mirror_info: Dict[str, Dict[str,
+ str]]):
"""Saves the specified mirror_info as GO_MIRROR_INFO into tools/go/go_mirrors.bzl."""
with open(filepath, "w") as file:
file.write("# This file is auto-generated. Do not edit.\n")
@@ -64,9 +66,10 @@
# Properly format the file now so that the linter doesn't complain.
r = runfiles.Create()
- subprocess.run(
- [
- r.Rlocation("com_github_bazelbuild_buildtools/buildifier/buildifier_/buildifier"),
- filepath,
- ],
- check=True)
+ subprocess.run([
+ r.Rlocation(
+ "com_github_bazelbuild_buildtools/buildifier/buildifier_/buildifier"
+ ),
+ filepath,
+ ],
+ check=True)
diff --git a/tools/go/tweak_gazelle_go_deps.py b/tools/go/tweak_gazelle_go_deps.py
index d9d7901..d7f3372 100644
--- a/tools/go/tweak_gazelle_go_deps.py
+++ b/tools/go/tweak_gazelle_go_deps.py
@@ -14,22 +14,27 @@
import org_frc971.tools.go.mirror_lib
+
def main(argv):
parser = argparse.ArgumentParser()
parser.add_argument("go_deps_bzl", type=str)
args = parser.parse_args(argv[1:])
- repos = org_frc971.tools.go.mirror_lib.parse_go_repositories(args.go_deps_bzl)
+ repos = org_frc971.tools.go.mirror_lib.parse_go_repositories(
+ args.go_deps_bzl)
with open(args.go_deps_bzl, "w") as file:
- file.write(textwrap.dedent("""\
+ file.write(
+ textwrap.dedent("""\
# This file is auto-generated. Do not edit.
load("//tools/go:mirrored_go_deps.bzl", "maybe_override_go_dep")
def go_dependencies():
"""))
for repo in repos:
- file.write(textwrap.indent(textwrap.dedent(f"""\
+ file.write(
+ textwrap.indent(
+ textwrap.dedent(f"""\
maybe_override_go_dep(
name = "{repo['name']}",
importpath = "{repo['importpath']}",
@@ -38,5 +43,6 @@
)
"""), " " * 4))
+
if __name__ == "__main__":
sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 77ccf14..488d792 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -51,8 +51,8 @@
# Input is [bottom power, top power - bottom power * J_top / J_bottom]
# Motor time constants. difference_bottom refers to the constant for how the
# bottom velocity affects the difference of the top and bottom velocities.
- self.common_motor_constant = -self.Kt / self.Kv / (
- self.G * self.G * self.R)
+ self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G *
+ self.R)
self.bottom_bottom = self.common_motor_constant / self.J_bottom
self.difference_bottom = -self.common_motor_constant * (
1 / self.J_bottom - 1 / self.J_top)
@@ -96,7 +96,8 @@
self.B_continuous, self.dt)
self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont,
- self.B_bottom_cont, self.dt)
+ self.B_bottom_cont,
+ self.dt)
self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont,
self.B_diff_cont, self.dt)
@@ -135,12 +136,12 @@
glog.debug(str(lstsq_A))
glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
- out_x = numpy.linalg.lstsq(
- lstsq_A,
- numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]),
- rcond=None)[0]
- self.K[1, 2] = -lstsq_A[0, 0] * (
- self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
+ out_x = numpy.linalg.lstsq(lstsq_A,
+ numpy.matrix([[self.A[1, 2]],
+ [self.A[3, 2]]]),
+ rcond=None)[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] -
+ out_x[0]) / lstsq_A[0, 1] + out_x[1]
glog.debug('K unaugmented')
glog.debug(str(self.K))
@@ -181,8 +182,9 @@
X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
#X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
#X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[3, 0] = 1 / (1 - A[3, 3]) * (
- X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ X_ss[3,
+ 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] +
+ B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
#X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
X_ss[1, 0] = (A[1, 2] * X_ss[2, 0]) + (A[1, 3] * X_ss[3, 0]) + (
@@ -247,7 +249,8 @@
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([
- self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl, 0.90
+ self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl,
+ 0.90
])
#print "A is"
#print self.A
@@ -284,8 +287,8 @@
numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
numpy.matrix([[12], [12], [12], [12]]))
- if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
- top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+ if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0]
+ or top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
position_K = K[:, 0:2]
velocity_K = K[:, 2:]
@@ -501,8 +504,8 @@
else:
namespaces = ['y2014', 'control_loops', 'claw']
claw = Claw('Claw')
- loop_writer = control_loop.ControlLoopWriter(
- 'Claw', [claw], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kClawMomentOfInertiaRatio', '%f',
claw.J_top / claw.J_bottom))
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index cac236f..d72c222 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -11,30 +11,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(J = 2.8,
- mass = 68,
- robot_radius = 0.647998644 / 2.0,
- wheel_radius = .04445,
- G_high = 28.0 / 50.0 * 18.0 / 50.0,
- G_low = 18.0 / 60.0 * 18.0 / 50.0,
- q_pos_low = 0.12,
- q_pos_high = 0.12,
- q_vel_low = 1.0,
- q_vel_high = 1.0,
- has_imu = False,
- dt = 0.005,
- controller_poles = [0.7, 0.7])
+kDrivetrain = drivetrain.DrivetrainParams(J=2.8,
+ mass=68,
+ robot_radius=0.647998644 / 2.0,
+ wheel_radius=.04445,
+ G_high=28.0 / 50.0 * 18.0 / 50.0,
+ G_low=18.0 / 60.0 * 18.0 / 50.0,
+ q_pos_low=0.12,
+ q_pos_high=0.12,
+ q_vel_low=1.0,
+ q_vel_high=1.0,
+ has_imu=False,
+ dt=0.005,
+ controller_poles=[0.7, 0.7])
+
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print("Expected .h file name and .cc file name")
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014', kDrivetrain)
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print("Expected .h file name and .cc file name")
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014', kDrivetrain)
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/dt_mpc.py b/y2014/control_loops/python/dt_mpc.py
index 0c229c1..2f13807 100755
--- a/y2014/control_loops/python/dt_mpc.py
+++ b/y2014/control_loops/python/dt_mpc.py
@@ -13,8 +13,9 @@
#
# Inital algorithm from http://www.ece.ufrgs.br/~fetter/icma05_608.pdf
+
def cartesian_to_polar(X_cartesian):
- """Converts a cartesian coordinate to polar.
+ """Converts a cartesian coordinate to polar.
Args:
X_cartesian, numpy.matrix[3, 1] with x, y, theta as rows.
@@ -22,13 +23,13 @@
Returns:
numpy.matrix[3, 1] with e, phi, alpha as rows.
"""
- phi = numpy.arctan2(X_cartesian[1, 0], X_cartesian[0, 0])
- return numpy.matrix([[numpy.hypot(X_cartesian[0, 0], X_cartesian[1, 0])],
- [phi],
- [X_cartesian[2, 0] - phi]])
+ phi = numpy.arctan2(X_cartesian[1, 0], X_cartesian[0, 0])
+ return numpy.matrix([[numpy.hypot(X_cartesian[0, 0], X_cartesian[1, 0])],
+ [phi], [X_cartesian[2, 0] - phi]])
+
def polar_to_cartesian(X_polar):
- """Converts a polar coordinate to cartesian.
+ """Converts a polar coordinate to cartesian.
Args:
X_polar, numpy.matrix[3, 1] with e, phi, alpha as rows.
@@ -36,12 +37,13 @@
Returns:
numpy.matrix[3, 1] with x, y, theta as rows.
"""
- return numpy.matrix([[X_polar[0, 0] * numpy.cos(X_polar[1, 0])],
- [X_polar[0, 0] * numpy.sin(X_polar[1, 0])],
- [X_polar[1, 0] + X_polar[2, 0]]])
+ return numpy.matrix([[X_polar[0, 0] * numpy.cos(X_polar[1, 0])],
+ [X_polar[0, 0] * numpy.sin(X_polar[1, 0])],
+ [X_polar[1, 0] + X_polar[2, 0]]])
+
def simulate_dynamics(X_cartesian, U):
- """Calculates the robot location after 1 timestep.
+ """Calculates the robot location after 1 timestep.
Args:
X_cartesian, numpy.matrix[3, 1] with the starting location in
@@ -51,15 +53,15 @@
Returns:
numpy.matrix[3, 1] with the cartesian coordinate.
"""
- X_cartesian += numpy.matrix(
- [[U[0, 0] * numpy.cos(X_cartesian[2, 0]) * dt],
- [U[0, 0] * numpy.sin(X_cartesian[2, 0]) * dt],
- [U[1, 0] * dt]])
+ X_cartesian += numpy.matrix([[U[0, 0] * numpy.cos(X_cartesian[2, 0]) * dt],
+ [U[0, 0] * numpy.sin(X_cartesian[2, 0]) * dt],
+ [U[1, 0] * dt]])
- return X_cartesian
+ return X_cartesian
+
def U_from_array(U_array):
- """Converts the U array from the optimizer to a bunch of column vectors.
+ """Converts the U array from the optimizer to a bunch of column vectors.
Args:
U_array, numpy.array[N] The U coordinates in v, av, v, av, ...
@@ -67,10 +69,11 @@
Returns:
numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
"""
- return numpy.matrix(U_array).reshape((2, -1), order='F')
+ return numpy.matrix(U_array).reshape((2, -1), order='F')
+
def U_to_array(U_matrix):
- """Converts the U matrix to the U array for the optimizer.
+ """Converts the U matrix to the U array for the optimizer.
Args:
U_matrix, numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
@@ -78,10 +81,11 @@
Returns:
numpy.array[N] The U coordinates in v, av, v, av, ...
"""
- return numpy.array(U_matrix.reshape((1, -1), order='F'))
+ return numpy.array(U_matrix.reshape((1, -1), order='F'))
+
def cost(U_array, X_cartesian):
- """Computes the cost given the inital position and the U array.
+ """Computes the cost given the inital position and the U array.
Args:
U_array: numpy.array[N] The U coordinates.
@@ -91,91 +95,93 @@
Returns:
double, The quadratic cost of evaluating U.
"""
- X_cartesian_mod = X_cartesian.copy()
- U_matrix = U_from_array(U_array)
- my_cost = 0
- Q = numpy.matrix([[0.01, 0, 0],
- [0, 0.01, 0],
- [0, 0, 0.01]]) / dt / dt
- R = numpy.matrix([[0.001, 0],
- [0, 0.001]]) / dt / dt
- for U in U_matrix.T:
- # TODO(austin): Let it go to the point from either side.
- U = U.T
- X_cartesian_mod = simulate_dynamics(X_cartesian_mod, U)
- X_current_polar = cartesian_to_polar(X_cartesian_mod)
- my_cost += U.T * R * U + X_current_polar.T * Q * X_current_polar
+ X_cartesian_mod = X_cartesian.copy()
+ U_matrix = U_from_array(U_array)
+ my_cost = 0
+ Q = numpy.matrix([[0.01, 0, 0], [0, 0.01, 0], [0, 0, 0.01]]) / dt / dt
+ R = numpy.matrix([[0.001, 0], [0, 0.001]]) / dt / dt
+ for U in U_matrix.T:
+ # TODO(austin): Let it go to the point from either side.
+ U = U.T
+ X_cartesian_mod = simulate_dynamics(X_cartesian_mod, U)
+ X_current_polar = cartesian_to_polar(X_cartesian_mod)
+ my_cost += U.T * R * U + X_current_polar.T * Q * X_current_polar
- return my_cost
+ return my_cost
+
if __name__ == '__main__':
- X_cartesian = numpy.matrix([[-1.0],
- [-1.0],
- [0.0]])
- x_array = []
- y_array = []
- theta_array = []
+ X_cartesian = numpy.matrix([[-1.0], [-1.0], [0.0]])
+ x_array = []
+ y_array = []
+ theta_array = []
- e_array = []
- phi_array = []
- alpha_array = []
+ e_array = []
+ phi_array = []
+ alpha_array = []
- cost_array = []
+ cost_array = []
- time_array = []
- u0_array = []
- u1_array = []
+ time_array = []
+ u0_array = []
+ u1_array = []
- num_steps = 20
+ num_steps = 20
- U_array = numpy.zeros((num_steps * 2))
- for i in range(400):
- print('Iteration', i)
- # Solve the NMPC
- U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(
- cost, U_array.copy(), bounds = [(-1, 1), (-7, 7)] * num_steps,
- args=(X_cartesian,), iter=500, full_output=True)
- U_matrix = U_from_array(U_array)
+ U_array = numpy.zeros((num_steps * 2))
+ for i in range(400):
+ print('Iteration', i)
+ # Solve the NMPC
+ U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(cost,
+ U_array.copy(),
+ bounds=[(-1, 1),
+ (-7, 7)] *
+ num_steps,
+ args=(X_cartesian, ),
+ iter=500,
+ full_output=True)
+ U_matrix = U_from_array(U_array)
- # Simulate the dynamics
- X_cartesian = simulate_dynamics(X_cartesian, U_matrix[:, 0])
+ # Simulate the dynamics
+ X_cartesian = simulate_dynamics(X_cartesian, U_matrix[:, 0])
- # Save variables for plotting.
- cost_array.append(fx[0, 0])
- u0_array.append(U_matrix[0, 0])
- u1_array.append(U_matrix[1, 0])
- x_array.append(X_cartesian[0, 0])
- y_array.append(X_cartesian[1, 0])
- theta_array.append(X_cartesian[2, 0])
- time_array.append(i * dt)
- X_polar = cartesian_to_polar(X_cartesian)
- e_array.append(X_polar[0, 0])
- phi_array.append(X_polar[1, 0])
- alpha_array.append(X_polar[2, 0])
+ # Save variables for plotting.
+ cost_array.append(fx[0, 0])
+ u0_array.append(U_matrix[0, 0])
+ u1_array.append(U_matrix[1, 0])
+ x_array.append(X_cartesian[0, 0])
+ y_array.append(X_cartesian[1, 0])
+ theta_array.append(X_cartesian[2, 0])
+ time_array.append(i * dt)
+ X_polar = cartesian_to_polar(X_cartesian)
+ e_array.append(X_polar[0, 0])
+ phi_array.append(X_polar[1, 0])
+ alpha_array.append(X_polar[2, 0])
- U_array = U_to_array(numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
+ U_array = U_to_array(
+ numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
- if fx < 1e-05:
- print('Cost is', fx, 'after', i, 'cycles, finishing early')
- break
+ if fx < 1e-05:
+ print('Cost is', fx, 'after', i, 'cycles, finishing early')
+ break
- # Plot
- pylab.figure('xy')
- pylab.plot(x_array, y_array, label = 'trajectory')
+ # Plot
+ pylab.figure('xy')
+ pylab.plot(x_array, y_array, label='trajectory')
- pylab.figure('time')
- pylab.plot(time_array, x_array, label='x')
- pylab.plot(time_array, y_array, label='y')
- pylab.plot(time_array, theta_array, label = 'theta')
- pylab.plot(time_array, e_array, label='e')
- pylab.plot(time_array, phi_array, label='phi')
- pylab.plot(time_array, alpha_array, label='alpha')
- pylab.plot(time_array, cost_array, label='cost')
- pylab.legend()
+ pylab.figure('time')
+ pylab.plot(time_array, x_array, label='x')
+ pylab.plot(time_array, y_array, label='y')
+ pylab.plot(time_array, theta_array, label='theta')
+ pylab.plot(time_array, e_array, label='e')
+ pylab.plot(time_array, phi_array, label='phi')
+ pylab.plot(time_array, alpha_array, label='alpha')
+ pylab.plot(time_array, cost_array, label='cost')
+ pylab.legend()
- pylab.figure('u')
- pylab.plot(time_array, u0_array, label='u0')
- pylab.plot(time_array, u1_array, label='u1')
- pylab.legend()
+ pylab.figure('u')
+ pylab.plot(time_array, u0_array, label='u0')
+ pylab.plot(time_array, u1_array, label='u1')
+ pylab.legend()
- pylab.show()
+ pylab.show()
diff --git a/y2014/control_loops/python/extended_lqr.py b/y2014/control_loops/python/extended_lqr.py
index b3f2372..699dd30 100755
--- a/y2014/control_loops/python/extended_lqr.py
+++ b/y2014/control_loops/python/extended_lqr.py
@@ -17,8 +17,9 @@
num_inputs = 2
x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
+
def dynamics(X, U):
- """Calculates the dynamics for a 2 wheeled robot.
+ """Calculates the dynamics for a 2 wheeled robot.
Args:
X, numpy.matrix(3, 1), The state. [x, y, theta]
@@ -27,29 +28,33 @@
Returns:
numpy.matrix(3, 1), The derivative of the dynamics.
"""
- #return numpy.matrix([[X[1, 0]],
- # [X[2, 0]],
- # [U[0, 0]]])
- return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
- [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
- [(U[1, 0] - U[0, 0]) / width]])
+ #return numpy.matrix([[X[1, 0]],
+ # [X[2, 0]],
+ # [U[0, 0]]])
+ return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
+ [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
+ [(U[1, 0] - U[0, 0]) / width]])
+
def RungeKutta(f, x, dt):
- """4th order RungeKutta integration of F starting at X."""
- a = f(x)
- b = f(x + dt / 2.0 * a)
- c = f(x + dt / 2.0 * b)
- d = f(x + dt * c)
- return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+ """4th order RungeKutta integration of F starting at X."""
+ a = f(x)
+ b = f(x + dt / 2.0 * a)
+ c = f(x + dt / 2.0 * b)
+ d = f(x + dt * c)
+ return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
def discrete_dynamics(X, U):
- return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+ return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+
def inverse_discrete_dynamics(X, U):
- return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+ return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+
def numerical_jacobian_x(fn, X, U, epsilon=1e-4):
- """Numerically estimates the jacobian around X, U in X.
+ """Numerically estimates the jacobian around X, U in X.
Args:
fn: A function of X, U.
@@ -62,20 +67,21 @@
numpy.matrix(num_states, num_states), The jacobian of fn with X as the
variable.
"""
- num_states = X.shape[0]
- nominal = fn(X, U)
- answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
- # It's more expensive, but +- epsilon will be more reliable
- for i in range(0, num_states):
- dX_plus = X.copy()
- dX_plus[i] += epsilon
- dX_minus = X.copy()
- dX_minus[i] -= epsilon
- answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
- return answer
+ num_states = X.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
+ # It's more expensive, but +- epsilon will be more reliable
+ for i in range(0, num_states):
+ dX_plus = X.copy()
+ dX_plus[i] += epsilon
+ dX_minus = X.copy()
+ dX_minus[i] -= epsilon
+ answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
+ return answer
+
def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
- """Numerically estimates the jacobian around X, U in U.
+ """Numerically estimates the jacobian around X, U in U.
Args:
fn: A function of X, U.
@@ -88,48 +94,56 @@
numpy.matrix(num_states, num_inputs), The jacobian of fn with U as the
variable.
"""
- num_states = X.shape[0]
- num_inputs = U.shape[0]
- nominal = fn(X, U)
- answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
- for i in range(0, num_inputs):
- dU_plus = U.copy()
- dU_plus[i] += epsilon
- dU_minus = U.copy()
- dU_minus[i] -= epsilon
- answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
- return answer
+ num_states = X.shape[0]
+ num_inputs = U.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
+ for i in range(0, num_inputs):
+ dU_plus = U.copy()
+ dU_plus[i] += epsilon
+ dU_minus = U.copy()
+ dU_minus[i] -= epsilon
+ answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
+ return answer
+
def numerical_jacobian_x_x(fn, X, U):
- return numerical_jacobian_x(
- lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_x_u(fn, X, U):
- return numerical_jacobian_x(
- lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_u_x(fn, X, U):
- return numerical_jacobian_u(
- lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_u_u(fn, X, U):
- return numerical_jacobian_u(
- lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+ X, U)
+
# Simple implementation for a quadratic cost function.
class CostFunction:
- def __init__(self):
- self.num_states = num_states
- self.num_inputs = num_inputs
- self.dt = dt
- self.Q = numpy.matrix([[0.1, 0, 0],
- [0, 0.6, 0],
- [0, 0, 0.1]]) / dt / dt
- self.R = numpy.matrix([[0.40, 0],
- [0, 0.40]]) / dt / dt
- def estimate_Q_final(self, X_hat):
- """Returns the quadraticized final Q around X_hat.
+ def __init__(self):
+ self.num_states = num_states
+ self.num_inputs = num_inputs
+ self.dt = dt
+ self.Q = numpy.matrix([[0.1, 0, 0], [0, 0.6, 0], [0, 0, 0.1]
+ ]) / dt / dt
+ self.R = numpy.matrix([[0.40, 0], [0, 0.40]]) / dt / dt
+
+ def estimate_Q_final(self, X_hat):
+ """Returns the quadraticized final Q around X_hat.
This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
@@ -139,11 +153,11 @@
Result:
numpy.matrix(self.num_states, self.num_states)
"""
- zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
- return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+ return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
- def estimate_partial_cost_partial_x_final(self, X_hat):
- """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+ def estimate_partial_cost_partial_x_final(self, X_hat):
+ """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
Args:
X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -151,14 +165,17 @@
Result:
numpy.matrix(self.num_states, 1)
"""
- return numerical_jacobian_x(self.final_cost, X_hat, numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+ return numerical_jacobian_x(
+ self.final_cost, X_hat,
+ numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
- def estimate_q_final(self, X_hat):
- """Returns q evaluated at X_hat for the final cost function."""
- return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
+ def estimate_q_final(self, X_hat):
+ """Returns q evaluated at X_hat for the final cost function."""
+ return self.estimate_partial_cost_partial_x_final(
+ X_hat) - self.estimate_Q_final(X_hat) * X_hat
- def final_cost(self, X, U):
- """Computes the final cost of being at X
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
Args:
X: numpy.matrix(self.num_states, 1)
@@ -167,10 +184,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of being at X
"""
- return X.T * self.Q * X * 1000
+ return X.T * self.Q * X * 1000
- def cost(self, X, U):
- """Computes the incremental cost given a position and U.
+ def cost(self, X, U):
+ """Computes the incremental cost given a position and U.
Args:
X: numpy.matrix(self.num_states, 1)
@@ -179,250 +196,334 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of evaluating U.
"""
- return U.T * self.R * U + X.T * self.Q * X
+ return U.T * self.R * U + X.T * self.Q * X
+
cost_fn_obj = CostFunction()
-S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+S_bar_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)
+]
s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
-L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+L_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)
+]
l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+L_bar_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)
+]
l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+S_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)
+]
s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
-
-last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+last_x_hat_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+]
for a in range(15):
- x_hat = x_hat_initial
- u_t = L_t[0] * x_hat + l_t[0]
- S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
- s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
- s_scalar_bar_t[0] = numpy.matrix([[0]])
+ x_hat = x_hat_initial
+ u_t = L_t[0] * x_hat + l_t[0]
+ S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
+ s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+ s_scalar_bar_t[0] = numpy.matrix([[0]])
- last_x_hat_t[0] = x_hat_initial
+ last_x_hat_t[0] = x_hat_initial
- Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
- P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
- R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
- q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial, u_t).T - Q_t * x_hat_initial - P_t.T * u_t
- r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial, u_t).T - P_t * x_hat_initial - R_t * u_t
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial,
+ u_t).T - Q_t * x_hat_initial - P_t.T * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial,
+ u_t).T - P_t * x_hat_initial - R_t * u_t
- q_scalar_t = cost_fn_obj.cost(x_hat_initial, u_t) - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T * (P_t * x_hat_initial + R_t * u_t)) - x_hat_initial.T * q_t - u_t.T * r_t
+ q_scalar_t = cost_fn_obj.cost(
+ x_hat_initial,
+ u_t) - 0.5 * (x_hat_initial.T *
+ (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T *
+ (P_t * x_hat_initial + R_t * u_t)
+ ) - x_hat_initial.T * q_t - u_t.T * r_t
- start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
- start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
- x_hat_next = discrete_dynamics(x_hat_initial, u_t)
- start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+ start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
+ start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
+ x_hat_next = discrete_dynamics(x_hat_initial, u_t)
+ start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
- B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
- B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
- B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
+ B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
+ B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
+ B_svd_sigma[0:B_svd_sigma_diag.shape[0],
+ 0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
- B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
- B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
- B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
+ B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
+ B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
+ 0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(
+ numpy.diag(B_svd_sigma_diag))
+ B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
- L_bar_t[1] = B_svd_inv
- l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
+ L_bar_t[1] = B_svd_inv
+ l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
- S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
+ S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
- TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
- Totals_1 = start_B_t.T * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
- Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
+ TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
+ Totals_1 = start_B_t.T * S_t[1] * (
+ start_c_t + start_A_t *
+ x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
+ Totals_scalar_1 = 0.5 * (
+ start_c_t.T + x_hat_initial.T * start_A_t.T
+ ) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[
+ 1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (
+ start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
- optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
- optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
+ optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
+ optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
- S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(S_bar_t[1])
- S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
- S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
- for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
- if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
- S_bar_1_eigh_eigenvalues_stiff[i] = max(S_bar_1_eigh_eigenvalues_stiff) * 1.0
+ S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(
+ S_bar_t[1])
+ S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
+ S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
+ for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
+ if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
+ S_bar_1_eigh_eigenvalues_stiff[i] = max(
+ S_bar_1_eigh_eigenvalues_stiff) * 1.0
- #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
- #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
+ #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
+ #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
- #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
- #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
+ #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
+ #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
- S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
+ S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(
+ numpy.diag(
+ S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
- print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
- print 'Min x_hat', optimal_x_1
- s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
- s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T * (S_bar_stiff + S_t[1]) * optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
+ print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+ print 'Min x_hat', optimal_x_1
+ s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
+ s_scalar_bar_t[1] = 0.5 * (
+ optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T *
+ (S_bar_stiff + S_t[1]) *
+ optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (
+ s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
- print 'optimal_u_1', optimal_u_1
- print 'TotalS_1', TotalS_1
- print 'Totals_1', Totals_1
- print 'Totals_scalar_1', Totals_scalar_1
- print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
- print 'overall cost 0', 0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial) + x_hat_initial.T * s_t[0] + s_scalar_t[0]
+ print 'optimal_u_1', optimal_u_1
+ print 'TotalS_1', TotalS_1
+ print 'Totals_1', Totals_1
+ print 'Totals_scalar_1', Totals_scalar_1
+ print 'overall cost 1', 0.5 * (
+ optimal_u_1.T * TotalS_1 *
+ optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
+ print 'overall cost 0', 0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial
+ ) + x_hat_initial.T * s_t[0] + s_scalar_t[0]
- print 't forward 0'
- print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
- print 'x_hat[%2d]: %s' % (0, x_hat.T)
- print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
- print 'u[%2d]: %s' % (0, u_t.T)
- print ('L[ 0]: %s' % (L_t[0],)).replace('\n', '\n ')
- print ('l[ 0]: %s' % (l_t[0],)).replace('\n', '\n ')
+ print 't forward 0'
+ print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
+ print 'x_hat[%2d]: %s' % (0, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
+ print 'u[%2d]: %s' % (0, u_t.T)
+ print('L[ 0]: %s' % (L_t[0], )).replace('\n', '\n ')
+ print('l[ 0]: %s' % (l_t[0], )).replace('\n', '\n ')
- print ('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
- print ('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
- print ('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
+ print('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
+ print('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
+ print('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
- # TODO(austin): optimal_x_1 is x_hat
- x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
- print 'new xhat', x_hat
+ # TODO(austin): optimal_x_1 is x_hat
+ x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
+ print 'new xhat', x_hat
- S_bar_t[1] = S_bar_stiff
+ S_bar_t[1] = S_bar_stiff
- last_x_hat_t[1] = x_hat
+ last_x_hat_t[1] = x_hat
- for t in range(1, l):
- print 't forward', t
- u_t = L_t[t] * x_hat + l_t[t]
+ for t in range(1, l):
+ print 't forward', t
+ u_t = L_t[t] * x_hat + l_t[t]
- x_hat_next = discrete_dynamics(x_hat, u_t)
- A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next, u_t)
- B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next, u_t)
- c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+ x_hat_next = discrete_dynamics(x_hat, u_t)
+ A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next,
+ u_t)
+ B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next,
+ u_t)
+ c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
- print 'x_hat[%2d]: %s' % (t, x_hat.T)
- print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
- print ('L[%2d]: %s' % (t, L_t[t],)).replace('\n', '\n ')
- print ('l[%2d]: %s' % (t, l_t[t],)).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
+ print('L[%2d]: %s' % (
+ t,
+ L_t[t],
+ )).replace('\n', '\n ')
+ print('l[%2d]: %s' % (
+ t,
+ l_t[t],
+ )).replace('\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
- print ('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace('\n', '\n ')
- print ('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace('\n', '\n ')
- print ('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace('\n', '\n ')
+ print('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace(
+ '\n', '\n ')
+ print('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace(
+ '\n', '\n ')
+ print('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace(
+ '\n', '\n ')
- Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
- P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
- R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
- q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat, u_t).T - Q_t * x_hat - P_t.T * u_t
- r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat, u_t).T - P_t * x_hat - R_t * u_t
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat,
+ u_t).T - Q_t * x_hat - P_t.T * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat,
+ u_t).T - P_t * x_hat - R_t * u_t
- q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
+ q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (
+ x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T *
+ (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
- C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
- D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
- E_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
- d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
- e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
+ C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
+ D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
+ E_bar_t = B_bar_t.T * (
+ S_bar_t[t] +
+ Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
+ d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] +
+ Q_t) * c_bar_t
+ e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (
+ S_bar_t[t] + Q_t) * c_bar_t
- L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
- l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+ L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+ l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
- S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
- s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
- s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
+ S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
+ s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
+ s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(
+ E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (
+ S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[
+ t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
- x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]), (s_t[t + 1] + s_bar_t[t + 1]))
+ x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]),
+ (s_t[t + 1] + s_bar_t[t + 1]))
- S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
- s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
- x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
+ S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
+ s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
+ x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
- for t in reversed(range(l)):
- print 't backward', t
- # TODO(austin): I don't think we can use L_t like this here.
- # I think we are off by 1 somewhere...
- u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
+ for t in reversed(range(l)):
+ print 't backward', t
+ # TODO(austin): I don't think we can use L_t like this here.
+ # I think we are off by 1 somewhere...
+ u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
- x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
- print 'x_hat[%2d]: %s' % (t, x_hat.T)
- print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
- print ('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace('\n', '\n ')
- print ('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
- # Now compute the linearized A, B, and C
- # Start by doing it numerically, and then optimize.
- A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
- B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
- c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+ x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
+ print('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
+ # Now compute the linearized A, B, and C
+ # Start by doing it numerically, and then optimize.
+ A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
+ B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
+ c_t = x_hat - A_t * x_hat_prev - B_t * u_t
- print ('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
- print ('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
- print ('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
+ print('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
+ print('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
+ print('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
- Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
- P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
- P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
- R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
+ P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
- q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev, u_t).T - Q_t * x_hat_prev - P_T_t * u_t
- r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev, u_t).T - P_t * x_hat_prev - R_t * u_t
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev,
+ u_t).T - Q_t * x_hat_prev - P_T_t * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev,
+ u_t).T - P_t * x_hat_prev - R_t * u_t
- q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T * (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
+ q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (
+ x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T *
+ (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
- C_t = P_t + B_t.T * S_t[t + 1] * A_t
- D_t = Q_t + A_t.T * S_t[t + 1] * A_t
- E_t = R_t + B_t.T * S_t[t + 1] * B_t
- d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
- e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
- L_t[t] = -numpy.linalg.inv(E_t) * C_t
- l_t[t] = -numpy.linalg.inv(E_t) * e_t
- s_t[t] = d_t + C_t.T * l_t[t]
- S_t[t] = D_t + C_t.T * L_t[t]
- s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[t + 1] + s_scalar_t[t + 1]
+ C_t = P_t + B_t.T * S_t[t + 1] * A_t
+ D_t = Q_t + A_t.T * S_t[t + 1] * A_t
+ E_t = R_t + B_t.T * S_t[t + 1] * B_t
+ d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
+ e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
+ L_t[t] = -numpy.linalg.inv(E_t) * C_t
+ l_t[t] = -numpy.linalg.inv(E_t) * e_t
+ s_t[t] = d_t + C_t.T * l_t[t]
+ S_t[t] = D_t + C_t.T * L_t[t]
+ s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(
+ E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[
+ t + 1] + s_scalar_t[t + 1]
- x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]), (s_t[t] + s_bar_t[t]))
- if t == 0:
- last_x_hat_t[t] = x_hat_initial
- else:
- last_x_hat_t[t] = x_hat
+ x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]),
+ (s_t[t] + s_bar_t[t]))
+ if t == 0:
+ last_x_hat_t[t] = x_hat_initial
+ else:
+ last_x_hat_t[t] = x_hat
- x_hat_t = [x_hat_initial]
+ x_hat_t = [x_hat_initial]
- pylab.figure('states %d' % a)
- pylab.ion()
- for dim in range(num_states):
- pylab.plot(numpy.arange(len(last_x_hat_t)),
- [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t], marker='o', label='Xhat[%d]'%dim)
- pylab.legend()
- pylab.draw()
+ pylab.figure('states %d' % a)
+ pylab.ion()
+ for dim in range(num_states):
+ pylab.plot(numpy.arange(len(last_x_hat_t)),
+ [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t],
+ marker='o',
+ label='Xhat[%d]' % dim)
+ pylab.legend()
+ pylab.draw()
- pylab.figure('xy %d' % a)
- pylab.ion()
- pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
- [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t], marker='o', label='trajectory')
- pylab.legend()
- pylab.draw()
+ pylab.figure('xy %d' % a)
+ pylab.ion()
+ pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
+ [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t],
+ marker='o',
+ label='trajectory')
+ pylab.legend()
+ pylab.draw()
final_u_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
cost_to_go = []
cost_to_come = []
cost = []
for t in range(l):
- cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
- cost_to_come.append((0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
- cost.append(cost_to_go[-1] + cost_to_come[-1])
- final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
+ cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] +
+ last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
+ cost_to_come.append(
+ (0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] +
+ last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
+ cost.append(cost_to_go[-1] + cost_to_come[-1])
+ final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
for t in range(l):
- A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
- B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
- c_t = discrete_dynamics(last_x_hat_t[t], final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
- print("Infeasability at", t, "is", ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) - last_x_hat_t[t + 1]).T)
+ A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t],
+ final_u_t[t])
+ B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t],
+ final_u_t[t])
+ c_t = discrete_dynamics(
+ last_x_hat_t[t],
+ final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
+ print("Infeasability at", t, "is",
+ ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) -
+ last_x_hat_t[t + 1]).T)
pylab.figure('u')
samples = numpy.arange(len(final_u_t))
for i in range(num_inputs):
- pylab.plot(samples, [u[i, 0] for u in final_u_t], label='u[%d]' % i, marker='o')
- pylab.legend()
+ pylab.plot(samples, [u[i, 0] for u in final_u_t],
+ label='u[%d]' % i,
+ marker='o')
+ pylab.legend()
pylab.figure('cost')
cost_samples = numpy.arange(len(cost))
diff --git a/y2014/control_loops/python/extended_lqr_derivation.py b/y2014/control_loops/python/extended_lqr_derivation.py
index 010c5de..6857654 100755
--- a/y2014/control_loops/python/extended_lqr_derivation.py
+++ b/y2014/control_loops/python/extended_lqr_derivation.py
@@ -6,7 +6,6 @@
import random
import sympy
-
'''
* `x_t1` means `x_{t + 1}`. Using `'x_t + 1'` as the symbol name makes the non-
latex output really confusing, so not doing that.
@@ -57,351 +56,335 @@
ub = sympy.MatrixSymbol('ubold', number_of_inputs, 1)
CONSTANTS = set([
- A_t, B_t, cb_t,
- S_t1, sb_t1, s_t1,
- A_tb, B_tb, cb_tb,
- S_tb, sb_tb, s_tb,
- P_t, Q_t, R_t, qb_t, rb_t, q_t,
- ])
+ A_t,
+ B_t,
+ cb_t,
+ S_t1,
+ sb_t1,
+ s_t1,
+ A_tb,
+ B_tb,
+ cb_tb,
+ S_tb,
+ sb_tb,
+ s_tb,
+ P_t,
+ Q_t,
+ R_t,
+ qb_t,
+ rb_t,
+ q_t,
+])
SYMMETRIC_CONSTANTS = set([
- S_t1, S_tb,
- Q_t, R_t,
- ])
+ S_t1,
+ S_tb,
+ Q_t,
+ R_t,
+])
+
def verify_equivalent(a, b, inverses={}):
- def get_matrices(m):
- matrices = m.atoms(sympy.MatrixSymbol)
- new_matrices = set()
- for matrix in matrices:
- if matrix in inverses:
- new_matrices.update(inverses[matrix].atoms(sympy.MatrixSymbol))
- matrices.update(new_matrices)
- a_matrices, b_matrices = get_matrices(a), get_matrices(b)
- if a_matrices != b_matrices:
- raise RuntimeError('matrices different: %s vs %s' % (a_matrices,
- b_matrices))
- a_symbols, b_symbols = a.atoms(sympy.Symbol), b.atoms(sympy.Symbol)
- if a_symbols != b_symbols:
- raise RuntimeError('symbols different: %s vs %s' % (a_symbols, b_symbols))
- if not a_symbols < DIMENSIONS:
- raise RuntimeError('not sure what to do with %s' % (a_symbols - DIMENSIONS))
- if a.shape != b.shape:
- raise RuntimeError('Different shapes: %s and %s' % (a.shape, b.shape))
+ def get_matrices(m):
+ matrices = m.atoms(sympy.MatrixSymbol)
+ new_matrices = set()
+ for matrix in matrices:
+ if matrix in inverses:
+ new_matrices.update(inverses[matrix].atoms(sympy.MatrixSymbol))
+ matrices.update(new_matrices)
- for _ in range(10):
- subs_symbols = {s: random.randint(1, 5) for s in a_symbols}
+ a_matrices, b_matrices = get_matrices(a), get_matrices(b)
+ if a_matrices != b_matrices:
+ raise RuntimeError('matrices different: %s vs %s' %
+ (a_matrices, b_matrices))
+ a_symbols, b_symbols = a.atoms(sympy.Symbol), b.atoms(sympy.Symbol)
+ if a_symbols != b_symbols:
+ raise RuntimeError('symbols different: %s vs %s' %
+ (a_symbols, b_symbols))
+ if not a_symbols < DIMENSIONS:
+ raise RuntimeError('not sure what to do with %s' %
+ (a_symbols - DIMENSIONS))
+
+ if a.shape != b.shape:
+ raise RuntimeError('Different shapes: %s and %s' % (a.shape, b.shape))
+
for _ in range(10):
- diff = a - b
- subs_matrices = {}
- def get_replacement(*args):
- try:
- m = sympy.MatrixSymbol(*args)
- if m not in subs_matrices:
- if m in inverses:
- i = inverses[m].replace(sympy.MatrixSymbol, get_replacement, simultaneous=False)
- i_evaled = sympy.ImmutableMatrix(i.rows, i.cols,
- lambda x,y: i[x, y].evalf())
- subs_matrices[m] = i_evaled.I
- else:
- rows = m.rows.subs(subs_symbols)
- cols = m.cols.subs(subs_symbols)
- new_m = sympy.ImmutableMatrix(rows, cols,
- lambda i,j: random.uniform(-5, 5))
- if m in SYMMETRIC_CONSTANTS:
- if rows != cols:
- raise RuntimeError('Non-square symmetric matrix %s' % m)
- def calculate_cell(i, j):
- if i > j:
- return new_m[i, j]
- else:
- return new_m[j, i]
- new_m = sympy.ImmutableMatrix(rows, cols, calculate_cell)
- subs_matrices[m] = new_m
- return subs_matrices[m]
- except AttributeError as e:
- # Stupid sympy silently eats AttributeErrors and treats them as
- # "no replacement"...
- raise RuntimeError(e)
- # subs fails when it tries doing matrix multiplies between fixed-size ones
- # and the rest of the equation which still has the symbolic-sized ones.
- # subs(simultaneous=True) wants to put dummies in for everything first,
- # and Dummy().transpose() is broken.
- # replace() has the same issue as subs with simultaneous being True.
- # lambdify() has no idea what to do with the transposes if you replace all
- # the matrices with ones of random sizes full of dummies.
- diff = diff.replace(sympy.MatrixSymbol, get_replacement,
- simultaneous=False)
- for row in range(diff.rows):
- for col in range(diff.cols):
- result = diff[row, col].evalf()
- if abs(result) > 1e-7:
- raise RuntimeError('difference at (%s, %s) is %s' % (row, col,
- result))
+ subs_symbols = {s: random.randint(1, 5) for s in a_symbols}
+ for _ in range(10):
+ diff = a - b
+ subs_matrices = {}
+
+ def get_replacement(*args):
+ try:
+ m = sympy.MatrixSymbol(*args)
+ if m not in subs_matrices:
+ if m in inverses:
+ i = inverses[m].replace(sympy.MatrixSymbol,
+ get_replacement,
+ simultaneous=False)
+ i_evaled = sympy.ImmutableMatrix(
+ i.rows, i.cols, lambda x, y: i[x, y].evalf())
+ subs_matrices[m] = i_evaled.I
+ else:
+ rows = m.rows.subs(subs_symbols)
+ cols = m.cols.subs(subs_symbols)
+ new_m = sympy.ImmutableMatrix(
+ rows, cols, lambda i, j: random.uniform(-5, 5))
+ if m in SYMMETRIC_CONSTANTS:
+ if rows != cols:
+ raise RuntimeError(
+ 'Non-square symmetric matrix %s' % m)
+
+ def calculate_cell(i, j):
+ if i > j:
+ return new_m[i, j]
+ else:
+ return new_m[j, i]
+
+ new_m = sympy.ImmutableMatrix(
+ rows, cols, calculate_cell)
+ subs_matrices[m] = new_m
+ return subs_matrices[m]
+ except AttributeError as e:
+ # Stupid sympy silently eats AttributeErrors and treats them as
+ # "no replacement"...
+ raise RuntimeError(e)
+
+ # subs fails when it tries doing matrix multiplies between fixed-size ones
+ # and the rest of the equation which still has the symbolic-sized ones.
+ # subs(simultaneous=True) wants to put dummies in for everything first,
+ # and Dummy().transpose() is broken.
+ # replace() has the same issue as subs with simultaneous being True.
+ # lambdify() has no idea what to do with the transposes if you replace all
+ # the matrices with ones of random sizes full of dummies.
+ diff = diff.replace(sympy.MatrixSymbol,
+ get_replacement,
+ simultaneous=False)
+ for row in range(diff.rows):
+ for col in range(diff.cols):
+ result = diff[row, col].evalf()
+ if abs(result) > 1e-7:
+ raise RuntimeError('difference at (%s, %s) is %s' %
+ (row, col, result))
+
def verify_arguments(f, *args):
- matrix_atoms = f.atoms(sympy.MatrixSymbol) - CONSTANTS
- if matrix_atoms != set(args):
- print('Arguments expected to be %s, but are %s, in:\n%s' % (
- sorted(args), sorted(list(matrix_atoms)), f), file=sys.stderr)
- raise RuntimeError
+ matrix_atoms = f.atoms(sympy.MatrixSymbol) - CONSTANTS
+ if matrix_atoms != set(args):
+ print('Arguments expected to be %s, but are %s, in:\n%s' %
+ (sorted(args), sorted(list(matrix_atoms)), f),
+ file=sys.stderr)
+ raise RuntimeError
+
def make_c_t():
- x_and_u = sympy.BlockMatrix(((xb,), (ub,)))
- c_t = (half * x_and_u.transpose() *
- sympy.BlockMatrix(((Q_t, P_t.T), (P_t, R_t))) * x_and_u +
- x_and_u.transpose() * sympy.BlockMatrix(((qb_t,), (rb_t,))) +
- q_t)
- verify_arguments(c_t, xb, ub)
- return c_t
+ x_and_u = sympy.BlockMatrix(((xb, ), (ub, )))
+ c_t = (half * x_and_u.transpose() * sympy.BlockMatrix(
+ ((Q_t, P_t.T),
+ (P_t, R_t))) * x_and_u + x_and_u.transpose() * sympy.BlockMatrix(
+ ((qb_t, ), (rb_t, ))) + q_t)
+ verify_arguments(c_t, xb, ub)
+ return c_t
+
def check_backwards_cost():
- g_t = A_t * xb_t + B_t * ub_t + cb_t
- verify_arguments(g_t, xb_t, ub_t)
- v_t1 = half * xb.transpose() * S_t1 * xb + xb.transpose() * sb_t1 + s_t1
- verify_arguments(v_t1, xb)
- v_t = (v_t1.subs(xb, g_t) + make_c_t()).subs({xb_t: xb, ub_t: ub})
- verify_arguments(v_t, xb, ub)
+ g_t = A_t * xb_t + B_t * ub_t + cb_t
+ verify_arguments(g_t, xb_t, ub_t)
+ v_t1 = half * xb.transpose() * S_t1 * xb + xb.transpose() * sb_t1 + s_t1
+ verify_arguments(v_t1, xb)
+ v_t = (v_t1.subs(xb, g_t) + make_c_t()).subs({xb_t: xb, ub_t: ub})
+ verify_arguments(v_t, xb, ub)
- v_t_for_cost = (
- half * (
- xb.transpose() * (A_t.transpose() * S_t1 * A_t + Q_t) * xb +
- ub.transpose() * (B_t.transpose() * S_t1 * A_t + P_t) * xb +
- xb.transpose() * (A_t.transpose() * S_t1 * B_t + P_t.T) * ub +
- ub.transpose() * (B_t.transpose() * S_t1 * B_t + R_t) * ub) +
- xb.transpose() * (A_t.transpose() * sb_t1 + qb_t) +
- ub.transpose() * (B_t.transpose() * sb_t1 + rb_t) +
- cb_t.transpose() * sb_t1 +
- s_t1 + q_t +
- half * (cb_t.transpose() * S_t1 * cb_t +
- xb.transpose() * A_t.transpose() * S_t1 * cb_t +
- ub.transpose() * B_t.transpose() * S_t1 * cb_t +
- cb_t.transpose() * S_t1 * B_t * ub +
- cb_t.transpose() * S_t1 * A_t * xb))
- verify_equivalent(v_t, v_t_for_cost)
+ v_t_for_cost = (
+ half * (xb.transpose() *
+ (A_t.transpose() * S_t1 * A_t + Q_t) * xb + ub.transpose() *
+ (B_t.transpose() * S_t1 * A_t + P_t) * xb + xb.transpose() *
+ (A_t.transpose() * S_t1 * B_t + P_t.T) * ub + ub.transpose() *
+ (B_t.transpose() * S_t1 * B_t + R_t) * ub) + xb.transpose() *
+ (A_t.transpose() * sb_t1 + qb_t) + ub.transpose() *
+ (B_t.transpose() * sb_t1 + rb_t) + cb_t.transpose() * sb_t1 + s_t1 +
+ q_t + half *
+ (cb_t.transpose() * S_t1 * cb_t + xb.transpose() * A_t.transpose() *
+ S_t1 * cb_t + ub.transpose() * B_t.transpose() * S_t1 * cb_t +
+ cb_t.transpose() * S_t1 * B_t * ub +
+ cb_t.transpose() * S_t1 * A_t * xb))
+ verify_equivalent(v_t, v_t_for_cost)
- v_t_now = (
- half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
- ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
- xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
- ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
- xb.T * (A_t.T * sb_t1 + qb_t) +
- ub.T * (B_t.T * sb_t1 + rb_t) +
- cb_t.T * sb_t1 + s_t1 + q_t +
- half * (cb_t.T * S_t1 * cb_t +
- xb.T * A_t.T * S_t1 * cb_t +
- ub.T * B_t.T * S_t1 * cb_t +
- cb_t.T * S_t1 * B_t * ub +
- cb_t.T * S_t1 * A_t * xb))
+ v_t_now = (
+ half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+ (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+ (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+ (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+ (A_t.T * sb_t1 + qb_t) + ub.T * (B_t.T * sb_t1 + rb_t) +
+ cb_t.T * sb_t1 + s_t1 + q_t + half *
+ (cb_t.T * S_t1 * cb_t + xb.T * A_t.T * S_t1 * cb_t + ub.T * B_t.T *
+ S_t1 * cb_t + cb_t.T * S_t1 * B_t * ub + cb_t.T * S_t1 * A_t * xb))
- v_t_now = (
- half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
- ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
- xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
- ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
- xb.T * (A_t.T * sb_t1 + qb_t + half * A_t.T * S_t1 * cb_t) +
- ub.T * (B_t.T * sb_t1 + rb_t + half * B_t.T * S_t1 * cb_t) +
- half * cb_t.T * S_t1 * (A_t * xb + B_t * ub + cb_t) +
- cb_t.T * sb_t1 + s_t1 + q_t)
+ v_t_now = (half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+ (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+ (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+ (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+ (A_t.T * sb_t1 + qb_t + half * A_t.T * S_t1 * cb_t) + ub.T *
+ (B_t.T * sb_t1 + rb_t + half * B_t.T * S_t1 * cb_t) +
+ half * cb_t.T * S_t1 * (A_t * xb + B_t * ub + cb_t) +
+ cb_t.T * sb_t1 + s_t1 + q_t)
- v_t_now = (
- half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
- ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
- xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
- ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
- xb.T * (A_t.T * sb_t1 + qb_t + A_t.T * S_t1 * cb_t) +
- ub.T * (B_t.T * sb_t1 + rb_t + B_t.T * S_t1 * cb_t) +
- half * cb_t.T * S_t1 * cb_t +
- cb_t.T * sb_t1 + s_t1 + q_t)
+ v_t_now = (half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+ (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+ (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+ (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+ (A_t.T * sb_t1 + qb_t + A_t.T * S_t1 * cb_t) + ub.T *
+ (B_t.T * sb_t1 + rb_t + B_t.T * S_t1 * cb_t) +
+ half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1 + q_t)
- C_t = B_t.T * S_t1 * A_t + P_t
- E_t = B_t.T * S_t1 * B_t + R_t
- E_t_I = sympy.MatrixSymbol('E_t^-1', E_t.cols, E_t.rows)
- L_t = -E_t_I * C_t
- eb_t = B_t.T * S_t1 * cb_t + B_t.T * sb_t1 + rb_t
- lb_t = -E_t_I * eb_t
- D_t = A_t.T * S_t1 * A_t + Q_t
- db_t = A_t.T * S_t1 * cb_t + A_t.T * sb_t1 + qb_t
+ C_t = B_t.T * S_t1 * A_t + P_t
+ E_t = B_t.T * S_t1 * B_t + R_t
+ E_t_I = sympy.MatrixSymbol('E_t^-1', E_t.cols, E_t.rows)
+ L_t = -E_t_I * C_t
+ eb_t = B_t.T * S_t1 * cb_t + B_t.T * sb_t1 + rb_t
+ lb_t = -E_t_I * eb_t
+ D_t = A_t.T * S_t1 * A_t + Q_t
+ db_t = A_t.T * S_t1 * cb_t + A_t.T * sb_t1 + qb_t
- v_t_now = (
- half * (xb.T * D_t * xb + ub.T * C_t * xb +
- xb.T * C_t.T * ub + ub.T * E_t * ub) +
- xb.T * db_t + ub.T * eb_t +
- half * cb_t.T * S_t1 * cb_t +
- cb_t.T * sb_t1 + s_t1 + q_t)
+ v_t_now = (half * (xb.T * D_t * xb + ub.T * C_t * xb + xb.T * C_t.T * ub +
+ ub.T * E_t * ub) + xb.T * db_t + ub.T * eb_t +
+ half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1 + q_t)
- v_t_final = (
- half * xb.T * (D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t) * xb +
- xb.T * (C_t.T * lb_t + L_t.T * E_t * lb_t + db_t + L_t.T * eb_t) +
- half * lb_t.T * E_t * lb_t +
- lb_t.T * eb_t +
- cb_t.T * sb_t1 + s_t1 + q_t + half * cb_t.T * S_t1 * cb_t
- )
- verify_arguments(v_t_final, xb, E_t_I)
- verify_equivalent(v_t.subs(ub, L_t * xb + lb_t), v_t_final, {E_t_I: E_t})
+ v_t_final = (half * xb.T *
+ (D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t) * xb +
+ xb.T *
+ (C_t.T * lb_t + L_t.T * E_t * lb_t + db_t + L_t.T * eb_t) +
+ half * lb_t.T * E_t * lb_t + lb_t.T * eb_t + cb_t.T * sb_t1 +
+ s_t1 + q_t + half * cb_t.T * S_t1 * cb_t)
+ verify_arguments(v_t_final, xb, E_t_I)
+ verify_equivalent(v_t.subs(ub, L_t * xb + lb_t), v_t_final, {E_t_I: E_t})
- def v_t_from_s(this_S_t, this_sb_t, this_s_t):
- return half * xb.T * this_S_t * xb + xb.T * this_sb_t + this_s_t
+ def v_t_from_s(this_S_t, this_sb_t, this_s_t):
+ return half * xb.T * this_S_t * xb + xb.T * this_sb_t + this_s_t
- S_t_new_first = D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t
- sb_t_new_first = db_t - C_t.T * E_t_I * eb_t
- s_t_new_first = (half * lb_t.T * E_t * lb_t +
- lb_t.T * eb_t +
- cb_t.T * sb_t1 +
- s_t1 + q_t +
- half * cb_t.T * S_t1 * cb_t)
- verify_equivalent(v_t_from_s(S_t_new_first, sb_t_new_first, s_t_new_first),
- v_t_final, {E_t_I: E_t})
+ S_t_new_first = D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t
+ sb_t_new_first = db_t - C_t.T * E_t_I * eb_t
+ s_t_new_first = (half * lb_t.T * E_t * lb_t + lb_t.T * eb_t +
+ cb_t.T * sb_t1 + s_t1 + q_t + half * cb_t.T * S_t1 * cb_t)
+ verify_equivalent(v_t_from_s(S_t_new_first, sb_t_new_first, s_t_new_first),
+ v_t_final, {E_t_I: E_t})
- S_t_new_end = D_t - C_t.T * E_t_I * C_t
- sb_t_new_end = db_t - C_t.T * E_t_I * eb_t
- s_t_new_end = (q_t - half * eb_t.T * E_t_I * eb_t +
- half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1)
- verify_equivalent(v_t_from_s(S_t_new_end, sb_t_new_end, s_t_new_end),
- v_t_final, {E_t_I: E_t})
+ S_t_new_end = D_t - C_t.T * E_t_I * C_t
+ sb_t_new_end = db_t - C_t.T * E_t_I * eb_t
+ s_t_new_end = (q_t - half * eb_t.T * E_t_I * eb_t +
+ half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1)
+ verify_equivalent(v_t_from_s(S_t_new_end, sb_t_new_end, s_t_new_end),
+ v_t_final, {E_t_I: E_t})
+
def check_forwards_cost():
- v_tb = half * xb.T * S_tb * xb + xb.T * sb_tb + s_tb
- verify_arguments(v_tb, xb)
- g_tb = A_tb * xb_t1 + B_tb * ub + cb_tb
- verify_arguments(g_tb, xb_t1, ub)
- c_t1b = make_c_t().subs(xb, g_tb)
- verify_arguments(c_t1b, xb_t1, ub)
- v_t1b = v_tb.subs(xb, g_tb) + c_t1b
- verify_arguments(v_t1b, xb_t1, ub)
+ v_tb = half * xb.T * S_tb * xb + xb.T * sb_tb + s_tb
+ verify_arguments(v_tb, xb)
+ g_tb = A_tb * xb_t1 + B_tb * ub + cb_tb
+ verify_arguments(g_tb, xb_t1, ub)
+ c_t1b = make_c_t().subs(xb, g_tb)
+ verify_arguments(c_t1b, xb_t1, ub)
+ v_t1b = v_tb.subs(xb, g_tb) + c_t1b
+ verify_arguments(v_t1b, xb_t1, ub)
- v_t1b_now = (
- half * g_tb.T * S_tb * g_tb +
- g_tb.T * sb_tb + s_tb +
- half * (g_tb.T * Q_t * g_tb +
- ub.T * P_t * g_tb +
- g_tb.T * P_t.T * ub +
- ub.T * R_t * ub) +
- g_tb.T * qb_t + ub.T * rb_t + q_t)
+ v_t1b_now = (half * g_tb.T * S_tb * g_tb + g_tb.T * sb_tb + s_tb + half *
+ (g_tb.T * Q_t * g_tb + ub.T * P_t * g_tb +
+ g_tb.T * P_t.T * ub + ub.T * R_t * ub) + g_tb.T * qb_t +
+ ub.T * rb_t + q_t)
- v_t1b_for_cost = (
- half * (xb_t1.T * A_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
- xb_t1.T * A_tb.T * (S_tb + Q_t) * B_tb * ub +
- xb_t1.T * A_tb.T * (S_tb + Q_t) * cb_tb +
- ub.T * B_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
- ub.T * B_tb.T * (S_tb + Q_t) * B_tb * ub +
- ub.T * B_tb.T * (S_tb + Q_t) * cb_tb +
- cb_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
- cb_tb.T * (S_tb + Q_t) * B_tb * ub +
- cb_tb.T * (S_tb + Q_t) * cb_tb) +
- xb_t1.T * A_tb.T * sb_tb +
- ub.T * B_tb.T * sb_tb +
- cb_tb.T * sb_tb +
- s_tb +
- half * (ub.T * P_t * A_tb * xb_t1 +
- ub.T * P_t * B_tb * ub +
- ub.T * P_t * cb_tb) +
- half * (xb_t1.T * A_tb.T * P_t.T * ub +
- ub.T * B_tb.T * P_t.T * ub +
- cb_tb.T * P_t.T * ub) +
- half * ub.T * R_t * ub +
- xb_t1.T * A_tb.T * qb_t + ub.T * B_tb.T * qb_t + cb_tb.T * qb_t +
- ub.T * rb_t + q_t)
- verify_equivalent(v_t1b, v_t1b_for_cost)
+ v_t1b_for_cost = (half * (xb_t1.T * A_tb.T *
+ (S_tb + Q_t) * A_tb * xb_t1 + xb_t1.T * A_tb.T *
+ (S_tb + Q_t) * B_tb * ub + xb_t1.T * A_tb.T *
+ (S_tb + Q_t) * cb_tb + ub.T * B_tb.T *
+ (S_tb + Q_t) * A_tb * xb_t1 + ub.T * B_tb.T *
+ (S_tb + Q_t) * B_tb * ub + ub.T * B_tb.T *
+ (S_tb + Q_t) * cb_tb + cb_tb.T *
+ (S_tb + Q_t) * A_tb * xb_t1 + cb_tb.T *
+ (S_tb + Q_t) * B_tb * ub + cb_tb.T *
+ (S_tb + Q_t) * cb_tb) +
+ xb_t1.T * A_tb.T * sb_tb + ub.T * B_tb.T * sb_tb +
+ cb_tb.T * sb_tb + s_tb + half *
+ (ub.T * P_t * A_tb * xb_t1 + ub.T * P_t * B_tb * ub +
+ ub.T * P_t * cb_tb) + half *
+ (xb_t1.T * A_tb.T * P_t.T * ub +
+ ub.T * B_tb.T * P_t.T * ub + cb_tb.T * P_t.T * ub) +
+ half * ub.T * R_t * ub + xb_t1.T * A_tb.T * qb_t +
+ ub.T * B_tb.T * qb_t + cb_tb.T * qb_t + ub.T * rb_t +
+ q_t)
+ verify_equivalent(v_t1b, v_t1b_for_cost)
- S_and_Q = S_tb + Q_t
+ S_and_Q = S_tb + Q_t
- v_t1b_now = (
- half * (xb_t1.T * A_tb.T * S_and_Q * A_tb * xb_t1 +
- xb_t1.T * A_tb.T * S_and_Q * B_tb * ub +
- xb_t1.T * A_tb.T * S_and_Q * cb_tb +
- ub.T * B_tb.T * S_and_Q * A_tb * xb_t1 +
- ub.T * B_tb.T * S_and_Q * B_tb * ub +
- ub.T * B_tb.T * S_and_Q * cb_tb +
- cb_tb.T * S_and_Q * A_tb * xb_t1 +
- cb_tb.T * S_and_Q * B_tb * ub +
- cb_tb.T * S_and_Q * cb_tb) +
- xb_t1.T * A_tb.T * sb_tb +
- ub.T * B_tb.T * sb_tb +
- cb_tb.T * sb_tb +
- s_tb +
- half * (ub.T * P_t * A_tb * xb_t1 +
- ub.T * P_t * B_tb * ub +
- ub.T * P_t * cb_tb) +
- half * (xb_t1.T * A_tb.T * P_t.T * ub +
- ub.T * B_tb.T * P_t.T * ub +
- cb_tb.T * P_t.T * ub) +
- half * ub.T * R_t * ub +
- xb_t1.T * A_tb.T * qb_t +
- ub.T * B_tb.T * qb_t +
- cb_tb.T * qb_t +
- ub.T * rb_t +
- q_t)
+ v_t1b_now = (
+ half *
+ (xb_t1.T * A_tb.T * S_and_Q * A_tb * xb_t1 + xb_t1.T * A_tb.T *
+ S_and_Q * B_tb * ub + xb_t1.T * A_tb.T * S_and_Q * cb_tb + ub.T *
+ B_tb.T * S_and_Q * A_tb * xb_t1 + ub.T * B_tb.T * S_and_Q * B_tb * ub
+ + ub.T * B_tb.T * S_and_Q * cb_tb + cb_tb.T * S_and_Q * A_tb * xb_t1 +
+ cb_tb.T * S_and_Q * B_tb * ub + cb_tb.T * S_and_Q * cb_tb) +
+ xb_t1.T * A_tb.T * sb_tb + ub.T * B_tb.T * sb_tb + cb_tb.T * sb_tb +
+ s_tb + half * (ub.T * P_t * A_tb * xb_t1 + ub.T * P_t * B_tb * ub +
+ ub.T * P_t * cb_tb) + half *
+ (xb_t1.T * A_tb.T * P_t.T * ub + ub.T * B_tb.T * P_t.T * ub +
+ cb_tb.T * P_t.T * ub) + half * ub.T * R_t * ub +
+ xb_t1.T * A_tb.T * qb_t + ub.T * B_tb.T * qb_t + cb_tb.T * qb_t +
+ ub.T * rb_t + q_t)
- C_tb = B_tb.T * S_and_Q * A_tb + P_t * A_tb
- E_tb = B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t
- E_tb_I = sympy.MatrixSymbol('Ebar_t^-1', E_tb.cols, E_tb.rows)
- L_tb = -E_tb_I * C_tb
- eb_tb = B_tb.T * S_and_Q * cb_tb + B_tb.T * sb_tb + P_t * cb_tb + B_tb.T * qb_t + rb_t
- lb_tb = -E_tb_I * eb_tb
- D_tb = A_tb.T * S_and_Q * A_tb
- db_tb = A_tb.T * S_and_Q * cb_tb + A_tb.T * (sb_tb + qb_t)
+ C_tb = B_tb.T * S_and_Q * A_tb + P_t * A_tb
+ E_tb = B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t
+ E_tb_I = sympy.MatrixSymbol('Ebar_t^-1', E_tb.cols, E_tb.rows)
+ L_tb = -E_tb_I * C_tb
+ eb_tb = B_tb.T * S_and_Q * cb_tb + B_tb.T * sb_tb + P_t * cb_tb + B_tb.T * qb_t + rb_t
+ lb_tb = -E_tb_I * eb_tb
+ D_tb = A_tb.T * S_and_Q * A_tb
+ db_tb = A_tb.T * S_and_Q * cb_tb + A_tb.T * (sb_tb + qb_t)
- v_t1b_now = (
- half * (xb_t1.T * D_tb * xb_t1 +
- xb_t1.T * C_tb.T * ub +
- ub.T * C_tb * xb_t1 +
- ub.T * E_tb * ub) +
- xb_t1.T * db_tb +
- ub.T * eb_tb +
- half * cb_tb.T * S_and_Q * cb_tb +
- cb_tb.T * sb_tb +
- cb_tb.T * qb_t +
- s_tb + q_t)
+ v_t1b_now = (half * (xb_t1.T * D_tb * xb_t1 + xb_t1.T * C_tb.T * ub +
+ ub.T * C_tb * xb_t1 + ub.T * E_tb * ub) +
+ xb_t1.T * db_tb + ub.T * eb_tb +
+ half * cb_tb.T * S_and_Q * cb_tb + cb_tb.T * sb_tb +
+ cb_tb.T * qb_t + s_tb + q_t)
- v_t1b_final = (
- half * xb_t1.T * (D_tb - C_tb.T * E_tb_I * C_tb) * xb_t1 +
- xb_t1.T * (db_tb - C_tb.T * E_tb_I * eb_tb) +
- -half * eb_tb.T * E_tb_I * eb_tb +
- half * cb_tb.T * S_and_Q * cb_tb +
- cb_tb.T * sb_tb +
- cb_tb.T * qb_t +
- s_tb + q_t)
- verify_arguments(v_t1b_final, xb_t1, E_tb_I)
- verify_equivalent(v_t1b.subs(ub, -E_tb_I * C_tb * xb_t1 - E_tb_I * eb_tb),
- v_t1b_final, {E_tb_I: E_tb})
+ v_t1b_final = (half * xb_t1.T * (D_tb - C_tb.T * E_tb_I * C_tb) * xb_t1 +
+ xb_t1.T * (db_tb - C_tb.T * E_tb_I * eb_tb) +
+ -half * eb_tb.T * E_tb_I * eb_tb +
+ half * cb_tb.T * S_and_Q * cb_tb + cb_tb.T * sb_tb +
+ cb_tb.T * qb_t + s_tb + q_t)
+ verify_arguments(v_t1b_final, xb_t1, E_tb_I)
+ verify_equivalent(v_t1b.subs(ub, -E_tb_I * C_tb * xb_t1 - E_tb_I * eb_tb),
+ v_t1b_final, {E_tb_I: E_tb})
+
def check_forwards_u():
- S_and_Q = S_tb + Q_t
+ S_and_Q = S_tb + Q_t
- diff_start = (
- half * (xb_t1.T * A_tb.T * S_and_Q * B_tb +
- (B_tb.T * S_and_Q * A_tb * xb_t1).T +
- 2 * ub.T * B_tb.T * S_and_Q * B_tb +
- (B_tb.T * S_and_Q * cb_tb).T +
- cb_tb.T * S_and_Q * B_tb) +
- sb_tb.T * B_tb +
- half * (P_t * A_tb * xb_t1).T +
- half * xb_t1.T * A_tb.T * P_t.T +
- half * ub.T * (P_t * B_tb + B_tb.T * P_t.T) +
- half * ub.T * (B_tb.T * P_t.T + P_t * B_tb) +
- half * (P_t * cb_tb).T +
- half * cb_tb.T * P_t.T +
- ub.T * R_t +
- (B_tb.T * qb_t).T + rb_t.T)
- verify_arguments(diff_start, xb_t1, ub)
+ diff_start = (half *
+ (xb_t1.T * A_tb.T * S_and_Q * B_tb +
+ (B_tb.T * S_and_Q * A_tb * xb_t1).T +
+ 2 * ub.T * B_tb.T * S_and_Q * B_tb +
+ (B_tb.T * S_and_Q * cb_tb).T + cb_tb.T * S_and_Q * B_tb) +
+ sb_tb.T * B_tb + half * (P_t * A_tb * xb_t1).T +
+ half * xb_t1.T * A_tb.T * P_t.T + half * ub.T *
+ (P_t * B_tb + B_tb.T * P_t.T) + half * ub.T *
+ (B_tb.T * P_t.T + P_t * B_tb) + half * (P_t * cb_tb).T +
+ half * cb_tb.T * P_t.T + ub.T * R_t + (B_tb.T * qb_t).T +
+ rb_t.T)
+ verify_arguments(diff_start, xb_t1, ub)
- diff_end = (
- xb_t1.T * (A_tb.T * S_and_Q * B_tb + A_tb.T * P_t.T) +
- ub.T * (B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t) +
- cb_tb.T * S_and_Q * B_tb +
- sb_tb.T * B_tb +
- cb_tb.T * P_t.T +
- qb_t.T * B_tb +
- rb_t.T)
- verify_equivalent(diff_start, diff_end)
+ diff_end = (xb_t1.T * (A_tb.T * S_and_Q * B_tb + A_tb.T * P_t.T) + ub.T *
+ (B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t) +
+ cb_tb.T * S_and_Q * B_tb + sb_tb.T * B_tb + cb_tb.T * P_t.T +
+ qb_t.T * B_tb + rb_t.T)
+ verify_equivalent(diff_start, diff_end)
+
def main():
- sympy.init_printing(use_unicode=True)
- check_backwards_cost()
- check_forwards_cost()
- check_forwards_u()
+ sympy.init_printing(use_unicode=True)
+ check_backwards_cost()
+ check_forwards_cost()
+ check_forwards_u()
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index cdaee1b..05b6658 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2014',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2014', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014/control_loops/python/polydrivetrain_test.py b/y2014/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2014/control_loops/python/polydrivetrain_test.py
+++ b/y2014/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index c3e8d86..e3ad903 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -264,8 +264,9 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter(
- "Shooter", [sprung_shooter, shooter], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter("Shooter",
+ [sprung_shooter, shooter],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant("kMaxExtension", "%f",
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index 2990773..955cf4c 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -11,31 +11,34 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(J = 1.8,
- mass = 37,
- robot_radius = 0.45 / 2.0,
- wheel_radius = 0.04445,
- G_high = 28.0 / 48.0 * 19.0 / 50.0,
- G_low = 28.0 / 60.0 * 19.0 / 50.0,
- q_pos_low = 0.12,
- q_pos_high = 0.14,
- q_vel_low = 1.0,
- q_vel_high = 0.95,
- has_imu = False,
- dt = 0.005,
- controller_poles = [0.83, 0.83])
+kDrivetrain = drivetrain.DrivetrainParams(J=1.8,
+ mass=37,
+ robot_radius=0.45 / 2.0,
+ wheel_radius=0.04445,
+ G_high=28.0 / 48.0 * 19.0 / 50.0,
+ G_low=28.0 / 60.0 * 19.0 / 50.0,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
+ has_imu=False,
+ dt=0.005,
+ controller_poles=[0.83, 0.83])
+
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print("Expected .h file name and .cc file name")
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014_bot3', kDrivetrain)
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print("Expected .h file name and .cc file name")
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014_bot3',
+ kDrivetrain)
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 08e5583..881b2b0 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -12,20 +12,23 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
- 'y2014_bot3', drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2014_bot3',
+ drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014_bot3/control_loops/python/polydrivetrain_test.py b/y2014_bot3/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain_test.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 3c47934..0b5d87c 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -44,8 +44,8 @@
self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
self.A_continuous[0:2, 0:2] = self._shoulder.A_continuous
- self.A_continuous[2:4, 0:2] = (
- self._shoulder.A_continuous - self._shooter.A_continuous)
+ self.A_continuous[2:4, 0:2] = (self._shoulder.A_continuous -
+ self._shooter.A_continuous)
self.A_continuous[2:4, 2:4] = self._shooter.A_continuous
self.B_continuous = numpy.matrix(numpy.zeros((4, 2)))
@@ -131,8 +131,11 @@
self.R[0, 0] = r_voltage**2.0
self.R[1, 1] = r_voltage**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.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.U_max = numpy.matrix([[12.0], [12.0]])
@@ -182,8 +185,11 @@
self.R[0, 0] = r_pos**2.0
self.R[1, 1] = 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.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.K_unaugmented = self.K
@@ -365,20 +371,19 @@
pylab.plot(self.t, self.x_shooter, label='x shooter')
pylab.plot(self.t, self.x_hat_shooter, label='x_hat shooter')
pylab.plot(self.t, self.goal_x_shooter, label='goal x shooter')
- pylab.plot(
- self.t,
- map(operator.add, self.x_shooter, self.x_shoulder),
- label='x shooter ground')
- pylab.plot(
- self.t,
- map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
- label='x_hat shooter ground')
+ pylab.plot(self.t,
+ map(operator.add, self.x_shooter, self.x_shoulder),
+ label='x shooter ground')
+ pylab.plot(self.t,
+ map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
+ label='x_hat shooter ground')
pylab.legend()
pylab.subplot(3, 1, 2)
pylab.plot(self.t, self.u_shoulder, label='u shoulder')
- pylab.plot(
- self.t, self.offset_shoulder, label='voltage_offset shoulder')
+ pylab.plot(self.t,
+ self.offset_shoulder,
+ label='voltage_offset shoulder')
pylab.plot(self.t, self.u_shooter, label='u shooter')
pylab.plot(self.t, self.offset_shooter, label='voltage_offset shooter')
pylab.legend()
@@ -401,8 +406,8 @@
J_decelerating = 7
arm = Arm(name='AcceleratingArm', J=J_accelerating)
- arm_integral_controller = IntegralArm(
- name='AcceleratingIntegralArm', J=J_accelerating)
+ arm_integral_controller = IntegralArm(name='AcceleratingIntegralArm',
+ J=J_accelerating)
arm_observer = IntegralArm()
# Test moving the shoulder with constant separation.
@@ -418,12 +423,11 @@
arm.X = initial_X[0:4, 0]
arm_observer.X = initial_X
- scenario_plotter.run_test(
- arm=arm,
- end_goal=R,
- iterations=300,
- controller=arm_integral_controller,
- observer=arm_observer)
+ scenario_plotter.run_test(arm=arm,
+ end_goal=R,
+ iterations=300,
+ controller=arm_integral_controller,
+ observer=arm_observer)
if len(argv) != 5:
glog.fatal(
@@ -432,8 +436,9 @@
else:
namespaces = ['y2016', 'control_loops', 'superstructure']
decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
- loop_writer = control_loop.ControlLoopWriter(
- 'Arm', [arm, decelerating_arm], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Arm',
+ [arm, decelerating_arm],
+ namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
decelerating_integral_arm_controller = IntegralArm(
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index 8abdd74..a0205e7 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -11,31 +11,34 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(J = 2.0,
- mass = 68,
- robot_radius = 0.601 / 2.0,
- wheel_radius = 0.097155 * 0.9811158901447808 / 118.0 * 115.75,
- G_high = 14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0,
- G_low = 14.0 / 48.0 * 18.0 / 60.0 * 18.0 / 36.0,
- q_pos_low = 0.12,
- q_pos_high = 0.14,
- q_vel_low = 1.0,
- q_vel_high = 0.95,
- has_imu = False,
- dt = 0.005,
- controller_poles = [0.67, 0.67])
+kDrivetrain = drivetrain.DrivetrainParams(
+ J=2.0,
+ mass=68,
+ robot_radius=0.601 / 2.0,
+ wheel_radius=0.097155 * 0.9811158901447808 / 118.0 * 115.75,
+ G_high=14.0 / 48.0 * 28.0 / 50.0 * 18.0 / 36.0,
+ G_low=14.0 / 48.0 * 18.0 / 60.0 * 18.0 / 36.0,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
+ has_imu=False,
+ dt=0.005,
+ controller_poles=[0.67, 0.67])
+
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print("Expected .h file name and .cc file name")
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2016', kDrivetrain)
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print("Expected .h file name and .cc file name")
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2016', kDrivetrain)
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/polydrivetrain.py b/y2016/control_loops/python/polydrivetrain.py
index 1b7a0ba..69a7be9 100755
--- a/y2016/control_loops/python/polydrivetrain.py
+++ b/y2016/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2016',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2016', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2016/control_loops/python/polydrivetrain_test.py b/y2016/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2016/control_loops/python/polydrivetrain_test.py
+++ b/y2016/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index 70af1b0..cde9c8d 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -135,8 +135,11 @@
r_pos = 0.05
self.R = numpy.matrix([[(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.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.K_unaugmented = self.K
@@ -256,12 +259,11 @@
initial_X = numpy.matrix([[0.0], [0.0]])
R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(
- shooter,
- goal=R,
- controller_shooter=shooter_controller,
- observer_shooter=observer_shooter,
- iterations=200)
+ scenario_plotter.run_test(shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter,
+ iterations=200)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -271,8 +273,8 @@
else:
namespaces = ['y2016', 'control_loops', 'shooter']
shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter(
- 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
+ namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
integral_shooter = IntegralShooter('IntegralShooter')
diff --git a/y2016/control_loops/python/shoulder.py b/y2016/control_loops/python/shoulder.py
index 7f46b2c..d406089 100755
--- a/y2016/control_loops/python/shoulder.py
+++ b/y2016/control_loops/python/shoulder.py
@@ -12,155 +12,168 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Shoulder(control_loop.ControlLoop):
- def __init__(self, name="Shoulder", J=None):
- super(Shoulder, 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) * (42.0 / 12.0)
+ def __init__(self, name="Shoulder", J=None):
+ super(Shoulder, 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
- if J is None:
- self.J = 10.0
- else:
- self.J = J
+ # 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) * (42.0 / 12.0)
- # Control loop time step
- self.dt = 0.005
+ if J is None:
+ self.J = 10.0
+ else:
+ self.J = J
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
- C2 = self.Kt * self.G / (self.J * self.R)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
+ C2 = self.Kt * self.G / (self.J * self.R)
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0],
- [C2]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix([[0], [C2]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- controllability = controls.ctrb(self.A, self.B)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- q_pos = 0.16
- q_vel = 0.95
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel ** 2.0))]])
+ controllability = controls.ctrb(self.A, self.B)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ q_pos = 0.16
+ q_vel = 0.95
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel**2.0))]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+ 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)
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
- q_pos = 0.05
- q_vel = 2.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ glog.debug('Poles are %s for %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
+ self._name)
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.05
+ q_vel = 2.65
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0], [0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
- # 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.L = self.A * self.KalmanGain
- self.InitializeState()
+ # 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()
+
class IntegralShoulder(Shoulder):
- def __init__(self, name="IntegralShoulder"):
- super(IntegralShoulder, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralShoulder"):
+ super(IntegralShoulder, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.08
- q_vel = 4.00
- q_voltage = 6.0
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.08
+ q_vel = 4.00
+ q_voltage = 6.0
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**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
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ 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()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, shoulder, goal, iterations=200, controller_shoulder=None,
- observer_shoulder=None):
- """Runs the shoulder plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self,
+ shoulder,
+ goal,
+ iterations=200,
+ controller_shoulder=None,
+ observer_shoulder=None):
+ """Runs the shoulder plant with an initial condition and goal.
Test for whether the goal has been reached and whether the separation
goes outside of the initial and goal values by more than
@@ -178,100 +191,106 @@
use the actual state.
"""
- if controller_shoulder is None:
- controller_shoulder = shoulder
+ if controller_shoulder is None:
+ controller_shoulder = shoulder
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + shoulder.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + shoulder.dt
+ else:
+ initial_t = 0
- for i in range(iterations):
- X_hat = shoulder.X
+ for i in range(iterations):
+ X_hat = shoulder.X
- if observer_shoulder is not None:
- X_hat = observer_shoulder.X_hat
- self.x_hat.append(observer_shoulder.X_hat[0, 0])
+ if observer_shoulder is not None:
+ X_hat = observer_shoulder.X_hat
+ self.x_hat.append(observer_shoulder.X_hat[0, 0])
- U = controller_shoulder.K * (goal - X_hat)
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(shoulder.X[0, 0])
+ U = controller_shoulder.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shoulder.X[0, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(shoulder.X[1, 0])
- self.a.append((self.v[-1] - last_v) / shoulder.dt)
+ self.v.append(shoulder.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / shoulder.dt)
- if observer_shoulder is not None:
- observer_shoulder.Y = shoulder.Y
- observer_shoulder.CorrectObserver(U)
- self.offset.append(observer_shoulder.X_hat[2, 0])
+ if observer_shoulder is not None:
+ observer_shoulder.Y = shoulder.Y
+ observer_shoulder.CorrectObserver(U)
+ self.offset.append(observer_shoulder.X_hat[2, 0])
- shoulder.Update(U + 2.0)
+ shoulder.Update(U + 2.0)
- if observer_shoulder is not None:
- observer_shoulder.PredictObserver(U)
+ if observer_shoulder is not None:
+ observer_shoulder.PredictObserver(U)
- self.t.append(initial_t + i * shoulder.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * shoulder.dt)
+ self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
+ glog.debug('Time: %f', self.t[-1])
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- shoulder = Shoulder()
- shoulder_controller = IntegralShoulder()
- observer_shoulder = IntegralShoulder()
+ shoulder = Shoulder()
+ shoulder_controller = IntegralShoulder()
+ observer_shoulder = IntegralShoulder()
- # Test moving the shoulder with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[numpy.pi / 2.0], [0.0], [0.0]])
- scenario_plotter.run_test(shoulder, goal=R, controller_shoulder=shoulder_controller,
- observer_shoulder=observer_shoulder, iterations=200)
+ # Test moving the shoulder with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0], [0.0]])
+ scenario_plotter.run_test(shoulder,
+ goal=R,
+ controller_shoulder=shoulder_controller,
+ observer_shoulder=observer_shoulder,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the shoulder and integral shoulder.')
- else:
- namespaces = ['y2016', 'control_loops', 'superstructure']
- shoulder = Shoulder("Shoulder")
- loop_writer = control_loop.ControlLoopWriter('Shoulder', [shoulder],
- namespaces=namespaces)
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the shoulder and integral shoulder.'
+ )
+ else:
+ namespaces = ['y2016', 'control_loops', 'superstructure']
+ shoulder = Shoulder("Shoulder")
+ loop_writer = control_loop.ControlLoopWriter('Shoulder', [shoulder],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_shoulder = IntegralShoulder("IntegralShoulder")
- integral_loop_writer = control_loop.ControlLoopWriter("IntegralShoulder", [integral_shoulder],
- namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_shoulder = IntegralShoulder("IntegralShoulder")
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ "IntegralShoulder", [integral_shoulder], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index ca6874b..12dcc39 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -11,152 +11,165 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Wrist(control_loop.ControlLoop):
- 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)
+ 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
- self.J = 0.35
+ # 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)
- # Control loop time step
- self.dt = 0.005
+ self.J = 0.35
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
- C2 = self.Kt * self.G / (self.J * self.R)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
+ C2 = self.Kt * self.G / (self.J * self.R)
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0],
- [C2]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix([[0], [C2]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- controllability = controls.ctrb(self.A, self.B)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- 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))]])
+ controllability = controls.ctrb(self.A, self.B)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ 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))]])
- glog.debug('Poles are %s for %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]), self._name)
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- q_pos = 0.05
- q_vel = 2.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ glog.debug('Poles are %s for %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
+ self._name)
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.05
+ q_vel = 2.65
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0], [0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
- # 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.L = self.A * self.KalmanGain
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+ # 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()
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
+
+ self.InitializeState()
+
class IntegralWrist(Wrist):
- def __init__(self, name="IntegralWrist"):
- super(IntegralWrist, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralWrist"):
+ super(IntegralWrist, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.08
- q_vel = 4.00
- q_voltage = 1.5
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.08
+ q_vel = 4.00
+ q_voltage = 1.5
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**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
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ 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()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, wrist, goal, iterations=200, controller_wrist=None,
- observer_wrist=None):
- """Runs the wrist plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self,
+ wrist,
+ goal,
+ iterations=200,
+ controller_wrist=None,
+ observer_wrist=None):
+ """Runs the wrist plant with an initial condition and goal.
Test for whether the goal has been reached and whether the separation
goes outside of the initial and goal values by more than
@@ -174,99 +187,105 @@
use the actual state.
"""
- if controller_wrist is None:
- controller_wrist = wrist
+ if controller_wrist is None:
+ controller_wrist = wrist
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + wrist.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + wrist.dt
+ else:
+ initial_t = 0
- for i in range(iterations):
- X_hat = wrist.X
+ for i in range(iterations):
+ X_hat = wrist.X
- if observer_wrist is not None:
- X_hat = observer_wrist.X_hat
- self.x_hat.append(observer_wrist.X_hat[0, 0])
+ if observer_wrist is not None:
+ X_hat = observer_wrist.X_hat
+ self.x_hat.append(observer_wrist.X_hat[0, 0])
- U = controller_wrist.K * (goal - X_hat)
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(wrist.X[0, 0])
+ U = controller_wrist.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(wrist.X[0, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(wrist.X[1, 0])
- self.a.append((self.v[-1] - last_v) / wrist.dt)
+ self.v.append(wrist.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / wrist.dt)
- if observer_wrist is not None:
- observer_wrist.Y = wrist.Y
- observer_wrist.CorrectObserver(U)
- self.offset.append(observer_wrist.X_hat[2, 0])
+ if observer_wrist is not None:
+ observer_wrist.Y = wrist.Y
+ observer_wrist.CorrectObserver(U)
+ self.offset.append(observer_wrist.X_hat[2, 0])
- wrist.Update(U + 2.0)
+ wrist.Update(U + 2.0)
- if observer_wrist is not None:
- observer_wrist.PredictObserver(U)
+ if observer_wrist is not None:
+ observer_wrist.PredictObserver(U)
- self.t.append(initial_t + i * wrist.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * wrist.dt)
+ self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
+ glog.debug('Time: %f', self.t[-1])
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- wrist = Wrist()
- wrist_controller = IntegralWrist()
- observer_wrist = IntegralWrist()
+ wrist = Wrist()
+ wrist_controller = IntegralWrist()
+ observer_wrist = IntegralWrist()
- # Test moving the wrist with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0]])
- scenario_plotter.run_test(wrist, goal=R, controller_wrist=wrist_controller,
- observer_wrist=observer_wrist, iterations=200)
+ # Test moving the wrist with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ scenario_plotter.run_test(wrist,
+ goal=R,
+ controller_wrist=wrist_controller,
+ observer_wrist=observer_wrist,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
- else:
- namespaces = ['y2016', 'control_loops', 'superstructure']
- wrist = Wrist('Wrist')
- loop_writer = control_loop.ControlLoopWriter(
- 'Wrist', [wrist], namespaces=namespaces)
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2016', 'control_loops', 'superstructure']
+ wrist = Wrist('Wrist')
+ loop_writer = control_loop.ControlLoopWriter('Wrist', [wrist],
+ namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_wrist = IntegralWrist('IntegralWrist')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralWrist', [integral_wrist], namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_wrist = IntegralWrist('IntegralWrist')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralWrist', [integral_wrist], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
index 0122b58..7573c16 100755
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -37,12 +37,12 @@
self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
- self.A_continuous[0, 0] = -(
- self.indexer.Kt / self.indexer.Kv /
- (self.indexer.J * self.indexer.resistance * self.indexer.G *
- self.indexer.G) + self.turret.Kt / self.turret.Kv /
- (self.indexer.J * self.turret.resistance * self.turret.G *
- self.turret.G))
+ self.A_continuous[0, 0] = -(self.indexer.Kt / self.indexer.Kv /
+ (self.indexer.J * self.indexer.resistance *
+ self.indexer.G * self.indexer.G) +
+ self.turret.Kt / self.turret.Kv /
+ (self.indexer.J * self.turret.resistance *
+ self.turret.G * self.turret.G))
self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (
self.indexer.J * self.turret.resistance * self.turret.G *
self.turret.G)
@@ -142,8 +142,11 @@
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.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()
@@ -209,8 +212,11 @@
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.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()
@@ -282,12 +288,13 @@
self.x_hat.append(observer_column.X_hat[0, 0])
next_goal = numpy.concatenate(
- (end_goal[0:2, :], profile.Update(
- end_goal[2, 0], end_goal[3, 0]), end_goal[4:6, :]),
+ (end_goal[0:2, :],
+ profile.Update(end_goal[2, 0],
+ end_goal[3, 0]), end_goal[4:6, :]),
axis=0)
- ff_U = controller_column.Kff * (
- next_goal - observer_column.A * goal)
+ ff_U = controller_column.Kff * (next_goal -
+ observer_column.A * goal)
fb_U = controller_column.K * (goal - observer_column.X_hat)
self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
self.ui_fb.append(fb_U[0, 0])
@@ -373,12 +380,11 @@
initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
- scenario_plotter.run_test(
- column,
- end_goal=R,
- controller_column=column_controller,
- observer_column=observer_column,
- iterations=400)
+ scenario_plotter.run_test(column,
+ end_goal=R,
+ controller_column=column_controller,
+ observer_column=observer_column,
+ iterations=400)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -388,8 +394,8 @@
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
column = Column('Column')
- loop_writer = control_loop.ControlLoopWriter(
- 'Column', [column], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Column', [column],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kIndexerFreeSpeed', '%f',
column.indexer.free_speed))
@@ -405,15 +411,15 @@
# IntegralColumn controller 1 will disable the indexer.
integral_column = IntegralColumn('IntegralColumn')
- disabled_integral_column = IntegralColumn(
- 'DisabledIntegralColumn', disable_indexer=True)
+ disabled_integral_column = IntegralColumn('DisabledIntegralColumn',
+ disable_indexer=True)
integral_loop_writer = control_loop.ControlLoopWriter(
'IntegralColumn', [integral_column, disabled_integral_column],
namespaces=namespaces)
integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_column = IntegralColumn(
- 'StuckIntegralColumn', voltage_error_noise=8.0)
+ stuck_integral_column = IntegralColumn('StuckIntegralColumn',
+ voltage_error_noise=8.0)
stuck_integral_loop_writer = control_loop.ControlLoopWriter(
'StuckIntegralColumn', [stuck_integral_column],
namespaces=namespaces)
diff --git a/y2017/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
index 652fcf4..b1ebcd8 100755
--- a/y2017/control_loops/python/drivetrain.py
+++ b/y2017/control_loops/python/drivetrain.py
@@ -11,17 +11,17 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(
- J=6.0,
- mass=52,
- robot_radius=0.59055 / 2.0,
- wheel_radius=0.08255 / 2.0,
- G=11.0 / 60.0,
- q_pos_low=0.12,
- q_pos_high=0.14,
- q_vel_low=1.0,
- q_vel_high=0.95,
- has_imu=False)
+kDrivetrain = drivetrain.DrivetrainParams(J=6.0,
+ mass=52,
+ robot_radius=0.59055 / 2.0,
+ wheel_radius=0.08255 / 2.0,
+ G=11.0 / 60.0,
+ q_pos_low=0.12,
+ q_pos_high=0.14,
+ q_vel_low=1.0,
+ q_vel_high=0.95,
+ has_imu=False)
+
def main(argv):
argv = FLAGS(argv)
@@ -35,5 +35,6 @@
# Write the generated constants out to a file.
drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2017', kDrivetrain)
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index c405bb2..c77d134 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -118,8 +118,11 @@
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.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
@@ -165,8 +168,11 @@
r_pos = 0.001
self.R = numpy.matrix([[(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.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.K_unaugmented = self.K
@@ -244,7 +250,8 @@
ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
- U_uncapped = controller_hood.K * (goal - observer_hood.X_hat) + ff_U
+ U_uncapped = controller_hood.K * (goal -
+ observer_hood.X_hat) + ff_U
U = U_uncapped.copy()
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(hood.X[0, 0])
@@ -310,12 +317,11 @@
# Test moving the hood with constant separation.
initial_X = numpy.matrix([[0.0], [0.0]])
R = numpy.matrix([[numpy.pi / 4.0], [0.0], [0.0]])
- scenario_plotter.run_test(
- hood,
- end_goal=R,
- controller_hood=hood_controller,
- observer_hood=observer_hood,
- iterations=200)
+ scenario_plotter.run_test(hood,
+ end_goal=R,
+ controller_hood=hood_controller,
+ observer_hood=observer_hood,
+ iterations=200)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -328,8 +334,8 @@
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'hood']
hood = Hood('Hood')
- loop_writer = control_loop.ControlLoopWriter(
- 'Hood', [hood], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Hood', [hood],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f', hood.free_speed))
loop_writer.AddConstant(
@@ -340,7 +346,8 @@
integral_loop_writer = control_loop.ControlLoopWriter(
'IntegralHood', [integral_hood], namespaces=namespaces)
integral_loop_writer.AddConstant(
- control_loop.Constant('kLastReduction', '%f', integral_hood.last_G))
+ control_loop.Constant('kLastReduction', '%f',
+ integral_hood.last_G))
integral_loop_writer.Write(argv[3], argv[4])
diff --git a/y2017/control_loops/python/indexer.py b/y2017/control_loops/python/indexer.py
index 7312e57..d4c3fe6 100755
--- a/y2017/control_loops/python/indexer.py
+++ b/y2017/control_loops/python/indexer.py
@@ -12,188 +12,203 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
gflags.DEFINE_bool('stall', False, 'If true, stall the indexer.')
+
class VelocityIndexer(control_loop.ControlLoop):
- 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)
+ 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
- # Motor inertia in kg m^2
- self.motor_inertia = 0.00001187
+ 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)
- # 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
+ # Motor inertia in kg m^2
+ self.motor_inertia = 0.00001187
- # 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
+ # 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
- # 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]])
+ # 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
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # 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.PlaceControllerPoles([.75])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceObserverPoles([0.3])
+ self.PlaceControllerPoles([.75])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
class Indexer(VelocityIndexer):
- def __init__(self, name='Indexer'):
- super(Indexer, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='Indexer'):
+ super(Indexer, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
- self.A_continuous[0, 1] = 1
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.rpl = .45
- self.ipl = 0.07
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.K[0, 1:2] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 2)))
- self.Kff[0, 1:2] = self.Kff_unaugmented
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl])
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class IntegralIndexer(Indexer):
- def __init__(self, name="IntegralIndexer", voltage_error_noise=None):
- super(IntegralIndexer, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralIndexer", voltage_error_noise=None):
+ super(IntegralIndexer, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.01
- q_vel = 2.0
- q_voltage = 0.6
- if voltage_error_noise is not None:
- q_voltage = voltage_error_noise
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ q_pos = 0.01
+ q_vel = 2.0
+ q_voltage = 0.6
+ if voltage_error_noise is not None:
+ q_voltage = voltage_error_noise
- r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**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
+ r_pos = 0.001
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ 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()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.stall_ratio = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, indexer, goal, iterations=200, controller_indexer=None,
- observer_indexer=None):
- """Runs the indexer plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.stall_ratio = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self,
+ indexer,
+ goal,
+ iterations=200,
+ controller_indexer=None,
+ observer_indexer=None):
+ """Runs the indexer plant with an initial condition and goal.
Args:
indexer: Indexer object to use.
@@ -205,141 +220,148 @@
should use the actual state.
"""
- if controller_indexer is None:
- controller_indexer = indexer
+ if controller_indexer is None:
+ controller_indexer = indexer
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + indexer.dt
- else:
- initial_t = 0
-
- for i in range(iterations):
- X_hat = indexer.X
-
- if observer_indexer is not None:
- X_hat = observer_indexer.X_hat
- observer_indexer.Y = indexer.Y
- observer_indexer.CorrectObserver(numpy.matrix([[0.0]]))
- self.x_hat.append(observer_indexer.X_hat[1, 0])
- self.offset.append(observer_indexer.X_hat[2, 0])
-
- ff_U = controller_indexer.Kff * (goal - observer_indexer.A * goal)
-
- U = controller_indexer.K * (goal - X_hat) + ff_U
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(indexer.X[0, 0])
-
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
-
- self.v.append(indexer.X[1, 0])
- self.a.append((self.v[-1] - last_v) / indexer.dt)
-
- applied_U = U.copy()
- if i >= 40:
- applied_U -= 2
-
- if FLAGS.stall and i >= 40:
- indexer.X[1, 0] = 0.0
- else:
- indexer.Update(applied_U)
-
- if observer_indexer is not None:
- clipped_u = U[0, 0]
- clip_u_value = 3.0
- if clipped_u < 0:
- clipped_u = min(clipped_u, -clip_u_value)
+ if self.t:
+ initial_t = self.t[-1] + indexer.dt
else:
- clipped_u = max(clipped_u, clip_u_value)
+ initial_t = 0
- self.stall_ratio.append(10 * (-self.offset[-1] / clipped_u))
+ for i in range(iterations):
+ X_hat = indexer.X
- observer_indexer.PredictObserver(U)
+ if observer_indexer is not None:
+ X_hat = observer_indexer.X_hat
+ observer_indexer.Y = indexer.Y
+ observer_indexer.CorrectObserver(numpy.matrix([[0.0]]))
+ self.x_hat.append(observer_indexer.X_hat[1, 0])
+ self.offset.append(observer_indexer.X_hat[2, 0])
- self.t.append(initial_t + i * indexer.dt)
- self.u.append(U[0, 0])
+ ff_U = controller_indexer.Kff * (goal - observer_indexer.A * goal)
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.v, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ U = controller_indexer.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(indexer.X[0, 0])
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.plot(self.t, self.stall_ratio, label='stall_ratio')
- pylab.plot(self.t,
- [10.0 if x > 6.0 else 0.0 for x in self.stall_ratio],
- label='is_stalled')
- pylab.legend()
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ self.v.append(indexer.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / indexer.dt)
- pylab.show()
+ applied_U = U.copy()
+ if i >= 40:
+ applied_U -= 2
+
+ if FLAGS.stall and i >= 40:
+ indexer.X[1, 0] = 0.0
+ else:
+ indexer.Update(applied_U)
+
+ if observer_indexer is not None:
+ clipped_u = U[0, 0]
+ clip_u_value = 3.0
+ if clipped_u < 0:
+ clipped_u = min(clipped_u, -clip_u_value)
+ else:
+ clipped_u = max(clipped_u, clip_u_value)
+
+ self.stall_ratio.append(10 * (-self.offset[-1] / clipped_u))
+
+ observer_indexer.PredictObserver(U)
+
+ self.t.append(initial_t + i * indexer.dt)
+ self.u.append(U[0, 0])
+
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.plot(self.t, self.stall_ratio, label='stall_ratio')
+ pylab.plot(self.t,
+ [10.0 if x > 6.0 else 0.0 for x in self.stall_ratio],
+ label='is_stalled')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- indexer = Indexer()
- indexer_controller = IntegralIndexer()
- observer_indexer = IntegralIndexer()
+ indexer = Indexer()
+ indexer_controller = IntegralIndexer()
+ observer_indexer = IntegralIndexer()
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [20.0], [0.0]])
- scenario_plotter.run_test(indexer, goal=R, controller_indexer=indexer_controller,
- observer_indexer=observer_indexer, iterations=200)
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [20.0], [0.0]])
+ scenario_plotter.run_test(indexer,
+ goal=R,
+ controller_indexer=indexer_controller,
+ observer_indexer=observer_indexer,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- indexer = Indexer()
- indexer_controller = IntegralIndexer(voltage_error_noise=1.5)
- observer_indexer = IntegralIndexer(voltage_error_noise=1.5)
+ indexer = Indexer()
+ indexer_controller = IntegralIndexer(voltage_error_noise=1.5)
+ observer_indexer = IntegralIndexer(voltage_error_noise=1.5)
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [20.0], [0.0]])
- scenario_plotter.run_test(indexer, goal=R, controller_indexer=indexer_controller,
- observer_indexer=observer_indexer, iterations=200)
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [20.0], [0.0]])
+ scenario_plotter.run_test(indexer,
+ goal=R,
+ controller_indexer=indexer_controller,
+ observer_indexer=observer_indexer,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- if len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file names')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'indexer']
- indexer = Indexer('Indexer')
- loop_writer = control_loop.ControlLoopWriter('Indexer', [indexer],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', indexer.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', indexer.G))
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file names')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'indexer']
+ indexer = Indexer('Indexer')
+ loop_writer = control_loop.ControlLoopWriter('Indexer', [indexer],
+ namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', indexer.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', indexer.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_indexer = IntegralIndexer('IntegralIndexer')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralIndexer', [integral_indexer], namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_indexer = IntegralIndexer('IntegralIndexer')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralIndexer', [integral_indexer], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer',
- voltage_error_noise=1.5)
- stuck_integral_loop_writer = control_loop.ControlLoopWriter(
- 'StuckIntegralIndexer', [stuck_integral_indexer], namespaces=namespaces)
- stuck_integral_loop_writer.Write(argv[5], argv[6])
+ stuck_integral_indexer = IntegralIndexer('StuckIntegralIndexer',
+ voltage_error_noise=1.5)
+ stuck_integral_loop_writer = control_loop.ControlLoopWriter(
+ 'StuckIntegralIndexer', [stuck_integral_indexer],
+ namespaces=namespaces)
+ stuck_integral_loop_writer.Write(argv[5], argv[6])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
index 498a2c3..e181ef5 100755
--- a/y2017/control_loops/python/polydrivetrain.py
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2017',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2017', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/polydrivetrain_test.py b/y2017/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2017/control_loops/python/polydrivetrain_test.py
+++ b/y2017/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index be4fb81..47b7217 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -204,8 +204,8 @@
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('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
@@ -220,15 +220,17 @@
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.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.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
@@ -363,13 +365,12 @@
shooter_controller = IntegralShooter()
observer_shooter_hybrid = IntegralShooter()
- scenario_plotter_int.run_test(
- shooter,
- goal=R,
- controller_shooter=shooter_controller,
- observer_shooter=observer_shooter_hybrid,
- iterations=iterations,
- hybrid_obs=True)
+ scenario_plotter_int.run_test(shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid,
+ iterations=iterations,
+ hybrid_obs=True)
scenario_plotter_int.Plot()
@@ -380,8 +381,8 @@
else:
namespaces = ['y2017', 'control_loops', 'superstructure', 'shooter']
shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter(
- 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f', shooter.free_speed))
loop_writer.AddConstant(
diff --git a/y2017/control_loops/python/turret.py b/y2017/control_loops/python/turret.py
index e67904d..6407133 100755
--- a/y2017/control_loops/python/turret.py
+++ b/y2017/control_loops/python/turret.py
@@ -12,166 +12,176 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Turret(control_loop.ControlLoop):
- def __init__(self, name='Turret'):
- super(Turret, 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
- # Resistance of the motor
- 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
- # Gear ratio
- self.G = (12.0 / 60.0) * (11.0 / 94.0)
+ def __init__(self, name='Turret'):
+ super(Turret, 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
- # Motor inertia in kg * m^2
- self.motor_inertia = 0.00001187
+ # Resistance of the motor
+ 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
+ # Gear ratio
+ self.G = (12.0 / 60.0) * (11.0 / 94.0)
- # Moment of inertia, measured in CAD.
- # Extra mass to compensate for friction is added on.
- self.J = 0.06 + self.motor_inertia * ((1.0 / self.G) ** 2.0)
- glog.debug('Turret J is: %f', self.J)
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.00001187
- # Control loop time step
- self.dt = 0.005
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.06 + self.motor_inertia * ((1.0 / self.G)**2.0)
+ glog.debug('Turret J is: %f', self.J)
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.Kt / (self.resistance * self.J * self.Kv * self.G * self.G)
- C2 = self.Kt / (self.J * self.resistance * self.G)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.Kt / (self.resistance * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.resistance * self.G)
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0],
- [C2]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix([[0], [C2]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- controllability = controls.ctrb(self.A, self.B)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- glog.debug('Free speed is %f',
- -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+ controllability = controls.ctrb(self.A, self.B)
- # Calculate the LQR controller gain
- 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))]])
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ # Calculate the LQR controller gain
+ 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))]])
- # Calculate the feed forwards gain.
- 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.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ # Calculate the feed forwards gain.
+ 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))]])
- q_pos = 0.10
- q_vel = 1.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.10
+ q_vel = 1.65
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0], [0.0, (q_vel**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
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- # 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.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()
+ # 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()
+
class IntegralTurret(Turret):
- def __init__(self, name='IntegralTurret'):
- super(IntegralTurret, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='IntegralTurret'):
+ super(IntegralTurret, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.12
- q_vel = 2.00
- q_voltage = 3.0
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.12
+ q_vel = 2.00
+ q_voltage = 3.0
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**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
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
+ 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.Kff = numpy.concatenate((self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
- self.InitializeState()
+ self.Kff = numpy.concatenate(
+ (self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+
+ self.InitializeState()
+
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, turret, end_goal,
- controller_turret,
- observer_turret=None,
- iterations=200):
- """Runs the turret plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self,
+ turret,
+ end_goal,
+ controller_turret,
+ observer_turret=None,
+ iterations=200):
+ """Runs the turret plant with an initial condition and goal.
Args:
turret: turret object to use.
@@ -183,130 +193,138 @@
iterations: Number of timesteps to run the model for.
"""
- if controller_turret is None:
- controller_turret = turret
+ if controller_turret is None:
+ controller_turret = turret
- vbat = 12.0
+ vbat = 12.0
- if self.t:
- initial_t = self.t[-1] + turret.dt
- else:
- initial_t = 0
+ if self.t:
+ initial_t = self.t[-1] + turret.dt
+ else:
+ initial_t = 0
- goal = numpy.concatenate((turret.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((turret.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- profile = TrapezoidProfile(turret.dt)
- profile.set_maximum_acceleration(100.0)
- profile.set_maximum_velocity(7.0)
- profile.SetGoal(goal[0, 0])
+ profile = TrapezoidProfile(turret.dt)
+ profile.set_maximum_acceleration(100.0)
+ profile.set_maximum_velocity(7.0)
+ profile.SetGoal(goal[0, 0])
- U_last = numpy.matrix(numpy.zeros((1, 1)))
- for i in range(iterations):
- observer_turret.Y = turret.Y
- observer_turret.CorrectObserver(U_last)
+ U_last = numpy.matrix(numpy.zeros((1, 1)))
+ for i in range(iterations):
+ observer_turret.Y = turret.Y
+ observer_turret.CorrectObserver(U_last)
- self.offset.append(observer_turret.X_hat[2, 0])
- self.x_hat.append(observer_turret.X_hat[0, 0])
+ self.offset.append(observer_turret.X_hat[2, 0])
+ self.x_hat.append(observer_turret.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)
+ 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_turret.Kff * (next_goal - observer_turret.A * goal)
+ ff_U = controller_turret.Kff * (next_goal -
+ observer_turret.A * goal)
- U_uncapped = controller_turret.K * (goal - observer_turret.X_hat) + ff_U
- U_uncapped = controller_turret.K * (end_goal - observer_turret.X_hat)
- U = U_uncapped.copy()
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(turret.X[0, 0])
+ U_uncapped = controller_turret.K * (goal -
+ observer_turret.X_hat) + ff_U
+ U_uncapped = controller_turret.K * (end_goal -
+ observer_turret.X_hat)
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(turret.X[0, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(turret.X[1, 0])
- self.a.append((self.v[-1] - last_v) / turret.dt)
+ self.v.append(turret.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / turret.dt)
- offset = 0.0
- if i > 100:
- offset = 2.0
- turret.Update(U + offset)
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ turret.Update(U + offset)
- observer_turret.PredictObserver(U)
+ observer_turret.PredictObserver(U)
- self.t.append(initial_t + i * turret.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * turret.dt)
+ self.u.append(U[0, 0])
- ff_U -= U_uncapped - U
- goal = controller_turret.A * goal + controller_turret.B * ff_U
+ ff_U -= U_uncapped - U
+ goal = controller_turret.A * goal + controller_turret.B * ff_U
- if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ 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_turret.X_hat - end_goal))
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_turret.X_hat - end_goal))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- turret = Turret()
- turret_controller = IntegralTurret()
- observer_turret = IntegralTurret()
+ turret = Turret()
+ turret_controller = IntegralTurret()
+ observer_turret = IntegralTurret()
- # Test moving the turret with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[numpy.pi/2.0], [0.0], [0.0]])
- scenario_plotter.run_test(turret, end_goal=R,
- controller_turret=turret_controller,
- observer_turret=observer_turret, iterations=200)
+ # Test moving the turret with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0], [0.0]])
+ scenario_plotter.run_test(turret,
+ end_goal=R,
+ controller_turret=turret_controller,
+ observer_turret=observer_turret,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the turret and integral turret.')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'turret']
- turret = Turret('Turret')
- loop_writer = control_loop.ControlLoopWriter('Turret', [turret],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', turret.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', turret.G))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the turret and integral turret.'
+ )
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'turret']
+ turret = Turret('Turret')
+ loop_writer = control_loop.ControlLoopWriter('Turret', [turret],
+ namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', turret.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', turret.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_turret = IntegralTurret('IntegralTurret')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralTurret', [integral_turret],
- namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_turret = IntegralTurret('IntegralTurret')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralTurret', [integral_turret], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2018/control_loops/python/arm_distal.py b/y2018/control_loops/python/arm_distal.py
index 2836c50..d443f8a 100755
--- a/y2018/control_loops/python/arm_distal.py
+++ b/y2018/control_loops/python/arm_distal.py
@@ -3,7 +3,7 @@
# This code was used to select the gear ratio for the distal arm.
# Run it from the command line and it displays the time required
# to move the distal arm 60 degrees.
-#
+#
# Michael Schuh
# January 20, 2018
@@ -14,206 +14,238 @@
# apt-get install python-scipy python3-scipy python-numpy python3-numpy
pi = math.pi
-pi2 = 2.0*pi
-rad_to_deg = 180.0/pi
+pi2 = 2.0 * pi
+rad_to_deg = 180.0 / pi
inches_to_meters = 0.0254
-lbs_to_kg = 1.0/2.2
+lbs_to_kg = 1.0 / 2.2
newton_to_lbf = 0.224809
newton_meters_to_ft_lbs = 0.73756
run_count = 0
theta_travel = 0.0
+
def to_deg(angle):
- return (angle*rad_to_deg)
+ return (angle * rad_to_deg)
+
def to_rad(angle):
- return (angle/rad_to_deg)
+ return (angle / rad_to_deg)
+
def to_rotations(angle):
- return (angle/pi2)
+ return (angle / pi2)
+
def time_derivative(x, t, voltage, c1, c2, c3):
- global run_count
- theta, omega = x
- dxdt = [omega, -c1*omega + c3*math.sin(theta) + c2*voltage]
- run_count = run_count + 1
+ global run_count
+ theta, omega = x
+ dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
+ run_count = run_count + 1
- #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
- return dxdt
-
+ #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
+ return dxdt
-def get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed):
- #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
- #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
- global run_count
- global theta_travel
- if ( False ):
- # Gravity is assisting the motion.
- theta_start = 0.0
- theta_target = pi
- elif ( False ):
- # Gravity is assisting the motion.
- theta_start = 0.0
- theta_target = -pi
- elif ( False ):
- # Gravity is slowing the motion.
- theta_start = pi
- theta_target = 0.0
- elif ( True ):
- # Gravity is slowing the motion.
- theta_start = -pi
- theta_target = 0.0
+def get_180_degree_time(c1, c2, c3, voltage, gear_ratio, motor_free_speed):
+ #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
+ #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
+ global run_count
+ global theta_travel
- theta_half = 0.5*(theta_start + theta_target)
- if (theta_start > theta_target):
- voltage = -voltage
- theta = theta_start
- theta_travel = theta_start - theta_target
- if ( run_count == 0 ):
- print ("# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (theta_start,theta_target,theta_travel,theta_half, voltage))
- print ("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (to_deg(theta_start),to_deg(theta_target),to_deg(theta_travel),to_deg(theta_half), voltage))
- omega = 0.0
- time = 0.0
- delta_time = 0.01 # time step in seconds
- for step in range(1, 5000):
- t = numpy.array([time, time + delta_time])
- time = time + delta_time
- x = [theta, omega]
- angular_acceleration = -c1*omega + c2*voltage
- x_n_plus_1 = scipy.integrate.odeint(time_derivative,x,t,args=(voltage,c1,c2,c3))
- #print ('x_n_plus_1 = ',x_n_plus_1)
- #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
- theta, omega = x_n_plus_1[1]
- #theta= x_n_plus_1[0]
- #omega = x_n_plus_1[1]
- if ( False ):
- print ("%4d %8.4f %8.2f %8.4f %8.4f %8.3f %8.3f %8.3f %8.3f" % \
- (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
- to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
- if (theta_start < theta_target):
- # Angle is increasing through the motion.
- if (theta > theta_half):
- break
- else:
- # Angle is decreasing through the motion.
- if (theta < theta_half):
- break
-
- #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
- #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
- #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
- return (2.0*time)
+ if (False):
+ # Gravity is assisting the motion.
+ theta_start = 0.0
+ theta_target = pi
+ elif (False):
+ # Gravity is assisting the motion.
+ theta_start = 0.0
+ theta_target = -pi
+ elif (False):
+ # Gravity is slowing the motion.
+ theta_start = pi
+ theta_target = 0.0
+ elif (True):
+ # Gravity is slowing the motion.
+ theta_start = -pi
+ theta_target = 0.0
+
+ theta_half = 0.5 * (theta_start + theta_target)
+ if (theta_start > theta_target):
+ voltage = -voltage
+ theta = theta_start
+ theta_travel = theta_start - theta_target
+ if (run_count == 0):
+ print(
+ "# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+ % (theta_start, theta_target, theta_travel, theta_half, voltage))
+ print(
+ "# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+ % (to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
+ to_deg(theta_half), voltage))
+ omega = 0.0
+ time = 0.0
+ delta_time = 0.01 # time step in seconds
+ for step in range(1, 5000):
+ t = numpy.array([time, time + delta_time])
+ time = time + delta_time
+ x = [theta, omega]
+ angular_acceleration = -c1 * omega + c2 * voltage
+ x_n_plus_1 = scipy.integrate.odeint(time_derivative,
+ x,
+ t,
+ args=(voltage, c1, c2, c3))
+ #print ('x_n_plus_1 = ',x_n_plus_1)
+ #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
+ theta, omega = x_n_plus_1[1]
+ #theta= x_n_plus_1[0]
+ #omega = x_n_plus_1[1]
+ if (False):
+ print ("%4d %8.4f %8.2f %8.4f %8.4f %8.3f %8.3f %8.3f %8.3f" % \
+ (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
+ to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
+ if (theta_start < theta_target):
+ # Angle is increasing through the motion.
+ if (theta > theta_half):
+ break
+ else:
+ # Angle is decreasing through the motion.
+ if (theta < theta_half):
+ break
+
+ #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
+ #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
+ #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
+ return (2.0 * time)
+
def main():
- gravity = 9.8 # m/sec^2 Gravity Constant
- voltage_nominal = 12 # Volts
-
- # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
- motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
- current_stall = 134 # amps stall current
- current_no_load = 0.7 # amps no load current
- torque_stall = 710/1000.0 # N-m Stall Torque
- speed_no_load_rpm = 18730 # RPM no load speed
-
- if ( False ):
- # Bag motor from https://www.vexrobotics.com/217-3351.html
- motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
- current_stall = 53.0 # amps stall current
- current_no_load = 1.8 # amps no load current
- torque_stall = 0.4 # N-m Stall Torque
- speed_no_load_rpm = 13180.0 # RPM no load speed
-
- if ( True ):
- # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
- motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
- current_stall = 89.0 # amps stall current
- current_no_load = 3.0 # amps no load current
- torque_stall = 1.4 # N-m Stall Torque
- speed_no_load_rpm = 5840.0 # RPM no load speed
+ gravity = 9.8 # m/sec^2 Gravity Constant
+ voltage_nominal = 12 # Volts
- # How many motors are we using?
- num_motors = 2
+ # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
+ motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
+ current_stall = 134 # amps stall current
+ current_no_load = 0.7 # amps no load current
+ torque_stall = 710 / 1000.0 # N-m Stall Torque
+ speed_no_load_rpm = 18730 # RPM no load speed
- # Motor values
- print ("# Motor: %s" % (motor_name))
- print ("# Number of motors: %d" % (num_motors))
- print ("# Stall torque: %.1f n-m" % (torque_stall))
- print ("# Stall current: %.1f amps" % (current_stall))
- print ("# No load current: %.1f amps" % (current_no_load))a
- print ("# No load speed: %.0f rpm" % (speed_no_load_rpm))
-
- # Constants from motor values
- resistance_motor = voltage_nominal/current_stall
- speed_no_load_rps = speed_no_load_rpm/60.0 # Revolutions per second no load speed
- speed_no_load = speed_no_load_rps*2.0*pi
- Kt = num_motors*torque_stall/current_stall # N-m/A torque constant
- Kv_rpm = speed_no_load_rpm /(voltage_nominal - resistance_motor*current_no_load) # rpm/V
- Kv = Kv_rpm*2.0*pi/60.0 # rpm/V
-
- # Robot Geometry and physics
- length_proximal_arm = inches_to_meters*47.34 # m Length of arm connected to the robot base
- length_distal_arm = inches_to_meters*44.0 # m Length of arm that holds the cube
- mass_cube = 6.0*lbs_to_kg # Weight of the cube in Kgrams
- mass_proximal_arm = 5.5*lbs_to_kg # Weight of proximal arm
- mass_distal_arm = 3.5*lbs_to_kg # Weight of distal arm
- mass_distal = mass_cube + mass_distal_arm
- radius_to_proximal_arm_cg = 22.0*inches_to_meters # m Length from arm pivot point to arm CG
- radius_to_distal_arm_cg = 10.0*inches_to_meters # m Length from arm pivot point to arm CG
+ if (False):
+ # Bag motor from https://www.vexrobotics.com/217-3351.html
+ motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
+ current_stall = 53.0 # amps stall current
+ current_no_load = 1.8 # amps no load current
+ torque_stall = 0.4 # N-m Stall Torque
+ speed_no_load_rpm = 13180.0 # RPM no load speed
- radius_to_distal_cg = ( length_distal_arm*mass_cube + radius_to_distal_arm_cg*mass_distal_arm)/mass_distal
- J_cube = length_distal_arm*length_distal_arm*mass_cube
- J_proximal_arm = radius_to_proximal_arm_cg*radius_to_proximal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the proximal arm
- J_distal_arm = radius_to_distal_arm_cg*radius_to_distal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
- J = J_cube + J_distal_arm # Moment of inertia of the arm with the cube on the end
+ if (True):
+ # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
+ motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
+ current_stall = 89.0 # amps stall current
+ current_no_load = 3.0 # amps no load current
+ torque_stall = 1.4 # N-m Stall Torque
+ speed_no_load_rpm = 5840.0 # RPM no load speed
- error_margine = 1.0
- voltage = 10.0 # voltage for the motor. Assuming a loaded robot so not using 12 V.
- # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
- # motor_free_speed = Kv*voltage
- motor_free_speed = speed_no_load
-
-
- print ("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
- print ("# %.2f Ohms Resistance of the motor " % (resistance_motor))
- print ("# %.2f kg Cube weight" % (mass_cube))
- print ("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
- print ("# %.2f kg Distal Arm mass" % (mass_distal_arm))
- print ("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
- print ("# %.2f m Length from distal arm pivot point to arm CG" % (radius_to_distal_arm_cg))
- print ("# %.2f m Length from distal arm pivot point to arm and cube cg" % (radius_to_distal_cg))
- print ("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
- print ("# %.2f kg-m^2 Moment of inertia of the arm with the cube on the end" % (J))
- print ("# %.2f m Proximal arm length" % (length_proximal_arm))
- print ("# %.2f m Distal arm length" % (length_distal_arm))
+ # How many motors are we using?
+ num_motors = 2
- print ("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (J_proximal_arm))
- print ("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (J_distal_arm))
- print ("# %.2f kg-m^2 Moment of inertia of the distal arm and cube about the arm pivot point" % (J))
- print ("# %d Number of motors" % (num_motors))
-
- print ("# %.2f V Motor voltage" % (voltage))
- for gear_ratio in range(30, 181, 10):
- c1 = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J)
- c2 = gear_ratio*Kt/(J*resistance_motor)
- c3 = radius_to_distal_cg*mass_distal*gravity/J
-
- if ( False ):
- print ("# %.8f 1/sec C1 constant" % (c1))
- print ("# %.2f 1/sec C2 constant" % (c2))
- print ("# %.2f 1/(V sec^2) C3 constant" % (c3))
- print ("# %.2f RPM Free speed at motor voltage" % (voltage*Kv_rpm))
-
- torque_90_degrees = radius_to_distal_cg*mass_distal*gravity
- voltage_90_degrees = resistance_motor*torque_90_degrees/(gear_ratio*Kt)
- torque_peak = gear_ratio*num_motors*torque_stall
- torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
- normal_force = torque_peak/length_distal_arm
- normal_force_lbf = newton_to_lbf*normal_force
- time_required = get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed)
- print ("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds. 90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at distal end %3.0f N %2.0f lbf" % \
- (to_deg(theta_travel),gear_ratio,time_required,torque_90_degrees,voltage_90_degrees,
- torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf))
-
+ # Motor values
+ print("# Motor: %s" % (motor_name))
+ print("# Number of motors: %d" % (num_motors))
+ print("# Stall torque: %.1f n-m" % (torque_stall))
+ print("# Stall current: %.1f amps" % (current_stall))
+ print("# No load current: %.1f amps" % (current_no_load))
+ print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
+
+ # Constants from motor values
+ resistance_motor = voltage_nominal / current_stall
+ speed_no_load_rps = speed_no_load_rpm / 60.0 # Revolutions per second no load speed
+ speed_no_load = speed_no_load_rps * 2.0 * pi
+ Kt = num_motors * torque_stall / current_stall # N-m/A torque constant
+ Kv_rpm = speed_no_load_rpm / (
+ voltage_nominal - resistance_motor * current_no_load) # rpm/V
+ Kv = Kv_rpm * 2.0 * pi / 60.0 # rpm/V
+
+ # Robot Geometry and physics
+ length_proximal_arm = inches_to_meters * 47.34 # m Length of arm connected to the robot base
+ length_distal_arm = inches_to_meters * 44.0 # m Length of arm that holds the cube
+ mass_cube = 6.0 * lbs_to_kg # Weight of the cube in Kgrams
+ mass_proximal_arm = 5.5 * lbs_to_kg # Weight of proximal arm
+ mass_distal_arm = 3.5 * lbs_to_kg # Weight of distal arm
+ mass_distal = mass_cube + mass_distal_arm
+ radius_to_proximal_arm_cg = 22.0 * inches_to_meters # m Length from arm pivot point to arm CG
+ radius_to_distal_arm_cg = 10.0 * inches_to_meters # m Length from arm pivot point to arm CG
+
+ radius_to_distal_cg = (
+ length_distal_arm * mass_cube +
+ radius_to_distal_arm_cg * mass_distal_arm) / mass_distal
+ J_cube = length_distal_arm * length_distal_arm * mass_cube
+ J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * mass_distal_arm # Kg m^2 Moment of inertia of the proximal arm
+ J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
+ J = J_cube + J_distal_arm # Moment of inertia of the arm with the cube on the end
+
+ error_margine = 1.0
+ voltage = 10.0 # voltage for the motor. Assuming a loaded robot so not using 12 V.
+ # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
+ # motor_free_speed = Kv*voltage
+ motor_free_speed = speed_no_load
+
+ print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" %
+ (Kt, Kv_rpm, Kv))
+ print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
+ print("# %.2f kg Cube weight" % (mass_cube))
+ print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
+ print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
+ print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
+ print("# %.2f m Length from distal arm pivot point to arm CG" %
+ (radius_to_distal_arm_cg))
+ print("# %.2f m Length from distal arm pivot point to arm and cube cg" %
+ (radius_to_distal_cg))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point"
+ % (J_cube))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the arm with the cube on the end" %
+ (J))
+ print("# %.2f m Proximal arm length" % (length_proximal_arm))
+ print("# %.2f m Distal arm length" % (length_distal_arm))
+
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point"
+ % (J_proximal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point"
+ % (J_distal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the distal arm and cube about the arm pivot point"
+ % (J))
+ print("# %d Number of motors" % (num_motors))
+
+ print("# %.2f V Motor voltage" % (voltage))
+ for gear_ratio in range(30, 181, 10):
+ c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
+ c2 = gear_ratio * Kt / (J * resistance_motor)
+ c3 = radius_to_distal_cg * mass_distal * gravity / J
+
+ if (False):
+ print("# %.8f 1/sec C1 constant" % (c1))
+ print("# %.2f 1/sec C2 constant" % (c2))
+ print("# %.2f 1/(V sec^2) C3 constant" % (c3))
+ print("# %.2f RPM Free speed at motor voltage" %
+ (voltage * Kv_rpm))
+
+ torque_90_degrees = radius_to_distal_cg * mass_distal * gravity
+ voltage_90_degrees = resistance_motor * torque_90_degrees / (
+ gear_ratio * Kt)
+ torque_peak = gear_ratio * num_motors * torque_stall
+ torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
+ normal_force = torque_peak / length_distal_arm
+ normal_force_lbf = newton_to_lbf * normal_force
+ time_required = get_180_degree_time(c1, c2, c3, voltage, gear_ratio,
+ motor_free_speed)
+ print ("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds. 90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at distal end %3.0f N %2.0f lbf" % \
+ (to_deg(theta_travel),gear_ratio,time_required,torque_90_degrees,voltage_90_degrees,
+ torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf))
+
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/y2018/control_loops/python/arm_proximal.py b/y2018/control_loops/python/arm_proximal.py
index d40ec8a..4abd1cf 100755
--- a/y2018/control_loops/python/arm_proximal.py
+++ b/y2018/control_loops/python/arm_proximal.py
@@ -3,7 +3,7 @@
# This code was used to select the gear ratio for the proximal arm.
# Run it from the command line and it displays the time required
# to move the proximal arm 180 degrees from straight down to straight up.
-#
+#
# Michael Schuh
# January 20, 2018
@@ -14,241 +14,291 @@
# apt-get install python-scipy python3-scipy python-numpy python3-numpy
pi = math.pi
-pi2 = 2.0*pi
-rad_to_deg = 180.0/pi
+pi2 = 2.0 * pi
+rad_to_deg = 180.0 / pi
inches_to_meters = 0.0254
-lbs_to_kg = 1.0/2.2
+lbs_to_kg = 1.0 / 2.2
newton_to_lbf = 0.224809
newton_meters_to_ft_lbs = 0.73756
run_count = 0
theta_travel = 0.0
+
def to_deg(angle):
- return (angle*rad_to_deg)
+ return (angle * rad_to_deg)
+
def to_rad(angle):
- return (angle/rad_to_deg)
+ return (angle / rad_to_deg)
+
def to_rotations(angle):
- return (angle/pi2)
+ return (angle / pi2)
+
def time_derivative(x, t, voltage, c1, c2, c3):
- global run_count
- theta, omega = x
- dxdt = [omega, -c1*omega + c3*math.sin(theta) + c2*voltage]
- run_count = run_count + 1
+ global run_count
+ theta, omega = x
+ dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
+ run_count = run_count + 1
- #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
- return dxdt
+ #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
+ return dxdt
+
def get_distal_angle(theta_proximal):
- # For the proximal angle = -50 degrees, the distal angle is -180 degrees
- # For the proximal angle = 10 degrees, the distal angle is -90 degrees
- distal_angle = to_rad(-180.0 - (-50.0-to_deg(theta_proximal))*(180.0-90.0)/(50.0+10.0))
- return distal_angle
+ # For the proximal angle = -50 degrees, the distal angle is -180 degrees
+ # For the proximal angle = 10 degrees, the distal angle is -90 degrees
+ distal_angle = to_rad(-180.0 - (-50.0 - to_deg(theta_proximal)) *
+ (180.0 - 90.0) / (50.0 + 10.0))
+ return distal_angle
+
def get_distal_omega(omega_proximal):
- # For the proximal angle = -50 degrees, the distal angle is -180 degrees
- # For the proximal angle = 10 degrees, the distal angle is -90 degrees
- distal_angle = omega_proximal*( (180.0-90.0) / (50.0+10.0) )
- return distal_angle
-
+ # For the proximal angle = -50 degrees, the distal angle is -180 degrees
+ # For the proximal angle = 10 degrees, the distal angle is -90 degrees
+ distal_angle = omega_proximal * ((180.0 - 90.0) / (50.0 + 10.0))
+ return distal_angle
-def get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed):
- #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
- #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
- global run_count
- global theta_travel
- if ( False ):
- # Gravity is assisting the motion.
- theta_start = 0.0
- theta_target = pi
- elif ( False ):
- # Gravity is assisting the motion.
- theta_start = 0.0
- theta_target = -pi
- elif ( False ):
- # Gravity is slowing the motion.
- theta_start = pi
- theta_target = 0.0
- elif ( False ):
- # Gravity is slowing the motion.
- theta_start = -pi
- theta_target = 0.0
- elif ( True ):
- # This is for the proximal arm motion.
- theta_start = to_rad(-50.0)
- theta_target = to_rad(10.0)
+def get_180_degree_time(c1, c2, c3, voltage, gear_ratio, motor_free_speed):
+ #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
+ #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
+ global run_count
+ global theta_travel
- theta_half = 0.5*(theta_start + theta_target)
- if (theta_start > theta_target):
- voltage = -voltage
- theta = theta_start
- theta_travel = theta_start - theta_target
- if ( run_count == 0 ):
- print ("# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (theta_start,theta_target,theta_travel,theta_half, voltage))
- print ("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (to_deg(theta_start),to_deg(theta_target),to_deg(theta_travel),to_deg(theta_half), voltage))
- omega = 0.0
- time = 0.0
- delta_time = 0.01 # time step in seconds
- for step in range(1, 5000):
- t = numpy.array([time, time + delta_time])
- time = time + delta_time
- x = [theta, omega]
- angular_acceleration = -c1*omega + c2*voltage
- x_n_plus_1 = scipy.integrate.odeint(time_derivative,x,t,args=(voltage,c1,c2,c3))
- #print ('x_n_plus_1 = ',x_n_plus_1)
- #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
- theta, omega = x_n_plus_1[1]
- #theta= x_n_plus_1[0]
- #omega = x_n_plus_1[1]
- if ( False ):
- print ("%4d %8.4f %8.2f %8.4f %8.4f %8.3f %8.3f %8.3f %8.3f" % \
- (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
- to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
- if (theta_start < theta_target):
- # Angle is increasing through the motion.
- if (theta > theta_half):
- break
- else:
- # Angle is decreasing through the motion.
- if (theta < theta_half):
- break
-
- #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
- #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
- #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
- return (2.0*time)
+ if (False):
+ # Gravity is assisting the motion.
+ theta_start = 0.0
+ theta_target = pi
+ elif (False):
+ # Gravity is assisting the motion.
+ theta_start = 0.0
+ theta_target = -pi
+ elif (False):
+ # Gravity is slowing the motion.
+ theta_start = pi
+ theta_target = 0.0
+ elif (False):
+ # Gravity is slowing the motion.
+ theta_start = -pi
+ theta_target = 0.0
+ elif (True):
+ # This is for the proximal arm motion.
+ theta_start = to_rad(-50.0)
+ theta_target = to_rad(10.0)
+
+ theta_half = 0.5 * (theta_start + theta_target)
+ if (theta_start > theta_target):
+ voltage = -voltage
+ theta = theta_start
+ theta_travel = theta_start - theta_target
+ if (run_count == 0):
+ print(
+ "# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+ % (theta_start, theta_target, theta_travel, theta_half, voltage))
+ print(
+ "# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+ % (to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
+ to_deg(theta_half), voltage))
+ omega = 0.0
+ time = 0.0
+ delta_time = 0.01 # time step in seconds
+ for step in range(1, 5000):
+ t = numpy.array([time, time + delta_time])
+ time = time + delta_time
+ x = [theta, omega]
+ angular_acceleration = -c1 * omega + c2 * voltage
+ x_n_plus_1 = scipy.integrate.odeint(time_derivative,
+ x,
+ t,
+ args=(voltage, c1, c2, c3))
+ #print ('x_n_plus_1 = ',x_n_plus_1)
+ #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
+ theta, omega = x_n_plus_1[1]
+ #theta= x_n_plus_1[0]
+ #omega = x_n_plus_1[1]
+ if (False):
+ print ("%4d %8.4f %8.2f %8.4f %8.4f %8.3f %8.3f %8.3f %8.3f" % \
+ (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
+ to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
+ if (theta_start < theta_target):
+ # Angle is increasing through the motion.
+ if (theta > theta_half):
+ break
+ else:
+ # Angle is decreasing through the motion.
+ if (theta < theta_half):
+ break
+
+ #print ("# step time theta angular_speed angular_acceleration theta angular_speed motor_speed motor_speed_fraction")
+ #print ("# (sec) (rad) (rad/sec) (rad/sec^2) (rotations) (rotations/sec) (rpm) (fraction)")
+ #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
+ return (2.0 * time)
+
def main():
- global run_count
- gravity = 9.8 # m/sec^2 Gravity Constant
- voltage_nominal = 12 # Volts
-
- # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
- motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
- current_stall = 134 # amps stall current
- current_no_load = 0.7 # amps no load current
- torque_stall = 710/1000.0 # N-m Stall Torque
- speed_no_load_rpm = 18730 # RPM no load speed
-
- if ( False ):
- # Bag motor from https://www.vexrobotics.com/217-3351.html
- motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
- current_stall = 53.0 # amps stall current
- current_no_load = 1.8 # amps no load current
- torque_stall = 0.4 # N-m Stall Torque
- speed_no_load_rpm = 13180.0 # RPM no load speed
-
- if ( True ):
- # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
- motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
- current_stall = 89.0 # amps stall current
- current_no_load = 3.0 # amps no load current
- torque_stall = 1.4 # N-m Stall Torque
- speed_no_load_rpm = 5840.0 # RPM no load speed
+ global run_count
+ gravity = 9.8 # m/sec^2 Gravity Constant
+ voltage_nominal = 12 # Volts
- # How many motors are we using?
- num_motors = 1
+ # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
+ motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
+ current_stall = 134 # amps stall current
+ current_no_load = 0.7 # amps no load current
+ torque_stall = 710 / 1000.0 # N-m Stall Torque
+ speed_no_load_rpm = 18730 # RPM no load speed
- # Motor values
- print ("# Motor: %s" % (motor_name))
- print ("# Number of motors: %d" % (num_motors))
- print ("# Stall torque: %.1f n-m" % (torque_stall))
- print ("# Stall current: %.1f amps" % (current_stall))
- print ("# No load current: %.1f amps" % (current_no_load))
- print ("# No load speed: %.0f rpm" % (speed_no_load_rpm))
-
- # Constants from motor values
- resistance_motor = voltage_nominal/current_stall
- speed_no_load_rps = speed_no_load_rpm/60.0 # Revolutions per second no load speed
- speed_no_load = speed_no_load_rps*2.0*pi
- Kt = num_motors*torque_stall/current_stall # N-m/A torque constant
- Kv_rpm = speed_no_load_rpm /(voltage_nominal - resistance_motor*current_no_load) # rpm/V
- Kv = Kv_rpm*2.0*pi/60.0 # rpm/V
-
- # Robot Geometry and physics
- length_proximal_arm = inches_to_meters*47.34 # m Length of arm connected to the robot base
- length_distal_arm = inches_to_meters*44.0 # m Length of arm that holds the cube
- mass_cube = 6.0*lbs_to_kg # Weight of the cube in Kgrams
- mass_proximal_arm = 5.5*lbs_to_kg # Weight of proximal arm
- mass_distal_arm = 3.5*lbs_to_kg # Weight of distal arm
- mass_distal = mass_cube + mass_distal_arm
- mass_proximal = mass_proximal_arm + mass_distal
- radius_to_proximal_arm_cg = 22.0*inches_to_meters # m Length from arm pivot point to arm CG
- radius_to_distal_arm_cg = 10.0*inches_to_meters # m Length from arm pivot point to arm CG
+ if (False):
+ # Bag motor from https://www.vexrobotics.com/217-3351.html
+ motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
+ current_stall = 53.0 # amps stall current
+ current_no_load = 1.8 # amps no load current
+ torque_stall = 0.4 # N-m Stall Torque
+ speed_no_load_rpm = 13180.0 # RPM no load speed
- radius_to_distal_cg = ( length_distal_arm*mass_cube + radius_to_distal_arm_cg*mass_distal_arm)/mass_distal
- radius_to_proximal_cg = ( length_proximal_arm*mass_distal + radius_to_proximal_arm_cg*mass_proximal_arm)/mass_proximal
- J_cube = length_distal_arm*length_distal_arm*mass_cube
- # Kg m^2 Moment of inertia of the proximal arm
- J_proximal_arm = radius_to_proximal_arm_cg*radius_to_proximal_arm_cg*mass_distal_arm
- # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
- J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm*length_proximal_arm*mass_distal
- J_distal_arm = radius_to_distal_arm_cg*radius_to_distal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
- J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm # Moment of inertia of the arm with the cube on the end
+ if (True):
+ # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
+ motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
+ current_stall = 89.0 # amps stall current
+ current_no_load = 3.0 # amps no load current
+ torque_stall = 1.4 # N-m Stall Torque
+ speed_no_load_rpm = 5840.0 # RPM no load speed
- error_margine = 1.0
- voltage = 10.0 # voltage for the motor. Assuming a loaded robot so not using 12 V.
- # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
- # motor_free_speed = Kv*voltage
- motor_free_speed = speed_no_load
-
- print ("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
- print ("# %.2f Ohms Resistance of the motor " % (resistance_motor))
- print ("# %.2f kg Cube weight" % (mass_cube))
- print ("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
- print ("# %.2f kg Distal Arm mass" % (mass_distal_arm))
- print ("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
- print ("# %.2f m Length from distal arm pivot point to arm CG" % (radius_to_distal_arm_cg))
- print ("# %.2f m Length from distal arm pivot point to arm and cube cg" % (radius_to_distal_cg))
- print ("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
- print ("# %.2f m Length from proximal arm pivot point to arm CG" % (radius_to_proximal_arm_cg))
- print ("# %.2f m Length from proximal arm pivot point to arm and cube cg" % (radius_to_proximal_cg))
- print ("# %.2f m Proximal arm length" % (length_proximal_arm))
- print ("# %.2f m Distal arm length" % (length_distal_arm))
+ # How many motors are we using?
+ num_motors = 1
- print ("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (J_distal_arm))
- print ("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (J_proximal_arm))
- print ("# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about the proximal arm pivot point" % (J_distal_arm_and_cube_at_end_of_proximal_arm))
- print ("# %.2f kg-m^2 Moment of inertia of the proximal arm and distal arm and cube about the arm pivot point" % (J))
- print ("# %d Number of motors" % (num_motors))
-
- print ("# %.2f V Motor voltage" % (voltage))
+ # Motor values
+ print("# Motor: %s" % (motor_name))
+ print("# Number of motors: %d" % (num_motors))
+ print("# Stall torque: %.1f n-m" % (torque_stall))
+ print("# Stall current: %.1f amps" % (current_stall))
+ print("# No load current: %.1f amps" % (current_no_load))
+ print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
- print ("\n# Min time is for proximal arm without any forces from distal arm. Max time puts all distal arm mass at the end of proximal arm.")
+ # Constants from motor values
+ resistance_motor = voltage_nominal / current_stall
+ speed_no_load_rps = speed_no_load_rpm / 60.0 # Revolutions per second no load speed
+ speed_no_load = speed_no_load_rps * 2.0 * pi
+ Kt = num_motors * torque_stall / current_stall # N-m/A torque constant
+ Kv_rpm = speed_no_load_rpm / (
+ voltage_nominal - resistance_motor * current_no_load) # rpm/V
+ Kv = Kv_rpm * 2.0 * pi / 60.0 # rpm/V
- for gear_ratio in range(60, 241, 10):
- c1 = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J)
- c2 = gear_ratio*Kt/(J*resistance_motor)
- c3 = radius_to_proximal_cg*mass_proximal*gravity/J
- c1_proximal_only = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J_proximal_arm)
- c2_proximal_only = gear_ratio*Kt/(J_proximal_arm*resistance_motor)
- c3_proximal_only = radius_to_proximal_arm_cg*mass_proximal_arm*gravity/J_proximal_arm
-
- if ( False and run_count < 1 ):
- print ("# %.8f 1/sec C1 constant" % (c1))
- print ("# %.2f 1/sec C2 constant" % (c2))
- print ("# %.2f 1/(V sec^2) C3 constant" % (c3))
- print ("# %.8f 1/sec C1 proximal only constant" % (c1_proximal_only))
- print ("# %.2f 1/sec C2 proximal only constant" % (c2_proximal_only))
- print ("# %.2f 1/(V sec^2) C3 proximal only constant" % (c3_proximal_only))
- print ("# %.2f RPM Free speed at motor voltage" % (voltage*Kv_rpm))
-
- torque_90_degrees = radius_to_proximal_cg*mass_proximal*gravity
- voltage_90_degrees = resistance_motor*torque_90_degrees/(gear_ratio*Kt)
- torque_peak = gear_ratio*num_motors*torque_stall
- torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
- normal_force = torque_peak/length_proximal_arm
- normal_force_lbf = newton_to_lbf*normal_force
- normal_distal_arm_end_force = torque_peak/(length_proximal_arm + length_distal_arm)
- normal_distal_arm_end_force_lbf = newton_to_lbf*normal_distal_arm_end_force
- time_required = get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed)
- time_required_proximal_only = get_180_degree_time(c1_proximal_only,c2_proximal_only,c3_proximal_only,voltage,gear_ratio,motor_free_speed)
- print ("Time for %.1f degrees for gear ratio %3.0f is %.2f (min) %.2f (max) seconds. 90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at proximal end %3.0f N %2.0f lbf distal end %3.0f N %2.0f lbf" % \
- (to_deg(theta_travel),gear_ratio,time_required_proximal_only,time_required,torque_90_degrees,voltage_90_degrees,
- torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf,normal_distal_arm_end_force,normal_distal_arm_end_force_lbf))
-
+ # Robot Geometry and physics
+ length_proximal_arm = inches_to_meters * 47.34 # m Length of arm connected to the robot base
+ length_distal_arm = inches_to_meters * 44.0 # m Length of arm that holds the cube
+ mass_cube = 6.0 * lbs_to_kg # Weight of the cube in Kgrams
+ mass_proximal_arm = 5.5 * lbs_to_kg # Weight of proximal arm
+ mass_distal_arm = 3.5 * lbs_to_kg # Weight of distal arm
+ mass_distal = mass_cube + mass_distal_arm
+ mass_proximal = mass_proximal_arm + mass_distal
+ radius_to_proximal_arm_cg = 22.0 * inches_to_meters # m Length from arm pivot point to arm CG
+ radius_to_distal_arm_cg = 10.0 * inches_to_meters # m Length from arm pivot point to arm CG
+
+ radius_to_distal_cg = (
+ length_distal_arm * mass_cube +
+ radius_to_distal_arm_cg * mass_distal_arm) / mass_distal
+ radius_to_proximal_cg = (
+ length_proximal_arm * mass_distal +
+ radius_to_proximal_arm_cg * mass_proximal_arm) / mass_proximal
+ J_cube = length_distal_arm * length_distal_arm * mass_cube
+ # Kg m^2 Moment of inertia of the proximal arm
+ J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * mass_distal_arm
+ # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
+ J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm * length_proximal_arm * mass_distal
+ J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
+ J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm # Moment of inertia of the arm with the cube on the end
+
+ error_margine = 1.0
+ voltage = 10.0 # voltage for the motor. Assuming a loaded robot so not using 12 V.
+ # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
+ # motor_free_speed = Kv*voltage
+ motor_free_speed = speed_no_load
+
+ print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" %
+ (Kt, Kv_rpm, Kv))
+ print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
+ print("# %.2f kg Cube weight" % (mass_cube))
+ print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
+ print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
+ print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
+ print("# %.2f m Length from distal arm pivot point to arm CG" %
+ (radius_to_distal_arm_cg))
+ print("# %.2f m Length from distal arm pivot point to arm and cube cg" %
+ (radius_to_distal_cg))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point"
+ % (J_cube))
+ print("# %.2f m Length from proximal arm pivot point to arm CG" %
+ (radius_to_proximal_arm_cg))
+ print("# %.2f m Length from proximal arm pivot point to arm and cube cg" %
+ (radius_to_proximal_cg))
+ print("# %.2f m Proximal arm length" % (length_proximal_arm))
+ print("# %.2f m Distal arm length" % (length_distal_arm))
+
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point"
+ % (J_distal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point"
+ % (J_proximal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about the proximal arm pivot point"
+ % (J_distal_arm_and_cube_at_end_of_proximal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the proximal arm and distal arm and cube about the arm pivot point"
+ % (J))
+ print("# %d Number of motors" % (num_motors))
+
+ print("# %.2f V Motor voltage" % (voltage))
+
+ print(
+ "\n# Min time is for proximal arm without any forces from distal arm. Max time puts all distal arm mass at the end of proximal arm."
+ )
+
+ for gear_ratio in range(60, 241, 10):
+ c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
+ c2 = gear_ratio * Kt / (J * resistance_motor)
+ c3 = radius_to_proximal_cg * mass_proximal * gravity / J
+ c1_proximal_only = Kt * gear_ratio * gear_ratio / (
+ Kv * resistance_motor * J_proximal_arm)
+ c2_proximal_only = gear_ratio * Kt / (J_proximal_arm *
+ resistance_motor)
+ c3_proximal_only = radius_to_proximal_arm_cg * mass_proximal_arm * gravity / J_proximal_arm
+
+ if (False and run_count < 1):
+ print("# %.8f 1/sec C1 constant" % (c1))
+ print("# %.2f 1/sec C2 constant" % (c2))
+ print("# %.2f 1/(V sec^2) C3 constant" % (c3))
+ print("# %.8f 1/sec C1 proximal only constant" %
+ (c1_proximal_only))
+ print("# %.2f 1/sec C2 proximal only constant" %
+ (c2_proximal_only))
+ print("# %.2f 1/(V sec^2) C3 proximal only constant" %
+ (c3_proximal_only))
+ print("# %.2f RPM Free speed at motor voltage" %
+ (voltage * Kv_rpm))
+
+ torque_90_degrees = radius_to_proximal_cg * mass_proximal * gravity
+ voltage_90_degrees = resistance_motor * torque_90_degrees / (
+ gear_ratio * Kt)
+ torque_peak = gear_ratio * num_motors * torque_stall
+ torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
+ normal_force = torque_peak / length_proximal_arm
+ normal_force_lbf = newton_to_lbf * normal_force
+ normal_distal_arm_end_force = torque_peak / (length_proximal_arm +
+ length_distal_arm)
+ normal_distal_arm_end_force_lbf = newton_to_lbf * normal_distal_arm_end_force
+ time_required = get_180_degree_time(c1, c2, c3, voltage, gear_ratio,
+ motor_free_speed)
+ time_required_proximal_only = get_180_degree_time(
+ c1_proximal_only, c2_proximal_only, c3_proximal_only, voltage,
+ gear_ratio, motor_free_speed)
+ print ("Time for %.1f degrees for gear ratio %3.0f is %.2f (min) %.2f (max) seconds. 90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at proximal end %3.0f N %2.0f lbf distal end %3.0f N %2.0f lbf" % \
+ (to_deg(theta_travel),gear_ratio,time_required_proximal_only,time_required,torque_90_degrees,voltage_90_degrees,
+ torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf,normal_distal_arm_end_force,normal_distal_arm_end_force_lbf))
+
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/y2018/control_loops/python/arm_trajectory.py b/y2018/control_loops/python/arm_trajectory.py
index bcfd9f0..9db7717 100755
--- a/y2018/control_loops/python/arm_trajectory.py
+++ b/y2018/control_loops/python/arm_trajectory.py
@@ -20,6 +20,7 @@
class Dynamics(object):
+
def __init__(self, dt):
self.dt = dt
@@ -61,29 +62,30 @@
[[self.G1 * self.Kt / self.R, 0.0],
[0.0, self.G2 * self.kNumDistalMotors * self.Kt / self.R]])
self.K4 = numpy.matrix(
- [[self.G1 * self.G1 * self.Kt / (self.Kv * self.R), 0.0], [
- 0.0, self.G2 * self.G2 * self.Kt * self.kNumDistalMotors /
- (self.Kv * self.R)
- ]])
+ [[self.G1 * self.G1 * self.Kt / (self.Kv * self.R), 0.0],
+ [
+ 0.0, self.G2 * self.G2 * self.Kt * self.kNumDistalMotors /
+ (self.Kv * self.R)
+ ]])
# These constants are for the Extended Kalman Filter
# Q is the covariance of the X values. Use the square of the standard
# deviation of the error accumulated each time step.
- self.Q_x_covariance = numpy.matrix([[0.001**2,0.0,0.0,0.0,0.0,0.0],
- [0.0,0.001**2,0.0,0.0,0.0,0.0],
- [0.0,0.0,0.001**2,0.0,0.0,0.0],
- [0.0,0.0,0.0,0.001**2,0.0,0.0],
- [0.0,0.0,0.0,0.0,10.0**2,0.0],
- [0.0,0.0,0.0,0.0,0.0,10.0**2]])
+ self.Q_x_covariance = numpy.matrix(
+ [[0.001**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.001**2, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.001**2, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.001**2, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 10.0**2, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 10.0**2]])
# R is the covariance of the Z values. Increase the responsiveness to
# changes by reducing coresponding term in the R matrix.
- self.R_y_covariance = numpy.matrix([[0.01**2, 0.0],[0.0, 0.01**2]])
+ self.R_y_covariance = numpy.matrix([[0.01**2, 0.0], [0.0, 0.01**2]])
# H is the jacobian of the h(x) measurement prediction function
- self.H_h_jacobian = numpy.matrix([[1.0,0.0,0.0,0.0,0.0,0.0],
- [0.0,0.0,1.0,0.0,0.0,0.0]])
+ self.H_h_jacobian = numpy.matrix([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]])
self.Identity_matrix = numpy.matrix(numpy.identity(6))
-
def discrete_dynamics_ekf_predict(self, X_hat, P_covariance_estimate, U,
sim_dt):
"""Updates the Extended Kalman Filter state for one timestep.
@@ -108,8 +110,8 @@
# Predict step
# Compute the state trasition matrix using the Jacobian of state
# estimate
- F_k = numerical_jacobian_x(self.unbounded_discrete_dynamics_ekf,
- X_hat, U)
+ F_k = numerical_jacobian_x(self.unbounded_discrete_dynamics_ekf, X_hat,
+ U)
# State estimate
X_hat = self.unbounded_discrete_dynamics_ekf(X_hat, U, sim_dt)
# Covariance estimate
@@ -134,7 +136,7 @@
# Update step
# Measurement residual error of proximal and distal
# joint angles.
- Y_hat = Y_reading - numpy.matrix([[X_hat[0,0]],[X_hat[2,0]]])
+ Y_hat = Y_reading - numpy.matrix([[X_hat[0, 0]], [X_hat[2, 0]]])
# Residual covariance
S = self.H_h_jacobian * P_covariance_estimate * self.H_h_jacobian.T + \
self.R_y_covariance
@@ -144,8 +146,8 @@
# Updated state estimate
X_hat = X_hat + Kalman_gain * Y_hat
# Updated covariance estimate
- P_covariance_estimate = (self.Identity_matrix -
- Kalman_gain * self.H_h_jacobian) * P_covariance_estimate
+ P_covariance_estimate = (self.Identity_matrix - Kalman_gain *
+ self.H_h_jacobian) * P_covariance_estimate
return X_hat, P_covariance_estimate
def NormilizedMatriciesForState(self, X):
@@ -158,8 +160,8 @@
c = numpy.cos(X[0, 0] - X[2, 0])
# K1 * d^2 theta/dt^2 + K2 * d theta/dt = torque
- K1 = numpy.matrix(
- [[self.alpha, c * self.beta], [c * self.beta, self.gamma]])
+ K1 = numpy.matrix([[self.alpha, c * self.beta],
+ [c * self.beta, self.gamma]])
K2 = numpy.matrix([[0.0, s * self.beta], [-s * self.beta, 0.0]])
@@ -183,8 +185,8 @@
"""
K1, K2, K3, K4 = self.MatriciesForState(X)
- return numpy.linalg.inv(K3) * (
- K1 * alpha_t + K2 * omega_t + K4 * omega_t)
+ return numpy.linalg.inv(K3) * (K1 * alpha_t + K2 * omega_t +
+ K4 * omega_t)
def ff_u_distance(self, trajectory, distance):
"""Computes the feed forward U at the distance on the trajectory.
@@ -252,15 +254,15 @@
[accel[1, 0]], [0.0], [0.0]])
def unbounded_discrete_dynamics(self, X, U, dt=None):
- return RungeKutta(lambda startingX: self.dynamics(startingX, U), X,
- dt or self.dt)
+ return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt
+ or self.dt)
def unbounded_discrete_dynamics_ekf(self, X, U, dt=None):
return RungeKutta(lambda startingX: self.dynamics_ekf(startingX, U), X,
dt or self.dt)
def discrete_dynamics(self, X, U, dt=None):
- assert((numpy.abs(U) <= (12.0 + 1e-6)).all())
+ assert ((numpy.abs(U) <= (12.0 + 1e-6)).all())
return self.unbounded_discrete_dynamics(X, U, dt)
@@ -324,8 +326,8 @@
q_vel = 4.0
Q = numpy.matrix(
numpy.diag([
- 1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0), 1.0 / (
- q_vel**2.0)
+ 1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0),
+ 1.0 / (q_vel**2.0)
]))
R = numpy.matrix(numpy.diag([1.0 / (12.0**2.0), 1.0 / (12.0**2.0)]))
@@ -335,19 +337,21 @@
return controls.dlqr(final_A, final_B, Q, R)
+
def get_encoder_values(X):
- """Returns simulated encoder readings.
+ """Returns simulated encoder readings.
This method returns the encoder readings. For now simply use values from X
with some noise added in to make the simulation more interesting.
"""
- introduced_random_measurement_noise = 0.005
- introduced_random_measurement_noise = 0.05
- theta1_measured = X[0,0] + introduced_random_measurement_noise * \
- 2.0 * ( numpy.random.random() - 0.5 )
- theta2_measured = X[2,0] + introduced_random_measurement_noise * \
- 2.0 * ( numpy.random.random() - 0.5 )
- return numpy.matrix([[theta1_measured ],[theta2_measured]])
+ introduced_random_measurement_noise = 0.005
+ introduced_random_measurement_noise = 0.05
+ theta1_measured = X[0,0] + introduced_random_measurement_noise * \
+ 2.0 * ( numpy.random.random() - 0.5 )
+ theta2_measured = X[2,0] + introduced_random_measurement_noise * \
+ 2.0 * ( numpy.random.random() - 0.5 )
+ return numpy.matrix([[theta1_measured], [theta2_measured]])
+
class Trajectory:
"""This class represents a trajectory in theta space."""
@@ -358,8 +362,12 @@
numpy.matrix([[numpy.pi / 2.0 - x[0]], [numpy.pi / 2.0 - x[1]]])
for x in self.path_points
]
- self._omegas = [numpy.matrix([[-x[2]], [-x[3]]]) for x in self.path_points]
- self._alphas = [numpy.matrix([[-x[4]], [-x[5]]]) for x in self.path_points]
+ self._omegas = [
+ numpy.matrix([[-x[2]], [-x[3]]]) for x in self.path_points
+ ]
+ self._alphas = [
+ numpy.matrix([[-x[4]], [-x[5]]]) for x in self.path_points
+ ]
self.path_step_size = path_step_size
self.point_distances = [0.0]
last_point = self._thetas[0]
@@ -389,8 +397,9 @@
return points[0]
elif distance >= self._length:
return points[-1]
- after_index = numpy.searchsorted(
- self.point_distances, distance, side='right')
+ after_index = numpy.searchsorted(self.point_distances,
+ distance,
+ side='right')
before_index = after_index - 1
return (distance - self.point_distances[before_index]) / (
self.point_distances[after_index] -
@@ -420,15 +429,15 @@
alpha = self.alpha(distance)
X = numpy.matrix([[theta[0, 0]], [0.0], [theta[1, 0]], [0.0]])
K1, K2, K3, K4 = dynamics.NormilizedMatriciesForState(X)
- omega_square = numpy.matrix(
- [[omega[0, 0], 0.0], [0.0, omega[1, 0]]])
+ omega_square = numpy.matrix([[omega[0, 0], 0.0],
+ [0.0, omega[1, 0]]])
# Here, we can say that
# d^2/dt^2 theta = d^2/dd^2 theta(d) * (d d/dt)^2
# Normalize so that the max accel is 1, and take magnitudes. This
# gives us the max velocity we can be at each point due to
# curvature.
- vk1 = numpy.linalg.inv(K3) * (
- K1 * alpha + K2 * omega_square * omega)
+ vk1 = numpy.linalg.inv(K3) * (K1 * alpha +
+ K2 * omega_square * omega)
vk2 = numpy.linalg.inv(K3) * K4 * omega
ddots = []
for c in [-vmax, vmax]:
@@ -470,8 +479,8 @@
voltage_accel_list = []
for c in [-vmax, vmax]:
- for a, b in [(k_constant[0, 0], k_scalar[0, 0]), (k_constant[1, 0],
- k_scalar[1, 0])]:
+ for a, b in [(k_constant[0, 0], k_scalar[0, 0]),
+ (k_constant[1, 0], k_scalar[1, 0])]:
# This time, we are doing the other pass. So, find all
# the decelerations (and flip them) to find the prior
# velocity.
@@ -483,14 +492,16 @@
filtered_voltage_accel_list.append(a)
goal_acceleration = numpy.sqrt(
- max(0.0, 1.0 -
+ max(
+ 0.0, 1.0 -
(numpy.linalg.norm(alpha_unitizer * alpha) * velocity *
velocity)**2.0)) / numpy.linalg.norm(alpha_unitizer * omega)
if filtered_voltage_accel_list:
# TODO(austin): The max of the list seems right, but I'm
# not seeing many lists with a size > 1, so it's hard to
# tell. Max is conservative, for sure.
- goal_acceleration = min(-max(filtered_voltage_accel_list), goal_acceleration)
+ goal_acceleration = min(-max(filtered_voltage_accel_list),
+ goal_acceleration)
return goal_acceleration
def back_trajectory_pass(self, previous_pass, dynamics, alpha_unitizer,
@@ -516,8 +527,8 @@
integration_step_size = self.path_step_size / float(num_steps)
int_d += integration_step_size
- int_vel = numpy.sqrt(2.0 * int_accel_t * integration_step_size
- + int_vel * int_vel)
+ int_vel = numpy.sqrt(2.0 * int_accel_t *
+ integration_step_size + int_vel * int_vel)
max_dvelocity_back_pass[index] = min(
int_vel, max_dvelocity_back_pass[index])
return max_dvelocity_back_pass
@@ -539,17 +550,18 @@
omega_square = numpy.matrix([[omega[0, 0], 0.0], [0.0, omega[1, 0]]])
k_constant = numpy.linalg.inv(K3) * (
- (K1 * alpha + K2 * omega_square * omega
- ) * goal_velocity * goal_velocity + K4 * omega * goal_velocity)
+ (K1 * alpha + K2 * omega_square * omega) * goal_velocity *
+ goal_velocity + K4 * omega * goal_velocity)
k_scalar = numpy.linalg.inv(K3) * K1 * omega
voltage_accel_list = []
for c in [-vmax, vmax]:
- for a, b in [(k_constant[0, 0], k_scalar[0, 0]), (k_constant[1, 0],
- k_scalar[1, 0])]:
+ for a, b in [(k_constant[0, 0], k_scalar[0, 0]),
+ (k_constant[1, 0], k_scalar[1, 0])]:
voltage_accel_list.append((c - a) / b)
goal_acceleration = numpy.sqrt(
- max(0.0, 1.0 -
+ max(
+ 0.0, 1.0 -
(numpy.linalg.norm(alpha_unitizer * alpha) * goal_velocity *
goal_velocity)**2.0)) / numpy.linalg.norm(
alpha_unitizer * omega)
@@ -564,8 +576,8 @@
# TODO(austin): The max of the list seems right, but I'm not
# seeing many lists with a size > 1, so it's hard to tell.
# Min is conservative, for sure.
- goal_acceleration = min(
- min(filtered_voltage_accel_list), goal_acceleration)
+ goal_acceleration = min(min(filtered_voltage_accel_list),
+ goal_acceleration)
return goal_acceleration
@@ -592,8 +604,8 @@
integration_step_size = self.path_step_size / float(num_steps)
int_d += integration_step_size
- int_vel = numpy.sqrt(2.0 * int_accel_t * integration_step_size
- + int_vel * int_vel)
+ int_vel = numpy.sqrt(2.0 * int_accel_t *
+ integration_step_size + int_vel * int_vel)
max_dvelocity_forward_pass[index] = min(
int_vel, max_dvelocity_forward_pass[index])
@@ -623,11 +635,12 @@
def interpolate_velocity(self, d, d0, d1, v0, v1):
if v0 + v1 > 0:
- return numpy.sqrt(v0 * v0 +
- (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0))
+ return numpy.sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) /
+ (d1 - d0))
else:
- return -numpy.sqrt(v0 * v0 +
- (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0))
+ return -numpy.sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) /
+ (d1 - d0))
+
def get_dvelocity(self, d):
"""Computes the path distance velocity of the plan as a function of the distance."""
after_index = numpy.argmax(self.distance_array > d)
@@ -662,8 +675,8 @@
if d > self._length:
return numpy.matrix(numpy.zeros((2, 1)))
return self.alpha(d) * (
- (velocity or self.get_dvelocity(d))**2.0) + self.omega(d) * (
- acceleration or self.get_dacceleration(d))
+ (velocity or self.get_dvelocity(d))**
+ 2.0) + self.omega(d) * (acceleration or self.get_dacceleration(d))
def R(self, d, velocity=None):
theta_t = self.theta(d)
@@ -682,21 +695,17 @@
# TODO(austin): use computed forward dynamics velocity here.
theta_t = trajectory.theta(saturation_goal_distance)
saturation_goal_velocity = trajectory.interpolate_velocity(
- saturation_goal_distance, last_goal_distance,
- goal_distance, last_goal_velocity, goal_velocity)
+ saturation_goal_distance, last_goal_distance, goal_distance,
+ last_goal_velocity, goal_velocity)
saturation_goal_acceleration = trajectory.interpolate_acceleration(
- last_goal_distance, goal_distance, last_goal_velocity,
- goal_velocity)
- omega_t = trajectory.omega_t(
- saturation_goal_distance,
- velocity=saturation_goal_velocity)
- alpha_t = trajectory.alpha_t(
- saturation_goal_distance,
- velocity=saturation_goal_velocity,
- acceleration=saturation_goal_acceleration)
- R = trajectory.R(
- saturation_goal_distance,
- velocity=saturation_goal_velocity)
+ last_goal_distance, goal_distance, last_goal_velocity, goal_velocity)
+ omega_t = trajectory.omega_t(saturation_goal_distance,
+ velocity=saturation_goal_velocity)
+ alpha_t = trajectory.alpha_t(saturation_goal_distance,
+ velocity=saturation_goal_velocity,
+ acceleration=saturation_goal_acceleration)
+ R = trajectory.R(saturation_goal_distance,
+ velocity=saturation_goal_velocity)
U_ff = numpy.clip(dynamics.ff_u(R, omega_t, alpha_t), -12.0, 12.0)
return U_ff + K * (
R - X), saturation_goal_velocity, saturation_goal_acceleration
@@ -767,12 +776,15 @@
alpha0_max = 40.0
alpha1_max = 60.0
- alpha_unitizer = numpy.matrix(
- [[1.0 / alpha0_max, 0.0], [0.0, 1.0 / alpha1_max]])
+ alpha_unitizer = numpy.matrix([[1.0 / alpha0_max, 0.0],
+ [0.0, 1.0 / alpha1_max]])
# Compute the trajectory taking into account our velocity, acceleration
# and voltage constraints.
- trajectory.compute_trajectory(dynamics, alpha_unitizer, distance_array, vmax=vmax)
+ trajectory.compute_trajectory(dynamics,
+ alpha_unitizer,
+ distance_array,
+ vmax=vmax)
print 'Computed trajectory'
@@ -820,8 +832,8 @@
theta_t = trajectory.theta(goal_distance)
X = numpy.matrix([[theta_t[0, 0]], [0.0], [theta_t[1, 0]], [0.0]])
# X_hat is for the Extended Kalman Filter state estimate
- X_hat = numpy.matrix([[theta_t[0, 0]], [0.0], [theta_t[1, 0]],
- [0.0], [0.0], [0.0]])
+ X_hat = numpy.matrix([[theta_t[0, 0]], [0.0], [theta_t[1, 0]], [0.0],
+ [0.0], [0.0]])
# P is the Covariance Estimate for the Etended Kalman Filter
P_covariance_estimate = dynamics.Q_x_covariance.copy()
@@ -856,10 +868,9 @@
t_array.append(t)
theta_t = trajectory.theta(goal_distance)
omega_t = trajectory.omega_t(goal_distance, velocity=goal_velocity)
- alpha_t = trajectory.alpha_t(
- goal_distance,
- velocity=goal_velocity,
- acceleration=goal_acceleration)
+ alpha_t = trajectory.alpha_t(goal_distance,
+ velocity=goal_velocity,
+ acceleration=goal_acceleration)
theta0_goal_t_array.append(theta_t[0, 0])
theta1_goal_t_array.append(theta_t[1, 0])
@@ -887,7 +898,7 @@
# available. For now, simulate the sensor reading by using the X
# position and adding some noise to it.
X_hat, P_covariance_estimate = dynamics.discrete_dynamics_ekf_update(
- X_hat, P_covariance_estimate, sim_dt, get_encoder_values(X))
+ X_hat, P_covariance_estimate, sim_dt, get_encoder_values(X))
R = trajectory.R(goal_distance, velocity=goal_velocity)
U_ff = numpy.clip(dynamics.ff_u(R, omega_t, alpha_t), -12.0, 12.0)
@@ -927,8 +938,9 @@
fraction_along_path += step_size
print "Fraction", fraction_along_path, "at", goal_distance, "rad,", t, "sec", goal_velocity
- goal_distance = ((goal_distance - last_goal_distance) *
- fraction_along_path + last_goal_distance)
+ goal_distance = (
+ (goal_distance - last_goal_distance) * fraction_along_path +
+ last_goal_distance)
goal_velocity = saturation_goal_velocity
goal_acceleration = saturation_goal_acceleration
@@ -965,8 +977,7 @@
# Push Extended Kalman filter state forwards.
# Predict step - call for each time step
X_hat, P_covariance_estimate = dynamics.discrete_dynamics_ekf_predict(
- X_hat, P_covariance_estimate, U, sim_dt)
-
+ X_hat, P_covariance_estimate, U, sim_dt)
if abs(goal_distance - trajectory.length()) < 1e-2:
# If we go backwards along the path near the goal, snap us to the
@@ -999,12 +1010,15 @@
pylab.figure()
pylab.title("Path Velocity Plan")
- pylab.plot(
- distance_array, trajectory.max_dvelocity_unfiltered, label="pass0")
- pylab.plot(
- distance_array, trajectory.max_dvelocity_back_pass, label="passb")
- pylab.plot(
- distance_array, trajectory.max_dvelocity_forward_pass, label="passf")
+ pylab.plot(distance_array,
+ trajectory.max_dvelocity_unfiltered,
+ label="pass0")
+ pylab.plot(distance_array,
+ trajectory.max_dvelocity_back_pass,
+ label="passb")
+ pylab.plot(distance_array,
+ trajectory.max_dvelocity_forward_pass,
+ label="passf")
pylab.legend(loc='center')
pylab.legend()
@@ -1062,11 +1076,14 @@
pylab.figure()
pylab.title("Disturbance Force from Extended Kalman Filter State Values")
- pylab.plot(t_array, torque_disturbance_0_hat_array, label="torque_disturbance_0_hat")
- pylab.plot(t_array, torque_disturbance_1_hat_array, label="torque_disturbance_1_hat")
+ pylab.plot(t_array,
+ torque_disturbance_0_hat_array,
+ label="torque_disturbance_0_hat")
+ pylab.plot(t_array,
+ torque_disturbance_1_hat_array,
+ label="torque_disturbance_1_hat")
pylab.legend()
-
pylab.show()
diff --git a/y2018/control_loops/python/drivetrain.py b/y2018/control_loops/python/drivetrain.py
index 675930a..0e5ed43 100644
--- a/y2018/control_loops/python/drivetrain.py
+++ b/y2018/control_loops/python/drivetrain.py
@@ -47,5 +47,6 @@
# Write the generated constants out to a file.
drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2018', kDrivetrain)
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2018/control_loops/python/extended_lqr.py b/y2018/control_loops/python/extended_lqr.py
index 4eecee9..9a4705b 100755
--- a/y2018/control_loops/python/extended_lqr.py
+++ b/y2018/control_loops/python/extended_lqr.py
@@ -11,16 +11,17 @@
class ArmDynamics(object):
- def __init__(self, dt):
- self.dt = dt
- self.l1 = 1.0
- self.l2 = 0.8
- self.num_states = 4
- self.num_inputs = 2
+ def __init__(self, dt):
+ self.dt = dt
- def dynamics(self, X, U):
- """Calculates the dynamics for a double jointed arm.
+ self.l1 = 1.0
+ self.l2 = 0.8
+ self.num_states = 4
+ self.num_inputs = 2
+
+ def dynamics(self, X, U):
+ """Calculates the dynamics for a double jointed arm.
Args:
X, numpy.matrix(4, 1), The state. [theta1, omega1, theta2, omega2]
@@ -29,48 +30,53 @@
Returns:
numpy.matrix(4, 1), The derivative of the dynamics.
"""
- return numpy.matrix([[X[1, 0]],
- [U[0, 0]],
- [X[3, 0]],
- [U[1, 0]]])
+ return numpy.matrix([[X[1, 0]], [U[0, 0]], [X[3, 0]], [U[1, 0]]])
- def discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
+ def discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
- def inverse_discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X, dt)
+ def inverse_discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X,
+ dt)
+
# Simple implementation for a quadratic cost function.
class ArmCostFunction:
- def __init__(self, dt, dynamics):
- self.num_states = 4
- self.num_inputs = 2
- self.dt = dt
- self.dynamics = dynamics
- q_pos = 0.5
- q_vel = 1.65
- self.Q = numpy.matrix(numpy.diag([
- 1.0 / (q_pos ** 2.0), 1.0 / (q_vel ** 2.0),
- 1.0 / (q_pos ** 2.0), 1.0 / (q_vel ** 2.0)]))
+ def __init__(self, dt, dynamics):
+ self.num_states = 4
+ self.num_inputs = 2
+ self.dt = dt
+ self.dynamics = dynamics
- self.R = numpy.matrix(numpy.diag([1.0 / (12.0 ** 2.0),
- 1.0 / (12.0 ** 2.0)]))
+ q_pos = 0.5
+ q_vel = 1.65
+ self.Q = numpy.matrix(
+ numpy.diag([
+ 1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0),
+ 1.0 / (q_vel**2.0)
+ ]))
- final_A = numerical_jacobian_x(self.dynamics.discrete_dynamics,
- numpy.matrix(numpy.zeros((4, 1))),
- numpy.matrix(numpy.zeros((2, 1))))
- final_B = numerical_jacobian_u(self.dynamics.discrete_dynamics,
- numpy.matrix(numpy.zeros((4, 1))),
- numpy.matrix(numpy.zeros((2, 1))))
- print 'Final A', final_A
- print 'Final B', final_B
- K, self.S = controls.dlqr(
- final_A, final_B, self.Q, self.R, optimal_cost_function=True)
- print 'Final eig:', numpy.linalg.eig(final_A - final_B * K)
+ self.R = numpy.matrix(
+ numpy.diag([1.0 / (12.0**2.0), 1.0 / (12.0**2.0)]))
- def final_cost(self, X, U):
- """Computes the final cost of being at X
+ final_A = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ numpy.matrix(numpy.zeros((4, 1))),
+ numpy.matrix(numpy.zeros((2, 1))))
+ final_B = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ numpy.matrix(numpy.zeros((4, 1))),
+ numpy.matrix(numpy.zeros((2, 1))))
+ print 'Final A', final_A
+ print 'Final B', final_B
+ K, self.S = controls.dlqr(final_A,
+ final_B,
+ self.Q,
+ self.R,
+ optimal_cost_function=True)
+ print 'Final eig:', numpy.linalg.eig(final_A - final_B * K)
+
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
Args:
X: numpy.matrix(self.num_states, 1)
@@ -79,10 +85,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of being at X
"""
- return 0.5 * X.T * self.S * X
+ return 0.5 * X.T * self.S * X
- def cost(self, X, U):
- """Computes the incremental cost given a position and U.
+ def cost(self, X, U):
+ """Computes the incremental cost given a position and U.
Args:
X: numpy.matrix(self.num_states, 1)
@@ -91,10 +97,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of evaluating U.
"""
- return U.T * self.R * U + X.T * self.Q * X
+ return U.T * self.R * U + X.T * self.Q * X
- def estimate_Q_final(self, X_hat):
- """Returns the quadraticized final Q around X_hat.
+ def estimate_Q_final(self, X_hat):
+ """Returns the quadraticized final Q around X_hat.
This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
@@ -104,13 +110,13 @@
Result:
numpy.matrix(self.num_states, self.num_states)
"""
- zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
- print 'S', self.S
- print 'Q_final', numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
- return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+ print 'S', self.S
+ print 'Q_final', numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
- def estimate_partial_cost_partial_x_final(self, X_hat):
- """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+ def estimate_partial_cost_partial_x_final(self, X_hat):
+ """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
Args:
X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -118,24 +124,26 @@
Result:
numpy.matrix(self.num_states, 1)
"""
- return numerical_jacobian_x(self.final_cost, X_hat,
- numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+ return numerical_jacobian_x(
+ self.final_cost, X_hat,
+ numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
- def estimate_q_final(self, X_hat):
- """Returns q evaluated at X_hat for the final cost function."""
- return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
-
+ def estimate_q_final(self, X_hat):
+ """Returns q evaluated at X_hat for the final cost function."""
+ return self.estimate_partial_cost_partial_x_final(
+ X_hat) - self.estimate_Q_final(X_hat) * X_hat
class SkidSteerDynamics(object):
- def __init__(self, dt):
- self.width = 0.2
- self.dt = dt
- self.num_states = 3
- self.num_inputs = 2
- def dynamics(self, X, U):
- """Calculates the dynamics for a 2 wheeled robot.
+ def __init__(self, dt):
+ self.width = 0.2
+ self.dt = dt
+ self.num_states = 3
+ self.num_inputs = 2
+
+ def dynamics(self, X, U):
+ """Calculates the dynamics for a 2 wheeled robot.
Args:
X, numpy.matrix(3, 1), The state. [x, y, theta]
@@ -144,34 +152,34 @@
Returns:
numpy.matrix(3, 1), The derivative of the dynamics.
"""
- #return numpy.matrix([[X[1, 0]],
- # [X[2, 0]],
- # [U[0, 0]]])
- return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
- [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
- [(U[1, 0] - U[0, 0]) / self.width]])
+ #return numpy.matrix([[X[1, 0]],
+ # [X[2, 0]],
+ # [U[0, 0]]])
+ return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
+ [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
+ [(U[1, 0] - U[0, 0]) / self.width]])
- def discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
+ def discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
- def inverse_discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X, dt)
+ def inverse_discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X,
+ dt)
# Simple implementation for a quadratic cost function.
class CostFunction:
- def __init__(self, dt):
- self.num_states = 3
- self.num_inputs = 2
- self.dt = dt
- self.Q = numpy.matrix([[0.1, 0, 0],
- [0, 0.6, 0],
- [0, 0, 0.1]]) / self.dt / self.dt
- self.R = numpy.matrix([[0.40, 0],
- [0, 0.40]]) / self.dt / self.dt
- def final_cost(self, X, U):
- """Computes the final cost of being at X
+ def __init__(self, dt):
+ self.num_states = 3
+ self.num_inputs = 2
+ self.dt = dt
+ self.Q = numpy.matrix([[0.1, 0, 0], [0, 0.6, 0], [0, 0, 0.1]
+ ]) / self.dt / self.dt
+ self.R = numpy.matrix([[0.40, 0], [0, 0.40]]) / self.dt / self.dt
+
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
Args:
X: numpy.matrix(self.num_states, 1)
@@ -180,10 +188,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of being at X
"""
- return X.T * self.Q * X * 1000
+ return X.T * self.Q * X * 1000
- def cost(self, X, U):
- """Computes the incremental cost given a position and U.
+ def cost(self, X, U):
+ """Computes the incremental cost given a position and U.
Args:
X: numpy.matrix(self.num_states, 1)
@@ -192,10 +200,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of evaluating U.
"""
- return U.T * self.R * U + X.T * self.Q * X
+ return U.T * self.R * U + X.T * self.Q * X
- def estimate_Q_final(self, X_hat):
- """Returns the quadraticized final Q around X_hat.
+ def estimate_Q_final(self, X_hat):
+ """Returns the quadraticized final Q around X_hat.
This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
@@ -205,11 +213,11 @@
Result:
numpy.matrix(self.num_states, self.num_states)
"""
- zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
- return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+ return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
- def estimate_partial_cost_partial_x_final(self, X_hat):
- """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+ def estimate_partial_cost_partial_x_final(self, X_hat):
+ """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
Args:
X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -217,23 +225,27 @@
Result:
numpy.matrix(self.num_states, 1)
"""
- return numerical_jacobian_x(self.final_cost, X_hat, numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+ return numerical_jacobian_x(
+ self.final_cost, X_hat,
+ numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
- def estimate_q_final(self, X_hat):
- """Returns q evaluated at X_hat for the final cost function."""
- return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
+ def estimate_q_final(self, X_hat):
+ """Returns q evaluated at X_hat for the final cost function."""
+ return self.estimate_partial_cost_partial_x_final(
+ X_hat) - self.estimate_Q_final(X_hat) * X_hat
def RungeKutta(f, x, dt):
- """4th order RungeKutta integration of F starting at X."""
- a = f(x)
- b = f(x + dt / 2.0 * a)
- c = f(x + dt / 2.0 * b)
- d = f(x + dt * c)
- return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+ """4th order RungeKutta integration of F starting at X."""
+ a = f(x)
+ b = f(x + dt / 2.0 * a)
+ c = f(x + dt / 2.0 * b)
+ d = f(x + dt * c)
+ return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
def numerical_jacobian_x(fn, X, U, epsilon=1e-4):
- """Numerically estimates the jacobian around X, U in X.
+ """Numerically estimates the jacobian around X, U in X.
Args:
fn: A function of X, U.
@@ -246,20 +258,21 @@
numpy.matrix(num_states, num_states), The jacobian of fn with X as the
variable.
"""
- num_states = X.shape[0]
- nominal = fn(X, U)
- answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
- # It's more expensive, but +- epsilon will be more reliable
- for i in range(0, num_states):
- dX_plus = X.copy()
- dX_plus[i] += epsilon
- dX_minus = X.copy()
- dX_minus[i] -= epsilon
- answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
- return answer
+ num_states = X.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
+ # It's more expensive, but +- epsilon will be more reliable
+ for i in range(0, num_states):
+ dX_plus = X.copy()
+ dX_plus[i] += epsilon
+ dX_minus = X.copy()
+ dX_minus[i] -= epsilon
+ answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
+ return answer
+
def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
- """Numerically estimates the jacobian around X, U in U.
+ """Numerically estimates the jacobian around X, U in U.
Args:
fn: A function of X, U.
@@ -272,355 +285,426 @@
numpy.matrix(num_states, num_inputs), The jacobian of fn with U as the
variable.
"""
- num_states = X.shape[0]
- num_inputs = U.shape[0]
- nominal = fn(X, U)
- answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
- for i in range(0, num_inputs):
- dU_plus = U.copy()
- dU_plus[i] += epsilon
- dU_minus = U.copy()
- dU_minus[i] -= epsilon
- answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
- return answer
+ num_states = X.shape[0]
+ num_inputs = U.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
+ for i in range(0, num_inputs):
+ dU_plus = U.copy()
+ dU_plus[i] += epsilon
+ dU_minus = U.copy()
+ dU_minus[i] -= epsilon
+ answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
+ return answer
+
def numerical_jacobian_x_x(fn, X, U):
- return numerical_jacobian_x(
- lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_x_u(fn, X, U):
- return numerical_jacobian_x(
- lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_u_x(fn, X, U):
- return numerical_jacobian_u(
- lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_u_u(fn, X, U):
- return numerical_jacobian_u(
- lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+ X, U)
class ELQR(object):
- def __init__(self, dynamics, cost):
- self.dynamics = dynamics
- self.cost = cost
-
- def Solve(self, x_hat_initial, horizon, iterations):
- l = horizon
- num_states = self.dynamics.num_states
- num_inputs = self.dynamics.num_inputs
- self.S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
- self.s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
- self.s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
- self.L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
- self.l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
- self.L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
- self.l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
+ def __init__(self, dynamics, cost):
+ self.dynamics = dynamics
+ self.cost = cost
- self.S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
- self.s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
- self.s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
+ def Solve(self, x_hat_initial, horizon, iterations):
+ l = horizon
+ num_states = self.dynamics.num_states
+ num_inputs = self.dynamics.num_inputs
+ self.S_bar_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.s_bar_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+ ]
+ self.s_scalar_bar_t = [
+ numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)
+ ]
- self.last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+ self.L_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.l_t = [
+ numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+ ]
+ self.L_bar_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.l_bar_t = [
+ numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+ ]
- # Iterate the solver
- for a in range(iterations):
- x_hat = x_hat_initial
- u_t = self.L_t[0] * x_hat + self.l_t[0]
- self.S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
- self.s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
- self.s_scalar_bar_t[0] = numpy.matrix([[0]])
+ self.S_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.s_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+ ]
+ self.s_scalar_t = [
+ numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)
+ ]
- self.last_x_hat_t[0] = x_hat_initial
+ self.last_x_hat_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+ ]
- Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_initial, u_t)
- P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_initial, u_t)
- R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_initial, u_t)
+ # Iterate the solver
+ for a in range(iterations):
+ x_hat = x_hat_initial
+ u_t = self.L_t[0] * x_hat + self.l_t[0]
+ self.S_bar_t[0] = numpy.matrix(
+ numpy.zeros((num_states, num_states)))
+ self.s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+ self.s_scalar_bar_t[0] = numpy.matrix([[0]])
- q_t = numerical_jacobian_x(self.cost.cost, x_hat_initial, u_t).T \
- - Q_t * x_hat_initial - P_t.T * u_t
- r_t = numerical_jacobian_u(self.cost.cost, x_hat_initial, u_t).T \
- - P_t * x_hat_initial - R_t * u_t
+ self.last_x_hat_t[0] = x_hat_initial
- q_scalar_t = self.cost.cost(x_hat_initial, u_t) \
- - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) \
- + u_t.T * (P_t * x_hat_initial + R_t * u_t)) \
- - x_hat_initial.T * q_t - u_t.T * r_t
+ Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_initial, u_t)
+ P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_initial, u_t)
+ R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_initial, u_t)
- start_A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics, x_hat_initial, u_t)
- start_B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics, x_hat_initial, u_t)
- x_hat_next = self.dynamics.discrete_dynamics(x_hat_initial, u_t)
- start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+ q_t = numerical_jacobian_x(self.cost.cost, x_hat_initial, u_t).T \
+ - Q_t * x_hat_initial - P_t.T * u_t
+ r_t = numerical_jacobian_u(self.cost.cost, x_hat_initial, u_t).T \
+ - P_t * x_hat_initial - R_t * u_t
- B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
- B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
- B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = \
- numpy.diag(B_svd_sigma_diag)
+ q_scalar_t = self.cost.cost(x_hat_initial, u_t) \
+ - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) \
+ + u_t.T * (P_t * x_hat_initial + R_t * u_t)) \
+ - x_hat_initial.T * q_t - u_t.T * r_t
- B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
- B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
- 0:B_svd_sigma_diag.shape[0]] = \
- numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
- B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
+ start_A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ x_hat_initial, u_t)
+ start_B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ x_hat_initial, u_t)
+ x_hat_next = self.dynamics.discrete_dynamics(x_hat_initial, u_t)
+ start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
- self.L_bar_t[1] = B_svd_inv
- self.l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
+ B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
+ B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
+ B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = \
+ numpy.diag(B_svd_sigma_diag)
- self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
+ B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
+ B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
+ 0:B_svd_sigma_diag.shape[0]] = \
+ numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
+ B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
- TotalS_1 = start_B_t.T * self.S_t[1] * start_B_t + R_t
- Totals_1 = start_B_t.T * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
- + start_B_t.T * self.s_t[1] + P_t * x_hat_initial + r_t
- Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
- + self.s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t \
- + 0.5 * x_hat_initial.T * Q_t * x_hat_initial \
- + (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.s_t[1]
+ self.L_bar_t[1] = B_svd_inv
+ self.l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial +
+ start_c_t)
- optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
- optimal_x_1 = start_A_t * x_hat_initial \
- + start_B_t * optimal_u_1 + start_c_t
+ self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
- # TODO(austin): Disable this if we are controlable. It should not be needed then.
- S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = \
- numpy.linalg.eigh(self.S_bar_t[1])
- S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
- S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
- for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
- if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
- S_bar_1_eigh_eigenvalues_stiff[i] = max(S_bar_1_eigh_eigenvalues_stiff) * 1.0
+ TotalS_1 = start_B_t.T * self.S_t[1] * start_B_t + R_t
+ Totals_1 = start_B_t.T * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
+ + start_B_t.T * self.s_t[1] + P_t * x_hat_initial + r_t
+ Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
+ + self.s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t \
+ + 0.5 * x_hat_initial.T * Q_t * x_hat_initial \
+ + (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.s_t[1]
- S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
+ optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
+ optimal_x_1 = start_A_t * x_hat_initial \
+ + start_B_t * optimal_u_1 + start_c_t
- print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
- print 'Min x_hat', optimal_x_1
- self.s_bar_t[1] = -self.s_t[1] - (S_bar_stiff + self.S_t[1]) * optimal_x_1
- self.s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 \
- - optimal_x_1.T * (S_bar_stiff + self.S_t[1]) * optimal_x_1) \
- + optimal_u_1.T * Totals_1 \
- - optimal_x_1.T * (self.s_bar_t[1] + self.s_t[1]) \
- - self.s_scalar_t[1] + Totals_scalar_1
+ # TODO(austin): Disable this if we are controlable. It should not be needed then.
+ S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = \
+ numpy.linalg.eigh(self.S_bar_t[1])
+ S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
+ S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
+ for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
+ if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
+ S_bar_1_eigh_eigenvalues_stiff[i] = max(
+ S_bar_1_eigh_eigenvalues_stiff) * 1.0
- print 'optimal_u_1', optimal_u_1
- print 'TotalS_1', TotalS_1
- print 'Totals_1', Totals_1
- print 'Totals_scalar_1', Totals_scalar_1
- print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) \
- + optimal_u_1.T * Totals_1 + Totals_scalar_1
- print 'overall cost 0', 0.5 * (x_hat_initial.T * self.S_t[0] * x_hat_initial) \
- + x_hat_initial.T * self.s_t[0] + self.s_scalar_t[0]
+ S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(
+ numpy.diag(S_bar_1_eigh_eigenvalues_stiff)
+ ) * S_bar_1_eigh_eigenvectors.T
- print 't forward 0'
- print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
- print 'x_hat[%2d]: %s' % (0, x_hat.T)
- print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
- print 'u[%2d]: %s' % (0, u_t.T)
- print ('L[ 0]: %s' % (self.L_t[0],)).replace('\n', '\n ')
- print ('l[ 0]: %s' % (self.l_t[0],)).replace('\n', '\n ')
+ print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+ print 'Min x_hat', optimal_x_1
+ self.s_bar_t[1] = -self.s_t[1] - (S_bar_stiff +
+ self.S_t[1]) * optimal_x_1
+ self.s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 \
+ - optimal_x_1.T * (S_bar_stiff + self.S_t[1]) * optimal_x_1) \
+ + optimal_u_1.T * Totals_1 \
+ - optimal_x_1.T * (self.s_bar_t[1] + self.s_t[1]) \
+ - self.s_scalar_t[1] + Totals_scalar_1
- print ('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
- print ('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
- print ('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
+ print 'optimal_u_1', optimal_u_1
+ print 'TotalS_1', TotalS_1
+ print 'Totals_1', Totals_1
+ print 'Totals_scalar_1', Totals_scalar_1
+ print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) \
+ + optimal_u_1.T * Totals_1 + Totals_scalar_1
+ print 'overall cost 0', 0.5 * (x_hat_initial.T * self.S_t[0] * x_hat_initial) \
+ + x_hat_initial.T * self.s_t[0] + self.s_scalar_t[0]
- # TODO(austin): optimal_x_1 is x_hat
- x_hat = -numpy.linalg.solve((self.S_t[1] + S_bar_stiff),
- (self.s_t[1] + self.s_bar_t[1]))
- print 'new xhat', x_hat
+ print 't forward 0'
+ print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
+ print 'x_hat[%2d]: %s' % (0, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
+ print 'u[%2d]: %s' % (0, u_t.T)
+ print('L[ 0]: %s' % (self.L_t[0], )).replace('\n', '\n ')
+ print('l[ 0]: %s' % (self.l_t[0], )).replace('\n', '\n ')
- self.S_bar_t[1] = S_bar_stiff
+ print('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
+ print('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
+ print('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
- self.last_x_hat_t[1] = x_hat
+ # TODO(austin): optimal_x_1 is x_hat
+ x_hat = -numpy.linalg.solve((self.S_t[1] + S_bar_stiff),
+ (self.s_t[1] + self.s_bar_t[1]))
+ print 'new xhat', x_hat
- for t in range(1, l):
- print 't forward', t
- u_t = self.L_t[t] * x_hat + self.l_t[t]
+ self.S_bar_t[1] = S_bar_stiff
- x_hat_next = self.dynamics.discrete_dynamics(x_hat, u_t)
- A_bar_t = numerical_jacobian_x(self.dynamics.inverse_discrete_dynamics,
- x_hat_next, u_t)
- B_bar_t = numerical_jacobian_u(self.dynamics.inverse_discrete_dynamics,
- x_hat_next, u_t)
- c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+ self.last_x_hat_t[1] = x_hat
- print 'x_hat[%2d]: %s' % (t, x_hat.T)
- print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
- print ('L[%2d]: %s' % (t, self.L_t[t],)).replace('\n', '\n ')
- print ('l[%2d]: %s' % (t, self.l_t[t],)).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
+ for t in range(1, l):
+ print 't forward', t
+ u_t = self.L_t[t] * x_hat + self.l_t[t]
- print ('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace('\n', '\n ')
- print ('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace('\n', '\n ')
- print ('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace('\n', '\n ')
+ x_hat_next = self.dynamics.discrete_dynamics(x_hat, u_t)
+ A_bar_t = numerical_jacobian_x(
+ self.dynamics.inverse_discrete_dynamics, x_hat_next, u_t)
+ B_bar_t = numerical_jacobian_u(
+ self.dynamics.inverse_discrete_dynamics, x_hat_next, u_t)
+ c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
- Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat, u_t)
- P_t = numerical_jacobian_x_u(self.cost.cost, x_hat, u_t)
- R_t = numerical_jacobian_u_u(self.cost.cost, x_hat, u_t)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
+ print('L[%2d]: %s' % (
+ t,
+ self.L_t[t],
+ )).replace('\n', '\n ')
+ print('l[%2d]: %s' % (
+ t,
+ self.l_t[t],
+ )).replace('\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
- q_t = numerical_jacobian_x(self.cost.cost, x_hat, u_t).T \
- - Q_t * x_hat - P_t.T * u_t
- r_t = numerical_jacobian_u(self.cost.cost, x_hat, u_t).T \
- - P_t * x_hat - R_t * u_t
+ print('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace(
+ '\n', '\n ')
+ print('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace(
+ '\n', '\n ')
+ print('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace(
+ '\n', '\n ')
- q_scalar_t = self.cost.cost(x_hat, u_t) \
- - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) \
- + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
+ Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat, u_t)
+ P_t = numerical_jacobian_x_u(self.cost.cost, x_hat, u_t)
+ R_t = numerical_jacobian_u_u(self.cost.cost, x_hat, u_t)
- C_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
- D_bar_t = A_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t
- E_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * B_bar_t + R_t \
- + P_t * B_bar_t + B_bar_t.T * P_t.T
- d_bar_t = A_bar_t.T * (self.s_bar_t[t] + q_t) \
- + A_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
- e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * self.s_bar_t[t] \
- + B_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
+ q_t = numerical_jacobian_x(self.cost.cost, x_hat, u_t).T \
+ - Q_t * x_hat - P_t.T * u_t
+ r_t = numerical_jacobian_u(self.cost.cost, x_hat, u_t).T \
+ - P_t * x_hat - R_t * u_t
- self.L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
- self.l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+ q_scalar_t = self.cost.cost(x_hat, u_t) \
+ - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) \
+ + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
- self.S_bar_t[t + 1] = D_bar_t + C_bar_t.T * self.L_bar_t[t + 1]
- self.s_bar_t[t + 1] = d_bar_t + C_bar_t.T * self.l_bar_t[t + 1]
- self.s_scalar_bar_t[t + 1] = \
- -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t \
- + 0.5 * c_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t \
- + c_bar_t.T * self.s_bar_t[t] + c_bar_t.T * q_t \
- + self.s_scalar_bar_t[t] + q_scalar_t
+ C_bar_t = B_bar_t.T * (self.S_bar_t[t] +
+ Q_t) * A_bar_t + P_t * A_bar_t
+ D_bar_t = A_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t
+ E_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * B_bar_t + R_t \
+ + P_t * B_bar_t + B_bar_t.T * P_t.T
+ d_bar_t = A_bar_t.T * (self.s_bar_t[t] + q_t) \
+ + A_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
+ e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * self.s_bar_t[t] \
+ + B_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
- x_hat = -numpy.linalg.solve((self.S_t[t + 1] + self.S_bar_t[t + 1]),
- (self.s_t[t + 1] + self.s_bar_t[t + 1]))
- self.S_t[l] = self.cost.estimate_Q_final(x_hat)
- self.s_t[l] = self.cost.estimate_q_final(x_hat)
- x_hat = -numpy.linalg.inv(self.S_t[l] + self.S_bar_t[l]) \
- * (self.s_t[l] + self.s_bar_t[l])
+ self.L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+ self.l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
- for t in reversed(range(l)):
- print 't backward', t
- # TODO(austin): I don't think we can use L_t like this here.
- # I think we are off by 1 somewhere...
- u_t = self.L_bar_t[t + 1] * x_hat + self.l_bar_t[t + 1]
+ self.S_bar_t[t + 1] = D_bar_t + C_bar_t.T * self.L_bar_t[t + 1]
+ self.s_bar_t[t + 1] = d_bar_t + C_bar_t.T * self.l_bar_t[t + 1]
+ self.s_scalar_bar_t[t + 1] = \
+ -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t \
+ + 0.5 * c_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t \
+ + c_bar_t.T * self.s_bar_t[t] + c_bar_t.T * q_t \
+ + self.s_scalar_bar_t[t] + q_scalar_t
- x_hat_prev = self.dynamics.inverse_discrete_dynamics(x_hat, u_t)
- print 'x_hat[%2d]: %s' % (t, x_hat.T)
- print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
- print ('L_bar[%2d]: %s' % (t + 1, self.L_bar_t[t + 1])).replace('\n', '\n ')
- print ('l_bar[%2d]: %s' % (t + 1, self.l_bar_t[t + 1])).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
- # Now compute the linearized A, B, and C
- # Start by doing it numerically, and then optimize.
- A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics, x_hat_prev, u_t)
- B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics, x_hat_prev, u_t)
- c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+ x_hat = -numpy.linalg.solve(
+ (self.S_t[t + 1] + self.S_bar_t[t + 1]),
+ (self.s_t[t + 1] + self.s_bar_t[t + 1]))
+ self.S_t[l] = self.cost.estimate_Q_final(x_hat)
+ self.s_t[l] = self.cost.estimate_q_final(x_hat)
+ x_hat = -numpy.linalg.inv(self.S_t[l] + self.S_bar_t[l]) \
+ * (self.s_t[l] + self.s_bar_t[l])
- print ('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
- print ('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
- print ('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
+ for t in reversed(range(l)):
+ print 't backward', t
+ # TODO(austin): I don't think we can use L_t like this here.
+ # I think we are off by 1 somewhere...
+ u_t = self.L_bar_t[t + 1] * x_hat + self.l_bar_t[t + 1]
- Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_prev, u_t)
- P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_prev, u_t)
- P_T_t = numerical_jacobian_u_x(self.cost.cost, x_hat_prev, u_t)
- R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_prev, u_t)
+ x_hat_prev = self.dynamics.inverse_discrete_dynamics(
+ x_hat, u_t)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
+ print('L_bar[%2d]: %s' % (t + 1, self.L_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print('l_bar[%2d]: %s' % (t + 1, self.l_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
+ # Now compute the linearized A, B, and C
+ # Start by doing it numerically, and then optimize.
+ A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ x_hat_prev, u_t)
+ B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ x_hat_prev, u_t)
+ c_t = x_hat - A_t * x_hat_prev - B_t * u_t
- q_t = numerical_jacobian_x(self.cost.cost, x_hat_prev, u_t).T \
- - Q_t * x_hat_prev - P_T_t * u_t
- r_t = numerical_jacobian_u(self.cost.cost, x_hat_prev, u_t).T \
- - P_t * x_hat_prev - R_t * u_t
+ print('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
+ print('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
+ print('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
- q_scalar_t = self.cost.cost(x_hat_prev, u_t) \
- - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) \
- + u_t.T * (P_t * x_hat_prev + R_t * u_t)) \
- - x_hat_prev.T * q_t - u_t.T * r_t
+ Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_prev, u_t)
+ P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_prev, u_t)
+ P_T_t = numerical_jacobian_u_x(self.cost.cost, x_hat_prev, u_t)
+ R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_prev, u_t)
- C_t = P_t + B_t.T * self.S_t[t + 1] * A_t
- D_t = Q_t + A_t.T * self.S_t[t + 1] * A_t
- E_t = R_t + B_t.T * self.S_t[t + 1] * B_t
- d_t = q_t + A_t.T * self.s_t[t + 1] + A_t.T * self.S_t[t + 1] * c_t
- e_t = r_t + B_t.T * self.s_t[t + 1] + B_t.T * self.S_t[t + 1] * c_t
- self.L_t[t] = -numpy.linalg.inv(E_t) * C_t
- self.l_t[t] = -numpy.linalg.inv(E_t) * e_t
- self.s_t[t] = d_t + C_t.T * self.l_t[t]
- self.S_t[t] = D_t + C_t.T * self.L_t[t]
- self.s_scalar_t[t] = q_scalar_t \
- - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t \
- + 0.5 * c_t.T * self.S_t[t + 1] * c_t \
- + c_t.T * self.s_t[t + 1] \
- + self.s_scalar_t[t + 1]
+ q_t = numerical_jacobian_x(self.cost.cost, x_hat_prev, u_t).T \
+ - Q_t * x_hat_prev - P_T_t * u_t
+ r_t = numerical_jacobian_u(self.cost.cost, x_hat_prev, u_t).T \
+ - P_t * x_hat_prev - R_t * u_t
- x_hat = -numpy.linalg.solve((self.S_t[t] + self.S_bar_t[t]),
- (self.s_t[t] + self.s_bar_t[t]))
- if t == 0:
- self.last_x_hat_t[t] = x_hat_initial
- else:
- self.last_x_hat_t[t] = x_hat
+ q_scalar_t = self.cost.cost(x_hat_prev, u_t) \
+ - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) \
+ + u_t.T * (P_t * x_hat_prev + R_t * u_t)) \
+ - x_hat_prev.T * q_t - u_t.T * r_t
- x_hat_t = [x_hat_initial]
+ C_t = P_t + B_t.T * self.S_t[t + 1] * A_t
+ D_t = Q_t + A_t.T * self.S_t[t + 1] * A_t
+ E_t = R_t + B_t.T * self.S_t[t + 1] * B_t
+ d_t = q_t + A_t.T * self.s_t[t + 1] + A_t.T * self.S_t[t +
+ 1] * c_t
+ e_t = r_t + B_t.T * self.s_t[t + 1] + B_t.T * self.S_t[t +
+ 1] * c_t
+ self.L_t[t] = -numpy.linalg.inv(E_t) * C_t
+ self.l_t[t] = -numpy.linalg.inv(E_t) * e_t
+ self.s_t[t] = d_t + C_t.T * self.l_t[t]
+ self.S_t[t] = D_t + C_t.T * self.L_t[t]
+ self.s_scalar_t[t] = q_scalar_t \
+ - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t \
+ + 0.5 * c_t.T * self.S_t[t + 1] * c_t \
+ + c_t.T * self.s_t[t + 1] \
+ + self.s_scalar_t[t + 1]
- pylab.figure('states %d' % a)
- pylab.ion()
- for dim in range(num_states):
- pylab.plot(numpy.arange(len(self.last_x_hat_t)),
- [x_hat_loop[dim, 0] for x_hat_loop in self.last_x_hat_t],
- marker='o', label='Xhat[%d]' % dim)
- pylab.legend()
- pylab.draw()
+ x_hat = -numpy.linalg.solve((self.S_t[t] + self.S_bar_t[t]),
+ (self.s_t[t] + self.s_bar_t[t]))
+ if t == 0:
+ self.last_x_hat_t[t] = x_hat_initial
+ else:
+ self.last_x_hat_t[t] = x_hat
- pylab.figure('xy %d' % a)
- pylab.ion()
- pylab.plot([x_hat_loop[0, 0] for x_hat_loop in self.last_x_hat_t],
- [x_hat_loop[1, 0] for x_hat_loop in self.last_x_hat_t],
- marker='o', label='trajectory')
- pylab.legend()
- pylab.draw()
+ x_hat_t = [x_hat_initial]
- final_u_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
- cost_to_go = []
- cost_to_come = []
- cost = []
- for t in range(l):
- cost_to_go.append(
- (0.5 * self.last_x_hat_t[t].T * self.S_t[t] * self.last_x_hat_t[t] \
- + self.last_x_hat_t[t].T * self.s_t[t] + self.s_scalar_t[t])[0, 0])
- cost_to_come.append(
- (0.5 * self.last_x_hat_t[t].T * self.S_bar_t[t] * self.last_x_hat_t[t] \
- + self.last_x_hat_t[t].T * self.s_bar_t[t] + self.s_scalar_bar_t[t])[0, 0])
- cost.append(cost_to_go[-1] + cost_to_come[-1])
- final_u_t[t] = self.L_t[t] * self.last_x_hat_t[t] + self.l_t[t]
+ pylab.figure('states %d' % a)
+ pylab.ion()
+ for dim in range(num_states):
+ pylab.plot(
+ numpy.arange(len(self.last_x_hat_t)),
+ [x_hat_loop[dim, 0] for x_hat_loop in self.last_x_hat_t],
+ marker='o',
+ label='Xhat[%d]' % dim)
+ pylab.legend()
+ pylab.draw()
- for t in range(l):
- A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
- self.last_x_hat_t[t], final_u_t[t])
- B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
- self.last_x_hat_t[t], final_u_t[t])
- c_t = self.dynamics.discrete_dynamics(self.last_x_hat_t[t], final_u_t[t]) \
- - A_t * self.last_x_hat_t[t] - B_t * final_u_t[t]
- print("Infeasability at", t, "is",
- ((A_t * self.last_x_hat_t[t] + B_t * final_u_t[t] + c_t) \
- - self.last_x_hat_t[t + 1]).T)
+ pylab.figure('xy %d' % a)
+ pylab.ion()
+ pylab.plot([x_hat_loop[0, 0] for x_hat_loop in self.last_x_hat_t],
+ [x_hat_loop[1, 0] for x_hat_loop in self.last_x_hat_t],
+ marker='o',
+ label='trajectory')
+ pylab.legend()
+ pylab.draw()
- pylab.figure('u')
- samples = numpy.arange(len(final_u_t))
- for i in range(num_inputs):
- pylab.plot(samples, [u[i, 0] for u in final_u_t],
- label='u[%d]' % i, marker='o')
- pylab.legend()
+ final_u_t = [
+ numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+ ]
+ cost_to_go = []
+ cost_to_come = []
+ cost = []
+ for t in range(l):
+ cost_to_go.append(
+ (0.5 * self.last_x_hat_t[t].T * self.S_t[t] * self.last_x_hat_t[t] \
+ + self.last_x_hat_t[t].T * self.s_t[t] + self.s_scalar_t[t])[0, 0])
+ cost_to_come.append(
+ (0.5 * self.last_x_hat_t[t].T * self.S_bar_t[t] * self.last_x_hat_t[t] \
+ + self.last_x_hat_t[t].T * self.s_bar_t[t] + self.s_scalar_bar_t[t])[0, 0])
+ cost.append(cost_to_go[-1] + cost_to_come[-1])
+ final_u_t[t] = self.L_t[t] * self.last_x_hat_t[t] + self.l_t[t]
- pylab.figure('cost')
- cost_samples = numpy.arange(len(cost))
- pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
- pylab.plot(cost_samples, cost_to_come, label='cost to come', marker='o')
- pylab.plot(cost_samples, cost, label='cost', marker='o')
- pylab.legend()
+ for t in range(l):
+ A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ self.last_x_hat_t[t], final_u_t[t])
+ B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ self.last_x_hat_t[t], final_u_t[t])
+ c_t = self.dynamics.discrete_dynamics(self.last_x_hat_t[t], final_u_t[t]) \
+ - A_t * self.last_x_hat_t[t] - B_t * final_u_t[t]
+ print("Infeasability at", t, "is",
+ ((A_t * self.last_x_hat_t[t] + B_t * final_u_t[t] + c_t) \
+ - self.last_x_hat_t[t + 1]).T)
- pylab.ioff()
- pylab.show()
+ pylab.figure('u')
+ samples = numpy.arange(len(final_u_t))
+ for i in range(num_inputs):
+ pylab.plot(samples, [u[i, 0] for u in final_u_t],
+ label='u[%d]' % i,
+ marker='o')
+ pylab.legend()
+
+ pylab.figure('cost')
+ cost_samples = numpy.arange(len(cost))
+ pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
+ pylab.plot(cost_samples,
+ cost_to_come,
+ label='cost to come',
+ marker='o')
+ pylab.plot(cost_samples, cost, label='cost', marker='o')
+ pylab.legend()
+
+ pylab.ioff()
+ pylab.show()
+
if __name__ == '__main__':
- dt = 0.05
- #arm_dynamics = ArmDynamics(dt=dt)
- #elqr = ELQR(arm_dynamics, ArmCostFunction(dt=dt, dynamics=arm_dynamics))
- #x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0], [0.0]])
- #elqr.Solve(x_hat_initial, 100, 3)
+ dt = 0.05
+ #arm_dynamics = ArmDynamics(dt=dt)
+ #elqr = ELQR(arm_dynamics, ArmCostFunction(dt=dt, dynamics=arm_dynamics))
+ #x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0], [0.0]])
+ #elqr.Solve(x_hat_initial, 100, 3)
- elqr = ELQR(SkidSteerDynamics(dt=dt), CostFunction(dt=dt))
- x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
- elqr.Solve(x_hat_initial, 100, 15)
- sys.exit(1)
+ elqr = ELQR(SkidSteerDynamics(dt=dt), CostFunction(dt=dt))
+ x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
+ elqr.Solve(x_hat_initial, 100, 15)
+ sys.exit(1)
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 22bd1d2..e7f8ce1 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -23,16 +23,16 @@
alpha_unitizer = "(::Eigen::Matrix<double, 2, 2>() << %f, %f, %f, %f).finished()" % (
segment.alpha_unitizer[0, 0], segment.alpha_unitizer[0, 1],
segment.alpha_unitizer[1, 0], segment.alpha_unitizer[1, 1])
- cc_file.append( " trajectories->emplace_back(%s," % (vmax))
- cc_file.append( " %s," % (alpha_unitizer))
+ cc_file.append(" trajectories->emplace_back(%s," % (vmax))
+ cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
" Trajectory(Path::Reversed(%s()), 0.005));"
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(%s(), 0.005));"
- % (path_function_name(str(name))))
+ " Trajectory(%s(), 0.005));" %
+ (path_function_name(str(name))))
start_index = None
end_index = None
@@ -45,15 +45,15 @@
if reverse:
start_index, end_index = end_index, start_index
- cc_file.append(" edges.push_back(SearchGraph::Edge{%s(), %s()," %
- (index_function_name(start_index),
- index_function_name(end_index)))
cc_file.append(
- " (trajectories->back().trajectory.path().length() + 0.2)});")
+ " edges.push_back(SearchGraph::Edge{%s(), %s()," %
+ (index_function_name(start_index), index_function_name(end_index)))
+ cc_file.append(
+ " (trajectories->back().trajectory.path().length() + 0.2)});"
+ )
# TODO(austin): Allow different vmaxes for different paths.
- cc_file.append(
- " trajectories->back().trajectory.OptimizeTrajectory(")
+ cc_file.append(" trajectories->back().trajectory.OptimizeTrajectory(")
cc_file.append(" trajectories->back().alpha_unitizer,")
cc_file.append(" trajectories->back().vmax);")
cc_file.append("")
@@ -116,15 +116,16 @@
h_file.append("")
h_file.append("constexpr uint32_t %s() { return %d; }" %
(index_function_name(point[1]), index))
- h_file.append(
- "inline ::Eigen::Matrix<double, 2, 1> %sPoint() {" % point[1])
+ h_file.append("inline ::Eigen::Matrix<double, 2, 1> %sPoint() {" %
+ point[1])
h_file.append(
" return (::Eigen::Matrix<double, 2, 1>() << %f, %f).finished();"
% (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
h_file.append("}")
front_points = [
- index_function_name(point[1]) + "()" for point in graph_generate.front_points
+ index_function_name(point[1]) + "()"
+ for point in graph_generate.front_points
]
h_file.append("")
h_file.append("constexpr ::std::array<uint32_t, %d> FrontPoints() {" %
@@ -134,7 +135,8 @@
h_file.append("}")
back_points = [
- index_function_name(point[1]) + "()" for point in graph_generate.back_points
+ index_function_name(point[1]) + "()"
+ for point in graph_generate.back_points
]
h_file.append("")
h_file.append("constexpr ::std::array<uint32_t, %d> BackPoints() {" %
@@ -149,10 +151,10 @@
for name, segment in list(enumerate(graph_generate.unnamed_segments)) + [
(x.name, x) for x in graph_generate.named_segments
]:
- h_file.append(
- "::std::unique_ptr<Path> %s();" % path_function_name(name))
- cc_file.append(
- "::std::unique_ptr<Path> %s() {" % path_function_name(name))
+ h_file.append("::std::unique_ptr<Path> %s();" %
+ path_function_name(name))
+ cc_file.append("::std::unique_ptr<Path> %s() {" %
+ path_function_name(name))
cc_file.append(" return ::std::unique_ptr<Path>(new Path({")
for point in segment.ToThetaPoints():
cc_file.append(" {{%.12f, %.12f, %.12f," %
@@ -188,7 +190,8 @@
"::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
- cc_file.append(" " "double vmax) {")
+ cc_file.append(" "
+ "double vmax) {")
cc_file.append(" ::std::vector<SearchGraph::Edge> edges;")
index = 0
@@ -202,8 +205,8 @@
add_edge(cc_file, name, segment, index, True)
index += 1
- cc_file.append(" return SearchGraph(%d, ::std::move(edges));" % len(
- graph_generate.points))
+ cc_file.append(" return SearchGraph(%d, ::std::move(edges));" %
+ len(graph_generate.points))
cc_file.append("}")
h_file.append("")
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index fa9c9e5..7b6179c 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -7,6 +7,7 @@
import random
import gi
import numpy
+
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk
import cairo
@@ -70,8 +71,8 @@
# right hand side lines
lines2 = [(joint_center[0] + derr, 0.3048), (0.422275, 0.3048),
- (0.422275, 0.1397), (0.826135, 0.1397), (0.826135,
- inter_y(0.826135))]
+ (0.422275, 0.1397), (0.826135, 0.1397),
+ (0.826135, inter_y(0.826135))]
t1_min = get_angle((32.525 - 4.0) * 0.0254)
t2_min = -7.0 / 4.0 * numpy.pi
@@ -119,8 +120,8 @@
p1 = Polygon(lines_theta)
-p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max), (t1_min,
- t2_max)])
+p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max),
+ (t1_min, t2_max)])
# Fully computed theta constrints.
lines_theta = list(p1.intersection(p2).exterior.coords)
@@ -157,7 +158,6 @@
abs(dpx * pdx + dpy * pdy)
-
def closest_segment(lines, pt):
c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
for i in range(1, len(lines)):
@@ -172,6 +172,7 @@
# Create a GTK+ widget on which we will draw using Cairo
class Silly(basic_window.BaseWindow):
+
def __init__(self):
super(Silly, self).__init__()
@@ -230,6 +231,7 @@
self.window.queue_draw()
def method_connect(self, event, cb):
+
def handler(obj, *args):
cb(*args)
@@ -332,7 +334,8 @@
cr.stroke()
if not self.theta_version:
- theta1, theta2 = to_theta(self.last_pos, self.circular_index_select)
+ theta1, theta2 = to_theta(self.last_pos,
+ self.circular_index_select)
x, y = joint_center[0], joint_center[1]
cr.move_to(x, y)
@@ -473,16 +476,16 @@
else:
self.segments[0].control2 = self.now_segment_pt
- print('Clicked at theta: %s' % (repr(self.now_segment_pt,)))
+ print('Clicked at theta: %s' % (repr(self.now_segment_pt, )))
if not self.theta_version:
print('Clicked at xy, circular index: (%f, %f, %f)' %
(self.last_pos[0], self.last_pos[1],
self.circular_index_select))
- print('c1: numpy.array([%f, %f])' % (self.segments[0].control1[0],
- self.segments[0].control1[1]))
- print('c2: numpy.array([%f, %f])' % (self.segments[0].control2[0],
- self.segments[0].control2[1]))
+ print('c1: numpy.array([%f, %f])' %
+ (self.segments[0].control1[0], self.segments[0].control1[1]))
+ print('c2: numpy.array([%f, %f])' %
+ (self.segments[0].control2[0], self.segments[0].control2[1]))
self.redraw()
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index dae7caa..046b9dd 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -195,6 +195,7 @@
# Segment in angle space.
class AngleSegment:
+
def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
"""Creates an angle segment.
@@ -321,8 +322,8 @@
a = alpha_blend(start, control1, alpha)
b = alpha_blend(control1, control2, alpha)
c = alpha_blend(control2, end, alpha)
- return alpha_blend(
- alpha_blend(a, b, alpha), alpha_blend(b, c, alpha), alpha)
+ return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+ alpha)
def subdivide_spline(start, control1, control2, end):
@@ -333,6 +334,7 @@
class SplineSegment:
+
def __init__(self,
start,
control1,
@@ -350,10 +352,9 @@
self.vmax = vmax
def __repr__(self):
- return "SplineSegment(%s, %s, %s, %s)" % (repr(self.start),
- repr(self.control1),
- repr(self.control2),
- repr(self.end))
+ return "SplineSegment(%s, %s, %s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
def DrawTo(self, cr, theta_version):
if theta_version:
@@ -364,9 +365,8 @@
end = get_xy(self.end)
draw_lines(cr, [
- to_theta(
- spline_eval(start, control1, control2, end, alpha),
- c_i_select)
+ to_theta(spline_eval(start, control1, control2, end, alpha),
+ c_i_select)
for alpha in subdivide_spline(start, control1, control2, end)
])
cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
@@ -418,6 +418,7 @@
class ThetaSplineSegment:
+
def __init__(self,
start,
control1,
@@ -435,10 +436,9 @@
self.vmax = vmax
def __repr__(self):
- return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(self.start),
- repr(self.control1),
- repr(self.control2),
- repr(self.end))
+ return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
def DrawTo(self, cr, theta_version):
if (theta_version):
@@ -485,30 +485,36 @@
short_box_x = 0.431
short_box_y = 0.082
-ready_above_box = to_theta_with_circular_index(
- tall_box_x, tall_box_y + 0.08, circular_index=-1)
-tall_box_grab = to_theta_with_circular_index(
- tall_box_x, tall_box_y, circular_index=-1)
-short_box_grab = to_theta_with_circular_index(
- short_box_x, short_box_y, circular_index=-1)
+ready_above_box = to_theta_with_circular_index(tall_box_x,
+ tall_box_y + 0.08,
+ circular_index=-1)
+tall_box_grab = to_theta_with_circular_index(tall_box_x,
+ tall_box_y,
+ circular_index=-1)
+short_box_grab = to_theta_with_circular_index(short_box_x,
+ short_box_y,
+ circular_index=-1)
# TODO(austin): Drive the front/back off the same numbers a bit better.
front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
-front_middle3_box = to_theta_with_circular_index(
- 0.700, 2.125, circular_index=-1.000000)
-front_middle2_box = to_theta_with_circular_index(
- 0.700, 2.268, circular_index=-1)
-front_middle1_box = to_theta_with_circular_index(
- 0.800, 1.915, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(0.700,
+ 2.125,
+ circular_index=-1.000000)
+front_middle2_box = to_theta_with_circular_index(0.700,
+ 2.268,
+ circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(0.800,
+ 1.915,
+ circular_index=-1)
front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
-back_middle2_box = to_theta_with_circular_index(
- -0.700, 2.27, circular_index=0)
-back_middle1_box = to_theta_with_circular_index(
- -0.800, 1.93, circular_index=0)
+back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
-back_extra_low_box = to_theta_with_circular_index(-0.87, 1.52, circular_index=0)
+back_extra_low_box = to_theta_with_circular_index(-0.87,
+ 1.52,
+ circular_index=0)
front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
@@ -517,26 +523,20 @@
up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
-front_switch_auto = to_theta_with_circular_index(
- 0.750, 2.20, circular_index=-1.000000)
+front_switch_auto = to_theta_with_circular_index(0.750,
+ 2.20,
+ circular_index=-1.000000)
-duck = numpy.array(
- [numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
+duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
-starting = numpy.array(
- [numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
-vertical_starting = numpy.array(
- [numpy.pi / 2.0, -numpy.pi / 2.0])
+starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
-self_hang = numpy.array(
- [numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
-partner_hang = numpy.array(
- [numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
+self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
-above_hang = numpy.array(
- [numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
-below_hang = numpy.array(
- [numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
+above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
+below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
up_c1 = to_theta((0.63, 1.17), circular_index=-1)
up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -603,25 +603,25 @@
num_points = int(numpy.ceil(norm / max_distance))
last_iteration_point = previous_point
for subindex in range(1, num_points):
- subpoint = to_theta(
- alpha_blend(previous_point_xy, point_xy,
- float(subindex) / num_points),
- circular_index=circular_index)
- result_points.append((subpoint, '%s%dof%d' % (name, subindex,
- num_points)))
+ subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
+ float(subindex) / num_points),
+ circular_index=circular_index)
+ result_points.append(
+ (subpoint, '%s%dof%d' % (name, subindex, num_points)))
result_paths.append(
XYSegment(last_iteration_point, subpoint, vmax=6.0))
if (last_iteration_point != previous_point).any():
result_paths.append(XYSegment(previous_point, subpoint))
if subindex == num_points - 1:
- result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+ result_paths.append(XYSegment(subpoint, point, vmax=6.0))
else:
- result_paths.append(XYSegment(subpoint, point))
+ result_paths.append(XYSegment(subpoint, point))
last_iteration_point = subpoint
result_points.append((point, name))
return result_points, result_paths
+
front_points, front_paths = expand_points(sparse_front_points, 0.06)
back_points, back_paths = expand_points(sparse_back_points, 0.06)
@@ -650,7 +650,6 @@
front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
-
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
@@ -670,30 +669,58 @@
neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
-
named_segments = [
- ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2, back_switch, "BackSwitch"),
-
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2, back_high_box, "NeutralBoxToHigh", alpha_unitizer=long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "NeutralBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "NeutralBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2, back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
-
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "ReadyBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "ReadyBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_low_c2, back_low_box, "ReadyBoxToLow", long_alpha_unitizer),
-
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_high_box, "ShortBoxToHigh", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "ShortBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "ShortBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_low_box, "ShortBoxToLow", long_alpha_unitizer),
-
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_high_box, "TallBoxToHigh", long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "TallBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "TallBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_low_box, "TallBoxToLow", long_alpha_unitizer),
-
+ ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
+ back_switch, "BackSwitch"),
+ ThetaSplineSegment(neutral,
+ neutral_to_back_low_c1,
+ tall_to_back_high_c2,
+ back_high_box,
+ "NeutralBoxToHigh",
+ alpha_unitizer=long_alpha_unitizer),
+ ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
+ back_middle2_box, "NeutralBoxToMiddle2",
+ long_alpha_unitizer),
+ ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+ back_middle1_box, "NeutralBoxToMiddle1",
+ long_alpha_unitizer),
+ ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+ back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
+ long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_high_c2, back_middle2_box,
+ "ReadyBoxToMiddle2", long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_low_c2, back_middle1_box,
+ "ReadyBoxToMiddle1", long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
+ long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
+ long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_middle2_box,
+ "ShortBoxToMiddle2", long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_low_c2, back_middle1_box,
+ "ShortBoxToMiddle1", long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
+ long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
+ long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_middle2_box,
+ "TallBoxToMiddle2", long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+ back_middle1_box, "TallBoxToMiddle1",
+ long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+ back_low_box, "TallBoxToLow", long_alpha_unitizer),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
ready_above_box, "ReadyToNeutral"),
XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
@@ -713,32 +740,29 @@
]
unnamed_segments = [
- SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2, front_switch_auto),
+ SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
+ front_switch_auto),
SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
-
XYSegment(ready_above_box, front_low_box),
XYSegment(ready_above_box, front_switch),
XYSegment(ready_above_box, front_middle1_box),
XYSegment(ready_above_box, front_middle2_box),
XYSegment(ready_above_box, front_middle3_box),
- SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, front_high_box),
-
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
+ front_high_box),
AngleSegment(starting, vertical_starting),
AngleSegment(vertical_starting, neutral),
-
XYSegment(neutral, front_low_box),
XYSegment(up, front_high_box),
XYSegment(up, front_middle2_box),
-
XYSegment(front_middle3_box, up),
XYSegment(front_middle3_box, front_high_box),
XYSegment(front_middle3_box, front_middle2_box),
XYSegment(front_middle3_box, front_middle1_box),
-
XYSegment(up, front_middle1_box),
XYSegment(up, front_low_box),
XYSegment(front_high_box, front_middle2_box),
@@ -760,7 +784,6 @@
XYSegment(back_middle2_box, back_middle1_box),
XYSegment(back_middle2_box, back_low_box),
XYSegment(back_middle1_box, back_low_box),
-
AngleSegment(up, above_hang),
AngleSegment(above_hang, below_hang),
AngleSegment(up, below_hang),
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index 00f737b..92b7c7a 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -105,8 +105,11 @@
r_nm = 0.025
self.R = numpy.matrix(numpy.diag([(r_nm**2.0), (r_nm**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.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
@@ -181,8 +184,9 @@
self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
- self.K_transformed = controls.dlqr(
- self.A_transformed, self.B_transformed, self.Q_lqr, self.R)
+ self.K_transformed = controls.dlqr(self.A_transformed,
+ self.B_transformed, self.Q_lqr,
+ self.R)
# Write the controller back out in the absolute coordinate system.
self.K = numpy.hstack(
@@ -215,8 +219,11 @@
repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
self._name)
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
self.InitializeState()
@@ -359,11 +366,10 @@
observer_intake.X_hat[0, 0] = intake.X[0, 0]
# Test moving the intake with constant separation.
- scenario_plotter.run_test(
- intake,
- controller_intake=intake_controller,
- observer_intake=observer_intake,
- iterations=200)
+ scenario_plotter.run_test(intake,
+ controller_intake=intake_controller,
+ observer_intake=observer_intake,
+ iterations=200)
if FLAGS.plot:
scenario_plotter.Plot()
@@ -376,8 +382,8 @@
else:
namespaces = ['y2018', 'control_loops', 'superstructure', 'intake']
intake = Intake('Intake')
- loop_writer = control_loop.ControlLoopWriter(
- 'Intake', [intake], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Intake', [intake],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kGearRatio', '%f', intake.G))
loop_writer.AddConstant(
@@ -387,8 +393,9 @@
loop_writer.Write(argv[1], argv[2])
delayed_intake = DelayedIntake('DelayedIntake')
- loop_writer = control_loop.ControlLoopWriter(
- 'DelayedIntake', [delayed_intake], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('DelayedIntake',
+ [delayed_intake],
+ namespaces=namespaces)
loop_writer.Write(argv[3], argv[4])
diff --git a/y2018/control_loops/python/intake_simple.py b/y2018/control_loops/python/intake_simple.py
index 9b6ffb1..4d34f4b 100644
--- a/y2018/control_loops/python/intake_simple.py
+++ b/y2018/control_loops/python/intake_simple.py
@@ -21,253 +21,273 @@
run_count = 0
theta_travel = 0.0
+
def to_deg(angle):
- return angle * rad_to_deg
+ return angle * rad_to_deg
+
def to_rad(angle):
- return angle / rad_to_deg
+ return angle / rad_to_deg
+
def to_rotations(angle):
- return angle / pi2
+ return angle / pi2
+
def time_derivative(x, t, voltage, c1, c2, c3):
- global run_count
- theta, omega = x
- dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
- run_count = run_count + 1
+ global run_count
+ theta, omega = x
+ dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
+ run_count = run_count + 1
- return dxdt
+ return dxdt
+
def get_distal_angle(theta_proximal):
- # For the proximal angle = -50 degrees, the distal angle is -180 degrees
- # For the proximal angle = 10 degrees, the distal angle is -90 degrees
- distal_angle = to_rad(-180.0 - (-50.0 - to_deg(theta_proximal)) * \
- (180.0 - 90.0) / (50.0 + 10.0))
- return distal_angle
+ # For the proximal angle = -50 degrees, the distal angle is -180 degrees
+ # For the proximal angle = 10 degrees, the distal angle is -90 degrees
+ distal_angle = to_rad(-180.0 - (-50.0 - to_deg(theta_proximal)) * \
+ (180.0 - 90.0) / (50.0 + 10.0))
+ return distal_angle
def get_180_degree_time(c1, c2, c3, voltage, gear_ratio, motor_free_speed):
- global run_count
- global theta_travel
+ global run_count
+ global theta_travel
- if ( True ):
- # Gravity is assisting the motion.
- theta_start = 0.0
- theta_target = pi
- elif ( False ):
- # Gravity is assisting the motion.
- theta_start = 0.0
- theta_target = -pi
- elif ( False ):
- # Gravity is slowing the motion.
- theta_start = pi
- theta_target = 0.0
- elif ( False ):
- # Gravity is slowing the motion.
- theta_start = -pi
- theta_target = 0.0
- elif ( False ):
- # This is for the proximal arm motion.
- theta_start = to_rad(-50.0)
- theta_target = to_rad(10.0)
+ if (True):
+ # Gravity is assisting the motion.
+ theta_start = 0.0
+ theta_target = pi
+ elif (False):
+ # Gravity is assisting the motion.
+ theta_start = 0.0
+ theta_target = -pi
+ elif (False):
+ # Gravity is slowing the motion.
+ theta_start = pi
+ theta_target = 0.0
+ elif (False):
+ # Gravity is slowing the motion.
+ theta_start = -pi
+ theta_target = 0.0
+ elif (False):
+ # This is for the proximal arm motion.
+ theta_start = to_rad(-50.0)
+ theta_target = to_rad(10.0)
- theta_half = 0.5 * (theta_start + theta_target)
- if theta_start > theta_target:
- voltage = -voltage
- theta = theta_start
- theta_travel = theta_start - theta_target
- if run_count == 0:
- print("# Theta Start = %.2f radians End = %.2f Theta travel %.2f "
- "Theta half = %.2f Voltage = %.2f" % (
- theta_start, theta_target, theta_travel, theta_half, voltage))
- print("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f "
- "Theta half = %.2f Voltage = %.2f" % (
- to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
- to_deg(theta_half), voltage))
- omega = 0.0
- time = 0.0
- delta_time = 0.01 # time step in seconds
- for step in range(1, 5000):
- t = numpy.array([time, time + delta_time])
- time = time + delta_time
- x = [theta, omega]
- angular_acceleration = -c1 * omega + c2 * voltage
- x_n_plus_1 = scipy.integrate.odeint(time_derivative, x, t,
- args=(voltage, c1, c2, c3))
- theta, omega = x_n_plus_1[1]
+ theta_half = 0.5 * (theta_start + theta_target)
+ if theta_start > theta_target:
+ voltage = -voltage
+ theta = theta_start
+ theta_travel = theta_start - theta_target
+ if run_count == 0:
+ print("# Theta Start = %.2f radians End = %.2f Theta travel %.2f "
+ "Theta half = %.2f Voltage = %.2f" %
+ (theta_start, theta_target, theta_travel, theta_half, voltage))
+ print("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f "
+ "Theta half = %.2f Voltage = %.2f" %
+ (to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
+ to_deg(theta_half), voltage))
+ omega = 0.0
+ time = 0.0
+ delta_time = 0.01 # time step in seconds
+ for step in range(1, 5000):
+ t = numpy.array([time, time + delta_time])
+ time = time + delta_time
+ x = [theta, omega]
+ angular_acceleration = -c1 * omega + c2 * voltage
+ x_n_plus_1 = scipy.integrate.odeint(time_derivative,
+ x,
+ t,
+ args=(voltage, c1, c2, c3))
+ theta, omega = x_n_plus_1[1]
- if ( False ):
- print("%4d %8.4f %8.2f %8.4f %8.4f %8.3f "
- "%8.3f %8.3f %8.3f" % (
- step, time, theta, omega, angular_acceleration,
- to_rotations(theta), to_rotations(omega),
- omega * gear_ratio * 60.0 / pi2,
- omega * gear_ratio / motor_free_speed))
- if theta_start < theta_target:
- # Angle is increasing through the motion.
- if theta > theta_half:
- break
- else:
- # Angle is decreasing through the motion.
- if (theta < theta_half):
- break
+ if (False):
+ print(
+ "%4d %8.4f %8.2f %8.4f %8.4f %8.3f "
+ "%8.3f %8.3f %8.3f" %
+ (step, time, theta, omega, angular_acceleration,
+ to_rotations(theta), to_rotations(omega), omega * gear_ratio *
+ 60.0 / pi2, omega * gear_ratio / motor_free_speed))
+ if theta_start < theta_target:
+ # Angle is increasing through the motion.
+ if theta > theta_half:
+ break
+ else:
+ # Angle is decreasing through the motion.
+ if (theta < theta_half):
+ break
- return 2.0 * time
+ return 2.0 * time
+
def main():
- # m/sec^2 Gravity Constant
- gravity = 9.8
- # m/sec^2 Gravity Constant - Use 0.0 for the intake. It is horizontal.
- gravity = 0.0
- # Volts
- voltage_nominal = 12
+ # m/sec^2 Gravity Constant
+ gravity = 9.8
+ # m/sec^2 Gravity Constant - Use 0.0 for the intake. It is horizontal.
+ gravity = 0.0
+ # Volts
+ voltage_nominal = 12
- # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
- motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
- current_stall = 134 # amps stall current
- current_no_load = 0.7 # amps no load current
- torque_stall = 710/1000.0 # N-m Stall Torque
- speed_no_load_rpm = 18730 # RPM no load speed
+ # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
+ motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
+ current_stall = 134 # amps stall current
+ current_no_load = 0.7 # amps no load current
+ torque_stall = 710 / 1000.0 # N-m Stall Torque
+ speed_no_load_rpm = 18730 # RPM no load speed
- if ( True ):
- # Bag motor from https://www.vexrobotics.com/217-3351.html
- motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
- current_stall = 53.0 # amps stall current
- current_no_load = 1.8 # amps no load current
- torque_stall = 0.4 # N-m Stall Torque
- speed_no_load_rpm = 13180.0 # RPM no load speed
+ if (True):
+ # Bag motor from https://www.vexrobotics.com/217-3351.html
+ motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
+ current_stall = 53.0 # amps stall current
+ current_no_load = 1.8 # amps no load current
+ torque_stall = 0.4 # N-m Stall Torque
+ speed_no_load_rpm = 13180.0 # RPM no load speed
- if ( False ):
- # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
- motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
- current_stall = 89.0 # amps stall current
- current_no_load = 3.0 # amps no load current
- torque_stall = 1.4 # N-m Stall Torque
- speed_no_load_rpm = 5840.0 # RPM no load speed
+ if (False):
+ # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
+ motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
+ current_stall = 89.0 # amps stall current
+ current_no_load = 3.0 # amps no load current
+ torque_stall = 1.4 # N-m Stall Torque
+ speed_no_load_rpm = 5840.0 # RPM no load speed
- # How many motors are we using?
- num_motors = 1
+ # How many motors are we using?
+ num_motors = 1
- # Motor values
- print("# Motor: %s" % (motor_name))
- print("# Number of motors: %d" % (num_motors))
- print("# Stall torque: %.1f n-m" % (torque_stall))
- print("# Stall current: %.1f amps" % (current_stall))
- print("# No load current: %.1f amps" % (current_no_load))
- print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
+ # Motor values
+ print("# Motor: %s" % (motor_name))
+ print("# Number of motors: %d" % (num_motors))
+ print("# Stall torque: %.1f n-m" % (torque_stall))
+ print("# Stall current: %.1f amps" % (current_stall))
+ print("# No load current: %.1f amps" % (current_no_load))
+ print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
- # Constants from motor values
- resistance_motor = voltage_nominal / current_stall
- speed_no_load_rps = speed_no_load_rpm / 60.0 # Revolutions per second no load speed
- speed_no_load = speed_no_load_rps * 2.0 * pi
- Kt = num_motors * torque_stall / current_stall # N-m/A torque constant
- Kv_rpm = speed_no_load_rpm / (voltage_nominal -
- resistance_motor * current_no_load) # rpm/V
- Kv = Kv_rpm * 2.0 * pi / 60.0 # rpm/V
+ # Constants from motor values
+ resistance_motor = voltage_nominal / current_stall
+ speed_no_load_rps = speed_no_load_rpm / 60.0 # Revolutions per second no load speed
+ speed_no_load = speed_no_load_rps * 2.0 * pi
+ Kt = num_motors * torque_stall / current_stall # N-m/A torque constant
+ Kv_rpm = speed_no_load_rpm / (
+ voltage_nominal - resistance_motor * current_no_load) # rpm/V
+ Kv = Kv_rpm * 2.0 * pi / 60.0 # rpm/V
- # Robot Geometry and physics
- # m Length of arm connected to the robot base
- length_proximal_arm = inches_to_meters * 47.34
- # m Length of arm that holds the cube
- length_distal_arm = inches_to_meters * 44.0
- # m Length of intake arm from the pivot point to where the big roller contacts a cube.
- length_intake_arm = inches_to_meters * 9.0
- mass_cube = 6.0 * lbs_to_kg # Weight of the cube in Kgrams
- mass_proximal_arm = 5.5 * lbs_to_kg # Weight of proximal arm
- mass_distal_arm = 3.5 * lbs_to_kg # Weight of distal arm
- mass_distal = mass_cube + mass_distal_arm
- mass_proximal = mass_proximal_arm + mass_distal
- # m Length from arm pivot point to arm CG
- radius_to_proximal_arm_cg = 22.0 * inches_to_meters
- # m Length from arm pivot point to arm CG
- radius_to_distal_arm_cg = 10.0 * inches_to_meters
+ # Robot Geometry and physics
+ # m Length of arm connected to the robot base
+ length_proximal_arm = inches_to_meters * 47.34
+ # m Length of arm that holds the cube
+ length_distal_arm = inches_to_meters * 44.0
+ # m Length of intake arm from the pivot point to where the big roller contacts a cube.
+ length_intake_arm = inches_to_meters * 9.0
+ mass_cube = 6.0 * lbs_to_kg # Weight of the cube in Kgrams
+ mass_proximal_arm = 5.5 * lbs_to_kg # Weight of proximal arm
+ mass_distal_arm = 3.5 * lbs_to_kg # Weight of distal arm
+ mass_distal = mass_cube + mass_distal_arm
+ mass_proximal = mass_proximal_arm + mass_distal
+ # m Length from arm pivot point to arm CG
+ radius_to_proximal_arm_cg = 22.0 * inches_to_meters
+ # m Length from arm pivot point to arm CG
+ radius_to_distal_arm_cg = 10.0 * inches_to_meters
- radius_to_distal_cg = (length_distal_arm * mass_cube +
- radius_to_distal_arm_cg * mass_distal_arm) / \
- mass_distal
- radius_to_proximal_cg = (length_proximal_arm * mass_distal +
- radius_to_proximal_arm_cg * mass_proximal_arm) / \
- mass_proximal
- J_cube = length_distal_arm * length_distal_arm*mass_cube
- # Kg m^2 Moment of inertia of the proximal arm
- J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * \
- mass_distal_arm
- # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
- J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm * \
- length_proximal_arm * mass_distal
- # Kg m^2 Moment of inertia of the distal arm
- J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm
- # Moment of inertia of the arm with the cube on the end
- J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm
- # Intake claw
- J_intake = 0.295 # Kg m^2 Moment of inertia of intake
- J = J_intake
+ radius_to_distal_cg = (length_distal_arm * mass_cube +
+ radius_to_distal_arm_cg * mass_distal_arm) / \
+ mass_distal
+ radius_to_proximal_cg = (length_proximal_arm * mass_distal +
+ radius_to_proximal_arm_cg * mass_proximal_arm) / \
+ mass_proximal
+ J_cube = length_distal_arm * length_distal_arm * mass_cube
+ # Kg m^2 Moment of inertia of the proximal arm
+ J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * \
+ mass_distal_arm
+ # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
+ J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm * \
+ length_proximal_arm * mass_distal
+ # Kg m^2 Moment of inertia of the distal arm
+ J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm
+ # Moment of inertia of the arm with the cube on the end
+ J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm
+ # Intake claw
+ J_intake = 0.295 # Kg m^2 Moment of inertia of intake
+ J = J_intake
- gear_ratio = 140.0 # Guess at the gear ratio
- gear_ratio = 100.0 # Guess at the gear ratio
- gear_ratio = 90.0 # Guess at the gear ratio
+ gear_ratio = 140.0 # Guess at the gear ratio
+ gear_ratio = 100.0 # Guess at the gear ratio
+ gear_ratio = 90.0 # Guess at the gear ratio
- error_margine = 1.0
- voltage = 10.0 # voltage for the motor. Assuming a loaded robot so not using 12 V.
- # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
- # motor_free_speed = Kv * voltage
- motor_free_speed = speed_no_load
+ error_margine = 1.0
+ voltage = 10.0 # voltage for the motor. Assuming a loaded robot so not using 12 V.
+ # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
+ # motor_free_speed = Kv * voltage
+ motor_free_speed = speed_no_load
- print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
- print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
- print("# %.2f kg Cube weight" % (mass_cube))
- print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
- print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
- print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
- print("# %.2f m Length from distal arm pivot point to arm CG" % (
- radius_to_distal_arm_cg))
- print("# %.2f m Length from distal arm pivot point to arm and cube cg" % (
- radius_to_distal_cg))
- print("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
- print("# %.2f m Length from proximal arm pivot point to arm CG" % (radius_to_proximal_arm_cg))
- print("# %.2f m Length from proximal arm pivot point to arm and cube cg" % (
- radius_to_proximal_cg))
- print("# %.2f m Proximal arm length" % (length_proximal_arm))
- print("# %.2f m Distal arm length" % (length_distal_arm))
+ print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" %
+ (Kt, Kv_rpm, Kv))
+ print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
+ print("# %.2f kg Cube weight" % (mass_cube))
+ print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
+ print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
+ print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
+ print("# %.2f m Length from distal arm pivot point to arm CG" %
+ (radius_to_distal_arm_cg))
+ print("# %.2f m Length from distal arm pivot point to arm and cube cg" %
+ (radius_to_distal_cg))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point"
+ % (J_cube))
+ print("# %.2f m Length from proximal arm pivot point to arm CG" %
+ (radius_to_proximal_arm_cg))
+ print("# %.2f m Length from proximal arm pivot point to arm and cube cg" %
+ (radius_to_proximal_cg))
+ print("# %.2f m Proximal arm length" % (length_proximal_arm))
+ print("# %.2f m Distal arm length" % (length_distal_arm))
- print("# %.2f kg-m^2 Moment of inertia of the intake about the intake pivot point" % (
- J_intake))
- print("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (
- J_distal_arm))
- print("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (
- J_proximal_arm))
- print("# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about "
- "the proximal arm pivot point" % (
- J_distal_arm_and_cube_at_end_of_proximal_arm))
- print("# %.2f kg-m^2 Moment of inertia of the intake the intake pivot point "
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the intake about the intake pivot point"
+ % (J_intake))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point"
+ % (J_distal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point"
+ % (J_proximal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about "
+ "the proximal arm pivot point" %
+ (J_distal_arm_and_cube_at_end_of_proximal_arm))
+ print(
+ "# %.2f kg-m^2 Moment of inertia of the intake the intake pivot point "
"(J value used in simulation)" % (J))
- print("# %d Number of motors" % (num_motors))
+ print("# %d Number of motors" % (num_motors))
- print("# %.2f V Motor voltage" % (voltage))
- for gear_ratio in range(60, 241, 10):
- c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
- c2 = gear_ratio * Kt / (J * resistance_motor)
- c3 = radius_to_proximal_cg * mass_proximal * gravity / J
+ print("# %.2f V Motor voltage" % (voltage))
+ for gear_ratio in range(60, 241, 10):
+ c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
+ c2 = gear_ratio * Kt / (J * resistance_motor)
+ c3 = radius_to_proximal_cg * mass_proximal * gravity / J
- if ( False ):
- print("# %.8f 1/sec C1 constant" % (c1))
- print("# %.2f 1/sec C2 constant" % (c2))
- print("# %.2f 1/(V sec^2) C3 constant" % (c3))
- print("# %.2f RPM Free speed at motor voltage" % (voltage * Kv_rpm))
+ if (False):
+ print("# %.8f 1/sec C1 constant" % (c1))
+ print("# %.2f 1/sec C2 constant" % (c2))
+ print("# %.2f 1/(V sec^2) C3 constant" % (c3))
+ print("# %.2f RPM Free speed at motor voltage" %
+ (voltage * Kv_rpm))
- torque_90_degrees = radius_to_distal_cg * mass_distal * gravity
- voltage_90_degrees = resistance_motor * torque_90_degrees / (gear_ratio * Kt)
- torque_peak = gear_ratio * num_motors * torque_stall
- torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
- normal_force = torque_peak / length_intake_arm
- normal_force_lbf = newton_to_lbf * normal_force
- time_required = get_180_degree_time(c1, c2, c3, voltage,
- gear_ratio, motor_free_speed)
- print("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds. "
- "Peak (stall) torque %3.0f nm %3.0f ft-lb Normal force at intake "
- "end %3.0f N %2.0f lbf" % \
- (to_deg(theta_travel), gear_ratio, time_required,
- torque_peak, torque_peak_ft_lbs, normal_force, normal_force_lbf))
+ torque_90_degrees = radius_to_distal_cg * mass_distal * gravity
+ voltage_90_degrees = resistance_motor * torque_90_degrees / (
+ gear_ratio * Kt)
+ torque_peak = gear_ratio * num_motors * torque_stall
+ torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
+ normal_force = torque_peak / length_intake_arm
+ normal_force_lbf = newton_to_lbf * normal_force
+ time_required = get_180_degree_time(c1, c2, c3, voltage, gear_ratio,
+ motor_free_speed)
+ print("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds. "
+ "Peak (stall) torque %3.0f nm %3.0f ft-lb Normal force at intake "
+ "end %3.0f N %2.0f lbf" % \
+ (to_deg(theta_travel), gear_ratio, time_required,
+ torque_peak, torque_peak_ft_lbs, normal_force, normal_force_lbf))
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/y2018/control_loops/python/path_points.py b/y2018/control_loops/python/path_points.py
index eed9804..8802629 100644
--- a/y2018/control_loops/python/path_points.py
+++ b/y2018/control_loops/python/path_points.py
@@ -2,9 +2,422 @@
# Path points from Parker.
# Points in (theta0, theta1) of the boundry of the safe region.
-points_bounary = ([1.8577014383575772, -1.7353804562372057], [1.8322288438826315, -1.761574570216062], [1.7840339881582052, -1.8122752826851365], [1.7367354460218392, -1.863184376466228], [1.69054633425053, -1.9140647421906793], [1.6456590387358871, -1.9646939485526782], [1.6022412524081968, -2.0148691793459164], [1.5604334435784202, -2.0644108218872432], [1.5203477477575325, -2.1131646494909866], [1.4820681545944325, -2.1610026706370666], [1.4456517763321752, -2.207822815007094], [1.4111309389855604, -2.253547685496041], [1.3785158286698027, -2.298122627340264], [1.3477974448369014, -2.3415133576238922], [1.318950649522341, -2.3837033700108243], [1.2919371475531436, -2.4246912899477153], [1.266708279449626, -2.464488312575651], [1.2432075513427807, -2.5031158147261996], [1.2213728618107609, -2.5406031969824228], [1.2011384131259828, -2.5769859833310096], [1.1824363142639513, -2.61230418457078], [1.165197896140849, -2.64660091671642], [1.149354767212547, -2.6799212560849437], [1.134839641107821, -2.712311307404836], [1.121586968578607, -2.7438174590386177], [1.1095334047223224, -2.774485799309971], [1.09861813993779, -2.8043616692157007], [1.088783119985477, -2.8334893289039758], [1.079973177230731, -2.861911717797479], [1.0721360919151535, -2.8896702908486835], [1.065222599283597, -2.916804915950962], [1.0591863556764607, -2.943353819884641], [1.0539838743128325, -2.969353572295195], [1.049574439440948, -2.9948390990606946], [1.0459200058000553, -3.01984371800932], [1.0429850888932353, -3.044399191310831], [1.0407366503803421, -3.0685357900111128], [1.039143981929867, -3.092282367132034], [1.0381785900853944, -3.1156664365461078], [1.0378140840766794, -3.1387142554815886], [1.038026068010855, -3.1614509090414518], [1.0387920384931424, -3.1839003955494407], [1.0400912884293725, -3.2060857118856374], [1.0419048175385754, -3.2280289382581637], [1.0442152499395827, -3.2497513220895704], [1.047006759060362, -3.2712733608874625], [1.050265000044113, -3.2926148841283163], [1.0513266200838918, -3.2986722862692828], [0.6882849734317291, -3.2986722862692828], [0.6882849734317291, -2.40847516476558], [0.8062364039734171, -2.2816832357742984], [0.9846964491122989, -2.0749837238115147], [1.080856081125841, -1.9535526052833936], [1.1403399741223543, -1.8700303125904767], [1.1796460643255697, -1.8073338252603661], [1.206509908068604, -1.7574623871869075], [1.225108933139178, -1.7160975113819317], [1.237917343016644, -1.6806816402603253], [1.2465009152225068, -1.6495957958330445], [1.251901644035212, -1.6217619064422375], [1.2548410275662123, -1.5964327471863136], [1.2558349967266738, -1.5730732293972975], [1.2552624664807475, -1.551289614657788], [1.2534080548485127, -1.5307854126408047], [1.2504896957865235, -1.5113328783804112], [1.2466770509718135, -1.492754008779478], [1.2421041193998992, -1.4749075280685116], [1.236878076935188, -1.457679763958034], [1.231085601347444, -1.4409781183381307], [1.2247974811461413, -1.4247263085140511], [1.2180720288351026, -1.408860841660136], [1.2109576458572935, -1.3933283641188086], [1.2034947755974974, -1.378083641634472], [1.1957174082841977, -1.363088001457586], [1.1876542532510088, -1.3483081171759035], [1.179329661157153, -1.3337150510329991], [1.1707643560768641, -1.3192834919003447], [1.1385726725405734, -1.3512941162901886], [1.1061744838535637, -1.3828092440478137], [1.0736355415504857, -1.4137655512448706], [1.0410203155384732, -1.444102884084807], [1.0083912519665894, -1.4737649313813326], [0.975808099297045, -1.5026998012630925], [0.9433273192311136, -1.5308604887780404], [0.941597772428203, -1.50959779639341], [0.9392183822389457, -1.488861961175901], [0.9362399962318921, -1.4685999567644576], [0.9327074201772598, -1.448764371832554], [0.9286602163651203, -1.4293126388801052], [0.9241333769591611, -1.410206381002334], [0.9191578939466147, -1.3914108561164176], [0.9137612431353617, -1.3728944819981919], [0.9079677963791273, -1.3546284285619337], [0.9017991735984072, -1.3365862662768213], [0.8952745440670551, -1.3187436615861219], [0.888410884742945, -1.3010781117818295], [0.8812232020507231, -1.2835687130679965], [0.8737247224089426, -1.2661959565824437], [0.8659270558807739, -1.2489415479874506], [0.8578403365760008, -1.2317882469234374], [0.8810761292150717, -1.2118184809610988], [0.9041762360363244, -1.1914283310662528], [0.9271196683064211, -1.1706361926784383], [0.949885054142765, -1.1494613527499984], [0.9724507572661958, -1.1279238910069835], [0.9947950004998649, -1.1060445714951375], [1.0168959923240788, -1.0838447258650978], [1.0387320546908576, -1.0613461300367468], [1.0602817502430675, -1.0385708760262022], [1.0815240070740941, -1.0155412408147642], [1.102438239206001, -0.9922795541836177], [1.123004461055328, -0.9688080674301042], [1.1432033942926907, -0.9451488248218312], [1.1630165656794818, -0.9213235395370803], [1.1824263946752058, -0.8973534756890716], [1.2014162698440272, -0.8732593378443982], [1.2199706133398625, -0.8490611692304734], [1.2380749330068292, -0.8247782595916947], [1.2557158618869284, -0.800429063408306], [1.2728811851714203, -0.776031128944234], [1.2895598548592355, -0.7516010383486005], [1.3057419925890068, -0.7271543588072293], [1.3214188812865908, -0.702705604531139], [1.3365829464142562, -0.6782682091832445], [1.351227727719687, -0.6538545081853449], [1.365347842462401, -0.6294757302167044], [1.3789389411433586, -0.6051419971134862], [1.391997656782351, -0.5808623313043475], [1.4045215487801634, -0.5566446698699925], [1.4165090423718034, -0.5324958842910054], [1.4279593646268474, -0.5084218049460658], [1.4388724778869602, -0.4844272494383845], [1.449249011452494, -0.460516053858597], [1.4590901922431447, -0.4366911061340692], [1.468397775065033, -0.4129543806643518], [1.4771739730209745, -0.38930697349737264], [1.485421388504271, -0.36574913735813025], [1.4931429451209484, -0.3422803158986589], [1.5003418207921726, -0.3188991765927855], [1.5070213821985838, -0.2956036417497373], [1.513185120641734, -0.2723909171654731], [1.5188365893149296, -0.24925751796822435], [1.5239793418959948, -0.22619929124396096], [1.5286168722972349, -0.2032114350471563], [1.5327525553319497, -0.1802885134112242], [1.5363895879810432, -0.15742446697009113], [1.5395309308654115, -0.1346126187862511], [1.5421792494481048, -0.11184567494963411], [1.5443368544016174, -0.0891157194637005], [1.5460056404769755, -0.06641420286771664], [1.5471870230984175, -0.043731923953761714], [1.547881871775246, -0.021059003819238205], [1.5480904392645911, 0.0016151486553661097], [1.547812285227133, 0.024301881005710235], [1.5470461928818935, 0.047013352405288866], [1.5457900768723736, 0.06976260156314103], [1.5440408801865422, 0.09256362934244797], [1.5417944575035705, 0.11543149864415554], [1.5390454417383017, 0.13838245474060726], [1.5357870897759938, 0.16143407007284788], [1.5320111023738603, 0.18460541860588778], [1.5277074118667517, 0.20791728625864334], [1.5228639295308124, 0.23139242581719505], [1.5174662420569858, 0.25505586728497265], [1.5114972433117844, 0.27893529808946027], [1.5049366830391921, 0.30306153234932315], [1.4977606078174102, 0.32746909510770755], [1.4899406605544634, 0.35219695697813347], [1.4814431917184283, 0.37728946847450484], [1.4722281161523716, 0.40279756372788583], [1.4622474200998372, 0.4287803341537376], [1.4514431778273647, 0.45530712040457033], [1.4397448652396179, 0.48246034696312606], [1.427065639662639, 0.5103394485717625], [1.4132970536897833, 0.5390664502570618], [1.3983013135749314, 0.5687941401756967], [1.3818995257862978, 0.5997184788509978], [1.3638530549860057, 0.6320982830132342], [1.3438323057823602, 0.6662881915757862], [1.3213606855701236, 0.7027978462604986], [1.2957042956404132, 0.7424084023365868], [1.2656247456808565, 0.786433646353686], [1.2287034601894442, 0.8374338794107618], [1.1786904071656892, 0.902017503822168], [1.0497616572686415, 1.0497616572686426], [0.9097522267255194, 1.1864251300690412], [0.8484431816837388, 1.2397127624624213], [0.8000049977037023, 1.2791960970308718], [0.7581818853313602, 1.3114777786351866], [0.7205493452982701, 1.3391121846078957], [0.6882849734317291, 1.3617105524217008], [0.6882849734317291, 2.356194490192345], [1.3376784164442164, 2.356194490192345], [1.3516056511178856, 2.3360528189227487], [1.3708660530144583, 2.308792458458483], [1.3913233553973305, 2.2804469909122105], [1.4131687110627962, 2.2508089759388485], [1.436656614785, 2.219604401551449], [1.4621380338543308, 2.18645769238238], [1.4901198983671067, 2.1508288797419346], [1.5213822452925796, 2.111889875368928], [1.5572408030205178, 2.0682471032649676], [1.6002650085871786, 2.0171829478543404], [1.657096323745671, 1.9516778011159774], [1.7977393629734166, 1.7977393629734166], [1.8577014383575772, 1.7364646168980775], [1.8577014383575772, -1.7353804562372057])
+points_bounary = (
+ [1.8577014383575772,
+ -1.7353804562372057], [1.8322288438826315, -1.761574570216062],
+ [1.7840339881582052,
+ -1.8122752826851365], [1.7367354460218392, -1.863184376466228
+ ], [1.69054633425053, -1.9140647421906793],
+ [1.6456590387358871, -1.9646939485526782
+ ], [1.6022412524081968, -2.0148691793459164
+ ], [1.5604334435784202, -2.0644108218872432
+ ], [1.5203477477575325, -2.1131646494909866
+ ], [1.4820681545944325, -2.1610026706370666
+ ], [1.4456517763321752, -2.207822815007094
+ ], [1.4111309389855604, -2.253547685496041
+ ], [1.3785158286698027, -2.298122627340264],
+ [1.3477974448369014, -2.3415133576238922
+ ], [1.318950649522341, -2.3837033700108243
+ ], [1.2919371475531436, -2.4246912899477153
+ ], [1.266708279449626, -2.464488312575651
+ ], [1.2432075513427807, -2.5031158147261996
+ ], [1.2213728618107609, -2.5406031969824228
+ ], [1.2011384131259828, -2.5769859833310096
+ ], [1.1824363142639513, -2.61230418457078
+ ], [1.165197896140849, -2.64660091671642],
+ [1.149354767212547, -2.6799212560849437
+ ], [1.134839641107821, -2.712311307404836
+ ], [1.121586968578607, -2.7438174590386177
+ ], [1.1095334047223224, -2.774485799309971
+ ], [1.09861813993779, -2.8043616692157007
+ ], [1.088783119985477, -2.8334893289039758
+ ], [1.079973177230731, -2.861911717797479
+ ], [1.0721360919151535, -2.8896702908486835
+ ], [1.065222599283597, -2.916804915950962],
+ [1.0591863556764607, -2.943353819884641
+ ], [1.0539838743128325, -2.969353572295195
+ ], [1.049574439440948, -2.9948390990606946
+ ], [1.0459200058000553, -3.01984371800932
+ ], [1.0429850888932353, -3.044399191310831
+ ], [1.0407366503803421, -3.0685357900111128
+ ], [1.039143981929867, -3.092282367132034
+ ], [1.0381785900853944, -3.1156664365461078
+ ], [1.0378140840766794, -3.1387142554815886],
+ [1.038026068010855,
+ -3.1614509090414518], [1.0387920384931424, -3.1839003955494407], [
+ 1.0400912884293725,
+ -3.2060857118856374
+ ], [1.0419048175385754, -3.2280289382581637
+ ], [1.0442152499395827, -3.2497513220895704
+ ], [1.047006759060362, -3.2712733608874625
+ ], [1.050265000044113, -3.2926148841283163
+ ], [1.0513266200838918, -3.2986722862692828
+ ], [0.6882849734317291, -3.2986722862692828
+ ], [0.6882849734317291, -2.40847516476558
+ ], [0.8062364039734171, -2.2816832357742984],
+ [0.9846964491122989,
+ -2.0749837238115147], [1.080856081125841, -1.9535526052833936], [
+ 1.1403399741223543,
+ -1.8700303125904767
+ ], [1.1796460643255697,
+ -1.8073338252603661], [1.206509908068604, -1.7574623871869075], [
+ 1.225108933139178,
+ -1.7160975113819317
+ ], [1.237917343016644, -1.6806816402603253
+ ], [1.2465009152225068, -1.6495957958330445
+ ], [1.251901644035212, -1.6217619064422375
+ ], [1.2548410275662123, -1.5964327471863136
+ ], [1.2558349967266738, -1.5730732293972975
+ ], [1.2552624664807475, -1.551289614657788
+ ], [1.2534080548485127, -1.5307854126408047],
+ [1.2504896957865235,
+ -1.5113328783804112], [1.2466770509718135, -1.492754008779478], [
+ 1.2421041193998992,
+ -1.4749075280685116
+ ], [1.236878076935188, -1.457679763958034
+ ], [1.231085601347444, -1.4409781183381307
+ ], [1.2247974811461413, -1.4247263085140511
+ ], [1.2180720288351026, -1.408860841660136
+ ], [1.2109576458572935, -1.3933283641188086
+ ], [1.2034947755974974, -1.378083641634472
+ ], [1.1957174082841977, -1.363088001457586
+ ], [1.1876542532510088, -1.3483081171759035],
+ [1.179329661157153,
+ -1.3337150510329991], [1.1707643560768641, -1.3192834919003447], [
+ 1.1385726725405734,
+ -1.3512941162901886
+ ], [1.1061744838535637,
+ -1.3828092440478137], [1.0736355415504857, -1.4137655512448706], [
+ 1.0410203155384732,
+ -1.444102884084807
+ ], [1.0083912519665894,
+ -1.4737649313813326], [0.975808099297045, -1.5026998012630925], [
+ 0.9433273192311136,
+ -1.5308604887780404
+ ], [0.941597772428203, -1.50959779639341
+ ], [0.9392183822389457, -1.488861961175901
+ ], [0.9362399962318921, -1.4685999567644576
+ ], [0.9327074201772598, -1.448764371832554
+ ], [0.9286602163651203, -1.4293126388801052
+ ], [0.9241333769591611, -1.410206381002334],
+ [0.9191578939466147,
+ -1.3914108561164176], [0.9137612431353617, -1.3728944819981919], [
+ 0.9079677963791273,
+ -1.3546284285619337
+ ], [0.9017991735984072,
+ -1.3365862662768213], [0.8952745440670551, -1.3187436615861219], [
+ 0.888410884742945,
+ -1.3010781117818295
+ ], [0.8812232020507231,
+ -1.2835687130679965], [0.8737247224089426, -1.2661959565824437], [
+ 0.8659270558807739,
+ -1.2489415479874506
+ ], [0.8578403365760008, -1.2317882469234374
+ ], [0.8810761292150717, -1.2118184809610988
+ ], [0.9041762360363244, -1.1914283310662528
+ ], [0.9271196683064211, -1.1706361926784383
+ ], [0.949885054142765, -1.1494613527499984
+ ], [0.9724507572661958, -1.1279238910069835],
+ [0.9947950004998649,
+ -1.1060445714951375], [1.0168959923240788, -1.0838447258650978], [
+ 1.0387320546908576,
+ -1.0613461300367468
+ ], [1.0602817502430675,
+ -1.0385708760262022], [1.0815240070740941, -1.0155412408147642], [
+ 1.102438239206001,
+ -0.9922795541836177
+ ], [1.123004461055328,
+ -0.9688080674301042], [1.1432033942926907, -0.9451488248218312], [
+ 1.1630165656794818,
+ -0.9213235395370803
+ ], [1.1824263946752058, -0.8973534756890716
+ ], [1.2014162698440272, -0.8732593378443982
+ ], [1.2199706133398625, -0.8490611692304734
+ ], [1.2380749330068292, -0.8247782595916947
+ ], [1.2557158618869284, -0.800429063408306
+ ], [1.2728811851714203, -0.776031128944234],
+ [1.2895598548592355,
+ -0.7516010383486005], [1.3057419925890068, -0.7271543588072293], [
+ 1.3214188812865908,
+ -0.702705604531139
+ ], [1.3365829464142562,
+ -0.6782682091832445], [1.351227727719687, -0.6538545081853449], [
+ 1.365347842462401, -0.6294757302167044
+ ], [1.3789389411433586,
+ -0.6051419971134862], [1.391997656782351, -0.5808623313043475], [
+ 1.4045215487801634,
+ -0.5566446698699925
+ ], [1.4165090423718034, -0.5324958842910054
+ ], [1.4279593646268474, -0.5084218049460658
+ ], [1.4388724778869602, -0.4844272494383845], [
+ 1.449249011452494, -0.460516053858597
+ ], [1.4590901922431447, -0.4366911061340692
+ ], [1.468397775065033, -0.4129543806643518
+ ], [1.4771739730209745, -0.38930697349737264
+ ], [1.485421388504271, -0.36574913735813025],
+ [1.4931429451209484,
+ -0.3422803158986589], [1.5003418207921726, -0.3188991765927855], [
+ 1.5070213821985838,
+ -0.2956036417497373
+ ], [1.513185120641734,
+ -0.2723909171654731], [1.5188365893149296, -0.24925751796822435], [
+ 1.5239793418959948,
+ -0.22619929124396096
+ ], [1.5286168722972349,
+ -0.2032114350471563], [1.5327525553319497, -0.1802885134112242], [
+ 1.5363895879810432,
+ -0.15742446697009113
+ ], [1.5395309308654115, -0.1346126187862511
+ ], [1.5421792494481048, -0.11184567494963411
+ ], [1.5443368544016174, -0.0891157194637005
+ ], [1.5460056404769755, -0.06641420286771664
+ ], [1.5471870230984175, -0.043731923953761714],
+ [1.547881871775246,
+ -0.021059003819238205], [1.5480904392645911, 0.0016151486553661097], [
+ 1.547812285227133, 0.024301881005710235
+ ], [1.5470461928818935, 0.047013352405288866
+ ], [1.5457900768723736,
+ 0.06976260156314103], [1.5440408801865422, 0.09256362934244797], [
+ 1.5417944575035705,
+ 0.11543149864415554
+ ], [1.5390454417383017, 0.13838245474060726
+ ], [1.5357870897759938, 0.16143407007284788
+ ], [1.5320111023738603, 0.18460541860588778
+ ], [1.5277074118667517, 0.20791728625864334
+ ], [1.5228639295308124, 0.23139242581719505
+ ], [1.5174662420569858, 0.25505586728497265],
+ [1.5114972433117844,
+ 0.27893529808946027], [1.5049366830391921, 0.30306153234932315], [
+ 1.4977606078174102,
+ 0.32746909510770755
+ ], [1.4899406605544634,
+ 0.35219695697813347], [1.4814431917184283, 0.37728946847450484], [
+ 1.4722281161523716,
+ 0.40279756372788583
+ ], [1.4622474200998372,
+ 0.4287803341537376], [1.4514431778273647, 0.45530712040457033], [
+ 1.4397448652396179,
+ 0.48246034696312606
+ ], [1.427065639662639, 0.5103394485717625
+ ], [1.4132970536897833, 0.5390664502570618
+ ], [1.3983013135749314, 0.5687941401756967
+ ], [1.3818995257862978, 0.5997184788509978
+ ], [1.3638530549860057, 0.6320982830132342
+ ], [1.3438323057823602, 0.6662881915757862],
+ [1.3213606855701236,
+ 0.7027978462604986], [1.2957042956404132, 0.7424084023365868], [
+ 1.2656247456808565,
+ 0.786433646353686
+ ], [1.2287034601894442,
+ 0.8374338794107618], [1.1786904071656892, 0.902017503822168], [
+ 1.0497616572686415,
+ 1.0497616572686426
+ ], [0.9097522267255194,
+ 1.1864251300690412], [0.8484431816837388, 1.2397127624624213], [
+ 0.8000049977037023,
+ 1.2791960970308718
+ ], [0.7581818853313602, 1.3114777786351866
+ ], [0.7205493452982701, 1.3391121846078957
+ ], [0.6882849734317291, 1.3617105524217008
+ ], [0.6882849734317291, 2.356194490192345
+ ], [1.3376784164442164, 2.356194490192345
+ ], [1.3516056511178856, 2.3360528189227487],
+ [1.3708660530144583,
+ 2.308792458458483], [1.3913233553973305, 2.2804469909122105], [
+ 1.4131687110627962,
+ 2.2508089759388485
+ ], [1.436656614785,
+ 2.219604401551449], [1.4621380338543308, 2.18645769238238], [
+ 1.4901198983671067,
+ 2.1508288797419346
+ ], [1.5213822452925796, 2.111889875368928
+ ], [1.5572408030205178, 2.0682471032649676
+ ], [1.6002650085871786, 2.0171829478543404
+ ], [1.657096323745671, 1.9516778011159774
+ ], [1.7977393629734166, 1.7977393629734166
+ ], [1.8577014383575772, 1.7364646168980775
+ ], [1.8577014383575772, -1.7353804562372057])
# Test data set in terms of (theta0, theta1) of a motion from Parker.
# This is an example box lift profile.
-path_with_accelerations = [(1.3583511559969876, 0.99753029519739866, 0.63708920330895369, -0.77079007974101643, -0.21483375398380378, -0.17756921128311187), (1.4037744780290744, 0.94141413786797179, 0.62102298265172207, -0.78379235452915641, -0.23084099683729808, -0.18290283743090688), (1.4401396868545593, 0.89467165253531666, 0.606891738771278, -0.79478451004733031, -0.24685749210031543, -0.18849894149927623), (1.4710837705397868, 0.85345617877170399, 0.59376006111610313, -0.80464215016577489, -0.26311393698286972, -0.19415687958799988), (1.4982967960378542, 0.81597755723156562, 0.58119389263733834, -0.81376511301545618, -0.27984413029527577, -0.19986598102433009), (1.5227269571172215, 0.78122893136122973, 0.56894801707769227, -0.82237348805962973, -0.29704891442706877, -0.20550961547441812), (1.5449685847030388, 0.74857685672553398, 0.55686625320351735, -0.83060217676278458, -0.31482379304256025, -0.21106978462024978), (1.5654226738527495, 0.71759174238125301, 0.54484048864732038, -0.83853970802255351, -0.33321287621660045, -0.21650513557965106), (1.5843743655705165, 0.68796601612512265, 0.53279106733541426, -0.84624681894089859, -0.35226023809265267, -0.22178093106252236), (1.6020345343012865, 0.65947020123078393, 0.52065635488289785, -0.85376633812774205, -0.37188463022164508, -0.22678852570730737), (1.6185639244576269, 0.63192742228662813, 0.50838674358661662, -0.86112886314731996, -0.39227952043050979, -0.23159139751917215), (1.6340880277208036, 0.60519768777296334, 0.49594100637973321, -0.868356216187261, -0.41330592927121557, -0.23605025316897368), (1.6487067026409843, 0.57916772325998889, 0.48328397846038601, -0.87546364639743945, -0.43479093110952094, -0.24001893904573279), (1.6625006419641934, 0.55374413183503834, 0.47038503527185527, -0.88246128447218319, -0.45715591769925784, -0.2436817242766752), (1.6755358637715485, 0.52884863988931285, 0.45721707379330323, -0.88935513009814537, -0.48005035022840359, -0.24679412703629519), (1.6878669166029825, 0.50441469954774187, 0.44375581873319114, -0.89614774079971626, -0.50380737957408728, -0.24947659419788909), (1.6995392207123023, 0.48038500205371104, 0.42997935653089725, -0.90283871923908732, -0.52783372117172589, -0.25138272577381626), (1.7105908129416818, 0.45670961972392227, 0.41586782149756141, -0.90942506840468851, -0.55267961927927023, -0.25273346631273114), (1.7210536699519405, 0.43334459201815007, 0.40140320353386438, -0.91590145113584731, -0.57770009966870806, -0.25318354738409404), (1.7309547270317198, 0.41025083198931545, 0.38656923494570777, -0.92226038979969771, -0.60317645612010684, -0.25282447346857195), (1.7403166729890138, 0.38739326814551689, 0.37135135004406289, -0.92849242044318914, -0.62907036438628838, -0.25159784841893079), (1.7491585775728877, 0.36474016215239319, 0.35573669259875446, -0.93458622156486959, -0.6551663269414254, -0.24938020059543903), (1.7574963917487576, 0.34226255982896742, 0.33971416817223166, -0.94052872574050006, -0.68164592193873064, -0.24620769226541636), (1.7653433501187576, 0.31993384454025714, 0.32327453032164366, -0.94630522456833177, -0.70757451300020135, -0.24172061458903144), (1.7727102970907476, 0.29772937021195395, 0.30641049133414705, -0.95189947515500117, -0.73378453575677771, -0.23620138680600353), (1.7796059529520747, 0.27562615695138953, 0.28911685939500692, -0.95729381154041093, -0.75990897245769828, -0.22950448838605128), (1.7860371320859008, 0.25360263640618674, 0.27139068895919466, -0.9624692690918778, -0.78523056852177775, -0.22141489322699751), (1.7920089227108962, 0.23163843702152076, 0.25323144050682866, -0.96740572540110403, -0.81025448009679157, -0.21209581939563796), (1.7975248354162883, 0.20971420159976159, 0.2346411508301583, -0.97208205946674009, -0.83408589705170777, -0.20133249883457041), (1.8025869261905516, 0.187811431247703, 0.21562459548465329, -0.97647633551565383, -0.85752536967991499, -0.18935884543741083), (1.8071958984562269, 0.16591235107247332, 0.19618945367704119, -0.98056600913243175, -0.8793458743889202, -0.17593847769244828), (1.8113511877225346, 0.14399979396679621, 0.17634645105214913, -0.98432816133711831, -0.89983233183814704, -0.16120962647016274), (1.8150510317784807, 0.12205709958524935, 0.15610948821796866, -0.98773975706575867, -0.91852354093170219, -0.14517104001739675), (1.8182925288197413, 0.10006802621144681, 0.13549574094137548, -0.99077792879471627, -0.93602333307674279, -0.12800867856143744), (1.821071685494891, 0.078016673692446373, 0.11452572935697461, -0.99342028231522084, -0.95112385045254688, -0.10965064890920556), (1.8233834565427998, 0.055887416001137141, 0.093223341590826445, -0.99564522224667962, -0.96368362602021329, -0.090231797498176183), (1.8252217774528923, 0.03366484230234558, 0.071615815013953282, -0.99743229095507402, -0.97395276760288163, -0.069931055757802438), (1.8265795913984428, 0.011333705660752967, 0.049733665943173709, -0.99876251555204698, -0.98177893706111263, -0.048889055531294821), (1.8274488715589303, -0.011121121248675203, 0.02761056315188902, -0.99961875572762016, -0.98689335559342106, -0.027260179511112551), (1.8278206398523622, -0.033714683874300592, 0.0052831412013899706, -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384), (1.8276849830361703, -0.056461977241467148, -0.017209244112191453, -0.99985190999321838, -0.98812791978941406, 0.017006330622021722), (1.8270310671011307, -0.079377971247817633, -0.039824819694542858, -0.99920667718760625, -0.98450390283226485, 0.039237693528591036), (1.8258471508733223, -0.10247762977900209, -0.062519753866286318, -0.99804372668560926, -0.97719363903338485, 0.061212532653782022), (1.8241205997516765, -0.12577592348843702, -0.085248569812080663, -0.99635971483445407, -0.9676884196650537, 0.08279433361404552), (1.8218379005412455, -0.14928783595476772, -0.10796459505798382, -0.99415473957224865, -0.95496997071328249, 0.103708042997466), (1.8189846783932371, -0.17302836280361969, -0.13062045461685196, -0.99143244693508337, -0.93935720014890423, 0.12375848445745616), (1.8155457169306066, -0.19701250325490266, -0.15316858191907523, -0.98820007362522466, -0.92129127811992539, 0.14279680293047756), (1.8115049827211411, -0.22125524343262917, -0.17556174489393769, -0.98446842190585071, -0.90059657633599033, 0.16060369204333666), (1.8068456553568204, -0.24577153065112339, -0.19775357131740673, -0.98025176614541798, -0.87754964758424281, 0.17703366424976713), (1.8015501645066898, -0.27057623777063011, -0.21969906367786421, -0.97556769186923664, -0.85256620853260556, 0.19199796793136439), (1.795600235427889, -0.29568411659857541, -0.24135508622397667, -0.97043687190554384, -0.82563764988942245, 0.20534143268497296), (1.7889769445422168, -0.32110973920321939, -0.26268082100774776, -0.96488278369690872, -0.79733324597202848, 0.2170659935022537), (1.7816607868090406, -0.34686742590848585, -0.2836381738799823, -0.95893137727265387, -0.76782873106500538, 0.22711183686998365), (1.7736317567432636, -0.37297115865829411, -0.30419213280652752, -0.95261070030659223, -0.73738784843863214, 0.23546528518662449), (1.7648694450316516, -0.39943447838330554, -0.32431106313855179, -0.94595049253433039, -0.70610753433104578, 0.2420821140887684), (1.7553531527820141, -0.42627036498226889, -0.34396694344403017, -0.93898175797923322, -0.67467492193224388, 0.24714550708779132), (1.7450620254853675, -0.45349109855564618, -0.36313553561943973, -0.93173632684916963, -0.64280482438985487, 0.25052642309609846), (1.7339752087663065, -0.48110810061485765, -0.38179649614837224, -0.92424641493966653, -0.61125522193929116, 0.2525024014306243), (1.7220720279237476, -0.50913175415228962, -0.39993342129901005, -0.91654419343972093, -0.58016017720123902, 0.25315181203742848), (1.7093321931028456, -0.53757120171300954, -0.41753384214477446, -0.90866137293483673, -0.54964898595174738, 0.25256545404213349), (1.6957360316664474, -0.56643412097840984, -0.43458916639025197, -0.9006288116955985, -0.51972307699036913, 0.25078642241782151), (1.6812647489267576, -0.59572647787451594, -0.45109457862962754, -0.89247615157546845, -0.49088885529812037, 0.24811508685641395), (1.6659007178300929, -0.62545225787260295, -0.46704890024189227, -0.88423148823305242, -0.46296888429931843, 0.24453849915066983), (1.6496277974361373, -0.65561317697342414, -0.48245442463761296, -0.87592107415428122, -0.43630053035835431, 0.24031239163872659), (1.6324316790779632, -0.68620837486997288, -0.49731672574028513, -0.86756906024763358, -0.41095471910587472, 0.23557116495526012), (1.6143002579172752, -0.7172340939700278, -0.51164445121259294, -0.85919727393850864, -0.38675403181117768, 0.23030820687574571), (1.5952240262184549, -0.748683349319481, -0.52544910775441078, -0.85082503204836046, -0.36408233991880601, 0.22484810685703166), (1.5751964830679697, -0.780545595975073, -0.53874483689504815, -0.84246899095392713, -0.34276938388236106, 0.21919491474821423), (1.5542145534957212, -0.81280640198469889, -0.55154819264623711, -0.83414302801658013, -0.32291038254364612, 0.21351295685970414), (1.5322790080695103, -0.84544713677503402, -0.56387791937564813, -0.82585815491559456, -0.30444498192901781, 0.20786805829508584), (1.509394872119298, -0.87844468632349793, -0.57575473429345714, -0.81762245929198296, -0.28737482236853346, 0.20236376870812761), (1.4855718119204786, -0.91177120788178356, -0.58720111439137757, -0.8094410733694728, -0.27173204177162957, 0.1971250636942595), (1.4608244835702884, -0.94539393807497152, -0.59824108887263983, -0.80131616705547515, -0.25741396251510884, 0.19217806498346204), (1.4351728290976418, -0.97927506876108006, -0.60890003734400222, -0.79324696313473042, -0.24447483076597848, 0.18765975242966434), (1.4086423037364302, -1.0133717049339268, -0.61920449072176709, -0.78522977444184905, -0.2328103488745005, 0.18358578028961331), (1.3812640184459468, -1.047635918033063, -0.62918193604047223, -0.77725805969469564, -0.22240924347971955, 0.18003762666126619), (1.3530747828369902, -1.0820149061676485, -0.63886062143319422, -0.76932249829443622, -0.21321450222713362, 0.17705731568712871), (1.324117035769417, -1.1164512699023377, -0.64826936036845306, -0.76141108240389876, -0.20523063810290823, 0.17473421668463424), (1.294438654066159, -1.1508834084076087, -0.6574373330314689, -0.75350922564788103, -0.19832793065660315, 0.17304104866571926), (1.2640926339871807, -1.1852460360561055, -0.66639388399919663, -0.74559988691553936, -0.1925723524639939, 0.17211494490833781), (1.2331366451651338, -1.2194708141674491, -0.67516831336894412, -0.73766370970960415, -0.18786875722059337, 0.17195217715824379), (1.2016324623545263, -1.2534870868845998, -0.68378966242702388, -0.72967917440333774, -0.18419866624510314, 0.17261421082264014), (1.1696452862242701, -1.2872227045046678, -0.6922864921531301, -0.72162276348679166, -0.18150423850780073, 0.17412530594411615), (1.1372429700984137, -1.3206049124326309, -0.70068665547523046, -0.71346913797229905, -0.17977253485932801, 0.17655152952198189), (1.104495174566031, -1.3535612797241663, -0.70901706362016093, -0.70519132403585671, -0.17898758721428376, 0.17995840096834861), (1.0714724758096574, -1.3860206383273435, -0.71730344800943324, -0.69676090839955884, -0.17907637813730631, 0.18435585053172451), (1.0382454559924987, -1.4179140029119093, -0.72557011960647111, -0.68814824095848082, -0.18003314612431659, 0.18982321746977993), (1.0048838048727911, -1.4491754417344611, -0.73383972725116353, -0.67932264404179699, -0.18184420373071258, 0.19643734513631667), (0.97145546090878643, -1.4797428713062153, -0.74213301743428883, -0.67025262732336799, -0.18446052990619061, 0.20424250843873848)]
+path_with_accelerations = [
+ (1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
+ -0.77079007974101643, -0.21483375398380378, -0.17756921128311187),
+ (1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
+ -0.78379235452915641, -0.23084099683729808, -0.18290283743090688),
+ (1.4401396868545593, 0.89467165253531666, 0.606891738771278,
+ -0.79478451004733031, -0.24685749210031543, -0.18849894149927623),
+ (1.4710837705397868, 0.85345617877170399, 0.59376006111610313,
+ -0.80464215016577489, -0.26311393698286972, -0.19415687958799988),
+ (1.4982967960378542, 0.81597755723156562, 0.58119389263733834,
+ -0.81376511301545618, -0.27984413029527577, -0.19986598102433009),
+ (1.5227269571172215, 0.78122893136122973, 0.56894801707769227,
+ -0.82237348805962973, -0.29704891442706877, -0.20550961547441812),
+ (1.5449685847030388, 0.74857685672553398, 0.55686625320351735,
+ -0.83060217676278458, -0.31482379304256025, -0.21106978462024978),
+ (1.5654226738527495, 0.71759174238125301, 0.54484048864732038,
+ -0.83853970802255351, -0.33321287621660045, -0.21650513557965106),
+ (1.5843743655705165, 0.68796601612512265, 0.53279106733541426,
+ -0.84624681894089859, -0.35226023809265267, -0.22178093106252236),
+ (1.6020345343012865, 0.65947020123078393, 0.52065635488289785,
+ -0.85376633812774205, -0.37188463022164508, -0.22678852570730737),
+ (1.6185639244576269, 0.63192742228662813, 0.50838674358661662,
+ -0.86112886314731996, -0.39227952043050979, -0.23159139751917215),
+ (1.6340880277208036, 0.60519768777296334, 0.49594100637973321,
+ -0.868356216187261, -0.41330592927121557, -0.23605025316897368),
+ (1.6487067026409843, 0.57916772325998889, 0.48328397846038601,
+ -0.87546364639743945, -0.43479093110952094, -0.24001893904573279),
+ (1.6625006419641934, 0.55374413183503834, 0.47038503527185527,
+ -0.88246128447218319, -0.45715591769925784, -0.2436817242766752),
+ (1.6755358637715485, 0.52884863988931285, 0.45721707379330323,
+ -0.88935513009814537, -0.48005035022840359, -0.24679412703629519),
+ (1.6878669166029825, 0.50441469954774187, 0.44375581873319114,
+ -0.89614774079971626, -0.50380737957408728, -0.24947659419788909),
+ (1.6995392207123023, 0.48038500205371104, 0.42997935653089725,
+ -0.90283871923908732, -0.52783372117172589, -0.25138272577381626),
+ (1.7105908129416818, 0.45670961972392227, 0.41586782149756141,
+ -0.90942506840468851, -0.55267961927927023, -0.25273346631273114),
+ (1.7210536699519405, 0.43334459201815007, 0.40140320353386438,
+ -0.91590145113584731, -0.57770009966870806, -0.25318354738409404),
+ (1.7309547270317198, 0.41025083198931545, 0.38656923494570777,
+ -0.92226038979969771, -0.60317645612010684, -0.25282447346857195),
+ (1.7403166729890138, 0.38739326814551689, 0.37135135004406289,
+ -0.92849242044318914, -0.62907036438628838, -0.25159784841893079),
+ (1.7491585775728877, 0.36474016215239319, 0.35573669259875446,
+ -0.93458622156486959, -0.6551663269414254, -0.24938020059543903),
+ (1.7574963917487576, 0.34226255982896742, 0.33971416817223166,
+ -0.94052872574050006, -0.68164592193873064, -0.24620769226541636),
+ (1.7653433501187576, 0.31993384454025714, 0.32327453032164366,
+ -0.94630522456833177, -0.70757451300020135, -0.24172061458903144),
+ (1.7727102970907476, 0.29772937021195395, 0.30641049133414705,
+ -0.95189947515500117, -0.73378453575677771, -0.23620138680600353),
+ (1.7796059529520747, 0.27562615695138953, 0.28911685939500692,
+ -0.95729381154041093, -0.75990897245769828, -0.22950448838605128),
+ (1.7860371320859008, 0.25360263640618674, 0.27139068895919466,
+ -0.9624692690918778, -0.78523056852177775, -0.22141489322699751),
+ (1.7920089227108962, 0.23163843702152076, 0.25323144050682866,
+ -0.96740572540110403, -0.81025448009679157, -0.21209581939563796),
+ (1.7975248354162883, 0.20971420159976159, 0.2346411508301583,
+ -0.97208205946674009, -0.83408589705170777, -0.20133249883457041),
+ (1.8025869261905516, 0.187811431247703, 0.21562459548465329,
+ -0.97647633551565383, -0.85752536967991499, -0.18935884543741083),
+ (1.8071958984562269, 0.16591235107247332, 0.19618945367704119,
+ -0.98056600913243175, -0.8793458743889202, -0.17593847769244828),
+ (1.8113511877225346, 0.14399979396679621, 0.17634645105214913,
+ -0.98432816133711831, -0.89983233183814704, -0.16120962647016274),
+ (1.8150510317784807, 0.12205709958524935, 0.15610948821796866,
+ -0.98773975706575867, -0.91852354093170219, -0.14517104001739675),
+ (1.8182925288197413, 0.10006802621144681, 0.13549574094137548,
+ -0.99077792879471627, -0.93602333307674279, -0.12800867856143744),
+ (1.821071685494891, 0.078016673692446373, 0.11452572935697461,
+ -0.99342028231522084, -0.95112385045254688, -0.10965064890920556),
+ (1.8233834565427998, 0.055887416001137141, 0.093223341590826445,
+ -0.99564522224667962, -0.96368362602021329, -0.090231797498176183),
+ (1.8252217774528923, 0.03366484230234558, 0.071615815013953282,
+ -0.99743229095507402, -0.97395276760288163, -0.069931055757802438),
+ (1.8265795913984428, 0.011333705660752967, 0.049733665943173709,
+ -0.99876251555204698, -0.98177893706111263, -0.048889055531294821),
+ (1.8274488715589303, -0.011121121248675203, 0.02761056315188902,
+ -0.99961875572762016, -0.98689335559342106, -0.027260179511112551),
+ (1.8278206398523622, -0.033714683874300592, 0.0052831412013899706,
+ -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384),
+ (1.8276849830361703, -0.056461977241467148, -0.017209244112191453,
+ -0.99985190999321838, -0.98812791978941406, 0.017006330622021722),
+ (1.8270310671011307, -0.079377971247817633, -0.039824819694542858,
+ -0.99920667718760625, -0.98450390283226485, 0.039237693528591036),
+ (1.8258471508733223, -0.10247762977900209, -0.062519753866286318,
+ -0.99804372668560926, -0.97719363903338485, 0.061212532653782022),
+ (1.8241205997516765, -0.12577592348843702, -0.085248569812080663,
+ -0.99635971483445407, -0.9676884196650537, 0.08279433361404552),
+ (1.8218379005412455, -0.14928783595476772, -0.10796459505798382,
+ -0.99415473957224865, -0.95496997071328249, 0.103708042997466),
+ (1.8189846783932371, -0.17302836280361969, -0.13062045461685196,
+ -0.99143244693508337, -0.93935720014890423, 0.12375848445745616),
+ (1.8155457169306066, -0.19701250325490266, -0.15316858191907523,
+ -0.98820007362522466, -0.92129127811992539, 0.14279680293047756),
+ (1.8115049827211411, -0.22125524343262917, -0.17556174489393769,
+ -0.98446842190585071, -0.90059657633599033, 0.16060369204333666),
+ (1.8068456553568204, -0.24577153065112339, -0.19775357131740673,
+ -0.98025176614541798, -0.87754964758424281, 0.17703366424976713),
+ (1.8015501645066898, -0.27057623777063011, -0.21969906367786421,
+ -0.97556769186923664, -0.85256620853260556, 0.19199796793136439),
+ (1.795600235427889, -0.29568411659857541, -0.24135508622397667,
+ -0.97043687190554384, -0.82563764988942245, 0.20534143268497296),
+ (1.7889769445422168, -0.32110973920321939, -0.26268082100774776,
+ -0.96488278369690872, -0.79733324597202848, 0.2170659935022537),
+ (1.7816607868090406, -0.34686742590848585, -0.2836381738799823,
+ -0.95893137727265387, -0.76782873106500538, 0.22711183686998365),
+ (1.7736317567432636, -0.37297115865829411, -0.30419213280652752,
+ -0.95261070030659223, -0.73738784843863214, 0.23546528518662449),
+ (1.7648694450316516, -0.39943447838330554, -0.32431106313855179,
+ -0.94595049253433039, -0.70610753433104578, 0.2420821140887684),
+ (1.7553531527820141, -0.42627036498226889, -0.34396694344403017,
+ -0.93898175797923322, -0.67467492193224388, 0.24714550708779132),
+ (1.7450620254853675, -0.45349109855564618, -0.36313553561943973,
+ -0.93173632684916963, -0.64280482438985487, 0.25052642309609846),
+ (1.7339752087663065, -0.48110810061485765, -0.38179649614837224,
+ -0.92424641493966653, -0.61125522193929116, 0.2525024014306243),
+ (1.7220720279237476, -0.50913175415228962, -0.39993342129901005,
+ -0.91654419343972093, -0.58016017720123902, 0.25315181203742848),
+ (1.7093321931028456, -0.53757120171300954, -0.41753384214477446,
+ -0.90866137293483673, -0.54964898595174738, 0.25256545404213349),
+ (1.6957360316664474, -0.56643412097840984, -0.43458916639025197,
+ -0.9006288116955985, -0.51972307699036913, 0.25078642241782151),
+ (1.6812647489267576, -0.59572647787451594, -0.45109457862962754,
+ -0.89247615157546845, -0.49088885529812037, 0.24811508685641395),
+ (1.6659007178300929, -0.62545225787260295, -0.46704890024189227,
+ -0.88423148823305242, -0.46296888429931843, 0.24453849915066983),
+ (1.6496277974361373, -0.65561317697342414, -0.48245442463761296,
+ -0.87592107415428122, -0.43630053035835431, 0.24031239163872659),
+ (1.6324316790779632, -0.68620837486997288, -0.49731672574028513,
+ -0.86756906024763358, -0.41095471910587472, 0.23557116495526012),
+ (1.6143002579172752, -0.7172340939700278, -0.51164445121259294,
+ -0.85919727393850864, -0.38675403181117768, 0.23030820687574571),
+ (1.5952240262184549, -0.748683349319481, -0.52544910775441078,
+ -0.85082503204836046, -0.36408233991880601, 0.22484810685703166),
+ (1.5751964830679697, -0.780545595975073, -0.53874483689504815,
+ -0.84246899095392713, -0.34276938388236106, 0.21919491474821423),
+ (1.5542145534957212, -0.81280640198469889, -0.55154819264623711,
+ -0.83414302801658013, -0.32291038254364612, 0.21351295685970414),
+ (1.5322790080695103, -0.84544713677503402, -0.56387791937564813,
+ -0.82585815491559456, -0.30444498192901781, 0.20786805829508584),
+ (1.509394872119298, -0.87844468632349793, -0.57575473429345714,
+ -0.81762245929198296, -0.28737482236853346, 0.20236376870812761),
+ (1.4855718119204786, -0.91177120788178356, -0.58720111439137757,
+ -0.8094410733694728, -0.27173204177162957, 0.1971250636942595),
+ (1.4608244835702884, -0.94539393807497152, -0.59824108887263983,
+ -0.80131616705547515, -0.25741396251510884, 0.19217806498346204),
+ (1.4351728290976418, -0.97927506876108006, -0.60890003734400222,
+ -0.79324696313473042, -0.24447483076597848, 0.18765975242966434),
+ (1.4086423037364302, -1.0133717049339268, -0.61920449072176709,
+ -0.78522977444184905, -0.2328103488745005, 0.18358578028961331),
+ (1.3812640184459468, -1.047635918033063, -0.62918193604047223,
+ -0.77725805969469564, -0.22240924347971955, 0.18003762666126619),
+ (1.3530747828369902, -1.0820149061676485, -0.63886062143319422,
+ -0.76932249829443622, -0.21321450222713362, 0.17705731568712871),
+ (1.324117035769417, -1.1164512699023377, -0.64826936036845306,
+ -0.76141108240389876, -0.20523063810290823, 0.17473421668463424),
+ (1.294438654066159, -1.1508834084076087, -0.6574373330314689,
+ -0.75350922564788103, -0.19832793065660315, 0.17304104866571926),
+ (1.2640926339871807, -1.1852460360561055, -0.66639388399919663,
+ -0.74559988691553936, -0.1925723524639939, 0.17211494490833781),
+ (1.2331366451651338, -1.2194708141674491, -0.67516831336894412,
+ -0.73766370970960415, -0.18786875722059337, 0.17195217715824379),
+ (1.2016324623545263, -1.2534870868845998, -0.68378966242702388,
+ -0.72967917440333774, -0.18419866624510314, 0.17261421082264014),
+ (1.1696452862242701, -1.2872227045046678, -0.6922864921531301,
+ -0.72162276348679166, -0.18150423850780073, 0.17412530594411615),
+ (1.1372429700984137, -1.3206049124326309, -0.70068665547523046,
+ -0.71346913797229905, -0.17977253485932801, 0.17655152952198189),
+ (1.104495174566031, -1.3535612797241663, -0.70901706362016093,
+ -0.70519132403585671, -0.17898758721428376, 0.17995840096834861),
+ (1.0714724758096574, -1.3860206383273435, -0.71730344800943324,
+ -0.69676090839955884, -0.17907637813730631, 0.18435585053172451),
+ (1.0382454559924987, -1.4179140029119093, -0.72557011960647111,
+ -0.68814824095848082, -0.18003314612431659, 0.18982321746977993),
+ (1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
+ -0.67932264404179699, -0.18184420373071258, 0.19643734513631667),
+ (0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
+ -0.67025262732336799, -0.18446052990619061, 0.20424250843873848)
+]
diff --git a/y2018/control_loops/python/polydrivetrain.py b/y2018/control_loops/python/polydrivetrain.py
index c92e432..f406aa2 100644
--- a/y2018/control_loops/python/polydrivetrain.py
+++ b/y2018/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2018',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2018', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2018/control_loops/python/polydrivetrain_test.py b/y2018/control_loops/python/polydrivetrain_test.py
index ccc3b77..3a1987a 100644
--- a/y2018/control_loops/python/polydrivetrain_test.py
+++ b/y2018/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2018/control_loops/python/spline_generate.py b/y2018/control_loops/python/spline_generate.py
index cea3d7e..0dc5804 100644
--- a/y2018/control_loops/python/spline_generate.py
+++ b/y2018/control_loops/python/spline_generate.py
@@ -7,7 +7,7 @@
# see yXXXX/control_loops/python/drivetrain.py for the current values
kDrivetrain = drivetrain.DrivetrainParams(
- J = 6.0,
+ J=6.0,
mass=68.0,
robot_radius=0.616 / 2.0,
wheel_radius=0.127 / 2.0 * 120.0 / 118.0,
@@ -26,12 +26,15 @@
drivetrain = drivetrain.Drivetrain(kDrivetrain)
# set up coefficients for Hemrite basis function evaluation
-coeffs = np.array([[1, 0, 0, -10, 15, -6], [0, 1, 0, -6, 8, -3], [0, 0, 0.5, -1.5, 1.5, -0.5], [0, 0, 0, 0.5, -1, 0.5], [0, 0, 0, -4, 7, -3], [0, 0, 0, 10, -15, 6]])
+coeffs = np.array([[1, 0, 0, -10, 15, -6], [0, 1, 0, -6, 8, -3],
+ [0, 0, 0.5, -1.5, 1.5, -0.5], [0, 0, 0, 0.5, -1, 0.5],
+ [0, 0, 0, -4, 7, -3], [0, 0, 0, 10, -15, 6]])
coeffs_prime = np.empty_like(coeffs)
for ii in range(0, len(coeffs)):
for jj in range(0, len(coeffs[ii]) - 1):
coeffs_prime[ii][jj] = (jj + 1) * coeffs[ii][jj]
+
def RungeKutta(f, x, dt):
"""4th order RungeKutta integration of F starting at X."""
a = f(x)
@@ -41,24 +44,31 @@
return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
def normalize(v):
norm = np.linalg.norm(v)
return v / norm
+
def theta(v):
return np.arctan2(v[1], v[0])
+
# evaluate Nth hermite basis function at t
def nth_H(N, t):
- return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4 + coeffs[N][5]*t**5
+ return coeffs[N][0] + coeffs[N][1] * t + coeffs[N][2] * t**2 + coeffs[N][
+ 3] * t**3 + coeffs[N][4] * t**4 + coeffs[N][5] * t**5
+
def nth_H_prime(N, t):
- return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4
+ return coeffs[N][0] + coeffs[N][1] * t + coeffs[N][2] * t**2 + coeffs[N][
+ 3] * t**3 + coeffs[N][4] * t**4
+
# class defining a quintic Hermite spline, with utilities for modification and plotting
class Hermite_Spline:
# init method given known parameters, ie savefile loading(if necessary)
- def __init__(self, start, control1, control2, end, resolution = 200):
+ def __init__(self, start, control1, control2, end, resolution=200):
self.start = start
self.end = end
self.control1 = control1
@@ -86,16 +96,18 @@
# take paramters and compute coeffcicents for Hermite basis functions, to be called every time he change control points
def compute_coefficients(self):
self.coeffs = np.append(self.coeffs, np.array(self.start))
- self.coeffs = np.append(self.coeffs, np.array(self.control1) - np.array(self.start))
- self.coeffs = np.append(self.coeffs, [0,0])
- self.coeffs = np.append(self.coeffs, [0,0])
- self.coeffs = np.append(self.coeffs, np.array(self.end) - np.array(self.control2))
+ self.coeffs = np.append(self.coeffs,
+ np.array(self.control1) - np.array(self.start))
+ self.coeffs = np.append(self.coeffs, [0, 0])
+ self.coeffs = np.append(self.coeffs, [0, 0])
+ self.coeffs = np.append(self.coeffs,
+ np.array(self.end) - np.array(self.control2))
self.coeffs = np.append(self.coeffs, np.array(self.end))
- self.coeffs = np.reshape(self.coeffs, newshape = (6, 2))
+ self.coeffs = np.reshape(self.coeffs, newshape=(6, 2))
# setters for control points, set coefficients
- def set_positions(self, p1 = None, p2 = None):
+ def set_positions(self, p1=None, p2=None):
if p1 != None:
self.start = p1
if p2 != None:
@@ -103,7 +115,7 @@
self.compute_coefficients()
- def set_controls(self, c1 = None, c2 = None):
+ def set_controls(self, c1=None, c2=None):
if c1 != None:
self.control1 = c1
if c2 != None:
@@ -111,12 +123,12 @@
self.compute_coefficients()
- def set_velocities(self, v1 = None, v2 = None):
+ def set_velocities(self, v1=None, v2=None):
if v1 != None:
self.control1 = self.start + v1
if v2 != None:
self.control2 = self.end + v2
-
+
self.compute_coefficients()
def get_smoothness(self):
@@ -125,14 +137,25 @@
# given Basis functions and controls compute coordinate given t
def spline_eval_hermite(self, t):
- return np.array(self.coeffs[0]*nth_H(0, t) + self.coeffs[1]*nth_H(1, t)+ self.coeffs[2]*nth_H(2, t) + self.coeffs[3]*nth_H(3, t) + self.coeffs[4]* nth_H(4, t)+ self.coeffs[5]*nth_H(5, t))
-
+ return np.array(self.coeffs[0] * nth_H(0, t) +
+ self.coeffs[1] * nth_H(1, t) +
+ self.coeffs[2] * nth_H(2, t) +
+ self.coeffs[3] * nth_H(3, t) +
+ self.coeffs[4] * nth_H(4, t) +
+ self.coeffs[5] * nth_H(5, t))
+
# given Basis functions and controls compute velocity given t
def spline_eval_hermite_v(self, t):
- return normalize(np.array(self.coeffs[0]*nth_H_prime(0, t) + self.coeffs[1]*nth_H_prime(1, t)+ self.coeffs[2]*nth_H_prime(2, t) + self.coeffs[3]*nth_H_prime(3, t) + self.coeffs[4]* nth_H_prime(4, t)+ self.coeffs[5]*nth_H_prime(5, t)))
+ return normalize(
+ np.array(self.coeffs[0] * nth_H_prime(0, t) +
+ self.coeffs[1] * nth_H_prime(1, t) +
+ self.coeffs[2] * nth_H_prime(2, t) +
+ self.coeffs[3] * nth_H_prime(3, t) +
+ self.coeffs[4] * nth_H_prime(4, t) +
+ self.coeffs[5] * nth_H_prime(5, t)))
# take coefficients and compute spline points/properties
- def setup(self, resolution_multiplier = 10, dt = .000001):
+ def setup(self, resolution_multiplier=10, dt=.000001):
points = []
velocities = []
accelerations = []
@@ -145,7 +168,7 @@
distance = 0
# iterate through interim points and compute pos_vectors, and at predefined points arc length,
- # velocity, and acceleration vectors and store them at their associated index
+ # velocity, and acceleration vectors and store them at their associated index
for i in range(0, self.resolution * resolution_multiplier):
t = i / (1.0 * self.resolution * resolution_multiplier)
@@ -157,7 +180,7 @@
distance = current_s + distance
# at important points compute important values and store
- if i % resolution_multiplier == 0:
+ if i % resolution_multiplier == 0:
s.append(distance)
points.append(current_point)
@@ -171,7 +194,8 @@
if np.linalg.norm(v) == 0:
curvature = 0
else:
- curvature = np.linalg.det(np.column_stack((v, a)) / (np.linalg.norm(v)**(3/2)))
+ curvature = np.linalg.det(
+ np.column_stack((v, a)) / (np.linalg.norm(v)**(3 / 2)))
velocities.append(v)
accelerations.append(a)
@@ -181,18 +205,17 @@
curvatures.append(0.0001)
else:
curvatures.append(curvature)
-
+
last_point = current_point
self.arc_lengths = np.array(s)
- self.points = np.reshape(points, newshape = (-1, 2))
- self.velocities = np.reshape(velocities, newshape = (-1, 2))
- self.accelerations = np.reshape(accelerations, newshape = (-1, 2))
+ self.points = np.reshape(points, newshape=(-1, 2))
+ self.velocities = np.reshape(velocities, newshape=(-1, 2))
+ self.accelerations = np.reshape(accelerations, newshape=(-1, 2))
self.thetas = np.array(thetas)
self.omegas = np.array(omegas)
self.curvatures = np.array(curvatures)
-
def plot_diagnostics(self):
plt.figure("Spline")
plt.title('Spline')
@@ -201,38 +224,48 @@
plt.figure("Diagnostics")
- plt.subplot(2, 2, 1)
+ plt.subplot(2, 2, 1)
plt.title('theta')
plt.xlabel('arc_length')
plt.ylabel('theta')
- theta, = plt.plot(self.arc_lengths, self.thetas, label = 'theta')
- plt.legend(handles = [theta])
+ theta, = plt.plot(self.arc_lengths, self.thetas, label='theta')
+ plt.legend(handles=[theta])
plt.subplot(2, 2, 2)
plt.title('omegas')
plt.xlabel('arc_length')
plt.ylabel('omega')
- omega, = plt.plot(self.arc_lengths, self.omegas, label = 'omega')
- plt.legend(handles = [omega])
+ omega, = plt.plot(self.arc_lengths, self.omegas, label='omega')
+ plt.legend(handles=[omega])
plt.subplot(2, 2, 3)
plt.title('Velocities')
plt.xlabel('arc_length')
plt.ylabel('velocity')
- dxds, = plt.plot(self.arc_lengths, self.velocities[:, 0], label = 'dx/ds')
- dyds, = plt.plot(self.arc_lengths, self.velocities[:, 1], label = 'dy/ds')
- plt.legend(handles = [dxds, dyds])
+ dxds, = plt.plot(self.arc_lengths,
+ self.velocities[:, 0],
+ label='dx/ds')
+ dyds, = plt.plot(self.arc_lengths,
+ self.velocities[:, 1],
+ label='dy/ds')
+ plt.legend(handles=[dxds, dyds])
plt.subplot(2, 2, 4)
plt.title('Accelerations')
plt.xlabel('arc_length')
plt.ylabel('acceleration')
- dx2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 0], label = 'd^2x/ds^2')
- dy2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 1], label = 'd^2x/ds^2')
- plt.legend(handles = [dx2ds2, dy2ds2])
+ dx2ds2, = plt.plot(self.arc_lengths,
+ self.accelerations[:, 0],
+ label='d^2x/ds^2')
+ dy2ds2, = plt.plot(self.arc_lengths,
+ self.accelerations[:, 1],
+ label='d^2x/ds^2')
+ plt.legend(handles=[dx2ds2, dy2ds2])
+
# class defining a number of splines with convinience methods
class Path:
+
def __init__(self):
self.splines = []
self.knot_accels = []
@@ -270,7 +303,7 @@
else:
print("index %f out of bounds, no spline of that index" % i)
- def join(self, first_priority = False):
+ def join(self, first_priority=False):
for i in range(0, len(self.splines)):
if first_priority & i != len(self.splines):
print("unfinished")
@@ -278,7 +311,12 @@
# class which takes a Path object along with constraints and reparamterizes it with respect to time
class Trajectory:
- def __init__(self, path, max_angular_accel=3, max_voltage=11, max_normal_accel = .2):
+
+ def __init__(self,
+ path,
+ max_angular_accel=3,
+ max_voltage=11,
+ max_normal_accel=.2):
self.path = path
self.A = drivetrain.A_continuous
self.B = drivetrain.B_continuous
@@ -291,43 +329,46 @@
self.max_velocities_adhering_to_normal_accel = []
self.max_velocities_adhering_to_voltage = []
- self.path.splines[0].setup(resolution_multiplier = 100)
+ self.path.splines[0].setup(resolution_multiplier=100)
self.set_max_v_adhering_to_normal_accel()
self.max_voltageK_pass()
def set_max_v_adhering_to_normal_accel(self):
Ks = self.path.get_K()
- accels = np.full_like(Ks, fill_value = self.max_normal_accel)
+ accels = np.full_like(Ks, fill_value=self.max_normal_accel)
max_velocities = np.sqrt(np.abs(accels / Ks))
self.max_velocities_adhering_to_normal_accel = max_velocities
def max_voltageK_pass(self):
max_ds_dt = []
Ks = self.path.get_K()
- turning_radii = np.full_like(Ks, fill_value = 1) / np.abs(Ks)
+ turning_radii = np.full_like(Ks, fill_value=1) / np.abs(Ks)
-
-
- # compute max steady-state velocity given voltage constraints
+ # compute max steady-state velocity given voltage constraints
for i in range(0, len(Ks)):
- v_ratio = (turning_radii[i] + self.robot_radius) / (turning_radii[i] - self.robot_radius)
- matrix = np.array([[self.A[1, 1], self.A[1, 3], self.B[1, 1]], [self.A[3, 1] - 1, self.A[3, 3], self.B[3, 1]], [-1, v_ratio, 0]])
- sols = np.array([-1 * self.max_voltage * self.B[1, 0], -1 * self.max_voltage * self.B[3, 0], 0])
+ v_ratio = (turning_radii[i] + self.robot_radius) / (
+ turning_radii[i] - self.robot_radius)
+ matrix = np.array([[self.A[1, 1], self.A[1, 3], self.B[1, 1]],
+ [self.A[3, 1] - 1, self.A[3, 3], self.B[3, 1]],
+ [-1, v_ratio, 0]])
+ sols = np.array([
+ -1 * self.max_voltage * self.B[1, 0],
+ -1 * self.max_voltage * self.B[3, 0], 0
+ ])
Vs = np.dot(np.linalg.inv(matrix), sols)
max_ds_dt.append((Vs[0] + Vs[1]) / 2)
-
+
self.max_velocities_adhering_to_voltage = max_ds_dt
+
# compute the maximum acceleration we can ask for given voltage and, ya know, staying on the path.
-
-
'''
These methods use the continuous form of our drivetrain state equation
in order to compute the maximum acceleration which adheres to the path
and voltage constraints, as well as any arbitary set of constraints
on velocity as a function of arc_length
'''
-
+
def forward_accel_pass(self):
points = self.path.get_points()
velocities = self.path.get_velocities()
@@ -335,27 +376,37 @@
arc_lenghts = self.path.get_S()
for i in range(0, len(points)):
- Xn1 =
-
-
+ #Xn1 =
+ pass
+
def backward_accelaration_pass(self):
- print("max backward accel pass")
+ print("max backward accel pass")
-
- def plot_diagnostics(self, i = 0):
+ def plot_diagnostics(self, i=0):
plt.figure('max velocity')
plt.title('max_v_normal_accel')
plt.xlabel('arc_length')
plt.ylabel('max V')
- max_v_normal = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_normal_accel, label = 'ds/dt (normal)')# , label = 'ds/dt')
- curvature = plt.plot(self.path.get_S(), 1000 * np.abs(self.path.get_K()), label = 'K')
- max_v_K_V = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_voltage, label = 'ds/dt (voltage)')
- plt.legend(handles = [max_v_normal[0], curvature[0], max_v_K_V[0]])
+ max_v_normal = plt.plot(self.path.get_S(),
+ self.max_velocities_adhering_to_normal_accel,
+ label='ds/dt (normal)') # , label = 'ds/dt')
+ curvature = plt.plot(self.path.get_S(),
+ 1000 * np.abs(self.path.get_K()),
+ label='K')
+ max_v_K_V = plt.plot(self.path.get_S(),
+ self.max_velocities_adhering_to_voltage,
+ label='ds/dt (voltage)')
+ plt.legend(handles=[max_v_normal[0], curvature[0], max_v_K_V[0]])
+
def main():
- A = Hermite_Spline(np.array([0,0]), np.array([0,400]), np.array([200,300]), np.array([200,200]), resolution = 200)
+ A = Hermite_Spline(np.array([0, 0]),
+ np.array([0, 400]),
+ np.array([200, 300]),
+ np.array([200, 200]),
+ resolution=200)
A.plot_diagnostics()
path = Path()
path.add_spline(A)
@@ -363,4 +414,5 @@
trajectory.plot_diagnostics()
plt.show()
+
main()
diff --git a/y2019/control_loops/python/drivetrain.py b/y2019/control_loops/python/drivetrain.py
index 9c4db49..0ef54ef 100644
--- a/y2019/control_loops/python/drivetrain.py
+++ b/y2019/control_loops/python/drivetrain.py
@@ -47,5 +47,6 @@
# Write the generated constants out to a file.
drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2019', kDrivetrain)
+
if __name__ == '__main__':
sys.exit(main(sys.argv))
diff --git a/y2019/control_loops/python/elevator.py b/y2019/control_loops/python/elevator.py
index b7fc74b..546ead5 100755
--- a/y2019/control_loops/python/elevator.py
+++ b/y2019/control_loops/python/elevator.py
@@ -18,18 +18,18 @@
first_stage_mass = 0.7957
carriage_mass = 2.754
-kElevator = linear_system.LinearSystemParams(
- name='Elevator',
- motor=control_loop.Vex775Pro(),
- G=(8.0 / 82.0),
- radius=2.25 * 0.0254 / 2.0,
- mass=first_stage_mass + carriage_mass,
- q_pos=0.070,
- q_vel=1.35,
- kalman_q_pos=0.12,
- kalman_q_vel=2.00,
- kalman_q_voltage=35.0,
- kalman_r_position=0.05)
+kElevator = linear_system.LinearSystemParams(name='Elevator',
+ motor=control_loop.Vex775Pro(),
+ G=(8.0 / 82.0),
+ radius=2.25 * 0.0254 / 2.0,
+ mass=first_stage_mass +
+ carriage_mass,
+ q_pos=0.070,
+ q_vel=1.35,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.00,
+ kalman_q_voltage=35.0,
+ kalman_r_position=0.05)
kElevatorBall = copy.copy(kElevator)
kElevatorBall.q_pos = 0.15
@@ -43,8 +43,10 @@
if FLAGS.plot:
R = numpy.matrix([[1.5], [0.0]])
linear_system.PlotKick(kElevatorBall, R, plant_params=kElevatorModel)
- linear_system.PlotMotion(
- kElevatorBall, R, max_velocity=5.0, plant_params=kElevatorModel)
+ linear_system.PlotMotion(kElevatorBall,
+ R,
+ max_velocity=5.0,
+ plant_params=kElevatorModel)
# Write the generated constants out to a file.
if len(argv) != 5:
diff --git a/y2019/control_loops/python/intake.py b/y2019/control_loops/python/intake.py
index 828f54e..36d737c 100755
--- a/y2019/control_loops/python/intake.py
+++ b/y2019/control_loops/python/intake.py
@@ -20,7 +20,7 @@
kIntake = angular_system.AngularSystemParams(
name='Intake',
motor=control_loop.BAG(),
- G=(1.0 / 7.0) * (1.0 / 4.0) * (1.0 / 4.0)* (18.0 / 38.0),
+ G=(1.0 / 7.0) * (1.0 / 4.0) * (1.0 / 4.0) * (18.0 / 38.0),
# Suneel: Sampled moment of inertia at 6 different positions
# J = the average of the six.
# 1. 0.686
diff --git a/y2019/control_loops/python/polydrivetrain.py b/y2019/control_loops/python/polydrivetrain.py
index eaccb92..a045af1 100644
--- a/y2019/control_loops/python/polydrivetrain.py
+++ b/y2019/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2019',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2019', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2019/control_loops/python/wrist.py b/y2019/control_loops/python/wrist.py
index c983a1d..f20975c 100755
--- a/y2019/control_loops/python/wrist.py
+++ b/y2019/control_loops/python/wrist.py
@@ -25,17 +25,17 @@
# Wrist with hatch
# 0.446
-kWrist = angular_system.AngularSystemParams(
- name='Wrist',
- motor=control_loop.BAG(),
- G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
- J=0.30,
- q_pos=0.20,
- q_vel=5.0,
- kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
- kalman_r_position=0.05)
+kWrist = angular_system.AngularSystemParams(name='Wrist',
+ motor=control_loop.BAG(),
+ G=(6.0 / 60.0) * (20.0 / 100.0) *
+ (24.0 / 84.0),
+ J=0.30,
+ q_pos=0.20,
+ q_vel=5.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05)
kWristBall = copy.copy(kWrist)
kWristBall.J = 0.4007
diff --git a/y2019/vision/undistort.py b/y2019/vision/undistort.py
index 8e1cbd2..feeb7c8 100755
--- a/y2019/vision/undistort.py
+++ b/y2019/vision/undistort.py
@@ -74,8 +74,9 @@
cv2.destroyAllWindows()
fd.close()
- ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
- objpoints, imgpoints, grey.shape[::-1], None, None)
+ ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
+ grey.shape[::-1], None,
+ None)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (rows, cols),
1, (rows, cols))
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index bb6871c..54ec9d3 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -20,20 +20,19 @@
# Gear ratio to the final wheel.
G = (30.0 / 40.0) * numpy.power(G_per_wheel, 3.0)
# Overall flywheel inertia.
-J = J_wheel * (
- 1.0 + numpy.power(G_per_wheel, -2.0) + numpy.power(G_per_wheel, -4.0) + numpy.power(G_per_wheel, -6.0))
+J = J_wheel * (1.0 + numpy.power(G_per_wheel, -2.0) +
+ numpy.power(G_per_wheel, -4.0) + numpy.power(G_per_wheel, -6.0))
# The position and velocity are measured for the final wheel.
-kAccelerator = flywheel.FlywheelParams(
- name='Accelerator',
- motor=control_loop.Falcon(),
- G=G,
- J=J * 1.3,
- q_pos=0.01,
- q_vel=40.0,
- q_voltage=1.0,
- r_pos=0.03,
- controller_poles=[.89])
+kAccelerator = flywheel.FlywheelParams(name='Accelerator',
+ motor=control_loop.Falcon(),
+ G=G,
+ J=J * 1.3,
+ q_pos=0.01,
+ q_vel=40.0,
+ q_voltage=1.0,
+ r_pos=0.03,
+ controller_poles=[.89])
def main(argv):
diff --git a/y2020/control_loops/python/control_panel.py b/y2020/control_loops/python/control_panel.py
index 7a5bdf9..6bda841 100644
--- a/y2020/control_loops/python/control_panel.py
+++ b/y2020/control_loops/python/control_panel.py
@@ -17,17 +17,16 @@
except gflags.DuplicateFlagError:
pass
-kControlPanel = angular_system.AngularSystemParams(
- name='ControlPanel',
- motor=control_loop.BAG(),
- G=1.0,
- J=0.000009,
- q_pos=0.20,
- q_vel=5.0,
- kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
- kalman_r_position=0.05)
+kControlPanel = angular_system.AngularSystemParams(name='ControlPanel',
+ motor=control_loop.BAG(),
+ G=1.0,
+ J=0.000009,
+ q_pos=0.20,
+ q_vel=5.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05)
def main(argv):
@@ -42,7 +41,9 @@
'Expected .h file name and .cc file name for the control_panel and integral control_panel.'
)
else:
- namespaces = ['y2020', 'control_loops', 'superstructure', 'control_panel']
+ namespaces = [
+ 'y2020', 'control_loops', 'superstructure', 'control_panel'
+ ]
angular_system.WriteAngularSystem(kControlPanel, argv[1:3], argv[3:5],
namespaces)
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 8c33cc3..7e703fc 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -22,14 +22,17 @@
J = 0.00507464
J = 0.0035
+
def AddResistance(motor, resistance):
motor.resistance += resistance
return motor
+
def ScaleKv(motor, scale):
motor.Kv *= scale
return motor
+
# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 6f41a17..9153bc6 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -7,6 +7,7 @@
class FlywheelParams(object):
+
def __init__(self,
name,
motor,
@@ -31,6 +32,7 @@
class VelocityFlywheel(control_loop.HybridControlLoop):
+
def __init__(self, params, name="Flywheel"):
super(VelocityFlywheel, self).__init__(name=name)
self.params = params
@@ -76,6 +78,7 @@
class Flywheel(VelocityFlywheel):
+
def __init__(self, params, name="Flywheel"):
super(Flywheel, self).__init__(params, name=name)
@@ -112,6 +115,7 @@
class IntegralFlywheel(Flywheel):
+
def __init__(self, params, name="IntegralFlywheel"):
super(IntegralFlywheel, self).__init__(params, name=name)
@@ -125,7 +129,6 @@
self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
-
# states
# [position, velocity, voltage_error]
self.C_unaugmented = self.C
@@ -146,24 +149,26 @@
q_vel = self.params.q_vel
q_voltage = self.params.q_voltage
self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
- [0.0, (q_vel**2.0), 0.0],
- [0.0, 0.0, (q_voltage**2.0)]])
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
r_pos = self.params.r_pos
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.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)
glog.debug('Q_discrete %s' % (str(self.Q)))
glog.debug('R_discrete %s' % (str(self.R)))
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ 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
@@ -283,34 +288,32 @@
for index, param in enumerate(params):
flywheels.append(Flywheel(param, name=param.name + str(index)))
integral_flywheels.append(
- IntegralFlywheel(
- param, name='Integral' + param.name + str(index)))
+ IntegralFlywheel(param,
+ name='Integral' + param.name + str(index)))
else:
name = params.name
flywheels.append(Flywheel(params, params.name))
integral_flywheels.append(
IntegralFlywheel(params, name='Integral' + params.name))
- loop_writer = control_loop.ControlLoopWriter(
- name, flywheels, namespaces=namespace)
+ loop_writer = control_loop.ControlLoopWriter(name,
+ flywheels,
+ namespaces=namespace)
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f',
- flywheels[0].G))
+ control_loop.Constant('kOutputRatio', '%f', flywheels[0].G))
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f',
flywheels[0].motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant(
- 'kBemf',
- '%f',
- flywheels[0].motor.Kv * flywheels[0].G,
- comment="// Radians/sec / volt"))
+ control_loop.Constant('kBemf',
+ '%f',
+ flywheels[0].motor.Kv * flywheels[0].G,
+ comment="// Radians/sec / volt"))
loop_writer.AddConstant(
- control_loop.Constant(
- 'kResistance',
- '%f',
- flywheels[0].motor.resistance,
- comment="// Ohms"))
+ control_loop.Constant('kResistance',
+ '%f',
+ flywheels[0].motor.resistance,
+ comment="// Ohms"))
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index eacb7fd..8125f8b 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -29,17 +29,17 @@
radians_per_turn_of_motor = 12.0 / 60.0 * radians_per_turn
-kHood = angular_system.AngularSystemParams(
- name='Hood',
- motor=control_loop.BAG(),
- G=radians_per_turn_of_motor / (2.0 * numpy.pi),
- J=0.1,
- q_pos=0.15,
- q_vel=10.0,
- kalman_q_pos=0.12,
- kalman_q_vel=10.0,
- kalman_q_voltage=30.0,
- kalman_r_position=0.02)
+kHood = angular_system.AngularSystemParams(name='Hood',
+ motor=control_loop.BAG(),
+ G=radians_per_turn_of_motor /
+ (2.0 * numpy.pi),
+ J=0.1,
+ q_pos=0.15,
+ q_vel=10.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=10.0,
+ kalman_q_voltage=30.0,
+ kalman_r_position=0.02)
def main(argv):
diff --git a/y2020/control_loops/python/intake.py b/y2020/control_loops/python/intake.py
index 1035070..87896dd 100644
--- a/y2020/control_loops/python/intake.py
+++ b/y2020/control_loops/python/intake.py
@@ -17,17 +17,17 @@
except gflags.DuplicateFlagError:
pass
-kIntake = angular_system.AngularSystemParams(
- name='Intake',
- motor=control_loop.BAG(),
- G=(12.0 / 24.0) * (1.0 / 5.0) * (1.0 / 5.0) * (16.0 / 32.0),
- J=8 * 0.139 * 0.139,
- q_pos=0.40,
- q_vel=20.0,
- kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
- kalman_r_position=0.05)
+kIntake = angular_system.AngularSystemParams(name='Intake',
+ motor=control_loop.BAG(),
+ G=(12.0 / 24.0) * (1.0 / 5.0) *
+ (1.0 / 5.0) * (16.0 / 32.0),
+ J=8 * 0.139 * 0.139,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05)
def main(argv):
diff --git a/y2020/control_loops/python/polydrivetrain.py b/y2020/control_loops/python/polydrivetrain.py
index dad06b0..dbd5268 100644
--- a/y2020/control_loops/python/polydrivetrain.py
+++ b/y2020/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2020',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2020', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index 6f15057..5f77f84 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -18,17 +18,18 @@
except gflags.DuplicateFlagError:
pass
-kTurret = angular_system.AngularSystemParams(
- name='Turret',
- motor=control_loop.MiniCIM(),
- G=(26.0 / 150.0) * (14.0 / 60.0) * (20.0 / 60.0),
- J=0.20,
- q_pos=0.30,
- q_vel=4.5,
- kalman_q_pos=0.12,
- kalman_q_vel=10.0,
- kalman_q_voltage=20.0,
- kalman_r_position=0.05)
+kTurret = angular_system.AngularSystemParams(name='Turret',
+ motor=control_loop.MiniCIM(),
+ G=(26.0 / 150.0) * (14.0 / 60.0) *
+ (20.0 / 60.0),
+ J=0.20,
+ q_pos=0.30,
+ q_vel=4.5,
+ kalman_q_pos=0.12,
+ kalman_q_vel=10.0,
+ kalman_q_voltage=20.0,
+ kalman_r_position=0.05)
+
def main(argv):
if FLAGS.plot:
@@ -38,13 +39,11 @@
# Write the generated constants out to a file.
if len(argv) != 5:
- glog.fatal(
- 'Expected .h file name and .cc file name for the turret.'
- )
+ glog.fatal('Expected .h file name and .cc file name for the turret.')
else:
namespaces = ['y2020', 'control_loops', 'superstructure', 'turret']
- angular_system.WriteAngularSystem([kTurret],
- argv[1:3], argv[3:5], namespaces)
+ angular_system.WriteAngularSystem([kTurret], argv[1:3], argv[3:5],
+ namespaces)
if __name__ == '__main__':
diff --git a/y2020/vision/galactic_search_config.py b/y2020/vision/galactic_search_config.py
index 90988e3..870e039 100755
--- a/y2020/vision/galactic_search_config.py
+++ b/y2020/vision/galactic_search_config.py
@@ -20,7 +20,7 @@
import os
import sys
-_num_rects = 3 # can be 3 or 2, can be specified in commang line arg
+_num_rects = 3 # can be 3 or 2, can be specified in commang line arg
_path = Path(Letter.kA, Alliance.kRed, [Rect(None, None, None, None)])
@@ -29,20 +29,24 @@
_fig, _img_ax = plt.subplots()
-_txt = _img_ax.text(0, 0, "", size = 10, backgroundcolor = "white")
+_txt = _img_ax.text(0, 0, "", size=10, backgroundcolor="white")
_confirm = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Confirm")
_cancel = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Cancel")
_submit = Button(plt.axes([0.4, 0.05, 0.1, 0.1]), "Submit")
+
def draw_txt(txt):
- txt.set_text("Click on top left point and bottom right point for rect #%u" % (_rect_index + 1))
+ txt.set_text(
+ "Click on top left point and bottom right point for rect #%u" %
+ (_rect_index + 1))
txt.set_color(_path.alliance.value)
def on_confirm(event):
global _rect_index
- if _path.rects[_rect_index].x1 != None and _path.rects[_rect_index].x2 != None:
+ if _path.rects[_rect_index].x1 != None and _path.rects[
+ _rect_index].x2 != None:
_confirm.ax.set_visible(False)
_cancel.ax.set_visible(False)
_rect_index += 1
@@ -54,6 +58,7 @@
_path.rects.append(Rect(None, None, None, None))
plt.show()
+
def on_cancel(event):
global _rect_index
if _rect_index < _num_rects:
@@ -66,6 +71,7 @@
_path.rects[_rect_index].y2 = None
plt.show()
+
def on_submit(event):
plt.close("all")
dict = None
@@ -74,9 +80,12 @@
dict[_path.letter.name] = {}
if _path.alliance.name not in dict[_path.letter.name]:
dict[_path.letter.name][_path.alliance.name] = []
- dict[_path.letter.name][_path.alliance.name] = [rect.to_list() for rect in _path.rects]
+ dict[_path.letter.name][_path.alliance.name] = [
+ rect.to_list() for rect in _path.rects
+ ]
with open(RECTS_JSON_PATH, 'w') as rects_json:
- json.dump(dict, rects_json, indent = 2)
+ json.dump(dict, rects_json, indent=2)
+
# Clears rect on screen
def clear_rect():
@@ -85,6 +94,7 @@
else:
_img_ax.patches[-1].remove()
+
def on_click(event):
# This gets called for each click of the rectangle corners,
# but also gets called when the user clicks on the Submit button.
@@ -94,9 +104,11 @@
# the bounds of the axis
if _rect_index < _num_rects and event.xdata != None and event.ydata != None:
if _path.rects[_rect_index].x1 == None:
- _path.rects[_rect_index].x1, _path.rects[_rect_index].y1 = int(event.xdata), int(event.ydata)
+ _path.rects[_rect_index].x1, _path.rects[_rect_index].y1 = int(
+ event.xdata), int(event.ydata)
elif _path.rects[_rect_index].x2 == None:
- _path.rects[_rect_index].x2, _path.rects[_rect_index].y2 = int(event.xdata), int(event.ydata)
+ _path.rects[_rect_index].x2, _path.rects[_rect_index].y2 = int(
+ event.xdata), int(event.ydata)
if _path.rects[_rect_index].x2 < _path.rects[_rect_index].x1:
tmp = _path.rects[_rect_index].x1
_path.rects[_rect_index].x1 = _path.rects[_rect_index].x2
@@ -106,20 +118,27 @@
_path.rects[_rect_index].y1 = _path.rects[_rect_index].y2
_path.rects[_rect_index].y2 = tmp
- _img_ax.add_patch(patches.Rectangle((_path.rects[_rect_index].x1, _path.rects[_rect_index].y1),
- _path.rects[_rect_index].x2 - _path.rects[_rect_index].x1,
- _path.rects[_rect_index].y2 - _path.rects[_rect_index].y1,
- edgecolor = 'r', linewidth = 1, facecolor="none"))
+ _img_ax.add_patch(
+ patches.Rectangle(
+ (_path.rects[_rect_index].x1, _path.rects[_rect_index].y1),
+ _path.rects[_rect_index].x2 - _path.rects[_rect_index].x1,
+ _path.rects[_rect_index].y2 - _path.rects[_rect_index].y1,
+ edgecolor='r',
+ linewidth=1,
+ facecolor="none"))
_confirm.ax.set_visible(True)
_cancel.ax.set_visible(True)
plt.show()
else:
- glog.info("Either submitted or user pressed out of the bounds of the axis")
+ glog.info(
+ "Either submitted or user pressed out of the bounds of the axis")
+
def setup_button(button, on_clicked):
button.on_clicked(on_clicked)
button.ax.set_visible(False)
+
def setup_ui():
_img_ax.imshow(capture_img())
release_stream()
@@ -131,11 +150,12 @@
draw_txt(_txt)
plt.show()
+
def view_rects():
rects_dict = load_json()
- if (_path.letter.name in rects_dict and
- _path.alliance.name in rects_dict[_path.letter.name]):
+ if (_path.letter.name in rects_dict
+ and _path.alliance.name in rects_dict[_path.letter.name]):
_confirm.ax.set_visible(False)
_cancel.ax.set_visible(False)
_submit.ax.set_visible(False)
@@ -143,20 +163,26 @@
for rect_list in rects_dict[_path.letter.name][_path.alliance.name]:
rect = Rect.from_list(rect_list)
- _img_ax.add_patch(patches.Rectangle((rect.x1, rect.y1),
- rect.x2 - rect.x1, rect.y2 - rect.y1,
- edgecolor = 'r', linewidth = 1, facecolor="none"))
+ _img_ax.add_patch(
+ patches.Rectangle((rect.x1, rect.y1),
+ rect.x2 - rect.x1,
+ rect.y2 - rect.y1,
+ edgecolor='r',
+ linewidth=1,
+ facecolor="none"))
plt.show()
else:
glog.error("Could not find path %s %s in rects.json",
- _path.alliance.value, _path.letter.value)
+ _path.alliance.value, _path.letter.value)
+
def main(argv):
global _num_rects
glog.setLevel("INFO")
- opts = getopt.getopt(argv[1 : ], "a:l:n:",
- ["alliance = ", "letter = ", "_num_rects = ", "view"])[0]
+ opts = getopt.getopt(
+ argv[1:], "a:l:n:",
+ ["alliance = ", "letter = ", "_num_rects = ", "view"])[0]
view = False
for opt, arg in opts:
if opt in ["-a", "--alliance"]:
diff --git a/y2020/vision/galactic_search_path.py b/y2020/vision/galactic_search_path.py
index 00b572a..11ec73d 100755
--- a/y2020/vision/galactic_search_path.py
+++ b/y2020/vision/galactic_search_path.py
@@ -8,6 +8,7 @@
import numpy as np
import os
+
class Rect:
# x1 and y1 are top left corner, x2 and y2 are bottom right
@@ -41,11 +42,14 @@
@staticmethod
def from_value(value):
- return (Alliance.kRed if value == Alliance.kRed.value else Alliance.kBlue)
+ return (Alliance.kRed
+ if value == Alliance.kRed.value else Alliance.kBlue)
@staticmethod
def from_name(name):
- return (Alliance.kRed if name == Alliance.kRed.name else Alliance.kBlue)
+ return (Alliance.kRed
+ if name == Alliance.kRed.name else Alliance.kBlue)
+
class Letter(Enum):
kA = 'A'
@@ -59,6 +63,7 @@
def from_name(name):
return (Letter.kA if name == Letter.kA.name else Letter.kB)
+
class Path:
def __init__(self, letter, alliance, rects):
@@ -72,15 +77,18 @@
def to_dict(self):
return {"alliance": self.alliance.name, "letter": self.letter.name}
+
RECTS_JSON_PATH = "rects.json"
AOS_SEND_PATH = "bazel-bin/aos/aos_send"
+
def setup_if_pi():
if os.path.isdir("/home/pi/bin"):
AOS_SEND_PATH = "/home/pi/bin/aos_send.stripped"
os.system("./starter_cmd stop camera_reader")
+
setup_if_pi()
# The minimum percentage of yellow for a region of a image to
@@ -89,12 +97,14 @@
_paths = []
+
def load_json():
rects_dict = None
with open(RECTS_JSON_PATH, 'r') as rects_json:
rects_dict = json.load(rects_json)
return rects_dict
+
def _run_detection_loop():
global img_fig, rects_dict
@@ -104,7 +114,9 @@
rects = []
for rect_list in rects_dict[letter][alliance]:
rects.append(Rect.from_list(rect_list))
- _paths.append(Path(Letter.from_name(letter), Alliance.from_name(alliance), rects))
+ _paths.append(
+ Path(Letter.from_name(letter), Alliance.from_name(alliance),
+ rects))
plt.ion()
img_fig = plt.figure()
@@ -113,6 +125,7 @@
while running:
_detect_path()
+
def _detect_path():
img = capture_img()
img_fig.figimage(img)
@@ -138,7 +151,8 @@
current_path = path
num_current_paths += 1
else:
- glog.error("Error: len of pcts (%u) != len of rects: (%u)", len(pcts), len(rects))
+ glog.error("Error: len of pcts (%u) != len of rects: (%u)",
+ len(pcts), len(rects))
if num_current_paths != 1:
if num_current_paths == 0:
@@ -147,24 +161,27 @@
glog.warn("Expected 1 path but detected %u", num_current_paths)
return
-
path_dict = current_path.to_dict()
glog.info("Path is %s", path_dict)
os.system(AOS_SEND_PATH +
- " /pi2/camera y2020.vision.GalacticSearchPath '" + json.dumps(path_dict) + "'")
+ " /pi2/camera y2020.vision.GalacticSearchPath '" +
+ json.dumps(path_dict) + "'")
+
KERNEL = np.ones((5, 5), np.uint8)
+
def _create_mask(img):
hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
- lower_yellow = np.array([23, 100, 75], dtype = np.uint8)
- higher_yellow = np.array([40, 255, 255], dtype = np.uint8)
+ lower_yellow = np.array([23, 100, 75], dtype=np.uint8)
+ higher_yellow = np.array([40, 255, 255], dtype=np.uint8)
mask = cv.inRange(hsv, lower_yellow, higher_yellow)
- mask = cv.erode(mask, KERNEL, iterations = 1)
- mask = cv.dilate(mask, KERNEL, iterations = 3)
+ mask = cv.erode(mask, KERNEL, iterations=1)
+ mask = cv.dilate(mask, KERNEL, iterations=3)
return mask
+
# This function finds the percentage of yellow pixels in the rectangles
# given that are regions of the given image. This allows us to determine
# whether there is a ball in those rectangles
@@ -172,25 +189,30 @@
pcts = np.zeros(len(rects))
for i in range(len(rects)):
rect = rects[i]
- slice = mask[rect.y1 : rect.y2, rect.x1 : rect.x2]
+ slice = mask[rect.y1:rect.y2, rect.x1:rect.x2]
yellow_px = np.count_nonzero(slice)
pcts[i] = yellow_px / (slice.shape[0] * slice.shape[1])
return pcts
+
_video_stream = cv.VideoCapture(0)
+
def capture_img():
global _video_stream
return _video_stream.read()[1]
+
def release_stream():
global _video_stream
_video_stream.release()
+
def main():
_run_detection_loop()
release_stream()
+
if __name__ == "__main__":
main()
diff --git a/y2020/vision/sift/demo_sift_training.py b/y2020/vision/sift/demo_sift_training.py
index 67874c9..4e9c306 100644
--- a/y2020/vision/sift/demo_sift_training.py
+++ b/y2020/vision/sift/demo_sift_training.py
@@ -8,87 +8,89 @@
import frc971.vision.sift.TrainingData as TrainingData
import frc971.vision.sift.Feature as Feature
+
def main():
- image4_cleaned_path = sys.argv[1]
- output_path = sys.argv[2]
+ image4_cleaned_path = sys.argv[1]
+ output_path = sys.argv[2]
- image4_cleaned = cv2.imread(image4_cleaned_path)
+ image4_cleaned = cv2.imread(image4_cleaned_path)
- image = cv2.cvtColor(image4_cleaned, cv2.COLOR_BGR2GRAY)
- image = cv2.resize(image, (640, 480))
- sift = cv2.SIFT_create()
- keypoints, descriptors = sift.detectAndCompute(image, None)
+ image = cv2.cvtColor(image4_cleaned, cv2.COLOR_BGR2GRAY)
+ image = cv2.resize(image, (640, 480))
+ sift = cv2.SIFT_create()
+ keypoints, descriptors = sift.detectAndCompute(image, None)
- fbb = flatbuffers.Builder(0)
+ fbb = flatbuffers.Builder(0)
- features_vector = []
+ features_vector = []
- for keypoint, descriptor in zip(keypoints, descriptors):
- Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
- for n in reversed(descriptor):
- assert n == round(n)
- fbb.PrependUint8(int(round(n)))
- descriptor_vector = fbb.EndVector()
+ for keypoint, descriptor in zip(keypoints, descriptors):
+ Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
+ for n in reversed(descriptor):
+ assert n == round(n)
+ fbb.PrependUint8(int(round(n)))
+ descriptor_vector = fbb.EndVector()
- Feature.FeatureStart(fbb)
+ Feature.FeatureStart(fbb)
- Feature.FeatureAddDescriptor(fbb, descriptor_vector)
- Feature.FeatureAddX(fbb, keypoint.pt[0])
- Feature.FeatureAddY(fbb, keypoint.pt[1])
- Feature.FeatureAddSize(fbb, keypoint.size)
- Feature.FeatureAddAngle(fbb, keypoint.angle)
- Feature.FeatureAddResponse(fbb, keypoint.response)
- Feature.FeatureAddOctave(fbb, keypoint.octave)
+ Feature.FeatureAddDescriptor(fbb, descriptor_vector)
+ Feature.FeatureAddX(fbb, keypoint.pt[0])
+ Feature.FeatureAddY(fbb, keypoint.pt[1])
+ Feature.FeatureAddSize(fbb, keypoint.size)
+ Feature.FeatureAddAngle(fbb, keypoint.angle)
+ Feature.FeatureAddResponse(fbb, keypoint.response)
+ Feature.FeatureAddOctave(fbb, keypoint.octave)
- features_vector.append(Feature.FeatureEnd(fbb))
+ features_vector.append(Feature.FeatureEnd(fbb))
- TrainingImage.TrainingImageStartFeaturesVector(fbb, len(features_vector))
- for feature in reversed(features_vector):
- fbb.PrependUOffsetTRelative(feature)
- features_vector_table = fbb.EndVector()
+ TrainingImage.TrainingImageStartFeaturesVector(fbb, len(features_vector))
+ for feature in reversed(features_vector):
+ fbb.PrependUOffsetTRelative(feature)
+ features_vector_table = fbb.EndVector()
- TrainingImage.TrainingImageStart(fbb)
- TrainingImage.TrainingImageAddFeatures(fbb, features_vector_table)
- # TODO(Brian): Fill out the transformation matrices.
- training_image_offset = TrainingImage.TrainingImageEnd(fbb)
+ TrainingImage.TrainingImageStart(fbb)
+ TrainingImage.TrainingImageAddFeatures(fbb, features_vector_table)
+ # TODO(Brian): Fill out the transformation matrices.
+ training_image_offset = TrainingImage.TrainingImageEnd(fbb)
- TrainingData.TrainingDataStartImagesVector(fbb, 1)
- fbb.PrependUOffsetTRelative(training_image_offset)
- images_offset = fbb.EndVector()
+ TrainingData.TrainingDataStartImagesVector(fbb, 1)
+ fbb.PrependUOffsetTRelative(training_image_offset)
+ images_offset = fbb.EndVector()
- TrainingData.TrainingDataStart(fbb)
- TrainingData.TrainingDataAddImages(fbb, images_offset)
- fbb.Finish(TrainingData.TrainingDataEnd(fbb))
+ TrainingData.TrainingDataStart(fbb)
+ TrainingData.TrainingDataAddImages(fbb, images_offset)
+ fbb.Finish(TrainingData.TrainingDataEnd(fbb))
- bfbs = fbb.Output()
+ bfbs = fbb.Output()
- output_prefix = [
- b'#ifndef Y2020_VISION_SIFT_DEMO_SIFT_H_',
- b'#define Y2020_VISION_SIFT_DEMO_SIFT_H_',
- b'#include <string_view>',
- b'namespace frc971 {',
- b'namespace vision {',
- b'inline std::string_view DemoSiftData() {',
- ]
- output_suffix = [
- b' return std::string_view(kData, sizeof(kData));',
- b'}',
- b'} // namespace vision',
- b'} // namespace frc971',
- b'#endif // Y2020_VISION_SIFT_DEMO_SIFT_H_',
- ]
+ output_prefix = [
+ b'#ifndef Y2020_VISION_SIFT_DEMO_SIFT_H_',
+ b'#define Y2020_VISION_SIFT_DEMO_SIFT_H_',
+ b'#include <string_view>',
+ b'namespace frc971 {',
+ b'namespace vision {',
+ b'inline std::string_view DemoSiftData() {',
+ ]
+ output_suffix = [
+ b' return std::string_view(kData, sizeof(kData));',
+ b'}',
+ b'} // namespace vision',
+ b'} // namespace frc971',
+ b'#endif // Y2020_VISION_SIFT_DEMO_SIFT_H_',
+ ]
- with open(output_path, 'wb') as output:
- for line in output_prefix:
- output.write(line)
- output.write(b'\n')
- output.write(b'alignas(64) static constexpr char kData[] = "')
- for byte in fbb.Output():
- output.write(b'\\x' + (b'%x' % byte).zfill(2))
- output.write(b'";\n')
- for line in output_suffix:
- output.write(line)
- output.write(b'\n')
+ with open(output_path, 'wb') as output:
+ for line in output_prefix:
+ output.write(line)
+ output.write(b'\n')
+ output.write(b'alignas(64) static constexpr char kData[] = "')
+ for byte in fbb.Output():
+ output.write(b'\\x' + (b'%x' % byte).zfill(2))
+ output.write(b'";\n')
+ for line in output_suffix:
+ output.write(line)
+ output.write(b'\n')
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/y2020/vision/sift/fast_gaussian_runner.py b/y2020/vision/sift/fast_gaussian_runner.py
index d812f3f..9767300 100755
--- a/y2020/vision/sift/fast_gaussian_runner.py
+++ b/y2020/vision/sift/fast_gaussian_runner.py
@@ -8,190 +8,211 @@
from bazel_tools.tools.python.runfiles import runfiles
+
def main(params):
- r = runfiles.Create()
- generator = r.Rlocation('org_frc971/y2020/vision/sift/fast_gaussian_generator')
+ r = runfiles.Create()
+ generator = r.Rlocation(
+ 'org_frc971/y2020/vision/sift/fast_gaussian_generator')
- ruledir = sys.argv[2]
- target_cpu = sys.argv[3]
+ ruledir = sys.argv[2]
+ target_cpu = sys.argv[3]
- target = {
- 'aarch64': 'arm-64-linux-no_asserts',
- 'armv7': 'arm-32-linux-no_asserts',
- 'k8': 'x86-64-linux-no_asserts',
- }[target_cpu]
+ target = {
+ 'aarch64': 'arm-64-linux-no_asserts',
+ 'armv7': 'arm-32-linux-no_asserts',
+ 'k8': 'x86-64-linux-no_asserts',
+ }[target_cpu]
- commands = []
+ commands = []
- amd64_debian_sysroot = r.Rlocation('amd64_debian_sysroot/usr/lib/x86_64-linux-gnu/libc.so.6').rsplit('/', 4)[0]
- env = os.environ.copy()
- env['LD_LIBRARY_PATH'] = ':'.join([
- # TODO(brian): Figure this out again. It is a bit aggressive.
- #amd64_debian_sysroot + '/lib/x86_64-linux-gnu',
- #amd64_debian_sysroot + '/lib',
- #amd64_debian_sysroot + '/usr/lib/x86_64-linux-gnu',
- #amd64_debian_sysroot + '/usr/lib',
- ])
+ amd64_debian_sysroot = r.Rlocation(
+ 'amd64_debian_sysroot/usr/lib/x86_64-linux-gnu/libc.so.6').rsplit(
+ '/', 4)[0]
+ env = os.environ.copy()
+ env['LD_LIBRARY_PATH'] = ':'.join([
+ # TODO(brian): Figure this out again. It is a bit aggressive.
+ #amd64_debian_sysroot + '/lib/x86_64-linux-gnu',
+ #amd64_debian_sysroot + '/lib',
+ #amd64_debian_sysroot + '/usr/lib/x86_64-linux-gnu',
+ #amd64_debian_sysroot + '/usr/lib',
+ ])
- all_header = [
- '#ifndef Y2020_VISION_SIFT_FAST_GAUSSIAN_ALL_H_',
- '#define Y2020_VISION_SIFT_FAST_GAUSSIAN_ALL_H_',
- '#include "HalideBuffer.h"',
- ]
+ all_header = [
+ '#ifndef Y2020_VISION_SIFT_FAST_GAUSSIAN_ALL_H_',
+ '#define Y2020_VISION_SIFT_FAST_GAUSSIAN_ALL_H_',
+ '#include "HalideBuffer.h"',
+ ]
- for cols, rows in params['sizes']:
- for sigma, sigma_name, filter_width in params['sigmas']:
- name = "fast_gaussian_%dx%d_%s" % (cols, rows, sigma_name)
+ for cols, rows in params['sizes']:
+ for sigma, sigma_name, filter_width in params['sigmas']:
+ name = "fast_gaussian_%dx%d_%s" % (cols, rows, sigma_name)
- commands.append([
- generator,
- '-g', 'gaussian_generator',
- '-o', ruledir,
- '-f', name,
- '-e', 'o,h,html',
- 'target=%s-no_runtime' % target,
- 'cols=%s' % cols,
- 'rows=%s' % rows,
- 'sigma=%s' % sigma,
- 'filter_width=%s' % filter_width,
- ])
- all_header += [
- '#include "y2020/vision/sift/%s.h"' % name,
- ]
+ commands.append([
+ generator,
+ '-g',
+ 'gaussian_generator',
+ '-o',
+ ruledir,
+ '-f',
+ name,
+ '-e',
+ 'o,h,html',
+ 'target=%s-no_runtime' % target,
+ 'cols=%s' % cols,
+ 'rows=%s' % rows,
+ 'sigma=%s' % sigma,
+ 'filter_width=%s' % filter_width,
+ ])
+ all_header += [
+ '#include "y2020/vision/sift/%s.h"' % name,
+ ]
- name = "fast_gaussian_subtract_%dx%d_%s" % (cols, rows, sigma_name)
+ name = "fast_gaussian_subtract_%dx%d_%s" % (cols, rows, sigma_name)
- commands.append([
- generator,
- '-g', 'gaussian_and_subtract_generator',
- '-o', ruledir,
- '-f', name,
- '-e', 'o,h,html',
- 'target=%s-no_runtime' % target,
- 'cols=%s' % cols,
- 'rows=%s' % rows,
- 'sigma=%s' % sigma,
- 'filter_width=%s' % filter_width,
- ])
- all_header += [
- '#include "y2020/vision/sift/%s.h"' % name,
- ]
+ commands.append([
+ generator,
+ '-g',
+ 'gaussian_and_subtract_generator',
+ '-o',
+ ruledir,
+ '-f',
+ name,
+ '-e',
+ 'o,h,html',
+ 'target=%s-no_runtime' % target,
+ 'cols=%s' % cols,
+ 'rows=%s' % rows,
+ 'sigma=%s' % sigma,
+ 'filter_width=%s' % filter_width,
+ ])
+ all_header += [
+ '#include "y2020/vision/sift/%s.h"' % name,
+ ]
- name = 'fast_subtract_%dx%d' % (cols, rows)
+ name = 'fast_subtract_%dx%d' % (cols, rows)
+ commands.append([
+ generator,
+ '-g',
+ 'subtract_generator',
+ '-o',
+ ruledir,
+ '-f',
+ name,
+ '-e',
+ 'o,h,html',
+ 'target=%s-no_runtime' % target,
+ 'cols=%s' % cols,
+ 'rows=%s' % rows,
+ ])
+ all_header += [
+ '#include "y2020/vision/sift/%s.h"' % name,
+ ]
commands.append([
generator,
- '-g', 'subtract_generator',
- '-o', ruledir,
- '-f', name,
- '-e', 'o,h,html',
- 'target=%s-no_runtime' % target,
- 'cols=%s' % cols,
- 'rows=%s' % rows,
+ '-r',
+ 'fast_gaussian_runtime',
+ '-o',
+ ruledir,
+ '-e',
+ 'o',
+ 'target=%s' % target,
])
+
all_header += [
- '#include "y2020/vision/sift/%s.h"' % name,
+ 'namespace frc971 {',
+ 'namespace vision {',
+ '// 0 is success. 1 is non-implemented size. Negative is a Halide error.',
+ 'inline int DoGeneratedFastGaussian(',
+ ' Halide::Runtime::Buffer<const int16_t, 2> input,',
+ ' Halide::Runtime::Buffer<int16_t, 2> output,',
+ ' double sigma) {',
]
- commands.append([
- generator,
- '-r', 'fast_gaussian_runtime',
- '-o', ruledir,
- '-e', 'o',
- 'target=%s' % target,
- ])
- all_header += [
- 'namespace frc971 {',
- 'namespace vision {',
- '// 0 is success. 1 is non-implemented size. Negative is a Halide error.',
- 'inline int DoGeneratedFastGaussian(',
- ' Halide::Runtime::Buffer<const int16_t, 2> input,',
- ' Halide::Runtime::Buffer<int16_t, 2> output,',
- ' double sigma) {',
- ]
+ for sigma, sigma_name, filter_width in params['sigmas']:
+ for cols, rows in params['sizes']:
+ name = "fast_gaussian_%dx%d_%s" % (cols, rows, sigma_name)
+ all_header += [
+ ' if (input.dim(0).extent() == %s' % cols,
+ ' && input.dim(1).extent() == %s' % rows,
+ ' && sigma == %s) {' % sigma,
+ ' return %s(input, output);' % name,
+ ' }',
+ ]
- for sigma, sigma_name, filter_width in params['sigmas']:
- for cols, rows in params['sizes']:
- name = "fast_gaussian_%dx%d_%s" % (cols, rows, sigma_name)
- all_header += [
- ' if (input.dim(0).extent() == %s' % cols,
- ' && input.dim(1).extent() == %s' % rows,
- ' && sigma == %s) {' % sigma,
- ' return %s(input, output);' % name,
- ' }',
- ]
-
- all_header += [
- ' return 1;',
- '}',
- 'inline int DoGeneratedFastGaussianAndSubtract(',
- ' Halide::Runtime::Buffer<const int16_t, 2> input,',
- ' Halide::Runtime::Buffer<int16_t, 2> blurred,',
- ' Halide::Runtime::Buffer<int16_t, 2> difference,',
- ' double sigma) {',
- ]
-
- for sigma, sigma_name, filter_width in params['sigmas']:
- for cols, rows in params['sizes']:
- name = "fast_gaussian_subtract_%dx%d_%s" % (cols, rows, sigma_name)
- all_header += [
- ' if (input.dim(0).extent() == %s' % cols,
- ' && input.dim(1).extent() == %s' % rows,
- ' && sigma == %s) {' % sigma,
- ' return %s(input, blurred, difference);' % name,
- ' }',
- ]
-
- all_header += [
- ' return 1;',
- '}',
- 'inline int DoGeneratedFastSubtract('
- ' Halide::Runtime::Buffer<const int16_t, 2> input_a,',
- ' Halide::Runtime::Buffer<const int16_t, 2> input_b,',
- ' Halide::Runtime::Buffer<int16_t, 2> output) {',
- ]
- for cols, rows in params['sizes']:
- name = 'fast_subtract_%dx%d' % (cols, rows)
all_header += [
- ' if (input_a.dim(0).extent() == %s' % cols,
- ' && input_a.dim(1).extent() == %s) {' % rows,
- ' return %s(input_a, input_b, output);' % name,
- ' }',
+ ' return 1;',
+ '}',
+ 'inline int DoGeneratedFastGaussianAndSubtract(',
+ ' Halide::Runtime::Buffer<const int16_t, 2> input,',
+ ' Halide::Runtime::Buffer<int16_t, 2> blurred,',
+ ' Halide::Runtime::Buffer<int16_t, 2> difference,',
+ ' double sigma) {',
]
- all_header += [
- ' return 1;',
- '}',
- '} // namespace vision',
- '} // namespace frc971',
- '#endif // Y2020_VISION_SIFT_FAST_GAUSSIAN_ALL_H_',
- ]
- with open(os.path.join(ruledir, 'fast_gaussian_all.h'), 'w') as f:
- f.writelines([line + '\n' for line in all_header])
+ for sigma, sigma_name, filter_width in params['sigmas']:
+ for cols, rows in params['sizes']:
+ name = "fast_gaussian_subtract_%dx%d_%s" % (cols, rows, sigma_name)
+ all_header += [
+ ' if (input.dim(0).extent() == %s' % cols,
+ ' && input.dim(1).extent() == %s' % rows,
+ ' && sigma == %s) {' % sigma,
+ ' return %s(input, blurred, difference);' % name,
+ ' }',
+ ]
- commands_lock = threading.Lock()
- success = [True]
+ all_header += [
+ ' return 1;',
+ '}',
+ 'inline int DoGeneratedFastSubtract('
+ ' Halide::Runtime::Buffer<const int16_t, 2> input_a,',
+ ' Halide::Runtime::Buffer<const int16_t, 2> input_b,',
+ ' Halide::Runtime::Buffer<int16_t, 2> output) {',
+ ]
+ for cols, rows in params['sizes']:
+ name = 'fast_subtract_%dx%d' % (cols, rows)
+ all_header += [
+ ' if (input_a.dim(0).extent() == %s' % cols,
+ ' && input_a.dim(1).extent() == %s) {' % rows,
+ ' return %s(input_a, input_b, output);' % name,
+ ' }',
+ ]
+ all_header += [
+ ' return 1;',
+ '}',
+ '} // namespace vision',
+ '} // namespace frc971',
+ '#endif // Y2020_VISION_SIFT_FAST_GAUSSIAN_ALL_H_',
+ ]
- def run_commands():
- while True:
- with commands_lock:
- if not commands:
- return
- if not success[0]:
- return
- command = commands.pop()
- try:
- subprocess.check_call(command, env=env)
- except:
- with commands_lock:
- success[0] = False
- raise
- threads = [threading.Thread(target=run_commands) for _ in range(4)]
- for thread in threads:
- thread.start()
- for thread in threads:
- thread.join()
- if not success[0]:
- sys.exit(1)
+ with open(os.path.join(ruledir, 'fast_gaussian_all.h'), 'w') as f:
+ f.writelines([line + '\n' for line in all_header])
+
+ commands_lock = threading.Lock()
+ success = [True]
+
+ def run_commands():
+ while True:
+ with commands_lock:
+ if not commands:
+ return
+ if not success[0]:
+ return
+ command = commands.pop()
+ try:
+ subprocess.check_call(command, env=env)
+ except:
+ with commands_lock:
+ success[0] = False
+ raise
+
+ threads = [threading.Thread(target=run_commands) for _ in range(4)]
+ for thread in threads:
+ thread.start()
+ for thread in threads:
+ thread.join()
+ if not success[0]:
+ sys.exit(1)
+
if __name__ == '__main__':
- main(json.loads(sys.argv[1]))
+ main(json.loads(sys.argv[1]))
diff --git a/y2020/vision/tools/python_code/calibrate_intrinsics.py b/y2020/vision/tools/python_code/calibrate_intrinsics.py
index fb95599..46c275c 100644
--- a/y2020/vision/tools/python_code/calibrate_intrinsics.py
+++ b/y2020/vision/tools/python_code/calibrate_intrinsics.py
@@ -10,6 +10,7 @@
# From: https://pynative.com/python-serialize-numpy-ndarray-into-json/
class NumpyArrayEncoder(json.JSONEncoder):
+
def default(self, obj):
if isinstance(obj, np.integer):
return int(obj)
@@ -24,8 +25,8 @@
def get_robot_info(hostname):
hostname_split = hostname.split("-")
if hostname_split[0] != "pi":
- print(
- "ERROR: expected hostname to start with pi! Got '%s'" % hostname)
+ print("ERROR: expected hostname to start with pi! Got '%s'" %
+ hostname)
quit()
team_number = int(hostname_split[1])
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 3e3623d..50e3ea4 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -11,6 +11,7 @@
class CameraIntrinsics:
+
def __init__(self):
self.camera_matrix = []
self.dist_coeffs = []
@@ -19,12 +20,14 @@
class CameraExtrinsics:
+
def __init__(self):
self.R = []
self.T = []
class CameraParameters:
+
def __init__(self):
self.camera_int = CameraIntrinsics()
self.camera_ext = CameraExtrinsics()
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
index 76e73da..0435f95 100644
--- a/y2020/vision/tools/python_code/define_training_data.py
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -28,8 +28,10 @@
image = cv2.circle(image, (point[0], point[1]), 5, (255, 0, 0), -1)
if (len(polygon) > 1):
np_poly = np.array(polygon)
- image = cv2.polylines(
- image, [np_poly], close_polygon, color, thickness=3)
+ image = cv2.polylines(image, [np_poly],
+ close_polygon,
+ color,
+ thickness=3)
return image
@@ -249,7 +251,8 @@
try:
from bazel_tools.tools.python.runfiles import runfiles
r = runfiles.Create()
- ret_name = r.Rlocation('org_frc971/y2020/vision/tools/python_code/' + filename)
+ ret_name = r.Rlocation('org_frc971/y2020/vision/tools/python_code/' +
+ filename)
except:
pass
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index 0266997..a3aea4c 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -22,6 +22,7 @@
class TargetData:
+
def __init__(self, filename=None):
if filename:
self.image_filename = dtd.bazel_name_fix(filename)
diff --git a/y2020/vision/tools/python_code/transform_projection_test.py b/y2020/vision/tools/python_code/transform_projection_test.py
index 01fe695..166b5ec 100644
--- a/y2020/vision/tools/python_code/transform_projection_test.py
+++ b/y2020/vision/tools/python_code/transform_projection_test.py
@@ -23,10 +23,12 @@
# Create fake set of points relative to camera capture (target) frame
# at +/- 1 meter in x, 5 meter depth, and every 1 meter in y from 0 to 4m (above the camera, so negative y values)
-pts_3d_target = np.array(
- [[-1., 0., depth], [1., 0., depth], [-1., -1., depth], [1., -1., depth],
- [-1., -2., depth], [0., -2., depth], [1., -2., depth], [-1., -3., depth],
- [1., -3., depth], [-1., -4., depth], [1., -4., depth]])
+pts_3d_target = np.array([[-1., 0., depth], [1., 0., depth], [-1., -1., depth],
+ [1., -1., depth], [-1., -2.,
+ depth], [0., -2., depth],
+ [1., -2., depth], [-1., -3.,
+ depth], [1., -3., depth],
+ [-1., -4., depth], [1., -4., depth]])
# Ground truth shift of camera from (cam) to (cam2), to compute projections
R_cam_cam2_gt = np.array([[0., 0.2, 0.2]]).T
@@ -46,9 +48,10 @@
#pts_proj_3d = cam_mat.dot(pts_3d_target_shifted.T).T
#pts_proj_2d = np.divide(pts_proj_3d[:,0:2],(pts_proj_3d[:,2].reshape(-1,1)))
-pts_proj_2d_cam2, jac_2d = cv2.projectPoints(
- pts_3d_target, R_cam_cam2_gt_mat_inv, T_cam_cam2_gt_inv, cam_mat,
- dist_coeffs)
+pts_proj_2d_cam2, jac_2d = cv2.projectPoints(pts_3d_target,
+ R_cam_cam2_gt_mat_inv,
+ T_cam_cam2_gt_inv, cam_mat,
+ dist_coeffs)
# Now, solve for the pose using the original 3d points (pts_3d_T_t) and the projections from the new location
retval, R_cam2_cam_est, T_cam2_cam_est, inliers = cv2.solvePnPRansac(
diff --git a/y2021_bot3/control_loops/python/drivetrain.py b/y2021_bot3/control_loops/python/drivetrain.py
index fdd08c2..f3d4682 100644
--- a/y2021_bot3/control_loops/python/drivetrain.py
+++ b/y2021_bot3/control_loops/python/drivetrain.py
@@ -16,10 +16,10 @@
J=6.0,
mass=58.0,
# TODO(austin): Measure radius a bit better.
- robot_radius= 0.39,
- wheel_radius= 3/39.37,
+ robot_radius=0.39,
+ wheel_radius=3 / 39.37,
motor_type=control_loop.Falcon(),
- num_motors = 3,
+ num_motors=3,
G=8.0 / 80.0,
q_pos=0.24,
q_vel=2.5,
@@ -40,7 +40,8 @@
print("Expected .h file name and .cc file name")
else:
# Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2021_bot3', kDrivetrain)
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2021_bot3',
+ kDrivetrain)
if __name__ == '__main__':
diff --git a/y2021_bot3/control_loops/python/polydrivetrain.py b/y2021_bot3/control_loops/python/polydrivetrain.py
index 83576cc..f8c3417 100644
--- a/y2021_bot3/control_loops/python/polydrivetrain.py
+++ b/y2021_bot3/control_loops/python/polydrivetrain.py
@@ -12,20 +12,23 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2021_bot3',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2021_bot3',
+ drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
index 9e9adf7..e984478 100755
--- a/y2022/control_loops/python/catapult.py
+++ b/y2022/control_loops/python/catapult.py
@@ -40,7 +40,6 @@
M_cup = (1750 * 0.0254 * 0.04 * 2 * math.pi * (ball_diameter / 2.)**2.0)
J_cup = M_cup * lever**2.0 + M_cup * (ball_diameter / 2.)**2.0
-
J = (0.0 * J_ball + J_bar + J_cup * 0.0)
JEmpty = (J_bar + J_cup * 0.0)
@@ -83,6 +82,7 @@
# We really want our cost function to be robust so that we can tolerate the
# battery not delivering like we want at the end.
+
def quadratic_cost(catapult, X_initial, X_final, horizon):
Q_final = numpy.matrix([[10000.0, 0.0], [0.0, 10000.0]])
@@ -98,28 +98,32 @@
P_final = 2.0 * Bf.transpose() * Q_final * Bf
q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
- Bf).transpose()
+ Bf).transpose()
constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
Af * X_initial - X_final)
- m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(horizon)])
+ m = numpy.matrix([[catapult.A[1, 1]**(n + 1)] for n in range(horizon)])
M = Bs[1:horizon * 2:2, :]
- W = numpy.matrix(numpy.identity(horizon) - numpy.eye(horizon, horizon, -1)) / catapult.dt
+ W = numpy.matrix(
+ numpy.identity(horizon) -
+ numpy.eye(horizon, horizon, -1)) / catapult.dt
w = -numpy.matrix(numpy.eye(horizon, 1, 0)) / catapult.dt
-
Pi = numpy.diag([
- (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (horizon - n)) / 20.0) ** 2.0 for n in range(horizon)
+ (0.01**2.0) + (0.02 * max(0.0, 20 - (horizon - n)) / 20.0)**2.0
+ for n in range(horizon)
])
P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
- q_accel = 2.0 * (((W * m + w) * X_initial[1, 0]).transpose() * Pi * W * M).transpose()
+ q_accel = 2.0 * ((
+ (W * m + w) * X_initial[1, 0]).transpose() * Pi * W * M).transpose()
constant_accel = ((W * m + w) * X_initial[1, 0]).transpose() * Pi * (
(W * m + w) * X_initial[1, 0])
- return ((P_accel + P_final), (q_accel + q_final), (constant_accel + constant_final))
+ return ((P_accel + P_final), (q_accel + q_final),
+ (constant_accel + constant_final))
def new_cost(catapult, X_initial, X_final, u):
@@ -138,25 +142,28 @@
P_final = 2.0 * Bf.transpose() * Q_final * Bf
q_final = (2.0 * (Af * X_initial - X_final).transpose() * Q_final *
- Bf).transpose()
+ Bf).transpose()
constant_final = (Af * X_initial - X_final).transpose() * Q_final * (
Af * X_initial - X_final)
- m = numpy.matrix([[catapult.A[1, 1] ** (n + 1)] for n in range(len(u))])
+ m = numpy.matrix([[catapult.A[1, 1]**(n + 1)] for n in range(len(u))])
M = Bs[1:len(u) * 2:2, :]
- W = numpy.matrix(numpy.identity(len(u)) - numpy.eye(len(u), len(u), -1)) / catapult.dt
+ W = numpy.matrix(numpy.identity(len(u)) -
+ numpy.eye(len(u), len(u), -1)) / catapult.dt
w = -numpy.matrix(numpy.eye(len(u), 1, 0)) * X_initial[1, 0] / catapult.dt
accel = W * (M * u_matrix + m * X_initial[1, 0]) + w
Pi = numpy.diag([
- (0.01 ** 2.0) + (0.02 * max(0.0, 20 - (len(u) - n)) / 20.0) ** 2.0 for n in range(len(u))
+ (0.01**2.0) + (0.02 * max(0.0, 20 - (len(u) - n)) / 20.0)**2.0
+ for n in range(len(u))
])
P_accel = 2.0 * M.transpose() * W.transpose() * Pi * W * M
- q_accel = 2.0 * ((W * m * X_initial[1, 0] + w).transpose() * Pi * W * M).transpose()
+ q_accel = 2.0 * (
+ (W * m * X_initial[1, 0] + w).transpose() * Pi * W * M).transpose()
constant_accel = (W * m * X_initial[1, 0] +
w).transpose() * Pi * (W * m * X_initial[1, 0] + w)
@@ -205,6 +212,7 @@
def SolveCatapult(catapult, X_initial, X_final, u):
""" Solves for the optimal action given a seed, state, and target """
+
def vbat_constraint(z, i):
return 12.0 - z[i]
@@ -213,13 +221,11 @@
P, q, c = quadratic_cost(catapult, X_initial, X_final, len(u))
-
def mpc_cost2(u_solver):
u_matrix = numpy.matrix(u_solver).transpose()
cost = mpc_cost(catapult, X_initial, X_final, u_solver)
return cost
-
def mpc_cost3(u_solver):
u_matrix = numpy.matrix(u_solver).transpose()
return (0.5 * u_matrix.transpose() * P * u_matrix +
@@ -268,7 +274,6 @@
X_initial = numpy.matrix([[0.0], [0.0]])
X_final = numpy.matrix([[2.0], [25.0]])
-
X_initial = numpy.matrix([[0.0], [0.0]])
X = X_initial.copy()
@@ -382,7 +387,8 @@
)
else:
namespaces = ['y2022', 'control_loops', 'superstructure', 'catapult']
- catapult_lib.WriteCatapult([kCatapultWithBall, kCatapultEmpty], argv[1:3], argv[3:5], namespaces)
+ catapult_lib.WriteCatapult([kCatapultWithBall, kCatapultEmpty],
+ argv[1:3], argv[3:5], namespaces)
return 0
diff --git a/y2022/control_loops/python/catapult_lib.py b/y2022/control_loops/python/catapult_lib.py
index 4a88b14..eceb2d2 100644
--- a/y2022/control_loops/python/catapult_lib.py
+++ b/y2022/control_loops/python/catapult_lib.py
@@ -13,6 +13,7 @@
# TODO(austin): This is mostly the same as angular_system. Can we either wrap an angular_system or assign it?
class Catapult(angular_system.AngularSystem):
+
def __init__(self, params, name="Catapult"):
super(Catapult, self).__init__(params, name)
# Signal that we have a 2 cycle output delay to compensate for in
@@ -23,6 +24,7 @@
class IntegralCatapult(angular_system.IntegralAngularSystem):
+
def __init__(self, params, name="IntegralCatapult"):
super(IntegralCatapult, self).__init__(params, name=name)
# Signal that we have a 2 cycle output delay to compensate for in
diff --git a/y2022/control_loops/python/climber.py b/y2022/control_loops/python/climber.py
index 5f5ae38..12d9cd0 100755
--- a/y2022/control_loops/python/climber.py
+++ b/y2022/control_loops/python/climber.py
@@ -27,12 +27,15 @@
kalman_q_voltage=35.0,
kalman_r_position=0.05)
+
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.2], [0.0]])
linear_system.PlotKick(kClimber, R, plant_params=kClimber)
- linear_system.PlotMotion(
- kClimber, R, max_velocity=5.0, plant_params=kClimber)
+ linear_system.PlotMotion(kClimber,
+ R,
+ max_velocity=5.0,
+ plant_params=kClimber)
# Write the generated constants out to a file.
if len(argv) != 5:
@@ -41,8 +44,8 @@
)
else:
namespaces = ['y2022', 'control_loops', 'superstructure', 'climber']
- linear_system.WriteLinearSystem(kClimber,
- argv[1:3], argv[3:5], namespaces)
+ linear_system.WriteLinearSystem(kClimber, argv[1:3], argv[3:5],
+ namespaces)
if __name__ == '__main__':
diff --git a/y2022/control_loops/python/polydrivetrain.py b/y2022/control_loops/python/polydrivetrain.py
index d749580..cf54470 100644
--- a/y2022/control_loops/python/polydrivetrain.py
+++ b/y2022/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2022',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2022', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index dbb23ad..911418b 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -28,6 +28,7 @@
class CameraIntrinsics:
+
def __init__(self):
self.camera_matrix = []
self.dist_coeffs = []
@@ -36,12 +37,14 @@
class CameraExtrinsics:
+
def __init__(self):
self.R = []
self.T = []
class CameraParameters:
+
def __init__(self):
self.camera_int = CameraIntrinsics()
self.camera_ext = CameraExtrinsics()