Initial Commit (tested training, testing, and TRT conversion)

flightlib/third_party/arc_utilities/src/__init__.py

@@ -0,0 +1 @@
+__author__ = 'calderpg'

flightlib/third_party/arc_utilities/src/arc_utilities/__init__.py

@@ -0,0 +1,4 @@
+import color_mapping
+import transformation_helper
+import extra_functions_to_be_put_in_the_right_place
+import numpy_conversions

flightlib/third_party/arc_utilities/src/arc_utilities/base64_helpers.cpp

@@ -0,0 +1,97 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <array>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+#include "arc_utilities/base64_helpers.hpp"
+
+namespace Base64Helpers
+{
+    /*
+     * Implementations derived from post at http://stackoverflow.com/a/41094722
+     */
+
+    static const std::array<int32_t, 256> B64IndexTable = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 62, 63, 62, 62,
+                                                           63, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 0, 0,
+                                                           0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
+                                                           11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22,
+                                                           23, 24, 25, 0, 0, 0, 0, 63, 0, 26, 27, 28, 29,
+                                                           30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41,
+                                                           42, 43, 44, 45, 46, 47, 48, 49, 50, 51 };
+
+    std::vector<uint8_t> Decode(const std::string& encoded)
+    {
+        const size_t encoded_length = encoded.size();
+        const size_t pad = encoded_length > 0 && (encoded_length % 4 || encoded[encoded_length - 1] == '=');
+        const size_t L = ((encoded_length + 3) / 4 - pad) * 4;
+        std::vector<uint8_t> buffer(L / 4 * 3 + pad, 0x00);
+        for (size_t i = 0, j = 0; i < L; i += 4)
+        {
+            const int32_t n = B64IndexTable[encoded[i]] << 18 | B64IndexTable[encoded[i + 1]] << 12 | B64IndexTable[encoded[i + 2]] << 6 | B64IndexTable[encoded[i + 3]];
+            buffer[j++] = (uint8_t)(n >> 16);
+            buffer[j++] = (uint8_t)(n >> 8 & 0xFF);
+            buffer[j++] = (uint8_t)(n & 0xFF);
+        }
+        if (pad > 0)
+        {
+            int32_t n = B64IndexTable[encoded[L]] << 18 | B64IndexTable[encoded[L + 1]] << 12;
+            buffer[buffer.size() - 1] = (uint8_t)(n >> 16);
+            if (encoded_length > L + 2 && encoded[L + 2] != '=')
+            {
+                n |= B64IndexTable[encoded[L + 2]] << 6;
+                buffer.push_back((uint8_t)(n >> 8 & 0xFF));
+            }
+        }
+        return buffer;
+    }
+
+    static const std::array<uint8_t, 64> B64ValueTable = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
+                                                          'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
+                                                          'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
+                                                          'w', 'x', 'y', 'z', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '+', '/'};
+
+    std::string Encode(const std::vector<uint8_t>& binary)
+    {
+        const size_t binary_length = binary.size();
+        const size_t encoded_length = 4 * ((binary_length + 2) / 3); /* 3-byte blocks to 4-byte */
+        if (encoded_length < binary_length)
+        {
+            return std::string(); /* integer overflow */
+        }
+        std::string encoded;
+        encoded.resize(encoded_length);
+        size_t input_position = 0;
+        size_t output_position = 0;
+        while (binary_length - input_position >= 3)
+        {
+            encoded[output_position++] = B64ValueTable[binary[input_position + 0] >> 2];
+            encoded[output_position++] = B64ValueTable[((binary[input_position + 0] & 0x03) << 4) | (binary[input_position + 1] >> 4)];
+            encoded[output_position++] = B64ValueTable[((binary[input_position + 1] & 0x0f) << 2) | (binary[input_position + 2] >> 6)];
+            encoded[output_position++] = B64ValueTable[binary[input_position + 2] & 0x3f];
+            input_position += 3;
+        }
+        if (input_position < binary_length)
+        {
+            encoded[output_position++] = B64ValueTable[binary[input_position + 0] >> 2];
+            if ((binary_length - input_position) == 1)
+            {
+                encoded[output_position++] = B64ValueTable[(binary[input_position + 0] & 0x03) << 4];
+                encoded[output_position++] = '=';
+            }
+            else
+            {
+                encoded[output_position++] = B64ValueTable[((binary[input_position + 0] & 0x03) << 4) | (binary[input_position + 1] >> 4)];
+                encoded[output_position++] = B64ValueTable[(binary[input_position + 1] & 0x0f) << 2];
+            }
+            encoded[output_position++] = '=';
+        }
+        return encoded;
+    }
+}

