Feature/external controller

external-control/.gitignore

@@ -0,0 +1 @@
+cppbuild/

external-control/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.5)
+set(CMAKE_CXX_STANDARD 17)
+
+project(external-control CXX)
+
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+
+file(GLOB_RECURSE SOURCES
+    "src/main/cpp/*.cpp"
+    "src/main/cpp/*.hpp"
+)
+
+add_library(external-control SHARED ${SOURCES})
+target_include_directories(external-control PUBLIC
+        src/main/cpp)
+
+install(TARGETS external-control
+    RUNTIME DESTINATION bin
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib)
+install(DIRECTORY src/main/cpp/ DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.hpp")
+
+target_link_libraries(external-control Eigen3::Eigen zmq)

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,15 @@
+plugins {
+   id("us.ihmc.ihmc-build")
+   id("us.ihmc.log-tools-plugin") version "0.6.3"
+}
+
+ihmc {
+   loadProductProperties("../product.properties")
+
+   configureDependencyResolution()
+   configurePublications()
+}
+
+mainDependencies {
+   api("us.ihmc:ihmc-avatar-interfaces:source")
+}

external-control/cppbuild.bash

@@ -0,0 +1,51 @@
+#!/bin/bash
+rm -rf cppbuild
+mkdir cppbuild
+cd cppbuild
+
+#### Installing Eigen headers ####
+EIGEN_VERSION=3.4.0
+curl https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-$EIGEN_VERSION.zip -O
+unzip -n eigen-$EIGEN_VERSION.zip
+cd eigen-$EIGEN_VERSION
+mkdir build
+cd build
+cmake -DCMAKE_INSTALL_PREFIX=../../ ..
+cmake --install .
+cd ../../
+
+# Build external-control
+cmake -DCMAKE_INSTALL_PREFIX=. ..
+cmake --build . --config Release --target install
+
+### Java generation ####
+cp -r ../src/main/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/ExternalControlInfoMapper.java
+java -jar javacpp.jar us/ihmc/externalControl/global/ExternalControlWrapper.java -d javainstall
+
+# Copy newly generated Java into global
+cp us/ihmc/externalControl/*.java ../src/main/java/us/ihmc/externalControl
+cp us/ihmc/externalControl/global/*.java ../src/main/java/us/ihmc/externalControl/global
+
+
+
+##### Copy shared libs to resources ####
+# Linux
+mkdir -p ../src/main/resources/externalControl/native/linux-x86_64
+if [ -f "javainstall/libjniExternalControlWrapper.so" ]; then
+  cp javainstall/libjniExternalControlWrapper.so ../src/main/resources/externalControl/native/linux-x86_64
+fi
+if [ -f "lib/libexternal-control.so" ]; then
+  cp lib/libexternal-control.so ../src/main/resources/externalControl/native/linux-x86_64
+fi

external-control/gradle.properties

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

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()

external-control/src/main/cpp/controllers/constant-position-controller.cpp

@@ -0,0 +1,76 @@
+#include "constant-position-controller.hpp"
+
+#include <iostream>
+
+namespace ihmc
+{
+    ConstantPositionController::ConstantPositionController(const double default_stiffness, const double default_damping, const int number_of_joints)
+    {
+        number_of_joints_ = number_of_joints;
+        desired_joint_velocities_.resize(number_of_joints_);
+        desired_joint_torques_.resize(number_of_joints_);
+        desired_joint_stiffnesses_.resize(number_of_joints_);
+        desired_joint_damping_.resize(number_of_joints_);
+
+        desired_joint_velocities_.setZero();
+        desired_joint_torques_.setZero();
+        desired_joint_stiffnesses_.fill(default_stiffness);
+        desired_joint_damping_.fill(default_damping);
+    }
+
+    void ConstantPositionController::resize(const double default_stiffness, const double default_damping, const int number_of_joints)
+    {
+        std::cout << "resizing constant position controller" << std::endl;
+        number_of_joints_ = number_of_joints;
+        desired_joint_velocities_.resize(number_of_joints_);
+        desired_joint_torques_.resize(number_of_joints_);
+        desired_joint_stiffnesses_.resize(number_of_joints_);
+        desired_joint_damping_.resize(number_of_joints_);
+
+        desired_joint_velocities_.setZero();
+        desired_joint_torques_.setZero();
+        desired_joint_stiffnesses_.fill(default_stiffness);
+        desired_joint_damping_.fill(default_damping);
+    }
+
+    bool ConstantPositionController::setHomeJointConfiguration(const double* configuration_data, int configuration_rows)
+    {
+        if (configuration_rows != number_of_joints_)
+        {
+            std::cout << "The size of the home configuration is incorrect" << std::endl;
+            return false;
+        }
+
+        const VectorViewReadOnly configuration(configuration_data, configuration_rows);
+        home_configuration_ = configuration;
+
+        std::cout << "home configuration is set" << std::endl;
+
+        return true;
+    }
+
+    Eigen::VectorXd ConstantPositionController::get_desired_joint_positions() const
+    {
+        return home_configuration_;
+    }
+
+    Eigen::VectorXd ConstantPositionController::get_desired_joint_velocities() const
+    {
+        return desired_joint_velocities_;
+    }
+
+    Eigen::VectorXd ConstantPositionController::get_desired_joint_torques() const
+    {
+        return desired_joint_torques_;
+    }
+
+    Eigen::VectorXd ConstantPositionController::get_desired_joint_stiffnesses() const
+    {
+        return desired_joint_stiffnesses_;
+    }
+
+    Eigen::VectorXd ConstantPositionController::get_desired_joint_damping() const
+    {
+        return desired_joint_damping_;
+    }
+}

external-control/src/main/cpp/controllers/constant-position-controller.hpp

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "types.hpp"
+
+namespace ihmc
+{
+    class ConstantPositionController
+    {
+    public:
+
+        ConstantPositionController() = default;
+
+        explicit ConstantPositionController(const double default_stiffness, const double default_damping, const int number_of_joints);
+
+        virtual ~ConstantPositionController() = default;
+
+        void resize(const double default_stiffness, const double default_damping, const int number_of_joints);
+
+        bool setHomeJointConfiguration(const double* configuration_data, int rows);
+
+        Eigen::VectorXd get_desired_joint_positions() const;
+
+        Eigen::VectorXd get_desired_joint_velocities() const;
+
+        Eigen::VectorXd get_desired_joint_torques() const;
+
+        Eigen::VectorXd get_desired_joint_stiffnesses() const;
+
+        Eigen::VectorXd get_desired_joint_damping() const;
+
+    private:
+        int number_of_joints_;
+
+        Eigen::VectorXd home_configuration_;
+        Eigen::VectorXd desired_joint_velocities_;
+        Eigen::VectorXd desired_joint_torques_;
+        Eigen::VectorXd desired_joint_stiffnesses_;
+        Eigen::VectorXd desired_joint_damping_;
+    };
+}