diff --git a/.bazelproject b/.bazelproject
deleted file mode 100644
index 47ea888..0000000
--- a/.bazelproject
+++ /dev/null
@@ -1,7 +0,0 @@
-# SPDX-License-Identifier: MIT-0
-
-directories:
- .
-
-targets:
- //...:all
diff --git a/.bazelrc b/.bazelrc
deleted file mode 100644
index 1081b8c..0000000
--- a/.bazelrc
+++ /dev/null
@@ -1,35 +0,0 @@
-# SPDX-License-Identifier: MIT-0
-
-# Disable native Python rules in Bazel versions before 3.0.
-build --incompatible_load_python_rules_from_bzl=yes
-
-# Default to an optimized build.
-build --compilation_mode=opt
-
-# Default build options.
-build --force_pic=yes
-build --strip=never
-
-# Default test options.
-build --test_output=errors
-build --test_summary=terse
-
-# Use C++20.
-build --cxxopt=-std=c++20
-build --host_cxxopt=-std=c++20
-
-# https://github.com/bazelbuild/bazel/issues/1164
-build --action_env=CCACHE_DISABLE=1
-
-# Escalate warnings to errors
-build --copt=-Werror
-
-# Enable OpenMP
-build:omp --copt=-DEIGEN_DONT_PARALLELIZE
-build:omp --copt=-fopenmp
-build:omp --linkopt=-fopenmp
-build --config=omp
-
-# Try to import user-specific configuration local to workspace.
-try-import %workspace%/user.bazelrc
-
diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000..0dcd501
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,3 @@
+build/
+img/
+README.md
\ No newline at end of file
diff --git a/.gitignore b/.gitignore
index d504152..ee566ed 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,7 +1,5 @@
-# SPDX-License-Identifier: MIT-0
-
*.swp
.vscode
-/bazel-*
-/user.bazelrc
__pycache__
+build
+*.egg-info
diff --git a/BUILD.bazel b/BUILD.bazel
deleted file mode 100644
index 55d137f..0000000
--- a/BUILD.bazel
+++ /dev/null
@@ -1,23 +0,0 @@
-# SPDX-License-Identifier: MIT-0
-
-# This is an empty BUILD file, to ensure that this project's root directory is
-# a Bazel package.
-
-exports_files([
- ".bazelproject",
- "CPPLINT.cfg"
-])
-
-load("//tools:shared_library.bzl", "idto_cc_shared_library")
-
-idto_cc_shared_library(
- name = "idto_shared_library",
- deps = ["//utils", "//optimizer"],
- visibility = ["//visibility:public"],
-)
-
-idto_cc_shared_library(
- name = "idto_shared_library_examples",
- deps = [":idto_shared_library", "//examples"],
- visibility = ["//visibility:public"],
-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..1feb5f8
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.10.2)
+project(idto)
+
+# N.B. This is a temporary flag. It only really applies to Linux, as Mac
+# does not need X11.
+option(RUN_X11_TESTS "Run tests that require X11" OFF)
+
+include(CTest)
+
+find_package(drake CONFIG REQUIRED)
+
+find_package(PythonInterp ${drake_PYTHON_VERSION} MODULE REQUIRED EXACT)
+
+find_program(PYTHON_EXECUTABLE NAMES python3
+ PATHS "${FIND_PYTHON_EXECUTABLE_PATHS}"
+ NO_DEFAULT_PATH
+)
+
+execute_process(COMMAND ${PYTHON_EXECUTABLE}-config --exec-prefix
+ OUTPUT_VARIABLE PYTHON_EXEC_PREFIX
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+)
+list(APPEND CMAKE_PREFIX_PATH "${PYTHON_EXEC_PREFIX}")
+find_package(PythonLibs ${drake_PYTHON_VERSION} MODULE REQUIRED EXACT)
+
+get_filename_component(PYTHONPATH
+ "${drake_DIR}/../../python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages"
+ REALPATH
+)
+
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_options(-Werror -Wall) # treat warnings as errors
+add_compile_options(-O3)
+# add_compile_definitions(ENABLE_TIMERS) # extra profiling
+
+# Allow us to find resources (e.g. urdfs) in the build directory.
+add_compile_definitions(IDTO_BINARY_DIR="${CMAKE_BINARY_DIR}")
+
+add_subdirectory(third_party)
+add_subdirectory(python_bindings)
+add_subdirectory(utils)
+add_subdirectory(optimizer)
+add_subdirectory(models)
+add_subdirectory(examples)
+
diff --git a/CPPLINT.cfg b/CPPLINT.cfg
deleted file mode 100644
index 27c1bfc..0000000
--- a/CPPLINT.cfg
+++ /dev/null
@@ -1,37 +0,0 @@
-# Copyright 2016 Robot Locomotion Group @ CSAIL. All rights reserved.
-#
-# All components of Drake are licensed under the BSD 3-Clause License.
-# See LICENSE.TXT or https://drake.mit.edu/ for details.
-
-# Stop searching for additional config files.
-set noparent
-
-# Disable a warning about C++ features that were not in the original
-# C++11 specification (and so might not be well-supported). In the
-# case of Drake, our supported minimum platforms are new enough that
-# this warning is irrelevant.
-filter=-build/c++11
-
-# Drake uses `#pragma once`, not the `#ifndef FOO_H` guard.
-# https://drake.mit.edu/styleguide/cppguide.html#The__pragma_once_Guard
-filter=-build/header_guard
-filter=+build/pragma_once
-
-# Disable cpplint's include order. We have our own via //tools:drakelint.
-filter=-build/include_order
-
-# We do not care about the whitespace details of a TODO comment. It is not
-# relevant for easy grepping, and the GSG does not specify any particular
-# whitespace style. (We *do* care what the "TODO(username)" itself looks like
-# because GSG forces a particular style there, but that formatting is covered
-# by the readability/todo rule, which we leave enabled.)
-filter=-whitespace/todo
-
-# TODO(#1805) Fix this.
-filter=-legal/copyright
-
-# Ignore code that isn't ours.
-exclude_files=third_party
-
-# It's not worth lint-gardening the documentation.
-exclude_files=doc
diff --git a/Dockerfile b/Dockerfile
new file mode 100644
index 0000000..f0f10a7
--- /dev/null
+++ b/Dockerfile
@@ -0,0 +1,45 @@
+# syntax=docker/dockerfile:1
+
+FROM ubuntu:jammy
+SHELL ["/bin/bash", "-c", "-i"]
+ENV TZ=America/Los_Angeles
+RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone
+
+WORKDIR /home
+
+# Set up python 3.10 and alias it with 'python'
+RUN apt update -y
+RUN apt install -y \
+ build-essential \
+ vim \
+ cmake \
+ git \
+ python3.10 \
+ python-is-python3 \
+ wget \
+ python3-pip
+
+# Run drake installation
+RUN wget https://github.com/RobotLocomotion/drake/releases/download/v1.30.0/drake-1.30.0-jammy.tar.gz && \
+ tar -xvzf drake-1.30.0-jammy.tar.gz && \
+ rm drake-1.30.0-jammy.tar.gz
+WORKDIR /home/drake
+RUN ./share/drake/setup/install_prereqs -y && \
+ apt-get install -y libgflags-dev ninja-build && \
+ pip install scipy==1.12
+RUN echo 'export CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:/home/drake' >> ~/.bashrc && \
+ echo 'export PYTHONPATH=${PYTHONPATH}:/home/drake/lib/python3.10/site-packages' >> ~/.bashrc && \
+ source ~/.bashrc
+
+# Copy the current directory contents into the container
+COPY . /home/idto
+WORKDIR /home/idto
+
+# Build the C++ binaries
+RUN mkdir build
+WORKDIR /home/idto/build
+RUN cmake .. && make -j
+
+# Build the python bindings
+WORKDIR /home/idto/
+RUN pip install .
diff --git a/LICENSE b/LICENSE
deleted file mode 100644
index 0b4b879..0000000
--- a/LICENSE
+++ /dev/null
@@ -1,20 +0,0 @@
-MIT License
-
-Copyright (c) 2023 Toyota Research Institute
-
-Permission is hereby granted, free of charge, to any person obtaining a copy of
-this software and associated documentation files (the "Software"), to deal in
-the Software without restriction, including without limitation the rights to
-use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
-the Software, and to permit persons to whom the Software is furnished to do so,
-subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
-FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
-COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
-IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
-CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
diff --git a/README.md b/README.md
index 90b14b0..48c1997 100644
--- a/README.md
+++ b/README.md
@@ -1,69 +1,166 @@
# Inverse Dynamics Trajectory Optimization
-Implements the contact-implicit trajectory optimization algorithm described in
+Implements the contact-implicit trajectory optimization algorithm described in
[Inverse Dynamics Trajectory Optimization for Contact-Implicit Model Predictive
Control](https://idto.github.io/) by Vince Kurtz, Alejandro Castro, Aykut Özgün
Önol, and Hai Lin. https://arxiv.org/abs/2309.01813.
-## Dependencies
+
-This software is built on [Drake](https://drake.mit.edu). You do not need a
-separate Drake installation, but all the requirements for [building Drake from
-source](https://drake.mit.edu/from_source.html) apply. Most notably, that
-includes Bazel and a C++17 compiler.
+
-The easiest way to install these dependencies is with Drake's
-[`install_prereqs.sh`](https://drake.mit.edu/from_source.html#mandatory-platform-specific-instructions)
-script:
+
+## Docker Quickstart
+
+Clone this repository:
+
+```bash
+git clone https://github.com/ToyotaResearchInstitute/idto/
+cd idto
```
-git clone https://github.com/RobotLocomotion/drake.git
-cd drake
-sudo ./setup/ubuntu/install_prereqs.sh
+
+Build the docker image, compiling the C++ code and python bindings.
+
+```bash
+docker build -t idto .
```
-For Mac OS, replace the last line with `./setup/mac/install_prereqs.sh`.
+Enter the docker container, forwarding
+port 7000 for meshcat visualization:
+
+```bash
+docker run -p 7000:7000 -it idto
+```
+
+Inside the container, you can run any of the examples, for instance:
+```bash
+# Quadruped locomotion
+$ ./build/examples/mini_cheetah/mini_cheetah
+
+# Bi-manual box manipulation
+$ ./build/examples/dual_jaco/dual_jaco
+
+# A robot hand rotates a sphere
+$ ./build/examples/allegro_hand/allegro_hand
+
+# A two-link finger flicks a spinner
+$ ./build/examples/spinner/spinner
+
+# A robot arm rolls a ball
+$ ./build/examples/jaco_ball/jaco_ball
-## Installation
+# A simple planar hopping robot
+$ ./build/examples/hopper/hopper
-Install the dependencies (see above).
+# Pendulum swingup
+$ ./build/examples/pendulum/pendulum
+
+# Acrobot swingup
+$ ./build/examples/acrobot/acrobot
+
+# Single arm box manipulation
+$ ./build/examples/jaco/jaco
+
+# A compliant humanoid lifts a large ball
+$ ./build/examples/punyo/punyo
+
+# Drive the quadruped around with a joystick
+$ ./python_examples/mini_cheetah_mpc.py
+```
+
+Open a web browser to http://localhost:7000 to see the visualization.
+
+## Local Installation
+
+The only dependency is [Drake](https://drake.mit.edu/installation.html).
+We recommend Ubuntu 22.04 and Drake v1.30.0. Other configurations may work
+but are untested.
+
+For example, for a binary Drake installation at `$HOME/drake`:
+
+Download and extract the binaries:
+
+```bash
+cd $HOME
+wget https://github.com/RobotLocomotion/drake/releases/download/v1.30.0/drake-1.30.0-jammy.tar.gz
+tar -xvzf drake-1.30.0-jammy.tar.gz
+```
+
+Install Drake dependencies and gflags:
+
+```bash
+cd $HOME/drake
+sudo ./share/drake/setup/install_prereqs
+sudo apt-get install libgflags-dev
+```
+
+Update environment variables:
+
+```bash
+echo 'export CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:${HOME}/drake' >> ~/.bashrc
+echo 'export PYTHONPATH=${PYTHONPATH}:${HOME}/drake/lib/python3.10/site-packages' >> ~/.bashrc
+source ~/.bashrc
+```
+
+### C++
Clone this repository:
+
+```bash
+git clone https://github.com/ToyotaResearchInstitute/idto/
```
-git clone https://github.com/ToyotaResearchInstitute/idto
+
+Configure and build:
+
+```bash
cd idto
+mkdir build && cd build
+cmake ..
+make -j
```
-Compile the package:
-```
-bazel build //...
+(Optional) run unit tests:
+
+```bash
+ctest
```
-## Python Bindings
+### Python
-A limited subset of solver functionality is available in python via
-[pybind11](https://github.com/pybind/pybind11). To use the python bindings,
-first install the dependencies and compile with bazel, as described above.
+A limited subset of functionality is available via python bindings.
-Then update the python path:
+Build and install with pip:
+
+```bash
+git clone https://github.com/ToyotaResearchInstitute/idto/
+cd idto
+pip install .
```
-export PYTHONPATH=${PYTHONPATH}:"/path/to/idto/bazel-bin"
+
+(Optional) run unit tests:
+
+```bash
+pytest
```
-This line can be added to `.bashrc` if you want to permanently update the path.
-Some examples of using these bindings for open-loop trajectory optimization and
-MPC can be found in the `python_examples` folder.
+**Note:** The python bindings assume that the `pydrake` version you are using
+is the one used to build `pyidto`. A separate `pydrake` installation (e.g.,
+from `pip install drake`) may not work.
## Examples
+All of the examples use meshcat for visualization. A link will appear in the
+terminal, or just go ahead and open http://localhost:7000 in a browser.
+
+### C++
+
The `examples` folder contains various examples, including those described in
-[our paper](https://idto.github.io). Run them with, e.g.,
+[our paper](https://idto.github.io). After building, run them with, e.g.,
```
-bazel run //examples/spinner:spinner
+./build/examples/spinner/spinner
```
-A link will appear (e.g., `http://localhost:7000`), which you can use to open
-the Meshcat visualizer in a web browser.
Most of the examples (e.g., `spinner`) run a simulation with contact-implicit
model predictive control. Some others (e.g., `kuka`) perform a single open-loop
@@ -92,37 +189,21 @@ playback via the dropdown menu. For example, if `play_target_trajectory`,
play the target trajectory, followed by the open-loop solution, followed by a
simulation with MPC. Only the simulation will be saved for playback.
-## Other Tips and Tricks
+### Python
-### Use an existing Drake installation
+Examples that use the python bindings are in the `python_examples` folder.
-By default, Bazel pulls in a copy of Drake as an external and compiles it. If
-you have an existing local checkout at `/home/user/stuff/drake` that you would
-like to use instead, set the environment variable
+For instance, to run open-loop optimization with the spinner:
+```bash
+./python_examples/spinner_open_loop.py
```
-export IDTO_LOCAL_DRAKE_PATH=/home/user/stuff/drake
-```
-before building.
-
-### Run unit tests
-Run a (fairly minimal) set of unit tests and lint checks with
-```
-bazel test //...
+To run quaduped MPC, with the target velocity read from joystick commands:
+```bash
+./python_examples/mini_cheetah_mpc.py
```
-## Contributing
-
-We welcome your contributions, whether they are bug fixes, solver improvements,
-or new features! To make a contribution:
-
-1. Open a new pull request
-2. Make sure all the unit tests and lint checks pass
-3. Obtain a review from a code owner (e.g., @vincekurtz, @amcastro-tri, or
- @aykut-tri)
-4. Once the review is approved, we'll merge it into the `main` branch!
-
-Since this is research code, we will not review to the same production-quality
-standards as Drake. Nonetheless, new contributions should be clean and
-well-documented, and unit tests are encouraged. The standard of review should be
-*improving the health of the codebase* rather than perfection.
+**WARNING**
+The python bindings do not currently perform all the size checks that the C++
+version does. If you implement a new system, be sure to check the dimensions
+of the cost matrices and target configurations carefully.
diff --git a/WORKSPACE b/WORKSPACE
deleted file mode 100644
index 69b5c5b..0000000
--- a/WORKSPACE
+++ /dev/null
@@ -1,71 +0,0 @@
-# SPDX-License-Identifier: MIT-0
-
-workspace(name = "idto")
-
-# Set the version of Drake that we'll use
-DRAKE_COMMIT = "d39ca97d935d94d8c0e82abb61acb4696c4ba993"
-DRAKE_CHECKSUM = "a8cf6779a81a97d0b8fe71d77fc96ea3b409c6293c1c834e8856d0ad04232434"
-
-# Or choose a specific revision of Drake to use.
-# DRAKE_COMMIT = "251bd6982d48343cbd727d0a46d887461b2d13cc"
-# DRAKE_CHECKSUM = "72d106c3a61383170f564916cfbae0c25285cc3cb946fca4785beea8c342b8f6"
-#
-# You can also use DRAKE_COMMIT to choose a Drake release; eg:
-# DRAKE_COMMIT = "v0.15.0"
-#
-# Before changing the COMMIT, temporarily uncomment the next line so that Bazel
-# displays the suggested new value for the CHECKSUM.
-# DRAKE_CHECKSUM = "0" * 64
-
-# Or to temporarily build against a local checkout of Drake, at the bash prompt
-# set an environment variable before building:
-# export IDTO_LOCAL_DRAKE_PATH=/home/user/stuff/drake
-
-# Load an environment variable.
-load("//:environ.bzl", "environ_repository")
-environ_repository(name = "environ", vars = ["IDTO_LOCAL_DRAKE_PATH"])
-load("@environ//:environ.bzl", IDTO_LOCAL_DRAKE_PATH = "IDTO_LOCAL_DRAKE_PATH")
-
-# This declares the `@drake` repository as an http_archive from github,
-# iff IDTO_LOCAL_DRAKE_PATH is unset. When it is set, this declares a
-# `@drake_ignored` package which is never referenced, and thus is ignored.
-load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive")
-http_archive(
- name = "drake" if not IDTO_LOCAL_DRAKE_PATH else "drake_ignored",
- urls = [x.format(DRAKE_COMMIT) for x in [
- "https://github.com/RobotLocomotion/drake/archive/{}.tar.gz",
- ]],
- sha256 = DRAKE_CHECKSUM,
- strip_prefix = "drake-{}".format(DRAKE_COMMIT.lstrip("v")),
-)
-
-# This declares the `@drake` repository as a local directory,
-# iff IDTO_LOCAL_DRAKE_PATH is set. When it is unset, this declares a
-# `@drake_ignored` package which is never referenced, and thus is ignored.
-local_repository(
- name = "drake" if IDTO_LOCAL_DRAKE_PATH else "drake_ignored",
- path = IDTO_LOCAL_DRAKE_PATH,
-)
-print("Using IDTO_LOCAL_DRAKE_PATH={}".format(IDTO_LOCAL_DRAKE_PATH)) if IDTO_LOCAL_DRAKE_PATH else None # noqa
-
-# Reference external software libraries, tools, and toolchains per Drake's
-# defaults. Some software will come from the host system (Ubuntu or macOS);
-# other software will be downloaded in source or binary form from GitHub or
-# other sites.
-load("@drake//tools/workspace:default.bzl", "add_default_workspace")
-add_default_workspace()
-
-# Load pybind11 for python bindings
-http_archive(
- name = "pybind11_bazel",
- strip_prefix = "pybind11_bazel-2.11.1",
- urls = ["https://github.com/pybind/pybind11_bazel/archive/v2.11.1.zip"],
-)
-http_archive(
- name = "pybind11",
- build_file = "@pybind11_bazel//:pybind11.BUILD",
- strip_prefix = "pybind11-2.11.1",
- urls = ["https://github.com/pybind/pybind11/archive/v2.11.1.tar.gz"],
-)
-load("@pybind11_bazel//:python_configure.bzl", "python_configure")
-python_configure(name = "local_config_python")
diff --git a/environ.bzl b/environ.bzl
deleted file mode 100644
index 1403d12..0000000
--- a/environ.bzl
+++ /dev/null
@@ -1,45 +0,0 @@
-# SPDX-License-Identifier: MIT-0
-
-# Write out a repository that contains:
-# - An empty BUILD file, to define a package.
-# - An environ.bzl file with variable assignments for each ENV_NAMES item.
-def _impl(repository_ctx):
- vars = repository_ctx.attr.vars
- bzl_content = []
- for key in vars:
- value = repository_ctx.os.environ.get(key, "")
- bzl_content.append("{}='{}'\n".format(key, value))
- repository_ctx.file(
- "BUILD.bazel",
- content = "\n",
- executable = False,
- )
- repository_ctx.file(
- "environ.bzl",
- content = "".join(bzl_content),
- executable = False,
- )
-
-_string_list = attr.string_list()
-
-def environ_repository(name = None, vars = []):
- """Provide specific environment variables for use in a WORKSPACE file.
- The `vars` are the environment variables to provide.
-
- Example:
- environ_repository(name = "foo", vars = ["BAR", "BAZ"])
- load("@foo//:environ.bzl", "BAR", "BAZ")
- print(BAR)
- """
- rule = repository_rule(
- implementation = _impl,
- attrs = {
- "vars": _string_list,
- },
- local = True,
- environ = vars,
- )
- rule(
- name = name,
- vars = vars,
- )
diff --git a/examples/BUILD.bazel b/examples/BUILD.bazel
deleted file mode 100644
index 590a2b0..0000000
--- a/examples/BUILD.bazel
+++ /dev/null
@@ -1,82 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools:shared_library.bzl", "idto_cc_shared_library")
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-idto_cc_shared_library(
- name = "mpc_controller",
- srcs = ["mpc_controller.cc"],
- hdrs = ["mpc_controller.h"],
- deps = [
- "//optimizer:problem_definition",
- "//optimizer:trajectory_optimizer",
- "//optimizer:warm_start",
- "@drake//common/trajectories:piecewise_polynomial",
- "@drake//multibody/plant",
- "@drake//systems/framework:diagram",
- "@drake//systems/framework:leaf_system",
- "@drake//systems/framework:vector",
- ],
-)
-
-idto_cc_shared_library(
- name = "pd_plus_controller",
- srcs = ["pd_plus_controller.cc"],
- hdrs = ["pd_plus_controller.h"],
- deps = [
- "@drake//systems/framework:leaf_system",
- "@drake//systems/framework:vector",
- ],
-)
-
-idto_cc_shared_library(
- name = "example_base",
- srcs = ["example_base.cc"],
- hdrs = ["example_base.h"],
- deps = [
- ":mpc_controller",
- ":pd_plus_controller",
- ":yaml_config",
- "//utils:find_resource",
- "//optimizer:problem_definition",
- "//optimizer:trajectory_optimizer",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@drake//systems/analysis:simulator",
- "@drake//systems/framework:diagram",
- "@drake//systems/primitives:constant_vector_source",
- "@drake//systems/primitives:discrete_time_delay",
- "@drake//visualization:visualization_config_functions",
- ],
-)
-
-idto_cc_shared_library(
- name = "yaml_config",
- srcs = [],
- hdrs = ["yaml_config.h"],
- deps = [
- "@drake//common:essential",
- "@drake//common/yaml",
- ],
-)
-
-idto_cc_shared_library(
- name = "examples",
- deps = [
- ":example_base",
- ":mpc_controller",
- ":pd_plus_controller",
- ":yaml_config",
- ],
-)
-
-filegroup(
- name = "models",
- srcs = glob([
- "models/**",
- ]),
-)
-
-add_lint_tests()
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
new file mode 100644
index 0000000..0441f60
--- /dev/null
+++ b/examples/CMakeLists.txt
@@ -0,0 +1,36 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_library(yaml_config INTERFACE yaml_config.h)
+
+add_library(pd_plus_controller
+ pd_plus_controller.cc
+ pd_plus_controller.h)
+target_link_libraries(pd_plus_controller drake::drake)
+
+add_library(mpc_controller
+ mpc_controller.cc
+ mpc_controller.h)
+target_link_libraries(mpc_controller
+ drake::drake
+ pd_plus_controller
+ trajectory_optimizer)
+
+add_library(example_base example_base.cc example_base.h)
+target_link_libraries(example_base
+ drake::drake
+ mpc_controller
+ yaml_config
+ find_resource)
+
+add_subdirectory(acrobot)
+add_subdirectory(airhockey)
+add_subdirectory(allegro_hand)
+add_subdirectory(dual_jaco)
+add_subdirectory(hopper)
+add_subdirectory(jaco)
+add_subdirectory(jaco_ball)
+add_subdirectory(kuka)
+add_subdirectory(mini_cheetah)
+add_subdirectory(pendulum)
+add_subdirectory(punyo)
+add_subdirectory(spinner)
\ No newline at end of file
diff --git a/examples/acrobot/BUILD.bazel b/examples/acrobot/BUILD.bazel
deleted file mode 100644
index 91920e4..0000000
--- a/examples/acrobot/BUILD.bazel
+++ /dev/null
@@ -1,40 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "acrobot",
- srcs = ["acrobot.cc"],
- data = [
- ":acrobot.yaml",
- "@drake//examples/acrobot:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "acrobot_test",
- srcs = ["acrobot.cc"],
- args = ["--test"],
- data = [
- ":acrobot.yaml",
- "@drake//examples/acrobot:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/acrobot/CMakeLists.txt b/examples/acrobot/CMakeLists.txt
new file mode 100644
index 0000000..25eb67c
--- /dev/null
+++ b/examples/acrobot/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(acrobot acrobot.cc)
+target_link_libraries(acrobot
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(acrobot.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/acrobot/acrobot.yaml COPYONLY)
+
+add_test(NAME acrobot_test COMMAND acrobot --test)
diff --git a/examples/acrobot/acrobot.cc b/examples/acrobot/acrobot.cc
index edfb610..038182f 100644
--- a/examples/acrobot/acrobot.cc
+++ b/examples/acrobot/acrobot.cc
@@ -4,6 +4,8 @@
#include
#include
+#include "utils/find_resource.h"
+
DEFINE_bool(test, false,
"whether this example is being run in test mode, where we solve a "
"simpler problem");
@@ -15,6 +17,7 @@ namespace acrobot {
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Vector3d;
+using idto::utils::FindIdtoResource;
class AcrobotExample : public TrajOptExample {
public:
@@ -28,9 +31,8 @@ class AcrobotExample : public TrajOptExample {
private:
void CreatePlantModel(MultibodyPlant* plant) const {
const std::string urdf_file =
- "external/drake/examples/acrobot/Acrobot_no_collision.urdf";
+ FindIdtoResource("idto/models/acrobot/acrobot.urdf");
Parser(plant).AddModels(urdf_file);
- plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("base_link"));
}
};
diff --git a/examples/airhockey/BUILD.bazel b/examples/airhockey/BUILD.bazel
deleted file mode 100644
index 8f968fe..0000000
--- a/examples/airhockey/BUILD.bazel
+++ /dev/null
@@ -1,38 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "airhockey",
- srcs = ["airhockey.cc"],
- data = [
- ":airhockey.yaml",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "airhockey_test",
- srcs = ["airhockey.cc"],
- args = ["--test"],
- data = [
- ":airhockey.yaml",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/airhockey/CMakeLists.txt b/examples/airhockey/CMakeLists.txt
new file mode 100644
index 0000000..7192c02
--- /dev/null
+++ b/examples/airhockey/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(airhockey airhockey.cc)
+target_link_libraries(airhockey
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(airhockey.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/airhockey/airhockey.yaml COPYONLY)
+
+add_test(NAME airhockey_test COMMAND airhockey --test)
diff --git a/examples/allegro_hand/BUILD.bazel b/examples/allegro_hand/BUILD.bazel
deleted file mode 100644
index cece526..0000000
--- a/examples/allegro_hand/BUILD.bazel
+++ /dev/null
@@ -1,43 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "allegro_hand",
- srcs = ["allegro_hand.cc"],
- data = [
- ":allegro_hand.yaml",
- ":allegro_hand_upside_down.yaml",
- "//examples:models",
- "@drake//manipulation/models/allegro_hand_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-cc_test(
- name = "allegro_hand_test",
- srcs = ["allegro_hand.cc"],
- args = ["--test"],
- data = [
- ":allegro_hand.yaml",
- "//examples:models",
- "@drake//manipulation/models/allegro_hand_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-add_lint_tests()
diff --git a/examples/allegro_hand/CMakeLists.txt b/examples/allegro_hand/CMakeLists.txt
new file mode 100644
index 0000000..5e8899c
--- /dev/null
+++ b/examples/allegro_hand/CMakeLists.txt
@@ -0,0 +1,15 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(allegro_hand allegro_hand.cc)
+target_link_libraries(allegro_hand
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(allegro_hand.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/allegro_hand/allegro_hand.yaml COPYONLY)
+configure_file(allegro_hand_upside_down.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/allegro_hand/allegro_hand_upside_down.yaml COPYONLY)
+
+add_test(NAME allegro_hand_test COMMAND allegro_hand --test)
diff --git a/examples/allegro_hand/allegro_hand.cc b/examples/allegro_hand/allegro_hand.cc
index 6c112f2..e452c8b 100644
--- a/examples/allegro_hand/allegro_hand.cc
+++ b/examples/allegro_hand/allegro_hand.cc
@@ -33,6 +33,7 @@ using drake::multibody::RigidBody;
using drake::multibody::SpatialInertia;
using drake::multibody::UnitInertia;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class AllegroHandExample : public TrajOptExample {
public:
@@ -84,8 +85,7 @@ class AllegroHandExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 1.0);
// Add a model of the hand
- std::string sdf_file =
- idto::FindIdtoResourceOrThrow("idto/examples/models/allegro_hand.sdf");
+ std::string sdf_file = FindIdtoResource("idto/models/allegro_hand.sdf");
Parser(plant).AddModels(sdf_file);
RigidTransformd X_hand(RollPitchYawd(0, -M_PI_2, 0), Vector3d(0, 0, 0));
plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("hand_root"),
@@ -150,8 +150,7 @@ class AllegroHandExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 1.0);
// Add a model of the hand
- std::string sdf_file =
- idto::FindIdtoResourceOrThrow("idto/examples/models/allegro_hand.sdf");
+ std::string sdf_file = FindIdtoResource("idto/models/allegro_hand.sdf");
Parser(plant).AddModels(sdf_file);
RigidTransformd X_hand(RollPitchYawd(0, -M_PI_2, 0), Vector3d(0, 0, 0));
plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("hand_root"),
diff --git a/examples/dual_jaco/BUILD.bazel b/examples/dual_jaco/BUILD.bazel
deleted file mode 100644
index 95d597a..0000000
--- a/examples/dual_jaco/BUILD.bazel
+++ /dev/null
@@ -1,42 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "dual_jaco",
- srcs = ["dual_jaco.cc"],
- data = [
- ":dual_jaco.yaml",
- "//examples:models",
- "@drake//manipulation/models/jaco_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "dual_jaco_test",
- srcs = ["dual_jaco.cc"],
- args = ["--test"],
- data = [
- ":dual_jaco.yaml",
- "//examples:models",
- "@drake//manipulation/models/jaco_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/dual_jaco/CMakeLists.txt b/examples/dual_jaco/CMakeLists.txt
new file mode 100644
index 0000000..ba1d67c
--- /dev/null
+++ b/examples/dual_jaco/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(dual_jaco dual_jaco.cc)
+target_link_libraries(dual_jaco
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(dual_jaco.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/dual_jaco/dual_jaco.yaml COPYONLY)
+
+add_test(NAME dual_jaco_test COMMAND dual_jaco --test)
diff --git a/examples/dual_jaco/dual_jaco.cc b/examples/dual_jaco/dual_jaco.cc
index 82f9b19..6b37ff0 100644
--- a/examples/dual_jaco/dual_jaco.cc
+++ b/examples/dual_jaco/dual_jaco.cc
@@ -24,6 +24,7 @@ using drake::multibody::ModelInstanceIndex;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class DualJacoExample : public TrajOptExample {
public:
@@ -37,8 +38,8 @@ class DualJacoExample : public TrajOptExample {
private:
void CreatePlantModel(MultibodyPlant* plant) const final {
// Add jaco arms
- std::string robot_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/j2s7s300_arm_sphere_collision_v2.sdf");
+ std::string robot_file = FindIdtoResource(
+ "idto/models/j2s7s300_arm_sphere_collision_v2.sdf");
ModelInstanceIndex jaco_left = Parser(plant).AddModels(robot_file)[0];
plant->RenameModelInstance(jaco_left, "jaco_left");
@@ -58,7 +59,7 @@ class DualJacoExample : public TrajOptExample {
// Add a manipuland
std::string manipuland_file =
- idto::FindIdtoResourceOrThrow("idto/examples/models/box_15cm.sdf");
+ FindIdtoResource("idto/models/box_15cm.sdf");
Parser(plant).AddModels(manipuland_file);
// Add the ground
@@ -78,8 +79,8 @@ class DualJacoExample : public TrajOptExample {
void CreatePlantModelForSimulation(
MultibodyPlant* plant) const final {
// Add jaco arms, including gravity
- std::string robot_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/j2s7s300_arm_hydro_collision.sdf");
+ std::string robot_file = FindIdtoResource(
+ "idto/models/j2s7s300_arm_hydro_collision.sdf");
ModelInstanceIndex jaco_left = Parser(plant).AddModels(robot_file)[0];
plant->RenameModelInstance(jaco_left, "jaco_left");
@@ -98,8 +99,8 @@ class DualJacoExample : public TrajOptExample {
plant->set_gravity_enabled(jaco_right, false);
// Add a manipuland with compliant hydroelastic contact
- std::string manipuland_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/box_15cm_hydro.sdf");
+ std::string manipuland_file = FindIdtoResource(
+ "idto/models/box_15cm_hydro.sdf");
Parser(plant).AddModels(manipuland_file);
// Add the ground with compliant hydroelastic contact
diff --git a/examples/example_base.cc b/examples/example_base.cc
index 559a964..48721ca 100644
--- a/examples/example_base.cc
+++ b/examples/example_base.cc
@@ -23,6 +23,7 @@ using Eigen::Matrix4d;
using mpc::Interpolator;
using mpc::ModelPredictiveController;
using pd_plus::PdPlusController;
+using utils::FindIdtoResource;
void TrajOptExample::RunExample(const std::string options_file,
const bool test) const {
@@ -30,7 +31,7 @@ void TrajOptExample::RunExample(const std::string options_file,
TrajOptExampleParams default_options;
TrajOptExampleParams options =
drake::yaml::LoadYamlFile(
- idto::FindIdtoResourceOrThrow(options_file), {}, default_options);
+ FindIdtoResource(options_file), {}, default_options);
if (test) {
// Use simplified options for a smoke test
diff --git a/examples/hopper/BUILD.bazel b/examples/hopper/BUILD.bazel
deleted file mode 100644
index 373ee0f..0000000
--- a/examples/hopper/BUILD.bazel
+++ /dev/null
@@ -1,40 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "hopper",
- srcs = ["hopper.cc"],
- data = [
- ":hopper.yaml",
- "//examples:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "hopper_test",
- srcs = ["hopper.cc"],
- args = ["--test"],
- data = [
- ":hopper.yaml",
- "//examples:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/hopper/CMakeLists.txt b/examples/hopper/CMakeLists.txt
new file mode 100644
index 0000000..c472849
--- /dev/null
+++ b/examples/hopper/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(hopper hopper.cc)
+target_link_libraries(hopper
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(hopper.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/hopper/hopper.yaml COPYONLY)
+
+add_test(NAME hopper_test COMMAND hopper --test)
diff --git a/examples/hopper/hopper.cc b/examples/hopper/hopper.cc
index ea3693d..f1f045b 100644
--- a/examples/hopper/hopper.cc
+++ b/examples/hopper/hopper.cc
@@ -18,6 +18,7 @@ using drake::multibody::CoulombFriction;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
/**
* A simple planar hopper, inspired by https://youtu.be/uWADBSmHebA?t=893.
@@ -36,8 +37,7 @@ class HopperExample : public TrajOptExample {
const drake::Vector4 green(0.3, 0.6, 0.4, 1.0);
// Add a hopper
- std::string urdf_file =
- idto::FindIdtoResourceOrThrow("idto/examples/models/hopper.urdf");
+ std::string urdf_file = FindIdtoResource("idto/models/hopper.urdf");
Parser(plant).AddModels(urdf_file);
// Add collision with the ground
diff --git a/examples/jaco/BUILD.bazel b/examples/jaco/BUILD.bazel
deleted file mode 100644
index 7044ebb..0000000
--- a/examples/jaco/BUILD.bazel
+++ /dev/null
@@ -1,42 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "jaco",
- srcs = ["jaco.cc"],
- data = [
- ":jaco.yaml",
- "//examples:models",
- "@drake//manipulation/models/jaco_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "jaco_test",
- srcs = ["jaco.cc"],
- args = ["--test"],
- data = [
- ":jaco.yaml",
- "//examples:models",
- "@drake//manipulation/models/jaco_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/jaco/CMakeLists.txt b/examples/jaco/CMakeLists.txt
new file mode 100644
index 0000000..a606e5a
--- /dev/null
+++ b/examples/jaco/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(jaco jaco.cc)
+target_link_libraries(jaco
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(jaco.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/jaco/jaco.yaml COPYONLY)
+
+add_test(NAME jaco_test COMMAND jaco --test)
diff --git a/examples/jaco/jaco.cc b/examples/jaco/jaco.cc
index fd21f72..832f240 100644
--- a/examples/jaco/jaco.cc
+++ b/examples/jaco/jaco.cc
@@ -24,6 +24,7 @@ using drake::multibody::ModelInstanceIndex;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class JacoExample : public TrajOptExample {
public:
@@ -37,8 +38,8 @@ class JacoExample : public TrajOptExample {
private:
void CreatePlantModel(MultibodyPlant* plant) const final {
// Add a jaco arm without gravity
- std::string robot_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/j2s7s300_arm_sphere_collision_v2.sdf");
+ std::string robot_file =
+ FindIdtoResource("idto/models/j2s7s300_arm_sphere_collision_v2.sdf");
ModelInstanceIndex jaco = Parser(plant).AddModels(robot_file)[0];
RigidTransformd X_jaco(RollPitchYaw(0, 0, M_PI_2),
Vector3d(0, 0.27, 0.11));
@@ -47,8 +48,7 @@ class JacoExample : public TrajOptExample {
plant->set_gravity_enabled(jaco, false);
// Add a manipuland with sphere contact
- std::string manipuland_file =
- idto::FindIdtoResourceOrThrow("idto/examples/models/box_15cm.sdf");
+ std::string manipuland_file = FindIdtoResource("idto/models/box_15cm.sdf");
Parser(plant).AddModels(manipuland_file);
// Add the ground
@@ -71,8 +71,8 @@ class JacoExample : public TrajOptExample {
plant->set_contact_model(drake::multibody::ContactModel::kHydroelastic);
// Add a jaco arm, including gravity, with rigid hydroelastic contact
- std::string robot_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/j2s7s300_arm_hydro_collision.sdf");
+ std::string robot_file =
+ FindIdtoResource("idto/models/j2s7s300_arm_hydro_collision.sdf");
ModelInstanceIndex jaco = Parser(plant).AddModels(robot_file)[0];
RigidTransformd X_jaco(RollPitchYaw(0, 0, M_PI_2),
Vector3d(0, 0.27, 0.11));
@@ -81,8 +81,8 @@ class JacoExample : public TrajOptExample {
plant->set_gravity_enabled(jaco, false);
// Add a manipuland with compliant hydroelastic contact
- std::string manipuland_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/box_15cm_hydro.sdf");
+ std::string manipuland_file =
+ FindIdtoResource("idto/models/box_15cm_hydro.sdf");
Parser(plant).AddModels(manipuland_file);
// Add the ground with compliant hydroelastic contact
diff --git a/examples/jaco_ball/BUILD.bazel b/examples/jaco_ball/BUILD.bazel
deleted file mode 100644
index 1daf137..0000000
--- a/examples/jaco_ball/BUILD.bazel
+++ /dev/null
@@ -1,42 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "jaco_ball",
- srcs = ["jaco_ball.cc"],
- data = [
- ":jaco_ball.yaml",
- "//examples:models",
- "@drake//manipulation/models/jaco_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "jaco_ball_test",
- srcs = ["jaco_ball.cc"],
- args = ["--test"],
- data = [
- ":jaco_ball.yaml",
- "//examples:models",
- "@drake//manipulation/models/jaco_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/jaco_ball/CMakeLists.txt b/examples/jaco_ball/CMakeLists.txt
new file mode 100644
index 0000000..3052705
--- /dev/null
+++ b/examples/jaco_ball/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(jaco_ball jaco_ball.cc)
+target_link_libraries(jaco_ball
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(jaco_ball.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/jaco_ball/jaco_ball.yaml COPYONLY)
+
+add_test(NAME jaco_ball_test COMMAND jaco_ball --test)
diff --git a/examples/jaco_ball/jaco_ball.cc b/examples/jaco_ball/jaco_ball.cc
index 4aafb1b..8fdf522 100644
--- a/examples/jaco_ball/jaco_ball.cc
+++ b/examples/jaco_ball/jaco_ball.cc
@@ -29,6 +29,7 @@ using drake::multibody::RigidBody;
using drake::multibody::SpatialInertia;
using drake::multibody::UnitInertia;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class JacoBallExample : public TrajOptExample {
public:
@@ -45,8 +46,8 @@ class JacoBallExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 0.5);
// Add a jaco arm without gravity
- std::string robot_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/j2s7s300_arm_sphere_collision_v2.sdf");
+ std::string robot_file =
+ FindIdtoResource("idto/models/j2s7s300_arm_sphere_collision_v2.sdf");
ModelInstanceIndex jaco = Parser(plant).AddModels(robot_file)[0];
RigidTransformd X_jaco(RollPitchYawd(0, 0, M_PI_2),
Vector3d(0, 0.27, 0.11));
@@ -102,8 +103,8 @@ class JacoBallExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 0.5);
// Add a jaco arm without gravity
- std::string robot_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/j2s7s300_arm_hydro_collision.sdf");
+ std::string robot_file =
+ FindIdtoResource("idto/models/j2s7s300_arm_hydro_collision.sdf");
ModelInstanceIndex jaco = Parser(plant).AddModels(robot_file)[0];
RigidTransformd X_jaco(RollPitchYawd(0, 0, M_PI_2),
Vector3d(0, 0.27, 0.11));
diff --git a/examples/kuka/BUILD.bazel b/examples/kuka/BUILD.bazel
deleted file mode 100644
index 074b7fa..0000000
--- a/examples/kuka/BUILD.bazel
+++ /dev/null
@@ -1,42 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "kuka",
- srcs = ["kuka.cc"],
- data = [
- ":kuka.yaml",
- "//examples:models",
- "@drake//manipulation/models/iiwa_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-cc_test(
- name = "kuka_test",
- srcs = ["kuka.cc"],
- args = ["--test"],
- data = [
- ":kuka.yaml",
- "//examples:models",
- "@drake//manipulation/models/iiwa_description:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags"
- ],
-)
-
-add_lint_tests()
diff --git a/examples/kuka/CMakeLists.txt b/examples/kuka/CMakeLists.txt
new file mode 100644
index 0000000..cb911ca
--- /dev/null
+++ b/examples/kuka/CMakeLists.txt
@@ -0,0 +1,14 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(kuka kuka.cc)
+target_link_libraries(kuka
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(kuka.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/kuka/kuka.yaml COPYONLY)
+
+add_test(NAME kuka_test COMMAND kuka --test)
+
diff --git a/examples/kuka/kuka.cc b/examples/kuka/kuka.cc
index 8adfa14..b9ec2c3 100644
--- a/examples/kuka/kuka.cc
+++ b/examples/kuka/kuka.cc
@@ -19,6 +19,7 @@ using drake::multibody::ModelInstanceIndex;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class KukaExample : public TrajOptExample {
void CreatePlantModel(MultibodyPlant* plant) const final {
@@ -27,16 +28,16 @@ class KukaExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 1.0);
// Add a kuka arm
- std::string robot_file = drake::FindResourceOrThrow(
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_spheres_collision.urdf");
- ModelInstanceIndex kuka = Parser(plant).AddModels(robot_file)[0];
+ std::string url =
+ "package://drake_models/iiwa_description/urdf/"
+ "iiwa14_spheres_collision.urdf";
+ ModelInstanceIndex kuka = Parser(plant).AddModelsFromUrl(url)[0];
plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("base"));
plant->set_gravity_enabled(kuka, false);
// Add a manipuland
std::string manipuland_file =
- FindIdtoResourceOrThrow("idto/examples/models/box_intel_nuc.sdf");
+ FindIdtoResource("idto/models/box_intel_nuc.sdf");
Parser(plant).AddModels(manipuland_file);
// Add the ground
diff --git a/examples/mini_cheetah/BUILD.bazel b/examples/mini_cheetah/BUILD.bazel
deleted file mode 100644
index 81aa892..0000000
--- a/examples/mini_cheetah/BUILD.bazel
+++ /dev/null
@@ -1,40 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "mini_cheetah",
- srcs = ["mini_cheetah.cc"],
- data = [
- ":mini_cheetah.yaml",
- "//examples:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-cc_test(
- name = "mini_cheetah_test",
- srcs = ["mini_cheetah.cc"],
- args = ["--test"],
- data = [
- ":mini_cheetah.yaml",
- "//examples:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-add_lint_tests()
diff --git a/examples/mini_cheetah/CMakeLists.txt b/examples/mini_cheetah/CMakeLists.txt
new file mode 100644
index 0000000..99bc1b9
--- /dev/null
+++ b/examples/mini_cheetah/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(mini_cheetah mini_cheetah.cc)
+target_link_libraries(mini_cheetah
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(mini_cheetah.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/mini_cheetah/mini_cheetah.yaml COPYONLY)
+
+add_test(NAME mini_cheetah_test COMMAND mini_cheetah --test)
diff --git a/examples/mini_cheetah/mini_cheetah.cc b/examples/mini_cheetah/mini_cheetah.cc
index 0b96bc8..95d63af 100644
--- a/examples/mini_cheetah/mini_cheetah.cc
+++ b/examples/mini_cheetah/mini_cheetah.cc
@@ -23,6 +23,7 @@ using drake::multibody::CoulombFriction;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
/**
* A model of the MIT mini cheetah quadruped.
@@ -41,8 +42,8 @@ class MiniCheetahExample : public TrajOptExample {
const drake::Vector4 green(0.3, 0.6, 0.4, 1.0);
// Add the robot
- std::string urdf_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/mini_cheetah_mesh.urdf");
+ std::string urdf_file =
+ FindIdtoResource("idto/models/mini_cheetah_mesh.urdf");
Parser(plant).AddModels(urdf_file);
// Add collision with the ground
diff --git a/examples/mpc_controller.cc b/examples/mpc_controller.cc
index baee076..108cb7a 100644
--- a/examples/mpc_controller.cc
+++ b/examples/mpc_controller.cc
@@ -42,6 +42,8 @@ ModelPredictiveController::ModelPredictiveController(
EventStatus ModelPredictiveController::UpdateAbstractState(
const Context& context, State* state) const {
+ DRAKE_DEMAND(optimizer_.params().q_nom_relative_to_q_init.size() ==
+ optimizer_.plant().num_positions());
std::cout << "Resolving at t=" << context.get_time() << std::endl;
// Get the latest initial condition
diff --git a/examples/pendulum/BUILD.bazel b/examples/pendulum/BUILD.bazel
deleted file mode 100644
index 8264df1..0000000
--- a/examples/pendulum/BUILD.bazel
+++ /dev/null
@@ -1,40 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "pendulum",
- srcs = ["pendulum.cc"],
- data = [
- ":pendulum.yaml",
- "@drake//examples/pendulum:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-cc_test(
- name = "pendulum_test",
- srcs = ["pendulum.cc"],
- args = ["--test"],
- data = [
- ":pendulum.yaml",
- "@drake//examples/pendulum:models",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-add_lint_tests()
diff --git a/examples/pendulum/CMakeLists.txt b/examples/pendulum/CMakeLists.txt
new file mode 100644
index 0000000..1b227a4
--- /dev/null
+++ b/examples/pendulum/CMakeLists.txt
@@ -0,0 +1,14 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(pendulum pendulum.cc)
+target_link_libraries(pendulum
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(pendulum.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/pendulum/pendulum.yaml COPYONLY)
+
+add_test(NAME pendulum_test COMMAND pendulum --test)
+
diff --git a/examples/punyo/BUILD.bazel b/examples/punyo/BUILD.bazel
deleted file mode 100644
index 3760c11..0000000
--- a/examples/punyo/BUILD.bazel
+++ /dev/null
@@ -1,40 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "punyo",
- srcs = ["punyo.cc"],
- data = [
- "//examples:models",
- ":punyo.yaml",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-cc_test(
- name = "punyo_test",
- srcs = ["punyo.cc"],
- args = ["--test"],
- data = [
- "//examples:models",
- ":punyo.yaml",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-add_lint_tests()
diff --git a/examples/punyo/CMakeLists.txt b/examples/punyo/CMakeLists.txt
new file mode 100644
index 0000000..a2789aa
--- /dev/null
+++ b/examples/punyo/CMakeLists.txt
@@ -0,0 +1,14 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(punyo punyo.cc)
+target_link_libraries(punyo
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(punyo.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/punyo/punyo.yaml COPYONLY)
+
+add_test(NAME punyo_test COMMAND punyo --test)
+
diff --git a/examples/punyo/punyo.cc b/examples/punyo/punyo.cc
index d067174..9e963a0 100644
--- a/examples/punyo/punyo.cc
+++ b/examples/punyo/punyo.cc
@@ -30,6 +30,7 @@ using drake::multibody::RigidBody;
using drake::multibody::SpatialInertia;
using drake::multibody::UnitInertia;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class PunyoExample : public TrajOptExample {
public:
@@ -47,8 +48,7 @@ class PunyoExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 1.0);
// Add a humanoid model
- std::string sdf_file =
- idto::FindIdtoResourceOrThrow("idto/examples/models/punyoid.sdf");
+ std::string sdf_file = FindIdtoResource("idto/models/punyoid.sdf");
ModelInstanceIndex humanoid = Parser(plant).AddModels(sdf_file)[0];
plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("base"));
plant->set_gravity_enabled(humanoid, false);
@@ -99,8 +99,7 @@ class PunyoExample : public TrajOptExample {
const drake::Vector4 black(0.0, 0.0, 0.0, 1.0);
// Add a humanoid model
- std::string sdf_file =
- FindIdtoResourceOrThrow("idto/examples/models/punyoid.sdf");
+ std::string sdf_file = FindIdtoResource("idto/models/punyoid.sdf");
ModelInstanceIndex humanoid = Parser(plant).AddModels(sdf_file)[0];
plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("base"));
plant->set_gravity_enabled(humanoid, false);
diff --git a/examples/spinner/BUILD.bazel b/examples/spinner/BUILD.bazel
deleted file mode 100644
index 6fa61b5..0000000
--- a/examples/spinner/BUILD.bazel
+++ /dev/null
@@ -1,40 +0,0 @@
-# -*- python -*-
-
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-
-cc_binary(
- name = "spinner",
- srcs = ["spinner.cc"],
- data = [
- "//examples:models",
- ":spinner.yaml",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-cc_test(
- name = "spinner_test",
- srcs = ["spinner.cc"],
- args = ["--test"],
- data = [
- "//examples:models",
- ":spinner.yaml",
- ],
- deps = [
- "//examples:example_base",
- "//utils:find_resource",
- "@drake//multibody/parsing",
- "@drake//multibody/plant",
- "@gflags",
- ],
-)
-
-add_lint_tests()
diff --git a/examples/spinner/CMakeLists.txt b/examples/spinner/CMakeLists.txt
new file mode 100644
index 0000000..b63d21d
--- /dev/null
+++ b/examples/spinner/CMakeLists.txt
@@ -0,0 +1,13 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+add_executable(spinner spinner.cc)
+target_link_libraries(spinner
+ drake::drake
+ example_base
+ find_resource
+ gflags)
+
+configure_file(spinner.yaml
+ ${CMAKE_BINARY_DIR}/idto/examples/spinner/spinner.yaml COPYONLY)
+
+add_test(NAME spinner_test COMMAND spinner --test)
diff --git a/examples/spinner/spinner.cc b/examples/spinner/spinner.cc
index b6b7823..f2ecf4f 100644
--- a/examples/spinner/spinner.cc
+++ b/examples/spinner/spinner.cc
@@ -17,6 +17,7 @@ using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using Eigen::Matrix4d;
using Eigen::Vector3d;
+using utils::FindIdtoResource;
class SpinnerExample : public TrajOptExample {
public:
@@ -32,8 +33,8 @@ class SpinnerExample : public TrajOptExample {
// N.B. geometry of the spinner is chosen via gflags rather than yaml so
// that we can use the same yaml format for all of the examples, without
// cluttering it with spinner-specific options.
- std::string urdf_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/spinner_friction.urdf");
+ std::string urdf_file =
+ FindIdtoResource("idto/models/spinner_friction.urdf");
Parser(plant).AddModels(urdf_file);
}
};
diff --git a/img/acrobot.png b/img/acrobot.png
new file mode 100644
index 0000000..1042722
Binary files /dev/null and b/img/acrobot.png differ
diff --git a/img/allegro.png b/img/allegro.png
new file mode 100644
index 0000000..ac96f70
Binary files /dev/null and b/img/allegro.png differ
diff --git a/img/dual_jaco.png b/img/dual_jaco.png
new file mode 100644
index 0000000..96bffa2
Binary files /dev/null and b/img/dual_jaco.png differ
diff --git a/img/hopper.png b/img/hopper.png
new file mode 100644
index 0000000..3aaa6d2
Binary files /dev/null and b/img/hopper.png differ
diff --git a/img/jaco.png b/img/jaco.png
new file mode 100644
index 0000000..6a7034b
Binary files /dev/null and b/img/jaco.png differ
diff --git a/img/jaco_ball.png b/img/jaco_ball.png
new file mode 100644
index 0000000..95f5e44
Binary files /dev/null and b/img/jaco_ball.png differ
diff --git a/img/mini_cheetah.png b/img/mini_cheetah.png
new file mode 100644
index 0000000..79fe329
Binary files /dev/null and b/img/mini_cheetah.png differ
diff --git a/img/pendulum.png b/img/pendulum.png
new file mode 100644
index 0000000..b975e18
Binary files /dev/null and b/img/pendulum.png differ
diff --git a/img/punyo.png b/img/punyo.png
new file mode 100644
index 0000000..3a4569c
Binary files /dev/null and b/img/punyo.png differ
diff --git a/img/spinner.png b/img/spinner.png
new file mode 100644
index 0000000..b96eb51
Binary files /dev/null and b/img/spinner.png differ
diff --git a/examples/models/2dof_spinner.urdf b/models/2dof_spinner.urdf
similarity index 100%
rename from examples/models/2dof_spinner.urdf
rename to models/2dof_spinner.urdf
diff --git a/examples/models/2dof_spinner_capsule.urdf b/models/2dof_spinner_capsule.urdf
similarity index 100%
rename from examples/models/2dof_spinner_capsule.urdf
rename to models/2dof_spinner_capsule.urdf
diff --git a/models/CMakeLists.txt b/models/CMakeLists.txt
new file mode 100644
index 0000000..ab034ee
--- /dev/null
+++ b/models/CMakeLists.txt
@@ -0,0 +1,10 @@
+# Copy all files in this directory to idto/models/... in the build directory
+file(GLOB_RECURSE SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*)
+foreach(SRC_FILE ${SRC_FILES})
+ file(RELATIVE_PATH RELATIVE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${SRC_FILE})
+ configure_file(
+ ${SRC_FILE}
+ ${CMAKE_BINARY_DIR}/idto/models/${RELATIVE_PATH}
+ COPYONLY
+ )
+endforeach()
diff --git a/models/acrobot/acrobot.urdf b/models/acrobot/acrobot.urdf
new file mode 100644
index 0000000..cda2b0a
--- /dev/null
+++ b/models/acrobot/acrobot.urdf
@@ -0,0 +1,74 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
diff --git a/examples/models/allegro_hand.sdf b/models/allegro_hand.sdf
similarity index 98%
rename from examples/models/allegro_hand.sdf
rename to models/allegro_hand.sdf
index ce00412..1904c92 100644
--- a/examples/models/allegro_hand.sdf
+++ b/models/allegro_hand.sdf
@@ -9,7 +9,7 @@
unofficial ROS package of Allegro Hand (https://github.com/felixduvallet/
allegro-hand-ros/tree/master/allegro_hand_description). The conversion is
applied using the ROS package xacro to convert into urdf files, then
- converted to sdf files. The model files (.obj files and .mtl files) are
+ converted to sdf files. The model files (.gltf files and .mtl files) are
converted from the .STL files from the same source. The physical parameters
of the model in the cited unofficial fork of the Allegro hand have
non-physical values. More precisely, many of the inertia matrices are not
@@ -73,7 +73,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/base_link_left.obj
+ package://drake_models/allegro_hand_description/meshes/base_link_left.gltf
@@ -116,7 +116,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_0.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_0.0.gltf
@@ -174,7 +174,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_1.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_1.0.gltf
@@ -232,7 +232,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_2.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_2.0.gltf
@@ -311,7 +311,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_3.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_3.0.gltf
@@ -320,7 +320,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_3.0_tip.obj
+ package://drake_models/allegro_hand_description/meshes/link_3.0_tip.gltf
@@ -379,7 +379,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_12.0_left.obj
+ package://drake_models/allegro_hand_description/meshes/link_12.0_left.gltf
@@ -416,7 +416,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_13.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_13.0.gltf
@@ -474,7 +474,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_14.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_14.0.gltf
@@ -554,7 +554,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_15.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_15.0.gltf
@@ -563,7 +563,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_15.0_tip.obj
+ package://drake_models/allegro_hand_description/meshes/link_15.0_tip.gltf
@@ -621,7 +621,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_0.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_0.0.gltf
@@ -679,7 +679,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_1.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_1.0.gltf
@@ -737,7 +737,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_2.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_2.0.gltf
@@ -816,7 +816,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_3.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_3.0.gltf
@@ -825,7 +825,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_3.0_tip.obj
+ package://drake_models/allegro_hand_description/meshes/link_3.0_tip.gltf
@@ -883,7 +883,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_0.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_0.0.gltf
@@ -941,7 +941,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_1.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_1.0.gltf
@@ -999,7 +999,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_2.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_2.0.gltf
@@ -1078,7 +1078,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_3.0.obj
+ package://drake_models/allegro_hand_description/meshes/link_3.0.gltf
@@ -1087,7 +1087,7 @@
1 1 1
- package://drake_models/allegro_hand_description/meshes/link_3.0_tip.obj
+ package://drake_models/allegro_hand_description/meshes/link_3.0_tip.gltf
diff --git a/examples/models/box_15cm.sdf b/models/box_15cm.sdf
similarity index 100%
rename from examples/models/box_15cm.sdf
rename to models/box_15cm.sdf
diff --git a/examples/models/box_15cm_hydro.sdf b/models/box_15cm_hydro.sdf
similarity index 100%
rename from examples/models/box_15cm_hydro.sdf
rename to models/box_15cm_hydro.sdf
diff --git a/examples/models/box_intel_nuc.sdf b/models/box_intel_nuc.sdf
similarity index 100%
rename from examples/models/box_intel_nuc.sdf
rename to models/box_intel_nuc.sdf
diff --git a/examples/models/hopper.urdf b/models/hopper.urdf
similarity index 100%
rename from examples/models/hopper.urdf
rename to models/hopper.urdf
diff --git a/examples/models/j2s7s300_arm_hydro_collision.sdf b/models/j2s7s300_arm_hydro_collision.sdf
similarity index 98%
rename from examples/models/j2s7s300_arm_hydro_collision.sdf
rename to models/j2s7s300_arm_hydro_collision.sdf
index c3faaf4..aa4082e 100644
--- a/examples/models/j2s7s300_arm_hydro_collision.sdf
+++ b/models/j2s7s300_arm_hydro_collision.sdf
@@ -27,7 +27,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/base.obj
+ package://drake_models/jaco_description/meshes/base.gltf
@@ -74,7 +74,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/shoulder.obj
+ package://drake_models/jaco_description/meshes/shoulder.gltf
@@ -106,7 +106,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_big.obj
+ package://drake_models/jaco_description/meshes/ring_big.gltf
@@ -173,7 +173,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/arm_half_1.obj
+ package://drake_models/jaco_description/meshes/arm_half_1.gltf
@@ -240,7 +240,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/arm_half_2.obj
+ package://drake_models/jaco_description/meshes/arm_half_2.gltf
@@ -272,7 +272,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_big.obj
+ package://drake_models/jaco_description/meshes/ring_big.gltf
@@ -339,7 +339,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/forearm.obj
+ package://drake_models/jaco_description/meshes/forearm.gltf
@@ -371,7 +371,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_small.obj
+ package://drake_models/jaco_description/meshes/ring_small.gltf
@@ -438,7 +438,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/wrist_spherical_1.obj
+ package://drake_models/jaco_description/meshes/wrist_spherical_1.gltf
@@ -470,7 +470,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_small.obj
+ package://drake_models/jaco_description/meshes/ring_small.gltf
@@ -537,7 +537,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/wrist_spherical_2.obj
+ package://drake_models/jaco_description/meshes/wrist_spherical_2.gltf
@@ -569,7 +569,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_small.obj
+ package://drake_models/jaco_description/meshes/ring_small.gltf
diff --git a/examples/models/j2s7s300_arm_sphere_collision_v2.sdf b/models/j2s7s300_arm_sphere_collision_v2.sdf
similarity index 98%
rename from examples/models/j2s7s300_arm_sphere_collision_v2.sdf
rename to models/j2s7s300_arm_sphere_collision_v2.sdf
index 8636e5f..01d774d 100644
--- a/examples/models/j2s7s300_arm_sphere_collision_v2.sdf
+++ b/models/j2s7s300_arm_sphere_collision_v2.sdf
@@ -27,7 +27,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/base.obj
+ package://drake_models/jaco_description/meshes/base.gltf
@@ -54,7 +54,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/shoulder.obj
+ package://drake_models/jaco_description/meshes/shoulder.gltf
@@ -66,7 +66,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_big.obj
+ package://drake_models/jaco_description/meshes/ring_big.gltf
@@ -113,7 +113,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/arm_half_1.obj
+ package://drake_models/jaco_description/meshes/arm_half_1.gltf
@@ -160,7 +160,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/arm_half_2.obj
+ package://drake_models/jaco_description/meshes/arm_half_2.gltf
@@ -172,7 +172,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_big.obj
+ package://drake_models/jaco_description/meshes/ring_big.gltf
@@ -219,7 +219,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/forearm.obj
+ package://drake_models/jaco_description/meshes/forearm.gltf
@@ -231,7 +231,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_small.obj
+ package://drake_models/jaco_description/meshes/ring_small.gltf
@@ -278,7 +278,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/wrist_spherical_1.obj
+ package://drake_models/jaco_description/meshes/wrist_spherical_1.gltf
@@ -290,7 +290,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_small.obj
+ package://drake_models/jaco_description/meshes/ring_small.gltf
@@ -337,7 +337,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/wrist_spherical_2.obj
+ package://drake_models/jaco_description/meshes/wrist_spherical_2.gltf
@@ -349,7 +349,7 @@ Later edited by hand to:
1 1 1
- package://drake_models/jaco_description/meshes/ring_small.obj
+ package://drake_models/jaco_description/meshes/ring_small.gltf
diff --git a/examples/models/meshes/jaco_coarse/arm_half_1.obj b/models/meshes/jaco_coarse/arm_half_1.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/arm_half_1.obj
rename to models/meshes/jaco_coarse/arm_half_1.obj
diff --git a/examples/models/meshes/jaco_coarse/arm_half_2.obj b/models/meshes/jaco_coarse/arm_half_2.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/arm_half_2.obj
rename to models/meshes/jaco_coarse/arm_half_2.obj
diff --git a/examples/models/meshes/jaco_coarse/base.obj b/models/meshes/jaco_coarse/base.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/base.obj
rename to models/meshes/jaco_coarse/base.obj
diff --git a/examples/models/meshes/jaco_coarse/forearm.obj b/models/meshes/jaco_coarse/forearm.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/forearm.obj
rename to models/meshes/jaco_coarse/forearm.obj
diff --git a/examples/models/meshes/jaco_coarse/jaco_nub.obj b/models/meshes/jaco_coarse/jaco_nub.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/jaco_nub.obj
rename to models/meshes/jaco_coarse/jaco_nub.obj
diff --git a/examples/models/meshes/jaco_coarse/ring_big.obj b/models/meshes/jaco_coarse/ring_big.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/ring_big.obj
rename to models/meshes/jaco_coarse/ring_big.obj
diff --git a/examples/models/meshes/jaco_coarse/ring_small.obj b/models/meshes/jaco_coarse/ring_small.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/ring_small.obj
rename to models/meshes/jaco_coarse/ring_small.obj
diff --git a/examples/models/meshes/jaco_coarse/shoulder.obj b/models/meshes/jaco_coarse/shoulder.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/shoulder.obj
rename to models/meshes/jaco_coarse/shoulder.obj
diff --git a/examples/models/meshes/jaco_coarse/wrist_spherical_1.obj b/models/meshes/jaco_coarse/wrist_spherical_1.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/wrist_spherical_1.obj
rename to models/meshes/jaco_coarse/wrist_spherical_1.obj
diff --git a/examples/models/meshes/jaco_coarse/wrist_spherical_2.obj b/models/meshes/jaco_coarse/wrist_spherical_2.obj
similarity index 100%
rename from examples/models/meshes/jaco_coarse/wrist_spherical_2.obj
rename to models/meshes/jaco_coarse/wrist_spherical_2.obj
diff --git a/examples/models/meshes/jaco_nub.obj b/models/meshes/jaco_nub.obj
similarity index 100%
rename from examples/models/meshes/jaco_nub.obj
rename to models/meshes/jaco_nub.obj
diff --git a/examples/models/meshes/mini_abad.obj b/models/meshes/mini_abad.obj
similarity index 100%
rename from examples/models/meshes/mini_abad.obj
rename to models/meshes/mini_abad.obj
diff --git a/examples/models/meshes/mini_body.obj b/models/meshes/mini_body.obj
similarity index 100%
rename from examples/models/meshes/mini_body.obj
rename to models/meshes/mini_body.obj
diff --git a/examples/models/meshes/mini_lower_link.obj b/models/meshes/mini_lower_link.obj
similarity index 100%
rename from examples/models/meshes/mini_lower_link.obj
rename to models/meshes/mini_lower_link.obj
diff --git a/examples/models/meshes/mini_lower_link_.stl b/models/meshes/mini_lower_link_.stl
similarity index 100%
rename from examples/models/meshes/mini_lower_link_.stl
rename to models/meshes/mini_lower_link_.stl
diff --git a/examples/models/meshes/mini_upper_link.obj b/models/meshes/mini_upper_link.obj
similarity index 100%
rename from examples/models/meshes/mini_upper_link.obj
rename to models/meshes/mini_upper_link.obj
diff --git a/examples/models/mini_cheetah_mesh.urdf b/models/mini_cheetah_mesh.urdf
similarity index 100%
rename from examples/models/mini_cheetah_mesh.urdf
rename to models/mini_cheetah_mesh.urdf
diff --git a/examples/models/mini_cheetah_simple_v2.urdf b/models/mini_cheetah_simple_v2.urdf
similarity index 100%
rename from examples/models/mini_cheetah_simple_v2.urdf
rename to models/mini_cheetah_simple_v2.urdf
diff --git a/examples/models/mini_cheetah_with_ground.urdf b/models/mini_cheetah_with_ground.urdf
similarity index 100%
rename from examples/models/mini_cheetah_with_ground.urdf
rename to models/mini_cheetah_with_ground.urdf
diff --git a/examples/models/punyoid.sdf b/models/punyoid.sdf
similarity index 100%
rename from examples/models/punyoid.sdf
rename to models/punyoid.sdf
diff --git a/examples/models/spinner_capsule.urdf b/models/spinner_capsule.urdf
similarity index 100%
rename from examples/models/spinner_capsule.urdf
rename to models/spinner_capsule.urdf
diff --git a/examples/models/spinner_friction.urdf b/models/spinner_friction.urdf
similarity index 100%
rename from examples/models/spinner_friction.urdf
rename to models/spinner_friction.urdf
diff --git a/examples/models/spinner_rectangle.urdf b/models/spinner_rectangle.urdf
similarity index 100%
rename from examples/models/spinner_rectangle.urdf
rename to models/spinner_rectangle.urdf
diff --git a/examples/models/spinner_sphere.urdf b/models/spinner_sphere.urdf
similarity index 100%
rename from examples/models/spinner_sphere.urdf
rename to models/spinner_sphere.urdf
diff --git a/examples/models/spinner_square.urdf b/models/spinner_square.urdf
similarity index 100%
rename from examples/models/spinner_square.urdf
rename to models/spinner_square.urdf
diff --git a/examples/models/wall_ball.urdf b/models/wall_ball.urdf
similarity index 100%
rename from examples/models/wall_ball.urdf
rename to models/wall_ball.urdf
diff --git a/optimizer/CMakeLists.txt b/optimizer/CMakeLists.txt
new file mode 100644
index 0000000..ab3d236
--- /dev/null
+++ b/optimizer/CMakeLists.txt
@@ -0,0 +1,108 @@
+include_directories(${PROJECT_SOURCE_DIR})
+
+find_package(OpenMP)
+
+add_library(convergence_criteria_tolerances
+ INTERFACE
+ convergence_criteria_tolerances.h)
+
+add_library(inverse_dynamics_partials
+ inverse_dynamics_partials.cc
+ inverse_dynamics_partials.h)
+target_link_libraries(inverse_dynamics_partials drake::drake)
+
+add_library(penta_diagonal_matrix
+ penta_diagonal_matrix.cc
+ penta_diagonal_matrix.h)
+target_link_libraries(penta_diagonal_matrix drake::drake)
+
+add_library(penta_diagonal_solver
+ INTERFACE
+ penta_diagonal_solver.h)
+
+add_library(problem_definition
+ INTERFACE
+ problem_definition.h)
+
+add_library(solver_parameters
+ INTERFACE
+ solver_parameters.h)
+
+add_library(trajectory_optimizer_solution
+ trajectory_optimizer_solution.cc
+ trajectory_optimizer_solution.h)
+target_link_libraries(trajectory_optimizer_solution drake::drake)
+
+add_library(trajectory_optimizer_state
+ trajectory_optimizer_state.cc
+ trajectory_optimizer_state.h)
+target_link_libraries(trajectory_optimizer_state
+ drake::drake
+ inverse_dynamics_partials
+ penta_diagonal_matrix
+ trajectory_optimizer_workspace
+ velocity_partials)
+
+add_library(trajectory_optimizer_workspace
+ trajectory_optimizer_workspace.cc
+ trajectory_optimizer_workspace.h)
+target_link_libraries(trajectory_optimizer_workspace drake::drake)
+
+add_library(trajectory_optimizer
+ trajectory_optimizer.cc
+ trajectory_optimizer.h)
+if (OpenMP_CXX_FOUND)
+ target_link_libraries(trajectory_optimizer
+ inverse_dynamics_partials
+ penta_diagonal_matrix
+ penta_diagonal_solver
+ problem_definition
+ solver_parameters
+ trajectory_optimizer_solution
+ trajectory_optimizer_state
+ trajectory_optimizer_workspace
+ velocity_partials
+ warm_start
+ drake::drake
+ OpenMP::OpenMP_CXX)
+else()
+ message(WARNING "OpenMP not found. Trajectory optimizer will not be parallelized.")
+ target_link_libraries(trajectory_optimizer
+ inverse_dynamics_partials
+ penta_diagonal_matrix
+ penta_diagonal_solver
+ problem_definition
+ solver_parameters
+ trajectory_optimizer_solution
+ trajectory_optimizer_state
+ trajectory_optimizer_workspace
+ velocity_partials
+ warm_start
+ drake::drake)
+endif()
+
+
+add_library(velocity_partials velocity_partials.cc velocity_partials.h)
+target_link_libraries(velocity_partials drake::drake)
+
+add_library(warm_start INTERFACE warm_start.h)
+
+add_executable(penta_diagonal_solver_test test/penta_diagonal_solver_test.cc)
+target_include_directories(penta_diagonal_solver_test PRIVATE
+ "${CMAKE_CURRENT_SOURCE_DIR}" "${PROJECT_SOURCE_DIR}")
+target_link_libraries(penta_diagonal_solver_test
+ penta_diagonal_matrix
+ penta_diagonal_solver
+ drake::drake
+ gtest)
+add_test(NAME penta_diagonal_solver_test COMMAND penta_diagonal_solver_test)
+
+add_executable(trajectory_optimizer_test test/trajectory_optimizer_test.cc)
+target_include_directories(trajectory_optimizer_test PRIVATE
+ "${CMAKE_CURRENT_SOURCE_DIR}" "${PROJECT_SOURCE_DIR}")
+target_link_libraries(trajectory_optimizer_test
+ trajectory_optimizer
+ find_resource
+ drake::drake
+ gtest)
+add_test(NAME trajectory_optimizer_test COMMAND trajectory_optimizer_test)
\ No newline at end of file
diff --git a/optimizer/test/penta_diagonal_solver_test.cc b/optimizer/test/penta_diagonal_solver_test.cc
index fb85b86..a775453 100644
--- a/optimizer/test/penta_diagonal_solver_test.cc
+++ b/optimizer/test/penta_diagonal_solver_test.cc
@@ -1,11 +1,11 @@
#include "optimizer/penta_diagonal_solver.h"
+#include "utils/eigen_matrix_compare.h"
#include
#include
#include "optimizer/penta_diagonal_matrix.h"
#include
-#include
#include
using std::chrono::steady_clock;
@@ -373,3 +373,8 @@ GTEST_TEST(PentaDiagonalMatrixTest, ScaleByDiagonal) {
} // namespace internal
} // namespace optimizer
} // namespace idto
+
+int main(int argc, char** argv) {
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/optimizer/test/trajectory_optimizer_test.cc b/optimizer/test/trajectory_optimizer_test.cc
index d72f7f3..a0fcc2c 100644
--- a/optimizer/test/trajectory_optimizer_test.cc
+++ b/optimizer/test/trajectory_optimizer_test.cc
@@ -2,7 +2,6 @@
#include
-#include
#include "optimizer/inverse_dynamics_partials.h"
#include "optimizer/penta_diagonal_matrix.h"
#include "optimizer/penta_diagonal_solver.h"
@@ -10,21 +9,23 @@
#include "optimizer/trajectory_optimizer_state.h"
#include "optimizer/trajectory_optimizer_workspace.h"
#include "optimizer/velocity_partials.h"
-
+#include "utils/eigen_matrix_compare.h"
+#include "utils/find_resource.h"
#include
-#include
-#include
#include
#include
#include
#include
#include
-#include "utils/find_resource.h"
+#include
#define PRINT_VAR(a) std::cout << #a ": " << a << std::endl;
#define PRINT_VARn(a) std::cout << #a ":\n" << a << std::endl;
namespace idto {
+
+using utils::FindIdtoResource;
+
namespace optimizer {
class TrajectoryOptimizerTester {
@@ -101,7 +102,6 @@ using drake::multibody::Parser;
using drake::multibody::PlanarJoint;
using drake::multibody::RigidBody;
using drake::systems::DiagramBuilder;
-using drake::test::LimitMalloc;
using Eigen::Matrix2d;
using Eigen::Matrix3d;
using Eigen::MatrixXd;
@@ -142,7 +142,6 @@ GTEST_TEST(TrajectoryOptimizerTest, QuaternionDofs) {
opt_prob.q_nom.resize(num_steps + 1, VectorXd::Zero(nq));
opt_prob.v_nom.resize(num_steps + 1, VectorXd::Zero(nv));
-
TrajectoryOptimizer optimizer(diagram.get(), &plant, opt_prob);
TrajectoryOptimizerState state = optimizer.CreateState();
@@ -188,8 +187,7 @@ GTEST_TEST(TrajectoryOptimizerTest, ContactGradientMethods) {
config.time_step = 1.0;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- Parser(&plant).AddModels(idto::FindIdtoResourceOrThrow(
- "idto/examples/models/spinner_sphere.urdf"));
+ Parser(&plant).AddModels(FindIdtoResource("idto/models/spinner_sphere.urdf"));
plant.Finalize();
auto diagram = builder.Build();
@@ -521,8 +519,8 @@ GTEST_TEST(TrajectoryOptimizerTest, HessianAcrobot) {
config.time_step = dt;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- const std::string urdf_file = drake::FindResourceOrThrow(
- "drake/multibody/benchmarks/acrobot/acrobot.urdf");
+ const std::string urdf_file =
+ FindIdtoResource("idto/models/acrobot/acrobot.urdf");
Parser(&plant).AddModels(urdf_file);
plant.Finalize();
auto diagram = builder.Build();
@@ -785,10 +783,10 @@ GTEST_TEST(TrajectoryOptimizerTest, CalcGradientKuka) {
config.time_step = dt;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- const std::string urdf_file = drake::FindResourceOrThrow(
- "drake/manipulation/models/iiwa_description/urdf/"
- "iiwa14_no_collision.urdf");
- Parser(&plant).AddModels(urdf_file);
+ std::string url =
+ "package://drake_models/iiwa_description/urdf/"
+ "iiwa14_spheres_collision.urdf";
+ Parser(&plant).AddModelsFromUrl(url);
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"));
plant.Finalize();
auto diagram = builder.Build();
@@ -1260,8 +1258,8 @@ GTEST_TEST(TrajectoryOptimizerTest, CalcCost) {
config.time_step = dt;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- const std::string urdf_file = drake::FindResourceOrThrow(
- "drake/multibody/benchmarks/acrobot/acrobot.urdf");
+ const std::string urdf_file =
+ FindIdtoResource("idto/models/acrobot/acrobot.urdf");
Parser(&plant).AddModels(urdf_file);
plant.Finalize();
auto diagram = builder.Build();
@@ -1403,8 +1401,8 @@ GTEST_TEST(TrajectoryOptimizerTest, CalcVelocities) {
config.time_step = dt;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- const std::string urdf_file = drake::FindResourceOrThrow(
- "drake/multibody/benchmarks/acrobot/acrobot.urdf");
+ const std::string urdf_file =
+ FindIdtoResource("idto/models/acrobot/acrobot.urdf");
Parser(&plant).AddModels(urdf_file);
plant.Finalize();
auto diagram = builder.Build();
@@ -1477,7 +1475,7 @@ GTEST_TEST(TrajectoryOptimizerTest, SpinnerEqualityConstraints) {
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
const std::string urdf_file =
- FindIdtoResourceOrThrow("idto/examples/models/spinner_friction.urdf");
+ FindIdtoResource("idto/models/spinner_friction.urdf");
Parser(&plant).AddModels(urdf_file);
plant.Finalize();
auto diagram = builder.Build();
@@ -1571,8 +1569,8 @@ GTEST_TEST(TrajectoryOptimizerTest, HopperEqualityConstraints) {
config.time_step = dt;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- const std::string urdf_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/hopper.urdf");
+ const std::string urdf_file =
+ idto::FindIdtoResource("idto/models/hopper.urdf");
Parser(&plant).AddModels(urdf_file);
plant.Finalize();
auto diagram = builder.Build();
@@ -1668,8 +1666,7 @@ GTEST_TEST(TrajectoryOptimizerTest, EqualityConstraintsAndScaling) {
config.time_step = dt;
auto [plant, scene_graph] =
drake::multibody::AddMultibodyPlant(config, &builder);
- const std::string urdf_file = idto::FindIdtoResourceOrThrow(
- "idto/examples/models/hopper.urdf");
+ const std::string urdf_file = FindIdtoResource("idto/models/hopper.urdf");
Parser(&plant).AddModels(urdf_file);
plant.Finalize();
auto diagram = builder.Build();
@@ -1832,3 +1829,8 @@ GTEST_TEST(TrajectoryOptimizerTest, UpdateNominalTrajectory) {
} // namespace internal
} // namespace optimizer
} // namespace idto
+
+int main(int argc, char** argv) {
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/optimizer/trajectory_optimizer.cc b/optimizer/trajectory_optimizer.cc
index e0babbf..20af53f 100644
--- a/optimizer/trajectory_optimizer.cc
+++ b/optimizer/trajectory_optimizer.cc
@@ -1054,10 +1054,10 @@ void TrajectoryOptimizer::CalcGradient(
if (t == num_steps() - 1) {
// The terminal cost needs to be handled differently
gt += (v[t + 1] - prob_.v_nom[t + 1]).transpose() * 2 * prob_.Qf_v *
- dvt_dqm[t + 1];
+ dvt_dqm[t + 1];
} else {
- gt += (v[t + 1] - prob_.v_nom[t + 1]).transpose() * 2 * prob_.Qv *
- dt * dvt_dqm[t + 1];
+ gt += (v[t + 1] - prob_.v_nom[t + 1]).transpose() * 2 * prob_.Qv * dt *
+ dvt_dqm[t + 1];
}
// Contribution from control cost
@@ -1073,7 +1073,7 @@ void TrajectoryOptimizer::CalcGradient(
// exist
auto gT = g->tail(nq);
gT = tau[num_steps() - 1].transpose() * 2 * prob_.R * dt *
- dtau_dqp[num_steps() - 1];
+ dtau_dqp[num_steps() - 1];
gT +=
(q[num_steps()] - prob_.q_nom[num_steps()]).transpose() * 2 * prob_.Qf_q;
gT += (v[num_steps()] - prob_.v_nom[num_steps()]).transpose() * 2 *
@@ -1273,7 +1273,7 @@ void TrajectoryOptimizer::CalcEqualityConstraintViolations(
for (int t = 0; t < num_steps(); ++t) {
for (int j = 0; j < num_unactuated_dofs; ++j) {
- (*violations)(t* num_unactuated_dofs + j) = tau[t][unactuated_dofs()[j]];
+ (*violations)(t * num_unactuated_dofs + j) = tau[t][unactuated_dofs()[j]];
}
}
}
@@ -1344,6 +1344,22 @@ const MatrixX& TrajectoryOptimizer::EvalEqualityConstraintJacobian(
return state.cache().constraint_jacobian;
}
+template
+std::unique_ptr TrajectoryOptimizer::CreateWarmStart(
+ const std::vector>&) const {
+ throw std::runtime_error("CreateWarmStart() only supports T=double.");
+}
+
+template <>
+std::unique_ptr TrajectoryOptimizer::CreateWarmStart(
+ const std::vector& q_guess) const {
+ DRAKE_DEMAND(static_cast(q_guess.size()) == num_steps() + 1);
+ DRAKE_DEMAND(static_cast(q_guess[0].size()) == plant().num_positions());
+ return std::make_unique(num_steps(), diagram(), plant(),
+ num_equality_constraints(), q_guess,
+ params().Delta0);
+}
+
template
void TrajectoryOptimizer::CalcLagrangeMultipliers(
const TrajectoryOptimizerState&, VectorX*) const {
@@ -2416,10 +2432,9 @@ SolverFlag TrajectoryOptimizer::SolveWithTrustRegion(
// Allocate a warm start, which includes the initial guess along with state
// variables and the trust region radius.
- WarmStart warm_start(num_steps(), diagram(), plant(),
- num_equality_constraints(), q_guess, params_.Delta0);
+ std::unique_ptr warm_start = CreateWarmStart(q_guess);
- return SolveFromWarmStart(&warm_start, solution, stats, reason_out);
+ return SolveFromWarmStart(warm_start.get(), solution, stats, reason_out);
return SolverFlag::kSuccess;
}
diff --git a/optimizer/trajectory_optimizer.h b/optimizer/trajectory_optimizer.h
index de7c328..df09317 100644
--- a/optimizer/trajectory_optimizer.h
+++ b/optimizer/trajectory_optimizer.h
@@ -115,7 +115,7 @@ class TrajectoryOptimizer {
const ProblemDefinition& prob() const { return prob_; }
/**
- * Create a state object which contains the decision variables (generalized
+ * Create a state object that contains the decision variables (generalized
* positions at each timestep), along with a cache of other things that are
* computed from positions, such as velocities, accelerations, forces, and
* various derivatives.
@@ -128,6 +128,17 @@ class TrajectoryOptimizer {
num_equality_constraints());
}
+ /**
+ * Create a warm-start object that contains the initial guess, trust region
+ * radius, and other state variables for the optimization problem.
+ *
+ * @param q_guess Initial guess of the sequence of generalized positions
+ *
+ * @return WarmStart
+ */
+ std::unique_ptr CreateWarmStart(
+ const std::vector>& q_guess) const;
+
/**
* Compute the gradient of the unconstrained cost L(q).
*
@@ -452,8 +463,6 @@ class TrajectoryOptimizer {
void ResetInitialConditions(const VectorXd& q_init, const VectorXd& v_init) {
DRAKE_DEMAND(q_init.size() == plant().num_positions());
DRAKE_DEMAND(v_init.size() == plant().num_velocities());
- DRAKE_DEMAND(params_.q_nom_relative_to_q_init.size() ==
- plant().num_positions());
prob_.q_init = q_init;
prob_.v_init = v_init;
}
diff --git a/optimizer/velocity_partials.cc b/optimizer/velocity_partials.cc
index 62d7d9e..02ad5eb 100644
--- a/optimizer/velocity_partials.cc
+++ b/optimizer/velocity_partials.cc
@@ -1,4 +1,4 @@
-#include "optimizer/velocity_partials.h"
+#include "velocity_partials.h"
#include
diff --git a/pyidto/BUILD.bazel b/pyidto/BUILD.bazel
deleted file mode 100644
index 73002a3..0000000
--- a/pyidto/BUILD.bazel
+++ /dev/null
@@ -1,138 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-load("//tools/lint:lint.bzl", "add_lint_tests")
-load("@pybind11_bazel//:build_defs.bzl", "pybind_extension")
-
-pybind_extension(
- name = "problem_definition", # This name should match the *.so below
- srcs = ["problem_definition_py.cc"],
- deps = [
- "//optimizer:problem_definition",
- ]
-)
-py_library(
- name = "problem_definition",
- data = [":problem_definition.so"],
-)
-py_test(
- name = "problem_definition_test",
- srcs = ["test/problem_definition_test.py"],
- deps = [
- ":problem_definition"
- ],
-)
-
-pybind_extension(
- name = "solver_parameters",
- srcs = ["solver_parameters_py.cc"],
- deps = [
- "//optimizer:solver_parameters",
- ]
-)
-py_library(
- name = "solver_parameters",
- data = [":solver_parameters.so"],
-)
-py_test(
- name = "solver_parameters_test",
- srcs = ["test/solver_parameters_test.py"],
- deps = [
- ":solver_parameters"
- ],
-)
-
-pybind_extension(
- name="find_idto_resource",
- srcs=["find_idto_resource_py.cc"],
- deps=[
- "//utils:find_resource",
- ]
-)
-py_library(
- name="find_idto_resource",
- data=[":find_idto_resource.so"],
-)
-
-pybind_extension(
- name="trajectory_optimizer_solution",
- srcs=["trajectory_optimizer_solution_py.cc"],
- deps=[
- "//optimizer:trajectory_optimizer_solution",
- ]
-)
-py_library(
- name="trajectory_optimizer_solution",
- data=[":trajectory_optimizer_solution.so"],
-)
-py_test(
- name="trajectory_optimizer_solution_test",
- srcs=["test/trajectory_optimizer_solution_test.py"],
- deps=[
- ":trajectory_optimizer_solution",
- ],
-)
-
-pybind_extension(
- name="trajectory_optimizer_stats",
- srcs=["trajectory_optimizer_stats_py.cc"],
- deps=[
- "//optimizer:trajectory_optimizer_solution",
- ]
-)
-py_library(
- name="trajectory_optimizer_stats",
- data=[":trajectory_optimizer_stats.so"],
-)
-py_test(
- name="trajectory_optimizer_stats_test",
- srcs=["test/trajectory_optimizer_stats_test.py"],
- deps=[
- ":trajectory_optimizer_stats",
- ],
-)
-
-pybind_extension(
- name = "trajectory_optimizer",
- srcs = ["trajectory_optimizer_py.cc"],
- deps = [
- "//optimizer:trajectory_optimizer",
- "@drake//multibody/parsing",
- ]
-)
-py_library(
- name = "trajectory_optimizer",
- data = [":trajectory_optimizer.so"],
-)
-py_test(
- name = "trajectory_optimizer_test",
- srcs = ["test/trajectory_optimizer_test.py"],
- deps = [
- ":trajectory_optimizer",
- ":problem_definition",
- ":solver_parameters",
- ":trajectory_optimizer_solution",
- ":trajectory_optimizer_stats",
- ":find_idto_resource",
- ],
- data = [
- "//examples:models"
- ]
-)
-
-py_test(
- name = "warm_start_test",
- srcs = ["test/warm_start_test.py"],
- deps = [
- ":trajectory_optimizer",
- ":problem_definition",
- ":solver_parameters",
- ":trajectory_optimizer_solution",
- ":trajectory_optimizer_stats",
- ":find_idto_resource",
- ],
- data = [
- "//examples:models"
- ]
-)
-
-add_lint_tests()
diff --git a/pyidto/find_idto_resource_py.cc b/pyidto/find_idto_resource_py.cc
deleted file mode 100644
index 06876f3..0000000
--- a/pyidto/find_idto_resource_py.cc
+++ /dev/null
@@ -1,10 +0,0 @@
-#include "utils/find_resource.h"
-
-#include
-
-namespace py = pybind11;
-using idto::FindIdtoResourceOrThrow;
-
-PYBIND11_MODULE(find_idto_resource, m) {
- m.def("FindIdtoResourceOrThrow", &FindIdtoResourceOrThrow);
-}
diff --git a/pyidto/test/problem_definition_test.py b/pyidto/test/problem_definition_test.py
deleted file mode 100644
index 599b7ac..0000000
--- a/pyidto/test/problem_definition_test.py
+++ /dev/null
@@ -1,38 +0,0 @@
-import numpy as np
-from pyidto.problem_definition import ProblemDefinition
-
-# Construct a problem definition object
-problem_definition = ProblemDefinition()
-
-# Make sure we can set the various parameters
-problem_definition.num_steps = 10
-assert problem_definition.num_steps == 10
-
-problem_definition.q_init = np.array([1, 2, 3])
-assert problem_definition.q_init.shape == (3,)
-
-problem_definition.v_init = np.array([2,3,4,5])
-assert problem_definition.v_init.shape == (4,)
-
-problem_definition.Qq = np.eye(3)
-assert problem_definition.Qq.shape == (3,3)
-
-problem_definition.Qv = np.eye(4)
-assert problem_definition.Qv.shape == (4,4)
-
-problem_definition.Qf_q = np.eye(3)
-assert problem_definition.Qf_q.shape == (3,3)
-
-problem_definition.Qf_v = np.eye(4)
-assert problem_definition.Qf_v.shape == (4,4)
-
-problem_definition.R = np.eye(4)
-assert problem_definition.R.shape == (4,4)
-
-problem_definition.q_nom = [np.array([2,3,4.5]) for i in range(10)]
-assert(len(problem_definition.q_nom) == 10)
-assert(problem_definition.q_nom[0][2] == 4.5)
-
-problem_definition.v_nom = [np.array([2,3,4.5,5.5]) for i in range(10)]
-assert(len(problem_definition.v_nom) == 10)
-assert(problem_definition.v_nom[0][3] == 5.5)
diff --git a/pyidto/test/solver_parameters_test.py b/pyidto/test/solver_parameters_test.py
deleted file mode 100644
index aad0b4d..0000000
--- a/pyidto/test/solver_parameters_test.py
+++ /dev/null
@@ -1,56 +0,0 @@
-import numpy as np
-from pyidto.solver_parameters import SolverParameters
-
-params = SolverParameters()
-
-assert params.max_iterations == 100 # default value
-params.max_iterations = 1
-assert params.max_iterations == 1
-
-assert params.normalize_quaternions == False
-params.normalize_quaternions = True
-assert params.normalize_quaternions == True
-
-assert params.verbose == True
-params.verbose = False
-assert params.verbose == False
-
-assert params.contact_stiffness == 100
-params.contact_stiffness = 1.2
-assert params.contact_stiffness == 1.2
-
-assert params.dissipation_velocity == 0.1
-params.dissipation_velocity = 0.2
-assert params.dissipation_velocity == 0.2
-
-assert params.stiction_velocity == 0.05
-params.stiction_velocity = 0.1
-assert params.stiction_velocity == 0.1
-
-assert params.friction_coefficient == 0.5
-params.friction_coefficient = 0.6
-assert params.friction_coefficient == 0.6
-
-assert params.smoothing_factor == 0.1
-params.smoothing_factor = 0.2
-assert params.smoothing_factor == 0.2
-
-assert params.scaling == True
-params.scaling = False
-assert params.scaling == False
-
-assert params.equality_constraints == True
-params.equality_constraints = False
-assert params.equality_constraints == False
-
-assert params.Delta0 == 1e-1
-params.Delta0 = 1e-2
-assert params.Delta0 == 1e-2
-
-assert params.Delta_max == 1e5
-params.Delta_max = 1e6
-assert params.Delta_max == 1e6
-
-assert params.num_threads == 1
-params.num_threads = 4
-assert params.num_threads == 4
diff --git a/pyidto/test/trajectory_optimizer_solution_test.py b/pyidto/test/trajectory_optimizer_solution_test.py
deleted file mode 100644
index 50b8b26..0000000
--- a/pyidto/test/trajectory_optimizer_solution_test.py
+++ /dev/null
@@ -1,8 +0,0 @@
-from pyidto.trajectory_optimizer_solution import TrajectoryOptimizerSolution
-import numpy as np
-
-solution = TrajectoryOptimizerSolution()
-
-assert len(solution.q) == 0
-assert len(solution.v) == 0
-assert len(solution.tau) == 0
diff --git a/pyidto/test/trajectory_optimizer_stats_test.py b/pyidto/test/trajectory_optimizer_stats_test.py
deleted file mode 100644
index e925814..0000000
--- a/pyidto/test/trajectory_optimizer_stats_test.py
+++ /dev/null
@@ -1,4 +0,0 @@
-from pyidto.trajectory_optimizer_stats import TrajectoryOptimizerStats
-
-stats = TrajectoryOptimizerStats()
-assert stats.is_empty()
diff --git a/pyidto/test/trajectory_optimizer_test.py b/pyidto/test/trajectory_optimizer_test.py
deleted file mode 100644
index cf4324f..0000000
--- a/pyidto/test/trajectory_optimizer_test.py
+++ /dev/null
@@ -1,93 +0,0 @@
-import numpy as np
-
-from pyidto.trajectory_optimizer import TrajectoryOptimizer
-from pyidto.problem_definition import ProblemDefinition
-from pyidto.solver_parameters import SolverParameters
-from pyidto.trajectory_optimizer_solution import TrajectoryOptimizerSolution
-from pyidto.trajectory_optimizer_stats import TrajectoryOptimizerStats
-from pyidto.find_idto_resource import FindIdtoResourceOrThrow
-
-# Get the absolute path to a model file
-model_file = FindIdtoResourceOrThrow(
- "idto/examples/models/spinner_friction.urdf")
-
-# Define the optimization problem
-problem = ProblemDefinition()
-problem.num_steps = 40
-problem.q_init = np.array([0.3, 1.5, 0.0])
-problem.v_init = np.array([0.0, 0.0, 0.0])
-problem.Qq = 1.0 * np.eye(3)
-problem.Qv = 0.1 * np.eye(3)
-problem.R = np.diag([0.1, 0.1, 1e3])
-problem.Qf_q = 10 * np.eye(3)
-problem.Qf_v = 0.1 * np.eye(3)
-
-q_nom = [] # Can't use list comprehension here because of Eigen conversion
-v_nom = []
-for i in range(problem.num_steps + 1):
- q_nom.append(np.array([0.3, 1.5, 2.0]))
- v_nom.append(np.array([0.0, 0.0, 0.0]))
-problem.q_nom = q_nom
-problem.v_nom = v_nom
-
-# Set solver parameters
-params = SolverParameters()
-params.max_iterations = 200
-params.scaling = True
-params.equality_constraints = True
-params.Delta0 = 1e1
-params.Delta_max = 1e5
-params.num_threads = 1
-
-params.contact_stiffness = 200
-params.dissipation_velocity = 0.1
-params.smoothing_factor = 0.01
-params.friction_coefficient = 0.5
-params.stiction_velocity = 0.05
-
-params.verbose = False
-
-# Define an initial guess
-q_guess = []
-for i in range(problem.num_steps + 1):
- q_guess.append(np.array([0.3, 1.5, 0.0]))
-
-# Create the optimizer object
-time_step = 0.05
-opt = TrajectoryOptimizer(model_file, problem, params, time_step)
-
-assert opt.time_step() == time_step
-assert opt.num_steps() == problem.num_steps
-
-# Solve the optimization problem
-solution = TrajectoryOptimizerSolution()
-stats = TrajectoryOptimizerStats()
-opt.Solve(q_guess, solution, stats)
-
-solve_time = np.sum(stats.iteration_times)
-print("Solved in ", solve_time, "seconds")
-
-assert len(solution.q) == problem.num_steps + 1
-expected_qN = np.array([0.287, 1.497, 1.995]) # from CPP version
-assert np.linalg.norm(solution.q[-1]-expected_qN) < 1e-3
-
-# Get the problem definition
-problem = opt.prob()
-assert problem.num_steps == 40
-
-# Get the solver parameters
-params = opt.params()
-assert params.max_iterations == 200
-
-# Make sure we can change the nominal trajectory
-assert np.all(problem.q_nom[10] == np.array([0.3, 1.5, 2.0]))
-assert np.all(problem.v_nom[10] == np.array([0.0, 0.0, 0.0]))
-new_q_nom = problem.q_nom.copy()
-new_v_nom = problem.v_nom.copy()
-new_q_nom[10] = np.array([0.4, 1.6, 2.1])
-new_v_nom[10] = np.array([0.1, 0.1, 0.1])
-
-opt.UpdateNominalTrajectory(new_q_nom, new_v_nom)
-problem = opt.prob()
-assert np.all(problem.q_nom[10] == np.array([0.4, 1.6, 2.1]))
-assert np.all(problem.v_nom[10] == np.array([0.1, 0.1, 0.1]))
diff --git a/pyidto/trajectory_optimizer_py.cc b/pyidto/trajectory_optimizer_py.cc
deleted file mode 100644
index a19165e..0000000
--- a/pyidto/trajectory_optimizer_py.cc
+++ /dev/null
@@ -1,128 +0,0 @@
-#include
-
-#include "optimizer/trajectory_optimizer.h"
-#include "optimizer/warm_start.h"
-#include
-#include
-#include
-#include
-#include
-#include
-
-namespace py = pybind11;
-
-using drake::geometry::SceneGraph;
-using drake::multibody::AddMultibodyPlant;
-using drake::multibody::MultibodyPlant;
-using drake::multibody::MultibodyPlantConfig;
-using drake::multibody::Parser;
-using drake::systems::Diagram;
-using drake::systems::DiagramBuilder;
-using Eigen::VectorXd;
-using idto::optimizer::ProblemDefinition;
-using idto::optimizer::SolverParameters;
-using idto::optimizer::TrajectoryOptimizer;
-using idto::optimizer::TrajectoryOptimizerSolution;
-using idto::optimizer::TrajectoryOptimizerStats;
-using idto::optimizer::WarmStart;
-
-/**
- * A python wrapper class for TrajectoryOptimizer. Has access to a limited
- * set of methods, and is initialized from a URDF or SDF file instead of a
- * Drake diagram and MultibodyPlant.
- */
-class TrajectoryOptimizerPy {
- public:
- TrajectoryOptimizerPy(const std::string& model_file,
- const ProblemDefinition& problem,
- const SolverParameters& params,
- const double time_step) {
- DiagramBuilder builder;
-
- MultibodyPlantConfig config;
- config.time_step = time_step;
- std::tie(plant_, scene_graph_) = AddMultibodyPlant(config, &builder);
- Parser(plant_).AddModels(model_file);
- plant_->Finalize();
- diagram_ = builder.Build();
-
- // For python we create a new set of parameters, where
- // q_nom_relative_to_q_init is false for all DoFs.
- SolverParameters py_params = params;
- py_params.q_nom_relative_to_q_init =
- drake::VectorX::Zero(plant_->num_positions());
-
- optimizer_ = std::make_unique>(
- diagram_.get(), plant_, problem, py_params);
- }
-
- void Solve(const std::vector& q_guess,
- TrajectoryOptimizerSolution* solution,
- TrajectoryOptimizerStats* stats) {
- optimizer_->Solve(q_guess, solution, stats);
- }
-
- void SolveFromWarmStart(WarmStart* warm_start,
- TrajectoryOptimizerSolution* solution,
- TrajectoryOptimizerStats* stats) {
- optimizer_->SolveFromWarmStart(warm_start, solution, stats);
- }
-
- std::unique_ptr MakeWarmStart(
- const std::vector& q_guess) const {
- return std::make_unique(
- optimizer_->num_steps(), optimizer_->diagram(), optimizer_->plant(),
- optimizer_->num_equality_constraints(), q_guess,
- optimizer_->params().Delta0);
- }
-
- void ResetInitialConditions(const VectorXd& q0, const VectorXd& v0) {
- optimizer_->ResetInitialConditions(q0, v0);
- }
-
- void UpdateNominalTrajectory(const std::vector& q_nom,
- const std::vector& v_nom) {
- optimizer_->UpdateNominalTrajectory(q_nom, v_nom);
- }
-
- const SolverParameters& params() const { return optimizer_->params(); }
-
- const ProblemDefinition& prob() const { return optimizer_->prob(); }
-
- double time_step() const { return optimizer_->time_step(); }
-
- int num_steps() const { return optimizer_->num_steps(); }
-
- private:
- // Plant and scene graph are owned by the diagram
- MultibodyPlant* plant_{nullptr};
- SceneGraph* scene_graph_{nullptr};
-
- std::unique_ptr> diagram_;
- std::unique_ptr> optimizer_;
-};
-
-PYBIND11_MODULE(trajectory_optimizer, m) {
- py::class_(m, "TrajectoryOptimizer")
- .def(py::init())
- .def("time_step", &TrajectoryOptimizerPy::time_step)
- .def("num_steps", &TrajectoryOptimizerPy::num_steps)
- .def("Solve", &TrajectoryOptimizerPy::Solve)
- .def("SolveFromWarmStart", &TrajectoryOptimizerPy::SolveFromWarmStart)
- .def("MakeWarmStart", &TrajectoryOptimizerPy::MakeWarmStart)
- .def("ResetInitialConditions",
- &TrajectoryOptimizerPy::ResetInitialConditions)
- .def("UpdateNominalTrajectory",
- &TrajectoryOptimizerPy::UpdateNominalTrajectory)
- .def("params", &TrajectoryOptimizerPy::params)
- .def("prob", &TrajectoryOptimizerPy::prob);
- py::class_(m, "WarmStart")
- // Warm start is not default constructible: it should be created
- // in python using the TrajectoryOptimizer.MakeWarmStart method.
- .def("set_q", &WarmStart::set_q)
- .def("get_q", &WarmStart::get_q)
- .def_readonly("Delta", &WarmStart::Delta)
- .def_readonly("dq", &WarmStart::dq)
- .def_readonly("dqH", &WarmStart::dqH);
-}
diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 0000000..830f76a
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,25 @@
+[build-system]
+requires = [
+ "setuptools>=42",
+ "wheel",
+ "ninja",
+ "cmake>=3.12",
+]
+build-backend = "setuptools.build_meta"
+
+[tool.mypy]
+files = "setup.py"
+python_version = "3.7"
+strict = true
+show_error_codes = true
+enable_error_code = ["ignore-without-code", "redundant-expr", "truthy-bool"]
+warn_unreachable = true
+
+[[tool.mypy.overrides]]
+module = ["ninja"]
+ignore_missing_imports = true
+
+[tool.pytest.ini_options]
+testpaths = [
+ "python_bindings/test",
+]
diff --git a/python_bindings/CMakeLists.txt b/python_bindings/CMakeLists.txt
new file mode 100644
index 0000000..00ed4e3
--- /dev/null
+++ b/python_bindings/CMakeLists.txt
@@ -0,0 +1,32 @@
+find_package(pybind11 CONFIG REQUIRED)
+
+include_directories(${PROJECT_SOURCE_DIR})
+
+pybind11_add_module(pyidto MODULE pyidto.cc
+ problem_definition_py.cc
+ solver_parameters_py.cc
+ trajectory_optimizer_py.cc
+ trajectory_optimizer_solution_py.cc
+ trajectory_optimizer_stats_py.cc
+ find_resource_py.cc)
+target_link_libraries(pyidto PUBLIC
+ trajectory_optimizer drake::drake find_resource)
+set_target_properties(pyidto PROPERTIES CXX_VISIBILITY_PRESET default)
+
+target_compile_definitions(pyidto
+ PRIVATE VERSION_INFO=${EXAMPLE_VERSION_INFO})
+
+add_test(NAME problem_definition_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/problem_definition_test.py)
+add_test(NAME solver_parameters_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/solver_parameters_test.py)
+add_test(NAME trajectory_optimizer_solution_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/trajectory_optimizer_solution_test.py)
+add_test(NAME trajectory_optimizer_stats_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/trajectory_optimizer_stats_test.py)
+add_test(NAME find_resource_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/find_resource_test.py)
+add_test(NAME trajectory_optimizer_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/trajectory_optimizer_test.py)
+add_test(NAME warm_start_test_py COMMAND ${PYTHON_EXECUTABLE} -m pytest
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/warm_start_test.py)
diff --git a/python_bindings/find_resource_py.cc b/python_bindings/find_resource_py.cc
new file mode 100644
index 0000000..acdd1bd
--- /dev/null
+++ b/python_bindings/find_resource_py.cc
@@ -0,0 +1,9 @@
+#include "utils/find_resource.h"
+#include
+
+namespace py = pybind11;
+using idto::utils::FindIdtoResource;
+
+void bind_find_resource(py::module_& m) {
+ m.def("FindIdtoResource", &FindIdtoResource);
+}
\ No newline at end of file
diff --git a/pyidto/problem_definition_py.cc b/python_bindings/problem_definition_py.cc
similarity index 98%
rename from pyidto/problem_definition_py.cc
rename to python_bindings/problem_definition_py.cc
index f802361..c4739c8 100644
--- a/pyidto/problem_definition_py.cc
+++ b/python_bindings/problem_definition_py.cc
@@ -6,7 +6,7 @@
namespace py = pybind11;
using idto::optimizer::ProblemDefinition;
-PYBIND11_MODULE(problem_definition, m) {
+void bind_problem_definition(py::module_& m) {
py::class_(m, "ProblemDefinition")
.def(py::init<>())
.def_readwrite("num_steps", &ProblemDefinition::num_steps)
diff --git a/python_bindings/pyidto.cc b/python_bindings/pyidto.cc
new file mode 100644
index 0000000..4c2e8f0
--- /dev/null
+++ b/python_bindings/pyidto.cc
@@ -0,0 +1,23 @@
+#include
+
+namespace py = pybind11;
+
+void bind_problem_definition(py::module_&);
+void bind_solver_parameters(py::module_&);
+void bind_trajectory_optimizer(py::module_&);
+void bind_trajectory_optimizer_solution(py::module_&);
+void bind_trajectory_optimizer_stats(py::module_&);
+void bind_find_resource(py::module_&);
+
+PYBIND11_MODULE(pyidto, m) {
+ m.doc() = "Inverse Dynamics Trajectory Optimization (IDTO) python bindings.";
+
+ py::module::import("pydrake.multibody.plant");
+
+ bind_problem_definition(m);
+ bind_solver_parameters(m);
+ bind_trajectory_optimizer(m);
+ bind_trajectory_optimizer_solution(m);
+ bind_trajectory_optimizer_stats(m);
+ bind_find_resource(m);
+}
diff --git a/pyidto/solver_parameters_py.cc b/python_bindings/solver_parameters_py.cc
similarity index 96%
rename from pyidto/solver_parameters_py.cc
rename to python_bindings/solver_parameters_py.cc
index 6416c46..37e9608 100644
--- a/pyidto/solver_parameters_py.cc
+++ b/python_bindings/solver_parameters_py.cc
@@ -6,7 +6,7 @@
namespace py = pybind11;
using idto::optimizer::SolverParameters;
-PYBIND11_MODULE(solver_parameters, m) {
+void bind_solver_parameters(py::module_& m) {
py::class_(m, "SolverParameters")
.def(py::init<>())
.def_readwrite("max_iterations", &SolverParameters::max_iterations)
diff --git a/python_bindings/test/find_resource_test.py b/python_bindings/test/find_resource_test.py
new file mode 100644
index 0000000..2ebae2f
--- /dev/null
+++ b/python_bindings/test/find_resource_test.py
@@ -0,0 +1,16 @@
+import os
+
+import pytest
+from pyidto import FindIdtoResource
+
+
+def test_find_resource():
+ """Test the FindResource function."""
+ with pytest.raises(RuntimeError):
+ FindIdtoResource("non_existent_file")
+
+ spinner_urdf = FindIdtoResource("idto/models/hopper.urdf")
+ assert os.path.isfile(spinner_urdf)
+
+ nonexistent = FindIdtoResource("idto/non_existent_file")
+ assert not os.path.isfile(nonexistent)
diff --git a/python_bindings/test/problem_definition_test.py b/python_bindings/test/problem_definition_test.py
new file mode 100644
index 0000000..49fa8ee
--- /dev/null
+++ b/python_bindings/test/problem_definition_test.py
@@ -0,0 +1,41 @@
+import numpy as np
+from pyidto import ProblemDefinition
+
+
+def test_problem_definition():
+ """Smoke test for an IDTO problem definition."""
+ # Construct a problem definition object
+ problem_definition = ProblemDefinition()
+
+ # Make sure we can set the various parameters
+ problem_definition.num_steps = 10
+ assert problem_definition.num_steps == 10
+
+ problem_definition.q_init = np.array([1, 2, 3])
+ assert problem_definition.q_init.shape == (3,)
+
+ problem_definition.v_init = np.array([2, 3, 4, 5])
+ assert problem_definition.v_init.shape == (4,)
+
+ problem_definition.Qq = np.eye(3)
+ assert problem_definition.Qq.shape == (3, 3)
+
+ problem_definition.Qv = np.eye(4)
+ assert problem_definition.Qv.shape == (4, 4)
+
+ problem_definition.Qf_q = np.eye(3)
+ assert problem_definition.Qf_q.shape == (3, 3)
+
+ problem_definition.Qf_v = np.eye(4)
+ assert problem_definition.Qf_v.shape == (4, 4)
+
+ problem_definition.R = np.eye(4)
+ assert problem_definition.R.shape == (4, 4)
+
+ problem_definition.q_nom = [np.array([2, 3, 4.5]) for i in range(10)]
+ assert (len(problem_definition.q_nom) == 10)
+ assert (problem_definition.q_nom[0][2] == 4.5)
+
+ problem_definition.v_nom = [np.array([2, 3, 4.5, 5.5]) for i in range(10)]
+ assert (len(problem_definition.v_nom) == 10)
+ assert (problem_definition.v_nom[0][3] == 5.5)
diff --git a/python_bindings/test/solver_parameters_test.py b/python_bindings/test/solver_parameters_test.py
new file mode 100644
index 0000000..5798bb8
--- /dev/null
+++ b/python_bindings/test/solver_parameters_test.py
@@ -0,0 +1,58 @@
+from pyidto import SolverParameters
+
+
+def test_solver_parameters():
+ """Smoke test for IDTO solver parameters."""
+ params = SolverParameters()
+
+ assert params.max_iterations == 100 # default value
+ params.max_iterations = 1
+ assert params.max_iterations == 1
+
+ assert params.normalize_quaternions == False
+ params.normalize_quaternions = True
+ assert params.normalize_quaternions == True
+
+ assert params.verbose == True
+ params.verbose = False
+ assert params.verbose == False
+
+ assert params.contact_stiffness == 100
+ params.contact_stiffness = 1.2
+ assert params.contact_stiffness == 1.2
+
+ assert params.dissipation_velocity == 0.1
+ params.dissipation_velocity = 0.2
+ assert params.dissipation_velocity == 0.2
+
+ assert params.stiction_velocity == 0.05
+ params.stiction_velocity = 0.1
+ assert params.stiction_velocity == 0.1
+
+ assert params.friction_coefficient == 0.5
+ params.friction_coefficient = 0.6
+ assert params.friction_coefficient == 0.6
+
+ assert params.smoothing_factor == 0.1
+ params.smoothing_factor = 0.2
+ assert params.smoothing_factor == 0.2
+
+ assert params.scaling == True
+ params.scaling = False
+ assert params.scaling == False
+
+ assert params.equality_constraints == True
+ params.equality_constraints = False
+ assert params.equality_constraints == False
+
+ assert params.Delta0 == 1e-1
+ params.Delta0 = 1e-2
+ assert params.Delta0 == 1e-2
+
+ assert params.Delta_max == 1e5
+ params.Delta_max = 1e6
+ assert params.Delta_max == 1e6
+
+ assert params.num_threads == 1
+ params.num_threads = 4
+ assert params.num_threads == 4
diff --git a/python_bindings/test/trajectory_optimizer_solution_test.py b/python_bindings/test/trajectory_optimizer_solution_test.py
new file mode 100644
index 0000000..67ab587
--- /dev/null
+++ b/python_bindings/test/trajectory_optimizer_solution_test.py
@@ -0,0 +1,15 @@
+import pytest
+from pyidto import TrajectoryOptimizerSolution
+
+
+def test_optimizer_solution():
+ """Smoke test for an IDTO optimizer solution."""
+ solution = TrajectoryOptimizerSolution()
+
+ assert len(solution.q) == 0
+ assert len(solution.v) == 0
+ assert len(solution.tau) == 0
+
+ # We cannot directly assign to the solution
+ with pytest.raises(AttributeError):
+ solution.q = [1, 2, 3]
diff --git a/python_bindings/test/trajectory_optimizer_stats_test.py b/python_bindings/test/trajectory_optimizer_stats_test.py
new file mode 100644
index 0000000..512f983
--- /dev/null
+++ b/python_bindings/test/trajectory_optimizer_stats_test.py
@@ -0,0 +1,7 @@
+from pyidto import TrajectoryOptimizerStats
+
+
+def test_optimizer_stats():
+ """Smoke test for an IDTO optimizer stats."""
+ stats = TrajectoryOptimizerStats()
+ assert stats.is_empty()
diff --git a/python_bindings/test/trajectory_optimizer_test.py b/python_bindings/test/trajectory_optimizer_test.py
new file mode 100644
index 0000000..690ebed
--- /dev/null
+++ b/python_bindings/test/trajectory_optimizer_test.py
@@ -0,0 +1,106 @@
+import numpy as np
+
+from pyidto import (
+ TrajectoryOptimizer,
+ ProblemDefinition,
+ SolverParameters,
+ TrajectoryOptimizerSolution,
+ TrajectoryOptimizerStats,
+ FindIdtoResource
+)
+
+from pydrake.systems.framework import DiagramBuilder
+from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
+from pydrake.multibody.parsing import Parser
+
+
+def test_optimizer():
+ """Test that we can solve a small optimization problem."""
+ # Define the system model
+ time_step = 0.05
+ model_file = FindIdtoResource("idto/models/spinner_friction.urdf")
+ builder = DiagramBuilder()
+ plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+
+ # Define the optimization problem
+ problem = ProblemDefinition()
+ problem.num_steps = 40
+ problem.q_init = np.array([0.3, 1.5, 0.0])
+ problem.v_init = np.array([0.0, 0.0, 0.0])
+ problem.Qq = 1.0 * np.eye(3)
+ problem.Qv = 0.1 * np.eye(3)
+ problem.R = np.diag([0.1, 0.1, 1e3])
+ problem.Qf_q = 10 * np.eye(3)
+ problem.Qf_v = 0.1 * np.eye(3)
+
+ q_nom = [] # Can't use list comprehension here because of Eigen conversion
+ v_nom = []
+ for i in range(problem.num_steps + 1):
+ q_nom.append(np.array([0.3, 1.5, 2.0]))
+ v_nom.append(np.array([0.0, 0.0, 0.0]))
+ problem.q_nom = q_nom
+ problem.v_nom = v_nom
+ assert len(problem.q_nom) == problem.num_steps + 1
+ assert len(problem.v_nom) == problem.num_steps + 1
+
+ # Set solver parameters
+ params = SolverParameters()
+ params.max_iterations = 200
+ params.scaling = True
+ params.equality_constraints = True
+ params.Delta0 = 1e1
+ params.Delta_max = 1e5
+ params.num_threads = 1
+
+ params.contact_stiffness = 200
+ params.dissipation_velocity = 0.1
+ params.smoothing_factor = 0.01
+ params.friction_coefficient = 0.5
+ params.stiction_velocity = 0.05
+
+ params.verbose = True
+
+ # Define an initial guess
+ q_guess = []
+ for i in range(problem.num_steps + 1):
+ q_guess.append(np.array([0.3, 1.5, 0.0]))
+
+ # Create the optimizer object
+ opt = TrajectoryOptimizer(diagram, plant, problem, params)
+ assert opt.time_step() == time_step
+ assert opt.num_steps() == problem.num_steps
+
+ # Solve the optimization problem
+ solution = TrajectoryOptimizerSolution()
+ stats = TrajectoryOptimizerStats()
+ opt.Solve(q_guess, solution, stats)
+ solve_time = np.sum(stats.iteration_times)
+ print("Solved in ", solve_time, "seconds")
+
+ assert len(solution.q) == problem.num_steps + 1
+ expected_qN = np.array([0.287, 1.497, 1.995]) # from CPP version
+ assert np.linalg.norm(solution.q[-1]-expected_qN) < 1e-3
+
+ # Get the problem definition
+ problem = opt.prob()
+ assert problem.num_steps == 40
+
+ # Get the solver parameters
+ params = opt.params()
+ assert params.max_iterations == 200
+
+ # Make sure we can change the nominal trajectory
+ assert np.all(problem.q_nom[10] == np.array([0.3, 1.5, 2.0]))
+ assert np.all(problem.v_nom[10] == np.array([0.0, 0.0, 0.0]))
+ new_q_nom = problem.q_nom.copy()
+ new_v_nom = problem.v_nom.copy()
+ new_q_nom[10] = np.array([0.4, 1.6, 2.1])
+ new_v_nom[10] = np.array([0.1, 0.1, 0.1])
+
+ opt.UpdateNominalTrajectory(new_q_nom, new_v_nom)
+ problem = opt.prob()
+ assert np.all(problem.q_nom[10] == np.array([0.4, 1.6, 2.1]))
+ assert np.all(problem.v_nom[10] == np.array([0.1, 0.1, 0.1]))
diff --git a/pyidto/test/warm_start_test.py b/python_bindings/test/warm_start_test.py
similarity index 66%
rename from pyidto/test/warm_start_test.py
rename to python_bindings/test/warm_start_test.py
index c58d117..fcaeb62 100644
--- a/pyidto/test/warm_start_test.py
+++ b/python_bindings/test/warm_start_test.py
@@ -1,19 +1,24 @@
import numpy as np
-from pyidto.trajectory_optimizer import TrajectoryOptimizer
-from pyidto.problem_definition import ProblemDefinition
-from pyidto.solver_parameters import SolverParameters
-from pyidto.trajectory_optimizer_solution import TrajectoryOptimizerSolution
-from pyidto.trajectory_optimizer_stats import TrajectoryOptimizerStats
-from pyidto.find_idto_resource import FindIdtoResourceOrThrow
+from pyidto import (
+ TrajectoryOptimizer,
+ ProblemDefinition,
+ SolverParameters,
+ TrajectoryOptimizerSolution,
+ TrajectoryOptimizerStats,
+ FindIdtoResource
+)
+
+from pydrake.systems.framework import DiagramBuilder
+from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
+from pydrake.multibody.parsing import Parser
+
def define_problem_parameters():
- """
- Define some optimization parameters for a test problem.
- """
+ """Define some optimization parameters for a test problem."""
# Get the absolute path to a model file
- model_file = FindIdtoResourceOrThrow(
- "idto/examples/models/spinner_friction.urdf")
+ model_file = FindIdtoResource(
+ "idto/models/spinner_friction.urdf")
# Define the optimization problem
problem = ProblemDefinition()
@@ -66,13 +71,18 @@ def solve_once():
# Create the optimizer object
time_step = 0.05
- opt = TrajectoryOptimizer(model_file, problem, params, time_step)
+ builder = DiagramBuilder()
+ plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+ opt = TrajectoryOptimizer(diagram, plant, problem, params)
# Solve the optimization problem
solution = TrajectoryOptimizerSolution()
stats = TrajectoryOptimizerStats()
opt.Solve(q_guess, solution, stats)
-
+
solve_time = np.sum(stats.iteration_times)
print("Solved in ", solve_time, "seconds")
@@ -87,12 +97,17 @@ def solve_step_by_step():
# Create the optimizer object
time_step = 0.05
- opt = TrajectoryOptimizer(model_file, problem, params, time_step)
+ builder = DiagramBuilder()
+ plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+ opt = TrajectoryOptimizer(diagram, plant, problem, params)
# Solve the optimization problem
solution = TrajectoryOptimizerSolution()
stats = TrajectoryOptimizerStats()
- warm_start = opt.MakeWarmStart(q_guess)
+ warm_start = opt.CreateWarmStart(q_guess)
for _ in range(10):
opt.SolveFromWarmStart(warm_start, solution, stats)
@@ -100,11 +115,18 @@ def solve_step_by_step():
print("Solved in ", solve_time, "seconds")
return solution, stats
-def reset_initial_conditions():
+
+def test_reset_initial_conditions():
"""Test resetting the initial conditions."""
model_file, problem, params, q_guess = define_problem_parameters()
+
time_step = 0.05
- opt = TrajectoryOptimizer(model_file, problem, params, time_step)
+ builder = DiagramBuilder()
+ plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+ opt = TrajectoryOptimizer(diagram, plant, problem, params)
new_q_init = np.array([0.5, 1.2, -0.1])
new_v_init = np.array([0.04, 0.3, 0.2])
@@ -116,14 +138,19 @@ def reset_initial_conditions():
assert np.linalg.norm(new_solution.q[0] - new_q_init) < 1e-8
assert np.linalg.norm(new_solution.v[0] - new_v_init) < 1e-8
-def set_q():
- """
- Test setting the joint trajectory stored in the warm start.
- """
+
+def test_set_q():
+ """Test setting the joint trajectory stored in the warm start."""
model_file, problem, params, q_guess = define_problem_parameters()
time_step = 0.05
- opt = TrajectoryOptimizer(model_file, problem, params, time_step)
- warm_start = opt.MakeWarmStart(q_guess)
+ builder = DiagramBuilder()
+ plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+ opt = TrajectoryOptimizer(diagram, plant, problem, params)
+
+ warm_start = opt.CreateWarmStart(q_guess)
assert np.all(q_guess[10] == np.array([0.3, 1.5, 0.0]))
assert np.all(warm_start.get_q()[10] == np.array([0.3, 1.5, 0.0]))
@@ -133,26 +160,23 @@ def set_q():
warm_start.set_q(new_guess)
assert np.all(warm_start.get_q()[10] == np.array([0.5, 1.2, -0.1]))
-
-if __name__=="__main__":
- # Solve once open-loop and once with warm starts
+
+
+def test_compare_warm_start():
+ """Make sure open-loop and warm-start solutions are the same."""
one_shot_solution, one_shot_stats = solve_once()
warm_stat_solution, warm_start_stats = solve_step_by_step()
# Solutions should be the same
- assert np.linalg.norm(one_shot_solution.q[-1] - warm_stat_solution.q[-1]) < 1e-8
- assert np.linalg.norm(one_shot_solution.v[-1] - warm_stat_solution.v[-1]) < 1e-8
+ assert np.linalg.norm(
+ one_shot_solution.q[-1] - warm_stat_solution.q[-1]) < 1e-8
+ assert np.linalg.norm(
+ one_shot_solution.v[-1] - warm_stat_solution.v[-1]) < 1e-8
# Stats should be the same
- assert np.linalg.norm(np.array(one_shot_stats.iteration_costs)
+ assert np.linalg.norm(np.array(one_shot_stats.iteration_costs)
- np.array(warm_start_stats.iteration_costs)) < 1e-8
- assert np.linalg.norm(np.array(one_shot_stats.trust_region_radii)
+ assert np.linalg.norm(np.array(one_shot_stats.trust_region_radii)
- np.array(warm_start_stats.trust_region_radii)) < 1e-8
assert np.linalg.norm(np.array(one_shot_stats.gradient_norms)
- np.array(warm_start_stats.gradient_norms)) < 1e-8
-
- # Test resetting initial conditions
- reset_initial_conditions()
-
- # Test setting q in the warm start
- set_q()
diff --git a/python_bindings/trajectory_optimizer_py.cc b/python_bindings/trajectory_optimizer_py.cc
new file mode 100644
index 0000000..65c6963
--- /dev/null
+++ b/python_bindings/trajectory_optimizer_py.cc
@@ -0,0 +1,68 @@
+#include
+
+#include "optimizer/trajectory_optimizer.h"
+#include "optimizer/warm_start.h"
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace py = pybind11;
+
+using drake::geometry::SceneGraph;
+using drake::multibody::AddMultibodyPlant;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::MultibodyPlantConfig;
+using drake::multibody::Parser;
+using drake::systems::Diagram;
+using drake::systems::DiagramBuilder;
+using Eigen::VectorXd;
+using idto::optimizer::ProblemDefinition;
+using idto::optimizer::SolverParameters;
+using idto::optimizer::TrajectoryOptimizer;
+using idto::optimizer::TrajectoryOptimizerSolution;
+using idto::optimizer::TrajectoryOptimizerStats;
+using idto::optimizer::WarmStart;
+using idto::optimizer::ConvergenceReason;
+
+void bind_trajectory_optimizer(py::module_& m) {
+ py::module::import("pydrake.multibody.plant");
+ py::module::import("pydrake.systems.framework");
+
+ py::class_>(m, "TrajectoryOptimizer")
+ .def(py::init*, const MultibodyPlant*,
+ const ProblemDefinition&, const SolverParameters&>())
+ .def("time_step", &TrajectoryOptimizer::time_step)
+ .def("num_steps", &TrajectoryOptimizer::num_steps)
+ .def("Solve",
+ [](TrajectoryOptimizer& optimizer,
+ const std::vector& q_guess,
+ TrajectoryOptimizerSolution* solution,
+ TrajectoryOptimizerStats* stats) {
+ optimizer.Solve(q_guess, solution, stats);
+ })
+ .def("SolveFromWarmStart",
+ [](TrajectoryOptimizer& optimizer,
+ WarmStart* warm_start,
+ TrajectoryOptimizerSolution* solution,
+ TrajectoryOptimizerStats* stats) {
+ optimizer.SolveFromWarmStart(warm_start, solution, stats);
+ })
+ .def("CreateWarmStart", &TrajectoryOptimizer::CreateWarmStart)
+ .def("ResetInitialConditions",
+ &TrajectoryOptimizer::ResetInitialConditions)
+ .def("UpdateNominalTrajectory",
+ &TrajectoryOptimizer::UpdateNominalTrajectory)
+ .def("params", &TrajectoryOptimizer::params)
+ .def("prob", &TrajectoryOptimizer::prob);
+ py::class_(m, "WarmStart")
+ // Warm start is not default constructible: it should be created
+ // in python using the TrajectoryOptimizer.CreateWarmStart method.
+ .def("set_q", &WarmStart::set_q)
+ .def("get_q", &WarmStart::get_q)
+ .def_readonly("Delta", &WarmStart::Delta)
+ .def_readonly("dq", &WarmStart::dq)
+ .def_readonly("dqH", &WarmStart::dqH);
+}
diff --git a/pyidto/trajectory_optimizer_solution_py.cc b/python_bindings/trajectory_optimizer_solution_py.cc
similarity index 91%
rename from pyidto/trajectory_optimizer_solution_py.cc
rename to python_bindings/trajectory_optimizer_solution_py.cc
index a826799..2a723c1 100644
--- a/pyidto/trajectory_optimizer_solution_py.cc
+++ b/python_bindings/trajectory_optimizer_solution_py.cc
@@ -7,7 +7,7 @@
namespace py = pybind11;
using idto::optimizer::TrajectoryOptimizerSolution;
-PYBIND11_MODULE(trajectory_optimizer_solution, m) {
+void bind_trajectory_optimizer_solution(py::module_& m) {
py::class_>(m,
"TrajectoryOptimizerSolution")
.def(py::init<>())
diff --git a/pyidto/trajectory_optimizer_stats_py.cc b/python_bindings/trajectory_optimizer_stats_py.cc
similarity index 96%
rename from pyidto/trajectory_optimizer_stats_py.cc
rename to python_bindings/trajectory_optimizer_stats_py.cc
index 352324a..6f9d4d0 100644
--- a/pyidto/trajectory_optimizer_stats_py.cc
+++ b/python_bindings/trajectory_optimizer_stats_py.cc
@@ -6,7 +6,7 @@
namespace py = pybind11;
using idto::optimizer::TrajectoryOptimizerStats;
-PYBIND11_MODULE(trajectory_optimizer_stats, m) {
+void bind_trajectory_optimizer_stats(py::module_& m) {
py::class_>(m, "TrajectoryOptimizerStats")
.def(py::init<>())
// Optimizer stats should only be written to by the solver
diff --git a/python_examples/mini_cheetah_mpc_example.py b/python_examples/mini_cheetah_mpc.py
similarity index 90%
rename from python_examples/mini_cheetah_mpc_example.py
rename to python_examples/mini_cheetah_mpc.py
index 2ea8d83..76aa59e 100755
--- a/python_examples/mini_cheetah_mpc_example.py
+++ b/python_examples/mini_cheetah_mpc.py
@@ -3,20 +3,11 @@
##
#
# An example of using the python bindings to perform Model Predictive Control
-# on the mini cheetah quadruped. The project must be compiled with bazel first:
-#
-# bazel build //...
+# on the mini cheetah quadruped.
#
##
-# Note: this could be added to the PYTHONPATH environment variable instead,
-# as a better long-term solution
-import os
-import sys
-sys.path.insert(-1, os.getcwd() + "../bazel-bin/")
-
import numpy as np
-import time
from pydrake.all import (AddDefaultVisualization, AddMultibodyPlantSceneGraph,
DiagramBuilder, Parser, Simulator, StartMeshcat,
@@ -24,13 +15,17 @@
DiscreteContactApproximation, LeafSystem, BasicVector,
RollPitchYaw, Quaternion, RotationMatrix)
-from pyidto.trajectory_optimizer import TrajectoryOptimizer
-from pyidto.problem_definition import ProblemDefinition
-from pyidto.solver_parameters import SolverParameters
+from pyidto import (
+ TrajectoryOptimizer,
+ SolverParameters,
+ ProblemDefinition,
+ FindIdtoResource,
+)
from mpc_utils import Interpolator, ModelPredictiveController
-def create_optimizer():
+
+def create_optimizer(model_file):
"""
Create a trajectory optimizer object for the mini cheetah.
"""
@@ -104,11 +99,17 @@ def create_optimizer():
params.verbose = True
# Create the optimizer
- time_step = 0.05
- model_file = "../examples/models/mini_cheetah_with_ground.urdf"
- optimizer = TrajectoryOptimizer(model_file, problem, params, time_step)
+ builder = DiagramBuilder()
+ plant, _ = AddMultibodyPlantSceneGraph(builder, 0.05)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+ optimizer = TrajectoryOptimizer(diagram, plant, problem, params)
- return optimizer
+ # N.B. the diagram and plant must not go out of scope while the optimizer
+ # is being used, since under the hood the optimizer is just looking at
+ # pointers to the plant and diagram objects.
+ return optimizer, diagram, plant
def create_initial_guess(num_steps):
@@ -229,12 +230,13 @@ def UpdateNominalTrajectory(self, context):
if __name__ == "__main__":
meshcat = StartMeshcat()
+ model_file = FindIdtoResource("idto/models/mini_cheetah_with_ground.urdf")
# Set up a Drake diagram for simulation
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)
plant.set_discrete_contact_approximation(DiscreteContactApproximation.kLagged)
- models = Parser(plant).AddModels("../examples/models/mini_cheetah_with_ground.urdf")
+ models = Parser(plant).AddModels(model_file)
# Add implicit PD controllers (must use kLagged or kSimilar)
Kp = 50 * np.ones(plant.num_actuators())
@@ -247,7 +249,9 @@ def UpdateNominalTrajectory(self, context):
plant.Finalize()
# Set up the trajectory optimization problem
- optimizer = create_optimizer()
+ # Note that the diagram and plant must stay in scope while the optimizer is
+ # being used
+ optimizer, ctrl_diagram, ctrl_plant = create_optimizer(model_file)
q_guess = create_initial_guess(optimizer.num_steps())
# Create the MPC controller and interpolator systems
diff --git a/python_examples/mpc_utils.py b/python_examples/mpc_utils.py
index 11b40bb..aca17e4 100644
--- a/python_examples/mpc_utils.py
+++ b/python_examples/mpc_utils.py
@@ -5,19 +5,15 @@
##
import numpy as np
-from pydrake.all import LeafSystem, Value, BasicVector
+from pydrake.all import (
+ LeafSystem,
+ BasicVector,
+ EventStatus,
+ PiecewisePolynomial,
+ Value,
+)
-from pydrake.all import EventStatus, PiecewisePolynomial, LeafSystem, BasicVector, Value
-
-
-# Note: this could be added to the PYTHONPATH environment variable instead,
-# as a better long-term solution
-import os
-import sys
-sys.path.insert(-1, os.getcwd() + "/bazel-bin/")
-
-from pyidto.trajectory_optimizer_solution import TrajectoryOptimizerSolution
-from pyidto.trajectory_optimizer_stats import TrajectoryOptimizerStats
+from pyidto import TrajectoryOptimizerSolution, TrajectoryOptimizerStats
class StoredTrajectory:
@@ -37,6 +33,7 @@ class Interpolator(LeafSystem):
actuated state reference x(t) and input u(t) at a given time t. This is
useful for passing a reference trajectory to a low-level controller.
"""
+
def __init__(self, Bq, Bv):
"""
Construct the interpolator system, which takes StoredTrajectory as input
@@ -61,16 +58,18 @@ def __init__(self, Bq, Bv):
self.Bq = Bq
self.Bv = Bv
- # Declare the input and output ports
+ # Declare the input and output ports
trajectory_input_port = self.DeclareAbstractInputPort("trajectory",
Value(StoredTrajectory()))
- state_output_port = self.DeclareVectorOutputPort("state",
- BasicVector(2 * num_actuators),
+ state_output_port = self.DeclareVectorOutputPort("state",
+ BasicVector(
+ 2 * num_actuators),
self.SendState)
control_output_port = self.DeclareVectorOutputPort("control",
- BasicVector(num_actuators),
- self.SendControl)
-
+ BasicVector(
+ num_actuators),
+ self.SendControl)
+
def SendState(self, context, output):
"""
Send the state at the current time.
@@ -95,6 +94,7 @@ class ModelPredictiveController(LeafSystem):
"""
A Drake system that implements an MPC controller.
"""
+
def __init__(self, optimizer, q_guess, nq, nv, mpc_rate):
"""
Construct the MPC controller system, which takes the current state as
@@ -118,11 +118,11 @@ def __init__(self, optimizer, q_guess, nq, nv, mpc_rate):
self.optimizer = optimizer
self.nq = nq
-
+
# Allocate a warm-start
self.q_guess = q_guess
- self.warm_start = self.optimizer.MakeWarmStart(self.q_guess)
-
+ self.warm_start = self.optimizer.CreateWarmStart(self.q_guess)
+
# Specify the timestep we'll use to discretize the trajectory
self.time_step = self.optimizer.time_step()
self.num_steps = self.optimizer.num_steps()
@@ -146,7 +146,7 @@ def __init__(self, optimizer, q_guess, nq, nv, mpc_rate):
"state", BasicVector(nq + nv))
self.trajectory_output_port = self.DeclareStateOutputPort(
"optimal_trajectory", self.stored_trajectory)
-
+
def StoreOptimizerSolution(self, solution, start_time):
"""
Store a solution to the optimization problem in a StoredTrajectory object.
@@ -177,9 +177,9 @@ def StoreOptimizerSolution(self, solution, start_time):
time_steps, v_knots)
trajectory.tau = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
time_steps, tau_knots)
-
+
return trajectory
-
+
def UpdateAbstractState(self, context, state):
"""
Resolve the MPC problem and store the optimal trajectory in the abstract
@@ -213,9 +213,9 @@ def UpdateAbstractState(self, context, state):
# Store the solution in the abstract state
state.get_mutable_abstract_state(0).SetFrom(
Value(self.StoreOptimizerSolution(solution, context.get_time())))
-
+
return EventStatus.Succeeded()
-
+
def UpdateNominalTrajectory(self, context):
"""
Shift the nominal trajectory to account for the current state.
diff --git a/python_examples/spinner_mpc_example.py b/python_examples/spinner_mpc.py
similarity index 79%
rename from python_examples/spinner_mpc_example.py
rename to python_examples/spinner_mpc.py
index 553d41b..e2e58fe 100755
--- a/python_examples/spinner_mpc_example.py
+++ b/python_examples/spinner_mpc.py
@@ -3,31 +3,28 @@
##
#
# An example of using the python bindings to perform Model Predictive Control
-# on the spinner. The project must be compiled with bazel first:
-#
-# bazel build //...
+# on the spinner.
#
##
-# Note: this could be added to the PYTHONPATH environment variable instead,
-# as a better long-term solution
-import os
-import sys
-sys.path.insert(-1, os.getcwd() + "../bazel-bin/")
import numpy as np
from pydrake.all import (AddDefaultVisualization, AddMultibodyPlantSceneGraph,
- DiagramBuilder, Parser, Simulator, StartMeshcat,
- PdControllerGains, JointActuatorIndex,
+ DiagramBuilder, Parser, Simulator, StartMeshcat,
+ PdControllerGains, JointActuatorIndex,
DiscreteContactApproximation)
-from pyidto.trajectory_optimizer import TrajectoryOptimizer
-from pyidto.problem_definition import ProblemDefinition
-from pyidto.solver_parameters import SolverParameters
+from pyidto import (
+ TrajectoryOptimizer,
+ SolverParameters,
+ ProblemDefinition,
+ FindIdtoResource,
+)
from mpc_utils import Interpolator, ModelPredictiveController
+
def define_spinner_optimization_problem():
"""
Create a problem definition for the spinner.
@@ -52,6 +49,7 @@ def define_spinner_optimization_problem():
return problem
+
def define_spinner_solver_parameters():
"""
Create a set of solver parameters for the spinner.
@@ -75,6 +73,7 @@ def define_spinner_solver_parameters():
return params
+
def define_spinner_initial_guess(num_steps):
"""
Create an initial guess for the spinner
@@ -90,9 +89,11 @@ class SpinnerMPC(ModelPredictiveController):
"""
Model predictive controller for the spinner.
"""
+
def __init__(self, optimizer, q_guess, nq, nv, mpc_rate):
- ModelPredictiveController.__init__(self, optimizer, q_guess, nq, nv, mpc_rate)
-
+ ModelPredictiveController.__init__(
+ self, optimizer, q_guess, nq, nv, mpc_rate)
+
def UpdateNominalTrajectory(self, context):
"""
Shift the nominal trajectory to account for the current state,
@@ -114,28 +115,37 @@ def UpdateNominalTrajectory(self, context):
if __name__ == "__main__":
+ model_file = FindIdtoResource("idto/models/spinner_friction.urdf")
+
# Set up a Drake diagram for simulation
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)
- plant.set_discrete_contact_approximation(DiscreteContactApproximation.kLagged)
- models = Parser(plant).AddModels("../examples/models/spinner_friction.urdf")
+ plant.set_discrete_contact_approximation(
+ DiscreteContactApproximation.kLagged)
+ models = Parser(plant).AddModels(model_file)
# Add implicit PD controllers (must use kLagged or kSimilar)
Kp = 100 * np.ones(plant.num_actuators())
Kd = 10 * np.ones(plant.num_actuators())
- actuator_indices = [JointActuatorIndex(i) for i in range(plant.num_actuators())]
+ actuator_indices = [JointActuatorIndex(
+ i) for i in range(plant.num_actuators())]
for actuator_index, Kp, Kd in zip(actuator_indices, Kp, Kd):
plant.get_joint_actuator(actuator_index).set_controller_gains(
PdControllerGains(p=Kp, d=Kd))
plant.Finalize()
- # Set up the trajectory optimization problem
+ # Set up the trajectory optimization problem. Note that this uses a different
+ # plant with a larger timestep than the one used for simulation.
problem = define_spinner_optimization_problem()
params = define_spinner_solver_parameters()
q_guess = define_spinner_initial_guess(problem.num_steps)
- model_file = "../examples/models/spinner_friction.urdf"
- optimizer = TrajectoryOptimizer(model_file, problem, params, 0.05)
+ ctrl_builder = DiagramBuilder()
+ ctrl_plant, _ = AddMultibodyPlantSceneGraph(ctrl_builder, 0.05)
+ Parser(ctrl_plant).AddModels(model_file)
+ ctrl_plant.Finalize()
+ ctrl_diagram = ctrl_builder.Build()
+ optimizer = TrajectoryOptimizer(ctrl_diagram, ctrl_plant, problem, params)
# Create the MPC controller and interpolator systems
mpc_rate = 200 # Hz
@@ -149,19 +159,19 @@ def UpdateNominalTrajectory(self, context):
# Wire the systems together
builder.Connect(
- plant.get_state_output_port(),
+ plant.get_state_output_port(),
controller.GetInputPort("state"))
builder.Connect(
- controller.GetOutputPort("optimal_trajectory"),
+ controller.GetOutputPort("optimal_trajectory"),
interpolator.GetInputPort("trajectory"))
builder.Connect(
- interpolator.GetOutputPort("control"),
+ interpolator.GetOutputPort("control"),
plant.get_actuation_input_port())
builder.Connect(
- interpolator.GetOutputPort("state"),
+ interpolator.GetOutputPort("state"),
plant.get_desired_state_input_port(models[0])
)
-
+
# Connect the plant to meshcat for visualization
meshcat = StartMeshcat()
AddDefaultVisualization(builder, meshcat)
diff --git a/python_examples/spinner_open_loop_example.py b/python_examples/spinner_open_loop.py
similarity index 77%
rename from python_examples/spinner_open_loop_example.py
rename to python_examples/spinner_open_loop.py
index c093f48..6b43d81 100755
--- a/python_examples/spinner_open_loop_example.py
+++ b/python_examples/spinner_open_loop.py
@@ -3,28 +3,28 @@
##
#
# An example of using the python bindings to solve the spinner optimization
-# problem. The project must be compiled with bazel first:
-#
-# bazel build //...
+# problem.
#
##
-from pydrake.all import (StartMeshcat, DiagramBuilder,
- AddMultibodyPlantSceneGraph, AddDefaultVisualization, Parser)
+from pydrake.all import (
+ StartMeshcat,
+ DiagramBuilder,
+ AddMultibodyPlantSceneGraph,
+ AddDefaultVisualization,
+ Parser,
+)
import time
import numpy as np
-import os
-import sys
-
-# Note: this could be added to the PYTHONPATH environment variable instead,
-# as a better long-term solution
-sys.path.insert(-1, os.getcwd() + "../bazel-bin/")
-from pyidto.trajectory_optimizer_stats import TrajectoryOptimizerStats
-from pyidto.trajectory_optimizer_solution import TrajectoryOptimizerSolution
-from pyidto.solver_parameters import SolverParameters
-from pyidto.problem_definition import ProblemDefinition
-from pyidto.trajectory_optimizer import TrajectoryOptimizer
+from pyidto import (
+ TrajectoryOptimizer,
+ TrajectoryOptimizerSolution,
+ TrajectoryOptimizerStats,
+ SolverParameters,
+ ProblemDefinition,
+ FindIdtoResource,
+)
def define_spinner_optimization_problem():
@@ -122,8 +122,8 @@ def visualize_trajectory(q, time_step, model_file, meshcat=None):
# Start up meshcat (for viewing the result)
meshcat = StartMeshcat()
- # Relative path to the model file that we'll use
- model_file = "../examples/models/spinner_friction.urdf"
+ # Absolute path of the model file that we'll use
+ model_file = FindIdtoResource("idto/models/spinner_friction.urdf")
# Specify a cost function and target trajectory
problem = define_spinner_optimization_problem()
@@ -131,14 +131,16 @@ def visualize_trajectory(q, time_step, model_file, meshcat=None):
# Specify solver parameters, including contact modeling parameters
params = define_spinner_solver_parameters()
- # Specify the timestep we'll use to discretize the trajectory
- time_step = 0.05
-
# Specify an initial guess
q_guess = define_spinner_initial_guess(problem.num_steps)
# Create the optimizer object
- opt = TrajectoryOptimizer(model_file, problem, params, time_step)
+ builder = DiagramBuilder()
+ plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.05)
+ Parser(plant).AddModels(model_file)
+ plant.Finalize()
+ diagram = builder.Build()
+ opt = TrajectoryOptimizer(diagram, plant, problem, params)
# Allocate some structs that will hold the solution
solution = TrajectoryOptimizerSolution()
@@ -151,4 +153,4 @@ def visualize_trajectory(q, time_step, model_file, meshcat=None):
print(f"Solved in {solve_time:.4f} seconds")
# Play back the solution on meshcat
- visualize_trajectory(solution.q, time_step, model_file, meshcat)
+ visualize_trajectory(solution.q, plant.time_step(), model_file, meshcat)
diff --git a/scripts/README.md b/scripts/README.md
deleted file mode 100644
index 4de38a4..0000000
--- a/scripts/README.md
+++ /dev/null
@@ -1,43 +0,0 @@
-This directory contains a few scripts for making convergence plots.
-
-## Plot detailed convergence data
-
-`plot_convergence_data.py` makes a `matplotlib` plot of open-loop convergence
-data for a given example.
-
-Usage:
-- Run an example, e.g., `bazel run //examples/kuka:kuka`
- - Make sure the example YAML is configured for open-loop optimization
- (`run_mpc : false`) and `save_solver_stats_csv : true`.
-- Run the plotting script, specifying the example as a command line argument,
- e.g.,
-```
-$ ./scripts/plot_convergence_data.py kuka
-```
-
-
-## Compare convergence with different settings
-
-`compare_convergence.py` makes a less detailed plot comparing two or more
-open-loop optimizations with the same example but different settings.
-
-Usage:
-- Run an example, e.g., `bazel run //examples/acrobot:acrobot`
- - Make sure the example YAML is configured for open-loop optimization
- (`run_mpc : false`) and `save_solver_stats_csv : true`.
- - The resulting CSV file is stored in
- `bazel-out/k8-opt/bin/examples/acrobot/acrobot.runfiles/idt/solver_stats.csv`.
- Make a copy of this file with a new name, e.g,
- `solver_stats_trust_region.csv`, also in `bazel-out/k8-opt/...`
-- Run the example again with different settings
- - For example, set `method : linesearch`
- - Make a copy of `solver_stats.csv`, e.g. `solver_stats_linesearch.csv`.
-- Repeat as many times as you like with various settings, each time making a new
- copy of `solver_stats.csv`.
-- Edit `compare_convergence.py` to set `example_name`, `csv_names`, and `labels`
- to match the example you used, the new CSV names you created, and the labels
- you want to show up on the plot
-- Run the plotting script:
-```
-$ ./scripts/compare_convergence.py
-```
diff --git a/scripts/compare_convergence.py b/scripts/compare_convergence.py
deleted file mode 100755
index 2985093..0000000
--- a/scripts/compare_convergence.py
+++ /dev/null
@@ -1,73 +0,0 @@
-#!/usr/bin/env python
-
-##
-#
-# Quick script to compare basic convergence data from several csv log files.
-# These log files should be manually copied from solver_stats.csv first.
-#
-# This script must be run from the "idto/" directory.
-#
-##
-
-import matplotlib.pyplot as plt
-from matplotlib.ticker import MaxNLocator
-
-import numpy as np
-import os
-
-# Basic parameters: set these to define the location and name of the log files
-# that we'll compare, as well as corresponding legend labels
-example_name = "acrobot"
-csv_names = ["solver_stats_linesearch.csv",
- "solver_stats_trust_region.csv"]
-labels = ["linesearch",
- "trust region"]
-
-# Get file locations
-idto_root = os.getcwd()
-if idto_root[-5:] != "/idto":
- print("This script must be run from the 'idto/' directory")
- sys.exit(1)
-
-data_root = idto_root + f"/bazel-out/k8-opt/bin/examples/{example_name}/{example_name}.runfiles/idto/"
-
-# Make plots
-fig, ax = plt.subplots(3,1,sharex=True,figsize=(8,6))
-fig.suptitle(f"{example_name} convergence data")
-
-# Get a baseline cost
-N = len(csv_names)
-baseline = np.inf
-for i in range(N):
- data_file = data_root + csv_names[i]
- data = np.genfromtxt(data_file, delimiter=',', names=True)
- baseline = np.min([baseline, data["cost"][-1]])
-
-# Get the main data
-for i in range(N):
- # Read data from the file and format nicely
- data_file = data_root + csv_names[i]
- data = np.genfromtxt(data_file, delimiter=',', names=True)
- iters = data["iter"]
-
- ax[0].plot(iters, data["cost"] - baseline, label=labels[i])
- ax[0].set_ylabel("Cost $L(q_k) - L(q^*)$")
- ax[0].set_yscale("log")
-
- ax[1].plot(iters, data["h_norm"], label=labels[i])
- ax[1].set_ylabel("Constraint violation $||h(q_k)||$")
- ax[1].set_yscale("log")
-
- ax[2].plot(iters, data["grad_norm"]/data["cost"], label=labels[i])
- ax[2].set_ylabel("Gradient norm $||g||/L(q)$")
- ax[2].set_yscale("log")
-
-ax[0].legend()
-ax[0].grid()
-ax[1].grid()
-ax[2].grid()
-ax[2].set_xlabel("Iteration k")
-ax[2].xaxis.set_major_locator(MaxNLocator(integer=True))
-
-plt.show()
-
diff --git a/scripts/plot_convergence_data.py b/scripts/plot_convergence_data.py
deleted file mode 100755
index bce71bc..0000000
--- a/scripts/plot_convergence_data.py
+++ /dev/null
@@ -1,90 +0,0 @@
-#!/usr/bin/env python
-
-##
-#
-# A quick script to make a plot of solution data from a trajectory optimization
-# example (see example_base.h).
-#
-# This script must be run from the "idto/" directory.
-#
-##
-
-import matplotlib.pyplot as plt
-from matplotlib.ticker import MaxNLocator
-
-import numpy as np
-import os
-import sys
-
-# Command-line flags determine which example (e.g., pendulum, acrobot, spinner) we're
-# dealing with.
-if (len(sys.argv) != 2):
- print(f"Usage: {sys.argv[0]} [example_name]")
- print("\nThe corresponding example must be run first (e.g. 'bazel run //examples/acrobot:acrobot`), with 'save_solver_stats_csv=true'")
- sys.exit(1)
-example_name = sys.argv[1]
-
-# idto/ directory, contains bazel-out symlink
-idto_root = os.getcwd()
-if idto_root[-5:] != "/idto":
- print("This script must be run from the 'idto/' directory")
- sys.exit(1)
-
-# Bazel stores files in strange places
-data_file = idto_root + f"/bazel-out/k8-opt/bin/examples/{example_name}/{example_name}.runfiles/idto/solver_stats.csv"
-
-# Read data from the file and format nicely
-data = np.genfromtxt(data_file, delimiter=',', names=True)
-iters = data["iter"]
-
-# Make plots
-fig, ax = plt.subplots(5,2,sharex=True,figsize=(16,11))
-
-fig.suptitle(f"{example_name} convergence data")
-
-ax[0,0].plot(iters, data["trust_ratio"])
-ax[0,0].set_ylabel("trust ratio")
-ax[0,0].set_ylim((-1,3))
-
-ax[1,0].plot(iters, data["delta"], label="$\Delta$")
-ax[1,0].plot(iters, data["dq_norm"], label="$\|\Delta q\|$")
-ax[1,0].set_ylabel("$||\Delta q||$")
-ax[1,0].set_yscale("log")
-ax[1,0].legend()
-
-ax[2,0].plot(iters, data["grad_norm"] / data["cost"])
-ax[2,0].set_ylabel("$||g|| / cost$")
-ax[2,0].set_yscale("log")
-
-ax[3,0].plot(iters, data["grad_norm"])
-ax[3,0].set_ylabel("$||g||$")
-ax[3,0].set_yscale("log")
-
-ax[4,0].plot(iters, -data["dL_dq"])
-ax[4,0].set_ylabel(r"$\frac{|g' \Delta q|}{cost}$")
-ax[4,0].set_yscale("log")
-
-ax[0,1].plot(iters, data["merit"] - np.min(data["merit"]))
-ax[0,1].set_ylabel("Merit (- baseline)")
-ax[0,1].set_yscale("log")
-
-ax[1,1].plot(iters, data["cost"] - np.min(data["cost"]))
-ax[1,1].set_ylabel("Cost (- baseline)")
-ax[1,1].set_yscale("log")
-
-ax[2,1].plot(iters, data["h_norm"])
-ax[2,1].set_ylabel("Constraint Viol.")
-ax[2,1].set_yscale("log")
-
-ax[3,1].plot(iters, data["ls_iters"])
-ax[3,1].set_ylabel("Linesearch Iters")
-
-ax[4,1].plot(iters, data["alpha"])
-ax[4,1].set_ylabel(r"Linesearch Param $\alpha$")
-
-ax[4,0].set_xlabel("Iteration")
-ax[4,0].xaxis.set_major_locator(MaxNLocator(integer=True))
-ax[4,1].set_xlabel("Iteration")
-ax[4,1].xaxis.set_major_locator(MaxNLocator(integer=True))
-
-plt.show()
diff --git a/setup.py b/setup.py
new file mode 100644
index 0000000..bf079d2
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,140 @@
+import os
+import re
+import subprocess
+import sys
+from pathlib import Path
+
+from setuptools import Extension, setup
+from setuptools.command.build_ext import build_ext
+
+# Convert distutils Windows platform specifiers to CMake -A arguments
+PLAT_TO_CMAKE = {
+ "win32": "Win32",
+ "win-amd64": "x64",
+ "win-arm32": "ARM",
+ "win-arm64": "ARM64",
+}
+
+
+# A CMakeExtension needs a sourcedir instead of a file list.
+# The name must be the _single_ output extension from the CMake build.
+# If you need multiple extensions, see scikit-build.
+class CMakeExtension(Extension):
+ def __init__(self, name: str, sourcedir: str = "") -> None:
+ super().__init__(name, sources=[])
+ self.sourcedir = os.fspath(Path(sourcedir).resolve())
+
+
+class CMakeBuild(build_ext):
+ def build_extension(self, ext: CMakeExtension) -> None:
+ # Must be in this form due to bug in .resolve() only fixed in Python 3.10+
+ ext_fullpath = Path.cwd() / self.get_ext_fullpath(ext.name)
+ extdir = ext_fullpath.parent.resolve()
+
+ # Using this requires trailing slash for auto-detection & inclusion of
+ # auxiliary "native" libs
+
+ debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug
+ cfg = "Debug" if debug else "Release"
+
+ # CMake lets you override the generator - we need to check this.
+ # Can be set with Conda-Build, for example.
+ cmake_generator = os.environ.get("CMAKE_GENERATOR", "")
+
+ # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON
+ # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code
+ # from Python.
+ cmake_args = [
+ f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}{os.sep}",
+ f"-DPYTHON_EXECUTABLE={sys.executable}",
+ f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm
+ ]
+ build_args = []
+ # Adding CMake arguments set as environment variable
+ # (needed e.g. to build for ARM OSx on conda-forge)
+ if "CMAKE_ARGS" in os.environ:
+ cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item]
+
+ # In this example, we pass in the version to C++. You might not need to.
+ cmake_args += [f"-DEXAMPLE_VERSION_INFO={self.distribution.get_version()}"]
+
+ if self.compiler.compiler_type != "msvc":
+ # Using Ninja-build since it a) is available as a wheel and b)
+ # multithreads automatically. MSVC would require all variables be
+ # exported for Ninja to pick it up, which is a little tricky to do.
+ # Users can override the generator with CMAKE_GENERATOR in CMake
+ # 3.15+.
+ if not cmake_generator or cmake_generator == "Ninja":
+ try:
+ import ninja
+
+ ninja_executable_path = Path(ninja.BIN_DIR) / "ninja"
+ cmake_args += [
+ "-GNinja",
+ f"-DCMAKE_MAKE_PROGRAM:FILEPATH={ninja_executable_path}",
+ ]
+ except ImportError:
+ pass
+
+ else:
+ # Single config generators are handled "normally"
+ single_config = any(x in cmake_generator for x in {"NMake", "Ninja"})
+
+ # CMake allows an arch-in-generator style for backward compatibility
+ contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"})
+
+ # Specify the arch if using MSVC generator, but only if it doesn't
+ # contain a backward-compatibility arch spec already in the
+ # generator name.
+ if not single_config and not contains_arch:
+ cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]]
+
+ # Multi-config generators have a different way to specify configs
+ if not single_config:
+ cmake_args += [
+ f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}"
+ ]
+ build_args += ["--config", cfg]
+
+ if sys.platform.startswith("darwin"):
+ # Cross-compile support for macOS - respect ARCHFLAGS if set
+ archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", ""))
+ if archs:
+ cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))]
+
+ # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level
+ # across all generators.
+ if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ:
+ # self.parallel is a Python 3 only way to set parallel jobs by hand
+ # using -j in the build_ext call, not supported by pip or PyPA-build.
+ if hasattr(self, "parallel") and self.parallel:
+ # CMake 3.12+ only.
+ build_args += [f"-j{self.parallel}"]
+
+ build_temp = Path(self.build_temp) / ext.name
+ if not build_temp.exists():
+ build_temp.mkdir(parents=True)
+
+ subprocess.run(
+ ["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True
+ )
+ subprocess.run(
+ ["cmake", "--build", ".", *build_args], cwd=build_temp, check=True
+ )
+
+
+# The information here can also be placed in setup.cfg - better separation of
+# logic and declaration, and simpler if you include description/version in a file.
+setup(
+ name="pyidto",
+ version="0.0.1",
+ author="Vince Kurtz",
+ author_email="vjkurtz@gmail.com",
+ description="Inverse Dynamics Trajectory Optimization (IDTO)",
+ long_description="",
+ ext_modules=[CMakeExtension("pyidto")],
+ cmdclass={"build_ext": CMakeBuild},
+ zip_safe=False,
+ extras_require={"test": ["pytest>=6.0"]},
+ python_requires=">=3.7",
+)
diff --git a/third_party/CMakeLists.txt b/third_party/CMakeLists.txt
new file mode 100644
index 0000000..829b108
--- /dev/null
+++ b/third_party/CMakeLists.txt
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: MIT-0
+
+find_package(Threads)
+
+add_library(gtest STATIC googletest/gtest-all.cc googletest/gtest/gtest.h)
+target_include_directories(gtest PUBLIC googletest)
+target_link_libraries(gtest Threads::Threads)
diff --git a/third_party/googletest/LICENSE b/third_party/googletest/LICENSE
new file mode 100644
index 0000000..136b4cf
--- /dev/null
+++ b/third_party/googletest/LICENSE
@@ -0,0 +1,27 @@
+Copyright 2008, Google Inc.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+* Neither the name of Google Inc. nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/third_party/googletest/gtest-all.cc b/third_party/googletest/gtest-all.cc
new file mode 100644
index 0000000..cef5397
--- /dev/null
+++ b/third_party/googletest/gtest-all.cc
@@ -0,0 +1,11824 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+//
+// Google C++ Testing and Mocking Framework (Google Test)
+//
+// Sometimes it's desirable to build Google Test by compiling a single file.
+// This file serves this purpose.
+
+// This line ensures that gtest.h can be compiled on its own, even
+// when it's fused.
+#include "gtest/gtest.h"
+
+// The following lines pull in the real gtest *.cc files.
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+//
+// The Google C++ Testing and Mocking Framework (Google Test)
+
+// Copyright 2007, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+//
+// Utilities for testing Google Test itself and code that uses Google Test
+// (e.g. frameworks built on top of Google Test).
+
+// GOOGLETEST_CM0004 DO NOT DELETE
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_SPI_H_
+#define GTEST_INCLUDE_GTEST_GTEST_SPI_H_
+
+
+GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \
+/* class A needs to have dll-interface to be used by clients of class B */)
+
+namespace testing {
+
+// This helper class can be used to mock out Google Test failure reporting
+// so that we can test Google Test or code that builds on Google Test.
+//
+// An object of this class appends a TestPartResult object to the
+// TestPartResultArray object given in the constructor whenever a Google Test
+// failure is reported. It can either intercept only failures that are
+// generated in the same thread that created this object or it can intercept
+// all generated failures. The scope of this mock object can be controlled with
+// the second argument to the two arguments constructor.
+class GTEST_API_ ScopedFakeTestPartResultReporter
+ : public TestPartResultReporterInterface {
+ public:
+ // The two possible mocking modes of this object.
+ enum InterceptMode {
+ INTERCEPT_ONLY_CURRENT_THREAD, // Intercepts only thread local failures.
+ INTERCEPT_ALL_THREADS // Intercepts all failures.
+ };
+
+ // The c'tor sets this object as the test part result reporter used
+ // by Google Test. The 'result' parameter specifies where to report the
+ // results. This reporter will only catch failures generated in the current
+ // thread. DEPRECATED
+ explicit ScopedFakeTestPartResultReporter(TestPartResultArray* result);
+
+ // Same as above, but you can choose the interception scope of this object.
+ ScopedFakeTestPartResultReporter(InterceptMode intercept_mode,
+ TestPartResultArray* result);
+
+ // The d'tor restores the previous test part result reporter.
+ ~ScopedFakeTestPartResultReporter() override;
+
+ // Appends the TestPartResult object to the TestPartResultArray
+ // received in the constructor.
+ //
+ // This method is from the TestPartResultReporterInterface
+ // interface.
+ void ReportTestPartResult(const TestPartResult& result) override;
+
+ private:
+ void Init();
+
+ const InterceptMode intercept_mode_;
+ TestPartResultReporterInterface* old_reporter_;
+ TestPartResultArray* const result_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ScopedFakeTestPartResultReporter);
+};
+
+namespace internal {
+
+// A helper class for implementing EXPECT_FATAL_FAILURE() and
+// EXPECT_NONFATAL_FAILURE(). Its destructor verifies that the given
+// TestPartResultArray contains exactly one failure that has the given
+// type and contains the given substring. If that's not the case, a
+// non-fatal failure will be generated.
+class GTEST_API_ SingleFailureChecker {
+ public:
+ // The constructor remembers the arguments.
+ SingleFailureChecker(const TestPartResultArray* results,
+ TestPartResult::Type type, const std::string& substr);
+ ~SingleFailureChecker();
+ private:
+ const TestPartResultArray* const results_;
+ const TestPartResult::Type type_;
+ const std::string substr_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(SingleFailureChecker);
+};
+
+} // namespace internal
+
+} // namespace testing
+
+GTEST_DISABLE_MSC_WARNINGS_POP_() // 4251
+
+// A set of macros for testing Google Test assertions or code that's expected
+// to generate Google Test fatal failures. It verifies that the given
+// statement will cause exactly one fatal Google Test failure with 'substr'
+// being part of the failure message.
+//
+// There are two different versions of this macro. EXPECT_FATAL_FAILURE only
+// affects and considers failures generated in the current thread and
+// EXPECT_FATAL_FAILURE_ON_ALL_THREADS does the same but for all threads.
+//
+// The verification of the assertion is done correctly even when the statement
+// throws an exception or aborts the current function.
+//
+// Known restrictions:
+// - 'statement' cannot reference local non-static variables or
+// non-static members of the current object.
+// - 'statement' cannot return a value.
+// - You cannot stream a failure message to this macro.
+//
+// Note that even though the implementations of the following two
+// macros are much alike, we cannot refactor them to use a common
+// helper macro, due to some peculiarity in how the preprocessor
+// works. The AcceptsMacroThatExpandsToUnprotectedComma test in
+// gtest_unittest.cc will fail to compile if we do that.
+#define EXPECT_FATAL_FAILURE(statement, substr) \
+ do { \
+ class GTestExpectFatalFailureHelper {\
+ public:\
+ static void Execute() { statement; }\
+ };\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kFatalFailure, (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter:: \
+ INTERCEPT_ONLY_CURRENT_THREAD, >est_failures);\
+ GTestExpectFatalFailureHelper::Execute();\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+#define EXPECT_FATAL_FAILURE_ON_ALL_THREADS(statement, substr) \
+ do { \
+ class GTestExpectFatalFailureHelper {\
+ public:\
+ static void Execute() { statement; }\
+ };\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kFatalFailure, (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter:: \
+ INTERCEPT_ALL_THREADS, >est_failures);\
+ GTestExpectFatalFailureHelper::Execute();\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+// A macro for testing Google Test assertions or code that's expected to
+// generate Google Test non-fatal failures. It asserts that the given
+// statement will cause exactly one non-fatal Google Test failure with 'substr'
+// being part of the failure message.
+//
+// There are two different versions of this macro. EXPECT_NONFATAL_FAILURE only
+// affects and considers failures generated in the current thread and
+// EXPECT_NONFATAL_FAILURE_ON_ALL_THREADS does the same but for all threads.
+//
+// 'statement' is allowed to reference local variables and members of
+// the current object.
+//
+// The verification of the assertion is done correctly even when the statement
+// throws an exception or aborts the current function.
+//
+// Known restrictions:
+// - You cannot stream a failure message to this macro.
+//
+// Note that even though the implementations of the following two
+// macros are much alike, we cannot refactor them to use a common
+// helper macro, due to some peculiarity in how the preprocessor
+// works. If we do that, the code won't compile when the user gives
+// EXPECT_NONFATAL_FAILURE() a statement that contains a macro that
+// expands to code containing an unprotected comma. The
+// AcceptsMacroThatExpandsToUnprotectedComma test in gtest_unittest.cc
+// catches that.
+//
+// For the same reason, we have to write
+// if (::testing::internal::AlwaysTrue()) { statement; }
+// instead of
+// GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement)
+// to avoid an MSVC warning on unreachable code.
+#define EXPECT_NONFATAL_FAILURE(statement, substr) \
+ do {\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kNonFatalFailure, \
+ (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter:: \
+ INTERCEPT_ONLY_CURRENT_THREAD, >est_failures);\
+ if (::testing::internal::AlwaysTrue()) { statement; }\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+#define EXPECT_NONFATAL_FAILURE_ON_ALL_THREADS(statement, substr) \
+ do {\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kNonFatalFailure, \
+ (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter::INTERCEPT_ALL_THREADS, \
+ >est_failures);\
+ if (::testing::internal::AlwaysTrue()) { statement; }\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_SPI_H_
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include