flightlib/third_party/arc_utilities/src/arc_utilities/color_mapping.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python
+
+import math
+import numpy
+
+
+def convert_rgb_to_xyz(r, g, b):
+    rgb_np = numpy.array([r, g, b])
+    rgb_xyz_cvt_np = numpy.array([[0.4124, 0.2126, 0.0193], [0.3576, 0.7152, 0.1192], [0.1805, 0.0722, 0.9505]])
+    xyz_np = numpy.dot(rgb_np, rgb_xyz_cvt_np)
+    return [xyz_np[0], xyz_np[1], xyz_np[2]]
+
+
+def convert_xyz_to_rgb(x, y, z):
+    xyz_np = numpy.array([x, y, z])
+    rgb_xyz_cvt_np = numpy.array([[0.4124, 0.2126, 0.0193], [0.3576, 0.7152, 0.1192], [0.1805, 0.0722, 0.9505]])
+    xyz_rgb_cvt_np = numpy.linalg.inv(rgb_xyz_cvt_np)
+    rgb_np = numpy.dot(xyz_np, xyz_rgb_cvt_np)
+    return [rgb_np[0], rgb_np[1], rgb_np[2]]
+
+
+def cielab_f(val):
+    if val > (216.0 / 24389.0):
+        return math.pow(val, (1.0 / 3.0))
+    else:
+        return (841.0 / 108.0) * val + (16.0 / 116.0)
+
+
+def cielab_finv(val):
+    if val > (6.0 / 29.0):
+        return val ** 3
+    else:
+        return (108.0 / 841.0) * (val - (16.0 / 116.0))
+
+
+def get_ref_xyz():
+    xref = 95.047
+    yref = 100.0
+    zref = 108.883
+    return [xref, yref, zref]
+
+
+def convert_xyz_to_lab(x, y, z):
+    [xref, yref, zref] = get_ref_xyz()
+    l = 116.0 * (cielab_f(y / yref) - (16.0 / 116.0))
+    a = 500.0 * (cielab_f(x / xref) - cielab_f(y / yref))
+    b = 200.0 * (cielab_f(y / yref) - cielab_f(z / zref))
+    return [l, a, b]
+
+
+def convert_lab_to_xyz(l, a, b):
+    intermediate = (l + 16.0) * (1.0 / 116.0)
+    x = cielab_finv(intermediate + a * (1.0 / 500.0))
+    y = cielab_finv(intermediate)
+    z = cielab_finv(intermediate - b * (1.0 / 200.0))
+    return [x, y, z]
+
+
+def convert_lab_to_msh(l, a, b):
+    m = math.sqrt(l ** 2 + a ** 2 + b ** 2)
+    s = math.acos(l / m)
+    #h = math.atan2(b, a)
+    h = math.atan(b / a)
+    return [m, s, h]
+
+
+def convert_msh_to_lab(m, s, h):
+    l = m * math.cos(s)
+    a = m * math.sin(s) * math.cos(h)
+    b = m * math.sin(s) * math.sin(h)
+    return [l, a, b]
+
+
+def rgb_to_msh(r, g, b):
+    [x, y, z] = convert_rgb_to_xyz(r, g, b)
+    [l, a, b] = convert_xyz_to_lab(x, y, z)
+    [m, s, h] = convert_lab_to_msh(l, a, b)
+    return [m, s, h]
+
+
+def msh_to_rgb(m, s, h):
+    [l, a, b] = convert_msh_to_lab(m, s, h)
+    [x, y, z] = convert_lab_to_xyz(l, a, b)
+    [r, g, b] = convert_xyz_to_rgb(x, y, z)
+    return [r, g, b]
+
+
+def rad_diff(x, y):
+    diff = abs(x - y)
+    if diff > math.pi:
+        return (math.pi * 2.0) - diff
+    else:
+        return diff
+
+
+def adjust_hue((m_saturated, s_saturated, h_saturated), m_unsaturated):
+    if m_saturated >= m_unsaturated:
+        return h_saturated
+    else:
+        h_spin = (s_saturated * math.sqrt(m_unsaturated ** 2 - m_saturated ** 2)) / (m_saturated * math.sin(s_saturated))
+        if h_saturated > -(math.pi / 3.0):
+            return h_saturated + h_spin
+        else:
+            return h_saturated - h_spin
+
+
+def interpolate_coolwarm((r1, g1, b1), (r2, g2, b2), interpolation):
+    assert(0.0 <= interpolation <= 1.0)
+    [m1, s1, h1] = rgb_to_msh(r1, g1, b1)
+    [m2, s2, h2] = rgb_to_msh(r2, g2, b2)
+    # If points saturated and distinct, place white in middle
+    if (s1 > 0.05) and (s2 > 0.05) and (rad_diff(h1, h2) > (math.pi / 3.0)):
+        m_mid = max((m1, m2, 88.0))
+        if interpolation < 0.5:
+            m2 = m_mid
+            s2 = 0.0
+            h2 = 0.0
+            interpolation *= 2.0
+        else:
+            m1 = m_mid
+            s1 = 0.0
+            h1 = 0.0
+            interpolation = 2.0 * interpolation - 1.0
+    # Adjust hue of unsaturated colors
+    if (s1 < 0.05) and (s2 > 0.05):
+        h1 = adjust_hue((m2, s2, h2), m1)
+    elif (s2 < 0.05) and (s1 > 0.05):
+        h2 = adjust_hue((m1, s1, h1), m2)
+    # Linear interpolation on adjusted control points
+    m_mid = ((1.0 - interpolation) * m1) + (interpolation * m2)
+    s_mid = ((1.0 - interpolation) * s1) + (interpolation * s2)
+    h_mid = ((1.0 - interpolation) * h1) + (interpolation * h2)
+    return msh_to_rgb(m_mid, s_mid, h_mid)
+
+
+def jet_interpolate(value, y0, x0, y1, x1):
+    return (value - x0) * (y1 - y0) / (x1 - x0) + y0
+
+
+def jet_base(value):
+    if value <= -0.75:
+        return 0.0
+    elif value <= -0.25:
+        return jet_interpolate(value, 0.0, -0.75, 1.0, -0.25)
+    elif value <= 0.25:
+        return 1.0
+    elif value <= 0.75:
+        return jet_interpolate(value, 1.0, 0.25, 0.0, 0.75)
+    else:
+        return 0.0
+
+
+def interpolate_jet(value, use_negative_range=False):
+    if use_negative_range:
+        assert(-1.0 <= value <= 1.0)
+        r = jet_base(value - 0.5)
+        g = jet_base(value)
+        b = jet_base(value + 0.5)
+        return [r, g, b]
+    else:
+        assert(0.0 <= value <= 1.0)
+        if value > 0.5:
+            value = (value - 0.5) * 2.0
+        elif value < 0.5:
+            value = -(0.5 - value) * 2.0
+        else:
+            value = 0.0
+        r = jet_base(value - 0.5)
+        g = jet_base(value)
+        b = jet_base(value + 0.5)
+        return [r, g, b]
+
+
+def interpolate_hot_to_cold(value, min_value=0.0, max_value=1.0):
+    # Safety checks
+    assert(min_value < max_value)
+    if value < min_value:
+        value = min_value
+    elif value > max_value:
+        value = max_value
+    val_range = max_value - min_value
+    # Start with white
+    r = 1.0
+    g = 1.0
+    b = 1.0
+    # Interpolate
+    if value < (min_value + 0.25 * val_range):
+        r = 0.0
+        g = 4.0 * (value - min_value) / val_range
+    elif value < (min_value + 0.5 * val_range):
+        r = 0.0
+        b = 1.0 + 4.0 * (min_value + 0.25 * val_range - value) / val_range
+    elif value < (min_value + 0.75 * val_range):
+        r = 4 * (value - min_value - 0.5 * val_range) / val_range
+        b = 0.0
+    else:
+        g = 1.0 + 4.0 * (min_value + 0.75 * val_range - min_value) / val_range
+        b = 0.0
+    return [r, g, b]

flightlib/third_party/arc_utilities/src/arc_utilities/extra_functions_to_be_put_in_the_right_place.py

@@ -0,0 +1,82 @@
+from std_msgs.msg import *
+from geometry_msgs.msg import *
+from color_mapping import *
+
+def make_pose((px, py, pz), (rx, ry, rz, rw)):
+    new_pose = Pose()
+    new_pose.position.x = px
+    new_pose.position.y = py
+    new_pose.position.z = pz
+    new_pose.orientation.x = rx
+    new_pose.orientation.y = ry
+    new_pose.orientation.z = rz
+    new_pose.orientation.w = rw
+    return new_pose
+
+
+def make_vector(x, y, z):
+    new_vector = Vector3()
+    new_vector.x = x
+    new_vector.y = y
+    new_vector.z = z
+    return new_vector
+
+
+def make_point(x, y, z):
+    new_point = Point()
+    new_point.x = x
+    new_point.y = y
+    new_point.z = z
+    return new_point
+
+
+def make_plane((px, py, pz), (nx, ny, nz)):
+    new_plane = CollisionPlane()
+    new_plane.point = make_point(px, py, pz)
+    new_plane.normal = make_vector(nx, ny, nz)
+    return new_plane
+
+
+def make_unit_point(x, y, z):
+    mag = math.sqrt(x ** 2 + y ** 2 + z ** 2)
+    assert(mag > 0.0)
+    unit_point = Point()
+    unit_point.x = x / mag
+    unit_point.y = y / mag
+    unit_point.z = z / mag
+    return unit_point
+
+
+def normalize_point(raw_point):
+    return make_unit_point(raw_point.x, raw_point.y, raw_point.z)
+
+
+def make_plane_from_points(point, normal):
+    new_plane = CollisionPlane()
+    new_plane.point = point
+    new_plane.normal = normal
+    return new_plane
+
+
+def safe_color_val(val):
+    if val >= 1.0:
+        return 1.0
+    elif val <= 0.0:
+        return 0.0
+    else:
+        return val
+
+
+def make_color(r, g, b, a):
+    new_color = ColorRGBA()
+    new_color.r = safe_color_val(r)
+    new_color.g = safe_color_val(g)
+    new_color.b = safe_color_val(b)
+    new_color.a = safe_color_val(a)
+    return new_color
+
+
+def map_color(value):
+    # [r, g, b] = interpolate_jet(value)
+    [r, g, b] = interpolate_hot_to_cold(value)
+    return make_color(r, g, b, 1.0)

flightlib/third_party/arc_utilities/src/arc_utilities/first_order_deformation.cpp

