Feature/external controller

external-control/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.5)
+set(CMAKE_CXX_STANDARD 17)
+
+project(external-control CXX)
+
+set(INSTALL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/lib" CACHE PATH "Installation directory for libraries")
+set(INSTALL_INC_DIR "${CMAKE_INSTALL_PREFIX}/include" CACHE PATH "Installation directory for headers")
+
+find_package(Catch2 2 REQUIRED)
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+file(GLOB CONTROL_SOURCES
+        "src/main/cpp/controllers/*.hpp"
+        "src/main/cpp/controllers/*.cpp")
+set(SOURCES
+        ${CONTROL_SOURCES}
+        src/main/cpp/external-control.hpp
+        src/main/cpp/external-control.cpp
+        src/main/cpp/types.hpp)
+
+add_library(external-wrapper SHARED ${SOURCES})
+target_include_directories(external-wrapper PUBLIC
+        src/main/cpp)
+target_link_libraries(external-wrapper PUBLIC
+        Eigen3::Eigen)
+
+install(TARGETS external-wrapper
+        EXPORT targets
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+install(DIRECTORY
+        src/main/cpp/
+        DESTINATION "${INSTALL_INC_DIR}"
+        FILES_MATCHING PATTERN "*.hpp")

external-control/Dockerfile

@@ -0,0 +1,71 @@
+# Current version: 0.1
+FROM ubuntu:22.04
+
+ARG DEBIAN_FRONTEND=noninteractive
+
+# Install commonly required libraries and packages
+RUN apt-get --quiet 2 --yes update
+
+# Common utilities
+RUN apt-get --quiet 2 --yes install \
+    sudo \
+    apt-transport-https \
+    ca-certificates \
+    software-properties-common \
+    nano \
+    git \
+    wget \
+    curl \
+    unzip \
+    python3 \
+    > /dev/null
+
+# Java
+RUN apt-get --quiet 2 --yes install \
+    openjdk-17-jdk \
+    > /dev/null
+
+# C++ development utilities -- we need to add Kitware's repositories in order to get a specific version of CMake
+RUN wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null
+RUN apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main"
+RUN apt-get update
+RUN apt-get install kitware-archive-keyring
+RUN rm /etc/apt/trusted.gpg.d/kitware.gpg
+RUN apt-get --quiet 2 --yes install \
+    build-essential \
+    gdb \
+    ninja-build \
+    clang \
+    mold \
+    cmake \
+    valgrind \
+    patchelf \
+    catch2 \
+    > /dev/null
+
+# Install Eigen
+RUN apt-get --quiet 2 --yes install libeigen3-dev
+# Ensure eigen is available at /usr/include/Eigen, Ubuntu installs the headers at /usr/include/eigen3/Eigen
+RUN ln -s /usr/include/eigen3/Eigen /usr/include/Eigen
+# Ditto for unsupported
+RUN ln -s /usr/include/eigen3/unsupported /usr/include/unsupported
+# Install Python 3 which is required for a gdb extension that allows pretty printing of Eigen types in debugger.
+# Because of docker integration with CLion, we must place the .gdbinit file in the root directory -- all containers will
+# run with / as the $HOME directory, which is where .gdbinit should be placed
+WORKDIR /
+RUN touch .gdbinit
+RUN printf "python\n\
+import sys\n\
+sys.path.insert(0, '/home/robotlab/gdb-eigen')\n\
+from printers import register_eigen_printers\n\
+register_eigen_printers(None)\n\
+end" > .gdbinit
+WORKDIR /home/robotlab/gdb-eigen
+RUN wget -O printers.py https://gitlab.com/libeigen/eigen/-/raw/master/debug/gdb/printers.py
+RUN touch __init__.py
+
+# Update alternatives so that clang is the default compiler
+RUN update-alternatives --install /usr/bin/cc cc /usr/bin/clang 100
+RUN update-alternatives --install /usr/bin/c++ c++ /usr/bin/clang++ 100
+# Symlink mold to ld so it is the default linker, recommended by: https://lld.llvm.org/ (Using LLD)
+RUN ln -sf /usr/bin/mold /usr/bin/ld

external-control/README.md

@@ -0,0 +1,81 @@
+# IHMC-External Control
+
+**NOTE: We currently ONLY support Ubuntu 22.04. Other versions of Ubuntu, and Windows / macOS operating systems are not supported and are almost certain to break.**
+
+After reading this and installing, be sure to read [ARCHITECTURE.md](ARCHITECTURE.md) and [CONTRIBUTING.md](CONTRIBUTING.md) for more information on how the code in this repository is structured, and how to contribute effectively.
+
+## Contents
+
+* [Overview](#overview)
+* [Dependencies](#dependencies)
+* [Install](#install)
+* [Development](#development)
+* [Troubleshooting](#troubleshooting)
+    * [Failed Docker container creation](#failed-docker-container-creation)
+    * [Annoying permissions issues in generated-java](#annoying-permissions-issues-in-generated-java)
+
+## Overview
+
+* This project contains a thin C++ wrapper to allow setting up calls to a custom controller on the C++ side, and then interfacing with it from Java
+* We create JNI bindings for the C++ wrapper library
+
+## Dependencies
+
+* Docker (check with `docker --version`). You'll want to run `docker` commands without having to prefix `sudo` all the
+  time, follow the instructions [here](https://askubuntu.com/questions/477551/how-can-i-use-docker-without-sudo).
+
+## Install
+
+Starting from a typical IHMC setup consisting of a `repository-group` workspace
+with `ihmc-open-robotics-software`, perform the following steps:
+
+1. Clone this repository into `repository-group`.
+
+(NOTE: you may need to `chmod +x <script>` to run the following scripts)
+
+2. From the command line or your IDE terminal, navigate to `external-control` and run the
+   script `./createDockerImage.sh` to create the docker image used for isolated building of the dependencies.
+3. From the command line or your IDE terminal, navigate to `external-control` and run the
+   script `./generateJavaBindingsCI.sh` (if building for development) or `./generateJavaBindingsRelease.sh` (if building
+   for robot deployment, this is in-development). This builds the C++ wrapper in the docker container, and
+   is subsequently used to generate the Java bindings. The native Java bindings are generated inside the docker
+   container, and then everything is copied (including the required shared libraries) from inside the docker container
+   to the `src/main/resources` directory of this project to be used by IHMC's native library loader. *NOTE: this step
+   may take a short while*.
+
+## Development
+
+* If using JetBrains IDEs (IntelliJ for Java, CLion for C++), they will not play nice if they are opened in the same
+  directory. To combat this, open IntelliJ in the overall `repository-group` workspace, and CLion in
+  the `ihmc-open-robotics-software` directory. This way, the `.idea` directories are separate and won't conflict with each
+  other.
+* When developing the wrapper in CLion, set up a Docker CMake toolchain to build the project, the image must be set to
+  the image created when `createDockerImage.sh` was
+  run: https://www.jetbrains.com/help/clion/clion-toolchains-in-docker.html
+* When working in C++, please use [this style guide](IHMC-MPC-CodeStyle.xml). You can import the XML into CLion so that
+  you can auto-format your code.
+
+## Troubleshooting
+
+### Failed Docker container creation
+
+If running `createDockerImage.sh` fails for any reason, it is highly likely that docker will cache some image
+layers that are incorrect and will cause future attempts at image creation to fail.
+
+Before running `createDockerImage.sh` again:
+
+1. run `docker image remove ihmc-external-wrapper:0.1` (or whatever version of the wrapper you're using)
+2. run `docker container prune` and `docker image prune`
+3. verify that `docker image list -a` is empty, this means all the cached images have been deleted
+4. run `createDockerImage.sh` again
+
+
+### Annoying permissions issues in `generated-java`
+
+Due to the way our shell scripts build the wrapper and generate the bindings, it is quite likely that the `src/main/generated-java`
+and `src/main/resources` directories will be owned by `root`. This is annoying because it means that you can't delete or rollback
+the changes in git. To fix this, run this from the project root:
+
+```bash
+sudo chown -R $USER:$USER ihmc-mpc-core ihmc-mpc-nadia
+```

external-control/build.gradle.kts

@@ -0,0 +1,16 @@
+plugins {
+   id("us.ihmc.ihmc-build")
+   id("us.ihmc.log-tools-plugin") version "0.6.3"
+}
+
+ihmc {
+   loadProductProperties("../product.properties")
+
+   configureDependencyResolution()
+   javaDirectory("main", "generated-java")
+   configurePublications()
+}
+
+mainDependencies {
+   api("us.ihmc:ihmc-avatar-interfaces:source")
+}

external-control/createDockerImage.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+set -e -o xtrace
+
+# NOTE: the version of the image should be updated when the image is updated
+docker build -t ihmc-external-wrapper:0.1 .

external-control/generateJavaBindingsCI.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+set -e -o xtrace
+
+docker run \
+  --rm \
+  --volume $(pwd):/home/robotlab/ihmc-external-wrapper \
+  --workdir /home/robotlab/ihmc-external-wrapper \
+  ihmc-external-wrapper:0.1 bash -c """
+  /home/robotlab/ihmc-external-wrapper/scripts/buildWrapper.sh --test && \
+  /home/robotlab/ihmc-external-wrapper/scripts/generateBindings.sh
+  """

external-control/gradle.properties

@@ -0,0 +1,6 @@
+kebabCasedName = external-control
+pascalCasedName = ExternalControl
+extraSourceSets = ["test"]
+publishUrl = local
+compositeSearchHeight = 2
+excludeFromCompositeBuild = false

external-control/scripts/buildWrapper.sh

@@ -0,0 +1,24 @@
+#!/bin/bash
+set -e -o xtrace
+
+cd /home/robotlab/ihmc-external-wrapper
+
+# Create build directory if it doesn't already exist
+mkdir -p build/lib && cd build
+
+# Default to not building tests, unless --test flag is passed in
+TEST="OFF"
+if [ "$1" == "--test" ]; then
+  TEST="ON"
+fi
+
+# Build and install the wrapper
+cmake \
+-G Ninja \
+-DCMAKE_INSTALL_PREFIX=. \
+-DCMAKE_BUILD_TYPE=Release \
+-DBUILD_EXAMPLES=OFF \
+-DBUILD_TESTING="$TEST" \
+-DCMAKE_CXX_FLAGS="-O3" \
+..
+cmake --build . --target install

external-control/scripts/generateBindings.sh

@@ -0,0 +1,56 @@
+#!/bin/bash
+set -e -o xtrace
+
+cd /home/robotlab/ihmc-external-wrapper/build
+
+# Copy all Java code from the root of external-control into the build directory
+cp -r ../src/main/java/ .
+
+# Move into the java directory; javacpp.jar needs to reside here
+cd java
+
+# Clone and checkout JavaCPP from specific tag; should update periodically
+JAVACPP_VERSION=1.5.10
+# Download and unzip javacpp into the java source directory
+# Check if the javacpp.jar already exists -- we can skip the fetching step if it does
+if [ ! -f javacpp.jar ]; then
+  curl -L https://github.com/bytedeco/javacpp/releases/download/$JAVACPP_VERSION/javacpp-platform-$JAVACPP_VERSION-bin.zip -o javacpp-platform-$JAVACPP_VERSION-bin.zip
+  unzip -j javacpp-platform-$JAVACPP_VERSION-bin.zip
+fi
+
+# This will generate the JNI shared library and place it in the classpath resources dir
+java -jar javacpp.jar us/ihmc/externalControl/presets/ExternalControlInfoMapper.java
+java -jar javacpp.jar us/ihmc/externalControl/ExternalControlWrapper.java -d ../src/main/resources/externalControl/linux-x86_64 \
+-Dplatform.compiler="clang++" \
+-Dplatform.compiler.default="-O3"
+
+# Clean old generated code
+rm -rf ../../src/main/generated-java/*
+
+# Copy newly generated Java into generated-java
+mkdir -p ../../src/main/generated-java/us/ihmc/externalControl
+cp -r us/ihmc/externalControl/ExternalControlWrapper.java ../../src/main/generated-java/us/ihmc/externalControl
+chmod 664 ../../src/main/generated-java/us/ihmc/externalControl/ExternalControlWrapper.java # Add write permissions to the generated file, so it can be modified
+
+# Copy over the wrapper and its dependencies -- this will beexternal control, as well as their
+# dependencies recursively
+LINUX_RESOURCES_DIR='../../src/main/resources/externalControl/linux-x86_64'
+cp ../lib/libexternal-wrapper.so $LINUX_RESOURCES_DIR
+# All other dependencies are system libraries, so we look to /usr/lib/x86_64-linux-gnu and loop over them
+#DEP_NAMES=('libboost_filesystem.so.1.74.0' \
+#           'libboost_system.so.1.74.0' \
+#           'libboost_serialization.so.1.74.0' \
+#           'liburdfdom_sensor.so.3.0' \
+#           'liburdfdom_model_state.so.3.0' \
+#           'liburdfdom_model.so.3.0' \
+#           'liburdfdom_world.so.3.0' \
+#           'libconsole_bridge.so.1.0' \
+#           'libomp.so.5')
+for LIB_NAME in "${DEP_NAMES[@]}"; do
+  cp /usr/lib/x86_64-linux-gnu/$LIB_NAME $LINUX_RESOURCES_DIR
+done
+# We also modify all the libraries that end up in the resources folder also have a relative runtime path to their own directory
+RPATH='/usr/lib/x86_64-linux-gnu:$ORIGIN'
+for LIB_NAME in $LINUX_RESOURCES_DIR/*; do
+  patchelf --set-rpath $RPATH $LIB_NAME
+done

external-control/settings.gradle.kts

@@ -0,0 +1,23 @@
+pluginManagement {
+   plugins {
+      id("us.ihmc.ihmc-build") version "1.1.0"
+   }
+}
+
+buildscript {
+   repositories {
+      maven { url = uri("https://plugins.gradle.org/m2/") }
+      mavenLocal()
+   }
+   dependencies {
+      classpath("us.ihmc:ihmc-build:1.1.0")
+   }
+}
+
+/**
+ * Browse source at https://github.com/ihmcrobotics/ihmc-build
+ */
+val ihmcSettingsConfigurator = us.ihmc.build.IHMCSettingsConfigurator(settings, logger, extra)
+ihmcSettingsConfigurator.checkRequiredPropertiesAreSet()
+ihmcSettingsConfigurator.configureExtraSourceSets()
+ihmcSettingsConfigurator.findAndIncludeCompositeBuilds()