Merge "Delete receive_socket, send_socket, and socket"
diff --git a/WORKSPACE b/WORKSPACE
index 725df52..5bd89d2 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -250,3 +250,10 @@
strip_prefix = "gcc-arm-none-eabi-7-2018-q2-update/",
url = "http://frc971.org/Build-Dependencies/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2",
)
+
+new_http_archive(
+ name = "cgal_repo",
+ sha256 = "d564dda558570344b4caa66c5bae2cdae9ef68e07829d64f5651b25f2c6a0e9e",
+ url = "http://frc971.org/Build-Dependencies/cgal-dev-4.5-2.tar.gz",
+ build_file = 'debian/cgal.BUILD',
+)
diff --git a/aos/config/setup_roborio.sh b/aos/config/setup_roborio.sh
index c0112f6..474a969 100755
--- a/aos/config/setup_roborio.sh
+++ b/aos/config/setup_roborio.sh
@@ -1,4 +1,7 @@
#!/bin/bash
+#
+# Note: this should be run from within bazel
+
set -Eeuo pipefail
if [ $# != 1 ];
@@ -17,12 +20,10 @@
echo "Looking to see if l is aliased right."
-readonly HAS_ALIAS=$(ssh "admin@${ROBOT_HOSTNAME}" "cat /etc/profile" | grep -Fq "alias l")
-
-if [[ $? -ne 0 ]]; then
+if ! HAS_ALIAS=$(ssh "admin@${ROBOT_HOSTNAME}" "cat /etc/profile"); then
echo "ssh command failed remotely"
exit 1
-elif $HAS_ALIAS
+elif echo "${HAS_ALIAS}" | grep -Fq "alias l"; then
echo "Already has l alias"
else
echo "Adding l alias"
@@ -31,11 +32,6 @@
ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1 logs
fi
-# Make sure starter.sh has the correct permissions to run the robot code.
-# If missing o+rx, the robot code will not start. No error messages on
-# some driver stations.
-ssh "admin@${ROBOT_HOSTNAME}" 'chmod go+rx robot_code/starter.sh'
-
ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t'
echo "Deploying robotCommand startup script"
diff --git a/debian/cgal.BUILD b/debian/cgal.BUILD
new file mode 100644
index 0000000..0063d73
--- /dev/null
+++ b/debian/cgal.BUILD
@@ -0,0 +1,18 @@
+
+
+cc_library(
+ name = 'cgal',
+ hdrs = glob([
+ 'usr/include/**/*.h',
+ 'usr/include/**/*.hpp',
+ ]),
+ includes = [
+ 'usr/include',
+ 'usr/include/x86_64-linux-gnu',
+ ],
+ srcs = [
+ 'usr/lib/x86_64-linux-gnu/libgmp.so.10.2.0',
+ 'usr/lib/libCGAL.so.10',
+ ],
+ visibility = ['//visibility:public'],
+)
diff --git a/debian/pandoc.BUILD b/debian/pandoc.BUILD
index 238c786..0dfbd9f 100644
--- a/debian/pandoc.BUILD
+++ b/debian/pandoc.BUILD
@@ -5,7 +5,7 @@
"cat > $@ <<END",
"#!/bin/bash",
"export LD_LIBRARY_PATH=external/pandoc/usr/lib/x86_64-linux-gnu:external/pandoc/lib/x86_64-linux-gnu",
- "exec external/pandoc/usr/bin/pandoc \"\\$$@\"",
+ "exec external/pandoc/usr/bin/pandoc --data-dir=external/pandoc/usr/share/pandoc/data \"\\$$@\"",
"END",
]),
executable = True,
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 55c4267..7505413 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -123,7 +123,7 @@
return output
-def dlqr(A, B, Q, R):
+def dlqr(A, B, Q, R, optimal_cost_function=False):
"""Solves for the optimal lqr controller.
x(n+1) = A * x(n) + B * u(n)
@@ -131,12 +131,16 @@
"""
# P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
+ # 0.5 * X.T * P * X -> optimal cost to infinity
P, rcond, w, S, T = slycot.sb02od(
n=A.shape[0], m=B.shape[1], A=A, B=B, Q=Q, R=R, dico='D')
- F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
- return F
+ F = numpy.linalg.inv(R + B.T * P * B) * B.T * P * A
+ if optimal_cost_function:
+ return F, P
+ else:
+ return F
def kalman(A, B, C, Q, R):
"""Solves for the steady state kalman gain and covariance matricies.
diff --git a/motors/NOTES.md b/motors/NOTES.md
index 37ba894..995e97f 100644
--- a/motors/NOTES.md
+++ b/motors/NOTES.md
@@ -1,28 +1,34 @@
* Teensy 3.5 has a MK64FX512VMD12 (Freescale Kinetis K64 sub-family)
* It's a Cortex-M4F, which means it has an FPU and DSP instructions.
* 512 KB of flash
- * 192 kB SRAM (64 kB SRAM\_L, 128 kB SRAM\_U)
+ * 192 KB SRAM (64 KB SRAM\_L, 128 KB SRAM\_U)
* Up to 120 MHz
-* [datasheet](http://cache.freescale.com/files/microcontrollers/doc/data_sheet/K64P144M120SF5.pdf)
-* [errata](https://www.nxp.com/docs/en/errata/Kinetis_K_1N83J.pdf)
- * TODO(Brian): Are all of our parts this revision?
-* [reference manual](http://cache.nxp.com/assets/documents/data/en/reference-manuals/K64P144M120SF5RM.pdf)
-* [schematic](https://www.pjrc.com/teensy/schematic.html).
+ * [datasheet](http://cache.freescale.com/files/microcontrollers/doc/data_sheet/K64P144M120SF5.pdf)
+ * [errata](https://www.nxp.com/docs/en/errata/Kinetis_K_1N83J.pdf)
+ * TODO(Brian): Are all of our parts this revision?
+ * [K64 reference manual](http://cache.nxp.com/assets/documents/data/en/reference-manuals/K64P144M120SF5RM.pdf)
+ * [schematic](https://www.pjrc.com/teensy/schematic.html).
* [actual docs on the bit banding](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0439b/Behcjiic.html)
* [ARM Cortex-M Programming Guide to Memory Barrier Instructions](https://static.docs.arm.com/dai0321/a/DAI0321A_programming_guide_memory_barriers_for_m_profile.pdf)
* [Cortex-M4 instruction timings](http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0439b/CHDDIGAC.html)
-* RM08, 5v, 2048 edges per revolution, single ended incremental, with index pulse
+* [Optimizing Performance on Kinetis K-series MCUs](https://www.nxp.com/docs/en/application-note/AN4745.pdf)
* [motor](https://hobbyking.com/en_us/turnigy-aquastar-t20-3t-730kv-1280kv-water-cooled-brushless-motor.html)
* 2 pole pairs per revolution
-
-* Change MOSFET gate resistors to 1 ohm (or something else?)
-* Reshuffle input capacitor connections
- * Thermals?
- * Farther apart
- * Other side?
-* More capacitors on battery power
- * Like across individual half bridges
-* No paste on wire pads
+* [RM08](https://www.rls.si/eng/rm08-super-small-non-contact-rotary-encoder), 5v, 2048 edges per revolution, single ended incremental, with index pulse
+* fet12v2 has a MK22FN1M0AVLK12
+ * It's a Cortex-M4F, which means it has an FPU and DSP instructions.
+ * 1 MB of flash
+ * 128 KB SRAM (64 KB SRAM\_L, 64 KB SRAM\_U)
+ * Alternative part is MK22FX512AVLK12 with 512 KB of flash
+ * [datasheet](https://www.nxp.com/docs/en/data-sheet/K22P80M120SF5V2.pdf)
+ * [reference manual](https://www.nxp.com/docs/en/reference-manual/K22P80M120SF5V2RM.pdf)
+* fet12v2accessory has a MK22FN1M0AVLH12
+ * It's a Cortex-M4F, which means it has an FPU and DSP instructions.
+ * 1 MB of flash
+ * 128 KB SRAM (64 KB SRAM\_L, 64 KB SRAM\_U)
+ * Alternative part is MK22FX512AVLH12 with 512 KB of flash
+ * [datasheet](https://www.nxp.com/docs/en/data-sheet/K22P64M120SF5V2.pdf)
+ * [reference manual](https://www.nxp.com/docs/en/reference-manual/K22P64M120SF5V2RM.pdf)
### Clocking
* Running the core clock at its maximum of 120 MHz
@@ -33,49 +39,11 @@
to keep everything in sync based on the FTM module(s) which drive the motor
outputs. They trigger the ADCs too.
-FTM0 and FTM3 are synced using the global time base.
+FTM0 and FTM3 are synced using the global time base when driving two motors
+for the pistol grip controller.
The timing is divided into "cycles". Each cycle is a fixed length of time.
The start of the cycle is when the FTM module(s) have a count of 0.
-TODO(Brian): Update the rest of this.
-
-Both PDBs are used to trigger ADC samples, in conjunction with DMA to
-automatically setup the correct sequence and record the results. This gives
-tight control over the timing (DMA can take a bit to get there, and interrupts
-would be worse) and doesn't require CPU time besides reading the result at the
-end.
-The PDB is triggered by the initialization external trigger of an FTM.
-From there, DMA triggered by the ADC conversion complete event finishes and
-prepares for the next round.
-PDB triggers of ADCs alternate between channels so DMA can read+update the other
-one in the mean time.
-One DMA channel copies the result out, and then links to another channel which
-reconfigures that ADC conversion setup register to prepare for the next time.
-This doesn't allow skipping ADC conversions, but conversion results can always
-be ignored later.
-This also doesn't allow scheduling ADC conversions at any points besides at
-fixed multiples from thes start of the cycle, but that should be fine.
-The ADC setup should stop converting by the end of the cycle so the new PDB
-trigger will restart everything, and both DMA channels should reset themselves
-after two cycles (so that the code can read ADC results in one area while
-they're being written to the other).
-
-Both PDBs are used to trigger ADC samples, in conjunction with DMA to
-automatically setup the correct sequence and record the results. This gives
-tight control over the timing (DMA can take a bit to get there, and interrupts
-would be worse) and doesn't require CPU time besides reading the result at the
-end.
-The PDB is triggered by the initialization external trigger of an FTM.
-From there, DMA triggered by two unused channels of the FTMs alternates between
-copying results and reconfiguring each of the sets of ADC registers.
-The DMA uses the SMOD functionality to get the corresponding registers of both
-ADCs in one go, which means using a total of 4 DMA channels (1 to configure the
-A registers, 1 to configure the B registers, 1 to copy the A results, and 1 to
-copy the B results).
-
-Currently, ADC sampling all happens at the beginning of a cycle, then the
-computation of new output values happens, then these values need to be
-written to the hardware before the next cycle starts.
-ADC sampling may move to DMA-based at some point to increase flexibility and
-reduce CPU time spent on it.
+See `//motors/peripheral:adc_dma.cc` for details on how hardware-triggered ADC
+sampling works.
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
index 0eed7ef..a637a55 100644
--- a/motors/fet12/BUILD
+++ b/motors/fet12/BUILD
@@ -33,6 +33,7 @@
"//motors:util",
"//motors/core",
"//motors/peripheral:adc",
+ "//motors/peripheral:adc_dma",
"//motors/peripheral:can",
"//motors/peripheral:uart",
],
@@ -80,20 +81,37 @@
],
)
-py_binary(
- name = "current_equalize",
+py_library(
+ name = "calib_sensors",
srcs = [
- "current_equalize.py",
+ "calib_sensors.py",
],
data = [
"@python_repo//:scipy",
],
)
+py_binary(
+ name = "current_equalize",
+ srcs = [
+ "current_equalize.py",
+ ],
+ data = [
+ ":calib_sensors",
+ "@python_repo//:scipy",
+ ],
+)
+
genrule(
name = "gen_current_equalization",
+ srcs = [
+ "calib_data_6030.csv",
+ "calib_data_60a.csv",
+ "calib_data_60b.csv",
+ "calib_data_60c.csv",
+ ],
outs = ["current_equalization.h"],
- cmd = "./$(location current_equalize) > \"$@\"",
+ cmd = "./$(location current_equalize) $(location calib_data_60a.csv) $(location calib_data_60b.csv) $(location calib_data_60c.csv) $(location calib_data_6030.csv) > \"$@\"",
restricted_to = mcu_cpus,
tools = ["current_equalize"],
)
diff --git a/motors/fet12/calib_data_6030.csv b/motors/fet12/calib_data_6030.csv
new file mode 100644
index 0000000..eb6105e
--- /dev/null
+++ b/motors/fet12/calib_data_6030.csv
@@ -0,0 +1,124 @@
+60,30,0, 75,73,-80, -154,-80,-151
+60,30,0, 74,72,-88, -160,-88,-156
+60,30,0, 67,64,-93, -157,-96,-156
+60,30,0, 72,68,-84, -154,-86,-150
+60,30,0, 70,69,-89, -156,-88,-153
+60,30,0, 90,94,-65, -155,-64,-153
+60,30,0, 81,79,-80, -157,-78,-156
+60,30,0, 66,65,-94, -156,-93,-159
+60,30,0, 70,73,-81, -154,-83,-151
+60,30,0, 75,77,-87, -157,-85,-159
+60,30,0, 83,81,-75, -156,-77,-157
+60,30,0, 71,72,-89, -155,-89,-154
+60,30,0, 88,91,-72, -155,-68,-161
+60,30,0, 71,67,-90, -158,-91,-155
+60,30,0, 71,74,-84, -153,-81,-151
+60,30,0, 85,84,-81, -158,-76,-161
+60,30,0, 88,88,-68, -153,-68,-154
+60,30,0, 74,75,-83, -152,-78,-153
+60,30,0, 87,89,-74, -162,-75,-159
+60,30,0, 77,77,-81, -159,-81,-155
+60,30,0, 65,69,-84, -154,-86,-154
+60,30,0, 75,78,-78, -151,-76,-151
+60,30,0, 69,68,-89, -153,-86,-156
+60,30,0, 94,97,-66, -161,-66,-161
+60,30,0, 82,81,-75, -157,-78,-153
+60,30,0, 82,84,-79, -156,-76,-162
+60,30,0, 78,73,-87, -161,-88,-154
+60,30,0, 69,68,-90, -153,-90,-151
+60,30,0, 76,73,-83, -154,-79,-154
+60,30,0, 73,74,-79, -149,-81,-151
+60,30,0, 83,83,-75, -155,-77,-157
+60,30,0, 82,84,-75, -161,-79,-155
+60,30,0, 80,83,-78, -156,-79,-157
+60,30,0, 66,65,-91, -159,-94,-155
+60,30,0, 78,78,-82, -157,-82,-154
+60,30,0, 84,84,-76, -161,-79,-154
+60,30,0, 94,96,-65, -154,-63,-154
+60,30,0, 73,76,-79, -157,-83,-155
+60,30,0, 79,79,-82, -156,-81,-156
+60,30,0, 67,67,-86, -154,-86,-151
+60,30,0, 72,72,-86, -150,-84,-154
+60,30,0, 75,79,-80, -154,-81,-154
+60,30,0, 66,66,-88, -152,-89,-152
+60,30,0, 76,74,-85, -157,-85,-156
+60,30,0, 78,81,-78, -158,-81,-157
+60,30,0, 83,82,-71, -151,-69,-151
+60,30,0, 69,72,-83, -151,-82,-156
+60,30,0, 64,71,-82, -146,-82,-148
+60,30,0, 82,80,-78, -154,-77,-156
+ 0,60,30, -127,-129,-58, 78,-54,76
+ 0,60,30, -112,-112,-45, 69,-42,74
+ 0,60,30, -120,-120,-47, 75,-45,76
+ 0,60,30, -112,-115,-47, 74,-42,74
+ 0,60,30, -127,-126,-51, 77,-50,77
+ 0,60,30, -121,-120,-46, 75,-46,77
+ 0,60,30, -128,-128,-55, 76,-52,78
+ 0,60,30, -119,-122,-53, 71,-48,75
+ 0,60,30, -124,-121,-52, 74,-49,74
+ 0,60,30, -122,-120,-52, 75,-47,74
+ 0,60,30, -127,-126,-54, 73,-54,77
+ 0,60,30, -123,-122,-56, 74,-52,70
+ 0,60,30, -121,-121,-50, 72,-49,75
+ 0,60,30, -121,-122,-56, 72,-51,69
+ 0,60,30, -120,-115,-48, 73,-46,71
+ 0,60,30, -115,-113,-37, 76,-40,77
+ 0,60,30, -118,-115,-53, 73,-45,70
+ 0,60,30, -129,-126,-48, 83,-46,80
+ 0,60,30, -129,-129,-55, 76,-52,76
+ 0,60,30, -123,-119,-52, 82,-45,71
+ 0,60,30, -119,-120,-50, 76,-45,78
+ 0,60,30, -120,-118,-46, 77,-42,76
+ 0,60,30, -104,-105,-30, 77,-28,79
+ 0,60,30, -114,-112,-44, 76,-40,73
+ 0,60,30, -127,-128,-57, 76,-54,80
+ 0,60,30, -119,-118,-51, 71,-48,74
+ 0,60,30, -113,-113,-36, 79,-34,80
+ 0,60,30, -130,-126,-54, 79,-49,80
+ 0,60,30, -125,-120,-56, 78,-50,71
+ 0,60,30, -134,-136,-60, 76,-60,81
+ 0,60,30, -122,-121,-51, 74,-49,73
+ 0,60,30, -104,-106,-32, 77,-29,77
+ 0,60,30, -118,-117,-46, 72,-47,76
+ 0,60,30, -128,-128,-59, 77,-53,70
+ 0,60,30, -125,-126,-55, 75,-50,76
+ 0,60,30, -127,-126,-47, 78,-47,84
+ 0,60,30, -126,-126,-56, 72,-55,76
+ 0,60,30, -119,-118,-49, 75,-44,74
+ 0,60,30, -121,-119,-49, 76,-43,74
+ 0,60,30, -130,-131,-61, 79,-56,77
+ 0,60,30, -122,-120,-55, 70,-52,73
+ 0,60,30, -121,-121,-50, 77,-46,77
+ 0,60,30, -124,-125,-52, 74,-50,75
+ 0,60,30, -126,-125,-55, 77,-51,75
+ 0,60,30, -113,-113,-40, 77,-37,78
+ 0,60,30, -127,-126,-52, 78,-51,78
+ 0,60,30, -120,-116,-44, 78,-43,77
+ 0,60,30, -127,-126,-54, 74,-53,73
+ 0,60,30, -127,-126,-56, 75,-52,77
+ 0,60,30, -120,-118,-50, 72,-48,71
+ 0,60,30, -129,-128,-54, 76,-52,79
+ 0,60,30, -122,-117,-48, 77,-44,73
+ 30,0,60, 64,65,139, 80,140,75
+ 30,0,60, 50,50,126, 78,127,77
+ 30,0,60, 69,68,136, 79,143,72
+ 30,0,60, 65,64,133, 74,135,76
+ 30,0,60, 69,69,135, 70,137,68
+ 30,0,60, 60,59,132, 78,132,74
+ 30,0,60, 57,58,136, 76,136,77
+ 30,0,60, 58,60,135, 78,134,75
+ 30,0,60, 68,65,135, 73,135,74
+ 30,0,60, 67,68,141, 76,141,75
+ 30,0,60, 69,68,142, 77,144,75
+ 30,0,60, 56,62,129, 79,131,70
+ 30,0,60, 62,65,135, 75,135,73
+ 30,0,60, 72,71,145, 73,144,78
+ 30,0,60, 65,64,136, 74,135,73
+ 30,0,60, 62,59,136, 74,137,77
+ 30,0,60, 64,65,137, 76,140,78
+ 30,0,60, 64,62,141, 76,138,79
+ 30,0,60, 67,70,135, 72,137,71
+ 30,0,60, 74,74,147, 73,146,77
+ 30,0,60, 68,67,142, 75,144,78
+ 30,0,60, 59,59,126, 75,128,68
+
diff --git a/motors/fet12/calib_data_60a.csv b/motors/fet12/calib_data_60a.csv
new file mode 100644
index 0000000..3ef66cf
--- /dev/null
+++ b/motors/fet12/calib_data_60a.csv
@@ -0,0 +1,94 @@
+60,0,0, 139,140,-16, -153,-13,-153
+60,0,0, 141,147,-12, -152,-11,-157
+60,0,0, 145,148,-8, -152,-8,-150
+60,0,0, 137,141,-17, -154,-18,-157
+60,0,0, 146,143,-14, -158,-15,-155
+60,0,0, 141,147,-14, -156,-16,-161
+60,0,0, 143,145,-15, -154,-15,-161
+60,0,0, 141,147,-13, -153,-12,-158
+60,0,0, 141,145,-17, -156,-18,-162
+60,0,0, 144,151,-10, -156,-11,-160
+60,0,0, 142,146,-14, -156,-16,-159
+60,0,0, 149,152,-11, -156,-10,-161
+60,0,0, 137,144,-11, -152,-13,-157
+60,0,0, 135,141,-17, -151,-12,-155
+60,0,0, 136,142,-16, -152,-13,-156
+60,0,0, 144,147,-6, -151,-9,-151
+60,0,0, 143,149,-11, -155,-10,-158
+60,0,0, 139,143,-14, -151,-11,-154
+60,0,0, 140,150,-6, -151,-8,-154
+60,0,0, 143,147,-12, -150,-7,-153
+60,0,0, 139,145,-7, -145,-8,-155
+60,0,0, 139,142,-16, -152,-14,-158
+60,0,0, 142,147,-12, -154,-11,-157
+60,0,0, 137,139,-22, -154,-18,-159
+60,0,0, 138,140,-12, -152,-15,-150
+60,0,0, 138,146,-8, -147,-6,-152
+60,0,0, 144,148,-14, -155,-15,-160
+60,0,0, 141,141,-15, -151,-12,-156
+60,0,0, 141,144,-19, -154,-18,-163
+60,0,0, 135,140,-15, -150,-16,-153
+60,0,0, 140,142,-13, -149,-12,-157
+60,0,0, 150,157,-8, -154,-3,-160
+60,0,0, 141,145,-12, -153,-11,-154
+60,0,0, 147,146,-14, -156,-15,-158
+60,0,0, 140,142,-15, -150,-13,-155
+60,0,0, 136,139,-18, -147,-16,-153
+60,0,0, 136,139,-17, -152,-18,-153
+60,0,0, 146,147,-6, -149,-2,-150
+60,0,0, 140,143,-19, -155,-18,-159
+60,0,0, 142,141,-16, -152,-11,-154
+60,0,0, 137,142,-13, -148,-9,-154
+60,0,0, 143,149,-18, -153,-14,-163
+60,0,0, 145,153,-5, -152,-6,-158
+60,0,0, 130,135,-27, -154,-25,-158
+60,0,0, 146,151,-6, -155,-7,-154
+60,0,0, 146,151,-11, -154,-7,-158
+60,0,0, 137,142,-16, -151,-12,-157
+60,0,0, 142,145,-12, -152,-11,-155
+60,0,0, 143,144,-17, -156,-15,-161
+60,0,0, 138,139,-13, -148,-13,-149
+60,0,0, 139,141,-15, -148,-11,-152
+60,0,0, 142,144,-13, -148,-12,-155
+60,0,0, 137,140,-20, -152,-18,-157
+60,0,0, 141,141,-13, -154,-15,-153
+60,0,0, 145,146,-13, -151,-8,-154
+60,0,0, 139,143,-15, -148,-12,-156
+60,0,0, 138,140,-16, -151,-14,-150
+60,0,0, 140,143,-14, -150,-12,-155
+60,0,0, 138,140,-12, -152,-13,-153
+60,0,0, 139,144,-9, -147,-9,-155
+60,0,0, 141,142,-18, -154,-17,-160
+60,0,0, 142,142,-13, -154,-15,-152
+60,0,0, 133,135,-18, -148,-19,-154
+60,0,0, 138,134,-19, -155,-21,-152
+60,0,0, 145,147,-12, -156,-13,-159
+60,0,0, 148,149,-5, -155,-6,-153
+60,0,0, 143,142,-14, -157,-16,-156
+60,0,0, 135,139,-21, -152,-18,-158
+60,0,0, 143,147,-6, -150,-8,-151
+60,0,0, 137,143,-19, -154,-20,-163
+60,0,0, 136,140,-16, -150,-17,-156
+60,0,0, 146,147,-8, -156,-12,-154
+60,0,0, 145,145,-13, -152,-9,-153
+60,0,0, 137,140,-18, -151,-14,-157
+60,0,0, 137,142,-18, -153,-17,-158
+60,0,0, 143,146,-14, -154,-11,-159
+60,0,0, 141,143,-10, -149,-11,-156
+60,0,0, 139,138,-13, -151,-14,-156
+60,0,0, 135,138,-12, -148,-15,-153
+60,0,0, 132,140,-13, -148,-15,-153
+60,0,0, 141,147,-12, -154,-12,-158
+60,0,0, 143,146,-7, -150,-8,-152
+60,0,0, 137,140,-17, -152,-14,-158
+60,0,0, 137,139,-12, -149,-13,-152
+60,0,0, 145,150,-7, -152,-5,-158
+60,0,0, 140,142,-9, -148,-10,-152
+60,0,0, 144,151,-4, -145,-3,-155
+60,0,0, 134,136,-15, -147,-14,-154
+60,0,0, 139,140,-10, -148,-10,-147
+60,0,0, 142,146,-10, -152,-7,-153
+60,0,0, 154,153,6, -145,7,-149
+60,0,0, 140,137,-14, -150,-14,-152
+60,0,0, 138,143,-17, -151,-13,-155
+60,0,0, 149,151,-2, -151,0,-154
diff --git a/motors/fet12/calib_data_60b.csv b/motors/fet12/calib_data_60b.csv
new file mode 100644
index 0000000..e44b7ff
--- /dev/null
+++ b/motors/fet12/calib_data_60b.csv
@@ -0,0 +1,61 @@
+ 0,60,0, -118,-115,-130, -10,-126,-12
+ 0,60,0, -108,-106,-120, -9,-116,-10
+ 0,60,0, -114,-113,-133, -12,-125,-17
+ 0,60,0, -116,-114,-131, -11,-128,-14
+ 0,60,0, -112,-109,-124, -12,-120,-10
+ 0,60,0, -111,-109,-120, -7,-115,-10
+ 0,60,0, -112,-111,-130, -15,-124,-12
+ 0,60,0, -100,-98,-110, -4,-108,-6
+ 0,60,0, -114,-110,-117, -1,-116,-4
+ 0,60,0, -116,-115,-122, -7,-121,-4
+ 0,60,0, -123,-118,-132, -7,-130,-12
+ 0,60,0, -106,-103,-119, -8,-115,-11
+ 0,60,0, -120,-119,-130, -11,-128,-9
+ 0,60,0, -116,-116,-130, -6,-122,-9
+ 0,60,0, -117,-115,-126, -5,-124,-9
+ 0,60,0, -107,-104,-117, -6,-112,-9
+ 0,60,0, -103,-106,-120, -11,-120,-13
+ 0,60,0, -114,-114,-129, -9,-123,-15
+ 0,60,0, -102,-100,-111, -9,-109,-12
+ 0,60,0, -114,-110,-124, -8,-120,-11
+ 0,60,0, -115,-113,-124, -6,-121,-6
+ 0,60,0, -118,-118,-128, -6,-125,-6
+ 0,60,0, -115,-111,-127, -11,-127,-15
+ 0,60,0, -112,-109,-129, -11,-123,-13
+ 0,60,0, -115,-111,-129, -11,-125,-14
+ 0,60,0, -117,-115,-128, -12,-128,-13
+ 0,60,0, -114,-110,-126, -10,-124,-15
+ 0,60,0, -115,-111,-129, -11,-126,-14
+ 0,60,0, -115,-116,-129, -9,-124,-11
+ 0,60,0, -116,-112,-130, -11,-127,-10
+ 0,60,0, -116,-113,-119, -4,-120,-5
+ 0,60,0, -109,-108,-126, -14,-123,-16
+ 0,60,0, -113,-115,-130, -11,-127,-12
+ 0,60,0, -119,-118,-138, -11,-131,-22
+ 0,60,0, -104,-104,-120, -10,-117,-12
+ 0,60,0, -113,-108,-115, -3,-116,-7
+ 0,60,0, -108,-106,-122, -6,-119,-14
+ 0,60,0, -110,-107,-113, -6,-114,-5
+ 0,60,0, -104,-102,-113, -7,-110,-8
+ 0,60,0, -118,-118,-132, -8,-127,-11
+ 0,60,0, -117,-113,-127, -7,-123,-14
+ 0,60,0, -118,-115,-128, -9,-125,-10
+ 0,60,0, -109,-108,-128, -12,-122,-13
+ 0,60,0, -103,-104,-113, -8,-113,-10
+ 0,60,0, -112,-110,-130, -11,-125,-12
+ 0,60,0, -115,-112,-126, -7,-122,-12
+ 0,60,0, -115,-112,-127, -6,-122,-12
+ 0,60,0, -113,-112,-127, -10,-123,-13
+ 0,60,0, -112,-112,-128, -11,-123,-15
+ 0,60,0, -115,-108,-121, -4,-121,-12
+ 0,60,0, -111,-109,-126, -7,-121,-14
+ 0,60,0, -112,-111,-128, -7,-122,-12
+ 0,60,0, -104,-101,-114, -7,-109,-12
+ 0,60,0, -117,-120,-135, -4,-129,-9
+ 0,60,0, -119,-117,-128, -7,-124,-11
+ 0,60,0, -111,-112,-122, -7,-118,-6
+ 0,60,0, -119,-116,-125, -4,-122,-9
+ 0,60,0, -112,-109,-121, -9,-120,-7
+ 0,60,0, -116,-115,-130, -6,-124,-14
+ 0,60,0, -116,-115,-127, -8,-123,-9
+ 0,60,0, -104,-105,-118, -10,-114,-12
diff --git a/motors/fet12/calib_data_60c.csv b/motors/fet12/calib_data_60c.csv
new file mode 100644
index 0000000..eb1bc52
--- /dev/null
+++ b/motors/fet12/calib_data_60c.csv
@@ -0,0 +1,49 @@
+ 0,0,60, -14,-18,135, 151,135,156
+ 0,0,60, -11,-8,139, 155,142,148
+ 0,0,60, -1,2,156, 156,155,155
+ 0,0,60, -14,-15,140, 155,138,158
+ 0,0,60, -6,-7,143, 155,148,157
+ 0,0,60, -16,-15,133, 158,136,156
+ 0,0,60, -17,-20,135, 155,134,155
+ 0,0,60, -14,-11,138, 149,139,156
+ 0,0,60, -19,-15,135, 157,135,152
+ 0,0,60, -18,-17,140, 162,141,161
+ 0,0,60, -8,-6,149, 157,147,156
+ 0,0,60, -3,-6,142, 149,144,155
+ 0,0,60, -6,-2,144, 150,145,152
+ 0,0,60, -17,-15,138, 155,138,156
+ 0,0,60, -11,-8,145, 151,142,156
+ 0,0,60, -15,-15,140, 150,136,158
+ 0,0,60, -16,-12,145, 156,141,156
+ 0,0,60, -20,-17,135, 156,134,159
+ 0,0,60, -6,0,151, 149,148,155
+ 0,0,60, -17,-14,140, 156,141,156
+ 0,0,60, -16,-13,141, 161,146,157
+ 0,0,60, -9,-9,150, 159,146,162
+ 0,0,60, -16,-13,136, 150,132,150
+ 0,0,60, -22,-19,140, 159,140,163
+ 0,0,60, -14,-13,140, 155,138,156
+ 0,0,60, -14,-15,139, 158,139,158
+ 0,0,60, -8,-6,150, 157,150,159
+ 0,0,60, -4,0,153, 158,154,156
+ 0,0,60, -16,-11,138, 158,140,152
+ 0,0,60, -14,-15,138, 156,140,158
+ 0,0,60, 6,7,157, 156,158,155
+ 0,0,60, -2,-4,150, 151,146,157
+ 0,0,60, -15,-14,139, 153,135,154
+ 0,0,60, -7,-6,143, 155,146,157
+ 0,0,60, -7,-9,144, 156,146,155
+ 0,0,60, -10,-8,150, 156,150,162
+ 0,0,60, -7,-4,144, 155,147,152
+ 0,0,60, -18,-14,143, 158,142,160
+ 0,0,60, -10,-10,140, 155,141,154
+ 0,0,60, -19,-19,135, 156,135,156
+ 0,0,60, -7,-11,140, 151,140,153
+ 0,0,60, -3,-6,151, 157,150,158
+ 0,0,60, -12,-10,143, 156,141,153
+ 0,0,60, -10,-4,148, 155,146,156
+ 0,0,60, -12,-9,140, 153,141,151
+ 0,0,60, -7,-9,146, 154,148,156
+ 0,0,60, -21,-19,137, 160,136,157
+ 0,0,60, -20,-21,131, 158,135,154
+ 0,0,60, -14,-12,141, 157,141,155
diff --git a/motors/fet12/calib_sensors.py b/motors/fet12/calib_sensors.py
new file mode 100755
index 0000000..777ea37
--- /dev/null
+++ b/motors/fet12/calib_sensors.py
@@ -0,0 +1,43 @@
+#!/usr/bin/python3
+import sys
+import numpy as np
+
+# Note on associated data files:
+# 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.
+
+ CSVs should be of format:
+ command_a, command_b, command_c, adc0, adc0, adc1, adc2, adc1, adc2
+ Where The adc columns in this case are the 6 samples taken from the
+ ADC where each pair of columns with the same name correspond with
+ the same measurement (we average samples that are of the same value
+ because otherwise the solution matrix can't be solved for in a stable
+ manner).
+ """
+ data = np.zeros((1, 9))
+ for fname in fnames:
+ data = np.vstack((data, np.genfromtxt(fname, delimiter=',')))
+ data = data[1:, :]
+
+ if data.shape[1] == 9:
+ data[:, 3] = (data[:, 3] + data[:, 4]) / 2.0
+ data[:, 4] = (data[:, 5] + data[:, 7]) / 2.0
+ data[:, 5] = (data[:, 6] + data[:, 8]) / 2.0
+ data = data[:, :6]
+
+ b = data[:, 0:3]
+ b = b - np.tile(np.mean(b, axis=1), (3, 1)).T
+ # Vcc / 3000 / R
+ b *= 30.8 / 3000.0 / 0.0084
+ A = data[:, 3:]
+
+ 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:]))
diff --git a/motors/fet12/current_equalize.py b/motors/fet12/current_equalize.py
index 698983f..ea44916 100755
--- a/motors/fet12/current_equalize.py
+++ b/motors/fet12/current_equalize.py
@@ -2,8 +2,9 @@
import numpy
import sys
+import calib_sensors
-def main():
+def manual_calibrate():
# 38 27 -84
# 36 -64 39
# -74 21 35
@@ -17,6 +18,13 @@
[-current / 2.0, current, -current / 2.0],
[-current / 2.0, -current / 2.0, current]])
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:])
print("#ifndef MOTORS_FET12_CURRENT_MATRIX_")
print("#define MOTORS_FET12_CURRENT_MATRIX_")
diff --git a/motors/fet12/fet12v2.cc b/motors/fet12/fet12v2.cc
index d95d4a2..5f8568b 100644
--- a/motors/fet12/fet12v2.cc
+++ b/motors/fet12/fet12v2.cc
@@ -10,6 +10,7 @@
#include "motors/fet12/motor_controls.h"
#include "motors/motor.h"
#include "motors/peripheral/adc.h"
+#include "motors/peripheral/adc_dma.h"
#include "motors/peripheral/can.h"
#include "motors/peripheral/uart.h"
#include "motors/util.h"
@@ -25,6 +26,10 @@
constexpr double kR = 0.0084;
struct Fet12AdcReadings {
+ // Averages of the pairs of ADC DMA channels corresponding with each channel
+ // pair. Individual values in motor_currents correspond to current sensor
+ // values, rather than the actual currents themselves (and so they still need
+ // to be decoupled).
int16_t motor_currents[3];
int16_t throttle, fuse_voltage;
};
@@ -57,34 +62,9 @@
PORTB_PCR3 = PORT_PCR_MUX(0);
}
-Fet12AdcReadings AdcReadFet12(const DisableInterrupts &) {
- Fet12AdcReadings r;
-
- ADC1_SC1A = 5;
- ADC0_SC1A = 23;
- while (!(ADC1_SC1A & ADC_SC1_COCO)) {
- }
- ADC1_SC1A = 6;
- r.motor_currents[0] = static_cast<int16_t>(ADC1_RA) - 2032;
- while (!(ADC0_SC1A & ADC_SC1_COCO)) {
- }
- ADC0_SC1A = 13;
- r.throttle = static_cast<int16_t>(ADC0_RA);
- while (!(ADC1_SC1A & ADC_SC1_COCO)) {
- }
- ADC1_SC1A = 7;
- r.motor_currents[1] = static_cast<int16_t>(ADC1_RA) - 2032;
- while (!(ADC0_SC1A & ADC_SC1_COCO)) {
- }
- r.fuse_voltage = static_cast<int16_t>(ADC0_RA);
- while (!(ADC1_SC1A & ADC_SC1_COCO)) {
- }
- r.motor_currents[2] = static_cast<int16_t>(ADC1_RA) - 2032;
-
- return r;
-}
-
::std::atomic<Motor *> global_motor{nullptr};
+::std::atomic<teensy::AdcDmaSampler *> global_adc_dma{nullptr};
+
::std::atomic<teensy::InterruptBufferedUart *> global_stdout{nullptr};
extern "C" {
@@ -154,20 +134,31 @@
DebugBuffer global_debug_buffer;
void ftm0_isr(void) {
- const auto wrapped_encoder =
- global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+ static uint32_t i = 0;
+ teensy::AdcDmaSampler *const adc_dma =
+ global_adc_dma.load(::std::memory_order_relaxed);
+
Fet12AdcReadings adc_readings;
- {
- DisableInterrupts disable_interrupts;
- adc_readings = AdcReadFet12(disable_interrupts);
+ // TODO(Brian): Switch to the DMA interrupt instead of spinning.
+ while (!adc_dma->CheckDone()) {
}
+
+ adc_readings.motor_currents[0] =
+ (adc_dma->adc_result(0, 0) + adc_dma->adc_result(0, 1)) / 2;
+ adc_readings.motor_currents[1] =
+ (adc_dma->adc_result(0, 2) + adc_dma->adc_result(1, 1)) / 2;
+ adc_readings.motor_currents[2] =
+ (adc_dma->adc_result(1, 0) + adc_dma->adc_result(1, 2)) / 2;
+ adc_readings.throttle = adc_dma->adc_result(0, 3);
const ::std::array<float, 3> decoupled =
DecoupleCurrents(adc_readings.motor_currents);
-
+ adc_dma->Reset();
+ const uint32_t wrapped_encoder =
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+#if 1
const BalancedReadings balanced =
BalanceSimpleReadings(decoupled);
- static int i = 0;
static float fuse_badness = 0;
static uint32_t cycles_since_start = 0u;
@@ -265,17 +256,6 @@
->SetGoalCurrent(goal_current);
global_motor.load(::std::memory_order_relaxed)
->HandleInterrupt(balanced, wrapped_encoder);
-#else
- (void)balanced;
- FTM0->SC &= ~FTM_SC_TOF;
- FTM0->C0V = 0;
- FTM0->C1V = 0;
- FTM0->C2V = 0;
- FTM0->C3V = 0;
- FTM0->C4V = 0;
- FTM0->C5V = 60;
- FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
-#endif
global_debug_buffer.count.fetch_add(1);
@@ -322,7 +302,9 @@
global_debug_buffer.size.fetch_add(1);
}
+ ++i;
if (buffer_size == global_debug_buffer.samples.size()) {
+ i = 0;
GPIOC_PCOR = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4);
GPIOD_PCOR = (1 << 4) | (1 << 5);
@@ -340,11 +322,19 @@
PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(1);
PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
}
+#else
+#endif
+#else
+ FTM0->SC &= ~FTM_SC_TOF;
+ FTM0->C0V = 0;
+ FTM0->C1V = 0;
+ FTM0->C2V = 0;
+ FTM0->C3V = 0;
+ FTM0->C4V = 0;
+ FTM0->C5V = 0;
+ FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
+#endif
- ++i;
- if (i > 1000) {
- i = 0;
- }
}
} // extern "C"
@@ -501,8 +491,69 @@
motor.set_encoder_offset(810);
motor.set_deadtime_compensation(9);
ConfigurePwmFtm(FTM0);
+
+ FTM2->CONF = FTM_CONF_GTBEEN;
+ FTM2->MODE = FTM_MODE_WPDIS;
+ FTM2->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ FTM2->SC = FTM_SC_CLKS(0) /* Disable counting for now */;
+ FTM2->CNTIN = 0;
+ FTM2->CNT = 0;
+ // TODO(Brian): Don't duplicate this.
+ FTM2->MOD = BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY;
+ FTM2->OUTINIT = 0;
+ // All of the channels are active high.
+ FTM2->POL = 0;
+ FTM2->SYNCONF = FTM_SYNCONF_HWWRBUF | FTM_SYNCONF_SWWRBUF |
+ FTM_SYNCONF_SWRSTCNT | FTM_SYNCONF_SYNCMODE;
+ // Don't want any intermediate loading points.
+ FTM2->PWMLOAD = 0;
+
+ // Need to set them to some kind of output mode so we can actually change
+ // them.
+ FTM2->C0SC = FTM_CSC_MSA;
+ FTM2->C1SC = FTM_CSC_MSA;
+
+ // This has to happen after messing with SYNCONF, and should happen after
+ // messing with various other things so the values can get flushed out of the
+ // buffers.
+ FTM2->SYNC =
+ FTM_SYNC_SWSYNC /* Flush everything out right now */ |
+ FTM_SYNC_CNTMAX /* Load new values at the end of the cycle */;
+ // Wait for the software synchronization to finish.
+ while (FTM2->SYNC & FTM_SYNC_SWSYNC) {
+ }
+ FTM2->SC = FTM_SC_CLKS(1) /* Use the system clock */ |
+ FTM_SC_PS(0) /* Don't prescale the clock */;
+ // TODO:
+ //FTM2->MODE &= ~FTM_MODE_WPDIS;
+
+ FTM2->EXTTRIG = FTM_EXTTRIG_CH0TRIG | FTM_EXTTRIG_CH1TRIG;
+
+ teensy::AdcDmaSampler adc_dma;
+ // ADC0_Dx0 is 1-0
+ // ADC0_Dx2 is 1-2
+ // ADC0_Dx3 is 2-0
+ // ADC1_Dx0 is 2-0
+ // ADC1_Dx3 is 1-0
+ // Sample 0: 1-2,2-0
+ // Sample 1: 1-2,1-0
+ // Sample 2: 1-0,2-0
+ // Sample 3: 23(SENSE0),18(VIN)
+ adc_dma.set_adc0_samples({V_ADC_ADCH(2) | M_ADC_DIFF,
+ V_ADC_ADCH(2) | M_ADC_DIFF,
+ V_ADC_ADCH(0) | M_ADC_DIFF, V_ADC_ADCH(23)});
+ adc_dma.set_adc1_samples({V_ADC_ADCH(0) | M_ADC_DIFF,
+ V_ADC_ADCH(3) | M_ADC_DIFF,
+ V_ADC_ADCH(0) | M_ADC_DIFF, V_ADC_ADCH(18)});
+ adc_dma.set_ftm_delays({&FTM2->C0V, &FTM2->C1V});
+ adc_dma.set_pdb_input(PDB_IN_FTM2);
+
+ adc_dma.Initialize();
+ FTM0->CONF = FTM_CONF_GTBEEN;
motor.Init();
global_motor.store(&motor, ::std::memory_order_relaxed);
+ global_adc_dma.store(&adc_dma, ::std::memory_order_relaxed);
+
// Output triggers to things like the PDBs on initialization.
FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
// Don't let any memory accesses sneak past here, because we actually
@@ -529,7 +580,11 @@
printf("Zeroed motor!\n");
// Give stuff a chance to recover from interrupts-disabled.
delay(100);
+ adc_dma.Reset();
motor.Start();
+ // Now poke the GTB to actually start both timers.
+ FTM0->CONF = FTM_CONF_GTBEEN | FTM_CONF_GTBEOUT;
+
NVIC_ENABLE_IRQ(IRQ_FTM0);
GPIOC_PSOR = 1 << 5;
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h b/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
index 54742eb..86be90e 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
@@ -198,7 +198,22 @@
solver_.changeInitialState(x_start);
- bool solveSuccessful = solver_.finishMPCIteration();
+ bool solveSuccessful = true;
+
+ bool foundBetter = true;
+ int numIterations = 0;
+
+ while (true) {
+ foundBetter = solver_.finishIteration();
+
+ numIterations++;
+ if (foundBetter &&
+ (numIterations < solver_.getSettings().max_iterations)) {
+ solver_.prepareMPCIteration();
+ } else {
+ break;
+ }
+ }
if (solveSuccessful)
{
diff --git a/third_party/matplotlib-cpp/matplotlibcpp.h b/third_party/matplotlib-cpp/matplotlibcpp.h
index 0391b3f..eb7b7cc 100644
--- a/third_party/matplotlib-cpp/matplotlibcpp.h
+++ b/third_party/matplotlib-cpp/matplotlibcpp.h
@@ -56,6 +56,7 @@
PyObject *s_python_function_errorbar;
PyObject *s_python_function_annotate;
PyObject *s_python_function_tight_layout;
+ PyObject *s_python_colormap;
PyObject *s_python_empty_tuple;
PyObject *s_python_function_stem;
PyObject *s_python_function_xkcd;
@@ -107,9 +108,13 @@
PyObject* matplotlibname = PyString_FromString("matplotlib");
PyObject* pyplotname = PyString_FromString("matplotlib.pyplot");
+ PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
+ PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
PyObject* pylabname = PyString_FromString("pylab");
- if (!pyplotname || !pylabname || !matplotlibname) {
- throw std::runtime_error("couldnt create string");
+ PyObject* cmname = PyString_FromString("matplotlib.cm");
+ if (!pyplotname || !pylabname || !matplotlibname || !mpl_toolkits ||
+ !axis3d || !cmname) {
+ throw std::runtime_error("couldnt create string");
}
PyObject* matplotlib = PyImport_Import(matplotlibname);
@@ -126,11 +131,22 @@
Py_DECREF(pyplotname);
if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); }
+ s_python_colormap = PyImport_Import(cmname);
+ Py_DECREF(cmname);
+ if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); }
PyObject* pylabmod = PyImport_Import(pylabname);
Py_DECREF(pylabname);
if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); }
+ PyObject* mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
+ Py_DECREF(mpl_toolkitsmod);
+ if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
+
+ PyObject* axis3dmod = PyImport_Import(axis3d);
+ Py_DECREF(axis3dmod);
+ if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
+
s_python_function_show = PyObject_GetAttrString(pymod, "show");
s_python_function_close = PyObject_GetAttrString(pymod, "close");
s_python_function_draw = PyObject_GetAttrString(pymod, "draw");
@@ -293,6 +309,30 @@
return varray;
}
+template<typename Numeric>
+PyObject* get_2darray(const std::vector<::std::vector<Numeric>>& v)
+{
+ detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work
+ if (v.size() < 1) throw std::runtime_error("get_2d_array v too small");
+
+ npy_intp vsize[2] = {static_cast<npy_intp>(v.size()),
+ static_cast<npy_intp>(v[0].size())};
+
+ PyArrayObject *varray =
+ (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE);
+
+ double *vd_begin = static_cast<double *>(PyArray_DATA(varray));
+
+ for (const ::std::vector<Numeric> &v_row : v) {
+ if (v_row.size() != static_cast<size_t>(vsize[1]))
+ throw std::runtime_error("Missmatched array size");
+ std::copy(v_row.begin(), v_row.end(), vd_begin);
+ vd_begin += vsize[1];
+ }
+
+ return reinterpret_cast<PyObject *>(varray);
+}
+
#else // fallback if we don't have numpy: copy every element of the given vector
template<typename Numeric>
@@ -337,6 +377,76 @@
return res;
}
+template <typename Numeric>
+void plot_surface(const std::vector<::std::vector<Numeric>> &x,
+ const std::vector<::std::vector<Numeric>> &y,
+ const std::vector<::std::vector<Numeric>> &z,
+ const std::map<std::string, std::string> &keywords =
+ std::map<std::string, std::string>()) {
+ assert(x.size() == y.size());
+ assert(y.size() == z.size());
+
+ // using numpy arrays
+ PyObject *xarray = get_2darray(x);
+ PyObject *yarray = get_2darray(y);
+ PyObject *zarray = get_2darray(z);
+
+ // construct positional args
+ PyObject *args = PyTuple_New(3);
+ PyTuple_SetItem(args, 0, xarray);
+ PyTuple_SetItem(args, 1, yarray);
+ PyTuple_SetItem(args, 2, zarray);
+
+ // Build up the kw args.
+ PyObject *kwargs = PyDict_New();
+ PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1));
+ PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1));
+
+ PyObject *python_colormap_coolwarm = PyObject_GetAttrString(
+ detail::_interpreter::get().s_python_colormap, "coolwarm");
+
+ PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm);
+
+ for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
+ it != keywords.end(); ++it) {
+ PyDict_SetItemString(kwargs, it->first.c_str(),
+ PyString_FromString(it->second.c_str()));
+ }
+
+
+ PyObject *fig =
+ PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
+ detail::_interpreter::get().s_python_empty_tuple);
+ if (!fig) throw std::runtime_error("Call to figure() failed.");
+
+ PyObject *gca_kwargs = PyDict_New();
+ PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
+
+ PyObject *gca = PyObject_GetAttrString(fig, "gca");
+ if (!gca) throw std::runtime_error("No gca");
+ Py_INCREF(gca);
+ PyObject *axis = PyObject_Call(
+ gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
+
+ if (!axis) throw std::runtime_error("No axis");
+ Py_INCREF(axis);
+
+ Py_DECREF(gca);
+ Py_DECREF(gca_kwargs);
+
+ PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface");
+ if (!plot_surface) throw std::runtime_error("No surface");
+ Py_INCREF(plot_surface);
+ PyObject *res = PyObject_Call(plot_surface, args, kwargs);
+ if (!res) throw std::runtime_error("failed surface");
+ Py_DECREF(plot_surface);
+
+ Py_DECREF(axis);
+ Py_DECREF(args);
+ Py_DECREF(kwargs);
+ if (res) Py_DECREF(res);
+}
+
template<typename Numeric>
bool stem(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords)
{
diff --git a/third_party/protobuf/BUILD b/third_party/protobuf/BUILD
index 619bbee..5cad921 100644
--- a/third_party/protobuf/BUILD
+++ b/third_party/protobuf/BUILD
@@ -502,6 +502,7 @@
tools = [":protoc"],
)
+"""
java_library(
name = "protobuf_java",
srcs = glob([
@@ -511,6 +512,7 @@
],
visibility = ["//visibility:public"],
)
+"""
################################################################################
# Python support
diff --git a/y2018/BUILD b/y2018/BUILD
index e4c4128..cccc2cf 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -47,6 +47,7 @@
"//aos/common:time",
"//aos/common/actions:action_lib",
"//aos/common/logging",
+ "//aos/common/network:team_number",
"//aos/common/util:log_interval",
"//aos/input:drivetrain_input",
"//aos/input:joystick_input",
diff --git a/y2018/constants.cc b/y2018/constants.cc
index 2ad0c97..70c58f0 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -135,7 +135,7 @@
}
const Values &DoGetValues() {
- uint16_t team = ::aos::network::GetTeamNumber();
+ const uint16_t team = ::aos::network::GetTeamNumber();
LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
return GetValuesForTeam(team);
}
diff --git a/y2018/control_loops/python/2d_plot.cc b/y2018/control_loops/python/2d_plot.cc
new file mode 100644
index 0000000..52b8b34
--- /dev/null
+++ b/y2018/control_loops/python/2d_plot.cc
@@ -0,0 +1,36 @@
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_double(yrange, 1.0, "+- y max");
+
+double fx(double x, double yrange) {
+ return 2.0 * ((1.0 / (1.0 + ::std::exp(-x * 2.0 / yrange)) - 0.5)) *
+ yrange;
+}
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ matplotlibcpp::figure();
+ ::std::vector<double> x;
+ ::std::vector<double> y;
+ ::std::vector<double> slope_y;
+
+ for (double i = -5.0; i < 5.0; i += 0.01) {
+ x.push_back(i);
+ y.push_back(fx(i, FLAGS_yrange));
+ slope_y.push_back(
+ (fx(i + 0.0001, FLAGS_yrange) - fx(i - 0.0001, FLAGS_yrange)) /
+ (2.0 * 0.0001));
+ }
+
+ matplotlibcpp::plot(x, y, {{"label", "saturated x"}});
+ matplotlibcpp::plot(x, slope_y, {{"label", "slope"}});
+ matplotlibcpp::legend();
+ matplotlibcpp::show();
+
+}
diff --git a/y2018/control_loops/python/3d_plot.cc b/y2018/control_loops/python/3d_plot.cc
new file mode 100644
index 0000000..ff1e293
--- /dev/null
+++ b/y2018/control_loops/python/3d_plot.cc
@@ -0,0 +1,75 @@
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/python/arm_bounds.h"
+
+DEFINE_double(boundary_scalar, 20000.0, "quadratic slope");
+DEFINE_double(boundary_rate, 1000.0, "linear slope");
+DEFINE_double(bounds_offset, 0.02, "Offset the quadratic boundary in by this");
+
+using ::y2018::control_loops::Point;
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ ::y2018::control_loops::BoundsCheck arm_space =
+ ::y2018::control_loops::MakeClippedArmSpace();
+
+ matplotlibcpp::figure();
+ ::std::vector<double> bounds_x;
+ ::std::vector<double> bounds_y;
+ for (const Point p : arm_space.points()) {
+ bounds_x.push_back(p.x());
+ bounds_y.push_back(p.y());
+ }
+ matplotlibcpp::plot(bounds_x, bounds_y, {{"label", "actual region"}});
+ matplotlibcpp::legend();
+
+ ::std::vector<::std::vector<double>> cost_x;
+ ::std::vector<::std::vector<double>> cost_y;
+ ::std::vector<::std::vector<double>> cost_z;
+ ::std::vector<::std::vector<double>> cost_z2;
+ ::std::vector<::std::vector<double>> cost_z3;
+
+ for (double x_coordinate = -0.5; x_coordinate < 1.2; x_coordinate += 0.05) {
+ ::std::vector<double> cost_x_row;
+ ::std::vector<double> cost_y_row;
+ ::std::vector<double> cost_z_row;
+ ::std::vector<double> cost_z2_row;
+ ::std::vector<double> cost_z3_row;
+
+ for (double y_coordinate = -1.0; y_coordinate < 5.0; y_coordinate += 0.05) {
+ cost_x_row.push_back(x_coordinate);
+ cost_y_row.push_back(y_coordinate);
+
+ ::Eigen::Matrix<double, 2, 1> norm;
+ double min_distance =
+ arm_space.min_distance(Point(x_coordinate, y_coordinate), &norm);
+ cost_z_row.push_back(::std::min(
+ 500.0 * ::std::max(min_distance + FLAGS_bounds_offset, 0.0), 40.0));
+
+ cost_z2_row.push_back(::std::min(
+ FLAGS_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) +
+ FLAGS_boundary_rate *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset),
+ 200.0));
+
+ cost_z3_row.push_back(min_distance);
+ }
+ cost_x.push_back(cost_x_row);
+ cost_y.push_back(cost_y_row);
+ cost_z.push_back(cost_z_row);
+ cost_z2.push_back(cost_z2_row);
+ cost_z3.push_back(cost_z3_row);
+ }
+
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z);
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z2);
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z3);
+ matplotlibcpp::show();
+}
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index eebbfe2..3982722 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -95,14 +95,66 @@
],
)
+cc_library(
+ name = 'dlqr',
+ hdrs = [
+ 'dlqr.h',
+ ],
+ deps = [
+ '@slycot_repo//:slicot',
+ ],
+)
+
cc_binary(
name = "arm_mpc",
srcs = [
"arm_mpc.cc",
],
+ deps = [
+ ":arm_bounds",
+ ':dlqr',
+ "//third_party/ct",
+ "//third_party/gflags",
+ "//third_party/matplotlib-cpp",
+ "@cgal_repo//:cgal",
+ ],
+ restricted_to = ["//tools:k8"],
+)
+
+cc_binary(
+ name = "3d_plot",
+ srcs = [
+ "3d_plot.cc",
+ ],
restricted_to = ["//tools:k8"],
deps = [
- "//third_party/ct",
+ ":arm_bounds",
+ "//third_party/gflags",
+ "//third_party/matplotlib-cpp",
+ ],
+)
+
+cc_library(
+ name = "arm_bounds",
+ srcs = [
+ "arm_bounds.cc",
+ ],
+ hdrs = [
+ "arm_bounds.h",
+ ],
+ deps = [
+ "//third_party/eigen",
+ "@cgal_repo//:cgal",
+ ],
+)
+
+cc_binary(
+ name = "2d_plot",
+ srcs = [
+ "2d_plot.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
"//third_party/gflags",
"//third_party/matplotlib-cpp",
],
@@ -143,3 +195,30 @@
visibility = ["//visibility:public"],
deps = ["//y2018/control_loops:python_init"],
)
+
+py_binary(
+ name = "extended_lqr",
+ srcs = [
+ "extended_lqr.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
+py_binary(
+ name = 'arm_mpc_py',
+ srcs = [
+ 'arm_mpc.py',
+ ],
+ main = 'arm_mpc.py',
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+ restricted_to = ['//tools:k8'],
+)
diff --git a/y2018/control_loops/python/arm_bounds.cc b/y2018/control_loops/python/arm_bounds.cc
new file mode 100644
index 0000000..2c7b94c
--- /dev/null
+++ b/y2018/control_loops/python/arm_bounds.cc
@@ -0,0 +1,537 @@
+#include "y2018/control_loops/python/arm_bounds.h"
+
+#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
+#include <CGAL/Polygon_2_algorithms.h>
+#include <CGAL/squared_distance_2.h>
+#include <math.h>
+#include <iostream>
+
+namespace y2018 {
+namespace control_loops {
+
+static BoundsCheck MakeArbitraryArmSpace(::std::vector<Point> points) {
+ for (Point &point : points) {
+ point = Point(M_PI / 2.0 - point.x(), M_PI / 2.0 - point.y());
+ }
+ return BoundsCheck(points);
+}
+
+Vector GetNormal(Point pt, Segment segment) {
+ auto perp_line = segment.supporting_line().perpendicular(pt);
+ if (CGAL::do_intersect(perp_line, segment)) {
+ return Vector(segment.supporting_line().projection(pt), pt);
+ } else {
+ if (CGAL::squared_distance(segment.vertex(0), pt) <
+ CGAL::squared_distance(segment.vertex(1), pt)) {
+ return Vector(segment.vertex(0), pt);
+ } else {
+ return Vector(segment.vertex(1), pt);
+ }
+ }
+}
+
+double BoundsCheck::min_distance(Point pt,
+ ::Eigen::Matrix<double, 2, 1> *norm) const {
+ auto *cell = grid_.GetCell(pt);
+ const auto &points = grid_.points();
+ bool inside = false;
+ double dist = 0;
+ Segment best_segment;
+ if (cell) {
+ if (cell->IsBorderline()) {
+ inside = check_inside(pt, points);
+ } else {
+ inside = cell->IsInside(pt);
+ }
+ dist = cell->Distance(pt, &best_segment);
+ } else {
+ dist = min_dist(pt, points, &best_segment);
+ // TODO(austin): Return norm.
+ }
+ Vector normal = GetNormal(pt, best_segment);
+ norm->x() = normal.x();
+ norm->y() = normal.y();
+
+ norm->normalize();
+
+ if (!inside) {
+ *norm = -(*norm);
+ }
+
+ return inside ? -dist : dist;
+}
+
+BoundsCheck MakeFullArmSpace() {
+ return MakeArbitraryArmSpace({{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.0539770497854413, -3.3137951343195065},
+ {1.0581313547182551, -3.3348328475243303},
+ {1.0627176864909802, -3.355746333744654},
+ {1.0677271057021644, -3.376553557661551},
+ {1.0731519339297724, -3.397272220341514},
+ {1.0789857343709142, -3.4179198426302158},
+ {1.08522330151719, -3.438513851083329},
+ {1.0918606604276442, -3.4590716674313335},
+ {1.0988950763314502, -3.4796108027504618},
+ {1.1063250755031675, -3.5001489577245133},
+ {1.114150478614587, -3.5207041306442908},
+ {1.1223724480923383, -3.541294735118319},
+ {1.1309935514178655, -3.561939729880535},
+ {1.1400178428208325, -3.5826587636046994},
+ {1.1494509664724388, -3.6034723383075913},
+ {1.1593002851278627, -3.6244019957930833},
+ {1.1695750392617108, -3.6454705327253025},
+ {1.1802865431772855, -3.6667022514168814},
+ {1.191448426478074, -3.6881232554134113},
+ {1.203076931852669, -3.709761801642497},
+ {1.2151912836117884, -3.731648724559698},
+ {1.2278141462274539, -3.753817952785642},
+ {1.2409721988621754, -3.7763071458245956},
+ {1.2546968614661016, -3.7991584885622527},
+ {1.2690252219158191, -3.82241969589366},
+ {1.2840012342034617, -3.846145301494764},
+ {1.2996772887032604, -3.870398337481948},
+ {1.316116303559539, -3.895252562382127},
+ {1.3333945626553043, -3.920795475499501},
+ {1.3516056511178856, -3.9471324882568375},
+ {1.3708660530144583, -3.974392848721103},
+ {1.3913233553973305, -4.002738316267376},
+ {1.4131687110627962, -4.032376331240737},
+ {1.436656614785, -4.063580905628138},
+ {1.4621380338543308, -4.0967276147972065},
+ {1.4901198983671067, -4.132356427437651},
+ {1.5213822452925796, -4.171295431810658},
+ {1.5572408030205178, -4.214938203914619},
+ {1.6002650085871786, -4.266002359325245},
+ {1.657096323745671, -4.331507506063609},
+ {1.7977393629734166, -4.48544594420617},
+ {1.8577014383575772, -4.546720690281509},
+ {1.8577014383575772, -5.497787143782138},
+ {1.2663322229932439, -5.497787143782138},
+ {1.2656247456808565, -5.4967516608259},
+ {1.2287034601894442, -5.445751427768824},
+ {1.1786904071656892, -5.381167803357418},
+ {1.0497616572686415, -5.233423649910944},
+ {0.9097522267255194, -5.096760177110545},
+ {0.8484431816837388, -5.043472544717165},
+ {0.8000049977037023, -5.003989210148714},
+ {0.7581818853313602, -4.9717075285444},
+ {0.7205493452982701, -4.944073122571691},
+ {0.6882849734317291, -4.921474754757885},
+ {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}});
+}
+
+BoundsCheck MakeClippedArmSpace() {
+ return MakeArbitraryArmSpace({{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}});
+}
+
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/python/arm_bounds.h b/y2018/control_loops/python/arm_bounds.h
new file mode 100644
index 0000000..b69f661
--- /dev/null
+++ b/y2018/control_loops/python/arm_bounds.h
@@ -0,0 +1,246 @@
+#ifndef Y2018_CONTORL_LOOPS_PYTHON_ARM_BOUNDS_H_
+#define Y2018_CONTORL_LOOPS_PYTHON_ARM_BOUNDS_H_
+
+#include <CGAL/Bbox_2.h>
+#include <CGAL/Boolean_set_operations_2.h>
+#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Polygon_2_algorithms.h>
+#include <CGAL/Polygon_with_holes_2.h>
+#include <CGAL/squared_distance_2.h>
+
+#include <Eigen/Dense>
+
+// Prototype level code to find the nearest point and distance to a polygon.
+
+namespace y2018 {
+namespace control_loops {
+
+typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
+typedef K::Point_2 Point;
+typedef K::Segment_2 Segment;
+typedef CGAL::Bbox_2 Bbox;
+typedef CGAL::Polygon_2<K> SimplePolygon;
+typedef CGAL::Polygon_with_holes_2<K> Polygon;
+typedef K::Line_2 Line;
+typedef K::Vector_2 Vector;
+
+
+// Returns true if the point p3 is to the left of the vector from p1 to p2.
+inline bool is_left(Point p1, Point p2, Point p3) {
+ switch (CGAL::orientation(p1, p2, p3)) {
+ case CGAL::LEFT_TURN:
+ case CGAL::COLLINEAR:
+ return true;
+ case CGAL::RIGHT_TURN:
+ return false;
+ }
+}
+
+// Returns true if the segments intersect.
+inline bool intersects(Segment s1, Segment s2) {
+ return CGAL::do_intersect(s1, s2);
+}
+
+class BoundsCheck {
+ public:
+ BoundsCheck(const std::vector<Point> &points)
+ : points_(points), grid_(points_, 6) {}
+
+ double min_distance(Point point, ::Eigen::Matrix<double, 2, 1> *normal) const;
+
+ const std::vector<Point> &points() const { return points_; }
+
+ private:
+ static Bbox ToBbox(const std::vector<Point> &points) {
+ Bbox out;
+ out += Segment(points.back(), points.front()).bbox();
+ for (size_t i = 0; i < points.size() - 1; ++i) {
+ out += Segment(points[i], points[i + 1]).bbox();
+ }
+ return out;
+ }
+
+ static SimplePolygon ToPolygon(Bbox bbox) {
+ Point points[4]{{bbox.xmin(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymax()},
+ {bbox.xmin(), bbox.ymax()}};
+ return SimplePolygon(&points[0], &points[4]);
+ }
+
+ static double min_dist(Point pt, const std::vector<Point> &points,
+ Segment *best_segment) {
+ *best_segment = Segment(points.back(), points.front());
+ double min_dist_sqr = CGAL::squared_distance(pt, *best_segment);
+ for (size_t i = 0; i < points.size() - 1; ++i) {
+ Segment s(points[i], points[i + 1]);
+ double segment_distance = CGAL::squared_distance(pt, s);
+ if (segment_distance < min_dist_sqr) {
+ min_dist_sqr = segment_distance;
+ *best_segment = s;
+ }
+ }
+ return sqrt(min_dist_sqr);
+ }
+
+ static std::vector<Segment> ToSegment(Bbox bbox) {
+ Point points[4]{{bbox.xmin(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymax()},
+ {bbox.xmin(), bbox.ymax()}};
+
+ return std::vector<Segment>({{points[0], points[1]},
+ {points[1], points[2]},
+ {points[2], points[3]},
+ {points[3], points[0]}});
+ }
+
+ static bool check_inside(Point pt, const std::vector<Point> &points) {
+ switch (CGAL::bounded_side_2(&points[0], &points[points.size()], pt, K())) {
+ case CGAL::ON_BOUNDED_SIDE:
+ case CGAL::ON_BOUNDARY:
+ return true;
+ case CGAL::ON_UNBOUNDED_SIDE:
+ return false;
+ }
+ return false;
+ }
+
+ const std::vector<Point> points_;
+
+ class GridCell {
+ public:
+ GridCell(const std::vector<Point> &points, Bbox bbox) {
+ bool has_intersect = false;
+
+ Point center{(bbox.xmin() + bbox.xmax()) / 2,
+ (bbox.ymin() + bbox.ymax()) / 2};
+ // Purposefully overestimate.
+ double r = bbox.ymax() - bbox.ymin();
+
+ Segment best_segment;
+ double best = min_dist(center, points, &best_segment);
+ dist_upper_bound_ = best + 2 * r;
+ dist_lower_bound_ = std::max(best - 2 * r, 0.0);
+
+ double sq_upper_bound = dist_upper_bound_ * dist_upper_bound_;
+
+ auto try_add_segment = [&](Segment segment) {
+ for (const auto &bbox_segment : ToSegment(bbox)) {
+ if (CGAL::do_intersect(bbox_segment, segment)) {
+ has_intersect = true;
+ }
+ }
+
+ double dist_sqr = CGAL::squared_distance(center, segment);
+ if (dist_sqr < sq_upper_bound) {
+ segments_.push_back(segment);
+ }
+ };
+
+ try_add_segment(Segment(points.back(), points.front()));
+ for (size_t i = 0; i < points.size() - 1; ++i) {
+ try_add_segment(Segment(points[i], points[i + 1]));
+ }
+ if (has_intersect) {
+ is_borderline = true;
+ } else {
+ is_inside = check_inside(center, points);
+ }
+ }
+
+ bool IsInside(Point pt) const {
+ (void)pt;
+ return is_inside;
+ }
+
+ bool IsBorderline() const { return is_borderline; }
+
+ double DistanceSqr(Point pt, Segment *best_segment) const {
+ double min_dist_sqr = CGAL::squared_distance(pt, segments_[0]);
+ *best_segment = segments_[0];
+ for (size_t i = 1; i < segments_.size(); ++i) {
+ double new_distance = CGAL::squared_distance(pt, segments_[i]);
+ if (new_distance < min_dist_sqr) {
+ min_dist_sqr = new_distance;
+ *best_segment = segments_[i];
+ }
+ }
+ return min_dist_sqr;
+ }
+ double Distance(Point pt, Segment *best_segment) const {
+ return sqrt(DistanceSqr(pt, best_segment));
+ }
+
+ bool is_inside = false;
+ bool is_borderline = false;
+ double dist_upper_bound_;
+ double dist_lower_bound_;
+ std::vector<Segment> segments_;
+ std::vector<std::vector<Point>> polygons_;
+ };
+
+ class GridSystem {
+ public:
+ // Precision is really 2**-precision and must be positive.
+ GridSystem(const std::vector<Point> &points, int precision)
+ : points_(points), scale_factor_(1 << precision) {
+ auto bbox = ToBbox(points);
+ fprintf(stderr, "%g %g, %g %g\n", bbox.xmin(), bbox.ymin(), bbox.xmax(),
+ bbox.ymax());
+ x_min_ = static_cast<int>(std::floor(bbox.xmin() * scale_factor_)) - 1;
+ y_min_ = static_cast<int>(std::floor(bbox.ymin() * scale_factor_)) - 1;
+
+ stride_ = static_cast<int>(bbox.xmax() * scale_factor_) + 3 - x_min_;
+ height_ = static_cast<int>(bbox.ymax() * scale_factor_) + 3 - y_min_;
+
+ fprintf(stderr, "num_cells: %d\n", stride_ * height_);
+ cells_.reserve(stride_ * height_);
+ for (int y_cell = 0; y_cell < height_; ++y_cell) {
+ for (int x_cell = 0; x_cell < stride_; ++x_cell) {
+ cells_.push_back(
+ GridCell(points, Bbox(static_cast<double>(x_cell + x_min_) /
+ static_cast<double>(scale_factor_),
+ static_cast<double>(y_cell + y_min_) /
+ static_cast<double>(scale_factor_),
+ static_cast<double>(x_cell + x_min_ + 1) /
+ static_cast<double>(scale_factor_),
+ static_cast<double>(y_cell + y_min_ + 1) /
+ static_cast<double>(scale_factor_))));
+ }
+ }
+ }
+
+ const GridCell *GetCell(Point pt) const {
+ int x_cell =
+ static_cast<int>(std::floor(pt.x() * scale_factor_)) - x_min_;
+ int y_cell =
+ static_cast<int>(std::floor(pt.y() * scale_factor_)) - y_min_;
+ if (x_cell < 0 || x_cell >= stride_) return nullptr;
+ if (y_cell < 0 || y_cell >= height_) return nullptr;
+ return &cells_[stride_ * y_cell + x_cell];
+ }
+
+ const std::vector<Point> &points() const { return points_; }
+
+ private:
+ std::vector<Point> points_;
+ int scale_factor_;
+ int x_min_;
+ int y_min_;
+ int stride_;
+ int height_;
+ std::vector<GridCell> cells_;
+ };
+
+ GridSystem grid_;
+};
+
+BoundsCheck MakeClippedArmSpace();
+BoundsCheck MakeFullArmSpace();
+
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTORL_LOOPS_PYTHON_ARM_BOUNDS_H_
diff --git a/y2018/control_loops/python/arm_mpc.cc b/y2018/control_loops/python/arm_mpc.cc
index 2727545..a032cae 100644
--- a/y2018/control_loops/python/arm_mpc.cc
+++ b/y2018/control_loops/python/arm_mpc.cc
@@ -3,14 +3,112 @@
#include <thread>
#include <ct/optcon/optcon.h>
+#include <Eigen/Eigenvalues>
#include "third_party/gflags/include/gflags/gflags.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/python/arm_bounds.h"
+#include "y2018/control_loops/python/dlqr.h"
-DEFINE_double(boundary_scalar, 12.0, "Test command-line flag");
-DEFINE_double(boundary_rate, 25.0, "Sigmoid rate");
-DEFINE_bool(sigmoid, true, "If true, sigmoid, else exponential.");
+DEFINE_double(boundary_scalar, 1500.0, "Test command-line flag");
+DEFINE_double(velocity_boundary_scalar, 10.0, "Test command-line flag");
+DEFINE_double(boundary_rate, 20.0, "Sigmoid rate");
+DEFINE_bool(linear, false, "If true, linear, else see sigmoid.");
+DEFINE_bool(sigmoid, false, "If true, sigmoid, else exponential.");
DEFINE_double(round_corner, 0.0, "Corner radius of the constraint box.");
+DEFINE_double(convergance, 1e-12, "Residual before finishing the solver.");
+DEFINE_double(position_allowance, 5.0,
+ "Distance to Velocity at which we have 0 penalty conversion.");
+DEFINE_double(bounds_offset, 0.02, "Offset the quadratic boundary in by this");
+DEFINE_double(linear_bounds_offset, 0.00,
+ "Offset the linear boundary in by this");
+DEFINE_double(yrange, 1.0,
+ "+- y max for saturating out the state for the cost function.");
+DEFINE_bool(debug_print, false, "Print the debugging print from the solver.");
+DEFINE_bool(print_starting_summary, true,
+ "Print the summary on the pre-solution.");
+DEFINE_bool(print_summary, false, "Print the summary on each iteration.");
+DEFINE_bool(quadratic, true, "If true, quadratic bounds penalty.");
+
+DEFINE_bool(reset_every_cycle, false,
+ "If true, reset the initial guess every cycle.");
+
+DEFINE_double(seconds, 1.5, "The number of seconds to simulate.");
+
+DEFINE_double(theta0, 1.0, "Starting theta0");
+DEFINE_double(theta1, 0.9, "Starting theta1");
+
+DEFINE_double(goal_theta0, -0.5, "Starting theta0");
+DEFINE_double(goal_theta1, -0.5, "Starting theta1");
+
+DEFINE_double(qpos1, 0.2, "qpos1");
+DEFINE_double(qvel1, 4.0, "qvel1");
+DEFINE_double(qpos2, 0.2, "qpos2");
+DEFINE_double(qvel2, 4.0, "qvel2");
+
+DEFINE_double(u_over_linear, 0.0, "Linear penalty for too much U.");
+DEFINE_double(u_over_quadratic, 4.0, "Quadratic penalty for too much U.");
+
+DEFINE_double(time_horizon, 0.75, "MPC time horizon");
+
+DEFINE_bool(only_print_eigenvalues, false,
+ "If true, stop after computing the final eigenvalues");
+
+DEFINE_bool(plot_xy, false, "If true, plot the xy trajectory of the end of the arm.");
+DEFINE_bool(plot_cost, false, "If true, plot the cost function.");
+DEFINE_bool(plot_state_cost, false,
+ "If true, plot the state portion of the cost function.");
+DEFINE_bool(plot_states, false, "If true, plot the states.");
+DEFINE_bool(plot_u, false, "If true, plot the control signal.");
+
+static constexpr double kDt = 0.00505;
+
+namespace y2018 {
+namespace control_loops {
+
+::Eigen::Matrix<double, 4, 4> NumericalJacobianX(
+ ::Eigen::Matrix<double, 4, 1> (*fn)(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt),
+ ::Eigen::Matrix<double, 4, 1> X, ::Eigen::Matrix<double, 2, 1> U, double dt,
+ const double kEpsilon = 1e-4) {
+ constexpr int num_states = 4;
+ ::Eigen::Matrix<double, 4, 4> answer = ::Eigen::Matrix<double, 4, 4>::Zero();
+
+ // It's more expensive, but +- epsilon will be more reliable
+ for (int i = 0; i < num_states; ++i) {
+ ::Eigen::Matrix<double, 4, 1> dX_plus = X;
+ dX_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, 4, 1> dX_minus = X;
+ dX_minus(i, 0) -= kEpsilon;
+ answer.block<4, 1>(0, i) =
+ (fn(dX_plus, U, dt) - fn(dX_minus, U, dt)) / kEpsilon / 2.0;
+ }
+ return answer;
+}
+
+::Eigen::Matrix<double, 4, 2> NumericalJacobianU(
+ ::Eigen::Matrix<double, 4, 1> (*fn)(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt),
+ ::Eigen::Matrix<double, 4, 1> X, ::Eigen::Matrix<double, 2, 1> U, double dt,
+ const double kEpsilon = 1e-4) {
+ constexpr int num_states = 4;
+ constexpr int num_inputs = 2;
+ ::Eigen::Matrix<double, num_states, num_inputs> answer =
+ ::Eigen::Matrix<double, num_states, num_inputs>::Zero();
+
+ // It's more expensive, but +- epsilon will be more reliable
+ for (int i = 0; i < num_inputs; ++i) {
+ ::Eigen::Matrix<double, 2, 1> dU_plus = U;
+ dU_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, 2, 1> dU_minus = U;
+ dU_minus(i, 0) -= kEpsilon;
+ answer.block<4, 1>(0, i) =
+ (fn(X, dU_plus, dt) - fn(X, dU_minus, dt)) / kEpsilon / 2.0;
+ }
+ return answer;
+}
// This code is for analysis and simulation of a double jointed arm. It is an
// attempt to see if a MPC could work for arm control under constraints.
@@ -38,6 +136,29 @@
}
virtual ~MySecondOrderSystem() {}
+ static constexpr SCALAR l1 = 46.25 * 0.0254;
+ static constexpr SCALAR l2 = 41.80 * 0.0254;
+
+ static constexpr SCALAR m1 = 9.34 / 2.2;
+ static constexpr SCALAR m2 = 9.77 / 2.2;
+
+ static constexpr SCALAR J1 = 2957.05 * 0.0002932545454545454;
+ static constexpr SCALAR J2 = 2824.70 * 0.0002932545454545454;
+
+ static constexpr SCALAR r1 = 21.64 * 0.0254;
+ static constexpr SCALAR r2 = 26.70 * 0.0254;
+
+ static constexpr SCALAR G1 = 140.0;
+ static constexpr SCALAR G2 = 90.0;
+
+ static constexpr SCALAR stall_torque = 1.41;
+ static constexpr SCALAR free_speed = (5840.0 / 60.0) * 2.0 * M_PI;
+ static constexpr SCALAR stall_current = 89.0;
+ static constexpr SCALAR R = 12.0 / stall_current;
+
+ static constexpr SCALAR Kv = free_speed / 12.0;
+ static constexpr SCALAR Kt = stall_torque / stall_current;
+
// Evaluate the system dynamics.
//
// Args:
@@ -49,11 +170,312 @@
const ::ct::core::StateVector<4, SCALAR> &state, const SCALAR & /*t*/,
const ::ct::core::ControlVector<2, SCALAR> &control,
::ct::core::StateVector<4, SCALAR> &derivative) override {
- derivative(0) = state(1);
- derivative(1) = control(0);
- derivative(2) = state(3);
- derivative(3) = control(1);
+ derivative = Dynamics(state, control);
}
+
+ static ::Eigen::Matrix<double, 4, 1> Dynamics(
+ const ::ct::core::StateVector<4, SCALAR> &X,
+ const ::ct::core::ControlVector<2, SCALAR> &U) {
+ ::ct::core::StateVector<4, SCALAR> derivative;
+ const SCALAR alpha = J1 + r1 * r1 * m1 + l1 * l1 * m2;
+ const SCALAR beta = l1 * r2 * m2;
+ const SCALAR gamma = J2 + r2 * r2 * m2;
+
+ const SCALAR s = sin(X(0) - X(2));
+ const SCALAR c = cos(X(0) - X(2));
+
+ // K1 * d^2 theta/dt^2 + K2 * d theta/dt = torque
+ ::Eigen::Matrix<SCALAR, 2, 2> K1;
+ K1(0, 0) = alpha;
+ K1(1, 0) = K1(0, 1) = c * beta;
+ K1(1, 1) = gamma;
+
+ ::Eigen::Matrix<SCALAR, 2, 2> K2 = ::Eigen::Matrix<SCALAR, 2, 2>::Zero();
+ K2(0, 1) = s * beta * X(3);
+ K2(1, 0) = -s * beta * X(1);
+
+ const SCALAR kNumDistalMotors = 2.0;
+ ::Eigen::Matrix<SCALAR, 2, 1> torque;
+ torque(0, 0) = G1 * (U(0) * Kt / R - X(1) * G1 * Kt / (Kv * R));
+ torque(1, 0) = G2 * (U(1) * kNumDistalMotors * Kt / R -
+ X(3) * G2 * Kt * kNumDistalMotors / (Kv * R));
+
+ ::Eigen::Matrix<SCALAR, 2, 1> velocity;
+ velocity(0, 0) = X(0);
+ velocity(1, 0) = X(2);
+
+ const ::Eigen::Matrix<SCALAR, 2, 1> accel =
+ K1.inverse() * (torque - K2 * velocity);
+
+ derivative(0) = X(1);
+ derivative(1) = accel(0);
+ derivative(2) = X(3);
+ derivative(3) = accel(1);
+
+ return derivative;
+ }
+
+ // Runge-Kutta.
+ static ::Eigen::Matrix<double, 4, 1> DiscreteDynamics(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt) {
+ const double half_dt = dt * 0.5;
+ ::Eigen::Matrix<double, 4, 1> k1 = Dynamics(X, U);
+ ::Eigen::Matrix<double, 4, 1> k2 = Dynamics(X + half_dt * k1, U);
+ ::Eigen::Matrix<double, 4, 1> k3 = Dynamics(X + half_dt * k2, U);
+ ::Eigen::Matrix<double, 4, 1> k4 = Dynamics(X + dt * k3, U);
+ return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+ }
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
+ typename SCALAR = SCALAR_EVAL>
+class ObstacleAwareQuadraticCost
+ : public ::ct::optcon::TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL,
+ SCALAR> {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> state_vector_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
+ control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>
+ state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>
+ control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
+ control_state_matrix_double_t;
+
+ ObstacleAwareQuadraticCost(const ::Eigen::Matrix<double, 2, 2> &R,
+ const ::Eigen::Matrix<double, 4, 4> &Q)
+ : R_(R), Q_(Q) {}
+
+ ObstacleAwareQuadraticCost(const ObstacleAwareQuadraticCost &arg)
+ : R_(arg.R_), Q_(arg.Q_) {}
+ static constexpr double kEpsilon = 1.0e-5;
+
+ virtual ~ObstacleAwareQuadraticCost() {}
+
+ ObstacleAwareQuadraticCost<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+ *clone() const override {
+ return new ObstacleAwareQuadraticCost(*this);
+ }
+
+ double SaturateX(double x, double yrange) {
+ return 2.0 * ((1.0 / (1.0 + ::std::exp(-x * 2.0 / yrange)) - 0.5)) * yrange;
+ }
+
+ SCALAR distance(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/) {
+ constexpr double kCornerNewUpper0 = 0.35;
+ // constexpr double kCornerUpper1 = 3.13;
+ // Push it up a bit further (non-real) until we have an actual path cost.
+ constexpr double kCornerNewUpper1 = 3.39;
+ constexpr double kCornerNewUpper0_far = 10.0;
+
+ // Push it up a bit further (non-real) until we have an actual path cost.
+ // constexpr double kCornerUpper0 = 0.315;
+ constexpr double kCornerUpper0 = 0.310;
+ // constexpr double kCornerUpper1 = 3.13;
+ constexpr double kCornerUpper1 = 3.25;
+ constexpr double kCornerUpper0_far = 10.0;
+
+ constexpr double kCornerLower0 = 0.023;
+ constexpr double kCornerLower1 = 1.57;
+ constexpr double kCornerLower0_far = 10.0;
+
+ const Segment new_upper_segment(
+ Point(kCornerNewUpper0, kCornerNewUpper1),
+ Point(kCornerNewUpper0_far, kCornerNewUpper1));
+ const Segment upper_segment(Point(kCornerUpper0, kCornerUpper1),
+ Point(kCornerUpper0_far, kCornerUpper1));
+ const Segment lower_segment(Point(kCornerLower0, kCornerLower1),
+ Point(kCornerLower0_far, kCornerLower1));
+
+ Point current_point(x(0, 0), x(2, 0));
+
+ SCALAR result = 0.0;
+ if (intersects(new_upper_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerNewUpper0,
+ current_point.y() - kCornerNewUpper1);
+ current_point = Point(kCornerNewUpper0, kCornerNewUpper1);
+ }
+
+ if (intersects(upper_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerUpper0,
+ current_point.y() - kCornerUpper1);
+ current_point = Point(kCornerUpper0, kCornerUpper1);
+ }
+
+ if (intersects(lower_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerLower0,
+ current_point.y() - kCornerLower1);
+ current_point = Point(kCornerLower0, kCornerLower1);
+ }
+ result += hypot(current_point.x() - FLAGS_goal_theta0,
+ current_point.y() - FLAGS_goal_theta1);
+ return result;
+ }
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR & /*t*/) override {
+ // Positive means violation.
+ Eigen::Matrix<SCALAR, STATE_DIM, 1> saturated_x = x;
+ SCALAR d = distance(x, u);
+ saturated_x(0, 0) = d;
+ saturated_x(2, 0) = 0.0;
+
+ saturated_x(0, 0) = SaturateX(saturated_x(0, 0), FLAGS_yrange);
+ saturated_x(2, 0) = 0.0;
+
+ //SCALAR saturation_scalar = saturated_x(0, 0) / d;
+ //saturated_x(1, 0) *= saturation_scalar;
+ //saturated_x(3, 0) *= saturation_scalar;
+
+ SCALAR result = (saturated_x.transpose() * Q_ * saturated_x +
+ u.transpose() * R_ * u)(0, 0);
+
+ if (::std::abs(u(0, 0)) > 11.0) {
+ result += (::std::abs(u(0, 0)) - 11.0) * FLAGS_u_over_linear;
+ result += (::std::abs(u(0, 0)) - 11.0) * (::std::abs(u(0, 0)) - 11.0) *
+ FLAGS_u_over_quadratic;
+ }
+ if (::std::abs(u(1, 0)) > 11.0) {
+ result += (::std::abs(u(1, 0)) - 11.0) * FLAGS_u_over_linear;
+ result += (::std::abs(u(1, 0)) - 11.0) * (::std::abs(u(1, 0)) - 11.0) *
+ FLAGS_u_over_quadratic;
+ }
+ return result;
+ }
+
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> result =
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL>::Zero();
+
+ // Perterb x for both position axis and return the result.
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
+ plus_perterbed_x[i] += epsilon;
+ minus_perterbed_x[i] -= epsilon;
+ result[i] = (evaluate(plus_perterbed_x, u, t) -
+ evaluate(minus_perterbed_x, u, t)) /
+ (epsilon * 2.0);
+ }
+ return result;
+ }
+
+ // Compute second order derivative of this cost term w.r.t. the state
+ state_matrix_t stateSecondDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ state_matrix_t result = state_matrix_t::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ // Perterb x a second time.
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
+ plus_perterbed_x[i] += epsilon;
+ minus_perterbed_x[i] -= epsilon;
+ state_vector_t delta = (stateDerivative(plus_perterbed_x, u, t) -
+ stateDerivative(minus_perterbed_x, u, t)) /
+ (epsilon * 2.0);
+
+ result.col(i) = delta;
+ }
+ //::std::cout << "Q_numeric " << result << " endQ" << ::std::endl;
+ return result;
+ }
+
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> result =
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ // Perterb x a second time.
+ for (size_t i = 0; i < CONTROL_DIM; i += 1) {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> plus_perterbed_u = u;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> minus_perterbed_u = u;
+ plus_perterbed_u[i] += epsilon;
+ minus_perterbed_u[i] -= epsilon;
+ SCALAR delta = (evaluate(x, plus_perterbed_u, t) -
+ evaluate(x, minus_perterbed_u, t)) /
+ (epsilon * 2.0);
+
+ result[i] = delta;
+ }
+ //::std::cout << "cd " << result(0, 0) << " " << result(1, 0) << " endcd"
+ //<< ::std::endl;
+
+ return result;
+ }
+
+ control_state_matrix_t stateControlDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> & /*x*/,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> & /*u*/,
+ const SCALAR_EVAL & /*t*/) override {
+ // No coupling here, so let's not bother to calculate it.
+ control_state_matrix_t result = control_state_matrix_t::Zero();
+ return result;
+ }
+
+ control_matrix_t controlSecondDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ control_matrix_t result = control_matrix_t::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ //static int j = 0;
+ //::std::this_thread::sleep_for(::std::chrono::milliseconds(j % 10));
+ //int k = ++j;
+ // Perterb x a second time.
+ for (size_t i = 0; i < CONTROL_DIM; i += 1) {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> plus_perterbed_u = u;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> minus_perterbed_u = u;
+ plus_perterbed_u[i] += epsilon;
+ minus_perterbed_u[i] -= epsilon;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> delta =
+ (controlDerivative(x, plus_perterbed_u, t) -
+ controlDerivative(x, minus_perterbed_u, t)) /
+ (epsilon * 2.0);
+
+ //::std::cout << "delta: " << delta(0, 0) << " " << delta(1, 0) << " k "
+ //<< k << ::std::endl;
+ result.col(i) = delta;
+ }
+ //::std::cout << "R_numeric " << result << " endR 0.013888888888888888 k:" << k
+ //<< ::std::endl;
+ //::std::cout << "x " << x << " u " << u << " k " << k << ::std::endl;
+
+ return result;
+ }
+
+ private:
+ const ::Eigen::Matrix<double, 2, 2> R_;
+ const ::Eigen::Matrix<double, 4, 4> Q_;
};
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
@@ -75,11 +497,12 @@
typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
control_state_matrix_double_t;
- MyTermStateBarrier() {}
+ MyTermStateBarrier(BoundsCheck *bounds_check) : bounds_check_(bounds_check) {}
- MyTermStateBarrier(const MyTermStateBarrier & /*arg*/) {}
+ MyTermStateBarrier(const MyTermStateBarrier &arg)
+ : bounds_check_(arg.bounds_check_) {}
- static constexpr double kEpsilon = 1.0e-7;
+ static constexpr double kEpsilon = 5.0e-6;
virtual ~MyTermStateBarrier() {}
@@ -88,36 +511,57 @@
return new MyTermStateBarrier(*this);
}
+ SCALAR distance(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/,
+ const SCALAR & /*t*/, Eigen::Matrix<SCALAR, 2, 1> *norm) {
+ return bounds_check_->min_distance(Point(x(0, 0), x(2, 0)), norm);
+ }
+
virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
- const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/,
- const SCALAR & /*t*/) override {
- SCALAR min_distance;
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & u,
+ const SCALAR & t) override {
+ Eigen::Matrix<SCALAR, 2, 1> norm = Eigen::Matrix<SCALAR, 2, 1>::Zero();
+ SCALAR min_distance = distance(x, u, t, &norm);
- // Round the corner by this amount.
- SCALAR round_corner = SCALAR(FLAGS_round_corner);
+ // Velocity component (+) towards the wall.
+ SCALAR velocity_penalty = -(x(1, 0) * norm(0, 0) + x(3, 0) * norm(1, 0));
+ if (min_distance + FLAGS_bounds_offset < 0.0) {
+ velocity_penalty = 0.0;
+ }
- // Positive means violation.
- SCALAR theta0_distance = x(0, 0) - (0.5 + round_corner);
- SCALAR theta1_distance = (0.8 - round_corner) - x(2, 0);
- if (theta0_distance < SCALAR(0.0) && theta1_distance < SCALAR(0.0)) {
- // Ok, both outside. Return corner distance.
- min_distance = -hypot(theta1_distance, theta0_distance);
- } else if (theta0_distance < SCALAR(0.0) && theta1_distance > SCALAR(0.0)) {
- min_distance = theta0_distance;
- } else if (theta0_distance > SCALAR(0.0) && theta1_distance < SCALAR(0.0)) {
- min_distance = theta1_distance;
- } else {
- min_distance = ::std::min(theta0_distance, theta1_distance);
- }
- min_distance += round_corner;
- if (FLAGS_sigmoid) {
- return FLAGS_boundary_scalar /
- (1.0 + ::std::exp(-min_distance * FLAGS_boundary_rate));
- } else {
- // Values of 4 and 15 work semi resonably.
- return FLAGS_boundary_scalar *
- ::std::exp(min_distance * FLAGS_boundary_rate);
- }
+ SCALAR result;
+ //if (FLAGS_quadratic) {
+ result = FLAGS_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) +
+ FLAGS_boundary_rate *
+ ::std::max(0.0, min_distance + FLAGS_linear_bounds_offset) +
+ FLAGS_velocity_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_linear_bounds_offset) *
+ ::std::max(0.0, velocity_penalty) *
+ ::std::max(0.0, velocity_penalty);
+ /*
+} else if (FLAGS_linear) {
+result =
+FLAGS_boundary_scalar * ::std::max(0.0, min_distance) +
+FLAGS_velocity_boundary_scalar * ::std::max(0.0, -velocity_penalty);
+} else if (FLAGS_sigmoid) {
+result = FLAGS_boundary_scalar /
+ (1.0 + ::std::exp(-min_distance * FLAGS_boundary_rate)) +
+FLAGS_velocity_boundary_scalar /
+ (1.0 + ::std::exp(-velocity_penalty * FLAGS_boundary_rate));
+} else {
+// Values of 4 and 15 work semi resonably.
+result = FLAGS_boundary_scalar *
+ ::std::exp(min_distance * FLAGS_boundary_rate) +
+FLAGS_velocity_boundary_scalar *
+ ::std::exp(velocity_penalty * FLAGS_boundary_rate);
+}
+if (result < 0.0) {
+printf("Result negative %f\n", result);
+}
+*/
+ return result;
}
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
@@ -152,7 +596,7 @@
SCALAR epsilon = SCALAR(kEpsilon);
// Perturb x a second time.
- for (size_t i = 0; i < STATE_DIM; i += 2) {
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
plus_perterbed_x[i] += epsilon;
@@ -202,11 +646,14 @@
return c;
}
*/
+
+ BoundsCheck *bounds_check_;
};
-int main(int argc, char **argv) {
- gflags::ParseCommandLineFlags(&argc, &argv, false);
- // PRELIMINIARIES, see example NLOC.cpp
+int Main() {
+ // PRELIMINIARIES
+ BoundsCheck arm_space = MakeClippedArmSpace();
+
constexpr size_t state_dim = MySecondOrderSystem<double>::STATE_DIM;
constexpr size_t control_dim = MySecondOrderSystem<double>::CONTROL_DIM;
@@ -217,32 +664,64 @@
ad_linearizer(new ::ct::core::SystemLinearizer<state_dim, control_dim>(
oscillator_dynamics));
- constexpr double kQPos = 0.5;
- constexpr double kQVel = 1.65;
+ const double kQPos1 = FLAGS_qpos1;
+ const double kQVel1 = FLAGS_qvel1;
+ const double kQPos2 = FLAGS_qpos2;
+ const double kQVel2 = FLAGS_qvel2;
+
::Eigen::Matrix<double, 4, 4> Q_step;
- Q_step << 1.0 / (kQPos * kQPos), 0.0, 0.0, 0.0, 0.0, 1.0 / (kQVel * kQVel),
- 0.0, 0.0, 0.0, 0.0, 1.0 / (kQPos * kQPos), 0.0, 0.0, 0.0, 0.0,
- 1.0 / (kQVel * kQVel);
+ Q_step << 1.0 / (kQPos1 * kQPos1), 0.0, 0.0, 0.0, 0.0,
+ 1.0 / (kQVel1 * kQVel1), 0.0, 0.0, 0.0, 0.0, 1.0 / (kQPos2 * kQPos2), 0.0,
+ 0.0, 0.0, 0.0, 1.0 / (kQVel2 * kQVel2);
::Eigen::Matrix<double, 2, 2> R_step;
R_step << 1.0 / (12.0 * 12.0), 0.0, 0.0, 1.0 / (12.0 * 12.0);
- ::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
- intermediate_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
- Q_step, R_step));
+ ::std::shared_ptr<::ct::optcon::TermQuadratic<state_dim, control_dim>>
+ quadratic_intermediate_cost(
+ new ::ct::optcon::TermQuadratic<state_dim, control_dim>(Q_step,
+ R_step));
+ // TODO(austin): Move back to this with the new Q and R
+ ::std::shared_ptr<ObstacleAwareQuadraticCost<state_dim, control_dim>>
+ intermediate_cost(new ObstacleAwareQuadraticCost<4, 2>(R_step, Q_step));
- // TODO(austin): DARE for these.
- ::Eigen::Matrix<double, 4, 4> Q_final = Q_step;
- ::Eigen::Matrix<double, 2, 2> R_final = R_step;
+ ::Eigen::Matrix<double, 4, 4> final_A =
+ NumericalJacobianX(MySecondOrderSystem<double>::DiscreteDynamics,
+ Eigen::Matrix<double, 4, 1>::Zero(),
+ Eigen::Matrix<double, 2, 1>::Zero(), kDt);
+
+ ::Eigen::Matrix<double, 4, 2> final_B =
+ NumericalJacobianU(MySecondOrderSystem<double>::DiscreteDynamics,
+ Eigen::Matrix<double, 4, 1>::Zero(),
+ Eigen::Matrix<double, 2, 1>::Zero(), kDt);
+
+ ::Eigen::Matrix<double, 4, 4> S_lqr;
+ ::Eigen::Matrix<double, 2, 4> K_lqr;
+ ::frc971::controls::dlqr(final_A, final_B, Q_step, R_step, &K_lqr, &S_lqr);
+ ::std::cout << "A -> " << ::std::endl << final_A << ::std::endl;
+ ::std::cout << "B -> " << ::std::endl << final_B << ::std::endl;
+ ::std::cout << "K -> " << ::std::endl << K_lqr << ::std::endl;
+ ::std::cout << "S -> " << ::std::endl << S_lqr << ::std::endl;
+ ::std::cout << "Q -> " << ::std::endl << Q_step << ::std::endl;
+ ::std::cout << "R -> " << ::std::endl << R_step << ::std::endl;
+ ::std::cout << "Eigenvalues: " << (final_A - final_B * K_lqr).eigenvalues()
+ << ::std::endl;
+
+ ::Eigen::Matrix<double, 4, 4> Q_final = 0.5 * S_lqr;
+ ::Eigen::Matrix<double, 2, 2> R_final = ::Eigen::Matrix<double, 2, 2>::Zero();
::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
final_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
Q_final, R_final));
+ if (FLAGS_only_print_eigenvalues) {
+ return 0;
+ }
- ::std::shared_ptr<ct::optcon::TermBase<state_dim, control_dim>> bounds_cost(
- new MyTermStateBarrier<4, 2>());
+ ::std::shared_ptr<MyTermStateBarrier<state_dim, control_dim>> bounds_cost(
+ new MyTermStateBarrier<4, 2>(&arm_space));
// TODO(austin): Cost function needs constraints.
::std::shared_ptr<::ct::optcon::CostFunctionQuadratic<state_dim, control_dim>>
cost_function(
new ::ct::optcon::CostFunctionAnalytical<state_dim, control_dim>());
+ //cost_function->addIntermediateTerm(quadratic_intermediate_cost);
cost_function->addIntermediateTerm(intermediate_cost);
cost_function->addIntermediateTerm(bounds_cost);
cost_function->addFinalTerm(final_cost);
@@ -254,8 +733,8 @@
Eigen::VectorXd u_ub(control_dim);
u_ub.setConstant(12.0);
u_lb = -u_ub;
- ::std::cout << "uub " << u_ub << ::std::endl;
- ::std::cout << "ulb " << u_lb << ::std::endl;
+ //::std::cout << "uub " << u_ub << ::std::endl;
+ //::std::cout << "ulb " << u_lb << ::std::endl;
// constraint terms
std::shared_ptr<::ct::optcon::ControlInputConstraint<state_dim, control_dim>>
@@ -275,42 +754,45 @@
// Starting point.
::ct::core::StateVector<state_dim> x0;
- x0 << 1.0, 0.0, 0.9, 0.0;
+ x0 << FLAGS_theta0, 0.0, FLAGS_theta1, 0.0;
- constexpr ::ct::core::Time kTimeHorizon = 1.5;
+ const ::ct::core::Time kTimeHorizon = FLAGS_time_horizon;
::ct::optcon::OptConProblem<state_dim, control_dim> opt_con_problem(
kTimeHorizon, x0, oscillator_dynamics, cost_function, ad_linearizer);
::ct::optcon::NLOptConSettings ilqr_settings;
- ilqr_settings.dt = 0.00505; // the control discretization in [sec]
+ ilqr_settings.nThreads = 4;
+ ilqr_settings.dt = kDt; // the control discretization in [sec]
ilqr_settings.integrator = ::ct::core::IntegrationType::RK4;
+ ilqr_settings.debugPrint = FLAGS_debug_print;
ilqr_settings.discretization =
::ct::optcon::NLOptConSettings::APPROXIMATION::FORWARD_EULER;
// ilqr_settings.discretization =
// NLOptConSettings::APPROXIMATION::MATRIX_EXPONENTIAL;
- ilqr_settings.max_iterations = 20;
- ilqr_settings.min_cost_improvement = 1.0e-11;
+ ilqr_settings.max_iterations = 40;
+ ilqr_settings.min_cost_improvement = FLAGS_convergance;
ilqr_settings.nlocp_algorithm =
- ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ //::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::GNMS;
// the LQ-problems are solved using a custom Gauss-Newton Riccati solver
- // ilqr_settings.lqocp_solver =
- // NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
ilqr_settings.lqocp_solver =
- ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER;
- ilqr_settings.printSummary = true;
+ ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ //ilqr_settings.lqocp_solver =
+ //::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER;
+ ilqr_settings.printSummary = FLAGS_print_starting_summary;
if (ilqr_settings.lqocp_solver ==
::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER) {
- opt_con_problem.setBoxConstraints(box_constraints);
+ //opt_con_problem.setBoxConstraints(box_constraints);
}
- size_t K = ilqr_settings.computeK(kTimeHorizon);
- printf("Using %d steps\n", static_cast<int>(K));
+ const size_t num_steps = ilqr_settings.computeK(kTimeHorizon);
+ printf("Using %d steps\n", static_cast<int>(num_steps));
// Vector of feeback matricies.
::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
- K, ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ num_steps, ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
::ct::core::ControlVectorArray<control_dim> u0_ff(
- K, ::ct::core::ControlVector<control_dim>::Zero());
- ::ct::core::StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ num_steps, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(num_steps + 1, x0);
::ct::core::StateFeedbackController<state_dim, control_dim>
initial_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
@@ -336,9 +818,9 @@
// 1) settings for the iLQR instance used in MPC. Of course, we use the same
// settings as for solving the initial problem ...
::ct::optcon::NLOptConSettings ilqr_settings_mpc = ilqr_settings;
- ilqr_settings_mpc.max_iterations = 20;
+ ilqr_settings_mpc.max_iterations = 40;
// and we limited the printouts, too.
- ilqr_settings_mpc.printSummary = false;
+ ilqr_settings_mpc.printSummary = FLAGS_print_summary;
// 2) settings specific to model predictive control. For a more detailed
// description of those, visit ct/optcon/mpc/MpcSettings.h
::ct::optcon::mpc_settings mpc_settings;
@@ -368,7 +850,7 @@
///
auto start_time = ::std::chrono::high_resolution_clock::now();
// limit the maximum number of runs in this example
- size_t maxNumRuns = 400;
+ size_t maxNumRuns = FLAGS_seconds / kDt;
::std::cout << "Starting to run MPC" << ::std::endl;
::std::vector<double> time_array;
@@ -380,6 +862,11 @@
::std::vector<double> u0_array;
::std::vector<double> u1_array;
+ ::std::vector<double> x_array;
+ ::std::vector<double> y_array;
+
+ // TODO(austin): Plot x, y of the end of the arm.
+
for (size_t i = 0; i < maxNumRuns; i++) {
::std::cout << "Solving iteration " << i << ::std::endl;
// Time which has passed since start of MPC
@@ -389,6 +876,25 @@
::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
start_time)
.count();
+ {
+ if (FLAGS_reset_every_cycle) {
+ ::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
+ num_steps,
+ ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ ::ct::core::ControlVectorArray<control_dim> u0_ff(
+ num_steps, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(num_steps + 1, x0);
+ ::ct::core::StateFeedbackController<state_dim, control_dim>
+ resolved_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+ iLQR.setInitialGuess(initial_controller);
+ // we solve the optimal control problem and retrieve the solution
+ iLQR.solve();
+ resolved_controller = iLQR.getSolution();
+ ilqr_mpc.setInitialGuess(resolved_controller);
+ }
+ }
+
// prepare mpc iteration
ilqr_mpc.prepareIteration(t);
// new optimal policy
@@ -402,6 +908,9 @@
::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
start_time)
.count();
+ // TODO(austin): This is only iterating once... I need to fix that...
+ // NLOptConSolver::solve() runs for upto N iterations. This call runs
+ // runIteration() effectively once. (nlocAlgorithm_ is iLQR)
bool success = ilqr_mpc.finishIteration(x0, t, *newPolicy, ts_newPolicy);
// we break the loop in case the time horizon is reached or solve() failed
if (ilqr_mpc.timeHorizonReached() | !success) break;
@@ -427,28 +936,95 @@
ilqr_mpc.doForwardIntegration(0.0, ilqr_settings.dt, x0, newPolicy);
::std::cout << "Next X: " << x0.transpose() << ::std::endl;
+ x_array.push_back(MySecondOrderSystem<double>::l1 * sin(x0(0)) +
+ MySecondOrderSystem<double>::r2 * sin(x0(2)));
+ y_array.push_back(MySecondOrderSystem<double>::l1 * cos(x0(0)) +
+ MySecondOrderSystem<double>::r2 * cos(x0(2)));
+
// TODO(austin): Re-use the policy. Maybe? Or maybe mpc already does that.
}
// The summary contains some statistical data about time delays, etc.
ilqr_mpc.printMpcSummary();
- // Now plot our simulation.
- matplotlibcpp::plot(time_array, theta1_array, {{"label", "theta1"}});
- matplotlibcpp::plot(time_array, omega1_array, {{"label", "omega1"}});
- matplotlibcpp::plot(time_array, theta2_array, {{"label", "theta2"}});
- matplotlibcpp::plot(time_array, omega2_array, {{"label", "omega2"}});
- matplotlibcpp::legend();
+ if (FLAGS_plot_states) {
+ // Now plot our simulation.
+ matplotlibcpp::plot(time_array, theta1_array, {{"label", "theta1"}});
+ matplotlibcpp::plot(time_array, omega1_array, {{"label", "omega1"}});
+ matplotlibcpp::plot(time_array, theta2_array, {{"label", "theta2"}});
+ matplotlibcpp::plot(time_array, omega2_array, {{"label", "omega2"}});
+ matplotlibcpp::legend();
+ }
+
+ if (FLAGS_plot_xy) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(x_array, y_array, {{"label", "xy trajectory"}});
+ matplotlibcpp::legend();
+ }
+
+ if (FLAGS_plot_u) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
+ matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
+ matplotlibcpp::legend();
+ }
+
+ ::std::vector<::std::vector<double>> cost_x;
+ ::std::vector<::std::vector<double>> cost_y;
+ ::std::vector<::std::vector<double>> cost_z;
+ ::std::vector<::std::vector<double>> cost_state_z;
+
+ for (double x_coordinate = -0.5; x_coordinate < 1.2; x_coordinate += 0.05) {
+ ::std::vector<double> cost_x_row;
+ ::std::vector<double> cost_y_row;
+ ::std::vector<double> cost_z_row;
+ ::std::vector<double> cost_state_z_row;
+
+ for (double y_coordinate = -1.0; y_coordinate < 6.0; y_coordinate += 0.05) {
+ cost_x_row.push_back(x_coordinate);
+ cost_y_row.push_back(y_coordinate);
+ Eigen::Matrix<double, 4, 1> state_matrix;
+ state_matrix << x_coordinate, 0.0, y_coordinate, 0.0;
+ Eigen::Matrix<double, 2, 1> u_matrix =
+ Eigen::Matrix<double, 2, 1>::Zero();
+ cost_state_z_row.push_back(
+ intermediate_cost->distance(state_matrix, u_matrix));
+ cost_z_row.push_back(
+ ::std::min(bounds_cost->evaluate(state_matrix, u_matrix, 0.0), 50.0));
+ }
+ cost_x.push_back(cost_x_row);
+ cost_y.push_back(cost_y_row);
+ cost_z.push_back(cost_z_row);
+ cost_state_z.push_back(cost_state_z_row);
+ }
+
+ if (FLAGS_plot_cost) {
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z);
+ }
+
+ if (FLAGS_plot_state_cost) {
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_state_z);
+ }
matplotlibcpp::figure();
matplotlibcpp::plot(theta1_array, theta2_array, {{"label", "trajectory"}});
- ::std::vector<double> box_x{0.5, 0.5, 1.0, 1.0};
- ::std::vector<double> box_y{0.0, 0.8, 0.8, 0.0};
- matplotlibcpp::plot(box_x, box_y, {{"label", "keepout zone"}});
+ ::std::vector<double> bounds_x;
+ ::std::vector<double> bounds_y;
+ for (const Point p : arm_space.points()) {
+ bounds_x.push_back(p.x());
+ bounds_y.push_back(p.y());
+ }
+ matplotlibcpp::plot(bounds_x, bounds_y, {{"label", "allowed region"}});
matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
- matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
- matplotlibcpp::legend();
matplotlibcpp::show();
+
+ return 0;
+}
+
+} // namespace control_loops
+} // namespace y2018
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+ return ::y2018::control_loops::Main();
}
diff --git a/y2018/control_loops/python/arm_mpc.py b/y2018/control_loops/python/arm_mpc.py
new file mode 100755
index 0000000..fcb3129
--- /dev/null
+++ b/y2018/control_loops/python/arm_mpc.py
@@ -0,0 +1,253 @@
+#!/usr/bin/python3
+
+import numpy
+import time
+import scipy.optimize
+from matplotlib import pylab
+from frc971.control_loops.python import controls
+
+dt = 0.05
+
+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 dynamics(X, U):
+ """Calculates the dynamics for a double jointed arm.
+
+ Args:
+ X, numpy.matrix(4, 1), The state. [theta1, omega1, theta2, omega2]
+ U, numpy.matrix(2, 1), The input. [torque1, torque2]
+
+ 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]]])
+
+
+def discrete_dynamics(X, U):
+ return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+
+
+def U_from_array(U_array):
+ """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, ...
+
+ Returns:
+ numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
+ """
+ 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.
+
+ Args:
+ U_matrix, numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
+
+ Returns:
+ numpy.array[N] The U coordinates in v, av, v, av, ...
+ """
+ return numpy.array(U_matrix.reshape((1, -1), order='F'))
+
+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_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
+
+class Cost(object):
+ def __init__(self):
+ 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)]))
+
+ self.R = numpy.matrix(numpy.diag([1.0 / (12.0 ** 2.0),
+ 1.0 / (12.0 ** 2.0)]))
+
+ final_A = numerical_jacobian_x(discrete_dynamics,
+ numpy.matrix(numpy.zeros((4, 1))),
+ numpy.matrix(numpy.zeros((2, 1))))
+ final_B = numerical_jacobian_u(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 K
+ print self.S
+
+ def cost(self, U_array, X):
+ """Computes the cost given the inital position and the U array.
+
+ Args:
+ U_array: numpy.array[N] The U coordinates.
+ X: numpy.matrix[3, 1] The cartesian coordinates of the starting
+ location.
+
+ Returns:
+ double, The quadratic cost of evaluating U.
+ """
+
+ X_mod = X.copy()
+ U_matrix = U_from_array(U_array)
+ my_cost = 0
+
+ for U in U_matrix.T:
+ # Handle a keep out zone.
+ penalized_cost = 0.0
+ if X_mod[0, 0] > 0.5 and X_mod[2, 0] < 0.8:
+ out_of_bound1 = 0.8 - X_mod[2, 0]
+ penalized_cost += 1000.0 * (out_of_bound1 ** 2.0 + 0.1 * out_of_bound1)
+
+ out_of_bound2 = X_mod[0, 0] - 0.5
+ penalized_cost += 1000.0 * (out_of_bound2 ** 2.0 + 0.1 * out_of_bound2)
+
+ U = U.T
+ my_cost += U.T * self.R * U + X_mod.T * self.Q * X_mod + penalized_cost
+ X_mod = discrete_dynamics(X_mod, U)
+
+ return my_cost + 0.5 * X_mod.T * self.S * X_mod
+
+
+# TODO(austin): Add Parker's constraints in.
+# TODO(austin): Real dynamics from dad.
+# TODO(austin): Look at grid resolution needed. Grid a section in open space
+# and look at curvature of the grid. Try motions using the grid.
+#
+# https://docs.scipy.org/doc/scipy-0.16.1/reference/generated/scipy.interpolate.RegularGridInterpolator.html
+# Look at a C++ version so we can build a large space.
+
+# TODO(austin): Larger timesteps further out?
+
+
+if __name__ == '__main__':
+ X = numpy.matrix([[1.0],
+ [0.0],
+ [1.0],
+ [0.0]])
+ theta1_array = []
+ omega1_array = []
+ theta2_array = []
+ omega2_array = []
+
+ cost_array = []
+
+ time_array = []
+ u0_array = []
+ u1_array = []
+
+ num_steps = 40
+
+ cost_obj = Cost()
+
+ U_array = numpy.zeros((num_steps * 2))
+ for i in range(400):
+ print('Iteration', i)
+ start_time = time.time()
+ # Solve the NMPC
+ U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(
+ cost_obj.cost, U_array.copy(), bounds = [(-12, 12), (-12, 12)] * num_steps,
+ args=(X,), iter=500, full_output=True)
+ U_matrix = U_from_array(U_array)
+
+ # Save variables for plotting.
+ cost_array.append(fx[0, 0])
+ u0_array.append(U_matrix[0, 0])
+ u1_array.append(U_matrix[1, 0])
+
+ theta1_array.append(X[0, 0])
+ omega1_array.append(X[1, 0])
+ theta2_array.append(X[2, 0])
+ omega2_array.append(X[3, 0])
+
+ time_array.append(i * dt)
+
+ # Simulate the dynamics
+ X = discrete_dynamics(X, U_matrix[:, 0])
+
+ U_array = U_to_array(numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
+ print 'Took %f to evaluate' % (time.time() - start_time)
+
+ if fx < 1e-05:
+ print('Cost is', fx, 'after', i, 'cycles, finishing early')
+ break
+
+ # Plot
+ pylab.figure('trajectory')
+ pylab.plot(theta1_array, theta2_array, label = 'trajectory')
+
+ fig, ax1 = fig, ax1 = pylab.subplots()
+ fig.suptitle('time')
+ ax1.plot(time_array, theta1_array, label='theta1')
+ ax1.plot(time_array, omega1_array, label='omega1')
+ ax1.plot(time_array, theta2_array, label = 'theta2')
+ ax1.plot(time_array, omega2_array, label='omega2')
+ ax2 = ax1.twinx()
+ ax2.plot(time_array, cost_array, 'k', label='cost')
+ ax1.legend(loc=2)
+ ax2.legend()
+
+ pylab.figure('u')
+ pylab.plot(time_array, u0_array, label='u0')
+ pylab.plot(time_array, u1_array, label='u1')
+ pylab.legend()
+
+ pylab.show()
diff --git a/y2018/control_loops/python/dlqr.h b/y2018/control_loops/python/dlqr.h
new file mode 100644
index 0000000..e4411ab
--- /dev/null
+++ b/y2018/control_loops/python/dlqr.h
@@ -0,0 +1,100 @@
+#ifndef FRC971_CONTROL_LOOPS_DLQR_H_
+#define FRC971_CONTROL_LOOPS_DLQR_H_
+
+#include <Eigen/Dense>
+
+
+namespace frc971 {
+namespace controls {
+
+extern "C" {
+// Forward declaration for slicot fortran library.
+void sb02od_(char *DICO, char *JOBB, char *FACT, char *UPLO, char *JOBL,
+ char *SORT, int *N, int *M, int *P, double *A, int *LDA, double *B,
+ int *LDB, double *Q, int *LDQ, double *R, int *LDR, double *L,
+ int *LDL, double *RCOND, double *X, int *LDX, double *ALFAR,
+ double *ALFAI, double *BETA, double *S, int *LDS, double *T,
+ int *LDT, double *U, int *LDU, double *TOL, int *IWORK,
+ double *DWORK, int *LDWORK, int *BWORK, int *INFO);
+}
+
+template <int kN, int kM>
+void dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B,
+ ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R,
+ ::Eigen::Matrix<double, kM, kN> *K,
+ ::Eigen::Matrix<double, kN, kN> *S) {
+ // Discrete (not continuous)
+ char DICO = 'D';
+ // B and R are provided instead of G.
+ char JOBB = 'B';
+ // Not factored, Q and R are provide.
+ char FACT = 'N';
+ // Store the upper triangle of Q and R.
+ char UPLO = 'U';
+ // L is zero.
+ char JOBL = 'Z';
+ // Stable eigenvalues first in the sort order
+ char SORT = 'S';
+
+ int N = 4;
+ int M = 2;
+ // Not needed since FACT = N
+ int P = 0;
+
+ int LDL = 1;
+
+ double RCOND = 0.0;
+ ::Eigen::Matrix<double, kN, kN> X = ::Eigen::Matrix<double, kN, kN>::Zero();
+
+ double ALFAR[kN * 2];
+ double ALFAI[kN * 2];
+ double BETA[kN * 2];
+ memset(ALFAR, 0, kN * 2);
+ memset(ALFAI, 0, kN * 2);
+ memset(BETA, 0, kN * 2);
+
+ int LDS = 2 * kN + kM;
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN + kM> S_schur =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN + kM>::Zero();
+
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN> T =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN>::Zero();
+ int LDT = 2 * kN + kM;
+
+ ::Eigen::Matrix<double, 2 * kN, 2 *kN> U =
+ ::Eigen::Matrix<double, 2 * kN, 2 * kN>::Zero();
+ int LDU = 2 * kN;
+
+ double TOL = 0.0;
+
+ int IWORK[2 * kN > kM ? 2 * kN : kM];
+ memset(IWORK, 0, 2 * kN > kM ? 2 * kN : kM);
+
+ int LDWORK = 16 * kN + 3 * kM + 16;
+
+ double DWORK[LDWORK];
+ memset(DWORK, 0, LDWORK);
+
+ int INFO = 0;
+
+ int BWORK[2 * kN];
+ memset(BWORK, 0, 2 * kN);
+
+ // TODO(austin): I can't tell if anything here is transposed...
+ sb02od_(&DICO, &JOBB, &FACT, &UPLO, &JOBL, &SORT, &N, &M, &P, A.data(),
+ &N, B.data(), &N, Q.data(), &N, R.data(), &M, nullptr, &LDL,
+ &RCOND, X.data(), &N, ALFAR, ALFAI, BETA, S_schur.data(), &LDS,
+ T.data(), &LDT, U.data(), &LDU, &TOL, IWORK, DWORK, &LDWORK, BWORK,
+ &INFO);
+ //::std::cout << "slycot result: " << INFO << ::std::endl;
+
+ *K = (R + B.transpose() * X * B).inverse() * B.transpose() * X * A;
+ if (S != nullptr) {
+ *S = X;
+ }
+}
+
+} // namespace controls
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DLQR_H_
diff --git a/y2018/control_loops/python/extended_lqr.py b/y2018/control_loops/python/extended_lqr.py
new file mode 100755
index 0000000..3c8d0e6
--- /dev/null
+++ b/y2018/control_loops/python/extended_lqr.py
@@ -0,0 +1,626 @@
+#!/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
+
+
+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 dynamics(self, X, U):
+ """Calculates the dynamics for a double jointed arm.
+
+ Args:
+ X, numpy.matrix(4, 1), The state. [theta1, omega1, theta2, omega2]
+ U, numpy.matrix(2, 1), The input. [torque1, torque2]
+
+ 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]]])
+
+ 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)
+
+# 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)]))
+
+ self.R = numpy.matrix(numpy.diag([1.0 / (12.0 ** 2.0),
+ 1.0 / (12.0 ** 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)
+
+ 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 0.5 * X.T * self.S * X
+
+ 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
+
+ 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)))
+ 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.
+
+ 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
+
+
+
+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.
+
+ 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]) / self.width]])
+
+ 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)
+
+
+# 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
+
+ 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
+
+ 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 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 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)
+
+
+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)]
+
+ 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 = [numpy.matrix(numpy.zeros((num_states, 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.last_x_hat_t[0] = x_hat_initial
+
+ 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)
+
+ 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
+
+ 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
+
+ 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
+
+ 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
+
+ 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)
+
+ self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
+
+ 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]
+
+ 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
+
+ # 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
+
+ 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
+ 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 '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]
+
+ 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 ('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((self.S_t[1] + S_bar_stiff),
+ (self.s_t[1] + self.s_bar_t[1]))
+ print 'new xhat', x_hat
+
+ self.S_bar_t[1] = S_bar_stiff
+
+ self.last_x_hat_t[1] = x_hat
+
+ for t in range(1, l):
+ print 't forward', t
+ u_t = self.L_t[t] * x_hat + self.l_t[t]
+
+ 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
+
+ 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)
+
+ 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(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)
+
+ 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
+
+ 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
+
+ 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
+
+ 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
+
+ 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 = -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])
+
+ 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]
+
+ 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
+
+ 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(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)
+
+ 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
+
+ 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
+
+ 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]
+
+ 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
+
+ x_hat_t = [x_hat_initial]
+
+ 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()
+
+ 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()
+
+ 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]
+
+ 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('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)
+
+ 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/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 812f091..a8c00cb 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -15,9 +15,15 @@
namespace superstructure {
namespace arm {
+namespace {
+
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+constexpr int kMaxBrownoutCount = 4;
+
+} // namespace
+
Arm::Arm()
: proximal_zeroing_estimator_(constants::GetValues().arm_proximal.zeroing),
distal_zeroing_estimator_(constants::GetValues().arm_distal.zeroing),
@@ -50,6 +56,11 @@
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
(release_arm_brake == nullptr) || (claw_closed == nullptr));
+ if (outputs_disabled) {
+ ++brownout_count_;
+ } else {
+ brownout_count_ = 0;
+ }
uint32_t filtered_goal = 0;
if (unsafe_goal != nullptr) {
@@ -178,7 +189,7 @@
distal_zeroing_estimator_.error()) {
LOG(ERROR, "Zeroing error ESTOP\n");
state_ = State::ESTOP;
- } else if (outputs_disabled) {
+ } else if (outputs_disabled && brownout_count_ > kMaxBrownoutCount) {
state_ = State::DISABLED;
} else if (trajectory_override) {
follower_.SwitchTrajectory(nullptr);
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index d3dd2df..a9b3590 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -110,6 +110,7 @@
bool close_enough_for_full_power_ = false;
int32_t climb_count_ = 0;
+ int32_t brownout_count_ = 0;
EKF arm_ekf_;
TrajectoryFollower follower_;
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 173e6ef..45b7302 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -3,10 +3,12 @@
#include <string.h>
#include <unistd.h>
#include <mutex>
+#include <google/protobuf/stubs/stringprintf.h>
#include "aos/common/actions/actions.h"
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/network/team_number.h"
#include "aos/common/stl_mutex.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
@@ -41,6 +43,7 @@
namespace joysticks {
namespace arm = ::y2018::control_loops::superstructure::arm;
+using google::protobuf::StringPrintf;
const ButtonLocation kIntakeClosed(3, 2);
const ButtonLocation kDuck(3, 9);
@@ -49,19 +52,19 @@
const ButtonLocation kIntakeIn(3, 4);
const ButtonLocation kIntakeOut(3, 3);
-const ButtonLocation kArmFrontHighBox(4, 11);
-const ButtonLocation kArmFrontExtraHighBox(4, 1);
-const ButtonLocation kArmFrontMiddle2Box(4, 9);
-const ButtonLocation kArmFrontMiddle1Box(4, 7);
-const ButtonLocation kArmFrontLowBox(4, 5);
-const ButtonLocation kArmFrontSwitch(3, 7);
+const ButtonLocation kArmBackHighBox(4, 11);
+const ButtonLocation kArmBackExtraHighBox(4, 1);
+const ButtonLocation kArmBackMiddle2Box(4, 9);
+const ButtonLocation kArmBackMiddle1Box(4, 7);
+const ButtonLocation kArmBackLowBox(4, 5);
+const ButtonLocation kArmBackSwitch(3, 7);
-const ButtonLocation kArmBackHighBox(4, 12);
-const ButtonLocation kArmBackExtraHighBox(3, 14);
-const ButtonLocation kArmBackMiddle2Box(4, 10);
-const ButtonLocation kArmBackMiddle1Box(4, 8);
-const ButtonLocation kArmBackLowBox(4, 6);
-const ButtonLocation kArmBackSwitch(3, 10);
+const ButtonLocation kArmFrontHighBox(4, 12);
+const ButtonLocation kArmFrontExtraHighBox(3, 14);
+const ButtonLocation kArmFrontMiddle2Box(4, 10);
+const ButtonLocation kArmFrontMiddle1Box(4, 8);
+const ButtonLocation kArmFrontLowBox(4, 6);
+const ButtonLocation kArmFrontSwitch(3, 10);
const ButtonLocation kArmAboveHang(3, 15);
const ButtonLocation kArmBelowHang(3, 16);
@@ -86,10 +89,15 @@
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() : video_tx_("10.9.71.179", 5000) {
+ Reader() {
+ const uint16_t team = ::aos::network::GetTeamNumber();
+
drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kPistol,
+ team == 971 ? DrivetrainInputReader::InputType::kPistol
+ : DrivetrainInputReader::InputType::kSteeringWheel,
::y2018::control_loops::drivetrain::GetDrivetrainConfig());
+ video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
+ StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
}
void RunIteration(const ::aos::input::driver_station::Data &data) override {
@@ -349,7 +357,7 @@
LOG(ERROR, "Sending superstructure goal failed.\n");
}
- video_tx_.Send(vision_control_);
+ video_tx_->Send(vision_control_);
}
private:
@@ -391,7 +399,7 @@
::aos::common::actions::ActionQueue action_queue_;
- ProtoTXUdpSocket<VisionControl> video_tx_;
+ ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
};
} // namespace joysticks
diff --git a/y2018/vision/deploy.sh b/y2018/vision/deploy.sh
index ba9d40d..d248f57 100755
--- a/y2018/vision/deploy.sh
+++ b/y2018/vision/deploy.sh
@@ -5,20 +5,13 @@
JETSON="root@$1"
# To build for the Jetson, use
-bazel build -c opt //y2018/vision:image_streamer \
- //aos/vision/tools:camera_primer --cpu=armhf-debian
-
-# Remove files before copying them.
-# Doing so avoids problems with trying to copy over a file in use.
-ssh "${JETSON}" rm -f image_streamer camera_primer exposure_2018.sh startup.sh
+bazel build -c opt //y2018/vision:image_streamer --cpu=armhf-debian
# Copy files to Jetson
-scp bazel-bin/y2018/vision/image_streamer "${JETSON}":.
-scp bazel-bin/aos/vision/tools/camera_primer "${JETSON}":.
-scp y2018/vision/exposure_2018.sh "${JETSON}":.
-scp y2018/vision/startup.sh "${JETSON}":.
+rsync -av --progress bazel-bin/y2018/vision/image_streamer y2018/vision/exposure_loop.sh "${JETSON}":.
+rsync -av --progress y2018/vision/exposure_loop.conf y2018/vision/vision.conf "${JETSON}":/etc/supervisor/conf.d/
ssh "${JETSON}" sync
# Can't restart with supervisorctl because the USB devices don't come up reliably...
-echo "You just restart the Jetson now" >&2
\ No newline at end of file
+echo "Restart the Jetson now" >&2
diff --git a/y2018/vision/exposure_loop.conf b/y2018/vision/exposure_loop.conf
new file mode 100644
index 0000000..0970954
--- /dev/null
+++ b/y2018/vision/exposure_loop.conf
@@ -0,0 +1,5 @@
+[program:exposure_loop]
+command=/root/exposure_loop.sh
+redirect_stderr=false
+autostart=true
+autorestart=true
diff --git a/y2018/vision/image_streamer.cc b/y2018/vision/image_streamer.cc
index 93f8c58..f6da93c 100644
--- a/y2018/vision/image_streamer.cc
+++ b/y2018/vision/image_streamer.cc
@@ -334,13 +334,16 @@
ProtoUdpClient<VisionControl> udp_client(
5000, [&camera0, &camera1](const VisionControl &vision_control) {
- LOG(INFO, "Got control packet\n");
+ bool cam0_active = false;
if (camera1) {
+ cam0_active = !vision_control.high_video();
camera0->set_active(!vision_control.high_video());
camera1->set_active(vision_control.high_video());
} else {
+ cam0_active = true;
camera0->set_active(true);
}
+ LOG(INFO, "Got control packet, cam%d active\n", cam0_active ? 0 : 1);
});
// Default to camera0
diff --git a/y2018/vision/vision.conf b/y2018/vision/vision.conf
new file mode 100644
index 0000000..ef1d78e
--- /dev/null
+++ b/y2018/vision/vision.conf
@@ -0,0 +1,5 @@
+[program:vision]
+command=/root/image_streamer --single_camera=false --camera0_exposure=300 --camera1_exposure=300
+redirect_stderr=false
+autostart=true
+autorestart=true