@@ -0,0 +1,99 @@
+#include "arc_utilities/first_order_deformation.h"
+
+#include <queue>
+#include <Eigen/Core>
+
+using namespace arc_utilities;
+using namespace FirstOrderDeformation;
+
+static double ConfigTypeDistance(const ConfigType& c1, const ConfigType& c2)
+{
+    return Eigen::Vector2d((double)(c1.first - c2.first), (double)(c1.second - c2.second)).norm();
+}
+
+static std::vector<ConfigType> GetNeighbours(const ConfigType& config, const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
+{
+    std::vector<ConfigType> neighbours;
+    neighbours.reserve(8);
+
+    const ssize_t row_min = std::max(0L, config.first - 1);
+    const ssize_t row_max = std::min(rows - 1, config.first + 1);
+
+    const ssize_t col_min = std::max(0L, config.second - 1);
+    const ssize_t col_max = std::min(cols - 1, config.second + 1);
+
+    for (ssize_t col = col_min; col <= col_max; col++)
+    {
+        for (ssize_t row = row_min; row <= row_max; row++)
+        {
+            if (!(row == config. first && col == config.second) && validity_check_fn(row, col) == true)
+            {
+                neighbours.push_back(ConfigType(row, col));
+            }
+        }
+    }
+
+    return neighbours;
+}
+
+bool FirstOrderDeformation::CheckFirstOrderDeformation(const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
+{
+    struct BestFirstSearchComparator
+    {
+        public:
+            // Defines a "less" operation"; by using "greater" the smallest element will appear at the top of the priority queue
+            bool operator()(const ConfigAndDistType& c1, const ConfigAndDistType& c2) const
+            {
+                // We want to explore the one with the smaller expected distance
+                return (c1.second > c2.second);
+            }
+    };
+
+    assert(rows > 0 && cols > 0);
+    typedef Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> ArrayXb;
+
+    // Setup the strucutres needed to keep track of the search
+    std::priority_queue<ConfigAndDistType, std::vector<ConfigAndDistType>, BestFirstSearchComparator> frontier;
+    ArrayXb explored = ArrayXb::Constant(rows, cols, false);
+
+    // Setup the start and goal
+    const ConfigType start(0, 0), goal(rows - 1, cols - 1);
+    const double start_heuristic_distance = ConfigTypeDistance(start, goal);
+    frontier.push(ConfigAndDistType(start, start_heuristic_distance));
+
+    // Perform the best first search
+    bool path_found = false;
+    while (!path_found && frontier.size() > 0)
+    {
+        const ConfigAndDistType current = frontier.top();
+        frontier.pop();
+        const ConfigType& current_config = current.first;
+
+        if (current_config.first == goal.first && current_config.second == goal.second)
+        {
+            path_found = true;
+        }
+        // Double check if we've already explored this node:
+        //    a single node can be inserted into the frontier multiple times at the same or different priorities
+        //    so we want to avoid the expense of re-exploring it, and just discard this one once we pop it
+        else if (explored(current_config.first, current_config.second) == false)
+        {
+            explored(current_config.first, current_config.second) = true;
+
+            // Expand the node to find all neighbours, adding them to the frontier if we have not already explored them
+            const auto neighbours = GetNeighbours(current_config, rows, cols, validity_check_fn);
+            for (const auto neighbour : neighbours)
+            {
+                // Check if we've already explored this neighbour to avoid re-adding it to the frontier
+                if (explored(neighbour.first, neighbour.second) == false)
+                {
+                    // We only need the heuristic distance as we are doing a best first search
+                    const double neighbour_heuristic_distance = ConfigTypeDistance(neighbour, goal);
+                    frontier.push(ConfigAndDistType(neighbour, neighbour_heuristic_distance));
+                }
+            }
+        }
+    }
+
+    return path_found;
+}

flightlib/third_party/arc_utilities/src/arc_utilities/numpy_conversions.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+def ListPointsToNpArray(points, transform=None):
+    """
+    :param points: Anything that is itterable, with each element having x, y, and z elements
+    :type points: :class:`list[geometry_msgs.msgs.Point]`
+    :param transform:
+    :type transform: :class:`np.array`
+    :return:
+    """
+    arr = np.empty(shape=(3, len(points)))
+    if transform is not None:
+        for ind in range(len(points)):
+            point = transform.dot([points[ind].x, points[ind].y, points[ind].z, 1])
+            arr[:, ind] = point[0:3]
+    else:
+        for ind in range(len(points)):
+            arr[:, ind] = [points[ind].x, points[ind].y, points[ind].z]
+    return arr

flightlib/third_party/arc_utilities/src/arc_utilities/ros_helpers.py

@@ -0,0 +1,109 @@
+#! /usr/bin/env python
+
+import rospy
+import time
+from threading import Lock
+from sensor_msgs.msg import Joy
+
+class Listener:
+    def __init__(self, topic_name, topic_type, wait_for_data=False):
+        """
+        Listener is a wrapper around a subscriber where the callback simply records the latest msg.
+
+        Listener does not consume the message 
+            (for consuming behavior, use the standard ros callback pattern)
+        Listener does not check timestamps of message headers
+
+        Parameters:
+            topic_name (str):      name of topic to subscribe to
+            topic_type (msg_type): type of message received on topic
+            wait_for_data (bool):  block constructor until a message has been received
+        """
+
+        self.data = None
+        self.lock = Lock()
+            
+        self.subscriber = rospy.Subscriber(topic_name, topic_type, self.callback)
+        self.get(wait_for_data)
+
+        
+    def callback(self, msg):
+        with self.lock:
+            self.data = msg
+
+    def get(self, block_until_data=True):
+        """
+        Returns the latest msg from the subscribed topic
+
+        Parameters:
+            block_until_data (bool): block if no message has been received yet. 
+                                     Guarantees a msg is returned (not None)
+        """
+        wait_for(lambda: not (block_until_data and self.data is None))
+            
+        with self.lock:
+            return self.data
+
+    
+def wait_for(func):
+    """
+    Waits for function evaluation to be true. Exits cleanly from ros.
+
+    Introduces sleep delay, not recommended for time critical operations
+    """
+    
+    while not func() and not rospy.is_shutdown():
+        time.sleep(0.01)
+
+        
+def joy_to_xbox(joy):
+    """
+    Transforms a joystick sensor_msg to a XBox controller for easier code readability
+    
+    Parameters:
+    joy (sensor_msgs/Joy): xbox msg
+
+    Returns:
+    xbox struct where fields are the button names
+    """
+    class Xbox_msg():
+        pass
+    x = Xbox_msg()
+    x.A, x.B, x.X, x.Y, x.LB, x.RB, \
+        x.back, x.start, x.power,\
+        x.stick_button_left, x.stick_button_right, \
+        x.DL, x.DR, x.DU, x.DD = joy.buttons
+    x.LH, x.LV, x.LT, x.RH, x.RV, x.RT, x.DH, x.DV = joy.axes
+    return x
+
+
+class Xbox():
+    def __init__(self, joystick_topic="joy"):
+        self.xbox_listener = Listener(joystick_topic, Joy)
+
+    def get_buttons_state(self):
+        """
+        Returns an xbox struct of the last joystick message received
+        """
+        return joy_to_xbox(self.xbox_listener.get())
+
+    def get_button(self, button):
+        """
+        Return value of button or axis of the controller
+        0 or 1 for buttons
+        -1.0 to 1.0 (at most) for axes
+        """
+        return getattr(self.get_buttons_state(), button)
+
+    def wait_for_button(self, button, message=True):
+        """
+        Waits for button press on xbox.
+
+        Parameters:
+        button (str):   Name of xbox button. "A", "B", "X", ...
+        message (bool): log a message informing the user?
+        """
+        if message:
+            rospy.loginfo("Waiting for xbox button: " + button)
+            
+        wait_for(lambda: not self.get_button(button) == 0)

flightlib/third_party/arc_utilities/src/arc_utilities/transformation_helper.py

@@ -0,0 +1,407 @@
+#!/usr/bin/env python
+
+#   Calder Phillips-Grafflin -- ARC Lab
+#
+#   Various helper functions for operations with transformations
+#   using quaternions + vectors and 4x4 matrices
+#
+####################################################################################################
+#
+# A note to the user - this code was written because the last time I needed to do one of the
+# operations provided in this API (composing a pose with a transform), it required ~20 lines of code
+#
+# It relies exclusively on the powerful tranformations.py library and numpy linear algebra library
+#
+####################################################################################################
+
+import rospy
+import math
+import numpy
+from numpy import *
+from tf.transformations import *
+from geometry_msgs.msg import *
+
+####################################################################################################
+#
+#   API Documentation
+#
+#   Storage conventions:
+#
+#   translations = [x,y,z]
+#   quaternions = [x,y,z,w]
+#
+##################################################
+# Math functions
+#
+# geometry_msgs/Transform composed = ComposeTransforms(geometry_msgs/Transform transform1, geometry_msgs/Transform transform2)
+# geometry_msgs/Pose composed = ComposePoses(geometry_msgs/Pose pose1, geometry_msgs/Pose pose2)
+#
+# geometry_msgs/Transform inverted = InvertTransform(geometry_msgs/Transform old_transform)
+# geometry_msgs/Pose inverted = InvertPose(geometry_msgs/Pose old_pose)
+#
+# numpy/array composed = ComposeMatrices(numpy/array matrix1, numpy/array matrix2)
+# numpy/array inverted = InvertMatrix(numpy/array old_matrix)
+#
+##################################################
+# Assist functions
+#
+# list quaternion = NormalizeQuaternion(list quaternion)
+#
+# geometry_msgs/Quaternion quaternion = NormalizeQuaternionRos(geometry_msgs/Quaternion quaternion)
+#
+##################################################
+# Conversion functions
+#
+# geometry_msgs/Transform new_transform = TransformFromComponents(list translation, list quaternion)
+# [list translation, list quaternion] = ComponentsFromTransform(geometry_msgs/Transform old_transform)
+#
+# geometry_msgs/Pose new_pose = PoseFromTransform(geometry_msgs/Transform old_transform)
+# geometry_msgs/Transform new_transform = PoseToTransform(geometry_msgs/Pose old_pose)
+#
+# numpy/array tfmatrix = TransformToMatrix(geometry_msgs/Transform old_transform)
+# geometry_msgs/Transform new_transform = TransformFromMatrix(numpy/array old_matrix)
+#
+# numpy/array tfmatrix = PoseToMatrix(geometry_msgs/Pose old_pose)
+# geometry_msgs/Pose new_pose = PoseFromMatrix(numpy/array old_matrix)
+#
+# [numpy/array rotation, list translation] = ExtractRawFromMatrix(numpy/array old_matrix)
+# numpy/array tfmatrix = BuildRawMatrix(numpy/array rotation, list translation)
+#
+# [list translation, list quaternion] = ExtractFromMatrix(numpy/array old_matrix)
+# numpy/array tfmatrix = BuildMatrix(list translation, list quaternion)
+#
+##################################################
+# Generation functions -- these generate various transform representations from D-H parameters
+#                         we recommend against using these functions unless necessary
+#
+# numpy/array tfmatrix = BuildMatrixFromDH(float d, float a, float theta, float alpha)
+# [list translation, list quaternion] = ExtractFromDH(float d, float a, float theta, float alpha)
+# geometry_msgs/Transform new_transform = TransformFromDH(float d, float a, float theta, float alpha)
+# geometry_msgs/Pose new_pose = PoseFromDH(float d, float a, float theta, float alpha)
+#
+####################################################################################################
+
+'''High-level functions'''
+
+
+def AddPoints(point1, point2):
+    sum_point = Point()
+    sum_point.x = point2.x + point1.x
+    sum_point.y = point2.y + point1.y
+    sum_point.z = point2.z + point1.z
+    return sum_point
+
+
+def SubtractPoints(point1, point2):
+    difference = Point()
+    difference.x = point2.x - point1.x
+    difference.y = point2.y - point1.y
+    difference.z = point2.z - point1.z
+    return difference
+
+
+def TranslationNorm(trans):
+    return math.sqrt(trans.x**2 + trans.y**2 + trans.z**2)
+
+
+def ComposePoseWithPoint(pose, point):
+    matrix = TransformToMatrix(PoseToTransform(pose))
+    vector = PointToVector(point)
+    composed = numpy.dot(matrix, vector)
+    composed_point = PointFromVector(composed)
+    return composed_point
+
+
+def ComposeTransformWithPoint(transform, point):
+    matrix = TransformToMatrix(transform)
+    vector = PointToVector(point)
+    composed = numpy.dot(matrix, vector)
+    composed_point = PointFromVector(composed)
+    return composed_point
+
+
+def ComposeTransforms(transform1, transform2):
+    tfmatrix1 = TransformToMatrix(transform1)
+    tfmatrix2 = TransformToMatrix(transform2)
+    composedmatrix = dot(tfmatrix1, tfmatrix2)
+    composed = TransformFromMatrix(composedmatrix)
+    return composed
+
+
+def ComposePoses(pose1, pose2):
+    transform1 = PoseToTransform(pose1)
+    transform2 = PoseToTransform(pose2)
+    composed = PoseFromTransform(ComposeTransforms(transform1, transform2))
+    return composed
+
+
+def InvertTransform(old_transform):
+    tfmatrix = TransformToMatrix(old_transform)
+    xirtamft = numpy.linalg.inv(tfmatrix)
+    inverted = TransformFromMatrix(xirtamft)
+    return inverted
+
+
+def InvertPose(old_pose):
+    old_transform = PoseToTransform(old_pose)
+    inverted = InvertTransform(old_transform)
+    posed = PoseFromTransform(inverted)
+    return posed
+
+
+def ComposeMatrices(matrix1, matrix2):
+    return dot(matrix1, matrix2)
+
+
+def InvertMatrix(old_matrix):
+    return numpy.linalg.inv(old_matrix)
+
+
+def ComposeQuaternions(q1, q2):
+    nq1 = NormalizeQuaternion(q1)
+    nq2 = NormalizeQuaternion(q2)
+    x = nq1[3]*nq2[0] + nq2[3]*nq1[0] + nq1[1]*nq2[2] - nq2[1]*nq1[2]
+    y = nq1[3]*nq2[1] + nq2[3]*nq1[1] + nq2[0]*nq1[2] - nq1[0]*nq2[2]
+    z = nq1[3]*nq2[2] + nq2[3]*nq1[2] + nq1[0]*nq2[1] - nq2[0]*nq1[1]
+    w = nq1[3]*nq2[3] - nq1[0]*nq2[0] - nq1[1]*nq2[1] - nq1[2]*nq2[2]
+    return NormalizeQuaternion([x, y, z, w])
+
+
+def AxisFromQuaternion(q):
+    nq = NormalizeQuaternion(q)
+    a = math.acos(nq[3]) * 2.0
+    sina2 = math.sin(a * 0.5)
+    if abs(sina2) > 0.000000001:
+        i = nq[0] / sina2
+        j = nq[1] / sina2
+        k = nq[2] / sina2
+        return [a, [i, j, k]]
+    else:
+        return [0.0, [0.0, 0.0, 1.0]]
+
+
+def NormalizeVector3(axis):
+    assert(len(axis) == 3)
+    mag = math.sqrt(axis[0] ** 2 + axis[1] ** 2 + axis[2] ** 2)
+    assert(mag > 0.0)
+    return [axis[0] / mag, axis[1] / mag, axis[2] / mag]
+
+
+def QuaternionFromAxisAngle(axis, angle):
+    try:
+        naxis = NormalizeVector3(axis)
+        w = math.cos(angle * 0.5)
+        x = math.sin(angle * 0.5) * naxis[0]
+        y = math.sin(angle * 0.5) * naxis[1]
+        z = math.sin(angle * 0.5) * naxis[2]
+        return NormalizeQuaternion([x, y, z, w])
+    except AssertionError:
+        print("Vector normalize error, returning identity quaternion")
+        return [0.0, 0.0, 0.0, 1.0]
+
+
+def AngleBetweenQuaternions(q1, q2):
+    nq1 = NormalizeQuaternion(q1)
+    nq2 = NormalizeQuaternion(q2)
+    dot_product = abs(nq1[0] * nq2[0] + nq1[1] * nq2[1] + nq1[2] * nq2[2] + nq1[3] * nq2[3])
+    if dot_product < 0.9999:
+        return math.acos(2.0 * (dot_product ** 2) - 1.0)
+    else:
+        return 0.0
+
+
+def AngleBetweenQuaternionsRos(q1, q2):
+    nq1 = NormalizeQuaternionRos(q1)
+    nq2 = NormalizeQuaternionRos(q2)
+    dot_product = abs(nq1.x * nq2.x + nq1.y * nq2.y + nq1.z * nq2.z + nq1.z * nq2.w)
+    if dot_product < 0.9999:
+        return math.acos(2.0 * (dot_product ** 2) - 1.0)
+    else:
+        return 0
+
+
+'''Assist functions'''
+
+
+def NormalizeQuaternion(q_raw):
+    magnitude = math.sqrt(q_raw[0]**2 + q_raw[1]**2 + q_raw[2]**2 + q_raw[3]**2)
+    x = q_raw[0] / magnitude
+    y = q_raw[1] / magnitude
+    z = q_raw[2] / magnitude
+    w = q_raw[3] / magnitude
+    return [x, y, z, w]
+
+
+def NormalizeQuaternionRos(q_raw):
+    magnitude = math.sqrt(q_raw.x**2 + q_raw.y**2 + q_raw.z**2 + q_raw.w**2)
+    q_unit = Quaternion()
+    q_unit.x = q_raw.x / magnitude
+    q_unit.y = q_raw.y / magnitude
+    q_unit.z = q_raw.z / magnitude
+    q_unit.w = q_raw.w / magnitude
+    return q_unit
+
+
+'''Conversion functions'''
+
+
+def PointToVector(point):
+    return numpy.array([point.x, point.y, point.z, 1.0]).transpose()
+
+
+def PointFromVector(vector):
+    new_point = Point()
+    new_point.x = float(vector[0])
+    new_point.y = float(vector[1])
+    new_point.z = float(vector[2])
+    return new_point
+
+
+def PoseFromTransform(old_transform):
+    posed = Pose()
+    posed.position.x = old_transform.translation.x
+    posed.position.y = old_transform.translation.y
+    posed.position.z = old_transform.translation.z
+    posed.orientation.x = old_transform.rotation.x
+    posed.orientation.y = old_transform.rotation.y
+    posed.orientation.z = old_transform.rotation.z
+    posed.orientation.w = old_transform.rotation.w
+    return posed
+
+
+def PoseToTransform(old_pose):
+    transformed = Transform()
+    transformed.translation.x = old_pose.position.x
+    transformed.translation.y = old_pose.position.y
+    transformed.translation.z = old_pose.position.z
+    transformed.rotation.x = old_pose.orientation.x
+    transformed.rotation.y = old_pose.orientation.y
+    transformed.rotation.z = old_pose.orientation.z
+    transformed.rotation.w = old_pose.orientation.w
+    return transformed
+
+
+def PoseFromComponents(translation, quaternion):
+    posed = Pose()
+    posed.position.x = translation[0]
+    posed.position.y = translation[1]
+    posed.position.z = translation[2]
+    posed.orientation.x = quaternion[0]
+    posed.orientation.y = quaternion[1]
+    posed.orientation.z = quaternion[2]
+    posed.orientation.w = quaternion[3]
+    return posed
+
+
+def ComponentsFromPose(old_pose):
+    translation = [old_pose.position.x, old_pose.position.y, old_pose.position.z]
+    quaternion = [old_pose.orientation.x, old_pose.orientation.y, old_pose.orientation.z, old_pose.orientation.w]
+    return [translation, quaternion]
+
+
+def TransformFromComponents(translation, quaternion):
+    transformed = Transform()
+    transformed.translation.x = translation[0]
+    transformed.translation.y = translation[1]
+    transformed.translation.z = translation[2]
+    transformed.rotation.x = quaternion[0]
+    transformed.rotation.y = quaternion[1]
+    transformed.rotation.z = quaternion[2]
+    transformed.rotation.w = quaternion[3]
+    return transformed
+
+
+def ComponentsFromTransform(old_transform):
+    translation = [old_transform.translation.x, old_transform.translation.y, old_transform.translation.z]
+    quaternion = [old_transform.rotation.x, old_transform.rotation.y, old_transform.rotation.z, old_transform.rotation.w]
+    return [translation, quaternion]
+
+
+def TransformToMatrix(old_transform):
+    [translation, quaternion] = ComponentsFromTransform(old_transform)
+    tfmatrix = BuildMatrix(translation, quaternion)
+    return tfmatrix
+
+
+def TransformFromMatrix(old_matrix):
+    [translation, quaternion] = ExtractFromMatrix(old_matrix)
+    transformed = TransformFromComponents(translation, quaternion)
+    return transformed
+
+
+def PoseToMatrix(old_pose):
+    old_transform = PoseToTransform(old_pose)
+    [translation, quaternion] = ComponentsFromTransform(old_transform)
+    tfmatrix = BuildMatrix(translation, quaternion)
+    return tfmatrix
+
+
+def PoseFromMatrix(old_matrix):
+    [translation, quaternion] = ExtractFromMatrix(old_matrix)
+    transformed = TransformFromComponents(translation, quaternion)
+    posed = PoseFromTransform(transformed)
+    return posed
+
+
+def ExtractRawFromMatrix(tm):
+    rmat = array([[tm[0][0], tm[0][1], tm[0][2]], [tm[1][0], tm[1][1], tm[1][2]], [tm[2][0], tm[2][1], tm[2][2]]])
+    tvec = [tm[0][3], tm[1][3], tm[2][3]]
+    return [rmat, tvec]
+
+
+def BuildRawMatrix(rm, tv):
+    tfmatrix = array([[rm[0][0], rm[0][1], rm[0][2], tv[0]], [rm[1][0], rm[1][1], rm[1][2], tv[1]], [rm[2][0], rm[2][1], rm[2][2], tv[2]], [0.0, 0.0, 0.0, 1.0]])
+    return tfmatrix
+
+
+def ExtractFromMatrix(old_matrix):
+    quaternion = quaternion_from_matrix(old_matrix)
+    translation = [old_matrix[0][3], old_matrix[1][3], old_matrix[2][3]]
+    return [translation, quaternion]
+
+
+def BuildMatrix(translation, quaternion):
+    tfmatrix = quaternion_matrix(quaternion)
+    tfmatrix[0][3] = translation[0]
+    tfmatrix[1][3] = translation[1]
+    tfmatrix[2][3] = translation[2]
+    return tfmatrix
+
+
+def BuildMatrixRos(translation, quaternion):
+    quat = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
+    trans = [translation.x, translation.y, translation.z]
+    return BuildMatrix(trans, quat)
+
+
+''' Generation Functions '''
+
+def BuildMatrixFromTransRot(trans, rot):
+    transform = numpy.empty(shape=[4, 4])
+    transform[0:3, 0:3] = rot
+    transform[0:3, 3] = trans
+    transform[3, 0:3] = 0
+    transform[3, 3] = 1
+    return transform
+
+
+def BuildMatrixFromDH(d, a, theta, alpha):
+    #Do math here
+    tfmatrix = array([[math.cos(theta), -sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha), alpha * math.cos(theta)], [math.sin(theta), math.cos(theta) * math.cos(alpha), -math.cos(theta) * math.sin(alpha), alpha * math.sin(theta)], [0.0, math.sin(alpha), math.cos(alpha), d], [0.0, 0.0, 0.0, 1.0]])
+    return tfmatrix
+
+
+def ExtractFromDH(d, a, theta, alpha):
+    tfmatrix = BuildMatrixFromDH(d, a, theta, alpha)
+    [translation, quaternion] = ExtractFromMatrix(tfmatrix)
+    return [translation, quaternion]
+
+
+def TransformFromDH(d, a, theta, alpha):
+    tfmatrix = BuildMatrixFromDH(d, a, theta, alpha)
+    return TransformFromMatrix(tfmatrix)
+
+
+def PoseFromDH(d, a, theta, alpha):
+    tfmatrix = BuildMatrixFromDH(d, a, theta, alpha)
+    return PoseFromMatrix(tfmatrix)

flightlib/third_party/arc_utilities/src/arc_utilities/zlib_helpers.cpp

@@ -0,0 +1,141 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+#include "arc_utilities/arc_exceptions.hpp"
+#include "arc_utilities/zlib_helpers.hpp"
+
+namespace ZlibHelpers
+{
+    // MAKE SURE THE INPUT BUFFER IS LESS THAN 4GB IN SIZE
+    std::vector<uint8_t> DecompressBytes(const std::vector<uint8_t>& compressed)
+    {
+        z_stream strm;
+        std::vector<uint8_t> buffer;
+        const size_t BUFSIZE = 1024 * 1024;
+        uint8_t temp_buffer[BUFSIZE];
+        strm.zalloc = Z_NULL;
+        strm.zfree = Z_NULL;
+        strm.opaque = Z_NULL;
+        int ret = inflateInit(&strm);
+        if (ret != Z_OK)
+        {
+            (void)inflateEnd(&strm);
+            std::cerr << "ZLIB unable to init inflate stream" << std::endl;
+            throw std::invalid_argument("ZLIB unable to init inflate stream");
+        }
+        strm.avail_in = (uint32_t)compressed.size();
+        strm.next_in = const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(compressed.data()));
+        do
+        {
+            strm.next_out = temp_buffer;
+            strm.avail_out = BUFSIZE;
+            ret = inflate(&strm, Z_NO_FLUSH);
+            if (buffer.size() < strm.total_out)
+            {
+                buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE - strm.avail_out);
+            }
+        }
+        while (ret == Z_OK);
+        if (ret != Z_STREAM_END)
+        {
+            (void)inflateEnd(&strm);
+            std::cerr << "ZLIB unable to inflate stream with ret=" << ret << std::endl;
+            throw std::invalid_argument("ZLIB unable to inflate stream");
+        }
+        (void)inflateEnd(&strm);
+        std::vector<uint8_t> decompressed(buffer);
+        return decompressed;
+    }
+
+    // MAKE SURE THE INPUT BUFFER IS LESS THAN 4GB IN SIZE
+    std::vector<uint8_t> CompressBytes(const std::vector<uint8_t>& uncompressed)
+    {
+        z_stream strm;
+        std::vector<uint8_t> buffer;
+        const size_t BUFSIZE = 1024 * 1024;
+        uint8_t temp_buffer[BUFSIZE];
+        strm.zalloc = Z_NULL;
+        strm.zfree = Z_NULL;
+        strm.opaque = Z_NULL;
+        strm.avail_in = (uint32_t)uncompressed.size();
+        strm.next_in = const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(uncompressed.data()));
+        strm.next_out = temp_buffer;
+        strm.avail_out = BUFSIZE;
+        int ret = deflateInit(&strm, Z_BEST_SPEED);
+        if (ret != Z_OK)
+        {
+            (void)deflateEnd(&strm);
+            std::cerr << "ZLIB unable to init deflate stream" << std::endl;
+            throw std::invalid_argument("ZLIB unable to init deflate stream");
+        }
+        while (strm.avail_in != 0)
+        {
+            ret = deflate(&strm, Z_NO_FLUSH);
+            if (ret != Z_OK)
+            {
+                (void)deflateEnd(&strm);
+                std::cerr << "ZLIB unable to deflate stream" << std::endl;
+                throw std::invalid_argument("ZLIB unable to deflate stream");
+            }
+            if (strm.avail_out == 0)
+            {
+                buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE);
+                strm.next_out = temp_buffer;
+                strm.avail_out = BUFSIZE;
+            }
+        }
+        int deflate_ret = Z_OK;
+        while (deflate_ret == Z_OK)
+        {
+            if (strm.avail_out == 0)
+            {
+                buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE);
+                strm.next_out = temp_buffer;
+                strm.avail_out = BUFSIZE;
+            }
+            deflate_ret = deflate(&strm, Z_FINISH);
+        }
+        if (deflate_ret != Z_STREAM_END)
+        {
+            (void)deflateEnd(&strm);
+            std::cerr << "ZLIB unable to deflate stream" << std::endl;
+            throw std::invalid_argument("ZLIB unable to deflate stream");
+        }
+        buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE - strm.avail_out);
+        (void)deflateEnd(&strm);
+        std::vector<uint8_t> compressed(buffer);
+        return compressed;
+    }
+
+    std::vector<uint8_t> LoadFromFileAndDecompress(const std::string& path)
+    {
+        std::ifstream input_file(path, std::ios::binary | std::ios::in | std::ios::ate);
+        if (!input_file.is_open())
+        {
+            throw_arc_exception(std::runtime_error, "Couldn't open file " + path);
+        }
+        std::streamsize size = input_file.tellg();
+        input_file.seekg(0, std::ios::beg);
+        std::vector<uint8_t> file_buffer((size_t)size);
+        if (!(input_file.read(reinterpret_cast<char*>(file_buffer.data()), size)))
+        {
+            throw_arc_exception(std::runtime_error, "Unable to read entire contents of file");
+        }
+        const std::vector<uint8_t> decompressed = ZlibHelpers::DecompressBytes(file_buffer);
+        return decompressed;
+    }
+
+    void CompressAndWriteToFile(const std::vector<uint8_t>& uncompressed, const std::string& path)
+    {
+        const auto compressed = CompressBytes(uncompressed);
+        std::ofstream output_file(path, std::ios::out | std::ios::binary);
+        output_file.write(reinterpret_cast<const char*>(compressed.data()), (std::streamsize)compressed.size());
+        output_file.close();
+    }
+}

