Merge "Converted y2016_bot3 to monotonic_clock and fixed tests"
diff --git a/aos/build/queues.bzl b/aos/build/queues.bzl
index 87d5b54..91b4978 100644
--- a/aos/build/queues.bzl
+++ b/aos/build/queues.bzl
@@ -18,10 +18,10 @@
progress_message = 'Generating C++ code for %s' % ctx.file.src.short_path,
)
-def _single_queue_file_outputs(attrs):
+def _single_queue_file_outputs(src):
return {
- 'header': attrs.src.name + '.h',
- 'cc': attrs.src.name + '.cc',
+ 'header': src.name + '.h',
+ 'cc': src.name + '.cc',
}
_single_queue_file = rule(
@@ -30,7 +30,7 @@
'src': attr.label(
mandatory = True,
single_file = True,
- allow_files = FileType(['.q']),
+ allow_files = ['.q'],
),
'q_deps': attr.label(
providers = ['transitive_q_files'],
@@ -42,6 +42,7 @@
'_queue_compiler': attr.label(
executable = True,
default = Label('//aos/build/queues:compiler'),
+ cfg = 'host',
),
},
outputs = _single_queue_file_outputs,
@@ -60,7 +61,7 @@
'srcs': attr.label_list(
mandatory = True,
non_empty = True,
- allow_files = FileType(['.q']),
+ allow_files = ['.q'],
),
'deps': attr.label_list(
mandatory = True,
diff --git a/aos/common/controls/BUILD b/aos/common/controls/BUILD
index 44e6db4..de100bb 100644
--- a/aos/common/controls/BUILD
+++ b/aos/common/controls/BUILD
@@ -31,6 +31,7 @@
'//aos/testing:googletest',
'//aos/testing:test_shm',
],
+ testonly = True,
)
cc_library(
diff --git a/aos/downloader/downloader.bzl b/aos/downloader/downloader.bzl
index e5179c9..0219cf7 100644
--- a/aos/downloader/downloader.bzl
+++ b/aos/downloader/downloader.bzl
@@ -61,7 +61,7 @@
attrs = {
'_downloader': attr.label(
executable = True,
- cfg = HOST_CFG,
+ cfg = 'host',
default = Label('//aos/downloader'),
),
'start_srcs': attr.label_list(
diff --git a/aos/externals/seasocks/gen_embedded.bzl b/aos/externals/seasocks/gen_embedded.bzl
index 4bba776..07a3a33 100644
--- a/aos/externals/seasocks/gen_embedded.bzl
+++ b/aos/externals/seasocks/gen_embedded.bzl
@@ -23,6 +23,7 @@
'_gen_embedded': attr.label(
executable = True,
default = Label('//aos/externals/seasocks:gen_embedded'),
+ cfg = 'host',
),
},
outputs = {
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index 3a11256..609a864 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -1,3 +1,5 @@
+package(default_visibility = ["//visibility:public"])
+
cc_library(
name = 'epoll_events',
srcs = ['epoll_events.cc'],
diff --git a/aos/vision/image/BUILD b/aos/vision/image/BUILD
new file mode 100644
index 0000000..662addc
--- /dev/null
+++ b/aos/vision/image/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_library(
+ name = 'image_types',
+ hdrs = ['image_types.h'],
+ deps = [
+ '//aos/common/logging:logging',
+ ],
+)
+
+cc_library(
+ name = 'reader',
+ srcs = ['reader.cc'],
+ hdrs = ['V4L2.h', 'reader.h'],
+ deps = [
+ '//aos/common:time',
+ '//aos/common/logging:logging',
+ ':image_types',
+ ],
+)
+
+cc_library(
+ name = 'jpeg_routines',
+ srcs = ['jpeg_routines.cc'],
+ hdrs = ['jpeg_routines.h'],
+ deps = [
+ '//third_party/libjpeg',
+ '//aos/common/logging:logging',
+ ':image_types'
+ ],
+)
+
+cc_library(name = 'image_stream',
+ hdrs = ['image_stream.h'],
+ deps = [
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/image:reader'
+ ]
+)
diff --git a/aos/vision/image/V4L2.h b/aos/vision/image/V4L2.h
new file mode 100644
index 0000000..58e5161
--- /dev/null
+++ b/aos/vision/image/V4L2.h
@@ -0,0 +1,24 @@
+#ifndef AOS_LINUX_CODE_CAMREA_V4L2_H_
+#define AOS_LINUX_CODE_CAMREA_V4L2_H_
+
+// This file handles including everything needed to use V4L2 and has some
+// utility functions.
+
+#include <sys/ioctl.h>
+
+#include <asm/types.h> /* for videodev2.h */
+#include <linux/videodev2.h>
+
+namespace camera {
+
+static inline int xioctl(int fd, int request, void *arg) {
+ int r;
+ do {
+ r = ioctl(fd, request, reinterpret_cast<uintptr_t>(arg));
+ } while (r == -1 && errno == EINTR);
+ return r;
+}
+
+} // namespace camera
+
+#endif
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
new file mode 100644
index 0000000..ca2d8be
--- /dev/null
+++ b/aos/vision/image/image_stream.h
@@ -0,0 +1,50 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+#define _AOS_VISION_IMAGE_IMAGE_STREAM_H_
+
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/reader.h"
+
+#include <memory>
+
+namespace aos {
+namespace vision {
+
+class ImageStreamEvent : public ::aos::events::EpollEvent {
+ public:
+ static std::unique_ptr<::camera::Reader> GetCamera(
+ const std::string &fname, ImageStreamEvent *obj,
+ camera::CameraParams params) {
+ using namespace std::placeholders;
+ std::unique_ptr<::camera::Reader> camread(new ::camera::Reader(
+ fname,
+ std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params);
+ camread->StartAsync();
+ return camread;
+ }
+
+ explicit ImageStreamEvent(std::unique_ptr<::camera::Reader> reader)
+ : ::aos::events::EpollEvent(reader->fd()), reader_(reader) {}
+
+ explicit ImageStreamEvent(const std::string &fname,
+ camera::CameraParams params)
+ : ImageStreamEvent(GetCamera(fname, this, params)) {}
+
+ void ProcessHelper(DataRef data, uint64_t timestamp) {
+ if (data.size() < 300) {
+ LOG(INFO, "got bad img: %d of size(%lu)\n", (int)timestamp, data.size());
+ return;
+ }
+ ProcessImage(data, timestamp);
+ }
+ virtual void ProcessImage(DataRef data, uint64_t timestamp) = 0;
+
+ void ReadEvent(Context /*ctx*/) override { reader_->HandleFrame(); }
+
+ private:
+ std::unique_ptr<::camera::Reader> reader_;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_DEBUG_IMAGE_STREAM_H_
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
new file mode 100644
index 0000000..2c9ba56
--- /dev/null
+++ b/aos/vision/image/image_types.h
@@ -0,0 +1,104 @@
+#ifndef _AOS_VISION_IMAGE_IMAGE_TYPES_H_
+#define _AOS_VISION_IMAGE_IMAGE_TYPES_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <memory>
+#include <sstream>
+
+#include <experimental/string_view>
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace vision {
+
+// This will go into c++17. No sense writing my own version.
+using DataRef = std::experimental::string_view;
+
+// Represents the dimensions of an image.
+struct ImageFormat {
+ ImageFormat() : w(0), h(0) {}
+ ImageFormat(int nw, int nh) : w(nw), h(nh) {}
+ std::string ToString() {
+ std::ostringstream s;
+ s << "ImageFormat {" << w << ", " << h << "}";
+ return s.str();
+ }
+ int ImgSize() const { return w * h; }
+ bool Equals(const ImageFormat &other) const {
+ return w == other.w && h == other.h;
+ }
+
+ int w;
+ int h;
+};
+
+// Alias for RGB triple. Should be align 1 size 3.
+struct PixelRef {
+ uint8_t r;
+ uint8_t g;
+ uint8_t b;
+};
+
+// Just to be extra safe.
+static_assert(sizeof(PixelRef) == 3, "Problem with cows in fields!");
+static_assert(alignof(PixelRef) == 1, "Problem with cows in fields!");
+
+// ptr version of a ValueArray. Allows operations on buffers owned by other
+// entities. Prefer this for const-ref versions.
+//
+// Templatized because there was a grayscale version, and cairo buffers are
+// only RGBA.
+template <typename ImageType>
+class Array2dPtr {
+ public:
+ Array2dPtr() : Array2dPtr({0, 0}, nullptr) {}
+ Array2dPtr(ImageFormat fmt, ImageType *data) : fmt_(fmt), data_(data) {}
+ ImageType &get_px(int x, int y) const {
+#ifndef NDEBUG
+ if (x < 0 || x >= fmt_.w || y < 0 || y >= fmt_.h) {
+ LOG(FATAL, "%d, %d out of range [%dx %d]\n", x, y, fmt_.w, fmt_.h);
+ }
+#endif // NBOUNDSCHECK
+ return data_[(x + y * fmt_.w)];
+ }
+ void CopyFrom(const Array2dPtr &other) const {
+ memcpy(data_, other.data_, sizeof(ImageType) * fmt_.ImgSize());
+ }
+
+ const ImageFormat &fmt() const { return fmt_; }
+ ImageType *data() const { return data_; }
+
+ private:
+ ImageFormat fmt_;
+ ImageType *data_;
+};
+
+// unique_ptr version of above.
+template <typename ImageType>
+class ValueArray2d {
+ public:
+ ValueArray2d() : fmt_({0, 0}) {}
+ explicit ValueArray2d(ImageFormat fmt) : fmt_(fmt) {
+ data_.reset(new ImageType[fmt.ImgSize()]);
+ }
+
+ Array2dPtr<ImageType> get() {
+ return Array2dPtr<ImageType>{fmt_, data_.get()};
+ }
+
+ const ImageFormat &fmt() const { return fmt_; }
+ ImageType *data() { return data_.get(); }
+
+ private:
+ ImageFormat fmt_;
+ std::unique_ptr<ImageType[]> data_;
+};
+
+using ImagePtr = Array2dPtr<PixelRef>;
+using ImageValue = ValueArray2d<PixelRef>;
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_IMAGE_TYPES_H_
diff --git a/aos/vision/image/jpeg_routines.cc b/aos/vision/image/jpeg_routines.cc
new file mode 100644
index 0000000..e0c93dc
--- /dev/null
+++ b/aos/vision/image/jpeg_routines.cc
@@ -0,0 +1,249 @@
+#include "aos/vision/image/jpeg_routines.h"
+
+#include <errno.h>
+#include <setjmp.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <unistd.h>
+#include <cstring>
+
+#include "aos/common/logging/logging.h"
+#include "third_party/libjpeg/jpeglib.h"
+
+namespace aos {
+namespace vision {
+
+namespace {
+
+void decompress_add_huff_table(j_decompress_ptr cinfo, JHUFF_TBL **htblptr,
+ const UINT8 *bits, const UINT8 *val);
+
+void standard_huff_tables(j_decompress_ptr cinfo);
+
+// Error handling form libjpeg
+struct JpegErrorManager {
+ /// "public" fields
+ struct jpeg_error_mgr pub;
+ // for return to caller
+ jmp_buf setjmp_buffer;
+};
+
+char JpegLastErrorMsg[JMSG_LENGTH_MAX];
+
+// TODO(parker): Error handling needs to be investigated bettter.
+void JpegErrorExit(j_common_ptr cinfo) {
+ JpegErrorManager myerr;
+ // cinfo->err actually points to a JpegErrorManager struct
+ ::std::memcpy(&myerr, cinfo->err, sizeof(myerr));
+ // JpegErrorManager* myerr = (JpegErrorManager*) cinfo->err;
+ // note : *(cinfo->err) is now equivalent to myerr->pub
+
+ // output_message is a method to print an error message
+ //(* (cinfo->err->output_message) ) (cinfo);
+
+ // Create the message
+ (*(cinfo->err->format_message))(cinfo, JpegLastErrorMsg);
+
+ // Jump to the setjmp point
+ longjmp(myerr.setjmp_buffer, 1);
+}
+
+// This is also adapted from libjpeg to be used on decompression tables rather
+// than compression tables as it was originally intended.
+void decompress_add_huff_table(j_decompress_ptr cinfo, JHUFF_TBL **htblptr,
+ const UINT8 *bits, const UINT8 *val) {
+ if (*htblptr == NULL) *htblptr = jpeg_alloc_huff_table((j_common_ptr)cinfo);
+
+ // Copy the number-of-symbols-of-each-code-length counts.
+ memcpy((*htblptr)->bits, bits, sizeof((*htblptr)->bits));
+
+ // Validate the counts. We do this here mainly so we can copy the right
+ // number of symbols from the val[] array, without risking marching off
+ // the end of memory. jchuff.c will do a more thorough test later.
+ int nsymbols = 0;
+ for (int len = 1; len <= 16; len++) nsymbols += bits[len];
+ if (nsymbols < 1 || nsymbols > 256) {
+ LOG(FATAL, "%s:%d: Error, bad huffman table", __FILE__, __LINE__);
+ }
+
+ memcpy((*htblptr)->huffval, val, nsymbols * sizeof(uint8_t));
+}
+
+// standard_huff_tables is taken from libjpeg compression stuff
+// and is here used to set up the same tables in the decompression structure.
+// Set up the standard Huffman tables (cf. JPEG standard section K.3)
+// IMPORTANT: these are only valid for 8-bit data precision!
+void standard_huff_tables(j_decompress_ptr cinfo) {
+ /* 0-base on first 0, */
+ static const UINT8 bits_dc_luminance[17] = {0, 0, 1, 5, 1, 1, 1, 1, 1,
+ 1, 0, 0, 0, 0, 0, 0, 0};
+ static const UINT8 val_dc_luminance[] = {0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11};
+
+ /* 0-base on first 0 */
+ static const UINT8 bits_dc_chrominance[17] = {0, 0, 3, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 0, 0, 0, 0, 0};
+ static const UINT8 val_dc_chrominance[] = {0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11};
+
+ /* 0-base on first 0 */
+ static const UINT8 bits_ac_luminance[17] = {0, 0, 2, 1, 3, 3, 2, 4, 3,
+ 5, 5, 4, 4, 0, 0, 1, 0x7d};
+ static const UINT8 val_ac_luminance[] = {
+ 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06,
+ 0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
+ 0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72,
+ 0x82, 0x09, 0x0a, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
+ 0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45,
+ 0x46, 0x47, 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
+ 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75,
+ 0x76, 0x77, 0x78, 0x79, 0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
+ 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3,
+ 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
+ 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9,
+ 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
+ 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf1, 0xf2, 0xf3, 0xf4,
+ 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa};
+
+ /* 0-base on first 0 */
+ static const UINT8 bits_ac_chrominance[17] = {0, 0, 2, 1, 2, 4, 4, 3, 4,
+ 7, 5, 4, 4, 0, 1, 2, 0x77};
+ static const UINT8 val_ac_chrominance[] = {
+ 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06, 0x12, 0x41,
+ 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
+ 0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1,
+ 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26,
+ 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44,
+ 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
+ 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74,
+ 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+ 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a,
+ 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
+ 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7,
+ 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
+ 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf2, 0xf3, 0xf4,
+ 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa};
+
+ decompress_add_huff_table(cinfo, &cinfo->dc_huff_tbl_ptrs[0],
+ bits_dc_luminance, val_dc_luminance);
+ decompress_add_huff_table(cinfo, &cinfo->ac_huff_tbl_ptrs[0],
+ bits_ac_luminance, val_ac_luminance);
+ decompress_add_huff_table(cinfo, &cinfo->dc_huff_tbl_ptrs[1],
+ bits_dc_chrominance, val_dc_chrominance);
+ decompress_add_huff_table(cinfo, &cinfo->ac_huff_tbl_ptrs[1],
+ bits_ac_chrominance, val_ac_chrominance);
+}
+
+void local_emit_message(jpeg_common_struct * /*cinfo*/, int /*msg_level*/) {
+ return;
+}
+
+} // namespace
+
+ImageFormat GetFmt(DataRef data) {
+ ImageFormat fmt;
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+
+ cinfo.err = jpeg_std_error(&jerr);
+ jerr.emit_message = local_emit_message;
+
+ cinfo.out_color_space = JCS_RGB;
+ jpeg_create_decompress(&cinfo);
+
+ jpeg_mem_src(&cinfo, reinterpret_cast<unsigned char *>(
+ const_cast<char *>(data.data())),
+ data.size());
+
+ jpeg_read_header(&cinfo, TRUE);
+ fmt.w = cinfo.image_width;
+ fmt.h = cinfo.image_height;
+ jpeg_destroy_decompress(&cinfo);
+ return fmt;
+}
+
+// Returns true if successful false if an error was encountered.
+bool ProcessJpeg(DataRef data, PixelRef *out) {
+ /*
+ TODO(parker): sort of error handling.
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+
+ cinfo.err = jpeg_std_error( &jerr );
+ jerr.emit_message = local_emit_message;
+
+ cinfo.out_color_space = JCS_RGB;
+ jpeg_create_decompress( &cinfo );
+ */
+
+ static bool lost_camera_connect = false;
+ struct jpeg_decompress_struct cinfo;
+
+ // We set up the normal JPEG error routines, then override error_exit.
+ JpegErrorManager jerr;
+ cinfo.err = jpeg_std_error(&jerr.pub);
+ jerr.pub.emit_message = local_emit_message;
+ jerr.pub.error_exit = JpegErrorExit;
+ // Establish the setjmp return context for my_error_exit to use.
+ if (setjmp(jerr.setjmp_buffer)) {
+ // If we get here, the JPEG code has signaled an error.
+ if (!lost_camera_connect) {
+ printf(
+ "Lost camera connection in process_jpeg.\nLooking for reconnect "
+ "...\n");
+ fflush(stdout);
+ lost_camera_connect = true;
+ }
+ jpeg_destroy_decompress(&cinfo);
+ return false;
+ }
+
+ cinfo.out_color_space = JCS_RGB;
+ jpeg_create_decompress(&cinfo);
+
+ jpeg_mem_src(&cinfo, reinterpret_cast<unsigned char *>(
+ const_cast<char *>(data.data())),
+ data.size());
+
+ jpeg_read_header(&cinfo, TRUE);
+ standard_huff_tables(&cinfo);
+
+ /*printf( "JPEG File Information: \n" );
+ printf( "Image width and height: %d pixels and %d pixels.\n",
+ cinfo.image_width, cinfo.image_height );
+ printf( "Color components per pixel: %d.\n", cinfo.num_components );
+ printf( "Color space: %d.\n", cinfo.jpeg_color_space );
+ printf("JpegDecompressed\n");*/
+
+ jpeg_start_decompress(&cinfo);
+
+ int offset = 0;
+ int step = cinfo.num_components * cinfo.image_width;
+ unsigned char *buffers[cinfo.image_height];
+ for (size_t i = 0; i < cinfo.image_height; ++i) {
+ buffers[i] = reinterpret_cast<unsigned char *>(&out[offset]);
+ offset += step;
+ }
+
+ while (cinfo.output_scanline < cinfo.image_height) {
+ jpeg_read_scanlines(&cinfo, &buffers[cinfo.output_scanline],
+ cinfo.image_height - cinfo.output_scanline);
+ }
+
+ jpeg_finish_decompress(&cinfo);
+ jpeg_destroy_decompress(&cinfo);
+
+ if (lost_camera_connect) {
+ printf("Camera connection restablished.\n");
+ fflush(stdout);
+ lost_camera_connect = false;
+ }
+
+ return true;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/image/jpeg_routines.h b/aos/vision/image/jpeg_routines.h
new file mode 100644
index 0000000..7c8fd67
--- /dev/null
+++ b/aos/vision/image/jpeg_routines.h
@@ -0,0 +1,36 @@
+#ifndef _AOS_VISION_IMAGE_JPEGROUTINES_H_
+#define _AOS_VISION_IMAGE_JPEGROUTINES_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+// Returns true if successful false if an error was encountered.
+// Will decompress data into out. Out must be of the right size
+// as determined below.
+bool ProcessJpeg(DataRef data, PixelRef *out);
+
+// Gets the format for the particular jpeg.
+ImageFormat GetFmt(DataRef data);
+
+// Decodes jpeg from data. Will resize if necessary.
+// (Should not be necessary in most normal cases).
+//
+// Consider this the canonical way to decode jpegs if no other
+// choice is given.
+inline bool DecodeJpeg(DataRef data, ImageValue *value) {
+ auto fmt = GetFmt(data);
+ if (!value->fmt().Equals(fmt)) {
+ *value = ImageValue(fmt);
+ }
+ return ProcessJpeg(data, value->data());
+}
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_JPEGROUTINES_H_
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
new file mode 100644
index 0000000..95729da
--- /dev/null
+++ b/aos/vision/image/reader.cc
@@ -0,0 +1,273 @@
+#include "aos/common/time.h"
+
+#include "aos/common/logging/logging.h"
+#include "aos/vision/image/reader.h"
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+namespace camera {
+
+struct Reader::Buffer {
+ void *start;
+ size_t length; // for munmap
+};
+
+Reader::Reader(const std::string &dev_name, ProcessCb process,
+ CameraParams params)
+ : dev_name_(dev_name), process_(std::move(process)), params_(params) {
+ struct stat st;
+ if (stat(dev_name.c_str(), &st) == -1) {
+ PLOG(FATAL, "Cannot identify '%s'", dev_name.c_str());
+ }
+ if (!S_ISCHR(st.st_mode)) {
+ PLOG(FATAL, "%s is no device\n", dev_name.c_str());
+ }
+
+ fd_ = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
+ if (fd_ == -1) {
+ PLOG(FATAL, "Cannot open '%s'", dev_name.c_str());
+ }
+
+ Init();
+}
+
+void Reader::QueueBuffer(v4l2_buffer *buf) {
+ if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
+ PLOG(WARNING,
+ "ioctl VIDIOC_QBUF(%d, %p)."
+ " losing buf #%" PRIu32 "\n",
+ fd_, &buf, buf->index);
+ } else {
+ // LOG(DEBUG, "put buf #%" PRIu32 " into driver's queue\n", buf->index);
+ ++queued_;
+ }
+}
+
+void Reader::HandleFrame() {
+ v4l2_buffer buf;
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
+ if (errno != EAGAIN) {
+ PLOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p)", fd_, &buf);
+ }
+ return;
+ }
+ --queued_;
+
+ // Get a timestamp now as proxy for when the image was taken
+ // TODO(ben): the image should come with a timestamp, parker
+ // will know how to get it.
+ auto time = aos::monotonic_clock::now();
+
+ process_(aos::vision::DataRef(
+ reinterpret_cast<const char *>(buffers_[buf.index].start),
+ buf.bytesused),
+ time);
+ QueueBuffer(&buf);
+}
+
+void Reader::MMapBuffers() {
+ buffers_ = new Buffer[kNumBuffers];
+ v4l2_buffer buf;
+ for (unsigned int n = 0; n < kNumBuffers; ++n) {
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n;
+ if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p)", fd_, &buf);
+ }
+ buffers_[n].length = buf.length;
+ buffers_[n].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
+ MAP_SHARED, fd_, buf.m.offset);
+ if (buffers_[n].start == MAP_FAILED) {
+ PLOG(FATAL,
+ "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)",
+ (size_t)buf.length, fd_, static_cast<intmax_t>(buf.m.offset));
+ }
+ }
+}
+
+void Reader::InitMMap() {
+ v4l2_requestbuffers req;
+ CLEAR(req);
+ req.count = kNumBuffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s does not support memory mapping\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p)\n", fd_, &req);
+ }
+ }
+ queued_ = kNumBuffers;
+ if (req.count < kNumBuffers) {
+ LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name_.c_str());
+ }
+}
+
+// Sets one of the camera's user-control values.
+// Prints the old and new values.
+// Just prints a message if the camera doesn't support this control or value.
+bool Reader::SetCameraControl(uint32_t id, const char *name, int value) {
+ struct v4l2_control getArg = {id, 0U};
+ int r = xioctl(fd_, VIDIOC_G_CTRL, &getArg);
+ if (r == 0) {
+ if (getArg.value == value) {
+ LOG(DEBUG, "Camera control %s was already %d\n", name, getArg.value);
+ return true;
+ }
+ } else if (errno == EINVAL) {
+ LOG(DEBUG, "Camera control %s is invalid\n", name);
+ errno = 0;
+ return false;
+ }
+
+ struct v4l2_control setArg = {id, value};
+ r = xioctl(fd_, VIDIOC_S_CTRL, &setArg);
+ if (r == 0) {
+ LOG(DEBUG, "Set camera control %s from %d to %d\n", name, getArg.value,
+ value);
+ return true;
+ }
+
+ LOG(DEBUG, "Couldn't set camera control %s to %d", name, value);
+ errno = 0;
+ return false;
+}
+
+void Reader::Init() {
+ v4l2_capability cap;
+ if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s is no V4L2 device\n", dev_name_.c_str());
+ } else {
+ PLOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p)", fd_, &cap);
+ }
+ }
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ LOG(FATAL, "%s is no video capture device\n", dev_name_.c_str());
+ }
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ LOG(FATAL, "%s does not support streaming i/o\n", dev_name_.c_str());
+ }
+
+ /* Select video input, video standard and tune here. */
+
+ v4l2_cropcap cropcap;
+ CLEAR(cropcap);
+ cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_CROPCAP, &cropcap) == 0) {
+ v4l2_crop crop;
+ crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ crop.c = cropcap.defrect; /* reset to default */
+
+ if (xioctl(fd_, VIDIOC_S_CROP, &crop) == -1) {
+ switch (errno) {
+ case EINVAL:
+ /* Cropping not supported. */
+ break;
+ default:
+ /* Errors ignored. */
+ PLOG(WARNING, "xioctl VIDIOC_S_CROP");
+ break;
+ }
+ }
+ } else {
+ PLOG(WARNING, "xioctl VIDIOC_CROPCAP");
+ }
+
+ v4l2_format fmt;
+ CLEAR(fmt);
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = params_.width;
+ fmt.fmt.pix.height = params_.height;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+ fmt.fmt.pix.field = V4L2_FIELD_ANY;
+ // fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ // fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n", fd_, &fmt,
+ errno, strerror(errno));
+ }
+ /* Note VIDIOC_S_FMT may change width and height. */
+
+ /* Buggy driver paranoia. */
+ unsigned int min = fmt.fmt.pix.width * 2;
+ if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
+ min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+ if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
+
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
+ V4L2_EXPOSURE_MANUAL)) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
+ "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure)) {
+ LOG(FATAL, "Failed to set exposure\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_BRIGHTNESS, "V4L2_CID_BRIGHTNESS",
+ params_.brightness)) {
+ LOG(FATAL, "Failed to set up camera\n");
+ }
+
+ if (!SetCameraControl(V4L2_CID_GAIN, "V4L2_CID_GAIN", params_.gain)) {
+ LOG(FATAL, "Failed to set up camera\n");
+ }
+
+ // #if 0
+ // set framerate
+ struct v4l2_streamparm *setfps;
+ setfps = (struct v4l2_streamparm *)calloc(1, sizeof(struct v4l2_streamparm));
+ memset(setfps, 0, sizeof(struct v4l2_streamparm));
+ setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ setfps->parm.capture.timeperframe.numerator = 1;
+ setfps->parm.capture.timeperframe.denominator = params_.fps;
+ if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_S_PARM(%d, %p)\n", fd_, setfps);
+ }
+ LOG(INFO, "framerate ended up at %d/%d\n",
+ setfps->parm.capture.timeperframe.numerator,
+ setfps->parm.capture.timeperframe.denominator);
+ // #endif
+
+ InitMMap();
+ LOG(INFO, "Bat Vision Successfully Initialized.\n");
+}
+
+aos::vision::ImageFormat Reader::get_format() {
+ struct v4l2_format fmt;
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_G_FMT, &fmt) == -1) {
+ PLOG(FATAL, "ioctl VIDIC_G_FMT(%d, %p)\n", fd_, &fmt);
+ }
+
+ return aos::vision::ImageFormat{(int)fmt.fmt.pix.width,
+ (int)fmt.fmt.pix.height};
+}
+
+void Reader::Start() {
+ LOG(DEBUG, "queueing buffers for the first time\n");
+ v4l2_buffer buf;
+ for (unsigned int i = 0; i < kNumBuffers; ++i) {
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ QueueBuffer(&buf);
+ }
+ LOG(DEBUG, "done with first queue\n");
+
+ v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
+ PLOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p)\n", fd_, &type);
+ }
+}
+
+} // namespace camera
diff --git a/aos/vision/image/reader.h b/aos/vision/image/reader.h
new file mode 100644
index 0000000..eabb80a
--- /dev/null
+++ b/aos/vision/image/reader.h
@@ -0,0 +1,78 @@
+#ifndef AOS_VISION_IMAGE_READER_H_
+#define AOS_VISION_IMAGE_READER_H_
+#include <errno.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <inttypes.h>
+#include <functional>
+#include <string>
+
+#include "aos/common/time.h"
+#include "aos/vision/image/V4L2.h"
+#include "aos/vision/image/image_types.h"
+
+namespace camera {
+
+struct CameraParams {
+ int32_t width;
+ int32_t height;
+ int32_t exposure;
+ int32_t brightness;
+ int32_t gain;
+ int32_t fps;
+};
+
+class Reader {
+ public:
+ using ProcessCb = std::function<void(
+ aos::vision::DataRef data, aos::monotonic_clock::time_point timestamp)>;
+ Reader(const std::string &dev_name, ProcessCb process, CameraParams params);
+
+ aos::vision::ImageFormat get_format();
+
+ void HandleFrame();
+ void StartAsync() {
+ Start();
+ MMapBuffers();
+ }
+ int fd() { return fd_; }
+
+ private:
+ void QueueBuffer(v4l2_buffer *buf);
+ void InitMMap();
+ bool SetCameraControl(uint32_t id, const char *name, int value);
+ void Init();
+ void Start();
+ void MMapBuffers();
+ // File descriptor of the camera
+ int fd_;
+ // Name of the camera device.
+ std::string dev_name_;
+
+ ProcessCb process_;
+
+ // The number of buffers currently queued in v4l2.
+ uint32_t queued_;
+ struct Buffer;
+ // TODO(parker): This should be a smart pointer, but it cannot
+ // because the buffers are not ummapped.
+ Buffer *buffers_;
+
+ static const unsigned int kNumBuffers = 10;
+
+ // set only at initialize
+ CameraParams params_;
+};
+
+} // namespace camera
+
+#endif // AOS_VISION_IMAGE_READER_H_
diff --git a/compilers/linaro_linux_gcc_4.9.BUILD b/compilers/linaro_linux_gcc_4.9.BUILD
index c1d40b5..52748d5 100644
--- a/compilers/linaro_linux_gcc_4.9.BUILD
+++ b/compilers/linaro_linux_gcc_4.9.BUILD
@@ -91,7 +91,8 @@
'libexec/**',
'lib/gcc/arm-linux-gnueabihf/**',
'include/**',
- ], [
+ ],
+ exclude=[
# Exclude empty files so Bazel's caching works.
# TODO(Brian): remove this once the Bazel bug is fixed.
'**/.install',
diff --git a/doc/make_bazel_package.sh b/doc/make_bazel_package.sh
index a42acfa..ff79ce1 100755
--- a/doc/make_bazel_package.sh
+++ b/doc/make_bazel_package.sh
@@ -15,7 +15,7 @@
"${BAZEL_SOURCE}/compile.sh" compile
(
cd "${BAZEL_SOURCE}"
-./output/bazel build -c opt //scripts/packages:bazel-debian --embed_label="${VERSION}"
+./output/bazel build -c opt //scripts/packages:bazel-debian --embed_label="${VERSION}" --stamp=yes
)
cp "${BAZEL_SOURCE}/bazel-bin/scripts/packages/bazel-debian.deb" "${DEB}"
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index a1b0e68..2075ca7 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -14,6 +14,7 @@
'//aos/common/network:team_number',
'//aos/testing:googletest',
],
+ testonly = True,
)
cc_test(
diff --git a/third_party/allwpilib_2016/BUILD b/third_party/allwpilib_2016/BUILD
index edc42d5..2d789d7 100644
--- a/third_party/allwpilib_2016/BUILD
+++ b/third_party/allwpilib_2016/BUILD
@@ -50,8 +50,8 @@
] + ['wpilibc/shared/include/%s/**/*' % d
for d in _excluded_shared_directories] +
['wpilibc/*/include/%s.h' % d for d in _excluded_devices])
-_h_hdrs = glob([d + '/**/*.h' for d in _header_dirs], _bad_hdrs)
-_hpp_hdrs = glob([d + '/**/*.hpp' for d in _header_dirs], _bad_hdrs)
+_h_hdrs = glob([d + '/**/*.h' for d in _header_dirs], exclude=_bad_hdrs)
+_hpp_hdrs = glob([d + '/**/*.hpp' for d in _header_dirs], exclude=_bad_hdrs)
cc_library(
name = 'wpilib',
@@ -65,8 +65,8 @@
'hal/lib/Athena/cpp/*.cpp',
'hal/lib/Athena/ctre/*.cpp',
'hal/lib/Shared/*.cpp',
- ], (['wpilibc/shared/src/%s/**/*' % d for d in _excluded_shared_directories] +
- ['wpilibc/*/src/%s.cpp' % d for d in _excluded_devices])),
+ ], exclude=(['wpilibc/shared/src/%s/**/*' % d for d in _excluded_shared_directories] +
+ ['wpilibc/*/src/%s.cpp' % d for d in _excluded_devices])),
copts = [
'-Wno-unused-parameter',
'-Wno-switch-enum',
diff --git a/third_party/eigen/BUILD b/third_party/eigen/BUILD
index fd6f889..7012271 100644
--- a/third_party/eigen/BUILD
+++ b/third_party/eigen/BUILD
@@ -5,7 +5,7 @@
visibility = ['//visibility:public'],
srcs = glob(['Eigen/src/**/*.h']),
includes = ['.'],
- hdrs = glob(['Eigen/*'], [
+ hdrs = glob(['Eigen/*'], exclude=[
# Stuff that we don't have the dependencies for.
'Eigen/CholmodSupport',
'Eigen/MetisSupport',
diff --git a/third_party/gflags/BUILD b/third_party/gflags/BUILD
index 0aec151..cffd8fe 100644
--- a/third_party/gflags/BUILD
+++ b/third_party/gflags/BUILD
@@ -31,7 +31,7 @@
srcs = glob([
'src/*.cc',
'src/*.h',
- ], [
+ ], exclude=[
'src/windows_*',
]) + [
'include/gflags/gflags_declare.h',
diff --git a/third_party/libjpeg/BUILD b/third_party/libjpeg/BUILD
index a518300..a139773 100644
--- a/third_party/libjpeg/BUILD
+++ b/third_party/libjpeg/BUILD
@@ -53,7 +53,7 @@
]),
hdrs = glob([
'*.h',
- ], [
+ ], exclude=[
'jmemdos.h',
'jmemmac.h',
]),
diff --git a/third_party/ntcore_2016/BUILD b/third_party/ntcore_2016/BUILD
index 7e79831..acddc7f 100644
--- a/third_party/ntcore_2016/BUILD
+++ b/third_party/ntcore_2016/BUILD
@@ -6,7 +6,7 @@
srcs = glob([
'src/**/*.cpp',
'src/**/*.h',
- ], [
+ ], exclude=[
'src/networktables/**',
]),
copts = [
diff --git a/third_party/protobuf/protobuf.bzl b/third_party/protobuf/protobuf.bzl
index d91cde4..cfcb69a 100644
--- a/third_party/protobuf/protobuf.bzl
+++ b/third_party/protobuf/protobuf.bzl
@@ -90,7 +90,7 @@
"deps": attr.label_list(providers = ["proto"]),
"includes": attr.string_list(),
"protoc": attr.label(
- cfg = HOST_CFG,
+ cfg = 'host',
executable = True,
single_file = True,
mandatory = True,
diff --git a/third_party/seasocks/BUILD b/third_party/seasocks/BUILD
index d85e3c3..0daf8ea 100644
--- a/third_party/seasocks/BUILD
+++ b/third_party/seasocks/BUILD
@@ -6,7 +6,7 @@
srcs = glob(['src/main/c/**/*.cpp']),
hdrs = glob([
'src/main/c/**/*.h',
- ], [
+ ], exclude=[
'src/main/internal/**/*',
]),
includes = ['src/main/c'],
diff --git a/tools/bazel b/tools/bazel
index 415aeff..f6f4c52 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -24,7 +24,7 @@
exec "${BAZEL_OVERRIDE}" "$@"
fi
-readonly VERSION="201607070016+7a0d360"
+readonly VERSION="201701021712+4964f2c"
readonly DOWNLOAD_DIR="$(dirname "${BASH_SOURCE[0]}")/../bazel-downloads"
# Directory to unpack bazel into. This must change whenever bazel changes.
diff --git a/tools/bazel.rc b/tools/bazel.rc
index 7df645e..7530102 100644
--- a/tools/bazel.rc
+++ b/tools/bazel.rc
@@ -46,3 +46,5 @@
build --show_result 5
# Dump the output of the failing test to stdout.
test --test_output=errors
+
+build --experimental_sandbox_shm
diff --git a/tools/build_rules/fortran.bzl b/tools/build_rules/fortran.bzl
index f8d0add..2d6e4db 100644
--- a/tools/build_rules/fortran.bzl
+++ b/tools/build_rules/fortran.bzl
@@ -36,11 +36,11 @@
progress_message = 'Building %s' % ctx.outputs.pic_o.short_path,
)
-def _define_fortran_output(attrs):
- if not attrs.src.name.endswith('.f'):
+def _define_fortran_output(src):
+ if not src.name.endswith('.f'):
fail('Fortran files must end in \'.f\'', 'src')
- fortran_file_base = attrs.src.name[:-2]
+ fortran_file_base = src.name[:-2]
return {
'pic_o': fortran_file_base + '.pic.o',
}
diff --git a/tools/build_rules/protobuf.bzl b/tools/build_rules/protobuf.bzl
index fe6af4f..d5f4284 100644
--- a/tools/build_rules/protobuf.bzl
+++ b/tools/build_rules/protobuf.bzl
@@ -19,8 +19,8 @@
],
)
-def _do_proto_cc_library_outputs(attr):
- basename = attr.src.name[:-len('.proto')]
+def _do_proto_cc_library_outputs(src):
+ basename = src.name[:-len('.proto')]
return {
'pb_h': '%s.pb.h' % basename,
'pb_cc': '%s.pb.cc' % basename,
@@ -37,7 +37,7 @@
'_protoc': attr.label(
default = Label('//third_party/protobuf:protoc'),
executable = True,
- cfg = HOST_CFG,
+ cfg = 'host',
),
'_well_known_protos': attr.label(
default = Label('//third_party/protobuf:well_known_protos'),
diff --git a/tools/build_rules/ruby.bzl b/tools/build_rules/ruby.bzl
index 7c02745..2c93596 100644
--- a/tools/build_rules/ruby.bzl
+++ b/tools/build_rules/ruby.bzl
@@ -51,7 +51,7 @@
),
'data': attr.label_list(
allow_files = True,
- cfg = DATA_CFG,
+ cfg = 'data',
),
}
@@ -84,6 +84,7 @@
attrs = _ruby_attrs + {
'_ruby_linker': attr.label(
executable = True,
+ cfg = 'host',
default = Label('//tools/ruby:standalone_ruby'),
)
},
diff --git a/tools/cpp/BUILD b/tools/cpp/BUILD
index 40e674f..053b0e4 100644
--- a/tools/cpp/BUILD
+++ b/tools/cpp/BUILD
@@ -23,27 +23,13 @@
srcs = [],
)
-# This is the entry point for --crosstool_top. Toolchains are found
-# by lopping off the name of --crosstool_top and searching for
-# 'cc-compiler-${CPU}' in this BUILD file, where CPU is the target CPU
-# specified in --cpu.
-#
-# This file group should include
-# * all cc_toolchain targets supported
-# * all file groups that said cc_toolchain might refer to,
-# including the default_grte_top setting in the CROSSTOOL
-# protobuf.
-filegroup(
+# This is the entry point for --crosstool_top.
+cc_toolchain_suite(
name = 'toolchain',
- srcs = [
- ':cc-compiler-k8',
- ':cc-compiler-roborio',
- '@arm_frc_linux_gnueabi_repo//:compiler_components',
- ':roborio-compiler-files',
- ':flags_compiler_inputs',
- '@linaro_linux_gcc_4_9_repo//:compiler_components',
- ':linaro-gcc-files',
- ],
+ toolchains = {
+ 'k8|clang': ':cc-compiler-k8',
+ 'roborio|gcc': ':cc-compiler-roborio',
+ },
)
# Compiler inputs given by --copt etc in //tools:bazel.rc.
@@ -90,6 +76,7 @@
'//tools/cpp/arm-frc-linux-gnueabi:ld',
'//tools/cpp/arm-frc-linux-gnueabi:ar',
'//tools/cpp/arm-frc-linux-gnueabi:gcc',
+ '//tools/cpp/arm-frc-linux-gnueabi:libs',
'@arm_frc_linux_gnueabi_repo//:compiler_pieces',
],
)
@@ -98,6 +85,8 @@
srcs = [
'//tools/cpp/arm-frc-linux-gnueabi:gcc',
'//tools/cpp/arm-frc-linux-gnueabi:ld',
+ '@arm_frc_linux_gnueabi_repo//:compiler_components',
+ '@arm_frc_linux_gnueabi_repo//:compiler_pieces',
],
)
diff --git a/tools/cpp/CROSSTOOL b/tools/cpp/CROSSTOOL
index 96fb1b9..5a374af 100644
--- a/tools/cpp/CROSSTOOL
+++ b/tools/cpp/CROSSTOOL
@@ -351,7 +351,7 @@
cxx_builtin_include_directory: "%package(@arm_frc_linux_gnueabi_repo//usr/arm-frc-linux-gnueabi/usr/include)%"
linker_flag: "-lstdc++"
- #linker_flag: "-B/usr/bin/"
+ linker_flag: "-Ltools/cpp/arm-frc-linux-gnueabi/libs"
feature {
name: "opt"
diff --git a/tools/cpp/arm-frc-linux-gnueabi/BUILD b/tools/cpp/arm-frc-linux-gnueabi/BUILD
index 17a10c2..95000c3 100644
--- a/tools/cpp/arm-frc-linux-gnueabi/BUILD
+++ b/tools/cpp/arm-frc-linux-gnueabi/BUILD
@@ -77,3 +77,10 @@
':strip',
],
)
+
+filegroup(
+ name = 'libs',
+ srcs = glob([
+ 'libs/**',
+ ]),
+)
diff --git a/y2014/control_loops/python/BUILD b/y2014/control_loops/python/BUILD
index 84e73a5..3802cb3 100644
--- a/y2014/control_loops/python/BUILD
+++ b/y2014/control_loops/python/BUILD
@@ -62,3 +62,15 @@
'//frc971/control_loops/python:controls',
]
)
+
+py_binary(
+ name = 'extended_lqr',
+ srcs = [
+ 'extended_lqr.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
diff --git a/y2014/control_loops/python/extended_lqr.py b/y2014/control_loops/python/extended_lqr.py
new file mode 100755
index 0000000..095a43a
--- /dev/null
+++ b/y2014/control_loops/python/extended_lqr.py
@@ -0,0 +1,437 @@
+#!/usr/bin/python
+
+# This is an initial, hacky implementation of the extended LQR paper. It's just a proof of concept,
+# so don't trust it too much.
+
+import numpy
+import scipy.optimize
+from matplotlib import pylab
+import sys
+
+from frc971.control_loops.python import controls
+
+l = 100
+width = 0.2
+dt = 0.05
+num_states = 3
+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.
+
+ Args:
+ X, numpy.matrix(3, 1), The state. [x, y, theta]
+ U, numpy.matrix(2, 1), The input. [left velocity, right velocity]
+
+ 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]])
+
+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
+
+def discrete_dynamics(X, U):
+ return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+
+def inverse_discrete_dynamics(X, U):
+ 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.
+
+ Args:
+ fn: A function of X, U.
+ X: numpy.matrix(num_states, 1), The state vector to take the jacobian
+ around.
+ U: numpy.matrix(num_inputs, 1), The input vector to take the jacobian
+ around.
+
+ Returns:
+ 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
+
+def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
+ """Numerically estimates the jacobian around X, U in U.
+
+ Args:
+ fn: A function of X, U.
+ X: numpy.matrix(num_states, 1), The state vector to take the jacobian
+ around.
+ U: numpy.matrix(num_inputs, 1), The input vector to take the jacobian
+ around.
+
+ Returns:
+ 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
+
+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)
+
+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)
+
+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)
+
+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)
+
+# 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.
+
+ This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
+
+ Args:
+ X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
+
+ 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)
+
+ 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.
+
+ 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
+
+ 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
+
+ Args:
+ X: numpy.matrix(self.num_states, 1)
+ U: numpy.matrix(self.num_inputs, 1), ignored
+
+ Returns:
+ numpy.matrix(1, 1), The quadratic cost of being at X
+ """
+ return X.T * self.Q * X * 1000
+
+ def cost(self, X, U):
+ """Computes the incremental cost given a position and U.
+
+ Args:
+ X: numpy.matrix(self.num_states, 1)
+ U: numpy.matrix(self.num_inputs, 1)
+
+ Returns:
+ numpy.matrix(1, 1), The quadratic cost of evaluating U.
+ """
+ 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, 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, 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, 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, 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)]
+
+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]])
+
+ 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(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
+
+ 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_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)
+
+ 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]
+
+ 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
+
+ #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])
+
+ 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 '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 ('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
+
+ S_bar_t[1] = S_bar_stiff
+
+ 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]
+
+ 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 ('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(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
+
+ 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
+
+ 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]))
+
+ 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]
+
+ 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 ')
+
+ 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_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]
+
+ 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]
+
+ 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()
+
+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]
+
+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)
+
+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()
+
+sys.exit(1)
diff --git a/y2016/dashboard/BUILD b/y2016/dashboard/BUILD
index 65e650c..fd2fdbb 100644
--- a/y2016/dashboard/BUILD
+++ b/y2016/dashboard/BUILD
@@ -3,7 +3,7 @@
gen_embedded(
name = 'gen_embedded',
- srcs = glob(['www_defaults/**/*'], ['www/**/*']),
+ srcs = glob(['www_defaults/**/*'], exclude=['www/**/*']),
)
aos_downloader_dir(