From 641050563b877ae8747d1e8366d538e01287729b Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Mon, 9 Dec 2024 15:42:19 -0600 Subject: [PATCH 01/19] started setting up the external controller --- external-control/CMakeLists.txt | 46 + external-control/build.gradle.kts | 16 + external-control/createDockerImage.sh | 5 + external-control/generateJavaBindingsCI.sh | 11 + external-control/gradle.properties | 6 + external-control/scripts/buildWrapper.sh | 24 + external-control/scripts/generateBindings.sh | 56 ++ external-control/settings.gradle.kts | 23 + .../src/main/cpp/external-control.cpp | 823 ++++++++++++++++++ .../src/main/cpp/external-control.hpp | 56 ++ 10 files changed, 1066 insertions(+) create mode 100644 external-control/CMakeLists.txt create mode 100644 external-control/build.gradle.kts create mode 100755 external-control/createDockerImage.sh create mode 100755 external-control/generateJavaBindingsCI.sh create mode 100644 external-control/gradle.properties create mode 100755 external-control/scripts/buildWrapper.sh create mode 100755 external-control/scripts/generateBindings.sh create mode 100644 external-control/settings.gradle.kts create mode 100644 external-control/src/main/cpp/external-control.cpp create mode 100644 external-control/src/main/cpp/external-control.hpp diff --git a/external-control/CMakeLists.txt b/external-control/CMakeLists.txt new file mode 100644 index 00000000000..6f50ac19445 --- /dev/null +++ b/external-control/CMakeLists.txt @@ -0,0 +1,46 @@ +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 CONTACT_SOURCES + "src/main/cpp/contact/*.hpp" + "src/main/cpp/contact/*.cpp") +file(GLOB COST_SOURCES + "src/main/cpp/cost/*.hpp" + "src/main/cpp/cost/*.cpp") +file(GLOB PROBLEM_SOURCES + "src/main/cpp/problem/*.hpp" + "src/main/cpp/problem/*.cpp") +set(SOURCES + ${CONTACT_SOURCES} + ${PROBLEM_SOURCES} + ${COST_SOURCES} + src/main/cpp/frame.hpp + src/main/cpp/stage.hpp + src/main/cpp/stage.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") \ No newline at end of file diff --git a/external-control/build.gradle.kts b/external-control/build.gradle.kts new file mode 100644 index 00000000000..fef55ab8c4d --- /dev/null +++ b/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") +} diff --git a/external-control/createDockerImage.sh b/external-control/createDockerImage.sh new file mode 100755 index 00000000000..ff44d5d8334 --- /dev/null +++ b/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 . \ No newline at end of file diff --git a/external-control/generateJavaBindingsCI.sh b/external-control/generateJavaBindingsCI.sh new file mode 100755 index 00000000000..7bd4bfa727a --- /dev/null +++ b/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 + """ \ No newline at end of file diff --git a/external-control/gradle.properties b/external-control/gradle.properties new file mode 100644 index 00000000000..a345a448a9b --- /dev/null +++ b/external-control/gradle.properties @@ -0,0 +1,6 @@ +kebabCasedName = ihmc-external-control +pascalCasedName = IHMCExternalControl +extraSourceSets = ["test"] +publishUrl = local +compositeSearchHeight = 2 +excludeFromCompositeBuild = false \ No newline at end of file diff --git a/external-control/scripts/buildWrapper.sh b/external-control/scripts/buildWrapper.sh new file mode 100755 index 00000000000..5fe4e1d795f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/external-control/scripts/generateBindings.sh b/external-control/scripts/generateBindings.sh new file mode 100755 index 00000000000..52d44d988e9 --- /dev/null +++ b/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 crocoddyl-wrapper into the build directory +cp -r ../external-control/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/ExternalControl.java -d ../../external-control/src/main/resources/externalControl/linux-x86_64 \ +-Dplatform.compiler="clang++" \ +-Dplatform.compiler.default="-O3" + +# Clean old generated code +rm -rf ../../external-control/src/main/generated-java/* + +# Copy newly generated Java into generated-java +mkdir -p ../../external-control/src/main/generated-java/us/ihmc/externalControl +cp -r us/ihmc/externalControl/ExternalControl.java ../../external-control/src/main/generated-java/us/ihmc/externalControl +chmod 664 ../../external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControl.java # Add write permissions to the generated file, so it can be modified + +# Copy over the wrapper and its dependencies -- this will be pinocchio and crocoddyl, as well as their +# dependencies recursively +LINUX_RESOURCES_DIR='../../external-control/src/main/resources/externalControl/linux-x86_64' +cp ../lib/libexternal-control.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 \ No newline at end of file diff --git a/external-control/settings.gradle.kts b/external-control/settings.gradle.kts new file mode 100644 index 00000000000..1bb33db1262 --- /dev/null +++ b/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() diff --git a/external-control/src/main/cpp/external-control.cpp b/external-control/src/main/cpp/external-control.cpp new file mode 100644 index 00000000000..f3714db44fe --- /dev/null +++ b/external-control/src/main/cpp/external-control.cpp @@ -0,0 +1,823 @@ +#include "mpc-problem.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ihmc +{ + MPCProblemImpl::MPCProblemImpl(const MPCProblemParameters& problem_parameters) + : problem_parameters_(problem_parameters) + {} + + bool MPCProblemImpl::setupProblem(const double* q_data, + const int q_rows, + const double* v_data, + const int v_rows) + { + std::vector> running_models; + for (int i = 0; i < problem_parameters_.number_of_running_stages; ++i) + running_models.emplace_back(stages_[i].getIntegratedModel()); + boost::shared_ptr terminal_model = stages_.back().getIntegratedModel(); + problem_ = boost::make_shared(initial_state_, running_models, terminal_model); + solver_ = boost::make_shared(problem_); + + setupCallbacks(); + + setInitialState(q_data, q_rows, v_data, v_rows); + + return true; + } + + bool MPCProblemImpl::setInitialState(const double* configuration_data, + const int configuration_rows, + const double* velocity_data, + const int velocity_rows) + { + if (configuration_rows + velocity_rows != getStateSize()) + { + std::cout << "Argument state size is wrong when setting the initial state." << std::endl; + return false; + } + + const VectorViewReadOnly configuration(configuration_data, configuration_rows); + const VectorViewReadOnly velocity(velocity_data, velocity_rows); + + pinocchio::framesForwardKinematics(model_, data_, configuration); + + initial_state_ << configuration, velocity; + problem_->set_x0(initial_state_); + + return true; + } + + bool MPCProblemImpl::solve() const + { + return solver_->solve(); + } + + bool MPCProblemImpl::solve(const int iteration_limit) const + { + return solver_->solve(solver_->get_xs(), solver_->get_us(), iteration_limit, false, 1e-9); + } + + std::size_t MPCProblemImpl::getIterations() const + { + return solver_->get_iter(); + } + + bool MPCProblemImpl::getInitialState(double* state_data_to_pack, const int rows) const + { + if (rows != getStateSize()) + { + std::cout << "The number of rows in the data arguments don't match the state size " << getStateSize() << + " when getting the initial state." << std::endl; + return false; + } + + if (state_data_to_pack == nullptr) + { + return false; + } + + VectorView state(state_data_to_pack, rows); + state = initial_state_; + + return true; + } + + bool MPCProblemImpl::getSolutionState(const int stage_id, double* state_data_to_pack, const int rows) const + { + if (rows != getStateSize()) + { + std::cout << "The number of rows in the data arguments don't match the state size " << getStateSize() << + " when getting the solution state." << std::endl; + return false; + } + + if (state_data_to_pack == nullptr) + { + return false; + } + + VectorView state(state_data_to_pack, rows); + state = solver_->get_xs()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getSolutionControl(const int stage_id, double* control_data_to_pack, const int rows) const + { + if (rows != getControlSize()) + { + std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << + " when getting the solution control." << std::endl; + return false; + } + + if (control_data_to_pack == nullptr) + { + return false; + } + + VectorView control(control_data_to_pack, rows); + control = solver_->get_us()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getSolutionFeedbackGain(const int stage_id, + double* gain_data_to_pack, + const int rows, + const int cols) const + { + if (rows != getControlSize()) + { + std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << + " when getting the solution feedback gain." << std::endl; + return false; + } + if (cols != getStateDifferentialSize()) + { + std::cout << "The number of cols in the data arguments don't match the state differential size " << + getStateDifferentialSize() << " when getting the solution feedback gain." << std::endl; + return false; + } + + if (gain_data_to_pack == nullptr) + { + return false; + } + + MatrixViewRowMajor gain(gain_data_to_pack, rows, cols); + gain = solver_->get_K()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getQuu(const int stage_id, double* hessian_data_to_pack, const int rows, const int cols) const + { + if (rows != getControlSize()) + { + std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << + " when getting Quu." << std::endl; + return false; + } + if (cols != getControlSize()) + { + std::cout << "The number of cols in the data arguments don't match the control size " << getControlSize() << + " when getting Quu." << std::endl; + return false; + } + + if (hessian_data_to_pack == nullptr) + { + return false; + } + + MatrixViewRowMajor hessian(hessian_data_to_pack, rows, cols); + hessian = solver_->get_Quu()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getQxx(const int stage_id, double* hessian_data_to_pack, const int rows, const int cols) const + { + if (rows != getStateDifferentialSize()) + { + std::cout << "The number of rows in the data arguments don't match the state differential size " << + getStateDifferentialSize() << " when getting Qxx." << std::endl; + return false; + } + if (cols != getStateDifferentialSize()) + { + std::cout << "The number of cols in the data arguments don't match the state differential size " << + getStateDifferentialSize() << " when getting Qxx." << std::endl; + return false; + } + + if (hessian_data_to_pack == nullptr) + { + return false; + } + + MatrixViewRowMajor hessian(hessian_data_to_pack, rows, cols); + hessian = solver_->get_Qxx()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getQxu(const int stage_id, double* hessian_data_to_pack, const int rows, const int cols) const + { + if (rows != getStateDifferentialSize()) + { + std::cout << "The number of rows in the data arguments don't match the state differential size " << + getStateDifferentialSize() << " when getting Qxu." << std::endl; + return false; + } + if (cols != getControlSize()) + { + std::cout << "The number of cols in the data arguments don't match the control size " << getControlSize() << + " when getting Qxu." << std::endl; + return false; + } + + if (hessian_data_to_pack == nullptr) + { + return false; + } + + MatrixViewRowMajor hessian(hessian_data_to_pack, rows, cols); + hessian = solver_->get_Qxu()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getQx(const int stage_id, double* gradient_data_to_pack, const int rows) const + { + if (rows != getStateDifferentialSize()) + { + std::cout << "The number of rows in the data arguments don't match the state differential size " << + getStateDifferentialSize() << " when getting Qx." << std::endl; + return false; + } + + if (gradient_data_to_pack == nullptr) + { + return false; + } + + VectorView gradient(gradient_data_to_pack, rows); + gradient = solver_->get_Qx()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getQu(const int stage_id, double* gradient_data_to_pack, const int rows) const + { + if (rows != getControlSize()) + { + std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << + " when getting Qu." << std::endl; + return false; + } + + if (gradient_data_to_pack == nullptr) + { + return false; + } + + VectorView gradient(gradient_data_to_pack, rows); + gradient = solver_->get_Qu()[stage_id]; + + return true; + } + + bool MPCProblemImpl::getSolutionFrameTranslation(const int stage_id, + const std::string& frame_name, + double* translation_data_to_pack, + const int rows) const + { + if (rows != 3) + { + std::cout << + "The number of rows in the data arguments should be 3 when getting the solution frame translation." + << std::endl; + return false; + } + + if (translation_data_to_pack == nullptr) + { + return false; + } + const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); + + VectorView translation(translation_data_to_pack, rows); + + const auto& action_data_euler = boost::static_pointer_cast( + problem_->get_runningDatas()[stage_id]); + const auto& forward_dynamics = boost::static_pointer_cast( + action_data_euler->differential); + + translation = forward_dynamics->pinocchio.oMf[frame_id].translation(); + + return true; + } + + bool MPCProblemImpl::getSolutionFrameRotation(const int stage_id, + const std::string& frame_name, + double* rotation_data_to_pack, + const int rows, + const int cols) const + { + if (rows != 3 || cols != 3) + { + std::cout << "The size of the matrix should be 3x3 when getting the solution frame rotation" << std::endl; + return false; + } + + if (rotation_data_to_pack == nullptr) + { + return false; + } + const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); + MatrixViewRowMajor rotation(rotation_data_to_pack, rows, cols); + + const auto& action_data_euler = boost::static_pointer_cast( + problem_->get_runningDatas()[stage_id]); + const auto& forward_dynamics = boost::static_pointer_cast( + action_data_euler->differential); + + rotation = forward_dynamics->pinocchio.oMf[frame_id].rotation(); + + return true; + } + + bool MPCProblemImpl::getSolutionFramePose(const int stage_id, + const std::string& frame_name, + double* translation_data_to_pack, + const int translation_size, + double* rotation_data_to_pack, + const int rows, + const int cols) const + { + if (translation_size != 3) + { + std::cout << "The size of the translation matrix should be 3 when getting the solution frame pose" << + std::endl; + return false; + } + if (rows != 3 || cols != 3) + { + std::cout << "The size of the matrix should be 3x3 when getting the solution frame pose" << std::endl; + return false; + } + + if (rotation_data_to_pack == nullptr || translation_data_to_pack == nullptr) + { + return false; + } + const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); + + VectorView translation(translation_data_to_pack, translation_size); + MatrixViewRowMajor rotation(rotation_data_to_pack, rows, cols); + + const auto& action_data_euler = boost::static_pointer_cast( + problem_->get_runningDatas()[stage_id]); + const auto& forward_dynamics = boost::static_pointer_cast( + action_data_euler->differential); + const auto& transform = forward_dynamics->pinocchio.oMf[frame_id]; + + translation = transform.translation(); + rotation = transform.rotation(); + + return true; + } + + bool MPCProblemImpl::getSolutionFrameTwist(const int stage_id, + const std::string& frame_name, + double* twist_data_to_pack, + const int rows) const + { + if (rows != 6) + { + std::cout << "The number of rows in the data arguments should be 6 when getting the solution frame twist." + << std::endl; + return false; + } + + if (twist_data_to_pack == nullptr) + { + return false; + } + const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); + VectorView twist(twist_data_to_pack, rows); + + const auto& action_data_euler = boost::static_pointer_cast( + problem_->get_runningDatas()[stage_id]); + const auto& forward_dynamics = boost::static_pointer_cast( + action_data_euler->differential); + const auto& data = forward_dynamics->pinocchio; + twist = pinocchio::getFrameVelocity(model_, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED).toVector(); + + return true; + } + + bool MPCProblemImpl::getSolutionContactWrench(const int stage_id, + const std::string& frame_name, + double* wrench_data_to_pack, + const int rows) const + { + if (rows != 6) + { + std::cout << + "The number of rows in the data arguments should be 6 when getting the solution contact wrench." << + std::endl; + return false; + } + + if (wrench_data_to_pack == nullptr) + { + return false; + } + VectorView wrench(wrench_data_to_pack, rows); + + const auto& action_data_euler = boost::static_pointer_cast( + problem_->get_runningDatas()[stage_id]); + const auto& forward_dynamics = boost::static_pointer_cast( + action_data_euler->differential); + wrench = forward_dynamics->multibody.contacts->contacts.at(frame_name)->f.toVector(); + + return true; + } + + bool MPCProblemImpl::getSolutionCoMPosition(const int stage_id, double* com_data_to_pack, const int rows) const + { + if (rows != 3) + { + std::cout << "The number of rows in the data arguments should be 3 when getting the solution com position." + << std::endl; + return false; + } + + if (com_data_to_pack == nullptr) + { + return false; + } + + VectorView com_position(com_data_to_pack, rows); + + const auto& action_data_euler = boost::static_pointer_cast( + problem_->get_runningDatas()[stage_id]); + const auto& forward_dynamics = boost::static_pointer_cast( + action_data_euler->differential); + const auto& data = forward_dynamics->pinocchio; + + com_position = data.com[0]; // data.com is a vector of subtree CoMs, the first entry is for the whole model + + return true; + } + + bool MPCProblemImpl::getCurrentFramePose(const std::string& frame_name, + double* translation_data_to_pack, + const int translation_size, + double* rotation_data_to_pack, + const int rows, + const int cols) const + { + const auto& frame_id = model_.getFrameId(frame_name); + + VectorView translation(translation_data_to_pack, translation_size); + MatrixViewRowMajor rotation(rotation_data_to_pack, rows, cols); + + translation = data_.oMf[frame_id].translation(); + rotation = data_.oMf[frame_id].rotation(); + + return true; + } + + const Stage& MPCProblemImpl::getStage(const int stage_id) const + { + return stages_[stage_id]; + } + + double MPCProblemImpl::getCostContribution(const int stage_id, const std::string& cost_name) const + { + const bool is_terminal = stage_id == static_cast(stages_.size()) - 1; + const auto model = boost::static_pointer_cast( + is_terminal ? problem_->get_terminalModel() : problem_->get_runningModels()[stage_id]); + const auto data = boost::static_pointer_cast( + is_terminal ? problem_->get_terminalData() : problem_->get_runningDatas()[stage_id]); + + if (problem_parameters_.contact_frames.empty()) + return getCostContributionForActionType(cost_name, + is_terminal, + model, + data, + problem_parameters_); + else + return getCostContributionForActionType(cost_name, + is_terminal, + model, + data, + problem_parameters_); + } + + double MPCProblemImpl::getCostContribution(const std::string& cost_name) const + { + double cost = 0.0; + for (size_t i = 0; i < stages_.size(); ++i) + cost += getCostContribution(i, cost_name); + + return cost; + } + + double MPCProblemImpl::getCost() const + { + return solver_->get_cost(); + } + + double MPCProblemImpl::getMerit() const + { + return solver_->get_merit(); + } + + double MPCProblemImpl::getStoppingCriteria() const + { + return solver_->get_stop(); + } + + double MPCProblemImpl::getPrimalRegularization() const + { + return solver_->get_preg(); + } + + double MPCProblemImpl::getDualRegularization() const + { + return solver_->get_dreg(); + } + + double MPCProblemImpl::getStepSize() const + { + return solver_->get_steplength(); + } + + double MPCProblemImpl::getDynamicFeasibility() const + { + return solver_->get_ffeas(); + } + + double MPCProblemImpl::getInequalityFeasibility() const + { + return solver_->get_gfeas(); + } + + double MPCProblemImpl::getEqualityFeasibility() const + { + return solver_->get_hfeas(); + } + + double MPCProblemImpl::getCostReduction() const + { + return solver_->get_dV(); + } + + double MPCProblemImpl::getExpectedCostReduction() const + { + return solver_->get_dVexp(); + } + + double MPCProblemImpl::getMeritReduction() const + { + return solver_->get_dPhi(); + } + + double MPCProblemImpl::getExpectedMeritReduction() const + { + return solver_->get_dPhiexp(); + } + + double MPCProblemImpl::getKineticEnergy() + { + return pinocchio::computeKineticEnergy(model_, data_); + } + + double MPCProblemImpl::getPotentialEnergy() + { + return pinocchio::computePotentialEnergy(model_, data_); + } + + double MPCProblemImpl::getMechanicalEnergy() + { + return getKineticEnergy() + getPotentialEnergy(); + } + + int MPCProblemImpl::getConfigurationSize() const + { + return model_.nq; + } + + int MPCProblemImpl::getVelocitySize() const + { + return model_.nv; + } + + int MPCProblemImpl::getAccelerationSize() const + { + return getVelocitySize(); + } + + int MPCProblemImpl::getStateSize() const + { + return state_->get_nx(); + } + + int MPCProblemImpl::getStateDifferentialSize() const + { + return state_->get_ndx(); + } + + int MPCProblemImpl::getControlSize() const + { + return actuation_->get_nu(); + } + + void MPCProblemImpl::printProblemInfo() const + { + std::cout << *problem_ << std::endl; + } + + void MPCProblemImpl::printStageInfo(const int stage_id) const + { + if (stage_id < static_cast(stages_.size()) - 1) + std::cout << *problem_->get_runningModels()[stage_id] << std::endl; + else + std::cout << *problem_->get_terminalModel() << std::endl; + } + + void MPCProblemImpl::printContactInfo(const int stage_id) const + { + if (problem_parameters_.contact_frames.empty()) + { + std::cout << "No contacts -- not printing any contact information." << std::endl; + return; + } + + if (stage_id < static_cast(stages_.size()) - 1) + { + const auto& integrated_model = boost::static_pointer_cast( + problem_->get_runningModels()[stage_id]); + const auto& differential_model = boost::static_pointer_cast< + crocoddyl::DifferentialActionModelContactFwdDynamics>(integrated_model->get_differential()); + const auto& contact_model_multiple = differential_model->get_contacts(); + std::cout << *contact_model_multiple << std::endl; + } + else + { + const auto& integrated_model = boost::static_pointer_cast( + problem_->get_terminalModel()); + const auto& differential_model = boost::static_pointer_cast< + crocoddyl::DifferentialActionModelContactFwdDynamics>(integrated_model->get_differential()); + const auto& contact_model_multiple = differential_model->get_contacts(); + std::cout << *contact_model_multiple << std::endl; + } + } + + void MPCProblemImpl::printPhaseDiagram() const + { + std::cout << std::endl; + for (const auto& contact : contacts_) + { + for (size_t j = 0; j < stages_.size(); ++j) + { + if (constexpr int width = 10; j % width == 0 && j != 0) + std::cout << " "; + + std::cout << (stages_[j].getContactInformation().getContactStatus(contact) ? 'x' : 'o'); + } + std::cout << std::endl; + } + } + + void MPCProblemImpl::buildFromModelAndData(const pinocchio::Model& model) + { + setupThreading(); + + setupModelAndDataFromModel(model); + setupProblemVariables(); + } + + void MPCProblemImpl::buildFromURDF() + { + setupThreading(); + + setupModelAndDataFromURDF(); + setupProblemVariables(); + } + + void MPCProblemImpl::setupModelAndDataFromModel(const pinocchio::Model& model) + { + model_ = model; + setupAdditionalFrames(); + data_ = pinocchio::Data(model_); + } + + MPCProblemImpl::MPCProblemImpl(const pinocchio::Model& model, const MPCProblemParameters& problem_parameters) + : MPCProblemImpl(problem_parameters) + { + buildFromModelAndData(model); + } + + MPCProblemImpl::MPCProblemImpl(const std::string& urdf_path, const MPCProblemParameters& problem_parameters) + : MPCProblemImpl(problem_parameters) + { + urdf_path_ = urdf_path; + buildFromURDF(); + } + + void MPCProblemImpl::setupThreading() const + { + if (const std::set threads = problem_parameters_.thread_ids; !threads.empty()) + { + std::stringstream concatenated; + for (auto it = threads.begin(); it != threads.end(); ++it) + { + concatenated << *it; + if (std::next(it) != threads.end()) + concatenated << ","; // Add comma only if it's not the last element + } + const std::string info = "{" + concatenated.str() + "}"; + setenv("OMP_PLACES", info.c_str(), 1); // Indices of the threads that can be used + setenv("OMP_PROC_BIND", "TRUE", 1); // Rule that binds to just these threads + } + if (problem_parameters_.verbose) + setenv("OMP_DISPLAY_ENV", "TRUE", 1); + } + + void MPCProblemImpl::setupAdditionalFrames() + { + for (const auto& extra_frame : problem_parameters_.extra_frames) + { + for (const auto& frame : model_.frames) + { + if (frame.name == extra_frame.getName()) + throw std::runtime_error("Frame " + extra_frame.getName() + " already exists in the model"); + } + + model_.addFrame(pinocchio::Frame(extra_frame.getName(), + model_.getJointId(extra_frame.getParentJointName()), + 0, + pinocchio::SE3(extra_frame.getRotationFromParent(), + extra_frame.getTranslationFromParent()), + pinocchio::FrameType::OP_FRAME), + false); + std::cout << "Added frame " << extra_frame.getName() << " to model" << std::endl; + std::cout << "Translation from " << extra_frame.getParentJointName() << ":\n" << extra_frame. + getTranslationFromParent() << std::endl; + std::cout << "Rotation from " << extra_frame.getParentJointName() << ":\n" << extra_frame. + getRotationFromParent() << std::endl; + } + } + + void MPCProblemImpl::setupModelAndDataFromURDF() + { + if (problem_parameters_.floating_base) + pinocchio::urdf::buildModel(urdf_path_, + pinocchio::JointModelFreeFlyer(), + model_, + problem_parameters_.verbose); + else + pinocchio::urdf::buildModel(urdf_path_, model_, problem_parameters_.verbose); + + setupAdditionalFrames(); + data_ = pinocchio::Data(model_); + } + + void MPCProblemImpl::setupProblemVariables() + { + state_ = boost::make_shared(boost::make_shared(model_)); + initial_state_ = Eigen::VectorXd::Zero(state_->get_nx()); + + if (problem_parameters_.floating_base) + actuation_ = boost::make_shared(state_); + else + actuation_ = boost::make_shared(state_); + + for (const auto& frame : problem_parameters_.contact_frames) + contacts_.emplace_back(frame, state_, actuation_, problem_parameters_.contact_stabilization_gains); + + for (int i = 0; i < problem_parameters_.number_of_running_stages + 1; ++i) // + 1 for the terminal stage + { + ContactInformation stagewise_contact_information(state_, actuation_); + for (const auto& contact : contacts_) + stagewise_contact_information.addContact(contact); + + if (stagewise_contact_information.empty()) + stages_.emplace_back(i, state_, actuation_, problem_parameters_); + else + stages_.emplace_back(i, state_, actuation_, stagewise_contact_information, problem_parameters_); + } + } + + /** + * @details By default callbacks are only created if the solver is in verbose mode + */ + void MPCProblemImpl::setupCallbacks() const + { + if (problem_parameters_.verbose) + { + std::cout << "calling set up callbacks with verbosity " << problem_parameters_.verbose << std::endl; + std::vector> callbacks; + callbacks.emplace_back(boost::make_shared()); + solver_->setCallbacks(callbacks); + } + } +} diff --git a/external-control/src/main/cpp/external-control.hpp b/external-control/src/main/cpp/external-control.hpp new file mode 100644 index 00000000000..c63c46adb87 --- /dev/null +++ b/external-control/src/main/cpp/external-control.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace ihmc +{ + class ExternalControlImpl + { + public: + explicit ExternalControlImpl(); + + virtual ~ExternalControlImpl() = default; + + ExternalControlImpl(const ExternalControlImpl&) = delete; + + ExternalControlImpl& operator=(const ExternalControlImpl&) = delete; + + ExternalControlImpl(ExternalControlImpl&&) = delete; + + ExternalControlImpl& operator=(ExternalControlImpl&&) = delete; + + bool updateRobotState(const double current_time, + const double* x_data, int x_rows, + const double* u_data, int u_rows, + const bool left_in_contact, const bool right_in_contact, + const double* foot_locations, int foot_locations_rows, + const int hardware_status); + + bool getSolution(double* state_data_to_pack, int state_rows, + double* control_data_to_pack, int control_rows, + double* p_gains_to_pack, int p_gain_rows, + double* d_gains_to_pack, int d_gain_rows) const; + + private: + std::string urdf_path_; + + pinocchio::Data data_; + + boost::shared_ptr state_; + boost::shared_ptr actuation_; + + Eigen::VectorXd initial_state_; + + boost::shared_ptr problem_; + boost::shared_ptr solver_; + + + + }; +} From a384c9e90b3eddeb29ad32d7e124fc7b3cd692a9 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Mon, 9 Dec 2024 16:27:21 -0600 Subject: [PATCH 02/19] added a docker file --- external-control/CMakeLists.txt | 22 +- external-control/Dockerfile | 71 ++ .../src/main/cpp/external-control.cpp | 819 +----------------- .../src/main/cpp/external-control.hpp | 9 - 4 files changed, 87 insertions(+), 834 deletions(-) create mode 100644 external-control/Dockerfile diff --git a/external-control/CMakeLists.txt b/external-control/CMakeLists.txt index 6f50ac19445..9986aadfc43 100644 --- a/external-control/CMakeLists.txt +++ b/external-control/CMakeLists.txt @@ -10,24 +10,12 @@ find_package(Catch2 2 REQUIRED) find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) - -file(GLOB CONTACT_SOURCES - "src/main/cpp/contact/*.hpp" - "src/main/cpp/contact/*.cpp") -file(GLOB COST_SOURCES - "src/main/cpp/cost/*.hpp" - "src/main/cpp/cost/*.cpp") -file(GLOB PROBLEM_SOURCES - "src/main/cpp/problem/*.hpp" - "src/main/cpp/problem/*.cpp") +file(GLOB CONTROL_SOURCES + "src/main/cpp/*.hpp" + "src/main/cpp/*.cpp") set(SOURCES - ${CONTACT_SOURCES} - ${PROBLEM_SOURCES} - ${COST_SOURCES} - src/main/cpp/frame.hpp - src/main/cpp/stage.hpp - src/main/cpp/stage.cpp - src/main/cpp/types.hpp) + ${CONTROL_SOURCES} + ) add_library(external-wrapper SHARED ${SOURCES}) target_include_directories(external-wrapper PUBLIC diff --git a/external-control/Dockerfile b/external-control/Dockerfile new file mode 100644 index 00000000000..c755f0516bc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/external-control/src/main/cpp/external-control.cpp b/external-control/src/main/cpp/external-control.cpp index f3714db44fe..729fc3c6ea1 100644 --- a/external-control/src/main/cpp/external-control.cpp +++ b/external-control/src/main/cpp/external-control.cpp @@ -1,823 +1,26 @@ -#include "mpc-problem.hpp" +#include "external-control.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include namespace ihmc { - MPCProblemImpl::MPCProblemImpl(const MPCProblemParameters& problem_parameters) - : problem_parameters_(problem_parameters) - {} - bool MPCProblemImpl::setupProblem(const double* q_data, - const int q_rows, - const double* v_data, - const int v_rows) + bool ExternalControlImpl::updateRobotState(const double current_time, + const double* x_data, int x_rows, + const double* u_data, int u_rows, + const bool left_in_contact, const bool right_in_contact, + const double* foot_locations, int foot_locations_rows, + const int hardware_status) { - std::vector> running_models; - for (int i = 0; i < problem_parameters_.number_of_running_stages; ++i) - running_models.emplace_back(stages_[i].getIntegratedModel()); - boost::shared_ptr terminal_model = stages_.back().getIntegratedModel(); - problem_ = boost::make_shared(initial_state_, running_models, terminal_model); - solver_ = boost::make_shared(problem_); - - setupCallbacks(); - - setInitialState(q_data, q_rows, v_data, v_rows); - - return true; - } - - bool MPCProblemImpl::setInitialState(const double* configuration_data, - const int configuration_rows, - const double* velocity_data, - const int velocity_rows) - { - if (configuration_rows + velocity_rows != getStateSize()) - { - std::cout << "Argument state size is wrong when setting the initial state." << std::endl; - return false; - } - - const VectorViewReadOnly configuration(configuration_data, configuration_rows); - const VectorViewReadOnly velocity(velocity_data, velocity_rows); - - pinocchio::framesForwardKinematics(model_, data_, configuration); - - initial_state_ << configuration, velocity; - problem_->set_x0(initial_state_); - - return true; - } - - bool MPCProblemImpl::solve() const - { - return solver_->solve(); - } - - bool MPCProblemImpl::solve(const int iteration_limit) const - { - return solver_->solve(solver_->get_xs(), solver_->get_us(), iteration_limit, false, 1e-9); - } - - std::size_t MPCProblemImpl::getIterations() const - { - return solver_->get_iter(); - } - - bool MPCProblemImpl::getInitialState(double* state_data_to_pack, const int rows) const - { - if (rows != getStateSize()) - { - std::cout << "The number of rows in the data arguments don't match the state size " << getStateSize() << - " when getting the initial state." << std::endl; - return false; - } - - if (state_data_to_pack == nullptr) - { - return false; - } - - VectorView state(state_data_to_pack, rows); - state = initial_state_; - - return true; - } - - bool MPCProblemImpl::getSolutionState(const int stage_id, double* state_data_to_pack, const int rows) const - { - if (rows != getStateSize()) - { - std::cout << "The number of rows in the data arguments don't match the state size " << getStateSize() << - " when getting the solution state." << std::endl; - return false; - } - - if (state_data_to_pack == nullptr) - { - return false; - } - - VectorView state(state_data_to_pack, rows); - state = solver_->get_xs()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getSolutionControl(const int stage_id, double* control_data_to_pack, const int rows) const - { - if (rows != getControlSize()) - { - std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << - " when getting the solution control." << std::endl; - return false; - } - - if (control_data_to_pack == nullptr) - { - return false; - } - - VectorView control(control_data_to_pack, rows); - control = solver_->get_us()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getSolutionFeedbackGain(const int stage_id, - double* gain_data_to_pack, - const int rows, - const int cols) const - { - if (rows != getControlSize()) - { - std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << - " when getting the solution feedback gain." << std::endl; - return false; - } - if (cols != getStateDifferentialSize()) - { - std::cout << "The number of cols in the data arguments don't match the state differential size " << - getStateDifferentialSize() << " when getting the solution feedback gain." << std::endl; - return false; - } - - if (gain_data_to_pack == nullptr) - { - return false; - } - - MatrixViewRowMajor gain(gain_data_to_pack, rows, cols); - gain = solver_->get_K()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getQuu(const int stage_id, double* hessian_data_to_pack, const int rows, const int cols) const - { - if (rows != getControlSize()) - { - std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << - " when getting Quu." << std::endl; - return false; - } - if (cols != getControlSize()) - { - std::cout << "The number of cols in the data arguments don't match the control size " << getControlSize() << - " when getting Quu." << std::endl; - return false; - } - - if (hessian_data_to_pack == nullptr) - { - return false; - } - - MatrixViewRowMajor hessian(hessian_data_to_pack, rows, cols); - hessian = solver_->get_Quu()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getQxx(const int stage_id, double* hessian_data_to_pack, const int rows, const int cols) const - { - if (rows != getStateDifferentialSize()) - { - std::cout << "The number of rows in the data arguments don't match the state differential size " << - getStateDifferentialSize() << " when getting Qxx." << std::endl; - return false; - } - if (cols != getStateDifferentialSize()) - { - std::cout << "The number of cols in the data arguments don't match the state differential size " << - getStateDifferentialSize() << " when getting Qxx." << std::endl; - return false; - } - - if (hessian_data_to_pack == nullptr) - { - return false; - } - - MatrixViewRowMajor hessian(hessian_data_to_pack, rows, cols); - hessian = solver_->get_Qxx()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getQxu(const int stage_id, double* hessian_data_to_pack, const int rows, const int cols) const - { - if (rows != getStateDifferentialSize()) - { - std::cout << "The number of rows in the data arguments don't match the state differential size " << - getStateDifferentialSize() << " when getting Qxu." << std::endl; - return false; - } - if (cols != getControlSize()) - { - std::cout << "The number of cols in the data arguments don't match the control size " << getControlSize() << - " when getting Qxu." << std::endl; - return false; - } - - if (hessian_data_to_pack == nullptr) - { - return false; - } - - MatrixViewRowMajor hessian(hessian_data_to_pack, rows, cols); - hessian = solver_->get_Qxu()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getQx(const int stage_id, double* gradient_data_to_pack, const int rows) const - { - if (rows != getStateDifferentialSize()) - { - std::cout << "The number of rows in the data arguments don't match the state differential size " << - getStateDifferentialSize() << " when getting Qx." << std::endl; - return false; - } - - if (gradient_data_to_pack == nullptr) - { - return false; - } - - VectorView gradient(gradient_data_to_pack, rows); - gradient = solver_->get_Qx()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getQu(const int stage_id, double* gradient_data_to_pack, const int rows) const - { - if (rows != getControlSize()) - { - std::cout << "The number of rows in the data arguments don't match the control size " << getControlSize() << - " when getting Qu." << std::endl; - return false; - } - - if (gradient_data_to_pack == nullptr) - { - return false; - } - - VectorView gradient(gradient_data_to_pack, rows); - gradient = solver_->get_Qu()[stage_id]; - - return true; - } - - bool MPCProblemImpl::getSolutionFrameTranslation(const int stage_id, - const std::string& frame_name, - double* translation_data_to_pack, - const int rows) const - { - if (rows != 3) - { - std::cout << - "The number of rows in the data arguments should be 3 when getting the solution frame translation." - << std::endl; - return false; - } - - if (translation_data_to_pack == nullptr) - { - return false; - } - const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); - - VectorView translation(translation_data_to_pack, rows); - - const auto& action_data_euler = boost::static_pointer_cast( - problem_->get_runningDatas()[stage_id]); - const auto& forward_dynamics = boost::static_pointer_cast( - action_data_euler->differential); - - translation = forward_dynamics->pinocchio.oMf[frame_id].translation(); - - return true; - } - - bool MPCProblemImpl::getSolutionFrameRotation(const int stage_id, - const std::string& frame_name, - double* rotation_data_to_pack, - const int rows, - const int cols) const - { - if (rows != 3 || cols != 3) - { - std::cout << "The size of the matrix should be 3x3 when getting the solution frame rotation" << std::endl; - return false; - } - - if (rotation_data_to_pack == nullptr) - { - return false; - } - const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); - MatrixViewRowMajor rotation(rotation_data_to_pack, rows, cols); - - const auto& action_data_euler = boost::static_pointer_cast( - problem_->get_runningDatas()[stage_id]); - const auto& forward_dynamics = boost::static_pointer_cast( - action_data_euler->differential); - - rotation = forward_dynamics->pinocchio.oMf[frame_id].rotation(); - - return true; - } - - bool MPCProblemImpl::getSolutionFramePose(const int stage_id, - const std::string& frame_name, - double* translation_data_to_pack, - const int translation_size, - double* rotation_data_to_pack, - const int rows, - const int cols) const - { - if (translation_size != 3) - { - std::cout << "The size of the translation matrix should be 3 when getting the solution frame pose" << - std::endl; - return false; - } - if (rows != 3 || cols != 3) - { - std::cout << "The size of the matrix should be 3x3 when getting the solution frame pose" << std::endl; - return false; - } - - if (rotation_data_to_pack == nullptr || translation_data_to_pack == nullptr) - { - return false; - } - const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); - - VectorView translation(translation_data_to_pack, translation_size); - MatrixViewRowMajor rotation(rotation_data_to_pack, rows, cols); - - const auto& action_data_euler = boost::static_pointer_cast( - problem_->get_runningDatas()[stage_id]); - const auto& forward_dynamics = boost::static_pointer_cast( - action_data_euler->differential); - const auto& transform = forward_dynamics->pinocchio.oMf[frame_id]; - - translation = transform.translation(); - rotation = transform.rotation(); - - return true; - } - - bool MPCProblemImpl::getSolutionFrameTwist(const int stage_id, - const std::string& frame_name, - double* twist_data_to_pack, - const int rows) const - { - if (rows != 6) - { - std::cout << "The number of rows in the data arguments should be 6 when getting the solution frame twist." - << std::endl; - return false; - } - - if (twist_data_to_pack == nullptr) - { - return false; - } - const auto& frame_id = state_->get_pinocchio()->getFrameId(frame_name); - VectorView twist(twist_data_to_pack, rows); - - const auto& action_data_euler = boost::static_pointer_cast( - problem_->get_runningDatas()[stage_id]); - const auto& forward_dynamics = boost::static_pointer_cast( - action_data_euler->differential); - const auto& data = forward_dynamics->pinocchio; - twist = pinocchio::getFrameVelocity(model_, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED).toVector(); - - return true; - } - - bool MPCProblemImpl::getSolutionContactWrench(const int stage_id, - const std::string& frame_name, - double* wrench_data_to_pack, - const int rows) const - { - if (rows != 6) - { - std::cout << - "The number of rows in the data arguments should be 6 when getting the solution contact wrench." << - std::endl; - return false; - } - - if (wrench_data_to_pack == nullptr) - { - return false; - } - VectorView wrench(wrench_data_to_pack, rows); - - const auto& action_data_euler = boost::static_pointer_cast( - problem_->get_runningDatas()[stage_id]); - const auto& forward_dynamics = boost::static_pointer_cast( - action_data_euler->differential); - wrench = forward_dynamics->multibody.contacts->contacts.at(frame_name)->f.toVector(); - - return true; - } - - bool MPCProblemImpl::getSolutionCoMPosition(const int stage_id, double* com_data_to_pack, const int rows) const - { - if (rows != 3) - { - std::cout << "The number of rows in the data arguments should be 3 when getting the solution com position." - << std::endl; - return false; - } - - if (com_data_to_pack == nullptr) - { - return false; - } - - VectorView com_position(com_data_to_pack, rows); - - const auto& action_data_euler = boost::static_pointer_cast( - problem_->get_runningDatas()[stage_id]); - const auto& forward_dynamics = boost::static_pointer_cast( - action_data_euler->differential); - const auto& data = forward_dynamics->pinocchio; - - com_position = data.com[0]; // data.com is a vector of subtree CoMs, the first entry is for the whole model - return true; } - bool MPCProblemImpl::getCurrentFramePose(const std::string& frame_name, - double* translation_data_to_pack, - const int translation_size, - double* rotation_data_to_pack, - const int rows, - const int cols) const + bool ExternalControlImpl::getSolution(double* state_data_to_pack, int state_rows, + double* control_data_to_pack, int control_rows, + double* p_gains_to_pack, int p_gain_rows, + double* d_gains_to_pack, int d_gain_rows) { - const auto& frame_id = model_.getFrameId(frame_name); - - VectorView translation(translation_data_to_pack, translation_size); - MatrixViewRowMajor rotation(rotation_data_to_pack, rows, cols); - - translation = data_.oMf[frame_id].translation(); - rotation = data_.oMf[frame_id].rotation(); - return true; } - const Stage& MPCProblemImpl::getStage(const int stage_id) const - { - return stages_[stage_id]; - } - - double MPCProblemImpl::getCostContribution(const int stage_id, const std::string& cost_name) const - { - const bool is_terminal = stage_id == static_cast(stages_.size()) - 1; - const auto model = boost::static_pointer_cast( - is_terminal ? problem_->get_terminalModel() : problem_->get_runningModels()[stage_id]); - const auto data = boost::static_pointer_cast( - is_terminal ? problem_->get_terminalData() : problem_->get_runningDatas()[stage_id]); - - if (problem_parameters_.contact_frames.empty()) - return getCostContributionForActionType(cost_name, - is_terminal, - model, - data, - problem_parameters_); - else - return getCostContributionForActionType(cost_name, - is_terminal, - model, - data, - problem_parameters_); - } - - double MPCProblemImpl::getCostContribution(const std::string& cost_name) const - { - double cost = 0.0; - for (size_t i = 0; i < stages_.size(); ++i) - cost += getCostContribution(i, cost_name); - - return cost; - } - - double MPCProblemImpl::getCost() const - { - return solver_->get_cost(); - } - - double MPCProblemImpl::getMerit() const - { - return solver_->get_merit(); - } - - double MPCProblemImpl::getStoppingCriteria() const - { - return solver_->get_stop(); - } - - double MPCProblemImpl::getPrimalRegularization() const - { - return solver_->get_preg(); - } - - double MPCProblemImpl::getDualRegularization() const - { - return solver_->get_dreg(); - } - - double MPCProblemImpl::getStepSize() const - { - return solver_->get_steplength(); - } - - double MPCProblemImpl::getDynamicFeasibility() const - { - return solver_->get_ffeas(); - } - - double MPCProblemImpl::getInequalityFeasibility() const - { - return solver_->get_gfeas(); - } - - double MPCProblemImpl::getEqualityFeasibility() const - { - return solver_->get_hfeas(); - } - - double MPCProblemImpl::getCostReduction() const - { - return solver_->get_dV(); - } - - double MPCProblemImpl::getExpectedCostReduction() const - { - return solver_->get_dVexp(); - } - - double MPCProblemImpl::getMeritReduction() const - { - return solver_->get_dPhi(); - } - double MPCProblemImpl::getExpectedMeritReduction() const - { - return solver_->get_dPhiexp(); - } - - double MPCProblemImpl::getKineticEnergy() - { - return pinocchio::computeKineticEnergy(model_, data_); - } - - double MPCProblemImpl::getPotentialEnergy() - { - return pinocchio::computePotentialEnergy(model_, data_); - } - - double MPCProblemImpl::getMechanicalEnergy() - { - return getKineticEnergy() + getPotentialEnergy(); - } - - int MPCProblemImpl::getConfigurationSize() const - { - return model_.nq; - } - - int MPCProblemImpl::getVelocitySize() const - { - return model_.nv; - } - - int MPCProblemImpl::getAccelerationSize() const - { - return getVelocitySize(); - } - - int MPCProblemImpl::getStateSize() const - { - return state_->get_nx(); - } - - int MPCProblemImpl::getStateDifferentialSize() const - { - return state_->get_ndx(); - } - - int MPCProblemImpl::getControlSize() const - { - return actuation_->get_nu(); - } - - void MPCProblemImpl::printProblemInfo() const - { - std::cout << *problem_ << std::endl; - } - - void MPCProblemImpl::printStageInfo(const int stage_id) const - { - if (stage_id < static_cast(stages_.size()) - 1) - std::cout << *problem_->get_runningModels()[stage_id] << std::endl; - else - std::cout << *problem_->get_terminalModel() << std::endl; - } - - void MPCProblemImpl::printContactInfo(const int stage_id) const - { - if (problem_parameters_.contact_frames.empty()) - { - std::cout << "No contacts -- not printing any contact information." << std::endl; - return; - } - - if (stage_id < static_cast(stages_.size()) - 1) - { - const auto& integrated_model = boost::static_pointer_cast( - problem_->get_runningModels()[stage_id]); - const auto& differential_model = boost::static_pointer_cast< - crocoddyl::DifferentialActionModelContactFwdDynamics>(integrated_model->get_differential()); - const auto& contact_model_multiple = differential_model->get_contacts(); - std::cout << *contact_model_multiple << std::endl; - } - else - { - const auto& integrated_model = boost::static_pointer_cast( - problem_->get_terminalModel()); - const auto& differential_model = boost::static_pointer_cast< - crocoddyl::DifferentialActionModelContactFwdDynamics>(integrated_model->get_differential()); - const auto& contact_model_multiple = differential_model->get_contacts(); - std::cout << *contact_model_multiple << std::endl; - } - } - - void MPCProblemImpl::printPhaseDiagram() const - { - std::cout << std::endl; - for (const auto& contact : contacts_) - { - for (size_t j = 0; j < stages_.size(); ++j) - { - if (constexpr int width = 10; j % width == 0 && j != 0) - std::cout << " "; - - std::cout << (stages_[j].getContactInformation().getContactStatus(contact) ? 'x' : 'o'); - } - std::cout << std::endl; - } - } - - void MPCProblemImpl::buildFromModelAndData(const pinocchio::Model& model) - { - setupThreading(); - - setupModelAndDataFromModel(model); - setupProblemVariables(); - } - - void MPCProblemImpl::buildFromURDF() - { - setupThreading(); - - setupModelAndDataFromURDF(); - setupProblemVariables(); - } - - void MPCProblemImpl::setupModelAndDataFromModel(const pinocchio::Model& model) - { - model_ = model; - setupAdditionalFrames(); - data_ = pinocchio::Data(model_); - } - - MPCProblemImpl::MPCProblemImpl(const pinocchio::Model& model, const MPCProblemParameters& problem_parameters) - : MPCProblemImpl(problem_parameters) - { - buildFromModelAndData(model); - } - - MPCProblemImpl::MPCProblemImpl(const std::string& urdf_path, const MPCProblemParameters& problem_parameters) - : MPCProblemImpl(problem_parameters) - { - urdf_path_ = urdf_path; - buildFromURDF(); - } - - void MPCProblemImpl::setupThreading() const - { - if (const std::set threads = problem_parameters_.thread_ids; !threads.empty()) - { - std::stringstream concatenated; - for (auto it = threads.begin(); it != threads.end(); ++it) - { - concatenated << *it; - if (std::next(it) != threads.end()) - concatenated << ","; // Add comma only if it's not the last element - } - const std::string info = "{" + concatenated.str() + "}"; - setenv("OMP_PLACES", info.c_str(), 1); // Indices of the threads that can be used - setenv("OMP_PROC_BIND", "TRUE", 1); // Rule that binds to just these threads - } - if (problem_parameters_.verbose) - setenv("OMP_DISPLAY_ENV", "TRUE", 1); - } - - void MPCProblemImpl::setupAdditionalFrames() - { - for (const auto& extra_frame : problem_parameters_.extra_frames) - { - for (const auto& frame : model_.frames) - { - if (frame.name == extra_frame.getName()) - throw std::runtime_error("Frame " + extra_frame.getName() + " already exists in the model"); - } - - model_.addFrame(pinocchio::Frame(extra_frame.getName(), - model_.getJointId(extra_frame.getParentJointName()), - 0, - pinocchio::SE3(extra_frame.getRotationFromParent(), - extra_frame.getTranslationFromParent()), - pinocchio::FrameType::OP_FRAME), - false); - std::cout << "Added frame " << extra_frame.getName() << " to model" << std::endl; - std::cout << "Translation from " << extra_frame.getParentJointName() << ":\n" << extra_frame. - getTranslationFromParent() << std::endl; - std::cout << "Rotation from " << extra_frame.getParentJointName() << ":\n" << extra_frame. - getRotationFromParent() << std::endl; - } - } - - void MPCProblemImpl::setupModelAndDataFromURDF() - { - if (problem_parameters_.floating_base) - pinocchio::urdf::buildModel(urdf_path_, - pinocchio::JointModelFreeFlyer(), - model_, - problem_parameters_.verbose); - else - pinocchio::urdf::buildModel(urdf_path_, model_, problem_parameters_.verbose); - - setupAdditionalFrames(); - data_ = pinocchio::Data(model_); - } - - void MPCProblemImpl::setupProblemVariables() - { - state_ = boost::make_shared(boost::make_shared(model_)); - initial_state_ = Eigen::VectorXd::Zero(state_->get_nx()); - - if (problem_parameters_.floating_base) - actuation_ = boost::make_shared(state_); - else - actuation_ = boost::make_shared(state_); - - for (const auto& frame : problem_parameters_.contact_frames) - contacts_.emplace_back(frame, state_, actuation_, problem_parameters_.contact_stabilization_gains); - - for (int i = 0; i < problem_parameters_.number_of_running_stages + 1; ++i) // + 1 for the terminal stage - { - ContactInformation stagewise_contact_information(state_, actuation_); - for (const auto& contact : contacts_) - stagewise_contact_information.addContact(contact); - - if (stagewise_contact_information.empty()) - stages_.emplace_back(i, state_, actuation_, problem_parameters_); - else - stages_.emplace_back(i, state_, actuation_, stagewise_contact_information, problem_parameters_); - } - } - - /** - * @details By default callbacks are only created if the solver is in verbose mode - */ - void MPCProblemImpl::setupCallbacks() const - { - if (problem_parameters_.verbose) - { - std::cout << "calling set up callbacks with verbosity " << problem_parameters_.verbose << std::endl; - std::vector> callbacks; - callbacks.emplace_back(boost::make_shared()); - solver_->setCallbacks(callbacks); - } - } } diff --git a/external-control/src/main/cpp/external-control.hpp b/external-control/src/main/cpp/external-control.hpp index c63c46adb87..45ad0167a59 100644 --- a/external-control/src/main/cpp/external-control.hpp +++ b/external-control/src/main/cpp/external-control.hpp @@ -38,18 +38,9 @@ namespace ihmc double* d_gains_to_pack, int d_gain_rows) const; private: - std::string urdf_path_; - - pinocchio::Data data_; - - boost::shared_ptr state_; - boost::shared_ptr actuation_; Eigen::VectorXd initial_state_; - boost::shared_ptr problem_; - boost::shared_ptr solver_; - }; From 1bc4d03484c2ba91d99277b3bfda61b4e7c0b95e Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 11 Dec 2024 09:30:32 -0600 Subject: [PATCH 03/19] got external control compiling --- external-control/scripts/generateBindings.sh | 38 +++++----- .../src/main/cpp/external-control.cpp | 6 +- .../src/main/cpp/external-control.hpp | 15 ---- .../ExternalControlWrapper.java | 75 +++++++++++++++++++ .../presets/ExternalControlInfoMapper.java | 41 ++++++++++ .../linux-x86_64/libexternal-wrapper.so | 3 + 6 files changed, 139 insertions(+), 39 deletions(-) create mode 100644 external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControlWrapper.java create mode 100644 external-control/src/main/java/us/ihmc/externalControl/presets/ExternalControlInfoMapper.java create mode 100644 external-control/src/main/resources/externalControl/linux-x86_64/libexternal-wrapper.so diff --git a/external-control/scripts/generateBindings.sh b/external-control/scripts/generateBindings.sh index 52d44d988e9..46d88a8df98 100755 --- a/external-control/scripts/generateBindings.sh +++ b/external-control/scripts/generateBindings.sh @@ -3,8 +3,8 @@ set -e -o xtrace cd /home/robotlab/ihmc-external-wrapper/build -# Copy all Java code from the root of crocoddyl-wrapper into the build directory -cp -r ../external-control/src/main/java/ . +# 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 @@ -20,32 +20,32 @@ 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/ExternalControl.java -d ../../external-control/src/main/resources/externalControl/linux-x86_64 \ +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 ../../external-control/src/main/generated-java/* +rm -rf ../../src/main/generated-java/* # Copy newly generated Java into generated-java -mkdir -p ../../external-control/src/main/generated-java/us/ihmc/externalControl -cp -r us/ihmc/externalControl/ExternalControl.java ../../external-control/src/main/generated-java/us/ihmc/externalControl -chmod 664 ../../external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControl.java # Add write permissions to the generated file, so it can be modified +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 be pinocchio and crocoddyl, as well as their +# Copy over the wrapper and its dependencies -- this will beexternal control, as well as their # dependencies recursively -LINUX_RESOURCES_DIR='../../external-control/src/main/resources/externalControl/linux-x86_64' -cp ../lib/libexternal-control.so $LINUX_RESOURCES_DIR +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') +#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 diff --git a/external-control/src/main/cpp/external-control.cpp b/external-control/src/main/cpp/external-control.cpp index 729fc3c6ea1..387c49600dd 100644 --- a/external-control/src/main/cpp/external-control.cpp +++ b/external-control/src/main/cpp/external-control.cpp @@ -1,9 +1,7 @@ #include "external-control.hpp" - namespace ihmc { - bool ExternalControlImpl::updateRobotState(const double current_time, const double* x_data, int x_rows, const double* u_data, int u_rows, @@ -17,10 +15,8 @@ namespace ihmc bool ExternalControlImpl::getSolution(double* state_data_to_pack, int state_rows, double* control_data_to_pack, int control_rows, double* p_gains_to_pack, int p_gain_rows, - double* d_gains_to_pack, int d_gain_rows) + double* d_gains_to_pack, int d_gain_rows) const { return true; } - - } diff --git a/external-control/src/main/cpp/external-control.hpp b/external-control/src/main/cpp/external-control.hpp index 45ad0167a59..e6e3367a2b3 100644 --- a/external-control/src/main/cpp/external-control.hpp +++ b/external-control/src/main/cpp/external-control.hpp @@ -1,13 +1,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include - namespace ihmc { class ExternalControlImpl @@ -36,12 +28,5 @@ namespace ihmc double* control_data_to_pack, int control_rows, double* p_gains_to_pack, int p_gain_rows, double* d_gains_to_pack, int d_gain_rows) const; - - private: - - Eigen::VectorXd initial_state_; - - - }; } diff --git a/external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControlWrapper.java b/external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControlWrapper.java new file mode 100644 index 00000000000..5a345f7f5a9 --- /dev/null +++ b/external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControlWrapper.java @@ -0,0 +1,75 @@ +// Targeted by JavaCPP version 1.5.10: DO NOT EDIT THIS FILE + +package us.ihmc.externalControl; + +import java.nio.*; +import org.bytedeco.javacpp.*; +import org.bytedeco.javacpp.annotation.*; + +public class ExternalControlWrapper extends us.ihmc.externalControl.presets.ExternalControlInfoMapper { + static { Loader.load(); } + +// Parsed from external-control.hpp + +// #pragma once + @Namespace("ihmc") public static class ExternalControlImpl extends Pointer { + static { Loader.load(); } + /** Pointer cast constructor. Invokes {@link Pointer#Pointer(Pointer)}. */ + public ExternalControlImpl(Pointer p) { super(p); } + /** Native array allocator. Access with {@link Pointer#position(long)}. */ + public ExternalControlImpl(long size) { super((Pointer)null); allocateArray(size); } + private native void allocateArray(long size); + @Override public ExternalControlImpl position(long position) { + return (ExternalControlImpl)super.position(position); + } + @Override public ExternalControlImpl getPointer(long i) { + return new ExternalControlImpl((Pointer)this).offsetAddress(i); + } + + public ExternalControlImpl() { super((Pointer)null); allocate(); } + private native void allocate(); + + + + + + + + + + public native @Cast("bool") boolean updateRobotState(double current_time, + @Const DoublePointer x_data, int x_rows, + @Const DoublePointer u_data, int u_rows, + @Cast("const bool") boolean left_in_contact, @Cast("const bool") boolean right_in_contact, + @Const DoublePointer foot_locations, int foot_locations_rows, + int hardware_status); + public native @Cast("bool") boolean updateRobotState(double current_time, + @Const DoubleBuffer x_data, int x_rows, + @Const DoubleBuffer u_data, int u_rows, + @Cast("const bool") boolean left_in_contact, @Cast("const bool") boolean right_in_contact, + @Const DoubleBuffer foot_locations, int foot_locations_rows, + int hardware_status); + public native @Cast("bool") boolean updateRobotState(double current_time, + @Const double[] x_data, int x_rows, + @Const double[] u_data, int u_rows, + @Cast("const bool") boolean left_in_contact, @Cast("const bool") boolean right_in_contact, + @Const double[] foot_locations, int foot_locations_rows, + int hardware_status); + + public native @Cast("bool") boolean getSolution(DoublePointer state_data_to_pack, int state_rows, + DoublePointer control_data_to_pack, int control_rows, + DoublePointer p_gains_to_pack, int p_gain_rows, + DoublePointer d_gains_to_pack, int d_gain_rows); + public native @Cast("bool") boolean getSolution(DoubleBuffer state_data_to_pack, int state_rows, + DoubleBuffer control_data_to_pack, int control_rows, + DoubleBuffer p_gains_to_pack, int p_gain_rows, + DoubleBuffer d_gains_to_pack, int d_gain_rows); + public native @Cast("bool") boolean getSolution(double[] state_data_to_pack, int state_rows, + double[] control_data_to_pack, int control_rows, + double[] p_gains_to_pack, int p_gain_rows, + double[] d_gains_to_pack, int d_gain_rows); + } + + + +} diff --git a/external-control/src/main/java/us/ihmc/externalControl/presets/ExternalControlInfoMapper.java b/external-control/src/main/java/us/ihmc/externalControl/presets/ExternalControlInfoMapper.java new file mode 100644 index 00000000000..259424f33a2 --- /dev/null +++ b/external-control/src/main/java/us/ihmc/externalControl/presets/ExternalControlInfoMapper.java @@ -0,0 +1,41 @@ +package us.ihmc.externalControl.presets; + +import org.bytedeco.javacpp.annotation.Platform; +import org.bytedeco.javacpp.annotation.Properties; +import org.bytedeco.javacpp.tools.Info; +import org.bytedeco.javacpp.tools.InfoMap; +import org.bytedeco.javacpp.tools.InfoMapper; + +// @formatter:off +@Properties(value = + @Platform(value = "linux", + includepath = "../include", + include = {"external-control.hpp"}, + linkpath = "../lib", + link = "external-wrapper", + preload = {"external-wrapper", "jniExternalWrapper"} + ), + target = "us.ihmc.externalControl.ExternalControlWrapper" +) +// @formatter:on + +public class ExternalControlInfoMapper implements InfoMapper +{ + public void map(InfoMap infoMap) + { + + // Types + + // Renaming, example: infoMap.put(new Info("ihmc::Frame").pointerTypes("FrameImpl")); + + // Enums + + // Abstract + infoMap.put(new Info("ihmc::ExternalControlImpl").virtualize()); + + // Skipping some parts of the headers + String skipStartRegex = "^ *// JAVACPP START SKIP *$"; + String skipEndRegex = "^ *// JAVACPP END SKIP *$"; + infoMap.put(new Info("external-control.hpp").linePatterns(skipStartRegex, skipEndRegex).skip()); + } +} diff --git a/external-control/src/main/resources/externalControl/linux-x86_64/libexternal-wrapper.so b/external-control/src/main/resources/externalControl/linux-x86_64/libexternal-wrapper.so new file mode 100644 index 00000000000..0be26b3a6fe --- /dev/null +++ b/external-control/src/main/resources/externalControl/linux-x86_64/libexternal-wrapper.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24de5edffd066252ba7c8c735ad95f33c46a572f8dfb1e7c7afa8ea0442cffa4 +size 37920 From c2195005164e72cea824ea7f234fd249951a1bcf Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 11 Dec 2024 09:56:09 -0600 Subject: [PATCH 04/19] made the external control class and wrote its interfacing iwth the native code --- external-control/gradle.properties | 4 +- .../ihmc/externalControl/ExternalControl.java | 174 ++++++++++++++++++ 2 files changed, 176 insertions(+), 2 deletions(-) create mode 100644 external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControl.java diff --git a/external-control/gradle.properties b/external-control/gradle.properties index a345a448a9b..665c13cea63 100644 --- a/external-control/gradle.properties +++ b/external-control/gradle.properties @@ -1,5 +1,5 @@ -kebabCasedName = ihmc-external-control -pascalCasedName = IHMCExternalControl +kebabCasedName = external-control +pascalCasedName = ExternalControl extraSourceSets = ["test"] publishUrl = local compositeSearchHeight = 2 diff --git a/external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControl.java b/external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControl.java new file mode 100644 index 00000000000..223d9354d29 --- /dev/null +++ b/external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControl.java @@ -0,0 +1,174 @@ +package us.ihmc.externalControl; + +import org.ejml.data.DMatrixRMaj; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointReadOnly; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; + +import java.util.HashMap; + +public class ExternalControl +{ + private final ExternalControlWrapper.ExternalControlImpl externalControlImpl; + + private final DMatrixRMaj robotState; + private final DMatrixRMaj robotControl; + private final DMatrixRMaj feetPositions; + private boolean leftInContact; + private boolean rightInContact; + + private final DMatrixRMaj solutionRobotState; + private final DMatrixRMaj solutionTorqueVector; + private final DMatrixRMaj solutionStiffnessVector; + private final DMatrixRMaj solutionDampingVector; + + private final RigidBodyBasics baseBody; + private final OneDoFJointReadOnly[] joints; + + private final FramePose3D basePose = new FramePose3D(); + + private final FrameVector3D tempVector = new FrameVector3D(); + private final FrameVector3D tempPoint = new FrameVector3D(); + + private final FramePose3D solutionBasePose = new FramePose3D(); + private final HashMap solutionJointData = new HashMap<>(); + + public ExternalControl(RigidBodyBasics baseBody, OneDoFJointReadOnly[] joints) + { + this.baseBody = baseBody; + this.joints = joints; + + robotState = new DMatrixRMaj(2 * joints.length + 13, 1); + robotControl = new DMatrixRMaj(joints.length, 1); + feetPositions = new DMatrixRMaj(6, 1); + + solutionRobotState = new DMatrixRMaj(2 * joints.length + 13, 1); + solutionTorqueVector = new DMatrixRMaj(joints.length, 1); + solutionStiffnessVector = new DMatrixRMaj(joints.length, 1); + solutionDampingVector = new DMatrixRMaj(joints.length, 1); + + for (OneDoFJointReadOnly joint : joints) + solutionJointData.put(joint, new SolutionJointData()); + + externalControlImpl = new ExternalControlWrapper.ExternalControlImpl(); + } + + public void setFootStates(SideDependentList soleFrames, boolean leftInContact, boolean rightInContact) + { + int start = 0; + for (RobotSide robotSide : RobotSide.values) + { + tempPoint.setToZero(soleFrames.get(robotSide)); + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + tempPoint.get(start, feetPositions); + start += 3; + } + + this.leftInContact = leftInContact; + this.rightInContact = rightInContact; + } + + public void writeRobotState(double currentTime, int hardwareStatus) + { + setRobotState(); + setRobotControl(); + + if (!externalControlImpl.updateRobotState(currentTime, + robotState.data, + robotState.getNumRows(), + robotControl.data, + robotControl.getNumRows(), + leftInContact, + rightInContact, + feetPositions.data, + feetPositions.getNumRows(), + hardwareStatus)) + throw new RuntimeException("Failed to successfully write the hardware state across the barrier."); + } + + public void readControlSolution() + { + if (!externalControlImpl.getSolution(solutionRobotState.data, + solutionRobotState.numRows, + solutionTorqueVector.data, + solutionTorqueVector.numRows, + solutionStiffnessVector.data, + solutionStiffnessVector.numRows, + solutionDampingVector.data, + solutionDampingVector.numRows)) + { + throw new RuntimeException("Failed to retrieve solution data."); + } + + solutionBasePose.getPosition().set(solutionRobotState); + solutionBasePose.getOrientation().set(3, 0, solutionRobotState); + + int positionStart = 7; + int velocityStart = 7 + 6 + joints.length; + for (int i = 0; i < joints.length; i++) + { + SolutionJointData data = solutionJointData.get(joints[i]); + data.desiredPosition = solutionRobotState.get(positionStart + i, 0); + data.desiredVelocity = solutionRobotState.get(velocityStart + i, 0); + data.torque = solutionTorqueVector.get(i, 0); + data.stiffness = solutionStiffnessVector.get(i, 0); + data.damping = solutionDampingVector.get(i, 0); + } + } + + private void setRobotState() + { + basePose.setToZero(baseBody.getBodyFixedFrame()); + basePose.changeFrame(ReferenceFrame.getWorldFrame()); + + // set the configuration state for the robot + basePose.getPosition().get(0, robotState); + basePose.getOrientation().get(3, robotState); + + int start = 7; + for (int i = 0; i < joints.length; i++) + { + robotState.set(start + i, joints[i].getQ()); + } + + // set linear velocity in body frame + start += joints.length; + tempVector.setIncludingFrame(baseBody.getBodyFixedFrame().getTwistOfFrame().getLinearPart()); + tempVector.changeFrame(baseBody.getBodyFixedFrame()); + tempVector.get(start, robotState); + + // set angular velocity in body frame + start += 3; + tempVector.setIncludingFrame(baseBody.getBodyFixedFrame().getTwistOfFrame().getAngularPart()); + tempVector.changeFrame(baseBody.getBodyFixedFrame()); + tempVector.get(start, robotState); + + // set joint velocity + start += 3; + for (int i = 0; i < joints.length; i++) + { + robotState.set(start + i, joints[i].getQd()); + } + } + + private void setRobotControl() + { + for (int i = 0; i < joints.length; i++) + { + robotControl.set(i, joints[i].getTau()); + } + } + + public static class SolutionJointData + { + double desiredPosition; + double desiredVelocity; + double stiffness; + double damping; + double torque; + } +} From 3f5d2e498cbc9a6bbf1a813c542b390213b62d04 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 11 Dec 2024 10:00:34 -0600 Subject: [PATCH 05/19] added a readme --- external-control/README.md | 81 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100644 external-control/README.md diff --git a/external-control/README.md b/external-control/README.md new file mode 100644 index 00000000000..e53ec67bf29 --- /dev/null +++ b/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