flightlib/third_party/arc_utilities/src/test_arc_utilities.cpp

@@ -0,0 +1,101 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <chrono>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/pretty_print.hpp>
+#include <arc_utilities/abb_irb1600_145_fk_fast.hpp>
+#include <arc_utilities/iiwa_7_fk_fast.hpp>
+#include <arc_utilities/iiwa_14_fk_fast.hpp>
+#include <arc_utilities/simple_dtw.hpp>
+#include <arc_utilities/simple_prm_planner.hpp>
+
+int main(int argc, char** argv)
+{
+    printf("%d arguments\n", argc);
+    for (int idx = 0; idx < argc; idx++)
+    {
+        printf("Argument %d: %s\n", idx, argv[idx]);
+    }
+    std::cout << "Testing PrettyPrints..." << std::endl;
+    std::cout << PrettyPrint::PrettyPrint(Eigen::Isometry3d::Identity()) << std::endl;
+    std::cout << PrettyPrint::PrettyPrint(Eigen::Vector3d(0.0, 0.0, 0.0)) << std::endl;
+    std::cout << PrettyPrint::PrettyPrint(std::vector<bool>{true, false, true, false}) << std::endl;
+    std::cout << "...done" << std::endl;
+    const std::vector<double> abb_irb_1600_145_base_config = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    const EigenHelpers::VectorIsometry3d abb_irb_1600_145_link_transforms = ABB_IRB1600_145_FK_FAST::GetLinkTransforms(abb_irb_1600_145_base_config);
+    std::cout << "ABB IRB1600-145 Link transforms:\n" << PrettyPrint::PrettyPrint(abb_irb_1600_145_link_transforms, false, "\n") << std::endl;
+    const std::vector<double> iiwa_7_base_config = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    const EigenHelpers::VectorIsometry3d iiwa_7_link_transforms = IIWA_7_FK_FAST::GetLinkTransforms(iiwa_7_base_config);
+    std::cout << "IIWA 7 Link transforms:\n" << PrettyPrint::PrettyPrint(iiwa_7_link_transforms, false, "\n") << std::endl;
+    const std::vector<double> iiwa_14_base_config = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    const EigenHelpers::VectorIsometry3d iiwa_14_link_transforms = IIWA_14_FK_FAST::GetLinkTransforms(iiwa_14_base_config);
+    std::cout << "IIWA 14 Link transforms:\n" << PrettyPrint::PrettyPrint(iiwa_14_link_transforms, false, "\n") << std::endl;
+    std::cout << std::endl;
+
+    // Test Vector3d averaging
+    EigenHelpers::VectorVector3d testvecs(8, Eigen::Vector3d::Zero());
+    testvecs[0] = Eigen::Vector3d(-1.0, -1.0, -1.0);
+    testvecs[1] = Eigen::Vector3d(-1.0, -1.0, 1.0);
+    testvecs[2] = Eigen::Vector3d(-1.0, 1.0, -1.0);
+    testvecs[3] = Eigen::Vector3d(-1.0, 1.0, 1.0);
+    testvecs[4] = Eigen::Vector3d(1.0, -1.0, -1.0);
+    testvecs[5] = Eigen::Vector3d(1.0, -1.0, 1.0);
+    testvecs[6] = Eigen::Vector3d(1.0, 1.0, -1.0);
+    testvecs[7] = Eigen::Vector3d(1.0, 1.0, 1.0);
+    std::cout << "Individual vectors:\n" << PrettyPrint::PrettyPrint(testvecs, true, "\n") << std::endl;
+    Eigen::Vector3d averagevec = EigenHelpers::AverageEigenVector3d(testvecs);
+    std::cout << "Average vector: " << PrettyPrint::PrettyPrint(averagevec) << std::endl;
+    std::cout << std::endl;
+    assert(averagevec.isMuchSmallerThan(10^-10) && "The result of this average should be (0, 0, 0)");
+
+
+    // Test weighted dot product functions
+    Eigen::Vector3d weights(1.0, 2.0, 3.0);
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[0]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[0], weights) << std::endl;
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[7]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[7], weights) << std::endl;
+    std::cout << "Weighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[0], testvecs[7], weights) << std::endl;
+    std::cout << "Unweighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[0], testvecs[7], Eigen::Vector3d::Ones()) << std::endl;
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[1]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[1], weights) << std::endl;
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[2]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[2], weights) << std::endl;
+    std::cout << "Weighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[1], testvecs[2], weights) << std::endl;
+    std::cout << "Unweighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[1], testvecs[2], Eigen::Vector3d::Ones()) << std::endl;
+    std::cout << std::endl;
+
+    // Test truncated normal distribution
+    auto seed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+    std::mt19937_64 prng(seed);
+    arc_helpers::TruncatedNormalDistribution dist(0.0, 1.0, -5.0, 5.0);
+    std::vector<double> test_trunc_normals(1000, 0.0);
+    for (size_t idx = 0; idx < test_trunc_normals.size(); idx++)
+    {
+        test_trunc_normals[idx] = dist(prng);
+    }
+    std::cout << "Truncated normal test:\n" << PrettyPrint::PrettyPrint(test_trunc_normals, false, ",") << std::endl;
+    std::cout << std::endl;
+
+    // Test multivariate gaussian
+    std::cout << "MVN Gaussian test:\n";
+    Eigen::Vector2d mean(0.0, 1.0);
+    Eigen::Matrix2d covar;
+    covar << 10.0, 5.0,
+             5.0, 10.0;
+    arc_helpers::MultivariteGaussianDistribution mvn_dist(mean, covar);
+    std::vector<Eigen::VectorXd> mvn_gaussians(3000);
+    for (size_t idx = 0; idx < mvn_gaussians.size(); idx++)
+    {
+        mvn_gaussians[idx] = mvn_dist(prng);
+        std::cout << mvn_gaussians[idx].transpose() << std::endl;
+    }
+    std::cout << std::endl;
+
+    // Test DTW
+    std::vector<double> reversed_test_trunc_normals = test_trunc_normals;
+    std::reverse(reversed_test_trunc_normals.begin(), reversed_test_trunc_normals.end());
+    std::function<double(const double&, const double&)> double_distance_fn = [] (const double& d1, const double& d2) { return std::abs(d1 - d2); };
+    const double test_dist = simple_dtw::ComputeDTWDistance(test_trunc_normals, test_trunc_normals, double_distance_fn);
+    const double reversed_test_dist = simple_dtw::ComputeDTWDistance(test_trunc_normals, reversed_test_trunc_normals, double_distance_fn);
+    std::cout << "DTW distance test = " << test_dist << ", reversed = " << reversed_test_dist << std::endl;
+    std::cout << std::endl;
+}

flightlib/third_party/arc_utilities/src/test_averaging.cpp

@@ -0,0 +1,47 @@
+#include "arc_utilities/eigen_helpers.hpp"
+#include "arc_utilities/pretty_print.hpp"
+
+using namespace Eigen;
+using namespace EigenHelpers;
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    const std::vector<double> weights = {1.0, 2.0, 3.0};
+
+    VectorVector3d test_vectors(3);
+    test_vectors[0] = Vector3d::UnitX();
+    test_vectors[1] = Vector3d::UnitY();
+    test_vectors[2] = Vector3d::UnitZ();
+
+    const Vector3d weighted_average = AverageEigenVector(test_vectors, weights);
+    std::cout << "Test weights: " << PrettyPrint::PrettyPrint(weights) << std::endl;
+    std::cout << "Test vectors:\n" << PrettyPrint::PrettyPrint(test_vectors, true, "\n") << std::endl;
+    std::cout << "Weighted Average:\n" << PrettyPrint::PrettyPrint(weighted_average) << std::endl << std::endl;
+    assert(weighted_average.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0) / 6.0));
+
+
+    VectorVector3d repeated_test_vectors;
+    for (size_t idx = 0; idx < test_vectors.size(); ++idx)
+    {
+        repeated_test_vectors.insert(repeated_test_vectors.end(), (size_t)weights[idx], test_vectors[idx]);
+    }
+
+    const Vector3d repeated_average = AverageEigenVector(repeated_test_vectors);
+    std::cout << "Repeated vectors:\n" << PrettyPrint::PrettyPrint(repeated_test_vectors, true, "\n") << std::endl;
+    std::cout << "Repeated Average:\n" << PrettyPrint::PrettyPrint(repeated_average) << std::endl << std::endl;;
+    assert(repeated_average.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0) / 6.0));
+
+
+
+    const std::vector<double> all_zero_weights = {0.0, 0.0, 0.0};
+    const Vector3d weighted_average_all_zero_weights = AverageEigenVector(test_vectors, all_zero_weights);
+    std::cout << "Test weights: " << PrettyPrint::PrettyPrint(all_zero_weights) << std::endl;
+    std::cout << "Test vectors:\n" << PrettyPrint::PrettyPrint(test_vectors, true, "\n") << std::endl;
+    std::cout << "All Zero Weights Average:\n" << PrettyPrint::PrettyPrint(weighted_average_all_zero_weights) << std::endl << std::endl;
+    assert(weighted_average_all_zero_weights.isMuchSmallerThan(10^-10) && "The result of this average should be (0, 0, 0)");
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_closest_point.cpp

@@ -0,0 +1,27 @@
+#include "arc_utilities/eigen_helpers.hpp"
+#include "arc_utilities/pretty_print.hpp"
+
+using namespace Eigen;
+using namespace EigenHelpers;
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    const Vector3d base_point(-0.2, 0.035, 0.2055);
+    const Vector3d normal(0, 0, 1);
+
+    const Vector3d test_point(-0.198425, 0.0342693, 0.0417458);
+
+    const auto distances = DistanceToLine(base_point, normal, test_point);
+
+    std::cout << "Distance to line: " << distances.first << " DISPLACEMENT along line (can be negative): " << distances.second << std::endl;
+
+    const Vector3d point_on_line = base_point + distances.second * normal;
+
+    std::cout << "Point on line: " << point_on_line.transpose() << std::endl;
+    std::cout << "Dot product: " << normal.dot(test_point - point_on_line) << std::endl;
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_dijkstras.cpp

@@ -0,0 +1,124 @@
+#include <iostream>
+
+#include <arc_utilities/dijkstras.hpp>
+#include <arc_utilities/pretty_print.hpp>
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    std::cout << "Creating graph with nodes indices:\n\n";
+    std::cout << "  2 | 5 | 8\n"
+              << "  --+---+--\n"
+              << "  1 | 4 | 7\n"
+              << "  --+---+--\n"
+              << "  0 | 3 | 6\n\n";
+
+    arc_dijkstras::Graph<Eigen::Vector2d> graph(9);
+    for (double x = -1; x <= 1; x += 1)
+    {
+        for (double y = -1; y <= 1; y += 1)
+        {
+            graph.AddNode(Eigen::Vector2d(x, y));
+        }
+    }
+
+    // Y-direction edges
+    graph.AddEdgeBetweenNodes(0, 1, 1.0);
+    graph.AddEdgesBetweenNodes(1, 2, 1.0);
+    graph.AddEdgesBetweenNodes(3, 4, 1.0);
+    graph.AddEdgesBetweenNodes(4, 5, 1.0);
+    graph.AddEdgesBetweenNodes(6, 7, 1.0);
+    graph.AddEdgesBetweenNodes(7, 8, 1.0);
+
+    // X-direction edges
+    graph.AddEdgesBetweenNodes(0, 3, 1.0);
+    graph.AddEdgesBetweenNodes(3, 6, 1.0);
+    graph.AddEdgesBetweenNodes(1, 4, 1.0);
+    graph.AddEdgesBetweenNodes(4, 7, 1.0);
+    graph.AddEdgesBetweenNodes(2, 5, 1.0);
+    graph.AddEdgesBetweenNodes(5, 8, 1.0);
+
+    assert(graph.CheckGraphLinkage());
+
+
+    auto dijkstras_result_4connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
+
+    std::cout << "4-connected edges\n"
+              << "Node index            : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
+    std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.first) << std::endl;
+    std::cout << "Distance              : " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.second) << std::endl;
+
+    // Diagonal edges
+    graph.AddEdgesBetweenNodes(0, 4, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(1, 5, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(3, 7, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(4, 8, std::sqrt(2));
+
+    graph.AddEdgesBetweenNodes(1, 3, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(2, 4, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(4, 6, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(5, 7, std::sqrt(2));
+
+    assert(graph.CheckGraphLinkage());
+    auto dijkstras_result_8connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
+
+    std::cout << "\n8-connected edges\n"
+              << "Node index            : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
+    std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.first) << std::endl;
+    std::cout << "Distance              : " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.second) << std::endl;
+
+    std::cout << "\nSerialization test... ";
+
+    arc_dijkstras::Graph<Eigen::Vector2d> serialization_test_graph(2);
+    serialization_test_graph.AddNode(Eigen::Vector2d(0,0));
+    serialization_test_graph.AddNode(Eigen::Vector2d(1,1));
+    serialization_test_graph.AddEdgesBetweenNodes(0, 1, 1.0);
+
+    // Define the graph value serialization function
+    const auto value_serializer_fn = [] (const Eigen::Vector2d& value, std::vector<uint8_t>& buffer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        uint64_t running_total = 0;
+
+        running_total += arc_utilities::SerializeFixedSizePOD<double>(value(0), buffer);
+        running_total += arc_utilities::SerializeFixedSizePOD<double>(value(1), buffer);
+
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+
+        assert(running_total == bytes_written);
+
+        return bytes_written;
+    };
+
+    // Define the graph value serialization function
+    const auto value_deserializer_fn = [] (const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        uint64_t current_position = current;
+
+        // Deserialze 2 doubles
+        std::pair<double, uint64_t> x = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+        current_position += x.second;
+        std::pair<double, uint64_t> y = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+        current_position += y.second;
+
+        const Eigen::Vector2d deserialized(x.first, y.first);
+
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    };
+
+    // Serialze the graph
+    std::vector<uint8_t> buffer;
+    serialization_test_graph.SerializeSelf(buffer, value_serializer_fn);
+
+    auto deserialized_result = arc_dijkstras::Graph<Eigen::Vector2d>::Deserialize(buffer, 0, value_deserializer_fn);
+    assert(deserialized_result.first.CheckGraphLinkage());
+
+    std::cout << "passed" << std::endl;
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_eigen_math.cpp

@@ -0,0 +1,96 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <chrono>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/aligned_eigen_types.hpp>
+#include <arc_utilities/pretty_print.hpp>
+
+inline void TestVector3d(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    std::chrono::time_point<std::chrono::steady_clock> vector3_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Vector3d test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx);
+        results.block<3, 1>(idx * 4, 0) = base_transform * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> vector3_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> vector3_test_time(vector3_end_time - vector3_start_time);
+    std::cout << "Isometry3d * Vector3d test - " << vector3_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+inline void TestVector4d(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    std::chrono::time_point<std::chrono::steady_clock> vector4_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Vector4d test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx, 1.0);
+        results.block<4, 1>(idx * 4, 0) = base_transform * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> vector4_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> vector4_test_time(vector4_end_time - vector4_start_time);
+    std::cout << "Isometry3d * Vector4d test - " << vector4_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+inline void TestAlignedVector3d(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    std::chrono::time_point<std::chrono::steady_clock> alignedvector3_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Aligned4Vector3<double> test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx);
+        results.block<3, 1>(idx * 4, 0) = base_transform * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> alignedvector3_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> alignedvector3_test_time(alignedvector3_end_time - alignedvector3_start_time);
+    std::cout << "Isometry3d * AlignedVector3d test - " << alignedvector3_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+inline void TestManual(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    const Eigen::Matrix4d& base_transform_matrix = base_transform.matrix();
+    std::chrono::time_point<std::chrono::steady_clock> manual_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Vector4d test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx, 1.0);
+        results.block<4, 1>(idx * 4, 0) = base_transform_matrix * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> manual_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> manual_test_time(manual_end_time - manual_start_time);
+    std::cout << "Matrix4d * Vector4d test - " << manual_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+int main(int argc, char** argv)
+{
+    printf("%d arguments\n", argc);
+    for (int idx = 0; idx < argc; idx++)
+    {
+        printf("Argument %d: %s\n", idx, argv[idx]);
+    }
+    const ssize_t iterations = 100000000;
+    const Eigen::Isometry3d base_transform = Eigen::Translation3d(10.0, 10.0, 10.0) * EigenHelpers::QuaternionFromRPY(0.25, 0.5, 0.75);
+    Eigen::MatrixXd results = Eigen::MatrixXd::Ones(iterations * 4, 1);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    //
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    //
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    //
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_hierarchical_clustering.cpp

@@ -0,0 +1,64 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <chrono>
+#include <iostream>
+#include <fstream>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/pretty_print.hpp>
+#include <arc_utilities/simple_hierarchical_clustering.hpp>
+#include <arc_utilities/abb_irb1600_145_fk_fast.hpp>
+
+int main(int argc, char** argv)
+{
+    printf("%d arguments\n", argc);
+    for (int idx = 0; idx < argc; idx++)
+    {
+        printf("Argument %d: %s\n", idx, argv[idx]);
+    }
+    const size_t num_points = (argc >= 2) ? (size_t)(atoi(argv[1])) : 1000;
+    std::cout << "Generating " << num_points << " random points..." << std::endl;
+    const auto seed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+    std::mt19937_64 prng(seed);
+    std::uniform_real_distribution<double> dist(0.0, 10.0);
+    EigenHelpers::VectorVector3d random_points(num_points);
+    std::vector<size_t> indices(num_points);
+    for (size_t idx = 0; idx < num_points; idx++)
+    {
+        const double x = dist(prng);
+        const double y = dist(prng);
+        random_points[idx] = Eigen::Vector3d(x, y, 0.0);
+        indices[idx] = idx;
+    }
+    std::cout << "Clustering " << num_points << " points..." << std::endl;
+    std::function<double(const Eigen::Vector3d&, const Eigen::Vector3d&)> distance_fn = [] (const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) { return EigenHelpers::Distance(v1, v2); };
+    const Eigen::MatrixXd distance_matrix = arc_helpers::BuildDistanceMatrix(random_points, distance_fn);
+    const std::vector<std::vector<size_t>> clusters = simple_hierarchical_clustering::SimpleHierarchicalClustering::Cluster(indices, distance_matrix, 1.0).first;
+    for (size_t cluster_idx = 0; cluster_idx < clusters.size(); cluster_idx++)
+    {
+        const std::vector<size_t>& current_cluster = clusters[cluster_idx];
+        const double cluster_num = 1.0 + (double)cluster_idx;
+        for (size_t element_idx = 0; element_idx < current_cluster.size(); element_idx++)
+        {
+            const size_t index = current_cluster[element_idx];
+            random_points[index].z() = cluster_num;
+        }
+    }
+    std::cout << "Saving to CSV..." << std::endl;
+    const std::string log_file_name = (argc >=3 ) ? std::string(argv[2]) : "/tmp/test_hierarchical_clustering.csv";
+    std::ofstream log_file(log_file_name, std::ios_base::out);
+    if (!log_file.is_open())
+    {
+        std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << log_file_name << "\x1b[0m \n";
+        throw std::invalid_argument("Log filename must be write-openable");
+    }
+    for (size_t idx = 0; idx < num_points; idx++)
+    {
+        const Eigen::Vector3d& point = random_points[idx];
+        log_file << point.x() << "," << point.y() << "," << point.z() << std::endl;
+    }
+    log_file.close();
+    std::cout << "Done saving, you can import into matlab and draw with \"scatter3(x, y, z, 50, z, '.')\"" << std::endl;
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_shortcut_smoothing.cpp

@@ -0,0 +1,26 @@
+#include <iostream>
+#include "arc_utilities/shortcut_smoothing.hpp"
+#include "arc_utilities/pretty_print.hpp"
+
+int main(int argc, char** argv)
+{
+    (void)argc;
+    (void)argv;
+
+    EigenHelpers::VectorVector3d vec(10);
+    for (int i = 0; i < 10; ++i)
+    {
+        vec[i] = Eigen::Vector3d(i, 0.0, 0.0);
+    }
+
+    const double step_size = 0.1;
+    const auto collision_fn = [] (const Eigen::Vector3d& location) { (void)location; return location.norm() > 4.0; };
+
+    auto vec_smoothed = shortcut_smoothing::InterpolateWithCollisionCheck(vec, 1, 4, step_size, collision_fn);
+
+
+    std::cout << "Original:\n" << PrettyPrint::PrettyPrint(vec, true, "\n") << std::endl << std::endl;
+    std::cout << "Smoothed:\n" << PrettyPrint::PrettyPrint(vec_smoothed, true, "\n") << std::endl << std::endl;
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/timing.cpp

@@ -0,0 +1,7 @@
+#include "arc_utilities/timing.hpp"
+
+double arc_utilities::GlobalStopwatch(const StopwatchControl control)
+{
+    static arc_utilities::Stopwatch global_stopwatch;
+    return global_stopwatch(control);
+}