diff --git a/.bazeliskrc b/.bazeliskrc new file mode 100644 index 0000000000..6db7b5ce5e --- /dev/null +++ b/.bazeliskrc @@ -0,0 +1,6 @@ +# Keep this version number in sync with the Ubuntu deb installed by +# drake/setup/ubuntu/source_distribution/install_bazel.sh. +# +# These files should also be updated: +# drake/tools/workspace/drake_visualizer/image/provision.sh +USE_BAZEL_VERSION=8.2.1 \ No newline at end of file diff --git a/.bazelrc b/.bazelrc index b01befb8ec..21f2cb1abd 100644 --- a/.bazelrc +++ b/.bazelrc @@ -39,8 +39,8 @@ test --test_output=errors test --test_summary=terse # Use C++14. -build --cxxopt=-std=c++17 -build --host_cxxopt=-std=c++17 +build --cxxopt=-std=c++20 +build --host_cxxopt=-std=c++20 # https://github.com/bazelbuild/bazel/issues/1164 build --action_env=CCACHE_DISABLE=1 @@ -52,7 +52,14 @@ build --action_env=LD_LIBRARY_PATH= # customizations somehow. # build with snopt -build --define=WITH_SNOPT=ON +build --define=WITH_GUROBI=ON +build --define=NO_CLARABEL=ON + +build:omp --copt=-DEIGEN_DONT_PARALLELIZE +build:omp --copt=-fopenmp +build:omp --linkopt=-fopenmp # use python3 by default build --python_path=python3 + +common --enable_bzlmod \ No newline at end of file diff --git a/.cirrus.yml b/.cirrus.yml index 0e75243699..d43a3a69a9 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -1,91 +1,58 @@ +registry_config: ENCRYPTED[!88cf0d757d2f8b93dca9e57dc166b65ddedef6378e7ac12a91a022ab3fb28dd47b10d452dc5c53a68e144e6bdbae999b!] build_jammy_task: timeout_in: 120m container: - dockerfile: install/jammy/Dockerfile + image: ghcr.io/dairlab/docker-dair/jammy-dair-base:v1.42 cpu: 8 - memory: 24 + memory: 24G test_script: - - export CC=clang-12 - - export CXX=clang++-12 - bazel build - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 + --local_resources=memory=24000 + --local_resources=cpu=8 --jobs=8 + --remote_http_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - bazel test - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 + --local_resources=memory=24000 + --local_resources=cpu=8 + --remote_http_cache=http://$CIRRUS_HTTP_CACHE_HOST //... -build_focal_task: + +build_noble_task: timeout_in: 120m container: - dockerfile: install/focal/Dockerfile + image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42 cpu: 8 - memory: 24 + memory: 24G test_script: - - export CC=clang-12 - - export CXX=clang++-12 - bazel build - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 + --local_resources=memory=24000 + --local_resources=cpu=8 --jobs=8 + --remote_http_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - bazel test - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 - //... -build_with_ros_task: - timeout_in: 120m - container: - dockerfile: install/focal/ros/Dockerfile - cpu: 8 - memory: 24 - env: - DAIRLIB_WITH_ROS: ON - test_script: - - cd tools/workspace/ros - - ./compile_ros_workspace.sh - - cd ../../../ - - bazel build - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 - --jobs=8 - //... - - bazel test - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 - --jobs=8 + --local_resources=memory=24000 + --local_resources=cpu=8 + --remote_http_cache=http://$CIRRUS_HTTP_CACHE_HOST //... + drake_master_build_task: timeout_in: 120m container: - dockerfile: install/focal/ros/Dockerfile + image: ghcr.io/dairlab/docker-dair/jammy-dair-base:v1.42 cpu: 8 - memory: 24 + memory: 24G allow_failures: true - env: - DAIRLIB_WITH_ROS: ON test_script: - git clone https://github.com/RobotLocomotion/drake.git ../drake - - export DAIRLIB_LOCAL_DRAKE_PATH=$PWD/../drake - - cd tools/workspace/ros - - ./compile_ros_workspace.sh - - cd ../../../ - bazel build - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 - --jobs=8 + --override_module=drake=$PWD/../drake + --local_resources=memory=24000 + --local_resources=cpu=8 //... - bazel test - --define=WITH_SNOPT=OFF - --local_ram_resources=24000 - --local_cpu_resources=8 - --jobs=8 + --override_module=drake=$PWD/../drake + --local_resources=memory=24000 + --local_resources=cpu=8 //... diff --git a/.gitignore b/.gitignore index a9f31f7fdd..7d25deb06a 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,4 @@ attic/multibody/solver_log/ .vscode *.csv /examples/Cassie/saved_trajectories/ +/MODULE.bazel.lock diff --git a/BUILD.bazel b/BUILD.bazel index 1841fa978c..f24256eb35 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -32,14 +32,3 @@ # This is an empty BUILD file, to ensure that the project's root directory is a # bazel package. - -load("@com_github_bazelbuild_buildtools//buildifier:def.bzl", "buildifier") - -buildifier( - name = "buildifier", -) - -load("@bazel_gazelle//:def.bzl", "gazelle") - -# gazelle:prefix github.com/example/project -gazelle(name = "gazelle") diff --git a/MODULE.bazel b/MODULE.bazel new file mode 100644 index 0000000000..7b559c2896 --- /dev/null +++ b/MODULE.bazel @@ -0,0 +1,122 @@ +## MODULE.bazel +module( + name = "dairlib" +) + +drake_dep_repositories = use_extension( + "@drake//tools/workspace:default.bzl", + "drake_dep_repositories", +) + +# If you don't want to support building on macOS, you may remove the next line. +# Note that it must appear prior to loading "rules_cc", per the documentation: +# https://github.com/bazelbuild/apple_support?tab=readme-ov-file#bazel-7-setup +bazel_dep(name = "apple_support", version = "1.17.1") + +# Add the Bazel rules we need. +bazel_dep(name = "rules_cc", version = "0.1.1") +bazel_dep(name = "rules_python", version = "1.0.0") +bazel_dep(name = "bazel_skylib", version = "1.7.1") + +use_repo( + drake_dep_repositories, + "blas", + "buildifier", + "drake_models", + "eigen", + "fmt", + "gflags", + "glib", + "gtest", + "lapack", + "lcm", + "meshcat", + "mosek", + "opencl", + "pybind11", + "pycodestyle", + "snopt", + "spdlog", + "styleguide", + "x11", + "zlib", + "gurobi", +) + +bazel_dep(name = "gazelle", version = "0.44.0") + +bazel_dep(name = "osqp", version = "0.6.3.bcr.2") + +# Use gurobi_cxx, which Drake deprecated +gurobi_extension = use_extension("@dairlib//tools/workspace/gurobi:repository.bzl", "gurobi_extension") +use_repo(gurobi_extension, "my_gurobi") + +# Replace Drake's Gurobi repository with ours. +override_repo( + drake_dep_repositories, + gurobi = "my_gurobi", +) + +# Here we introduce Drake as a module dependency, but note that Drake is not +# published to any Bazel registry. Below, we'll override it with a github +# source archive. +bazel_dep(name = "drake") + +bazel_dep(name = "inekf") + +# By default, this example always uses the latest Drake master branch. + +# To use a local version of drake, use +# build --override_module=drake=/home/user/stuff/drake +# this will also work to override inekf + +# You can also use DRAKE_COMMIT to choose a Drake release; e.g.: +DRAKE_VERSION = "v1.42.0" +DRAKE_CHECKSUM = "d860c15f50397c8a946fcc79e0a58a91ebc56f2189ef9edfcac929aa04157f8b" + +# Before changing the DRAKE_VERSION, temporarily uncomment the next line so that Bazel +# displays the suggested new value for the CHECKSUM. +# DRAKE_CHECKSUM = "0" * 64 + +# This declares the `@drake` module as a source code archive from github. +# See README.md for instructions to use a local path, instead. +archive_override( + module_name = "drake", + urls = [x.format(DRAKE_VERSION) for x in [ + "https://github.com/RobotLocomotion/drake/archive/{}.tar.gz", + ]], + sha256 = DRAKE_CHECKSUM, + strip_prefix = "drake-{}".format(DRAKE_VERSION.lstrip("v")), +) + +INEKF_COMMIT = "297c308e50fa599af92ce3bd5f11d71e2bf8af69" +INEKF_CHECKSUM = "c5a056ce00e1625e52f5a71b1d5c202acd53c1a8c7bca33da458db1e4f3f2edf" + + +# Before changing the COMMIT, temporarily uncomment the next line so that Bazel +# displays the suggested new value for the CHECKSUM. +#INEKF_CHECKSUM = "0" * 64 + +# This declares the `@inekf` module as a source code archive from github. +archive_override( + module_name = "inekf", + urls = [x.format(INEKF_COMMIT) for x in [ + "https://github.com/DAIRLab/invariant-ekf/archive/{}.tar.gz", + ]], + sha256 = INEKF_CHECKSUM, + strip_prefix = "invariant-ekf-{}".format(INEKF_COMMIT), +) + +# Use the host system /usr/bin/python3. +python_repository = use_repo_rule( + "@drake//tools/workspace/python:repository.bzl", + "python_repository", +) + +python_repository( + name = "python", + linux_interpreter_path = "/usr/bin/python3", + requirements_flavor = "build", +) + +register_toolchains("@python//:all") \ No newline at end of file diff --git a/README.md b/README.md index 836682cfbf..18729094d9 100644 --- a/README.md +++ b/README.md @@ -2,10 +2,9 @@ Warning! This is very much "development-level" code and is provided as-is. APIs are likely to be unstable and, while we hope for the documentation to be thorough and accurate, we make no guarantees. ## Current Continuous Integration Status -* `master` branch build and unit tests (Ubuntu 18.04): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) -* `master` branch build and unit tests (Ubuntu 20.04): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build_focal&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) -* `master` branch build and unit tests (Ubuntu 20.04 with ROS): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build_with_ros&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) -* Experimental build against Drake's `master` branch: [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=drake_master_build&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) +* `main` branch build and unit tests (Ubuntu Jammy 22.04): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build_jammy&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) +* `main` branch build and unit tests (Ubuntu Focal 24.04): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=build_focal&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) +* Experimental build against Drake's `master` branch (Jammy): [![Build Status](https://api.cirrus-ci.com/github/DAIRLab/dairlib.svg?task=drake_master_build&script=test)](https://cirrus-ci.com/github/DAIRLab/dairlib) ## Complete Build Instructions ### Download dairlib @@ -29,21 +28,13 @@ There is no need to extract the tar. The library is meant to be built with Drake (see http://drake.mit.edu/ for more details). There are two ways to use Drake within dairlib: #### Option 1: use pegged revision (Note - These steps may need repeated if switching to a branch with a different pegged revision of drake). -The only specific action needed here is to install all of Drake's prerequisites. There are two choices for completing this step: -a) In `dairlib/install`, run the appropriate `install_prereqs_xxx.sh`. This is untested on mac, and has not been tested to get every dependency for a fresh install. - -b) Download a source copy of drake, and install pre-requisites as described here: http://drake.mit.edu/from_source.html. Drake dependencies can change without notice. For full compatiability, you may need to checkout the drake commit which is pegged in WORKSPACE to install the correct dependencies. There is no need to build Drake itself. Proceed only until you have run the Drake setup script. - -bazel will automatically download the pegged revision, specified in the WORKSPACE file. dairlib developers hope to keep this pegged revision current, and ensure that the pegged version will always work with a specific version of dairlib. +In `dairlib/install`, run the `install_prereqs_ubuntu.sh`. Our build process does not currently support MacOS, though it has in the past and likely will in the future. This option is recommended for users who are not currently editing any source code in Drake itself. #### Option 2: source install of Drake -Complete both steps (a) and (b) above. By running the drake install script after the dairlib install script, you are capturing any dependency changes between the pegged revision and the current Drake master, while still getting any aditional dairlib dependencies we may add. There is no need to build Drake. Next, to tell dairlib to use your local install, set the environment variable `DAIRLIB_LOCAL_DRAKE_PATH`, e.g. -``` -export DAIRLIB_LOCAL_DRAKE_PATH=/home/user/my-workspace/drake -``` +If you would like to use your own local install of Drake, likely because you are modifying it, when you build with Bazel you will need to use `bazel build --override_module=drake=/home/user/my-workspace/drake ` (using the appropriate directory for your own install). There is no need to build Drake. ### IDE setup JetBrains IDEs have worked well for us and are available for free to students. For C++ development using the CLion Bazel plugin, see https://drake.mit.edu/clion.html and replace `drake` with `dairlib` in the "Setting up Drake in CLion" section. diff --git a/WORKSPACE b/WORKSPACE index 0b48c9f153..edd72bf514 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -1,212 +1,2 @@ -# -*- mode: python -*- -# vi: set ft=python : - -workspace(name = "dairlib") - -# dairlib can use either a local version of drake or a pegged revision -# If the environment variable DAIRLIB_LOCAL_DRAKE_PATH is set, it will use -# a local version, ad the specified path. Otherwise, it will get a pegged -# revision from github. -# As an example, -# export DAIRLIB_LOCAL_DRAKE_PATH=/home/user/workspace/drake - -# Choose a revision of Drake to use. -DRAKE_COMMIT = "v1.22.0" - -DRAKE_CHECKSUM = "78cf62c177c41f8415ade172c1e6eb270db619f07c4b043d5148e1f35be8da09" -# Before changing the COMMIT, temporarily uncomment the next line so that Bazel -# displays the suggested new value for the CHECKSUM. -#DRAKE_CHECKSUM = "0" * 64 - -# Load an environment variable. -load("//:environ.bzl", "environ_repository") - -environ_repository( - name = "environ", - vars = ["DAIRLIB_LOCAL_DRAKE_PATH"], -) - -load("@environ//:environ.bzl", "DAIRLIB_LOCAL_DRAKE_PATH") - -# The WORKSPACE file does not permit `if` statements, so we handle the local -# option by toying with the repository names. The selected repository is named -# "@drake", the other is named "@drake_ignored". -(_http_drake_repo_name, _local_drake_repo_name) = ( - "drake_ignored" if DAIRLIB_LOCAL_DRAKE_PATH else "drake", - "drake" if DAIRLIB_LOCAL_DRAKE_PATH else "drake_ignored", -) - -# Maybe download Drake. -load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") - -http_archive( - name = _http_drake_repo_name, - sha256 = DRAKE_CHECKSUM, - strip_prefix = "drake-{}".format(DRAKE_COMMIT.strip("v")), - urls = [x.format(DRAKE_COMMIT) for x in [ - "https://github.com/RobotLocomotion/drake/archive/{}.tar.gz", - ]], -) - -# Maybe use a local checkout of Drake. -print("Using DAIRLIB_LOCAL_DRAKE_PATH={}".format(DAIRLIB_LOCAL_DRAKE_PATH)) if DAIRLIB_LOCAL_DRAKE_PATH else None # noqa - -local_repository( - name = _local_drake_repo_name, - path = DAIRLIB_LOCAL_DRAKE_PATH, -) - -# Reference external software libraries and tools 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("@dairlib//tools/workspace/osqp:repository.bzl", "osqp_repository") - -osqp_repository(name = "osqp") - -load("@dairlib//tools/workspace/signal_scope:repository.bzl", "signal_scope_repository") - -signal_scope_repository(name = "signal_scope") - -load("@dairlib//tools/workspace/pydrake:repository.bzl", "pydrake_repository") - -pydrake_repository(name = "pydrake_pegged") - -# Prebuilt ROS workspace -new_local_repository( - name = "ros", - build_file = "tools/workspace/ros/ros.bazel", - path = "tools/workspace/ros/bundle_ws/install", -) - -# Other catkin packages from source -# TODO: generate this automatically from rosinstall_generator - -http_archive( - name = "genmsg_repo", - build_file = "@//tools/workspace/ros/bazel:genmsg.BUILD", - sha256 = "d7627a2df169e4e8208347d9215e47c723a015b67ef3ed8cda8b61b6cfbf94d2", - strip_prefix = "genmsg-0.5.8", - urls = ["https://github.com/ros/genmsg/archive/0.5.8.tar.gz"], -) - -http_archive( - name = "genpy_repo", - build_file = "@//tools/workspace/ros/bazel:genpy.BUILD", - sha256 = "35e5cd2032f52a1f77190df5c31c02134dc460bfeda3f28b5a860a95309342b9", - strip_prefix = "genpy-0.6.5", - urls = ["https://github.com/ros/genpy/archive/0.6.5.tar.gz"], -) - -# dairlib can use either a local version of invariant-ekf or a pegged revision -# If the environment variable DAIRLIB_LOCAL_INEKF_PATH is set, it will use -# a local version, ad the specified path. Otherwise, it will get a pegged -# revision from github. -# As an example, -# export DAIRLIB_LOCAL_INEKF_PATH=/home/user/workspace/invariant-ekf - -# Choose a revision of InEKF to use. -INEKF_COMMIT = "7fde9f84dbe536ba9439a3b8c319efb51ff760dd" - -INEKF_CHECKSUM = "f87e3262b0c9c9237881fcd539acd1c60000f97dfdfa47b0ae53cb7a0f3256e4" - -# Before changing the COMMIT, temporarily uncomment the next line so that Bazel -# displays the suggested new value for the CHECKSUM. -# INEKF_CHECKSUM = "0" * 64 - -# Load an environment variable. -environ_repository( - name = "environ_inekf", - vars = ["DAIRLIB_LOCAL_INEKF_PATH"], -) - -load("@environ_inekf//:environ.bzl", "DAIRLIB_LOCAL_INEKF_PATH") - -# The WORKSPACE file does not permit `if` statements, so we handle the local -# option by toying with the repository names. The selected repository is named -# "@inekf", the other is named "@inekf_ignored". -(_http_inekf_repo_name, _local_inekf_repo_name) = ( - "inekf_ignored" if DAIRLIB_LOCAL_INEKF_PATH else "inekf", - "inekf" if DAIRLIB_LOCAL_INEKF_PATH else "inekf_ignored", -) - -# Maybe download InEKF. -load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") - -http_archive( - name = _http_inekf_repo_name, - sha256 = INEKF_CHECKSUM, - strip_prefix = "invariant-ekf-{}".format(INEKF_COMMIT), - urls = [x.format(INEKF_COMMIT) for x in [ - "https://github.com/DAIRLab/invariant-ekf/archive/{}.tar.gz", - ]], -) - -# Maybe use a local checkout of InEKF. -print("Using DAIRLIB_LOCAL_INEKF_PATH={}".format(DAIRLIB_LOCAL_INEKF_PATH)) if DAIRLIB_LOCAL_INEKF_PATH else None # noqa - -local_repository( - name = _local_inekf_repo_name, - path = DAIRLIB_LOCAL_INEKF_PATH, -) - -load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") - -# buildifier is written in Go and hence needs rules_go to be built. -# See https://github.com/bazelbuild/rules_go for the up to date setup instructions. -http_archive( - name = "io_bazel_rules_go", - sha256 = "d6b2513456fe2229811da7eb67a444be7785f5323c6708b38d851d2b51e54d83", - urls = [ - "https://mirror.bazel.build/github.com/bazelbuild/rules_go/releases/download/v0.30.0/rules_go-v0.30.0.zip", - "https://github.com/bazelbuild/rules_go/releases/download/v0.30.0/rules_go-v0.30.0.zip", - ], -) - -load("@io_bazel_rules_go//go:deps.bzl", "go_rules_dependencies") - -go_rules_dependencies() - -load("@io_bazel_rules_go//go:deps.bzl", "go_register_toolchains") - -go_register_toolchains(version = "1.17.2") - -http_archive( - name = "bazel_gazelle", - sha256 = "de69a09dc70417580aabf20a28619bb3ef60d038470c7cf8442fafcf627c21cb", - urls = [ - "https://mirror.bazel.build/github.com/bazelbuild/bazel-gazelle/releases/download/v0.24.0/bazel-gazelle-v0.24.0.tar.gz", - "https://github.com/bazelbuild/bazel-gazelle/releases/download/v0.24.0/bazel-gazelle-v0.24.0.tar.gz", - ], -) - -load("@bazel_gazelle//:deps.bzl", "gazelle_dependencies") - -# If you use WORKSPACE.bazel, use the following line instead of the bare gazelle_dependencies(): -# gazelle_dependencies(go_repository_default_config = "@//:WORKSPACE.bazel") -gazelle_dependencies() - -http_archive( - name = "com_google_protobuf", - sha256 = "3bd7828aa5af4b13b99c191e8b1e884ebfa9ad371b0ce264605d347f135d2568", - strip_prefix = "protobuf-3.19.4", - urls = [ - "https://github.com/protocolbuffers/protobuf/archive/v3.19.4.tar.gz", - ], -) - -load("@com_google_protobuf//:protobuf_deps.bzl", "protobuf_deps") - -protobuf_deps() - -http_archive( - name = "com_github_bazelbuild_buildtools", - sha256 = "ae34c344514e08c23e90da0e2d6cb700fcd28e80c02e23e4d5715dddcb42f7b3", - strip_prefix = "buildtools-4.2.2", - urls = [ - "https://github.com/bazelbuild/buildtools/archive/refs/tags/4.2.2.tar.gz", - ], -) +# This file marks the root of the Bazel workspace. +# See MODULE.bazel for external dependencies setup. \ No newline at end of file diff --git a/tools/workspace/ros/bazel/BUILD b/bindings/__init__.py similarity index 100% rename from tools/workspace/ros/bazel/BUILD rename to bindings/__init__.py diff --git a/bindings/pydairlib/analysis/BUILD.bazel b/bindings/pydairlib/analysis/BUILD.bazel index 5c7b90d387..996b39a0b7 100644 --- a/bindings/pydairlib/analysis/BUILD.bazel +++ b/bindings/pydairlib/analysis/BUILD.bazel @@ -12,9 +12,21 @@ load( ) py_binary( - name = "process_lcm_log", - srcs = ["process_lcm_log.py"], - deps = ["@lcm//:lcm-python"], + name = "qp_test", + srcs = ["qp_test.py"], + deps = [ + "//lcmtypes:lcmtypes_robot_py", + "@lcm//:lcm-python", + ], +) + +py_binary( + name = "osqp_settings_sandbox", + srcs = ["osqp_settings_sandbox.py"], + deps = [ + "//lcmtypes:lcmtypes_robot_py", + "@lcm//:lcm-python", + ], ) py_library( @@ -29,6 +41,25 @@ py_library( deps = [], ) +py_library( + name = "franka_plot_config", + srcs = ["franka_plot_config.py"], + deps = [], +) + +py_library( + name = "spring_compensation", + srcs = ["spring_compensation.py"], + deps = [ + ":osc_debug_py", + "//bindings/pydairlib/common:plot_styler", + "//bindings/pydairlib/common:plotting_utils", + "//bindings/pydairlib/lcm", + "//bindings/pydairlib/multibody:kinematic_py", + "//bindings/pydairlib/multibody:multibody_py", + ], +) + py_library( name = "mbp_plotting_utils", srcs = ["mbp_plotting_utils.py"], @@ -51,7 +82,7 @@ py_library( "@lcm//:lcm-python", ], deps = [ - "//bindings/pydairlib/cassie:cassie_utils", + "//bindings/pydairlib/cassie", "//bindings/pydairlib/lcm", "//bindings/pydairlib/multibody:kinematic_py", "//bindings/pydairlib/multibody:multibody_py", @@ -60,6 +91,20 @@ py_library( ], ) +py_library( + name = "franka_plotting_utils", + srcs = ["franka_plotting_utils.py"], + data = [ + "@drake_models//:franka_description", + "@lcm//:lcm-python", + ], + deps = [ + "//bindings/pydairlib/lcm", + "//lcmtypes:lcmtypes_robot_archive_py", + "//lcmtypes:lcmtypes_robot_py", + ], +) + py_binary( name = "log_plotter_cassie", srcs = ["log_plotter_cassie.py"], @@ -69,6 +114,28 @@ py_binary( ], deps = [ ":cassie_plot_config", + "//bindings/pydairlib/analysis:mbp_plotting_utils", + "//bindings/pydairlib/cassie:cassie_utils_py", + "//bindings/pydairlib/common", + "//bindings/pydairlib/common:plot_styler", + "//bindings/pydairlib/lcm", + "//bindings/pydairlib/multibody:kinematic_py", + "//bindings/pydairlib/multibody:multibody_py", + "//lcmtypes:lcmtypes_robot_archive_py", + "//lcmtypes:lcmtypes_robot_py", + ], +) + +py_binary( + name = "log_plotter_franka", + srcs = ["log_plotter_franka.py"], + data = [ + "@lcm//:lcm-python", + ], + deps = [ + ":franka_plot_config", + "//bindings/pydairlib/analysis:franka_plotting_utils", + "//bindings/pydairlib/analysis:mbp_plotting_utils", "//bindings/pydairlib/cassie:cassie_utils_py", "//bindings/pydairlib/common", "//bindings/pydairlib/common:plot_styler", @@ -78,3 +145,34 @@ py_binary( "//lcmtypes:lcmtypes_robot_py", ], ) + +# This determines how `PYTHONPATH` is configured, and how to install the +# bindings. +PACKAGE_INFO = get_pybind_package_info("//bindings") + +py_library( + name = "module_py", + srcs = [ + "__init__.py", + ], + imports = PACKAGE_INFO.py_imports, + deps = [ + "//bindings/pydairlib:module_py", + ], +) + +PY_LIBRARIES = [ + ":cassie_plotting_utils", + ":franka_plotting_utils", + ":mbp_plotting_utils", + ":spring_compensation", + ":osc_debug_py", + ":cassie_plot_config", +] + +# Package roll-up (for Bazel dependencies). +py_library( + name = "analysis", + imports = PACKAGE_INFO.py_imports, + deps = PY_LIBRARIES, +) diff --git a/bindings/pydairlib/analysis/__init__.py b/bindings/pydairlib/analysis/__init__.py new file mode 100644 index 0000000000..c4361f107d --- /dev/null +++ b/bindings/pydairlib/analysis/__init__.py @@ -0,0 +1,2 @@ +# Importing everything in this directory to this package +from .analysis import * \ No newline at end of file diff --git a/bindings/pydairlib/analysis/cassie_plot_config.py b/bindings/pydairlib/analysis/cassie_plot_config.py index 0ab6e3c1db..c91ed33627 100644 --- a/bindings/pydairlib/analysis/cassie_plot_config.py +++ b/bindings/pydairlib/analysis/cassie_plot_config.py @@ -14,6 +14,7 @@ def __init__(self, filename): self.channel_u = data['channel_u'] self.channel_osc = data['channel_osc'] self.use_archived_lcmtypes = data['use_archived_lcmtypes'] + self.plot_style = data['plot_style'] self.use_floating_base = data['use_floating_base'] self.use_springs = data['use_springs'] self.start_time = data['start_time'] @@ -48,5 +49,7 @@ def __init__(self, filename): self.plot_tracking_costs = data['plot_tracking_costs'] self.plot_qp_solve_time = data['plot_qp_solve_time'] self.fsm_state_names = data['fsm_state_names'] + self.plot_qp_solutions = data['plot_qp_solutions'] + self.plot_active_tracking_datas = data['plot_active_tracking_datas'] self.tracking_datas_to_plot = \ data['tracking_datas_to_plot'] if data['tracking_datas_to_plot'] else [] diff --git a/bindings/pydairlib/analysis/cassie_plotting_utils.py b/bindings/pydairlib/analysis/cassie_plotting_utils.py index 672cb6574a..0f7a564d15 100644 --- a/bindings/pydairlib/analysis/cassie_plotting_utils.py +++ b/bindings/pydairlib/analysis/cassie_plotting_utils.py @@ -1,11 +1,9 @@ # python imports import lcm import numpy as np -import matplotlib.pyplot as plt # lcmtype imports import dairlib -import archive.dairlib import drake # dairlib python binding imports @@ -39,11 +37,7 @@ 'OSC_STANDING': dairlib.lcmt_robot_input, 'OSC_JUMPING': dairlib.lcmt_robot_input, 'OSC_RUNNING': dairlib.lcmt_robot_input, - 'CASSIE_OUTPUT': dairlib.lcmt_cassie_out, - 'OSC_DEBUG_STANDING': archive.dairlib.lcmt_osc_output, - 'OSC_DEBUG_WALKING': archive.dairlib.lcmt_osc_output, - 'OSC_DEBUG_JUMPING': archive.dairlib.lcmt_osc_output, - 'OSC_DEBUG_RUNNING': archive.dairlib.lcmt_osc_output} + 'CASSIE_OUTPUT': dairlib.lcmt_cassie_out} cassie_contact_channels = \ {'CASSIE_CONTACT_DRAKE': drake.lcmt_contact_results_for_viz, @@ -55,10 +49,10 @@ def make_plant_and_context(floating_base=True, springs=True): plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) if springs: AddCassieMultibody(plant, scene_graph, - floating_base, cassie_urdf, True, True) + floating_base, cassie_urdf, True, True, True) else: AddCassieMultibody(plant, scene_graph, - floating_base, cassie_urdf_no_springs, False, True) + floating_base, cassie_urdf_no_springs, False, True, True) plant.Finalize() return plant, plant.CreateDefaultContext() diff --git a/bindings/pydairlib/analysis/franka_plot_config.py b/bindings/pydairlib/analysis/franka_plot_config.py new file mode 100644 index 0000000000..cae37dc58f --- /dev/null +++ b/bindings/pydairlib/analysis/franka_plot_config.py @@ -0,0 +1,51 @@ +import io +from yaml import load, dump + +try: + from yaml import CLoader as Loader, CDumper as Dumper +except ImportError: + from yaml import Loader, Dumper + + +class FrankaPlotConfig(): + def __init__(self, filename): + data = load(io.open(filename, 'r'), Loader=Loader) + self.channel_x = data['channel_x'] + self.channel_u = data['channel_u'] + self.channel_osc = data['channel_osc'] + self.channel_c3 = data['channel_c3'] + self.channel_tray = data['channel_tray'] + self.channel_c3_target = data['channel_c3_target'] + self.channel_c3_actual = data['channel_c3_actual'] + self.plot_style = data['plot_style'] + + self.start_time = data['start_time'] + self.duration = data['duration'] + self.plot_joint_positions = data['plot_joint_positions'] + self.plot_joint_velocities = data['plot_joint_velocities'] + self.plot_measured_efforts = data['plot_measured_efforts'] + self.plot_commanded_efforts = data['plot_commanded_efforts'] + self.plot_contact_forces = data['plot_contact_forces'] + self.plot_end_effector = data['plot_end_effector'] + self.pos_names = \ + data['special_positions_to_plot'] if \ + data['special_positions_to_plot'] else [] + self.vel_names = \ + data['special_velocities_to_plot'] if \ + data['special_velocities_to_plot'] else [] + self.act_names = \ + data['special_efforts_to_plot'] if \ + data['special_efforts_to_plot'] else [] + + self.fsm_state_names = data['fsm_state_names'] + + self.plot_qp_costs = data['plot_qp_costs'] + self.plot_qp_solve_time = data['plot_qp_solve_time'] + self.plot_qp_solutions = data['plot_qp_solutions'] + self.plot_tracking_costs = data['plot_tracking_costs'] + self.plot_active_tracking_datas = data['plot_active_tracking_datas'] + self.tracking_datas_to_plot = \ + data['tracking_datas_to_plot'] if data['tracking_datas_to_plot'] else [] + self.plot_c3_debug = data['plot_c3_debug'] + self.plot_c3_tracking = data['plot_c3_tracking'] + self.plot_object_state = data['plot_object_state'] diff --git a/bindings/pydairlib/analysis/franka_plotting_utils.py b/bindings/pydairlib/analysis/franka_plotting_utils.py new file mode 100644 index 0000000000..e9c6cb8b86 --- /dev/null +++ b/bindings/pydairlib/analysis/franka_plotting_utils.py @@ -0,0 +1,63 @@ +# python imports +import lcm +import numpy as np +import matplotlib.pyplot as plt + +# lcmtype imports +import dairlib +import drake + +# drake imports +from pydrake.multibody.plant import AddMultibodyPlantSceneGraph +from pydrake.systems.framework import DiagramBuilder +from pydrake.all import MultibodyPlant, Parser, RigidTransform, \ + FindResourceOrThrow + +franka_urdf = "package://drake_models/franka_description/urdf/panda_arm.urdf" +end_effector_model = "examples/franka/urdf/plate_end_effector.urdf" +tray_model = "examples/franka/urdf/tray.sdf" +tool_attachment_frame = np.array([0, 0, 0.107]) + +franka_default_channels = \ + {'FRANKA_STATE': dairlib.lcmt_robot_output, + 'FRANKA_STATE_SIMULATION': dairlib.lcmt_robot_output, + 'TRAY_STATE': dairlib.lcmt_object_state, + 'TRAY_STATE_SIMULATION': dairlib.lcmt_object_state, + 'FRANKA_INPUT': dairlib.lcmt_robot_input, + 'OSC_FRANKA': dairlib.lcmt_robot_input, + 'FRANKA_INPUT_ECHO': dairlib.lcmt_robot_input, + 'C3_TRAJECTORY_ACTOR': dairlib.lcmt_timestamped_saved_traj, + 'C3_TRAJECTORY_TRAY': dairlib.lcmt_timestamped_saved_traj, + 'C3_ACTUAL': dairlib.lcmt_c3_state, + 'C3_TARGET': dairlib.lcmt_c3_state, + 'OSC_DEBUG_FRANKA': dairlib.lcmt_osc_output, + 'RADIO': dairlib.lcmt_radio_out, + 'CONTACT_RESULTS': drake.lcmt_contact_results_for_viz} + + +def make_plant_and_context(): + builder = DiagramBuilder() + + franka_plant = MultibodyPlant(0.0) + franka_parser = Parser(franka_plant) + franka_parser.AddModelsFromUrl(franka_urdf) + end_effector_index = \ + franka_parser.AddModels(end_effector_model)[0] + T_EE_W = RigidTransform(tool_attachment_frame) + + franka_plant.WeldFrames(franka_plant.world_frame(), + franka_plant.GetFrameByName("panda_link0"), + RigidTransform()) + franka_plant.WeldFrames(franka_plant.GetFrameByName("panda_link7"), + franka_plant.GetFrameByName("plate", + end_effector_index), + T_EE_W) + + tray_plant = MultibodyPlant(0.0) + tray_parser = Parser(tray_plant) + tray_parser.AddModels(tray_model) + + franka_plant.Finalize() + tray_plant.Finalize() + return (franka_plant, franka_plant.CreateDefaultContext(), tray_plant, + tray_plant.CreateDefaultContext()) diff --git a/bindings/pydairlib/analysis/impact_invariant_control/BUILD.bazel b/bindings/pydairlib/analysis/impact_invariant_control/BUILD.bazel new file mode 100644 index 0000000000..f6e3b5bddc --- /dev/null +++ b/bindings/pydairlib/analysis/impact_invariant_control/BUILD.bazel @@ -0,0 +1,51 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +py_binary( + name = "plot_five_link_biped", + srcs = ["plot_five_link_biped.py"], + data = [ + "@lcm//:lcm-python", + ], + deps = [ + "//bindings/pydairlib/analysis:mbp_plotting_utils", + "//bindings/pydairlib/analysis:osc_debug_py", + "//bindings/pydairlib/cassie:cassie_utils_py", + "//bindings/pydairlib/common", + "//bindings/pydairlib/lcm", + "//bindings/pydairlib/lcm:process_lcm_log", + "//bindings/pydairlib/multibody:kinematic_py", + "//bindings/pydairlib/multibody:multibody_py", + "//lcmtypes:lcmtypes_robot_py", + ], +) + +py_binary( + name = "jumping_parameter_study", + srcs = ["jumping_parameter_study.py"], + data = [ + "@lcm//:lcm-python", + ], + deps = [ + "//bindings/pydairlib/analysis:cassie_plotting_utils", + "//bindings/pydairlib/analysis:mbp_plotting_utils", + "//bindings/pydairlib/analysis:osc_debug_py", + "//bindings/pydairlib/cassie:cassie_utils_py", + "//bindings/pydairlib/common", + "//bindings/pydairlib/lcm", + "//bindings/pydairlib/lcm:process_lcm_log", + "//bindings/pydairlib/multibody:kinematic_py", + "//bindings/pydairlib/multibody:multibody_py", + "//lcmtypes:lcmtypes_robot_py", + ], +) diff --git a/bindings/pydairlib/analysis/impact_invariant_control/jumping_parameter_study.py b/bindings/pydairlib/analysis/impact_invariant_control/jumping_parameter_study.py new file mode 100644 index 0000000000..081fc1ffbb --- /dev/null +++ b/bindings/pydairlib/analysis/impact_invariant_control/jumping_parameter_study.py @@ -0,0 +1,397 @@ +import subprocess +import time +from tqdm import * +import lcm +import matplotlib.pyplot as plt +from matplotlib.patches import Patch +import matplotlib.colors +import fileinput +import dairlib +from bindings.pydairlib.common import plot_styler, plotting_utils +from bindings.pydairlib.cassie.cassie_utils import * +from bindings.pydairlib.lcm.process_lcm_log import get_log_data +import pydairlib.analysis.mbp_plotting_utils as mbp_plots +from bindings.pydairlib.lcm import lcm_trajectory +from bindings.pydairlib.multibody import MakeNameToPositionsMap, \ + MakeNameToVelocitiesMap, MakeNameToActuatorsMap, \ + CreateStateNameVectorFromMap, CreateActuatorNameVectorFromMap +import pydairlib.analysis.cassie_plotting_utils as cassie_plots +import numpy as np +import glob +from matplotlib.ticker import FormatStrFormatter + + + +def main(): + trajectory_path = "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/" + controller_type = "jumping" + parameter = "landing_delay" + simulator = 'DRAKE' + # trajectory_names = ['jump', 'box_jump', 'long_jump', 'down_jump'] + trajectory_names = ['down_jump'] + gains_path = '' + trajectory_name = '' + results_folder = '' + delay_time = 1.0 + traj_time = 3.0 + sim_time = 0 + start_time = 0 + if controller_type == 'jumping': + gains_path = "/home/yangwill/workspace/dairlib/examples/Cassie/osc_jump/" + results_folder = "/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/sim/jumping/logs/2023/param_study/" + sim_time = delay_time + traj_time + + landing_times = np.arange(0.000, 0.055, 0.005) + # nominal_delay = 0.040 # box + # nominal_delay = 0.030 # long + nominal_delay = 0.030 # down + # nominal_delay = 0.000 # jump + # nominal_threshold = 0.000 # box + # nominal_threshold = 0.000 # long + nominal_threshold = 0.000 # down + # nominal_threshold = 0.000 # jump + landing_times += nominal_delay - 0.5 * 0.05 + # impact_thresholds = np.arange(0.000, 0.100, 0.025) + impact_thresholds = np.arange(0.000, 0.110, 0.010) + + realtime_rate = 0.25 + publish_rate = 2000.0 + + # Add an extra second to the runtime of the simulator process to account for start up and stopping time + sim_run_time = sim_time / realtime_rate + 1.0 + controller_startup_time = 1.0 + parameter_file = open(results_folder + 'command_list.txt', 'w') + controller_cmd = '' + simulator_cmd = '' + lcm_logger_cmd = '' + save_gains_cmd = '' + + # for i in range(0, disturbances.shape[0]): + for i in range(5): + for traj in trajectory_names: + for impact_threshold in impact_thresholds: + for landing_time in landing_times: + log_suffix = '' + if parameter == 'landing_delay': + log_suffix = 't_%.3f' % (landing_time) + '_duration_%.3f' % ( + impact_threshold) + log_path = results_folder + traj + '_' + str(i) + '/lcmlog-' + log_suffix + print(log_path) + gain_filename = '' + if traj == 'jump' or traj == 'long_jump': + if traj == 'jump': + gain_filename = 'osc_jumping_gains.yaml' + elif traj == 'long_jump': + gain_filename = 'osc_jumping_gains_long.yaml' + controller_cmd = [ + 'bazel-bin/examples/Cassie/run_osc_jumping_controller', + '--delay_time=%.1f' % delay_time, + '--channel_u=OSC_JUMPING', + '--gains_filename=examples/Cassie/osc_jump/osc_jumping_gains_param.yaml', + '--traj_name=%s' % traj, + ] + simulator_cmd = ['bazel-bin/examples/Cassie/multibody_sim', + '--init_height=%.1f' % 0.9, + '--toe_spread=0.12', + '--target_realtime_rate=%.2f' % realtime_rate, + '--dt=%.5f' % 1e-3, + '--publish_rate=%d' % publish_rate, + '--end_time=%.3f' % sim_time, + '--channel_u=OSC_JUMPING', + ] + elif traj == 'box_jump': + controller_cmd = [ + 'bazel-bin/examples/Cassie/run_osc_jumping_controller', + '--delay_time=%.1f' % delay_time, + '--channel_u=OSC_JUMPING', + '--traj_name=%s' % traj, + '--gains_filename=examples/Cassie/osc_jump/osc_jumping_gains_param.yaml', + ] + simulator_cmd = ['bazel-bin/examples/Cassie/multibody_sim_w_platform', + '--init_height=%.1f' % 0.9, + '--target_realtime_rate=%.2f' % realtime_rate, + '--dt=%.5f' % 1e-3, + '--publish_rate=%d' % publish_rate, + '--end_time=%.3f' % sim_time, + '--channel_u=OSC_JUMPING', + '--traj_name=box_jump', + '--platform_height=0.5', + '--platform_x=0.25', + '--visualize=1', + ] + gain_filename = 'osc_jumping_gains_box.yaml' + elif traj == 'down_jump': + controller_cmd = [ + 'bazel-bin/examples/Cassie/run_osc_jumping_controller', + '--delay_time=%.1f' % delay_time, + '--channel_u=OSC_JUMPING', + '--traj_name=%s' % traj, + '--gains_filename=examples/Cassie/osc_jump/osc_jumping_gains_param.yaml', + ] + simulator_cmd = ['bazel-bin/examples/Cassie/multibody_sim_w_platform', + '--init_height=%.1f' % 0.9, + '--target_realtime_rate=%.2f' % realtime_rate, + '--dt=%.5f' % 1e-3, + '--publish_rate=%d' % publish_rate, + '--end_time=%.3f' % sim_time, + '--channel_u=OSC_JUMPING', + '--traj_name=down_jump', + '--platform_height=0.5', + '--platform_x=-0.99', + '--visualize=1', + ] + gain_filename = 'osc_jumping_gains_down.yaml' + + f = open(gains_path + gain_filename, 'r') + filedata = f.read() + f.close() + newdata = filedata.replace('impact_threshold: %.3f' % nominal_threshold, + 'impact_threshold: %.3f' % impact_threshold) + newdata = newdata.replace('landing_delay: %.3f' % nominal_delay, + 'landing_delay: %.3f' % landing_time) + f = open(gains_path + 'osc_jumping_gains_param.yaml', 'w') + f.write(newdata) + f.close() + + lcm_logger_cmd = ['lcm-logger', + '-f', + '%s' % log_path, + ] + + parameter_file.write(log_path + '\n') + parameter_file.write(' '.join(controller_cmd) + '\n') + parameter_file.write(' '.join(simulator_cmd) + '\n') + parameter_file.write('**********\n') + controller_process = subprocess.Popen(controller_cmd) + logger_process = subprocess.Popen(lcm_logger_cmd) + time.sleep(controller_startup_time) + simulator_process = subprocess.Popen(simulator_cmd) + time.sleep(sim_run_time) + simulator_process.kill() + controller_process.kill() + logger_process.kill() + + parameter_file.close() + + +def convert_logs_to_costs(): + results_folder = "/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/sim/jumping/logs/2023/param_study/" + data_directory = "/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/sim/jumping/logs/2023/param_study/data/" + + default_channels = {'CASSIE_STATE_SIMULATION': dairlib.lcmt_robot_output, + 'OSC_JUMPING': dairlib.lcmt_robot_input, + 'OSC_DEBUG_JUMPING': dairlib.lcmt_osc_output} + callback = mbp_plots.load_default_channels + plant, context = cassie_plots.make_plant_and_context( + floating_base=True, springs=True) + + nx = plant.num_positions() + plant.num_velocities() + nu = plant.num_actuators() + + osc_traj0 = "pelvis_trans_traj" + osc_traj1 = "left_ft_traj" + osc_traj2 = "right_ft_traj" + osc_traj3 = "pelvis_rot_traj" + + # nominal_delay = 0.040 # box + nominal_delay = 0.030 # long + # nominal_delay = 0.030 # down + # nominal_delay = 0.000 # jump + + # For full jumping traj + t_samples = 7500 + u_samples = 7500 + start_time = 0 + duration = -1 + landing_times = np.arange(0.000, 0.055, 0.005) + impact_thresholds = np.arange(0.000, 0.110, 0.010) + success = np.zeros((landing_times.shape[0], impact_thresholds.shape[0])) + + t_matrix = np.zeros((landing_times.shape[0], impact_thresholds.shape[0], t_samples)) + # x_matrix = np.zeros((landing_times.shape[0], impact_thresholds.shape[0], t_samples, nx)) + t_u_matrix = np.zeros((landing_times.shape[0], impact_thresholds.shape[0], u_samples)) + u_matrix = np.zeros((landing_times.shape[0], impact_thresholds.shape[0], u_samples, nu)) + t_osc_matrix = np.zeros((landing_times.shape[0], impact_thresholds.shape[0], u_samples)) + tracking_cost = np.zeros((landing_times.shape[0], impact_thresholds.shape[0], u_samples)) + + trial_name = 'long_jump_4' + all_logs = sorted(glob.glob(results_folder + trial_name + '/lcmlog-*')) + success = np.zeros((landing_times.shape[0], impact_thresholds.shape[0])) + # xx, yy = np.meshgrid(landing_times, impact_thresholds) + for log_filename in all_logs: + landing_time = log_filename.split('_')[-3] + impact_threshold = log_filename.split('_')[-1] + land_idx = int(np.round((float(landing_time) - (nominal_delay - 0.025)) / 0.005)) + impact_idx = int(np.round(float(impact_threshold)/0.010)) + print(land_idx) + print(impact_idx) + + log = lcm.EventLog(log_filename, "r") + robot_output, robot_input, osc_debug = \ + get_log_data(log, default_channels, start_time, duration, callback, + plant, + 'CASSIE_STATE_SIMULATION', 'OSC_JUMPING', + 'OSC_DEBUG_JUMPING') + + t_matrix[land_idx, impact_idx, :] = robot_output['t_x'][:t_samples] + # x_matrix[land_idx, impact_idx, :, :] = robot_output['q'][:t_samples] + t_u_matrix[land_idx, impact_idx, :] = robot_input['t_u'][:u_samples] + u_matrix[land_idx, impact_idx, :, :] = robot_input['u'][:u_samples] + t_osc_matrix[land_idx, impact_idx, :] = osc_debug['t_osc'][:u_samples] + tracking_cost[land_idx, impact_idx, :] = osc_debug['tracking_cost'][osc_traj0][:u_samples] + tracking_cost[land_idx, impact_idx, :] += osc_debug['tracking_cost'][osc_traj1][:u_samples] + tracking_cost[land_idx, impact_idx, :] += osc_debug['tracking_cost'][osc_traj2][:u_samples] + tracking_cost[land_idx, impact_idx, :] += osc_debug['tracking_cost'][osc_traj3][:u_samples] + + np.save(data_directory + trial_name + '/t_u', t_u_matrix) + np.save(data_directory + trial_name + '/u', u_matrix) + np.save(data_directory + trial_name + '/t_osc', t_osc_matrix) + np.save(data_directory + trial_name + '/tracking_cost', tracking_cost) + return + +def construct_success_plot(): + results_folder = "/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/sim/jumping/logs/2023/param_study/" + landing_times = np.arange(0.000, 0.055, 0.005) + # nominal_delay = 0.040 # box + # nominal_delay = 0.030 # long + nominal_delay = 0.030 # down + # nominal_delay = 0.000 # jump + # trial_name = 'down_jump_' + trial_name = 'long_jump_' + landing_times += nominal_delay - 0.5 * 0.05 + impact_thresholds = np.arange(0.000, 0.110, 0.010) + + start_time = 4 + duration = -1 + default_channels = {'CASSIE_STATE_SIMULATION': dairlib.lcmt_robot_output, + 'OSC_JUMPING': dairlib.lcmt_robot_input, + 'OSC_DEBUG_JUMPING': dairlib.lcmt_osc_output} + callback = mbp_plots.load_default_channels + plant, context = cassie_plots.make_plant_and_context( + floating_base=True, springs=True) + for i in range(5): + all_logs = sorted(glob.glob(results_folder + trial_name + str(i) + '/lcmlog-*')) + success = np.zeros((landing_times.shape[0], impact_thresholds.shape[0])) + for log_filename in all_logs: + landing_time = log_filename.split('_')[-3] + impact_threshold = log_filename.split('_')[-1] + + log = lcm.EventLog(log_filename, "r") + robot_output, robot_input, osc_debug = \ + get_log_data(log, default_channels, start_time, duration, callback, + plant, + 'CASSIE_STATE_SIMULATION', 'OSC_JUMPING', + 'OSC_DEBUG_JUMPING') + land_idx = int(np.round((float(landing_time) - (nominal_delay - 0.025)) / 0.005)) + print(landing_time) + impact_idx = int(np.round(float(impact_threshold)/0.010)) + print(land_idx) + print(impact_idx) + success[land_idx, impact_idx] = not np.any(robot_output['q'][:, 6] < 0.4) + np.save(trial_name + str(i) + '_success', success) + +def plot_success(): + trial_name = 'long_jump_' + landing_times = np.linspace(0.000, 0.050, 11) + landing_times -= 0.025 + impact_thresholds = np.linspace(0.000, 0.100, 11) + # success = np.load('long_jump_0_success.npy') + success = np.load(trial_name + '0_success.npy') + total_success = np.zeros(success.shape) + total_success += success + for i in range(1, 5): + success = np.load(trial_name + str(i) + '_success.npy') + # success = np.load('down_jump_' + str(i) + '_success.npy') + total_success += success + + # plt.imshow(success, cmap='tab20') + ps = plot_styler.PlotStyler() + plot_styler.PlotStyler.set_default_styling() + cmap = matplotlib.colors.ListedColormap([ps.grey, ps.blue]) + + # plt.imshow(total_success, cmap=cmap) + im = plt.imshow(total_success, cmap='Reds') + # im = plt.contour(total_success, cmap='Reds') + plt.colorbar(im) + plt.xlabel('Projection Window Duration (s)') + plt.ylabel('Deviation from Nominal Transition Time (s)') + ax = plt.gca() + np.set_printoptions(precision=3) + ax.set_xticks(np.arange(0, 11, 1)) + ax.set_yticks(np.arange(0, 11, 1)) + ax.set_xticklabels(np.around(impact_thresholds, 3).tolist()) + ax.set_yticklabels(np.around(landing_times, 3).tolist()) + ax.grid(which="minor", color='k', linestyle='-', linewidth=2) + + # legend_elements = [Patch(facecolor=ps.grey, alpha=0.7, label='Fail'), Patch(facecolor=ps.blue, alpha=0.7, label='Success')] + # legend = ax.legend(handles=legend_elements, loc=1) + plt.savefig(trial_name + 'success.png', dpi=240) + # plt.savefig('down_jump_success.png', dpi=240) + plt.show() + +def plot_costs(): + landing_times = np.linspace(0.000, 0.050, 11) + landing_times -= 0.025 + impact_thresholds = np.linspace(0.000, 0.100, 11) + results_folder = "/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/sim/jumping/logs/2023/param_study/" + data_directory = "/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/sim/jumping/logs/2023/param_study/data/" + t_u_matrix = np.load(data_directory + 'long_jump_0' + '/t_u.npy') + u_cost = np.zeros((t_u_matrix.shape[0], t_u_matrix.shape[1])) + u_slice = slice(3000, 7000) + w_u = np.array([0.5, 0.9, 0.5, 0.1, 0.1, 0.5, 0.9, 0.5, 0.1, 0.1]) + W_u = np.diag(w_u) + fails = np.zeros(u_cost.shape) + trial_name = 'long_jump_' + for num in trange(5): + exp_name = trial_name + str(num) + success = np.load(exp_name + '_success.npy') + t_u_matrix = np.load(data_directory + exp_name + '/t_u.npy') + u_matrix = np.load(data_directory + exp_name + '/u.npy') + t_osc_matrix = np.load(data_directory + exp_name + '/t_osc.npy') + tracking_cost = np.load(data_directory + exp_name + '/tracking_cost.npy') + u_cost_exp = np.zeros((t_u_matrix.shape[0], t_u_matrix.shape[1])) + for i in range(t_u_matrix.shape[0]): + for j in range(t_u_matrix.shape[1]): + # for t in range(t_u_matrix.shape[2] - 1): + for t in range(3000, 7000): + # print((u_matrix[i, j, t, :].T @ u_matrix[i, j, t, :])) + u_cost_exp[i, j] += (t_u_matrix[i, j, t+1] - t_u_matrix[i, j, t]) * (u_matrix[i, j, t, :].T @ W_u @ u_matrix[i, j, t, :]) + fail_bool = np.array(1 - success, dtype=bool) + fails += fail_bool + # u_cost_exp[u_cost_exp > 20000] = 20000 + # u_cost_exp[fail_bool] = 25000 * np.ones(u_cost.shape)[fail_bool] + u_cost += u_cost_exp + # u_cost[u_cost > 3] = 3 + # u_cost[u_cost < 2.7] = 2.7 + + u_cost *= 8e-6 / .5801 + # u_cost[u_cost > 1] = 1 + u_cost[fails >= 2] = -1 + u_cost_masked = np.ma.masked_where(u_cost == -1, u_cost) + cmap = plt.get_cmap('Blues') + cmap.set_bad(color='black') + im = plt.imshow(u_cost_masked, cmap=cmap) + ax = plt.gca() + legend_elements = [Patch(facecolor='black', alpha=1.0, label='Fail')] + legend = ax.legend(handles=legend_elements, loc=1) + cbar = plt.colorbar(im) + # im = plt.imshow(u_cost, cmap=cmap) + plt.xlabel('Projection Window Duration (s)') + plt.ylabel('Deviation from Nominal Transition Time (s)') + cbar.set_label('Normalized Controller Effort Cost') + ax = plt.gca() + np.set_printoptions(precision=3) + ax.set_xticks(np.arange(0, 11, 1)) + ax.set_yticks(np.arange(0, 11, 1)) + ax.set_xticklabels(np.around(impact_thresholds, 3).tolist()) + ax.set_yticklabels(np.around(landing_times, 3).tolist()) + ax.grid(which="minor", color='k', linestyle='-', linewidth=2) + plt.savefig(trial_name + 'cost.png', dpi=240) + plt.show() + +if __name__ == "__main__": + # main() + # construct_success_plot() + # convert_logs_to_costs() + # plot_success() + plot_costs() diff --git a/bindings/pydairlib/analysis/impact_invariant_control/plot_five_link_biped.py b/bindings/pydairlib/analysis/impact_invariant_control/plot_five_link_biped.py new file mode 100644 index 0000000000..93fad485c6 --- /dev/null +++ b/bindings/pydairlib/analysis/impact_invariant_control/plot_five_link_biped.py @@ -0,0 +1,340 @@ +import sys +import lcm +import matplotlib.pyplot as plt +import numpy as np + +from scipy.linalg import null_space + +import dairlib + +from bindings.pydairlib.lcm import lcm_trajectory +from bindings.pydairlib.multibody import MakeNameToPositionsMap, \ + MakeNameToVelocitiesMap, MakeNameToActuatorsMap, \ + CreateStateNameVectorFromMap, CreateActuatorNameVectorFromMap +from bindings.pydairlib.common import FindResourceOrThrow +from bindings.pydairlib.common import plot_styler, plotting_utils +from bindings.pydairlib.cassie.cassie_utils import * +from bindings.pydairlib.lcm.process_lcm_log import get_log_data +import pydairlib.analysis.mbp_plotting_utils as mbp_plots + +from pydrake.trajectories import PiecewisePolynomial +from pydrake.multibody.parsing import Parser +from pydrake.multibody.plant import AddMultibodyPlantSceneGraph +from pydrake.multibody.tree import JacobianWrtVariable +from pydrake.systems.framework import DiagramBuilder + + +def blend_sigmoid(t, tau, window): + x = (t + window) / tau + return np.exp(x) / (1 + np.exp(x)) + + +def compute_control_law(N, d): + nu = d.shape[0] + nc = N.shape[1] + # A = np.array([[np.eye(nu), N], [N.T, np.zeros((nc, nc))]]) + A = np.zeros((nc + nu, nc + nu)) + b = np.zeros((nc + nu)) + A[:nu, :nu] = np.eye(nu) + A[-nc:, :nu] = N.T + A[:nu, -nc:] = N + A[-nc:, -nc:] = np.zeros((nc, nc)) + b[:nu] = d + b[-nc:] = N.T @ d + + A_inv = np.linalg.inv(A) + u = A_inv @ b + + +def load_logs(plant, t_impact, window): + # log_dir = '/media/yangwill/backups/home/yangwill/Documents/research/projects/five_link_biped/invariant_impacts/logs/nominal/' + log_dir = '/home/yangwill/Documents/research/papers/impact_invariant_control/data/five_link_biped/' + # filename = 'lcmlog-000' + # filename = 'lcmlog-nominal' + filename = 'lcmlog-error' + print(log_dir + filename) + log = lcm.EventLog(log_dir + filename, "r") + default_channels = {'RABBIT_STATE': dairlib.lcmt_robot_output, + 'RABBIT_INPUT': dairlib.lcmt_robot_input, + 'OSC_DEBUG_WALKING': dairlib.lcmt_osc_output} + callback = mbp_plots.load_default_channels + start_time = 0 + duration = -1 + robot_output, robot_input, osc_debug = \ + get_log_data(log, default_channels, start_time, duration, callback, + plant, + 'RABBIT_STATE', 'RABBIT_INPUT', 'OSC_DEBUG_WALKING') + return robot_output, robot_input, osc_debug + + +def main(): + builder = DiagramBuilder() + plant, _ = AddMultibodyPlantSceneGraph(builder, 0.0) + Parser(plant).AddModels( + "examples/impact_invariant_control/five_link_biped.urdf") + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base")) + plant.Finalize() + context = plant.CreateDefaultContext() + world = plant.world_frame() + + l_contact_frame = plant.GetBodyByName("left_foot").body_frame() + r_contact_frame = plant.GetBodyByName("right_foot").body_frame() + + pos_map = MakeNameToPositionsMap(plant) + vel_map = MakeNameToVelocitiesMap(plant) + act_map = MakeNameToActuatorsMap(plant) + + nq = plant.num_positions() + nv = plant.num_velocities() + nx = plant.num_positions() + plant.num_velocities() + nu = plant.num_actuators() + nc = 2 + ny = 5 + n_samples = 1000 + window_length = 0.05 + tau = 0.002 + selected_joint_idxs = slice(0, 7) + + # Map to 2d + TXZ = np.array([[1, 0, 0], [0, 0, 1]]) + + filename = "examples/impact_invariant_control/saved_trajectories/rabbit_walking" + dircon_traj = lcm_trajectory.DirconTrajectory(plant, filename) + state_traj = dircon_traj.ReconstructStateTrajectory() + + transition_time = dircon_traj.GetStateBreaks(1)[0] + x_pre = state_traj.value(transition_time - 1e-6) + x_post = state_traj.value(transition_time) + + robot_output, robot_input, osc_debug = load_logs(plant, transition_time, window_length) + + plant.SetPositionsAndVelocities(context, x_pre) + M = plant.CalcMassMatrixViaInverseDynamics(context) + M_inv = np.linalg.inv(M) + pt_on_body = np.zeros(3) + J_r = TXZ @ plant.CalcJacobianTranslationalVelocity(context, + JacobianWrtVariable.kV, + r_contact_frame, + pt_on_body, world, + world) + J_l = TXZ @ plant.CalcJacobianTranslationalVelocity(context, + JacobianWrtVariable.kV, + l_contact_frame, + pt_on_body, world, + world) + M_Jt = M_inv @ J_r.T + P = null_space(M_Jt.T).T + + proj_ii = np.eye(nv) - M_Jt @ np.linalg.inv(M_Jt.T @ M_Jt) @ M_Jt.T + J_y_stack = np.vstack((J_l, J_r, np.array([0, 0, 1, 0, 0, 0, 0]))) + J_M_Jt = J_y_stack @ M_inv @ J_r.T + proj_y_ii = np.eye(5) - J_M_Jt @ np.linalg.inv( + J_M_Jt.T @ J_M_Jt) @ J_M_Jt.T + P_y = null_space(J_M_Jt.T) + P_y = P_y @ P_y.T + + v_pre = x_pre[-nv:] + v_post = x_post[-nv:] + t_impact = dircon_traj.GetStateBreaks(0)[-1] + + J_y = J_y_stack + # J_y = np.zeros((4, 7)) + # J_y = np.eye((7)) + # J_y[0, 0] = 1 + # J_y[0, 1] = 1 + # J_y[0, 2] = 1 + # J_y[0, 3] = 1 + # J_y[1, 4] = 1 + # J_y[2, 5] = 1 + # J_y[3, 6] = 1 + ydot_pre = J_r @ v_pre + ydot_post = J_r @ v_post + # transform = J_r @ M_inv @ J_r.T @ np.linalg.pinv(J_r @ M_inv @ J_r.T) + transform = M_inv @ J_r.T @ np.linalg.pinv(J_y @ M_inv @ J_r.T) + joint_vel_plot = plot_styler.PlotStyler() + start_time = state_traj.start_time() + end_time = state_traj.end_time() + t = np.linspace(start_time, end_time, 1000) + v_samples = [] + for i in t: + v_samples.append(state_traj.value(i)[7:14, 0]) + joint_vel_plot.plot(t, v_samples, xlabel='Time (s)', ylabel='Velocity (rad/s)', grid=False) + # joint_vel_plot.add_legend(["Left Hip", "Right Hip", "Left Knee", "Right Knee"]) + joint_vel_plot.tight_layout() + joint_vel_plot.save_fig('rabbit_gen_vel.png') + plt.show() + + start_time = t_impact - 2 * window_length + end_time = t_impact + 2 * window_length + + + + start_idx = np.argwhere(np.abs(robot_output['t_x'] - start_time) < 1e-3)[0][0] + end_idx = np.argwhere(np.abs(robot_output['t_x'] - end_time) < 1e-3)[0][0] + t = np.linspace(start_time, end_time, + end_idx - start_idx) + print(start_idx) + print(end_idx) + t_proj = np.array( + [t_impact[0] - 1.5*window_length, t_impact[0] + 1.5*window_length]) + + vel_proj_desired = np.zeros((t.shape[0], nv)) + vel_proj_actual = np.zeros((t.shape[0], nv)) + vel_time_varying_proj = np.zeros((t.shape[0], nv)) + vel_desired = np.zeros((t.shape[0], nv)) + vel_actual = np.zeros((t.shape[0], nv)) + taskspace_vel_actual = np.zeros((t.shape[0], ny)) + taskspace_vel_desired = np.zeros((t.shape[0], ny)) + taskspace_vel_error = np.zeros((t.shape[0], ny)) + taskspace_vel_actual_proj = np.zeros((t.shape[0], ny)) + taskspace_vel_desired_proj = np.zeros((t.shape[0], ny)) + taskspace_vel_error_proj = np.zeros((t.shape[0], ny)) + taskspace_vel_error_osc = np.zeros((t.shape[0], ny)) + vel_corrected = np.zeros((t.shape[0], nv)) + vel_correction = np.zeros((t.shape[0], nv)) + vel_corrected_blend = np.zeros((t.shape[0], nv)) + alphas = np.zeros((t.shape[0], 1)) + for i in range(t.shape[0]): + # for i in range(0, end_idx - start_idx): + # x = state_traj.value(t[i]) + x = state_traj.value(robot_output['t_x'][i + start_idx]) + vel_desired[i] = x[-nv:, 0] + vel_actual[i] = robot_output['v'][i + start_idx] + vel_proj_desired[i] = P.T @ P @ vel_desired[i] + vel_proj_actual[i] = P.T @ P @ vel_actual[i] + taskspace_vel_actual[i] = J_y @ vel_actual[i] + taskspace_vel_desired[i] = J_y @ vel_desired[i] + taskspace_vel_error[i] = taskspace_vel_desired[i] - taskspace_vel_actual[i] + taskspace_vel_actual_proj[i] = proj_y_ii @ taskspace_vel_actual[i] + taskspace_vel_desired_proj[i] = proj_y_ii @ taskspace_vel_desired[i] + taskspace_vel_error_proj[i] = proj_y_ii @ taskspace_vel_error[i] + + plant.SetPositions(context, x[:nq]) + M = plant.CalcMassMatrixViaInverseDynamics(context) + M_inv = np.linalg.inv(M) + J_r = TXZ @ plant.CalcJacobianTranslationalVelocity(context, + JacobianWrtVariable.kV, + r_contact_frame, + pt_on_body, world, + world) + TV_J = M_inv @ J_r.T + TV_J_space = null_space(TV_J.T).T + alpha = 0 + if (np.abs(t[i] - transition_time) < 1.5 * window_length): + if (t[i] < transition_time): + alpha = blend_sigmoid(t[i] - transition_time, tau, + window_length) + else: + alpha = blend_sigmoid(transition_time - t[i], tau, + window_length) + alphas[i] = alpha + vel_time_varying_proj[i] = TV_J_space.T @ TV_J_space @ vel_desired[i] + # vel_correction[i] = transform @ (J_r @ (vel_desired[i] - vel_actual[i])) + vel_correction[i] = transform @ (J_y @ vel_desired[i] - J_y @ vel_actual[i]) + vel_corrected[i] = vel_actual[i] + vel_correction[i] + taskspace_vel_error_osc[i] = J_y @ (vel_desired[i] - vel_corrected[i]) + vel_corrected_blend[i] = vel_actual[i] + alpha * vel_correction[i] + # t_plot = robot_output['t_x'][start_idx:end_idx] + + gen_vel_plot = plot_styler.PlotStyler() + gen_vel_plot.plot(t, vel_desired[:, selected_joint_idxs], + xlabel='time (s)', ylabel='velocity (m/s)') + gen_vel_plot.plot(t, vel_actual[:, selected_joint_idxs], + xlabel='time (s)', ylabel='velocity (m/s)') + gen_vel_plot.plot(t, vel_desired[:, selected_joint_idxs] - vel_actual[:, selected_joint_idxs], + xlabel='time (s)', ylabel='velocity (m/s)', grid=False) + gen_vel_plot.add_legend(['Desired Velocity', 'Measured Velocity', 'Velocity Error']) + ylim = gen_vel_plot.fig.gca().get_ylim() + # gen_vel_plot.save_fig('gen_vel_plot.png') + # ps.save_fig('generalized_velocities_around_impact.png') + + proj_vel_plot = plot_styler.PlotStyler() + proj_vel_plot.plot(t, vel_proj_desired, + title='Constant Impact-Invariant Projection', + xlabel='time (s)', ylabel='velocity (m/s)', ylim=ylim) + proj_vel_plot.plot(t, vel_proj_actual, + title='Constant Impact-Invariant Projection', + xlabel='time (s)', ylabel='velocity (m/s)', ylim=ylim) + # proj_vel_plot.save_fig('proj_vel_plot.png') + + corrected_vel_plot = plot_styler.PlotStyler() + corrected_vel_plot.plot(t, vel_corrected, + title='Impact-Invariant Correction', + xlabel='time (s)', ylabel='velocity (m/s)', + ylim=ylim) + # corrected_vel_plot.save_fig('corrected_vel_plot.png') + + blended_vel_plot = plot_styler.PlotStyler() + ax = blended_vel_plot.fig.axes[0] + # blended_vel_plot.plot(t, vel_actual[:, selected_joint_idxs], + # title='Blended Impact-Invariant Correction', + # xlabel='time (s)', ylabel='velocity (m/s)', ylim=ylim) + blended_vel_plot.plot(t, vel_corrected_blend[:, selected_joint_idxs], + title='Blended Impact-Invariant Correction', + xlabel='time (s)', ylabel='velocity (m/s)', ylim=ylim) + ax.fill_between(t_proj, ylim[0], ylim[1], color=blended_vel_plot.grey, + alpha=0.2) + # blended_vel_plot.save_fig('blended_vel_plot.png') + + + + gen_vel_error = plot_styler.PlotStyler() + ax = gen_vel_error.fig.axes[0] + gen_vel_error.plot(t, vel_desired[:, selected_joint_idxs], + xlabel='time (s)', ylabel='velocity (m/s)', color=gen_vel_error.blue) + gen_vel_error.plot(t, vel_actual[:, selected_joint_idxs], + xlabel='time (s)', ylabel='velocity (m/s)', color=gen_vel_error.red) + gen_vel_error.plot(t, vel_desired[:, selected_joint_idxs] - vel_actual[:, selected_joint_idxs], + xlabel='Time', ylabel='Velocity', grid=False, color=gen_vel_error.grey) + gen_vel_error.add_legend(['Desired Velocity', 'Measured Velocity', 'Velocity Error']) + ax.set_yticklabels([]) + ax.set_xticklabels([]) + # ax.spines['bottom'].set_visible(False) + # ax.spines['left'].set_visible(False) + ylim = gen_vel_error.fig.gca().get_ylim() + # gen_vel_error.save_fig('gen_vel_error.png') + + projected_vel_error = plot_styler.PlotStyler() + ax = projected_vel_error.fig.axes[0] + projected_vel_error.plot(t, vel_proj_desired[:, selected_joint_idxs] - vel_proj_actual[:, selected_joint_idxs], + title='Impact-Invariant Projection Error', + xlabel='time (s)', ylabel='velocity (m/s)', ylim=ylim) + ax.fill_between(t_proj, ylim[0], ylim[1], color=projected_vel_error.grey, + alpha=0.2) + # projected_vel_error.save_fig('projected_vel_error.png') + + corrected_vel_error = plot_styler.PlotStyler() + ax = corrected_vel_error.fig.axes[0] + corrected_vel_error.plot(t, vel_desired[:, selected_joint_idxs] - vel_corrected[:, selected_joint_idxs], + title='Impact-Invariant Correction Error', + xlabel='time (s)', ylabel='velocity (m/s)', ylim=ylim) + ax.fill_between(t_proj, ylim[0], ylim[1], color=corrected_vel_error.grey, + alpha=0.2) + + taskspace_error_plot = plot_styler.PlotStyler() + ax = taskspace_error_plot.fig.axes[0] + taskspace_error_plot.plot(t, taskspace_vel_actual, + title='Task Space', + xlabel='time (s)', ylabel='velocity (m/s)', color=gen_vel_error.blue) + taskspace_error_plot.plot(t, taskspace_vel_actual_proj, + title='Task Space', + xlabel='time (s)', ylabel='velocity (m/s)', color=gen_vel_error.red) + # taskspace_error_plot.plot(t, taskspace_vel_error_osc, + # title='Task Space', + # xlabel='time (s)', ylabel='velocity (m/s)', color=gen_vel_error.grey) + # ax.fill_between(t_proj, ylim[0], ylim[1], color=corrected_vel_error.grey, + # alpha=0.2) + # corrected_vel_error.save_fig('corrected_vel_error.png') + + blending_function_plot = plot_styler.PlotStyler() + ax = blending_function_plot.fig.axes[0] + blending_function_plot.plot(t, alphas, title="Blending Function") + ax.fill_between(t_proj, ax.get_ylim()[0], ax.get_ylim()[1], + color=blending_function_plot.grey, alpha=0.2) + # blending_function_plot.save_fig('blending_function_plot.png') + + plt.show() + + +if __name__ == '__main__': + main() diff --git a/bindings/pydairlib/analysis/log_plotter_cassie.py b/bindings/pydairlib/analysis/log_plotter_cassie.py index bcf59e0a4f..8d0081b310 100644 --- a/bindings/pydairlib/analysis/log_plotter_cassie.py +++ b/bindings/pydairlib/analysis/log_plotter_cassie.py @@ -1,20 +1,21 @@ import sys import lcm import matplotlib.pyplot as plt -import code import numpy as np -import dairlib -from process_lcm_log import get_log_data +from bindings.pydairlib.lcm.process_lcm_log import get_log_data from cassie_plot_config import CassiePlotConfig import cassie_plotting_utils as cassie_plots -import mbp_plotting_utils as mbp_plots +import pydairlib.analysis.mbp_plotting_utils as mbp_plots +from pydairlib.common import plot_styler def main(): - config_folder = 'bindings/pydairlib/analysis/plot_configs/' - config_file = config_folder + 'cassie_default_plot.yaml' - # config_file = config_folder + 'cassie_running_plot.yaml' + config_file = 'bindings/pydairlib/analysis/plot_configs/cassie_running_plot.yaml' + # config_file = 'bindings/pydairlib/analysis/plot_configs/cassie_kcmpc_plot.yaml' + # config_file = 'bindings/pydairlib/analysis/plot_configs/cassie_standing_plot.yaml' + # config_file = 'bindings/pydairlib/analysis/plot_configs/cassie_default_plot.yaml' + # config_file = 'bindings/pydairlib/analysis/plot_configs/cassie_jumping_plot.yaml' plot_config = CassiePlotConfig(config_file) use_floating_base = plot_config.use_floating_base @@ -24,6 +25,11 @@ def main(): channel_u = plot_config.channel_u channel_osc = plot_config.channel_osc + if plot_config.plot_style == "paper": + plot_styler.PlotStyler.set_default_styling() + elif plot_config.plot_style == "compact": + plot_styler.PlotStyler.set_compact_styling() + ''' Get the plant ''' plant, context = cassie_plots.make_plant_and_context( floating_base=use_floating_base, springs=use_springs) @@ -56,42 +62,56 @@ def main(): # Define x time slice t_x_slice = slice(robot_output['t_x'].size) t_osc_slice = slice(osc_debug['t_osc'].size) - print('Log start time: ', robot_output['t_x'][0]) + + print('Average OSC frequency: ', 1 / np.mean(np.diff(osc_debug['t_osc']))) ''' Plot Positions ''' # Plot floating base positions if applicable if use_floating_base and plot_config.plot_floating_base_positions: - mbp_plots.plot_floating_base_positions( + plot = mbp_plots.plot_floating_base_positions( robot_output, pos_names, 7, t_x_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + # Plot joint positions if plot_config.plot_joint_positions: - mbp_plots.plot_joint_positions(robot_output, pos_names, + plot = mbp_plots.plot_joint_positions(robot_output, pos_names, 7 if use_floating_base else 0, t_x_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + # Plot specific positions if plot_config.pos_names: - mbp_plots.plot_positions_by_name(robot_output, plot_config.pos_names, + plot = mbp_plots.plot_positions_by_name(robot_output, plot_config.pos_names, t_x_slice, pos_map) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + ''' Plot Velocities ''' # Plot floating base velocities if applicable if use_floating_base and plot_config.plot_floating_base_velocities: - mbp_plots.plot_floating_base_velocities( + plot = mbp_plots.plot_floating_base_velocities( robot_output, vel_names, 6, t_x_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) if plot_config.plot_floating_base_velocity_body_frame: - mbp_plots.plot_floating_base_body_frame_velocities( + plot = mbp_plots.plot_floating_base_body_frame_velocities( robot_output, t_x_slice, plant, context, "pelvis") + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + plot.tight_layout() + # plot.save_fig('running_speed_plot_kd0.png') # Plot all joint velocities - if plot_config.plot_joint_positions: - mbp_plots.plot_joint_velocities(robot_output, vel_names, - 6 if use_floating_base else 0, - t_x_slice) + if plot_config.plot_joint_velocities: + plot = mbp_plots.plot_joint_velocities(robot_output, vel_names, + 6 if use_floating_base else 0, + t_x_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + # Plot specific velocities if plot_config.vel_names: - mbp_plots.plot_velocities_by_name(robot_output, plot_config.vel_names, - t_x_slice, vel_map) + plot = mbp_plots.plot_velocities_by_name(robot_output, plot_config.vel_names, + t_x_slice, vel_map) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) ''' Plot Efforts ''' if plot_config.plot_measured_efforts: @@ -106,12 +126,23 @@ def main(): mbp_plots.plot_measured_efforts_by_name(robot_output, plot_config.act_names, t_x_slice, act_map) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + ''' Plot OSC ''' - if plot_config.plot_qp_costs: - mbp_plots.plot_qp_costs(osc_debug, t_osc_slice) if plot_config.plot_tracking_costs: - mbp_plots.plot_tracking_costs(osc_debug, t_osc_slice) + plot = mbp_plots.plot_tracking_costs(osc_debug, t_osc_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + # plt.ylim([0, 1e4]) + if plot_config.plot_qp_costs: + plot = mbp_plots.plot_qp_costs(osc_debug, t_osc_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + if plot_config.plot_qp_solutions: + plot = mbp_plots.plot_lambda_c_sol(osc_debug, t_osc_slice, slice(0, 12)) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + plot = mbp_plots.plot_lambda_h_sol(osc_debug, t_osc_slice, slice(0, 6)) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + if plot_config.tracking_datas_to_plot: for traj_name, config in plot_config.tracking_datas_to_plot.items(): @@ -120,6 +151,13 @@ def main(): plot = mbp_plots.plot_osc_tracking_data(osc_debug, traj_name, dim, deriv, t_osc_slice) mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + # plot.save_fig(traj_name + '_' + deriv + '_' + str(dim) + '.png') + tracking_data = osc_debug['osc_debug_tracking_datas'][traj_name] + # print(tracking_data.name + str(dim)) + # print('mean error: ' + str(np.nanmax(np.sqrt(tracking_data.error_y[:, dim].flatten()**2)))) + # print('mean error at end of tracking: ' + str(np.mean(np.sqrt(tracking_data.error_y[:, dim][np.append(np.isnan(tracking_data.error_y[:, dim][1:]), False)]**2)))) + # plot = mbp_plots.plot_osc_tracking_data_in_space(osc_debug, traj_name, config['dims'], deriv, t_osc_slice) + # plot.save_fig('swing_foot_target_trajectory.png') ''' Plot Foot Positions ''' if plot_config.foot_positions_to_plot: @@ -127,17 +165,25 @@ def main(): foot_frames = [] dims = {} pts = {} - for pos in plot_config.foot_positions_to_plot: - foot_frames.append('toe_' + pos) - dims['toe_' + pos] = plot_config.foot_xyz_to_plot[pos] - pts['toe_' + pos] = pts_map[plot_config.pt_on_foot_to_plot] - - mbp_plots.plot_points_positions(robot_output, t_x_slice, plant, context, - foot_frames, pts, dims) + plot = None + for pt_on_foot_to_plot in plot_config.pt_on_foot_to_plot: + for pos in plot_config.foot_positions_to_plot: + foot_frames.append('toe_' + pos) + dims['toe_' + pos] = plot_config.foot_xyz_to_plot[pos] + pts['toe_' + pos] = pts_map[pt_on_foot_to_plot] + + plot = mbp_plots.plot_points_positions(robot_output, t_x_slice, plant, context, + foot_frames, pts, dims, plot) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + plot.save_fig('foot_contact_vertical_position.png') if plot_config.plot_qp_solve_time: - mbp_plots.plot_qp_solve_time(osc_debug, t_osc_slice) + plot = mbp_plots.plot_qp_solve_time(osc_debug, t_osc_slice) + mbp_plots.add_fsm_to_plot(plot, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + if plot_config.plot_active_tracking_datas: + plot = mbp_plots.plot_active_tracking_datas(osc_debug, t_osc_slice, osc_debug['t_osc'], osc_debug['fsm'], plot_config.fsm_state_names) + plot.save_fig('active_tracking_datas.png') plt.show() diff --git a/bindings/pydairlib/analysis/log_plotter_franka.py b/bindings/pydairlib/analysis/log_plotter_franka.py new file mode 100644 index 0000000000..45a1ffcc54 --- /dev/null +++ b/bindings/pydairlib/analysis/log_plotter_franka.py @@ -0,0 +1,328 @@ +import sys +import lcm +import matplotlib.pyplot as plt +import numpy as np + +from bindings.pydairlib.analysis.franka_plot_config import FrankaPlotConfig +from bindings.pydairlib.lcm.process_lcm_log import get_log_data +from cassie_plot_config import CassiePlotConfig +import franka_plotting_utils as franka_plots +import pydairlib.analysis.mbp_plotting_utils as mbp_plots +from pydairlib.common import plot_styler + +from pydrake.all import JointIndex, JointActuatorIndex + +from matplotlib.patches import Patch + +def main(): + config_file = ('bindings/pydairlib/analysis/plot_configs' + '/franka_hardware_plot_config.yaml') + # config_file = ('bindings/pydairlib/analysis/plot_configs' + # '/franka_sim_plot_config.yaml') + plot_config = FrankaPlotConfig(config_file) + + channel_x = plot_config.channel_x + channel_u = plot_config.channel_u + channel_osc = plot_config.channel_osc + channel_c3 = plot_config.channel_c3 + channel_c3_target = plot_config.channel_c3_target + channel_c3_actual = plot_config.channel_c3_actual + channel_tray = plot_config.channel_tray + + if plot_config.plot_style == "paper": + plot_styler.PlotStyler.set_paper_styling() + elif plot_config.plot_style == "compact": + plot_styler.PlotStyler.set_compact_styling() + + ''' Get the plant ''' + franka_plant, franka_context, tray_plant, tray_context = ( + franka_plots.make_plant_and_context()) + + pos_map, vel_map, act_map = mbp_plots.make_name_to_mbp_maps(franka_plant) + pos_names, vel_names, act_names = mbp_plots.make_mbp_name_vectors( + franka_plant) + tray_pos_map, tray_vel_map, _ = mbp_plots.make_name_to_mbp_maps(tray_plant) + + tray_pos_names, tray_vel_names, _ = mbp_plots.make_mbp_name_vectors( + tray_plant) + + ''' Read the log ''' + filename = sys.argv[1] + log = lcm.EventLog(filename, "r") + default_channels = franka_plots.franka_default_channels + robot_output, robot_input, osc_debug = \ + get_log_data(log, # log + default_channels, # lcm channels + plot_config.start_time, + plot_config.duration, + mbp_plots.load_default_channels, # processing callback + True, franka_plant, channel_x, channel_u, + channel_osc) # processing callback arguments + + # processing callback arguments + if plot_config.plot_c3_debug: + c3_output, c3_tracking_target, c3_tracking_actual = get_log_data(log, default_channels, plot_config.start_time, + plot_config.duration, mbp_plots.load_c3_debug, True, + channel_c3, channel_c3_target, channel_c3_actual) + solve_times = np.diff(c3_tracking_target['t'], prepend=[c3_tracking_target['t'][0]]) + print('Average C3 frequency: ', 1 / np.mean(np.diff(c3_tracking_target['t']))) + + + # processing callback arguments + if plot_config.plot_object_state: + object_state = get_log_data(log, default_channels, + plot_config.start_time, + plot_config.duration, + mbp_plots.load_object_state, + channel_tray) + t_object_slice = slice(object_state['t'].size) + + print('Finished processing log - making plots') + # Define x time slice + t_x_slice = slice(robot_output['t'].size) + t_osc_slice = slice(osc_debug['t_osc'].size) + + # print('Average OSC frequency: ', 1 / np.mean(np.diff(osc_debug['t_osc']))) + + + (franka_joint_position_limit_range, franka_joint_velocity_limit_range, + franka_joint_actuator_limit_range) = mbp_plots.generate_joint_limits( + franka_plant) + + # Plot joint positions + if plot_config.plot_joint_positions: + plot = mbp_plots.plot_joint_positions(robot_output, pos_names, 0, + t_x_slice) + plt.ylim(franka_joint_position_limit_range) + # Plot specific positions + if plot_config.pos_names: + plot = mbp_plots.plot_positions_by_name(robot_output, + plot_config.pos_names, + t_x_slice, pos_map) + + if plot_config.plot_c3_tracking: + # plot = plot_styler.PlotStyler(nrows=2) + plot = plot_styler.PlotStyler() + + # plot.plot(c3_tracking_target['t'], c3_tracking_target['x'][:, 0:3], subplot_index = 0) + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 0:3], subplot_index = 0) + # plot.add_legend(['robot_des_x', 'robot_des_y', 'robot_des_z', 'robot_x', 'robot_y', 'robot_z'], subplot_index = 0) + # plot.plot(c3_tracking_target['t'], c3_tracking_target['x'][:, 7:10], subplot_index = 1) + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 7:10], subplot_index = 1) + # plot.add_legend(['tray_des_x', 'tray_des_y', 'tray_des_z', 'tray_x', 'tray_y', 'tray_z'], subplot_index = 1) + + # plot target + # plot.plot(c3_tracking_actual['t'], c3_tracking_target['x'][:, 7:10], subplot_index = 0, ylabel='target position (m)', xlabel='time (s)', grid=False) + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 7:10], subplot_index = 0, ylabel='target position (m)', xlabel='time (s)', grid=False) + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 0:3], subplot_index = 0, ylabel='target position (m)', xlabel='time (s)', grid=False) + + # plots between end effector y and tray y + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 8:9], subplot_index = 0, ylabel='y position (m)', xlabel='time (s)', grid=False) + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 1:2], subplot_index = 0, ylabel='y position (m)', xlabel='time (s)', grid=False) + + # plots y - z trajectories + # plot.axes[0].scatter(c3_tracking_target['x'][0, 7], c3_tracking_target['x'][0, 9], marker='s') + plot.plot(c3_tracking_actual['x'][:, 7:8], c3_tracking_actual['x'][:, 9:10], subplot_index = 0, xlabel='X Position (m)', ylabel='Z Position (m)', grid=False) + plot.plot(c3_tracking_actual['x'][:, 0:1], c3_tracking_actual['x'][:, 2:3], subplot_index = 0, xlabel='X Position (m)', ylabel='Z Position (m)', grid=False) + + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 8:9], subplot_index = 0, xlabel='y position (m)', ylabel='z position (m)', grid=False) + # plot.plot(c3_tracking_target['x'][:, 1:2], c3_tracking_target['x'][:, 2:3], subplot_index = 0) + # plot.plot(c3_tracking_actual['x'][:, 1:2], c3_tracking_actual['x'][:, 2:3], subplot_index = 0, xlabel='y position (m)', ylabel='z position (m)', grid=False) + + # plot.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 7:8], subplot_index = 1) + # plot.plot(c3_tracking_actual['x'][:, 7:8], c3_tracking_actual['x'][:, 7:8], subplot_index = 1) + + # plot.add_legend(['robot_des_x', 'robot_des_y', 'robot_des_z', 'robot_x', 'robot_y', 'robot_z'], subplot_index = 0) + + plot.axes[0].set_xlim([0.4, 0.8]) + plot.axes[0].set_ylim([0.35, 0.65]) + + plot.add_legend(['Tray Path', 'End Effector Path']) + # plot.add_legend(['tray target x', 'tray target y', 'tray target z']) + # plot.add_legend(['tray', 'end effector']) + # plot.save_fig('c3_actual_xz_plot') + + + # plot.save_fig('figure_8_tracking_over_time') + # plot.save_fig('figure_8_tracking') + plot.save_fig('c3_gaiting') + # plot.save_fig('c3_actual_trajectory_time') + + # plot.plot(c3_tracking_target['t'], c3_tracking_target['x'][:, 0:1], subplot_index = 0) + # plot.plot(c3_tracking_target['t'], c3_tracking_target['x'][:, 7:8], subplot_index = 1) + # plot.axes[0].set_ylim([0.4, 0.7]) + # plot.add_legend(['tray_des_x', 'tray_des_y', 'tray_des_z', 'tray_x', 'tray_y', 'tray_z'], subplot_index = 1) + + if True: + plotter = plot_styler.PlotStyler() + # zero_vel_threshold = 0.00001 + # tray_x_vel = np.diff(c3_tracking_actual['x'][:, 7]) + # end_effector_x_vel = np.diff(c3_tracking_actual['x'][:, 0]) + # tray_x_vel[np.abs(tray_x_vel) < zero_vel_threshold] = 0 + # end_effector_x_vel[np.abs(end_effector_x_vel) < zero_vel_threshold] = 0 + # tray_x_vel_dir = np.sign(tray_x_vel) + # end_effector_x_vel_dir = np.sign(end_effector_x_vel) + sticking_contacts = [slice(12, 18), slice(25, 28), slice(34, 53), slice(59, 210), slice(216, 221), slice(232, 235), slice(247, 290)] + sliding_contacts = [slice(17, 20), slice(22, 26), slice(27, 35), slice(52, 60), slice(209, 217), slice(220, 233), slice(234, 248)] + no_contacts = [slice(0, 13), slice(19, 23)] + + support_sticking_contacts = [slice(0, 13), slice(18, 27), slice(35, 40), slice(259, 290)] + support_sliding_contacts = [slice(12, 19), slice(26, 36), slice(39, 68), slice(218, 260)] + support_no_contacts = [slice(67, 219)] + + # plotter.plot(c3_tracking_actual['t'] - 0.15, c3_tracking_actual['x'][:, 7:8], subplot_index = 0, xlabel='Time (s)', ylabel='Position (m)', grid=False) + # plotter.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 0:1], subplot_index = 0, xlabel='Time (s)', ylabel='Position (m)', grid=False) + # plotter.plot(c3_tracking_actual['t'] - 0.15, c3_tracking_actual['x'][:, 9:10], subplot_index = 0, xlabel='Time (s)', ylabel='Position (m)', grid=False) + # plotter.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 2:3], subplot_index = 0, xlabel='Time (s)', ylabel='Position (m)', grid=False) + plotter.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 0:3], subplot_index = 0, xlabel='Time (s)', ylabel='Position (m)', grid=False) + plotter.plot(c3_tracking_actual['t'], c3_tracking_actual['x'][:, 7:10], subplot_index = 0, xlabel='Time (s)', ylabel='Position (m)', grid=False) + ax = plotter.fig.axes[0] + ymin, ymax = ax.get_ylim() + halfway = ymin + 0.5 * (ymax - ymin) + # colors = plt.get_cmap('tab20c') + for i in no_contacts: + ax.fill_between(c3_tracking_actual['t'][i], halfway, ymax, + color=plotter.cmap(0), alpha=0.2) + for i in sticking_contacts: + ax.fill_between(c3_tracking_actual['t'][i], halfway, ymax, + color=plotter.cmap(2), alpha=0.2) + for i in sliding_contacts: + ax.fill_between(c3_tracking_actual['t'][i], halfway, ymax, + color=plotter.cmap(4), alpha=0.2) + for i in support_no_contacts: + ax.fill_between(c3_tracking_actual['t'][i], ymin, halfway, + color=plotter.cmap(0), alpha=0.2) + for i in support_sticking_contacts: + ax.fill_between(c3_tracking_actual['t'][i], ymin, halfway, + color=plotter.cmap(2), alpha=0.2) + for i in support_sliding_contacts: + ax.fill_between(c3_tracking_actual['t'][i], ymin, halfway, + color=plotter.cmap(4), alpha=0.2) + legend_elements = [] + labels = ['No Contact', 'Sticking', 'Sliding'] + # legend_elements.append(Patch(color='none', label=labels[0])) + for i in range(3): + legend_elements.append( + Patch(facecolor=plotter.cmap(2 * i), alpha=0.3, + label=labels[i])) + # legend = ax.legend(handles=['Tray x', 'Tray y', 'Tray z', 'End Effector x', 'End Effector y', 'End Effector z'], loc=1) + # ax.add_artist(legend) + legend = ax.legend(handles=legend_elements, loc=(0.55, 0.765)) + ax.add_artist(legend) + plotter.add_legend(['Tray x', 'End Effector x', 'Tray z', 'End Effector z'], subplot_index = 0, loc=(0.27, 0.7)) + plotter.axes[0].set_xlim([7.4, 15.5]) + + # plotter.save_fig('c3_actual_trajectory_time') + + + # plot = plot_styler.PlotStyler(nrows=2) + # plot.plot(c3_tracking_target['t'], solve_times, subplot_index = 0) + # plot.plot(c3_tracking_target['t'], c3_tracking_actual['x'][:, 0:1], subplot_index = 0) + # plot.plot(c3_tracking_target['t'], c3_tracking_actual['x'][:, 7:8], subplot_index = 0) + # plot.plot(c3_tracking_target['t'], solve_times, subplot_index = 1) + # plot.plot(c3_tracking_target['t'], c3_tracking_actual['x'][:, 2:3], subplot_index = 1) + # plot.plot(c3_tracking_target['t'], c3_tracking_actual['x'][:, 9:10], subplot_index = 1) + + # Plot all joint velocities + if plot_config.plot_joint_velocities: + plot = mbp_plots.plot_joint_velocities(robot_output, vel_names, 0, + t_x_slice) + plt.ylim(franka_joint_velocity_limit_range) + plt.axhline(franka_joint_velocity_limit_range[0], linestyle='--') + plt.axhline(franka_joint_velocity_limit_range[1], linestyle='--') + + # Plot specific velocities + if plot_config.vel_names: + plot = mbp_plots.plot_velocities_by_name(robot_output, + plot_config.vel_names, + t_x_slice, vel_map) + + if plot_config.plot_end_effector: + end_effector_plotter = plot_styler.PlotStyler(nrows=2) + mbp_plots.plot_points_positions(robot_output, t_x_slice, franka_plant, + franka_context, ['plate'], + {'plate': np.zeros(3)}, + {'plate': [0, 1, 2]}, + ps=end_effector_plotter, + subplot_index=0) + + mbp_plots.plot_points_velocities(robot_output, t_x_slice, franka_plant, + franka_context, ['plate'], + {'plate': np.zeros(3)}, + {'plate': [0, 1, 2]}, + ps=end_effector_plotter, + subplot_index=1) + + ''' Plot Efforts ''' + if plot_config.plot_measured_efforts or plot_config.plot_commanded_efforts: + effort_plotter = plot_styler.PlotStyler(nrows=2) + + if plot_config.plot_measured_efforts: + plot = mbp_plots.plot_measured_efforts(robot_output, act_names, + t_x_slice, effort_plotter, + subplot_index=0) + plot.fig.axes[0].set_ylim(franka_joint_actuator_limit_range) + + if plot_config.plot_commanded_efforts: + plot = mbp_plots.plot_commanded_efforts(robot_input, act_names, + t_osc_slice, effort_plotter, + subplot_index=1) + plot.fig.axes[1].set_ylim(franka_joint_actuator_limit_range) + + if plot_config.act_names: + plot = mbp_plots.plot_measured_efforts_by_name(robot_output, + plot_config.act_names, + t_x_slice, act_map) + + ''' Plot OSC ''' + if plot_config.plot_tracking_costs: + plot = mbp_plots.plot_tracking_costs(osc_debug, t_osc_slice) + plt.ylim([0, 1e3]) + + if plot_config.plot_qp_costs: + plot = mbp_plots.plot_qp_costs(osc_debug, t_osc_slice) + + if plot_config.plot_qp_solutions: + plot = mbp_plots.plot_ddq_sol(osc_debug, t_osc_slice, pos_names, + slice(0, 7)) + plot = mbp_plots.plot_joint_velocities(robot_output, vel_names, 0, + t_x_slice) + plot = mbp_plots.plot_lambda_c_sol(osc_debug, t_osc_slice, slice(0, 3)) + # plot = mbp_plots.plot_lambda_h_sol(osc_debug, t_osc_slice, slice(0, + # 6)) + + if plot_config.tracking_datas_to_plot: + for traj_name, config in plot_config.tracking_datas_to_plot.items(): + for deriv in config['derivs']: + for dim in config['dims']: + plot = mbp_plots.plot_osc_tracking_data(osc_debug, + traj_name, dim, + deriv, t_osc_slice) + tracking_data = osc_debug['osc_debug_tracking_datas'][ + traj_name] + + if plot_config.plot_qp_solve_time: + plot = mbp_plots.plot_qp_solve_time(osc_debug, t_osc_slice) + + if plot_config.plot_active_tracking_datas: + plot = mbp_plots.plot_active_tracking_datas(osc_debug, t_osc_slice, + osc_debug['t_osc'], + osc_debug['fsm'], + plot_config.fsm_state_names) + # plot.save_fig('active_tracking_datas.png') + + if plot_config.plot_object_state: + ps = plot_styler.PlotStyler(nrows=2) + mbp_plots.plot_positions_by_name(object_state, + tray_pos_names[4:], + t_object_slice, tray_pos_map, ps = ps, subplot_index = 0) + mbp_plots.plot_positions_by_name(object_state, + tray_pos_names[:4], + t_object_slice, tray_pos_map, ps = ps, subplot_index = 1) + # plot.save_fig(('/').join(filenme.split('/')[-2:]) + '/object_position') + + plt.show() + + +if __name__ == '__main__': + main() diff --git a/bindings/pydairlib/analysis/mbp_plotting_utils.py b/bindings/pydairlib/analysis/mbp_plotting_utils.py index e5786d5bc9..718ec8707b 100644 --- a/bindings/pydairlib/analysis/mbp_plotting_utils.py +++ b/bindings/pydairlib/analysis/mbp_plotting_utils.py @@ -2,12 +2,19 @@ import matplotlib.pyplot as plt from matplotlib.patches import Patch -from pydairlib.common import plot_styler, plotting_utils -from osc_debug import lcmt_osc_tracking_data_t, osc_tracking_cost, osc_regularlization_tracking_cost -from pydairlib.multibody import MakeNameToPositionsMap, \ +from pydrake.all import JacobianWrtVariable, JointActuatorIndex, JointIndex + +from bindings.pydairlib.common import plot_styler, plotting_utils +from bindings.pydairlib.analysis.osc_debug import lcmt_osc_tracking_data_t, \ + osc_tracking_cost, osc_regularlization_tracking_cost +from bindings.pydairlib.multibody import MakeNameToPositionsMap, \ MakeNameToVelocitiesMap, MakeNameToActuatorsMap, \ CreateStateNameVectorFromMap, CreateActuatorNameVectorFromMap +OSC_DERIV_UNITS = {'pos': 'Position $(m)$', + 'vel': 'Velocity $(m/s)$', + 'accel': 'Acceleration $(m/s^2)$'} + def make_name_to_mbp_maps(plant): return MakeNameToPositionsMap(plant), \ @@ -51,11 +58,10 @@ def make_joint_order_permutations(robot_output_message, plant): def process_state_channel(state_data, plant): - t_x = [] + t = [] q = [] u = [] v = [] - pos_map = MakeNameToPositionsMap(plant) vel_map = MakeNameToVelocitiesMap(plant) act_map = MakeNameToActuatorsMap(plant) @@ -73,9 +79,9 @@ def process_state_channel(state_data, plant): q.append(q_temp) v.append(v_temp) u.append(u_temp) - t_x.append(msg.utime / 1e6) + t.append(msg.utime / 1e6) - return {'t_x': np.array(t_x), + return {'t': np.array(t), 'q': np.array(q), 'v': np.array(v), 'u': np.array(u)} @@ -93,7 +99,7 @@ def process_effort_channel(data, plant): t.append(msg.utime / 1e6) u.append(u_temp) - return {'t_u': np.array(t), 'u': np.array(u)} + return {'t': np.array(t), 'u': np.array(u)} def make_point_positions_from_q( @@ -110,6 +116,27 @@ def make_point_positions_from_q( return pos +def make_point_velocities_from_qv( + q, v, plant, context, frame, pt_on_frame, frame_measured_in=None, + frame_expressed_in=None): + if frame_measured_in is None: + frame_measured_in = plant.world_frame() + if frame_expressed_in is None: + frame_expressed_in = plant.world_frame() + + vel = np.zeros((v.shape[0], 3)) + for i, generalized_pos_vel in enumerate(zip(q, v)): + plant.SetPositions(context, generalized_pos_vel[0]) + J = plant.CalcJacobianTranslationalVelocity(context, + JacobianWrtVariable.kV, + frame, pt_on_frame, + frame_measured_in, + frame_expressed_in) + vel[i] = J @ generalized_pos_vel[1] + + return vel + + def get_floating_base_velocity_in_body_frame( robot_output, plant, context, fb_frame): vel = np.zeros((robot_output['q'].shape[0], 3)) @@ -125,9 +152,11 @@ def get_floating_base_velocity_in_body_frame( def process_osc_channel(data): t_osc = [] if hasattr(data[0], 'regularization_cost_names'): - regularization_costs = osc_regularlization_tracking_cost(data[0].regularization_cost_names) + regularization_costs = osc_regularlization_tracking_cost( + data[0].regularization_cost_names) else: - regularization_costs = osc_regularlization_tracking_cost(['input_cost', 'acceleration_cost', 'soft_constraint_cost']) + regularization_costs = osc_regularlization_tracking_cost( + ['input_cost', 'acceleration_cost', 'soft_constraint_cost']) qp_solve_time = [] u_sol = [] lambda_c_sol = [] @@ -141,11 +170,15 @@ def process_osc_channel(data): for msg in data: t_osc.append(msg.utime / 1e6) if hasattr(msg, 'regularization_cost_names'): - regularization_costs.append(msg.regularization_cost_names, msg.regularization_costs) + regularization_costs.append(msg.regularization_cost_names, + msg.regularization_costs) else: - regularization_cost_names = ['input_cost', 'acceleration_cost', 'soft_constraint_cost'] - regularization_cost_list = [msg.input_cost, msg.acceleration_cost, msg.soft_constraint_cost] - regularization_costs.append(regularization_cost_names, regularization_cost_list) + regularization_cost_names = ['input_cost', 'acceleration_cost', + 'soft_constraint_cost'] + regularization_cost_list = [msg.input_cost, msg.acceleration_cost, + msg.soft_constraint_cost] + regularization_costs.append(regularization_cost_names, + regularization_cost_list) qp_solve_time.append(msg.qp_output.solve_time) u_sol.append(msg.qp_output.u_sol) lambda_c_sol.append(msg.qp_output.lambda_c_sol) @@ -157,7 +190,7 @@ def process_osc_channel(data): for tracking_data in msg.tracking_data: if tracking_data.name not in osc_debug_tracking_datas: osc_debug_tracking_datas[tracking_data.name] = \ - lcmt_osc_tracking_data_t() + lcmt_osc_tracking_data_t(name=tracking_data.name) osc_debug_tracking_datas[tracking_data.name].append( tracking_data, msg.utime / 1e6) @@ -166,9 +199,11 @@ def process_osc_channel(data): tracking_cost_handler = osc_tracking_cost(osc_debug_tracking_datas.keys()) for msg in data: if hasattr(msg, 'tracking_costs'): - tracking_cost_handler.append(msg.tracking_data_names, msg.tracking_costs) + tracking_cost_handler.append(msg.tracking_data_names, + msg.tracking_costs) else: - tracking_cost_handler.append(msg.tracking_data_names, msg.tracking_cost) + tracking_cost_handler.append(msg.tracking_data_names, + msg.tracking_cost) tracking_cost = tracking_cost_handler.convertToNP() for name in osc_debug_tracking_datas: @@ -240,6 +275,61 @@ def process_contact_channel(data): 'lambda_c': contact_forces, 'p_lambda_c': contact_info_locs} +def process_c3_debug(data): + t = [] + states = [] + breaks = [] + contact_forces = [] + inputs = [] + for msg in data: + t.append(msg.utime / 1e6) + states.append(msg.c3_solution.x_sol) + breaks.append(msg.c3_solution.time_vec) + contact_forces.append(msg.c3_solution.lambda_sol) + inputs.append(msg.c3_solution.u_sol) + + t = np.array(t) + states = np.array(states) + breaks = np.array(breaks) + contact_forces = np.array(contact_forces) + inputs = np.array(inputs) + + return {'t': t, + 'time_vector': breaks, + 'x': states, + 'lambda': contact_forces, + 'u': inputs,} + +def process_c3_tracking(data): + t = [] + states = [] + for msg in data: + t.append(msg.utime / 1e6) + states.append(msg.state) + + t = np.array(t) + states = np.array(states) + + return {'t': t, + 'x': states, + } +def process_object_state_channel(data): + t = [] + positions = [] + velocities = [] + for msg in data: + t.append(msg.utime / 1e6) + positions.append(msg.position) + velocities.append(msg.velocity) + + t = np.array(t) + positions = np.array(positions) + velocities = np.array(velocities) + + return {'t': t, + 'q': positions, + 'v': velocities,} + def permute_osc_joint_ordering(osc_data, robot_output_msg, plant): _, vperm, uperm = make_joint_order_permutations(robot_output_msg, plant) @@ -255,6 +345,7 @@ def load_default_channels(data, plant, state_channel, input_channel, osc_debug = process_osc_channel(data[osc_debug_channel]) osc_debug = permute_osc_joint_ordering( osc_debug, data[state_channel][0], plant) + # osc_debug = None return robot_output, robot_input, osc_debug @@ -263,9 +354,22 @@ def load_force_channels(data, contact_force_channel): contact_info = process_contact_channel(data[contact_force_channel]) return contact_info - -def plot_q_or_v_or_u(robot_output, key, x_names, x_slice, time_slice, ylabel=None, title=None): - ps = plot_styler.PlotStyler() +def load_c3_debug(data, c3_debug_channel, c3_target_channel, c3_actual_channel): + # c3_debug = process_c3_debug(data[c3_debug_channel]) + c3_debug = None + c3_tracking_target = process_c3_tracking(data[c3_target_channel]) + c3_tracking_actual = process_c3_tracking(data[c3_actual_channel]) + return c3_debug, c3_tracking_target, c3_tracking_actual + +def load_object_state(data, object_state_channel): + object_state = process_object_state_channel(data[object_state_channel]) + return object_state + +def plot_q_or_v_or_u( + robot_output, key, x_names, x_slice, time_slice, + ylabel=None, title=None, ps=None, subplot_index=0): + if ps is None: + ps = plot_styler.PlotStyler() if ylabel is None: ylabel = key if title is None: @@ -273,19 +377,22 @@ def plot_q_or_v_or_u(robot_output, key, x_names, x_slice, time_slice, ylabel=Non plotting_utils.make_plot( robot_output, # data dict - 't_x', # time channel + 't', # time channel time_slice, [key], # key to plot {key: x_slice}, # slice of key to plot {key: x_names}, # legend entries {'xlabel': 'Time', 'ylabel': ylabel, - 'title': title}, ps) + 'title': title}, ps, subplot_index=subplot_index) return ps -def plot_u_cmd(robot_input, key, x_names, x_slice, time_slice, ylabel=None, title=None): - ps = plot_styler.PlotStyler() +def plot_u_cmd( + robot_input, key, x_names, x_slice, time_slice, + ylabel=None, title=None, ps=None, subplot_index=0): + if ps is None: + ps = plot_styler.PlotStyler() if ylabel is None: ylabel = key if title is None: @@ -293,34 +400,33 @@ def plot_u_cmd(robot_input, key, x_names, x_slice, time_slice, ylabel=None, titl plotting_utils.make_plot( robot_input, # data dict - 't_u', # time channel + 't', # time channel time_slice, [key], # key to plot {key: x_slice}, # slice of key to plot {key: x_names}, # legend entries {'xlabel': 'Time', 'ylabel': ylabel, - 'title': title}, ps) + 'title': title}, ps, subplot_index=subplot_index) return ps - def plot_floating_base_positions(robot_output, q_names, fb_dim, time_slice): return plot_q_or_v_or_u(robot_output, 'q', q_names[:fb_dim], slice(fb_dim), time_slice, ylabel='Position', title='Floating Base Positions') -def plot_joint_positions(robot_output, q_names, fb_dim, time_slice): +def plot_joint_positions(robot_output, q_names, fb_dim, time_slice, subplot_index=0): q_slice = slice(fb_dim, len(q_names)) return plot_q_or_v_or_u(robot_output, 'q', q_names[q_slice], q_slice, time_slice, ylabel='Joint Angle (rad)', - title='Joint Positions') + title='Joint Positions', subplot_index=subplot_index) -def plot_positions_by_name(robot_output, q_names, time_slice, pos_map): +def plot_positions_by_name(robot_output, q_names, time_slice, pos_map, ps = None, subplot_index = 0): q_slice = [pos_map[name] for name in q_names] return plot_q_or_v_or_u(robot_output, 'q', q_names, q_slice, time_slice, - ylabel='Position', title='Select Positions') + ylabel='Position', title='Select Positions', ps=ps, subplot_index=subplot_index) def plot_floating_base_velocities(robot_output, v_names, fb_dim, time_slice): @@ -329,11 +435,11 @@ def plot_floating_base_velocities(robot_output, v_names, fb_dim, time_slice): title='Floating Base Velocities') -def plot_joint_velocities(robot_output, v_names, fb_dim, time_slice): +def plot_joint_velocities(robot_output, v_names, fb_dim, time_slice, subplot_index=0): q_slice = slice(fb_dim, len(v_names)) return plot_q_or_v_or_u(robot_output, 'v', v_names[q_slice], q_slice, time_slice, ylabel='Joint Vel (rad/s)', - title='Joint Velocities') + title='Joint Velocities', subplot_index=subplot_index) def plot_velocities_by_name(robot_output, v_names, time_slice, vel_map): @@ -342,28 +448,31 @@ def plot_velocities_by_name(robot_output, v_names, time_slice, vel_map): ylabel='Velocity', title='Select Velocities') -def plot_measured_efforts(robot_output, u_names, time_slice): +def plot_measured_efforts(robot_output, u_names, time_slice, ps=None, subplot_index=0): return plot_q_or_v_or_u(robot_output, 'u', u_names, slice(len(u_names)), time_slice, ylabel='Efforts (Nm)', - title='Measured Joint Efforts') + title='Measured Joint Efforts', ps=ps, subplot_index=subplot_index) -def plot_measured_efforts_by_name(robot_output, u_names, time_slice, u_map): - u_slice = [u_map[name] for name in u_names] - return plot_q_or_v_or_u(robot_output, 'u', u_names, u_slice, time_slice, - ylabel='Efforts (Nm)', title='Select Joint Efforts') - - -def plot_commanded_efforts(robot_input, u_names, time_slice): +def plot_commanded_efforts(robot_input, u_names, time_slice, ps=None, subplot_index=0): return plot_u_cmd(robot_input, 'u', u_names, slice(len(u_names)), time_slice, ylabel='Efforts (Nm)', - title='Commanded Joint Efforts') + title='Commanded Joint Efforts', ps=ps, subplot_index=subplot_index) + + +def plot_measured_efforts_by_name(robot_output, u_names, time_slice, u_map, ps=None, subplot_index=0): + u_slice = [u_map[name] for name in u_names] + return plot_q_or_v_or_u(robot_output, 'u', u_names, u_slice, time_slice, + ylabel='Efforts (Nm)', title='Select Joint Efforts', ps=ps, subplot_index=subplot_index) def plot_points_positions(robot_output, time_slice, plant, context, frame_names, - pts, dims): + pts, dims, ps=None, subplot_index=0): + if ps is None: + ps = plot_styler.PlotStyler() + dim_map = ['_x', '_y', '_z'] - data_dict = {'t': robot_output['t_x']} + data_dict = {'t': robot_output['t']} legend_entries = {} for name in frame_names: frame = plant.GetBodyByName(name).body_frame() @@ -371,7 +480,7 @@ def plot_points_positions(robot_output, time_slice, plant, context, frame_names, data_dict[name] = make_point_positions_from_q(robot_output['q'], plant, context, frame, pt) legend_entries[name] = [name + dim_map[dim] for dim in dims[name]] - ps = plot_styler.PlotStyler() + plotting_utils.make_plot( data_dict, 't', @@ -381,18 +490,49 @@ def plot_points_positions(robot_output, time_slice, plant, context, frame_names, legend_entries, {'title': 'Point Positions', 'xlabel': 'time (s)', - 'ylabel': 'pos (m)'}, ps) + 'ylabel': 'pos (m)'}, ps, subplot_index=subplot_index) + + return ps + + +def plot_points_velocities(robot_output, time_slice, plant, context, frame_names, + pts, dims, ps=None, subplot_index=0): + if ps is None: + ps = plot_styler.PlotStyler() + + dim_map = ['_x', '_y', '_z'] + data_dict = {'t': robot_output['t']} + legend_entries = {} + for name in frame_names: + frame = plant.GetBodyByName(name).body_frame() + pt = pts[name] + data_dict[name] = make_point_velocities_from_qv(robot_output['q'], + robot_output['v'], + plant, context, + frame, np.zeros(3)) + legend_entries[name] = [name + dim_map[dim] for dim in dims[name]] + + plotting_utils.make_plot( + data_dict, + 't', + time_slice, + frame_names, + dims, + legend_entries, + {'title': 'Point Velocities', + 'xlabel': 'time (s)', + 'ylabel': 'pos (m)'}, ps, subplot_index=subplot_index) return ps def plot_floating_base_body_frame_velocities(robot_output, time_slice, plant, context, fb_frame_name): - data_dict = {'t': robot_output['t_x']} + data_dict = {'t': robot_output['t']} data_dict['base_vel'] = get_floating_base_velocity_in_body_frame( robot_output, plant, context, - plant.GetBodyByName(fb_frame_name).body_frame()) - legend_entries = {'base_vel': ['base_vx', 'base_vy', 'base_vz']} + plant.GetBodyByName(fb_frame_name).body_frame())[:, 0] + legend_entries = {'base_vel': ['forward', 'lateral', 'vertical']} ps = plot_styler.PlotStyler() plotting_utils.make_plot( data_dict, @@ -402,9 +542,8 @@ def plot_floating_base_body_frame_velocities(robot_output, time_slice, plant, {}, legend_entries, {'title': 'Floating Base Velocity (Body Frame)', - 'xlabel': 'time (s)', + 'xlabel': 'Time (s)', 'ylabel': 'Velocity (m/s)'}, ps) - return ps @@ -413,7 +552,8 @@ def plot_tracking_costs(osc_debug, time_slice): data_dict = \ {key: val for key, val in osc_debug['tracking_cost'].items()} data_dict['t_osc'] = osc_debug['t_osc'] - + # import pdb; pdb.set_trace() + # data_dict['t_osc'] = osc_debug['trac'] plotting_utils.make_plot( data_dict, 't_osc', @@ -438,7 +578,23 @@ def plot_general_osc_tracking_data(traj_name, deriv, dim, data, time_slice): {}, {key: [key] for key in keys}, {'xlabel': 'Time', - 'ylabel': '', + 'ylabel': OSC_DERIV_UNITS[deriv], + 'title': f'{traj_name} {deriv} tracking {dim}'}, ps) + return ps + + +def plot_general_osc_tracking_data(traj_name, deriv, dim, data, time_slice): + ps = plot_styler.PlotStyler() + keys = [key for key in data.keys() if key != 't'] + plotting_utils.make_plot( + data, + 't', + time_slice, + keys, + {}, + {key: [key] for key in keys}, + {'xlabel': 'Time', + 'ylabel': OSC_DERIV_UNITS[deriv], 'title': f'{traj_name} {deriv} tracking {dim}'}, ps) return ps @@ -453,17 +609,58 @@ def plot_osc_tracking_data(osc_debug, traj, dim, deriv, time_slice): elif deriv == 'vel': data['ydot_des'] = tracking_data.ydot_des[:, dim] data['ydot'] = tracking_data.ydot[:, dim] - data['error_ydot'] = tracking_data.error_ydot[:, dim] + data['error_ydot'] = tracking_data.ydot_des[:, + dim] - tracking_data.ydot[:, + dim] + data['projected_error_ydot'] = tracking_data.error_ydot[:, dim] elif deriv == 'accel': data['yddot_des'] = tracking_data.yddot_des[:, dim] + data['yddot_actual'] = 1000 * np.diff(tracking_data.ydot[:, dim], prepend=[0]) data['yddot_command'] = tracking_data.yddot_command[:, dim] data['yddot_command_sol'] = tracking_data.yddot_command_sol[:, dim] data['t'] = tracking_data.t + return plot_general_osc_tracking_data(traj, deriv, dim, data, time_slice) +def plot_osc_tracking_data_in_space(osc_debug, traj, dims, deriv, time_slice): + tracking_data = osc_debug['osc_debug_tracking_datas'][traj] + data = {} + for dim in dims: + if deriv == 'pos': + data['y_des_' + str(dim)] = tracking_data.y_des[:, dim] + data['y_' + str(dim)] = tracking_data.y[:, dim] + data['error_y_' + str(dim)] = tracking_data.error_y[:, dim] + elif deriv == 'vel': + data['ydot_des_' + str(dim)] = tracking_data.ydot_des[:, dim] + data['ydot_' + str(dim)] = tracking_data.ydot[:, dim] + data['error_ydot_' + str(dim)] = tracking_data.ydot_des[:, + dim] - tracking_data.ydot[:, dim] + data['projected_error_ydot_' + str(dim)] = tracking_data.error_ydot[ + :, + dim] + elif deriv == 'accel': + data['yddot_des_' + str(dim)] = tracking_data.yddot_des[:, dim] + data['yddot_command_' + str(dim)] = tracking_data.yddot_command[:, + dim] + data['yddot_command_sol_' + str( + dim)] = tracking_data.yddot_command_sol[:, + dim] + + data['t'] = tracking_data.t + ps = plot_styler.PlotStyler() + ps.plot(data['y_des_' + str(0)], + data['y_des_' + str(2)] - data['y_des_' + str(2)][0], + xlabel='Forward Position (m)', ylabel='Vertical Position (m)', + grid=False) + # ps.tight_layout() + # ps.axes[0].set_ylim([-0.05, 0.5]) + return ps + + def plot_qp_costs(osc_debug, time_slice): + ps = plot_styler.PlotStyler() regularization_cost = osc_debug['regularization_costs'].regularization_costs data_dict = \ {key: val for key, val in regularization_cost.items()} @@ -478,7 +675,22 @@ def plot_qp_costs(osc_debug, time_slice): {key: [key] for key in regularization_cost.keys()}, {'xlabel': 'Time', 'ylabel': 'Cost', - 'title': 'Regularization Costs'}, ps) + 'title': 'regularization_costs'}, ps) + return ps + + +def plot_qp_solutions(osc_debug, time_slice): + ps = plot_styler.PlotStyler() + plotting_utils.make_plot( + osc_debug, + 't_osc', + time_slice, + ['lambda_c_sol'], + {}, + {}, + {'xlabel': 'Timestamp', + 'ylabel': 'Solve Time ', + 'title': 'OSC Lambda Solutions'}, ps) return ps @@ -497,6 +709,20 @@ def plot_qp_solve_time(osc_debug, time_slice): return ps +def plot_ddq_sol(osc_debug, time_slice, joint_names, ddq_slice): + ps = plot_styler.PlotStyler() + plotting_utils.make_plot( + osc_debug, + 't_osc', + time_slice, + ['dv_sol'], + {'dv_sol': ddq_slice}, + {'dv_sol': joint_names[ddq_slice]}, + {'xlabel': 'time', + 'ylabel': 'joint accelerations (rad/s^2)', + 'title': 'OSC joint acceleration'}, ps) + return ps + def plot_lambda_c_sol(osc_debug, time_slice, lambda_slice): ps = plot_styler.PlotStyler() plotting_utils.make_plot( @@ -513,6 +739,22 @@ def plot_lambda_c_sol(osc_debug, time_slice, lambda_slice): return ps +def plot_lambda_h_sol(osc_debug, time_slice, lambda_slice): + ps = plot_styler.PlotStyler() + plotting_utils.make_plot( + osc_debug, + 't_osc', + time_slice, + ['lambda_h_sol'], + {'lambda_h_sol': lambda_slice}, + {'lambda_h_sol': ['lambda_h_' + i for i in + plotting_utils.slice_to_string_list(lambda_slice)]}, + {'xlabel': 'time', + 'ylabel': 'lambda', + 'title': 'OSC constraint force solution'}, ps) + return ps + + def plot_epsilon_sol(osc_debug, time_slice, epsilon_slice): ps = plot_styler.PlotStyler() plotting_utils.make_plot( @@ -536,11 +778,113 @@ def add_fsm_to_plot(ps, fsm_time, fsm_signal, fsm_state_names): # uses default color map legend_elements = [] for i in np.unique(fsm_signal): - ax.fill_between(fsm_time, ymin, ymax, where=(fsm_signal == i), color=ps.cmap(2 * i), alpha=0.2) + ax.fill_between(fsm_time, ymin, ymax, where=(fsm_signal == i), + color=ps.cmap(2 * i), alpha=0.2) if fsm_state_names: - legend_elements.append(Patch(facecolor=ps.cmap(2 * i), alpha=0.3, label=fsm_state_names[i])) + legend_elements.append( + Patch(facecolor=ps.cmap(2 * i), alpha=0.3, + label=fsm_state_names[i])) if len(legend_elements) > 0: legend = ax.legend(handles=legend_elements, loc=4) # ax.add_artist(legend) ax.relim() + ax.relim() + + +def plot_active_tracking_datas(osc_debug, time_slice, fsm_time, fsm_signal, + fsm_state_names): + ps = plot_styler.PlotStyler() + + ax = ps.fig.axes[0] + # ymin, ymax = ax.get_ylim() + ymin, ymax = 0, 1 + tracking_data_names = osc_debug['osc_debug_tracking_datas'].keys() + # import pdb; pdb.set_trace() + n_tracking_datas = len(tracking_data_names) + tracking_data_legend_elements = [] + + tracking_data_name_dict = {'pelvis_trans_traj': 'Pelvis Position', + 'left_ft_traj': 'Left Foot Position', + 'right_ft_traj': 'Right Foot Position', + 'pelvis_rot_tracking_data': 'Pelvis Orientation', + 'left_toe_angle_traj': 'Left Toe Joint Angle', + 'right_toe_angle_traj': 'Right Toe Joint Angle', + 'swing_hip_yaw_left_traj': 'Left Hip Yaw Angle', + 'swing_hip_yaw_right_traj': 'Right Hip Yaw Angle', + } + + for i, tracking_data_name in enumerate(tracking_data_names): + # tracking_data_name = tracking_data_names[i] + tracking_data = osc_debug['osc_debug_tracking_datas'][ + tracking_data_name] + ax.fill_between(fsm_time, ymax - (i + 0.25) / n_tracking_datas, ymax - + (i + 0.75) / n_tracking_datas, + where=tracking_data.is_active.astype(bool)[ + :fsm_time.shape[0]], + color=ps.cmap(2 * i), alpha=0.7) + tracking_data_legend_elements.append(Patch(facecolor=ps.cmap(2 * i), + alpha=0.7, + label= + tracking_data_name_dict[ + tracking_data_name])) + + # uses default color map + legend_elements = [] + for i in np.unique(fsm_signal): + ax.fill_between(fsm_time, ymin, ymax, where=(fsm_signal == i), + color=ps.cmap(2 * i), alpha=0.2) + if fsm_state_names: + legend_elements.append( + Patch(facecolor=ps.cmap(2 * i), alpha=0.3, + label=fsm_state_names[i])) + ps.tight_layout() + + if len(legend_elements) > 0: + legend = ax.legend(handles=legend_elements, loc=4) + ax.add_artist(legend) + legend = ax.legend(handles=tracking_data_legend_elements, loc=1) + ax.add_artist(legend) + ax.relim() + ax.set_yticklabels([]) + ax.set_yticks([]) + ax.set_xlabel('Time (s)') + ax.set_ylabel('Tracking Objective') + + return ps + +def generate_joint_limits(plant): + joint_position_limits_lower = np.zeros(plant.num_positions()) + joint_position_limits_upper = np.zeros(plant.num_positions()) + joint_velocity_limits_lower = np.zeros(plant.num_positions()) + joint_velocity_limits_upper = np.zeros(plant.num_positions()) + joint_actuator_limits_lower = np.zeros(plant.num_positions()) + joint_actuator_limits_upper = np.zeros(plant.num_positions()) + for i in range(plant.num_positions()): + + joint_position_limits_lower[i] = plant.get_joint(JointIndex(i)).position_lower_limits()[0] + joint_position_limits_upper[i] = plant.get_joint(JointIndex(i)).position_upper_limits()[0] + joint_velocity_limits_lower[i] = plant.get_joint(JointIndex(i)).velocity_lower_limits()[0] + joint_velocity_limits_upper[i] = plant.get_joint(JointIndex(i)).velocity_upper_limits()[0] + joint_actuator_limits_lower[i] = -plant.get_joint_actuator(JointActuatorIndex(i)).effort_limit() + joint_actuator_limits_upper[i] = plant.get_joint_actuator(JointActuatorIndex(i)).effort_limit() + franka_joint_position_limit_range = [np.min(joint_position_limits_lower), np.max(joint_position_limits_upper)] + franka_joint_velocity_limit_range = [np.min(joint_velocity_limits_lower), np.max(joint_velocity_limits_upper)] + franka_joint_actuator_limit_range = [np.min(joint_actuator_limits_lower), np.max(joint_actuator_limits_upper)] + return franka_joint_position_limit_range, franka_joint_velocity_limit_range, franka_joint_actuator_limit_range + +# Cannot plot multiple indices of the solution because the solution is already multi-dimensional (time) +def plot_c3_inputs(c3_output, time_slice, input_index, ps=None): + if ps == None: + ps = plot_styler.PlotStyler() + plotting_utils.make_plot( + c3_output, + 't', + time_slice, + ['u'], + {'u': input_index}, + {}, + {'xlabel': 'Timestamp', + 'ylabel': 'C3 Actor Inputs ', + 'title': 'C3 Actor Solution'}, ps) + return ps \ No newline at end of file diff --git a/bindings/pydairlib/analysis/osc_debug.py b/bindings/pydairlib/analysis/osc_debug.py index 1ba3100fef..5ba8d2741a 100644 --- a/bindings/pydairlib/analysis/osc_debug.py +++ b/bindings/pydairlib/analysis/osc_debug.py @@ -1,13 +1,14 @@ import numpy as np from math import nan + # Class to easily convert list of lcmt_osc_tracking_data_t to numpy arrays class lcmt_osc_tracking_data_t: - def __init__(self, gap_threshold=0.01): - self.t_thresh=gap_threshold + def __init__(self, gap_threshold=0.01, name=''): + self.t_thresh = gap_threshold self.t = [] self.y_dim = 0 - self.name = "" + self.name = name self.is_active = [] self.y = [] self.y_des = [] @@ -23,9 +24,6 @@ def append(self, msg, t): self.y_dim = len(msg.y) self.ydot_dim = len(msg.ydot) - # If there is a large gap between tracking datas, append - # NaNs as a mask for plotting to avoid fictitious lines - # appearing in plots if self.t and (t - self.t[-1]) > self.t_thresh: self.t.append(nan) self.is_active.append(nan) diff --git a/bindings/pydairlib/analysis/osqp_settings_sandbox.py b/bindings/pydairlib/analysis/osqp_settings_sandbox.py new file mode 100644 index 0000000000..d42000cb54 --- /dev/null +++ b/bindings/pydairlib/analysis/osqp_settings_sandbox.py @@ -0,0 +1,263 @@ +import lcm +import numpy as np +from bindings.pydairlib.lcm.process_lcm_log import get_log_data +from dairlib import lcmt_qp +import sys +import matplotlib.pyplot as plt + +import osqp +import proxsuite +from scipy.sparse import csc_matrix + +saved_sols_folder = '/media/yangwill/backups/home/yangwill/Documents/research/projects/cassie/hardware/gain_tuning/qp_settings/cost_tuning/qp_formulation/' + + +def ParseQP(data): + qp_list = [] + + for msg in data["QP_LOG"]: + # import pdb; pdb.set_trace() + qp = {"Q": np.asarray(msg.Q), + "w": np.asarray(msg.w), + "A_ineq": np.asarray(msg.A_ineq), + "ineq_lb": np.asarray(msg.ineq_lb), + "ineq_ub": np.asarray(msg.ineq_ub), + "A_eq": np.asarray(msg.A_eq), + "b_eq": np.asarray(msg.b_eq), + "x_lb": np.asarray(msg.x_lb), + "x_ub": np.asarray(msg.x_ub), + } + + qp_list.append(qp) + + return qp_list + + +def ConvertIneq(A_ineq, ineq_lb, ineq_ub): + is_equality = ineq_ub - ineq_lb < 1e-6 + + # Parse out equality constraints + b_eq = ineq_ub[is_equality] + A_eq = A_ineq[is_equality, :] + A_ineq = A_ineq[is_equality == False, :] + ineq_lb = ineq_lb[is_equality == False] + ineq_ub = ineq_ub[is_equality == False] + + return (A_ineq, ineq_lb, ineq_ub, A_eq, b_eq) + + +def solve_osqp(qp, settings): + Q = qp["Q"] + # Q = qp["Q"] + qp["Q"].T - np.diag(np.diag(qp["Q"])) + # Q = Q + 1e-6 * np.eye(Q.shape[0]) + m = osqp.OSQP() + m.setup( + P=csc_matrix(Q), q=qp["w"], + A=csc_matrix(qp["A_ineq"]), + l=qp["ineq_lb"], u=qp["ineq_ub"], + eps_abs=settings['eps_abs'], + eps_rel=settings['eps_rel'], + eps_prim_inf=settings['eps_prim_inf'], + eps_dual_inf=settings['eps_dual_inf'], + polish=settings['polish'], + scaled_termination=settings['scaled_termination'], + adaptive_rho_fraction=settings['adaptive_rho_fraction'], + verbose=settings['verbose'], + warm_start=settings['warm_start'], + rho=settings['rho'], + max_iter=settings['max_iter'], + check_termination=settings['check_termination']) + + qp_result = m.solve() + + # if qp_result.info.run_time > 5e-3: + # import pdb; pdb.set_trace() + return {'iter': qp_result.info.iter, + 'run_time': qp_result.info.run_time, + 'status': qp_result.info.status, + 'y_sol': qp_result.y, + 'obj_val': qp_result.info.obj_val, + 'pri_res': qp_result.info.pri_res, + 'dual_res': qp_result.info.dua_res, + 'sol': qp_result.x} + + +def solve_proxsuite(qp_list, settings): + run_times = np.zeros((len(qp_list),)) + obj_vals = np.zeros((len(qp_list),)) + iter = np.zeros((len(qp_list),)) + prim_res = np.zeros((len(qp_list),)) + dual_res = np.zeros((len(qp_list),)) + sol = np.zeros((len(qp_list),)) + # import pdb; pdb.set_trace() + +def solve_proxsuite(qp_list, settings): + run_times = np.zeros((len(qp_list),)) + obj_vals = np.zeros((len(qp_list),)) + iter = np.zeros((len(qp_list),)) + prim_res = np.zeros((len(qp_list),)) + dual_res = np.zeros((len(qp_list),)) + sol = np.zeros((len(qp_list),60)) + + for i, qp in enumerate(qp_list): + # Q = qp["Q"] + qp["Q"].T - np.diag(np.diag(qp["Q"])) + # Q = Q + 1e-7*np.eye(Q.shape[0]) + Q = qp["Q"] + C, l, u, A, b = ConvertIneq(qp["A_ineq"], qp["ineq_lb"], qp["ineq_ub"]) + H = Q + g = qp["w"] + n = H.shape[0] + n_eq = A.shape[0] + n_in = C.shape[0] + qp = proxsuite.proxqp.sparse.QP(n, n_eq, n_in) + # qp.init(csc_matrix(H), g, csc_matrix(A), b, csc_matrix(C), l, u, rho=settings['rho']) + qp.init(H, g, A, b, C, l, u, rho=settings['rho']) + + qp.settings.eps_abs = settings["eps_abs"] + qp.settings.eps_rel = settings["eps_rel"] + qp.settings.eps_primal_inf = settings["eps_prim_inf"] + qp.settings.eps_dual_inf = settings["eps_dual_inf"] + qp.settings.verbose = settings["verbose"] + qp.settings.max_iter = settings["max_iter"] + # qp.settings.default_rho = settings['rho'], + # check_termination=settings['check_termination'] + qp.settings.initial_guess = proxsuite.proxqp.InitialGuess.NO_INITIAL_GUESS + + qp.solve() + run_times[i] = qp.results.info.run_time * 1e-6 + obj_vals[i] = qp.results.info.objValue + iter[i] = qp.results.info.iter + prim_res[i] = qp.results.info.pri_res + dual_res[i] = qp.results.info.dua_res + sol[i] = qp.results.x + + return { + 'iter': iter, + 'run_time': run_times, + 'obj_val': obj_vals, + 'prim_res': prim_res, + 'dual_res': dual_res, + 'sol': sol, + } + +def main(): + # filename = "/home/brian/workspace/logs/qp_logging/lcmlog00" + filename = sys.argv[1] + + log = lcm.EventLog(filename, "r") + qp_list = get_log_data(log, {"QP_LOG": lcmt_qp}, 0, 5000, ParseQP) + + # qp_list = qp_list[:2000][::10] + # qp_list = qp_list[:5000][::10] + run_times = np.zeros((len(qp_list),)) + obj_vals = np.zeros((len(qp_list),)) + pri_eps_vals = np.zeros((len(qp_list, ))) + # sols = np.zeros((len(qp_list,), 60)) + sols = np.zeros((len(qp_list, ), 60)) + iters = np.zeros((len(qp_list, ),)) + + osqp_settings = \ + {'rho': 1e-4, + 'sigma': 1e-6, + 'max_iter': 500, + 'eps_abs': 1e-5, + 'eps_rel': 1e-5, + 'eps_prim_inf': 1e-6, + 'eps_dual_inf': 1e-6, + 'alpha': 1.6, + 'delta': 1e-6, + 'polish': 1, + 'polish_refine_iter': 3, + 'verbose': 0, + 'scaled_termination': 1, + 'check_termination': 25, + 'warm_start': 1, + 'adaptive_rho': 1, + 'adaptive_rho_interval': 0, + 'adaptive_rho_tolerance': 5, + 'adaptive_rho_fraction': 0.4} + + for i, qp in enumerate(qp_list): + osqp_result = solve_osqp(qp, osqp_settings) + run_times[i] = osqp_result['run_time'] + obj_vals[i] = osqp_result['obj_val'] + pri_eps_vals[i] = osqp_result['pri_res'] + sols[i] = osqp_result['sol'] + iters[i] = osqp_result['iter'] + + osqp_result = solve_proxsuite(qp_list, osqp_settings) + fig, axs = plt.subplots(4, 1, sharex='all') + axs[0].plot(osqp_result['sol'][:, 32:42]) + axs[0].legend(['0', '1', '2', '3', '4']) + axs[0].set_title('Objective values per QP') + axs[1].plot(osqp_result['run_time']) + axs[1].set_title('Solve times per QP') + axs[2].plot(osqp_result['sol'][:, 22:24]) + axs[2].plot(osqp_result['sol'][:, 28:30]) + axs[3].plot(osqp_result['sol'][:, 24:28]) + axs[3].plot(osqp_result['sol'][:, 30:32]) + axs[3].set_title('QP solutions') + + # for key in osqp_result.keys(): + # plt.figure() + # # plt.plot(osqp_result[key], label='osqp') + # plt.plot(osqp_result[key], label='proxqp') + # plt.title(key) + # plt.legend() + # plt.show() + + # import pdb; pdb.set_trace() + log_name = filename.split('/')[-1] + np.save(saved_sols_folder + log_name + '_' + str(osqp_settings['max_iter']), sols) + + print(f'OSQP mean runtime: {np.mean(run_times)}') + # print(f'PROXQP mean runtime: {np.mean(osqp_result['run_time'])}) + proxqp_runtime = osqp_result['run_time'] + print(f'PROXQP mean runtime: {np.mean(proxqp_runtime)}') + print(f'OSQP mean objective: {np.mean(obj_vals)}') + print(f'OSQP mean primal residuals: {np.mean(pri_eps_vals)}') + + fig, axs = plt.subplots(4, 1, sharex=True) + + # axs[0].plot(obj_vals, label='OSQP') + # axs[0].plot(sols[:, 44:50]) + # axs[0].plot(sols[:, 55:60]) + axs[0].plot(sols[:, 32:42]) + axs[0].legend(['0', '1', '2', '3', '4']) + axs[0].set_title('Objective values per QP') + + axs[1].plot(run_times) + axs[1].set_title('Solve times per QP') + + # axs[2].plot(iters) + # axs[2].set_title('QP solutions') + + axs[2].plot(sols[:, 22:24]) + axs[2].plot(sols[:, 28:30]) + axs[2].set_title('Knee Torque Solutions') + + axs[3].plot(sols[:, 24:28]) + axs[3].plot(sols[:, 30:32]) + axs[3].set_title('QP solutions') + + +def compare_sols(): + # sol0 = np.load(saved_sols_folder + 'lcmlog-drake0_1000.npy') + sol0 = np.load(saved_sols_folder + 'lcmlog-drake1_1000.npy') + # sol1 = np.load(saved_sols_folder + 'lcmlog-drake0_200.npy') + sol1 = np.load(saved_sols_folder + 'lcmlog-drake1_200.npy') + fig, axs = plt.subplots(5, 1, sharex=True) + # a = np.max(sol0 - sol1, axis=0) + a = np.average(sol0 - sol1, axis=0) + + axs[0].plot(a[0:22]) + axs[1].plot(a[22:32]) + axs[2].plot(a[32:44]) + axs[3].plot(a[44:50]) + axs[4].plot(a[50:60]) + + +if __name__ == '__main__': + # compare_sols() + main() + plt.show() diff --git a/bindings/pydairlib/analysis/plot_configs/cassie_default_plot.yaml b/bindings/pydairlib/analysis/plot_configs/cassie_default_plot.yaml index 93f59bc41a..b96dee5762 100644 --- a/bindings/pydairlib/analysis/plot_configs/cassie_default_plot.yaml +++ b/bindings/pydairlib/analysis/plot_configs/cassie_default_plot.yaml @@ -2,12 +2,12 @@ channel_x: "CASSIE_STATE_DISPATCHER" channel_u: "OSC_WALKING" channel_osc: "OSC_DEBUG_WALKING" -use_archived_lcmtypes: true +use_archived_lcmtypes: false # Approximate log time to start at relative to the first timestamp in the log -start_time: 0 +start_time: 5 # Duration of time to plot (seconds, -1 for whole log) -duration: 2 +duration: 10 # Plant properties use_springs: true @@ -22,12 +22,12 @@ plot_joint_velocities: false plot_measured_efforts: true plot_commanded_efforts: true plot_contact_forces: false -special_positions_to_plot: [ ] +special_positions_to_plot: ['knee_joint_left', 'knee_joint_right'] special_velocities_to_plot: [ ] special_efforts_to_plot: [ ] -# FSM Names -fsm_state_names: [ 'Left Stance (LS)', 'Right Stance (RS)', ' ', 'Double Support Post Left (DSPL)', 'Double Support Post Right (DSPR)' ] +# Finite State Machine Names +fsm_state_names: ['Left Stance (LS)', 'Left Flight (LF)', ' ', 'Right Stance (RS)', 'Right Flight (RF)'] # Foot position plots foot_positions_to_plot: [ 'right', 'left' ] @@ -37,7 +37,10 @@ pt_on_foot_to_plot: 'mid' # takes value 'front', 'mid', or 'rear' # Desired osc plots plot_qp_costs: true plot_tracking_costs: true +plot_qp_solutions: true plot_qp_solve_time: true +plot_active_tracking_datas: false tracking_datas_to_plot: - alip_com_traj: { 'dims': [ 0, 1 ], 'derivs': [ 'vel' ] } - swing_ft_traj: { 'dims': [ 2 ], 'derivs': [ 'pos', 'accel' ] } +# alip_com_traj: { 'dims': [ 0, 1 ], 'derivs': [ 'vel' ] } +# swing_ft_traj: { 'dims': [ 2 ], 'derivs': [ 'pos', 'accel' ] } + left_toe_angle_traj: { 'dims': [ 0 ], 'derivs': [ 'accel' ] } diff --git a/bindings/pydairlib/analysis/plot_configs/cassie_jumping_plot.yaml b/bindings/pydairlib/analysis/plot_configs/cassie_jumping_plot.yaml new file mode 100644 index 0000000000..20dd4a5740 --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/cassie_jumping_plot.yaml @@ -0,0 +1,55 @@ +# LCM channels to read data from +channel_x: "CASSIE_STATE_DISPATCHER" +#channel_x: "CASSIE_STATE_SIMULATION" +channel_u: "OSC_JUMPING" +channel_osc: "OSC_DEBUG_JUMPING" +use_archived_lcmtypes: false +use_default_styling: true + +# Log time to stop at (seconds, -1 for whole log) +start_time: 18.47 +duration: 1.5 + +# Plant properties +use_springs: true +use_floating_base: true + +# Desired RobotOutput plots +plot_floating_base_positions: false +plot_floating_base_velocities: false +plot_floating_base_velocity_body_frame: false +plot_joint_positions: false +plot_joint_velocities: false +plot_measured_efforts: false +plot_commanded_efforts: false +plot_contact_forces: false +special_positions_to_plot: [ 'hip_pitch_left'] +special_velocities_to_plot: ['hip_pitch_leftdot'] +special_efforts_to_plot: [ ] + +# Finite State Machine Names +fsm_state_names: ['READY,', 'BALANCE', 'CROUCH', 'FLIGHT', 'LAND'] + +# Foot position plots +foot_positions_to_plot: [ 'right', 'left' ] +#foot_positions_to_plot: [] +foot_xyz_to_plot: { 'right': [2], 'left': [2] } +#foot_xyz_to_plot: { } +pt_on_foot_to_plot: ['front', 'rear'] # takes value 'front', 'mid', or 'rear' + +# Desired osc plots +plot_qp_costs: true +plot_qp_solve_time: false +plot_qp_solutions: true +plot_tracking_costs: true +plot_active_tracking_datas: true +tracking_datas_to_plot: { + pelvis_trans_traj: { 'dims': [ 0, 2 ], 'derivs': ['pos', 'vel', 'accel'] }, + # pelvis_rot_traj: {'dims': [0, 1], 'derivs': ['vel']} + # hip_yaw_left_traj: {'dims': [0], 'derivs': ['vel']} + # right_ft_traj: {'dims': [2], 'derivs': ['pos'] }, +# left_ft_traj: {'dims': [2], 'derivs': ['pos']}, + # right_ft_z_traj: {'dims': [2], 'derivs': ['pos']} + # left_toe_angle_traj: {'dims': [0], 'derivs': ['vel']} + # right_toe_angle_traj: {'dims': [0], 'derivs': ['accel']} +} \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/cassie_running_plot.yaml b/bindings/pydairlib/analysis/plot_configs/cassie_running_plot.yaml index 7ed97998b3..528655174a 100644 --- a/bindings/pydairlib/analysis/plot_configs/cassie_running_plot.yaml +++ b/bindings/pydairlib/analysis/plot_configs/cassie_running_plot.yaml @@ -1,53 +1,58 @@ # LCM channels to read data from channel_x: "CASSIE_STATE_DISPATCHER" +#channel_x: "CASSIE_STATE_SIMULATION" channel_u: "OSC_RUNNING" channel_osc: "OSC_DEBUG_RUNNING" use_archived_lcmtypes: false +use_default_styling: false # Log time to stop at (seconds, -1 for whole log) -start_time: 0 -duration: 2 +start_time: 2 +#duration: 0.47 +duration: 50 # Plant properties use_springs: true use_floating_base: true # Desired RobotOutput plots -plot_floating_base_positions: false +plot_floating_base_positions: true plot_floating_base_velocities: false plot_floating_base_velocity_body_frame: true plot_joint_positions: false plot_joint_velocities: false -plot_measured_efforts: true -plot_commanded_efforts: true +plot_measured_efforts: false +plot_commanded_efforts: false plot_contact_forces: false -special_positions_to_plot: [ ] -special_velocities_to_plot: [] +special_positions_to_plot: [ 'knee_joint_left', 'knee_joint_right', 'ankle_spring_joint_left', 'ankle_spring_joint_right' ] +#special_positions_to_plot: [ 'hip_pitch_left', 'hip_pitch_right', 'hip_roll_left', 'hip_roll_right' ] +special_velocities_to_plot: [ 'hip_roll_leftdot', 'hip_roll_rightdot' ] special_efforts_to_plot: [ ] # Finite State Machine Names -fsm_state_names: ['Left Stance (LS)', 'Left Flight (LF)', 'Right Stance (RS)', 'Right Flight (RF)'] +fsm_state_names: [ 'Left Stance (LS)', 'Right Stance (RS)', 'Left Flight (LF)', 'Right Flight (RF)' ] # Foot position plots foot_positions_to_plot: [ 'right', 'left' ] #foot_positions_to_plot: [] -foot_xyz_to_plot: { 'right': [ 0, 2 ], 'left': [ 0, 2 ] } +foot_xyz_to_plot: { 'right': [ 2 ], 'left': [ 2 ] } #foot_xyz_to_plot: { } -pt_on_foot_to_plot: 'mid' # takes value 'front', 'mid', or 'rear' +pt_on_foot_to_plot: ['rear'] # takes value 'front', 'mid', or 'rear' # Desired osc plots plot_qp_costs: false -plot_qp_solve_time: true -plot_qp_solutions: true +plot_qp_solve_time: false +plot_qp_solutions: false plot_tracking_costs: false -tracking_datas_to_plot: -# pelvis_trans_traj: { 'dims': [ 2 ], 'derivs': [ 'vel' ] } -# pelvis_trans_traj: {'dims': [0, 1, 2], 'derivs': ['accel']} -# pelvis_rot_traj: {'dims': [0, 1], 'derivs': ['vel']} -# hip_yaw_left_traj: {'dims': [0], 'derivs': ['vel']} -# left_ft_traj: {'dims': [1], 'derivs': ['pos']} -# right_ft_traj: {'dims': [2], 'derivs': ['vel']} -# left_ft_z_traj: {'dims': [1], 'derivs': ['pos']} -# right_ft_z_traj: {'dims': [1, 2], 'derivs': ['pos']} -# left_toe_angle_traj: {'dims': [0], 'derivs': ['vel']} -# right_toe_angle_traj: {'dims': [0], 'derivs': ['accel']} +plot_active_tracking_datas: false +tracking_datas_to_plot: { + # pelvis_trans_traj: { 'dims': [0, 1, 2 ], 'derivs': [ 'accel' ] }, +# pelvis_trans_traj: { 'dims': [ 2 ], 'derivs': [ 'pos'] }, + pelvis_rot_traj: { 'dims': [ 0, 1, 2 ], 'derivs': [ 'pos' ] }, +# hip_yaw_left_traj: { 'dims': [ 0 ], 'derivs': [ 'pos' ] }, +# hip_yaw_right_traj: { 'dims': [ 0 ], 'derivs': [ 'pos' ] }, +# right_ft_traj: { 'dims': [ 0, 1, 2 ], 'derivs': [ 'pos' ] }, +# left_ft_traj: { 'dims': [ 0, 1, 2 ], 'derivs': [ 'pos' ] }, +# left_toe_angle_traj: { 'dims': [ 0 ], 'derivs': [ 'pos'] }, +# right_toe_angle_traj: { 'dims': [ 0 ], 'derivs': [ 'pos'] }, +} \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/cassie_standing_plot.yaml b/bindings/pydairlib/analysis/plot_configs/cassie_standing_plot.yaml new file mode 100644 index 0000000000..c890a47754 --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/cassie_standing_plot.yaml @@ -0,0 +1,55 @@ +# LCM channels to read data from +channel_x: "CASSIE_STATE_DISPATCHER" +#channel_x: "CASSIE_STATE_SIMULATION" +channel_u: "OSC_STANDING" +channel_osc: "OSC_DEBUG_STANDING" +use_archived_lcmtypes: false +use_default_styling: false + +# Log time to stop at (seconds, -1 for whole log) +start_time: 0 +duration: -1 + +# Plant properties +use_springs: true +use_floating_base: true + +# Desired RobotOutput plots +plot_floating_base_positions: true +plot_floating_base_velocities: true +plot_floating_base_velocity_body_frame: false +plot_joint_positions: true +plot_joint_velocities: true +plot_measured_efforts: true +plot_commanded_efforts: true +plot_contact_forces: false +special_positions_to_plot: [] +special_velocities_to_plot: [] +special_efforts_to_plot: [ ] + +# Finite State Machine Names +fsm_state_names: [] + +# Foot position plots +foot_positions_to_plot: [ 'right', 'left' ] +#foot_positions_to_plot: [] +foot_xyz_to_plot: { 'right': [2], 'left': [2] } +#foot_xyz_to_plot: { } +pt_on_foot_to_plot: ['rear'] # takes value 'front', 'mid', or 'rear' + +# Desired osc plots +plot_qp_costs: true +plot_qp_solve_time: true +plot_qp_solutions: true +plot_tracking_costs: true +plot_active_tracking_datas: false +tracking_datas_to_plot: { + pelvis_trans_traj: { 'dims': [ 0, 1, 2 ], 'derivs': ['pos', 'vel'] }, +# pelvis_rot_traj: {'dims': [0, 1, 2], 'derivs': ['vel', 'accel']} + # hip_yaw_left_traj: {'dims': [0], 'derivs': ['vel']} + # right_ft_traj: {'dims': [2], 'derivs': ['pos'] }, +# left_ft_traj: {'dims': [0, 2], 'derivs': ['pos']}, + # right_ft_z_traj: {'dims': [2], 'derivs': ['pos']} + # left_toe_angle_traj: {'dims': [0], 'derivs': ['vel']} + # right_toe_angle_traj: {'dims': [0], 'derivs': ['accel']} +} \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/franka_hardware_friction_plot_config.yaml b/bindings/pydairlib/analysis/plot_configs/franka_hardware_friction_plot_config.yaml new file mode 100644 index 0000000000..1d02070c7b --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/franka_hardware_friction_plot_config.yaml @@ -0,0 +1,43 @@ +# LCM channels to read data from +channel_x: "FRANKA_STATE" +channel_u: "OSC_FRANKA" +channel_osc: "OSC_DEBUG_FRANKA" +channel_c3: "C3_DEBUG" +channel_tray: "TRAY_STATE" +channel_c3_target: "C3_TARGET" +channel_c3_actual: "C3_ACTUAL" +plot_style: 'compact' # compact, paper, default + +start_time: 0 +#duration: 0.47 +duration: -1 +# Plant properties + +# Desired RobotOutput plots +plot_joint_positions: false +plot_joint_velocities: false +plot_measured_efforts: false +plot_commanded_efforts: false +plot_contact_forces: false +plot_end_effector: false + +special_positions_to_plot: [] +special_velocities_to_plot: [] +special_efforts_to_plot: [] + +# Finite State Machine Names +fsm_state_names: [ 'Left Stance (LS)', 'Right Stance (RS)', 'Left Flight (LF)', 'Right Flight (RF)' ] + +# Desired osc plots +plot_qp_costs: false +plot_qp_solve_time: false +plot_qp_solutions: false +plot_tracking_costs: false +plot_active_tracking_datas: false +tracking_datas_to_plot: +# end_effector_target: {'dims': [0], 'derivs': ['accel']} +# end_effector_orientation_target: {'dims': [0, 1, 2], 'derivs': ['pos']} +# end_effector_target: {'dims': [0, 1, 2], 'derivs': ['pos', 'vel', 'accel']} +plot_c3_debug: false +plot_c3_tracking: false +plot_object_state: true \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/franka_hardware_plot_config.yaml b/bindings/pydairlib/analysis/plot_configs/franka_hardware_plot_config.yaml new file mode 100644 index 0000000000..e2958a34cd --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/franka_hardware_plot_config.yaml @@ -0,0 +1,43 @@ +# LCM channels to read data from +channel_x: "FRANKA_STATE" +channel_u: "OSC_FRANKA" +channel_osc: "OSC_DEBUG_FRANKA" +channel_c3: "C3_DEBUG" +channel_tray: "TRAY_STATE" +channel_c3_target: "C3_TARGET" +channel_c3_actual: "C3_ACTUAL" +plot_style: 'compact' # compact, paper, default + +start_time: 0 +#duration: 0.47 +duration: -1 +# Plant properties + +# Desired RobotOutput plots +plot_joint_positions: false +plot_joint_velocities: false +plot_measured_efforts: false +plot_commanded_efforts: false +plot_contact_forces: false +plot_end_effector: false + +special_positions_to_plot: [] +special_velocities_to_plot: [] +special_efforts_to_plot: [] + +# Finite State Machine Names +fsm_state_names: [ 'Left Stance (LS)', 'Right Stance (RS)', 'Left Flight (LF)', 'Right Flight (RF)' ] + +# Desired osc plots +plot_qp_costs: true +plot_qp_solve_time: true +plot_qp_solutions: false +plot_tracking_costs: false +plot_active_tracking_datas: false +tracking_datas_to_plot: +# end_effector_target: {'dims': [0], 'derivs': ['accel']} +# end_effector_orientation_target: {'dims': [0, 1, 2], 'derivs': ['pos']} +# end_effector_target: {'dims': [0, 1, 2], 'derivs': ['pos', 'vel', 'accel']} +plot_c3_debug: true +plot_c3_tracking: true +plot_object_state: false \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/franka_sim_plot_config.yaml b/bindings/pydairlib/analysis/plot_configs/franka_sim_plot_config.yaml new file mode 100644 index 0000000000..e154b4ea47 --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/franka_sim_plot_config.yaml @@ -0,0 +1,43 @@ +# LCM channels to read data from +channel_x: "FRANKA_STATE_SIMULATION" +channel_u: "OSC_FRANKA" +channel_osc: "OSC_DEBUG_FRANKA" +channel_c3: "C3_DEBUG" +channel_tray: "TRAY_STATE_SIMULATION" +channel_c3_target: "C3_TARGET" +channel_c3_actual: "C3_ACTUAL" +plot_style: 'compact' # compact, paper, default + +start_time: 0 +#duration: 0.47 +duration: -1 +# Plant properties + +# Desired RobotOutput plots +plot_joint_positions: false +plot_joint_velocities: false +plot_measured_efforts: true +plot_commanded_efforts: false +plot_contact_forces: false +plot_end_effector: true + +special_positions_to_plot: [] +special_velocities_to_plot: [] +special_efforts_to_plot: [] + +# Finite State Machine Names +fsm_state_names: [ 'Left Stance (LS)', 'Right Stance (RS)', 'Left Flight (LF)', 'Right Flight (RF)' ] + +# Desired osc plots +plot_qp_costs: false +plot_qp_solve_time: true +plot_qp_solutions: true +plot_tracking_costs: false +plot_active_tracking_datas: false +tracking_datas_to_plot: + end_effector_target: {'dims': [1], 'derivs': ['pos', 'vel']} +# end_effector_orientation_target: {'dims': [0, 1, 2], 'derivs': ['pos']} +# end_effector_target: {'dims': [0, 1, 2], 'derivs': ['pos', 'vel', 'accel']} +plot_c3_debug: true +plot_c3_tracking: true +plot_object_state: true \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/franka_translation_plot.yaml b/bindings/pydairlib/analysis/plot_configs/franka_translation_plot.yaml new file mode 100644 index 0000000000..14ac9156b6 --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/franka_translation_plot.yaml @@ -0,0 +1,43 @@ +# LCM channels to read data from +channel_x: "FRANKA_STATE" +channel_u: "OSC_FRANKA" +channel_osc: "OSC_DEBUG_FRANKA" +channel_c3: "C3_DEBUG" +channel_tray: "TRAY_STATE" +channel_c3_target: "C3_TARGET" +channel_c3_actual: "C3_ACTUAL" +plot_style: 'compact' # compact, paper, default + +start_time: 0 +#duration: 0.47 +duration: -1 +# Plant properties + +# Desired RobotOutput plots +plot_joint_positions: false +plot_joint_velocities: false +plot_measured_efforts: false +plot_commanded_efforts: false +plot_contact_forces: false +plot_end_effector: true + +special_positions_to_plot: [] +special_velocities_to_plot: [] +special_efforts_to_plot: [] + +# Finite State Machine Names +fsm_state_names: [ 'Left Stance (LS)', 'Right Stance (RS)', 'Left Flight (LF)', 'Right Flight (RF)' ] + +# Desired osc plots +plot_qp_costs: false +plot_qp_solve_time: true +plot_qp_solutions: true +plot_tracking_costs: false +plot_active_tracking_datas: false +tracking_datas_to_plot: + end_effector_target: {'dims': [0], 'derivs': ['accel']} +# end_effector_orientation_target: {'dims': [0, 1, 2], 'derivs': ['pos']} +# end_effector_target: {'dims': [0, 1, 2], 'derivs': ['pos', 'vel', 'accel']} +plot_c3_debug: true +plot_c3_tracking: true +plot_object_state: true \ No newline at end of file diff --git a/bindings/pydairlib/analysis/plot_configs/running_hardware.yaml b/bindings/pydairlib/analysis/plot_configs/running_hardware.yaml new file mode 100644 index 0000000000..ee0506e387 --- /dev/null +++ b/bindings/pydairlib/analysis/plot_configs/running_hardware.yaml @@ -0,0 +1,41 @@ +# LCM channels to read data from +channel_x: "CASSIE_STATE_DISPATCHER" +channel_u: "OSC_RUNNING" +channel_osc: "OSC_DEBUG_RUNNING" + +# Plant properties +use_springs: true +use_floating_base: true + +# Desired RobotOutput plots +plot_floating_base_positions: false +plot_floating_base_velocities: false +plot_joint_positions: false +plot_joint_velocities: false +plot_measured_efforts: true +special_positions_to_plot: [] +special_velocities_to_plot: [] +special_efforts_to_plot: [] + +# Foot position plots +foot_positions_to_plot: ['right', 'left'] +foot_xyz_to_plot: {'right': [0, 1, 2], 'left': [0, 1, 2]} +pt_on_foot_to_plot: 'mid' # takes value 'front', 'mid', or 'rear' + +# Desired osc plots +plot_qp_costs: true +plot_qp_solve_time: true +plot_tracking_costs: true +tracking_datas_to_plot: + pelvis_trans_traj: {'dims': [2], 'derivs': ['pos', 'vel']} + # pelvis_trans_traj: {'dims': [0, 1, 2], 'derivs': ['accel']} + pelvis_rot_traj: {'dims': [0, 1], 'derivs': ['vel']} + hip_yaw_left_traj: {'dims': [0], 'derivs': ['vel']} +# hip_roll_left_traj: {'dims': [0], 'derivs': ['vel']} + # hip_pitch_left_traj: {'dims': [0], 'derivs': ['pos', 'vel', 'acc']} + left_ft_traj: {'dims': [2], 'derivs': ['vel']} + right_ft_traj: {'dims': [2], 'derivs': ['vel']} + left_ft_z_traj: {'dims': [2], 'derivs': ['vel']} + right_ft_z_traj: {'dims': [2], 'derivs': ['vel']} +# left_toe_angle_traj: {'dims': [0], 'derivs': ['accel']} +# right_toe_angle_traj: {'dims': [0], 'derivs': ['accel']} diff --git a/bindings/pydairlib/analysis/qp_test.py b/bindings/pydairlib/analysis/qp_test.py new file mode 100644 index 0000000000..0741a550af --- /dev/null +++ b/bindings/pydairlib/analysis/qp_test.py @@ -0,0 +1,179 @@ +import lcm +import numpy as np +from bindings.pydairlib.lcm.process_lcm_log import get_log_data +from dairlib import lcmt_qp +# import qpoases +import sys +# import qpsolvers + +import osqp +# import gurobipy as gp +from scipy.sparse import csc_matrix + +def ConvertIneq(A_ineq, ineq_lb, ineq_ub): + is_equality = ineq_ub - ineq_lb < 1e-6 + + # Parse out equality constraints + b_eq = ineq_ub[is_equality] + A_eq = A_ineq[is_equality, :] + A_ineq = A_ineq[is_equality == False, :] + ineq_lb = ineq_lb[is_equality == False] + ineq_ub = ineq_ub[is_equality == False] + + # Convert to Ax <= b + ub_active = ineq_ub <= 1e10 + lb_active = ineq_lb >= -1e10 + A_ineq = np.vstack((A_ineq[ub_active,:], -A_ineq[lb_active,:])) + b_ineq = np.hstack((ineq_ub[ub_active], -ineq_lb[lb_active])) + + return (A_ineq, b_ineq, A_eq, b_eq) + +def ParseQP(data): + qp_list = [] + + for msg in data["QP_LOG"]: + # import pdb; pdb.set_trace() + qp = {"Q": np.asarray(msg.Q), + "w": np.asarray(msg.w), + "A_ineq": np.asarray(msg.A_ineq), + "ineq_lb": np.asarray(msg.ineq_lb), + "ineq_ub": np.asarray(msg.ineq_ub), + "A_eq": np.asarray(msg.A_eq), + "b_eq": np.asarray(msg.b_eq), + "x_lb": np.asarray(msg.x_lb), + "x_ub": np.asarray(msg.x_ub), + } + + qp_list.append(qp) + + return qp_list + +# filename = "/home/posa/workspace/osc_qp.log" +filename = sys.argv[1] +log = lcm.EventLog(filename, "r") +qp_list = \ + get_log_data(log, + {"QP_LOG": lcmt_qp}, + ParseQP) + + +qp_list = qp_list[:200] +run_times = np.zeros((len(qp_list), 1)) +obj_vals = np.zeros((len(qp_list), 1)) +run_times_gr = np.zeros((len(qp_list), 1)) +obj_vals_gr = np.zeros((len(qp_list), 1)) +run_times_qpo = np.zeros((len(qp_list), 1)) +obj_vals_qpo = np.zeros((len(qp_list), 1)) +run_times_other = np.zeros((len(qp_list), 1)) +obj_vals_other = np.zeros((len(qp_list), 1)) + + +for i, qp in enumerate(qp_list): + Q = qp["Q"] + qp["Q"].T - np.diag(np.diag(qp["Q"])) + Q = Q + 1e-7*np.eye(Q.shape[0]) + # Q = qp["Q"] + + # qp["A_ineq"] = np.vstack((qp["A_ineq"][:68,:], qp["A_ineq"][69:, :])) + # qp["ineq_lb"] = np.hstack((qp["ineq_lb"][:68], qp["ineq_lb"][69:])) + # qp["ineq_ub"] = np.hstack((qp["ineq_ub"][:68], qp["ineq_ub"][69:])) + + # if i == 0:? + m = osqp.OSQP() + m.setup(P=csc_matrix(Q), q=qp["w"], A=csc_matrix(qp["A_ineq"]), l=qp["ineq_lb"], u=qp["ineq_ub"]) + m.update_settings(eps_abs=1e-7, einitps_rel=1e-7, eps_prim_inf=1e-6, eps_dual_inf=1e-6, polish=1, scaled_termination=1, adaptive_rho_fraction=1, verbose=0, warm_start=0, rho=.1, check_termination=3) + # m.update_settings(eps_abs=1e-7, eps_rel=1e-7, eps_prim_inf=1e-6, eps_dual_inf=1e-6, polish=1, scaled_termination=1, adaptive_rho_fraction=1, verbose=0, warm_start=0, rho=.1, check_termination=3) + # else: + # # import pdb; pdb.set_trace() + # m.update(q=qp["w"], Ax=csc_matrix(qp["A_ineq"]).data, l=qp["ineq_lb"], u=qp["ineq_ub"]) + results = m.solve() + run_times[i] = results.info.run_time + obj_vals[i] = results.info.obj_val + + (A_ineq, b_ineq, A_eq, b_eq) = ConvertIneq(qp["A_ineq"], qp["ineq_lb"], qp["ineq_ub"]) + + # + # GUROBI + # + # m_gr = gp.Model() + # m.Params.FeasibilityTol = 1e-4 + # m.Params.OptimalityTol = 1e-4 + # m_gr.Params.LogToConsole = 0 + # m_gr.Params.method = -1 + # x = m_gr.addMVar(Q.shape[0], lb=-float('inf'), ub=float('inf')) + # obj = x @ (Q/2) @ x + qp["w"] @ x + # obj = x @ x + # m_gr.setObjective(obj) + + # m_gr.addConstr(A_ineq @ x <= b_ineq) + # m_gr.addConstr(A_eq @ x == b_eq) + + # m_gr.optimize() + # run_times_gr[i] = m_gr.Runtime + # obj_vals_gr[i] = obj.getValue() + + # + # QPOASES + # + + # nWSR = np.array([10000]) + # nx = Q.shape[0] + # m_qpo = qpoases.PyQProblem(nx, qp["A_ineq"].shape[0]) + # options = qpoases.PyOptions() + # options.printLevel = qpoases.PyPrintLevel.NONE + # options.terminationTolerance = 1e-6 + # options.boundTolerance = 1e-6 + # options.enableEqualities = 1 + # options.enableFarBounds = 0 + # options.enableDriftCorrection = 0 + # options.enableFlippingBounds = 0 + # options.enableInertiaCorrection = 0 + # m_qpo.setOptions(options) + # + # start = time.time() + # m_qpo.init(Q, qp["w"], qp["A_ineq"].T, -np.full(nx, 1e10), np.full(nx, 1e10), qp["ineq_lb"], qp["ineq_ub"], nWSR) + # obj_vals_qpo[i] = m_qpo.getObjVal() + # run_times_qpo[i] = time.time() - start + + # # + # # qpsolvers + # # + # solvers = ['cvxopt', 'cvxpy', 'ecos', 'gurobi', 'osqp', 'qpoases', 'quadprog', 'scs'] + # other_i = 6 + # start = time.time() + # x_other = qpsolvers.solve_qp(Q, qp["w"], A_ineq, b_ineq, A_eq, b_eq, -np.full(nx, 1e10), np.full(nx, 1e10), solver=solvers[other_i]) + # run_times_other[i] = time.time() - start + # # import pdb; pdb.set_trace() + # if x_other is not None: + # obj_vals_other[i] = .5*x_other @ Q @ x_other + qp["w"]@x_other + # else: + # obj_vals_other[i] = 1e3 + + +print(np.mean(run_times)) +print(np.mean(obj_vals)) + +# print(np.mean(run_times_gr)) +# print(np.mean(obj_vals_gr)) +# +# print(np.mean(run_times_qpo)) +# print(np.mean(obj_vals_qpo)) + + +import matplotlib.pyplot as plt +fig, axs = plt.subplots(2, 1, sharex=True) + + +axs[0].plot(obj_vals, label='OSQP') +axs[0].plot(obj_vals_gr, label='Gurobi') +axs[0].plot(obj_vals_qpo, label='QPOASES') +# axs[0].plot(obj_vals_other, label=solvers[other_i]) +axs[0].legend() +axs[0].set_title('Objective values per QP') + +axs[1].plot(run_times) +axs[1].plot(run_times_gr) +axs[1].plot(run_times_qpo) +# axs[1].plot(run_times_other) +axs[1].set_title('Solve times per QP') +plt.show() +import pdb; pdb.set_trace() diff --git a/bindings/pydairlib/analysis/spring_compensation.py b/bindings/pydairlib/analysis/spring_compensation.py new file mode 100644 index 0000000000..864ea1f3a1 --- /dev/null +++ b/bindings/pydairlib/analysis/spring_compensation.py @@ -0,0 +1,82 @@ +import numpy as np + +import dairlib +from pydrake.multibody.plant import * +from pydairlib.common import FindResourceOrThrow +from pydairlib.cassie.cassie_utils import * +from pydrake.multibody.tree import MultibodyForces +from pydairlib.multibody import * +import cassie_plotting_utils as cassie_plots +from scipy.spatial.transform import Rotation as R + +def quat_dot(quat, omega): + """ + Borrowed from quadrotor sim from MEAM 620 + Parameters: + quat, [i,j,k,w] + omega, angular velocity of body in body axes + + Returns + duat_dot, [i,j,k,w] + + """ + # Adapted from "Quaternions And Dynamics" by Basile Graf. + (q0, q1, q2, q3) = (quat[0], quat[1], quat[2], quat[3]) + G = np.array([[ q3, q2, -q1, -q0], + [-q2, q3, q0, -q1], + [ q1, -q0, q3, -q2]]) + quat_dot = 0.5 * G.T @ omega + # Augment to maintain unit quaternion. + quat_err = np.sum(quat**2) - 1 + quat_err_grad = 2 * quat + quat_dot = quat_dot - quat_err * quat_err_grad + return quat_dot + +class SpringCompensation(): + def __init__(self): + self.dt = 8e-5 + self.plant, self.context = cassie_plots.make_plant_and_context( + floating_base=True, springs=True) + self.nq = self.plant.num_positions() + self.nv = self.plant.num_velocities() + self.nx = self.nq + self.nv + self.B = self.plant.MakeActuationMatrix() + + def sample_simulate_forward(self, q, v, u, n_samples): + x = np.hstack((q, v)) + self.plant.SetPositionsAndVelocities(self.context, x) + + samples = np.zeros((n_samples, self.nx)) + # t_samples = np.zeros((n_samples, 1)) + t_samples = 0.01 * np.random.sample(n_samples) + t_samples = np.sort(t_samples) + M = self.plant.CalcMassMatrix(self.context) + c = self.plant.CalcBiasTerm(self.context) + f = MultibodyForces(self.plant) + self.plant.CalcForceElementsContribution(self.context, f) + g = self.plant.CalcGravityGeneralizedForces(self.context) + M_inv = np.linalg.inv(M) + vdot = M_inv @ (-c + f.generalized_forces() + g + self.B @ u) + # import pdb; pdb.set_trace() + # t = 0.01 * np.random.sample() + for i in range(n_samples): + x_next = self.simulate_forward(x, vdot, t_samples[i]) + samples[i] = x_next + # t_samples[i] = t + return samples, t_samples + + def simulate_forward(self, x, vdot, t): + t0 = 0 + q = np.copy(x[:self.nq]) + v = np.copy(x[-self.nv:]) + while t0 < t: + q[:4] = q[:4] + self.dt * quat_dot(q[:4], v[:3]) + q[4:] = q[4:] + self.dt * v[3:] + v = v + self.dt * vdot + t0 += self.dt + v[12] = 0 + v[20] = 0 + x_next = np.hstack((q, v)) + + return x_next + diff --git a/bindings/pydairlib/analysis/tray_balance_study/BUILD.bazel b/bindings/pydairlib/analysis/tray_balance_study/BUILD.bazel new file mode 100644 index 0000000000..10aa865f41 --- /dev/null +++ b/bindings/pydairlib/analysis/tray_balance_study/BUILD.bazel @@ -0,0 +1,42 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +py_binary( + name = "run_tray_parameter_study", + srcs = ["run_tray_parameter_study.py"], + data = [ + "@drake_models//:franka_description", + "@lcm//:lcm-python", + ], + deps = [ + "//bindings/pydairlib/common", + "//lcmtypes:lcmtypes_robot_py", + ], +) + +py_binary( + name = "parameter_study_analysis", + srcs = ["parameter_study_analysis.py"], + data = [ + ":parameter_study_config.yaml", + "//examples/franka:parameters", + "@drake_models//:franka_description", + "@lcm//:lcm-python", + ], + deps = [ + "//bindings/pydairlib/analysis:mbp_plotting_utils", + "//bindings/pydairlib/common", + "//bindings/pydairlib/lcm:process_lcm_log", + "//lcmtypes:lcmtypes_robot_py", + ], +) diff --git a/bindings/pydairlib/analysis/tray_balance_study/generate_dataset.py b/bindings/pydairlib/analysis/tray_balance_study/generate_dataset.py new file mode 100644 index 0000000000..0ea6b5dd01 --- /dev/null +++ b/bindings/pydairlib/analysis/tray_balance_study/generate_dataset.py @@ -0,0 +1,41 @@ +import numpy as np +import os +import datetime +def gather_theta_batched_branin_data(n_datapoints, + batch_size, + gains_to_randomize, + thetas_to_randomize, + high_level_folder, + params_t=BraninFnParamsTrain): + + assert n_datapoints == int(n_datapoints / batch_size) * batch_size + + num_batches = int(n_datapoints / (batch_size)) + # Each batch is associated with a single set of system intrinsic parameters + intrinsics = np.zeros((num_batches, len(thetas_to_randomize))) + # for each parameter vector, generate $batch_size random gains to evaluate + gains_to_test = np.zeros((num_batches, batch_size, len(gains_to_randomize))) + + subfolder = "branin_data_" + datetime.now().strftime("%b_%d_%Y_%H%M") + "/" + if not os.path.exists(os.path.join(high_level_folder, subfolder)): + os.makedirs(os.path.join(high_level_folder, subfolder), exist_ok=True) + + with h5py.File(os.path.join(high_level_folder, subfolder, "dataset.hdf5"), 'w') as f: + # intrinsic vectors don't actually get used anywhere, but good to save for bookkeeping + f.create_dataset("intrinsics", shape=intrinsics.shape) + f.create_dataset("gains", shape=gains_to_test.shape) + # this system only has 1 performance metric, but in principle this could be any number + f.create_dataset("metrics", shape=(num_batches, batch_size, 1)) + for batch in range(num_batches): + # Generate a random intrinsic vector for this batch + batch_intrinsic_obj = params_t.generate_random(thetas_to_randomize) + f["intrinsics"][batch,...] = batch_intrinsic_obj.get_list() + robot = Branin(batch_intrinsic_obj, gains_to_randomize) + for point in range(batch_size): + # generate a random gain vector + gain = BraninInputs.generate_random(gains_to_randomize).get_list()[gains_to_randomize] + # run the simulation and obtain the performance metrics + metric = robot.evaluate_x(gain) + f["metrics"][batch,point] = metric + f["gains"][batch,point] = gain + print(f"finished batch {batch}") \ No newline at end of file diff --git a/bindings/pydairlib/analysis/tray_balance_study/parameter_study_analysis.py b/bindings/pydairlib/analysis/tray_balance_study/parameter_study_analysis.py new file mode 100644 index 0000000000..0e5aab7237 --- /dev/null +++ b/bindings/pydairlib/analysis/tray_balance_study/parameter_study_analysis.py @@ -0,0 +1,141 @@ +import subprocess +import time +from tqdm import * +import lcm +from bindings.pydairlib.common import plot_styler, plotting_utils +import pydairlib.analysis.mbp_plotting_utils as mbp_plots +from bindings.pydairlib.lcm.process_lcm_log import get_log_data +import matplotlib.pyplot as plt +import numpy as np +import yaml +import dairlib + + +def process_c3_channel(data): + t = [] + x = [] + for msg in data: + t.append(msg.utime / 1e6) + x.append(msg.state) + t = np.array(t) + x = np.array(x) + + return {'t': t, + 'x': x} + +def load_c3_channels(data, c3_target_state_channel, c3_actual_state_channel, c3_debug_output_channel): + c3_target = process_c3_channel(data[c3_target_state_channel]) + c3_actual = process_c3_channel(data[c3_actual_state_channel]) + c3_output = mbp_plots.process_c3_debug(data[c3_debug_output_channel]) + return c3_target, c3_actual, c3_output + +def load_lcm_logs(): + config_file = 'parameter_study_config.yaml' + parameters_directory = 'examples/franka/parameters/' + lcm_channels_sim = 'lcm_channels_simulation.yaml' + + with open(parameters_directory + lcm_channels_sim) as f: + filedata = f.read() + lcm_channels = yaml.load(filedata) + + with open('bindings/pydairlib/analysis/tray_balance_study/' + config_file) as f: + filedata = f.read() + param_study = yaml.load(filedata) + + start_time = 0 + duration = -1 + + c3_channels = {lcm_channels['c3_target_state_channel']: dairlib.lcmt_c3_state, + lcm_channels['c3_actual_state_channel']: dairlib.lcmt_c3_state, + lcm_channels['c3_debug_output_channel']: dairlib.lcmt_c3_output} + callback = load_c3_channels + mass_range = np.arange(0.5, 2.0, 0.05) + # mu_range = np.arange(0.3, 0.8, 0.01) + effective_mu_range = np.arange(0.1, 0.71, 0.1) + + print(effective_mu_range.shape[0]) + all_successes = np.zeros((effective_mu_range.shape[0], 3)) + for i in range(effective_mu_range.shape[0]): + print('%02d', effective_mu_range[i]) + # print(mass_range[i]) + # ps = plot_styler.PlotStyler(nrows=2) + + successes = np.zeros(3) + + for j in range(5): + # ps = plot_styler.PlotStyler() + log_filename = param_study['results_folder'] + param_study['parameter'][2] + '/simlog-' + '%02d_%1d' % (i, j) + log = lcm.EventLog(log_filename, "r") + c3_target, c3_actual, c3_output = \ + get_log_data(log, c3_channels, start_time, duration, callback, False, + lcm_channels['c3_target_state_channel'], lcm_channels['c3_actual_state_channel'], + lcm_channels['c3_debug_output_channel']) + length = min(c3_target['t'].shape[0], c3_actual['t'].shape[0]) + # ps.plot(c3_actual['t'], c3_actual['x'][:, 8:9], subplot_index=0) + # ps.plot(c3_actual['t'], c3_actual['x'][:, 9:10], subplot_index=0) + # ps.plot(c3_actual['t'], c3_actual['x'][:, 1:2], subplot_index=0) + # ps.plot(c3_actual['t'], c3_actual['x'][:, 2:3], subplot_index=0) + first_target = np.array([0.45, 0, 0.485]) + second_target = np.array([0.45, 0, 0.6]) + third_target = np.array([0.7, 0.0, 0.485]) + reached_first_target = np.any(np.all(np.isclose(c3_target['x'][:, 7:10], second_target, atol=1e-3), axis=1)) + reached_second_target = np.any(np.all(np.isclose(c3_target['x'][:, 7:10], third_target, atol=1e-3), axis=1)) + reached_third_target = reached_second_target and np.linalg.norm(c3_target['x'][:length, 7:10] - c3_actual['x'][:length, 7:10], axis=1)[-1] < 0.1 + task_success = np.array([reached_first_target, reached_second_target, reached_third_target]) + # print('reached_targets: ', task_success) + successes += task_success + # print('reached_second_target: ', reached_second_target) + # print('reached_third_target: ', reached_third_target) + t_c3_slice = slice(c3_output['t'].size) + # mbp_plots.plot_c3_inputs(c3_output, t_c3_slice, 1, ps) + # plt.show() + print(successes) + all_successes[i] = successes + # np.save('target_successes_23', all_successes ) + np.save('target_successes_03_26', all_successes ) + +def plot_logs(): + all_successes = np.load('target_successes_03_25.npy') + bar_width = 0.025 + bar_positions = np.arange(all_successes.shape[0]) + n = all_successes.shape[0] + # mu_range = np.arange(0.3, 0.74, 0.05) + effective_mu_range = np.arange(0.1, 0.71, 0.1) + + plt.rc('legend', fontsize=36) + plt.rc('axes', labelsize=36, titlesize=36) + plt.rc('xtick', labelsize=36) + plt.rc('ytick', labelsize=36) + # Plotting bars for each task + for i in range(n): + # for j in range(3): + # plt.bar(bar_positions[i], all_successes[i, 2] / 30, width=bar_width) + # plt.bar(effective_mu_range[i], all_successes[i, 2] / all_successes[i, 0], width=bar_width, color='grey') + plt.bar(effective_mu_range[i], all_successes[i, 0] / 5, width=bar_width, color='grey') + plt.ylabel('Target Success Rate') + plt.xlabel('Tray/End Effector Friction Coefficient') + + # plt.savefig('/home/yangwill/Pictures/plot_styler/' + 'target_1_success') + # plt.legend() + # for i in range(3): + # plt.bar(bar_positions + i * (n + 1), all_successes[:, i], label=f'Task {i + 1}') + + plt.show() + +def main(): + load_lcm_logs() + # plot_logs() + + + + + + + + + + + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/bindings/pydairlib/analysis/tray_balance_study/parameter_study_config.yaml b/bindings/pydairlib/analysis/tray_balance_study/parameter_study_config.yaml new file mode 100644 index 0000000000..32819883bb --- /dev/null +++ b/bindings/pydairlib/analysis/tray_balance_study/parameter_study_config.yaml @@ -0,0 +1,21 @@ +trajectory_path: /home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/ +gains_path: examples/franka/parameters/ +model_path: examples/franka/urdf/ +results_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/logs/2024/03_26_24/parameter_study/ +#results_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/logs/2024/01_31_24/parameter_study/ +processed_results_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/logs/2024/03_26_24/parameter_study/data/ +sim_cmd: bazel-bin/examples/franka/franka_sim +osc_cmd: bazel-bin/examples/franka/franka_osc_controller +c3_cmd: bazel-bin/examples/franka/franka_c3_controller +fix_inertia_cmd: 'python3 -m pydrake.multibody.fix_inertia --in_place examples/franka/urdf/tray_parameter_sweep.sdf' + +nominal_c3_gain_filename: franka_c3_options_rotated_supports.yaml +#nominal_model_filename: tray.sdf +nominal_model_filename: plate_end_effector.urdf + +modified_c3_gain_filename: franka_c3_options_parameter_sweep.yaml +#modified_model_filename: tray_parameter_sweep.sdf +modified_model_filename: plate_end_effector_parameter_sweep.urdf + +parameter: ['mu_c3', 'mass_real', 'mu_real'] + diff --git a/bindings/pydairlib/analysis/tray_balance_study/run_tray_parameter_study.py b/bindings/pydairlib/analysis/tray_balance_study/run_tray_parameter_study.py new file mode 100644 index 0000000000..141ba33efe --- /dev/null +++ b/bindings/pydairlib/analysis/tray_balance_study/run_tray_parameter_study.py @@ -0,0 +1,111 @@ +import subprocess +import time +from tqdm import * +import lcm +import dairlib.lcmt_radio_out + +import numpy as np +import yaml + + +def main(): + config_file = 'parameter_study_config.yaml' + sim_time = 20.0 + cooldown_time = 10.0 + start_time = 0 + realtime_rate = 1.0 + num_trials = 10 + c3_start_up_time = 1.0 + sim_run_time = sim_time / realtime_rate + controller_startup_time = 0.1 + + with open('bindings/pydairlib/analysis/tray_balance_study/' + config_file) as f: + filedata = f.read() + config = yaml.load(filedata) + + # Add an extra second to the runtime of the simulator process to account for start up and stopping time + + lcm_logger_cmd = '' + publisher = lcm.LCM() + + # parameter = 'mu_c3' + # parameter = 'mass_real' + parameter = config['parameter'][2] + + nominal_mu_value = 'mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1]' + # nominal_real_mu_value = '0.4' + nominal_real_mu_value = '' + nominal_tray_mass = '1' + + # mu_range = np.arange(0.3, 0.8, 0.05) + # mass_range = np.arange(0.5, 2.0, 0.05) + effective_mu_range = np.arange(0.1, 0.71, 0.1) + tray_mu = 0.4 + + initial_radio_msg = dairlib.lcmt_radio_out() + start_c3_radio_msg = dairlib.lcmt_radio_out() + initial_radio_msg.channel[13] = 1 + initial_radio_msg.channel[11] = 0 + initial_radio_msg.channel[14] = 1 + start_c3_radio_msg.channel[13] = 1 + start_c3_radio_msg.channel[11] = 0 + start_c3_radio_msg.channel[14] = 0 + + # for i in trange(mu_range.shape[0]): + for i in trange(effective_mu_range.shape[0]): + plate_mu = 0.5 * effective_mu_range[i] / (1 - effective_mu_range[i] / (2 * tray_mu)) + # modified_mu_value = 'mu: [%.2f, %.2f, %.2f, 0.1, 0.1, 0.1, 0.1]' % (mu_range[i], mu_range[i], mu_range[i]) + # modified_mass_value = ' %.1f ' % (mass_range[i]) + # modified_real_mu_value = '%.2f' % (mu_range[i]) + modified_real_mu_value = '' % (plate_mu) + + # f = open(gains_path + gain_filename, 'r') + f = open(config['model_path'] + config['nominal_model_filename'], 'r') + + filedata = f.read() + f.close() + # newdata = filedata.replace(nominal_mu_value, modified_mu_value) + newdata = filedata.replace(nominal_real_mu_value, modified_real_mu_value) + # newdata = filedata.replace(nominal_tray_mass, modified_mass_value) + + # f = open(config['gains_path'] + config['modified_c3_gain_filename.yaml'], 'w') + f = open(config['model_path'] + config['modified_model_filename'], 'w') + f.write(newdata) + f.close() + if parameter == 'mass_real': + fix_inertia_process = subprocess.Popen(config['fix_inertia_cmd'].split(' ')) + time.sleep(1.0) + + for j in trange(num_trials): + log_path = config['results_folder'] + parameter + '/simlog-' + '%02d_%1d' % (i, j) + lcm_logger_cmd = ['lcm-logger', + '-f', + '%s' % log_path, + ] + + osc_process = subprocess.Popen(config['osc_cmd'], stdout=subprocess.DEVNULL, + stderr=subprocess.STDOUT) + c3_process = subprocess.Popen(config['c3_cmd'], stdout=subprocess.DEVNULL, + stderr=subprocess.STDOUT) + sim_process = subprocess.Popen(config['sim_cmd'], stdout=subprocess.DEVNULL, + stderr=subprocess.STDOUT) + time.sleep(controller_startup_time) + publisher.publish("RADIO", initial_radio_msg.encode()) + logger_process = subprocess.Popen(lcm_logger_cmd, stdout=subprocess.DEVNULL, + stderr=subprocess.STDOUT) + time.sleep(c3_start_up_time) + publisher.publish("RADIO", start_c3_radio_msg.encode()) + time.sleep(sim_run_time) + osc_process.kill() + c3_process.kill() + sim_process.kill() + logger_process.kill() + time.sleep(cooldown_time) + + +if __name__ == "__main__": + main() + # construct_success_plot() + # convert_logs_to_costs() + # plot_success() + # plot_costs() diff --git a/bindings/pydairlib/cassie/BUILD.bazel b/bindings/pydairlib/cassie/BUILD.bazel index 6a3fe96e38..5a0b396612 100644 --- a/bindings/pydairlib/cassie/BUILD.bazel +++ b/bindings/pydairlib/cassie/BUILD.bazel @@ -27,6 +27,66 @@ pybind_py_library( py_imports = ["."], ) +pybind_py_library( + name = "input_supervisor_py", + cc_deps = [ + "//examples/Cassie/systems:input_supervisor", + "@drake//:drake_shared_library", + ], + cc_so_name = "input_supervisor", + cc_srcs = ["input_supervisor_py.cc"], + py_deps = [ + "@drake//bindings/pydrake", + ":module_py", + ], + py_imports = ["."], +) + +pybind_py_library( + name = "controllers_py", + cc_deps = [ + "//examples/Cassie/diagrams:osc_running_controller_diagram", + "//examples/Cassie/diagrams:osc_walking_controller_diagram", + "@drake//:drake_shared_library", + ], + cc_so_name = "controllers", + cc_srcs = ["controllers_py.cc"], + py_deps = [ + "@drake//bindings/pydrake", + ":module_py", + ], + py_imports = ["."], +) + +pybind_py_library( + name = "simulators_py", + cc_deps = [ + "//examples/Cassie/diagrams:cassie_sim_diagram", + "@drake//:drake_shared_library", + "@drake//bindings/pydrake/common:value_pybind", + ], + cc_so_name = "simulators", + cc_srcs = [ + "simulators_py.cc", + ], + py_deps = [ + "@drake//bindings/pydrake", + "//bindings/pydairlib/lcm:lcm", + ":module_py", + ], + py_imports = ["."], +) + +py_binary( + name = "learn_osc_gains", + srcs = ["learn_osc_gains.py"], + deps = [ + "//bindings/pydairlib/cassie", + "//bindings/pydairlib/cassie/gym_envs:cassie_gym_all", + "@drake//bindings/pydrake", + ], +) + # This determines how `PYTHONPATH` is configured, and how to install the # bindings. PACKAGE_INFO = get_pybind_package_info("//bindings") @@ -44,11 +104,13 @@ py_library( PY_LIBRARIES = [ ":cassie_utils_py", + ":controllers_py", + ":simulators_py", ] # Package roll-up (for Bazel dependencies). py_library( - name = "cassie_utils", + name = "cassie", imports = PACKAGE_INFO.py_imports, deps = PY_LIBRARIES, ) diff --git a/bindings/pydairlib/cassie/controllers_py.cc b/bindings/pydairlib/cassie/controllers_py.cc new file mode 100644 index 0000000000..2620f903dd --- /dev/null +++ b/bindings/pydairlib/cassie/controllers_py.cc @@ -0,0 +1,69 @@ +#include +#include +#include +#include + +#include "examples/Cassie/diagrams/osc_running_controller_diagram.h" +#include "examples/Cassie/diagrams/osc_walking_controller_diagram.h" + +#include "drake/multibody/plant/multibody_plant.h" + +namespace py = pybind11; + +namespace dairlib { +namespace pydairlib { + +using examples::controllers::OSCRunningControllerDiagram; +using examples::controllers::OSCWalkingControllerDiagram; + +PYBIND11_MODULE(controllers, m) { + m.doc() = "Binding controller factories for Cassie"; + + using py_rvp = py::return_value_policy; + + py::class_>( + m, "OSCRunningControllerFactory") + .def(py::init&, const std::string&, const std::string&>(), + py::arg("plant"), py::arg("osc_gains_filename"), py::arg("osqp_settings_filename")) + .def("get_plant", &OSCRunningControllerDiagram::get_plant, + py_rvp::reference_internal) + .def("get_input_port_state", + &OSCRunningControllerDiagram::get_input_port_state, + py_rvp::reference_internal) + .def("get_input_port_radio", + &OSCRunningControllerDiagram::get_input_port_radio, + py_rvp::reference_internal) + .def("get_output_port_robot_input", + &OSCRunningControllerDiagram::get_output_port_robot_input, + py_rvp::reference_internal) + .def("get_output_port_torque", + &OSCRunningControllerDiagram::get_output_port_torque, + py_rvp::reference_internal) + .def("get_output_port_controller_failure", + &OSCRunningControllerDiagram::get_output_port_controller_failure, + py_rvp::reference_internal); + + py::class_>( + m, "OSCWalkingControllerFactory") + .def(py::init&, bool, const std::string&, const std::string&>(), + py::arg("plant"), py::arg("has_double_stance"), py::arg("osc_gains_filename"), py::arg("osqp_settings_filename")) + .def("get_plant", &OSCWalkingControllerDiagram::get_plant, + py_rvp::reference_internal) + .def("get_input_port_state", + &OSCWalkingControllerDiagram::get_input_port_state, + py_rvp::reference_internal) + .def("get_input_port_radio", + &OSCWalkingControllerDiagram::get_input_port_radio, + py_rvp::reference_internal) + .def("get_output_port_robot_input", + &OSCWalkingControllerDiagram::get_output_port_robot_input, + py_rvp::reference_internal) + .def("get_output_port_torque", + &OSCWalkingControllerDiagram::get_output_port_torque, + py_rvp::reference_internal) + .def("get_output_port_controller_failure", + &OSCWalkingControllerDiagram::get_output_port_controller_failure, + py_rvp::reference_internal); +} +} // namespace pydairlib +} // namespace dairlib \ No newline at end of file diff --git a/bindings/pydairlib/cassie/gym_envs/BUILD.bazel b/bindings/pydairlib/cassie/gym_envs/BUILD.bazel new file mode 100644 index 0000000000..889ddffedd --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/BUILD.bazel @@ -0,0 +1,85 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +py_library( + name = "cassie_gym_all", + srcs = [], + deps = [ + ":cassie_env_rewards", + ":drake_cassie_gym", + ":mujoco_cassie_gym", + ], +) + +py_library( + name = "drake_cassie_gym", + srcs = ["drake_cassie_gym.py"], + deps = [ + ":cassie_env_state", + "//bindings/pydairlib", + "//bindings/pydairlib/cassie", + "//bindings/pydairlib/multibody", + "//bindings/pydairlib/systems", + "//bindings/pydairlib/systems:primitives_py", + ], +) + +py_binary( + name = "cassie_gym_test", + srcs = ["cassie_gym_test.py"], + deps = [ + ":cassie_env_rewards", + ":cassie_gym_all", + "//bindings/pydairlib/cassie", + "@drake//bindings/pydrake", + ], +) + +py_library( + name = "cassie_traj", + srcs = ["cassie_traj.py"], + deps = [ + ], +) + +py_library( + name = "cassie_env_state", + srcs = ["cassie_env_state.py"], + deps = [ + ], +) + +py_library( + name = "cassie_env_rewards", + srcs = [ + "reward_base.py", + "reward_osudrl.py", + ], + deps = [ + ":cassie_env_state", + ], +) + +py_library( + name = "mujoco_cassie_gym", + srcs = ["mujoco_cassie_gym.py"], + deps = [ + ":cassie_env_state", + "//bindings/pydairlib", + "//bindings/pydairlib/cassie", + "//bindings/pydairlib/cassie/mujoco:cassie_mujoco_all", + "//bindings/pydairlib/multibody", + "//bindings/pydairlib/systems", + "//bindings/pydairlib/systems:primitives_py", + ], +) diff --git a/bindings/pydairlib/cassie/gym_envs/cassie_actor.py b/bindings/pydairlib/cassie/gym_envs/cassie_actor.py new file mode 100644 index 0000000000..45c87e0d12 --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/cassie_actor.py @@ -0,0 +1,26 @@ +import numpy as np +from cassie.cassie_traj import * + +from pydrake.multibody.parsing import Parser +from pydrake.systems.framework import DiagramBuilder +from pydrake.multibody.plant import * +from pydrake.systems.analysis import Simulator + +from pydairlib.common import FindResourceOrThrow +from pydairlib.cassie.cassie_utils import * +from pydairlib.multibody import * +from pydairlib.systems.primitives import * + + +class CassieActor(): + def __init__(self): + pass + + def make(self): + pass + + def reset(self): + pass + + def eval(self, action=np.zeros(10)): + pass diff --git a/bindings/pydairlib/cassie/gym_envs/cassie_env_state.py b/bindings/pydairlib/cassie/gym_envs/cassie_env_state.py new file mode 100644 index 0000000000..dbe93318e1 --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/cassie_env_state.py @@ -0,0 +1,78 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation as R + +CASSIE_QUATERNION_SLICE = slice(0, 4) +CASSIE_POSITION_SLICE = slice(4, 23) +CASSIE_OMEGA_SLICE = slice(23, 26) +CASSIE_VELOCITY_SLICE = slice(26, 45) +CASSIE_JOINT_POSITION_SLICE = slice(7, 23) +CASSIE_JOINT_VELOCITY_SLICE = slice(29, 45) +CASSIE_FB_POSITION_SLICE = slice(4, 7) +CASSIE_FB_VELOCITY_SLICE = slice(26, 29) + +CASSIE_NX = 45 +CASSIE_NQ = 23 +CASSIE_NV = 22 +CASSIE_NU = 10 +CASSIE_NACTION = 18 + + +class CassieEnvState(): + def __init__(self, t, x, u, action): + self.t = t + self.x = x + self.u = u + self.action = action + + def get_positions(self): + return self.x[CASSIE_POSITION_SLICE] + + def get_orientations(self): + return self.x[CASSIE_QUATERNION_SLICE] + + def get_velocities(self): + return self.x[CASSIE_VELOCITY_SLICE] + + def get_omegas(self): + return self.x[CASSIE_OMEGA_SLICE] + + def get_inputs(self): + return self.u + + def get_action(self): + return self.action + + def get_fb_positions(self): + return self.x[CASSIE_FB_POSITION_SLICE] + + def get_fb_velocities(self): + return self.x[CASSIE_FB_VELOCITY_SLICE] + + def get_joint_positions(self): + return self.x[CASSIE_JOINT_POSITION_SLICE] + + def get_joint_velocities(self): + return self.x[CASSIE_JOINT_VELOCITY_SLICE] + + def get_desired_forward_velocity(self): + return self.action[2] + + def get_desired_lateral_velocity(self): + return self.action[3] + + +def quat_to_rotation(q): + return R.from_quat([q[1], q[2], q[3], q[0]]) + +def reexpress_state_local_to_global_omega(state): + new_state = state.flatten() + rot = quat_to_rotation(new_state[CASSIE_QUATERNION_SLICE]) + new_state[CASSIE_OMEGA_SLICE] = rot.apply(new_state[CASSIE_OMEGA_SLICE]) + return new_state + +def reexpress_state_global_to_local_omega(state): + new_state = state.flatten() + rot = quat_to_rotation(new_state[CASSIE_QUATERNION_SLICE]) + new_state[CASSIE_OMEGA_SLICE] = rot.apply(new_state[CASSIE_OMEGA_SLICE], inverse=True) + return new_state diff --git a/bindings/pydairlib/cassie/gym_envs/cassie_gym_test.py b/bindings/pydairlib/cassie/gym_envs/cassie_gym_test.py new file mode 100644 index 0000000000..5b3e91760b --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/cassie_gym_test.py @@ -0,0 +1,45 @@ +from drake_cassie_gym import * +from mujoco_cassie_gym import * +# from cassie_utils import * +from pydairlib.cassie.controllers import OSCRunningControllerFactory +from pydairlib.cassie.controllers import OSCWalkingControllerFactory +from pydairlib.cassie.simulators import CassieSimDiagram +from pydairlib.cassie.gym_envs.reward_osudrl import RewardOSUDRL +from pydrake.common.yaml import yaml_load + + +def main(): + osc_running_gains_filename = 'examples/Cassie/osc_run/osc_running_gains.yaml' + # osc_running_gains_filename = 'examples/Cassie/osc_run/learned_osc_running_gains.yaml' + osc_walking_gains_filename = 'examples/Cassie/osc/osc_walking_gains_alip.yaml' + osqp_settings = 'examples/Cassie/osc_run/osc_running_qp_settings.yaml' + default_osqp_settings = 'examples/Cassie/osc/solver_settings/osqp_options_walking.yaml' + urdf = 'examples/Cassie/urdf/cassie_v2_conservative.urdf' + reward_function_weights = '' + + action = np.zeros(18) + cumulative_reward = 0 + while 1: + controller_plant = MultibodyPlant(1e-3) + AddCassieMultibody(controller_plant, None, True, urdf, False, False, False) + controller_plant.Finalize() + controller = OSCRunningControllerFactory(controller_plant, osc_running_gains_filename, osqp_settings) + # controller = OSCWalkingControllerFactory(controller_plant, True, osc_walking_gains_filename, osqp_settings) + + # reward_func = RewardOSUDRL(reward_function_weights) + reward_func = RewardOSUDRL() + + # gym_env = DrakeCassieGym(reward_func, visualize=True) + gym_env = MuJoCoCassieGym(reward_func, visualize=True) + gym_env.make(controller) + cumulative_reward = gym_env.advance_to(25) + # print(cumulative_reward) + gym_env.free_sim() + # gym_env.reset() + # while gym_env.current_time < 5.0: + # state, reward = gym_env.step(action) + # cumulative_reward += reward + # print(cumulative_reward) + +if __name__ == '__main__': + main() diff --git a/bindings/pydairlib/cassie/gym_envs/cassie_traj.py b/bindings/pydairlib/cassie/gym_envs/cassie_traj.py new file mode 100644 index 0000000000..3557155952 --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/cassie_traj.py @@ -0,0 +1,79 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation as R + +CASSIE_QUATERNION_SLICE = slice(0, 4) +CASSIE_POSITION_SLICE = slice(4, 23) +CASSIE_OMEGA_SLICE = slice(23, 26) +CASSIE_VELOCITY_SLICE = slice(26, 45) +CASSIE_JOINT_POSITION_SLICE = slice(7, 23) +CASSIE_JOINT_VELOCITY_SLICE = slice(29, 45) +CASSIE_FB_POSITION_SLICE = slice(4, 7) +CASSIE_FB_VELOCITY_SLICE = slice(26, 29) + +CASSIE_NX = 45 +CASSIE_NQ = 23 +CASSIE_NV = 22 +CASSIE_NU = 10 +CASSIE_NL = 12 + +# 10000 dts / 2000Hz = 5 seconds +CASSIE_EPS_LENGTH = 100000 + + +class CassieTraj(): + + def __init__(self): + self.t = np.zeros((CASSIE_EPS_LENGTH,)) + self.x_samples = np.zeros((CASSIE_EPS_LENGTH, CASSIE_NX)) # Cannot be empty + self.u_samples = np.zeros((CASSIE_EPS_LENGTH, CASSIE_NU)) # Cannot be empty + self.lambda_traj = np.zeros((CASSIE_EPS_LENGTH, CASSIE_NL)) # May be empty + + def time_to_index(self, t): + if int(t * 2000) >= self.u_samples.shape[0]: + print("time %.2f is out of bounds" % t) + return int(t * 2000) + + def get_positions(self): + return self.x_samples[:, CASSIE_POSITION_SLICE] + + def get_orientations(self): + return self.x_samples[:, CASSIE_QUATERNION_SLICE] + + def get_velocities(self): + return self.x_samples[:, CASSIE_VELOCITY_SLICE] + + def get_omegas(self): + return self.x_samples[:, CASSIE_OMEGA_SLICE] + + def plot_positions(self): + plt.plot(self.t, self.x_samples[:, CASSIE_POSITION_SLICE]) + + def plot_velocities(self): + plt.plot(self.t, self.x_samples[:, CASSIE_VELOCITY_SLICE]) + + def plot_efforts(self): + plt.plot(self.t, self.u_samples) + + def update(self, t, state, action): + index = self.time_to_index(t) + self.x_samples[index] = state + self.u_samples[index] = action + self.t[index] = t + +def quat_to_rotation(q): + return R.from_quat([q[1], q[2], q[3], q[0]]) + + +def reexpress_state_local_to_global_omega(state): + new_state = state.flatten() + rot = quat_to_rotation(new_state[CASSIE_QUATERNION_SLICE]) + new_state[CASSIE_OMEGA_SLICE] = rot.apply(new_state[CASSIE_OMEGA_SLICE]) + return new_state + + +def reexpress_state_global_to_local_omega(state): + new_state = state.flatten() + rot = quat_to_rotation(new_state[CASSIE_QUATERNION_SLICE]) + new_state[CASSIE_OMEGA_SLICE] = rot.apply(new_state[CASSIE_OMEGA_SLICE], inverse=True) + return new_state diff --git a/bindings/pydairlib/cassie/gym_envs/drake_cassie_gym.py b/bindings/pydairlib/cassie/gym_envs/drake_cassie_gym.py new file mode 100644 index 0000000000..d8802db8fe --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/drake_cassie_gym.py @@ -0,0 +1,123 @@ +import numpy as np + +from pydrake.multibody.parsing import Parser +from pydrake.systems.framework import DiagramBuilder +from pydrake.multibody.plant import * +from pydrake.systems.analysis import Simulator + +from pydairlib.common import FindResourceOrThrow +from pydairlib.cassie.cassie_utils import * +from pydairlib.multibody import * +from pydairlib.systems.primitives import * +from pydairlib.systems.robot_lcm_systems import RobotOutputSender +from dairlib import lcmt_radio_out +from pydairlib.cassie.simulators import CassieSimDiagram +from pydairlib.cassie.gym_envs.cassie_env_state import CassieEnvState + + +class DrakeCassieGym(): + def __init__(self, reward_func, visualize=False): + self.sim_dt = 1e-3 + self.visualize = visualize + self.reward_func = reward_func + self.start_time = 0.35 + self.current_time = 0.00 + # self.end_time = 0.05 + self.hardware_traj = None + self.action_dim = 10 + self.state_dim = 45 + self.x_init = np.array( + [1, 0, 0, 0, 0, 0, 0.9, -0.0358636, 0, 0.67432, -1.588, -0.0458742, 1.90918, + -0.0381073, -1.82312, 0.0358636, 0, 0.67432, -1.588, -0.0457885, 1.90919, -0.0382424, -1.82321, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + self.prev_cassie_state = None + self.controller = None + self.terminated = False + self.initialized = False + + + def make(self, controller, urdf='examples/Cassie/urdf/cassie_v2.urdf'): + self.builder = DiagramBuilder() + self.dt = 1e-3 + self.plant = MultibodyPlant(self.dt) + self.controller = controller + self.simulator = CassieSimDiagram(self.plant, urdf, self.visualize, 0.8, 1e4, 1e2) + self.new_plant = self.simulator.get_plant() + self.builder.AddSystem(self.controller) + self.builder.AddSystem(self.simulator) + + self.builder.Connect(self.controller.get_output_port_robot_input(), self.simulator.get_input_port_actuation()) + self.builder.Connect(self.simulator.get_output_port_state(), self.controller.get_input_port_state()) + # self.builder.Connect(self.simulator.get_output_port_cassie_out(), + # self.controller.get_cassie_out_input_port()) + # self.builder.Connect(self.controller, self.simulator.get_input_port_radio()) + + self.diagram = self.builder.Build() + self.sim = Simulator(self.diagram) + self.simulator_context = self.diagram.GetMutableSubsystemContext(self.simulator, self.sim.get_mutable_context()) + self.controller_context = self.diagram.GetMutableSubsystemContext(self.controller, self.sim.get_mutable_context()) + self.controller_output_port = self.controller.get_output_port_torque() + self.sim.get_mutable_context().SetTime(self.start_time) + self.reset() + self.initialized = True + + def reset(self): + self.new_plant.SetPositionsAndVelocities(self.new_plant.GetMyMutableContextFromRoot( + self.sim.get_mutable_context()), self.x_init) + self.sim.get_mutable_context().SetTime(self.start_time) + x = self.plant.GetPositionsAndVelocities( + self.plant.GetMyMutableContextFromRoot( + self.sim.get_context())) + u = np.zeros(10) + self.sim.Initialize() + self.current_time = self.start_time + self.prev_cassie_state = CassieEnvState(self.current_time, x, u, np.zeros(18)) + self.cassie_state = CassieEnvState(self.current_time, x, u, np.zeros(18)) + self.terminated = False + return + + def advance_to(self, time): + cumulative_reward = 0 + while self.current_time < time and not self.terminated: + _, reward = self.step() + cumulative_reward += reward + return cumulative_reward + + def check_termination(self): + return self.cassie_state.get_fb_positions()[2] < 0.4 + + + def step(self, action=np.zeros(18)): + if not self.initialized: + print("Call make() before calling step() or advance()") + next_timestep = self.sim.get_context().get_time() + self.sim_dt + action = self.velocity_profile(next_timestep) + self.simulator.get_input_port_radio().FixValue(self.simulator_context, action) + self.controller.get_input_port_radio().FixValue(self.controller_context, action) + self.sim.AdvanceTo(next_timestep) + self.current_time = self.sim.get_context().get_time() + + x = self.plant.GetPositionsAndVelocities( + self.plant.GetMyMutableContextFromRoot( + self.sim.get_context())) + u = self.controller_output_port.Eval(self.controller_context)[:-1] # remove the timestamp + # u = self.controller_output_port.Eval(self.controller_context)[:-1] # remove the timestamp + # print(u) + self.cassie_state = CassieEnvState(self.current_time, x, u, action) + reward = self.reward_func.compute_reward(self.sim_dt, self.cassie_state, self.prev_cassie_state) + self.terminated = self.check_termination() + self.prev_cassie_state = self.cassie_state + return self.cassie_state, reward + + def velocity_profile(self, timestep): + velocity_command = np.zeros(18) + velocity_command[2] = min(0.1 * timestep, 1.0) + # velocity_command[2] = 5.0 + return velocity_command + + def get_traj(self): + return self.traj + + # Some simulators for Cassie require cleanup + def free_sim(self): + return diff --git a/bindings/pydairlib/cassie/gym_envs/mujoco_cassie_gym.py b/bindings/pydairlib/cassie/gym_envs/mujoco_cassie_gym.py new file mode 100644 index 0000000000..75040a596a --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/mujoco_cassie_gym.py @@ -0,0 +1,416 @@ +import copy + +import numpy as np +import math + +from pydrake.multibody.parsing import Parser +from pydrake.systems.framework import DiagramBuilder +from pydrake.multibody.plant import * +from pydrake.systems.analysis import Simulator + +from pydairlib.common import FindResourceOrThrow +from pydairlib.cassie.cassie_utils import * +from pydairlib.multibody import * +from pydairlib.systems.primitives import * +from dairlib import lcmt_cassie_out +from pydairlib.cassie.gym_envs.cassie_env_state import CassieEnvState, quat_to_rotation, \ + reexpress_state_local_to_global_omega, reexpress_state_global_to_local_omega +from pydairlib.cassie.mujoco.drake_to_mujoco_converter import DrakeToMujocoConverter +# from drake_to_mujoco_converter import DrakeToMujocoConverter + +from pydairlib.cassie.mujoco.cassiemujoco import * +from pydairlib.cassie.mujoco.mujoco_lcm_utils import * +from pydairlib.cassie.mujoco.drake_to_mujoco_converter import DrakeToMujocoConverter + +from pydairlib.systems import (RobotOutputSender, RobotOutputReceiver) + + +class MuJoCoCassieGym(): + def __init__(self, reward_func, visualize=False, model_xml='cassie.xml', dynamics_randomization=True): + self.sim_dt = 8e-5 + self.gym_dt = 1e-3 + self.visualize = visualize + self.reward_func = reward_func + # hardware logs are 50ms long and start approximately 5ms before impact + # the simulator will check to make sure ground reaction forces are first detected within 3-7ms + self.start_time = 0.3 + self.current_time = 0.00 + self.end_time = 7.5 + + self.default_model_directory = '/home/yangwill/workspace/cassie-mujoco-sim/model/' + self.default_model_file = '/home/yangwill/workspace/cassie-mujoco-sim/model/cassie.xml' + + self.simulator = CassieSim(self.default_model_directory + model_xml) + if self.visualize: + self.cassie_vis = CassieVis(self.simulator) + + ''' + Copied from apex/cassie.py + ''' + self.max_simrate = 1.3 * self.gym_dt + self.min_simrate = 0.7 * self.gym_dt + self.dynamics_randomization = dynamics_randomization + self.dynamics_randomization = dynamics_randomization + self.slope_rand = dynamics_randomization + self.joint_rand = dynamics_randomization + + self.max_pitch_incline = 0.03 + self.max_roll_incline = 0.03 + + self.encoder_noise = 0.01 + + # self.damping_low = 0.3 + self.damping_low = 0.8 + # self.damping_high = 5.0 + self.damping_high = 2.0 + + # self.mass_low = 0.5 + self.mass_low = 0.8 + # self.mass_high = 1.5 + self.mass_high = 1.2 + + # self.fric_low = 0.4 + self.fric_low = 0.6 + self.fric_high = 1.1 + + self.speed = 0 + self.side_speed = 0 + self.orient_add = 0 + + self.default_damping = self.simulator.get_dof_damping() + self.default_mass = self.simulator.get_body_mass() + self.default_ipos = self.simulator.get_body_ipos() + self.default_fric = self.simulator.get_geom_friction() + self.default_rgba = self.simulator.get_geom_rgba() + self.default_quat = self.simulator.get_geom_quat() + + self.motor_encoder_noise = np.zeros(10) + self.joint_encoder_noise = np.zeros(6) + + self.cassie_in = cassie_user_in_t() + self.cassie_out = cassie_out_t() + self.cassie_out_lcm = lcmt_cassie_out() + self.action_dim = 10 + self.state_dim = 45 + self.x_init = np.array( + [1, 0, 0, 0, 0, 0, 0.85, -0.0358636, 0, 0.67432, -1.588, -0.0458742, 1.90918, + -0.0381073, -1.82312, 0.0358636, 0, 0.67432, -1.588, -0.0457885, 1.90919, -0.0382424, -1.82321, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) + self.prev_cassie_state = None + self.controller = None + self.terminated = False + self.initialized = False + self.actuator_index_map = {'hip_roll_left_motor': 0, + 'hip_yaw_left_motor': 1, + 'hip_pitch_left_motor': 2, + 'knee_left_motor': 3, + 'toe_left_motor': 4, + 'hip_roll_right_motor': 5, + 'hip_yaw_right_motor': 6, + 'hip_pitch_right_motor': 7, + 'knee_right_motor': 8, + 'toe_right_motor': 9} + # sim states + self.l_foot_frc = 0 + self.r_foot_frc = 0 + foot_pos = np.zeros(6) + self.l_foot_pos = np.zeros(3) + self.r_foot_pos = np.zeros(3) + self.l_foot_orient_cost = 0 + self.r_foot_orient_cost = 0 + self.neutral_foot_orient = np.array( + [-0.24790886454547323, -0.24679713195445646, -0.6609396704367185, 0.663921021343526]) + + def make(self, controller): + self.builder = DiagramBuilder() + self.controller = controller + + self.builder.AddSystem(self.controller) + self.robot_output_sender = RobotOutputSender(self.controller.get_plant(), False) + self.builder.AddSystem(self.robot_output_sender) + + self.builder.Connect(self.robot_output_sender.get_output_port(), self.controller.get_input_port_state()) + self.drake_to_mujoco_converter = DrakeToMujocoConverter(self.sim_dt) + + self.diagram = self.builder.Build() + self.drake_sim = Simulator(self.diagram) + self.robot_output_sender_context = self.diagram.GetMutableSubsystemContext(self.robot_output_sender, + self.drake_sim.get_mutable_context()) + self.controller_context = self.diagram.GetMutableSubsystemContext(self.controller, + self.drake_sim.get_mutable_context()) + self.controller_output_port = self.controller.get_output_port_torque() + self.radio_input_port = self.controller.get_input_port_radio() + self.drake_sim.get_mutable_context().SetTime(self.start_time) + self.reset() + self.initialized = True + + def reset(self): + self.randomize_sim_params() + + q_mujoco, v_mujoco = self.drake_to_mujoco_converter.convert_to_mujoco( + reexpress_state_global_to_local_omega(self.x_init)) + mujoco_state = self.simulator.get_state() + mujoco_state.set_qpos(q_mujoco) + mujoco_state.set_qvel(v_mujoco) + mujoco_state.set_time(self.start_time) + self.simulator.set_state(mujoco_state) + + self.drake_sim.get_mutable_context().SetTime(self.start_time) + u = np.zeros(10) + self.drake_sim.Initialize() + self.current_time = self.start_time + self.prev_cassie_state = CassieEnvState(self.current_time, self.x_init, u, np.zeros(18)) + self.cassie_state = CassieEnvState(self.current_time, self.x_init, u, np.zeros(18)) + cassie_in, u_mujoco = self.pack_input(self.cassie_in, u) + self.cassie_out = self.simulator.step(cassie_in) + self.cassie_out_lcm = self.pack_cassie_out(self.cassie_out) + self.terminated = False + return + + def advance_to(self, time): + cumulative_reward = 0 + while self.current_time < time and not self.terminated: + _, reward = self.step() + cumulative_reward += reward + return cumulative_reward + + def check_termination(self): + return self.cassie_state.get_fb_positions()[2] < 0.4 + + def velocity_profile(self, timestep): + velocity_command = np.zeros(18) + velocity_command[2] = min(0.1 * timestep, 2.0) + # velocity_command[2] = 5.0 + return velocity_command + + def step(self, action=np.zeros(18)): + + if not self.initialized: + print("Call make() before calling step() or advance()") + + if self.dynamics_randomization: + timestep = np.random.uniform(self.max_simrate, self.min_simrate) + else: + timestep = self.gym_dt + next_timestep = self.drake_sim.get_context().get_time() + timestep + + self.robot_output_sender.get_input_port_state().FixValue(self.robot_output_sender_context, self.cassie_state.x) + action = self.velocity_profile(next_timestep) + self.radio_input_port.FixValue(self.controller_context, action) + u = self.controller_output_port.Eval(self.controller_context)[:-1] # remove the timestamp + cassie_in, u_mujoco = self.pack_input(self.cassie_in, u) + self.drake_sim.AdvanceTo(next_timestep) + # self.reward_func.reset_clock_reward() + while self.simulator.time() < next_timestep: + self.simulator.step(cassie_in) + foot_pos = self.simulator.foot_pos() + # self.reward_func.update_clock_reward(self.simulator.get_foot_forces(), foot_pos, + # self.simulator.xquat("left-foot"), self.simulator.xquat("right-foot")) + + if self.visualize: + self.cassie_vis.draw(self.simulator) + + self.current_time = next_timestep + t = self.simulator.time() + q, v = self.drake_to_mujoco_converter.convert_to_drake(self.simulator.qpos(), self.simulator.qvel()) + self.current_time = t + x = np.hstack((q, v)) + x = reexpress_state_local_to_global_omega(x) + self.cassie_state = CassieEnvState(self.current_time, x, u, action) + reward = self.reward_func.compute_reward(timestep, self.cassie_state, self.prev_cassie_state) + self.terminated = self.check_termination() + self.prev_cassie_state = self.cassie_state + return self.cassie_state, reward + + def pack_input(self, cassie_in, u_drake): + act_map = self.drake_to_mujoco_converter.act_map + # Set control parameters + u_mujoco = np.zeros(10) + for u_name in act_map: + cassie_in.torque[self.actuator_index_map[u_name]] = u_drake[act_map[u_name]] + u_mujoco[self.actuator_index_map[u_name]] = u_drake[act_map[u_name]] + return cassie_in, u_mujoco + + def pack_cassie_out(self, cassie_out): + cassie_out_lcm = lcmt_cassie_out() + cassie_out_lcm.utime = self.current_time * 1e6 + cassie_out_lcm.pelvis.targetPc.etherCatStatus = cassie_out.pelvis.targetPc.etherCatStatus + cassie_out_lcm.pelvis.targetPc.etherCatNotifications = cassie_out.pelvis.targetPc.etherCatNotifications + cassie_out_lcm.pelvis.targetPc.taskExecutionTime = cassie_out.pelvis.targetPc.taskExecutionTime + cassie_out_lcm.pelvis.targetPc.overloadCounter = cassie_out.pelvis.targetPc.overloadCounter + cassie_out_lcm.pelvis.targetPc.cpuTemperature = cassie_out.pelvis.targetPc.cpuTemperature + cassie_out_lcm.pelvis.battery.dataGood = cassie_out.pelvis.battery.dataGood + cassie_out_lcm.pelvis.battery.stateOfCharge = cassie_out.pelvis.battery.stateOfCharge + cassie_out_lcm.pelvis.battery.current = cassie_out.pelvis.battery.current + cassie_out_lcm.pelvis.battery.voltage = cassie_out.pelvis.battery.voltage + cassie_out_lcm.pelvis.battery.temperature = cassie_out.pelvis.battery.temperature + cassie_out_lcm.pelvis.radio.radioReceiverSignalGood = cassie_out.pelvis.radio.radioReceiverSignalGood + cassie_out_lcm.pelvis.radio.receiverMedullaSignalGood = cassie_out.pelvis.radio.receiverMedullaSignalGood + cassie_out_lcm.pelvis.radio.channel = cassie_out.pelvis.radio.channel + cassie_out_lcm.pelvis.radio.receiverMedullaSignalGood = cassie_out.pelvis.radio.receiverMedullaSignalGood + cassie_out_lcm.pelvis.battery.temperature = cassie_out.pelvis.battery.temperature + cassie_out_lcm.pelvis.vectorNav.dataGood = cassie_out.pelvis.vectorNav.dataGood + cassie_out_lcm.pelvis.vectorNav.vpeStatus = cassie_out.pelvis.vectorNav.vpeStatus + cassie_out_lcm.pelvis.vectorNav.pressure = cassie_out.pelvis.vectorNav.pressure + cassie_out_lcm.pelvis.vectorNav.temperature = cassie_out.pelvis.vectorNav.temperature + cassie_out_lcm.pelvis.vectorNav.magneticField = cassie_out.pelvis.vectorNav.magneticField + cassie_out_lcm.pelvis.vectorNav.angularVelocity = cassie_out.pelvis.vectorNav.angularVelocity + cassie_out_lcm.pelvis.vectorNav.linearAcceleration = cassie_out.pelvis.vectorNav.linearAcceleration + cassie_out_lcm.pelvis.vectorNav.orientation = cassie_out.pelvis.vectorNav.orientation + cassie_out_lcm.pelvis.medullaCounter = cassie_out.pelvis.medullaCounter + cassie_out_lcm.pelvis.medullaCpuLoad = cassie_out.pelvis.medullaCpuLoad + cassie_out_lcm.pelvis.bleederState = cassie_out.pelvis.bleederState + cassie_out_lcm.pelvis.leftReedSwitchState = cassie_out.pelvis.leftReedSwitchState + cassie_out_lcm.pelvis.rightReedSwitchState = cassie_out.pelvis.rightReedSwitchState + cassie_out_lcm.pelvis.vtmTemperature = cassie_out.pelvis.vtmTemperature + + cassie_out_lcm.isCalibrated = cassie_out.isCalibrated + cassie_out_lcm.pelvis.vectorNav.temperature = cassie_out.pelvis.vectorNav.temperature + + self.copy_leg(cassie_out_lcm.leftLeg, cassie_out.leftLeg) + self.copy_leg(cassie_out_lcm.rightLeg, cassie_out.rightLeg) + return cassie_out_lcm + + def copy_leg(self, cassie_leg_out_lcm, cassie_leg_out): + self.copy_elmo(cassie_leg_out_lcm.hipRollDrive, cassie_leg_out.hipRollDrive) + self.copy_elmo(cassie_leg_out_lcm.hipYawDrive, cassie_leg_out.hipYawDrive) + self.copy_elmo(cassie_leg_out_lcm.hipPitchDrive, cassie_leg_out.hipPitchDrive) + self.copy_elmo(cassie_leg_out_lcm.kneeDrive, cassie_leg_out.kneeDrive) + self.copy_elmo(cassie_leg_out_lcm.footDrive, cassie_leg_out.footDrive) + cassie_leg_out_lcm.shinJoint.position = cassie_leg_out.shinJoint.position + cassie_leg_out_lcm.shinJoint.velocity = cassie_leg_out.shinJoint.velocity + cassie_leg_out_lcm.tarsusJoint.position = cassie_leg_out.tarsusJoint.position + cassie_leg_out_lcm.tarsusJoint.velocity = cassie_leg_out.tarsusJoint.velocity + cassie_leg_out_lcm.footJoint.position = cassie_leg_out.footJoint.position + cassie_leg_out_lcm.footJoint.velocity = cassie_leg_out.footJoint.velocity + cassie_leg_out_lcm.medullaCounter = cassie_leg_out.medullaCounter + cassie_leg_out_lcm.medullaCpuLoad = cassie_leg_out.medullaCpuLoad + cassie_leg_out_lcm.reedSwitchState = cassie_leg_out.reedSwitchState + + def copy_elmo(self, elmo_out_lcm, elmo_out): + elmo_out_lcm.statusWord = elmo_out.statusWord + elmo_out_lcm.position = elmo_out.position + elmo_out_lcm.velocity = elmo_out.velocity + elmo_out_lcm.torque = elmo_out.torque + elmo_out_lcm.driveTemperature = elmo_out.driveTemperature + elmo_out_lcm.dcLinkVoltage = elmo_out.dcLinkVoltage + elmo_out_lcm.torqueLimit = elmo_out.torqueLimit + elmo_out_lcm.gearRatio = elmo_out.gearRatio + + def randomize_sim_params(self): + if self.dynamics_randomization: + damp = self.default_damping + + pelvis_damp_range = [[damp[0], damp[0]], + [damp[1], damp[1]], + [damp[2], damp[2]], + [damp[3], damp[3]], + [damp[4], damp[4]], + [damp[5], damp[5]]] # 0->5 + + hip_damp_range = [[damp[6] * self.damping_low, damp[6] * self.damping_high], + [damp[7] * self.damping_low, damp[7] * self.damping_high], + [damp[8] * self.damping_low, damp[8] * self.damping_high]] # 6->8 and 19->21 + + achilles_damp_range = [[damp[9] * self.damping_low, damp[9] * self.damping_high], + [damp[10] * self.damping_low, damp[10] * self.damping_high], + [damp[11] * self.damping_low, damp[11] * self.damping_high]] # 9->11 and 22->24 + + knee_damp_range = [[damp[12] * self.damping_low, damp[12] * self.damping_high]] # 12 and 25 + shin_damp_range = [[damp[13] * self.damping_low, damp[13] * self.damping_high]] # 13 and 26 + tarsus_damp_range = [[damp[14] * self.damping_low, damp[14] * self.damping_high]] # 14 and 27 + + heel_damp_range = [[damp[15], damp[15]]] # 15 and 28 + fcrank_damp_range = [[damp[16] * self.damping_low, damp[16] * self.damping_high]] # 16 and 29 + prod_damp_range = [[damp[17], damp[17]]] # 17 and 30 + foot_damp_range = [[damp[18] * self.damping_low, damp[18] * self.damping_high]] # 18 and 31 + + side_damp = hip_damp_range + achilles_damp_range + knee_damp_range + shin_damp_range + tarsus_damp_range + heel_damp_range + fcrank_damp_range + prod_damp_range + foot_damp_range + damp_range = pelvis_damp_range + side_damp + side_damp + damp_noise = [np.random.uniform(a, b) for a, b in damp_range] + + m = self.default_mass + pelvis_mass_range = [[self.mass_low * m[1], self.mass_high * m[1]]] # 1 + hip_mass_range = [[self.mass_low * m[2], self.mass_high * m[2]], # 2->4 and 14->16 + [self.mass_low * m[3], self.mass_high * m[3]], + [self.mass_low * m[4], self.mass_high * m[4]]] + + achilles_mass_range = [[self.mass_low * m[5], self.mass_high * m[5]]] # 5 and 17 + knee_mass_range = [[self.mass_low * m[6], self.mass_high * m[6]]] # 6 and 18 + knee_spring_mass_range = [[self.mass_low * m[7], self.mass_high * m[7]]] # 7 and 19 + shin_mass_range = [[self.mass_low * m[8], self.mass_high * m[8]]] # 8 and 20 + tarsus_mass_range = [[self.mass_low * m[9], self.mass_high * m[9]]] # 9 and 21 + heel_spring_mass_range = [[self.mass_low * m[10], self.mass_high * m[10]]] # 10 and 22 + fcrank_mass_range = [[self.mass_low * m[11], self.mass_high * m[11]]] # 11 and 23 + prod_mass_range = [[self.mass_low * m[12], self.mass_high * m[12]]] # 12 and 24 + foot_mass_range = [[self.mass_low * m[13], self.mass_high * m[13]]] # 13 and 25 + + side_mass = hip_mass_range + achilles_mass_range \ + + knee_mass_range + knee_spring_mass_range \ + + shin_mass_range + tarsus_mass_range \ + + heel_spring_mass_range + fcrank_mass_range \ + + prod_mass_range + foot_mass_range + + mass_range = [[0, 0]] + pelvis_mass_range + side_mass + side_mass + mass_noise = [np.random.uniform(a, b) for a, b in mass_range] + + delta = 0.0 + com_noise = [0, 0, 0] + [np.random.uniform(val - delta, val + delta) for val in self.default_ipos[3:]] + + fric_noise = [] + translational = np.random.uniform(self.fric_low, self.fric_high) + torsional = np.random.uniform(1e-4, 5e-4) + rolling = np.random.uniform(1e-4, 2e-4) + for _ in range(int(len(self.default_fric) / 3)): + fric_noise += [translational, torsional, rolling] + + self.simulator.set_dof_damping(np.clip(damp_noise, 0, None)) + self.simulator.set_body_mass(np.clip(mass_noise, 0, None)) + self.simulator.set_body_ipos(com_noise) + self.simulator.set_geom_friction(np.clip(fric_noise, 0, None)) + else: + self.simulator.set_body_mass(self.default_mass) + self.simulator.set_body_ipos(self.default_ipos) + self.simulator.set_dof_damping(self.default_damping) + self.simulator.set_geom_friction(self.default_fric) + + if self.slope_rand: + geom_plane = [np.random.uniform(-self.max_roll_incline, self.max_roll_incline), + np.random.uniform(-self.max_pitch_incline, self.max_pitch_incline), 0] + quat_plane = euler2quat(z=geom_plane[2], y=geom_plane[1], x=geom_plane[0]) + geom_quat = list(quat_plane) + list(self.default_quat[4:]) + self.simulator.set_geom_quat(geom_quat) + else: + self.simulator.set_geom_quat(self.default_quat) + + if self.joint_rand: + self.motor_encoder_noise = np.random.uniform(-self.encoder_noise, self.encoder_noise, size=10) + self.joint_encoder_noise = np.random.uniform(-self.encoder_noise, self.encoder_noise, size=6) + else: + self.motor_encoder_noise = np.zeros(10) + self.joint_encoder_noise = np.zeros(6) + + def free_sim(self): + del self.simulator + if self.visualize: + del self.cassie_vis + + +def euler2quat(z=0, y=0, x=0): + z = z / 2.0 + y = y / 2.0 + x = x / 2.0 + cz = math.cos(z) + sz = math.sin(z) + cy = math.cos(y) + sy = math.sin(y) + cx = math.cos(x) + sx = math.sin(x) + result = np.array([ + cx * cy * cz - sx * sy * sz, + cx * sy * sz + cy * cz * sx, + cx * cz * sy - sx * cy * sz, + cx * cy * sz + sx * cz * sy]) + if result[0] < 0: + result = -result + return result diff --git a/bindings/pydairlib/cassie/gym_envs/reward_base.py b/bindings/pydairlib/cassie/gym_envs/reward_base.py new file mode 100644 index 0000000000..01d7900963 --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/reward_base.py @@ -0,0 +1,23 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation as R +import pickle + +CASSIE_QUATERNION_SLICE = slice(0, 4) +CASSIE_POSITION_SLICE = slice(4, 23) +CASSIE_OMEGA_SLICE = slice(23, 26) +CASSIE_VELOCITY_SLICE = slice(26, 45) +CASSIE_JOINT_POSITION_SLICE = slice(7, 23) +CASSIE_JOINT_VELOCITY_SLICE = slice(29, 45) +CASSIE_FB_POSITION_SLICE = slice(4, 7) +CASSIE_FB_VELOCITY_SLICE = slice(26, 29) +LOSS_WEIGHTS_FOLDER = 'examples/contact_parameter_learning/cassie_loss_weights/' + +class RewardBase(): + + def __init__(self, weights_filename): + weights = self.load_weights(weights_filename) + + def compute_reward(self, state): + reward = 0 + return reward \ No newline at end of file diff --git a/bindings/pydairlib/cassie/gym_envs/reward_osudrl.py b/bindings/pydairlib/cassie/gym_envs/reward_osudrl.py new file mode 100644 index 0000000000..45d8eb087d --- /dev/null +++ b/bindings/pydairlib/cassie/gym_envs/reward_osudrl.py @@ -0,0 +1,137 @@ +import numpy as np +import copy +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation as R +import pickle + +CASSIE_QUATERNION_SLICE = slice(0, 4) +CASSIE_POSITION_SLICE = slice(4, 23) +CASSIE_OMEGA_SLICE = slice(23, 26) +CASSIE_VELOCITY_SLICE = slice(26, 45) +CASSIE_JOINT_POSITION_SLICE = slice(7, 23) +CASSIE_JOINT_VELOCITY_SLICE = slice(29, 45) +CASSIE_FB_POSITION_SLICE = slice(4, 7) +CASSIE_FB_VELOCITY_SLICE = slice(26, 29) +LOSS_WEIGHTS_FOLDER = 'examples/contact_parameter_learning/cassie_loss_weights/' + +class RewardOSUDRL(): + + def __init__(self, weights_filename=None): + if weights_filename: + weights = self.load_weights(weights_filename) + self.neutral_foot_orient = np.array([-0.24790886454547323, -0.24679713195445646, -0.6609396704367185, 0.663921021343526]) + self.l_foot_orient = 0 + self.r_foot_orient = 0 + self.l_foot_frc = 0 + self.r_foot_frc = 0 + self.l_foot_pos = np.zeros(3) + self.r_foot_pos = np.zeros(3) + self.l_foot_vel = np.zeros(3) + self.r_foot_vel = np.zeros(3) + self.l_foot_orient_cost = 0 + self.r_foot_orient_cost = 0 + + return + + def load_weights(self, filename): + with open(LOSS_WEIGHTS_FOLDER + filename + '.pkl', 'rb') as f: + return pickle.load(f) + + def reset_clock_reward(self): + self.l_foot_frc = 0 + self.r_foot_frc = 0 + foot_pos = np.zeros(6) + self.l_foot_pos = np.zeros(3) + self.r_foot_pos = np.zeros(3) + self.l_foot_vel = np.zeros(3) + self.r_foot_vel = np.zeros(3) + self.l_foot_orient_cost = 0 + self.r_foot_orient_cost = 0 + + def update_clock_reward(self, foot_forces, foot_pos, l_foot_quat, r_foot_quat): + prev_foot = copy.deepcopy(np.hstack((self.l_foot_pos, self.r_foot_pos))) + self.l_foot_frc += foot_forces[0] + self.r_foot_frc += foot_forces[1] + # Relative Foot Position tracking + self.l_foot_pos += foot_pos[0:3] + self.r_foot_pos += foot_pos[3:6] + # Foot Orientation Cost + self.l_foot_orient_cost += (1 - np.inner(self.neutral_foot_orient, l_foot_quat) ** 2) + self.r_foot_orient_cost += (1 - np.inner(self.neutral_foot_orient, r_foot_quat) ** 2) + self.l_foot_vel = (foot_pos[0:3] - prev_foot[0:3]) / 0.0005 + self.r_foot_vel = (foot_pos[3:6] - prev_foot[3:6]) / 0.0005 + + def compute_reward(self, timestep, cassie_env_state, prev_cassie_env_state): + pos = cassie_env_state.get_positions() + vel = cassie_env_state.get_velocities() + com_pos = cassie_env_state.get_fb_positions() + com_vel = cassie_env_state.get_fb_velocities() + joint_pos = cassie_env_state.get_joint_positions() + joint_vel = cassie_env_state.get_joint_velocities() + torques = cassie_env_state.get_inputs() + prev_torques = prev_cassie_env_state.get_inputs() + com_angular_velocity = cassie_env_state.get_omegas() + + des_forward_vel = cassie_env_state.get_desired_forward_velocity() + + orient_targ = np.array([1, 0, 0, 0]) + + com_orient_error = 0 + # hip_yaw_penalty = 0 + com_vel_error = 0 + straight_diff = 0 + foot_vel_error = 0 + foot_frc_error = 0 + foot_orient_error = 0 + + + self.l_foot_frc /= timestep + self.r_foot_frc /= timestep + self.l_foot_pos /= timestep + self.r_foot_pos /= timestep + self.l_foot_orient_cost /= timestep + self.r_foot_orient_cost /= timestep + + # com orient error + com_orient_error += 10 * (1 - np.inner(orient_targ, cassie_env_state.get_orientations()) ** 2) + # foot orient error + # hip_yaw_penalty += 10 * (self.l_foot_orient_cost + self.r_foot_orient_cost) + foot_orient_error += 10 * (self.l_foot_orient_cost + self.r_foot_orient_cost) + # hip_yaw_penalty += np.abs(joint_vel[1]) + np.abs(joint_vel[8]) + + # com vel error + com_vel_error += np.linalg.norm(com_vel[0] - des_forward_vel) + + # straight difference penalty + straight_diff = np.abs(com_pos[1]) # straight difference penalty + if straight_diff < 0.05: + straight_diff = 0 + height_diff = np.abs(com_pos[2] - 0.85) # height deadzone is range from 0.05 to 0.2 meters depending on speed + deadzone_size = 0.05 + 0.05 * des_forward_vel + if height_diff < deadzone_size: + height_diff = 0 + pelvis_acc = 0.25 * (np.abs(com_angular_velocity.sum())) + pelvis_motion = straight_diff + height_diff + pelvis_acc + # print("vel_tracking") + # print(com_vel_error) + # print("pelvis_motion") + # print(pelvis_motion) + + hip_roll_penalty = np.abs(joint_vel[0]) + np.abs(joint_vel[7]) + + torque_penalty = 0.5 * (sum(np.abs(prev_torques - torques)) / len(torques)) + + reward = 0.200 * np.exp(-(com_orient_error + foot_orient_error)) + \ + 0.150 * np.exp(-pelvis_motion) + \ + 0.250 * np.exp(-com_vel_error) + \ + 0.100 * np.exp(-hip_roll_penalty) + \ + 0.025 * np.exp(-torque_penalty) + + + # print(0.200 * np.exp(-(com_orient_error + hip_yaw_penalty))) + # print(0.150 * np.exp(-pelvis_motion)) + # print(0.150 * np.exp(-com_vel_error)) + # print(0.050 * np.exp(-hip_roll_penalty)) + # print(0.025 * np.exp(-torque_penalty)) + # import pdb; pdb.set_trace() + return reward \ No newline at end of file diff --git a/bindings/pydairlib/cassie/input_supervisor_py.cc b/bindings/pydairlib/cassie/input_supervisor_py.cc new file mode 100644 index 0000000000..7ee93ccbe1 --- /dev/null +++ b/bindings/pydairlib/cassie/input_supervisor_py.cc @@ -0,0 +1,39 @@ +#include +#include +#include +#include + +#include "examples/Cassie/systems/input_supervisor.h" + +#include "drake/multibody/plant/multibody_plant.h" + +namespace py = pybind11; + +namespace dairlib { +namespace pydairlib { + +PYBIND11_MODULE(cassie_utils, m) { + m.doc() = "Binding utility functions for Cassie"; + + using drake::multibody::MultibodyPlant; + + py::class_(m, "InputSupervisor") + .def(py::init&, + const std::string&, double, double, Eigen::VectorXd&>()) + .def("get_input_port_command", &InputSupervisor::get_input_port_command) + .def("get_input_port_safety_controller", + &InputSupervisor::get_input_port_safety_controller) + .def("get_input_port_state", &InputSupervisor::get_input_port_state) + .def("get_input_port_controller_switch", + &InputSupervisor::get_input_port_controller_switch) + .def("get_input_port_cassie", &InputSupervisor::get_input_port_cassie) + .def("get_input_port_controller_error", + &InputSupervisor::get_input_port_controller_error) + .def("get_output_port_command", &InputSupervisor::get_output_port_command) + .def("get_output_port_status", &InputSupervisor::get_output_port_status) + .def("get_output_port_failure", + &InputSupervisor::get_output_port_failure); +} + +} // namespace pydairlib +} // namespace dairlib \ No newline at end of file diff --git a/bindings/pydairlib/cassie/learn_osc_gains.py b/bindings/pydairlib/cassie/learn_osc_gains.py new file mode 100644 index 0000000000..9d38471355 --- /dev/null +++ b/bindings/pydairlib/cassie/learn_osc_gains.py @@ -0,0 +1,203 @@ +import numpy as np +import os +import pickle +from json import dump, load +import concurrent.futures as futures +import nevergrad as ng + +from pydairlib.cassie.gym_envs.drake_cassie_gym import * +from pydairlib.cassie.gym_envs.mujoco_cassie_gym import * + +from pydairlib.cassie.controllers import OSCRunningControllerFactory +from pydairlib.cassie.controllers import OSCWalkingControllerFactory +from pydairlib.cassie.simulators import CassieSimDiagram +from pydairlib.cassie.gym_envs.reward_osudrl import RewardOSUDRL +from pydrake.common.yaml import yaml_load, yaml_dump +import time +import matplotlib.pyplot as plt +import yaml + + +class OSCGainsOptimizer(): + + def __init__(self, budget, reward_function, visualize=False): + self.budget = budget + self.total_loss = 0 + self.reward_function = reward_function + self.end_time = 10.0 + self.gym_env = None + self.sim = None + self.controller = None + self.visualize = visualize + + self.urdf = 'examples/Cassie/urdf/cassie_v2.urdf' + self.controller_urdf = 'examples/Cassie/urdf/cassie_v2_conservative.urdf' + self.default_osc_running_gains_filename = 'examples/Cassie/osc_run/osc_running_gains.yaml' + self.osc_running_gains_filename = 'examples/Cassie/osc_run/learned_osc_running_gains.yaml' + self.osqp_settings = 'examples/Cassie/osc_run/osc_running_qp_settings.yaml' + + self.drake_params_folder = "bindings/pydairlib/cassie/optimal_gains/" + self.date_prefix = time.strftime("%Y_%m_%d_%H") + self.loss_over_time = [] + + self.default_osc_gains = { + 'w_soft_constraint': 100, + 'w_input_reg': 0.001, + 'mu': 0.6, + 'w_hip_yaw': 2.5, + 'hip_yaw_kp': 40, + 'PelvisKp': np.array([0, 0, 50]), + 'PelvisKd': np.array([0, 0, 5]), + # 'PelvisRotKp': np.array([50, 100, 0]), + 'PelvisRotKd': np.array([10, 5, 1]), + 'SwingFootKp': np.array([125, 80, 50]), + 'SwingFootKd': np.array([5, 5, 1]), + 'FootstepKd': np.array([0.2, 0.45, 0]), + 'footstep_sagital_offset': 0, + 'mid_foot_height': 0.05, + 'rest_length': 0.95, + 'footstep_lateral_offset': 0.045, + # 'stance_duration': 0.30, + # 'flight_duration': 0.08, + } + + def save_params(self, folder, params, budget): + with open(folder + self.date_prefix + '_' + str(budget) + '.pkl', 'wb') as f: + pickle.dump(params, f, pickle.HIGHEST_PROTOCOL) + + def load_params(self, param_file, folder): + with open(folder + param_file + '.pkl', 'rb') as f: + return pickle.load(f) + + def write_params(self, params): + gains = yaml_load(filename=self.default_osc_running_gains_filename, private=True) + new_gains = gains.copy() + for key in params: + if hasattr(params[key], "__len__"): + new_gains[key] = np.diag(params[key]).flatten().tolist() + else: + new_gains[key] = params[key] + yaml_dump(new_gains, filename=self.osc_running_gains_filename) + + def get_batch_loss(self, params): + batch_size = 10 + batch_reward = 0 + for i in range(batch_size): + batch_reward += self.get_single_loss(params) + self.loss_over_time.append(batch_reward) + return batch_reward + + def get_single_loss(self, params): + self.write_params(params) + if np.random.random() < 0.0: + print('drake_sim') + gym_env = DrakeCassieGym(self.reward_function, visualize=self.visualize) + else: + # print('mujoco') + gym_env = MuJoCoCassieGym(self.reward_function, visualize=self.visualize) + gym_env.end_time = self.end_time + controller_plant = MultibodyPlant(8e-5) + AddCassieMultibody(controller_plant, None, True, self.controller_urdf, False, False) + controller_plant.Finalize() + controller = OSCRunningControllerFactory(controller_plant, self.osc_running_gains_filename, + self.osqp_settings) + gym_env.make(controller) + # rollout a trajectory and compute the loss + cumulative_reward = 0 + while gym_env.current_time < gym_env.end_time and not gym_env.terminated: + state, reward = gym_env.step(np.zeros(18)) + cumulative_reward += reward + # print(-cumulative_reward) + gym_env.free_sim() + return -cumulative_reward + + def learn_gains(self, batch=True): + self.default_params = ng.p.Dict( + w_soft_constraint=ng.p.Log(lower=1.0, upper=1000.0), + w_input_reg=ng.p.Log(lower=1e-5, upper=1e-1), + mu=ng.p.Scalar(lower=0.4, upper=1.0), + w_hip_yaw=ng.p.Scalar(lower=0, upper=10), + hip_yaw_kp=ng.p.Scalar(lower=20, upper=80), + PelvisKp=ng.p.Array(lower=0., upper=75., shape=(3,)), + PelvisKd=ng.p.Array(lower=0., upper=10., shape=(3,)), + # PelvisRotKp=ng.p.Array(lower=20., upper=150., shape=(3,)), + PelvisRotKd=ng.p.Array(lower=0., upper=15., shape=(3,)), + SwingFootKp=ng.p.Array(lower=20., upper=150., shape=(3,)), + SwingFootKd=ng.p.Array(lower=0., upper=15., shape=(3,)), + FootstepKd=ng.p.Array(lower=0., upper=1., shape=(3,)), + footstep_sagital_offset=ng.p.Scalar(lower=-0.1, upper=0.1), + mid_foot_height=ng.p.Scalar(lower=0.03, upper=0.15), + rest_length=ng.p.Scalar(lower=0.8, upper=1.0), + footstep_lateral_offset=ng.p.Scalar(lower=-0.1, upper=0.05), + # stance_duration=ng.p.Scalar(lower=0.25, upper=0.40), + # flight_duration=ng.p.Scalar(lower=0.05, upper=0.15), + ) + self.loss_over_time = [] + self.default_params.value = self.default_osc_gains + # optimizer = ng.optimizers.NGOpt(parametrization=self.default_params, budget=self.budget) + optimizer = ng.optimizers.CMandAS3(parametrization=self.default_params, budget=self.budget) + optimizer.register_callback("tell", ng.callbacks.ProgressBar()) + # with futures.ThreadPoolExecutor(max_workers=optimizer.num_workers) as executor: + # recommendation = optimizer.minimize(square, executor=executor, batch_mode=False) + # params = optimizer.minimize(self.get_single_loss) + params = optimizer.minimize(self.get_batch_loss) + loss_samples = np.array(self.loss_over_time) + np.save(self.drake_params_folder + 'loss_trajectory_' + str(self.budget), loss_samples) + self.save_params(self.drake_params_folder, params, budget) + + def learn_gains_apex(self): + from optimizers.ppo import run_experiment + import argparse + parser = argparse.ArgumentParser() + + # general args + parser.add_argument("--logdir", type=str, default="./trained_models/ppo/") # Where to log diagnostics to + parser.add_argument("--seed", default=0, type=int) # Sets Gym, PyTorch and Numpy seeds + parser.add_argument("--history", default=0, type=int) # number of previous states to use as input + parser.add_argument("--redis_address", type=str, default=None) # address of redis server (for cluster setups) + parser.add_argument("--viz_port", default=8097) # (deprecated) visdom server port + + # PPO algo args + parser.add_argument("--input_norm_steps", type=int, default=10000) + parser.add_argument("--n_itr", type=int, default=10000, help="Number of iterations of the learning algorithm") + parser.add_argument("--lr", type=float, default=1e-4, help="Adam learning rate") # Xie + parser.add_argument("--eps", type=float, default=1e-5, help="Adam epsilon (for numerical stability)") + parser.add_argument("--lam", type=float, default=0.95, help="Generalized advantage estimate discount") + parser.add_argument("--gamma", type=float, default=0.99, help="MDP discount") + parser.add_argument("--anneal", default=1.0, action='store_true', help="anneal rate for stddev") + parser.add_argument("--learn_stddev", default=False, action='store_true', help="learn std_dev or keep it fixed") + parser.add_argument("--std_dev", type=int, default=-1.5, help="exponent of exploration std_dev") + parser.add_argument("--entropy_coeff", type=float, default=0.0, help="Coefficient for entropy regularization") + parser.add_argument("--clip", type=float, default=0.2, help="Clipping parameter for PPO surrogate loss") + parser.add_argument("--minibatch_size", type=int, default=64, help="Batch size for PPO updates") + parser.add_argument("--epochs", type=int, default=3, help="Number of optimization epochs per PPO update") #Xie + parser.add_argument("--num_steps", type=int, default=5096, help="Number of sampled timesteps per gradient estimate") + parser.add_argument("--use_gae", type=bool, default=True,help="Whether or not to calculate returns using Generalized Advantage Estimation") + parser.add_argument("--num_procs", type=int, default=30, help="Number of threads to train on") + parser.add_argument("--max_grad_norm", type=float, default=0.05, help="Value to clip gradients at.") + parser.add_argument("--max_traj_len", type=int, default=400, help="Max episode horizon") + parser.add_argument("--recurrent", action='store_true') + parser.add_argument("--bounded", type=bool, default=False) + args = parser.parse_args() + + args = parse_previous(args) + + run_experiment(args) + + + +if __name__ == '__main__': + # budget = 2000 + budget = 500 + + reward_function = RewardOSUDRL() + + optimizer = OSCGainsOptimizer(budget, reward_function, visualize=False) + optimizer.learn_gains() + # optimizer.learn_gains_apex() + + # optimal_params = optimizer.load_params('2022_06_09_15_500', optimizer.drake_params_folder).value + # optimizer.write_params(optimal_params) + # reward_over_time = np.load('bindings/pydairlib/cassie/optimal_gains/loss_trajectory_500.npy') + # plt.plot(reward_over_time) + # plt.show() diff --git a/bindings/pydairlib/cassie/mujoco/BUILD.bazel b/bindings/pydairlib/cassie/mujoco/BUILD.bazel new file mode 100644 index 0000000000..53d6928895 --- /dev/null +++ b/bindings/pydairlib/cassie/mujoco/BUILD.bazel @@ -0,0 +1,49 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +py_library( + name = "cassie_mujoco_all", + srcs = [], + deps = [ + ":cassie_mujoco_py", + ":drake_to_mujoco_converter", + ":mujoco_lcm_utils", + ], +) + +py_library( + name = "mujoco_lcm_utils", + srcs = ["mujoco_lcm_utils.py"], + deps = [ + "//lcmtypes:lcmtypes_robot_py", + ], +) + +py_library( + name = "drake_to_mujoco_converter", + srcs = ["drake_to_mujoco_converter.py"], + deps = [ + "//bindings/pydairlib/cassie:cassie_utils_py", + "//lcmtypes:lcmtypes_robot_py", + "@drake//bindings/pydrake", + ], +) + +py_library( + name = "cassie_mujoco_py", + srcs = [ + "cassiemujoco.py", + "cassiemujoco_ctypes.py", + ], + deps = [], +) diff --git a/bindings/pydairlib/cassie/mujoco/cassiemujoco.py b/bindings/pydairlib/cassie/mujoco/cassiemujoco.py new file mode 100644 index 0000000000..b1c4205fa3 --- /dev/null +++ b/bindings/pydairlib/cassie/mujoco/cassiemujoco.py @@ -0,0 +1,893 @@ +# Copyright (c) 2018 Dynamic Robotics Laboratory +# +# Permission to use, copy, modify, and distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +from pydairlib.cassie.mujoco.cassiemujoco_ctypes import * +import os +import ctypes +import numpy as np + +# from envs.terrains.rand import * + +# Get base directory +_dir_path = os.path.dirname(os.path.realpath(__file__)) + +# Initialize libcassiesim +default_model = "../model/cassie.xml" +cassie_mujoco_init(str.encode(default_model)) + +# Interface classes +# Note: Making the optional argument be a global var be default is perhaps not the safest thing to do +class CassieSim: + def __init__(self, modelfile=default_model, terrain=False, perception=False, reinit=False): + + if modelfile is not default_model: + self.modelfile = modelfile + else: + base = 'cassie' + if perception: + base += '_perception' + if terrain: + base += '_hfield' + self.modelfile = os.path.join(_dir_path, base + '.xml') + + self.c = cassie_sim_init(self.modelfile.encode('utf-8'), True) + + if terrain: + x_res, y_res = self.get_hfield_nrow(), self.get_hfield_ncol() + self.hfields = generate_perlin(x_res, y_res) + + params_array = (ctypes.c_int32 * 6)() + cassie_sim_params(self.c, params_array) + self.nv = cassie_sim_nv(self.c) + self.nbody = cassie_sim_nbody(self.c) + self.nq = cassie_sim_nq(self.c) + self.ngeom = cassie_sim_ngeom(self.c) + + def randomize_terrain(self): + hfield = self.hfields[np.random.randint(len(self.hfields))] + self.set_hfield_data(hfield.flatten()) + return hfield + + def step(self, u): + y = cassie_out_t() + cassie_sim_step(self.c, y, u) + return y + + def step_pd(self, u): + y = state_out_t() + cassie_sim_step_pd(self.c, y, u) + return y + + def integrate_pos(self): + y = state_out_t() + cassie_integrate_pos(self.c, y) + return y + + def get_state(self): + s = CassieState() + cassie_get_state(self.c, s.s) + return s + + def set_state(self, s): + cassie_set_state(self.c, s.s) + + def time(self): + timep = cassie_sim_time(self.c) + return timep[0] + + def qpos(self): + qposp = cassie_sim_qpos(self.c) + return qposp[:self.nq] + + def qpos_full(self): + qposp = cassie_sim_qpos(self.c) + return qposp[:self.nq] + + def qvel(self): + qvelp = cassie_sim_qvel(self.c) + return qvelp[:self.nv] + + def qvel_full(self): + qvelp = cassie_sim_qvel(self.c) + return qvelp[:self.nv] + + def qacc(self): + qaccp = cassie_sim_qacc(self.c) + return qaccp[:self.nv] + + def xpos(self, body_name): + xposp = cassie_sim_xpos(self.c, body_name.encode()) + return xposp[:3] + + def xquat(self, body_name): + xquatp = cassie_sim_xquat(self.c, body_name.encode()) + return xquatp[:4] + + def set_time(self, time): + timep = cassie_sim_time(self.c) + timep[0] = time + + def set_qpos(self, qpos): + qposp = cassie_sim_qpos(self.c) + for i in range(min(len(qpos), self.nq)): + qposp[i] = qpos[i] + + def set_qvel(self, qvel): + qvelp = cassie_sim_qvel(self.c) + for i in range(min(len(qvel), self.nv)): + qvelp[i] = qvel[i] + + def hold(self): + cassie_sim_hold(self.c) + + def release(self): + cassie_sim_release(self.c) + + def apply_force(self, xfrc, body_name="cassie-pelvis"): + xfrc_array = (ctypes.c_double * 6)() + for i in range(len(xfrc)): + xfrc_array[i] = xfrc[i] + cassie_sim_apply_force(self.c, xfrc_array, body_name.encode()) + + def sense_ground(self): + percept = (ctypes.c_double * 6)() + cassie_sim_read_rangefinder(self.c, percept) + + ret = np.zeros(6) + for i in range(6): + ret[i] = percept[i] + return ret + + def get_jacobian(self, name): + jacp = np.zeros(3*self.nv) + jacp_array = (ctypes.c_double * (3*self.nv))() + cassie_sim_get_jacobian(self.c, jacp_array, name.encode()) + for i in range(3*self.nv): + jacp[i] = jacp_array[i] + return jacp + + def get_jacobian_full(self, name): + jacp = np.zeros(3*self.nv) + jacp_array = (ctypes.c_double * (3*self.nv))() + jacr = np.zeros(3*self.nv) + jacr_array = (ctypes.c_double * (3*self.nv))() + cassie_sim_get_jacobian_full(self.c, jacp_array, jacr_array, name.encode()) + for i in range(3*self.nv): + jacp[i] = jacp_array[i] + jacr[i] = jacr_array[i] + return jacp, jacr + + def get_jacobian_full_site(self, name): + jacp = np.zeros(3*self.nv) + jacp_array = (ctypes.c_double * (3*self.nv))() + jacr = np.zeros(3*self.nv) + jacr_array = (ctypes.c_double * (3*self.nv))() + cassie_sim_get_jacobian_full_site(self.c, jacp_array, jacr_array, name.encode()) + for i in range(3*self.nv): + jacp[i] = jacp_array[i] + jacr[i] = jacr_array[i] + return jacp, jacr + + def get_foot_forces(self): + force = np.zeros(12) + frc_array = (ctypes.c_double * 12)() + cassie_sim_foot_forces(self.c, frc_array) + for i in range(12): + force[i] = frc_array[i] + lfrc = np.sqrt(np.power(force[0:3], 2).sum()) + rfrc = np.sqrt(np.power(force[6:9], 2).sum()) + return lfrc, rfrc + + # Returns 2 arrays each 6 long, the toe force and heel force. Each array is in order of + # left foot (3) and then right foot (3) + def get_heeltoe_forces(self): + toe_force = np.zeros(6) + heel_force = np.zeros(6) + toe_array = (ctypes.c_double * 6)() + heel_array = (ctypes.c_double * 6)() + cassie_sim_heeltoe_forces(self.c, toe_array, heel_array) + for i in range(6): + toe_force[i] = toe_array[i] + heel_force[i] = heel_array[i] + return toe_force, heel_force + + def foot_pos(self): + pos_array = (ctypes.c_double * 6)() + cassie_sim_foot_positions(self.c, pos_array) + pos = [] + for i in range(6): + pos.append(pos_array[i]) + return pos + + def foot_vel(self, vel): + vel_array = (ctypes.c_double * 12)() + cassie_sim_foot_velocities(self.c, vel_array) + for i in range(12): + vel[i] = vel_array[i] + + def body_vel(self, vel, body_name): + vel_array = (ctypes.c_double * 6)() + cassie_sim_body_vel(self.c, vel_array, body_name.encode()) + for i in range(6): + vel[i] = vel_array[i] + + # Returns the center of mass position vector in world frame + def center_of_mass_position(self): + pos_array = (ctypes.c_double * 3)() + cassie_sim_cm_position(self.c, pos_array) + pos = [] + for i in range(3): + pos.append(pos_array[i]) + return pos + + # Returns the center of mass velocity vector in world frame + def center_of_mass_velocity(self): + vel_array = (ctypes.c_double * 3)() + cassie_sim_cm_velocity(self.c, vel_array) + vel = [] + for i in range(3): + vel.append(vel_array[i]) + return vel + + # Returns 3x3 rotational intertia matrix of the robot around its center + # of mass in the pelvis frame. [kg*m^2] + def centroid_inertia(self): + I_array = (ctypes.c_double * 9)() + cassie_sim_centroid_inertia(self.c, I_array) + Icm = [] + for i in range(9): + Icm.append(I_array[i]) + return Icm + + # Return the angular momentum of the robot in the world frame. + def angular_momentum(self): + L_array = (ctypes.c_double * 3)() + cassie_sim_angular_momentum(self.c, L_array) + L_return = [] + for i in range(3): + L_return.append(L_array[i]) + return L_return + + # Return the full 32x32 mass matrix of Cassie. + def full_mass_matrix(self): + M_array = (ctypes.c_double * (32 * 32))() + cassie_sim_full_mass_matrix(self.c, M_array) + M_return = np.zeros((32,32)) + for i in range(32): + for j in range(32): + M_return[i,j] = M_array[i*32+j] + return M_return + + def constraint_jacobian(self): + J_array = (ctypes.c_double * (6 * 32))() + err_array = (ctypes.c_double * (6))() + cassie_sim_loop_constraint_info(self.c, J_array, err_array) + J_return = np.zeros((6,32)) + for i in range(6): + for j in range(32): + J_return[i,j] = J_array[i*32+j] + return J_return + + def constraint_error(self): + J_array = (ctypes.c_double * (6 * 32))() + err_array = (ctypes.c_double * (6))() + cassie_sim_loop_constraint_info(self.c, J_array, err_array) + err_return = np.zeros((6,1)) + for i in range(6): + err_return[i] = err_array[i] + return err_return + + # Return the minimal actuated mass matrix of Cassie. Contains 6 for floating + # base, 5 for left leg motors, 5 for right leg motors. + def minimal_mass_matrix(self): + ind_full_idx = [0,1,2,3,4,5,6,7,8,12,18,19,20,21,25,31] + dep_full_idx = [9,10,11,14,22,23,24,27] + ind_idx = np.arange(0, 16) + dep_idx = np.arange(16, 24) + spring_idx = [13, 15, 26, 28] + + J_c = self.constraint_jacobian() + J_c[:, 0:6] = 0 # Zero out floating base coordinates + J_c_div = J_c[:, ind_full_idx + dep_full_idx] + J_c_div[J_c_div < 1e-5] = 0 # Zero out very small terms for numerical stabilityL + + M = self.full_mass_matrix() + M_div_temp = M[:, ind_full_idx + dep_full_idx] + M_div = M_div_temp[ind_full_idx + dep_full_idx, :] + + G = np.linalg.lstsq(J_c_div[:, dep_idx], -J_c_div[:, ind_idx]) + + # print(G[0].shape) + P = np.block([[np.eye(16)], [G[0]]]) + M_minimal = P.T @ M_div @ P + + #J_c_dep = J_c[:, (13,24)] + # This gets hard + # import sys + # np.set_printoptions(threshold=sys.maxsize) + M_print = M_div[:, ind_idx] + M_print = M_print[ind_idx, :] + + # print("M") + # print(M_print) + # print("M_minimal") + # print(M_minimal) + + + return M_minimal + + + def foot_quat(self, quat): + quat_array = (ctypes.c_double * 4)() + cassie_sim_foot_quat(self.c, quat_array) + for i in range(4): + quat[i] = quat_array[i] + + def clear_forces(self): + cassie_sim_clear_forces(self.c) + + def get_dof_damping(self): + ptr = cassie_sim_dof_damping(self.c) + ret = np.zeros(self.nv) + for i in range(self.nv): + ret[i] = ptr[i] + return ret + + def get_body_mass(self): + ptr = cassie_sim_body_mass(self.c) + ret = np.zeros(self.nbody) + for i in range(self.nbody): + ret[i] = ptr[i] + return ret + + def get_body_ipos(self): + nbody = self.nbody * 3 + ptr = cassie_sim_body_ipos(self.c) + ret = np.zeros(nbody) + for i in range(nbody): + ret[i] = ptr[i] + return ret + + def get_body_pos(self, name): + ptr = cassie_sim_get_body_name_pos(self.c, name.encode()) + ret = np.zeros(3) + for i in range(3): + ret[i] = ptr[i] + return ret + + def get_geom_friction(self): + ptr = cassie_sim_geom_friction(self.c) + ret = np.zeros(self.ngeom * 3) + for i in range(self.ngeom * 3): + ret[i] = ptr[i] + return ret + + def get_geom_rgba(self, name=None): + if name is not None: + ptr = cassie_sim_geom_name_rgba(self.c, name.encode()) + ret = np.zeros(4) + for i in range(4): + ret[i] = ptr[i] + else: + ptr = cassie_sim_geom_rgba(self.c) + ret = np.zeros(self.ngeom * 4) + for i in range(self.ngeom * 4): + ret[i] = ptr[i] + return ret + + def get_geom_quat(self, name=None): + if name is not None: + ptr = cassie_sim_geom_name_quat(self.c, name.encode()) + ret = np.zeros(4) + for i in range(4): + ret[i] = ptr[i] + else: + ptr = cassie_sim_geom_quat(self.c) + ret = np.zeros(self.ngeom * 4) + for i in range(self.ngeom * 4): + ret[i] = ptr[i] + return ret + + def get_geom_pos(self, name=None): + if name is not None: + ptr = cassie_sim_geom_name_pos(self.c, name.encode()) + ret = np.zeros(3) + for i in range(3): + ret[i] = ptr[i] + else: + ptr = cassie_sim_geom_pos(self.c) + ret = np.zeros(self.ngeom * 3) + for i in range(self.ngeom * 3): + ret[i] = ptr[i] + return ret + + def get_geom_size(self, name=None): + if name is not None: + ptr = cassie_sim_geom_name_size(self.c, name.encode()) + ret = np.zeros(3) + for i in range(3): + ret[i] = ptr[i] + else: + ptr = cassie_sim_geom_size(self.c) + ret = np.zeros(self.ngeom * 3) + for i in range(self.ngeom * 3): + ret[i] = ptr[i] + return ret + + def set_dof_damping(self, data): + c_arr = (ctypes.c_double * self.nv)() + + if len(data) != self.nv: + print("SIZE MISMATCH SET_DOF_DAMPING()") + exit(1) + + for i in range(self.nv): + c_arr[i] = data[i] + + cassie_sim_set_dof_damping(self.c, c_arr) + + def set_body_mass(self, data, name=None): + # If no name is provided, set ALL body masses and assume "data" is array + # containing masses for every body + if name is None: + c_arr = (ctypes.c_double * self.nbody)() + + if len(data) != self.nbody: + print("SIZE MISMATCH SET_BODY_MASS()") + exit(1) + + for i in range(self.nbody): + c_arr[i] = data[i] + + cassie_sim_set_body_mass(self.c, c_arr) + # If name is provided, only set mass for specified body and assume + # "data" is a single double + else: + cassie_sim_set_body_name_mass(self.c, name.encode(), ctypes.c_double(data)) + + def set_body_pos(self, name, data): + if len(data) != 3: + print("SIZE MISMATCH SET BODY POS") + exit(1) + c_arr = (ctypes.c_double * 3)() + for i in range(3): + c_arr[i] = data[i] + cassie_sim_set_body_name_pos(self.c, name.encode(), c_arr) + + def set_body_ipos(self, data): + nbody = self.nbody * 3 + c_arr = (ctypes.c_double * nbody)() + + if len(data) != nbody: + print("SIZE MISMATCH SET_BODY_IPOS()") + exit(1) + + for i in range(nbody): + c_arr[i] = data[i] + + cassie_sim_set_body_ipos(self.c, c_arr) + + def set_geom_friction(self, data, name=None): + if name is None: + c_arr = (ctypes.c_double * (self.ngeom*3))() + + if len(data) != self.ngeom*3: + print("SIZE MISMATCH SET_GEOM_FRICTION()") + exit(1) + + for i in range(self.ngeom*3): + c_arr[i] = data[i] + + cassie_sim_set_geom_friction(self.c, c_arr) + else: + fric_array = (ctypes.c_double * 3)() + for i in range(3): + fric_array[i] = data[i] + cassie_sim_set_geom_name_friction(self.c, name.encode(), fric_array) + + + def set_geom_rgba(self, data, name=None): + if name is None: + ngeom = self.ngeom * 4 + + if len(data) != ngeom: + print("SIZE MISMATCH SET_GEOM_RGBA()") + exit(1) + + c_arr = (ctypes.c_float * ngeom)() + + for i in range(ngeom): + c_arr[i] = data[i] + + cassie_sim_set_geom_rgba(self.c, c_arr) + else: + rgba_array = (ctypes.c_float * 4)() + for i in range(4): + rgba_array[i] = data[i] + cassie_sim_set_geom_name_rgba(self.c, name.encode(), rgba_array) + + def set_geom_quat(self, data, name=None): + if name is None: + ngeom = self.ngeom * 4 + + if len(data) != ngeom: + print("SIZE MISMATCH SET_GEOM_QUAT()") + exit(1) + + c_arr = (ctypes.c_double * ngeom)() + + for i in range(ngeom): + c_arr[i] = data[i] + + cassie_sim_set_geom_quat(self.c, c_arr) + else: + quat_array = (ctypes.c_double * 4)() + for i in range(4): + quat_array[i] = data[i] + cassie_sim_set_geom_name_quat(self.c, name.encode(), quat_array) + + + def set_geom_pos(self, data, name=None): + if name is None: + ngeom = self.ngeom * 3 + + if len(data) != ngeom: + print("SIZE MISMATCH SET_GEOM_POS()") + exit(1) + + c_arr = (ctypes.c_double * ngeom)() + + for i in range(ngeom): + c_arr[i] = data[i] + + cassie_sim_set_geom_pos(self.c, c_arr) + else: + array = (ctypes.c_double * 3)() + for i in range(3): + array[i] = data[i] + cassie_sim_set_geom_name_pos(self.c, name.encode(), array) + + def set_geom_size(self, data, name=None): + if name is None: + ngeom = self.ngeom * 3 + + if len(data) != ngeom: + print("SIZE MISMATCH SET_GEOM_POS()") + exit(1) + + c_arr = (ctypes.c_double * ngeom)() + + for i in range(ngeom): + c_arr[i] = data[i] + + cassie_sim_set_geom_size(self.c, c_arr) + else: + array = (ctypes.c_double * 3)() + for i in range(3): + array[i] = data[i] + cassie_sim_set_geom_name_size(self.c, name.encode(), array) + + def set_const(self): + cassie_sim_set_const(self.c) + + def full_reset(self): + cassie_sim_full_reset(self.c) + + def get_hfield_nrow(self): + return cassie_sim_get_hfield_nrow(self.c) + + def get_hfield_ncol(self): + return cassie_sim_get_hfield_ncol(self.c) + + def get_nhfielddata(self): + return cassie_sim_get_nhfielddata(self.c) + + def get_hfield_size(self): + ret = np.zeros(4) + ptr = cassie_sim_get_hfield_size(self.c) + for i in range(4): + ret[i] = ptr[i] + return ret + + # Note that data has to be a flattened array. If flattening 2d numpy array, rows are y axis + # and cols are x axis. The data must also be normalized to (0-1) + def set_hfield_data(self, data, vis=None): + nhfielddata = self.get_nhfielddata() + if len(data) != nhfielddata: + print("SIZE MISMATCH SET_HFIELD_DATA") + exit(1) + data_arr = (ctypes.c_float * nhfielddata)(*data) + cassie_sim_set_hfielddata(self.c, ctypes.cast(data_arr, ctypes.POINTER(ctypes.c_float))) + + if vis is not None: + cassie_vis_remakeSceneCon(vis) + + def get_hfield_data(self): + nhfielddata = self.get_nhfielddata() + ret = np.zeros(nhfielddata) + ptr = cassie_sim_hfielddata(self.c) + for i in range(nhfielddata): + ret[i] = ptr[i] + return ret + + def set_hfield_size(self, data): + if len(data) != 4: + print("SIZE MISMATCH SET_HFIELD_SIZE") + exit(1) + size_array = (ctypes.c_double * 4)() + for i in range(4): + size_array[i] = data[i] + cassie_sim_set_hfield_size(self.c, size_array) + + # Returns a pointer to an array of joint_filter_t objects. Can be accessed/indexed as a usual python array of + # joint filter objects + def get_joint_filter(self): + j_filters = cassie_sim_joint_filter(self.c) + return j_filters + + # Set interal state of the joint filters. Takes in 2 arrays of values (x and y), which should be 6*4 and 6*3 long respectively. + # (6 joints, for each joint x has 4 values y has 3) + def set_joint_filter(self, x, y): + x_arr = (ctypes.c_double * (6*4))(*x) + y_arr = (ctypes.c_double * (6*3))(*y) + cassie_sim_set_joint_filter(self.c, ctypes.cast(x_arr, ctypes.POINTER(ctypes.c_double)), ctypes.cast(y_arr, ctypes.POINTER(ctypes.c_double))) + + # Returns a pointer to an array of drive_filter_t objects. Can be accessed/indexed as a usual python array of + # drive filter objects + def get_drive_filter(self): + d_filters = cassie_sim_drive_filter(self.c) + return d_filters + + # Set interal state of the drive filters. Takes in an array of values (x), which should be 10*9 long. + # (10 motor, for each motor have 9 values) + def set_drive_filter(self, x): + x_arr = (ctypes.c_int * (10*9))(*x) + cassie_sim_set_drive_filter(self.c, ctypes.cast(x_arr, ctypes.POINTER(ctypes.c_int))) + + # Get the current state of the torque delay array. Returns a 2d numpy array of size (10, 6), + # number of motors by number of delay cycles + def get_torque_delay(self): + t_arr = (ctypes.c_double * 60)() + cassie_sim_torque_delay(self.c, t_arr) + return np.array(t_arr[:]).reshape((10, 6)) + + # Set the torque delay state. Takes in a 2d numpy array of size (10, 6), number of motors by number of delay cycles + def set_torque_delay(self, data): + set_t_arr = (ctypes.c_double * 60)(*data.flatten()) + cassie_sim_set_torque_delay(self.c, ctypes.cast(set_t_arr, ctypes.POINTER(ctypes.c_double))) + + def __del__(self): + cassie_sim_free(self.c) + +class CassieState: + def __init__(self): + self.s = cassie_state_alloc() + + def time(self): + timep = cassie_state_time(self.s) + return timep[0] + + def qpos(self): + qposp = cassie_state_qpos(self.s) + return qposp[:35] + + def qvel(self): + qvelp = cassie_state_qvel(self.s) + return qvelp[:32] + + def set_time(self, time): + timep = cassie_state_time(self.s) + timep[0] = time + + def set_qpos(self, qpos): + qposp = cassie_state_qpos(self.s) + for i in range(min(len(qpos), 35)): + qposp[i] = qpos[i] + + def set_qvel(self, qvel): + qvelp = cassie_state_qvel(self.s) + for i in range(min(len(qvel), 32)): + qvelp[i] = qvel[i] + + def __del__(self): + cassie_state_free(self.s) + + +class CassieVis: + def __init__(self, c, offscreen=False): + self.v = cassie_vis_init(c.c, c.modelfile.encode('utf-8'), ctypes.c_bool(offscreen)) + self.is_recording = False + + def draw(self, c): + state = cassie_vis_draw(self.v, c.c) + return state + + def valid(self): + return cassie_vis_valid(self.v) + + def ispaused(self): + return cassie_vis_paused(self.v) + + def remake(self): + cassie_vis_remakeSceneCon(self.v) + + # Applies the inputted force to the inputted body. "xfrc_apply" should contain the force/torque to + # apply in Cartesian coords as a 6-long array (first 3 are force, last 3 are torque). "body_name" + # should be a string matching a body name in the XML file. If "body_name" doesn't match an existing + # body name, then no force will be applied. + def apply_force(self, xfrc_apply, body_name): + xfrc_array = (ctypes.c_double * 6)() + for i in range(len(xfrc_apply)): + xfrc_array[i] = xfrc_apply[i] + cassie_vis_apply_force(self.v, xfrc_array, body_name.encode()) + + def add_marker(self, pos, size, rgba, so3): + pos_array = (ctypes.c_double * 3)() + for i in range(len(pos)): + pos_array[i] = pos[i] + size_array = (ctypes.c_double * 3)() + for i in range(len(size)): + size_array[i] = size[i] + rgba_array = (ctypes.c_double * 4)() + for i in range(len(rgba)): + rgba_array[i] = rgba[i] + so3_array = (ctypes.c_double * 9)() + for i in range(len(so3)): + so3_array[i] = so3[i] + cassie_vis_add_marker(self.v, pos_array, size_array, rgba_array, so3_array) + + def remove_marker(self, id_val): + cassie_vis_remove_marker(self.v, id_val) + + def clear_markers(self): + cassie_vis_clear_markers(self.v) + + def update_marker(self, id_val, pos, size, rgba, so3): + pos_array = (ctypes.c_double * 3)() + size_array = (ctypes.c_double * 3)() + rgba_array = (ctypes.c_double * 4)() + so3_array = (ctypes.c_double * 9)() + for i in range(len(pos)): + pos_array[i] = pos[i] + for i in range(len(size)): + size_array[i] = size[i] + for i in range(len(rgba)): + rgba_array[i] = rgba[i] + for i in range(len(so3)): + so3_array[i] = so3[i] + cassie_vis_update_marker_pos(self.v, ctypes.c_int(id_val), pos_array) + cassie_vis_update_marker_size(self.v, ctypes.c_int(id_val), size_array) + cassie_vis_update_marker_rgba(self.v, ctypes.c_int(id_val), rgba_array) + cassie_vis_update_marker_orient(self.v, ctypes.c_int(id_val), so3_array) + + def reset(self, c): + cassie_vis_full_reset(self.v, c.c) + #cassie_vis_close(self.v) + #cassie_vis_free(self.v) + #delattr(self, 'v') + #self.v = cassie_vis_init(c.c, c.modelfile.encode('utf-8')) + + def set_cam(self, body_name, zoom, azimuth, elevation): + cassie_vis_set_cam(self.v, body_name.encode(), zoom, azimuth, elevation) + + def window_resize(self, width=1200, height=900): + cassie_vis_window_resize(self.v, ctypes.c_int(width), ctypes.c_int(height)) + + def attach_cam(self, cam_name='egocentric'): + cassie_vis_attach_cam(self.v, cam_name.encode()) + + def init_depth(self, width, height): + cassie_vis_init_depth(self.v, ctypes.c_int(width), ctypes.c_int(height)) + + def get_depth_size(self): + size = cassie_vis_get_depth_size(self.v) + return size + + def draw_depth(self, c, width=30, height=30): + depth = cassie_vis_draw_depth(self.v, c.c, ctypes.c_int(width), ctypes.c_int(height)) + return depth[:width*height] + + def init_recording(self, filename, width=1920, height=1080): + cassie_vis_init_recording(self.v, filename.encode(), ctypes.c_int(width), ctypes.c_int(height)) + self.is_recording = True + + def record_frame(self): + cassie_vis_record_frame(self.v) + + def close_recording(self): + cassie_vis_close_recording(self.v) + self.is_recording = False + + def __del__(self): + cassie_vis_free(self.v) + +class CassieUdp: + def __init__(self, remote_addr='127.0.0.1', remote_port='25000', + local_addr='0.0.0.0', local_port='25001'): + self.sock = udp_init_client(str.encode(remote_addr), + str.encode(remote_port), + str.encode(local_addr), + str.encode(local_port)) + self.packet_header_info = packet_header_info_t() + self.recvlen = 2 + 697 + self.sendlen = 2 + 58 + self.recvlen_pd = 2 + 493 + self.sendlen_pd = 2 + 476 + self.recvbuf = (ctypes.c_ubyte * max(self.recvlen, self.recvlen_pd))() + self.sendbuf = (ctypes.c_ubyte * max(self.sendlen, self.sendlen_pd))() + self.inbuf = ctypes.cast(ctypes.byref(self.recvbuf, 2), + ctypes.POINTER(ctypes.c_ubyte)) + self.outbuf = ctypes.cast(ctypes.byref(self.sendbuf, 2), + ctypes.POINTER(ctypes.c_ubyte)) + + def send(self, u): + pack_cassie_user_in_t(u, self.outbuf) + send_packet(self.sock, self.sendbuf, self.sendlen, None, 0) + + def send_pd(self, u): + pack_pd_in_t(u, self.outbuf) + send_packet(self.sock, self.sendbuf, self.sendlen_pd, None, 0) + + def recv_wait(self): + nbytes = -1 + while nbytes != self.recvlen: + nbytes = get_newest_packet(self.sock, self.recvbuf, self.recvlen, + None, None) + process_packet_header(self.packet_header_info, + self.recvbuf, self.sendbuf) + cassie_out = cassie_out_t() + unpack_cassie_out_t(self.inbuf, cassie_out) + return cassie_out + + def recv_wait_pd(self): + nbytes = -1 + while nbytes != self.recvlen_pd: + nbytes = get_newest_packet(self.sock, self.recvbuf, self.recvlen_pd, + None, None) + process_packet_header(self.packet_header_info, + self.recvbuf, self.sendbuf) + state_out = state_out_t() + unpack_state_out_t(self.inbuf, state_out) + return state_out + + def recv_newest(self): + nbytes = get_newest_packet(self.sock, self.recvbuf, self.recvlen, + None, None) + if nbytes != self.recvlen: + return None + process_packet_header(self.packet_header_info, + self.recvbuf, self.sendbuf) + cassie_out = cassie_out_t() + unpack_cassie_out_t(self.inbuf, cassie_out) + return cassie_out + + def recv_newest_pd(self): + nbytes = get_newest_packet(self.sock, self.recvbuf, self.recvlen_pd, + None, None) + if nbytes != self.recvlen_pd: + return None + process_packet_header(self.packet_header_info, + self.recvbuf, self.sendbuf) + state_out = state_out_t() + unpack_state_out_t(self.inbuf, state_out) + return state_out + + def delay(self): + return ord(self.packet_header_info.delay) + + def seq_num_in_diff(self): + return ord(self.packet_header_info.seq_num_in_diff) + + def __del__(self): + udp_close(self.sock) diff --git a/bindings/pydairlib/cassie/mujoco/cassiemujoco_ctypes.py b/bindings/pydairlib/cassie/mujoco/cassiemujoco_ctypes.py new file mode 100755 index 0000000000..ecbc48bd50 --- /dev/null +++ b/bindings/pydairlib/cassie/mujoco/cassiemujoco_ctypes.py @@ -0,0 +1,1116 @@ +# -*- coding: utf-8 -*- +# +# TARGET arch is: ['-I/usr/include/clang/6.0/include', '-Iinclude'] +# WORD_SIZE is: 8 +# POINTER_SIZE is: 8 +# LONGDOUBLE_SIZE is: 16 +# +import ctypes +import os +_dir_path = os.path.dirname(os.path.realpath(__file__)) + + +_libraries = {} +_libraries['./libcassiemujoco.so'] = ctypes.CDLL(_dir_path + '/libcassiemujoco.so') +# if local wordsize is same as target, keep ctypes pointer function. +if ctypes.sizeof(ctypes.c_void_p) == 8: + POINTER_T = ctypes.POINTER +else: + # required to access _ctypes + import _ctypes + # Emulate a pointer class using the approriate c_int32/c_int64 type + # The new class should have : + # ['__module__', 'from_param', '_type_', '__dict__', '__weakref__', '__doc__'] + # but the class should be submitted to a unique instance for each base type + # to that if A == B, POINTER_T(A) == POINTER_T(B) + ctypes._pointer_t_type_cache = {} + def POINTER_T(pointee): + # a pointer should have the same length as LONG + fake_ptr_base_type = ctypes.c_uint64 + # specific case for c_void_p + if pointee is None: # VOID pointer type. c_void_p. + pointee = type(None) # ctypes.c_void_p # ctypes.c_ulong + clsname = 'c_void' + else: + clsname = pointee.__name__ + if clsname in ctypes._pointer_t_type_cache: + return ctypes._pointer_t_type_cache[clsname] + # make template + class _T(_ctypes._SimpleCData,): + _type_ = 'L' + _subtype_ = pointee + def _sub_addr_(self): + return self.value + def __repr__(self): + return '%s(%d)'%(clsname, self.value) + def contents(self): + raise TypeError('This is not a ctypes pointer.') + def __init__(self, **args): + raise TypeError('This is not a ctypes pointer. It is not instanciable.') + _class = type('LP_%d_%s'%(8, clsname), (_T,),{}) + ctypes._pointer_t_type_cache[clsname] = _class + return _class + +c_int128 = ctypes.c_ubyte*16 +c_uint128 = c_int128 +void = None +if ctypes.sizeof(ctypes.c_longdouble) == 16: + c_long_double_t = ctypes.c_longdouble +else: + c_long_double_t = ctypes.c_ubyte*16 + + + +size_t = ctypes.c_uint64 +socklen_t = ctypes.c_uint32 +class struct_sockaddr(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('sa_family', ctypes.c_uint16), + ('sa_data', ctypes.c_char * 14), + ] + +ssize_t = ctypes.c_int64 +class struct_CassieCoreSim(ctypes.Structure): + pass + +cassie_core_sim_t = struct_CassieCoreSim +cassie_core_sim_alloc = _libraries['./libcassiemujoco.so'].cassie_core_sim_alloc +cassie_core_sim_alloc.restype = POINTER_T(struct_CassieCoreSim) +cassie_core_sim_alloc.argtypes = [] +cassie_core_sim_copy = _libraries['./libcassiemujoco.so'].cassie_core_sim_copy +cassie_core_sim_copy.restype = None +cassie_core_sim_copy.argtypes = [POINTER_T(struct_CassieCoreSim), POINTER_T(struct_CassieCoreSim)] +cassie_core_sim_free = _libraries['./libcassiemujoco.so'].cassie_core_sim_free +cassie_core_sim_free.restype = None +cassie_core_sim_free.argtypes = [POINTER_T(struct_CassieCoreSim)] +cassie_core_sim_setup = _libraries['./libcassiemujoco.so'].cassie_core_sim_setup +cassie_core_sim_setup.restype = None +cassie_core_sim_setup.argtypes = [POINTER_T(struct_CassieCoreSim)] +class struct_c__SA_cassie_user_in_t(ctypes.Structure): + pass + +class struct_c__SA_cassie_out_t(ctypes.Structure): + pass + +class struct_c__SA_cassie_in_t(ctypes.Structure): + pass + +cassie_core_sim_step = _libraries['./libcassiemujoco.so'].cassie_core_sim_step +cassie_core_sim_step.restype = None +cassie_core_sim_step.argtypes = [POINTER_T(struct_CassieCoreSim), POINTER_T(struct_c__SA_cassie_user_in_t), POINTER_T(struct_c__SA_cassie_out_t), POINTER_T(struct_c__SA_cassie_in_t)] +class struct_c__SA_elmo_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('controlWord', ctypes.c_uint16), + ('PADDING_0', ctypes.c_ubyte * 6), + ('torque', ctypes.c_double), + ] + +elmo_in_t = struct_c__SA_elmo_in_t +class struct_c__SA_cassie_leg_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('hipRollDrive', elmo_in_t), + ('hipYawDrive', elmo_in_t), + ('hipPitchDrive', elmo_in_t), + ('kneeDrive', elmo_in_t), + ('footDrive', elmo_in_t), + ] + +cassie_leg_in_t = struct_c__SA_cassie_leg_in_t +class struct_c__SA_radio_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('channel', ctypes.c_int16 * 14), + ] + +radio_in_t = struct_c__SA_radio_in_t +class struct_c__SA_cassie_pelvis_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('radio', radio_in_t), + ('sto', ctypes.c_bool), + ('piezoState', ctypes.c_bool), + ('piezoTone', ctypes.c_ubyte), + ('PADDING_0', ctypes.c_ubyte), + ] + +cassie_pelvis_in_t = struct_c__SA_cassie_pelvis_in_t +struct_c__SA_cassie_in_t._pack_ = True # source:False +struct_c__SA_cassie_in_t._fields_ = [ + ('pelvis', cassie_pelvis_in_t), + ('leftLeg', cassie_leg_in_t), + ('rightLeg', cassie_leg_in_t), +] + +cassie_in_t = struct_c__SA_cassie_in_t +pack_cassie_in_t = _libraries['./libcassiemujoco.so'].pack_cassie_in_t +pack_cassie_in_t.restype = None +pack_cassie_in_t.argtypes = [POINTER_T(struct_c__SA_cassie_in_t), POINTER_T(ctypes.c_ubyte)] +unpack_cassie_in_t = _libraries['./libcassiemujoco.so'].unpack_cassie_in_t +unpack_cassie_in_t.restype = None +unpack_cassie_in_t.argtypes = [POINTER_T(ctypes.c_ubyte), POINTER_T(struct_c__SA_cassie_in_t)] +DiagnosticCodes = ctypes.c_int16 +class struct_c__SA_battery_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('dataGood', ctypes.c_bool), + ('PADDING_0', ctypes.c_ubyte * 7), + ('stateOfCharge', ctypes.c_double), + ('voltage', ctypes.c_double * 12), + ('current', ctypes.c_double), + ('temperature', ctypes.c_double * 4), + ] + +battery_out_t = struct_c__SA_battery_out_t +class struct_c__SA_cassie_joint_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('position', ctypes.c_double), + ('velocity', ctypes.c_double), + ] + +cassie_joint_out_t = struct_c__SA_cassie_joint_out_t +class struct_c__SA_elmo_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('statusWord', ctypes.c_uint16), + ('PADDING_0', ctypes.c_ubyte * 6), + ('position', ctypes.c_double), + ('velocity', ctypes.c_double), + ('torque', ctypes.c_double), + ('driveTemperature', ctypes.c_double), + ('dcLinkVoltage', ctypes.c_double), + ('torqueLimit', ctypes.c_double), + ('gearRatio', ctypes.c_double), + ] + +elmo_out_t = struct_c__SA_elmo_out_t +class struct_c__SA_cassie_leg_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('hipRollDrive', elmo_out_t), + ('hipYawDrive', elmo_out_t), + ('hipPitchDrive', elmo_out_t), + ('kneeDrive', elmo_out_t), + ('footDrive', elmo_out_t), + ('shinJoint', cassie_joint_out_t), + ('tarsusJoint', cassie_joint_out_t), + ('footJoint', cassie_joint_out_t), + ('medullaCounter', ctypes.c_ubyte), + ('PADDING_0', ctypes.c_ubyte), + ('medullaCpuLoad', ctypes.c_uint16), + ('reedSwitchState', ctypes.c_bool), + ('PADDING_1', ctypes.c_ubyte * 3), + ] + +cassie_leg_out_t = struct_c__SA_cassie_leg_out_t +class struct_c__SA_radio_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('radioReceiverSignalGood', ctypes.c_bool), + ('receiverMedullaSignalGood', ctypes.c_bool), + ('PADDING_0', ctypes.c_ubyte * 6), + ('channel', ctypes.c_double * 16), + ] + +radio_out_t = struct_c__SA_radio_out_t +class struct_c__SA_target_pc_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('etherCatStatus', ctypes.c_int32 * 6), + ('etherCatNotifications', ctypes.c_int32 * 21), + ('PADDING_0', ctypes.c_ubyte * 4), + ('taskExecutionTime', ctypes.c_double), + ('overloadCounter', ctypes.c_uint32), + ('PADDING_1', ctypes.c_ubyte * 4), + ('cpuTemperature', ctypes.c_double), + ] + +target_pc_out_t = struct_c__SA_target_pc_out_t +class struct_c__SA_vectornav_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('dataGood', ctypes.c_bool), + ('PADDING_0', ctypes.c_ubyte), + ('vpeStatus', ctypes.c_uint16), + ('PADDING_1', ctypes.c_ubyte * 4), + ('pressure', ctypes.c_double), + ('temperature', ctypes.c_double), + ('magneticField', ctypes.c_double * 3), + ('angularVelocity', ctypes.c_double * 3), + ('linearAcceleration', ctypes.c_double * 3), + ('orientation', ctypes.c_double * 4), + ] + +vectornav_out_t = struct_c__SA_vectornav_out_t +class struct_c__SA_cassie_pelvis_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('targetPc', target_pc_out_t), + ('battery', battery_out_t), + ('radio', radio_out_t), + ('vectorNav', vectornav_out_t), + ('medullaCounter', ctypes.c_ubyte), + ('PADDING_0', ctypes.c_ubyte), + ('medullaCpuLoad', ctypes.c_uint16), + ('bleederState', ctypes.c_bool), + ('leftReedSwitchState', ctypes.c_bool), + ('rightReedSwitchState', ctypes.c_bool), + ('PADDING_1', ctypes.c_ubyte), + ('vtmTemperature', ctypes.c_double), + ] + +cassie_pelvis_out_t = struct_c__SA_cassie_pelvis_out_t +struct_c__SA_cassie_out_t._pack_ = True # source:False +struct_c__SA_cassie_out_t._fields_ = [ + ('pelvis', cassie_pelvis_out_t), + ('leftLeg', cassie_leg_out_t), + ('rightLeg', cassie_leg_out_t), + ('isCalibrated', ctypes.c_bool), + ('PADDING_0', ctypes.c_ubyte), + ('messages', ctypes.c_int16 * 4), + ('PADDING_1', ctypes.c_ubyte * 6), +] + +cassie_out_t = struct_c__SA_cassie_out_t +pack_cassie_out_t = _libraries['./libcassiemujoco.so'].pack_cassie_out_t +pack_cassie_out_t.restype = None +pack_cassie_out_t.argtypes = [POINTER_T(struct_c__SA_cassie_out_t), POINTER_T(ctypes.c_ubyte)] +unpack_cassie_out_t = _libraries['./libcassiemujoco.so'].unpack_cassie_out_t +unpack_cassie_out_t.restype = None +unpack_cassie_out_t.argtypes = [POINTER_T(ctypes.c_ubyte), POINTER_T(struct_c__SA_cassie_out_t)] +struct_c__SA_cassie_user_in_t._pack_ = True # source:False +struct_c__SA_cassie_user_in_t._fields_ = [ + ('torque', ctypes.c_double * 10), + ('telemetry', ctypes.c_int16 * 9), + ('PADDING_0', ctypes.c_ubyte * 6), +] + +cassie_user_in_t = struct_c__SA_cassie_user_in_t +pack_cassie_user_in_t = _libraries['./libcassiemujoco.so'].pack_cassie_user_in_t +pack_cassie_user_in_t.restype = None +pack_cassie_user_in_t.argtypes = [POINTER_T(struct_c__SA_cassie_user_in_t), POINTER_T(ctypes.c_ubyte)] +unpack_cassie_user_in_t = _libraries['./libcassiemujoco.so'].unpack_cassie_user_in_t +unpack_cassie_user_in_t.restype = None +unpack_cassie_user_in_t.argtypes = [POINTER_T(ctypes.c_ubyte), POINTER_T(struct_c__SA_cassie_user_in_t)] +class struct_cassie_sim(ctypes.Structure): + pass + +cassie_sim_t = struct_cassie_sim +class struct_cassie_vis(ctypes.Structure): + pass + +cassie_vis_t = struct_cassie_vis +class struct_cassie_state(ctypes.Structure): + pass + +cassie_state_t = struct_cassie_state +cassie_mujoco_init = _libraries['./libcassiemujoco.so'].cassie_mujoco_init +cassie_mujoco_init.restype = ctypes.c_bool +cassie_mujoco_init.argtypes = [POINTER_T(ctypes.c_char)] +cassie_cleanup = _libraries['./libcassiemujoco.so'].cassie_cleanup +cassie_cleanup.restype = None +cassie_cleanup.argtypes = [] +cassie_reload_xml = _libraries['./libcassiemujoco.so'].cassie_reload_xml +cassie_reload_xml.restype = ctypes.c_bool +cassie_reload_xml.argtypes = [ctypes.c_char_p] +cassie_sim_init = _libraries['./libcassiemujoco.so'].cassie_sim_init +cassie_sim_init.restype = POINTER_T(struct_cassie_sim) +cassie_sim_init.argtypes = [ctypes.c_char_p, ctypes.c_bool] +cassie_sim_duplicate = _libraries['./libcassiemujoco.so'].cassie_sim_duplicate +cassie_sim_duplicate.restype = POINTER_T(struct_cassie_sim) +cassie_sim_duplicate.argtypes = [POINTER_T(struct_cassie_sim)] +cassie_sim_copy = _libraries['./libcassiemujoco.so'].cassie_sim_copy +cassie_sim_copy.restype = None +cassie_sim_copy.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_cassie_sim)] +cassie_sim_free = _libraries['./libcassiemujoco.so'].cassie_sim_free +cassie_sim_free.restype = None +cassie_sim_free.argtypes = [POINTER_T(struct_cassie_sim)] +cassie_sim_step_ethercat = _libraries['./libcassiemujoco.so'].cassie_sim_step_ethercat +cassie_sim_step_ethercat.restype = None +cassie_sim_step_ethercat.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_c__SA_cassie_out_t), POINTER_T(struct_c__SA_cassie_in_t)] +cassie_sim_step = _libraries['./libcassiemujoco.so'].cassie_sim_step +cassie_sim_step.restype = None +cassie_sim_step.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_c__SA_cassie_out_t), POINTER_T(struct_c__SA_cassie_user_in_t)] +class struct_c__SA_state_out_t(ctypes.Structure): + pass + +class struct_c__SA_pd_in_t(ctypes.Structure): + pass + +class struct_c__SA_joint_filter_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('x', ctypes.c_double * 4), + ('y', ctypes.c_double * 3), + ] +joint_filter_t = struct_c__SA_joint_filter_t + +class struct_c__SA_drive_filter_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('x', ctypes.c_double * 9), + ] +drive_filter_t = struct_c__SA_drive_filter_t + +cassie_sim_step_pd = _libraries['./libcassiemujoco.so'].cassie_sim_step_pd +cassie_sim_step_pd.restype = None +cassie_sim_step_pd.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_c__SA_state_out_t), POINTER_T(struct_c__SA_pd_in_t)] + +cassie_integrate_pos = _libraries['./libcassiemujoco.so'].cassie_integrate_pos +cassie_integrate_pos.restype = None +cassie_integrate_pos.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_c__SA_state_out_t)] + +cassie_sim_time = _libraries['./libcassiemujoco.so'].cassie_sim_time +cassie_sim_time.restype = POINTER_T(ctypes.c_double) +cassie_sim_time.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_qpos = _libraries['./libcassiemujoco.so'].cassie_sim_qpos +cassie_sim_qpos.restype = POINTER_T(ctypes.c_double) +cassie_sim_qpos.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_qvel = _libraries['./libcassiemujoco.so'].cassie_sim_qvel +cassie_sim_qvel.restype = POINTER_T(ctypes.c_double) +cassie_sim_qvel.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_qacc = _libraries['./libcassiemujoco.so'].cassie_sim_qacc +cassie_sim_qacc.restype = POINTER_T(ctypes.c_double) +cassie_sim_qacc.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_mjmodel = _libraries['./libcassiemujoco.so'].cassie_sim_mjmodel +cassie_sim_mjmodel.restype = POINTER_T(None) +cassie_sim_mjmodel.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_mjdata = _libraries['./libcassiemujoco.so'].cassie_sim_mjdata +cassie_sim_mjdata.restype = POINTER_T(None) +cassie_sim_mjdata.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_check_obstacle_collision = _libraries['./libcassiemujoco.so'].cassie_sim_check_obstacle_collision +cassie_sim_check_obstacle_collision.restype = ctypes.c_bool +cassie_sim_check_obstacle_collision.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_check_self_collision = _libraries['./libcassiemujoco.so'].cassie_sim_check_self_collision +cassie_sim_check_self_collision.restype = ctypes.c_bool +cassie_sim_check_self_collision.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_foot_forces = _libraries['./libcassiemujoco.so'].cassie_sim_foot_forces +cassie_sim_foot_forces.restype = None +cassie_sim_foot_forces.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 12] + +cassie_sim_heeltoe_forces = _libraries['./libcassiemujoco.so'].cassie_sim_heeltoe_forces +cassie_sim_heeltoe_forces.restype = None +cassie_sim_heeltoe_forces.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 6, ctypes.c_double * 6] + +cassie_sim_foot_positions = _libraries['./libcassiemujoco.so'].cassie_sim_foot_positions +cassie_sim_foot_positions.restype = None +cassie_sim_foot_positions.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 6] + +cassie_sim_foot_velocities = _libraries['./libcassiemujoco.so'].cassie_sim_foot_velocities +cassie_sim_foot_velocities.restype = None +cassie_sim_foot_velocities.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 12] + +cassie_sim_cm_position = _libraries['./libcassiemujoco.so'].cassie_sim_cm_position +cassie_sim_cm_position.restype = None +cassie_sim_cm_position.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 3] + +cassie_sim_cm_velocity = _libraries['./libcassiemujoco.so'].cassie_sim_cm_velocity +cassie_sim_cm_velocity.restype = None +cassie_sim_cm_velocity.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 3] + +cassie_sim_centroid_inertia = _libraries['./libcassiemujoco.so'].cassie_sim_centroid_inertia +cassie_sim_centroid_inertia.restype = None +cassie_sim_centroid_inertia.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 9] + +cassie_sim_angular_momentum = _libraries['./libcassiemujoco.so'].cassie_sim_angular_momentum +cassie_sim_angular_momentum.restype = None +cassie_sim_angular_momentum.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 3] + +cassie_sim_full_mass_matrix = _libraries['./libcassiemujoco.so'].cassie_sim_full_mass_matrix +cassie_sim_full_mass_matrix.restype = None +cassie_sim_full_mass_matrix.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * (32*32)] + +cassie_sim_minimal_mass_matrix = _libraries['./libcassiemujoco.so'].cassie_sim_minimal_mass_matrix +cassie_sim_minimal_mass_matrix.restype = None +cassie_sim_minimal_mass_matrix.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * (16*16)] + +cassie_sim_loop_constraint_info = _libraries['./libcassiemujoco.so'].cassie_sim_loop_constraint_info +cassie_sim_loop_constraint_info.restype = None +cassie_sim_loop_constraint_info.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * (6*32), ctypes.c_double * (6)] + +cassie_sim_foot_quat = _libraries['./libcassiemujoco.so'].cassie_sim_foot_orient +cassie_sim_foot_quat.restype = None +cassie_sim_foot_quat.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 4] + +cassie_sim_body_vel = _libraries['./libcassiemujoco.so'].cassie_sim_body_velocities +cassie_sim_body_vel.restype = None +cassie_sim_body_vel.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 6, ctypes.c_char_p] + +cassie_sim_apply_force = _libraries['./libcassiemujoco.so'].cassie_sim_apply_force +cassie_sim_apply_force.restype = None +cassie_sim_apply_force.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 6, ctypes.c_char_p] + +cassie_sim_xpos = _libraries['./libcassiemujoco.so'].cassie_sim_xpos +cassie_sim_xpos.restype = POINTER_T(ctypes.c_double) +cassie_sim_xpos.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_xquat = _libraries['./libcassiemujoco.so'].cassie_sim_xquat +cassie_sim_xquat.restype = POINTER_T(ctypes.c_double) +cassie_sim_xquat.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_clear_forces = _libraries['./libcassiemujoco.so'].cassie_sim_clear_forces +cassie_sim_clear_forces.restype = None +cassie_sim_clear_forces.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_hold = _libraries['./libcassiemujoco.so'].cassie_sim_hold +cassie_sim_hold.restype = None +cassie_sim_hold.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_release = _libraries['./libcassiemujoco.so'].cassie_sim_release +cassie_sim_release.restype = None +cassie_sim_release.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_radio = _libraries['./libcassiemujoco.so'].cassie_sim_radio +cassie_sim_radio.restype = None +cassie_sim_radio.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 16] + +cassie_sim_full_reset = _libraries['./libcassiemujoco.so'].cassie_sim_full_reset +cassie_sim_full_reset.restype = None +cassie_sim_full_reset.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_hfield_nrow = _libraries['./libcassiemujoco.so'].cassie_sim_get_hfield_nrow +cassie_sim_get_hfield_nrow.restype = ctypes.c_int32 +cassie_sim_get_hfield_nrow.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_hfield_ncol = _libraries['./libcassiemujoco.so'].cassie_sim_get_hfield_ncol +cassie_sim_get_hfield_ncol.restype = ctypes.c_int32 +cassie_sim_get_hfield_ncol.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_nhfielddata = _libraries['./libcassiemujoco.so'].cassie_sim_get_nhfielddata +cassie_sim_get_nhfielddata.restype = ctypes.c_int32 +cassie_sim_get_nhfielddata.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_hfield_size = _libraries['./libcassiemujoco.so'].cassie_sim_get_hfield_size +cassie_sim_get_hfield_size.restype = POINTER_T(ctypes.c_double) +cassie_sim_get_hfield_size.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_set_hfield_size = _libraries['./libcassiemujoco.so'].cassie_sim_set_hfield_size +cassie_sim_set_hfield_size.restype = None +cassie_sim_set_hfield_size.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 4] + +cassie_sim_hfielddata = _libraries['./libcassiemujoco.so'].cassie_sim_hfielddata +cassie_sim_hfielddata.restype = POINTER_T(ctypes.c_float) +cassie_sim_hfielddata.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_set_hfielddata = _libraries['./libcassiemujoco.so'].cassie_sim_set_hfielddata +cassie_sim_set_hfielddata.restype = None +cassie_sim_set_hfielddata.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_float)] + +cassie_vis_init = _libraries['./libcassiemujoco.so'].cassie_vis_init +cassie_vis_init.restype = POINTER_T(struct_cassie_vis) +cassie_vis_init.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p,ctypes.c_bool] + +cassie_vis_close = _libraries['./libcassiemujoco.so'].cassie_vis_close +cassie_vis_close.restype = None +cassie_vis_close.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_free = _libraries['./libcassiemujoco.so'].cassie_vis_free +cassie_vis_free.restype = None +cassie_vis_free.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_draw = _libraries['./libcassiemujoco.so'].cassie_vis_draw +cassie_vis_draw.restype = ctypes.c_bool +cassie_vis_draw.argtypes = [POINTER_T(struct_cassie_vis), POINTER_T(struct_cassie_sim)] + +cassie_vis_set_cam = _libraries['./libcassiemujoco.so'].cassie_vis_set_cam +cassie_vis_set_cam.restype = None +cassie_vis_set_cam.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_char_p, ctypes.c_double, ctypes.c_double, ctypes.c_double] + +cassie_vis_window_resize = _libraries['./libcassiemujoco.so'].cassie_vis_window_resize +cassie_vis_window_resize.restype = None +cassie_vis_window_resize.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int32, ctypes.c_int32] + +cassie_vis_attach_cam = _libraries['./libcassiemujoco.so'].cassie_vis_attach_cam +cassie_vis_attach_cam.restype = None +cassie_vis_attach_cam.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_char_p] + +cassie_vis_draw_depth = _libraries['./libcassiemujoco.so'].cassie_vis_draw_depth +cassie_vis_draw_depth.restype = POINTER_T(ctypes.c_float) +cassie_vis_draw_depth.argtypes = [POINTER_T(struct_cassie_vis), POINTER_T(struct_cassie_sim), ctypes.c_int32, ctypes.c_int32] + +cassie_vis_get_depth_size = _libraries['./libcassiemujoco.so'].cassie_vis_get_depth_size +cassie_vis_get_depth_size.restype = ctypes.c_int32 +cassie_vis_get_depth_size.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int32, ctypes.c_int32] + +cassie_vis_init_depth = _libraries['./libcassiemujoco.so'].cassie_vis_init_depth +cassie_vis_init_depth.restype = None +cassie_vis_init_depth.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int, ctypes.c_int] + +cassie_vis_valid = _libraries['./libcassiemujoco.so'].cassie_vis_valid +cassie_vis_valid.restype = ctypes.c_bool +cassie_vis_valid.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_paused = _libraries['./libcassiemujoco.so'].cassie_vis_paused +cassie_vis_paused.restype = ctypes.c_bool +cassie_vis_paused.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_apply_force = _libraries['./libcassiemujoco.so'].cassie_vis_apply_force +cassie_vis_apply_force.restype = None +cassie_vis_apply_force.argtypes = [POINTER_T(struct_cassie_vis), POINTER_T(ctypes.c_double), ctypes.c_char_p] + +cassie_vis_add_marker = _libraries['./libcassiemujoco.so'].cassie_vis_add_marker +cassie_vis_add_marker.restype = None +cassie_vis_add_marker.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_double * 3, ctypes.c_double * 3, ctypes.c_double * 4, ctypes.c_double * 9] + +cassie_vis_remove_marker = _libraries['./libcassiemujoco.so'].cassie_vis_remove_marker +cassie_vis_remove_marker.restype = None +cassie_vis_remove_marker.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int] + +cassie_vis_clear_markers = _libraries['./libcassiemujoco.so'].cassie_vis_clear_markers +cassie_vis_clear_markers.restype = None +cassie_vis_clear_markers.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_update_marker_pos = _libraries['./libcassiemujoco.so'].cassie_vis_update_marker_pos +cassie_vis_update_marker_pos.restype = None +cassie_vis_update_marker_pos.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int, ctypes.c_double * 3] + +cassie_vis_update_marker_size = _libraries['./libcassiemujoco.so'].cassie_vis_update_marker_size +cassie_vis_update_marker_size.restype = None +cassie_vis_update_marker_size.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int, ctypes.c_double * 3] + +cassie_vis_update_marker_rgba = _libraries['./libcassiemujoco.so'].cassie_vis_update_marker_rgba +cassie_vis_update_marker_rgba.restype = None +cassie_vis_update_marker_rgba.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int, ctypes.c_double * 4] + +cassie_vis_update_marker_orient = _libraries['./libcassiemujoco.so'].cassie_vis_update_marker_orient +cassie_vis_update_marker_orient.restype = None +cassie_vis_update_marker_orient.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_int, ctypes.c_double * 9] + +cassie_vis_full_reset = _libraries['./libcassiemujoco.so'].cassie_vis_full_reset +cassie_vis_full_reset.restype = None +cassie_vis_full_reset.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_remakeSceneCon = _libraries['./libcassiemujoco.so'].cassie_vis_remakeSceneCon +cassie_vis_remakeSceneCon.restype = None +cassie_vis_remakeSceneCon.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_init_recording = _libraries['./libcassiemujoco.so'].cassie_vis_init_recording +cassie_vis_init_recording.restype = None +cassie_vis_init_recording.argtypes = [POINTER_T(struct_cassie_vis), ctypes.c_char_p, ctypes.c_int32, ctypes.c_int32] + +cassie_vis_record_frame = _libraries['./libcassiemujoco.so'].cassie_vis_record_frame +cassie_vis_record_frame.restype = None +cassie_vis_record_frame.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_vis_close_recording = _libraries['./libcassiemujoco.so'].cassie_vis_close_recording +cassie_vis_close_recording.restype = None +cassie_vis_close_recording.argtypes = [POINTER_T(struct_cassie_vis)] + +cassie_state_alloc = _libraries['./libcassiemujoco.so'].cassie_state_alloc +cassie_state_alloc.restype = POINTER_T(struct_cassie_state) +cassie_state_alloc.argtypes = [] + +cassie_state_duplicate = _libraries['./libcassiemujoco.so'].cassie_state_duplicate +cassie_state_duplicate.restype = POINTER_T(struct_cassie_state) +cassie_state_duplicate.argtypes = [POINTER_T(struct_cassie_state)] + +cassie_state_copy = _libraries['./libcassiemujoco.so'].cassie_state_copy +cassie_state_copy.restype = None +cassie_state_copy.argtypes = [POINTER_T(struct_cassie_state), POINTER_T(struct_cassie_state)] + +cassie_state_free = _libraries['./libcassiemujoco.so'].cassie_state_free +cassie_state_free.restype = None +cassie_state_free.argtypes = [POINTER_T(struct_cassie_state)] + +cassie_state_time = _libraries['./libcassiemujoco.so'].cassie_state_time +cassie_state_time.restype = POINTER_T(ctypes.c_double) +cassie_state_time.argtypes = [POINTER_T(struct_cassie_state)] + +cassie_state_qpos = _libraries['./libcassiemujoco.so'].cassie_state_qpos +cassie_state_qpos.restype = POINTER_T(ctypes.c_double) +cassie_state_qpos.argtypes = [POINTER_T(struct_cassie_state)] + +cassie_state_qvel = _libraries['./libcassiemujoco.so'].cassie_state_qvel +cassie_state_qvel.restype = POINTER_T(ctypes.c_double) +cassie_state_qvel.argtypes = [POINTER_T(struct_cassie_state)] + +cassie_get_state = _libraries['./libcassiemujoco.so'].cassie_get_state +cassie_get_state.restype = None +cassie_get_state.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_cassie_state)] + +cassie_set_state = _libraries['./libcassiemujoco.so'].cassie_set_state +cassie_set_state.restype = None +cassie_set_state.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(struct_cassie_state)] + +cassie_sim_read_rangefinder = _libraries['./libcassiemujoco.so'].cassie_sim_read_rangefinder +cassie_sim_read_rangefinder.restype = None +cassie_sim_read_rangefinder.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 6] + +#cassie_sim_foot_positions.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_double * 6] + +cassie_sim_dof_damping = _libraries['./libcassiemujoco.so'].cassie_sim_dof_damping +cassie_sim_dof_damping.restype = POINTER_T(ctypes.c_double) +cassie_sim_dof_damping.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_set_dof_damping = _libraries['./libcassiemujoco.so'].cassie_sim_set_dof_damping +cassie_sim_set_dof_damping.restype = None +cassie_sim_set_dof_damping.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_body_mass = _libraries['./libcassiemujoco.so'].cassie_sim_body_mass +cassie_sim_body_mass.restype = POINTER_T(ctypes.c_double) +cassie_sim_body_mass.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_set_body_mass = _libraries['./libcassiemujoco.so'].cassie_sim_set_body_mass +cassie_sim_set_body_mass.restype = None +cassie_sim_set_body_mass.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_body_name_mass = _libraries['./libcassiemujoco.so'].cassie_sim_set_body_name_mass +cassie_sim_set_body_name_mass.restype = None +cassie_sim_set_body_name_mass.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, ctypes.c_double] + +cassie_sim_body_ipos = _libraries['./libcassiemujoco.so'].cassie_sim_body_ipos +cassie_sim_body_ipos.restype = POINTER_T(ctypes.c_double) +cassie_sim_body_ipos.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_body_name_pos = _libraries['./libcassiemujoco.so'].cassie_sim_get_body_name_pos +cassie_sim_get_body_name_pos.restype = POINTER_T(ctypes.c_double) +cassie_sim_get_body_name_pos.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_set_body_name_pos = _libraries['./libcassiemujoco.so'].cassie_sim_set_body_name_pos +cassie_sim_set_body_name_pos.restype = None +cassie_sim_set_body_name_pos.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, POINTER_T(ctypes.c_double)] + +cassie_sim_set_body_ipos = _libraries['./libcassiemujoco.so'].cassie_sim_set_body_ipos +cassie_sim_set_body_ipos.restype = None +cassie_sim_set_body_ipos.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_geom_friction = _libraries['./libcassiemujoco.so'].cassie_sim_geom_friction +cassie_sim_geom_friction.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_friction.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_set_geom_friction = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_friction +cassie_sim_set_geom_friction.restype = None +cassie_sim_set_geom_friction.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_geom_name_friction = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_name_friction +cassie_sim_set_geom_name_friction.restype = None +cassie_sim_set_geom_name_friction.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, POINTER_T(ctypes.c_double)] + +cassie_sim_geom_rgba = _libraries['./libcassiemujoco.so'].cassie_sim_geom_rgba +cassie_sim_geom_rgba.restype = POINTER_T(ctypes.c_float) +cassie_sim_geom_rgba.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_geom_name_rgba = _libraries['./libcassiemujoco.so'].cassie_sim_geom_name_rgba +cassie_sim_geom_name_rgba.restype = POINTER_T(ctypes.c_float) +cassie_sim_geom_name_rgba.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_set_geom_rgba = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_rgba +cassie_sim_set_geom_rgba.restype = None +cassie_sim_set_geom_rgba.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_float)] + +cassie_sim_set_geom_name_rgba = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_name_rgba +cassie_sim_set_geom_name_rgba.restype = None +cassie_sim_set_geom_name_rgba.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, POINTER_T(ctypes.c_float)] + +cassie_sim_geom_quat = _libraries['./libcassiemujoco.so'].cassie_sim_geom_quat +cassie_sim_geom_quat.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_quat.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_geom_name_quat = _libraries['./libcassiemujoco.so'].cassie_sim_geom_name_quat +cassie_sim_geom_name_quat.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_name_quat.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_set_geom_quat = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_quat +cassie_sim_set_geom_quat.restype = None +cassie_sim_set_geom_quat.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_geom_name_quat = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_name_quat +cassie_sim_set_geom_name_quat.restype = None +cassie_sim_set_geom_name_quat.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, POINTER_T(ctypes.c_double)] + +cassie_sim_geom_pos = _libraries['./libcassiemujoco.so'].cassie_sim_geom_pos +cassie_sim_geom_pos.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_pos.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_geom_name_pos = _libraries['./libcassiemujoco.so'].cassie_sim_geom_name_pos +cassie_sim_geom_name_pos.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_name_pos.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_set_geom_pos = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_pos +cassie_sim_set_geom_pos.restype = None +cassie_sim_set_geom_pos.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_geom_name_pos = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_name_pos +cassie_sim_set_geom_name_pos.restype = None +cassie_sim_set_geom_name_pos.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, POINTER_T(ctypes.c_double)] + +cassie_sim_geom_size = _libraries['./libcassiemujoco.so'].cassie_sim_geom_size +cassie_sim_geom_size.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_size.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_geom_name_size = _libraries['./libcassiemujoco.so'].cassie_sim_geom_name_size +cassie_sim_geom_name_size.restype = POINTER_T(ctypes.c_double) +cassie_sim_geom_name_size.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p] + +cassie_sim_set_geom_size = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_size +cassie_sim_set_geom_size.restype = None +cassie_sim_set_geom_size.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_geom_name_size = _libraries['./libcassiemujoco.so'].cassie_sim_set_geom_name_size +cassie_sim_set_geom_name_size.restype = None +cassie_sim_set_geom_name_size.argtypes = [POINTER_T(struct_cassie_sim), ctypes.c_char_p, POINTER_T(ctypes.c_double)] + +cassie_sim_set_const = _libraries['./libcassiemujoco.so'].cassie_sim_set_const +cassie_sim_set_const.restype = None +cassie_sim_set_const.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_params = _libraries['./libcassiemujoco.so'].cassie_sim_params +cassie_sim_params.restype = None +cassie_sim_params.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_int32)] + +cassie_sim_joint_filter = _libraries['./libcassiemujoco.so'].cassie_sim_joint_filter +cassie_sim_joint_filter.restype = POINTER_T(joint_filter_t) +cassie_sim_joint_filter.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_joint_filter = _libraries['./libcassiemujoco.so'].cassie_sim_get_joint_filter +cassie_sim_get_joint_filter.restype = None +cassie_sim_get_joint_filter.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_joint_filter = _libraries['./libcassiemujoco.so'].cassie_sim_set_joint_filter +cassie_sim_set_joint_filter.restype = None +cassie_sim_set_joint_filter.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double), POINTER_T(ctypes.c_double)] + +cassie_sim_drive_filter = _libraries['./libcassiemujoco.so'].cassie_sim_drive_filter +cassie_sim_drive_filter.restype = POINTER_T(drive_filter_t) +cassie_sim_drive_filter.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_drive_filter = _libraries['./libcassiemujoco.so'].cassie_sim_drive_filter +cassie_sim_get_drive_filter.restype = None +cassie_sim_get_drive_filter.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_drive_filter = _libraries['./libcassiemujoco.so'].cassie_sim_set_drive_filter +cassie_sim_set_drive_filter.restype = None +cassie_sim_set_drive_filter.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_torque_delay = _libraries['./libcassiemujoco.so'].cassie_sim_torque_delay +cassie_sim_torque_delay.restype = None +cassie_sim_torque_delay.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] + +cassie_sim_set_torque_delay = _libraries['./libcassiemujoco.so'].cassie_sim_set_torque_delay +cassie_sim_set_torque_delay.restype = None +cassie_sim_set_torque_delay.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double)] +cassie_sim_nv = _libraries['./libcassiemujoco.so'].cassie_sim_nv +cassie_sim_nv.restype = ctypes.c_int32 +cassie_sim_nv.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_nbody = _libraries['./libcassiemujoco.so'].cassie_sim_nbody +cassie_sim_nbody.restype = ctypes.c_int32 +cassie_sim_nbody.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_ngeom = _libraries['./libcassiemujoco.so'].cassie_sim_ngeom +cassie_sim_ngeom.restype = ctypes.c_int32 +cassie_sim_ngeom.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_nq = _libraries['./libcassiemujoco.so'].cassie_sim_nq +cassie_sim_nq.restype = ctypes.c_int32 +cassie_sim_nq.argtypes = [POINTER_T(struct_cassie_sim)] + +cassie_sim_get_jacobian = _libraries['./libcassiemujoco.so'].cassie_sim_get_jacobian +cassie_sim_get_jacobian.restype = None +cassie_sim_get_jacobian.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double), ctypes.c_char_p] + +cassie_sim_get_jacobian_full = _libraries['./libcassiemujoco.so'].cassie_sim_get_jacobian_full +cassie_sim_get_jacobian_full.restype = None +cassie_sim_get_jacobian_full.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double), POINTER_T(ctypes.c_double), ctypes.c_char_p] + +cassie_sim_get_jacobian_full_site = _libraries['./libcassiemujoco.so'].cassie_sim_get_jacobian_full_site +cassie_sim_get_jacobian_full_site.restype = None +cassie_sim_get_jacobian_full_site.argtypes = [POINTER_T(struct_cassie_sim), POINTER_T(ctypes.c_double), POINTER_T(ctypes.c_double), ctypes.c_char_p] + +class struct_c__SA_pd_motor_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('torque', ctypes.c_double * 5), + ('pTarget', ctypes.c_double * 5), + ('dTarget', ctypes.c_double * 5), + ('pGain', ctypes.c_double * 5), + ('dGain', ctypes.c_double * 5), + ] + +pd_motor_in_t = struct_c__SA_pd_motor_in_t +class struct_c__SA_pd_task_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('torque', ctypes.c_double * 6), + ('pTarget', ctypes.c_double * 6), + ('dTarget', ctypes.c_double * 6), + ('pGain', ctypes.c_double * 6), + ('dGain', ctypes.c_double * 6), + ] + +pd_task_in_t = struct_c__SA_pd_task_in_t +class struct_c__SA_pd_leg_in_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('taskPd', pd_task_in_t), + ('motorPd', pd_motor_in_t), + ] + +pd_leg_in_t = struct_c__SA_pd_leg_in_t +struct_c__SA_pd_in_t._pack_ = True # source:False +struct_c__SA_pd_in_t._fields_ = [ + ('leftLeg', pd_leg_in_t), + ('rightLeg', pd_leg_in_t), + ('telemetry', ctypes.c_double * 9), +] + +pd_in_t = struct_c__SA_pd_in_t +pack_pd_in_t = _libraries['./libcassiemujoco.so'].pack_pd_in_t +pack_pd_in_t.restype = None +pack_pd_in_t.argtypes = [POINTER_T(struct_c__SA_pd_in_t), POINTER_T(ctypes.c_ubyte)] +unpack_pd_in_t = _libraries['./libcassiemujoco.so'].unpack_pd_in_t +unpack_pd_in_t.restype = None +unpack_pd_in_t.argtypes = [POINTER_T(ctypes.c_ubyte), POINTER_T(struct_c__SA_pd_in_t)] +class struct_PdInput(ctypes.Structure): + pass + +pd_input_t = struct_PdInput +pd_input_alloc = _libraries['./libcassiemujoco.so'].pd_input_alloc +pd_input_alloc.restype = POINTER_T(struct_PdInput) +pd_input_alloc.argtypes = [] +pd_input_copy = _libraries['./libcassiemujoco.so'].pd_input_copy +pd_input_copy.restype = None +pd_input_copy.argtypes = [POINTER_T(struct_PdInput), POINTER_T(struct_PdInput)] +pd_input_free = _libraries['./libcassiemujoco.so'].pd_input_free +pd_input_free.restype = None +pd_input_free.argtypes = [POINTER_T(struct_PdInput)] +pd_input_setup = _libraries['./libcassiemujoco.so'].pd_input_setup +pd_input_setup.restype = None +pd_input_setup.argtypes = [POINTER_T(struct_PdInput)] +pd_input_step = _libraries['./libcassiemujoco.so'].pd_input_step +pd_input_step.restype = None +pd_input_step.argtypes = [POINTER_T(struct_PdInput), POINTER_T(struct_c__SA_pd_in_t), POINTER_T(struct_c__SA_cassie_out_t), POINTER_T(struct_c__SA_cassie_user_in_t)] +class struct_c__SA_state_battery_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('stateOfCharge', ctypes.c_double), + ('current', ctypes.c_double), + ] + +state_battery_out_t = struct_c__SA_state_battery_out_t +class struct_c__SA_state_foot_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('position', ctypes.c_double * 3), + ('orientation', ctypes.c_double * 4), + ('footRotationalVelocity', ctypes.c_double * 3), + ('footTranslationalVelocity', ctypes.c_double * 3), + ('toeForce', ctypes.c_double * 3), + ('heelForce', ctypes.c_double * 3), + ] + +state_foot_out_t = struct_c__SA_state_foot_out_t +class struct_c__SA_state_joint_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('position', ctypes.c_double * 6), + ('velocity', ctypes.c_double * 6), + ] + +state_joint_out_t = struct_c__SA_state_joint_out_t +class struct_c__SA_state_motor_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('position', ctypes.c_double * 10), + ('velocity', ctypes.c_double * 10), + ('torque', ctypes.c_double * 10), + ] + +state_motor_out_t = struct_c__SA_state_motor_out_t +class struct_c__SA_state_pelvis_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('position', ctypes.c_double * 3), + ('orientation', ctypes.c_double * 4), + ('rotationalVelocity', ctypes.c_double * 3), + ('translationalVelocity', ctypes.c_double * 3), + ('translationalAcceleration', ctypes.c_double * 3), + ('externalMoment', ctypes.c_double * 3), + ('externalForce', ctypes.c_double * 3), + ] + +state_pelvis_out_t = struct_c__SA_state_pelvis_out_t +class struct_c__SA_state_radio_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('channel', ctypes.c_double * 16), + ('signalGood', ctypes.c_bool), + ('PADDING_0', ctypes.c_ubyte * 7), + ] + +state_radio_out_t = struct_c__SA_state_radio_out_t +class struct_c__SA_state_terrain_out_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('height', ctypes.c_double), + ('slope', ctypes.c_double * 2), + ] + +state_terrain_out_t = struct_c__SA_state_terrain_out_t +struct_c__SA_state_out_t._pack_ = True # source:False +struct_c__SA_state_out_t._fields_ = [ + ('pelvis', state_pelvis_out_t), + ('leftFoot', state_foot_out_t), + ('rightFoot', state_foot_out_t), + ('terrain', state_terrain_out_t), + ('motor', state_motor_out_t), + ('joint', state_joint_out_t), + ('radio', state_radio_out_t), + ('battery', state_battery_out_t), +] + +state_out_t = struct_c__SA_state_out_t +pack_state_out_t = _libraries['./libcassiemujoco.so'].pack_state_out_t +pack_state_out_t.restype = None +pack_state_out_t.argtypes = [POINTER_T(struct_c__SA_state_out_t), POINTER_T(ctypes.c_ubyte)] +unpack_state_out_t = _libraries['./libcassiemujoco.so'].unpack_state_out_t +unpack_state_out_t.restype = None +unpack_state_out_t.argtypes = [POINTER_T(ctypes.c_ubyte), POINTER_T(struct_c__SA_state_out_t)] +class struct_StateOutput(ctypes.Structure): + pass + +state_output_t = struct_StateOutput +state_output_alloc = _libraries['./libcassiemujoco.so'].state_output_alloc +state_output_alloc.restype = POINTER_T(struct_StateOutput) +state_output_alloc.argtypes = [] +state_output_copy = _libraries['./libcassiemujoco.so'].state_output_copy +state_output_copy.restype = None +state_output_copy.argtypes = [POINTER_T(struct_StateOutput), POINTER_T(struct_StateOutput)] +state_output_free = _libraries['./libcassiemujoco.so'].state_output_free +state_output_free.restype = None +state_output_free.argtypes = [POINTER_T(struct_StateOutput)] +state_output_setup = _libraries['./libcassiemujoco.so'].state_output_setup +state_output_setup.restype = None +state_output_setup.argtypes = [POINTER_T(struct_StateOutput)] +state_output_step = _libraries['./libcassiemujoco.so'].state_output_step +state_output_step.restype = None +state_output_step.argtypes = [POINTER_T(struct_StateOutput), POINTER_T(struct_c__SA_cassie_out_t), POINTER_T(struct_c__SA_state_out_t)] +class struct_c__SA_packet_header_info_t(ctypes.Structure): + _pack_ = True # source:False + _fields_ = [ + ('seq_num_out', ctypes.c_char), + ('seq_num_in_last', ctypes.c_char), + ('delay', ctypes.c_char), + ('seq_num_in_diff', ctypes.c_char), + ] + +packet_header_info_t = struct_c__SA_packet_header_info_t +process_packet_header = _libraries['./libcassiemujoco.so'].process_packet_header +process_packet_header.restype = None +process_packet_header.argtypes = [POINTER_T(struct_c__SA_packet_header_info_t), POINTER_T(ctypes.c_ubyte), POINTER_T(ctypes.c_ubyte)] +udp_init_host = _libraries['./libcassiemujoco.so'].udp_init_host +udp_init_host.restype = ctypes.c_int32 +udp_init_host.argtypes = [POINTER_T(ctypes.c_char), POINTER_T(ctypes.c_char)] +udp_init_client = _libraries['./libcassiemujoco.so'].udp_init_client +udp_init_client.restype = ctypes.c_int32 +udp_init_client.argtypes = [POINTER_T(ctypes.c_char), POINTER_T(ctypes.c_char), POINTER_T(ctypes.c_char), POINTER_T(ctypes.c_char)] +udp_close = _libraries['./libcassiemujoco.so'].udp_close +udp_close.restype = None +udp_close.argtypes = [ctypes.c_int32] +get_newest_packet = _libraries['./libcassiemujoco.so'].get_newest_packet +get_newest_packet.restype = ssize_t +get_newest_packet.argtypes = [ctypes.c_int32, POINTER_T(None), size_t, POINTER_T(struct_sockaddr), POINTER_T(ctypes.c_uint32)] +wait_for_packet = _libraries['./libcassiemujoco.so'].wait_for_packet +wait_for_packet.restype = ssize_t +wait_for_packet.argtypes = [ctypes.c_int32, POINTER_T(None), size_t, POINTER_T(struct_sockaddr), POINTER_T(ctypes.c_uint32)] +send_packet = _libraries['./libcassiemujoco.so'].send_packet +send_packet.restype = ssize_t +send_packet.argtypes = [ctypes.c_int32, POINTER_T(None), size_t, POINTER_T(struct_sockaddr), socklen_t] + +__all__ = \ + ['cassie_pelvis_in_t', 'struct_StateOutput', 'cassie_state_t', + 'cassie_sim_check_self_collision', 'cassie_vis_free', + 'cassie_in_t', 'state_terrain_out_t', 'struct_c__SA_pd_leg_in_t', + 'cassie_state_free', 'struct_c__SA_state_battery_out_t', + 'elmo_in_t', 'state_joint_out_t', 'send_packet', + 'cassie_pelvis_out_t', 'cassie_cleanup', + 'struct_c__SA_state_radio_out_t', 'cassie_vis_valid', + 'pd_input_setup', 'pd_leg_in_t', 'cassie_mujoco_init', + 'cassie_state_copy', 'cassie_core_sim_setup', 'battery_out_t', + 'cassie_sim_hold', 'struct_CassieCoreSim', 'cassie_core_sim_step', + 'pack_cassie_out_t', 'cassie_out_t', 'radio_in_t', + 'unpack_cassie_out_t', 'struct_c__SA_pd_task_in_t', + 'struct_PdInput', 'udp_init_client', 'pd_motor_in_t', + 'cassie_sim_t', 'cassie_core_sim_alloc', 'get_newest_packet', + 'size_t', 'struct_c__SA_vectornav_out_t', + 'struct_c__SA_pd_motor_in_t', 'cassie_get_state', + 'state_battery_out_t', 'struct_c__SA_state_pelvis_out_t', + 'cassie_state_qpos', 'cassie_state_qvel', 'state_radio_out_t', + 'struct_c__SA_pd_in_t', 'udp_close', 'state_output_free', + 'cassie_core_sim_free', 'pd_task_in_t', 'packet_header_info_t', + 'pd_in_t', 'struct_cassie_vis', 'struct_c__SA_elmo_out_t', + 'pack_pd_in_t', 'struct_c__SA_radio_out_t', 'pd_input_alloc', + 'DiagnosticCodes', 'unpack_state_out_t', 'target_pc_out_t', + 'cassie_sim_duplicate', 'cassie_state_alloc', 'cassie_sim_init', + 'struct_c__SA_cassie_user_in_t', 'struct_c__SA_radio_in_t', + 'socklen_t', 'cassie_vis_init', 'state_out_t', + 'struct_c__SA_cassie_in_t', 'pd_input_free', 'state_output_alloc', + 'struct_c__SA_cassie_leg_out_t', + 'struct_c__SA_cassie_pelvis_in_t', 'unpack_pd_in_t', + 'cassie_user_in_t', 'cassie_sim_clear_forces', 'cassie_vis_t', + 'struct_c__SA_target_pc_out_t', 'pd_input_step', + 'cassie_set_state', 'struct_c__SA_battery_out_t', + 'vectornav_out_t', 'struct_c__SA_packet_header_info_t', + 'cassie_sim_step_pd', 'struct_sockaddr', 'cassie_vis_draw','cassie_integrate_pos', + 'cassie_core_sim_copy', 'unpack_cassie_in_t', 'struct_cassie_sim', + 'unpack_cassie_user_in_t', 'cassie_sim_step', 'udp_init_host', + 'state_motor_out_t', 'cassie_core_sim_t', 'pack_state_out_t', + 'cassie_sim_mjdata', 'state_output_setup', 'cassie_sim_mjmodel', + 'state_foot_out_t', 'state_output_t', 'cassie_sim_time', + 'cassie_sim_step_ethercat', 'cassie_sim_check_obstacle_collision', + 'elmo_out_t', 'pack_cassie_in_t', 'cassie_sim_apply_force','cassie_sim_full_reset', + 'cassie_leg_out_t', 'wait_for_packet', + 'struct_c__SA_cassie_leg_in_t', 'struct_c__SA_state_joint_out_t', + 'process_packet_header', 'cassie_sim_release', 'cassie_sim_foot_forces', + 'cassie_sim_foot_positions', 'cassie_sim_foot_velocities', 'struct_c__SA_state_foot_out_t', + 'cassie_sim_cm_position','cassie_sim_centroid_inertia', + 'cassie_sim_cm_velocity','cassie_sim_angular_momentum', + 'cassie_sim_full_mass_matrix','cassie_sim_minimal_mass_matrix', + 'pd_input_t', 'pack_cassie_user_in_t', 'cassie_state_duplicate', + 'state_pelvis_out_t', 'struct_c__SA_state_terrain_out_t', + 'cassie_sim_free', 'cassie_sim_xpos', 'cassie_sim_xquat', 'ssize_t', 'state_output_copy', + 'cassie_sim_radio', 'cassie_vis_close', 'cassie_vis_paused', 'radio_out_t', + 'state_output_step', 'struct_c__SA_state_motor_out_t', + 'struct_cassie_state', 'cassie_state_time', 'cassie_sim_qvel', + 'cassie_sim_qpos', 'cassie_sim_qacc', 'struct_c__SA_elmo_in_t', 'cassie_joint_out_t', + 'cassie_leg_in_t', 'struct_c__SA_cassie_joint_out_t', + 'struct_c__SA_state_out_t', 'struct_c__SA_cassie_pelvis_out_t', + 'pd_input_copy', 'cassie_sim_copy', 'struct_c__SA_cassie_out_t', + 'cassie_sim_dof_damping', 'cassie_sim_set_dof_damping', + 'cassie_sim_body_mass', 'cassie_sim_set_body_mass', + 'cassie_sim_loop_constraint_info', + 'cassie_sim_body_ipos', 'cassie_sim_set_body_ipos', + 'cassie_sim_geom_friction', 'cassie_sim_set_geom_friction', + 'cassie_sim_set_const', + 'cassie_sim_geom_rgba', 'cassie_sim_geom_name_rgba', 'cassie_sim_set_geom_rgba', 'cassie_sim_set_geom_name_rgba', + 'cassie_sim_geom_quat', 'cassie_sim_geom_name_quat', 'cassie_sim_set_geom_quat', 'cassie_sim_set_geom_name_quat', + 'cassie_sim_geom_pos', 'cassie_sim_geom_name_pos', 'cassie_sim_set_geom_pos', 'cassie_sim_set_geom_name_pos', + 'cassie_sim_geom_size', 'cassie_sim_geom_name_size', 'cassie_sim_set_geom_size', 'cassie_sim_set_geom_name_size', + 'cassie_sim_set_geom_name_friction', 'cassie_reload_xml', 'cassie_vis_apply_force', + 'cassie_vis_add_marker', 'cassie_vis_remove_marker', 'cassie_vis_clear_markers', + 'cassie_vis_update_marker_pos', 'cassie_vis_update_marker_size', 'cassie_vis_update_marker_rgba', 'cassie_vis_update_marker_orient', + 'cassie_sim_foot_quat', 'cassie_sim_body_vel', 'cassie_sim_set_body_name_mass', + 'cassie_sim_get_hfield_nrow', 'cassie_sim_get_hfield_ncol', 'cassie_sim_get_nhfielddata', + 'cassie_sim_get_hfield_size', 'cassie_sim_set_hfield_size', 'cassie_sim_hfielddata', 'cassie_sim_set_hfielddata', + 'cassie_sim_foot_quat', 'cassie_sim_body_vel', 'cassie_sim_set_body_name_mass', 'cassie_vis_set_cam', + 'cassie_sim_joint_filter', 'cassie_sim_drive_filter', 'cassie_sim_set_joint_filter', 'cassie_sim_set_drive_filter', + 'cassie_sim_get_joint_filter', 'cassie_sim_get_drive_filter', + 'cassie_sim_torque_delay', 'cassie_sim_set_torque_delay', 'drive_filter_t', 'joint_filter_t', + 'cassie_sim_params', 'cassie_sim_nv', 'cassie_sim_nbody', 'cassie_sim_nq', 'cassie_sim_ngeom', + 'cassie_vis_record_frame', 'cassie_vis_init_recording', 'cassie_vis_close_recording', 'cassie_vis_window_resize', 'cassie_vis_attach_cam', + 'cassie_vis_draw_depth', 'cassie_vis_get_depth_size', 'cassie_vis_init_depth', 'cassie_vis_attach_cam', 'cassie_vis_remakeSceneCon', 'cassie_vis_full_reset', + 'cassie_sim_get_jacobian', 'cassie_sim_get_jacobian_full', 'cassie_sim_get_jacobian_full_site', 'cassie_sim_get_body_name_pos', 'cassie_sim_set_body_name_pos', + 'cassie_sim_heeltoe_forces'] + + diff --git a/bindings/pydairlib/cassie/mujoco/drake_to_mujoco_converter.py b/bindings/pydairlib/cassie/mujoco/drake_to_mujoco_converter.py new file mode 100644 index 0000000000..1e16c58b61 --- /dev/null +++ b/bindings/pydairlib/cassie/mujoco/drake_to_mujoco_converter.py @@ -0,0 +1,331 @@ +import numpy as np +from pydrake.multibody.inverse_kinematics import InverseKinematics +from pydrake.multibody.parsing import Parser +from pydrake.systems.framework import DiagramBuilder +from pydrake.multibody.plant import MultibodyPlant, AddMultibodyPlantSceneGraph, CoulombFriction +from pydairlib.common import FindResourceOrThrow +from pydairlib.cassie.cassie_utils import * +from scipy.spatial.transform import Rotation as R +from pydairlib.multibody import * +from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve +from pydrake.geometry import SceneGraph, DrakeVisualizer, HalfSpace, Box +from pydairlib.multibody import MultiposeVisualizer + + +class DrakeToMujocoConverter(): + + def __init__(self, drake_sim_dt=5e-5): + self.left_leg_urdf = FindResourceOrThrow('examples/Cassie/urdf/cassie_left_leg.urdf') + self.cassie_v2_urdf = FindResourceOrThrow('examples/Cassie/urdf/cassie_v2.urdf') + self.foot_crank_urdf = FindResourceOrThrow('examples/Cassie/urdf/cassie_foot_crank.urdf') + self.knee_linkage_urdf = FindResourceOrThrow('examples/Cassie/urdf/cassie_knee_linkage.urdf') + + self.builder = DiagramBuilder() + self.drake_sim_dt = drake_sim_dt + self.plant, self.scene_graph = AddMultibodyPlantSceneGraph(self.builder, drake_sim_dt) + AddCassieMultibody(self.plant, self.scene_graph, True, + 'examples/Cassie/urdf/cassie_v2.urdf', False, False, True) + self.plant.Finalize() + + self.foot_crank_builder = DiagramBuilder() + self.foot_crank_plant, self.foot_crank_scene_graph = AddMultibodyPlantSceneGraph(self.foot_crank_builder, + drake_sim_dt) + + Parser(self.foot_crank_plant).AddModels( + self.foot_crank_urdf) + self.foot_crank_plant.Finalize() + + self.knee_linkage_builder = DiagramBuilder() + self.knee_linkage_plant, self.knee_linkage_scene_graph = AddMultibodyPlantSceneGraph(self.knee_linkage_builder, + drake_sim_dt) + + Parser(self.knee_linkage_plant).AddModels(self.knee_linkage_urdf) + self.knee_linkage_plant.Finalize() + + self.diagram = self.builder.Build() + self.diagram_context = self.diagram.CreateDefaultContext() + + # Print the indices for the newly constructed plants + # self.print_pos_indices(self.knee_linkage_plant) + # self.print_pos_indices(self.plant) + + self.pos_map = MakeNameToPositionsMap(self.plant) + self.vel_map = MakeNameToVelocitiesMap(self.plant) + self.act_map = MakeNameToActuatorsMap(self.plant) + + self.map_q_drake_to_mujoco = np.zeros((23, 35)) + self.map_v_drake_to_mujoco = np.zeros((22, 32)) + + self.generate_matrices() + + self.world = self.plant.world_frame() + self.context = self.plant.CreateDefaultContext() + self.foot_crank_context = self.foot_crank_plant.CreateDefaultContext() + self.ik_solver = InverseKinematics(self.foot_crank_plant, self.foot_crank_context, with_joint_limits=True) + self.left_thigh_rod = LeftRodOnThigh(self.plant) + self.right_thigh_rod = RightRodOnThigh(self.plant) + self.left_heel_rod = LeftRodOnHeel(self.plant) + self.right_heel_rod = RightRodOnHeel(self.plant) + + self.left_achilles_frame = np.zeros((3, 3)) + self.left_achilles_frame[:, 0] = np.array([0.7922, -0.60599, -0.072096]) + self.left_achilles_frame[:, 1] = -np.array([0.60349, 0.79547, -0.054922]) + self.left_achilles_frame[:, 2] = np.cross(self.left_achilles_frame[:, 0], self.left_achilles_frame[:, 1]) + self.right_achilles_frame = np.zeros((3, 3)) + self.right_achilles_frame[:, 0] = np.array([0.7922, -0.60599, 0.072096]) + self.right_achilles_frame[:, 1] = -np.array([0.60349, 0.79547, 0.054922]) + self.right_achilles_frame[:, 2] = np.cross(self.right_achilles_frame[:, 0], self.right_achilles_frame[:, 1]) + + self.plantar_rod_frame = self.foot_crank_plant.GetBodyByName('plantar_rod_left').body_frame() + self.toe_left_frame = self.foot_crank_plant.GetBodyByName('toe_left').body_frame() + self.plantar_rod_anchor_point = np.array([0.35012, 0, 0]) + self.toe_left_anchor_point = np.array([0.04885482, 0.00394248, 0.01484]) + + self.ik_solver.AddPositionConstraint(self.plantar_rod_frame, self.plantar_rod_anchor_point, self.toe_left_frame, + self.toe_left_anchor_point, + self.toe_left_anchor_point) + self.toe_angle_constraint = self.ik_solver.prog().AddLinearEqualityConstraint(self.ik_solver.q()[7], 0) + self.ik_solver.prog().AddLinearEqualityConstraint(self.ik_solver.q()[0:7], np.array([1, 0, 0, 0, 0, 0, 0])) + + def print_pos_indices(self, plant): + name_map = MakeNameToPositionsMap(plant) + for name in name_map: + print(name + ': ' + str(name_map[name])) + + def print_vel_indices(self, plant): + name_map = MakeNameToVelocitiesMap(plant) + for name in name_map: + print(name + ': ' + str(name_map[name])) + + def convert_to_drake(self, q, v): + q_drake = np.zeros(23) + v_drake = np.zeros(22) + q_drake = self.map_q_drake_to_mujoco.T @ q + v_drake = self.map_v_drake_to_mujoco.T @ v + + return q_drake, v_drake + + def convert_to_mujoco(self, x_i): + q = x_i[0:23] + v = x_i[23:45] + q_missing, v_missing = self.solve_IK(x_i) + + q_copy = self.map_q_drake_to_mujoco @ q + v_copy = self.map_v_drake_to_mujoco @ v + q_full = q_missing + q_copy + v_full = v_missing + v_copy + return q_full, v_full + + def visualize_entire_leg(self, x): + self.plant.SetPositionsAndVelocities(self.context, x) + q_missing, _ = self.solve_IK(x) + quat = q_missing[10:14] + + rot = R.from_quat(np.hstack((quat[1:4], quat[0]))) + euler_vec = rot.as_euler('xyz') + + toe_left_ang = x[self.pos_map['toe_left']] + self.toe_angle_constraint.evaluator().UpdateCoefficients(Aeq=[[1]], beq=[toe_left_ang]) + self.ik_solver.prog().SetInitialGuess(self.ik_solver.q(), + np.array( + [1, 0, 0, 0, 0, 0, 0, toe_left_ang, toe_left_ang, -toe_left_ang])) + result = Solve(self.ik_solver.prog()) + left_foot_crank_state = result.GetSolution() + + builder = DiagramBuilder() + + plant = MultibodyPlant(self.drake_sim_dt) + scene_graph = builder.AddSystem(SceneGraph()) + + Parser(plant).AddModels(self.left_leg_urdf) + plant.Finalize() + self.print_pos_indices(plant) + + plant_context = plant.CreateDefaultContext() + print(plant.num_positions()) + fb_state = np.array([1, 0, 0, 0, 0, 0, 0]) + left_leg_state = np.zeros(17) + left_leg_state[0:7] = fb_state + left_leg_state[7:10] = euler_vec + left_leg_state[10] = x[self.pos_map["knee_left"]] + left_leg_state[11] = x[self.pos_map["knee_joint_left"]] + left_leg_state[12] = x[self.pos_map["ankle_joint_left"]] + left_leg_state[13] = x[self.pos_map["ankle_spring_joint_left"]] + left_leg_state[14:17] = left_foot_crank_state[-3:] + plant.SetPositions(plant_context, left_leg_state) + + visualizer = MultiposeVisualizer(self.left_leg_urdf, 1, '') + visualizer.DrawPoses(left_leg_state) + + def visualize_state_lower(self, x): + self.plant.SetPositionsAndVelocities(self.context, x) + toe_left_ang = x[self.pos_map['toe_left']] + self.toe_angle_constraint.evaluator().UpdateCoefficients(Aeq=[[1]], beq=[toe_left_ang]) + self.ik_solver.prog().SetInitialGuess(self.ik_solver.q(), + np.array( + [1, 0, 0, 0, 0, 0, 0, toe_left_ang, toe_left_ang, -toe_left_ang])) + result = Solve(self.ik_solver.prog()) + left_foot_crank_state = result.GetSolution() + visualizer = MultiposeVisualizer('examples/Cassie/urdf/cassie_foot_crank.urdf', 1, '') + visualizer.DrawPoses(left_foot_crank_state) + + def visualize_state_upper(self, x): + q_missing, _ = self.solve_IK(x) + quat = q_missing[10:14] + + rot = R.from_quat(np.hstack((quat[1:4], quat[0]))) + euler_vec = rot.as_euler('xyz') + + fb_state = np.array([1, 0, 0, 0, 0, 0, 0]) + knee_linkage_state = np.zeros(14) + knee_linkage_state[0:7] = fb_state + knee_linkage_state[7:10] = euler_vec + knee_linkage_state[10] = x[self.pos_map["knee_left"]] + knee_linkage_state[11] = x[self.pos_map["knee_joint_left"]] + knee_linkage_state[12] = x[self.pos_map["ankle_joint_left"]] + knee_linkage_state[13] = x[self.pos_map["ankle_spring_joint_left"]] + + visualizer = MultiposeVisualizer('examples/Cassie/urdf/cassie_knee_linkage.urdf', 1, '') + visualizer.DrawPoses(knee_linkage_state) + + def estimate_omega_bar(self, quat1, quat2, dt): + R1 = R.from_quat(np.hstack((quat1[1:4], quat1[0]))) + R2 = R.from_quat(np.hstack((quat2[1:4], quat2[0]))) + R_rel = R1.inv() * R2 + omega = R_rel.as_rotvec() / dt + return R1.apply(omega) + + def solve_for_achilles_rod_quats(self, q): + self.plant.SetPositions(self.context, q) + left_thigh = self.plant.CalcPointsPositions(self.context, self.left_thigh_rod[1], self.left_thigh_rod[0], + self.world) + right_thigh = self.plant.CalcPointsPositions(self.context, self.right_thigh_rod[1], self.right_thigh_rod[0], + self.world) + left_heel = self.plant.CalcPointsPositions(self.context, self.left_heel_rod[1], self.left_heel_rod[0], + self.world) + right_heel = self.plant.CalcPointsPositions(self.context, self.right_heel_rod[1], self.right_heel_rod[0], + self.world) + l_hip_pitch_frame = self.plant.CalcRelativeTransform(self.context, self.world, self.left_thigh_rod[1]) + r_hip_pitch_frame = self.plant.CalcRelativeTransform(self.context, self.world, self.right_thigh_rod[1]) + + world_frame = np.eye(3) + l_x_vec = (left_heel - left_thigh)[:, 0] + l_x_vec *= 1 / np.linalg.norm(l_x_vec) + l_y_vec = np.cross(l_x_vec, -world_frame[1]) + l_y_vec *= 1 / np.linalg.norm(l_y_vec) + l_z_vec = np.cross(l_x_vec, l_y_vec) + l_z_vec *= 1 / np.linalg.norm(l_z_vec) + + r_x_vec = (right_heel - right_thigh)[:, 0] + r_x_vec *= 1 / np.linalg.norm(r_x_vec) + r_y_vec = np.cross(r_x_vec, -world_frame[1]) + r_y_vec *= 1 / np.linalg.norm(r_y_vec) + r_z_vec = np.cross(r_x_vec, r_y_vec) + r_z_vec *= 1 / np.linalg.norm(r_z_vec) + + l_bar_frame = R.from_matrix(np.vstack((l_x_vec, l_y_vec, l_z_vec)).T) + r_bar_frame = R.from_matrix(np.vstack((r_x_vec, r_y_vec, r_z_vec)).T) + l_bar_euler = R.from_matrix( + self.left_achilles_frame @ l_hip_pitch_frame.rotation().matrix().T @ l_bar_frame.as_matrix()).as_euler( + 'xyz') + r_bar_euler = R.from_matrix( + self.right_achilles_frame @ r_hip_pitch_frame.rotation().matrix().T @ r_bar_frame.as_matrix()).as_euler( + 'xyz') + + l_bar_quat = R.from_euler('xyz', -l_bar_euler).as_quat() + r_bar_quat = R.from_euler('xyz', -r_bar_euler).as_quat() + l_bar_quat = np.hstack((l_bar_quat[3], l_bar_quat[0:3])) + r_bar_quat = np.hstack((r_bar_quat[3], r_bar_quat[0:3])) + + return l_bar_quat, r_bar_quat, l_hip_pitch_frame, r_hip_pitch_frame + + def solve_IK(self, x): + toe_ang = x[self.pos_map['toe_left']] + self.toe_angle_constraint.evaluator().UpdateCoefficients(Aeq=[[1]], beq=[toe_ang]) + self.ik_solver.prog().SetInitialGuess(self.ik_solver.q(), + np.array([1, 0, 0, 0, 0, 0, 0, toe_ang, toe_ang, -toe_ang])) + result = Solve(self.ik_solver.prog()) + left_foot_crank_state = result.GetSolution() + toe_ang = x[self.pos_map['toe_right']] + self.toe_angle_constraint.evaluator().UpdateCoefficients(Aeq=[[1]], beq=[toe_ang]) + self.ik_solver.prog().SetInitialGuess(self.ik_solver.q(), + np.array([1, 0, 0, 0, 0, 0, 0, toe_ang, toe_ang, -toe_ang])) + result = Solve(self.ik_solver.prog()) + right_foot_crank_state = result.GetSolution() + + q = x[:self.plant.num_positions()] + v = x[-self.plant.num_velocities():] + l_bar_quat, r_bar_quat, l_hip_pitch_frame, r_hip_pitch_frame = self.solve_for_achilles_rod_quats(q) + q_dt = q + self.plant.MapVelocityToQDot(self.context, v) * self.drake_sim_dt + l_bar_quat_dt, r_bar_quat_dt, _, _ = self.solve_for_achilles_rod_quats(q_dt) + + l_bar_omega = self.estimate_omega_bar(l_bar_quat, l_bar_quat_dt, self.drake_sim_dt) + r_bar_omega = self.estimate_omega_bar(r_bar_quat, r_bar_quat_dt, self.drake_sim_dt) + + l_bar_quat_gt = np.array([0.9785, -0.0164, 0.01787, -0.2049]) + r_bar_quat_gt = np.array([0.9786, 0.00386, -0.01524, -0.2051]) + + q_missing = np.zeros(35) + v_missing = np.zeros(32) + + left_foot_crank_ang = left_foot_crank_state[8] + left_plantar_rod = left_foot_crank_state[9] + right_foot_crank_ang = right_foot_crank_state[8] + right_plantar_rod = right_foot_crank_state[9] + q_missing[10:14] = l_bar_quat + q_missing[18] = left_foot_crank_ang + q_missing[19] = left_plantar_rod + q_missing[24:28] = r_bar_quat + q_missing[32] = right_foot_crank_ang + q_missing[33] = right_plantar_rod + + v_missing[16] = x[23 + self.vel_map['toe_leftdot']] + v_missing[17] = -x[23 + self.vel_map['toe_leftdot']] + v_missing[29] = x[23 + self.vel_map['toe_rightdot']] + v_missing[30] = -x[23 + self.vel_map['toe_rightdot']] + + v_missing[9:12] = l_bar_omega + v_missing[22:25] = r_bar_omega + + return q_missing, v_missing + + def generate_matrices(self): + self.map_q_drake_to_mujoco = np.zeros((35, 23)) + self.map_v_drake_to_mujoco = np.zeros((32, 22)) + self.map_q_drake_to_mujoco[0:3, 4:7] = np.eye(3) + self.map_q_drake_to_mujoco[3:7, 0:4] = np.eye(4) + self.map_q_drake_to_mujoco[7, self.pos_map["hip_roll_left"]] = 1 + self.map_q_drake_to_mujoco[8, self.pos_map["hip_yaw_left"]] = 1 + self.map_q_drake_to_mujoco[9, self.pos_map["hip_pitch_left"]] = 1 + self.map_q_drake_to_mujoco[14, self.pos_map["knee_left"]] = 1 + self.map_q_drake_to_mujoco[15, self.pos_map["knee_joint_left"]] = 1 + self.map_q_drake_to_mujoco[16, self.pos_map["ankle_joint_left"]] = 1 + self.map_q_drake_to_mujoco[17, self.pos_map["ankle_spring_joint_left"]] = 1 + self.map_q_drake_to_mujoco[20, self.pos_map["toe_left"]] = 1 + self.map_q_drake_to_mujoco[21, self.pos_map["hip_roll_right"]] = 1 + self.map_q_drake_to_mujoco[22, self.pos_map["hip_yaw_right"]] = 1 + self.map_q_drake_to_mujoco[23, self.pos_map["hip_pitch_right"]] = 1 + self.map_q_drake_to_mujoco[28, self.pos_map["knee_right"]] = 1 + self.map_q_drake_to_mujoco[29, self.pos_map["knee_joint_right"]] = 1 + self.map_q_drake_to_mujoco[30, self.pos_map["ankle_joint_right"]] = 1 + self.map_q_drake_to_mujoco[31, self.pos_map["ankle_spring_joint_right"]] = 1 + self.map_q_drake_to_mujoco[34, self.pos_map["toe_right"]] = 1 + + self.map_v_drake_to_mujoco[0:3, 3:6] = np.eye(3) + self.map_v_drake_to_mujoco[3:6, 0:3] = np.eye(3) + self.map_v_drake_to_mujoco[6, self.vel_map["hip_roll_leftdot"]] = 1 + self.map_v_drake_to_mujoco[7, self.vel_map["hip_yaw_leftdot"]] = 1 + self.map_v_drake_to_mujoco[8, self.vel_map["hip_pitch_leftdot"]] = 1 + self.map_v_drake_to_mujoco[12, self.vel_map["knee_leftdot"]] = 1 + self.map_v_drake_to_mujoco[13, self.vel_map["knee_joint_leftdot"]] = 1 + self.map_v_drake_to_mujoco[14, self.vel_map["ankle_joint_leftdot"]] = 1 + self.map_v_drake_to_mujoco[15, self.vel_map["ankle_spring_joint_leftdot"]] = 1 + self.map_v_drake_to_mujoco[18, self.vel_map["toe_leftdot"]] = 1 + self.map_v_drake_to_mujoco[19, self.vel_map["hip_roll_rightdot"]] = 1 + self.map_v_drake_to_mujoco[20, self.vel_map["hip_yaw_rightdot"]] = 1 + self.map_v_drake_to_mujoco[21, self.vel_map["hip_pitch_rightdot"]] = 1 + self.map_v_drake_to_mujoco[25, self.vel_map["knee_rightdot"]] = 1 + self.map_v_drake_to_mujoco[26, self.vel_map["knee_joint_rightdot"]] = 1 + self.map_v_drake_to_mujoco[27, self.vel_map["ankle_joint_rightdot"]] = 1 + self.map_v_drake_to_mujoco[28, self.vel_map["ankle_spring_joint_rightdot"]] = 1 + self.map_v_drake_to_mujoco[31, self.vel_map["toe_rightdot"]] = 1 diff --git a/bindings/pydairlib/cassie/mujoco/libcassiemujoco.so b/bindings/pydairlib/cassie/mujoco/libcassiemujoco.so new file mode 100755 index 0000000000..c34f8fa1a8 Binary files /dev/null and b/bindings/pydairlib/cassie/mujoco/libcassiemujoco.so differ diff --git a/bindings/pydairlib/cassie/mujoco/mujoco_lcm_utils.py b/bindings/pydairlib/cassie/mujoco/mujoco_lcm_utils.py new file mode 100644 index 0000000000..ad59047563 --- /dev/null +++ b/bindings/pydairlib/cassie/mujoco/mujoco_lcm_utils.py @@ -0,0 +1,164 @@ +import numpy as np +from scipy.spatial.transform import Rotation as R + +import dairlib + +def init_robot_output(): + robot_output_message = dairlib.lcmt_robot_output() + # Although this is different than the ordering of MBP, state receiver will rearrange accordingly + robot_output_message.position_names = 23*[''] + robot_output_message.position = 23 * [0] + robot_output_message.position_names[0] = 'base_x' + robot_output_message.position_names[1] = 'base_y' + robot_output_message.position_names[2] = 'base_z' + robot_output_message.position_names[3] = 'base_qw' + robot_output_message.position_names[4] = 'base_qx' + robot_output_message.position_names[5] = 'base_qy' + robot_output_message.position_names[6] = 'base_qz' + + robot_output_message.velocity_names = 22*[''] + robot_output_message.velocity = 23 * [0] + robot_output_message.velocity_names[0] = 'base_vx' + robot_output_message.velocity_names[1] = 'base_vy' + robot_output_message.velocity_names[2] = 'base_vz' + robot_output_message.velocity_names[3] = 'base_wx' + robot_output_message.velocity_names[4] = 'base_wy' + robot_output_message.velocity_names[5] = 'base_wz' + + qoffset = 7 + voffset = 6 + + robot_output_message.num_positions = 16 + qoffset + robot_output_message.num_velocities = 16 + voffset + + robot_output_message.position_names[qoffset] = 'hip_roll_left' + robot_output_message.position_names[qoffset + 1] = 'hip_yaw_left' + robot_output_message.position_names[qoffset + 2] = 'hip_pitch_left' + robot_output_message.position_names[qoffset + 3] = 'knee_left' + robot_output_message.position_names[qoffset + 4] = 'toe_left' + robot_output_message.position_names[qoffset + 5] = 'knee_joint_left' + robot_output_message.position_names[qoffset + 6] = 'ankle_joint_left' + robot_output_message.position_names[qoffset + 7] = 'ankle_spring_joint_left' + robot_output_message.position_names[qoffset + 8] = 'hip_roll_right' + robot_output_message.position_names[qoffset + 9] = 'hip_yaw_right' + robot_output_message.position_names[qoffset + 10] = 'hip_pitch_right' + robot_output_message.position_names[qoffset + 11] = 'knee_right' + robot_output_message.position_names[qoffset + 12] = 'toe_right' + robot_output_message.position_names[qoffset + 13] = 'knee_joint_right' + robot_output_message.position_names[qoffset + 14] = 'ankle_joint_right' + robot_output_message.position_names[qoffset + 15] = 'ankle_spring_joint_right' + + robot_output_message.velocity_names[voffset] = 'hip_roll_leftdot' + robot_output_message.velocity_names[voffset + 1] = 'hip_yaw_leftdot' + robot_output_message.velocity_names[voffset + 2] = 'hip_pitch_leftdot' + robot_output_message.velocity_names[voffset + 3] = 'knee_leftdot' + robot_output_message.velocity_names[voffset + 4] = 'toe_leftdot' + robot_output_message.velocity_names[voffset + 5] = 'knee_joint_leftdot' + robot_output_message.velocity_names[voffset + 6] = 'ankle_joint_leftdot' + robot_output_message.velocity_names[voffset + 7] = 'ankle_spring_joint_leftdot' + robot_output_message.velocity_names[voffset + 8] = 'hip_roll_rightdot' + robot_output_message.velocity_names[voffset + 9] = 'hip_yaw_rightdot' + robot_output_message.velocity_names[voffset + 10] = 'hip_pitch_rightdot' + robot_output_message.velocity_names[voffset + 11] = 'knee_rightdot' + robot_output_message.velocity_names[voffset + 12] = 'toe_rightdot' + robot_output_message.velocity_names[voffset + 13] = 'knee_joint_rightdot' + robot_output_message.velocity_names[voffset + 14] = 'ankle_joint_rightdot' + robot_output_message.velocity_names[voffset + 15] = 'ankle_spring_joint_rightdot' + + robot_output_message.num_efforts = 10 + robot_output_message.effort = 10 * [0] + robot_output_message.effort_names = 10 * [''] + robot_output_message.effort_names[0] = 'hip_roll_left_motor' + robot_output_message.effort_names[1] = 'hip_yaw_left_motor' + robot_output_message.effort_names[2] = 'hip_pitch_left_motor' + robot_output_message.effort_names[3] = 'knee_left_motor' + robot_output_message.effort_names[4] = 'toe_left_motor' + robot_output_message.effort_names[5] = 'hip_roll_right_motor' + robot_output_message.effort_names[6] = 'hip_yaw_right_motor' + robot_output_message.effort_names[7] = 'hip_pitch_right_motor' + robot_output_message.effort_names[8] = 'knee_right_motor' + robot_output_message.effort_names[9] = 'toe_right_motor' + return robot_output_message + +def init_cassie_out(): + return + +def pack_robot_output(robot_output, q, v, u, t): + q = np.array(q) + v = np.array(v) + u = np.array(u) + # import pdb; pdb.set_trace() + robot_output.utime = int(t * 1e6) + # If we are not in a floating base, need to skip the first 7 postions + # (x,y,z,quat) and the first 6 velocities (linear and ang. velocity) + qoffset = 0 + voffset = 0 + floating_base = True + if floating_base: + qoffset = 7 + voffset = 6 + + # Get positions + + # See cassiemujoco.h cassie_sim_qpos for ordering of q + # q starts with floating base coordinates and left hip, identical to + # the joint ordering chosen in cassiesim.c for the message + # The offsetting here is to (1) get the proper length and (2) skip floating + # base coordinates if necessary + for i in range(3 + qoffset): + robot_output.position[i] = q[7 - qoffset + i] + + # remainder of left leg + robot_output.position[3 + qoffset] = q[14] # knee + robot_output.position[4 + qoffset] = q[20] # foot + robot_output.position[5 + qoffset] = q[15] # shin + robot_output.position[6 + qoffset] = q[16] # tarsus + robot_output.position[7 + qoffset] = q[17] # heel spring + + # right hip, also in identical order + robot_output.position[8 + qoffset] = q[21] + robot_output.position[9 + qoffset] = q[22] + robot_output.position[10 + qoffset] = q[23] + + # remainder of right leg + robot_output.position[11 + qoffset] = q[28] # knee + robot_output.position[12 + qoffset] = q[34] # foot + robot_output.position[13 + qoffset] = q[29] # shin + robot_output.position[14 + qoffset] = q[30] # tarsus + robot_output.position[15 + qoffset] = q[31] # heel spring + + # get velocities + # See cassiemujoco.h cassie_sim_qvel for ordering of v + # floating base and left hip + for i in range(3 + voffset): + robot_output.velocity[i] = v[6 - voffset + i] + + # Convert rotational velocity to global frame + # rot = R.from_quat([q[6], q[3], q[4], q[5]]) + # robot_output.velocity[3:6] = rot.as_dcm() @ robot_output.velocity[3:6] + # remainder of left leg + robot_output.velocity[3 + voffset] = v[12] # knee + robot_output.velocity[4 + voffset] = v[18] # foot + robot_output.velocity[5 + voffset] = v[13] # shin + robot_output.velocity[6 + voffset] = v[14] # tarsus + robot_output.velocity[7 + voffset] = v[15] # heel spring + + # right hip + robot_output.velocity[8 + voffset] = v[19] + robot_output.velocity[9 + voffset] = v[20] + robot_output.velocity[10 + voffset] = v[21] + + + # remainder of right leg + robot_output.velocity[11 + voffset] = v[25] # knee + robot_output.velocity[12 + voffset] = v[31] # foot + robot_output.velocity[13 + voffset] = v[26] # shin + robot_output.velocity[14 + voffset] = v[27] # tarsus + robot_output.velocity[15 + voffset] = v[28] # heel spring + + # Efforts + # Ordered list of drive_out_t addresses + for i in range(10): + robot_output.effort[i] = u[i] + + return diff --git a/bindings/pydairlib/cassie/optimizers/ppo.py b/bindings/pydairlib/cassie/optimizers/ppo.py new file mode 100644 index 0000000000..4b106fe946 --- /dev/null +++ b/bindings/pydairlib/cassie/optimizers/ppo.py @@ -0,0 +1,586 @@ +"""Proximal Policy Optimization (clip objective).""" +from copy import deepcopy + +import torch +import torch.optim as optim +from torch.utils.data.sampler import BatchSampler, SubsetRandomSampler +from torch.distributions import kl_divergence + +from torch.nn.utils.rnn import pad_sequence + +import time + +import numpy as np +import os, sys + +import ray + +import pickle + +class PPOBuffer: + """ + A buffer for storing trajectory data and calculating returns for the policy + and critic updates. + This container is intentionally not optimized w.r.t. to memory allocation + speed because such allocation is almost never a bottleneck for policy + gradient. + + On the other hand, experience buffers are a frequent source of + off-by-one errors and other bugs in policy gradient implementations, so + this code is optimized for clarity and readability, at the expense of being + (very) marginally slower than some other implementations. + (Premature optimization is the root of all evil). + """ + def __init__(self, gamma=0.99, lam=0.95, use_gae=False): + self.states = [] + self.actions = [] + self.rewards = [] + self.values = [] + self.returns = [] + + self.ep_returns = [] # for logging + self.ep_lens = [] + + self.gamma, self.lam = gamma, lam + + self.ptr = 0 + self.traj_idx = [0] + + def __len__(self): + return len(self.states) + + def storage_size(self): + return len(self.states) + + def store(self, state, action, reward, value): + """ + Append one timestep of agent-environment interaction to the buffer. + """ + # TODO: make sure these dimensions really make sense + self.states += [state.squeeze(0)] + self.actions += [action.squeeze(0)] + self.rewards += [reward.squeeze(0)] + self.values += [value.squeeze(0)] + + self.ptr += 1 + + def finish_path(self, last_val=None): + self.traj_idx += [self.ptr] + rewards = self.rewards[self.traj_idx[-2]:self.traj_idx[-1]] + + returns = [] + + R = last_val.squeeze(0).copy() # Avoid copy? + for reward in reversed(rewards): + R = self.gamma * R + reward + returns.insert(0, R) # TODO: self.returns.insert(self.path_idx, R) ? + # also technically O(k^2), may be worth just reversing list + # BUG? This is adding copies of R by reference (?) + + self.returns += returns + + self.ep_returns += [np.sum(rewards)] + self.ep_lens += [len(rewards)] + + def get(self): + return( + self.states, + self.actions, + self.returns, + self.values + ) + +class PPO: + def __init__(self, args, save_path): + self.env_name = args['env_name'] + self.gamma = args['gamma'] + self.lam = args['lam'] + self.lr = args['lr'] + self.eps = args['eps'] + self.entropy_coeff = args['entropy_coeff'] + self.clip = args['clip'] + self.minibatch_size = args['minibatch_size'] + self.epochs = args['epochs'] + self.num_steps = args['num_steps'] + self.max_traj_len = args['max_traj_len'] + self.use_gae = args['use_gae'] + self.n_proc = args['num_procs'] + self.grad_clip = args['max_grad_norm'] + self.recurrent = args['recurrent'] + + self.total_steps = 0 + self.highest_reward = -1 + self.limit_cores = 0 + + self.save_path = save_path + + # os.environ['OMP_NUM_THREA DS'] = '1' + # if args['redis_address'] is not None: + # ray.init(num_cpos=self.n_proc, redis_address=args['redis_address']) + # else: + # ray.init(num_cpus=self.n_proc) + + def save(self, policy, critic): + + try: + os.makedirs(self.save_path) + except OSError: + pass + filetype = ".pt" # pytorch model + torch.save(policy, os.path.join(self.save_path, "actor" + filetype)) + torch.save(critic, os.path.join(self.save_path, "critic" + filetype)) + + @ray.remote + @torch.no_grad() + def sample(self, env_fn, policy, critic, min_steps, max_traj_len, deterministic=False, anneal=1.0, term_thresh=0): + """ + Sample at least min_steps number of total timesteps, truncating + trajectories only if they exceed max_traj_len number of timesteps + """ + torch.set_num_threads(1) # By default, PyTorch will use multiple cores to speed up operations. + # This can cause issues when Ray also uses multiple cores, especially on machines + # with a lot of CPUs. I observed a significant speedup when limiting PyTorch + # to a single core - I think it basically stopped ray workers from stepping on each + # other's toes. + + # env = WrapEnv(env_fn) # TODO + env = MuJoCoCassieGym(self.reward_function, visualize=self.visualize) + env.end_time = self.end_time + controller_plant = MultibodyPlant(8e-5) + AddCassieMultibody(controller_plant, None, True, self.controller_urdf, False, False) + controller_plant.Finalize() + controller = OSCRunningControllerFactory(controller_plant, self.osc_running_gains_filename, + self.osqp_settings) + env.make(controller) + + memory = PPOBuffer(self.gamma, self.lam) + + num_steps = 0 + while num_steps < min_steps: + state = torch.Tensor(env.reset()) + + done = False + value = 0 + traj_len = 0 + + if hasattr(policy, 'init_hidden_state'): + policy.init_hidden_state() + + if hasattr(critic, 'init_hidden_state'): + critic.init_hidden_state() + + while not done and traj_len < max_traj_len: + action = policy(state, deterministic=False, anneal=anneal) + value = critic(state) + + next_state, reward, done, _ = env.step(action.numpy(), term_thresh=term_thresh) + + memory.store(state.numpy(), action.numpy(), reward, value.numpy()) + + state = torch.Tensor(next_state) + + traj_len += 1 + num_steps += 1 + + value = critic(state) + memory.finish_path(last_val=(not done) * value.numpy()) + + return memory + + def sample_parallel(self, env_fn, policy, critic, min_steps, max_traj_len, deterministic=False, anneal=1.0, term_thresh=0): + + worker = self.sample + args = (self, env_fn, policy, critic, min_steps // self.n_proc, max_traj_len, deterministic, anneal, term_thresh) + + # Create pool of workers, each getting data for min_steps + workers = [worker.remote(*args) for _ in range(self.n_proc)] + + result = [] + total_steps = 0 + + while total_steps < min_steps: + # get result from a worker + ready_ids, _ = ray.wait(workers, num_returns=1) + + # update result + result.append(ray.get(ready_ids[0])) + + # remove ready_ids from workers (O(n)) but n isn't that big + workers.remove(ready_ids[0]) + + # update total steps + total_steps += len(result[-1]) + + # start a new worker + workers.append(worker.remote(*args)) + + # O(n) + def merge(buffers): + merged = PPOBuffer(self.gamma, self.lam) + for buf in buffers: + offset = len(merged) + + merged.states += buf.states + merged.actions += buf.actions + merged.rewards += buf.rewards + merged.values += buf.values + merged.returns += buf.returns + + merged.ep_returns += buf.ep_returns + merged.ep_lens += buf.ep_lens + + merged.traj_idx += [offset + i for i in buf.traj_idx[1:]] + merged.ptr += buf.ptr + + return merged + + total_buf = merge(result) + + return total_buf + + def sample_parallel_old(self, env_fn, policy, critic, min_steps, max_traj_len, deterministic=False, anneal=1.0): + + result = [] + total_steps = 0 + + real_proc = self.n_proc + if self.limit_cores: + real_proc = 48 - 16*int(np.log2(60 / env_fn().simrate)) + print("limit cores active, using {} cores".format(real_proc)) + args = (self, env_fn, policy, critic, min_steps*self.n_proc // real_proc, max_traj_len, deterministic) + result_ids = [self.sample.remote(*args) for _ in range(real_proc)] + result = ray.get(result_ids) + + # O(n) + def merge(buffers): + merged = PPOBuffer(self.gamma, self.lam) + for buf in buffers: + offset = len(merged) + + merged.states += buf.states + merged.actions += buf.actions + merged.rewards += buf.rewards + merged.values += buf.values + merged.returns += buf.returns + + merged.ep_returns += buf.ep_returns + merged.ep_lens += buf.ep_lens + + merged.traj_idx += [offset + i for i in buf.traj_idx[1:]] + merged.ptr += buf.ptr + + return merged + + total_buf = merge(result) + + return total_buf + + def update_policy(self, obs_batch, action_batch, return_batch, advantage_batch, mask, env_fn, mirror_observation=None, mirror_action=None): + policy = self.policy + critic = self.critic + old_policy = self.old_policy + + values = critic(obs_batch) + pdf = policy.distribution(obs_batch) + + # TODO, move this outside loop? + with torch.no_grad(): + old_pdf = old_policy.distribution(obs_batch) + old_log_probs = old_pdf.log_prob(action_batch).sum(-1, keepdim=True) + + log_probs = pdf.log_prob(action_batch).sum(-1, keepdim=True) + + ratio = (log_probs - old_log_probs).exp() + + cpi_loss = ratio * advantage_batch * mask + clip_loss = ratio.clamp(1.0 - self.clip, 1.0 + self.clip) * advantage_batch * mask + actor_loss = -torch.min(cpi_loss, clip_loss).mean() + + critic_loss = 0.5 * ((return_batch - values) * mask).pow(2).mean() + + entropy_penalty = -(self.entropy_coeff * pdf.entropy() * mask).mean() + + # Mirror Symmetry Loss + if mirror_observation is not None and mirror_action is not None: + env = env_fn() + deterministic_actions = policy(obs_batch) + if env.clock_based: + if self.recurrent: + mir_obs = torch.stack([mirror_observation(obs_batch[i,:,:], env.clock_inds) for i in range(obs_batch.shape[0])]) + mirror_actions = policy(mir_obs) + else: + mir_obs = mirror_observation(obs_batch, env.clock_inds) + mirror_actions = policy(mir_obs) + else: + if self.recurrent: + mirror_actions = policy(mirror_observation(torch.stack([mirror_observation(obs_batch[i,:,:]) for i in range(obs_batch.shape[0])]))) + else: + mirror_actions = policy(mirror_observation(obs_batch)) + mirror_actions = mirror_action(mirror_actions) + mirror_loss = 0.4 * (deterministic_actions - mirror_actions).pow(2).mean() + else: + mirror_loss = 0 + + self.actor_optimizer.zero_grad() + (actor_loss + mirror_loss + entropy_penalty).backward() + + # Clip the gradient norm to prevent "unlucky" minibatches from + # causing pathological updates + torch.nn.utils.clip_grad_norm_(policy.parameters(), self.grad_clip) + self.actor_optimizer.step() + + self.critic_optimizer.zero_grad() + critic_loss.backward() + + # Clip the gradient norm to prevent "unlucky" minibatches from + # causing pathological updates + torch.nn.utils.clip_grad_norm_(critic.parameters(), self.grad_clip) + self.critic_optimizer.step() + + with torch.no_grad(): + kl = kl_divergence(pdf, old_pdf) + + if mirror_observation is not None and mirror_action is not None: + mirror_loss_return = mirror_loss.item() + else: + mirror_loss_return = 0 + return actor_loss.item(), pdf.entropy().mean().item(), critic_loss.item(), ratio.mean().item(), kl.mean().item(), mirror_loss_return + + def train(self, + env_fn, + policy, + critic, + n_itr, + logger=None, anneal_rate=1.0): + + self.old_policy = deepcopy(policy) + self.policy = policy + self.critic = critic + + self.actor_optimizer = optim.Adam(policy.parameters(), lr=self.lr, eps=self.eps) + self.critic_optimizer = optim.Adam(critic.parameters(), lr=self.lr, eps=self.eps) + + start_time = time.time() + + env = env_fn() + obs_mirr, act_mirr = None, None + if hasattr(env, 'mirror_observation'): + if env.clock_based: + obs_mirr = env.mirror_clock_observation + else: + obs_mirr = env.mirror_observation + + if hasattr(env, 'mirror_action'): + act_mirr = env.mirror_action + + curr_anneal = 1.0 + curr_thresh = 0 + start_itr = 0 + ep_counter = 0 + do_term = False + for itr in range(n_itr): + print("********** Iteration {} ************".format(itr)) + + sample_start = time.time() + if self.highest_reward > (2/3)*self.max_traj_len and curr_anneal > 0.5: + curr_anneal *= anneal_rate + if do_term and curr_thresh < 0.35: + curr_thresh = .1 * 1.0006**(itr-start_itr) + batch = self.sample_parallel(env_fn, self.policy, self.critic, self.num_steps, self.max_traj_len, anneal=curr_anneal, term_thresh=curr_thresh) + + print("time elapsed: {:.2f} s".format(time.time() - start_time)) + samp_time = time.time() - sample_start + print("sample time elapsed: {:.2f} s".format(samp_time)) + + observations, actions, returns, values = map(torch.Tensor, batch.get()) + + advantages = returns - values + advantages = (advantages - advantages.mean()) / (advantages.std() + self.eps) + + minibatch_size = self.minibatch_size or advantages.numel() + + print("timesteps in batch: %i" % advantages.numel()) + self.total_steps += advantages.numel() + + self.old_policy.load_state_dict(policy.state_dict()) + + optimizer_start = time.time() + + for epoch in range(self.epochs): + losses = [] + entropies = [] + kls = [] + if self.recurrent: + random_indices = SubsetRandomSampler(range(len(batch.traj_idx)-1)) + sampler = BatchSampler(random_indices, minibatch_size, drop_last=False) + else: + random_indices = SubsetRandomSampler(range(advantages.numel())) + sampler = BatchSampler(random_indices, minibatch_size, drop_last=True) + + for indices in sampler: + if self.recurrent: + obs_batch = [observations[batch.traj_idx[i]:batch.traj_idx[i+1]] for i in indices] + action_batch = [actions[batch.traj_idx[i]:batch.traj_idx[i+1]] for i in indices] + return_batch = [returns[batch.traj_idx[i]:batch.traj_idx[i+1]] for i in indices] + advantage_batch = [advantages[batch.traj_idx[i]:batch.traj_idx[i+1]] for i in indices] + mask = [torch.ones_like(r) for r in return_batch] + + obs_batch = pad_sequence(obs_batch, batch_first=False) + action_batch = pad_sequence(action_batch, batch_first=False) + return_batch = pad_sequence(return_batch, batch_first=False) + advantage_batch = pad_sequence(advantage_batch, batch_first=False) + mask = pad_sequence(mask, batch_first=False) + else: + obs_batch = observations[indices] + action_batch = actions[indices] + return_batch = returns[indices] + advantage_batch = advantages[indices] + mask = 1 + + scalars = self.update_policy(obs_batch, action_batch, return_batch, advantage_batch, mask, env_fn, mirror_observation=obs_mirr, mirror_action=act_mirr) + actor_loss, entropy, critic_loss, ratio, kl, mirror_loss = scalars + + entropies.append(entropy) + kls.append(kl) + losses.append([actor_loss, entropy, critic_loss, ratio, kl, mirror_loss]) + + # TODO: add verbosity arguments to suppress this + print(' '.join(["%g"%x for x in np.mean(losses, axis=0)])) + + # Early stopping + if np.mean(kl) > 0.02: + print("Max kl reached, stopping optimization early.") + break + + opt_time = time.time() - optimizer_start + print("optimizer time elapsed: {:.2f} s".format(opt_time)) + + if np.mean(batch.ep_lens) >= self.max_traj_len * 0.75: + ep_counter += 1 + if do_term == False and ep_counter > 50: + do_term = True + start_itr = itr + + if logger is not None: + evaluate_start = time.time() + test = self.sample_parallel(env_fn, self.policy, self.critic, self.num_steps // 2, self.max_traj_len, deterministic=True) + eval_time = time.time() - evaluate_start + print("evaluate time elapsed: {:.2f} s".format(eval_time)) + + avg_eval_reward = np.mean(test.ep_returns) + avg_batch_reward = np.mean(batch.ep_returns) + avg_ep_len = np.mean(batch.ep_lens) + mean_losses = np.mean(losses, axis=0) + # print("avg eval reward: {:.2f}".format(avg_eval_reward)) + + sys.stdout.write("-" * 37 + "\n") + sys.stdout.write("| %15s | %15s |" % ('Return (test)', avg_eval_reward) + "\n") + sys.stdout.write("| %15s | %15s |" % ('Return (batch)', avg_batch_reward) + "\n") + sys.stdout.write("| %15s | %15s |" % ('Mean Eplen', avg_ep_len) + "\n") + sys.stdout.write("| %15s | %15s |" % ('Mean KL Div', "%8.3g" % kl) + "\n") + sys.stdout.write("| %15s | %15s |" % ('Mean Entropy', "%8.3g" % entropy) + "\n") + sys.stdout.write("-" * 37 + "\n") + sys.stdout.flush() + + entropy = np.mean(entropies) + kl = np.mean(kls) + + logger.add_scalar("Test/Return", avg_eval_reward, itr) + logger.add_scalar("Train/Return", avg_batch_reward, itr) + logger.add_scalar("Train/Mean Eplen", avg_ep_len, itr) + logger.add_scalar("Train/Mean KL Div", kl, itr) + logger.add_scalar("Train/Mean Entropy", entropy, itr) + + logger.add_scalar("Misc/Critic Loss", mean_losses[2], itr) + logger.add_scalar("Misc/Actor Loss", mean_losses[0], itr) + logger.add_scalar("Misc/Mirror Loss", mean_losses[5], itr) + logger.add_scalar("Misc/Timesteps", self.total_steps, itr) + + logger.add_scalar("Misc/Sample Times", samp_time, itr) + logger.add_scalar("Misc/Optimize Times", opt_time, itr) + logger.add_scalar("Misc/Evaluation Times", eval_time, itr) + logger.add_scalar("Misc/Termination Threshold", curr_thresh, itr) + + # TODO: add option for how often to save model + if self.highest_reward < avg_eval_reward: + self.highest_reward = avg_eval_reward + self.save(policy, critic) + +def run_experiment(args): + from util.env import env_factory + from util.log import create_logger + + # wrapper function for creating parallelized envs + env_fn = env_factory(args.env_name, simrate=args.simrate, command_profile=args.command_profile, input_profile=args.input_profile, learn_gains=args.learn_gains, dynamics_randomization=args.dyn_random, reward=args.reward, history=args.history, mirror=args.mirror, ik_baseline=args.ik_baseline, no_delta=args.no_delta, traj=args.traj) + obs_dim = env_fn().observation_space.shape[0] + action_dim = env_fn().action_space.shape[0] + + # Set up Parallelism + os.environ['OMP_NUM_THREADS'] = '1' + if not ray.is_initialized(): + if args.redis_address is not None: + ray.init(num_cpus=args.num_procs, redis_address=args.redis_address) + else: + ray.init(num_cpus=args.num_procs) + + # Set seeds + torch.manual_seed(args.seed) + np.random.seed(args.seed) + + if args.previous is not None: + policy = torch.load(os.path.join(args.previous, "actor.pt")) + critic = torch.load(os.path.join(args.previous, "critic.pt")) + # TODO: add ability to load previous hyperparameters, if this is something that we event want + # with open(args.previous + "experiment.pkl", 'rb') as file: + # args = pickle.loads(file.read()) + print("loaded model from {}".format(args.previous)) + else: + if args.recurrent: + policy = Gaussian_LSTM_Actor(obs_dim, action_dim, fixed_std=np.exp(-2), env_name=args.env_name) + critic = LSTM_V(obs_dim) + else: + if args.learn_stddev: + policy = Gaussian_FF_Actor(obs_dim, action_dim, fixed_std=None, env_name=args.env_name, bounded=args.bounded) + else: + policy = Gaussian_FF_Actor(obs_dim, action_dim, fixed_std=np.exp(args.std_dev), env_name=args.env_name, bounded=args.bounded) + critic = FF_V(obs_dim) + + with torch.no_grad(): + policy.obs_mean, policy.obs_std = map(torch.Tensor, get_normalization_params(iter=args.input_norm_steps, noise_std=1, policy=policy, env_fn=env_fn, procs=args.num_procs)) + critic.obs_mean = policy.obs_mean + critic.obs_std = policy.obs_std + + policy.train() + critic.train() + + print("obs_dim: {}, action_dim: {}".format(obs_dim, action_dim)) + + # create a tensorboard logging object + logger = create_logger(args) + + algo = PPO(args=vars(args), save_path=logger.dir) + + print() + print("Synchronous Distributed Proximal Policy Optimization:") + print(" ├ recurrent: {}".format(args.recurrent)) + print(" ├ run name: {}".format(args.run_name)) + print(" ├ max traj len: {}".format(args.max_traj_len)) + print(" ├ seed: {}".format(args.seed)) + print(" ├ num procs: {}".format(args.num_procs)) + print(" ├ lr: {}".format(args.lr)) + print(" ├ eps: {}".format(args.eps)) + print(" ├ lam: {}".format(args.lam)) + print(" ├ gamma: {}".format(args.gamma)) + print(" ├ learn stddev: {}".format(args.learn_stddev)) + print(" ├ std_dev: {}".format(args.std_dev)) + print(" ├ entropy coeff: {}".format(args.entropy_coeff)) + print(" ├ clip: {}".format(args.clip)) + print(" ├ minibatch size: {}".format(args.minibatch_size)) + print(" ├ epochs: {}".format(args.epochs)) + print(" ├ num steps: {}".format(args.num_steps)) + print(" ├ use gae: {}".format(args.use_gae)) + print(" ├ max grad norm: {}".format(args.max_grad_norm)) + print(" └ max traj len: {}".format(args.max_traj_len)) + print() + + algo.train(env_fn, policy, critic, args.n_itr, logger=logger, anneal_rate=args.anneal) \ No newline at end of file diff --git a/bindings/pydairlib/cassie/simulators_py.cc b/bindings/pydairlib/cassie/simulators_py.cc new file mode 100644 index 0000000000..96aba78413 --- /dev/null +++ b/bindings/pydairlib/cassie/simulators_py.cc @@ -0,0 +1,50 @@ +#include +#include +#include +#include + + +#include "drake/bindings/pydrake/pydrake_pybind.h" + +#include "examples/Cassie/diagrams/cassie_sim_diagram.h" + +namespace py = pybind11; + +namespace dairlib { +namespace pydairlib { + +using drake::pydrake::make_unowned_shared_ptr_from_raw; +using examples::CassieSimDiagram; + +PYBIND11_MODULE(simulators, m) { + m.doc() = "Binding controller factories for Cassie"; + + using py_rvp = py::return_value_policy; + + py::class_>(m, "CassieSimDiagram") + .def(py::init( + [](drake::multibody::MultibodyPlant& plant, + const std::string& urdf, bool visualize, double mu, + double stiffness, double dissipation_rate) { + return std::make_unique( + make_unowned_shared_ptr_from_raw(&plant), urdf, visualize, + mu); + }), + py::arg("plant"), py::arg("urdf"), py::arg("visualize"), py::arg("mu"), py::arg("stiffness"), + py::arg("dissipation_rate")) + .def("get_plant", &CassieSimDiagram::get_plant, + py_rvp::reference_internal) + .def("get_input_port_actuation", + &CassieSimDiagram::get_input_port_actuation, + py_rvp::reference_internal) + .def("get_input_port_radio", &CassieSimDiagram::get_input_port_radio, + py_rvp::reference_internal) + .def("get_output_port_state", &CassieSimDiagram::get_output_port_state, + py_rvp::reference_internal) + .def("get_output_port_cassie_out", + &CassieSimDiagram::get_output_port_cassie_out, + py_rvp::reference_internal); +} +} // namespace pydairlib +} // namespace dairlib \ No newline at end of file diff --git a/bindings/pydairlib/common/plot_styler.py b/bindings/pydairlib/common/plot_styler.py index 94bb199474..963ad1a9ae 100644 --- a/bindings/pydairlib/common/plot_styler.py +++ b/bindings/pydairlib/common/plot_styler.py @@ -8,8 +8,35 @@ class PlotStyler(): + @staticmethod + def set_paper_styling(): + # matplotlib.rcParams['figure.figsize'] = 20, 12 + # matplotlib.rcParams['figure.figsize'] = 20, 6 + matplotlib.rcParams['figure.figsize'] = 16, 12 + plt.rcParams['figure.dpi'] = 200 + matplotlib.rcParams['figure.autolayout'] = True + font = {'size': 20} + matplotlib.rc('font', **font) + matplotlib.rcParams['lines.linewidth'] = 4 + plt.set_cmap('tab20') + @staticmethod + def set_compact_styling(): + matplotlib.rcParams['figure.figsize'] = 20, 12 + font_size = 6 + plt.rc('legend', fontsize=font_size) + plt.rc('axes', labelsize=font_size, titlesize=font_size) + plt.rc('xtick', labelsize=font_size) + plt.rc('ytick', labelsize=font_size) + plt.rcParams['figure.dpi'] = 75 + matplotlib.rcParams['figure.autolayout'] = False + font = {'size': font_size} + matplotlib.rc('font', **font) + matplotlib.rcParams['lines.linewidth'] = 2 + plt.set_cmap('tab20') + + def __init__(self, figure=None, nrows=1, ncols=1): - def __init__(self, figure=None): + # plt.subplots_adjust(left=0.0, right=1.0, bottom=0.0, top=1.0) # List is [left, bottom, width, height] # self.cmap = plt.get_cmap('tab10') self.cmap = plt.get_cmap('tab20') self.blue = '#011F5B' @@ -17,54 +44,59 @@ def __init__(self, figure=None): self.yellow = '#F2C100' self.grey = '#909090' self.orange = '#FE7F0E' - self.directory = None - self.fig = plt.figure() if figure is None else figure - self.fig_id = self.fig.number + # self.directory = None + self.dpi = plt.rcParams['figure.dpi'] + self.directory = '/home/yangwill/Pictures/plot_styler/' + plt.rc('legend', fontsize=24) + plt.rc('axes', labelsize=24, titlesize=24) + plt.rc('xtick', labelsize=24) + plt.rc('ytick', labelsize=24) + matplotlib.rcParams['figure.figsize'] = 12, 7 + matplotlib.rcParams['figure.autolayout'] = True + matplotlib.rcParams['axes.xmargin'] = 0 + matplotlib.rcParams['axes.ymargin'] = 0 + # matplotlib.rcParams['toolbar'] = 'None' + matplotlib.rcParams['text.latex.preamble'] = r"\usepackage{amsmath}" + if figure is None: + self.fig, self.axes = plt.subplots(nrows=nrows, ncols=ncols, + sharex='all', dpi=self.dpi) + else: + self.fig = figure + self.axes = figure.get_axes() + if not isinstance(self.axes, np.ndarray): + self.axes = [self.axes] + # self.fig.add_axes([0.1, 0.15, 0.85, 0.75]) # List is [left, bottom, width, height] + self.fig_id = self.fig.number + plt.subplots_adjust(left=0.1, right=0.85, bottom=0.15, top=0.75) # List is [left, bottom, width, height] return def attach(self): plt.figure(self.fig_id) - def set_default_styling(self, directory=None): - plt.figure(self.fig_id) - self.directory = directory - matplotlib.rcParams["savefig.directory"] = directory - matplotlib.rcParams['text.latex.preamble'] = [r"\usepackage{amsmath}"] - matplotlib.rcParams['figure.figsize'] = 20, 12 - # matplotlib.rcParams['figure.figsize'] = 20, 6 - # matplotlib.rcParams['figure.figsize'] = 8, 5 - matplotlib.rcParams['figure.autolayout'] = True - font = {'size': 20} - matplotlib.rc('font', **font) - matplotlib.rcParams['lines.linewidth'] = 4 - plt.set_cmap('tab20') - self.directory = directory - - def plot(self, xdata, ydata, xlim=None, ylim=None, color=None, linestyle=None, - grid=True, xlabel=None, ylabel=None, title=None, legend=None, data_label=None): - - plt.figure(self.fig_id) - plt.plot(xdata, ydata, color=color, linestyle=linestyle, label=data_label) + def plot(self, xdata, ydata, xlim=None, ylim=None, color=None, + linestyle=None, + grid=False, xlabel=None, ylabel=None, title=None, legend=None, + data_label=None, subplot_index=0): + self.axes[subplot_index].plot(xdata, ydata, color=color, + linestyle=linestyle, label=data_label) if xlim: - plt.xlim(xlim) + self.axes[subplot_index].set_xlim(xlim) if ylim: - plt.ylim(ylim) + self.axes[subplot_index].set_ylim(ylim) if xlabel: - # plt.xlabel(xlabel, fontweight="bold") - plt.xlabel(xlabel) + self.axes[subplot_index].set_xlabel(xlabel) if ylabel: - # plt.ylabel(ylabel, fontweight="bold") - plt.ylabel(ylabel) + self.axes[subplot_index].set_ylabel(ylabel) if title: - # plt.title(title, fontweight="bold") - plt.title(title) + self.axes[subplot_index].set_title(title) if legend: - plt.legend(legend) - - plt.grid(grid, which='major') + self.axes[subplot_index].legend(legend) + self.axes[subplot_index].grid(grid, which='major') + self.axes[subplot_index].autoscale() - def plot_bands(self, x_low, x_high, y_low, y_high, color='C0'): + def plot_bands(self, x_low, x_high, y_low, y_high, color='C0', + subplot_index=0): plt.figure(self.fig_id) vertices = np.block([[x_low, x_high[::-1]], [y_low, y_high[::-1]]]).T @@ -72,30 +104,33 @@ def plot_bands(self, x_low, x_high, y_low, y_high, color='C0'): codes[0] = Path.MOVETO path = Path(vertices, codes) patch = PathPatch(path, facecolor=color, edgecolor='none', alpha=0.3) - ax = plt.gca() - # ax.plot(xdata, ydata) - ax.add_patch(patch) + self.axes[subplot_index].add_patch(patch) def show_fig(self): self.fig.show() return def save_fig(self, filename): - plt.figure(self.fig_id) - plt.savefig(self.directory + filename, dpi=200) + self.fig.savefig(self.directory + filename, dpi=400, bbox_inches='tight') return - def add_legend(self, labels, loc=0): - plt.figure(self.fig_id) - ax = plt.gca() - legend = ax.legend(labels, loc=loc) - ax.add_artist(legend) + def add_legend(self, labels, loc=0, subplot_index=0): + legend = self.axes[subplot_index].legend(labels, loc=loc) + self.axes[subplot_index].add_artist(legend) return - def annotate(self, text, x, y, x_text, y_text, arrowprops=None): - plt.figure(self.fig_id) - ax = plt.gca() + def annotate(self, text, x, y, x_text, y_text, arrowprops=None, subplot_index=0): if not arrowprops: arrowprops = dict(facecolor='black') # arrowstyle='->' - ax.annotate(text, xy=(x, y), xytext=( + self.axes[subplot_index].annotate(text, xy=(x, y), xytext=( x_text, y_text), arrowprops=arrowprops) + + def set_subplot_options(self, sharex=None, sharey=None): + if sharex is not None: + plt.setp(self.axes, sharex=sharex) + if sharey is not None: + plt.setp(self.axes, sharex=sharey) + + def tight_layout(self): + # self.axes[0].autoscale(enable=True, axis='y', tight=True) + self.axes[0].autoscale(enable=True, axis='x', tight=True) diff --git a/bindings/pydairlib/common/plotting_utils.py b/bindings/pydairlib/common/plotting_utils.py index a0ca2c762b..3023093578 100644 --- a/bindings/pydairlib/common/plotting_utils.py +++ b/bindings/pydairlib/common/plotting_utils.py @@ -19,51 +19,50 @@ def make_plot(data_dictionary, time_key, time_slice, keys_to_plot, slices_to_plot, legend_entries, plot_labels, - ps): - legend = [] - for key in keys_to_plot: - if key not in slices_to_plot: - ps.plot(data_dictionary[time_key][time_slice], - data_dictionary[key][time_slice]) - else: - ps.plot(data_dictionary[time_key][time_slice], - data_dictionary[key][time_slice, slices_to_plot[key]]) - if key in legend_entries: - legend.extend(legend_entries[key]) + ps, subplot_index=0): + legend = [] + for key in keys_to_plot: + if key not in slices_to_plot: + ps.plot(data_dictionary[time_key][time_slice], + data_dictionary[key][time_slice], xlabel=plot_labels['xlabel'], + ylabel=plot_labels['ylabel'], title=plot_labels['title'], subplot_index=subplot_index) + else: + ps.plot(data_dictionary[time_key][time_slice], + data_dictionary[key][time_slice, slices_to_plot[key]], + xlabel=plot_labels['xlabel'], ylabel=plot_labels['ylabel'], + title=plot_labels['title'], subplot_index=subplot_index) + if key in legend_entries: + legend.extend(legend_entries[key]) - # plt.legend(legend) - ps.add_legend(legend) - plt.xlabel(plot_labels['xlabel']) - plt.ylabel(plot_labels['ylabel']) - plt.title(plot_labels['title']) + ps.add_legend(legend, loc='upper right', subplot_index=subplot_index) def make_mixed_data_plot(data_dictionaries, time_keys, time_slices, keys_to_plot, slices_to_plot, legend_entries, plot_labels, ps): - legend = [] - for i, data_dictionary in enumerate(data_dictionaries): - time_key = time_keys[i] - time_slice = time_slices[i] - for key in keys_to_plot[i]: - if key not in slices_to_plot[i]: - ps.plot(data_dictionary[time_key][time_slice], - data_dictionary[key][time_slice]) - else: - ps.plot(data_dictionary[time_key][time_slice], - data_dictionary[key][time_slice, slices_to_plot[key]]) - legend.extend(legend_entries[key]) + legend = [] + for i, data_dictionary in enumerate(data_dictionaries): + time_key = time_keys[i] + time_slice = time_slices[i] + for key in keys_to_plot[i]: + if key not in slices_to_plot[i]: + ps.plot(data_dictionary[time_key][time_slice], + data_dictionary[key][time_slice]) + else: + ps.plot(data_dictionary[time_key][time_slice], + data_dictionary[key][time_slice, slices_to_plot[key]]) + legend.extend(legend_entries[key]) - ps.add_legend(legend) - plt.xlabel(plot_labels['xlabel']) - plt.ylabel(plot_labels['ylabel']) - plt.title(plot_labels['title']) + ps.add_legend(legend) + plt.xlabel(plot_labels['xlabel']) + plt.ylabel(plot_labels['ylabel']) + plt.title(plot_labels['title']) def slice_to_string_list(slice_): - if isinstance(slice_, slice): - return [str(i) for i in range(slice_.start, slice_.stop, - slice_.step if slice_.step else 1)] - if isinstance(slice_, list): - return [str(i) for i in slice_] + if isinstance(slice_, slice): + return [str(i) for i in range(slice_.start, slice_.stop, + slice_.step if slice_.step else 1)] + if isinstance(slice_, list): + return [str(i) for i in slice_] diff --git a/bindings/pydairlib/franka/BUILD.bazel b/bindings/pydairlib/franka/BUILD.bazel new file mode 100644 index 0000000000..503916512b --- /dev/null +++ b/bindings/pydairlib/franka/BUILD.bazel @@ -0,0 +1,94 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +package(default_visibility = ["//visibility:public"]) + +pybind_py_library( + name = "controllers_py", + cc_deps = [ + "//examples/franka/diagrams:franka_c3_controller_diagram", + "//examples/franka/diagrams:franka_osc_controller_diagram", + "@drake//:drake_shared_library", + ], + cc_so_name = "controllers", + cc_srcs = ["controllers_py.cc"], + py_deps = [ + "@drake//bindings/pydrake", + ":module_py", + ], + py_imports = ["."], +) + +py_binary( + name = "franka_env", + srcs = ["franka_env.py"], + data = [ + "//examples/franka:urdfs", + "@drake_models//:franka_description", + ], + deps = [ + "//bindings/pydairlib/franka", + "//bindings/pydairlib/lcm:lcm_trajectory_py", + "//bindings/pydairlib/systems:framework_py", + "//bindings/pydairlib/systems:primitives_py", + "//bindings/pydairlib/systems:robot_lcm_systems_py", + "//lcmtypes:lcmtypes_robot_py", + "@drake//bindings/pydrake", + ], +) + +py_binary( + name = "generate_dataset", + srcs = ["generate_dataset.py"], + data = [ + ":parameters/dataset_params.yaml", + ], + deps = [ + ":franka_env", + "@drake//bindings/pydrake", + ], +) + +py_binary( + name = "planar_box_example", + srcs = ["planar_box_example.py"], + data = [ + ], + deps = [ + "//bindings/pydairlib/solvers:c3_py", + "@drake//bindings/pydrake", + ], +) + +# This determines how `PYTHONPATH` is configured, and how to install the +# bindings. +PACKAGE_INFO = get_pybind_package_info("//bindings") + +py_library( + name = "module_py", + srcs = [ + "__init__.py", + ], + imports = PACKAGE_INFO.py_imports, + deps = [ + "//bindings/pydairlib:module_py", + ], +) + +PY_LIBRARIES = [ + ":controllers_py", +] + +# Package roll-up (for Bazel dependencies). +py_library( + name = "franka", + imports = PACKAGE_INFO.py_imports, + deps = PY_LIBRARIES, +) diff --git a/bindings/pydairlib/franka/__init__.py b/bindings/pydairlib/franka/__init__.py new file mode 100644 index 0000000000..fa9c628350 --- /dev/null +++ b/bindings/pydairlib/franka/__init__.py @@ -0,0 +1 @@ +# Importing everything in this directory to this package diff --git a/bindings/pydairlib/franka/controllers_py.cc b/bindings/pydairlib/franka/controllers_py.cc new file mode 100644 index 0000000000..074a2fd55f --- /dev/null +++ b/bindings/pydairlib/franka/controllers_py.cc @@ -0,0 +1,113 @@ +#include +#include +#include + +#include "examples/franka/diagrams/franka_c3_controller_diagram.h" +#include "examples/franka/diagrams/franka_osc_controller_diagram.h" +#include "solvers/c3_options.h" + +namespace py = pybind11; + +namespace dairlib { +namespace pydairlib { + +using examples::controllers::FrankaC3ControllerDiagram; +using examples::controllers::FrankaOSCControllerDiagram; + +PYBIND11_MODULE(controllers, m) { + m.doc() = "Binding controller factories for plate balancing demo"; + + using py_rvp = py::return_value_policy; + py::module::import("pydrake.math"); + + py::class_>( + m, "FrankaOSCControllerDiagram") + .def(py::init(), + py::arg("controller_settings"), py::arg("lcm_channels"), + py::arg("lcm")) + .def("get_input_port_robot_state", + &FrankaOSCControllerDiagram::get_input_port_robot_state, + py_rvp::reference_internal) + .def("get_input_port_end_effector_position", + &FrankaOSCControllerDiagram::get_input_port_end_effector_position, + py_rvp::reference_internal) + .def("get_input_port_end_effector_orientation", + &FrankaOSCControllerDiagram::get_input_port_end_effector_orientation, + py_rvp::reference_internal) + .def("get_input_port_end_effector_force", + &FrankaOSCControllerDiagram::get_input_port_end_effector_force, + py_rvp::reference_internal) + .def("get_input_port_radio", + &FrankaOSCControllerDiagram::get_input_port_radio, + py_rvp::reference_internal) + .def("get_output_port_robot_input", + &FrankaOSCControllerDiagram::get_output_port_robot_input, + py_rvp::reference_internal); + + py::class_>( + m, "FrankaC3ControllerDiagram") + .def(py::init(), + py::arg("controller_settings"), py::arg("c3_options"), + py::arg("lcm_channels"), py::arg("lcm")) + .def("get_input_port_robot_state", + &FrankaC3ControllerDiagram::get_input_port_robot_state, + py_rvp::reference_internal) + .def("get_input_port_object_state", + &FrankaC3ControllerDiagram::get_input_port_object_state, + py_rvp::reference_internal) + .def("get_input_port_radio", + &FrankaC3ControllerDiagram::get_input_port_radio, + py_rvp::reference_internal) + .def("get_output_port_mpc_plan", + &FrankaC3ControllerDiagram::get_output_port_mpc_plan, + py_rvp::reference_internal); + + py::class_(m, "C3Options") + .def(py::init<>()) + .def_readwrite("admm_iter", &C3Options::admm_iter) + .def_readwrite("rho", &C3Options::rho) + .def_readwrite("rho_scale", &C3Options::rho_scale) + .def_readwrite("num_threads", &C3Options::num_threads) + .def_readwrite("delta_option", &C3Options::delta_option) + .def_readwrite("projection_type", &C3Options::projection_type) + .def_readwrite("contact_model", &C3Options::contact_model) + .def_readwrite("warm_start", &C3Options::warm_start) + .def_readwrite("use_predicted_x0", &C3Options::use_predicted_x0) + .def_readwrite("solve_time_filter_alpha", &C3Options::solve_time_filter_alpha) + .def_readwrite("publish_frequency", &C3Options::publish_frequency) + .def_readwrite("world_x_limits", &C3Options::workspace_limits) + .def_readwrite("u_horizontal_limits", &C3Options::u_horizontal_limits) + .def_readwrite("u_vertical_limits", &C3Options::u_vertical_limits) + .def_readwrite("workspace_margins", &C3Options::workspace_margins) + .def_readwrite("N", &C3Options::N) + .def_readwrite("gamma", &C3Options::gamma) + .def_readwrite("q_vector", &C3Options::q_vector) + .def_readwrite("r_vector", &C3Options::r_vector) + .def_readwrite("g_vector", &C3Options::g_vector) + .def_readwrite("g_x", &C3Options::g_x) + .def_readwrite("g_gamma", &C3Options::g_gamma) + .def_readwrite("g_lambda_n", &C3Options::g_lambda_n) + .def_readwrite("g_lambda_t", &C3Options::g_lambda_t) + .def_readwrite("g_lambda", &C3Options::g_lambda) + .def_readwrite("g_u", &C3Options::g_u) + .def_readwrite("u_vector", &C3Options::u_vector) + .def_readwrite("u_x", &C3Options::u_x) + .def_readwrite("u_gamma", &C3Options::u_gamma) + .def_readwrite("u_lambda_n", &C3Options::u_lambda_n) + .def_readwrite("u_lambda_t", &C3Options::u_lambda_t) + .def_readwrite("u_lambda", &C3Options::u_lambda) + .def_readwrite("u_u", &C3Options::u_u) + .def_readwrite("mu", &C3Options::mu) + .def_readwrite("dt", &C3Options::dt) + .def_readwrite("solve_dt", &C3Options::solve_dt) + .def_readwrite("num_friction_directions", &C3Options::num_friction_directions) + .def_readwrite("num_contacts", &C3Options::num_contacts) + .def_readwrite("Q", &C3Options::Q) + .def_readwrite("R", &C3Options::R) + .def_readwrite("G", &C3Options::G) + .def_readwrite("U", &C3Options::U); +} +} // namespace pydairlib +} // namespace dairlib \ No newline at end of file diff --git a/bindings/pydairlib/franka/franka_env.py b/bindings/pydairlib/franka/franka_env.py new file mode 100644 index 0000000000..da50f5e476 --- /dev/null +++ b/bindings/pydairlib/franka/franka_env.py @@ -0,0 +1,144 @@ +from pydrake.all import * +from pydairlib.franka.controllers import FrankaOSCControllerDiagram, FrankaC3ControllerDiagram, C3Options + +from pydrake.common.yaml import yaml_load +from pydairlib.systems.primitives import * +from pydairlib.systems.robot_lcm_systems import * +from pydairlib.lcm.lcm_trajectory import * +import pydrake.systems.lcm as mut +from franka_env_helper_functions import * +import dairlib + + +def run_sim(intrinsics, gains): + osc_params_file = "examples/franka/parameters/franka_osc_controller_params.yaml" + c3_params_file = "examples/franka/parameters/franka_c3_controller_params.yaml" + lcm_params_file = "examples/franka/parameters/lcm_channels_simulation.yaml" + sim_params_file = "examples/franka/parameters/franka_sim_params.yaml" + + sim_params = yaml_load(filename=sim_params_file) + lcm_params = yaml_load(filename=lcm_params_file) + c3_options = load_c3_options(c3_params_file) + + lcm = DrakeLcm("udpm://239.255.76.67:7667?ttl=0") + builder = DiagramBuilder() + plant, scene_graph = AddMultibodyPlantSceneGraph(builder, sim_params['dt']) + + parser = Parser(plant) + parser.SetAutoRenaming(True) + + franka_index, = parser.AddModels(FindResourceOrThrow(sim_params['franka_model'])) + end_effector_index, = parser.AddModels(sim_params['end_effector_model']) + tray_index, = parser.AddModels(sim_params['tray_model']) + + T_X_W = RigidTransform(RotationMatrix(), np.zeros(3)) + T_EE_W = RigidTransform(RotationMatrix(), sim_params['tool_attachment_frame']) + + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName('panda_link0'), T_X_W) + frame = plant.GetFrameByName(name='plate', model_instance=end_effector_index) + plant.WeldFrames(plant.GetFrameByName('panda_link7'), + frame, + T_EE_W) + left_support_index, = parser.AddModels(sim_params['left_support_model']) + right_support_index, = parser.AddModels(sim_params['right_support_model']) + T_S1_W = RigidTransform(RollPitchYaw(sim_params['left_support_orientation']), + sim_params['left_support_position']) + T_S2_W = RigidTransform(RollPitchYaw(sim_params['right_support_orientation']), + sim_params['right_support_position']) + plant.WeldFrames(plant.world_frame(), + plant.GetFrameByName("support", left_support_index), + T_S1_W) + plant.WeldFrames(plant.world_frame(), + plant.GetFrameByName("support", right_support_index), + T_S2_W) + plant.Finalize() + + c3_options.publish_frequency = (gains[0] * 5) + 25 + c3_options.Q = (1 + 0.5 * gains[1]) * c3_options.Q + c3_options.R = (1 + 0.5 * gains[2]) * c3_options.R + c3_options.G = (1 + 0.5 * gains[3]) * c3_options.G + c3_options.U = (1 + 0.5 * gains[4]) * c3_options.U + + osc_controller = builder.AddSystem(FrankaOSCControllerDiagram( + "examples/franka/parameters/franka_osc_controller_params.yaml", + "examples/franka/parameters/lcm_channels_simulation.yaml", lcm)) + + c3_controller = builder.AddSystem(FrankaC3ControllerDiagram( + "examples/franka/parameters/franka_c3_controller_params.yaml", c3_options, + "examples/franka/parameters/lcm_channels_simulation.yaml", lcm)) + + passthrough = builder.AddSystem(SubvectorPassThrough(8, 0, 7)) + tray_state_sender = builder.AddSystem(ObjectStateSender(plant, tray_index)) + franka_state_sender = builder.AddSystem(RobotOutputSender(plant, franka_index, False, False)) + + # radio_sub = builder.AddSystem(LcmSubscriberSystem.Make(channel=lcm_params['radio_channel'], lcm_type=dairlib.lcmt_radio_out, lcm=lcm, use_cpp_serializer=True)) + sim_state_logger = builder.AddSystem( + VectorLogSink(plant.num_positions() + plant.num_velocities(), 2 * sim_params['dt'])) + + builder.Connect(c3_controller.get_output_port_mpc_plan(), + osc_controller.get_input_port_end_effector_position()) + builder.Connect(c3_controller.get_output_port_mpc_plan(), + osc_controller.get_input_port_end_effector_orientation()) + builder.Connect(c3_controller.get_output_port_mpc_plan(), + osc_controller.get_input_port_end_effector_force()) + + builder.Connect(osc_controller.get_output_port_robot_input(), + passthrough.get_input_port()) + + builder.Connect(franka_state_sender.get_output_port(), + osc_controller.get_input_port_robot_state()) + builder.Connect(franka_state_sender.get_output_port(), + c3_controller.get_input_port_robot_state()) + builder.Connect(tray_state_sender.get_output_port(), + c3_controller.get_input_port_object_state()) + + builder.Connect(plant.get_state_output_port(franka_index), + franka_state_sender.get_input_port_state()) + builder.Connect(plant.get_state_output_port(tray_index), + tray_state_sender.get_input_port_state()) + builder.Connect(plant.get_state_output_port(), + sim_state_logger.get_input_port()) + builder.Connect(passthrough.get_output_port(), + plant.get_actuation_input_port()) + + if sim_params['visualize_drake_sim']: + AddDefaultVisualization(builder) + + diagram = builder.Build() + simulator = Simulator(diagram) + + simulator.set_publish_every_time_step(True) + simulator.set_publish_at_initialization(True) + simulator.set_target_realtime_rate(sim_params['realtime_rate']) + + plant_context = diagram.GetMutableSubsystemContext(plant, simulator.get_mutable_context()) + osc_controller_context = diagram.GetMutableSubsystemContext(osc_controller, simulator.get_mutable_context()) + c3_controller_context = diagram.GetMutableSubsystemContext(c3_controller, simulator.get_mutable_context()) + osc_controller.get_input_port_radio().FixValue(osc_controller_context, np.zeros(18)) + c3_controller.get_input_port_radio().FixValue(c3_controller_context, np.zeros(18)) + logger_context = diagram.GetSubsystemContext(sim_state_logger, simulator.get_context()) + + q = np.hstack((sim_params['q_init_franka'], sim_params['q_init_tray'][sim_params['scene_index']])) + v = np.zeros(plant.num_velocities()) + plant.SetPositions(plant_context, q) + + simulator.Initialize() + simulator.AdvanceTo(5.0) + + sim_data = sim_state_logger.GetLog(logger_context) + sim_states = sim_data.data() + sim_times = sim_data.sample_times() + + # reward = compute_reward_sparse(sim_times, sim_states) + reward = compute_reward_mpc(sim_times, sim_states) + # print(reward) + # print(sim_states.data().shape) + # print(sim_states.sample_times().shape) + return reward, sim_states + + +if __name__ == '__main__': + parameters = 2 * np.random.random(5) - 1 # center around 0. [-1, 1] + # reward, sim_states = run_sim(np.zeros(5), np.ones(5)) + reward, sim_states = run_sim(np.zeros(5), parameters) + print(reward) diff --git a/bindings/pydairlib/franka/franka_env_helper_functions.py b/bindings/pydairlib/franka/franka_env_helper_functions.py new file mode 100644 index 0000000000..ce01e8fff3 --- /dev/null +++ b/bindings/pydairlib/franka/franka_env_helper_functions.py @@ -0,0 +1,85 @@ +from pydairlib.franka.controllers import C3Options +import numpy as np +from pydrake.common.yaml import yaml_load +from pydrake.all import * + +def compute_reward_sparse(times, sim_states): + first_target = np.array([0.45, 0, 0.485]) + second_target = np.array([0.45, 0, 0.6]) + third_target = np.array([0.7, 0.0, 0.485]) + length = min(times.shape[0], times.shape[0]) + reached_first_target = np.any(np.all(np.isclose(sim_states[:, 7:10], second_target, atol=1e-3), axis=1)) + reached_second_target = np.any(np.all(np.isclose(sim_states[:, 7:10], third_target, atol=1e-3), axis=1)) + reached_third_target = reached_second_target and np.linalg.norm(sim_states[:length, 7:10] - sim_states[:length, 7:10], axis=1)[-1] < 0.1 + # return reached_first_target + reached_second_target + reached_third_target + return reached_first_target + reached_second_target + reached_third_target + +def compute_reward_mpc(times, sim_states): + first_target = np.array([0.45, 0, 0.485]) + second_target = np.array([0.45, 0, 0.6]) + third_target = np.array([0.7, 0.0, 0.485]) + neutral_orientation = Quaternion() + cumulative_cost = 0 + for i in range(times.shape[0]): + tray_pos = sim_states[11:14, i] + tray_orientation = sim_states[7:11, i] + tray_quat = Quaternion(tray_orientation / np.linalg.norm(tray_orientation)) + cumulative_cost += np.linalg.norm(tray_pos - first_target) + # tray_quat /= np.linalg.norm(tray_quat.wxyz()) + angle_difference = Quaternion.multiply(neutral_orientation.conjugate(), tray_quat) + cumulative_cost += RotationMatrix(angle_difference).ToAngleAxis().angle() + cumulative_cost /= times.shape[0] + return cumulative_cost + +def load_c3_options(c3_params_file): + c3_params = yaml_load(filename=c3_params_file) + c3_options_file = c3_params['c3_options_file'][c3_params['scene_index']] + c3_options_dict = yaml_load(filename=c3_options_file) + c3_options = C3Options() + c3_options.admm_iter = c3_options_dict['admm_iter'] + c3_options.rho = c3_options_dict['rho'] + c3_options.rho_scale = c3_options_dict['rho_scale'] + c3_options.num_threads = c3_options_dict['num_threads'] + c3_options.delta_option = c3_options_dict['delta_option'] + c3_options.projection_type = c3_options_dict['projection_type'] + c3_options.contact_model = c3_options_dict['contact_model'] + c3_options.warm_start = c3_options_dict['warm_start'] + c3_options.use_predicted_x0 = c3_options_dict['use_predicted_x0'] + c3_options.solve_time_filter_alpha = c3_options_dict['solve_time_filter_alpha'] + c3_options.publish_frequency = c3_options_dict['publish_frequency'] + c3_options.world_x_limits = c3_options_dict['world_x_limits'] + c3_options.world_y_limits = c3_options_dict['world_y_limits'] + c3_options.world_z_limits = c3_options_dict['world_z_limits'] + c3_options.u_horizontal_limits = c3_options_dict['u_horizontal_limits'] + c3_options.u_vertical_limits = c3_options_dict['u_vertical_limits'] + c3_options.workspace_margins = c3_options_dict['workspace_margins'] + c3_options.N = c3_options_dict['N'] + c3_options.gamma = c3_options_dict['gamma'] + c3_options.gamma = c3_options_dict['gamma'] + c3_options.q_vector = c3_options_dict['q_vector'] + c3_options.r_vector = c3_options_dict['r_vector'] + c3_options.g_x = c3_options_dict['g_x'] + c3_options.g_gamma = c3_options_dict['g_gamma'] + c3_options.g_lambda_n = c3_options_dict['g_lambda_n'] + c3_options.g_lambda_t = c3_options_dict['g_lambda_t'] + c3_options.g_lambda = c3_options_dict['g_lambda'] + c3_options.g_u = c3_options_dict['g_u'] + c3_options.u_x = c3_options_dict['u_x'] + c3_options.u_gamma = c3_options_dict['u_gamma'] + c3_options.u_lambda_n = c3_options_dict['u_lambda_n'] + c3_options.u_lambda_t = c3_options_dict['u_lambda_t'] + c3_options.u_lambda = c3_options_dict['u_lambda'] + c3_options.u_u = c3_options_dict['u_u'] + c3_options.gamma = c3_options_dict['gamma'] + c3_options.mu = c3_options_dict['mu'] + c3_options.dt = c3_options_dict['dt'] + c3_options.solve_dt = c3_options_dict['solve_dt'] + c3_options.num_friction_directions = c3_options_dict['num_friction_directions'] + c3_options.num_contacts = c3_options_dict['num_contacts'] + c3_options.Q = c3_options_dict['w_Q'] * np.diag(np.array(c3_options_dict['q_vector'])) + c3_options.R = c3_options_dict['w_R'] * np.diag(np.array(c3_options_dict['r_vector'])) + g_vec = np.hstack((c3_options_dict['g_x'], c3_options_dict['g_lambda'], c3_options_dict['g_u'])) + u_vec = np.hstack((c3_options_dict['u_x'], c3_options_dict['u_lambda'], c3_options_dict['u_u'])) + c3_options.G = c3_options_dict['w_G'] * np.diag(g_vec) + c3_options.U = c3_options_dict['w_U'] * np.diag(u_vec) + return c3_options diff --git a/bindings/pydairlib/franka/generate_dataset.py b/bindings/pydairlib/franka/generate_dataset.py new file mode 100644 index 0000000000..f892686be0 --- /dev/null +++ b/bindings/pydairlib/franka/generate_dataset.py @@ -0,0 +1,53 @@ +from franka_env import * +from yaml import * +from datetime import datetime +import h5py + +# gains are controller params +# thetas/intrinsics are environment params +def generate_dataset(n_datapoints, + batch_size, + gains_to_randomize, + thetas_to_randomize, + high_level_folder): + assert n_datapoints == int(n_datapoints / batch_size) * batch_size + + num_batches = int(n_datapoints / (batch_size)) + # Each batch is associated with a single set of system intrinsic parameters + intrinsics = np.zeros((num_batches, len(thetas_to_randomize))) + # for each parameter vector, generate $batch_size random gains to evaluate + gains_to_test = np.zeros((num_batches, batch_size, len(gains_to_randomize))) + + subfolder = "occam_data" + datetime.now().strftime("%b_%d_%Y_%H%M") + "/" + if not os.path.exists(os.path.join(high_level_folder, subfolder)): + os.makedirs(os.path.join(high_level_folder, subfolder), exist_ok=True) + + with h5py.File(os.path.join(high_level_folder, subfolder, "dataset.hdf5"), 'w') as f: + # intrinsic vectors don't actually get used anywhere, but good to save for bookkeeping + f.create_dataset("intrinsics", shape=intrinsics.shape) + f.create_dataset("gains", shape=gains_to_test.shape) + # this system only has 1 performance metric, but in principle this could be any number + f.create_dataset("metrics", shape=(num_batches, batch_size, 1)) + # f.create_dataset("states", shape=(num_batches, batch_size, 1)) + for batch in range(num_batches): + # Generate a random intrinsic vector for this batch + # batch_intrinsic_obj = params_t.generate_random(thetas_to_randomize) + f["intrinsics"][batch, ...] = thetas_to_randomize + # robot = Branin(batch_intrinsic_obj, gains_to_randomize) + for point in range(batch_size): + # generate a random gain vector + # gain = BraninInputs.generate_random(gains_to_randomize).get_list()[gains_to_randomize] + gain = 2 * np.random.random(gains_to_randomize.shape[0]) - 1 # center around 0. [-1, 1] + # run the simulation and obtain the performance metrics + # metric = robot.evaluate_x(gain) + metric, states = run_sim(thetas_to_randomize, gain) + f["metrics"][batch, point] = metric + f["gains"][batch, point] = gain + # f["states"][batch, point] = states + print(f"finished batch {batch}") + + +if __name__ == '__main__': + hersh_dataset_params_file = 'bindings/pydairlib/franka/parameters/dataset_params.yaml' + params = yaml_load(filename=hersh_dataset_params_file) + generate_dataset(50 * 64, 64, np.ones(5), np.zeros(5), params['high_level_folder']) diff --git a/bindings/pydairlib/franka/parameters/dataset_params.yaml b/bindings/pydairlib/franka/parameters/dataset_params.yaml new file mode 100644 index 0000000000..77293d3cb3 --- /dev/null +++ b/bindings/pydairlib/franka/parameters/dataset_params.yaml @@ -0,0 +1 @@ +high_level_folder: /media/yangwill/backups/home/yangwill/Documents/research/projects/franka/occam/ diff --git a/bindings/pydairlib/franka/planar_box_example.py b/bindings/pydairlib/franka/planar_box_example.py new file mode 100644 index 0000000000..e3036ee1ef --- /dev/null +++ b/bindings/pydairlib/franka/planar_box_example.py @@ -0,0 +1,229 @@ +import numpy as np +from pydrake.all import ( + AddMultibodyPlantSceneGraph, + ContactVisualizer, + DiagramBuilder, + ExternallyAppliedSpatialForce, + LeafSystem, + List, + MeshcatVisualizer, + ModelVisualizer, + Parser, + Simulator, + SpatialForce, + StartMeshcat, + RigidTransform, + Value, + InitializeAutoDiff, + ModelInstanceIndex, + PiecewisePolynomial, +) + +from c3 import * + + +class Controller(LeafSystem): + def __init__(self, plant, plant_context): + LeafSystem.__init__(self) + forces_cls = Value[List[ExternallyAppliedSpatialForce]] + self.DeclareVectorOutputPort( + "controller_output", 1, self.CalcOutput + ) + self.DeclareVectorInputPort("system_state", plant.num_positions() + plant.num_velocities()) + self.plant = plant + self.plant_context = plant_context + self.c3_options = C3Options() + + lcs_builer = DiagramBuilder() + self.plant_for_lcs, self.scene_graph_for_lcs = AddMultibodyPlantSceneGraph( + lcs_builer, 0.0) + lcs_parser = Parser(self.plant_for_lcs) + self.passive_block_index = lcs_parser.AddModels( + "bindings/pydairlib/franka/urdf/passive_block_lcs.sdf")[0] + self.active_block_index = \ + lcs_parser.AddModels("bindings/pydairlib/franka/urdf/active_block.sdf")[ + 0] + self.plant_for_lcs.WeldFrames(self.plant_for_lcs.world_frame(), + self.plant_for_lcs.GetFrameByName("base", + self.passive_block_index)) + self.plant_for_lcs.Finalize() + + lcs_diagram = lcs_builer.Build() + + diagram_context = lcs_diagram.CreateDefaultContext() + self.context_for_lcs = lcs_diagram.GetMutableSubsystemContext( + self.plant_for_lcs, diagram_context) + self.plant_ad = self.plant_for_lcs.ToAutoDiffXd() + self.context_ad = self.plant_ad.CreateDefaultContext() + + passive_block_contact_points = self.plant_for_lcs.GetCollisionGeometriesForBody( + self.plant_for_lcs.GetBodyByName("passive_block", + self.passive_block_index)) + active_block_contact_points = self.plant_for_lcs.GetCollisionGeometriesForBody( + self.plant_for_lcs.GetBodyByName("active_block", + self.active_block_index)) + self.contact_geoms = list() + for geom_id in passive_block_contact_points: + self.contact_geoms.append((active_block_contact_points[0], geom_id)) + self.num_friction_directions = 1 + self.mu = [0.4, 0.4] + self.dt = 0.05 + self.N = 10 + self.contact_model = ContactModel.kAnitescu + # self.contact_model = ContactModel.kStewartAndTrinkle + self.Q = 50 * np.diag(np.array([1000, 5000, 10, 10, 5, 10, 5, 5])) + self.R = 5 * np.eye(1) + self.G = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 100 * np.ones(4), 5 * np.ones(1)))) + self.U = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 100 * np.ones(4), 5 * np.ones(1)))) + # self.G = 0.1 * np.diag(np.ones(13)) + # self.U = 0.1 * np.diag(np.ones(13)) + + # self.U = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 0.01 * np.ones(8), 5 * np.ones(1)))) + # self.G = 0.1 * np.diag(np.hstack((np.ones(4), 1 * np.ones(4), 0.01 * np.ones(8), 5 * np.ones(1)))) + self.c3_options.admm_iter = 2 + self.c3_options.rho = 0 + self.c3_options.rho_scale = 2 + self.c3_options.num_threads = 4 + self.c3_options.delta_option = 1 + self.c3_options.contact_model = "anitescu" + # self.c3_options.contact_model = "stewart_and_trinkle" + self.c3_options.warm_start = 1 + self.c3_options.use_predicted_x0 = 0 + self.c3_options.end_on_qp_step = 0 + self.c3_options.use_robust_formulation = 0 + self.c3_options.solve_time_filter_alpha = 0.95 + self.c3_options.publish_frequency = 0 + self.c3_options.u_horizontal_limits = np.array([-5, 5]) + self.c3_options.u_vertical_limits = np.array([0, 0]) + self.c3_options.workspace_limits = [np.array([1, 0, 0, -0.5, 0.5])] # unused + self.c3_options.workspace_margins = 0.05 + self.c3_options.N = self.N + self.c3_options.gamma = 1.0 + self.c3_options.mu = self.mu + self.c3_options.dt = self.dt + self.c3_options.solve_dt = 0.0 + self.c3_options.num_friction_directions = self.num_friction_directions + self.c3_options.num_contacts = len(self.mu) + self.c3_options.Q = self.Q + self.c3_options.R = self.R + self.c3_options.G = self.G + self.c3_options.U = self.U + self.u = PiecewisePolynomial(np.array([0])) + self.last_update_time = -1 + self.c3_solver = None + self.x_des = np.array([0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.predicted_x1 = self.x_des + self.lcs_pred = self.x_des + Qs = [] + for i in range(self.N + 1): + Qs.append(1.0 ** i * self.Q) + # costs = CostMatrices((self.N + 1) * [self.Q], self.N * [self.R], self.N * [self.G], self.N * [self.U]) + self.costs = CostMatrices(Qs, self.N * [self.R], self.N * [self.G], self.N * [self.U]) + + def CalcOutput(self, context, output): + + + if context.get_time() > self.last_update_time + (0.1 * self.dt): + x = self.EvalVectorInput(context, 0) + x0 = x.value() + # u0 = np.zeros(1) + u0 = self.u.value(context.get_time())[0] + x_u = np.hstack((x.value(), u0)) + x_u_ad = InitializeAutoDiff(x_u) + + if context.get_time() // 10 % 2 == 0: + self.x_des = np.array([0.0, -0.3, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0]) + else: + self.x_des = np.array([0.0, 0.3, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0]) + print("time: ", context.get_time()) + print("current cost: ", (x.value() - self.x_des).T @ (x.value() - self.x_des)) + print("desired x: ", self.x_des) + print("planned x1: ", self.predicted_x1) + print("lcs pred x1: ", self.lcs_pred) + print("actual x1: ", x.value()) + # import pdb; pdb.set_trace() + self.plant_for_lcs.SetPositionsAndVelocities(self.context_for_lcs, x0) + self.plant_for_lcs.get_actuation_input_port().FixValue(self.context_for_lcs, u0) + self.plant_ad.get_actuation_input_port().FixValue(self.context_ad, x_u_ad[-1]) + lcs = LinearizePlantToLCS(self.plant_for_lcs, self.context_for_lcs, + self.plant_ad, self.context_ad, self.contact_geoms, + self.num_friction_directions, self.mu, self.dt, self.N, + self.contact_model) + + if self.c3_solver == None: + self.c3_solver = C3MIQP(lcs, self.costs, (self.N + 1) * [self.x_des], self.c3_options) + else: + self.c3_solver.UpdateLCS(lcs) + self.c3_solver.UpdateTarget((self.N + 1) * [self.x_des]) + self.c3_solver.Solve(x0) + u_sol = self.c3_solver.GetInputSolution() + x_sol = self.c3_solver.GetStateSolution() + w_sol = self.c3_solver.GetDualWSolution() + delta_sol = self.c3_solver.GetDualDeltaSolution() + z_sol = self.c3_solver.GetFullSolution() + + u_sol = np.array(u_sol) + x_sol = np.array(x_sol) + w_sol = np.array(w_sol) + delta_sol = np.array(delta_sol) + z_sol = np.array(z_sol) + + self.predicted_x1 = x_sol[1, :] + self.lcs_pred = lcs.Simulate(x0, u_sol[0]) + # print(next_pred) + # next_pred = lcs.Simulate(next_pred, u_sol[0]) + # print(next_pred) + # import pdb; pdb.set_trace() + timestamps = context.get_time() + self.dt * np.arange(self.N) + self.u = PiecewisePolynomial.ZeroOrderHold(timestamps, u_sol.T) + # self.u = PiecewisePolynomial.FirstOrderHold(timestamps, np.array(u_sol).T) + self.last_update_time = context.get_time() + output.set_value(self.u.value(context.get_time())) + + +def main(): + # np.set_printoptions(3, threshold=3, suppress=True) + builder = DiagramBuilder() + + plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001) + parser = Parser(plant) + passive_block_index = \ + parser.AddModels("bindings/pydairlib/franka/urdf/passive_block.sdf")[0] + active_block_index = \ + parser.AddModels("bindings/pydairlib/franka/urdf/active_block.sdf")[0] + offset = RigidTransform(np.array([0.0, 0.0, 0.0])) + plant.WeldFrames(plant.world_frame(), + plant.GetFrameByName("base", passive_block_index), offset) + # plant.WeldFrames(plant.world_frame(), + # plant.GetFrameByName("base", active_block_index)) + plant.Finalize() + plant_context = plant.CreateDefaultContext() + meshcat = StartMeshcat() + visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat) + ContactVisualizer.AddToBuilder(builder, plant, meshcat) + controller = builder.AddSystem(Controller(plant, plant_context)) + builder.Connect(controller.get_output_port(), + plant.get_actuation_input_port()) + builder.Connect(plant.get_state_output_port(), + controller.get_input_port()) + diagram = builder.Build() + + meshcat.SetCameraPose(np.array([0, -3.0, 0.5]), np.array([0, 0.0, 0.0])) + simulator = Simulator(diagram) + context = simulator.get_context() + plant.SetPositions(diagram.GetMutableSubsystemContext(plant, + simulator.get_mutable_context()), + np.array([0.0, 0.0, 0.1, 0])) # active x, passive: x, z, theta + simulator.Initialize() + simulator.set_target_realtime_rate(1.0) + meshcat.StartRecording() + simulator.AdvanceTo(40.0) + meshcat.StopRecording() + meshcat.PublishRecording() + with open("planar_box_visualization.html", "r+") as f: + f.write(meshcat.StaticHtml()) + print("done simulating") + + +if __name__ == '__main__': + main() diff --git a/bindings/pydairlib/franka/urdf/active_block.sdf b/bindings/pydairlib/franka/urdf/active_block.sdf new file mode 100644 index 0000000000..0cf5c974ec --- /dev/null +++ b/bindings/pydairlib/franka/urdf/active_block.sdf @@ -0,0 +1,64 @@ + + + + + + + 0 0 0 0 0 0 + 1 + + 0.014167 + 0 + 0 + 0.33417 + 0 + 0.34667 + + + + 0 0 0 0 0 0 + + 0.5 0.2 0.2 0.6 + + + + 2.0 0.4 0.1 + + + + + 0 0 0 0 0 0 + + + 2.0 0.4 0.1 + + + + + 3.0e7 + 0.15 + 10 + 0.4 + + + + + world + active_block + + + 1 0 0 + + + -2.0 + 2.0 + 50.0 + + + 0.0 + + + + + + \ No newline at end of file diff --git a/bindings/pydairlib/franka/urdf/passive_block.sdf b/bindings/pydairlib/franka/urdf/passive_block.sdf new file mode 100644 index 0000000000..a9b43873f8 --- /dev/null +++ b/bindings/pydairlib/franka/urdf/passive_block.sdf @@ -0,0 +1,125 @@ + + + + + + + 0 0 0 0 0 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + 1 + + 0.014167 + 0 + 0 + 0.014167 + 0 + 0.026667 + + + + 0 0 0 0 0 0 + + 0.1 0.1 0.1 0.6 + + + + 0.4 0.4 0.1 + + + + + 0 0 0 0 0 0 + + + 0.4 0.4 0.1 + + + + + 3.0e7 + 0.15 + 10 + 0.4 + + + + + base + base_x + + + 1 0 0 + + + 0.0 + + + + + base_x + base_xz + + + 0 0 1 + + + 0.0 + + + + + base_xz + passive_block + + + 0 1 0 + + + 0.0 + + + + + + \ No newline at end of file diff --git a/bindings/pydairlib/franka/urdf/passive_block_lcs.sdf b/bindings/pydairlib/franka/urdf/passive_block_lcs.sdf new file mode 100644 index 0000000000..e218edb31e --- /dev/null +++ b/bindings/pydairlib/franka/urdf/passive_block_lcs.sdf @@ -0,0 +1,126 @@ + + + + + + + 0 0 0 0 0 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + 1 + + 0.014167 + 0 + 0 + 0.014167 + 0 + 0.026667 + + + + 0 0 0 0 0 0 + + 0.1 0.1 0.1 0.6 + + + + 0.4 0.4 0.1 + + + + + -0.2 0.0 -0.048 0 0 0 + + + 0.001" + + + + + 0.2 0.0 -0.048 0 0 0 + + + 0.001" + + + + + + base + base_x + + + 1 0 0 + + + 0.0 + + + + + base_x + base_xz + + + 0 0 1 + + + 0.0 + + + + + base_xz + passive_block + + + 0 1 0 + + + 0.0 + + + + + + \ No newline at end of file diff --git a/bindings/pydairlib/lcm/BUILD.bazel b/bindings/pydairlib/lcm/BUILD.bazel index 40396665e7..425661d6e4 100644 --- a/bindings/pydairlib/lcm/BUILD.bazel +++ b/bindings/pydairlib/lcm/BUILD.bazel @@ -15,6 +15,7 @@ pybind_py_library( name = "lcm_py", cc_deps = [ "//lcmtypes:lcmt_robot", + "@drake//bindings/pydrake/common:value_pybind", "@drake//bindings/pydrake/systems:lcm_pybind", ], cc_srcs = [ @@ -29,6 +30,14 @@ pybind_py_library( py_imports = ["."], ) +py_library( + name = "process_lcm_log", + srcs = ["process_lcm_log.py"], + data = ["@lcm//:lcm-python"], + deps = [ + ], +) + pybind_py_library( name = "lcm_trajectory_py", cc_deps = [ @@ -42,28 +51,6 @@ pybind_py_library( py_imports = ["."], ) -py_binary( - name = "lcm_trajectory_plotter", - srcs = [":lcm_trajectory_plotter.py"], - deps = [ - ":lcm_trajectory_py", - ":module_py", - "//lcmtypes:lcmtypes_robot_py", - ], -) - -py_binary( - name = "dircon_trajectory_plotter", - srcs = [":dircon_trajectory_plotter.py"], - deps = [ - ":lcm_trajectory_py", - ":module_py", - "//bindings/pydairlib/common", - "//bindings/pydairlib/multibody:multibody_py", - "//lcmtypes:lcmtypes_robot_py", - ], -) - # This determines how `PYTHONPATH` is configured, and how to install the # bindings. PACKAGE_INFO = get_pybind_package_info("//bindings") @@ -80,10 +67,9 @@ py_library( ) PY_LIBRARIES = [ - ":dircon_trajectory_plotter", - ":lcm_trajectory_plotter", ":lcm_trajectory_py", ":lcm_py", + ":process_lcm_log", ] # Package roll-up (for Bazel dependencies). diff --git a/bindings/pydairlib/lcm/__init__.py b/bindings/pydairlib/lcm/__init__.py index 718b93b986..d4f98f5d31 100644 --- a/bindings/pydairlib/lcm/__init__.py +++ b/bindings/pydairlib/lcm/__init__.py @@ -1,2 +1,2 @@ from .lcm_trajectory import * -from .lcm_py import * \ No newline at end of file +from .lcm_py import * diff --git a/bindings/pydairlib/lcm/lcm_py_bind_cpp_serializers.cc b/bindings/pydairlib/lcm/lcm_py_bind_cpp_serializers.cc index f41936d54b..dd08d27678 100644 --- a/bindings/pydairlib/lcm/lcm_py_bind_cpp_serializers.cc +++ b/bindings/pydairlib/lcm/lcm_py_bind_cpp_serializers.cc @@ -8,6 +8,8 @@ #include "dairlib/lcmt_osc_tracking_data.hpp" #include "dairlib/lcmt_robot_input.hpp" #include "dairlib/lcmt_saved_traj.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "dairlib/lcmt_radio_out.hpp" #include "drake/bindings/pydrake/systems/lcm_pybind.h" @@ -25,6 +27,8 @@ void BindCppSerializers() { drake::pydrake::pysystems::pylcm::BindCppSerializer("dairlib"); drake::pydrake::pysystems::pylcm::BindCppSerializer("dairlib"); drake::pydrake::pysystems::pylcm::BindCppSerializer("dairlib"); + drake::pydrake::pysystems::pylcm::BindCppSerializer("dairlib"); + drake::pydrake::pysystems::pylcm::BindCppSerializer("dairlib"); } } // namespace pydairlib diff --git a/bindings/pydairlib/lcm/lcm_trajectory_plotter.py b/bindings/pydairlib/lcm/lcm_trajectory_plotter.py deleted file mode 100644 index 81a562028f..0000000000 --- a/bindings/pydairlib/lcm/lcm_trajectory_plotter.py +++ /dev/null @@ -1,101 +0,0 @@ -import matplotlib.pyplot as plt -from pydairlib.lcm import lcm_trajectory -import numpy as np -from pydrake.trajectories import PiecewisePolynomial - - -def main(): - loadedTrajs = lcm_trajectory.LcmTrajectory() - # loadedTrajs.LoadFromFile( - # "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/jumping_0.15h_0.3d_processed") - # loadedTrajs.LoadFromFile( - # "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/walking_0.16.0_processed") - loadedTrajs.LoadFromFile( - "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/running_0.25_processed") - - lcm_left_foot_traj = loadedTrajs.GetTrajectory("left_foot_trajectory0") - lcm_right_foot_traj = loadedTrajs.GetTrajectory("right_foot_trajectory0") - lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_trans_trajectory0") - # lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_rot_trajectory0") - - # import pdb; pdb.set_trace() - # left_foot_traj = PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[0:6], - # lcm_left_foot_traj.datapoints[3:9]) - # right_foot_traj = PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector, - # lcm_right_foot_traj.datapoints[0:6], - # lcm_right_foot_traj.datapoints[3:9]) - # pelvis_traj = PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[0:6], - # lcm_pelvis_traj.datapoints[3:9]) - x_slice = slice(0,3) - xdot_slice = slice(3,6) - left_foot_traj = PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[x_slice], - lcm_left_foot_traj.datapoints[xdot_slice]) - right_foot_traj = PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector, - lcm_right_foot_traj.datapoints[x_slice], - lcm_right_foot_traj.datapoints[xdot_slice]) - pelvis_traj = PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[x_slice], - lcm_pelvis_traj.datapoints[xdot_slice]) - for mode in range(1, 6): - lcm_left_foot_traj = loadedTrajs.GetTrajectory("left_foot_trajectory" + str(mode)) - lcm_right_foot_traj = loadedTrajs.GetTrajectory("right_foot_trajectory" + str(mode)) - lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_trans_trajectory" + str(mode)) - - left_foot_traj.ConcatenateInTime( - PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[x_slice], - lcm_left_foot_traj.datapoints[xdot_slice])) - right_foot_traj.ConcatenateInTime( - PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints[x_slice], - lcm_right_foot_traj.datapoints[xdot_slice])) - pelvis_traj.ConcatenateInTime( - PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[x_slice], - lcm_pelvis_traj.datapoints[xdot_slice])) - - # plt.figure('accel') - # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,2:3]) - # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,5:6]) - # plt.plot(times, accel[:, -1]) - # plt.figure("left_foot pos") - # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,0:3]) - # plt.legend(['x','y','z']) - # plt.figure("left_foot vel") - # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,3:6]) - # plt.legend(['x','y','z']) - # plt.figure("right_foot pos") - # plt.plot(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints.T[:,0:3]) - # plt.legend(['x','y','z']) - # plt.figure("right_foot vel") - # plt.plot(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints.T[:,3:6]) - # plt.legend(['x','y','z']) - - plot_trajectory(pelvis_traj, 'pelvis') - # plot_trajectory(left_foot_traj, 'left_foot') - # plot_trajectory(right_foot_traj, 'right_foot') - plt.show() - - -def plot_trajectory(traj_of_interest, traj_name): - times = np.arange(traj_of_interest.start_time(), traj_of_interest.end_time(), 0.001) - # times = np.arange(traj_of_interest.start_time(), 0.2, 0.001) - y_dim = 3 - accel = np.zeros((times.shape[0], y_dim)) - pos = np.zeros((times.shape[0], y_dim)) - vel = np.zeros((times.shape[0], y_dim)) - for i in range(times.shape[0]): - pos[i, :] = traj_of_interest.value(times[i])[:, 0] - vel[i, :] = traj_of_interest.EvalDerivative(times[i], 1)[:, 0] - accel[i, :] = traj_of_interest.EvalDerivative(times[i], 2)[:, 0] - - plt.figure(traj_name + "_pos") - plt.plot(times, pos) - plt.legend(['x', 'y', 'z', 'xdot', 'ydot', 'zdot']) - plt.figure(traj_name + "_vel") - plt.plot(times, vel) - plt.legend(['xdot', 'ydot', 'zdot', 'xddot', 'yddot', 'zddot']) - plt.figure(traj_name + "_acc") - plt.plot(times, accel) - plt.legend(['xdot', 'ydot', 'zdot', 'xddot', 'yddot', 'zddot']) - # plt.legend(['pos','vel','accel']) - - -if __name__ == "__main__": - main() diff --git a/bindings/pydairlib/lcm/lcm_trajectory_py.cc b/bindings/pydairlib/lcm/lcm_trajectory_py.cc index 088add2010..ffd5c6cb9b 100644 --- a/bindings/pydairlib/lcm/lcm_trajectory_py.cc +++ b/bindings/pydairlib/lcm/lcm_trajectory_py.cc @@ -66,6 +66,7 @@ PYBIND11_MODULE(lcm_trajectory, m) { .def("GetBreaks", &DirconTrajectory::GetBreaks) .def("GetForceSamples", &DirconTrajectory::GetForceSamples) .def("GetForceBreaks", &DirconTrajectory::GetForceBreaks) + .def("GetImpulseSamples", &DirconTrajectory::GetImpulseSamples) .def("GetCollocationForceSamples", &DirconTrajectory::GetCollocationForceSamples) .def("GetCollocationForceBreaks", @@ -81,7 +82,10 @@ PYBIND11_MODULE(lcm_trajectory, m) { .def("ReconstructLambdaCTrajectory", &DirconTrajectory::ReconstructLambdaCTrajectory) .def("ReconstructGammaCTrajectory", - &DirconTrajectory::ReconstructGammaCTrajectory); + &DirconTrajectory::ReconstructGammaCTrajectory) + .def("ReconstructStateTrajectoryWithSprings", + &DirconTrajectory::ReconstructStateTrajectoryWithSprings, + py::arg("wo_spr_to_spr_map")); } } // namespace pydairlib diff --git a/bindings/pydairlib/analysis/process_lcm_log.py b/bindings/pydairlib/lcm/process_lcm_log.py similarity index 86% rename from bindings/pydairlib/analysis/process_lcm_log.py rename to bindings/pydairlib/lcm/process_lcm_log.py index 6bc8f9584e..0e913b29a0 100644 --- a/bindings/pydairlib/analysis/process_lcm_log.py +++ b/bindings/pydairlib/lcm/process_lcm_log.py @@ -1,4 +1,6 @@ -def get_log_data(lcm_log, lcm_channels, start_time, duration, data_processing_callback, *args, +import lcm + +def get_log_data(lcm_log, lcm_channels, start_time, duration, data_processing_callback, print_info, *args, **kwargs): """ Parses an LCM log and returns data as specified by a callback function @@ -14,14 +16,14 @@ def get_log_data(lcm_log, lcm_channels, start_time, duration, data_processing_ca """ data_to_process = {} - print('Processing LCM log (this may take a while)...') + # print('Processing LCM log (this may take a while)...') lcm_log.seek(0) while lcm_log.read_next_event().channel not in lcm_channels: pass first_timestamp = lcm_log.read_next_event().timestamp start_timestamp = int(first_timestamp + start_time * 1e6) - print('Start time: ' + str(start_time)) - print('Duration: ' + str(duration)) + # print('Start time: ' + str(start_time)) + # print('Duration: ' + str(duration)) lcm_log.seek_to_timestamp(start_timestamp) t = lcm_log.read_next_event().timestamp lcm_log.seek_to_timestamp(start_timestamp) @@ -34,7 +36,7 @@ def get_log_data(lcm_log, lcm_channels, start_time, duration, data_processing_ca else: data_to_process[event.channel] = \ [lcm_channels[event.channel].decode(event.data)] - if event.eventnum % 50000 == 0: + if print_info and event.eventnum % 50000 == 0: print(f'processed {(event.timestamp - t) * 1e-6:.1f}' f' seconds of log data') if 0 < duration <= (event.timestamp - t) * 1e-6: @@ -42,7 +44,6 @@ def get_log_data(lcm_log, lcm_channels, start_time, duration, data_processing_ca event = lcm_log.read_next_event() return data_processing_callback(data_to_process, *args, *kwargs) - def get_log_summary(lcm_log): channel_names_and_msg_counts = {} for event in lcm_log: @@ -64,15 +65,3 @@ def print_log_summary(filename, log): def passthrough_callback(data, *args, **kwargs): return data - -def main(): - import lcm - import sys - - logfile = sys.argv[1] - log = lcm.EventLog(logfile, "r") - print_log_summary(logfile, log) - - -if __name__ == "__main__": - main() diff --git a/bindings/pydairlib/lcm/visualization/BUILD.bazel b/bindings/pydairlib/lcm/visualization/BUILD.bazel new file mode 100644 index 0000000000..8e7de61057 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/BUILD.bazel @@ -0,0 +1,52 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +py_binary( + name = "lcm_trajectory_plotter", + srcs = ["lcm_trajectory_plotter.py"], + deps = [ + "//bindings/pydairlib/lcm", + "//lcmtypes:lcmtypes_robot_py", + ], +) + +py_library( + name = "visualize_params", + srcs = ["visualize_params.py"], + deps = [], +) + +py_binary( + name = "visualize_trajectory", + srcs = ["visualize_trajectory.py"], + deps = [ + ":visualize_params", + "//bindings/pydairlib/cassie:cassie_utils_py", + "//bindings/pydairlib/common", + "//bindings/pydairlib/lcm", + "//bindings/pydairlib/multibody", + "//lcmtypes:lcmtypes_robot_py", + ], +) + +py_binary( + name = "dircon_trajectory_plotter", + srcs = ["dircon_trajectory_plotter.py"], + deps = [ + "//bindings/pydairlib/cassie:cassie_utils_py", + "//bindings/pydairlib/common", + "//bindings/pydairlib/lcm", + "//bindings/pydairlib/multibody:multibody_py", + "//lcmtypes:lcmtypes_robot_py", + ], +) diff --git a/bindings/pydairlib/lcm/visualization/__init__.py b/bindings/pydairlib/lcm/visualization/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/bindings/pydairlib/lcm/dircon_trajectory_plotter.py b/bindings/pydairlib/lcm/visualization/dircon_trajectory_plotter.py similarity index 64% rename from bindings/pydairlib/lcm/dircon_trajectory_plotter.py rename to bindings/pydairlib/lcm/visualization/dircon_trajectory_plotter.py index fd1101bb5e..fc67afa21a 100644 --- a/bindings/pydairlib/lcm/dircon_trajectory_plotter.py +++ b/bindings/pydairlib/lcm/visualization/dircon_trajectory_plotter.py @@ -5,62 +5,95 @@ from pydrake.trajectories import PiecewisePolynomial import numpy as np +from pydrake.all import (DiagramBuilder, AddMultibodyPlantSceneGraph) +from pydairlib.cassie.cassie_utils import AddCassieMultibody def main(): + + builder = DiagramBuilder() + plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) + AddCassieMultibody(plant, scene_graph, + True, "examples/Cassie/urdf/cassie_fixed_springs.urdf", False, True) + + plant.Finalize() # Default filename for the example - # filename = FindResourceOrThrow("examples/Cassie/saved_trajectories/walking_0.16.0") - filename = FindResourceOrThrow("examples/Cassie/saved_trajectories/jumping_0.15h_0.3d") - # filename = "/home/yangwill/Documents/research/projects/cassie/hardware/backup/dair/saved_trajectories/jumping_0.15h_0.3d" + # filename = FindResourceOrThrow("examples/Cassie/saved_trajectories/jumping_0.15h_0.3d") + filename = FindResourceOrThrow( + "examples/Cassie/saved_trajectories/jumping_0.0h_0.6d_4") + # filename = FindResourceOrThrow( + # "examples/Cassie/saved_trajectories/jumping_box_0.5h_0.3d_1") + # filename = FindResourceOrThrow( + # "examples/Cassie/saved_trajectories/jumping_box_0.4h_0.3d_5") # filename = FindResourceOrThrow("examples/Cassie/saved_trajectories/" + sys.argv[1]) - dircon_traj = lcm_trajectory.DirconTrajectory(filename) + dircon_traj = lcm_trajectory.DirconTrajectory(plant, filename) # Reconstructing state and input trajectory as piecewise polynomials state_traj = dircon_traj.ReconstructStateTrajectory() input_traj = dircon_traj.ReconstructInputTrajectory() state_datatypes = dircon_traj.GetTrajectory("state_traj0").datatypes input_datatypes = dircon_traj.GetTrajectory("input_traj").datatypes - force_samples = dircon_traj.GetTrajectory("force_vars0").datapoints + force_samples = dircon_traj.GetTrajectory("force_vars2").datapoints force_t_samples = dircon_traj.GetStateBreaks(0) force_traj = dircon_traj.ReconstructLambdaTrajectory() # force_datatypes = dircon_traj.GetTrajectory("force_vars0").datatypes - force_datatypes = dircon_traj.GetTrajectory("force_vars1").datatypes + force_datatypes = dircon_traj.GetTrajectory("force_vars2").datatypes + # impulse_samples = dircon_traj.GetImpulseSamples(2) collocation_force_points = dircon_traj.GetCollocationForceSamples(0) - import pdb; pdb.set_trace() # M = reflected_joints() # # mirror_traj = lcm_trajectory.Trajectory() # mirror_traj.traj_name = 'mirror_matrix' # mirror_traj.time_vector = np.zeros(M.shape[0]) - # mirror_traj.datapoints = M + # mirror_traj.datapoints = MCc # mirror_traj.datatypes = [''] * M.shape[0] # # dircon_traj.AddTrajectory('mirror_matrix', mirror_traj) # dircon_traj.WriteToFile(filename) + + plot_only_at_knotpoints = False + n_points = 500 t = np.linspace(state_traj.start_time(), state_traj.end_time(), n_points) + if plot_only_at_knotpoints: + n_points = force_t_samples.shape[0] + t = force_t_samples state_samples = np.zeros((n_points, state_traj.value(0).shape[0])) + state_derivative_samples = np.zeros((n_points, state_traj.value(0).shape[0])) input_samples = np.zeros((n_points, input_traj.value(0).shape[0])) # force_samples = np.zeros((n_points, force_traj[0].value(0).shape[0])) for i in range(n_points): state_samples[i] = state_traj.value(t[i])[:, 0] + state_derivative_samples[i] = state_traj.EvalDerivative(t[i], 1)[:, 0] input_samples[i] = input_traj.value(t[i])[:, 0] # force_samples[i] = force_traj[0].value(t[i])[:, 0] # reflected_state_samples = state_samples @ M # Plotting reconstructed state trajectories plt.figure("state trajectory") - plt.plot(t, state_samples[:, 0:7]) + plt.plot(t, state_samples[:, 4:7]) + # plt.plot(t, state_samples[:, 22:25]) # plt.plot(t + state_traj.end_time(), reflected_state_samples[:, 0:7]) # plt.plot(t, state_samples[:, -18:]) # plt.plot(t + state_traj.end_time(), reflected_state_samples[:, 7:13]) # plt.plot(t, state_samples[:, 25:31]) # plt.plot(t + state_traj.end_time(), reflected_state_samples[:, 25:31]) - plt.legend(state_datatypes[0:7]) + plt.legend(state_datatypes[4:7]) + # plt.legend(state_datatypes[22:25]) + + # Velocity Error + # plt.figure('velocity_error') + # import pdb; pdb.set_trace() + # plt.plot(t, (state_samples[:, (19 + 3):] - state_derivative_samples[:, 4:19])) + # plt.plot(t, (state_samples[:, -2:] - state_derivative_samples[:, 21:23])) + # plt.legend(state_datatypes[26:]) + # plt.plot(t, state_samples[:, (23 + 3):]) + plt.figure("input trajectory") + print(np.diff(input_samples[:, 7])) plt.plot(t, input_samples[:, :]) plt.legend(input_datatypes[:]) diff --git a/bindings/pydairlib/lcm/visualization/lcm_trajectory_plotter.py b/bindings/pydairlib/lcm/visualization/lcm_trajectory_plotter.py new file mode 100644 index 0000000000..31738fe3c9 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/lcm_trajectory_plotter.py @@ -0,0 +1,188 @@ +import matplotlib.pyplot as plt +from pydairlib.lcm import lcm_trajectory +import numpy as np +from pydrake.trajectories import PiecewisePolynomial + + +def main(): + loadedTrajs = lcm_trajectory.LcmTrajectory() + # loadedTrajs.LoadFromFile( + # "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/jumping_0.15h_0.3d_processed") + # loadedTrajs.LoadFromFile( + # "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/walking_0.16.0_processed") + loadedTrajs.LoadFromFile( + "/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/running_0.00_processed_rel") + + lcm_left_foot_traj = loadedTrajs.GetTrajectory("left_foot_trajectory0") + lcm_right_foot_traj = loadedTrajs.GetTrajectory("right_foot_trajectory0") + lcm_left_hip_traj = loadedTrajs.GetTrajectory("left_hip_trajectory0") + lcm_right_hip_traj = loadedTrajs.GetTrajectory("right_hip_trajectory0") + lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_trans_trajectory0") + # lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_rot_trajectory0") + + x_slice = slice(0, 6) + xdot_slice = slice(3, 9) + left_foot_traj = PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, + lcm_left_foot_traj.datapoints[x_slice], + lcm_left_foot_traj.datapoints[xdot_slice]) + right_foot_traj = PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector, + lcm_right_foot_traj.datapoints[x_slice], + lcm_right_foot_traj.datapoints[xdot_slice]) + left_hip_traj = PiecewisePolynomial.CubicHermite(lcm_left_hip_traj.time_vector, + lcm_left_hip_traj.datapoints[x_slice], + lcm_left_hip_traj.datapoints[xdot_slice]) + right_hip_traj = PiecewisePolynomial.CubicHermite(lcm_right_hip_traj.time_vector, + lcm_right_hip_traj.datapoints[x_slice], + lcm_right_hip_traj.datapoints[xdot_slice]) + pelvis_traj = PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[x_slice], + lcm_pelvis_traj.datapoints[xdot_slice]) + for mode in range(1, 6): + lcm_left_foot_traj = loadedTrajs.GetTrajectory("left_foot_trajectory" + str(mode)) + lcm_right_foot_traj = loadedTrajs.GetTrajectory("right_foot_trajectory" + str(mode)) + lcm_left_hip_traj = loadedTrajs.GetTrajectory("left_hip_trajectory" + str(mode)) + lcm_right_hip_traj = loadedTrajs.GetTrajectory("right_hip_trajectory" + str(mode)) + lcm_pelvis_traj = loadedTrajs.GetTrajectory("pelvis_trans_trajectory" + str(mode)) + + left_foot_traj.ConcatenateInTime( + PiecewisePolynomial.CubicHermite(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints[x_slice], + lcm_left_foot_traj.datapoints[xdot_slice])) + right_foot_traj.ConcatenateInTime( + PiecewisePolynomial.CubicHermite(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints[x_slice], + lcm_right_foot_traj.datapoints[xdot_slice])) + left_hip_traj.ConcatenateInTime( + PiecewisePolynomial.CubicHermite(lcm_left_hip_traj.time_vector, lcm_left_hip_traj.datapoints[x_slice], + lcm_left_hip_traj.datapoints[xdot_slice])) + right_hip_traj.ConcatenateInTime( + PiecewisePolynomial.CubicHermite(lcm_right_hip_traj.time_vector, lcm_right_hip_traj.datapoints[x_slice], + lcm_right_hip_traj.datapoints[xdot_slice])) + pelvis_traj.ConcatenateInTime( + PiecewisePolynomial.CubicHermite(lcm_pelvis_traj.time_vector, lcm_pelvis_traj.datapoints[x_slice], + lcm_pelvis_traj.datapoints[xdot_slice])) + + # plt.figure('accel') + # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,2:3]) + # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,5:6]) + # plt.plot(times, accel[:, -1]) + # plt.figure("left_foot pos") + # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,0:3]) + # plt.legend(['x','y','z']) + # plt.figure("left_foot vel") + # plt.plot(lcm_left_foot_traj.time_vector, lcm_left_foot_traj.datapoints.T[:,3:6]) + # plt.legend(['x','y','z']) + # plt.figure("right_foot pos") + # plt.plot(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints.T[:,0:3]) + # plt.legend(['x','y','z']) + # plt.figure("right_foot vel") + # plt.plot(lcm_right_foot_traj.time_vector, lcm_right_foot_traj.datapoints.T[:,3:6]) + # plt.legend(['x','y','z']) + + # reconstruct_trajectory(left_foot_traj) + reconstruct_trajectory_left_ft(left_foot_traj) + + # plot_trajectory(pelvis_traj_, 'pelvis') + # plot_trajectory(left_foot_traj - left_hip_traj, 'left_foot') + # plot_trajectory(right_foot_traj, 'right_foot') + plt.show() + + +def plot_trajectory(traj_of_interest, traj_name): + times = np.arange(traj_of_interest.start_time(), traj_of_interest.end_time(), 5e-4) + # times = np.arange(traj_of_interest.start_time(), 0.2, 0.001) + y_dim = 3 + accel = np.zeros((times.shape[0], y_dim)) + pos = np.zeros((times.shape[0], y_dim)) + vel = np.zeros((times.shape[0], y_dim)) + for i in range(times.shape[0]): + pos[i, :] = traj_of_interest.value(times[i])[:3, 0] + vel[i, :] = traj_of_interest.EvalDerivative(times[i], 1)[:3, 0] + accel[i, :] = traj_of_interest.EvalDerivative(times[i], 2)[-3:, 0] + + plt.figure(traj_name + "_pos") + plt.plot(times, pos) + plt.legend(['x', 'y', 'z', 'xdot', 'ydot', 'zdot']) + plt.figure(traj_name + "xz") + plt.plot(pos[:, 0], pos[:, 2]) + plt.legend(['xz']) + plt.figure(traj_name + "yz") + plt.plot(pos[:, 1], pos[:, 2]) + plt.legend(['yz']) + plt.figure(traj_name + "_vel") + plt.plot(times, vel) + plt.legend(['xdot', 'ydot', 'zdot', 'xddot', 'yddot', 'zddot']) + plt.figure(traj_name + "_acc") + plt.plot(times, accel) + plt.legend(['xdot', 'ydot', 'zdot', 'xddot', 'yddot', 'zddot']) + # plt.legend(['pos','vel','accel']) + + +def reconstruct_trajectory(trajectory): + T_waypoints = np.array([0.4, 0.8, 1.0]) + Y = np.zeros((3, 3)) + start_pos = np.array([0, 0, 0]) + end_pos = np.array([0.2, 0.1, 0]) + Y[0] = start_pos + Y[1] = start_pos + 0.85 * (end_pos - start_pos) + Y[1, 2] += 0.05 + Y[2] = end_pos + Y = Y.T + print(Y) + + Ydot_start = np.zeros(3) + Ydot_end = np.zeros(3) + + # traj = PiecewisePolynomial.CubicShapePreserving(T_waypoints, Y, True) + # traj = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(T_waypoints, Y, Ydot_start, Ydot_end) + traj = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(T_waypoints, Y, Ydot_start, Ydot_end) + + plot_trajectory(traj, "reconstructed_trajectory") + return traj + + +def reconstruct_trajectory_left_ft(trajectory): + ref = reconstruct_trajectory(trajectory) + + T_waypoints_0 = np.array([0.0, 0.3, 0.5]) + T_waypoints_midpoint = np.array([0.5, 0.9]) + T_waypoints_1 = np.array([0.9, 1.0]) + Y = np.zeros((3, 3)) + + start_pos = ref.value(0.5)[:, 0] + mid_pos = ref.value(0.8)[:, 0] + # end_pos = np.array([0.2, 0.1, 0]) + end_pos = ref.value(1.0)[:, 0] + Y[0] = start_pos + Y[1] = mid_pos + Y[2] = end_pos + Y = Y.T + print(Y) + + segment_0_start = ref.value(0.5)[:, 0] + segment_0_midpoint = ref.value(0.8)[:, 0] + segment_0_end = ref.value(1.0)[:, 0] + d_segment_0_start = ref.EvalDerivative(0.5, 1)[:, 0] + d_segment_0_midpoint = ref.EvalDerivative(0.8, 1)[:, 0] + d_segment_0_end = ref.EvalDerivative(1.0, 1)[:, 0] + segment_1_start = ref.value(0.4)[:, 0] + segment_1_end = ref.value(0.5)[:, 0] + d_segment_1_start = ref.EvalDerivative(0.4, 1)[:, 0] + d_segment_1_end = ref.EvalDerivative(0.5, 1)[:, 0] + + Ydot_start = np.zeros(3) + Ydot_end = np.zeros(3) + + print(np.array([end_pos, segment_1_start])) + # traj = PiecewisePolynomial.CubicShapePreserving(T_waypoints, Y, True) + traj_0 = PiecewisePolynomial.CubicHermite(T_waypoints_0, + np.array([segment_0_start, segment_0_midpoint, segment_0_end]).T, + np.array([d_segment_0_start, d_segment_0_midpoint, d_segment_0_end]).T) + traj_midpoint = PiecewisePolynomial.ZeroOrderHold(T_waypoints_midpoint, np.array([end_pos, end_pos]).T) + traj_1 = PiecewisePolynomial.CubicHermite(T_waypoints_1, np.array([segment_1_start, segment_1_end]).T, + np.array([d_segment_1_start, d_segment_1_end]).T) + + traj_0.ConcatenateInTime(traj_midpoint) + traj_0.ConcatenateInTime(traj_1) + plot_trajectory(traj_0, "reconstructed_trajectory_left") + + +if __name__ == "__main__": + main() diff --git a/bindings/pydairlib/lcm/visualization/visualize_configs/box_jump.yaml b/bindings/pydairlib/lcm/visualization/visualize_configs/box_jump.yaml new file mode 100644 index 0000000000..3a18a9c795 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/visualize_configs/box_jump.yaml @@ -0,0 +1,8 @@ +filename: examples/Cassie/saved_trajectories/box_jump +spring_urdf: examples/Cassie/urdf/cassie_v2_shells.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs.urdf +visualize_mode: 2 +realtime_rate: 0.5 +num_poses: 1 +use_transparency: 1 +use_springs: 1ss \ No newline at end of file diff --git a/bindings/pydairlib/lcm/visualization/visualize_configs/down_jump.yaml b/bindings/pydairlib/lcm/visualization/visualize_configs/down_jump.yaml new file mode 100644 index 0000000000..ef9ceccbae --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/visualize_configs/down_jump.yaml @@ -0,0 +1,8 @@ +filename: examples/Cassie/saved_trajectories/down_jump +spring_urdf: examples/Cassie/urdf/cassie_v2_shells.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs.urdf +visualize_mode: 1 +realtime_rate: 0.5 +num_poses: 9 +use_transparency: 1 +use_springs: 1 \ No newline at end of file diff --git a/bindings/pydairlib/lcm/visualization/visualize_configs/jump.yaml b/bindings/pydairlib/lcm/visualization/visualize_configs/jump.yaml new file mode 100644 index 0000000000..1a15588279 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/visualize_configs/jump.yaml @@ -0,0 +1,8 @@ +filename: examples/Cassie/saved_trajectories/jump +spring_urdf: examples/Cassie/urdf/cassie_v2_shells.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs.urdf +visualize_mode: 1 +realtime_rate: 0.5 +num_poses: 5 +use_transparency: 1 +use_springs: 1 \ No newline at end of file diff --git a/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml b/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml new file mode 100644 index 0000000000..1c76ce8490 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml @@ -0,0 +1,8 @@ +filename: examples/Cassie/saved_trajectories/long_jump_conservative +spring_urdf: examples/Cassie/urdf/cassie_v2_shells.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs.urdf +visualize_mode: 1 +realtime_rate: 0.5 +num_poses: 5 +use_transparency: 1 +use_springs: 1 \ No newline at end of file diff --git a/bindings/pydairlib/lcm/visualization/visualize_params.py b/bindings/pydairlib/lcm/visualization/visualize_params.py new file mode 100644 index 0000000000..e7e7301107 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/visualize_params.py @@ -0,0 +1,21 @@ +import io +from yaml import load, dump + +try: + from yaml import CLoader as Loader, CDumper as Dumper +except ImportError: + from yaml import Loader, Dumper + + +class DirconVisualizationParams(): + def __init__(self, filename): + data = load(io.open(filename, 'r'), Loader=Loader) + self.filename = data['filename'] + self.spring_urdf = data['spring_urdf'] + self.fixed_spring_urdf = data['fixed_spring_urdf'] + self.visualize_mode = data['visualize_mode'] + self.realtime_rate = data['realtime_rate'] + self.num_poses = data['num_poses'] + self.use_transparency = data['use_transparency'] + self.use_springs = data['use_springs'] + diff --git a/bindings/pydairlib/lcm/visualization/visualize_trajectory.py b/bindings/pydairlib/lcm/visualization/visualize_trajectory.py new file mode 100644 index 0000000000..3827d2dc02 --- /dev/null +++ b/bindings/pydairlib/lcm/visualization/visualize_trajectory.py @@ -0,0 +1,126 @@ +import sys +import matplotlib.pyplot as plt +from pydairlib.lcm import lcm_trajectory +from pydairlib.common import FindResourceOrThrow +from pydrake.all import PiecewisePolynomial, Box, RigidTransform, Meshcat + +import numpy as np + +from pydrake.all import (DiagramBuilder, AddMultibodyPlantSceneGraph, Simulator, + SceneGraph, MultibodyPlant) +from pydrake.geometry import MeshcatVisualizer, StartMeshcat, \ + MeshcatVisualizerParams +from pydairlib.cassie.cassie_utils import AddCassieMultibody +from pydairlib.multibody import MultiposeVisualizer, \ + ConnectTrajectoryVisualizer, CreateWithSpringsToWithoutSpringsMapPos, \ + CreateWithSpringsToWithoutSpringsMapVel +from visualize_params import DirconVisualizationParams + + +def main(): + # visualization_config_file = 'bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml' + # visualization_config_file = 'bindings/pydairlib/lcm/visualization/visualize_configs/box_jump.yaml' + # visualization_config_file = 'bindings/pydairlib/lcm/visualization/visualize_configs/down_jump.yaml' + visualization_config_file = 'bindings/pydairlib/lcm/visualization/visualize_configs/jump.yaml' + params = DirconVisualizationParams(visualization_config_file) + + builder = DiagramBuilder() + scene_graph_wo_spr = builder.AddSystem(SceneGraph()) + scene_graph_w_spr = builder.AddSystem(SceneGraph()) + plant_wo_spr = MultibodyPlant(1e-3) + plant_w_spr = MultibodyPlant(1e-3) + AddCassieMultibody(plant_wo_spr, scene_graph_wo_spr, + True, params.fixed_spring_urdf, + False, False, False) + AddCassieMultibody(plant_w_spr, scene_graph_w_spr, + True, params.spring_urdf, + False, False, False) + plant_wo_spr.Finalize() + plant_w_spr.Finalize() + + pos_spr_map = CreateWithSpringsToWithoutSpringsMapPos(plant_w_spr, + plant_wo_spr) + vel_spr_map = CreateWithSpringsToWithoutSpringsMapVel(plant_w_spr, + plant_wo_spr) + + + + nq = plant_wo_spr.num_positions() + nv = plant_wo_spr.num_velocities() + nx = nq + nv + nq_spr = plant_w_spr.num_positions() + nv_spr = plant_w_spr.num_velocities() + nx_spr = nq_spr + nv_spr + state_spr_map = np.zeros((nx_spr, nx)) + state_spr_map[:nq_spr, :nq] = pos_spr_map.T + state_spr_map[-nv_spr:, -nv:] = vel_spr_map.T + + + filename = FindResourceOrThrow(params.filename) + dircon_traj = lcm_trajectory.DirconTrajectory(plant_wo_spr, filename) + + optimal_traj = dircon_traj.ReconstructStateTrajectory() + if params.use_springs: + optimal_traj = dircon_traj.ReconstructStateTrajectoryWithSprings(state_spr_map) + t_vec = optimal_traj.get_segment_times() + + vis_urdf = params.fixed_spring_urdf + vis_plant = plant_wo_spr + vis_scene_graph = scene_graph_wo_spr + nx_vis = nx + if params.use_springs: + vis_urdf = params.spring_urdf + nx_vis = nx_spr + vis_plant = plant_w_spr + vis_scene_graph = scene_graph_w_spr + + if params.visualize_mode == 0 or params.visualize_mode == 1: + + ConnectTrajectoryVisualizer(vis_plant, builder, vis_scene_graph, + optimal_traj) + meschat_params = MeshcatVisualizerParams() + meschat_params.publish_period = 1.0 / 60.0 + meshcat = StartMeshcat() + visualizer = MeshcatVisualizer.AddToBuilder( + builder, vis_scene_graph, meshcat, meschat_params) + diagram = builder.Build() + + while params.visualize_mode == 1: + simulator = Simulator(diagram) + simulator.set_target_realtime_rate(params.realtime_rate) + simulator.Initialize() + simulator.AdvanceTo(optimal_traj.end_time()) + + elif params.visualize_mode == 2: + poses = np.zeros((params.num_poses, nx_vis)) + for i in range(params.num_poses): + poses[i] = optimal_traj.value( + t_vec[int(i * len(t_vec) / params.num_poses)])[:, 0] + poses[i, 6] += 0.5 + # alpha_scale = np.linspace(0.2, 1.0, params.num_poses) + alpha_scale = np.linspace(1.0, 1.0, params.num_poses) + visualizer = MultiposeVisualizer(FindResourceOrThrow( + vis_urdf), + params.num_poses, np.square(alpha_scale), "") + # translation = np.array([-0.25, 0, 0.25]) + ortho_camera = Meshcat.OrthographicCamera() + ortho_camera.top = 1 + ortho_camera.bottom = -0.1 + ortho_camera.left = -1 + ortho_camera.right = 2 + ortho_camera.near = -10 + ortho_camera.far = 500 + ortho_camera.zoom = 1 + translation = np.array([0.5, 0, 0.25]) + origin = RigidTransform(translation) + box = Box(0.5, 1.0, 0.5) + visualizer.GetMeshcat().SetObject("box", box) + visualizer.GetMeshcat().SetTransform("box", origin) + visualizer.GetMeshcat().SetCamera(ortho_camera) + visualizer.DrawPoses(poses.T) + while(True): + continue + + +if __name__ == "__main__": + main() diff --git a/bindings/pydairlib/multibody/BUILD.bazel b/bindings/pydairlib/multibody/BUILD.bazel index 9de8f1de70..3f5fe996aa 100644 --- a/bindings/pydairlib/multibody/BUILD.bazel +++ b/bindings/pydairlib/multibody/BUILD.bazel @@ -16,12 +16,12 @@ pybind_py_library( cc_deps = [ "//multibody:multipose_visualizer", "//multibody:utils", + "//multibody:visualization_utils", "@drake//:drake_shared_library", ], cc_so_name = "multibody", cc_srcs = ["multibody_py.cc"], py_deps = [ - "@drake//bindings/pydrake", ":module_py", ], py_imports = ["."], diff --git a/bindings/pydairlib/multibody/multibody_py.cc b/bindings/pydairlib/multibody/multibody_py.cc index 896d7e4789..3e08e2e505 100644 --- a/bindings/pydairlib/multibody/multibody_py.cc +++ b/bindings/pydairlib/multibody/multibody_py.cc @@ -5,6 +5,7 @@ #include "multibody/multibody_utils.h" #include "multibody/multipose_visualizer.h" +#include "multibody/visualization_utils.h" namespace py = pybind11; @@ -22,12 +23,18 @@ PYBIND11_MODULE(multibody, m) { .def(py::init()) .def(py::init()) .def(py::init()) - .def("DrawPoses", &MultiposeVisualizer::DrawPoses, py::arg("poses")); + .def("DrawPoses", &MultiposeVisualizer::DrawPoses, py::arg("poses"), py::arg("poses")) + .def("GetMeshcat", &MultiposeVisualizer::GetMeshcat); + + m.def("ConnectTrajectoryVisualizer", + &dairlib::multibody::ConnectTrajectoryVisualizer, py::arg("plant"), + py::arg("builder"), py::arg("scene_graph"), py::arg("trajectory")); m.def("MakeNameToPositionsMap", - &dairlib::multibody::MakeNameToPositionsMap, py::arg("plant")) + py::overload_cast&>(&dairlib::multibody::MakeNameToPositionsMap), + py::arg("plant")) .def("MakeNameToVelocitiesMap", - &dairlib::multibody::MakeNameToVelocitiesMap, + py::overload_cast&>(&dairlib::multibody::MakeNameToVelocitiesMap), py::arg("plant")) .def("MakeNameToActuatorsMap", &dairlib::multibody::MakeNameToActuatorsMap, @@ -38,10 +45,17 @@ PYBIND11_MODULE(multibody, m) { .def("CreateActuatorNameVectorFromMap", &dairlib::multibody::CreateActuatorNameVectorFromMap, py::arg("plant")) + .def("CreateWithSpringsToWithoutSpringsMapPos", + &dairlib::multibody::CreateWithSpringsToWithoutSpringsMapPos, + py::arg("plant_w_spr"), py::arg("plant_wo_spr")) + .def("CreateWithSpringsToWithoutSpringsMapVel", + &dairlib::multibody::CreateWithSpringsToWithoutSpringsMapVel, + py::arg("plant_w_spr"), py::arg("plant_wo_spr")) .def("AddFlatTerrain", &dairlib::multibody::AddFlatTerrain, py::arg("plant"), py::arg("scene_graph"), py::arg("mu_static"), py::arg("mu_kinetic"), py::arg("normal_W") = Eigen::Vector3d(0, 0, 1), + py::arg("stiffness") = 0, py::arg("dissipation_rate") = 0, py::arg("show_ground") = 1); } diff --git a/bindings/pydairlib/multibody/test/multipose_visualizer_test.py b/bindings/pydairlib/multibody/test/multipose_visualizer_test.py index 0be0f8b706..4330f9c3ee 100644 --- a/bindings/pydairlib/multibody/test/multipose_visualizer_test.py +++ b/bindings/pydairlib/multibody/test/multipose_visualizer_test.py @@ -15,7 +15,7 @@ def main(): builder = pydrake.systems.framework.DiagramBuilder() plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph( builder, 0) - pydrake.multibody.parsing.Parser(plant).AddModelFromFile( + pydrake.multibody.parsing.Parser(plant).AddModels( FindResourceOrThrow("examples/Cassie/urdf/cassie_v2.urdf")) plant.Finalize() visualizer.DrawPoses(np.random.rand(plant.num_positions(), diff --git a/bindings/pydairlib/solvers/BUILD.bazel b/bindings/pydairlib/solvers/BUILD.bazel new file mode 100644 index 0000000000..5fe19d4692 --- /dev/null +++ b/bindings/pydairlib/solvers/BUILD.bazel @@ -0,0 +1,54 @@ +# -*- python -*- +load("@drake//tools/install:install.bzl", "install") +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) +#load("@rules_python//python:packaging.bzl", "py_package", "py_wheel") + +package(default_visibility = ["//visibility:public"]) + +pybind_py_library( + name = "c3_py", + cc_deps = [ + "//solvers:c3", + "@drake//bindings/pydrake/common:default_scalars_pybind", + "@drake//bindings/pydrake/common:deprecation_pybind", + "@drake//bindings/pydrake/common:sorted_pair_pybind", + ], + cc_so_name = "c3", + cc_srcs = ["c3_py.cc"], + py_deps = [ + ":module_py", + ], + py_imports = ["."], +) + +# This determines how `PYTHONPATH` is configured, and how to install the +# bindings. +PACKAGE_INFO = get_pybind_package_info("//bindings") + +py_library( + name = "module_py", + srcs = [ + "__init__.py", + ], + imports = PACKAGE_INFO.py_imports, + deps = [ + ], +) + +PY_LIBRARIES = [ + ":module_py", + ":c3_py", +] + +# Package roll-up (for Bazel dependencies). +py_library( + name = "pyc3", + imports = PACKAGE_INFO.py_imports, + deps = PY_LIBRARIES, +) diff --git a/bindings/pydairlib/solvers/__init__.py b/bindings/pydairlib/solvers/__init__.py new file mode 100644 index 0000000000..6faf2149f8 --- /dev/null +++ b/bindings/pydairlib/solvers/__init__.py @@ -0,0 +1,3 @@ +# Importing everything in this directory to this package +from . import * +from .c3 import * diff --git a/bindings/pydairlib/solvers/c3_py.cc b/bindings/pydairlib/solvers/c3_py.cc new file mode 100644 index 0000000000..427a660862 --- /dev/null +++ b/bindings/pydairlib/solvers/c3_py.cc @@ -0,0 +1,129 @@ +#include +#include +#include +#include + +#include "solvers/c3_miqp.h" +#include "solvers/lcs.h" +#include "solvers/lcs_factory.h" + +namespace py = pybind11; + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using py::arg; +using std::vector; + +using dairlib::solvers::C3MIQP; +using dairlib::solvers::LCS; + +namespace c3 { +namespace pyc3 { + +PYBIND11_MODULE(c3, m) { + py::class_(m, "LCS") + .def(py::init&, const vector&, + const vector&, const vector&, + const vector&, const vector&, + const vector&, const vector&, double>(), + arg("A"), arg("B"), arg("D"), arg("d"), arg("E"), arg("F"), arg("H"), + arg("c"), arg("dt")) + .def(py::init(), + arg("A"), arg("B"), arg("D"), arg("d"), arg("E"), arg("F"), arg("H"), + arg("c"), arg("N"), arg("dt")) + .def_readwrite("A_", &LCS::A_) + .def_readwrite("B_", &LCS::B_) + .def_readwrite("D_", &LCS::D_) + .def_readwrite("d_", &LCS::d_) + .def_readwrite("E_", &LCS::E_) + .def_readwrite("F_", &LCS::F_) + .def_readwrite("H_", &LCS::H_) + .def_readwrite("c_", &LCS::c_) + .def("Simulate", &LCS::Simulate); + + m.def("LinearizePlantToLCS", + &dairlib::solvers::LCSFactory::LinearizePlantToLCS, + py::arg("plant"), + py::arg("context"), + py::arg("plant_ad"), + py::arg("context_ad"), + py::arg("contact_geoms"), + py::arg("num_friction_directions"), + py::arg("mu"), + py::arg("dt"), + py::arg("N"), + py::arg("contact_model")); + + { + using Enum = dairlib::solvers::ContactModel; + py::enum_ enum_py(m, "ContactModel"); + enum_py // BR + .value("kStewartAndTrinkle", Enum::kStewartAndTrinkle) + .value("kAnitescu", Enum::kAnitescu); + } + + py::class_(m, "CostMatrices") + .def(py::init&, const std::vector&, + const std::vector&, const std::vector&>(), + arg("Q"), arg("R"), arg("G"), arg("U")); + + py::class_(m, "C3MIQP") + .def(py::init&, const C3Options&>(), + arg("LCS"), arg("costs"), arg("x_des"), arg("c3_options")) + .def("Solve", &C3MIQP::Solve, arg("x0"), arg("verbose") = false) + .def("UpdateTarget", &C3MIQP::UpdateTarget, arg("x0")) + .def("UpdateLCS", &C3MIQP::UpdateLCS, arg("lcs")) + .def("ADMMStep", &C3MIQP::ADMMStep, arg("x0"), arg("delta"), arg("w"), + arg("G"), arg("admm_iteration"), arg("verbose") = false) + .def("SolveQP", &C3MIQP::SolveQP, arg("x0"), arg("G"), arg("WD"), + arg("admm_iteration"), arg("is_final_solve")) + .def("SolveProjection", &C3MIQP::SolveProjection, arg("U"), arg("WZ"), arg("admm_iteration")) + .def("AddLinearConstraint", &C3MIQP::AddLinearConstraint, arg("A"), + arg("lower_bound"), arg("upper_bound"), arg("constraint")) + .def("RemoveConstraints", &C3MIQP::RemoveConstraints) + .def("GetFullSolution", &C3MIQP::GetFullSolution) + .def("GetStateSolution", &C3MIQP::GetStateSolution) + .def("GetForceSolution", &C3MIQP::GetForceSolution) + .def("GetInputSolution", &C3MIQP::GetInputSolution) + .def("GetDualDeltaSolution", &C3MIQP::GetDualDeltaSolution) + .def("GetDualWSolution", &C3MIQP::GetDualWSolution); + + py::class_ options(m, "C3Options"); + options.def(py::init<>()) + .def_readwrite("admm_iter", &C3Options::admm_iter) + .def_readwrite("rho", &C3Options::rho) + .def_readwrite("rho_scale", &C3Options::rho_scale) + .def_readwrite("num_threads", &C3Options::num_threads) + .def_readwrite("delta_option", &C3Options::delta_option) + .def_readwrite("contact_model", &C3Options::contact_model) + .def_readwrite("warm_start", &C3Options::warm_start) + .def_readwrite("use_predicted_x0", &C3Options::use_predicted_x0) + .def_readwrite("end_on_qp_step", &C3Options::end_on_qp_step) + .def_readwrite("use_robust_formulation", + &C3Options::use_robust_formulation) + .def_readwrite("solve_time_filter_alpha", + &C3Options::solve_time_filter_alpha) + .def_readwrite("publish_frequency", &C3Options::publish_frequency) + .def_readwrite("u_horizontal_limits", &C3Options::u_horizontal_limits) + .def_readwrite("u_vertical_limits", &C3Options::u_vertical_limits) + .def_readwrite("workspace_limits", &C3Options::workspace_limits) + .def_readwrite("workspace_margins", &C3Options::workspace_margins) + .def_readwrite("N", &C3Options::N) + .def_readwrite("gamma", &C3Options::gamma) + .def_readwrite("mu", &C3Options::mu) + .def_readwrite("dt", &C3Options::dt) + .def_readwrite("solve_dt", &C3Options::solve_dt) + .def_readwrite("num_friction_directions", + &C3Options::num_friction_directions) + .def_readwrite("num_contacts", &C3Options::num_contacts) + .def_readwrite("Q", &C3Options::Q) + .def_readwrite("R", &C3Options::R) + .def_readwrite("G", &C3Options::G) + .def_readwrite("U", &C3Options::U); +} + +} // namespace pyc3 +} // namespace c3 diff --git a/bindings/pydairlib/systems/BUILD.bazel b/bindings/pydairlib/systems/BUILD.bazel index 986f5f7fd7..682a0d8053 100644 --- a/bindings/pydairlib/systems/BUILD.bazel +++ b/bindings/pydairlib/systems/BUILD.bazel @@ -47,6 +47,7 @@ pybind_py_library( "//lcmtypes:lcmt_robot", "//systems/framework:lcm_driven_loop", "//systems/framework:vector", + "@drake//bindings/pydrake/common:value_pybind", "@drake//:drake_shared_library", ], cc_so_name = "framework", diff --git a/bindings/pydairlib/systems/framework_py.cc b/bindings/pydairlib/systems/framework_py.cc index cbb5c45ddc..1453d99fde 100644 --- a/bindings/pydairlib/systems/framework_py.cc +++ b/bindings/pydairlib/systems/framework_py.cc @@ -9,22 +9,38 @@ #include "dairlib/lcmt_robot_output.hpp" +#include "drake/bindings/pydrake/pydrake_pybind.h" + namespace py = pybind11; namespace dairlib { namespace pydairlib { +using drake::pydrake::make_unowned_shared_ptr_from_raw; using LcmOutputDrivenLoop = systems::LcmDrivenLoop; PYBIND11_MODULE(framework, m) { py::class_(m, "LcmOutputDrivenLoop") - .def(py::init>, - const drake::systems::LeafSystem*, - const std::string&, bool>(), py::arg("drake_lcm"), - py::arg("diagram"), py::arg("lcm_parser"), - py::arg("input_channel"), py::arg("is_forced_publish")) + .def(py::init( + [](drake::lcm::DrakeLcm* drake_lcm, + drake::systems::Diagram& diagram, + const drake::systems::LeafSystem* lcm_parser, + const std::string& input_channel, bool is_forced_publish) { + // The C++ constructor doesn't offer a bare-pointer overload, + // only shared_ptr. Because object lifetime is already + // handled by the ref_cycle annotation below (as required for + // all subclasses of Diagram), we can pass the `plant` as an + // unowned shared_ptr. + // (comment taken from Drakes controllers_py.cc) + return std::make_unique( + drake_lcm, make_unowned_shared_ptr_from_raw(&diagram), + lcm_parser, input_channel, is_forced_publish); + + }), + py::arg("drake_lcm"), + py::arg("diagram"), py::arg("lcm_parser"), + py::arg("input_channel"), py::arg("is_forced_publish")) .def("Simulate", &LcmOutputDrivenLoop::Simulate, py::arg("end_time") = std::numeric_limits::infinity()); diff --git a/bindings/pydairlib/systems/robot_lcm_systems_py.cc b/bindings/pydairlib/systems/robot_lcm_systems_py.cc index 8117020da2..eab647bfaf 100644 --- a/bindings/pydairlib/systems/robot_lcm_systems_py.cc +++ b/bindings/pydairlib/systems/robot_lcm_systems_py.cc @@ -8,7 +8,6 @@ #include "drake/multibody/plant/multibody_plant.h" namespace py = pybind11; -using py_rvp = py::return_value_policy; namespace dairlib { namespace pydairlib { @@ -18,31 +17,54 @@ PYBIND11_MODULE(robot_lcm_systems, m) { using drake::multibody::MultibodyPlant; using systems::RobotOutputSender; + using py_rvp = py::return_value_policy; py::class_>( m, "RobotOutputReceiver") - .def(py::init&>()); + .def(py::init&>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex>()); py::class_>( m, "RobotInputReceiver") .def(py::init&>()); py::class_>( m, "RobotOutputSender") - .def(py::init&, bool>()) + .def(py::init&, bool, bool>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex, + bool, bool>()) .def("get_input_port_state", &RobotOutputSender::get_input_port_state, - py_rvp::reference_internal) + py_rvp::reference_internal) .def("get_input_port_effort", &RobotOutputSender::get_input_port_effort, - py_rvp::reference_internal) + py_rvp::reference_internal) .def("get_input_port_imu", &RobotOutputSender::get_input_port_imu, - py_rvp::reference_internal); + py_rvp::reference_internal); + py::class_>( + m, "ObjectStateSender") + .def(py::init&, + drake::multibody::ModelInstanceIndex>()) + .def("get_input_port_state", &systems::ObjectStateSender::get_input_port_state, + py_rvp::reference_internal); + py::class_>( + m, "ObjectStateReceiver") + .def(py::init&>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex>()); + py::class_>( m, "RobotCommandSender") .def(py::init&>()); m.def("AddActuationRecieverAndStateSenderLcm", - &dairlib::systems::AddActuationRecieverAndStateSenderLcm, + py::overload_cast*, + const MultibodyPlant&, + drake::systems::lcm::LcmInterfaceSystem*, std::string, + std::string, double, + drake::multibody::ModelInstanceIndex, bool, double>( + &dairlib::systems::AddActuationRecieverAndStateSenderLcm), py::arg("builder"), py::arg("plant"), py::arg("lcm"), py::arg("actuator_channel"), py::arg("state_channel"), - py::arg("publish_rate"), py::arg("publish_efforts"), - py::arg("actuator_delay")); + py::arg("publish_rate"), py::arg("model_instance"), + py::arg("publish_efforts"), py::arg("actuator_delay")); } diff --git a/common/BUILD.bazel b/common/BUILD.bazel index e129ab8c9a..4ca908818a 100644 --- a/common/BUILD.bazel +++ b/common/BUILD.bazel @@ -3,7 +3,7 @@ package(default_visibility = ["//visibility:public"]) cc_library( name = "common", deps = [ - ":blending_utils", + ":math_utils", ":discrete_time_filter", ":eigen_utils", ":file_utils", @@ -49,12 +49,12 @@ cc_library( ) cc_library( - name = "blending_utils", + name = "math_utils", srcs = [ - "blending_utils.cc", + "math_utils.cc", ], hdrs = [ - "blending_utils.h", + "math_utils.h", ], deps = [ "@drake//:drake_shared_library", @@ -73,3 +73,30 @@ cc_library( "@drake//:drake_shared_library", ], ) + +cc_library( + name = "quaternion_error_hessian", + srcs = [ + "quaternion_error_hessian.cc" + ], + hdrs = [ + "quaternion_error_hessian.h" + ], + deps = [ + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "update_context", + srcs = [ + "update_context.cc" + ], + hdrs = [ + "update_context.h" + ], + deps = [ + "@drake//:drake_shared_library", + "//multibody:utils", + ], +) diff --git a/common/blending_utils.h b/common/blending_utils.h deleted file mode 100644 index 26b1c415c7..0000000000 --- a/common/blending_utils.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -enum BLEND_FUNC { kSigmoid, kExponential }; - -double blend_sigmoid(double t, double tau, double window); - -double blend_exp(double t, double tau, double window); diff --git a/common/discrete_time_filter.cc b/common/discrete_time_filter.cc index f38e92cfd9..525f892dc5 100644 --- a/common/discrete_time_filter.cc +++ b/common/discrete_time_filter.cc @@ -5,10 +5,8 @@ using Eigen::VectorXd; namespace dairlib { -void DiscreteTimeFilter::Update(Eigen::VectorXd value){ -} -void DiscreteTimeFilter::Reset(){ -} +void DiscreteTimeFilter::Update(Eigen::VectorXd value) {} +void DiscreteTimeFilter::Reset() {} FirstOrderLowPassFilter::FirstOrderLowPassFilter(double alpha, int vector_size) : alpha_(alpha) { @@ -31,4 +29,58 @@ void FirstOrderLowPassFilter::UpdateParameters(double alpha) { alpha_ = alpha; } +ButterworthFilter::ButterworthFilter(VectorXd& a, VectorXd& b, + int vector_size) { + data_size_ = vector_size; + this->nb_ = b.size(); + this->na_ = a.size(); + this->b_ = b; + this->a_ = a; + this->y_.resize(vector_size, na_); + this->y_.setZero(); + this->x_.resize(vector_size, nb_); + this->x_.setZero(); +} + +void ButterworthFilter::Reset() { + y_.setZero(); + x_.setZero(); +} + +void ButterworthFilter::Update(VectorXd value) { + DRAKE_DEMAND(value.size() == data_size_); + // Modified code from Jenna Reher's smoothing.cpp + + // Shift the data back and append the raw + for (long i = nb_ - 1; i > 0; i--) { + x_.col(i) = x_.col(i - 1); + } + for (long i = na_ - 1; i > 0; i--) { + y_.col(i) = y_.col(i - 1); + } + x_.col(0) = value; + + // Get the solution -- MATLAB pseudocode... + // a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) + // - a(2)*y(n-1) - ... - a(na+1)*y(n-na) + VectorXd y = VectorXd::Zero(data_size_); + for (long i = 0; i < nb_; i++) { + y += b_(i) * x_.col(i); + } + for (long i = 1; i < na_; i++) { + y -= a_(i) * y_.col(i); + } + + // Multiply solution... a(0) should be 1 from MATLAB + y *= a_(0); + y_.col(0) = y; +} + +void ButterworthFilter::UpdateParameters(Eigen::VectorXd &b, Eigen::VectorXd &a) { + DRAKE_DEMAND(b.size() == nb_); + DRAKE_DEMAND(a.size() == na_); + this->b_ = b; + this->a_ = a; +} + } // namespace dairlib \ No newline at end of file diff --git a/common/discrete_time_filter.h b/common/discrete_time_filter.h index 41bc40948b..75812d039f 100644 --- a/common/discrete_time_filter.h +++ b/common/discrete_time_filter.h @@ -38,6 +38,21 @@ class FirstOrderLowPassFilter : public DiscreteTimeFilter { double alpha_; }; -class ButterworthFilter : public DiscreteTimeFilter {}; +class ButterworthFilter : public DiscreteTimeFilter { + public: + ButterworthFilter(Eigen::VectorXd& a, Eigen::VectorXd& b, int vector_size); + void Reset() override; + void Update(Eigen::VectorXd) override; + void UpdateParameters(Eigen::VectorXd &b, Eigen::VectorXd &a); + + private: + int nb_; + int na_; + int data_size_; + Eigen::MatrixXd y_; // Array of previously filtered values + Eigen::MatrixXd x_; // Array of raw values + Eigen::VectorXd b_; // filter coefficient + Eigen::VectorXd a_; // filter coefficient +}; } // namespace dairlib \ No newline at end of file diff --git a/common/eigen_utils.cc b/common/eigen_utils.cc index fdaa6d9d1b..7689634857 100644 --- a/common/eigen_utils.cc +++ b/common/eigen_utils.cc @@ -1,17 +1,22 @@ #include "eigen_utils.h" -std::vector CopyVectorXdToStdVector( - const Eigen::VectorXd& eigen_vec) { +std::vector CopyVectorXdToStdVector(const Eigen::VectorXd& eigen_vec) { return std::vector(eigen_vec.data(), eigen_vec.data() + eigen_vec.size()); } -Eigen::VectorXd eigen_clamp( - const Eigen::VectorXd& value, const Eigen::VectorXd& lb, const Eigen::VectorXd& ub){ +Eigen::VectorXd StdVectorToVectorXd(std::vector std_vec) { + return Eigen::Map(std_vec.data(), + std_vec.size()); +} + +Eigen::VectorXd eigen_clamp(const Eigen::VectorXd& value, + const Eigen::VectorXd& lb, + const Eigen::VectorXd& ub) { DRAKE_DEMAND(value.size() == lb.size()); DRAKE_DEMAND(value.size() == ub.size()); Eigen::VectorXd clamped_value(value.size()); - for (int i = 0; i < value.size(); ++i){ + for (int i = 0; i < value.size(); ++i) { clamped_value[i] = std::clamp(value[i], lb[i], ub[i]); } return clamped_value; diff --git a/common/eigen_utils.h b/common/eigen_utils.h index 17d6cff4c0..e6e3fca3a1 100644 --- a/common/eigen_utils.h +++ b/common/eigen_utils.h @@ -8,6 +8,8 @@ /// from an Eigen::VectorXd. std::vector CopyVectorXdToStdVector(const Eigen::VectorXd& eigen_vec); +Eigen::VectorXd StdVectorToVectorXd(std::vector); + Eigen::VectorXd eigen_clamp(const Eigen::VectorXd& value, const Eigen::VectorXd& lb, const Eigen::VectorXd& ub); diff --git a/common/file_utils.h b/common/file_utils.h index b7583d4b75..8383a2a15d 100644 --- a/common/file_utils.h +++ b/common/file_utils.h @@ -5,6 +5,8 @@ #include #include +#include "drake/common/yaml/yaml_read_archive.h" + namespace dairlib { /// Read a CSV formatted file as an Eigen Matrix @@ -13,4 +15,16 @@ Eigen::MatrixXd readCSV(const std::string & path); /// Write an Eigen Matrix into a CSV formatted file void writeCSV(const std::string& path, const Eigen::MatrixXd& M); +/// Deserialize a YAML integer into a custom enum type. +template +void DeserializeEnum(const char* value_str, Archive* a, T* enum_out) { + int raw_value = static_cast(*enum_out); + a->Visit(drake::MakeNameValue(value_str, &raw_value)); + *enum_out = static_cast(raw_value); +} + } // namespace dairlib + +/// Deserializes a YAML integer by the name of the provided e_out variable into +/// the custom enum type of e_out. +#define ENUM_DESERIALIZE(a, e_out) dairlib::DeserializeEnum(#e_out, a, &e_out) diff --git a/common/blending_utils.cc b/common/math_utils.cc similarity index 89% rename from common/blending_utils.cc rename to common/math_utils.cc index 645cac6fe0..7a441c6165 100644 --- a/common/blending_utils.cc +++ b/common/math_utils.cc @@ -1,4 +1,4 @@ -#include "blending_utils.h" +#include "math_utils.h" #include diff --git a/common/math_utils.h b/common/math_utils.h new file mode 100644 index 0000000000..53a7128e03 --- /dev/null +++ b/common/math_utils.h @@ -0,0 +1,16 @@ +#pragma once +#include + +/// Blending utilities +enum BLEND_FUNC { kSigmoid, kExponential }; + +double blend_sigmoid(double t, double tau, double window); + +double blend_exp(double t, double tau, double window); + +/// Random sampling utility +inline double RandomUniform(double min, double max) { + static thread_local std::mt19937 rng{std::random_device{}()}; + std::uniform_real_distribution dist(min, max); + return dist(rng); +} diff --git a/common/parameters/BUILD.bazel b/common/parameters/BUILD.bazel new file mode 100644 index 0000000000..5205598698 --- /dev/null +++ b/common/parameters/BUILD.bazel @@ -0,0 +1,5 @@ +cc_library( + name = "franka_drake_lcm_driver_channels", + hdrs = ["franka_drake_lcm_driver_channels.h"], + visibility = ["//visibility:public"], +) diff --git a/common/parameters/franka_drake_lcm_driver_channels.h b/common/parameters/franka_drake_lcm_driver_channels.h new file mode 100644 index 0000000000..eaa7d3ee08 --- /dev/null +++ b/common/parameters/franka_drake_lcm_driver_channels.h @@ -0,0 +1,15 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + + +struct FrankaDrakeLcmDriverChannels { + std::string franka_status_channel; + std::string franka_command_channel; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(franka_status_channel)); + a->Visit(DRAKE_NVP(franka_command_channel)); + } +}; \ No newline at end of file diff --git a/common/parameters/franka_drake_lcm_driver_channels.yaml b/common/parameters/franka_drake_lcm_driver_channels.yaml new file mode 100644 index 0000000000..55cc785c3b --- /dev/null +++ b/common/parameters/franka_drake_lcm_driver_channels.yaml @@ -0,0 +1,2 @@ +franka_status_channel: PANDA_STATUS # lcmt_panda_status +franka_command_channel: PANDA_COMMAND # lcmt_panda_command diff --git a/common/quaternion_error_hessian.cc b/common/quaternion_error_hessian.cc new file mode 100644 index 0000000000..6b3ac7d064 --- /dev/null +++ b/common/quaternion_error_hessian.cc @@ -0,0 +1,119 @@ +#include "quaternion_error_hessian.h" +#include + + +using Eigen::VectorXd; +using Eigen::MatrixXd; + + +namespace dairlib{ +namespace systems{ + +Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( + const Eigen::VectorXd& quat, const Eigen::VectorXd& quat_desired) +{ + // Check the inputs are of expected shape. + DRAKE_DEMAND(quat.size() == 4); + DRAKE_DEMAND(quat_desired.size() == 4); + + // Extract the quaternion components. + double q_w = quat(0); + double q_x = quat(1); + double q_y = quat(2); + double q_z = quat(3); + + double r_w = quat_desired(0); + double r_x = quat_desired(1); + double r_y = quat_desired(2); + double r_z = quat_desired(3); + + // Define reusable expressions. + double exp_1 = std::atan2( + std::sqrt(std::pow(q_w*r_x - q_x*r_w + q_y*r_z - q_z*r_y, 2) + + std::pow(q_w*r_y - q_x*r_z - q_y*r_w + q_z*r_x, 2) + + std::pow(q_w*r_z + q_x*r_y - q_y*r_x - q_z*r_w, 2)), + q_w*r_w + q_x*r_x + q_y*r_y + q_z*r_z); + double exp_2 = std::pow(q_w, 2)*std::pow(r_x, 2) + + std::pow(q_w, 2)*std::pow(r_y, 2) + std::pow(q_w, 2)*std::pow(r_z, 2) - + 2*q_w*q_x*r_w*r_x - 2*q_w*q_y*r_w*r_y - 2*q_w*q_z*r_w*r_z + + std::pow(q_x, 2)*std::pow(r_w, 2) + std::pow(q_x, 2)*std::pow(r_y, 2) + + std::pow(q_x, 2)*std::pow(r_z, 2) - + 2*q_x*q_y*r_x*r_y - 2*q_x*q_z*r_x*r_z; + double exp_3 = std::pow(q_y, 2)*std::pow(r_w, 2) + + std::pow(q_y, 2)*std::pow(r_x, 2) + std::pow(q_y, 2)*std::pow(r_z, 2) - + 2*q_y*q_z*r_y*r_z + std::pow(q_z, 2)*std::pow(r_w, 2) + + std::pow(q_z, 2)*std::pow(r_x, 2) + std::pow(q_z, 2)*std::pow(r_y, 2); + double exp_4 = std::pow(q_w, 2) + std::pow(q_x, 2) + + std::pow(q_y, 2) + std::pow(q_z, 2); + double exp_5 = std::pow(exp_4, 2)*std::pow(exp_2 + exp_3, 5/2); + double exp_6 = std::pow(exp_2 + exp_3, 3/2); + double exp_7 = q_w*q_x*r_x + q_w*q_y*r_y + q_w*q_z*r_z - + std::pow(q_x, 2)*r_w - std::pow(q_y, 2)*r_w - std::pow(q_z, 2)*r_w; + double exp_8 = std::pow(q_w, 2)*r_y - q_w*q_y*r_w + std::pow(q_x, 2)*r_y - + q_x*q_y*r_x - q_y*q_z*r_z + std::pow(q_z, 2)*r_y; + double exp_9 = std::pow(q_w, 2)*r_x - q_w*q_x*r_w - q_x*q_y*r_y - + q_x*q_z*r_z + std::pow(q_y, 2)*r_x + std::pow(q_z, 2)*r_x; + double exp_10 = q_w*r_w*r_z + q_x*r_x*r_z + q_y*r_y*r_z - + q_z*std::pow(r_w, 2) - q_z*std::pow(r_x, 2) - q_z*std::pow(r_y, 2); + double exp_11 = std::pow(q_w, 2)*r_z - q_w*q_z*r_w + std::pow(q_x, 2)*r_z - + q_x*q_z*r_x + std::pow(q_y, 2)*r_z - q_y*q_z*r_y; + double exp_12 = q_w*r_w*r_y + q_x*r_x*r_y - q_y*std::pow(r_w, 2) - + q_y*std::pow(r_x, 2) - q_y*std::pow(r_z, 2) + q_z*r_y*r_z; + double exp_13 = q_w*r_w*r_x - q_x*std::pow(r_w, 2) - q_x*std::pow(r_y, 2) - + q_x*std::pow(r_z, 2) + q_y*r_x*r_y + q_z*r_x*r_z; + + // Define the Hessian elements. + double H_ww = 8*(-(2*q_w*(exp_7)*(exp_2 + exp_3) - + (q_x*r_x + q_y*r_y + q_z*r_z)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(q_w*std::pow(r_x, 2) + q_w*std::pow(r_y, 2) + + q_w*std::pow(r_z, 2) - q_x*r_w*r_x - q_y*r_w*r_y - q_z*r_w*r_z)* + (exp_7))*(exp_2 + exp_3)*exp_1 + std::pow(exp_7, 2)*(exp_6))/(exp_5); + double H_xx = 8*((2*q_x*(exp_9)*(exp_2 + exp_3) + + (q_w*r_w + q_y*r_y + q_z*r_z)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_9)*(exp_13))*(exp_2 + exp_3)*exp_1 + + std::pow(exp_9, 2)*(exp_6))/(exp_5); + double H_yy = 8*((2*q_y*(exp_8)*(exp_2 + exp_3) + + (q_w*r_w + q_x*r_x + q_z*r_z)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_8)*(exp_12))*(exp_2 + exp_3)*exp_1 + + std::pow(exp_8, 2)*(exp_6))/(exp_5); + double H_zz = 8*((2*q_z*(exp_11)*(exp_2 + exp_3) + + (q_w*r_w + q_x*r_x + q_y*r_y)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_11)*((exp_10)))*(exp_2 + exp_3)*exp_1 + + std::pow(exp_11, 2)*(exp_6))/(exp_5); + double H_wx = 8*((-2*q_x*(exp_7)*(exp_2 + exp_3) + + (q_w*r_x - 2*q_x*r_w)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(exp_7)*(exp_13))*(exp_2 + exp_3)*exp_1 - + (exp_9)*(exp_7)*(exp_6))/(exp_5); + double H_wy = 8*((-2*q_y*(exp_7)*(exp_2 + exp_3) + + (q_w*r_y - 2*q_y*r_w)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(exp_7)*(exp_12))*(exp_2 + exp_3)*exp_1 - + (exp_8)*(exp_7)*(exp_6))/(exp_5); + double H_wz = 8*((-2*q_z*(exp_7)*(exp_2 + exp_3) + + (q_w*r_z - 2*q_z*r_w)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(exp_7)*((exp_10)))*(exp_2 + exp_3)*exp_1 - + (exp_11)*(exp_7)*(exp_6))/(exp_5); + double H_xy = 8*((2*q_y*(exp_9)*(exp_2 + exp_3) + + (q_x*r_y - 2*q_y*r_x)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_9)*(exp_12))*(exp_2 + exp_3)*exp_1 + + (exp_9)*(exp_8)*(exp_6))/(exp_5); + double H_xz = 8*((2*q_z*(exp_9)*(exp_2 + exp_3) + + (q_x*r_z - 2*q_z*r_x)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_9)*((exp_10)))*(exp_2 + exp_3)*exp_1 + + (exp_9)*(exp_11)*(exp_6))/(exp_5); + double H_yz = 8*((2*q_z*(exp_8)*(exp_2 + exp_3) + + (q_y*r_z - 2*q_z*r_y)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_8)*((exp_10)))*(exp_2 + exp_3)*exp_1 + + (exp_8)*(exp_11)*(exp_6))/(exp_5); + + // Define the Hessian. + Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(4, 4); + hessian.col(0) << H_ww, H_wx, H_wy, H_wz; + hessian.col(1) << H_wx, H_xx, H_xy, H_xz; + hessian.col(2) << H_wy, H_xy, H_yy, H_yz; + hessian.col(3) << H_wz, H_xz, H_yz, H_zz; + + return hessian; +} + +} // namespace systems +} // namespace dairlib diff --git a/common/quaternion_error_hessian.h b/common/quaternion_error_hessian.h new file mode 100644 index 0000000000..a5c899336e --- /dev/null +++ b/common/quaternion_error_hessian.h @@ -0,0 +1,17 @@ +#include +#include +#include "drake/common/drake_assert.h" + +using Eigen::VectorXd; +using Eigen::MatrixXd; + + +namespace dairlib{ +namespace systems{ + +Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( + const Eigen::VectorXd& quat, + const Eigen::VectorXd& quat_desired); + +} // namespace systems +} // namespace dairlib diff --git a/common/update_context.cc b/common/update_context.cc new file mode 100644 index 0000000000..c8974bf247 --- /dev/null +++ b/common/update_context.cc @@ -0,0 +1,37 @@ +#include "update_context.h" + +#include + +using Eigen::VectorXd; + +namespace dairlib { + +void UpdateContext(const int& n_q, const int& n_v, const int& n_u, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const Eigen::VectorXd& x) { + // Update autodiff. + VectorXd xu_test(n_q + n_v + n_u); + + // TODO u has always been set to a vector of 1000s here but unsure why or if + // it matters. + VectorXd test_u = 1000 * VectorXd::Ones(n_u); + + // Update context with respect to positions and velocities associated with + // the candidate state. + VectorXd test_q = x.head(n_q); + VectorXd test_v = x.tail(n_v); + xu_test << test_q, test_v, test_u; + auto xu_ad_test = drake::math::InitializeAutoDiff(xu_test); + plant_ad.SetPositionsAndVelocities(context_ad, xu_ad_test.head(n_q + n_v)); + multibody::SetInputsIfNew(plant_ad, xu_ad_test.tail(n_u), + context_ad); + + plant.SetPositions(context, test_q); + plant.SetVelocities(context, test_v); + multibody::SetInputsIfNew(plant, test_u, context); +} + +} // namespace dairlib diff --git a/common/update_context.h b/common/update_context.h new file mode 100644 index 0000000000..2fae7e3a82 --- /dev/null +++ b/common/update_context.h @@ -0,0 +1,32 @@ +#include + +#include "multibody/multibody_utils.h" + +using drake::AutoDiffVecXd; +using drake::AutoDiffXd; +using Eigen::VectorXd; + +namespace dairlib { + +/// Updates the context of a MultibodyPlant with the given state vector. +/// +/// This function updates both the double and AutoDiff contexts of a MultibodyPlant +/// using the provided state vector. It is useful for setting the state of the +/// plant in simulation or optimization tasks. +/// +/// @param n_q Number of generalized positions (q). +/// @param n_v Number of generalized velocities (v). +/// @param n_u Number of control inputs (u). +/// @param plant The MultibodyPlant to update. +/// @param context The context for the MultibodyPlant. +/// @param plant_ad The MultibodyPlant with AutoDiff support. +/// @param context_ad The context for the MultibodyPlant with AutoDiff support. +/// @param x The state vector to set in the contexts. +void UpdateContext(const int& n_q, const int& n_v, const int& n_u, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const Eigen::VectorXd& x); + +} // namespace dairlib diff --git a/director/BUILD.bazel b/director/BUILD.bazel index 3b53d7ed2a..3c02eb9bbe 100644 --- a/director/BUILD.bazel +++ b/director/BUILD.bazel @@ -1,20 +1,21 @@ # -*- python -*- -py_binary( - name = "drake_director_py", - srcs = ["drake_director.py"], - main = "drake_director.py", - # Python libraries to import. - deps = [ - "//lcmtypes:lcmtypes_robot_py", - "@drake//tools:drake_visualizer_py", - "@pydrake_pegged", - ], -) - -load("@drake//tools/skylark:drake_runfiles_binary.bzl", "drake_runfiles_binary") - -drake_runfiles_binary( - name = "drake-director", - target = ":drake_director_py", -) +#py_binary( +# name = "drake_director_py", +# srcs = ["drake_director.py"], +# main = "drake_director.py", +# # Python libraries to import. +# deps = [ +# "//lcmtypes:lcmtypes_robot_py", +# "@drake//tools:drake_visualizer_py", +# # "//bindings/pydairlib/multibody:multibody_py", +# "@pydrake_pegged", +# ], +#) +# +#load("@drake//tools/skylark:drake_runfiles_binary.bzl", "drake_runfiles_binary") +# +#drake_runfiles_binary( +# name = "drake-director", +# target = ":drake_director_py", +#) diff --git a/director/scripts/VisualizationGUI.py b/director/scripts/VisualizationGUI.py index 9ecf17dfff..cbc85cbf9d 100644 --- a/director/scripts/VisualizationGUI.py +++ b/director/scripts/VisualizationGUI.py @@ -17,8 +17,10 @@ import pydrake.multibody.plant import pydrake.multibody.parsing from pydrake.common.eigen_geometry import Quaternion +from pydrake.geometry import StartMeshcat, Rgba -# stadard library imports +# from pydairlib.multibody import MakeNameToPositionsMap, MakeNameToVelocitiesMap, MakeNameToActuatorsMap +# standard library imports import numpy as np import json from collections import deque @@ -26,558 +28,647 @@ import os import pydrake + drake_root = f"{os.path.dirname(pydrake.__file__)}/share" os.environ["DRAKE_RESOURCE_ROOT"] = drake_root + class VisualizationGui(QWidget): - def __init__(self, parent = None): - super(VisualizationGui, self).__init__(parent) + def __init__(self, parent=None): + super(VisualizationGui, self).__init__(parent) - self.resetGUI() + self.resetGUI() - # create the GUI - self.setWindowTitle("Visualization GUI") - self.vbox = QVBoxLayout() - self.hbox = QHBoxLayout() - - # create the JSON directory reader - self.readJSON = QPushButton("Select JSON file") - self.readJSON.clicked.connect(self.readJSONFile) - self.hbox.addWidget(self.readJSON) - self.vbox.addLayout(self.hbox) - - def resetGUI(self): - ''' - Function for (re)setting the GUI and JSON attrbutes - ''' - # GUI attributes - self.checkBoxes = {} - self.resetBtn = None - self.clearBtn = None - self.checkBoxArea = None - self.delete = False - self.clear = False - self.ready = False - self.lcmObjects = {} - self.subscriptions = [] - - # JSON attributes - self.data = None - self.modelFile = None - self.weldBody = None - self.shapes = {} - self.plant = None - - - def readJSONFile(self): - ''' - Function for reading the JSON input file and populating the JSON - and GUI attributes - ''' - - # use dialog box to get JSON directory - mainWindow = director.applogic.getMainWindow() - fileFilters = "Data Files (*.json)"; - filename = QtGui.QFileDialog.getOpenFileName(mainWindow, "Open...", os.getcwd(), fileFilters, "", QtGui.QFileDialog.DontUseNativeDialog) - - if not filename: - return - - # load only if input is not empty - self.json_file = filename - with open(self.json_file) as json_file: - self.data = json.load(json_file) - - self.modelFile = self.data['model_file'] - - if ('weld-body' in self.data): - self.weldBody = self.data['weld-body'] - - # create each object/shape to be drawn - for data in self.data['data']: - newObject = ObjectToDraw(data) - - # if this is a shape of type lcm then create an additional separate object - if (newObject.category == "lcm"): - if (newObject.name not in self.lcmObjects): - if (newObject.type == "axes"): - self.lcmObjects[newObject.name] = LCMMessage(newObject.source_data, axis = True) - else: - self.lcmObjects[newObject.name] = LCMMessage(newObject.source_data) - else: - if (newObject.type == "axes"): - self.lcmObjects[newObject.name].update( - newObject.source_data, axis = True) - else: - self.lcmObjects[newObject.name].update( - newObject.source_data) - - # if there exists a shape with the given name then simply update it - if (newObject.name not in self.shapes): - self.shapes[newObject.name] = newObject - else: - self.shapes[newObject.name].update(newObject) - - # fill the checkboxes for each data with its name and add the "reset" and "clear" buttons - self.placeCheckBoxes() - if (self.resetBtn == None): - self.resetBtn = QPushButton('Reset') - self.resetBtn.clicked.connect(self.deleteShapes) - self.vbox.addWidget(self.resetBtn) - - if (self.clearBtn == None): - self.clearBtn = QPushButton('Clear History') - self.clearBtn.clicked.connect(self.clearHistory) - self.vbox.addWidget(self.clearBtn) - - if (self.plant == None): - # Create the plant - builder = pydrake.systems.framework.DiagramBuilder() - self.plant, scene_graph = \ - pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, 0) - - file = os.getcwd() + '/' + self.modelFile - pydrake.multibody.parsing.Parser(self.plant).AddModelFromFile(file) - - # determine if there is a need to use the weld a body part - if (self.weldBody != None): - self.plant.WeldFrames(self.plant.world_frame(), - self.plant.GetFrameByName(self.weldBody), RigidTransform.Identity()) - self.plant.Finalize() - self.context = self.plant.CreateDefaultContext() - - # start listenning to the main state LCM messages - lcmUtils.addSubscriber(self.data['channelName'], messageClass=dairlib.lcmt_robot_output, callback=self.state_handler) - - # add more LCM subscriptions depending on the number of "lcm" data - for name in self.lcmObjects: - lcmMessage = self.lcmObjects[name] - subscriber = lcmUtils.addSubscriber(lcmMessage.channel, messageClass=eval(lcmMessage.type), callback=lambda msg, lcmMessage=lcmMessage, name=name: self.abstract_handler(msg, name, lcmMessage)) - - self.subscriptions.append(subscriber) - - self.ready = True - - def deleteShapes(self): - ''' - Function for setting the flag for deleting all shapes currently present - ''' - self.delete = True - - def clearHistory(self): - ''' - Function for setting the flag for clearing the history of any line present - ''' - self.clear = True - - def placeCheckBoxes(self): - ''' - Function for placing the checkboxes of the GUI. Each checkbox corresponds - to a shape/object that has been drawn with the corresponding color and - extension - ''' - - addToGUI = False - if (self.checkBoxArea == None): - self.checkBoxArea = QVBoxLayout() - addToGUI = True - for name in self.shapes: - # create appropriate symbol extention - extension = " •" - type = self.shapes[name].type - if (type == "point"): - extension = " •" - elif (type == "line"): - extension = " ---" - elif (type == "axes"): - extension = " |/_" - - # create each checkbox and conditionally add it to the GUI - addToList = False - if (name not in self.checkBoxes): - self.checkBoxes[name] = QCheckBox(name + extension) - - if (self.shapes[name].type == "point" or self.shapes[name].type == "line"): - color = self.shapes[name].color - self.checkBoxes[name].setStyleSheet("color: rgb("+str(color[0] * 255)+", "+str(color[1] * 255)+", "+str(color[2] * 255)+")") - - addToList = True - self.checkBoxes[name].setChecked(True) - if (addToList == True): - self.checkBoxArea.addWidget(self.checkBoxes[name]) - if (addToGUI == True): - self.vbox.addLayout(self.checkBoxArea) - - def abstract_handler(self, msg, name, lcmMessage): - ''' - Function for handling LCM messages originating from a different - channel than the main one - - msg: the returning LCM message - field: the field name/path - name: name of corresponding shape - x, y, z: indices for each of the 3 dimensions of the location where - the shape is to be drawn. When the shape is an axis then x - will be the first index of the quaternion array and the other - 3 will be the next indices in sequence - ''' - self.handle_checkBox(name) - attribute = msg - field = lcmMessage.field - - # if there is a %d in the field replace it with the actual index - if ("%d" in field): - arr = self.getVector(attribute, lcmMessage.index_field) - if (lcmMessage.index_element in arr): - i = arr.index(lcmMessage.index_element) - field = field.replace("%d", str(i)) - - if ("%d" not in field): - # parse field to get the appropriate information. This is done by - # recursively searching for the right attribute in the given message's field - attribute = self.getVector(attribute, field) - currShape = self.shapes[name] - x = lcmMessage.x - - # special case of an axis data - if (currShape.type == "axes"): - # get quaternion array, normalize it, and get the corresponding rotation matrix - quaternion = [] - for i in range(x, x+4): - quaternion.append(attribute[i]) - norm = np.linalg.norm(quaternion) - quaternion = [x / norm for x in quaternion] - rot_matrix = Quaternion(quaternion).rotation() - - pt_world = self.plant.CalcPointsPositions(self.context, - self.plant.GetFrameByName(currShape.frame), - currShape.point, self.plant.world_frame()) - next_loc = pt_world.transpose()[0] - - self.drawShape(self.shapes[name], next_loc, msg, rotation_matrix = rot_matrix) + # create the GUI + self.setWindowTitle("Visualization GUI") + self.vbox = QVBoxLayout() + self.hbox = QHBoxLayout() - else: - next_loc = [attribute[x], attribute[x+1], attribute[x+2]] - self.drawShape(self.shapes[name], next_loc, msg) + self.meshcat = StartMeshcat() + + # create the JSON directory reader + self.readJSON = QPushButton("Select JSON file") + self.readJSON.clicked.connect(self.readJSONFile) + self.hbox.addWidget(self.readJSON) + self.vbox.addLayout(self.hbox) + + def resetGUI(self): + ''' + Function for (re)setting the GUI and JSON attrbutes + ''' + # GUI attributes + self.checkBoxes = {} + self.resetBtn = None + self.clearBtn = None + self.checkBoxArea = None + self.delete = False + self.clear = False + self.ready = False + self.lcmObjects = {} + self.subscriptions = [] + + # JSON attributes + self.data = None + self.modelFile = None + self.weldBody = None + self.shapes = {} + self.plant = None + + def readJSONFile(self): + ''' + Function for reading the JSON input file and populating the JSON + and GUI attributes + ''' + + # use dialog box to get JSON directory + mainWindow = director.applogic.getMainWindow() + fileFilters = "Data Files (*.json)"; + filename = QtGui.QFileDialog.getOpenFileName(mainWindow, "Open...", + os.getcwd() + '/director/scripts/', + fileFilters, "", + QtGui.QFileDialog.DontUseNativeDialog) + + if not filename: + return + + # load only if input is not empty + self.json_file = filename + with open(self.json_file) as json_file: + self.data = json.load(json_file) + + self.modelFile = self.data['model_file'] + + if ('weld-body' in self.data): + self.weldBody = self.data['weld-body'] + + # create each object/shape to be drawn + for data in self.data['data']: + newObject = ObjectToDraw(data) + + # if this is a shape of type lcm then create an additional separate object + if (newObject.category == "lcm"): + if (newObject.name not in self.lcmObjects): + if (newObject.type == "axes"): + self.lcmObjects[newObject.name] = LCMMessage(newObject.source_data, + axis=True) + else: + self.lcmObjects[newObject.name] = LCMMessage(newObject.source_data) else: - print(lcmMessage.index_element + "is not present in " + lcmMessage.index_field + " and so it cannot be visualized") - print("") - - def getVector(self, attribute, path): - ''' - Function for getting the pointer to a specific field of a given - attribute using the given path - - attribute: the object containing the desired field - path: the string path to get the attribute's field - ''' - field = path.split(".") - regExpr = re.compile('.+\[\d+\]') - newAtribute = attribute - for part in field: - index = None - attName = None - - if regExpr.match(part) is not None: - index = part.split("[")[1].split("]")[0] - attName = part.split("[")[0] - newAtribute = getattr(newAtribute, attName)[int(index)] - else: - index = None - attName = part - newAtribute = getattr(newAtribute, attName) + if (newObject.type == "axes"): + self.lcmObjects[newObject.name].update( + newObject.source_data, axis=True) + else: + self.lcmObjects[newObject.name].update( + newObject.source_data) + + # if there exists a shape with the given name then simply update it + if (newObject.name not in self.shapes): + self.shapes[newObject.name] = newObject + else: + self.shapes[newObject.name].update(newObject) + + # fill the checkboxes for each data with its name and add the "reset" and "clear" buttons + self.placeCheckBoxes() + if (self.resetBtn == None): + self.resetBtn = QPushButton('Reset') + self.resetBtn.clicked.connect(self.deleteShapes) + self.vbox.addWidget(self.resetBtn) + + if (self.clearBtn == None): + self.clearBtn = QPushButton('Clear History') + self.clearBtn.clicked.connect(self.clearHistory) + self.vbox.addWidget(self.clearBtn) + + if (self.plant == None): + # Create the plant + builder = pydrake.systems.framework.DiagramBuilder() + self.plant, scene_graph = \ + pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, 0) + + file = os.getcwd() + '/' + self.modelFile + pydrake.multibody.parsing.Parser(self.plant).AddModels(file) + + # determine if there is a need to use the weld a body part + if (self.weldBody != None): + self.plant.WeldFrames(self.plant.world_frame(), + self.plant.GetFrameByName(self.weldBody), + RigidTransform.Identity()) + self.plant.Finalize() + # self.position_idx_map = MakeNameToPositionsMap(self.plant) + # self.velocity_idx_map = MakeNameToVelocitiesMap(self.plant) + # self.effort_idx_map = MakeNameToActuatorsMap(self.plant) + # self.nq = self.plant.num_positions() + # self.nv = self.plant.num_velocities() + # self.nu = self.plant.num_actuators() + self.context = self.plant.CreateDefaultContext() + + # start listenning to the main state LCM messages + lcmUtils.addSubscriber(self.data['channelName'], + messageClass=dairlib.lcmt_robot_output, + callback=self.state_handler) + # self.state_recevier = RobotOutputReceiver(self.plant) + + # add more LCM subscriptions depending on the number of "lcm" data + for name in self.lcmObjects: + lcmMessage = self.lcmObjects[name] + subscriber = lcmUtils.addSubscriber(lcmMessage.channel, + messageClass=eval(lcmMessage.type), + callback=lambda msg, + lcmMessage=lcmMessage, + name=name: self.abstract_handler( + msg, name, lcmMessage)) + + self.subscriptions.append(subscriber) + + self.ready = True + + def deleteShapes(self): + ''' + Function for setting the flag for deleting all shapes currently present + ''' + self.delete = True + + def clearHistory(self): + ''' + Function for setting the flag for clearing the history of any line present + ''' + self.clear = True + self.state_handler(self.msg) + + def placeCheckBoxes(self): + ''' + Function for placing the checkboxes of the GUI. Each checkbox corresponds + to a shape/object that has been drawn with the corresponding color and + extension + ''' + + addToGUI = False + if (self.checkBoxArea == None): + self.checkBoxArea = QVBoxLayout() + addToGUI = True + for name in self.shapes: + # create appropriate symbol extention + extension = " •" + type = self.shapes[name].type + if (type == "point"): + extension = " •" + elif (type == "line"): + extension = " ---" + elif (type == "axes"): + extension = " |/_" + + # create each checkbox and conditionally add it to the GUI + addToList = False + if (name not in self.checkBoxes): + self.checkBoxes[name] = QCheckBox(name + extension) + + if (self.shapes[name].type == "point" or self.shapes[ + name].type == "line"): + color = self.shapes[name].color + self.checkBoxes[name].setStyleSheet( + "color: rgb(" + str(color[0] * 255) + ", " + str( + color[1] * 255) + ", " + str(color[2] * 255) + ")") + + addToList = True + self.checkBoxes[name].setChecked(True) + if (addToList == True): + self.checkBoxArea.addWidget(self.checkBoxes[name]) + if (addToGUI == True): + self.vbox.addLayout(self.checkBoxArea) + + def abstract_handler(self, msg, name, lcmMessage): + ''' + Function for handling LCM messages originating from a different + channel than the main one + + msg: the returning LCM message + field: the field name/path + name: name of corresponding shape + x, y, z: indices for each of the 3 dimensions of the location where + the shape is to be drawn. When the shape is an axis then x + will be the first index of the quaternion array and the other + 3 will be the next indices in sequence + ''' + self.handle_checkBox(name) + attribute = msg + field = lcmMessage.field + + # if there is a %d in the field replace it with the actual index + if ("%d" in field): + arr = self.GetVector(attribute, lcmMessage.index_field) + if (lcmMessage.index_element in arr): + i = arr.index(lcmMessage.index_element) + field = field.replace("%d", str(i)) + + if ("%d" not in field): + # parse field to get the appropriate information. This is done by + # recursively searching for the right attribute in the given message's field + attribute = self.GetVector(attribute, field) + curr_shape = self.shapes[name] + x = lcmMessage.x + + # special case of an axis data + if (curr_shape.type == "axes"): + # get quaternion array, normalize it, and get the corresponding rotation matrix + quaternion = [] + for i in range(x, x + 4): + quaternion.append(attribute[i]) + norm = np.linalg.norm(quaternion) + quaternion = [x / norm for x in quaternion] + rot_matrix = Quaternion(quaternion).rotation() + + pt_world = self.plant.CalcPointsPositions(self.context, + self.plant.GetFrameByName( + curr_shape.frame), + curr_shape.point, + self.plant.world_frame()) + next_loc = pt_world.transpose()[0] + + self.drawShape(self.shapes[name], next_loc, msg, + rotation_matrix=rot_matrix) + + else: + pelvis_pos = self.plant.CalcPointsPositions(self.context, + self.plant.GetFrameByName( + "pelvis"), + np.zeros(3), + self.plant.world_frame()) + # print(pelvis_pos.shape) + # print(np.array(attribute).shape) + attribute = np.array(attribute) + pelvis_pos[:, 0] + next_loc = [attribute[x], attribute[x + 1], attribute[x + 2]] + self.drawShape(self.shapes[name], next_loc, msg) + else: + print( + lcmMessage.index_element + "is not present in " + lcmMessage.index_field + " and so it cannot be visualized") + print("") + + def GetVector(self, attribute, path): + """ + Function for getting the pointer to a specific field of a given + attribute using the given path + + attribute: the object containing the desired field + path: the string path to get the attribute's field + """ + field = path.split(".") + regExpr = re.compile('.+\[\d+\]') + newAtribute = attribute + for part in field: + index = None + attName = None + + if regExpr.match(part) is not None: + index = part.split("[")[1].split("]")[0] + attName = part.split("[")[0] + newAtribute = getattr(newAtribute, attName)[int(index)] + else: + index = None + attName = part + newAtribute = getattr(newAtribute, attName) + + return newAtribute + + def handle_checkBox(self, name): + ''' + Function for checking if a checkBox is checked or not + + name: name of corresponding shape + ''' + if (self.checkBoxes[name].isChecked() == True): + if (self.shapes[name].object != None and self.shapes[ + name].object.getProperty('Visible') == False): + self.shapes[name].object.setProperty('Visible', True) + + else: + if (self.shapes[name].object != None and self.shapes[ + name].object.getProperty('Visible') == True): + self.shapes[name].object.setProperty('Visible', False) + + def state_handler(self, msg): + ''' + Function for handling main lcm channel - return newAtribute + msg: the returning LCM message + ''' + # start listening to messages once the JSON file has been read + if (self.ready == True): + # TODO (for Michael): bind our more robust decoding mechanisms to python + self.msg = msg + # positions = np.zeros(self.nq) + # for i in range(self.nq): + # j = self.position_idx_map[msg.position_names[i]] + # positions[j] = msg.position[i] + # velocities = np.zeros(self.nv) + # for i in range(self.nv): + # j = self.velocity_idx_map[msg.velocity_names[i]] + # velocities[j] = msg.velocity[i] + # efforts = np.zeros(self.nu) + # for i in range(self.nu): + # j = self.effort_idx_map[msg.effort_names[i]] + # efforts[j] = msg.effort[i] + self.plant.SetPositions(self.context, self.msg.position) + self.plant.SetVelocities(self.context, self.msg.velocity) + + # iterate through each shape to draw it + for name in self.shapes: + self.handle_checkBox(name) + currShape = self.shapes[name] + + # define next_loc according to each shape/object to be drawn + next_loc = None + if (currShape.category == "kinematic"): + # Use Drake's CalcPointsPositions to determine where that point is + # in the world + pt_world = self.plant.CalcPointsPositions(self.context, + self.plant.GetFrameByName( + currShape.frame), + currShape.point, + self.plant.world_frame()) + next_loc = pt_world.transpose()[0] + + elif (currShape.category == "com"): + next_loc = self.plant.CalcCenterOfMassPositionInWorld( + context=self.context) + + elif (currShape.category == "lcm"): + # in the case of an lcm message do not do anything as this is + # handled by the abstract handler + continue + + self.drawShape(currShape, next_loc, self.msg) + + # handle flags for clearing line histories + if (self.clear == True): + for shape in self.shapes.values(): + if (shape.type == "line"): + om.removeFromObjectModel(shape.object) + shape.object = None + shape.points = deque() + self.clear = False + # handle flags for deleting objects + if (self.delete == True): + for shape in self.shapes.values(): + om.removeFromObjectModel(shape.object) + + self.delete = False - def handle_checkBox(self, name): - ''' - Function for checking if a checkBox is checked or not + # delete checkboxes from GUI + for i in reversed(range(self.checkBoxArea.count())): + self.checkBoxArea.itemAt(i).widget().deleteLater() - name: name of corresponding shape - ''' - if (self.checkBoxes[name].isChecked() == True): - if (self.shapes[name].object != None and self.shapes[name].object.getProperty('Visible') == False): - self.shapes[name].object.setProperty('Visible', True) + # delete "reset" and "clear history" buttons + self.resetBtn.deleteLater() + self.clearBtn.deleteLater() + + # remove all lcm channels except for the state one + for subscription in self.subscriptions: + lcmUtils.removeSubscriber(subscription) + + self.resetGUI() + + def drawShape(self, curr_shape, next_loc, msg, rotation_matrix=None): + ''' + Function for drawing shapes. Currently this supports lines, points, and + 3D axes + + currShape: the current shape to be drawn + next_loc: the location where to draw the shape + msg: the message from the LCM hendler that called this function + rotation_matrix: the rotation matrix to be used for the axis shape. + Mainly used by the abstract handler + ''' + # draw a continuous line + if (curr_shape.type == "line"): + # check if the duration has been initialized + if (curr_shape.duration == None or len(curr_shape.points) == 0): + curr_shape.duration = msg.utime / 1000000 + + # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 + if ((( + msg.utime / 1000000) - curr_shape.duration <= curr_shape.history) or curr_shape.history <= 0): + # make sure to add at least 2 points before starting to check for the distance between points + if (len(curr_shape.points) < 2): + curr_shape.points.append(next_loc) + d = DebugData() + d.addPolyLine(curr_shape.points, radius=curr_shape.thickness, + color=curr_shape.color) + self.meshcat.SetLine(curr_shape.name, np.array(curr_shape.points).T, + line_width=curr_shape.thickness * 1000, + rgba=Rgba(curr_shape.color[0], + curr_shape.color[1], + curr_shape.color[2], + 1.0)) + + if (curr_shape.object == None): + curr_shape.object = vis.showPolyData(d.getPolyData(), + curr_shape.name) + curr_shape.object.setProperty('Color', curr_shape.color) + else: + curr_shape.object.setPolyData(d.getPolyData()) else: - if (self.shapes[name].object != None and self.shapes[name].object.getProperty('Visible') == True): - self.shapes[name].object.setProperty('Visible', False) - - def state_handler(self, msg): - ''' - Function for handling main lcm channel - - msg: the returning LCM message - ''' - # start listenning to messages once the JSON file has been read - if (self.ready == True): - # TODO (for Michael): bind our more robust decoding mechanisms to python - self.msg = msg - self.plant.SetPositions(self.context, self.msg.position) - self.plant.SetVelocities(self.context, self.msg.velocity) - - # iterate through each shape to draw it - for name in self.shapes: - self.handle_checkBox(name) - currShape = self.shapes[name] - - # define next_loc according to each shape/object to be drawn - next_loc = None - if (currShape.category == "kinematic"): - # Use Drake's CalcPointsPositions to determine where that point is - # in the world - pt_world = self.plant.CalcPointsPositions(self.context, - self.plant.GetFrameByName(currShape.frame), - currShape.point, self.plant.world_frame()) - next_loc = pt_world.transpose()[0] - - elif (currShape.category == "com"): - next_loc = self.plant.CalcCenterOfMassPositionInWorld(context = self.context) - - elif (currShape.category == "lcm"): - # in the case of an lcm message do not do anything as this is - # handled by the abstract handler - continue - - self.drawShape(currShape, next_loc, self.msg) - - # handle flags for clearing line histories - if (self.clear == True): - for shape in self.shapes.values(): - if (shape.type == "line"): - om.removeFromObjectModel(shape.object) - shape.object = None - shape.points = deque() - self.clear = False - - # handle flags for deleting objects - if (self.delete == True): - for shape in self.shapes.values(): - om.removeFromObjectModel(shape.object) - - self.delete = False - - # delete checkboxes from GUI - for i in reversed(range(self.checkBoxArea.count())): - self.checkBoxArea.itemAt(i).widget().deleteLater() - - # delete "reset" and "clear history" buttons - self.resetBtn.deleteLater() - self.clearBtn.deleteLater() - - # remove all lcm channels except for the state one - for subscription in self.subscriptions: - lcmUtils.removeSubscriber(subscription) - - self.resetGUI() - - - def drawShape(self, currShape, next_loc, msg, rotation_matrix = None): - ''' - Function for drawing shapes. Currently this supports lines, points, and - 3D axes - - currShape: the current shape to be drawn - next_loc: the location where to draw the shape - msg: the message from the LCM hendler that called this function - rotation_matrix: the rotation matrix to be used for the axis shape. - Mainly used by the abstract handler - ''' - # draw a continuous line - if (currShape.type == "line"): - # check if the duration has been initialized - if (currShape.duration == None or len(currShape.points) == 0): - currShape.duration = msg.utime / 1000000 - - # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 - if (((msg.utime / 1000000) - currShape.duration <= currShape.history) or currShape.history <= 0): - # make sure to add at least 2 points before starting to check for the distance between points - if (len(currShape.points) < 2): - currShape.points.append(next_loc) - d = DebugData() - d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) - - if (currShape.object == None): - currShape.object = vis.showPolyData(d.getPolyData(), currShape.name) - currShape.object.setProperty('Color', currShape.color) - else: - currShape.object.setPolyData(d.getPolyData()) - - else: - if (np.linalg.norm(np.array(next_loc) -np.array(currShape.points[-1])) >= 10e-5): - currShape.points.append(next_loc) - d = DebugData() - d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) - - if (currShape.object == None): - currShape.object = vis.showPolyData(d.getPolyData(), currShape.name) - else: - currShape.object.setPolyData(d.getPolyData()) - - elif (currShape.history > 0): - if (len(currShape.points) == 0): - currShape.duration = msg.utime / 1000000 - else: - # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 - if (np.linalg.norm(np.array(next_loc) -np.array(currShape.points[-1])) >= 10e-5): - currShape.points.popleft() - currShape.points.append(next_loc) - d = DebugData() - d.addPolyLine(currShape.points, radius=currShape.thickness, color=currShape.color) - if (currShape.object == None): - currShape.object = vis.showPolyData(d.getPolyData(), currShape.name) - else: - currShape.object.setPolyData(d.getPolyData()) - - # draw a point - elif (currShape.type == "point"): + if (np.linalg.norm( + np.array(next_loc) - np.array(curr_shape.points[-1])) >= 10e-5): + curr_shape.points.append(next_loc) d = DebugData() - d.addSphere(next_loc, radius = currShape.radius) - # create a new point - if (currShape.created == True): - currShape.object = vis.showPolyData(d.getPolyData(), currShape.name) - # set color and transparency of point - currShape.object.setProperty('Color', currShape.color) - currShape.object.setProperty('Alpha', currShape.alpha) - currShape.created = False + d.addPolyLine(curr_shape.points, radius=curr_shape.thickness, + color=curr_shape.color) + self.meshcat.SetLine(curr_shape.name, np.array(curr_shape.points).T, + line_width=curr_shape.thickness * 1000, + rgba=Rgba(curr_shape.color[0], + curr_shape.color[1], + curr_shape.color[2], + 1.0)) + + if (curr_shape.object == None): + curr_shape.object = vis.showPolyData(d.getPolyData(), + curr_shape.name) else: - # update the location of the last point - currShape.object.setPolyData(d.getPolyData()) - - # draw a set of axes - elif (currShape.type == "axes"): - # get the rotation matrix - rot_matrix = None - if (currShape.category != "lcm"): - rigTrans = self.plant.EvalBodyPoseInWorld(self.context, self.plant.GetBodyByName(currShape.frame)) - rot_matrix = rigTrans.rotation().matrix().transpose() - else: - rot_matrix = rotation_matrix.transpose() - - colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + curr_shape.object.setPolyData(d.getPolyData()) + elif (curr_shape.history > 0): + if (len(curr_shape.points) == 0): + curr_shape.duration = msg.utime / 1000000 + else: + # visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5 + if (np.linalg.norm( + np.array(next_loc) - np.array(curr_shape.points[-1])) >= 10e-5): + curr_shape.points.popleft() + curr_shape.points.append(next_loc) d = DebugData() - for i in range(3): - d.addArrow(next_loc, next_loc + (rot_matrix[i]*currShape.length), headRadius=0.03, color = colors[i]) - - # create the 3 axes - if (currShape.created == True): - currShape.object = vis.showPolyData(d.getPolyData(), currShape.name, colorByName='RGB255') - currShape.object.setProperty('Alpha', currShape.alpha) - currShape.created = False + d.addPolyLine(curr_shape.points, radius=curr_shape.thickness, + color=curr_shape.color) + self.meshcat.SetLine(curr_shape.name, np.array(curr_shape.points).T, + line_width=curr_shape.thickness * 1000, + rgba=Rgba(curr_shape.color[0], + curr_shape.color[1], + curr_shape.color[2], + 1.0)) + if (curr_shape.object == None): + curr_shape.object = vis.showPolyData(d.getPolyData(), + curr_shape.name) else: - # update the location of the last point - currShape.object.setPolyData(d.getPolyData()) + curr_shape.object.setPolyData(d.getPolyData()) + + # draw a point + elif (curr_shape.type == "point"): + d = DebugData() + d.addSphere(next_loc, radius=curr_shape.radius) + # create a new point + if (curr_shape.created == True): + curr_shape.object = vis.showPolyData(d.getPolyData(), curr_shape.name) + # set color and transparency of point + curr_shape.object.setProperty('Color', curr_shape.color) + curr_shape.object.setProperty('Alpha', curr_shape.alpha) + curr_shape.created = False + else: + # update the location of the last point + curr_shape.object.setPolyData(d.getPolyData()) + + # draw a set of axes + elif (curr_shape.type == "axes"): + # get the rotation matrix + rot_matrix = None + if (curr_shape.category != "lcm"): + rigTrans = self.plant.EvalBodyPoseInWorld(self.context, + self.plant.GetBodyByName( + curr_shape.frame)) + rot_matrix = rigTrans.rotation().matrix().transpose() + else: + rot_matrix = rotation_matrix + + colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + + d = DebugData() + for i in range(3): + d.addArrow(next_loc, next_loc + (rot_matrix[i] * curr_shape.length), + headRadius=0.03, color=colors[i]) + + # create the 3 axes + if (curr_shape.created == True): + curr_shape.object = vis.showPolyData(d.getPolyData(), curr_shape.name, + colorByName='RGB255') + curr_shape.object.setProperty('Alpha', curr_shape.alpha) + curr_shape.created = False + else: + # update the location of the last point + curr_shape.object.setPolyData(d.getPolyData()) + class ObjectToDraw(): + ''' + Wrapper class for any object/shape being drawn + ''' + + def __init__(self, data): + # set attributes from given data (originating from input JSON file) + self.name = data['name'] + + self.source_data = data['source_data'] + type_data = data['type_data'] + + self.category = self.source_data['category'] + + if (self.category == "kinematic"): + self.frame = self.source_data['frame'] + self.point = self.source_data['point'] + + self.alpha = type_data['alpha'] + self.type = type_data['type'] + self.created = True + self.object = None + + if (self.type == "line"): + self.color = type_data['color'] + self.thickness = type_data['thickness'] + self.history = type_data['history'] + self.duration = None + self.points = deque() + + elif (self.type == "point"): + self.color = type_data['color'] + self.radius = type_data['radius'] + + elif (self.type == "axes"): + if (self.category == "lcm"): + self.frame = self.source_data['frame'] + self.point = self.source_data['point'] + self.quaternion_index = self.source_data['quaternion_index'] + self.thickness = type_data['thickness'] + self.length = type_data['length'] + + def update(self, otherObject): ''' - Wrapper class for any object/shape being drawn + Function for updating certain attributes of already existing object + + otherObject: the other ObcectToDraw instance ''' - def __init__(self, data): - # set attributes from given data (originating from input JSON file) - self.name = data['name'] - - self.source_data = data['source_data'] - type_data = data['type_data'] - - self.category = self.source_data['category'] - - if (self.category == "kinematic"): - self.frame = self.source_data['frame'] - self.point = self.source_data['point'] - - self.alpha = type_data['alpha'] - self.type = type_data['type'] - self.created = True - self.object = None - - if (self.type == "line"): - self.color = type_data['color'] - self.thickness = type_data['thickness'] - self.history = type_data['history'] - self.duration = None - self.points = deque() - - elif (self.type == "point"): - self.color = type_data['color'] - self.radius = type_data['radius'] - - elif (self.type == "axes"): - if (self.category == "lcm"): - self.frame = self.source_data['frame'] - self.point = self.source_data['point'] - self.quaternion_index = self.source_data['quaternion_index'] - self.thickness = type_data['thickness'] - self.length = type_data['length'] - - def update(self, otherObject): - ''' - Function for updating certain attributes of already existing object - - otherObject: the other ObcectToDraw instance - ''' - self.alpha = otherObject.alpha - - if (self.category == "kinematic"): - self.frame = otherObject.frame - self.point = otherObject.point - - if (self.category == "lcm"): - self.source_data = otherObject.source_data - - if (self.type == "line"): - self.color = otherObject.color - self.thickness = otherObject.thickness - self.history = otherObject.history - - elif (self.type == "point"): - self.color = otherObject.color - self.radius = otherObject.radius - - elif (self.type == "axes"): - self.thickness = otherObject.thickness - self.length = otherObject.length + self.alpha = otherObject.alpha + + if (self.category == "kinematic"): + self.frame = otherObject.frame + self.point = otherObject.point + + if (self.category == "lcm"): + self.source_data = otherObject.source_data + + if (self.type == "line"): + self.color = otherObject.color + self.thickness = otherObject.thickness + self.history = otherObject.history + + elif (self.type == "point"): + self.color = otherObject.color + self.radius = otherObject.radius + + elif (self.type == "axes"): + self.thickness = otherObject.thickness + self.length = otherObject.length + class LCMMessage(): + ''' + Wrapper class for LCM messages + ''' + + def __init__(self, source_data, axis=False): ''' - Wrapper class for LCM messages + Constructor ''' - def __init__(self, source_data, axis = False): - ''' - Constructor - ''' - self.channel = source_data['abstract_channel'] - self.type = source_data['abstract_type'] - self.field = source_data['abstract_field'] - - if ("[%d]" in self.field): - self.index_field = source_data['index_field'] - self.index_element = source_data['index_element'] - - if (axis == True): - self.x = source_data["quaternion_index"] - else: - self.x = source_data["x_index"] - - def update(self, source_data, axis = False): - ''' - Same as constructor but is used to update an LCMMessage - object with new source_data - ''' - self.channel = source_data['abstract_channel'] - self.type = source_data['abstract_type'] - self.field = source_data['abstract_field'] - - if ("[%d]" in self.field): - self.index_field = source_data['index_field'] - self.index_element = source_data['index_element'] - - if (axis == True): - self.x = source_data["quaternion_index"] - else: - self.x = source_data["x_index"] + self.channel = source_data['abstract_channel'] + self.type = source_data['abstract_type'] + self.field = source_data['abstract_field'] + + if ("[%d]" in self.field): + self.index_field = source_data['index_field'] + self.index_element = source_data['index_element'] + + if (axis == True): + self.x = source_data["quaternion_index"] + else: + self.x = source_data["x_index"] + + def update(self, source_data, axis=False): + ''' + Same as constructor but is used to update an LCMMessage + object with new source_data + ''' + self.channel = source_data['abstract_channel'] + self.type = source_data['abstract_type'] + self.field = source_data['abstract_field'] + + if ("[%d]" in self.field): + self.index_field = source_data['index_field'] + self.index_element = source_data['index_element'] + + if (axis == True): + self.x = source_data["quaternion_index"] + else: + self.x = source_data["x_index"] + # Adding the widget panel = VisualizationGui() diff --git a/director/scripts/cassie_test.json b/director/scripts/cassie_test.json index 504954f2c6..66c4233f22 100644 --- a/director/scripts/cassie_test.json +++ b/director/scripts/cassie_test.json @@ -2,80 +2,166 @@ "model_file": "examples/Cassie/urdf/cassie_v2.urdf", "channelName": "CASSIE_STATE_SIMULATION", "data": [ - {"name": "toe_point", - "source_data" : {"category":"kinematic", - "frame":"toe_left", - "point":[0, 0, 0] - }, - "type_data": {"color":[0, 1, 0], - "alpha": 0.5, - "type": "point", - "radius": 0.05 - } - - }, - {"name": "right_toe_line", - "source_data" : {"category":"kinematic", - "frame":"toe_right", - "point":[0, 0, 0] - }, - "type_data" : {"color":[1, 0, 1], - "alpha": 0.5, - "type": "line", - "thickness": 0.01, - "history": 2 - } - }, - {"name": "left_toe_axes", - "source_data" : {"category":"kinematic", - "frame":"toe_left", - "point":[0, 0, 0] - }, - "type_data" : {"alpha": 0.5, - "type": "axes", - "thickness": 0.01, - "length": 0.25 - } - }, - {"name": "com_point", - "source_data" : {"category":"com"}, - "type_data" : {"color":[0, 1, 0], - "alpha": 0.5, - "type": "point", - "radius": 0.05 - } - }, - - {"name": "right_toe_axis", - "source_data" : {"category":"lcm", - "abstract_channel" : "CASSIE_STATE_SIMULATION", - "abstract_type" : "dairlib.lcmt_robot_output", - "abstract_field" : "position", - "quaternion_index" : 0, - "frame": "toe_right", - "point": [0, 0, 0] - }, - "type_data": {"alpha": 0.5, - "type": "axes", - "thickness": 0.01, - "length": 0.25 - } - }, - {"name": "lcm_line", - "source_data" : {"category":"lcm", - "abstract_channel" : "OSC_DEBUG", - "abstract_type" : "dairlib.lcmt_osc_output", - "abstract_field" : "tracking_data[%d].y", - "index_field": "tracking_data_names", - "index_element": "lipm_traj", - "x_index" : 0 - }, - "type_data": {"color":[0, 0, 1], - "alpha": 0.5, - "type": "line", - "thickness": 0.01, - "history": -1 - } - } - ] + { + "name": "pelvis", + "source_data": { + "category": "kinematic", + "frame": "pelvis", + "point": [ + 0, + 0, + 0 + ] + }, + "type_data": { + "color": [ + 0, + 0, + 1 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.01, + "history": 2 + } + }, + { + "name": "left_toe_line", + "source_data": { + "category": "kinematic", + "frame": "toe_left", + "point": [ + 0, + 0, + 0 + ] + }, + "type_data": { + "color": [ + 1, + 0, + 1 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.01, + "history": 2 + } + }, + { + "name": "right_toe_line", + "source_data": { + "category": "kinematic", + "frame": "toe_right", + "point": [ + 0, + 0, + 0 + ] + }, + "type_data": { + "color": [ + 1, + 0, + 1 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.01, + "history": 2 + } + }, + { + "name": "left_ft_traj", + "source_data": { + "category": "lcm", + "abstract_channel": "OSC_DEBUG_RUNNING", + "abstract_type": "dairlib.lcmt_osc_output", + "abstract_field": "tracking_data[%d].y", + "index_field": "tracking_data_names", + "index_element": "left_ft_traj", + "x_index": 0 + }, + "type_data": { + "color": [ + 0, + 0, + 1 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.005, + "history": 1 + } + }, + { + "name": "left_ft_traj_des", + "source_data": { + "category": "lcm", + "abstract_channel": "OSC_DEBUG_RUNNING", + "abstract_type": "dairlib.lcmt_osc_output", + "abstract_field": "tracking_data[%d].y_des", + "index_field": "tracking_data_names", + "index_element": "left_ft_traj", + "x_index": 0 + }, + "type_data": { + "color": [ + 1, + 1, + 1 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.005, + "history": 1 + } + }, + { + "name": "right_ft_traj", + "source_data": { + "category": "lcm", + "abstract_channel": "OSC_DEBUG_RUNNING", + "abstract_type": "dairlib.lcmt_osc_output", + "abstract_field": "tracking_data[%d].y", + "index_field": "tracking_data_names", + "index_element": "right_ft_traj", + "x_index": 0 + }, + "type_data": { + "color": [ + 1, + 0, + 0 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.005, + "history": 1 + } + }, + { + "name": "right_ft_traj_des", + "source_data": { + "category": "lcm", + "abstract_channel": "OSC_DEBUG_RUNNING", + "abstract_type": "dairlib.lcmt_osc_output", + "abstract_field": "tracking_data[%d].y_des", + "index_field": "tracking_data_names", + "index_element": "right_ft_traj", + "x_index": 0 + }, + "type_data": { + "color": [ + 1, + 1, + 1 + ], + "alpha": 0.5, + "type": "line", + "thickness": 0.005, + "history": 1 + } + } + ] } diff --git a/environ.bzl b/environ.bzl index 13eebd0625..62d1d3f06f 100644 --- a/environ.bzl +++ b/environ.bzl @@ -50,21 +50,37 @@ def _impl(repository_ctx): executable = False, ) -def environ_repository(name = None, vars = []): - """Provide specific environment variables for use in a WORKSPACE file. - The `vars` are the environment variables to provide. +#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": attr.string_list(default = vars), +# }, +# local = True, +# environ = vars, +# ) +# rule(name = name) + +#def +#def drake_repository(): +drake_repository = repository_rule( + implementation = _impl, + environ = ["DAIRLIB_LOCAL_DRAKE_PATH"], + local = True, + attrs = {"_vars": attr.string_list(default=["DAIRLIB_LOCAL_DRAKE_PATH"])}, +) +inekf_repository = repository_rule( + implementation = _impl, + environ = ["DAIRLIB_LOCAL_INEKF_PATH"], + local = True, + attrs = {"_vars": attr.string_list(default=["DAIRLIB_LOCAL_INEKF_PATH"])}, +) - Example: - environ_repository(name = "foo", vars = ["BAR", "BAZ"]) - load("@foo//:environ.bzl", "BAR", "BAZ") - print(BAR) - """ - rule = repository_rule( - implementation = _impl, - attrs = { - "_vars": attr.string_list(default = vars), - }, - local = True, - environ = vars, - ) - rule(name = name) diff --git a/examples/Cassie/BUILD.bazel b/examples/Cassie/BUILD.bazel index 0132877122..8ddcdceccf 100644 --- a/examples/Cassie/BUILD.bazel +++ b/examples/Cassie/BUILD.bazel @@ -66,7 +66,7 @@ cc_library( hdrs = ["cassie_state_estimator_settings.h"], deps = [ "@drake//:drake_shared_library", - ] + ], ) cc_library( @@ -75,9 +75,9 @@ cc_library( hdrs = ["cassie_state_estimator.h"], deps = [ ":cassie_utils", + "//examples/Cassie:cassie_state_estimator_settings", "//examples/Cassie/datatypes:cassie_names", "//examples/Cassie/datatypes:cassie_out_t", - "//examples/Cassie:cassie_state_estimator_settings", "//multibody:utils", "//multibody/kinematic", "//systems/framework:vector", @@ -95,20 +95,6 @@ cc_binary( ], ) -cc_test( - name = "input_supervisor_test", - size = "small", - srcs = ["test/input_supervisor_test.cc"], - deps = [ - "//examples/Cassie:cassie_urdf", - "//examples/Cassie:cassie_utils", - "//examples/Cassie/systems:input_supervisor", - "//systems/primitives", - "@drake//:drake_shared_library", - "@gtest//:main", - ], -) - cc_library( name = "cassie_fixed_point_solver", srcs = ["cassie_fixed_point_solver.cc"], @@ -185,9 +171,9 @@ cc_binary( "//examples/Cassie/systems:cassie_encoder", "//solvers:optimization_utils", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/framework:geared_motor", "//systems/primitives", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -249,9 +235,9 @@ cc_binary( "//lcmtypes:lcmt_robot", "//multibody:multibody_solvers", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/framework:lcm_driven_loop", "//systems/primitives", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -269,9 +255,9 @@ cc_binary( "//examples/Cassie/systems:input_supervisor", "//lcmtypes:lcmt_robot", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/controllers:linear_controller", "//systems/controllers:pd_config_lcm", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -296,8 +282,8 @@ cc_binary( deps = [ "//lcmtypes:lcmt_robot", "@drake//lcm", - "@lcm", "@gflags", + "@lcm", ], ) @@ -307,8 +293,8 @@ cc_binary( deps = [ "//lcmtypes:lcmt_robot", "@drake//lcm", - "@lcm", "@gflags", + "@lcm", ], ) @@ -350,18 +336,18 @@ cc_binary( deps = [ ":cassie_urdf", ":cassie_utils", - "//examples/Cassie/osc", "//examples/Cassie/contact_scheduler:all", - "//examples/Cassie/systems:cassie_out_to_radio", + "//examples/Cassie/osc", "//examples/Cassie/osc_jump", "//examples/Cassie/osc_run", + "//examples/Cassie/systems:cassie_out_to_radio", "//lcm:trajectory_saver", "//multibody:utils", "//systems:robot_lcm_systems", "//systems:system_utils", - "//systems/filters:floating_base_velocity_filter", - "//systems/controllers/osc:osc_tracking_datas", "//systems/controllers:controllers_all", + "//systems/controllers/osc:osc_tracking_datas", + "//systems/filters:floating_base_velocity_filter", "//systems/framework:lcm_driven_loop", "//systems/primitives", "@drake//:drake_shared_library", @@ -382,11 +368,11 @@ cc_binary( "//multibody:view_frame", "//multibody/kinematic", "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers:controllers_all", "//systems/controllers:fsm_event_time", "//systems/framework:lcm_driven_loop", "//systems/primitives", - "//systems/controllers:controllers_all", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -399,17 +385,17 @@ cc_binary( ":cassie_urdf", ":cassie_utils", "//examples/Cassie/osc", - "//examples/impact_invariant_control:impact_aware_time_based_fsm", "//examples/Cassie/systems:cassie_out_to_radio", "//examples/Cassie/systems:simulator_drift", + "//examples/impact_invariant_control:impact_aware_time_based_fsm", "//multibody:utils", "//multibody:view_frame", "//multibody/kinematic", "//solvers:solver_options_io", "//systems:robot_lcm_systems", "//systems:system_utils", - "//systems/controllers:fsm_event_time", "//systems/controllers:controllers_all", + "//systems/controllers:fsm_event_time", "//systems/filters:floating_base_velocity_filter", "//systems/framework:lcm_driven_loop", "//systems/primitives", @@ -429,11 +415,11 @@ cc_binary( "//multibody:utils", "//multibody/kinematic", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/controllers/osc:operational_space_control", "//systems/controllers/osc:osc_gains", "//systems/framework:lcm_driven_loop", "//systems/primitives", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -458,7 +444,7 @@ cc_binary( cc_binary( name = "run_dircon_squatting", srcs = ["run_dircon_squatting.cc"], - data = glob(["examples/Cassie/urdf/cassie_fixed_springs.urdf"]), + data = glob(["urdf/cassie_fixed_springs.urdf"]), deps = [ ":cassie_fixed_point_solver", ":cassie_utils", @@ -497,7 +483,7 @@ cc_binary( cc_binary( name = "run_dircon_walking", srcs = ["run_dircon_walking.cc"], - data = glob(["examples/Cassie/urdf/cassie_fixed_springs.urdf"]), + data = glob(["urdf/cassie_fixed_springs.urdf"]), deps = [ ":cassie_utils", "//common", diff --git a/examples/Cassie/README.md b/examples/Cassie/README.md index fbd298338c..c7f5eb3a73 100644 --- a/examples/Cassie/README.md +++ b/examples/Cassie/README.md @@ -2,7 +2,7 @@ Launch bot-procman-sherrif using the cassie_test.pmd configuration file. Currently, you must bein the root `dairlib` directory for this to work properly. ``` -bot-procman-sherrif examples/Cassie/cassie_test.pmd +bot-procman-sheriff -l examples/Cassie/cassie_simulation.pmd ``` Note: if you've installed libbot2, using apt, `bot-procman-sherrif` is probably located in `/opt/libbot2/0.0.1.20180130/bin/` or a similar directory. Running `bot-procman-sherrif -l` will allow you to skip the "Spawn local deputy" step. diff --git a/examples/Cassie/cassie_dynamic_motions.pmd b/examples/Cassie/cassie_dynamic_motions.pmd new file mode 100644 index 0000000000..a89d50f2e5 --- /dev/null +++ b/examples/Cassie/cassie_dynamic_motions.pmd @@ -0,0 +1,186 @@ +group "0.operator" { + cmd "drake-director-clean" { + exec = "bazel-bin/director/drake-director --use_builtin_scripts=point_pair_contact --script=examples/Cassie/director_scripts/show_time.py --script=examples/Cassie/director_scripts/controller_status.py"; + host = "localhost"; + } + cmd "switch_to_jump" { + exec = "bazel-bin/examples/Cassie/run_controller_switch --new_channel=\"PD_CONTROL\" --n_publishes=1"; + host = "localhost"; + } + cmd "4.state-visualizer-floating" { + exec = "bazel-bin/examples/Cassie/visualizer --channel=CASSIE_STATE_SIMULATION"; + host = "localhost"; + } + cmd "0.drake-director" { + exec = "bazel-bin/director/drake-director --use_builtin_scripts=point_pair_contact,frame,image --script examples/Cassie/director_scripts/pd_panel.py --script examples/Cassie/director_scripts/show_time.py"; + host = "localhost"; + } + cmd "state-visualizer-dispatcher" { + exec = "bazel-bin/examples/Cassie/visualizer --channel=CASSIE_STATE_DISPATCHER"; + host = "localhost"; + } + cmd "switch_to_running" { + exec = "bazel-bin/examples/Cassie/run_controller_switch --new_channel=\"OSC_RUNNING\" --n_publishes=1 --channel_x=CASSIE_STATE_SIMULATION --fsm_period=0.47 --fsm_offset=0.2 --blend_duration=0.05 --n_period_delay=1"; + host = "localhost"; + } +} + +group "5.trajectory-optimization" { + cmd "dircon_jumping" { + exec = "bazel-bin/examples/Cassie/run_dircon_jumping --height=0.4 --distance=0.3 --start_height=0.8 --knot_points=12 --save_filename=\"jumping_box_0.4h_0.3d_2\" --load_filename=\"jumping_box_0.4h_0.3d_9\" --same_knotpoints=1 --cost_scaling=5e-5 --tol=1e-7"; + host = "localhost"; + } + cmd "visualize_jumping_trajectory" { + exec = "bazel-bin/examples/Cassie/visualize_trajectory --folder_path=\"examples/Cassie/saved_trajectories/\" --realtime_rate=1.0 --num_poses=20 --visualize_mode=1 --use_transparency=1 --trajectory_name=\"jumping_box_0.4h_0.3d_1\""; + host = "localhost"; + } + cmd "convert_traj_for_sim_jumping" { + exec = "bazel-bin/examples/Cassie/osc_jump/convert_traj_for_sim --trajectory_name=\"jumping_0.15h_0.3d\" "; + host = "localhost"; + } + cmd "convert_traj_for_controller" { + exec = "bazel-bin/examples/Cassie/osc_jump/convert_traj_for_controller --folder_path=\"/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/\" --trajectory_name=\"jumping_box_0.5h_0.3d_1\" --relative_feet=1"; + host = "localhost"; + } + cmd "visualize_walking_trajectory" { + exec = "bazel-bin/examples/Cassie/visualize_trajectory --folder_path=\"/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/\" --trajectory_name=\"walking_0.5\" --realtime_rate=0.1 --num_poses=6 --visualize_mode=1 --use_transparency=1"; + host = "localhost"; + } + cmd "convert_traj_for_walking" { + exec = "bazel-bin/examples/Cassie/osc_run/convert_traj_for_controller --folder_path=\"/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/\" --trajectory_name=running_0.00 --mirror_traj=1"; + host = "localhost"; + } + cmd "dircon_walking" { + exec = "bazel-bin/examples/Cassie/run_dircon_walking --stride_length=0.2 --knot_points=16 --save_filename=walking_0.16.0 --load_filename=walking_0.5 --duration=0.8 --scale_constraints=1"; + host = "localhost"; + } + cmd "dircon_running" { + exec = "bazel-bin/examples/Cassie/run_dircon_running --stride_length=0.00 --start_height=0.85 --knot_points=16 --save_filename=\"running_0.00_work3\" --load_filename=\"running_0.00_work3\" --ipopt=0 --tol=1e-6 --stance_T=0.2 --flight_phase_T=0.10 --same_knotpoints=1"; + host = "localhost"; + } + cmd "visualize_running_trajectory" { + exec = "bazel-bin/examples/Cassie/visualize_trajectory --folder_path=\"/home/yangwill/workspace/dairlib/examples/Cassie/saved_trajectories/\" --realtime_rate=1.0 --num_poses=12 --visualize_mode=1 --use_transparency=1 --trajectory_name=\"running_0.25\" --mirror_traj=1"; + host = "localhost"; + } + cmd "dircon_jumping_w_springs" { + exec = "bazel-bin/examples/Cassie/run_dircon_jumping --height=0.4 --distance=0.3 --start_height=0.8 --knot_points=16 --save_filename=\"springs/jumping_box_0.4h_0.3d_6\" --load_filename=\"springs/jumping_box_0.4h_0.3d_5\" --same_knotpoints=1 --use_springs=1 --convert_to_springs=0 --tol=1e-3 --ipopt=0"; + host = "localhost"; + } +} + +group "1.simulated-robot" { + cmd "dispatcher-robot-in" { + exec = "bazel-bin/examples/Cassie/dispatcher_robot_in --port 25000 --floating_base=true --max_joint_velocity=60 --control_channel_name_initial=\"OSC_STANDING\" --sim=1"; + host = "localhost"; + } + cmd "dispatcher-robot-out (lcm)" { + exec = "bazel-bin/examples/Cassie/dispatcher_robot_out --port 25001 --simulation=true --floating_base=true --contact_force_threshold=60"; + host = "localhost"; + } + cmd "mbp_sim_default" { + exec = "bazel-bin/examples/Cassie/multibody_sim --init_height=0.9 --target_realtime_rate=0.5 --dt=8e-5 --publish_rate=2000 --actuator_delay=0.000 --start_time=0.0"; + host = "localhost"; + } + cmd "mbp_sim_playback" { + exec = "bazel-bin/examples/Cassie/multibody_sim_playback --publish_rate=2000 --end_time=40.0 --dt=8e-5 --target_realtime_rate=0.1 --spring_model=1 --log_num=28 --start_time=30.595"; + host = "localhost"; + } + cmd "mbp_sim_w_platform" { + exec = "bazel-bin/examples/Cassie/multibody_sim_w_platform --publish_rate=2000 --end_time=10.0 --dt=8e-5 --start_time=0.000 --target_realtime_rate=1.0 --traj_name=\"jumping_box_0.5h_0.3d_1\" --platform_height=0.4 --platform_x=0.25 --spring_model=1 --actuator_delay=0.000 --visualize=1"; + host = "localhost"; + } + cmd "mbp_sim_running" { + exec = "bazel-bin/examples/Cassie/multibody_sim_w_platform --publish_rate=2000 --end_time=60.0 --dt=8e-5 --start_time=0.000 --target_realtime_rate=1.0 --traj_name=\"running_0.00\" --spring_model=1 --actuator_delay=0.000 --visualize=0"; + host = "localhost"; + } + cmd "cassie-mujoco" { + exec = "/home/yangwill/workspace/experimental/cassie-mujoco-sim/test/cassiesim -r -s"; + host = "localhost"; + } +} + +group "3.lcm-tools" { + cmd "0.lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } + cmd "1.signal-scope" { + exec = "bazel-bin/signalscope/signal-scope"; + host = "localhost"; + } +} + +group "2.controllers" { + cmd "osc_standing_controller" { + exec = "bazel-bin/examples/Cassie/run_osc_standing_controller --height=0.92 --cassie_out_channel=CASSIE_OUTPUT --channel_u=OSC_STANDING"; + host = "localhost"; + } + cmd "pd-controller" { + exec = "bazel-bin/examples/Cassie/run_pd_controller --channel_u=\"PD_CONTROL\""; + host = "localhost"; + } + cmd "osc_jumping_controller" { + exec = "bazel-bin/examples/Cassie/run_osc_jumping_controller --channel_u=CASSIE_INPUT --delay_time=2.0 --contact_based_fsm=0 --channel_x=CASSIE_STATE_DISPATCHER --traj_name=\"jumping_box_0.5h_0.3d_1\""; + host = "localhost"; + } + cmd "osc_walking_controller" { + exec = "bazel-bin/examples/Cassie/run_osc_walking_controller --use_radio=1 --cassie_out_channel=CASSIE_OUTPUT --channel_u=OSC_WALKING"; + host = "localhost"; + } + cmd "osc_walking_controller_tracking" { + exec = "bazel-bin/examples/Cassie/run_osc_walking_controller_tracking --traj_name=\"walking_0.5\""; + host = "localhost"; + } + cmd "osc_running_controller" { + exec = "bazel-bin/examples/Cassie/run_osc_running_controller --channel_cassie_out=CASSIE_OUTPUT --fsm_time_offset=0.031 --channel_u=OSC_RUNNING"; + host = "localhost"; + } +} + + +script "osc-jumping (drake)" { + stop cmd "osc_jumping_controller"; + stop cmd "mbp_sim" wait "stopped"; + start cmd "osc_jumping_controller" wait "running"; + start cmd "mbp_sim"; +} + +script "osc-jumping (mujoco)" { + stop cmd "osc_jumping_controller (mujoco)"; + stop cmd "dispatcher-robot-in"; + stop cmd "cassie-mujoco" wait "stopped"; + start cmd "osc_jumping_controller (mujoco)" wait "running"; + start cmd "dispatcher-robot-in"; + start cmd "cassie-mujoco"; +} + +script "osc_standing (mujoco)" { + start cmd "cassie-mujoco"; + start cmd "dispatcher-robot-in"; + start cmd "osc_standing_controller"; +} + +script "run-mujoco-lcm-pd-control" { + run_script "start-operator-MBP"; + start cmd "3.cassie-mujoco-fixed-base"; + start cmd "2.a.dispatcher-robot-out (lcm)"; + start cmd "3.dispatcher-robot-in"; + start cmd "0.pd-controller"; +} + +script "run-real-robot-pd-control" { + run_script "start-operator-real-robot"; + start cmd "0.dispatcher-robot-out-real-robot"; + start cmd "1.dispatcher-robot-in-real-robot"; + start cmd "2.pd-controller-real-robot"; +} + +script "switch-to-standing" { + start cmd "osc_standing_controller"; + stop cmd "osc_walking_controller"; +} + +script "switch-to-walking" { + start cmd "osc_walking_controller"; + stop cmd "osc_standing_controller"; +} diff --git a/examples/Cassie/cassie_fixed_point_solver.cc b/examples/Cassie/cassie_fixed_point_solver.cc index e4b6d394f2..fdd5f9f00e 100644 --- a/examples/Cassie/cassie_fixed_point_solver.cc +++ b/examples/Cassie/cassie_fixed_point_solver.cc @@ -88,14 +88,14 @@ void CassieFixedPointSolver( program.AddJointLimitConstraints(q); // Fix floating base - program.AddConstraint(q(positions_map.at("base_qw")) == 1); - program.AddConstraint(q(positions_map.at("base_qx")) == 0); - program.AddConstraint(q(positions_map.at("base_qy")) == 0); - program.AddConstraint(q(positions_map.at("base_qz")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_qw")) == 1); + program.AddConstraint(q(positions_map.at("pelvis_qx")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_qy")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_qz")) == 0); - program.AddConstraint(q(positions_map.at("base_x")) == 0); - program.AddConstraint(q(positions_map.at("base_y")) == 0); - program.AddConstraint(q(positions_map.at("base_z")) == height); + program.AddConstraint(q(positions_map.at("pelvis_x")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_y")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_z")) == height); // Add symmetry constraints, and zero roll/pitch on the hip program.AddConstraint(q(positions_map.at("knee_left")) == @@ -143,7 +143,7 @@ void CassieFixedPointSolver( // Set initial guess/cost for q using a vaguely neutral position Eigen::VectorXd q_guess = Eigen::VectorXd::Zero(plant.num_positions()); q_guess(0) = 1; // quaternion - q_guess(positions_map.at("base_z")) = height; + q_guess(positions_map.at("pelvis_z")) = height; q_guess(positions_map.at("hip_pitch_left")) = 1; q_guess(positions_map.at("knee_left")) = -2; q_guess(positions_map.at("ankle_joint_left")) = 2; @@ -334,27 +334,27 @@ void CassieInitStateSolver( int n_v = plant.num_velocities(); // Fix floating base - program.AddBoundingBoxConstraint(1, 1, q(positions_map.at("base_qw"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qx"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qy"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qz"))); + program.AddBoundingBoxConstraint(1, 1, q(positions_map.at("pelvis_qw"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qx"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qy"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qz"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_x"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_y"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_x"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_y"))); program.AddBoundingBoxConstraint(height, height, - q(positions_map.at("base_z"))); + q(positions_map.at("pelvis_z"))); program.AddBoundingBoxConstraint(-10, 10, v); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wx"))); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wy"))); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wz"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wx"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wy"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wz"))); program.AddBoundingBoxConstraint(pelvis_xy_vel(0), pelvis_xy_vel(0), - v(vel_map.at("base_vx"))); + v(vel_map.at("pelvis_vx"))); program.AddBoundingBoxConstraint(pelvis_xy_vel(1), pelvis_xy_vel(1), - v(vel_map.at("base_vy"))); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_vz"))); + v(vel_map.at("pelvis_vy"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_vz"))); // Add symmetry constraints, and zero roll/pitch on the hip program.AddConstraint(q(positions_map.at("knee_left")) == diff --git a/examples/Cassie/cassie_lcm_driven_loop.h b/examples/Cassie/cassie_lcm_driven_loop.h index 9cf3f653e8..8059efa883 100644 --- a/examples/Cassie/cassie_lcm_driven_loop.h +++ b/examples/Cassie/cassie_lcm_driven_loop.h @@ -67,7 +67,7 @@ class CassieLcmDrivenLoop { /// @param input_channel The name of the input channel /// @param is_forced_publish A flag which enables publishing via diagram. CassieLcmDrivenLoop(drake::lcm::DrakeLcm* drake_lcm, - std::unique_ptr> diagram, + std::shared_ptr> diagram, const drake::systems::LeafSystem* lcm_parser, const std::string& input_channel, bool is_forced_publish) : CassieLcmDrivenLoop(drake_lcm, std::move(diagram), lcm_parser, @@ -84,7 +84,7 @@ class CassieLcmDrivenLoop { /// @param switch_channel The name of the switch channel /// @param is_forced_publish A flag which enables publishing via diagram. CassieLcmDrivenLoop(drake::lcm::DrakeLcm* drake_lcm, - std::unique_ptr> diagram, + std::shared_ptr> diagram, const drake::systems::LeafSystem* lcm_parser, std::vector input_channels, const std::string& active_channel, @@ -99,7 +99,7 @@ class CassieLcmDrivenLoop { } diagram_ptr_ = diagram.get(); simulator_ = - std::make_unique>(std::move(diagram)); + std::make_unique>(*diagram); // Create subscriber for the switch (in the case of multi-input) DRAKE_DEMAND(!input_channels.empty()); @@ -145,7 +145,7 @@ class CassieLcmDrivenLoop { /// @param is_forced_publish A flag which enables publishing via diagram. /// The use case is that the user only need the time from lcm message. CassieLcmDrivenLoop(drake::lcm::DrakeLcm* drake_lcm, - std::unique_ptr> diagram, + std::shared_ptr> diagram, const std::string& input_channel, bool is_forced_publish) : CassieLcmDrivenLoop(drake_lcm, std::move(diagram), nullptr, std::vector(1, input_channel), @@ -314,8 +314,6 @@ class CassieLcmDrivenLoop { // the LCM message used here successfully arrives at the input port of // the other LcmSubscriberSystem simulator_->AdvanceTo(time); - diagram_ptr_->CalcForcedUnrestrictedUpdate( - diagram_context, &diagram_context.get_mutable_state()); diagram_ptr_->CalcForcedDiscreteVariableUpdate( diagram_context, &diagram_context.get_mutable_discrete_state()); if (is_forced_publish_) { diff --git a/examples/Cassie/cassie_state_estimator.cc b/examples/Cassie/cassie_state_estimator.cc index 376d5838d1..f7bec41e15 100644 --- a/examples/Cassie/cassie_state_estimator.cc +++ b/examples/Cassie/cassie_state_estimator.cc @@ -67,7 +67,7 @@ CassieStateEstimator::CassieStateEstimator( print_info_to_terminal_(print_info_to_terminal), hardware_test_mode_(hardware_test_mode), contact_force_threshold_(contact_force_threshold), - joint_offsets_(MakeJointPositionOffsetFromMap(plant, joint_offset_map)){ + joint_offsets_(MakeJointPositionOffsetFromMap(plant, joint_offset_map)) { DRAKE_DEMAND(&fourbar_evaluator->plant() == &plant); DRAKE_DEMAND(&left_contact_evaluator->plant() == &plant); DRAKE_DEMAND(&right_contact_evaluator->plant() == &plant); @@ -416,7 +416,7 @@ void CassieStateEstimator::AssignNonFloatingBaseStateToOutputVector( double right_heel_spring = 0; VectorXd q = output->GetPositions() + joint_offsets_; output->SetPositions(q); - + if (is_floating_base_) { // Floating-base state doesn't affect the spring values // We assign the floating base of q in case output's floating base is @@ -435,20 +435,30 @@ void CassieStateEstimator::AssignNonFloatingBaseStateToOutputVector( void CassieStateEstimator::AssignFloatingBaseStateToOutputVector( const VectorXd& est_fb_state, OutputVector* output) const { - output->SetPositionAtIndex(position_idx_map_.at("base_qw"), est_fb_state(0)); - output->SetPositionAtIndex(position_idx_map_.at("base_qx"), est_fb_state(1)); - output->SetPositionAtIndex(position_idx_map_.at("base_qy"), est_fb_state(2)); - output->SetPositionAtIndex(position_idx_map_.at("base_qz"), est_fb_state(3)); - output->SetPositionAtIndex(position_idx_map_.at("base_x"), est_fb_state(4)); - output->SetPositionAtIndex(position_idx_map_.at("base_y"), est_fb_state(5)); - output->SetPositionAtIndex(position_idx_map_.at("base_z"), est_fb_state(6)); - - output->SetVelocityAtIndex(velocity_idx_map_.at("base_wx"), est_fb_state(7)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_wy"), est_fb_state(8)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_wz"), est_fb_state(9)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_vx"), est_fb_state(10)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_vy"), est_fb_state(11)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_vz"), est_fb_state(12)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qw"), + est_fb_state(0)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qx"), + est_fb_state(1)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qy"), + est_fb_state(2)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qz"), + est_fb_state(3)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_x"), est_fb_state(4)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_y"), est_fb_state(5)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_z"), est_fb_state(6)); + + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_wx"), + est_fb_state(7)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_wy"), + est_fb_state(8)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_wz"), + est_fb_state(9)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_vx"), + est_fb_state(10)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_vy"), + est_fb_state(11)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_vz"), + est_fb_state(12)); } /// EstimateContactFromSprings(). Conservative estimation. @@ -628,7 +638,7 @@ EventStatus CassieStateEstimator::Update( // is not triggered by CASSIE_STATE_SIMULATION message. // This wouldn't be an issue when you don't use ground truth state. if (output_gt.GetPositions().head(7).norm() == 0) { - output_gt.SetPositionAtIndex(position_idx_map_.at("base_qw"), 1); + output_gt.SetPositionAtIndex(position_idx_map_.at("pelvis_qw"), 1); } // Get kinematics cache for ground truth @@ -780,9 +790,9 @@ EventStatus CassieStateEstimator::Update( // TODO(yangwill): Decide whether to use both contacts per foot or just one. // Possibly leave it as an option to the state estimator contacts.push_back(std::pair(0, left_contact)); -// contacts.push_back(std::pair(1, left_contact)); + // contacts.push_back(std::pair(1, left_contact)); contacts.push_back(std::pair(2, right_contact)); -// contacts.push_back(std::pair(3, right_contact)); + // contacts.push_back(std::pair(3, right_contact)); ekf.setContacts(contacts); // Step 4 - EKF (measurement step) @@ -973,7 +983,7 @@ void CassieStateEstimator::CopyEstimatedContactForces( contact_info.timestamp = contact_msg->timestamp; contact_info.body1_name = toe_frames_[i]->body().name(); contact_info.body2_name = world_.name(); - memcpy(contact_info.contact_force, + memcpy(contact_info.contact_force.data(), context.get_discrete_state(contact_forces_idx_) .get_value() .segment(i * SPACE_DIM, (i + 1) * SPACE_DIM) @@ -1020,7 +1030,6 @@ void CassieStateEstimator::setPreviousImuMeasurement( void CassieStateEstimator::EstimateContactForces( const Context& context, const systems::OutputVector& output, VectorXd& lambda, int& left_contact, int& right_contact) const { - // TODO(yangwill) add a discrete time filter to the force estimate VectorXd v_prev = context.get_discrete_state(previous_velocity_idx_).get_value(); @@ -1087,11 +1096,12 @@ void CassieStateEstimator::DoCalcNextUpdateTime( *time = next_message_time_ - eps_; if (is_floating_base_) { - UnrestrictedUpdateEvent::UnrestrictedUpdateCallback callback = - [this](const Context& c, - const UnrestrictedUpdateEvent&, - drake::systems::State* s) { this->Update(c, s); }; - + auto callback = [](const System& system, const Context& c, + const UnrestrictedUpdateEvent&, + drake::systems::State* s) { + const auto& self = dynamic_cast(system); + return self.Update(c, s); + }; auto& uu_events = events->get_mutable_unrestricted_update_events(); uu_events.AddEvent(UnrestrictedUpdateEvent( drake::systems::TriggerType::kTimed, callback)); diff --git a/examples/Cassie/cassie_state_estimator.h b/examples/Cassie/cassie_state_estimator.h index 1aa90f5c99..fef0379d24 100644 --- a/examples/Cassie/cassie_state_estimator.h +++ b/examples/Cassie/cassie_state_estimator.h @@ -162,7 +162,7 @@ class CassieStateEstimator : public drake::systems::LeafSystem { const multibody::KinematicEvaluatorSet* fourbar_evaluator_; const multibody::KinematicEvaluatorSet* left_contact_evaluator_; const multibody::KinematicEvaluatorSet* right_contact_evaluator_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const bool is_floating_base_; std::unique_ptr> context_; diff --git a/examples/Cassie/cassie_utils.cc b/examples/Cassie/cassie_utils.cc index 662c5df09f..3e1127f0ac 100644 --- a/examples/Cassie/cassie_utils.cc +++ b/examples/Cassie/cassie_utils.cc @@ -110,7 +110,7 @@ void AddCassieMultibody(MultibodyPlant* plant, bool add_loop_closure, bool add_reflected_inertia) { std::string full_name = FindResourceOrThrow(filename); Parser parser(plant, scene_graph); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); if (!floating_base) { plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("pelvis"), diff --git a/examples/Cassie/cassie_virtual_remote.py b/examples/Cassie/cassie_virtual_remote.py deleted file mode 100644 index 72959b3158..0000000000 --- a/examples/Cassie/cassie_virtual_remote.py +++ /dev/null @@ -1,103 +0,0 @@ -''' -Uses Pygame (pip install pygame) to capture keyboard input. The virtual remote must be the active window to register commands. -W / S to increase/decrease sagittal velocity -A / D to increase/decrease longitudinal velocity - -use Keyboard Manager trim_x and trim_y member variables to add trim to the velocity commands -''' - -import sys -import pygame -from pygame.locals import * -import numpy as np - -import dairlib.lcmt_cassie_out - -import time -import lcm - - -class KeyboardManager(): - - def __init__(self): - pygame.init() - screen_size = 400 - self.screen = pygame.display.set_mode((screen_size, screen_size)) - self.vel = np.array([0.0, 0.0]) - self.trim_x = 0 - self.trim_y = 0 - self.delta_vx = 0.1 - self.delta_vy = 0.05 - self.delta_z = 0.05 - self.height_adjust = 0 - - - def switch_motion_key(self, key): - switcher = { - K_w: np.array([self.delta_vx, 0.0]), - K_s: np.array([-self.delta_vx, 0.0]), - K_a: np.array([0.0, self.delta_vy]), - K_d: np.array([0.0, -self.delta_vy]) - } - return switcher.get(key, np.array([0, 0])) - - def event_callback(self): - for event in pygame.event.get(): - if event.type == QUIT: - sys.exit(0) - elif event.type == KEYDOWN: - if (event.key == K_u): - self.height_adjust += self.delta_z - return - elif (event.key == K_d): - self.height_adjust -= self.delta_z - return - - self.vel += self.switch_motion_key(event.key) - if np.abs(self.vel[0]) < 1e-2: - self.vel[0] = 0.0 - if np.abs(self.vel[1]) < 1e-2: - self.vel[1] = 0.0 - - -def main(): - keyboard = KeyboardManager() - - lc = lcm.LCM() - - cassie_blue = (6, 61, 128) - white = (255, 255, 255) - pygame.display.set_caption('Cassie Virtual Radio Controller') - keyboard.screen.fill(cassie_blue) - fnt = pygame.font.Font('freesansbold.ttf', 32) - - while True: - # Get any keypresses - keyboard.event_callback() - - # Update display screen - display_text1 = 'Sagittal vel: ' + '%.2f'%(keyboard.vel[0]) - display_text2 = 'Coronal Vel: ' + '%.2f'%(keyboard.vel[1]) - text1 = fnt.render(display_text1, True, white, cassie_blue) - text2 = fnt.render(display_text2, True, white, cassie_blue) - text_rect1 = text1.get_rect() - text_rect2 = text2.get_rect() - text_rect1.center = (200, 150) - text_rect2.center = (200, 250) - keyboard.screen.fill(cassie_blue) - keyboard.screen.blit(text1, text_rect1) - keyboard.screen.blit(text2, text_rect2) - - # Send LCM message - cassie_out_msg = dairlib.lcmt_cassie_out() - cassie_out_msg.pelvis.radio.channel[0] = keyboard.vel[0] + keyboard.trim_x - cassie_out_msg.pelvis.radio.channel[1] = keyboard.vel[1] + keyboard.trim_y - cassie_out_msg.pelvis.radio.channel[3] = 0 - - lc.publish("CASSIE_OUTPUT_ECHO", cassie_out_msg.encode()) - - time.sleep(0.05) - pygame.display.update() - -if __name__ == '__main__': - main() diff --git a/examples/Cassie/cassie_xbox_remote.py b/examples/Cassie/cassie_xbox_remote.py index e767d78ead..21e3ac3a0f 100644 --- a/examples/Cassie/cassie_xbox_remote.py +++ b/examples/Cassie/cassie_xbox_remote.py @@ -39,19 +39,14 @@ def unindent(self): def main(): publisher = lcm.LCM() - pygame.display.set_caption('Cassie Virtual Radio Controller') - pygame.init() - screen_size = 500 - screen = pygame.display.set_mode((screen_size, screen_size)) # Used to manage how fast the screen updates clock = pygame.time.Clock() textPrint = TextPrint() - # radio spoof variables - radio_channel_6_pos = 0 - radio_channel_6_delta = 0.05 + latching_switch = 0 + # radio_msg.channel[14] = 0 if (pygame.joystick.get_count() != 1): raise RuntimeError("Please connect exactly one controller") @@ -60,53 +55,68 @@ def main(): joystick.init() done = False - + i = 0 + latching_switch_a = 1 + latching_switch_b = 1 + latching_switch_x = 0 + latching_switch_y = 0 + latching_switch_rt = 1 # right trigger + print("Teleop Status: " + str(latching_switch_a)) + print("Move C3 Target with Remote Status: " + str(latching_switch_b)) + print("Force Tracking Status: " + str(not latching_switch_x)) + print("Spatial Force on Object Status: " + str(not latching_switch_y)) + print("Set OSC Target with Remote Status: " + str(not latching_switch_rt)) while not done: # DRAWING STEP # First, clear the screen to blue. Don't put other drawing commands # above this, or they will be erased with this command. - screen.fill(cassie_blue) - textPrint.reset() + # textPrint.reset() # Get the name from the OS for the controller/joystick name = joystick.get_name() - textPrint.print(screen, "Welcome! remember to make this the active \nwindow when you wish to use the remote") - textPrint.print(screen, "Controller detected: {}".format(name) ) for event in pygame.event.get(): - if event.type == pygame.QUIT: # If user clicked close - done=True # Flag that we are done so we exit this loop - - if event.type == pygame.JOYHATMOTION: - hat_val = joystick.get_hat(0) - radio_channel_6_pos += radio_channel_6_delta * hat_val[1] - # saturate between -1 and 1 - radio_channel_6_pos = min(max(radio_channel_6_pos, -1), 1) - - - textPrint.print(screen, "Side dial position: {:.2f}".format(radio_channel_6_pos)) - - # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT - - # Go ahead and update the screen with what we've drawn. - pygame.display.flip() + if event.type == pygame.JOYBUTTONDOWN: + if event.button == 0: + latching_switch_a = not latching_switch_a + print("Teleop Status: " + str(latching_switch_a)) + if event.button == 1: + latching_switch_b = not latching_switch_b + print("Move C3 Target with Remote Status: " + str(latching_switch_b)) + if event.button == 2: + latching_switch_x = not latching_switch_x + print("Force Tracking Status: " + str(not latching_switch_x)) + if event.button == 3: + latching_switch_y = not latching_switch_y + print("Spatial Force on Object Status: " + str(latching_switch_y)) + if event.button == 5: + latching_switch_rt = not latching_switch_rt + print("Set OSC Target with Remote Status: " + str(latching_switch_rt)) # Send LCM message radio_msg = dairlib.lcmt_radio_out() radio_msg.channel[0] = -joystick.get_axis(1) - radio_msg.channel[1] = joystick.get_axis(0) + radio_msg.channel[1] = -joystick.get_axis(0) radio_msg.channel[2] = -joystick.get_axis(4) radio_msg.channel[3] = joystick.get_axis(3) - radio_msg.channel[6] = radio_channel_6_pos + # radio_msg.channel[2] = -joystick.get_axis(3) + # radio_msg.channel[3] = joystick.get_axis(2) + + radio_msg.channel[13] = latching_switch_b + radio_msg.channel[14] = latching_switch_a + radio_msg.channel[11] = latching_switch_x + radio_msg.channel[12] = latching_switch_y + radio_msg.channel[10] = latching_switch_rt radio_msg.channel[15] = -1 * np.rint(joystick.get_axis(5)) - publisher.publish("CASSIE_VIRTUAL_RADIO", radio_msg.encode()) + publisher.publish("RADIO", radio_msg.encode()) # Limit to 20 frames per second - clock.tick(20) + clock.tick(60) + i += 1 pygame.quit() diff --git a/examples/Cassie/closed_loop_running_sim.cc b/examples/Cassie/closed_loop_running_sim.cc new file mode 100644 index 0000000000..74aaf3cd41 --- /dev/null +++ b/examples/Cassie/closed_loop_running_sim.cc @@ -0,0 +1,85 @@ +#include +#include +#include + +#include "examples/Cassie/cassie_utils.h" +#include "examples/Cassie/diagrams/cassie_sim_diagram.h" +#include "examples/Cassie/diagrams/osc_running_controller_diagram.h" +#include "examples/Cassie/diagrams/osc_walking_controller_diagram.h" +#include "systems/controllers/pd_config_lcm.h" +#include "systems/system_utils.h" + +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/primitives/zero_order_hold.h" + +namespace dairlib { +using drake::multibody::MultibodyPlant; +using drake::systems::DiagramBuilder; + +namespace examples { + +int DoMain(int argc, char* argv[]) { + DiagramBuilder builder; + std::string urdf = "examples/Cassie/urdf/cassie_v2_conservative.urdf"; + std::string osc_running_gains = "examples/Cassie/osc_run/osc_running_gains.yaml"; + std::string osc_walking_gains = "examples/Cassie/osc/osc_walking_gains_alip.yaml"; + std::string osqp_settings = + "examples/Cassie/osc_run/osc_running_qp_settings.yaml"; + std::unique_ptr> plant = + std::make_unique>(1e-3); + MultibodyPlant controller_plant = + MultibodyPlant(1e-3); + // Built the Cassie MBPs + AddCassieMultibody(&controller_plant, nullptr, true, + "examples/Cassie/urdf/cassie_v2_conservative.urdf", + false /*spring model*/, false /*loop closure*/); + controller_plant.Finalize(); + + auto sim_diagram = builder.AddSystem( + std::move(plant), urdf, 0.4); + MultibodyPlant& new_plant = sim_diagram->get_plant(); + auto controller_diagram = + builder.AddSystem( + controller_plant, osc_running_gains, osqp_settings); +// auto controller_diagram = +// builder.AddSystem( +// controller_plant, true, osc_walking_gains, osqp_settings); + + builder.Connect(controller_diagram->get_output_port_robot_input(), + sim_diagram->get_input_port_actuation()); + builder.Connect(sim_diagram->get_output_port_state(), + controller_diagram->get_input_port_state()); +// builder.Connect(sim_diagram->get_output_port_cassie_out(), +// controller_diagram->get_cassie_out_input_port()); + + auto diagram = builder.Build(); + diagram->set_name("cassie_running_gym"); + DrawAndSaveDiagramGraph(*diagram); + std::cout << "built diagram: " << std::endl; + std::unique_ptr> diagram_context = + diagram->CreateDefaultContext(); + + VectorXd x_init = VectorXd::Zero(45); + x_init << 1, 0, 0, 0, 0, 0, 0.85, -0.0358636, 0, 0.67432, -1.588, -0.0458742, 1.90918, + -0.0381073, -1.82312, 0.0358636, 0, 0.67432, -1.588, -0.0457885, 1.90919, -0.0382424, -1.82321, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + Context& plant_context = + diagram->GetMutableSubsystemContext(new_plant, diagram_context.get()); + drake::systems::Simulator simulator(*diagram, + std::move(diagram_context)); + Context& simulator_context = diagram->GetMutableSubsystemContext(*sim_diagram, &simulator.get_mutable_context()); + Context& controller_context = diagram->GetMutableSubsystemContext(*controller_diagram, &simulator.get_mutable_context()); + + sim_diagram->get_input_port_radio().FixValue(&simulator_context, Eigen::VectorXd::Zero(18)); + controller_diagram->get_input_port_radio().FixValue(&controller_context, Eigen::VectorXd::Zero(18)); + + new_plant.SetPositionsAndVelocities(&plant_context, x_init); + simulator.Initialize(); + std::cout << "advancing simulator: " << std::endl; + simulator.AdvanceTo(5.0); +} +}} + +int main(int argc, char* argv[]) { + return dairlib::examples::DoMain(argc, argv); +} diff --git a/examples/Cassie/contact_scheduler/BUILD.bazel b/examples/Cassie/contact_scheduler/BUILD.bazel index 9f029cba6d..6c704f3854 100644 --- a/examples/Cassie/contact_scheduler/BUILD.bazel +++ b/examples/Cassie/contact_scheduler/BUILD.bazel @@ -4,10 +4,10 @@ package(default_visibility = ["//visibility:public"]) cc_library( - name="all", + name = "all", deps = [ ":contact_scheduler", - ] + ], ) cc_library( @@ -15,10 +15,10 @@ cc_library( srcs = ["contact_scheduler.cc"], hdrs = ["contact_scheduler.h"], deps = [ - "//lcmtypes:lcmt_robot", "//common", - "//multibody:utils", "//examples/Cassie:cassie_utils", + "//lcmtypes:lcmt_robot", + "//multibody:utils", "//systems/primitives", "@drake//:drake_shared_library", ], @@ -28,8 +28,8 @@ cc_binary( name = "state_based_controller_switch", srcs = ["state_based_controller_switch.cc"], deps = [ - "@drake//:drake_shared_library", "//lcmtypes:lcmt_robot", + "@drake//:drake_shared_library", "@gflags", ], ) diff --git a/examples/Cassie/contact_scheduler/contact_scheduler.cc b/examples/Cassie/contact_scheduler/contact_scheduler.cc index 8009479dd6..3a8b3368fa 100644 --- a/examples/Cassie/contact_scheduler/contact_scheduler.cc +++ b/examples/Cassie/contact_scheduler/contact_scheduler.cc @@ -89,7 +89,7 @@ SLIPContactScheduler::SLIPContactScheduler(const MultibodyPlant& plant, drake::Value>>{ initial_state_transitions}); - DeclarePerStepUnrestrictedUpdateEvent( + DeclareForcedUnrestrictedUpdateEvent( &SLIPContactScheduler::UpdateTransitionTimes); } @@ -112,7 +112,7 @@ EventStatus SLIPContactScheduler::UpdateTransitionTimes( state->get_mutable_abstract_state< std::vector>>( upcoming_transitions_index_); - + std::cout << "current time: " << current_time; auto active_state = stored_fsm_state; double transition_time = upcoming_transitions.at(3).first; RunningFsmState transition_state = upcoming_transitions.at(3).second; diff --git a/examples/Cassie/contact_scheduler/contact_scheduler.h b/examples/Cassie/contact_scheduler/contact_scheduler.h index d02637b05a..c0a5781dd3 100644 --- a/examples/Cassie/contact_scheduler/contact_scheduler.h +++ b/examples/Cassie/contact_scheduler/contact_scheduler.h @@ -5,7 +5,7 @@ #include #include -#include "common/blending_utils.h" +#include "common/math_utils.h" #include "dairlib/lcmt_contact_timing.hpp" #include "systems/framework/impact_info_vector.h" #include "systems/framework/output_vector.h" diff --git a/examples/Cassie/contact_scheduler/kinodynamic_planner.cc b/examples/Cassie/contact_scheduler/kinodynamic_planner.cc new file mode 100644 index 0000000000..f5fca87f4c --- /dev/null +++ b/examples/Cassie/contact_scheduler/kinodynamic_planner.cc @@ -0,0 +1,126 @@ +#include "kinodynamic_planner.h" + +using drake::AbstractValue; +using drake::multibody::RigidBodyFrame; +using drake::multibody::JacobianWrtVariable; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::EventStatus; +using drake::trajectories::PiecewisePolynomial; + +using drake::EigenPtr; + +using drake::multibody::CentroidalMomentumConstraint; +using drake::solvers::MathematicalProgram; +using drake::solvers::MathematicalProgramResult; +using drake::solvers::OsqpSolver; +using drake::solvers::OsqpSolverDetails; +using drake::solvers::Solve; +using drake::solvers::VectorXDecisionVariable; + +using drake::AutoDiffXd; +using Eigen::Matrix; +using Eigen::Matrix3d; +using Eigen::MatrixXd; +using Eigen::Vector2d; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using dairlib::LcmTrajectory; +using dairlib::multibody::MakeNameToPositionsMap; +using dairlib::multibody::MakeNameToVelocitiesMap; +using dairlib::multibody::SetPositionsAndVelocitiesIfNew; +using dairlib::systems::OutputVector; + +namespace dairlib { + +KinodynamicPlanner::KinodynamicPlanner( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad) + : plant_(plant), + context_(context), + plant_ad_(plant_ad), + context_ad_(context_ad), + n_q_(plant.num_positions()), + n_v_(plant.num_velocities()), + n_u_(plant.num_actuators()), + m_(plant.CalcTotalMass(context)), + g_(plant.gravity_field().gravity_vector()) { + // Create Ports + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(n_q_, n_v_, n_u_)) + .get_index(); + + traj_out_port_ = this->DeclareAbstractOutputPort( + "y(t)", &KinodynamicPlanner::GetMostRecentMotionPlan, + {this->all_state_ticket()}) + .get_index(); + + // Discrete update + DeclarePerStepDiscreteUpdateEvent( + &KinodynamicPlanner::DiscreteVariableUpdate); + // DeclarePeriodicDiscreteUpdateEvent(dt_, 0, + // &KinodynamicPlanner::PeriodicUpdate); + + x_des_ = VectorXd ::Zero(n_r_ + n_h_); +} + +void KinodynamicPlanner::AddCoMDynamicsConstraint() { + for (int i = 0; i < n_knot_points_; ++i) { + prog_.AddLinearEqualityConstraint( + m_ * ddr_[i] - F_[i][0] - F_[i][1] - F_[i][2] - F_[i][3], m_ * g_); + } +} + +void KinodynamicPlanner::AddCentroidalMomentumConstraint() { + for (int i = 0; i < n_knot_points_; ++i) { + centroidal_momentum_constraints_.push_back( + std::make_shared(&plant_ad_, std::nullopt, + &context_ad_, true)); + prog_.AddConstraint(centroidal_momentum_constraints_[i], + {q_.at(i), v_.at(i), h_.at(i)}); + } +} + +void KinodynamicPlanner::AddAngularMomentumDynamicsConstraint() { + for (int i = 0; i < n_knot_points_; ++i) { + centroidal_momentum_constraints_.push_back( + std::make_shared(&plant_ad_, std::nullopt, + &context_ad_, true)); + +// prog_.AddConstraint(dh_.at(i) == (c_[i][0] - r_[i][0]).cross(F_[i][0]), +// {q_.at(i), v_.at(i), h_.at(i)}); + } +} + +void KinodynamicPlanner::AddIntegrationConstraints(){ + for (int i = 1; i < n_knot_points_; ++i) { + // eq 7d + prog_.AddLinearEqualityConstraint(q_[i] - q_[i-1] == v_[i] * dt_[i]); + // eq 7e + prog_.AddLinearEqualityConstraint(h_[i] - h_[i-1] == dh_[i] * dt_[i]); + + // eq 7f +// prog_.AddLinearEqualityConstraint(r_[i] - r_[i-1] == (dr_[i] + dr_[i-1])/2 * dt_[i]); + prog_.AddLinearEqualityConstraint(r_[i] - r_[i-1] == dr_[i] * dt_[i]); + + // eq 7g + prog_.AddLinearEqualityConstraint(dr_[i] - dr_[i-1] == ddr_[i] * dt_[i]); + } +} + +void KinodynamicPlanner::AddDynamicsConstraint() { + AddCoMDynamicsConstraint(); + AddCentroidalMomentumConstraint(); + AddIntegrationConstraints(); +} + +void KinodynamicPlanner::AddKinematicConstraints(){ + // Linearize the kinematic constraints about current state + +} + + +} // namespace dairlib \ No newline at end of file diff --git a/examples/Cassie/contact_scheduler/kinodynamic_planner.h b/examples/Cassie/contact_scheduler/kinodynamic_planner.h new file mode 100644 index 0000000000..dba6ccefa8 --- /dev/null +++ b/examples/Cassie/contact_scheduler/kinodynamic_planner.h @@ -0,0 +1,248 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "examples/Cassie/contact_scheduler/kinodynamic_settings.h" +#include "lcm/lcm_trajectory.h" + +#include "multibody/multibody_utils.h" +#include "solvers/constraint_factory.h" +#include "solvers/fast_osqp_solver.h" +#include "systems/framework/output_vector.h" +#include "drake/common/trajectories/piecewise_polynomial.h" + +#include "drake/multibody/optimization/centroidal_momentum_constraint.h" +//#include "drake/solvers/gurobi_solver.h" +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +typedef struct LinearSrbdDynamics { + Eigen::MatrixXd A; + Eigen::MatrixXd B; + Eigen::VectorXd b; +} LinearSrbdDynamics; + +/// This kinodynamic planner is an implementation of the Dai 2014 paper +/// (Whole-body Motion Planning with Centroidal Dynamics and Full Kinematics) +/// This outputs: footstep timing +/// final footstep location +/// center of mass trajectory +/// swing foot trajectory (maybe)? + +/// TODOs: +/// + +/// Template borrowed from SrbdCMPC written by Brian Acosta in centroidal_models +/// branch + +class KinodynamicPlanner : public drake::systems::LeafSystem { + public: + KinodynamicPlanner(const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad); + + void SetMPCSettings(KinodynamicSettings& settings); + void SetTerminalCost(const Eigen::MatrixXd& Qf); + void AddInputRegularization(const Eigen::MatrixXd& R); + void AddTrackingObjective(const Eigen::VectorXd& xdes, + const Eigen::MatrixXd& Q); + // void AddFootPlacementRegularization(const Eigen::MatrixXd& W); + void Build(); + void CheckProblemDefinition(); + + // Input ports + const drake::systems::InputPort& get_state_input_port() const { + return this->get_input_port(state_port_); + } + const drake::systems::InputPort& get_fsm_input_port() const { + return this->get_input_port(fsm_port_); + } + const drake::systems::InputPort& get_target_input_port() const { + return this->get_input_port(x_des_port_); + } + const drake::systems::InputPort& get_warmstart_input_port() const { + return this->get_input_port(srb_warmstart_port_); + } + // const drake::systems::InputPort& get_foot_target_input_port() + // const { + // return this->get_input_port(foot_target_port_); + // } + const drake::systems::OutputPort& get_traj_out_port() const { + return this->get_output_port(traj_out_port_); + } + + void SetKinematicConstraints(); + + void SetContactConstraints(); + + drake::solvers::MathematicalProgramResult solve_problem_as_is() { + return drake::solvers::Solve(prog_); + } + + void print_constraint( + const std::vector& constraint) const; + void print_constraint( + const std::vector& constraint) const; + void print_constraint( + const std::vector& constraint) + const; + + private: + void CheckSquareMatrixDimensions(const Eigen::MatrixXd& M, int n) const; + void GetMostRecentMotionPlan(const drake::systems::Context& context, + lcmt_saved_traj* traj_msg) const; + void MakeTerrainConstraints(const Eigen::Vector3d& normal = {0, 0, 1}, + const Eigen::Vector3d& origin = {0, 0, 0}); + void MakeDynamicsConstraints(); + void MakeFrictionConeConstraints(); + void MakeInitialStateConstraint(); + void MakeCost(); + + lcmt_saved_traj MakeLcmTrajFromSol( + const drake::solvers::MathematicalProgramResult& result, double time, + double time_since_last_touchdown, const Eigen::VectorXd& state, + int fsm_state) const; + + void UpdateConstraints(const Eigen::VectorXd& x0, int fsm_state, + double t_since_last_switch) const; + void UpdateTrackingObjective(const Eigen::VectorXd& xdes) const; + void UpdateDynamicsConstraints(const Eigen::VectorXd& x, + int n_until_next_stance, int fsm_state) const; + void UpdateKinematicConstraints(int n_until_stance, int fsm_state) const; + void SetInitialGuess( + int fsm_state, double timestamp, + const Eigen::VectorXd& up_next_stance_target, + const Eigen::VectorXd& on_deck_stance_target, + const drake::trajectories::Trajectory& srb_traj) const; +// void CopyDiscreteDynamicsConstraint( +// const SrbdMode& mode, bool current_stance, +// const Eigen::Vector3d& foot_pos, +// const drake::EigenPtr& A, +// const drake::EigenPtr& b) const; + void CopyCollocationDynamicsConstraint( + const LinearSrbdDynamics& dyn, bool current_stance, + const Eigen::Vector3d& foot_pos, + const drake::EigenPtr& A, + const drake::EigenPtr& b) const; + + // discrete update + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::EventStatus PeriodicUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + + void AddCentroidalMomentumConstraint(); + void AddAngularMomentumDynamicsConstraint(); + void AddCoMDynamicsConstraint(); + void AddDynamicsConstraint(); + void AddIntegrationConstraints(); + void AddKinematicConstraints(); + + + // plant + const drake::multibody::MultibodyPlant& plant_; + drake::systems::Context& context_; + const drake::multibody::MultibodyPlant& plant_ad_; + drake::systems::Context& context_ad_; + double mu_ = 0; + int n_q_; + int n_v_; + int n_u_; + + // centroidal optimization parameters + double m_; + Eigen::Vector3d g_; + int n_contacts_ = 4; + int n_r_ = 3; + int n_h_ = 3; + + // mpc parameters + int n_knot_points_ = 4; + + + + static constexpr int kLinearDim_ = 3; + static constexpr int kAngularDim_ = 3; + // const double dt_; + int nmodes_ = 0; + + // port indices + int state_port_; + int fsm_port_; + int x_des_port_; + int srbd_residual_port_; + int traj_out_port_; + int foot_target_port_; + int srb_warmstart_port_; + + // discrete update indices + int current_fsm_state_idx_; + int prev_event_time_idx_; + + // Problem variables + Eigen::MatrixXd Q_; // For running cost x^TQx + Eigen::MatrixXd R_; // For running cost u^TRu + Eigen::MatrixXd Wp_; // regularizing cost on footstep location + Eigen::MatrixXd Qf_; // For terminal cost x_{T}^{T}Q_{f}x_{T} + Eigen::Vector3d kin_bounds_; + mutable Eigen::VectorXd x_des_; + mutable Eigen::MatrixXd x_des_mat_; + int total_knots_ = 0; + + // Solver + drake::solvers::OsqpSolver solver_; + mutable drake::solvers::MathematicalProgramResult result_; + mutable drake::solvers::MathematicalProgram prog_; + mutable lcmt_saved_traj most_recent_sol_; + + // Constraints + std::vector tracking_cost_; + std::vector input_cost_; + std::vector foot_target_cost_; + std::vector friction_cone_; + std::vector terrain_angle_; + drake::solvers::LinearEqualityConstraint* initial_state_; + mutable std::vector< + drake::solvers::Binding> + dynamics_; + mutable std::vector> + kinematic_constraint_; + + + // Constraints + std::vector> centroidal_momentum_constraints_; + + + // Decision Variables + std::vector q_; + std::vector v_; + std::vector dt_; + std::vector r_; + std::vector dr_; + std::vector ddr_; + std::vector> c_; + std::vector> F_; + std::vector> tau_; + std::vector h_; + std::vector dh_; + + std::vector xx; + std::vector uu; + std::vector pp; +}; +} // namespace dairlib \ No newline at end of file diff --git a/examples/Cassie/contact_scheduler/kinodynamic_settings.h b/examples/Cassie/contact_scheduler/kinodynamic_settings.h new file mode 100644 index 0000000000..d0922d5398 --- /dev/null +++ b/examples/Cassie/contact_scheduler/kinodynamic_settings.h @@ -0,0 +1,37 @@ +#pragma once + +#include "yaml-cpp/yaml.h" + +#include "drake/common/yaml/yaml_read_archive.h" + +using Eigen::MatrixXd; + +namespace dairlib { + +struct KinodynamicSettings { + + int N; + int N_per_mode; + std::vector q_q; + std::vector q_v; + + MatrixXd Q_q; + MatrixXd Q_v; + + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(N)); + a->Visit(DRAKE_NVP(N_per_mode)); + a->Visit(DRAKE_NVP(q_q)); + a->Visit(DRAKE_NVP(q_v)); + + Q_q = Eigen::Map< + Eigen::Matrix>( + this->q_q.data(), 3, 3); + Q_v = Eigen::Map< + Eigen::Matrix>( + this->q_v.data(), 3, 3); + } +}; +} // namespace dairlib \ No newline at end of file diff --git a/examples/Cassie/contact_scheduler/kinodynamic_settings.yaml b/examples/Cassie/contact_scheduler/kinodynamic_settings.yaml new file mode 100644 index 0000000000..56d9dbc717 --- /dev/null +++ b/examples/Cassie/contact_scheduler/kinodynamic_settings.yaml @@ -0,0 +1,10 @@ +# hyperparameters for kinodynamic planner + +N: 15 +N_per_mode: 5 +Q_q: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] +Q_v: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] diff --git a/examples/Cassie/diagrams/BUILD.bazel b/examples/Cassie/diagrams/BUILD.bazel new file mode 100644 index 0000000000..70a0917fca --- /dev/null +++ b/examples/Cassie/diagrams/BUILD.bazel @@ -0,0 +1,99 @@ +load("@drake//tools/lint:lint.bzl", "add_lint_tests") + +package(default_visibility = ["//visibility:public"]) + +load( + "@drake//tools/skylark:drake_lcm.bzl", + "drake_lcm_cc_library", + "drake_lcm_java_library", + "drake_lcm_py_library", +) +load( + "@drake//tools/skylark:drake_py.bzl", + "drake_py_binary", + "drake_py_library", + "drake_py_unittest", +) +load( + "@drake//tools/skylark:pybind.bzl", + "drake_pybind_library", + "get_drake_py_installs", + "get_pybind_package_info", + "pybind_py_library", +) + +cc_library( + name = "diagrams", + deps = [ + ":cassie_sim_diagram", + ":osc_running_controller_diagram", + ":osc_walking_controller_diagram", + ], +) + +cc_library( + name = "osc_running_controller_diagram", + srcs = ["osc_running_controller_diagram.cc"], + hdrs = ["osc_running_controller_diagram.h"], + deps = [ + "//examples/Cassie:cassie_urdf", + "//examples/Cassie:cassie_utils", + "//examples/Cassie/contact_scheduler:all", + "//examples/Cassie/osc", + "//examples/Cassie/osc_run", + "//examples/impact_invariant_control:impact_aware_time_based_fsm", + "//lcm:trajectory_saver", + "//multibody:utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers/osc:osc_tracking_datas", + "//systems/filters:floating_base_velocity_filter", + "//systems/framework:lcm_driven_loop", + "//systems/primitives", + "//systems/primitives:radio_parser", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "osc_walking_controller_diagram", + srcs = ["osc_walking_controller_diagram.cc"], + hdrs = ["osc_walking_controller_diagram.h"], + deps = [ + "//examples/Cassie:cassie_urdf", + "//examples/Cassie:cassie_utils", + "//examples/Cassie/osc", + "//examples/impact_invariant_control:impact_aware_time_based_fsm", + "//lcm:trajectory_saver", + "//multibody:utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers:fsm_event_time", + "//systems/controllers/osc:osc_gains", + "//systems/controllers/osc:osc_tracking_datas", + "//systems/filters:floating_base_velocity_filter", + "//systems/framework:lcm_driven_loop", + "//systems/primitives", + "//systems/primitives:radio_parser", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "cassie_sim_diagram", + srcs = ["cassie_sim_diagram.cc"], + hdrs = ["cassie_sim_diagram.h"], + deps = [ + "//examples/Cassie:cassie_fixed_point_solver", + "//examples/Cassie:cassie_urdf", + "//examples/Cassie:cassie_utils", + "//examples/Cassie/systems:cassie_encoder", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:geared_motor", + "//systems/primitives", + "//systems/primitives:radio_parser", + "@drake//:drake_shared_library", + "@gflags", + ], +) diff --git a/examples/Cassie/diagrams/cassie_sim_diagram.cc b/examples/Cassie/diagrams/cassie_sim_diagram.cc new file mode 100644 index 0000000000..f1a7805310 --- /dev/null +++ b/examples/Cassie/diagrams/cassie_sim_diagram.cc @@ -0,0 +1,121 @@ + +#include "cassie_sim_diagram.h" + +#include +#include +#include + +#include "dairlib/lcmt_robot_output.hpp" +#include "examples/Cassie/cassie_fixed_point_solver.h" +#include "examples/Cassie/cassie_utils.h" +#include "multibody/multibody_utils.h" +#include "systems/primitives/radio_parser.h" +#include "systems/primitives/subvector_pass_through.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +#include "drake/geometry/drake_visualizer.h" +#include "drake/lcm/drake_lcm.h" +#include "drake/multibody/plant/contact_results_to_lcm.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_publisher_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" +#include "drake/systems/primitives/discrete_time_delay.h" + +namespace dairlib { +namespace examples { + +using dairlib::systems::SubvectorPassThrough; +using drake::geometry::DrakeVisualizer; +using drake::geometry::SceneGraph; +using drake::math::RotationMatrix; +using drake::multibody::ContactResultsToLcmSystem; +using drake::multibody::MultibodyPlant; +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::Matrix3d; +using Eigen::Vector3d; +using Eigen::VectorXd; + +CassieSimDiagram::CassieSimDiagram( + std::shared_ptr> plant, + const std::string& urdf, bool visualize, double mu, double stiffness, + double dissipation_rate) { + DiagramBuilder builder; + scene_graph_ = builder.AddSystem(); + scene_graph_->set_name("scene_graph"); + + plant_ = builder.AddSystem(std::move(plant)); + plant_->set_discrete_contact_approximation( + drake::multibody::DiscreteContactApproximation::kSap); + AddCassieMultibody(plant_, scene_graph_, true, urdf, true, true); + multibody::AddFlatTerrain(plant_, scene_graph_, mu, mu, + Eigen::Vector3d(0, 0, 1)); + plant_->Finalize(); + + auto input_receiver = builder.AddSystem(*plant_); + auto passthrough = builder.AddSystem( + input_receiver->get_output_port(0).size(), 0, + plant_->get_actuation_input_port().size()); + auto discrete_time_delay = + builder.AddSystem( + actuator_update_rate, actuator_delay / actuator_update_rate, + plant_->num_actuators() + 1); + auto state_sender = + builder.AddSystem(*plant_, true); + + auto constant_source = + builder.AddSystem( + VectorXd::Zero(10)); + auto input_zero_order_hold = builder.AddSystem( + 0.001, plant_->num_actuators() + 1); + sensor_aggregator_ = &AddImuAndAggregator(&builder, *plant_, + constant_source->get_output_port()); + + cassie_motor_ = &AddMotorModel(&builder, *plant_); + + auto radio_parser = builder.AddSystem(); + + // connect leaf systems + builder.Connect(input_receiver->get_output_port(), + input_zero_order_hold->get_input_port()); + builder.Connect(input_zero_order_hold->get_output_port(), + discrete_time_delay->get_input_port()); + builder.Connect(discrete_time_delay->get_output_port(), + passthrough->get_input_port()); + builder.Connect(passthrough->get_output_port(), + cassie_motor_->get_input_port_command()); + builder.Connect(cassie_motor_->get_output_port(), + plant_->get_actuation_input_port()); + builder.Connect(plant_->get_state_output_port(), + state_sender->get_input_port_state()); + builder.Connect(plant_->get_state_output_port(), + cassie_motor_->get_input_port_state()); + builder.Connect(cassie_motor_->get_output_port(), + state_sender->get_input_port_effort()); + builder.Connect( + plant_->get_geometry_pose_output_port(), + scene_graph_->get_source_pose_port(plant_->get_source_id().value())); + builder.Connect(scene_graph_->get_query_output_port(), + plant_->get_geometry_query_input_port()); + builder.Connect(radio_parser->get_output_port(), + sensor_aggregator_->get_input_port_radio()); + + actuation_port_ = builder.ExportInput(input_receiver->get_input_port(), "lcmt_robot_input"); + radio_port_ = builder.ExportInput(radio_parser->get_input_port(), "raw_radio"); + state_port_ = builder.ExportOutput(state_sender->get_output_port(0), "lcmt_robot_output"); + cassie_out_port_ = builder.ExportOutput(sensor_aggregator_->get_output_port(0), + "lcmt_cassie_out"); + if (visualize) { + DrakeVisualizer::AddToBuilder(&builder, *scene_graph_); + } + builder.BuildInto(this); + this->set_name("cassie_sim_diagram"); + DrawAndSaveDiagramGraph(*this); +} +} // namespace examples +} // namespace dairlib \ No newline at end of file diff --git a/examples/Cassie/diagrams/cassie_sim_diagram.h b/examples/Cassie/diagrams/cassie_sim_diagram.h new file mode 100644 index 0000000000..71ac801e16 --- /dev/null +++ b/examples/Cassie/diagrams/cassie_sim_diagram.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include + +#include + +#include "examples/Cassie/systems/sim_cassie_sensor_aggregator.h" +#include "systems/framework/geared_motor.h" + +#include "drake/common/drake_copyable.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/system.h" + +namespace dairlib { +namespace examples { + +class CassieSimDiagram : public drake::systems::Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CassieSimDiagram) + + /// @param[in] urdf filepath containing the osc_running_gains. + CassieSimDiagram(std::shared_ptr> plant, + const std::string& urdf = "examples/Cassie/urdf/cassie_v2.urdf", bool visualize = false, + double mu = 0.8, double stiffness = 1e4, double dissipation_rate = 1e2); + + /// @return the input port for the actuation command. + const drake::systems::InputPort& get_input_port_actuation() const { + return this->get_input_port(actuation_port_); + } + + /// @return the input port for the radio struct. + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + /// @return the output port for the plant state as an OutputVector. + const drake::systems::OutputPort& get_output_port_state() const { + return this->get_output_port(state_port_); + } + + /// @return the output port for the lcmt_cassie_out_message + const drake::systems::OutputPort& get_output_port_cassie_out() + const { + return this->get_output_port(cassie_out_port_); + } + + drake::multibody::MultibodyPlant& get_plant() { + return *plant_; + } + // const drake::systems::System& get_sensor_aggregator() { return + // *sensor_aggregator_; } const drake::systems::System& + // get_radio_parser() { return *radio_parser_; } + + private: + drake::multibody::MultibodyPlant* plant_; + const systems::SimCassieSensorAggregator* sensor_aggregator_; + const systems::GearedMotor* cassie_motor_; + // const systems::RadioParser* radio_parser_; + drake::geometry::SceneGraph* scene_graph_; + drake::systems::InputPortIndex actuation_port_; + drake::systems::InputPortIndex radio_port_; + drake::systems::OutputPortIndex state_port_; + drake::systems::OutputPortIndex cassie_out_port_; + const double actuator_delay = 5e-3; // 5ms + const double actuator_update_rate = 1e-3; // 1ms + const double dt_ = 8e-5; +}; + +} // namespace examples +} // namespace dairlib diff --git a/examples/Cassie/diagrams/osc_running_controller_diagram.cc b/examples/Cassie/diagrams/osc_running_controller_diagram.cc new file mode 100644 index 0000000000..04f457b4b8 --- /dev/null +++ b/examples/Cassie/diagrams/osc_running_controller_diagram.cc @@ -0,0 +1,529 @@ +#include "osc_running_controller_diagram.h" + +#include +#include + +#include "common/find_resource.h" +#include "examples/Cassie/cassie_utils.h" +#include "examples/Cassie/contact_scheduler/contact_scheduler.h" +#include "examples/Cassie/osc/heading_traj_generator.h" +#include "examples/Cassie/osc/high_level_command.h" +#include "examples/Cassie/osc/swing_toe_traj_generator.h" +#include "examples/Cassie/osc_run/foot_traj_generator.h" +#include "examples/Cassie/osc_run/osc_running_gains.h" +#include "examples/Cassie/osc_run/pelvis_trans_traj_generator.h" +#include "multibody/kinematic/fixed_joint_evaluator.h" +#include "multibody/multibody_utils.h" +#include "systems/controllers/controller_failure_aggregator.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/operational_space_control.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" +#include "systems/filters/floating_base_velocity_filter.h" +#include "systems/primitives/radio_parser.h" +#include "systems/primitives/subvector_pass_through.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +#include "drake/common/yaml/yaml_read_archive.h" +#include "drake/systems/framework/diagram_builder.h" + +namespace dairlib { + +using std::map; +using std::pair; +using std::string; +using std::vector; + +using Eigen::Matrix3d; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using cassie::osc::SwingToeTrajGenerator; +using drake::geometry::SceneGraph; +using drake::multibody::Frame; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::trajectories::PiecewisePolynomial; +using examples::osc::PelvisTransTrajGenerator; +using examples::osc_run::FootTrajGenerator; +using multibody::FixedJointEvaluator; +using multibody::WorldYawViewFrame; +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RelativeTranslationTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; + +namespace examples { +namespace controllers { + +OSCRunningControllerDiagram::OSCRunningControllerDiagram( + drake::multibody::MultibodyPlant& plant, + const string& osc_running_gains_filename, + const string& osqp_settings_filename) + : plant_(&plant), + pos_map(multibody::MakeNameToPositionsMap(plant)), + vel_map(multibody::MakeNameToVelocitiesMap(plant)), + act_map(multibody::MakeNameToActuatorsMap(plant)), + left_toe(LeftToeFront(plant)), + left_heel(LeftToeRear(plant)), + right_toe(RightToeFront(plant)), + right_heel(RightToeRear(plant)), + left_foot_points({left_heel, left_toe}), + right_foot_points({right_heel, right_toe}), + view_frame(std::make_shared>( + plant.GetBodyByName("pelvis"))), + left_toe_evaluator(multibody::WorldPointEvaluator( + plant, left_toe.first, left_toe.second, *view_frame, + Matrix3d::Identity(), Vector3d::Zero(), {1, 2})), + left_heel_evaluator(multibody::WorldPointEvaluator( + plant, left_heel.first, left_heel.second, *view_frame, + Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2})), + right_toe_evaluator(multibody::WorldPointEvaluator( + plant, right_toe.first, right_toe.second, *view_frame, + Matrix3d::Identity(), Vector3d::Zero(), {1, 2})), + right_heel_evaluator(multibody::WorldPointEvaluator( + plant, right_heel.first, right_heel.second, *view_frame, + Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2})), + left_loop(LeftLoopClosureEvaluator(plant)), + right_loop(RightLoopClosureEvaluator(plant)), + evaluators(multibody::KinematicEvaluatorSet(plant)), + left_fixed_knee_spring( + FixedJointEvaluator(plant, pos_map.at("knee_joint_left"), + vel_map.at("knee_joint_leftdot"), 0)), + right_fixed_knee_spring( + FixedJointEvaluator(plant, pos_map.at("knee_joint_right"), + vel_map.at("knee_joint_rightdot"), 0)), + left_fixed_ankle_spring( + FixedJointEvaluator(plant, pos_map.at("ankle_spring_joint_left"), + vel_map.at("ankle_spring_joint_leftdot"), 0)), + right_fixed_ankle_spring( + FixedJointEvaluator(plant, pos_map.at("ankle_spring_joint_right"), + vel_map.at("ankle_spring_joint_rightdot"), 0)) { + // Build the controller diagram + DiagramBuilder builder; + plant_context = plant.CreateDefaultContext(); + feet_contact_points[0] = std::vector< + std::pair&>>( + {left_toe, left_heel}); + feet_contact_points[1] = std::vector< + std::pair&>>( + {right_toe, right_heel}); + + /**** OSC Gains ****/ + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + + OSCGains gains = drake::yaml::LoadYamlFile( + FindResourceOrThrow(osc_running_gains_filename), {}, {}, yaml_options); + OSCRunningGains osc_running_gains = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(osc_running_gains_filename)); + + /**** FSM and contact mode configuration ****/ + vector fsm_states = { + RunningFsmState::kLeftStance, RunningFsmState::kLeftFlight, + RunningFsmState::kRightStance, RunningFsmState::kRightFlight, + RunningFsmState::kLeftStance}; + + vector state_durations = {osc_running_gains.stance_duration, + osc_running_gains.flight_duration, + osc_running_gains.stance_duration, + osc_running_gains.flight_duration, 0.0}; + vector accumulated_state_durations; + accumulated_state_durations.push_back(0); + for (double state_duration : state_durations) { + accumulated_state_durations.push_back(accumulated_state_durations.back() + + state_duration); + } + accumulated_state_durations.pop_back(); + + std::set impact_states = {kLeftStance, kRightStance}; + auto contact_scheduler = builder.AddSystem( + plant, plant_context.get(), impact_states, gains.impact_threshold, + gains.impact_tau); + contact_scheduler->SetSLIPParams(osc_running_gains.rest_length); + contact_scheduler->SetNominalStepTimings(osc_running_gains.stance_duration, + osc_running_gains.flight_duration); + contact_scheduler->SetMaxStepTimingVariance( + osc_running_gains.stance_variance, osc_running_gains.flight_variance); + + auto state_receiver = builder.AddSystem(plant); + auto command_sender = builder.AddSystem(plant); + auto osc = builder.AddSystem( + plant, plant_context.get(), true); + auto radio_parser = builder.AddSystem(); + auto failure_aggregator = + builder.AddSystem( + control_channel_name_, 1); + + /**** OSC setup ****/ + /// REGULARIZATION COSTS + osc->SetAccelerationCostWeights(osc_running_gains.w_accel * + osc_running_gains.W_acceleration); + osc->SetInputSmoothingCostWeights(osc_running_gains.w_input_reg * + osc_running_gains.W_input_regularization); + osc->SetInputCostWeights(osc_running_gains.w_input * + osc_running_gains.W_input_regularization); + osc->SetLambdaContactRegularizationWeight( + osc_running_gains.w_lambda * osc_running_gains.W_lambda_c_regularization); + osc->SetLambdaHolonomicRegularizationWeight( + osc_running_gains.w_lambda * osc_running_gains.W_lambda_h_regularization); + // Soft constraint on contacts + osc->SetContactSoftConstraintWeight(osc_running_gains.w_soft_constraint); + osc->SetJointLimitWeight(osc_running_gains.w_joint_limit); + + // Contact information for OSC + osc->SetContactFriction(osc_running_gains.mu); + + osc->AddContactPoint("left_toe", + std::unique_ptr>( + &left_toe_evaluator), + {RunningFsmState::kLeftStance}); + osc->AddContactPoint("left_heel", + std::unique_ptr>( + &left_heel_evaluator), + {RunningFsmState::kLeftStance}); + osc->AddContactPoint("right_toe", + std::unique_ptr>( + &right_toe_evaluator), + {RunningFsmState::kRightStance}); + osc->AddContactPoint("right_heel", + std::unique_ptr>( + &right_heel_evaluator), + {RunningFsmState::kRightStance}); + + // Fix the springs in the dynamics + evaluators.add_evaluator(&left_fixed_knee_spring); + evaluators.add_evaluator(&right_fixed_knee_spring); + evaluators.add_evaluator(&left_fixed_ankle_spring); + evaluators.add_evaluator(&right_fixed_ankle_spring); + evaluators.add_evaluator(&left_loop); + evaluators.add_evaluator(&right_loop); + + osc->AddKinematicConstraint( + std::unique_ptr>( + &evaluators)); + + /**** Tracking Data *****/ + + cassie::osc::HighLevelCommand* high_level_command; + high_level_command = builder.AddSystem( + plant, plant_context.get(), osc_running_gains.vel_scale_rot, + osc_running_gains.vel_scale_trans_sagital, + osc_running_gains.vel_scale_trans_lateral); + + auto pelvis_trans_traj_generator = + builder.AddSystem(plant, plant_context.get(), + feet_contact_points); + pelvis_trans_traj_generator->SetSLIPParams( + osc_running_gains.rest_length, osc_running_gains.rest_length_offset); + auto l_foot_traj_generator = builder.AddSystem( + plant, plant_context.get(), "toe_left", "pelvis", kLeftStance); + auto r_foot_traj_generator = builder.AddSystem( + plant, plant_context.get(), "toe_right", "pelvis", kRightStance); + l_foot_traj_generator->SetFootstepGains(osc_running_gains.K_d_footstep); + r_foot_traj_generator->SetFootstepGains(osc_running_gains.K_d_footstep); + l_foot_traj_generator->SetFootPlacementOffsets( + osc_running_gains.rest_length, osc_running_gains.rest_length_offset, + osc_running_gains.footstep_lateral_offset, + osc_running_gains.footstep_sagital_offset, + osc_running_gains.mid_foot_height); + r_foot_traj_generator->SetFootPlacementOffsets( + osc_running_gains.rest_length, osc_running_gains.rest_length_offset, + osc_running_gains.footstep_lateral_offset, + osc_running_gains.footstep_sagital_offset, + osc_running_gains.mid_foot_height); + + pelvis_tracking_data = std::make_unique( + "pelvis_trans_traj", osc_running_gains.K_p_pelvis, + osc_running_gains.K_d_pelvis, osc_running_gains.W_pelvis, plant, plant); + stance_foot_tracking_data = std::make_unique( + "stance_ft_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant, + plant); + left_foot_tracking_data = std::make_unique( + "left_ft_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant, + plant); + right_foot_tracking_data = std::make_unique( + "right_ft_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant, + plant); + pelvis_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftStance, + "pelvis"); + pelvis_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightStance, + "pelvis"); + stance_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kLeftStance, "toe_left"); + stance_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kRightStance, "toe_right"); + left_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kRightStance, "toe_left"); + right_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kLeftStance, "toe_right"); + left_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kRightFlight, "toe_left"); + right_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kLeftFlight, "toe_right"); + + left_foot_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight, + "toe_left"); + right_foot_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kRightFlight, "toe_right"); + + left_hip_tracking_data = std::make_unique( + "left_hip_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant, + plant); + right_hip_tracking_data = std::make_unique( + "right_hip_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant, + plant); + left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightStance, + "pelvis"); + right_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftStance, + "pelvis"); + right_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight, + "pelvis"); + left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightFlight, + "pelvis"); + + left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight, + "pelvis"); + right_hip_tracking_data->AddStateAndPointToTrack( + RunningFsmState::kRightFlight, "pelvis"); + + left_foot_rel_tracking_data = + std::make_unique( + "left_ft_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, + plant, plant, left_foot_tracking_data.get(), + left_hip_tracking_data.get()); + right_foot_rel_tracking_data = + std::make_unique( + "right_ft_traj", osc_running_gains.K_p_swing_foot, + osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, + plant, plant, right_foot_tracking_data.get(), + right_hip_tracking_data.get()); + pelvis_trans_rel_tracking_data = + std::make_unique( + "pelvis_trans_traj", osc_running_gains.K_p_pelvis, + osc_running_gains.K_d_pelvis, osc_running_gains.W_pelvis, plant, + plant, pelvis_tracking_data.get(), stance_foot_tracking_data.get()); + left_foot_rel_tracking_data->SetViewFrame(view_frame); + right_foot_rel_tracking_data->SetViewFrame(view_frame); + pelvis_trans_rel_tracking_data->SetViewFrame(view_frame); + + auto foot_traj_weight_trajectory = + std::make_shared>( + PiecewisePolynomial::FirstOrderHold( + {0, osc_running_gains.stance_duration + + 2 * osc_running_gains.flight_duration}, + {0.5 * VectorXd::Ones(1), 4.0 * VectorXd::Ones(1)})); + auto foot_traj_gain_trajectory = + std::make_shared>( + PiecewisePolynomial::FirstOrderHold( + {0, osc_running_gains.stance_duration + + 2 * osc_running_gains.flight_duration}, + {0.5 * MatrixXd::Identity(3, 3), + 1.5 * MatrixXd::Identity(3, 3)})); + left_foot_rel_tracking_data->SetTimeVaryingWeights( + foot_traj_weight_trajectory); + right_foot_rel_tracking_data->SetTimeVaryingWeights( + foot_traj_weight_trajectory); + left_foot_rel_tracking_data->SetTimeVaryingPDGainMultiplier( + foot_traj_gain_trajectory); + right_foot_rel_tracking_data->SetTimeVaryingPDGainMultiplier( + foot_traj_gain_trajectory); + + auto heading_traj_generator = + builder.AddSystem(plant, + plant_context.get()); + + pelvis_rot_tracking_data = std::make_unique( + "pelvis_rot_traj", osc_running_gains.K_p_pelvis_rot, + osc_running_gains.K_d_pelvis_rot, osc_running_gains.W_pelvis_rot, plant, + plant); + pelvis_rot_tracking_data->AddStateAndFrameToTrack( + RunningFsmState::kLeftStance, "pelvis"); + pelvis_rot_tracking_data->AddStateAndFrameToTrack( + RunningFsmState::kRightStance, "pelvis"); + pelvis_rot_tracking_data->AddStateAndFrameToTrack( + RunningFsmState::kRightFlight, "pelvis"); + pelvis_rot_tracking_data->AddStateAndFrameToTrack( + RunningFsmState::kLeftFlight, "pelvis"); + + if (osc_running_gains.rot_filter_tau > 0) { + pelvis_rot_tracking_data->SetLowPassFilter(osc_running_gains.rot_filter_tau, + {0, 1, 2}); + } + pelvis_rot_tracking_data->SetViewFrame(view_frame); + + // Swing toe joint trajectory + auto left_toe_angle_traj_gen = builder.AddSystem( + plant, plant_context.get(), pos_map["toe_left"], left_foot_points, + "left_toe_angle_traj"); + auto right_toe_angle_traj_gen = builder.AddSystem( + plant, plant_context.get(), pos_map["toe_right"], right_foot_points, + "right_toe_angle_traj"); + + // Swing toe joint tracking + left_toe_angle_tracking_data = std::make_unique( + "left_toe_angle_traj", osc_running_gains.K_p_swing_toe, + osc_running_gains.K_d_swing_toe, osc_running_gains.W_swing_toe, plant, + plant); + right_toe_angle_tracking_data = std::make_unique( + "right_toe_angle_traj", osc_running_gains.K_p_swing_toe, + osc_running_gains.K_d_swing_toe, osc_running_gains.W_swing_toe, plant, + plant); + left_toe_angle_tracking_data->AddStateAndJointToTrack( + RunningFsmState::kRightStance, "toe_left", "toe_leftdot"); + left_toe_angle_tracking_data->AddStateAndJointToTrack( + RunningFsmState::kLeftFlight, "toe_left", "toe_leftdot"); + left_toe_angle_tracking_data->AddStateAndJointToTrack( + RunningFsmState::kRightFlight, "toe_left", "toe_leftdot"); + right_toe_angle_tracking_data->AddStateAndJointToTrack( + RunningFsmState::kLeftStance, "toe_right", "toe_rightdot"); + right_toe_angle_tracking_data->AddStateAndJointToTrack( + RunningFsmState::kLeftFlight, "toe_right", "toe_rightdot"); + right_toe_angle_tracking_data->AddStateAndJointToTrack( + RunningFsmState::kRightFlight, "toe_right", "toe_rightdot"); + + // Swing hip yaw joint tracking + left_hip_yaw_tracking_data = std::make_unique( + "hip_yaw_left_traj", osc_running_gains.K_p_hip_yaw, + osc_running_gains.K_d_hip_yaw, osc_running_gains.W_hip_yaw, plant, plant); + right_hip_yaw_tracking_data = std::make_unique( + "hip_yaw_right_traj", osc_running_gains.K_p_hip_yaw, + osc_running_gains.K_d_hip_yaw, osc_running_gains.W_hip_yaw, plant, plant); + left_hip_yaw_tracking_data->AddJointToTrack("hip_yaw_left", + "hip_yaw_leftdot"); + right_hip_yaw_tracking_data->AddJointToTrack("hip_yaw_right", + "hip_yaw_rightdot"); + + left_foot_rel_tracking_data->SetImpactInvariantProjection(true); + right_foot_rel_tracking_data->SetImpactInvariantProjection(true); + pelvis_trans_rel_tracking_data->SetImpactInvariantProjection(true); + left_hip_yaw_tracking_data->SetImpactInvariantProjection(true); + right_hip_yaw_tracking_data->SetImpactInvariantProjection(true); + pelvis_rot_tracking_data->SetImpactInvariantProjection(true); + left_toe_angle_tracking_data->SetImpactInvariantProjection(true); + right_toe_angle_tracking_data->SetImpactInvariantProjection(true); + osc->AddTrackingData(std::move(pelvis_rot_tracking_data)); + osc->AddTrackingData(std::move(pelvis_trans_rel_tracking_data)); + osc->AddTrackingData(std::move(left_foot_rel_tracking_data)); + osc->AddTrackingData(std::move(right_foot_rel_tracking_data)); + osc->AddTrackingData(std::move(left_toe_angle_tracking_data)); + osc->AddTrackingData(std::move(right_toe_angle_tracking_data)); + osc->AddConstTrackingData(std::move(left_hip_yaw_tracking_data), + VectorXd::Zero(1)); + osc->AddConstTrackingData(std::move(right_hip_yaw_tracking_data), + VectorXd::Zero(1)); + + // Build OSC problem + osc->SetOsqpSolverOptionsFromYaml(osqp_settings_filename); + osc->Build(); + + /*****Connect ports*****/ + + // OSC connections + builder.Connect(contact_scheduler->get_output_port_fsm(), + osc->get_input_port_fsm()); + builder.Connect(contact_scheduler->get_output_port_impact_info(), + osc->get_input_port_impact_info()); + builder.Connect(contact_scheduler->get_output_port_clock(), + osc->get_input_port_clock()); + builder.Connect(state_receiver->get_output_port(0), + osc->get_input_port_robot_output()); + // FSM connections + builder.Connect(state_receiver->get_output_port(0), + contact_scheduler->get_input_port_state()); + + // Trajectory generator connections + builder.Connect( + contact_scheduler->get_output_port_contact_scheduler(), + pelvis_trans_traj_generator->get_contact_scheduler_input_port()); + builder.Connect(contact_scheduler->get_output_port_contact_scheduler(), + l_foot_traj_generator->get_input_port_contact_scheduler()); + builder.Connect(contact_scheduler->get_output_port_contact_scheduler(), + r_foot_traj_generator->get_input_port_contact_scheduler()); + + builder.Connect(state_receiver->get_output_port(0), + pelvis_trans_traj_generator->get_state_input_port()); + builder.Connect(contact_scheduler->get_output_port_fsm(), + pelvis_trans_traj_generator->get_fsm_input_port()); + builder.Connect(contact_scheduler->get_output_port_clock(), + pelvis_trans_traj_generator->get_clock_input_port()); + builder.Connect(high_level_command->get_output_port_xy(), + l_foot_traj_generator->get_input_port_target_vel()); + builder.Connect(high_level_command->get_output_port_xy(), + r_foot_traj_generator->get_input_port_target_vel()); + builder.Connect(state_receiver->get_output_port(0), + l_foot_traj_generator->get_input_port_state()); + builder.Connect(state_receiver->get_output_port(0), + r_foot_traj_generator->get_input_port_state()); + builder.Connect(radio_parser->get_output_port(), + l_foot_traj_generator->get_input_port_radio()); + builder.Connect(radio_parser->get_output_port(), + r_foot_traj_generator->get_input_port_radio()); + + builder.Connect(contact_scheduler->get_output_port_clock(), + l_foot_traj_generator->get_input_port_clock()); + builder.Connect(contact_scheduler->get_output_port_clock(), + r_foot_traj_generator->get_input_port_clock()); + builder.Connect(contact_scheduler->get_output_port_fsm(), + l_foot_traj_generator->get_input_port_fsm()); + builder.Connect(contact_scheduler->get_output_port_fsm(), + r_foot_traj_generator->get_input_port_fsm()); + builder.Connect(state_receiver->get_output_port(0), + left_toe_angle_traj_gen->get_input_port_state()); + builder.Connect(state_receiver->get_output_port(0), + right_toe_angle_traj_gen->get_input_port_state()); + // OSC connections + builder.Connect(pelvis_trans_traj_generator->get_output_port(0), + osc->get_input_port_tracking_data("pelvis_trans_traj")); + builder.Connect(state_receiver->get_output_port(0), + heading_traj_generator->get_state_input_port()); + builder.Connect(high_level_command->get_output_port_yaw(), + heading_traj_generator->get_yaw_input_port()); + builder.Connect(heading_traj_generator->get_output_port(0), + osc->get_input_port_tracking_data("pelvis_rot_traj")); + builder.Connect(l_foot_traj_generator->get_output_port(0), + osc->get_input_port_tracking_data("left_ft_traj")); + builder.Connect(r_foot_traj_generator->get_output_port(0), + osc->get_input_port_tracking_data("right_ft_traj")); + // builder.Connect(l_foot_traj_generator->get_output_port(0), + // osc->get_input_port_tracking_data("left_ft_z_traj")); + // builder.Connect(r_foot_traj_generator->get_output_port(0), + // osc->get_input_port_tracking_data("right_ft_z_traj")); + builder.Connect(left_toe_angle_traj_gen->get_output_port(0), + osc->get_input_port_tracking_data("left_toe_angle_traj")); + builder.Connect(right_toe_angle_traj_gen->get_output_port(0), + osc->get_input_port_tracking_data("right_toe_angle_traj")); + builder.Connect(osc->get_output_port_osc_command(), + command_sender->get_input_port(0)); + builder.Connect(radio_parser->get_output_port(), + high_level_command->get_input_port_radio()); + + // Publisher connections + builder.ExportInput(state_receiver->get_input_port(), "lcmt_robot_output"); + builder.ExportInput(radio_parser->get_input_port(), "raw_radio"); + builder.ExportOutput(command_sender->get_output_port(), "lcmt_robot_input"); + builder.ExportOutput(osc->get_output_port_osc_command(), "u, t"); + builder.ExportOutput(failure_aggregator->get_status_output_port(), + "lcmt_controller_failure"); + + builder.BuildInto(this); + this->set_name(("osc_running_controller_diagram")); + DrawAndSaveDiagramGraph(*this); +} + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/Cassie/diagrams/osc_running_controller_diagram.h b/examples/Cassie/diagrams/osc_running_controller_diagram.h new file mode 100644 index 0000000000..1df206e906 --- /dev/null +++ b/examples/Cassie/diagrams/osc_running_controller_diagram.h @@ -0,0 +1,149 @@ +#pragma once + +#include +#include + +#include + +#include "examples/Cassie/osc_run/osc_running_gains.h" +#include "multibody/kinematic/distance_evaluator.h" +#include "multibody/kinematic/fixed_joint_evaluator.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" +#include "systems/controllers/osc/com_tracking_data.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/osc_gains.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" + +#include "drake/common/drake_copyable.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/system.h" + +namespace dairlib { +namespace examples { +namespace controllers { + +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RelativeTranslationTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; + +class OSCRunningControllerDiagram final + : public drake::systems::Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OSCRunningControllerDiagram) + + /// @param[in] osc_gains_filename filepath containing the osc_running_gains. + /// @param[in] osqp_settings filepath containing the osqp settings. + OSCRunningControllerDiagram(drake::multibody::MultibodyPlant& plant, + const std::string& osc_gains_filename, + const std::string& osqp_settings_filename); + + /// @return the input port for the plant state. + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_port_); + } + + /// @return the input port for the cassie_out struct (containing radio + /// commands). + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + /// @return the output port for the lcmt_robot_input message. + const drake::systems::OutputPort& get_output_port_robot_input() + const { + return this->get_output_port(control_port_); + } + + /// @return the output port for the controller torques. + const drake::systems::OutputPort& get_output_port_torque() const { + return this->get_output_port(torque_port_); + } + + /// @return the output port for the failure status of the controller. + const drake::systems::OutputPort& get_output_port_controller_failure() + const { + return this->get_output_port(controller_failure_port_); + } + + drake::multibody::MultibodyPlant& get_plant() { return *plant_; } + + private: + drake::multibody::MultibodyPlant* plant_; + std::map pos_map; + std::map vel_map; + std::map act_map; + std::pair&> + left_toe; + std::pair&> + left_heel; + std::pair&> + right_toe; + std::pair&> + right_heel; + std::vector< + std::pair&>> + left_foot_points; + std::vector< + std::pair&>> + right_foot_points; + std::shared_ptr> view_frame; + multibody::WorldPointEvaluator left_toe_evaluator; + multibody::WorldPointEvaluator left_heel_evaluator; + multibody::WorldPointEvaluator right_toe_evaluator; + multibody::WorldPointEvaluator right_heel_evaluator; + multibody::DistanceEvaluator left_loop; + multibody::DistanceEvaluator right_loop; + std::unordered_map< + int, std::vector&>>> + feet_contact_points; + std::vector fsm_states; + std::vector state_durations; + std::vector accumulated_state_durations; + std::unique_ptr> plant_context; + + multibody::KinematicEvaluatorSet evaluators; + + multibody::FixedJointEvaluator left_fixed_knee_spring; + multibody::FixedJointEvaluator right_fixed_knee_spring; + multibody::FixedJointEvaluator left_fixed_ankle_spring; + multibody::FixedJointEvaluator right_fixed_ankle_spring; + + std::unique_ptr pelvis_tracking_data; + std::unique_ptr stance_foot_tracking_data; + std::unique_ptr left_foot_tracking_data; + std::unique_ptr right_foot_tracking_data; + std::unique_ptr left_hip_tracking_data; + std::unique_ptr right_hip_tracking_data; + std::unique_ptr left_foot_rel_tracking_data; + std::unique_ptr right_foot_rel_tracking_data; + std::unique_ptr + pelvis_trans_rel_tracking_data; + std::unique_ptr pelvis_rot_tracking_data; + std::unique_ptr left_toe_angle_tracking_data; + std::unique_ptr right_toe_angle_tracking_data; + std::unique_ptr left_hip_yaw_tracking_data; + std::unique_ptr right_hip_yaw_tracking_data; + + const drake::systems::InputPortIndex state_port_ = + drake::systems::InputPortIndex(0); + const drake::systems::InputPortIndex radio_port_ = + drake::systems::InputPortIndex(1); + const drake::systems::OutputPortIndex control_port_ = + drake::systems::OutputPortIndex(0); + const drake::systems::OutputPortIndex torque_port_ = + drake::systems::OutputPortIndex(1); + const drake::systems::OutputPortIndex controller_failure_port_ = + drake::systems::OutputPortIndex(2); + + const std::string control_channel_name_ = "OSC_RUNNING"; +}; + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/Cassie/diagrams/osc_walking_controller_diagram.cc b/examples/Cassie/diagrams/osc_walking_controller_diagram.cc new file mode 100644 index 0000000000..7ac223fdee --- /dev/null +++ b/examples/Cassie/diagrams/osc_walking_controller_diagram.cc @@ -0,0 +1,502 @@ +#include "osc_walking_controller_diagram.h" + +#include "dairlib/lcmt_robot_input.hpp" +#include "dairlib/lcmt_robot_output.hpp" +#include "examples/Cassie/cassie_utils.h" +#include "examples/Cassie/osc/heading_traj_generator.h" +#include "examples/Cassie/osc/high_level_command.h" +#include "examples/Cassie/osc/osc_walking_gains.h" +#include "examples/Cassie/osc/osc_walking_gains_alip.h" +#include "examples/Cassie/osc/swing_toe_traj_generator.h" +#include "examples/Cassie/osc/walking_speed_control.h" +#include "multibody/kinematic/fixed_joint_evaluator.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/multibody_utils.h" +#include "systems/controllers/alip_swing_ft_traj_gen.h" +#include "systems/controllers/alip_traj_gen.h" +#include "systems/controllers/fsm_event_time.h" +#include "systems/controllers/lipm_traj_gen.h" +#include "systems/controllers/osc/com_tracking_data.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/operational_space_control.h" +#include "systems/controllers/osc/options_tracking_data.h" +#include "systems/controllers/osc/osc_tracking_data.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" +#include "systems/controllers/swing_ft_traj_gen.h" +#include "systems/controllers/time_based_fsm.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +#include "drake/common/yaml/yaml_io.h" +#include "drake/systems/framework/diagram_builder.h" + +namespace dairlib { + +using std::map; +using std::pair; +using std::string; +using std::vector; + +using Eigen::Matrix3d; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using cassie::osc::SwingToeTrajGenerator; +using drake::geometry::SceneGraph; +using drake::multibody::Frame; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +// using drake::systems::lcm::LcmPublisherSystem; +// using drake::systems::lcm::LcmSubscriberSystem; +using drake::trajectories::PiecewisePolynomial; +using multibody::FixedJointEvaluator; +using multibody::WorldYawViewFrame; +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RelativeTranslationTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; + +namespace examples { +namespace controllers { + +OSCWalkingControllerDiagram::OSCWalkingControllerDiagram( + drake::multibody::MultibodyPlant& plant, bool has_double_stance, + const string& osc_walking_gains_filename, + const string& osqp_settings_filename) + : plant_(&plant), + pos_map(multibody::MakeNameToPositionsMap(plant)), + vel_map(multibody::MakeNameToVelocitiesMap(plant)), + act_map(multibody::MakeNameToActuatorsMap(plant)), + left_toe(LeftToeFront(plant)), + left_heel(LeftToeRear(plant)), + right_toe(RightToeFront(plant)), + right_heel(RightToeRear(plant)), + left_toe_mid(std::pair&>( + (left_toe.first + left_heel.first) / 2, + plant.GetFrameByName("toe_left"))), + right_toe_mid(std::pair&>( + (left_toe.first + left_heel.first) / 2, + plant.GetFrameByName("toe_right"))), + left_toe_origin(std::pair&>( + Vector3d::Zero(), plant.GetFrameByName("toe_left"))), + right_toe_origin(std::pair&>( + Vector3d::Zero(), plant.GetFrameByName("toe_right"))), + left_right_foot({left_toe_origin, right_toe_origin}), + left_foot_points({left_heel, left_toe}), + right_foot_points({right_heel, right_toe}), + view_frame_(std::make_shared>( + plant.GetBodyByName("pelvis"))), + left_toe_evaluator(multibody::WorldPointEvaluator( + plant, left_toe.first, left_toe.second, *view_frame_, + Matrix3d::Identity(), Vector3d::Zero(), {1, 2})), + left_heel_evaluator(multibody::WorldPointEvaluator( + plant, left_heel.first, left_heel.second, *view_frame_, + Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2})), + right_toe_evaluator(multibody::WorldPointEvaluator( + plant, right_toe.first, right_toe.second, *view_frame_, + Matrix3d::Identity(), Vector3d::Zero(), {1, 2})), + right_heel_evaluator(multibody::WorldPointEvaluator( + plant, right_heel.first, right_heel.second, *view_frame_, + Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2})), + left_loop(LeftLoopClosureEvaluator(plant)), + right_loop(RightLoopClosureEvaluator(plant)), + evaluators(multibody::KinematicEvaluatorSet(plant)), + left_fixed_knee_spring( + FixedJointEvaluator(plant, pos_map.at("knee_joint_left"), + vel_map.at("knee_joint_leftdot"), 0)), + right_fixed_knee_spring( + FixedJointEvaluator(plant, pos_map.at("knee_joint_right"), + vel_map.at("knee_joint_rightdot"), 0)), + left_fixed_ankle_spring( + FixedJointEvaluator(plant, pos_map.at("ankle_spring_joint_left"), + vel_map.at("ankle_spring_joint_leftdot"), 0)), + right_fixed_ankle_spring( + FixedJointEvaluator(plant, pos_map.at("ankle_spring_joint_right"), + vel_map.at("ankle_spring_joint_rightdot"), 0)) { + // Build the controller diagram + DiagramBuilder builder; + plant_context = plant.CreateDefaultContext(); + feet_contact_points[0] = std::vector< + std::pair&>>( + {left_toe, left_heel}); + feet_contact_points[1] = std::vector< + std::pair&>>( + {right_toe, right_heel}); + + // Read-in the parameters + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + OSCGains osc_gains = drake::yaml::LoadYamlFile( + FindResourceOrThrow(osc_walking_gains_filename), {}, {}, yaml_options); + OSCWalkingGainsALIP osc_walking_gains = + drake::yaml::LoadYamlFile( + osc_walking_gains_filename); + + /**** FSM and contact mode configuration ****/ + int left_stance_state = 0; + int right_stance_state = 1; + int post_left_double_support_state = 3; + int post_right_double_support_state = 4; + double left_support_duration = osc_walking_gains.ss_time; + double right_support_duration = osc_walking_gains.ss_time; + double double_support_duration = osc_walking_gains.ds_time; + // vector fsm_states; + // vector state_durations; + if (has_double_stance) { + fsm_states = {left_stance_state, post_left_double_support_state, + right_stance_state, post_right_double_support_state}; + state_durations = {left_support_duration, double_support_duration, + right_support_duration, double_support_duration}; + unordered_fsm_states = {left_stance_state, right_stance_state, + post_left_double_support_state, + post_right_double_support_state}; + unordered_state_durations = {left_support_duration, right_support_duration, + double_support_duration, + double_support_duration}; + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + + } else { + fsm_states = {left_stance_state, right_stance_state}; + state_durations = {left_support_duration, right_support_duration}; + unordered_fsm_states = {left_stance_state, right_stance_state}; + unordered_state_durations = {left_support_duration, right_support_duration}; + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + } + single_support_states = {left_stance_state, right_stance_state}; + double_support_states = {post_left_double_support_state, + post_right_double_support_state}; + left_right_support_fsm_states = {left_stance_state, right_stance_state}; + left_right_support_state_durations = {left_support_duration, + right_support_duration}; + swing_ft_gain_multiplier_breaks = {0, left_support_duration / 2, + left_support_duration}; + swing_ft_gain_multiplier_samples = std::vector>( + 3, drake::MatrixX::Identity(3, 3)); + swing_ft_gain_multiplier_samples[2](2, 2) *= 0.3; + auto swing_ft_gain_multiplier_gain_multiplier = + std::make_shared>( + PiecewisePolynomial::FirstOrderHold( + swing_ft_gain_multiplier_breaks, + swing_ft_gain_multiplier_samples)); + swing_ft_accel_gain_multiplier_breaks = {0, left_support_duration / 2, + left_support_duration * 3 / 4, + left_support_duration}; + swing_ft_accel_gain_multiplier_samples = std::vector>( + 4, drake::MatrixX::Identity(3, 3)); + swing_ft_accel_gain_multiplier_samples[2](2, 2) *= 0; + swing_ft_accel_gain_multiplier_samples[3](2, 2) *= 0; + auto swing_ft_accel_gain_multiplier_gain_multiplier = + std::make_shared>( + PiecewisePolynomial::FirstOrderHold( + swing_ft_accel_gain_multiplier_breaks, + swing_ft_accel_gain_multiplier_samples)); + + /**** Initialize all the leaf systems ****/ + + auto state_receiver = builder.AddSystem(plant); + auto command_sender = builder.AddSystem(plant); + auto osc = builder.AddSystem( + plant, plant_context.get(), true); + auto fsm = builder.AddSystem( + plant, fsm_states, state_durations); + auto liftoff_event_time = + builder.AddSystem( + plant, single_support_states); + auto touchdown_event_time = + builder.AddSystem( + plant, double_support_states); + auto radio_parser = builder.AddSystem(); + liftoff_event_time->set_name("liftoff_time"); + touchdown_event_time->set_name("touchdown_time"); + /**** OSC setup ****/ + + /// REGULARIZATION COSTS + int n_v = plant.num_velocities(); + int n_u = plant.num_actuators(); + MatrixXd Q_accel = osc_walking_gains.w_accel * MatrixXd::Identity(n_v, n_v); + osc->SetAccelerationCostWeights(Q_accel); + osc->SetInputSmoothingCostWeights(osc_walking_gains.w_input_reg * + MatrixXd::Identity(n_u, n_u)); + // Soft constraint on contacts + osc->SetContactSoftConstraintWeight(osc_walking_gains.w_soft_constraint); + + // Contact information for OSC + osc->SetContactFriction(osc_walking_gains.mu); + + osc->AddContactPoint("left_toe", + std::unique_ptr>( + &left_toe_evaluator), + {left_stance_state, post_left_double_support_state, + post_right_double_support_state}); + osc->AddContactPoint("left_heel", + std::unique_ptr>( + &left_heel_evaluator), + {left_stance_state, post_left_double_support_state, + post_right_double_support_state}); + osc->AddContactPoint("right_toe", + std::unique_ptr>( + &right_toe_evaluator), + {right_stance_state, post_left_double_support_state, + post_right_double_support_state}); + osc->AddContactPoint("right_heel", + std::unique_ptr>( + &right_heel_evaluator), + {right_stance_state, post_left_double_support_state, + post_right_double_support_state}); + + evaluators.add_evaluator(&left_loop); + evaluators.add_evaluator(&right_loop); + + // Fix the springs in the dynamics + evaluators.add_evaluator(&left_fixed_knee_spring); + evaluators.add_evaluator(&right_fixed_knee_spring); + evaluators.add_evaluator(&left_fixed_ankle_spring); + evaluators.add_evaluator(&right_fixed_ankle_spring); + + osc->AddKinematicConstraint( + std::unique_ptr>(&evaluators)); + + /**** Trajectory Generators ****/ + + std::cout << "Creating output trajectory leaf systems. " << std::endl; + + cassie::osc::HighLevelCommand* high_level_command; + high_level_command = builder.AddSystem( + plant, plant_context.get(), osc_walking_gains.vel_scale_rot, + osc_walking_gains.vel_scale_trans_sagital, + osc_walking_gains.vel_scale_trans_lateral); + auto head_traj_gen = builder.AddSystem( + plant, plant_context.get()); + auto lipm_traj_generator = builder.AddSystem( + plant, plant_context.get(), osc_walking_gains.lipm_height, + unordered_fsm_states, unordered_state_durations, + contact_points_in_each_state); + auto pelvis_traj_generator = builder.AddSystem( + plant, plant_context.get(), osc_walking_gains.lipm_height, + unordered_fsm_states, unordered_state_durations, + contact_points_in_each_state, false); + auto swing_ft_traj_generator = + builder.AddSystem( + plant, plant_context.get(), left_right_support_fsm_states, + left_right_support_state_durations, left_right_foot, "pelvis", + double_support_duration, osc_walking_gains.mid_foot_height, + osc_walking_gains.final_foot_height, + osc_walking_gains.final_foot_velocity_z, + osc_walking_gains.max_CoM_to_footstep_dist, + osc_walking_gains.footstep_offset, + osc_walking_gains.center_line_offset); + auto left_toe_angle_traj_gen = + builder.AddSystem( + plant, plant_context.get(), pos_map["toe_left"], left_foot_points, + "left_toe_angle_traj"); + auto right_toe_angle_traj_gen = + builder.AddSystem( + plant, plant_context.get(), pos_map["toe_right"], right_foot_points, + "right_toe_angle_traj"); + + auto alip_traj_generator = builder.AddSystem( + plant, plant_context.get(), osc_walking_gains.lipm_height, + unordered_fsm_states, unordered_state_durations, + contact_points_in_each_state, + osc_walking_gains.Q_alip_kalman_filter.asDiagonal(), + osc_walking_gains.R_alip_kalman_filter.asDiagonal()); + + builder.Connect(fsm->get_output_port(0), + alip_traj_generator->get_input_port_fsm()); + builder.Connect(touchdown_event_time->get_output_port_event_time(), + alip_traj_generator->get_input_port_touchdown_time()); + builder.Connect(state_receiver->get_output_port(0), + alip_traj_generator->get_input_port_state()); + + /**** Tracking Data *****/ + + std::cout << "Creating tracking data. " << std::endl; + + swing_foot_data = std::make_unique( + "swing_ft_data", osc_walking_gains.K_p_swing_foot, + osc_walking_gains.K_d_swing_foot, osc_walking_gains.W_swing_foot, plant, + plant); + swing_foot_data->AddStateAndPointToTrack(left_stance_state, "toe_right"); + swing_foot_data->AddStateAndPointToTrack(right_stance_state, "toe_left"); + com_data = std::make_unique( + "com_data", osc_walking_gains.K_p_swing_foot, + osc_walking_gains.K_d_swing_foot, osc_walking_gains.W_swing_foot, plant, + plant); + com_data->AddFiniteStateToTrack(left_stance_state); + com_data->AddFiniteStateToTrack(right_stance_state); + swing_ft_traj_local_ = std::make_unique( + "swing_ft_traj", osc_walking_gains.K_p_swing_foot, + osc_walking_gains.K_d_swing_foot, osc_walking_gains.W_swing_foot, plant, + plant, swing_foot_data.get(), com_data.get()); + swing_ft_traj_local_->SetViewFrame(view_frame_); + + swing_ft_traj_global_ = std::make_unique( + "swing_ft_traj", osc_walking_gains.K_p_swing_foot, + osc_walking_gains.K_d_swing_foot, osc_walking_gains.W_swing_foot, plant, + plant); + swing_ft_traj_global_->AddStateAndPointToTrack(left_stance_state, + "toe_right"); + swing_ft_traj_global_->AddStateAndPointToTrack(right_stance_state, + "toe_left"); + swing_ft_traj_local_->SetTimeVaryingPDGainMultiplier( + swing_ft_gain_multiplier_gain_multiplier); + swing_ft_traj_local_->SetTimerVaryingFeedForwardAccelMultiplier( + swing_ft_accel_gain_multiplier_gain_multiplier); + osc->AddTrackingData(std::move(swing_ft_traj_local_)); + bool use_pelvis_for_lipm_tracking = true; + + pelvis_traj_ = std::make_unique( + "lipm_traj", osc_walking_gains.K_p_com, osc_walking_gains.K_d_com, + osc_walking_gains.W_com, plant, plant); + pelvis_traj_->AddPointToTrack("pelvis"); + osc->AddTrackingData(std::move(pelvis_traj_)); + + pelvis_balance_traj_ = std::make_unique( + "pelvis_balance_traj_", osc_walking_gains.K_p_pelvis_balance, + osc_walking_gains.K_d_pelvis_balance, osc_walking_gains.W_pelvis_balance, + plant, plant); + pelvis_balance_traj_->AddFrameToTrack("pelvis"); + osc->AddTrackingData(std::move(pelvis_balance_traj_)); + // Pelvis rotation tracking (yaw) + pelvis_heading_traj_ = std::make_unique( + "pelvis_heading_traj_", osc_walking_gains.K_p_pelvis_heading, + osc_walking_gains.K_d_pelvis_heading, osc_walking_gains.W_pelvis_heading, + plant, plant); + pelvis_heading_traj_->AddFrameToTrack("pelvis"); + osc->AddTrackingData(std::move(pelvis_heading_traj_), + osc_walking_gains.period_of_no_heading_control); + + swing_toe_traj_left_ = std::make_unique( + "left_toe_angle_traj", osc_walking_gains.K_p_swing_toe, + osc_walking_gains.K_d_swing_toe, osc_walking_gains.W_swing_toe, plant, + plant); + swing_toe_traj_right_ = std::make_unique( + "right_toe_angle_traj", osc_walking_gains.K_p_swing_toe, + osc_walking_gains.K_d_swing_toe, osc_walking_gains.W_swing_toe, plant, + plant); + swing_toe_traj_left_->AddStateAndJointToTrack(right_stance_state, "toe_left", + "toe_leftdot"); + swing_toe_traj_right_->AddStateAndJointToTrack(left_stance_state, "toe_right", + "toe_rightdot"); + osc->AddTrackingData(std::move(swing_toe_traj_left_)); + osc->AddTrackingData(std::move(swing_toe_traj_right_)); + + swing_hip_yaw_traj_ = std::make_unique( + "swing_hip_yaw_traj_", osc_walking_gains.K_p_hip_yaw, + osc_walking_gains.K_d_hip_yaw, osc_walking_gains.W_hip_yaw, plant, plant); + swing_hip_yaw_traj_->AddStateAndJointToTrack( + left_stance_state, "hip_yaw_right", "hip_yaw_rightdot"); + swing_hip_yaw_traj_->AddStateAndJointToTrack( + right_stance_state, "hip_yaw_left", "hip_yaw_leftdot"); + osc->AddConstTrackingData(std::move(swing_hip_yaw_traj_), VectorXd::Zero(1)); + + /**** OSC settings ****/ + + // Build OSC problem + // Set double support duration for force blending + osc->SetUpDoubleSupportPhaseBlending( + double_support_duration, left_stance_state, right_stance_state, + {post_left_double_support_state, post_right_double_support_state}); + + osc->SetOsqpSolverOptionsFromYaml(osqp_settings_filename); + + // Build OSC problem + osc->Build(); + std::cout << "Built OSC" << std::endl; + + /*****Connect ports*****/ + + // OSC connections + // builder.Connect(cassie_out_receiver->get_output_port(), + // high_level_command->get_cassie_out_input_port()); + builder.Connect(state_receiver->get_output_port(0), + high_level_command->get_input_port_state()); + builder.Connect(state_receiver->get_output_port(0), + head_traj_gen->get_state_input_port()); + builder.Connect(high_level_command->get_output_port_yaw(), + head_traj_gen->get_yaw_input_port()); + builder.Connect(state_receiver->get_output_port(0), + fsm->get_state_input_port()); + builder.Connect(fsm->get_output_port(0), + liftoff_event_time->get_input_port_fsm()); + builder.Connect(state_receiver->get_output_port(0), + liftoff_event_time->get_input_port_state()); + builder.Connect(fsm->get_output_port(0), + touchdown_event_time->get_input_port_fsm()); + builder.Connect(state_receiver->get_output_port(0), + touchdown_event_time->get_input_port_state()); + builder.Connect(fsm->get_output_port(0), + lipm_traj_generator->get_input_port_fsm()); + builder.Connect(touchdown_event_time->get_output_port_event_time(), + lipm_traj_generator->get_input_port_touchdown_time()); + builder.Connect(state_receiver->get_output_port(0), + lipm_traj_generator->get_input_port_state()); + builder.Connect(fsm->get_output_port(0), + pelvis_traj_generator->get_input_port_fsm()); + builder.Connect(touchdown_event_time->get_output_port_event_time(), + pelvis_traj_generator->get_input_port_touchdown_time()); + builder.Connect(state_receiver->get_output_port(0), + pelvis_traj_generator->get_input_port_state()); + builder.Connect(fsm->get_output_port(0), + swing_ft_traj_generator->get_input_port_fsm()); + builder.Connect(liftoff_event_time->get_output_port_event_time_of_interest(), + swing_ft_traj_generator->get_input_port_fsm_switch_time()); + builder.Connect(state_receiver->get_output_port(0), + swing_ft_traj_generator->get_input_port_state()); + builder.Connect(alip_traj_generator->get_output_port_alip_state(), + swing_ft_traj_generator->get_input_port_alip_state()); + builder.Connect(high_level_command->get_output_port_xy(), + swing_ft_traj_generator->get_input_port_vdes()); + builder.Connect(state_receiver->get_output_port(0), + left_toe_angle_traj_gen->get_input_port_state()); + builder.Connect(state_receiver->get_output_port(0), + right_toe_angle_traj_gen->get_input_port_state()); + builder.Connect(state_receiver->get_output_port(0), + osc->get_input_port_robot_output()); + builder.Connect(fsm->get_output_port(0), osc->get_input_port_fsm()); + builder.Connect(pelvis_traj_generator->get_output_port_lipm_from_touchdown(), + osc->get_input_port_tracking_data("lipm_traj")); + builder.Connect(swing_ft_traj_generator->get_output_port(0), + osc->get_input_port_tracking_data("swing_ft_traj")); + builder.Connect(head_traj_gen->get_output_port(0), + osc->get_input_port_tracking_data("pelvis_heading_traj_")); + builder.Connect(head_traj_gen->get_output_port(0), + osc->get_input_port_tracking_data("pelvis_balance_traj_")); + builder.Connect(left_toe_angle_traj_gen->get_output_port(0), + osc->get_input_port_tracking_data("left_toe_angle_traj")); + builder.Connect(right_toe_angle_traj_gen->get_output_port(0), + osc->get_input_port_tracking_data("right_toe_angle_traj")); + builder.Connect(osc->get_output_port(0), command_sender->get_input_port(0)); + builder.Connect(radio_parser->get_output_port(), + high_level_command->get_input_port_radio()); + + // Publisher connections + builder.ExportInput(state_receiver->get_input_port(), "x, u, t"); + builder.ExportInput(radio_parser->get_input_port(), "raw_radio"); + builder.ExportOutput(command_sender->get_output_port(), "lcmt_robot_input"); + builder.ExportOutput(osc->get_output_port_osc_command(), "u, t"); + + // builder.ExportOutput(failure_aggregator->get_status_output_port(), + // "failure_status"); + + builder.BuildInto(this); + this->set_name(("osc_walking_controller_diagram")); + DrawAndSaveDiagramGraph(*this); + std::cout << "Built controller" << std::endl; +} + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/Cassie/diagrams/osc_walking_controller_diagram.h b/examples/Cassie/diagrams/osc_walking_controller_diagram.h new file mode 100644 index 0000000000..34d50e720f --- /dev/null +++ b/examples/Cassie/diagrams/osc_walking_controller_diagram.h @@ -0,0 +1,173 @@ +#pragma once + +#include +#include + +#include +#include + +#include "examples/Cassie/osc/osc_walking_gains.h" +#include "multibody/kinematic/distance_evaluator.h" +#include "multibody/kinematic/fixed_joint_evaluator.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" +#include "systems/controllers/osc/com_tracking_data.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/osc_gains.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" + +#include "drake/common/drake_copyable.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/system.h" + +namespace dairlib { +namespace examples { +namespace controllers { + +using drake::multibody::Frame; +using Eigen::Vector3d; +using systems::controllers::ComTrackingData; +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RelativeTranslationTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; + +class OSCWalkingControllerDiagram final + : public drake::systems::Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OSCWalkingControllerDiagram) + + /// @param[in] osc_gains_filename filepath containing the osc_running_gains. + /// @param[in] osqp_settings filepath containing the osqp settings. + OSCWalkingControllerDiagram(drake::multibody::MultibodyPlant& plant, + bool has_double_stance, + const std::string& osc_gains_filename, + const std::string& osqp_settings_filename); + + /// @return the input port for the plant state. + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_input_port_index_); + } + + /// @return the input port for the cassie_out struct (containing radio + /// commands). + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_input_port_index_); + } + + /// @return the output port for the controller torques. + const drake::systems::OutputPort& get_output_port_robot_input() const { + return this->get_output_port(control_output_port_index_); + } + + /// @return the output port for the controller torques. + const drake::systems::OutputPort& get_output_port_torque() const { + return this->get_output_port(torque_output_port_index_); + } + + /// @return the output port for the failure status of the controller. + const drake::systems::OutputPort& get_output_port_controller_failure() + const { + return this->get_output_port(controller_failure_port_index_); + } + + drake::multibody::MultibodyPlant& get_plant() { + return *plant_; + } + + private: + drake::multibody::MultibodyPlant* plant_; + std::map pos_map; + std::map vel_map; + std::map act_map; + std::pair&> + left_toe; + std::pair&> + left_heel; + std::pair&> + right_toe; + std::pair&> + right_heel; + std::pair&> + left_toe_mid; + std::pair&> + right_toe_mid; + std::pair&> + left_toe_origin; + std::pair&> + right_toe_origin; + std::vector&>> left_right_foot; + std::vector< + std::pair&>> + left_foot_points; + std::vector< + std::pair&>> + right_foot_points; + std::shared_ptr> view_frame_; + multibody::WorldPointEvaluator left_toe_evaluator; + multibody::WorldPointEvaluator left_heel_evaluator; + multibody::WorldPointEvaluator right_toe_evaluator; + multibody::WorldPointEvaluator right_heel_evaluator; + multibody::DistanceEvaluator left_loop; + multibody::DistanceEvaluator right_loop; + std::unordered_map< + int, std::vector&>>> + feet_contact_points; + std::vector fsm_states; + std::vector state_durations; + std::vector single_support_states; + std::vector double_support_states; + std::vector unordered_fsm_states; + std::vector unordered_state_durations; + std::vector&>>> + contact_points_in_each_state; + std::unique_ptr> plant_context; + std::vector left_right_support_fsm_states; + std::vector left_right_support_state_durations; + std::vector swing_ft_gain_multiplier_breaks; + std::vector> swing_ft_gain_multiplier_samples; +// drake::trajectories::PiecewisePolynomial +// swing_ft_gain_multiplier_gain_multiplier; + std::vector swing_ft_accel_gain_multiplier_breaks; + std::vector> swing_ft_accel_gain_multiplier_samples; +// std::shared_ptr> +// swing_ft_accel_gain_multiplier_gain_multiplier; + + multibody::KinematicEvaluatorSet evaluators; + + multibody::FixedJointEvaluator left_fixed_knee_spring; + multibody::FixedJointEvaluator right_fixed_knee_spring; + multibody::FixedJointEvaluator left_fixed_ankle_spring; + multibody::FixedJointEvaluator right_fixed_ankle_spring; + + std::unique_ptr swing_foot_data; + std::unique_ptr com_data; + std::unique_ptr swing_ft_traj_local_; + std::unique_ptr swing_ft_traj_global_; + + std::unique_ptr pelvis_traj_; + + std::unique_ptr pelvis_balance_traj_; + std::unique_ptr pelvis_heading_traj_; + + std::unique_ptr swing_toe_traj_left_; + std::unique_ptr swing_toe_traj_right_; + + std::unique_ptr swing_hip_yaw_traj_; + + const int state_input_port_index_ = 0; + const int radio_input_port_index_ = 1; + const int control_output_port_index_ = 0; + const int torque_output_port_index_ = 1; + const int controller_failure_port_index_ = 2; + + const std::string control_channel_name_ = "OSC_WALKING"; +}; + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/Cassie/director_scripts/cassie_operator.py b/examples/Cassie/director_scripts/cassie_operator.py index 7e0324435a..a715593f33 100644 --- a/examples/Cassie/director_scripts/cassie_operator.py +++ b/examples/Cassie/director_scripts/cassie_operator.py @@ -34,7 +34,7 @@ # Set of gains with which COM is within support polygon when we lower the hoist -joint_default = [-0.01, .01, 0, 0, 0.55, 0.55, -1.5, -1.5, -1.8, -1.8] +joint_default = [0.05, -.05, 0, 0, 0.55, 0.55, -1.5, -1.5, -1.8, -1.8] kp_default = [80, 80, 50, 50, 50, 50, 50, 50, 10, 10] kd_default = [1, 1, 1, 1, 1, 1, 2, 2, 1, 1] diff --git a/examples/Cassie/director_scripts/controller_status.py b/examples/Cassie/director_scripts/controller_status.py index 9d354b8cce..24d24d2639 100644 --- a/examples/Cassie/director_scripts/controller_status.py +++ b/examples/Cassie/director_scripts/controller_status.py @@ -15,10 +15,9 @@ def __init__(self): self._subscriber = None self._current_channel_ = "" - # self.text_box = vis.PolyDataItem('safety_info', 'safety_info', view) self.text_box = vis.TextItem('safety_info', 'safety_info', view) - self.text_box.setProperty('Font Size', 24) - self.text_box.setProperty('Position', [0, 600]) + self.text_box.setProperty('Position', [10, 600]) + self.text_box.setProperty('Font Size', 36) self.text_box.setProperty('Bold', True) self.set_enabled(True) diff --git a/examples/Cassie/director_scripts/show_time.py b/examples/Cassie/director_scripts/show_time.py deleted file mode 100644 index 060da7676c..0000000000 --- a/examples/Cassie/director_scripts/show_time.py +++ /dev/null @@ -1,84 +0,0 @@ -# Note that this script runs in the main context of drake-visulizer, -# where many modules and variables already exist in the global scope. -from director import lcmUtils -from director import applogic -import time -import dairlib.lcmt_robot_output - - -class TimeVisualizer(object): - - def __init__(self): - self._name = "Time Visualizer" - self._real_time = [] - self._msg_time = [] - self._subscriber = None - # Number of messages used to average for real time factor. - self._num_msg_for_average = 50 - - self.set_enabled(True) - - def add_subscriber(self): - if (self._subscriber is not None): - return - - if 'pd_panel_state_channel' in globals(): - channel = pd_panel_state_channel - else: - channel = "CASSIE_STATE_SIMULATION" - - self._subscriber = lcmUtils.addSubscriber( - channel, - messageClass=dairlib.lcmt_robot_output, - callback=self.handle_message) - - def remove_subscriber(self): - if (self._subscriber is None): - return - - lcmUtils.removeSubscriber(self._subscriber) - self._subscriber = None - vis.updateText('', 'text') - - def is_enabled(self): - return self._subscriber is not None - - def set_enabled(self, enable): - if enable: - self.add_subscriber() - else: - self.remove_subscriber() - - def handle_message(self, msg): - msg_time = msg.utime * 1e-6 # convert from microseconds - self._real_time.append(time.time()) - self._msg_time.append(msg_time) - - my_text = 'time: %.3f' % msg_time - - if (len(self._real_time) >= self._num_msg_for_average): - self._real_time.pop(0) - self._msg_time.pop(0) - - dt = self._msg_time[-1] - self._msg_time[0] - dt_real_time = self._real_time[-1] - self._real_time[0] - - rt_ratio = dt / dt_real_time - - my_text = my_text + ', real time factor: %.2f' % rt_ratio - - vis.updateText(my_text, 'text') - - -def init_visualizer(): - time_viz = TimeVisualizer() - - # Adds to the "Tools" menu. - applogic.MenuActionToggleHelper( - 'Tools', time_viz._name, - time_viz.is_enabled, time_viz.set_enabled) - return time_viz - - -# Creates the visualizer when this script is executed. -time_viz = init_visualizer() diff --git a/examples/Cassie/director_scripts/show_time_hardware.py b/examples/Cassie/director_scripts/show_time_hardware.py index a48b0a6984..a145ab05c5 100644 --- a/examples/Cassie/director_scripts/show_time_hardware.py +++ b/examples/Cassie/director_scripts/show_time_hardware.py @@ -4,6 +4,7 @@ from director import applogic import time import dairlib.lcmt_robot_output +import numpy as np class TimeVisualizer(object): @@ -12,9 +13,10 @@ def __init__(self): self._name = "Time Visualizer" self._real_time = [] self._msg_time = [] + self._pelvis_velocity = [] self._subscriber = None # Number of messages used to average for real time factor. - self._num_msg_for_average = 50 + self._num_msg_for_average = 20 self.set_enabled(True) @@ -25,8 +27,8 @@ def add_subscriber(self): if 'pd_panel_state_channel' in globals(): channel = pd_panel_state_channel else: - channel = "NETWORK_CASSIE_STATE_DISPATCHER" - # channel = "CASSIE_STATE_SIMULATION" + # channel = "NETWORK_CASSIE_STATE_DISPATCHER" + channel = "CASSIE_STATE_SIMULATION" self._subscriber = lcmUtils.addSubscriber( channel, @@ -53,24 +55,34 @@ def set_enabled(self, enable): def handle_message(self, msg): msg_time = msg.utime * 1e-6 # convert from microseconds pelvis_height = (msg.position)[6] # convert from microseconds + pelvis_velocity = np.linalg.norm((msg.velocity)[4:6]) # convert from microseconds + # pelvis_height = (msg.position)[2] # convert from microseconds + # pelvis_velocity = np.linalg.norm((msg.velocity)[0:2]) # convert from microseconds self._real_time.append(time.time()) self._msg_time.append(msg_time) + self._pelvis_velocity.append(pelvis_velocity) my_text = 'time: %.3f' % msg_time pelvis_height_text = 'pelvis height: %.3f' % pelvis_height + pelvis_velocity_text = 'pelvis velocity: %.3f' % np.array(self._pelvis_velocity).mean() + realtime_text = '' + if (len(self._real_time) >= self._num_msg_for_average): self._real_time.pop(0) self._msg_time.pop(0) + self._pelvis_velocity.pop(0) + dt = self._msg_time[-1] - self._msg_time[0] dt_real_time = self._real_time[-1] - self._real_time[0] rt_ratio = dt / dt_real_time + realtime_text = 'realtime rate: %.2f' % rt_ratio + # my_text = my_text + ', real time factor: %.2f' % rt_ratio - #my_text = my_text + ', real time factor: %.2f' % rt_ratio - - vis.updateText(my_text + '\n' + pelvis_height_text, 'text') + vis.updateText(my_text + '\n' + pelvis_height_text + '\n' + pelvis_velocity_text +'\n' + realtime_text, 'text') + # vis.updateText(my_text + '\n' + pelvis_velocity_text, 'text') def init_visualizer(): diff --git a/examples/Cassie/director_scripts/show_time_sim.py b/examples/Cassie/director_scripts/show_time_sim.py index 0ed188c336..bf23264afc 100644 --- a/examples/Cassie/director_scripts/show_time_sim.py +++ b/examples/Cassie/director_scripts/show_time_sim.py @@ -16,8 +16,13 @@ def __init__(self): self._subscriber = None # Number of messages used to average for real time factor. self._num_msg_for_average = 20 - + self.text_time = vis.TextItem('sim_info', 'sim_info', view) + self.text_time.setProperty('Position', [10, 400]) + self.text_time.setProperty('Font Size', 36) + self.text_time.setProperty('Bold', True) self.set_enabled(True) + gridObj.setProperty('Grid Half Width', 100) + gridObj.setProperty('Major Tick Resolution', 100) def add_subscriber(self): if (self._subscriber is not None): @@ -55,7 +60,7 @@ def handle_message(self, msg): msg_time = msg.utime * 1e-6 # convert from microseconds # Drake Sim pelvis_height = (msg.position)[6] # convert from microseconds - pelvis_velocity = np.linalg.norm((msg.velocity)[3:4]) # convert from microseconds + pelvis_velocity = np.linalg.norm((msg.velocity)[3:5]) # convert from microseconds # pelvis_height = (msg.position)[2] # convert from microseconds # pelvis_velocity = np.linalg.norm((msg.velocity)[0:2]) # convert from microseconds self._real_time.append(time.time()) @@ -66,6 +71,7 @@ def handle_message(self, msg): pelvis_height_text = 'pelvis height: %.3f' % pelvis_height pelvis_velocity_text = 'pelvis velocity: %.3f' % np.array(self._pelvis_velocity).mean() + pelvis_position_text = 'forward position: %.3f' % np.array(msg.position[4]).mean() realtime_text = '' if (len(self._real_time) >= self._num_msg_for_average): @@ -81,7 +87,9 @@ def handle_message(self, msg): realtime_text = 'realtime rate: %.2f' % rt_ratio # my_text = my_text + ', real time factor: %.2f' % rt_ratio - vis.updateText(my_text + '\n' + pelvis_height_text + '\n' + pelvis_velocity_text +'\n' + realtime_text, 'text') + # vis.updateText(my_text + '\n' + pelvis_height_text + '\n' + pelvis_velocity_text +'\n' + realtime_text, 'text') + self.text_time.setProperty('Text', my_text + '\n' + pelvis_height_text + '\n' + pelvis_velocity_text + '\n' + pelvis_position_text + '\n' + realtime_text) + # vis.updateText(my_text + '\n' + pelvis_velocity_text, 'text') diff --git a/examples/Cassie/dispatcher_robot_in.cc b/examples/Cassie/dispatcher_robot_in.cc index 3993e9dba2..c7ac0dc607 100644 --- a/examples/Cassie/dispatcher_robot_in.cc +++ b/examples/Cassie/dispatcher_robot_in.cc @@ -27,6 +27,7 @@ namespace dairlib { using drake::lcm::Subscriber; using drake::systems::Context; +using drake::systems::Diagram; using drake::systems::DiagramBuilder; using drake::systems::Simulator; using drake::systems::TriggerType; @@ -188,7 +189,8 @@ int do_main(int argc, char* argv[]) { // Finish building the diagram auto owned_diagram = builder.Build(); - owned_diagram->set_name("dispatcher_robot_in"); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name("dispatcher_robot_in"); // Channel names of the controllers std::vector input_channels = {FLAGS_control_channel_name_initial, @@ -204,7 +206,7 @@ int do_main(int argc, char* argv[]) { // Run lcm-driven simulation CassieLcmDrivenLoop - loop(&lcm_local, std::move(owned_diagram), command_receiver, + loop(&lcm_local, shared_diagram, command_receiver, input_channels, FLAGS_control_channel_name_initial, switch_channel, true, FLAGS_state_channel_name); diff --git a/examples/Cassie/find_fixed_point.cc b/examples/Cassie/find_fixed_point.cc index cd8da78398..d39a68fade 100644 --- a/examples/Cassie/find_fixed_point.cc +++ b/examples/Cassie/find_fixed_point.cc @@ -3,6 +3,7 @@ #include #include "examples/Cassie/cassie_fixed_point_solver.h" +#include "drake/common/text_logging.h" DEFINE_double(height, 1, "Fixed height,"); DEFINE_double(toe_spread, .2, "Fixed height,"); diff --git a/examples/Cassie/forward_command.py b/examples/Cassie/forward_command.py new file mode 100644 index 0000000000..3659c99045 --- /dev/null +++ b/examples/Cassie/forward_command.py @@ -0,0 +1,53 @@ +import dairlib.lcmt_radio_out +import pygame +import lcm +import numpy as np + +# colors +cassie_blue = (6, 61, 128) +white = (255, 255, 255) + +def main(): + publisher = lcm.LCM() + + screen_size = 500 + # screen = pygame.display.set_mode((screen_size, screen_size)) + + # Used to manage how fast the screen updates + clock = pygame.time.Clock() + + done = False + max_speed = 1.4 + dt = .020 + acceleration = 2.0 # m/s^2 + deceleration = 0.5 # m/s^2 + max_speed_duration = 9 + velocity_step_up = acceleration * dt + velocity_step_down = deceleration * dt + ramp_up = np.arange(0, max_speed, velocity_step_up) + stay = max_speed * np.ones(int(max_speed_duration / dt)) + ramp_down = np.flip(np.arange(0, max_speed, velocity_step_down)) + speeds = np.hstack((ramp_up, stay, ramp_down)) + ramp_duration = speeds.shape[0] * .02 + print("total duration: ", ramp_duration) + i = 0 + while not done: + # DRAWING STEP + # First, clear the screen to blue. Don't put other drawing commands + # above this, or they will be erased with this command. + # screen.fill(cassie_blue) + + # Send LCM message + radio_msg = dairlib.lcmt_radio_out() + radio_msg.channel[0] = speeds[i] + + publisher.publish("CASSIE_VIRTUAL_RADIO", radio_msg.encode()) + + # Limit to 20 frames per second + clock.tick(dt * 1000) + i += 1 + + pygame.quit() + +if __name__ == '__main__': + main() diff --git a/examples/Cassie/integration_test.pmd b/examples/Cassie/integration_test.pmd index e47817f73d..814b61f035 100644 --- a/examples/Cassie/integration_test.pmd +++ b/examples/Cassie/integration_test.pmd @@ -43,7 +43,7 @@ group "2.simulators-and-dispatchers" { host = "localhost"; } cmd "cassie_sim" { - exec = "bazel-bin/examples/Cassie/multibody_sim --init_height=0.9"; + exec = "bazel-bin/examples/Cassie/multibody_sim --init_height=0.9 --publish_rate=2000"; host = "localhost"; } cmd "cassie_mujoco_sim" { @@ -157,8 +157,8 @@ script "jumping_integration_test" { start cmd "cassie_mujoco_sim"; wait ms 10000; stop cmd "state_visualizer (dispatcher)"; - stop group "1.controllers-and-dispatchers"; - stop group "2.simulators"; + stop group "1.controllers (standalone)"; + stop group "2.simulators-and-dispatchers"; } script "mujoco_standing" { @@ -197,8 +197,8 @@ script "standing_integration_test" { start cmd "cassie_mujoco_sim"; wait ms 10000; stop cmd "state_visualizer (dispatcher)"; - stop group "1.controllers-and-dispatchers"; - stop group "2.simulators"; + stop group "1.controllers (standalone)"; + stop group "2.simulators-and-dispatchers"; } script "walking_integration_test" { @@ -232,6 +232,6 @@ script "walking_integration_test" { start cmd "cassie_mujoco_sim"; wait ms 10000; stop cmd "state_visualizer (dispatcher)"; - stop group "1.controllers-and-dispatchers"; - stop group "2.simulators"; + stop group "1.controllers (standalone)"; + stop group "2.simulators-and-dispatchers"; } diff --git a/examples/Cassie/multibody_sim.cc b/examples/Cassie/multibody_sim.cc index be30842ac4..3973c3f0ed 100644 --- a/examples/Cassie/multibody_sim.cc +++ b/examples/Cassie/multibody_sim.cc @@ -93,11 +93,11 @@ DEFINE_string(contact_solver, "SAP", } if (FLAGS_contact_solver == "SAP") { - plant.set_discrete_contact_solver( - drake::multibody::DiscreteContactSolver::kSap); + plant.set_discrete_contact_approximation( + drake::multibody::DiscreteContactApproximation::kSap); } else if (FLAGS_contact_solver == "TAMSI") { - plant.set_discrete_contact_solver( - drake::multibody::DiscreteContactSolver::kTamsi); + plant.set_discrete_contact_approximation( + drake::multibody::DiscreteContactApproximation::kTamsi); } else { std::cerr << "Unknown contact solver setting." << std::endl; } @@ -165,7 +165,7 @@ DEFINE_string(contact_solver, "SAP", state_sender->get_input_port_effort()); builder.Connect(*state_sender, *state_pub); builder.Connect( - plant.get_geometry_poses_output_port(), + plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.get_source_id().value())); builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()); @@ -180,7 +180,7 @@ DEFINE_string(contact_solver, "SAP", auto diagram = builder.Build(); diagram->set_name(("multibody_sim")); - // DrawAndSaveDiagramGraph(*diagram); + DrawAndSaveDiagramGraph(*diagram); // Create a context for this system: std::unique_ptr> diagram_context = @@ -197,9 +197,9 @@ DEFINE_string(contact_solver, "SAP", double toe_spread = FLAGS_toe_spread; // Create a plant for CassieFixedPointSolver. // Note that we cannot use the plant from the above diagram, because after the - // diagram is built, plant.get_actuation_input_port().HasValue(*context) + // diagram is built, plant.get_input_port_actuation().HasValue(*context) // throws a segfault error - drake::multibody::MultibodyPlant plant_for_solver(0.0); + drake::multibody::MultibodyPlant plant_for_solver(FLAGS_dt); AddCassieMultibody(&plant_for_solver, nullptr, FLAGS_floating_base /*floating base*/, urdf, FLAGS_spring_model, false); diff --git a/examples/Cassie/multibody_sim_w_ground_incline.cc b/examples/Cassie/multibody_sim_w_ground_incline.cc index 246a4a4a2d..6db823cc7e 100644 --- a/examples/Cassie/multibody_sim_w_ground_incline.cc +++ b/examples/Cassie/multibody_sim_w_ground_incline.cc @@ -223,7 +223,7 @@ int do_main(int argc, char* argv[]) { state_sender->get_input_port_effort()); builder.Connect(*state_sender, *state_pub); builder.Connect( - plant.get_geometry_poses_output_port(), + plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.get_source_id().value())); builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()); @@ -254,7 +254,7 @@ int do_main(int argc, char* argv[]) { double toe_spread = .15; // Create a plant for CassieFixedPointSolver. // Note that we cannot use the plant from the above diagram, because after the - // diagram is built, plant.get_actuation_input_port().HasValue(*context) + // diagram is built, plant.get_input_port_actuation().HasValue(*context) // throws a segfault error drake::multibody::MultibodyPlant plant_for_solver(0.0); AddCassieMultibody(&plant_for_solver, nullptr, diff --git a/examples/Cassie/multibody_sim_w_platform.cc b/examples/Cassie/multibody_sim_w_platform.cc index a89ab4667d..3c105721b7 100644 --- a/examples/Cassie/multibody_sim_w_platform.cc +++ b/examples/Cassie/multibody_sim_w_platform.cc @@ -5,6 +5,7 @@ #include "dairlib/lcmt_cassie_out.hpp" #include "dairlib/lcmt_robot_input.hpp" #include "dairlib/lcmt_robot_output.hpp" +#include "examples/Cassie/cassie_fixed_point_solver.h" #include "examples/Cassie/cassie_utils.h" #include "lcm/dircon_saved_trajectory.h" #include "multibody/multibody_utils.h" @@ -47,7 +48,7 @@ DEFINE_bool(floating_base, true, "Fixed or floating base model"); DEFINE_double(target_realtime_rate, 1.0, "Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details."); -DEFINE_double(dt, 8e-5, +DEFINE_double(dt, 1e-3, "The step size to use for time_stepping, ignored for continuous"); DEFINE_double(v_stiction, 1e-3, "Stiction tolernace (m/s)"); DEFINE_double(penetration_allowance, 1e-5, @@ -59,6 +60,7 @@ DEFINE_double(publish_rate, 2000, "Publish rate for simulator"); DEFINE_double(init_height, .7, "Initial starting height of the pelvis above " "ground"); +DEFINE_string(channel_u, "CASSIE_INPUT", "LCM channel to receive controller commands from"); DEFINE_double(platform_height, 0.0, "Height of the platform"); DEFINE_double(platform_x, 0.0, "x location of the platform"); DEFINE_double(start_time, 0.0, @@ -71,6 +73,10 @@ DEFINE_bool(spring_model, true, "Use a URDF with or without legs springs"); DEFINE_bool(visualize, true, "Set to true to visualize the platform"); DEFINE_double(actuator_delay, 0.0, "Duration of actuator delay. Set to 0.0 by default."); +DEFINE_bool(use_traj_state, true, + "Whether to intialize the sim from a specific state"); +DEFINE_string(contact_solver, "SAP", + "Contact solver to use. Either TAMSI or SAP."); int do_main(int argc, char* argv[]) { gflags::ParseCommandLineFlags(&argc, &argv, true); @@ -83,36 +89,39 @@ int do_main(int argc, char* argv[]) { const double time_step = FLAGS_dt; MultibodyPlant& plant = *builder.AddSystem(time_step); if (FLAGS_floating_base) { - multibody::AddFlatTerrain(&plant, &scene_graph, .8, .8); + multibody::AddFlatTerrain(&plant, &scene_graph, .6, .6); } std::string urdf = FLAGS_spring_model ? "examples/Cassie/urdf/cassie_v2.urdf" : "examples/Cassie/urdf/cassie_fixed_springs.urdf"; + if (FLAGS_contact_solver == "SAP") { + plant.set_discrete_contact_approximation( + drake::multibody::DiscreteContactApproximation::kSap); + } else if (FLAGS_contact_solver == "TAMSI") { + plant.set_discrete_contact_approximation( + drake::multibody::DiscreteContactApproximation::kTamsi); + } else { + std::cerr << "Unknown contact solver setting." << std::endl; + } + if (FLAGS_platform_height != 0) { Parser parser(&plant, &scene_graph); std::string terrain_name = FindResourceOrThrow("examples/impact_invariant_control/platform.urdf"); - parser.AddModelFromFile(terrain_name); + parser.AddModels(terrain_name); Eigen::Vector3d offset; - offset << FLAGS_platform_x, 0, FLAGS_platform_height; + offset << FLAGS_platform_x, 0, 0.25 + FLAGS_platform_height; plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), drake::math::RigidTransform(offset)); } - plant.set_penetration_allowance(FLAGS_penetration_allowance); - plant.set_stiction_tolerance(FLAGS_v_stiction); - AddCassieMultibody(&plant, &scene_graph, FLAGS_floating_base, urdf, FLAGS_spring_model, true); plant.Finalize(); - int nq = plant.num_positions(); - int nv = plant.num_velocities(); - int nx = nq + nv; - // Create maps for joints std::map pos_map = multibody::MakeNameToPositionsMap(plant); @@ -124,7 +133,7 @@ int do_main(int argc, char* argv[]) { auto lcm = builder.AddSystem(); auto input_sub = builder.AddSystem(LcmSubscriberSystem::Make( - "CASSIE_INPUT", lcm)); + FLAGS_channel_u, lcm)); auto input_receiver = builder.AddSystem(plant); auto passthrough = builder.AddSystem( input_receiver->get_output_port(0).size(), 0, @@ -168,7 +177,7 @@ int do_main(int argc, char* argv[]) { state_sender->get_input_port_effort()); builder.Connect(*state_sender, *state_pub); builder.Connect( - plant.get_geometry_poses_output_port(), + plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.get_source_id().value())); builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()); @@ -201,7 +210,38 @@ int do_main(int argc, char* argv[]) { Eigen::VectorXd x_traj_init = state_traj.value(FLAGS_start_time); - plant.SetPositionsAndVelocities(&plant_context, x_traj_init); + if (FLAGS_use_traj_state){ + if(FLAGS_platform_x < 0){ + x_traj_init(6) += FLAGS_platform_height; + } + plant.SetPositionsAndVelocities(&plant_context, x_traj_init); + }else{ + Eigen::VectorXd q_init, u_init, lambda_init; + double mu_fp = 0; + double min_normal_fp = 70; + double toe_spread = 0.1; + // Create a plant for CassieFixedPointSolver. + // Note that we cannot use the plant from the above diagram, because after the + // diagram is built, plant.get_input_port_actuation().HasValue(*context) + // throws a segfault error + drake::multibody::MultibodyPlant plant_for_solver(0.0); + AddCassieMultibody(&plant_for_solver, nullptr, + FLAGS_floating_base /*floating base*/, urdf, + FLAGS_spring_model, true); + plant_for_solver.Finalize(); + if (FLAGS_floating_base) { + CassieFixedPointSolver(plant_for_solver, FLAGS_init_height, mu_fp, + min_normal_fp, true, toe_spread, &q_init, &u_init, + &lambda_init); + } else { + CassieFixedBaseFixedPointSolver(plant_for_solver, &q_init, &u_init, + &lambda_init); + } + plant.SetPositions(&plant_context, q_init); + plant.SetVelocities(&plant_context, Eigen::VectorXd::Zero(plant.num_velocities())); + } + + diagram_context->SetTime(FLAGS_start_time); Simulator simulator(*diagram, std::move(diagram_context)); diff --git a/examples/Cassie/networking/cassie_udp_publisher.cc b/examples/Cassie/networking/cassie_udp_publisher.cc index 1b787250ef..2e96483a6a 100644 --- a/examples/Cassie/networking/cassie_udp_publisher.cc +++ b/examples/Cassie/networking/cassie_udp_publisher.cc @@ -64,12 +64,9 @@ CassieUDPPublisher::CassieUDPPublisher(const std::string& address, DRAKE_DEMAND(publish_period == 0); } if (publish_triggers.find(TriggerType::kPerStep) != publish_triggers.end()) { - this->DeclarePerStepEvent( - drake::systems::PublishEvent([this]( - const drake::systems::Context& context, - const drake::systems::PublishEvent&) { - this->PublishInputAsUDPMessage(context); - })); + this->DeclarePerStepPublishEvent( + &CassieUDPPublisher::PublishInputAsUDPMessage + ); } } @@ -95,7 +92,7 @@ std::string CassieUDPPublisher::make_name(const std::string& address, // period or publish period = 0.0 was passed to the constructor). drake::systems::EventStatus CassieUDPPublisher::PublishInputAsUDPMessage( const drake::systems::Context& context) const { - SPDLOG_TRACE(drake::log(), "Publishing UDP {} message", address_); + SPDLOG_TRACE("Publishing UDP {} message", address_); // Converts the input into message bytes. const drake::AbstractValue* const input_value = diff --git a/examples/Cassie/osc/BUILD.bazel b/examples/Cassie/osc/BUILD.bazel index a57a3d6c78..e7adc3a496 100644 --- a/examples/Cassie/osc/BUILD.bazel +++ b/examples/Cassie/osc/BUILD.bazel @@ -8,9 +8,9 @@ cc_library( "//examples/Cassie/osc:heading_traj_generator", "//examples/Cassie/osc:high_level_command", "//examples/Cassie/osc:hip_yaw_traj_gen", + "//examples/Cassie/osc:osc_standing_gains", "//examples/Cassie/osc:osc_walking_gains", "//examples/Cassie/osc:osc_walking_gains_alip", - "//examples/Cassie/osc:osc_standing_gains", "//examples/Cassie/osc:standing_com_traj", "//examples/Cassie/osc:standing_pelvis_orientation_traj", "//examples/Cassie/osc:swing_toe_traj", @@ -76,9 +76,9 @@ cc_library( srcs = ["standing_com_traj.cc"], hdrs = ["standing_com_traj.h"], deps = [ + "//common", "//lcmtypes:lcmt_robot", "//multibody:utils", - "//common:common", "//systems/controllers:control_utils", "//systems/framework:vector", "@drake//:drake_shared_library", @@ -90,10 +90,10 @@ cc_library( srcs = ["standing_pelvis_orientation_traj.cc"], hdrs = ["standing_pelvis_orientation_traj.h"], deps = [ + "//common", "//lcmtypes:lcmt_robot", "//systems/controllers:control_utils", "//systems/framework:vector", - "//common:common", "@drake//:drake_shared_library", ], ) diff --git a/examples/Cassie/osc/heading_traj_generator.cc b/examples/Cassie/osc/heading_traj_generator.cc index 30bd5af970..cb0be9afd1 100644 --- a/examples/Cassie/osc/heading_traj_generator.cc +++ b/examples/Cassie/osc/heading_traj_generator.cc @@ -3,6 +3,7 @@ #include #include +#include #include "multibody/multibody_utils.h" @@ -21,6 +22,7 @@ using drake::systems::Context; using drake::systems::LeafSystem; using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; namespace dairlib { namespace cassie { @@ -43,8 +45,8 @@ HeadingTrajGenerator::HeadingTrajGenerator( this->DeclareVectorInputPort("pelvis_yaw", BasicVector(1)) .get_index(); // Provide an instance to allocate the memory first (for the output) - PiecewisePolynomial pp(VectorXd(0)); - drake::trajectories::Trajectory& traj_inst = pp; + PiecewiseQuaternionSlerp empty_slerp_traj; + drake::trajectories::Trajectory& traj_inst = empty_slerp_traj; this->DeclareAbstractOutputPort("pelvis_quat", traj_inst, &HeadingTrajGenerator::CalcHeadingTraj); } @@ -64,33 +66,13 @@ void HeadingTrajGenerator::CalcHeadingTraj( multibody::SetPositionsIfNew(plant_, q, context_); - // Get approximated heading angle of pelvis - Vector3d pelvis_heading_vec = - plant_.EvalBodyPoseInWorld(*context_, pelvis_).rotation().col(0); - double approx_pelvis_yaw_i = - atan2(pelvis_heading_vec(1), pelvis_heading_vec(0)); - - // Get quaternion from body rotation matrix instead of state directly, becasue - // this is how osc tracking data get the quaternion. Quaterniond quat( plant_.EvalBodyPoseInWorld(*context_, pelvis_).rotation().matrix()); quat.normalize(); - // Construct the PiecewisePolynomial. - /// Given yaw position p_i and velocity v_i, we want to generate affine - /// functions, p_i + v_i*t, for the desired trajectory. We use - /// FirstOrderHold() to approximately generate the function, so we need to - /// have the endpoint of the trajectory. We generate the endpoint by - /// looking ahead what the position is in dt second with fixed velocity v_i. - /// Note that we construct trajectories in R^4 (quaternion space), so we need - /// to transform the yaw trajectory into quaternion representation. - /// The value of dt changes the trajectory time horizon. As long as it's - /// larger than the recompute time, the value doesn't affect the control - /// outcome. + double dt = 0.1; double des_delta_yaw = des_yaw_vel(0) * dt; - // We set pitch and roll = 0, because we also use this traj for balance in - // some controller Eigen::Vector4d pelvis_rotation_i; pelvis_rotation_i << quat.w(), 0, 0, quat.z(); pelvis_rotation_i.normalize(); @@ -100,19 +82,15 @@ void HeadingTrajGenerator::CalcHeadingTraj( Quaterniond relative_quat(cos(des_delta_yaw / 2), 0, 0, sin(des_delta_yaw / 2)); Quaterniond final_quat = relative_quat * init_quat; - Eigen::Vector4d pelvis_rotation_f; - pelvis_rotation_f << final_quat.w(), final_quat.vec(); const std::vector breaks = {context.get_time(), context.get_time() + dt}; - std::vector knots(breaks.size(), MatrixXd::Zero(4, 1)); - knots[0] = pelvis_rotation_i; - knots[1] = pelvis_rotation_f; - const auto pp = PiecewisePolynomial::FirstOrderHold(breaks, knots); + const auto pp = drake::trajectories::PiecewiseQuaternionSlerp( + breaks, {quat, final_quat}); // Assign traj auto* pp_traj = - (PiecewisePolynomial*)dynamic_cast*>( + (PiecewiseQuaternionSlerp*)dynamic_cast*>( traj); *pp_traj = pp; } diff --git a/examples/Cassie/osc/heading_traj_generator.h b/examples/Cassie/osc/heading_traj_generator.h index b51ce32b78..9ddd4be152 100644 --- a/examples/Cassie/osc/heading_traj_generator.h +++ b/examples/Cassie/osc/heading_traj_generator.h @@ -41,7 +41,7 @@ class HeadingTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const drake::multibody::Body& pelvis_; int state_port_; diff --git a/examples/Cassie/osc/high_level_command.h b/examples/Cassie/osc/high_level_command.h index 5912d5c25d..9bc3c2346a 100644 --- a/examples/Cassie/osc/high_level_command.h +++ b/examples/Cassie/osc/high_level_command.h @@ -102,7 +102,7 @@ class HighLevelCommand : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const drake::multibody::Body& pelvis_; bool use_radio_command_; diff --git a/examples/Cassie/osc/osc_standing_gains_sim.yaml b/examples/Cassie/osc/osc_standing_gains_sim.yaml new file mode 100644 index 0000000000..3eecc8a92c --- /dev/null +++ b/examples/Cassie/osc/osc_standing_gains_sim.yaml @@ -0,0 +1,66 @@ +# Set xy PD gains so they do not effect passive LIPM dynamics at capture +# point, when x = sqrt(l/g) * xdot +# Passive dynamics: xddot = g/l * x +# +# -Kp * x - Kd * xdot = +# -Kp * x + Kd * sqrt(g/l) * x = g/l * x +# Kp = sqrt(g/l) * Kd - g/l +controller_frequency: 1000 + +w_input: 0.000001 +w_accel: 0.000001 +w_lambda: 0.01 +#W_accel: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, +# 1, 1, 1, 1, 0.01, 0.001, +# 1, 1, 1, 1, 0.01, 0.001 ] +W_accel: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001 ] +W_input_reg: [ 1, 0.9, 0.5, 0.3, 1, + 1, 0.9, 0.5, 0.3, 1 ] +W_lambda_c_reg: [ 1, 0.001, 0.01, + 1, 0.001, 0.01, + 1, 0.001, 0.01, + 1, 0.001, 0.01 ] +#W_lambda_h_reg: [ 0.02, 0.02 ] +W_lambda_h_reg: [ 0.001, 0.001, 0.001, + 0.001, 0.002, 0.002 ] +w_soft_constraint: 1000 +w_input_reg: 0.00001 +impact_threshold: 0.00 +center_of_mass_filter_tau: 0.00 +rot_filter_tau: 0.000 +center_of_mass_command_filter_alpha: 1.0 +orientation_command_filter_alpha: 1.0 +impact_tau: 0.00 +mu: 0.6 +weight_scaling: 1.0 +HipYawKp: 100 +HipYawKd: 5 +HipYawW: 2.5 +PelvisW: + [ 5, 0, 0, + 0, 2, 0, + 0, 0, 2 ] +PelvisKp: + [ 75, 0, 0, + 0, 50, 0, + 0, 0, 75 ] +PelvisKd: + [ 10, 0, 0, + 0, 5, 0, + 0, 0, 5] +PelvisRotW: + [ 5, 0, 0, + 0, 5, 0, + 0, 0, 10 ] +PelvisRotKp: + [150., 0, 0, + 0, 200., 0, + 0, 0, 50 ] +PelvisRotKd: + [ 10, 0, 0, + 0, 10, 0, + 0, 0, 5 ] + + diff --git a/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml b/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml index f82b9ba31c..d8a4e1eabe 100644 --- a/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml +++ b/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml @@ -2,7 +2,6 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 500 - linsys_solver: 0 verbose: 0 warm_start: 1 polish: 1 @@ -11,6 +10,7 @@ int_options: polish_refine_iter: 3 scaling: 10 adaptive_rho: 1 + adaptive_rho_interval: 0 double_options: rho: 0.0001 @@ -21,7 +21,6 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0.1 diff --git a/examples/Cassie/osc/standing_com_traj.cc b/examples/Cassie/osc/standing_com_traj.cc index 5e334c8112..e561a9d2a4 100644 --- a/examples/Cassie/osc/standing_com_traj.cc +++ b/examples/Cassie/osc/standing_com_traj.cc @@ -34,23 +34,22 @@ StandingComTraj::StandingComTraj( world_(plant_.world_frame()), feet_contact_points_(feet_contact_points), height_(height), - set_target_height_by_radio_(set_target_height_by_radio){ + set_target_height_by_radio_(set_target_height_by_radio) { + target_pos_filter_ = std::make_unique(1.0, 3); // Input/Output Setup - state_port_ = - this->DeclareVectorInputPort("x, u, t", - OutputVector(plant.num_positions(), + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(plant.num_positions(), plant.num_velocities(), plant.num_actuators())) - .get_index(); + .get_index(); target_height_port_ = this->DeclareAbstractInputPort( "lcmt_target_standing_height", drake::Value{}) .get_index(); - radio_port_ = - this->DeclareAbstractInputPort("radio_out", - drake::Value{}) - .get_index(); + radio_port_ = this->DeclareAbstractInputPort( + "radio_out", drake::Value{}) + .get_index(); // Provide an instance to allocate the memory first (for the output) PiecewisePolynomial pp(VectorXd(0)); drake::trajectories::Trajectory& traj_inst = pp; @@ -67,17 +66,20 @@ void StandingComTraj::CalcDesiredTraj( const auto& radio_out = this->EvalInputValue(context, radio_port_); - double target_height = height_; // Get target height from radio or lcm if (set_target_height_by_radio_) { - target_height = kTargetHeightMean + kTargetHeightScale * radio_out->channel[6]; + target_height = + kTargetHeightMean + kTargetHeightScale * radio_out->channel[6]; } else { if (this->EvalInputValue( - context, target_height_port_)->timestamp > 1e-3) { - target_height = this->EvalInputValue( - context, target_height_port_)->target_height; + context, target_height_port_) + ->timestamp > 1e-3) { + target_height = + this->EvalInputValue( + context, target_height_port_) + ->target_height; } } @@ -88,46 +90,69 @@ void StandingComTraj::CalcDesiredTraj( target_height = std::clamp(target_height, kMinHeight, kMaxHeight); double x_offset = kCoMXScale * radio_out->channel[4]; double y_offset = kCoMYScale * radio_out->channel[5]; + Vector3d target_pos; + target_pos << x_offset, y_offset, target_height; VectorXd q = robot_output->GetPositions(); multibody::SetPositionsIfNew(plant_, q, context_); // Get center of left/right feet contact points positions - Vector3d contact_pos_sum = Vector3d::Zero(); - Vector3d position; - MatrixXd J(3, plant_.num_velocities()); - for (const auto& point_and_frame : feet_contact_points_) { - plant_.CalcPointsPositions(*context_, point_and_frame.second, - point_and_frame.first, world_, &position); - contact_pos_sum += position; - } - Vector3d feet_center_pos = contact_pos_sum / 4; + // Vector3d contact_pos_sum = Vector3d::Zero(); + Vector3d left_toe_position; + Vector3d left_heel_position; + Vector3d right_toe_position; + Vector3d right_heel_position; + // MatrixXd J(3, plant_.num_velocities()); + // for (const auto& point_and_frame : feet_contact_points_) { + // for (const auto& point_and_frame : feet_contact_points_) { + plant_.CalcPointsPositions(*context_, feet_contact_points_[0].second, + feet_contact_points_[0].first, world_, + &left_toe_position); + plant_.CalcPointsPositions(*context_, feet_contact_points_[1].second, + feet_contact_points_[1].first, world_, + &left_heel_position); + plant_.CalcPointsPositions(*context_, feet_contact_points_[2].second, + feet_contact_points_[2].first, world_, + &right_toe_position); + plant_.CalcPointsPositions(*context_, feet_contact_points_[3].second, + feet_contact_points_[3].first, world_, + &right_heel_position); + Vector3d contact_pos_sum = (left_toe_position + left_heel_position + + right_toe_position + right_heel_position) / + 4; + // } + double mid_point_x = (left_toe_position[0] + right_toe_position[0] + + right_heel_position[0] - 3 * left_heel_position[0]) / + 6; + double mid_point_y = ((left_toe_position[1] - right_toe_position[1]) + + (left_heel_position[1] - right_heel_position[1])) / + 4; + // Vector3d feet_center_pos = contact_pos_sum / 4; // Testing -- filtering feet_center_pos - if (filtered_feet_center_pos_.norm() == 0) { - // Initialize - filtered_feet_center_pos_ = feet_center_pos; - } - if (robot_output->get_timestamp() != last_timestamp_) { - double dt = robot_output->get_timestamp() - last_timestamp_; - last_timestamp_ = robot_output->get_timestamp(); - double alpha = - 2 * M_PI * dt * cutoff_freq_ / (2 * M_PI * dt * cutoff_freq_ + 1); - filtered_feet_center_pos_ = - alpha * feet_center_pos + (1 - alpha) * filtered_feet_center_pos_; - } - feet_center_pos = filtered_feet_center_pos_; + + // if (robot_output->get_timestamp() != last_timestamp_) { + // double dt = robot_output->get_timestamp() - last_timestamp_; + // last_timestamp_ = robot_output->get_timestamp(); + // double alpha = + // 2 * M_PI * dt * cutoff_freq_ / (2 * M_PI * dt * cutoff_freq_ + 1); + // filtered_feet_center_pos_ = + // alpha * feet_center_pos + (1 - alpha) * filtered_feet_center_pos_; + // } + // feet_center_pos = filtered_feet_center_pos_; // Desired com pos - Vector3d desired_com_pos(feet_center_pos(0) + x_offset, - feet_center_pos(1) + y_offset, - feet_center_pos(2) + target_height); + Vector3d desired_com_pos_offset; + desired_com_pos_offset << mid_point_x - 0.02, -mid_point_y, 0; + Vector3d desired_com_pos = desired_com_pos_offset + target_pos; + + target_pos_filter_->Update(desired_com_pos); // Assign traj PiecewisePolynomial* pp_traj = (PiecewisePolynomial*)dynamic_cast*>( traj); - *pp_traj = PiecewisePolynomial(desired_com_pos); + *pp_traj = PiecewisePolynomial(target_pos_filter_->Value()); } } // namespace osc diff --git a/examples/Cassie/osc/standing_com_traj.h b/examples/Cassie/osc/standing_com_traj.h index 4abbae4e18..f49e50c5a5 100644 --- a/examples/Cassie/osc/standing_com_traj.h +++ b/examples/Cassie/osc/standing_com_traj.h @@ -7,6 +7,7 @@ #include "drake/common/trajectories/piecewise_polynomial.h" #include "drake/multibody/parsing/parser.h" #include "drake/systems/framework/leaf_system.h" +#include "common/discrete_time_filter.h" namespace dairlib { namespace cassie { @@ -47,13 +48,17 @@ class StandingComTraj : public drake::systems::LeafSystem { return this->get_input_port(radio_port_); } + void SetCommandFilter(double alpha){ + target_pos_filter_->UpdateParameters(alpha); + } + private: void CalcDesiredTraj(const drake::systems::Context& context, drake::trajectories::Trajectory* traj) const; const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; int state_port_; int target_height_port_; @@ -69,8 +74,11 @@ class StandingComTraj : public drake::systems::LeafSystem { // Testing -- filtering for center of support polygon - double cutoff_freq_ = 0.05; // in Hz. - mutable Eigen::Vector3d filtered_feet_center_pos_ = Eigen::Vector3d::Zero(); + double cutoff_freq_ = 0.5; // in Hz. +// mutable Eigen::Vector3d filtered_feet_center_pos_ = Eigen::Vector3d::Zero(); +// mutable Eigen::Vector3d filtered_target_pos_ = Eigen::Vector3d::Zero(); + + std::unique_ptr target_pos_filter_; mutable double last_timestamp_ = 0; }; diff --git a/examples/Cassie/osc/standing_pelvis_orientation_traj.cc b/examples/Cassie/osc/standing_pelvis_orientation_traj.cc index 0717cb49fc..77cf8fc498 100644 --- a/examples/Cassie/osc/standing_pelvis_orientation_traj.cc +++ b/examples/Cassie/osc/standing_pelvis_orientation_traj.cc @@ -3,12 +3,14 @@ #include #include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" #include "drake/common/trajectories/trajectory.h" #include "drake/math/wrap_to.h" using dairlib::systems::OutputVector; using drake::systems::BasicVector; using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; using drake::trajectories::Trajectory; using Eigen::MatrixXd; using Eigen::Vector3d; @@ -28,19 +30,20 @@ StandingPelvisOrientationTraj::StandingPelvisOrientationTraj( context_(context), world_(plant_.world_frame()), feet_contact_points_(feet_contact_points) { + target_orientation_filter_ = + std::make_unique(1.0, 3); + // Input/Output setup - state_port_ = - this->DeclareVectorInputPort("x, u, t", - OutputVector(plant.num_positions(), + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(plant.num_positions(), plant.num_velocities(), plant.num_actuators())) - .get_index(); - radio_port_ = - this->DeclareAbstractInputPort("radio_out", - drake::Value{}) - .get_index(); - PiecewisePolynomial empty_pp_traj(Eigen::VectorXd(0)); - Trajectory& traj_inst = empty_pp_traj; + .get_index(); + radio_port_ = this->DeclareAbstractInputPort( + "radio_out", drake::Value{}) + .get_index(); + PiecewiseQuaternionSlerp empty_slerp_traj; + Trajectory& traj_inst = empty_slerp_traj; this->set_name(traj_name); this->DeclareAbstractOutputPort(traj_name, traj_inst, &StandingPelvisOrientationTraj::CalcTraj); @@ -57,7 +60,7 @@ void StandingPelvisOrientationTraj::CalcTraj( VectorXd q = robot_output->GetPositions(); plant_.SetPositions(context_, q); auto* casted_traj = - (PiecewisePolynomial*)dynamic_cast*>( + (PiecewiseQuaternionSlerp*)dynamic_cast*>( traj); Vector3d pt_0; Vector3d pt_1; @@ -77,17 +80,18 @@ void StandingPelvisOrientationTraj::CalcTraj( VectorXd r_foot = pt_2 - pt_3; // l_foot_proj = l_foot.dot() Vector3d rpy; - rpy << radio_out->channel[1], - radio_out->channel[2], + rpy << radio_out->channel[1], radio_out->channel[2], drake::math::wrap_to( 0.5 * (atan2(l_foot(1), l_foot(0)) + atan2(r_foot(1), r_foot(0))), - -M_PI, M_PI) + + -M_PI/4, M_PI/4) + radio_out->channel[3]; - auto rot_mat = - drake::math::RotationMatrix(drake::math::RollPitchYaw(rpy)); - - *casted_traj = PiecewisePolynomial(rot_mat.ToQuaternionAsVector4()); + target_orientation_filter_->Update(rpy); + auto rot_mat = drake::math::RotationMatrix(drake::math::RollPitchYaw( + static_cast(target_orientation_filter_->Value()))); + const std::vector breaks = {0, 1}; + *casted_traj = drake::trajectories::PiecewiseQuaternionSlerp( + breaks, {rot_mat.ToQuaternion(), rot_mat.ToQuaternion()}); } } // namespace dairlib::cassie::osc diff --git a/examples/Cassie/osc/standing_pelvis_orientation_traj.h b/examples/Cassie/osc/standing_pelvis_orientation_traj.h index abc12ac37d..d52470406d 100644 --- a/examples/Cassie/osc/standing_pelvis_orientation_traj.h +++ b/examples/Cassie/osc/standing_pelvis_orientation_traj.h @@ -7,6 +7,7 @@ #include "drake/common/trajectories/piecewise_polynomial.h" #include "drake/systems/framework/leaf_system.h" +#include "common/discrete_time_filter.h" namespace dairlib::cassie::osc { @@ -26,6 +27,9 @@ class StandingPelvisOrientationTraj const drake::systems::InputPort& get_input_port_radio() const { return this->get_input_port(radio_port_); } + void SetCommandFilter(double alpha){ + target_orientation_filter_->UpdateParameters(alpha); + } private: void CalcTraj(const drake::systems::Context& context, @@ -33,7 +37,7 @@ class StandingPelvisOrientationTraj const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; // A list of pairs of contact body frame and contact point const std::vector< @@ -41,6 +45,9 @@ class StandingPelvisOrientationTraj feet_contact_points_; int state_port_; int radio_port_; + + std::unique_ptr target_orientation_filter_; + }; } // namespace dairlib::cassie::osc diff --git a/examples/Cassie/osc/swing_toe_traj_generator.h b/examples/Cassie/osc/swing_toe_traj_generator.h index 245ac574fe..d23fb1efdb 100644 --- a/examples/Cassie/osc/swing_toe_traj_generator.h +++ b/examples/Cassie/osc/swing_toe_traj_generator.h @@ -31,7 +31,7 @@ class SwingToeTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; int swing_toe_idx_; // A list of pairs of contact body frame and contact point diff --git a/examples/Cassie/osc/walking_speed_control.h b/examples/Cassie/osc/walking_speed_control.h index 55802195a3..afd2e88849 100644 --- a/examples/Cassie/osc/walking_speed_control.h +++ b/examples/Cassie/osc/walking_speed_control.h @@ -71,7 +71,7 @@ class WalkingSpeedControl : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const drake::multibody::Body& pelvis_; double swing_phase_duration_; diff --git a/examples/Cassie/osc_jump/convert_traj_for_controller.cc b/examples/Cassie/osc_jump/convert_traj_for_controller.cc index 45d7851fd0..30b9f5829f 100644 --- a/examples/Cassie/osc_jump/convert_traj_for_controller.cc +++ b/examples/Cassie/osc_jump/convert_traj_for_controller.cc @@ -47,7 +47,7 @@ int DoMain() { MultibodyPlant plant(1e-5); // non-zero timestep to avoid continuous // model warnings Parser parser(&plant, &scene_graph); - parser.AddModelFromFile( + parser.AddModels( FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf")); plant.mutable_gravity_field().set_gravity_vector(-9.81 * Eigen::Vector3d::UnitZ()); diff --git a/examples/Cassie/osc_jump/flight_foot_traj_generator.cc b/examples/Cassie/osc_jump/flight_foot_traj_generator.cc index 62ee294b3c..980d36a625 100644 --- a/examples/Cassie/osc_jump/flight_foot_traj_generator.cc +++ b/examples/Cassie/osc_jump/flight_foot_traj_generator.cc @@ -10,7 +10,7 @@ using Eigen::VectorXd; using std::string; using dairlib::systems::OutputVector; -using drake::multibody::BodyFrame; +using drake::multibody::RigidBodyFrame; using drake::multibody::Frame; using drake::multibody::JacobianWrtVariable; using drake::multibody::MultibodyPlant; diff --git a/examples/Cassie/osc_jump/osc_jumping_gains_box.yaml b/examples/Cassie/osc_jump/osc_jumping_gains_box.yaml new file mode 100644 index 0000000000..bb9c5a8ed2 --- /dev/null +++ b/examples/Cassie/osc_jump/osc_jumping_gains_box.yaml @@ -0,0 +1,78 @@ +controller_frequency: 1000 +w_input: 0.000001 +w_lambda: 0.1 +w_accel: 0.0001 +# roll, yaw, pitch, knee, knee_spring, ankle_joint, ankle_spring, toe +W_accel: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001] +W_input_reg: [0.5, 0.9, 0.5, 0.1, 0.1, + 0.5, 0.9, 0.5, 0.1, 0.1] +# left toe, left heel, right toe, right heel: x y z +W_lambda_c_reg: [1, 0.001, 0.01, + 1, 0.001, 0.01, + 1, 0.1, 0.01, + 1, 0.1, 0.01] +# left_knee_spring, right_knee_spring, left_ankle_spring, right_ankle_spring, left_loop, right_loop +W_lambda_h_reg: [0.01, 0.01, 0.01, + 0.01, 0.02, 0.02] +w_input_reg: 0.01 +w_soft_constraint: 1000 +crouch_x_offset: 0.04 +land_x_offset: 0.03 +relative_feet: true + +mu: 0.6 + +w_swing_toe: 0.01 +swing_toe_kp: 1500 +swing_toe_kd: 10 + +w_hip_yaw: 2.5 +hip_yaw_kp: 100 +hip_yaw_kd: 5 + +min_pelvis_acc: -25 +max_pelvis_acc: 100 +impact_threshold: 0.050 +impact_tau: 0.005 +landing_delay: 0.040 +#landing_delay: -0.0 + +CoMW: + [20, 0, 0, + 0, 2, 0, + 0, 0, 20] +CoMKp: + [40, 0, 0, + 0, 50, 0, + 0, 0, 40] +CoMKd: + [ 7.5, 0, 0, + 0, 5, 0, + 0, 0, 5] +PelvisRotW: + [10, 0, 0, + 0, 5, 0, + 0, 0, 1] +PelvisRotKp: + [150., 0, 0, + 0, 200., 0, + 0, 0, 0.] +PelvisRotKd: + [10, 0, 0, + 0, 10, 0, + 0, 0, 5.] +FlightFootW: + [10, 0, 0, + 0, 100, 0, + 0, 0, 10] +FlightFootKp: + [125, 0, 0, + 0, 50, 0, + 0, 0, 150] +FlightFootKd: + [2.5, 0, 0, + 0, 2.5, 0, + 0, 0, 0] + diff --git a/examples/Cassie/osc_jump/osc_jumping_gains_down.yaml b/examples/Cassie/osc_jump/osc_jumping_gains_down.yaml new file mode 100644 index 0000000000..333ece9911 --- /dev/null +++ b/examples/Cassie/osc_jump/osc_jumping_gains_down.yaml @@ -0,0 +1,78 @@ +controller_frequency: 1000 +w_input: 0.000001 +w_lambda: 0.1 +w_accel: 0.0001 +# roll, yaw, pitch, knee, knee_spring, ankle_joint, ankle_spring, toe +W_accel: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001] +W_input_reg: [0.5, 0.9, 0.5, 0.1, 0.1, + 0.5, 0.9, 0.5, 0.1, 0.1] +# left toe, left heel, right toe, right heel: x y z +W_lambda_c_reg: [1, 0.001, 0.01, + 1, 0.001, 0.01, + 1, 0.1, 0.01, + 1, 0.1, 0.01] +# left_knee_spring, right_knee_spring, left_ankle_spring, right_ankle_spring, left_loop, right_loop +W_lambda_h_reg: [0.01, 0.01, 0.01, + 0.01, 0.02, 0.02] +w_input_reg: 0.01 +w_soft_constraint: 1000 +crouch_x_offset: -0.01 +land_x_offset: 0.00 +relative_feet: true + +mu: 0.6 + +w_swing_toe: 0.01 +swing_toe_kp: 2500 +swing_toe_kd: 10 + +w_hip_yaw: 100 +hip_yaw_kp: 100 +hip_yaw_kd: 10 + +min_pelvis_acc: -7 +max_pelvis_acc: 10000 +impact_threshold: 0.050 +impact_tau: 0.005 +landing_delay: 0.070 +#landing_delay: 0. + +CoMW: + [20, 0, 0, + 0, 2, 0, + 0, 0, 20] +CoMKp: + [100, 0, 0, + 0, 50, 0, + 0, 0, 40] +CoMKd: + [ 15, 0, 0, + 0, 5, 0, + 0, 0, 5] +PelvisRotW: + [10, 0, 0, + 0, 5, 0, + 0, 0, 1] +PelvisRotKp: + [500., 0, 0, + 0, 750., 0, + 0, 0, 0.] +PelvisRotKd: + [10, 0, 0, + 0, 10, 0, + 0, 0, 5.] +FlightFootW: + [10, 0, 0, + 0, 100, 0, + 0, 0, 10] +FlightFootKp: + [125, 0, 0, + 0, 50, 0, + 0, 0, 150] +FlightFootKd: + [2.5, 0, 0, + 0, 2.5, 0, + 0, 0, 0] + diff --git a/examples/Cassie/osc_jump/osc_jumping_gains_long.yaml b/examples/Cassie/osc_jump/osc_jumping_gains_long.yaml new file mode 100644 index 0000000000..85ab3baeef --- /dev/null +++ b/examples/Cassie/osc_jump/osc_jumping_gains_long.yaml @@ -0,0 +1,77 @@ +controller_frequency: 1000 +w_input: 0.000001 +w_lambda: 0.1 +w_accel: 0.0001 +# roll, yaw, pitch, knee, knee_spring, ankle_joint, ankle_spring, toe +W_accel: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001] +W_input_reg: [0.5, 0.9, 0.5, 0.1, 0.1, + 0.5, 0.9, 0.5, 0.1, 0.1] +# left toe, left heel, right toe, right heel: x y z +W_lambda_c_reg: [1, 0.001, 0.01, + 5, 0.001, 0.01, + 1, 0.1, 0.01, + 5, 0.1, 0.01] +# left_knee_spring, right_knee_spring, left_ankle_spring, right_ankle_spring, left_loop, right_loop +W_lambda_h_reg: [0.01, 0.01, 0.01, + 0.01, 0.02, 0.02] +w_input_reg: 0.01 +w_soft_constraint: 1000 +crouch_x_offset: -0.033 +land_x_offset: 0.00 +relative_feet: true + +mu: 0.6 + +w_swing_toe: 1 +swing_toe_kp: 2500 +swing_toe_kd: 10 + +w_hip_yaw: 2.5 +hip_yaw_kp: 100 +hip_yaw_kd: 5 + +min_pelvis_acc: -8 +max_pelvis_acc: 10000 +impact_threshold: 0.050 +impact_tau: 0.005 +landing_delay: 0.03 +#landing_delay: 0. + +CoMW: + [20, 0, 0, + 0, 2, 0, + 0, 0, 20] +CoMKp: + [40, 0, 0, + 0, 50, 0, + 0, 0, 40] +CoMKd: + [ 7.5, 0, 0, + 0, 5, 0, + 0, 0, 5] +PelvisRotW: + [10, 0, 0, + 0, 5, 0, + 0, 0, 1] +PelvisRotKp: + [200., 0, 0, + 0, 500., 0, + 0, 0, 0.] +PelvisRotKd: + [20, 0, 0, + 0, 25, 0, + 0, 0, 5.] +FlightFootW: + [10, 0, 0, + 0, 100, 0, + 0, 0, 10] +FlightFootKp: + [125, 0, 0, + 0, 50, 0, + 0, 0, 150] +FlightFootKd: + [2.5, 0, 0, + 0, 2.5, 0, + 0, 0, 0] \ No newline at end of file diff --git a/examples/Cassie/osc_jump/pelvis_trans_traj_generator.cc b/examples/Cassie/osc_jump/pelvis_trans_traj_generator.cc index 0d488cdf54..fe2c1920d3 100644 --- a/examples/Cassie/osc_jump/pelvis_trans_traj_generator.cc +++ b/examples/Cassie/osc_jump/pelvis_trans_traj_generator.cc @@ -61,7 +61,7 @@ PelvisTransTrajGenerator::PelvisTransTrajGenerator( init_fsm << init_fsm_state; prev_fsm_idx_ = this->DeclareDiscreteState(init_fsm); - DeclarePerStepDiscreteUpdateEvent( + DeclareForcedDiscreteUpdateEvent( &PelvisTransTrajGenerator::DiscreteVariableUpdate); crouch_traj_.shiftRight(time_offset_); } diff --git a/examples/Cassie/osc_jump/pelvis_trans_traj_generator.h b/examples/Cassie/osc_jump/pelvis_trans_traj_generator.h index ddbf25ce8b..4508ff6a12 100644 --- a/examples/Cassie/osc_jump/pelvis_trans_traj_generator.h +++ b/examples/Cassie/osc_jump/pelvis_trans_traj_generator.h @@ -51,7 +51,7 @@ class PelvisTransTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; drake::systems::DiscreteStateIndex prev_fsm_idx_; drake::systems::DiscreteStateIndex pelvis_x_offset_idx_; diff --git a/examples/Cassie/osc_jump/toe_angle_traj_generator.h b/examples/Cassie/osc_jump/toe_angle_traj_generator.h index ab769aa420..ffa390a2e1 100644 --- a/examples/Cassie/osc_jump/toe_angle_traj_generator.h +++ b/examples/Cassie/osc_jump/toe_angle_traj_generator.h @@ -39,7 +39,7 @@ class FlightToeAngleTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; drake::trajectories::PiecewisePolynomial toe_traj_; bool use_traj_; diff --git a/examples/Cassie/osc_run/BUILD.bazel b/examples/Cassie/osc_run/BUILD.bazel index 53ffab7875..3535f79fe1 100644 --- a/examples/Cassie/osc_run/BUILD.bazel +++ b/examples/Cassie/osc_run/BUILD.bazel @@ -45,24 +45,23 @@ cc_library( srcs = ["pelvis_trans_traj_generator.cc"], hdrs = ["pelvis_trans_traj_generator.h"], deps = [ + "//examples/Cassie/contact_scheduler:all", "//multibody:utils", "//systems/controllers/osc:osc_gains", - "//examples/Cassie/contact_scheduler:all", "//systems/primitives", "@drake//:drake_shared_library", ], ) - cc_library( name = "foot_traj_generator", srcs = ["foot_traj_generator.cc"], hdrs = ["foot_traj_generator.h"], deps = [ - "//multibody:utils", + "//examples/Cassie/contact_scheduler:all", "//lcmtypes:lcmt_robot", + "//multibody:utils", "//systems/controllers/osc:osc_gains", - "//examples/Cassie/contact_scheduler:all", "//systems/primitives", "@drake//:drake_shared_library", ], diff --git a/examples/Cassie/osc_run/convert_traj_for_controller.cc b/examples/Cassie/osc_run/convert_traj_for_controller.cc new file mode 100644 index 0000000000..0e65ff54e9 --- /dev/null +++ b/examples/Cassie/osc_run/convert_traj_for_controller.cc @@ -0,0 +1,394 @@ +#include +#include +#include + +#include "examples/Cassie/cassie_utils.h" +#include "lcm/dircon_saved_trajectory.h" +#include "lcm/lcm_trajectory.h" + +#include "drake/multibody/plant/multibody_plant.h" + +using drake::geometry::SceneGraph; +using drake::multibody::JacobianWrtVariable; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Context; +using drake::trajectories::PiecewisePolynomial; +using Eigen::Matrix3Xd; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +DEFINE_string(trajectory_name, "", + "File name where the optimal trajectory is stored."); +DEFINE_string(folder_path, "", + "Folder path for where the trajectory names are stored"); +DEFINE_bool(mirror_traj, false, + "Whether or not to extend the trajectory by mirroring"); + +namespace dairlib { + +/// This program pre-pelvisputes the output trajectories (center of mass, pelvis +/// orientation, feet trajectories) for the OSC controller. +/// + +int DoMain() { + // Drake system initialization + drake::systems::DiagramBuilder builder; + SceneGraph& scene_graph = *builder.AddSystem(); + scene_graph.set_name("scene_graph"); + MultibodyPlant plant(1e-5); // non-zero timestep to avoid continuous + // model warnings + Parser parser(&plant, &scene_graph); + parser.AddModels( + FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf")); + plant.mutable_gravity_field().set_gravity_vector(-9.81 * + Eigen::Vector3d::UnitZ()); + plant.Finalize(); + + std::unique_ptr> context = plant.CreateDefaultContext(); + + int nq = plant.num_positions(); + int nv = plant.num_velocities(); + int nx = plant.num_positions() + plant.num_velocities(); + + auto l_toe_frame = &plant.GetBodyByName("toe_left").body_frame(); + auto r_toe_frame = &plant.GetBodyByName("toe_right").body_frame(); + auto hip_left_frame = &plant.GetBodyByName("hip_left").body_frame(); + auto hip_right_frame = &plant.GetBodyByName("hip_right").body_frame(); + auto pelvis_frame = &plant.GetBodyByName("pelvis").body_frame(); + auto world = &plant.world_frame(); + + DirconTrajectory dircon_traj(plant, FLAGS_folder_path + FLAGS_trajectory_name); + PiecewisePolynomial state_traj = + dircon_traj.ReconstructStateTrajectory(); + + MatrixXd M = dircon_traj.GetTrajectory("mirror_matrix").datapoints; + + double end_time = state_traj.end_time(); + + std::vector all_l_foot_points; + std::vector all_r_foot_points; + std::vector all_l_hip_points; + std::vector all_r_hip_points; + std::vector all_pelvis_points; + std::vector all_pelvis_orientation; + std::vector all_times; + Vector3d zero_offset = Vector3d::Zero(); + + for (int mode = 0; mode < dircon_traj.GetNumModes(); ++mode) { + if (dircon_traj.GetStateBreaks(mode).size() <= 1) { + continue; + } + VectorXd times = dircon_traj.GetStateBreaks(mode); + MatrixXd state_samples = dircon_traj.GetStateSamples(mode); + + int n_points = times.size(); + MatrixXd l_foot_points(9, n_points); + MatrixXd r_foot_points(9, n_points); + MatrixXd l_hip_points(9, n_points); + MatrixXd r_hip_points(9, n_points); + MatrixXd pelvis_points(9, n_points); + MatrixXd pelvis_orientation(12, n_points); + + for (unsigned int i = 0; i < times.size(); ++i) { + VectorXd x_i = state_samples.col(i).head(nx); + VectorXd xdot_i = state_samples.col(i).tail(nx); + + plant.SetPositionsAndVelocities(context.get(), x_i); + Eigen::Ref pelvis_pos_block = + pelvis_points.block(0, i, 3, 1); + Eigen::Ref l_foot_pos_block = + l_foot_points.block(0, i, 3, 1); + Eigen::Ref r_foot_pos_block = + r_foot_points.block(0, i, 3, 1); + Eigen::Ref l_hip_pos_block = + l_hip_points.block(0, i, 3, 1); + Eigen::Ref r_hip_pos_block = + r_hip_points.block(0, i, 3, 1); + plant.CalcPointsPositions(*context, *pelvis_frame, zero_offset, *world, + &pelvis_pos_block); + plant.CalcPointsPositions(*context, *l_toe_frame, zero_offset, *world, + &l_foot_pos_block); + plant.CalcPointsPositions(*context, *r_toe_frame, zero_offset, *world, + &r_foot_pos_block); + plant.CalcPointsPositions(*context, *hip_left_frame, zero_offset, *world, + &l_hip_pos_block); + plant.CalcPointsPositions(*context, *hip_right_frame, zero_offset, *world, + &r_hip_pos_block); + + pelvis_orientation.block(0, i, 4, 1) = x_i.head(4); + pelvis_orientation.block(4, i, 4, 1) = xdot_i.head(4); + + MatrixXd J_CoM(3, nv); + MatrixXd J_l_foot(3, nv); + MatrixXd J_r_foot(3, nv); + MatrixXd J_l_hip(3, nv); + MatrixXd J_r_hip(3, nv); + + plant.CalcJacobianTranslationalVelocity(*context, JacobianWrtVariable::kV, + *pelvis_frame, zero_offset, + *world, *world, &J_CoM); + plant.CalcJacobianTranslationalVelocity(*context, JacobianWrtVariable::kV, + *l_toe_frame, zero_offset, *world, + *world, &J_l_foot); + plant.CalcJacobianTranslationalVelocity(*context, JacobianWrtVariable::kV, + *r_toe_frame, zero_offset, *world, + *world, &J_r_foot); + plant.CalcJacobianTranslationalVelocity(*context, JacobianWrtVariable::kV, + *hip_left_frame, zero_offset, + *world, *world, &J_l_hip); + plant.CalcJacobianTranslationalVelocity(*context, JacobianWrtVariable::kV, + *hip_right_frame, zero_offset, + *world, *world, &J_r_hip); + + VectorXd v_i = x_i.tail(nv); + pelvis_points.block(3, i, 3, 1) = J_CoM * v_i; + l_foot_points.block(3, i, 3, 1) = J_l_foot * v_i; + r_foot_points.block(3, i, 3, 1) = J_r_foot * v_i; + l_hip_points.block(3, i, 3, 1) = J_l_hip * v_i; + r_hip_points.block(3, i, 3, 1) = J_r_hip * v_i; + + VectorXd JdotV_CoM = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *pelvis_frame, zero_offset, *world, + *world); + VectorXd JdotV_l_foot = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *l_toe_frame, zero_offset, *world, + *world); + VectorXd JdotV_r_foot = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *r_toe_frame, zero_offset, *world, + *world); + VectorXd JdotV_l_hip = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *hip_left_frame, zero_offset, + *world, *world); + VectorXd JdotV_r_hip = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *hip_right_frame, zero_offset, + *world, *world); + + pelvis_points.block(6, i, 3, 1) = JdotV_CoM + J_CoM * xdot_i.tail(nv); + l_foot_points.block(6, i, 3, 1) = + JdotV_l_foot + J_l_foot * xdot_i.tail(nv); + r_foot_points.block(6, i, 3, 1) = + JdotV_r_foot + J_r_foot * xdot_i.tail(nv); + l_hip_points.block(6, i, 3, 1) = JdotV_l_hip + J_l_hip * xdot_i.tail(nv); + r_hip_points.block(6, i, 3, 1) = JdotV_r_hip + J_r_hip * xdot_i.tail(nv); + } + + all_times.push_back(times); + all_l_foot_points.push_back(l_foot_points); + all_r_foot_points.push_back(r_foot_points); + all_l_hip_points.push_back(l_hip_points); + all_r_hip_points.push_back(r_hip_points); + all_pelvis_points.push_back(pelvis_points); + all_pelvis_orientation.push_back(pelvis_orientation); + } + + if (FLAGS_mirror_traj) { + double x_offset = state_traj.value(state_traj.end_time())(4) - + state_traj.value(state_traj.start_time())(4); + std::cout << "x_offset: " << x_offset << std::endl; + // Extended trajectory + for (int mode = 0; mode < dircon_traj.GetNumModes(); ++mode) { + if (dircon_traj.GetStateBreaks(mode).size() <= 1) { + continue; + } + VectorXd times = + dircon_traj.GetStateBreaks(mode) + + end_time * VectorXd::Ones(dircon_traj.GetStateBreaks(mode).size()); + MatrixXd x_samples = M * dircon_traj.GetStateSamples(mode).topRows(nx); + MatrixXd xdot_samples = + M * dircon_traj.GetStateSamples(mode).bottomRows(nx); + + int n_points = times.size(); + MatrixXd l_foot_points(9, n_points); + MatrixXd r_foot_points(9, n_points); + MatrixXd l_hip_points(9, n_points); + MatrixXd r_hip_points(9, n_points); + MatrixXd pelvis_points(9, n_points); + MatrixXd pelvis_orientation(12, n_points); + + for (unsigned int i = 0; i < times.size(); ++i) { + // VectorXd x_i = M * state_samples.col(i).head(nx); + // VectorXd xdot_i = M * state_samples.col(i).tail(nx); + VectorXd x_i = x_samples.col(i); + VectorXd xdot_i = xdot_samples.col(i); + x_i(4) = x_i(4) + x_offset; + + plant.SetPositionsAndVelocities(context.get(), x_i); + Eigen::Ref pelvis_pos_block = + pelvis_points.block(0, i, 3, 1); + Eigen::Ref l_foot_pos_block = + l_foot_points.block(0, i, 3, 1); + Eigen::Ref r_foot_pos_block = + r_foot_points.block(0, i, 3, 1); + Eigen::Ref l_hip_pos_block = + l_hip_points.block(0, i, 3, 1); + Eigen::Ref r_hip_pos_block = + r_hip_points.block(0, i, 3, 1); + plant.CalcPointsPositions(*context, *pelvis_frame, zero_offset, *world, + &pelvis_pos_block); + plant.CalcPointsPositions(*context, *l_toe_frame, zero_offset, *world, + &l_foot_pos_block); + plant.CalcPointsPositions(*context, *r_toe_frame, zero_offset, *world, + &r_foot_pos_block); + plant.CalcPointsPositions(*context, *hip_left_frame, zero_offset, + *world, &l_hip_pos_block); + plant.CalcPointsPositions(*context, *hip_right_frame, zero_offset, + *world, &r_hip_pos_block); + + pelvis_orientation.block(0, i, 4, 1) = x_i.head(4); + pelvis_orientation.block(4, i, 4, 1) = xdot_i.head(4); + + MatrixXd J_CoM(3, nv); + MatrixXd J_l_foot(3, nv); + MatrixXd J_r_foot(3, nv); + MatrixXd J_l_hip(3, nv); + MatrixXd J_r_hip(3, nv); + plant.CalcJacobianTranslationalVelocity( + *context, JacobianWrtVariable::kV, *pelvis_frame, zero_offset, + *world, *world, &J_CoM); + plant.CalcJacobianTranslationalVelocity( + *context, JacobianWrtVariable::kV, *l_toe_frame, zero_offset, + *world, *world, &J_l_foot); + plant.CalcJacobianTranslationalVelocity( + *context, JacobianWrtVariable::kV, *r_toe_frame, zero_offset, + *world, *world, &J_r_foot); + plant.CalcJacobianTranslationalVelocity( + *context, JacobianWrtVariable::kV, *hip_left_frame, zero_offset, + *world, *world, &J_l_hip); + plant.CalcJacobianTranslationalVelocity( + *context, JacobianWrtVariable::kV, *hip_right_frame, zero_offset, + *world, *world, &J_r_hip); + + VectorXd v_i = x_i.tail(nv); + pelvis_points.block(3, i, 3, 1) = J_CoM * v_i; + l_foot_points.block(3, i, 3, 1) = J_l_foot * v_i; + r_foot_points.block(3, i, 3, 1) = J_r_foot * v_i; + l_hip_points.block(3, i, 3, 1) = J_l_hip * v_i; + r_hip_points.block(3, i, 3, 1) = J_r_hip * v_i; + + VectorXd JdotV_CoM = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *pelvis_frame, zero_offset, + *world, *world); + VectorXd JdotV_l_foot = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *l_toe_frame, zero_offset, + *world, *world); + VectorXd JdotV_r_foot = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *r_toe_frame, zero_offset, + *world, *world); + VectorXd JdotV_l_hip = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *hip_left_frame, zero_offset, + *world, *world); + VectorXd JdotV_r_hip = plant.CalcBiasTranslationalAcceleration( + *context, JacobianWrtVariable::kV, *hip_right_frame, zero_offset, + *world, *world); + + pelvis_points.block(6, i, 3, 1) = JdotV_CoM + J_CoM * xdot_i.tail(nv); + l_foot_points.block(6, i, 3, 1) = + JdotV_l_foot + J_l_foot * xdot_i.tail(nv); + r_foot_points.block(6, i, 3, 1) = + JdotV_r_foot + J_r_foot * xdot_i.tail(nv); + l_hip_points.block(6, i, 3, 1) = + JdotV_l_hip + J_l_hip * xdot_i.tail(nv); + r_hip_points.block(6, i, 3, 1) = + JdotV_r_hip + J_r_hip * xdot_i.tail(nv); + } + + all_times.push_back(times); + all_l_foot_points.push_back(l_foot_points); + all_r_foot_points.push_back(r_foot_points); + all_l_hip_points.push_back(l_hip_points); + all_r_hip_points.push_back(r_hip_points); + all_pelvis_points.push_back(pelvis_points); + all_pelvis_orientation.push_back(pelvis_orientation); + } + } + + std::vector converted_trajectories; + std::vector trajectory_names; + + std::cout << "num_modes: " << all_times.size() << std::endl; + for (int mode = 0; mode < all_times.size(); ++mode) { + auto lfoot_traj_block = LcmTrajectory::Trajectory(); + lfoot_traj_block.traj_name = "left_foot_trajectory" + std::to_string(mode); + lfoot_traj_block.datapoints = all_l_foot_points[mode]; + lfoot_traj_block.time_vector = all_times[mode]; + lfoot_traj_block.datatypes = {"lfoot_x", "lfoot_y", "lfoot_z", + "lfoot_xdot", "lfoot_ydot", "lfoot_zdot", + "lfoot_xddot", "lfoot_yddot", "lfoot_zddot"}; + + auto rfoot_traj_block = LcmTrajectory::Trajectory(); + rfoot_traj_block.traj_name = "right_foot_trajectory" + std::to_string(mode); + rfoot_traj_block.datapoints = all_r_foot_points[mode]; + rfoot_traj_block.time_vector = all_times[mode]; + rfoot_traj_block.datatypes = {"rfoot_x", "rfoot_y", "rfoot_z", + "rfoot_xdot", "rfoot_ydot", "rfoot_zdot", + "rfoot_xddot", "rfoot_yddot", "rfoot_zddot"}; + + auto lhip_traj_block = LcmTrajectory::Trajectory(); + lhip_traj_block.traj_name = "left_hip_trajectory" + std::to_string(mode); + lhip_traj_block.datapoints = all_l_hip_points[mode]; + lhip_traj_block.time_vector = all_times[mode]; + lhip_traj_block.datatypes = {"lhip_x", "lhip_y", "lhip_z", + "lhip_xdot", "lhip_ydot", "lhip_zdot", + "lhip_xddot", "lhip_yddot", "lhip_zddot"}; + + auto rhip_traj_block = LcmTrajectory::Trajectory(); + rhip_traj_block.traj_name = "right_hip_trajectory" + std::to_string(mode); + rhip_traj_block.datapoints = all_r_hip_points[mode]; + rhip_traj_block.time_vector = all_times[mode]; + rhip_traj_block.datatypes = {"rhip_x", "rhip_y", "rhip_z", + "rhip_xdot", "rhip_ydot", "rhip_zdot", + "rhip_xddot", "rhip_yddot", "rhip_zddot"}; + + auto pelvis_traj_block = LcmTrajectory::Trajectory(); + pelvis_traj_block.traj_name = + "pelvis_trans_trajectory" + std::to_string(mode); + pelvis_traj_block.datapoints = all_pelvis_points[mode]; + pelvis_traj_block.time_vector = all_times[mode]; + pelvis_traj_block.datatypes = { + "pelvis_x", "pelvis_y", "pelvis_z", + "pelvis_xdot", "pelvis_ydot", "pelvis_zdot", + "pelvis_xddot", "pelvis_yddot", "pelvis_zddot"}; + + auto pelvis_orientation_block = LcmTrajectory::Trajectory(); + pelvis_orientation_block.traj_name = + "pelvis_rot_trajectory" + std::to_string(mode); + pelvis_orientation_block.datapoints = all_pelvis_orientation[mode]; + pelvis_orientation_block.time_vector = all_times[mode]; + pelvis_orientation_block.datatypes = { + "pelvis_rotw", "pelvis_rotx", "pelvis_roty", + "pelvis_rotz", "pelvis_rotwdot", "pelvis_rotxdot", + "pelvis_rotydot", "pelvis_rotzdot", "pelvis_rotwddot", + "pelvis_rotxddot", "pelvis_rotyddot", "pelvis_rotzddot"}; + + converted_trajectories.push_back(lfoot_traj_block); + converted_trajectories.push_back(rfoot_traj_block); + converted_trajectories.push_back(lhip_traj_block); + converted_trajectories.push_back(rhip_traj_block); + converted_trajectories.push_back(pelvis_traj_block); + converted_trajectories.push_back(pelvis_orientation_block); + trajectory_names.push_back(lfoot_traj_block.traj_name); + trajectory_names.push_back(rfoot_traj_block.traj_name); + trajectory_names.push_back(lhip_traj_block.traj_name); + trajectory_names.push_back(rhip_traj_block.traj_name); + trajectory_names.push_back(pelvis_traj_block.traj_name); + trajectory_names.push_back(pelvis_orientation_block.traj_name); + } + + auto processed_traj = LcmTrajectory(converted_trajectories, trajectory_names, + "walking_trajectory", + "Output trajectories " + "for Cassie walking"); + + processed_traj.WriteToFile(FLAGS_folder_path + FLAGS_trajectory_name + + "_processed_rel"); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + dairlib::DoMain(); +} \ No newline at end of file diff --git a/examples/Cassie/osc_run/foot_traj_generator.cc b/examples/Cassie/osc_run/foot_traj_generator.cc index 74cdec384b..9e2fd6e75b 100644 --- a/examples/Cassie/osc_run/foot_traj_generator.cc +++ b/examples/Cassie/osc_run/foot_traj_generator.cc @@ -15,7 +15,7 @@ using Eigen::VectorXd; using std::string; using dairlib::systems::OutputVector; -using drake::multibody::BodyFrame; +using drake::multibody::RigidBodyFrame; using drake::multibody::Frame; using drake::multibody::JacobianWrtVariable; using drake::multibody::MultibodyPlant; @@ -87,7 +87,7 @@ FootTrajGenerator::FootTrajGenerator(const MultibodyPlant& plant, last_stance_timestamp_idx_ = this->DeclareDiscreteState(1); // State variables inside this controller block - DeclarePerStepDiscreteUpdateEvent(&FootTrajGenerator::DiscreteVariableUpdate); + DeclareForcedDiscreteUpdateEvent(&FootTrajGenerator::DiscreteVariableUpdate); m_ = plant_.CalcTotalMass(*context_); } @@ -100,7 +100,6 @@ EventStatus FootTrajGenerator::DiscreteVariableUpdate( (OutputVector*)this->EvalVectorInput(context, state_port_); // Read in finite state machine VectorXd fsm_state = this->EvalVectorInput(context, fsm_port_)->get_value(); - VectorXd q = robot_output->GetPositions(); VectorXd v = robot_output->GetVelocities(); multibody::SetPositionsIfNew(plant_, q, context_); diff --git a/examples/Cassie/osc_run/learned_osc_running_gains.yaml b/examples/Cassie/osc_run/learned_osc_running_gains.yaml new file mode 100644 index 0000000000..dfc74fe8df --- /dev/null +++ b/examples/Cassie/osc_run/learned_osc_running_gains.yaml @@ -0,0 +1,47 @@ +FootstepKd: [0.03177352249613004, 0.0, 0.0, 0.0, 0.4389937568136904, 0.0, 0.0, 0.0, + 0.12716547061955297] +LiftoffSwingFootKd: [5, 0, 0, 0, 5, 0, 0, 0, 1] +LiftoffSwingFootKp: [50, 0, 0, 0, 50, 0, 0, 0, 35] +LiftoffSwingFootW: [1, 0, 0, 0, 1, 0, 0, 0, 1] +PelvisKd: [0.5890737665487785, 0.0, 0.0, 0.0, 0.3065697740681626, 0.0, 0.0, 0.0, 3.575709536524337] +PelvisKp: [16.038541830836348, 0.0, 0.0, 0.0, 3.9843429704843962, 0.0, 0.0, 0.0, 51.84941416797943] +PelvisRotKd: [10.004676756212609, 0.0, 0.0, 0.0, 3.3726977593874716, 0.0, 0.0, 0.0, + 4.360440654654406] +PelvisRotKp: [400.0, 0, 0, 0, 200.0, 0, 0, 0, 0.0] +PelvisRotW: [5, 0, 0, 0, 1, 0, 0, 0, 5] +PelvisW: [0, 0, 0, 0, 0, 0, 0, 0, 5] +SwingFootKd: [7.706143205070923, 0.0, 0.0, 0.0, 5.241286582894659, 0.0, 0.0, 0.0, + 1.127060223432193] +SwingFootKp: [145.14247825584522, 0.0, 0.0, 0.0, 79.71734912124174, 0.0, 0.0, 0.0, + 71.26378559542337] +SwingFootW: [10, 0, 0, 0, 100, 0, 0, 0, 5] +W_accel: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.001] +W_input_reg: [1, 0.9, 0.5, 0.1, 5, 1, 0.9, 0.5, 0.1, 5] +W_lambda_c_reg: [1, 0.001, 0.01, 1, 0.001, 0.01, 1, 0.1, 0.01, 1, 0.1, 0.01] +W_lambda_h_reg: [0.01, 0.01, 0.01, 0.01, 0.02, 0.02] +ekf_filter_tau: [0.05, 0.01, 0.01] +flight_duration: 0.1 +footstep_lateral_offset: 0.011344974721002179 +footstep_sagital_offset: 0.006634618253487809 +hip_yaw_kd: 1 +hip_yaw_kp: 34.98541652922292 +impact_tau: 0.05 +impact_threshold: 0.1 +mid_foot_height: 0.05032695569729413 +mu: 0.4715404404999052 +relative_feet: true +relative_pelvis: true +rest_length: 0.9560136356226284 +stance_duration: 0.2 +swing_toe_kd: 10 +swing_toe_kp: 1500 +vel_scale_rot: -0.5 +vel_scale_trans_lateral: -0.15 +vel_scale_trans_sagital: 0.25 +w_accel: 0.0001 +w_hip_yaw: 5.41862598246515 +w_input: 0.0 +w_input_reg: 0.00022359024445701495 +w_soft_constraint: 70.59455850641295 +w_swing_toe: 1 diff --git a/examples/Cassie/osc_run/osc_running_gains_fast.yaml b/examples/Cassie/osc_run/osc_running_gains_fast.yaml new file mode 100644 index 0000000000..8e8eb6ce74 --- /dev/null +++ b/examples/Cassie/osc_run/osc_running_gains_fast.yaml @@ -0,0 +1,110 @@ +controller_frequency: 1000 + +w_input: 0.000001 +w_input_reg: 0.01 +w_accel: 0.0001 +w_soft_constraint: 10000 +w_lambda: 0.05 +w_input_accel: 0.1 +w_joint_limit: 0 +impact_threshold: 0.050 +impact_tau: 0.005 +weight_scaling: 1.0 +mu: 0.6 +# roll, yaw, pitch, knee, knee_spring, ankle_joint, ankle_spring, toe +W_accel: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.0001, + 1, 1, 1, 1, 0.01, 0.01, 0.01, 0.0001 ] +W_input_reg: [ 1, 0.9, 0.5, 0.1, 1, + 1, 0.9, 0.5, 0.1, 1 ] +W_lambda_c_reg: [ 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01 ] +W_lambda_h_reg: [ 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001 ] +#w_soft_constraint: 1000000 + +no_derivative_feedback: false +rot_filter_tau: 0.0025 +ekf_filter_tau: [ 0.00, 0.00, 0.00 ] + +# High level command gains (with radio) +vel_scale_rot: -4 +vel_scale_trans_sagital: 15 +vel_scale_trans_lateral: -0.5 +target_vel_filter_alpha: 0.01 + +# SLIP parameters +rest_length: 0.9 +rest_length_offset: 0.05 +stance_duration: 0.2 +flight_duration: 0.1 +# max percent variance +stance_variance: 0.4 +flight_variance: 0.2 + +w_swing_toe: 0.1 +swing_toe_kp: 2000 +swing_toe_kd: 5 + +w_hip_yaw: 5 +hip_yaw_kp: 100 +hip_yaw_kd: 7 +# Foot placement parameters +#footstep_offset: -0.05 +footstep_sagital_offset: -0.00 +footstep_lateral_offset: 0.04 # drake +mid_foot_height: 0.3 +FootstepKd: + [ 0.012, 0, 0, + 0, 0.3, 0, + 0, 0, 0 ] +PelvisW: + [ 0, 0, 0, + 0, 0, 0, + 0, 0, 5] +PelvisKp: + [ 0, 0, 0, + 0, 0, 0, + 0, 0, 125] +PelvisKd: + [ 0, 0, 0, + 0, 0, 0, + 0, 0, 5] +PelvisRotW: + [10, 0, 0, + 0, 5, 0, + 0, 0, 1] +PelvisRotKp: + [150., 0, 0, + 0, 200., 0, + 0, 0, 0.] +PelvisRotKd: + [10, 0, 0, + 0, 10, 0, + 0, 0, 5.] +SwingFootW: + [10, 0, 0, + 0, 100, 0, + 0, 0, 50] +SwingFootKp: + [145, 0, 0, + 0, 150, 0, + 0, 0, 175] +SwingFootKd: + [5., 0, 0, + 0, 5., 0, + 0, 0, 7.5] +LiftoffSwingFootW: + [0, 0, 0, + 0, 0, 0, + 0, 0, 0] +LiftoffSwingFootKp: + [0, 0, 0, + 0, 0, 0, + 0, 0, 0] +LiftoffSwingFootKd: + [ 0, 0, 0, + 0, 0, 0, + 0, 0, 0] diff --git a/examples/Cassie/osc_run/osc_running_qp_settings.yaml b/examples/Cassie/osc_run/osc_running_qp_settings.yaml index 5cf72f59b4..f9939ffc7f 100644 --- a/examples/Cassie/osc_run/osc_running_qp_settings.yaml +++ b/examples/Cassie/osc_run/osc_running_qp_settings.yaml @@ -2,11 +2,11 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 250 - linsys_solver: 0 verbose: 0 warm_start: 1 scaling: 1 adaptive_rho: 1 + adaptive_rho_interval: 0 polish: 1 polish_refine_iter: 1 scaled_termination: 1 @@ -20,7 +20,6 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 diff --git a/examples/Cassie/osc_run/pelvis_trans_traj_generator.cc b/examples/Cassie/osc_run/pelvis_trans_traj_generator.cc index 9479471c7a..012207d1c1 100644 --- a/examples/Cassie/osc_run/pelvis_trans_traj_generator.cc +++ b/examples/Cassie/osc_run/pelvis_trans_traj_generator.cc @@ -63,7 +63,7 @@ PelvisTransTrajGenerator::PelvisTransTrajGenerator( this->DeclareAbstractOutputPort("pelvis_trans_traj", traj_inst, &PelvisTransTrajGenerator::CalcTraj); - DeclarePerStepDiscreteUpdateEvent( + DeclareForcedDiscreteUpdateEvent( &PelvisTransTrajGenerator::DiscreteVariableUpdate); } diff --git a/examples/Cassie/osc_run/pelvis_trans_traj_generator.h b/examples/Cassie/osc_run/pelvis_trans_traj_generator.h index a6193b549a..2337662ef4 100644 --- a/examples/Cassie/osc_run/pelvis_trans_traj_generator.h +++ b/examples/Cassie/osc_run/pelvis_trans_traj_generator.h @@ -51,7 +51,7 @@ class PelvisTransTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const drake::multibody::Body& pelvis_; // A list of pairs of contact body frame and contact point diff --git a/examples/Cassie/run_dircon_jumping.cc b/examples/Cassie/run_dircon_jumping.cc index a6c75a7df2..30af30f689 100644 --- a/examples/Cassie/run_dircon_jumping.cc +++ b/examples/Cassie/run_dircon_jumping.cc @@ -176,7 +176,7 @@ void DoMain() { false); Parser parser_vis(&plant_vis, &scene_graph); - parser_vis.AddModelFromFile(file_name); + parser_vis.AddModels(file_name); plant.Finalize(); plant_vis.Finalize(); @@ -189,7 +189,7 @@ void DoMain() { if (FLAGS_use_springs && FLAGS_convert_to_springs) { MultibodyPlant plant_wo_spr(0.0); Parser parser(&plant_wo_spr); - parser.AddModelFromFile( + parser.AddModels( "examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf"); plant_wo_spr.Finalize(); spr_map = CreateWithSpringsToWithoutSpringsMapPos(plant, plant_wo_spr); @@ -442,9 +442,9 @@ void SetKinematicConstraints(Dircon* trajopt, auto& prog = trajopt->prog(); // position constraints prog.AddBoundingBoxConstraint(0 - midpoint, 0 - midpoint, - x_0(pos_map.at("base_x"))); - prog.AddBoundingBoxConstraint(0, 0, x_0(pos_map.at("base_y"))); - prog.AddBoundingBoxConstraint(0, 0, x_f(pos_map.at("base_y"))); + x_0(pos_map.at("pelvis_x"))); + prog.AddBoundingBoxConstraint(0, 0, x_0(pos_map.at("pelvis_y"))); + prog.AddBoundingBoxConstraint(0, 0, x_f(pos_map.at("pelvis_y"))); // initial fb orientation constraint VectorXd quat_identity(4); @@ -472,13 +472,13 @@ void SetKinematicConstraints(Dircon* trajopt, // Jumping height constraints prog.AddBoundingBoxConstraint(rest_height - eps, rest_height + eps, - x_0(pos_map.at("base_z"))); + x_0(pos_map.at("pelvis_z"))); prog.AddBoundingBoxConstraint(0.5 * FLAGS_height + rest_height - eps, FLAGS_height + rest_height + eps, - x_top(pos_map.at("base_z"))); + x_top(pos_map.at("pelvis_z"))); prog.AddBoundingBoxConstraint(0.8 * FLAGS_height + rest_height - eps, 0.8 * FLAGS_height + rest_height + eps, - x_f(pos_map.at("base_z"))); + x_f(pos_map.at("pelvis_z"))); // Zero starting and final velocities prog.AddLinearConstraint(VectorXd::Zero(n_v) == x_0.tail(n_v)); diff --git a/examples/Cassie/run_dircon_running.cc b/examples/Cassie/run_dircon_running.cc new file mode 100644 index 0000000000..044accf0e8 --- /dev/null +++ b/examples/Cassie/run_dircon_running.cc @@ -0,0 +1,606 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "examples/Cassie/cassie_utils.h" +#include "lcm/dircon_saved_trajectory.h" +#include "lcm/lcm_trajectory.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" +#include "multibody/multibody_utils.h" +#include "multibody/visualization_utils.h" +#include "solvers/cost_function_utils.h" +#include "systems/trajectory_optimization/dircon/dircon_mode.h" + +#include "drake/solvers/solve.h" +#include "drake/systems/trajectory_optimization/multiple_shooting.h" + +using std::cout; +using std::endl; +using std::map; +using std::shared_ptr; +using std::string; +using std::unordered_map; +using std::vector; + +using drake::geometry::DrakeVisualizer; +using drake::geometry::SceneGraph; +using drake::multibody::Parser; +using Eigen::Matrix3Xd; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using dairlib::multibody::KinematicEvaluator; +using dairlib::multibody::KinematicEvaluatorSet; +using dairlib::systems::trajectory_optimization::Dircon; +using dairlib::systems::trajectory_optimization::DirconMode; +using dairlib::systems::trajectory_optimization::DirconModeSequence; +using dairlib::systems::trajectory_optimization::PointPositionConstraint; +using drake::math::RotationMatrix; +using drake::multibody::Body; +using drake::multibody::MultibodyPlant; +using drake::solvers::MathematicalProgram; +using drake::solvers::MathematicalProgramResult; +using drake::solvers::MatrixXDecisionVariable; +using drake::solvers::SolutionResult; +using drake::solvers::VectorXDecisionVariable; +using drake::systems::trajectory_optimization::MultipleShooting; +using drake::trajectories::PiecewisePolynomial; + +DEFINE_int32(knot_points, 16, "Number of knot points per stance mode"); +DEFINE_double(stride_length, 0.2, "Stride length."); +DEFINE_double(start_height, 0.90, "Starting pelvis height for the trajectory."); +DEFINE_double(stance_T, 0.2, "Single stance duration (s)."); +DEFINE_double(flight_phase_T, 0.1, "Flight phase duration (s)."); +DEFINE_int32(scale_option, 2, + "Scale option of SNOPT" + "Use 2 if seeing snopta exit 40 in log file"); +DEFINE_bool(scale_variables, true, "Set to false if using default scaling"); +DEFINE_bool(scale_constraints, true, "Set to false if using default scaling"); +DEFINE_double(tol, 1e-7, "Tolerance for constraint violation and dual gap"); +DEFINE_string(load_filename, "", "File to load decision vars from."); +DEFINE_string(data_directory, "examples/Cassie/saved_trajectories/", + "Directory path to save decision vars to."); +DEFINE_string(save_filename, "default_filename", + "Filename to save decision " + "vars to."); +DEFINE_bool(ipopt, false, "Set flag to true to use ipopt instead of snopt"); +DEFINE_bool(same_knotpoints, false, + "Set flag to true if seeding with a trajectory with the same " + "number of knotpoints"); + +namespace dairlib { + +void setKinematicConstraints(Dircon& trajopt, + const MultibodyPlant& plant); +void SetInitialGuessFromTrajectory( + Dircon& trajopt, + const MultibodyPlant& plant, const string& filepath, + bool same_knot_points = false); +vector createStateNameVectorFromMap(const map& pos_map, + const map& vel_map, + const map& act_map); +void DoMain() { + // Drake system initialization stuff + drake::systems::DiagramBuilder builder; + SceneGraph& scene_graph = *builder.AddSystem(); + scene_graph.set_name("scene_graph"); + MultibodyPlant plant(0.0); + MultibodyPlant plant_vis(0.0); + + string file_name = "examples/Cassie/urdf/cassie_fixed_springs.urdf"; + Parser parser(&plant); + Parser parser_vis(&plant_vis, &scene_graph); + + parser.AddModels(file_name); + parser_vis.AddModels(file_name); + plant.Finalize(); + plant_vis.Finalize(); + + int n_q = plant.num_positions(); + int n_v = plant.num_velocities(); + int n_x = n_q + n_v; + + std::cout << "nq: " << n_q << " n_v: " << n_v << " n_x: " << n_x << std::endl; + // Create maps for joints + map pos_map = multibody::MakeNameToPositionsMap(plant); + map vel_map = multibody::MakeNameToVelocitiesMap(plant); + map act_map = multibody::MakeNameToActuatorsMap(plant); + + // Set up contact/distance constraints + auto left_toe_pair = LeftToeFront(plant); + auto left_heel_pair = LeftToeRear(plant); + auto right_toe_pair = RightToeFront(plant); + auto right_heel_pair = RightToeRear(plant); + + std::vector toe_active_inds{0, 1, 2}; + std::vector heel_active_inds{1, 2}; + + double mu = 1; + + auto left_toe_eval = multibody::WorldPointEvaluator( + plant, left_toe_pair.first, left_toe_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), toe_active_inds); + left_toe_eval.set_frictional(); + left_toe_eval.set_mu(mu); + auto left_heel_eval = multibody::WorldPointEvaluator( + plant, left_heel_pair.first, left_heel_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), heel_active_inds); + left_heel_eval.set_frictional(); + left_heel_eval.set_mu(mu); + auto right_toe_eval = multibody::WorldPointEvaluator( + plant, right_toe_pair.first, right_toe_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), toe_active_inds); + right_toe_eval.set_frictional(); + right_toe_eval.set_mu(mu); + auto right_heel_eval = multibody::WorldPointEvaluator( + plant, right_heel_pair.first, right_heel_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), heel_active_inds); + right_heel_eval.set_frictional(); + right_heel_eval.set_mu(mu); + + auto left_loop_eval = LeftLoopClosureEvaluator(plant); + auto right_loop_eval = RightLoopClosureEvaluator(plant); + + auto left_stance_constraints = KinematicEvaluatorSet(plant); + int left_toe_eval_ind = left_stance_constraints.add_evaluator(&left_toe_eval); + int left_heel_eval_ind = + left_stance_constraints.add_evaluator(&left_heel_eval); + left_stance_constraints.add_evaluator(&left_loop_eval); + left_stance_constraints.add_evaluator(&right_loop_eval); + + auto right_stance_constraints = KinematicEvaluatorSet(plant); + int right_toe_eval_ind = + right_stance_constraints.add_evaluator(&right_toe_eval); + int right_heel_eval_ind = + right_stance_constraints.add_evaluator(&right_heel_eval); + right_stance_constraints.add_evaluator(&left_loop_eval); + right_stance_constraints.add_evaluator(&right_loop_eval); + + auto flight_phase_constraints = KinematicEvaluatorSet(plant); + flight_phase_constraints.add_evaluator(&left_loop_eval); + flight_phase_constraints.add_evaluator(&right_loop_eval); + + int stance_knotpoints = FLAGS_knot_points; + int flight_phase_knotpoints = FLAGS_knot_points / 2; + double flight_phase_T = FLAGS_flight_phase_T; + double min_T = FLAGS_stance_T; + double max_T = FLAGS_stance_T; + + auto left_stance = DirconMode(left_stance_constraints, + stance_knotpoints, min_T, max_T); + auto right_stance = DirconMode(right_stance_constraints, + stance_knotpoints, min_T, max_T); + left_stance.MakeConstraintRelative(left_toe_eval_ind, 0); // x + left_stance.MakeConstraintRelative(left_toe_eval_ind, 1); // y + left_stance.MakeConstraintRelative(left_heel_eval_ind, 0); // x + left_stance.MakeConstraintRelative(left_heel_eval_ind, 1); // y + right_stance.MakeConstraintRelative(right_toe_eval_ind, 0); // x + right_stance.MakeConstraintRelative(right_toe_eval_ind, 1); // y + right_stance.MakeConstraintRelative(right_heel_eval_ind, 0); // x + right_stance.MakeConstraintRelative(right_heel_eval_ind, 1); // y + auto flight_phase = + DirconMode(flight_phase_constraints, flight_phase_knotpoints, + flight_phase_T, flight_phase_T); + + auto all_modes = DirconModeSequence(plant); + all_modes.AddMode(&left_stance); + all_modes.AddMode(&flight_phase); + all_modes.AddMode(&right_stance); + // all_modes.AddMode(&flight_phase); + + auto trajopt = Dircon(all_modes); + auto& prog = trajopt.prog(); + + double tol = FLAGS_tol; + if (FLAGS_ipopt) { + // Ipopt settings adapted from CaSaDi and FROST + auto id = drake::solvers::IpoptSolver::id(); + prog.SetSolverOption(id, "tol", tol); + prog.SetSolverOption(id, "dual_inf_tol", tol); + prog.SetSolverOption(id, "constr_viol_tol", tol); + prog.SetSolverOption(id, "compl_inf_tol", tol); + prog.SetSolverOption(id, "max_iter", 1e5); + prog.SetSolverOption(id, "nlp_lower_bound_inf", -1e6); + prog.SetSolverOption(id, "nlp_upper_bound_inf", 1e6); + prog.SetSolverOption(id, "print_timing_statistics", "yes"); + prog.SetSolverOption(id, "print_level", 5); + prog.SetSolverOption(id, "output_file", "../ipopt.out"); + + // Set to ignore overall tolerance/dual infeasibility, but terminate when + // primal feasible and objective fails to increase over 5 iterations. + prog.SetSolverOption(id, "acceptable_compl_inf_tol", tol); + prog.SetSolverOption(id, "acceptable_constr_viol_tol", tol); + prog.SetSolverOption(id, "acceptable_obj_change_tol", 1e-3); + prog.SetSolverOption(id, "acceptable_tol", 1e2); + prog.SetSolverOption(id, "acceptable_iter", 5); + } else { + // Snopt settings + auto id = drake::solvers::SnoptSolver::id(); + prog.SetSolverOption(id, "Print file", "../snopt.out"); + prog.SetSolverOption(id, "Major iterations limit", 1e5); + prog.SetSolverOption(id, "Iterations limit", 100000); + prog.SetSolverOption(id, "Verify level", 0); + + // snopt doc said try 2 if seeing snopta exit 40 + prog.SetSolverOption(id, "Scale option", 2); + prog.SetSolverOption(id, "Solution", "No"); + + // target nonlinear constraint violation + prog.SetSolverOption(id, "Major optimality tolerance", 1e-4); + + // target complementarity gap + prog.SetSolverOption(id, "Major feasibility tolerance", tol); + } + + std::cout << "Adding kinematic constraints: " << std::endl; + setKinematicConstraints(trajopt, plant); + std::cout << "Setting initial conditions: " << std::endl; + + if (!FLAGS_load_filename.empty()) { + std::cout << "Loading: " << FLAGS_load_filename << std::endl; + SetInitialGuessFromTrajectory(trajopt, plant, + FLAGS_data_directory + FLAGS_load_filename, + FLAGS_same_knotpoints); + } else { + prog.SetInitialGuessForAllVariables( + VectorXd::Random(prog.decision_variables().size())); + } + + // auto loaded_traj = + // DirconTrajectory(plant, FLAGS_data_directory + FLAGS_load_filename); + // + // std::vector> x_trajs; + // x_trajs.push_back(PiecewisePolynomial::CubicHermite( + // loaded_traj.GetStateBreaks(0), loaded_traj.GetStateSamples(0), + // loaded_traj.GetStateDerivativeSamples(0))); + // x_trajs.push_back(PiecewisePolynomial::CubicHermite( + // loaded_traj.GetStateBreaks(1), loaded_traj.GetStateSamples(1), + // loaded_traj.GetStateDerivativeSamples(1))); + // x_trajs.push_back(PiecewisePolynomial::CubicHermite( + // loaded_traj.GetStateBreaks(2), loaded_traj.GetStateSamples(2), + // loaded_traj.GetStateDerivativeSamples(2))); + + // To avoid NaN quaternions + for (int i = 0; i < trajopt.N(); i++) { + auto xi = trajopt.state(i); + if ((prog.GetInitialGuess(xi.head(4)).norm() == 0) || + std::isnan(prog.GetInitialGuess(xi.head(4)).norm())) { + prog.SetInitialGuess(xi(0), 1); + prog.SetInitialGuess(xi(1), 0); + prog.SetInitialGuess(xi(2), 0); + prog.SetInitialGuess(xi(3), 0); + } + } + + double alpha = .2; + int num_poses = std::min(FLAGS_knot_points, 10); + trajopt.CreateVisualizationCallback(file_name, num_poses, alpha); + + drake::solvers::SolverId solver_id(""); + if (FLAGS_ipopt) { + solver_id = drake::solvers::IpoptSolver().id(); + cout << "\nChose manually: " << solver_id.name() << endl; + } else { + solver_id = drake::solvers::ChooseBestSolver(prog); + cout << "\nChose the best solver: " << solver_id.name() << endl; + } + + cout << "Solving DIRCON\n\n"; + auto start = std::chrono::high_resolution_clock::now(); + const auto result = drake::solvers::Solve(prog, prog.initial_guess()); + // SolutionResult solution_result = result.get_solution_result(); + auto finish = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = finish - start; + cout << "Solve time:" << elapsed.count() << std::endl; + std::cout << "Cost:" << result.get_optimal_cost() << std::endl; + std::cout << "Solve result: " << result.get_solution_result() << std::endl; + + // Save trajectory to file + DirconTrajectory saved_traj(plant, trajopt, result, "running_trajectory", + "Decision variables and state/input trajectories " + "for walking"); + saved_traj.WriteToFile(FLAGS_data_directory + FLAGS_save_filename); + std::cout << "Wrote to file: " << FLAGS_data_directory + FLAGS_save_filename + << std::endl; + drake::trajectories::PiecewisePolynomial optimal_traj = + trajopt.ReconstructStateTrajectory(result); + multibody::ConnectTrajectoryVisualizer(&plant_vis, &builder, &scene_graph, + optimal_traj); + + DrakeVisualizer::AddToBuilder(&builder, scene_graph); + auto diagram = builder.Build(); + + // while (true) { + // drake::systems::Simulator simulator(*diagram); + // simulator.set_target_realtime_rate(0.1); + // simulator.Initialize(); + // simulator.AdvanceTo(optimal_traj.end_time()); + // } +} + +void setKinematicConstraints(Dircon& trajopt, + const MultibodyPlant& plant) { + auto& prog = trajopt.prog(); + + // Create maps for joints + map pos_map = multibody::MakeNameToPositionsMap(plant); + map vel_map = multibody::MakeNameToVelocitiesMap(plant); + map act_map = multibody::MakeNameToActuatorsMap(plant); + + int n_q = plant.num_positions(); + int n_v = plant.num_velocities(); + int n_u = plant.num_actuators(); + + // Get the decision variables that will be used + int N = trajopt.N(); + auto x = trajopt.state(); + auto u = trajopt.input(); + auto x0 = trajopt.initial_state(); + auto x_mid = trajopt.state(N / 2); + auto xf = trajopt.final_state(); + auto u0 = trajopt.input(0); + auto uf = trajopt.input(N - 1); + + // Standing constraints + double start_height = FLAGS_start_height; + + // position constraints + prog.AddBoundingBoxConstraint(-0.25, 0.25, x0(pos_map.at("base_x"))); + prog.AddLinearConstraint(x0(pos_map.at("base_x")) + FLAGS_stride_length == + xf(pos_map.at("base_x"))); + prog.AddBoundingBoxConstraint(start_height, start_height, + x0(pos_map.at("base_z"))); + // trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("base_y")) <= 0.05); + // trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("base_y")) >= -0.05); + // initial fb orientation constraint + VectorXd quat_identity(4); + quat_identity << 1, 0, 0, 0; + prog.AddBoundingBoxConstraint(quat_identity, quat_identity, x0.head(4)); + prog.AddBoundingBoxConstraint(quat_identity, quat_identity, x_mid.head(4)); + prog.AddBoundingBoxConstraint(quat_identity, quat_identity, xf.head(4)); + + // periodicity constraint + prog.AddLinearConstraint(x0(pos_map.at("base_y")) == + -xf(pos_map.at("base_y"))); + prog.AddLinearConstraint(x0(pos_map.at("base_z")) == + xf(pos_map.at("base_z"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wx")) == + xf(n_q + vel_map.at("base_wx"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wy")) == + -xf(n_q + vel_map.at("base_wy"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wz")) == + xf(n_q + vel_map.at("base_wz"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vx")) == + xf(n_q + vel_map.at("base_vx"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vy")) == + -xf(n_q + vel_map.at("base_vy"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vz")) == + xf(n_q + vel_map.at("base_vz"))); + + // create joint/motor names + vector> l_r_pairs{ + std::pair("_left", "_right"), + std::pair("_right", "_left"), + }; + vector asy_joint_names{"hip_roll", "hip_yaw"}; + vector sym_joint_names{"hip_pitch", "knee", "ankle_joint", "toe"}; + vector joint_names{}; + vector motor_names{}; + for (const auto& l_r_pair : l_r_pairs) { + for (const auto& asy_joint_name : asy_joint_names) { + joint_names.push_back(asy_joint_name + l_r_pair.first); + motor_names.push_back(asy_joint_name + l_r_pair.first + "_motor"); + } + for (const auto& sym_joint_name : sym_joint_names) { + joint_names.push_back(sym_joint_name + l_r_pair.first); + if (sym_joint_name != "ankle_joint") { + motor_names.push_back(sym_joint_name + l_r_pair.first + "_motor"); + } + } + } + + for (const auto& l_r_pair : l_r_pairs) { + // Symmetry constraints + for (const auto& sym_joint_name : sym_joint_names) { + prog.AddLinearConstraint( + x0(pos_map[sym_joint_name + l_r_pair.first]) == + xf(pos_map[sym_joint_name + l_r_pair.second])); + prog.AddLinearConstraint( + x0(n_q + vel_map.at(sym_joint_name + l_r_pair.first + "dot")) == + xf(n_q + vel_map.at(sym_joint_name + l_r_pair.second + "dot"))); + if (sym_joint_name != "ankle_joint") { // No actuator at ankle + prog.AddLinearConstraint( + u0(act_map.at(sym_joint_name + l_r_pair.first + "_motor")) == + uf(act_map.at(sym_joint_name + l_r_pair.second + "_motor"))); + } + } + // Asymmetry constraints + for (const auto& asy_joint_name : asy_joint_names) { + prog.AddLinearConstraint( + x0(pos_map[asy_joint_name + l_r_pair.first]) == + -xf(pos_map[asy_joint_name + l_r_pair.second])); + prog.AddLinearConstraint( + x0(n_q + vel_map.at(asy_joint_name + l_r_pair.first + "dot")) == + -xf(n_q + vel_map.at(asy_joint_name + l_r_pair.second + "dot"))); + if (asy_joint_name != "ankle_joint") { // No actuator at ankle + prog.AddLinearConstraint( + u0(act_map.at(asy_joint_name + l_r_pair.first + "_motor")) == + -uf(act_map.at(asy_joint_name + l_r_pair.second + "_motor"))); + } + } + } + + // joint limits + std::cout << "Joint limit constraints: " << std::endl; + for (const auto& member : joint_names) { + trajopt.AddConstraintToAllKnotPoints( + x(pos_map.at(member)) <= + plant.GetJointByName(member).position_upper_limits()(0)); + trajopt.AddConstraintToAllKnotPoints( + x(pos_map.at(member)) >= + plant.GetJointByName(member).position_lower_limits()(0)); + } + + // actuator limits + std::cout << "Actuator limit constraints: " << std::endl; + for (int i = 0; i < trajopt.N(); i++) { + auto ui = trajopt.input(i); + prog.AddBoundingBoxConstraint(VectorXd::Constant(n_u, -200), + VectorXd::Constant(n_u, +200), ui); + } + + std::cout << "Foot placement constraints: " << std::endl; + // toe position constraint in y direction (avoid leg crossing) + // tighter constraint than before + auto left_foot_y_constraint = + std::make_shared>( + plant, "toe_left", Vector3d::Zero(), Eigen::RowVector3d(0, 1, 0), + 0.10 * VectorXd::Ones(1), 0.25 * VectorXd::Ones(1)); + auto right_foot_y_constraint = + std::make_shared>( + plant, "toe_right", Vector3d::Zero(), Eigen::RowVector3d(0, 1, 0), + -0.25 * VectorXd::Ones(1), -0.1 * VectorXd::Ones(1)); + + for (int i = 0; i < N; ++i) { + auto x_i = trajopt.state(i); + prog.AddConstraint(left_foot_y_constraint, x_i.head(n_q)); + prog.AddConstraint(right_foot_y_constraint, x_i.head(n_q)); + } + + for (int mode = 0; mode < trajopt.num_modes(); ++mode) { + for (int index = 0; index < trajopt.mode_length(mode); index++) { + // Assumes mode_lengths are the same across modes + + auto lambda = trajopt.force_vars(mode, index); + if (mode == 0 || mode == 2) { + prog.AddLinearConstraint(lambda(2) >= 10); + prog.AddLinearConstraint(lambda(5) >= 10); + } + } + } + std::cout << "Stride length constraints: " << std::endl; + // stride length constraint + auto right_foot_x_constraint = + std::make_shared>( + plant, "toe_right", Vector3d::Zero(), Eigen::RowVector3d(1, 0, 0), + FLAGS_stride_length * VectorXd::Ones(1), + FLAGS_stride_length * VectorXd::Ones(1)); + auto left_foot_x_constraint = + std::make_shared>( + plant, "toe_left", Vector3d::Zero(), Eigen::RowVector3d(1, 0, 0), + 0 * VectorXd::Ones(1), 0 * VectorXd::Ones(1)); + prog.AddConstraint(left_foot_x_constraint, x0.head(n_q)); + prog.AddConstraint(right_foot_x_constraint, xf.head(n_q)); + + std::cout << "Foot clearance constraints: " << std::endl; + // Foot clearance constraint + auto left_foot_z_constraint_clearance = + std::make_shared>( + plant, "toe_left", Vector3d::Zero(), Eigen::RowVector3d(0, 0, 1), + 0.055 * VectorXd::Ones(1), (0.15) * VectorXd::Ones(1)); + auto right_foot_z_constraint_clearance = + std::make_shared>( + plant, "toe_right", Vector3d::Zero(), Eigen::RowVector3d(0, 0, 1), + 0.055 * VectorXd::Ones(1), (0.15) * VectorXd::Ones(1)); + for (int i = 0; i < N; i++) { + auto x_i = trajopt.state(i); + prog.AddConstraint(left_foot_z_constraint_clearance, x_i.head(n_q)); + prog.AddConstraint(right_foot_z_constraint_clearance, x_i.head(n_q)); + prog.AddBoundingBoxConstraint(-1.8, -1.5, x_i[pos_map["toe_left"]]); + prog.AddBoundingBoxConstraint(-1.8, -1.5, x_i[pos_map["toe_right"]]); + } + + std::cout << "Miscellaneous constraints" << std::endl; + // Miscellaneous constraints + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_yaw_left")) >= -0.01); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_yaw_left")) <= 0.01); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_yaw_right")) >= -0.01); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_yaw_right")) <= 0.01); + // Miscellaneous constraints + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_roll_left")) >= 0.0); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_roll_left")) <= 0.10); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_roll_right")) >= + -0.10); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_roll_right")) <= 0.0); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_pitch_left")) >= 0.50); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_pitch_left")) <= 0.90); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_pitch_right")) >= + 0.50); + trajopt.AddConstraintToAllKnotPoints(x(pos_map.at("hip_pitch_right")) <= + 0.90); + + std::cout << "Adding costs: " << std::endl; + double W = 1e-1; + MatrixXd Q = MatrixXd::Identity(n_v, n_v); + Q(7, 7) = 10; + Q(8, 8) = 10; + // MatrixXd R = 1e-3 * MatrixXd::Identity(n_u, n_u); + // R(8, 8) = 1; + // R(9, 9) = 1; + trajopt.AddRunningCost((x.tail(n_v).transpose() * Q * x.tail(n_v))); + // trajopt.AddRunningCost(u.transpose() * R * u); + solvers::AddPositiveWorkCost(trajopt, plant, W); + + // MatrixXd S = MatrixXd::Zero(n_u, n_v); + // S(0, 6) = 1; + // S(1, 7) = 1; + // S(2, 8) = 1; + // S(3, 9) = 1; + // S(4, 10) = 1; + // S(5, 11) = 1; + // S(6, 12) = 1; + // S(7, 13) = 1; + // S(8, 16) = 1; + // S(9, 17) = 1; + // const drake::symbolic::Expression e_max_{max(static_cast>( + // u.transpose() * S * x.tail(n_v) + u.transpose() * R * u), + // VectorXd::Zero(1))}; + // drake::symbolic::max(u.transpose() * S * x.tail(n_v), 0); + // trajopt.AddRunningCost(drake::symbolic::max(u.transpose() * S * + // x.tail(n_v), 0)); trajopt.AddRunningCost(u.transpose() * S * x.tail(n_v) + // + u.transpose() * R * u); +} + +void SetInitialGuessFromTrajectory(Dircon& trajopt, + const MultibodyPlant& plant, + const string& filepath, + bool same_knot_points) { + DirconTrajectory previous_traj = DirconTrajectory(plant, filepath); + if (same_knot_points) { + trajopt.prog().SetInitialGuessForAllVariables( + previous_traj.GetDecisionVariables()); + return; + } + auto state_traj = previous_traj.ReconstructStateTrajectory(); + auto input_traj = previous_traj.ReconstructInputTrajectory(); + auto lambda_traj = previous_traj.ReconstructLambdaTrajectory(); + auto lambda_c_traj = previous_traj.ReconstructLambdaCTrajectory(); + auto gamma_traj = previous_traj.ReconstructGammaCTrajectory(); + + trajopt.SetInitialTrajectory(input_traj, state_traj); + for (int mode = 0; mode < trajopt.num_modes() - 1; ++mode) { + if (trajopt.mode_length(mode) > 1) { + std::cout << "mode: " << mode << std::endl; + trajopt.SetInitialForceTrajectory(mode, lambda_traj[mode], + lambda_c_traj[mode], gamma_traj[mode]); + } + } +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + dairlib::DoMain(); +} \ No newline at end of file diff --git a/examples/Cassie/run_dircon_squatting.cc b/examples/Cassie/run_dircon_squatting.cc index f5d5e2a36b..d3b88bd889 100644 --- a/examples/Cassie/run_dircon_squatting.cc +++ b/examples/Cassie/run_dircon_squatting.cc @@ -104,8 +104,8 @@ void DoMain(double duration, int max_iter, const string& data_directory, string full_name = FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf"); - parser.AddModelFromFile(full_name); - parser_vis.AddModelFromFile(full_name); + parser.AddModels(full_name); + parser_vis.AddModels(full_name); plant.Finalize(); plant_vis.Finalize(); @@ -114,13 +114,13 @@ void DoMain(double duration, int max_iter, const string& data_directory, map velocities_map = multibody::MakeNameToVelocitiesMap(plant); map actuators_map = multibody::MakeNameToActuatorsMap(plant); - int base_qw_idx = positions_map.at("base_qw"); - int base_qx_idx = positions_map.at("base_qx"); - int base_qy_idx = positions_map.at("base_qy"); - int base_qz_idx = positions_map.at("base_qz"); - int base_x_idx = positions_map.at("base_x"); - int base_y_idx = positions_map.at("base_y"); - int base_z_idx = positions_map.at("base_z"); + int pelvis_qw_idx = positions_map.at("pelvis_qw"); + int pelvis_qx_idx = positions_map.at("pelvis_qx"); + int pelvis_qy_idx = positions_map.at("pelvis_qy"); + int pelvis_qz_idx = positions_map.at("pelvis_qz"); + int pelvis_x_idx = positions_map.at("pelvis_x"); + int pelvis_y_idx = positions_map.at("pelvis_y"); + int pelvis_z_idx = positions_map.at("pelvis_z"); int hip_roll_left_idx = positions_map.at("hip_roll_left"); int hip_roll_right_idx = positions_map.at("hip_roll_right"); int hip_yaw_left_idx = positions_map.at("hip_yaw_left"); @@ -134,12 +134,12 @@ void DoMain(double duration, int max_iter, const string& data_directory, int toe_left_idx = positions_map.at("toe_left"); int toe_right_idx = positions_map.at("toe_right"); - int base_wx_idx = velocities_map.at("base_wx"); - int base_wy_idx = velocities_map.at("base_wy"); - int base_wz_idx = velocities_map.at("base_wz"); - int base_vx_idx = velocities_map.at("base_vx"); - int base_vy_idx = velocities_map.at("base_vy"); - int base_vz_idx = velocities_map.at("base_vz"); + int pelvis_wx_idx = velocities_map.at("pelvis_wx"); + int pelvis_wy_idx = velocities_map.at("pelvis_wy"); + int pelvis_wz_idx = velocities_map.at("pelvis_wz"); + int pelvis_vx_idx = velocities_map.at("pelvis_vx"); + int pelvis_vy_idx = velocities_map.at("pelvis_vy"); + int pelvis_vz_idx = velocities_map.at("pelvis_vz"); int hip_roll_leftdot_idx = velocities_map.at("hip_roll_leftdot"); int hip_roll_rightdot_idx = velocities_map.at("hip_roll_rightdot"); int hip_yaw_leftdot_idx = velocities_map.at("hip_yaw_leftdot"); @@ -238,8 +238,8 @@ void DoMain(double duration, int max_iter, const string& data_directory, double s_dyn_2 = (FLAGS_scale_variable) ? 6.0 : 1.0; double s_dyn_3 = (FLAGS_scale_variable) ? 85.0 : 1.0; double_support.SetDynamicsScale( - {base_qw_idx, base_qx_idx, base_qy_idx, base_qz_idx, base_x_idx, - base_y_idx, base_z_idx, hip_roll_left_idx, hip_roll_right_idx, + {pelvis_qw_idx, pelvis_qx_idx, pelvis_qy_idx, pelvis_qz_idx, pelvis_x_idx, + pelvis_y_idx, pelvis_z_idx, hip_roll_left_idx, hip_roll_right_idx, hip_yaw_left_idx, hip_yaw_right_idx, hip_pitch_left_idx, hip_pitch_right_idx, knee_left_idx, knee_right_idx}, 1.0 / 150.0); @@ -248,8 +248,8 @@ void DoMain(double duration, int max_iter, const string& data_directory, 1.0 / 150.0 / 3.33 / s_dyn_1); double_support.SetDynamicsScale({toe_left_idx, toe_right_idx}, 1.0 / 150.0); double_support.SetDynamicsScale( - {base_wx_idx, base_wy_idx, base_wz_idx, base_vx_idx, base_vy_idx, - base_vz_idx, hip_roll_leftdot_idx, hip_roll_rightdot_idx}, + {pelvis_wx_idx, pelvis_wy_idx, pelvis_wz_idx, pelvis_vx_idx, pelvis_vy_idx, + pelvis_vz_idx, hip_roll_leftdot_idx, hip_roll_rightdot_idx}, 1.0 / 150.0 / s_dyn_1); double_support.SetDynamicsScale({hip_yaw_leftdot_idx, hip_yaw_rightdot_idx}, 1.0 / 150.0 / s_dyn_2); @@ -338,20 +338,20 @@ void DoMain(double duration, int max_iter, const string& data_directory, auto xmid = trajopt.state_vars(0, (num_knotpoints - 1) / 2); // height constraint - prog.AddBoundingBoxConstraint(1, 1, x0(positions_map.at("base_z"))); - prog.AddBoundingBoxConstraint(1.1, 1.1, xf(positions_map.at("base_z"))); + prog.AddBoundingBoxConstraint(1, 1, x0(positions_map.at("pelvis_z"))); + prog.AddBoundingBoxConstraint(1.1, 1.1, xf(positions_map.at("pelvis_z"))); // initial pelvis position - prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("base_x"))); - prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("base_y"))); + prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("pelvis_x"))); + prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("pelvis_y"))); // pelvis pose constraints for (int i = 0; i < num_knotpoints; i++) { auto xi = trajopt.state(i); - prog.AddBoundingBoxConstraint(1, 1, xi(positions_map.at("base_qw"))); - prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("base_qx"))); - prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("base_qy"))); - prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("base_qz"))); + prog.AddBoundingBoxConstraint(1, 1, xi(positions_map.at("pelvis_qw"))); + prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("pelvis_qx"))); + prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("pelvis_qy"))); + prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("pelvis_qz"))); } // start/end velocity constraints @@ -449,9 +449,9 @@ void DoMain(double duration, int max_iter, const string& data_directory, trajopt.ScaleTimeVariables(0.015); // state std::vector idx_list = { - n_q + base_wx_idx, n_q + base_wy_idx, - n_q + base_wz_idx, n_q + base_vx_idx, - n_q + base_vy_idx, n_q + base_vz_idx, + n_q + pelvis_wx_idx, n_q + pelvis_wy_idx, + n_q + pelvis_wz_idx, n_q + pelvis_vx_idx, + n_q + pelvis_vy_idx, n_q + pelvis_vz_idx, n_q + hip_roll_leftdot_idx, n_q + hip_roll_rightdot_idx, n_q + hip_yaw_leftdot_idx, n_q + hip_yaw_rightdot_idx}; trajopt.ScaleStateVariables(idx_list, 6); @@ -546,13 +546,13 @@ void DoMain(double duration, int max_iter, const string& data_directory, // produces NAN value in some calculation. for (int i = 0; i < num_knotpoints; i++) { auto xi = trajopt.state(i); - if ((prog.GetInitialGuess(xi.segment<4>(base_qw_idx)).norm() == 0) || + if ((prog.GetInitialGuess(xi.segment<4>(pelvis_qw_idx)).norm() == 0) || std::isnan( - prog.GetInitialGuess(xi.segment<4>(base_qw_idx)).norm())) { - prog.SetInitialGuess(xi(base_qw_idx), 1); - prog.SetInitialGuess(xi(base_qx_idx), 0); - prog.SetInitialGuess(xi(base_qy_idx), 0); - prog.SetInitialGuess(xi(base_qz_idx), 0); + prog.GetInitialGuess(xi.segment<4>(pelvis_qw_idx)).norm())) { + prog.SetInitialGuess(xi(pelvis_qw_idx), 1); + prog.SetInitialGuess(xi(pelvis_qx_idx), 0); + prog.SetInitialGuess(xi(pelvis_qy_idx), 0); + prog.SetInitialGuess(xi(pelvis_qz_idx), 0); } } diff --git a/examples/Cassie/run_dircon_walking.cc b/examples/Cassie/run_dircon_walking.cc index ba3b2b4450..2c0c6ee77b 100644 --- a/examples/Cassie/run_dircon_walking.cc +++ b/examples/Cassie/run_dircon_walking.cc @@ -119,13 +119,13 @@ vector GetInitGuessForQ(int N, double stride_length, map pos_value_map; Eigen::Vector4d quat(2000.06, -0.339462, -0.609533, -0.760854); quat.normalize(); - pos_value_map["base_qw"] = quat(0); - pos_value_map["base_qx"] = quat(1); - pos_value_map["base_qy"] = quat(2); - pos_value_map["base_qz"] = quat(3); - pos_value_map["base_x"] = 0.000889849; - pos_value_map["base_y"] = 0.000626865; - pos_value_map["base_z"] = 1.0009; + pos_value_map["pelvis_qw"] = quat(0); + pos_value_map["pelvis_qx"] = quat(1); + pos_value_map["pelvis_qy"] = quat(2); + pos_value_map["pelvis_qz"] = quat(3); + pos_value_map["pelvis_x"] = 0.000889849; + pos_value_map["pelvis_y"] = 0.000626865; + pos_value_map["pelvis_z"] = 1.0009; pos_value_map["hip_roll_left"] = -0.0112109; pos_value_map["hip_roll_right"] = 0.00927845; pos_value_map["hip_yaw_left"] = -0.000600725; @@ -206,7 +206,7 @@ vector GetInitGuessForQ(int N, double stride_length, Parser parser(&plant_ik, &scene_graph_ik); string full_name = FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf"); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant_ik.Finalize(); // Visualize @@ -298,7 +298,7 @@ void DoMain(double duration, double stride_length, double ground_incline, string full_name = FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf"); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.Finalize(); // Create maps for joints @@ -411,13 +411,13 @@ void DoMain(double duration, double stride_length, double ground_incline, options_list[i].setConstraintRelative(3, true); } - int base_qw_idx = pos_map.at("base_qw"); - int base_qx_idx = pos_map.at("base_qx"); - int base_qy_idx = pos_map.at("base_qy"); - int base_qz_idx = pos_map.at("base_qz"); - int base_x_idx = pos_map.at("base_x"); - int base_y_idx = pos_map.at("base_y"); - int base_z_idx = pos_map.at("base_z"); + int pelvis_qw_idx = pos_map.at("pelvis_qw"); + int pelvis_qx_idx = pos_map.at("pelvis_qx"); + int pelvis_qy_idx = pos_map.at("pelvis_qy"); + int pelvis_qz_idx = pos_map.at("pelvis_qz"); + int pelvis_x_idx = pos_map.at("pelvis_x"); + int pelvis_y_idx = pos_map.at("pelvis_y"); + int pelvis_z_idx = pos_map.at("pelvis_z"); int hip_roll_left_idx = pos_map.at("hip_roll_left"); int hip_roll_right_idx = pos_map.at("hip_roll_right"); int hip_yaw_left_idx = pos_map.at("hip_yaw_left"); @@ -431,12 +431,12 @@ void DoMain(double duration, double stride_length, double ground_incline, int toe_left_idx = pos_map.at("toe_left"); int toe_right_idx = pos_map.at("toe_right"); - int base_wx_idx = vel_map.at("base_wx"); - int base_wy_idx = vel_map.at("base_wy"); - int base_wz_idx = vel_map.at("base_wz"); - int base_vx_idx = vel_map.at("base_vx"); - int base_vy_idx = vel_map.at("base_vy"); - int base_vz_idx = vel_map.at("base_vz"); + int pelvis_wx_idx = vel_map.at("pelvis_wx"); + int pelvis_wy_idx = vel_map.at("pelvis_wy"); + int pelvis_wz_idx = vel_map.at("pelvis_wz"); + int pelvis_vx_idx = vel_map.at("pelvis_vx"); + int pelvis_vy_idx = vel_map.at("pelvis_vy"); + int pelvis_vz_idx = vel_map.at("pelvis_vz"); int hip_roll_leftdot_idx = vel_map.at("hip_roll_leftdot"); int hip_roll_rightdot_idx = vel_map.at("hip_roll_rightdot"); int hip_yaw_leftdot_idx = vel_map.at("hip_yaw_leftdot"); @@ -467,9 +467,9 @@ void DoMain(double duration, double stride_length, double ground_incline, double s = 1; // scale everything together // Dynamic constraints options_list[i].setDynConstraintScaling( - {base_qw_idx, base_qx_idx, base_qy_idx, base_qz_idx}, s * 1.0 / 30.0); + {pelvis_qw_idx, pelvis_qx_idx, pelvis_qy_idx, pelvis_qz_idx}, s * 1.0 / 30.0); options_list[i].setDynConstraintScaling( - {base_x_idx, base_y_idx, base_z_idx, hip_roll_left_idx, + {pelvis_x_idx, pelvis_y_idx, pelvis_z_idx, hip_roll_left_idx, hip_roll_right_idx, hip_yaw_left_idx, hip_yaw_right_idx, hip_pitch_left_idx, hip_pitch_right_idx, knee_left_idx, knee_right_idx, ankle_joint_left_idx, ankle_joint_right_idx}, @@ -477,8 +477,8 @@ void DoMain(double duration, double stride_length, double ground_incline, options_list[i].setDynConstraintScaling({toe_left_idx, toe_right_idx}, s * 1.0 / 300.0); options_list[i].setDynConstraintScaling( - {n_q + base_wx_idx, n_q + base_wy_idx, n_q + base_wz_idx, - n_q + base_vx_idx, n_q + base_vy_idx, n_q + base_vz_idx, + {n_q + pelvis_wx_idx, n_q + pelvis_wy_idx, n_q + pelvis_wz_idx, + n_q + pelvis_vx_idx, n_q + pelvis_vy_idx, n_q + pelvis_vz_idx, n_q + hip_roll_leftdot_idx, n_q + hip_roll_rightdot_idx, n_q + hip_yaw_leftdot_idx, n_q + hip_yaw_rightdot_idx}, s * 1.0 / 600.0); @@ -503,9 +503,9 @@ void DoMain(double duration, double stride_length, double ground_incline, s * 20); // Impact constraints options_list[i].setImpConstraintScaling( - {base_wx_idx, base_wy_idx, base_wz_idx}, s / 50.0); + {pelvis_wx_idx, pelvis_wy_idx, pelvis_wz_idx}, s / 50.0); options_list[i].setImpConstraintScaling( - {base_vx_idx, base_vy_idx, base_vz_idx}, s / 300.0); + {pelvis_vx_idx, pelvis_vy_idx, pelvis_vz_idx}, s / 300.0); options_list[i].setImpConstraintScaling( {hip_roll_leftdot_idx, hip_roll_rightdot_idx}, s / 24.0); options_list[i].setImpConstraintScaling( @@ -577,23 +577,23 @@ void DoMain(double duration, double stride_length, double ground_incline, } // x position constraint - prog.AddBoundingBoxConstraint(0, 0, x0(pos_map.at("base_x"))); + prog.AddBoundingBoxConstraint(0, 0, x0(pos_map.at("pelvis_x"))); prog.AddBoundingBoxConstraint(stride_length, stride_length, - xf(pos_map.at("base_x"))); + xf(pos_map.at("pelvis_x"))); // height constraint - // prog.AddLinearConstraint(x0(pos_map.at("base_z")) == 1); - // prog.AddLinearConstraint(xf(pos_map.at("base_z")) == 1.1); + // prog.AddLinearConstraint(x0(pos_map.at("pelvis_z")) == 1); + // prog.AddLinearConstraint(xf(pos_map.at("pelvis_z")) == 1.1); // initial pelvis position - // prog.AddLinearConstraint(x0(pos_map.at("base_y")) == 0); + // prog.AddLinearConstraint(x0(pos_map.at("pelvis_y")) == 0); // pelvis pose constraints - // prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qw")) == - // 1); prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qx")) + // prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qw")) == + // 1); prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qx")) // == 0); - // prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qy")) == - // 0); prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qz")) + // prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qy")) == + // 0); prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qz")) // == 0); // start/end velocity constraints @@ -601,32 +601,32 @@ void DoMain(double duration, double stride_length, double ground_incline, // prog.AddLinearConstraint(xf.tail(n_v) == VectorXd::Zero(n_v)); // Floating base periodicity - prog.AddLinearConstraint(x0(pos_map.at("base_qw")) == - xf(pos_map.at("base_qw"))); - prog.AddLinearConstraint(x0(pos_map.at("base_qx")) == - -xf(pos_map.at("base_qx"))); - prog.AddLinearConstraint(x0(pos_map.at("base_qy")) == - xf(pos_map.at("base_qy"))); - prog.AddLinearConstraint(x0(pos_map.at("base_qz")) == - -xf(pos_map.at("base_qz"))); - prog.AddLinearConstraint(x0(pos_map.at("base_y")) == - -xf(pos_map.at("base_y"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qw")) == + xf(pos_map.at("pelvis_qw"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qx")) == + -xf(pos_map.at("pelvis_qx"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qy")) == + xf(pos_map.at("pelvis_qy"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qz")) == + -xf(pos_map.at("pelvis_qz"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_y")) == + -xf(pos_map.at("pelvis_y"))); if (ground_incline == 0) { - prog.AddLinearConstraint(x0(pos_map.at("base_z")) == - xf(pos_map.at("base_z"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_z")) == + xf(pos_map.at("pelvis_z"))); } - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wx")) == - xf(n_q + vel_map.at("base_wx"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wy")) == - -xf(n_q + vel_map.at("base_wy"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wz")) == - xf(n_q + vel_map.at("base_wz"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vx")) == - xf(n_q + vel_map.at("base_vx"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vy")) == - -xf(n_q + vel_map.at("base_vy"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vz")) == - xf(n_q + vel_map.at("base_vz"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_wx")) == + xf(n_q + vel_map.at("pelvis_wx"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_wy")) == + -xf(n_q + vel_map.at("pelvis_wy"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_wz")) == + xf(n_q + vel_map.at("pelvis_wz"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_vx")) == + xf(n_q + vel_map.at("pelvis_vx"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_vy")) == + -xf(n_q + vel_map.at("pelvis_vy"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_vz")) == + xf(n_q + vel_map.at("pelvis_vz"))); // The legs joint positions/velocities/torque should be mirrored between legs // (notice that hip yaw and roll should be asymmetric instead of symmetric.) @@ -719,7 +719,7 @@ void DoMain(double duration, double stride_length, double ground_incline, prog.AddConstraint(right_foot_constraint_z, x_mid.head(n_q)); // Optional -- constraint on initial floating base - prog.AddConstraint(x0(pos_map.at("base_qw")) == 1); + prog.AddConstraint(x0(pos_map.at("pelvis_qw")) == 1); // Optional -- constraint on the forces magnitude /*for (unsigned int mode = 0; mode < num_time_samples.size(); mode++) { for (int index = 0; index < num_time_samples[mode]; index++) { @@ -774,7 +774,7 @@ void DoMain(double duration, double stride_length, double ground_incline, trajopt->ScaleTimeVariables(0.008); // state trajopt->ScaleStateVariables( - {base_qw_idx, base_qx_idx, base_qy_idx, base_qz_idx}, 0.5); + {pelvis_qw_idx, pelvis_qx_idx, pelvis_qy_idx, pelvis_qz_idx}, 0.5); if (s_q_toe > 1) { trajopt->ScaleStateVariables({toe_left_idx, toe_right_idx}, s_q_toe); } @@ -882,7 +882,7 @@ void DoMain(double duration, double stride_length, double ground_incline, } if (w_q_quat_xyz) { for (int i = 0; i < N; i++) { - auto q = trajopt->state(i).segment(base_qx_idx, 3); + auto q = trajopt->state(i).segment(pelvis_qx_idx, 3); prog.AddCost(w_q_quat_xyz * q.transpose() * q); } } @@ -953,23 +953,23 @@ void DoMain(double duration, double stride_length, double ground_incline, // initial condition for post impact velocity auto vp_var = trajopt->v_post_impact_vars_by_mode(0); prog.SetInitialGuess( - vp_var(vel_map.at("base_wx")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_wx")))); + vp_var(vel_map.at("pelvis_wx")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_wx")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_wy")), - -prog.GetInitialGuess(x0(n_q + vel_map.at("base_wy")))); + vp_var(vel_map.at("pelvis_wy")), + -prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_wy")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_wz")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_wz")))); + vp_var(vel_map.at("pelvis_wz")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_wz")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_vx")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_vx")))); + vp_var(vel_map.at("pelvis_vx")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_vx")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_vy")), - -prog.GetInitialGuess(x0(n_q + vel_map.at("base_vy")))); + vp_var(vel_map.at("pelvis_vy")), + -prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_vy")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_vz")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_vz")))); + vp_var(vel_map.at("pelvis_vz")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_vz")))); for (auto l_r_pair : l_r_pairs) { for (unsigned int i = 0; i < asy_joint_names.size(); i++) { // velocities @@ -994,13 +994,13 @@ void DoMain(double duration, double stride_length, double ground_incline, // produces NAN value in some calculation. for (int i = 0; i < N; i++) { auto xi = trajopt->state(i); - if ((prog.GetInitialGuess(xi.segment<4>(base_qw_idx)).norm() == + if ((prog.GetInitialGuess(xi.segment<4>(pelvis_qw_idx)).norm() == 0) || std::isnan(prog.GetInitialGuess( - xi.segment<4>(base_qw_idx)).norm())) { - prog.SetInitialGuess(xi(pos_map.at("base_qw")), 1); - prog.SetInitialGuess(xi(pos_map.at("base_qx")), 0); - prog.SetInitialGuess(xi(pos_map.at("base_qy")), 0); - prog.SetInitialGuess(xi(pos_map.at("base_qz")), 0); + xi.segment<4>(pelvis_qw_idx)).norm())) { + prog.SetInitialGuess(xi(pos_map.at("pelvis_qw")), 1); + prog.SetInitialGuess(xi(pos_map.at("pelvis_qx")), 0); + prog.SetInitialGuess(xi(pos_map.at("pelvis_qy")), 0); + prog.SetInitialGuess(xi(pos_map.at("pelvis_qz")), 0); } } @@ -1200,7 +1200,7 @@ void DoMain(double duration, double stride_length, double ground_incline, // add cost on quaternion double cost_q_quat_xyz = 0; for (int i = 0; i < N; i++) { - auto q = result.GetSolution(trajopt->state(i).segment(base_qx_idx, 3)); + auto q = result.GetSolution(trajopt->state(i).segment(pelvis_qx_idx, 3)); cost_q_quat_xyz += w_q_quat_xyz * q.transpose() * q; } total_cost += cost_q_quat_xyz; diff --git a/examples/Cassie/run_osc_jumping_controller.cc b/examples/Cassie/run_osc_jumping_controller.cc index 8e39aa5b6e..2c795988fc 100644 --- a/examples/Cassie/run_osc_jumping_controller.cc +++ b/examples/Cassie/run_osc_jumping_controller.cc @@ -36,6 +36,7 @@ using std::map; using std::pair; using std::string; using std::vector; +using std::unique_ptr; using Eigen::Matrix3d; using Eigen::MatrixXd; @@ -46,6 +47,7 @@ using drake::geometry::SceneGraph; using drake::multibody::Frame; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; +using drake::systems::Diagram; using drake::systems::DiagramBuilder; using drake::systems::TriggerType; using drake::systems::TriggerTypeSet; @@ -266,7 +268,7 @@ int DoMain(int argc, char* argv[]) { auto command_sender = builder.AddSystem(plant_w_spr); auto osc = builder.AddSystem( - plant_w_spr, plant_w_spr, context_w_spr.get(), context_w_spr.get(), true); + plant_w_spr, context_w_spr.get(), true); auto osc_debug_pub = builder.AddSystem(LcmPublisherSystem::Make( "OSC_DEBUG_JUMPING", &lcm, TriggerTypeSet({TriggerType::kForced}))); @@ -325,14 +327,29 @@ int DoMain(int argc, char* argv[]) { auto right_heel_evaluator = multibody::WorldPointEvaluator( plant_w_spr, right_heel.first, right_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); - vector stance_modes = { + vector stance_modes = { osc_jump::BALANCE, osc_jump::CROUCH, osc_jump::LAND}; - for (auto mode : stance_modes) { - osc->AddStateAndContactPoint(mode, &left_toe_evaluator); - osc->AddStateAndContactPoint(mode, &left_heel_evaluator); - osc->AddStateAndContactPoint(mode, &right_toe_evaluator); - osc->AddStateAndContactPoint(mode, &right_heel_evaluator); - } + + osc->AddContactPoint( + "left_toe", + unique_ptr>(&left_toe_evaluator), + stance_modes + ); + osc->AddContactPoint( + "left_heel", + unique_ptr>(&left_heel_evaluator), + stance_modes + ); + osc->AddContactPoint( + "right_toe", + unique_ptr>(&right_toe_evaluator), + stance_modes + ); + osc->AddContactPoint( + "right_heel", + unique_ptr>(&right_heel_evaluator), + stance_modes + ); multibody::KinematicEvaluatorSet evaluators(plant_w_spr); @@ -361,7 +378,8 @@ int DoMain(int argc, char* argv[]) { evaluators.add_evaluator(&left_loop); evaluators.add_evaluator(&right_loop); - osc->AddKinematicConstraint(&evaluators); + osc->AddKinematicConstraint( + std::unique_ptr>(&evaluators)); /**** Tracking Data for OSC *****/ auto pelvis_tracking_data = std::make_unique( @@ -549,11 +567,12 @@ int DoMain(int argc, char* argv[]) { // Run lcm-driven simulation // Create the diagram auto owned_diagram = builder.Build(); - owned_diagram->set_name(("osc_jumping_controller")); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("osc_jumping_controller")); // Run lcm-driven simulation systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), state_receiver, FLAGS_channel_x, true); + &lcm, shared_diagram, state_receiver, FLAGS_channel_x, true); DrawAndSaveDiagramGraph(*loop.get_diagram()); loop.Simulate(); diff --git a/examples/Cassie/run_osc_running_controller.cc b/examples/Cassie/run_osc_running_controller.cc index 287d4a782c..0574b81405 100644 --- a/examples/Cassie/run_osc_running_controller.cc +++ b/examples/Cassie/run_osc_running_controller.cc @@ -40,6 +40,7 @@ using std::map; using std::pair; using std::string; using std::vector; +using std::unique_ptr; using Eigen::Matrix3d; using Eigen::MatrixXd; @@ -51,6 +52,7 @@ using drake::geometry::SceneGraph; using drake::multibody::Frame; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; +using drake::systems::Diagram; using drake::systems::DiagramBuilder; using drake::systems::TriggerType; using drake::systems::TriggerTypeSet; @@ -172,7 +174,7 @@ int DoMain(int argc, char* argv[]) { FLAGS_channel_u, &lcm, TriggerTypeSet({TriggerType::kForced}))); auto command_sender = builder.AddSystem(plant); auto osc = builder.AddSystem( - plant, plant, plant_context.get(), plant_context.get(), true); + plant, plant_context.get(), true); auto osc_debug_pub = builder.AddSystem(LcmPublisherSystem::Make( "OSC_DEBUG_RUNNING", &lcm, TriggerTypeSet({TriggerType::kForced}))); @@ -193,15 +195,13 @@ int DoMain(int argc, char* argv[]) { /**** OSC setup ****/ // Cost /// REGULARIZATION COSTS - osc->SetAccelerationCostWeights(osc_gains.w_accel * osc_gains.W_acceleration); - osc->SetInputSmoothingCostWeights(osc_gains.w_input_reg * - osc_gains.W_input_regularization); - osc->SetInputCostWeights(osc_gains.w_input * - osc_gains.W_input_regularization); + osc->SetAccelerationCostWeights(osc_gains.W_acceleration); + osc->SetInputSmoothingCostWeights(osc_gains.W_input_regularization); + osc->SetInputCostWeights(osc_gains.W_input_smoothing_regularization); osc->SetLambdaContactRegularizationWeight( - osc_gains.w_lambda * osc_gains.W_lambda_c_regularization); + osc_gains.W_lambda_c_regularization); osc->SetLambdaHolonomicRegularizationWeight( - osc_gains.w_lambda * osc_gains.W_lambda_h_regularization); + osc_gains.W_lambda_h_regularization); // Soft constraint on contacts osc->SetContactSoftConstraintWeight(osc_gains.w_soft_constraint); osc->SetJointLimitWeight(osc_gains.w_joint_limit); @@ -224,18 +224,29 @@ int DoMain(int argc, char* argv[]) { plant, right_heel.first, right_heel.second, *pelvis_view_frame, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); - osc->AddStateAndContactPoint(RunningFsmState::kLeftStance, - &left_toe_evaluator); - osc->AddStateAndContactPoint(RunningFsmState::kLeftStance, - &left_heel_evaluator); - osc->AddStateAndContactPoint(RunningFsmState::kRightStance, - &right_toe_evaluator); - osc->AddStateAndContactPoint(RunningFsmState::kRightStance, - &right_heel_evaluator); + osc->AddContactPoint( + "left_toe", + unique_ptr>(&left_toe_evaluator), + {RunningFsmState::kLeftStance} + ); + osc->AddContactPoint( + "left_heel", + unique_ptr>(&left_heel_evaluator), + {RunningFsmState::kLeftStance} + ); + osc->AddContactPoint( + "right_toe", + unique_ptr>(&right_toe_evaluator), + {RunningFsmState::kRightStance} + ); + osc->AddContactPoint( + "right_heel", + unique_ptr>(&right_heel_evaluator), + {RunningFsmState::kRightStance} + ); multibody::KinematicEvaluatorSet evaluators(plant); - // Fix the springs in the dynamics auto pos_idx_map = multibody::MakeNameToPositionsMap(plant); auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant); @@ -261,7 +272,8 @@ int DoMain(int argc, char* argv[]) { evaluators.add_evaluator(&left_loop); evaluators.add_evaluator(&right_loop); - osc->AddKinematicConstraint(&evaluators); + osc->AddKinematicConstraint( + unique_ptr>(&evaluators)); /**** Tracking Data *****/ @@ -610,10 +622,11 @@ int DoMain(int argc, char* argv[]) { contact_scheduler_debug_publisher->get_input_port()); auto owned_diagram = builder.Build(); - owned_diagram->set_name(("osc_running_controller")); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("osc_running_controller")); systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), state_receiver, FLAGS_channel_x, true); + &lcm, shared_diagram, state_receiver, FLAGS_channel_x, true); DrawAndSaveDiagramGraph(*loop.get_diagram()); loop.Simulate(); diff --git a/examples/Cassie/run_osc_standing_controller.cc b/examples/Cassie/run_osc_standing_controller.cc index c953253575..cedc6a2bfb 100644 --- a/examples/Cassie/run_osc_standing_controller.cc +++ b/examples/Cassie/run_osc_standing_controller.cc @@ -8,6 +8,7 @@ #include "examples/Cassie/osc/standing_com_traj.h" #include "examples/Cassie/osc/standing_pelvis_orientation_traj.h" #include "examples/Cassie/systems/cassie_out_to_radio.h" +#include "multibody/kinematic/fixed_joint_evaluator.h" #include "multibody/kinematic/kinematic_evaluator_set.h" #include "multibody/multibody_utils.h" #include "systems/controllers/osc/com_tracking_data.h" @@ -28,6 +29,7 @@ namespace dairlib { using std::cout; using std::endl; +using std::unique_ptr; using Eigen::Matrix3d; using Eigen::MatrixXd; @@ -74,28 +76,21 @@ int DoMain(int argc, char* argv[]) { "examples/Cassie/urdf/cassie_v2.urdf", true /*spring model*/, false /*loop closure*/); plant_w_springs.Finalize(); - // Build fix-spring Cassie MBP - drake::multibody::MultibodyPlant plant_wo_springs(0.0); - AddCassieMultibody(&plant_wo_springs, nullptr, true, - "examples/Cassie/urdf/cassie_fixed_springs.urdf", false, - false); - plant_wo_springs.Finalize(); auto context_w_spr = plant_w_springs.CreateDefaultContext(); - auto context_wo_spr = plant_wo_springs.CreateDefaultContext(); // Get contact frames and position (doesn't matter whether we use - // plant_w_springs or plant_wo_springs because the contact frames exit in both + // plant_w_springs or plant_w_springs because the contact frames exit in both // plants) - auto left_toe = LeftToeFront(plant_wo_springs); - auto left_heel = LeftToeRear(plant_wo_springs); - auto right_toe = RightToeFront(plant_wo_springs); - auto right_heel = RightToeRear(plant_wo_springs); + auto left_toe = LeftToeFront(plant_w_springs); + auto left_heel = LeftToeRear(plant_w_springs); + auto right_toe = RightToeFront(plant_w_springs); + auto right_heel = RightToeRear(plant_w_springs); // Build the controller diagram DiagramBuilder builder; - drake::lcm::DrakeLcm lcm_local; + drake::lcm::DrakeLcm lcm_local("udpm://239.255.76.67:7667?ttl=0"); drake::yaml::LoadYamlOptions yaml_options; yaml_options.allow_yaml_with_no_cpp = true; OSCGains gains = drake::yaml::LoadYamlFile( @@ -157,16 +152,36 @@ int DoMain(int argc, char* argv[]) { // Create Operational space control auto osc = builder.AddSystem( - plant_w_springs, plant_wo_springs, context_w_spr.get(), - context_wo_spr.get(), false); + plant_w_springs, context_w_spr.get(), false); // Distance constraint - multibody::KinematicEvaluatorSet evaluators(plant_wo_springs); - auto left_loop = LeftLoopClosureEvaluator(plant_wo_springs); - auto right_loop = RightLoopClosureEvaluator(plant_wo_springs); + multibody::KinematicEvaluatorSet evaluators(plant_w_springs); + auto left_loop = LeftLoopClosureEvaluator(plant_w_springs); + auto right_loop = RightLoopClosureEvaluator(plant_w_springs); evaluators.add_evaluator(&left_loop); evaluators.add_evaluator(&right_loop); - osc->AddKinematicConstraint(&evaluators); + + auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_springs); + auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_springs); + auto left_fixed_knee_spring = multibody::FixedJointEvaluator( + plant_w_springs, pos_idx_map.at("knee_joint_left"), + vel_idx_map.at("knee_joint_leftdot"), 0); + auto right_fixed_knee_spring = multibody::FixedJointEvaluator( + plant_w_springs, pos_idx_map.at("knee_joint_right"), + vel_idx_map.at("knee_joint_rightdot"), 0); + auto left_fixed_ankle_spring = multibody::FixedJointEvaluator( + plant_w_springs, pos_idx_map.at("ankle_spring_joint_left"), + vel_idx_map.at("ankle_spring_joint_leftdot"), 0); + auto right_fixed_ankle_spring = multibody::FixedJointEvaluator( + plant_w_springs, pos_idx_map.at("ankle_spring_joint_right"), + vel_idx_map.at("ankle_spring_joint_rightdot"), 0); + evaluators.add_evaluator(&left_fixed_knee_spring); + evaluators.add_evaluator(&right_fixed_knee_spring); + evaluators.add_evaluator(&left_fixed_ankle_spring); + evaluators.add_evaluator(&right_fixed_ankle_spring); + + osc->AddKinematicConstraint( + std::unique_ptr>(&evaluators)); // Soft constraint // We don't want w_contact_relax to be too big, cause we want tracking // error to be important @@ -177,34 +192,43 @@ int DoMain(int argc, char* argv[]) { osc->SetContactFriction(mu); // Add contact points auto left_toe_evaluator = multibody::WorldPointEvaluator( - plant_wo_springs, left_toe.first, left_toe.second, Matrix3d::Identity(), + plant_w_springs, left_toe.first, left_toe.second, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); - osc->AddContactPoint(&left_toe_evaluator); auto left_heel_evaluator = multibody::WorldPointEvaluator( - plant_wo_springs, left_heel.first, left_heel.second, Matrix3d::Identity(), + plant_w_springs, left_heel.first, left_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); - osc->AddContactPoint(&left_heel_evaluator); auto right_toe_evaluator = multibody::WorldPointEvaluator( - plant_wo_springs, right_toe.first, right_toe.second, Matrix3d::Identity(), + plant_w_springs, right_toe.first, right_toe.second, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); - osc->AddContactPoint(&right_toe_evaluator); auto right_heel_evaluator = multibody::WorldPointEvaluator( - plant_wo_springs, right_heel.first, right_heel.second, + plant_w_springs, right_heel.first, right_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); - osc->AddContactPoint(&right_heel_evaluator); + + osc->AddContactPoint( + "left_toe", + unique_ptr>(&left_toe_evaluator)); + osc->AddContactPoint( + "left_heel", + unique_ptr>(&left_heel_evaluator)); + osc->AddContactPoint( + "right_toe", + unique_ptr>(&right_toe_evaluator)); + osc->AddContactPoint("right_heel", + unique_ptr>( + &right_heel_evaluator)); // Cost - int n_v = plant_wo_springs.num_velocities(); + int n_v = plant_w_springs.num_velocities(); MatrixXd Q_accel = gains.w_accel * MatrixXd::Identity(n_v, n_v); osc->SetAccelerationCostWeights(Q_accel); // Center of mass tracking // Weighting x-y higher than z, as they are more important to balancing // ComTrackingData center_of_mass_traj("com_traj", K_p_com, K_d_com, // W_com * FLAGS_cost_weight_multiplier, - // plant_w_springs, plant_wo_springs); + // plant_w_springs, plant_w_springs); auto center_of_mass_traj = std::make_unique( "com_traj", osc_gains.K_p_pelvis, osc_gains.K_d_pelvis, osc_gains.W_pelvis * FLAGS_cost_weight_multiplier, plant_w_springs, - plant_wo_springs); + plant_w_springs); center_of_mass_traj->AddPointToTrack("pelvis"); double cutoff_freq = 5; // in Hz double tau = 1 / (2 * M_PI * cutoff_freq); @@ -213,7 +237,7 @@ int DoMain(int argc, char* argv[]) { auto pelvis_rot_tracking_data = std::make_unique( "pelvis_rot_traj", osc_gains.K_p_pelvis_rot, osc_gains.K_d_pelvis_rot, osc_gains.W_pelvis_rot * FLAGS_cost_weight_multiplier, plant_w_springs, - plant_wo_springs); + plant_w_springs); pelvis_rot_tracking_data->AddFrameToTrack("pelvis"); osc->AddTrackingData(std::move(pelvis_rot_tracking_data)); @@ -227,11 +251,11 @@ int DoMain(int argc, char* argv[]) { auto left_hip_yaw_traj = std::make_unique( "left_hip_yaw_traj", hip_yaw_kp * MatrixXd::Ones(1, 1), hip_yaw_kd * MatrixXd::Ones(1, 1), w_hip_yaw * MatrixXd::Ones(1, 1), - plant_w_springs, plant_wo_springs); + plant_w_springs, plant_w_springs); auto right_hip_yaw_traj = std::make_unique( "right_hip_yaw_traj", hip_yaw_kp * MatrixXd::Ones(1, 1), hip_yaw_kd * MatrixXd::Ones(1, 1), w_hip_yaw * MatrixXd::Ones(1, 1), - plant_w_springs, plant_wo_springs); + plant_w_springs, plant_w_springs); left_hip_yaw_traj->AddJointToTrack("hip_yaw_left", "hip_yaw_leftdot"); osc->AddConstTrackingData(std::move(left_hip_yaw_traj), VectorXd::Zero(1)); right_hip_yaw_traj->AddJointToTrack("hip_yaw_right", "hip_yaw_rightdot"); @@ -267,4 +291,4 @@ int DoMain(int argc, char* argv[]) { } // namespace dairlib -int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/Cassie/run_osc_walking_controller.cc b/examples/Cassie/run_osc_walking_controller.cc index c0bfd4d236..a697961e30 100644 --- a/examples/Cassie/run_osc_walking_controller.cc +++ b/examples/Cassie/run_osc_walking_controller.cc @@ -5,12 +5,10 @@ #include "examples/Cassie/cassie_utils.h" #include "examples/Cassie/osc/heading_traj_generator.h" #include "examples/Cassie/osc/high_level_command.h" -#include "examples/Cassie/osc/hip_yaw_traj_gen.h" #include "examples/Cassie/osc/osc_walking_gains.h" #include "examples/Cassie/osc/swing_toe_traj_generator.h" #include "examples/Cassie/osc/walking_speed_control.h" #include "examples/Cassie/systems/cassie_out_to_radio.h" -#include "examples/Cassie/systems/simulator_drift.h" #include "multibody/kinematic/fixed_joint_evaluator.h" #include "multibody/kinematic/kinematic_evaluator_set.h" #include "multibody/multibody_utils.h" @@ -28,6 +26,7 @@ #include "systems/controllers/time_based_fsm.h" #include "systems/framework/lcm_driven_loop.h" #include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" #include "drake/common/yaml/yaml_io.h" #include "drake/systems/framework/diagram_builder.h" @@ -47,6 +46,7 @@ using Eigen::Vector3d; using Eigen::VectorXd; using drake::multibody::Frame; +using drake::systems::Diagram; using drake::systems::DiagramBuilder; using drake::systems::TriggerType; using drake::systems::TriggerTypeSet; @@ -78,6 +78,8 @@ DEFINE_string( "The name of the channel to receive the cassie out structure from."); DEFINE_string(gains_filename, "examples/Cassie/osc/osc_walking_gains.yaml", "Filepath containing gains"); +DEFINE_string(osqp_settings, "solvers/default_osc_osqp_settings.yaml", + "Filepath containing qp settings"); DEFINE_bool(publish_osc_data, true, "whether to publish lcm messages for OscTrackData"); @@ -96,15 +98,9 @@ int DoMain(int argc, char* argv[]) { // Build Cassie MBP drake::multibody::MultibodyPlant plant_w_spr(0.0); - if (FLAGS_spring_model) { - AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, + AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, "examples/Cassie/urdf/cassie_v2.urdf", true /*spring model*/, false /*loop closure*/); - } else { - AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, - "examples/Cassie/urdf/cassie_fixed_springs.urdf", - false /*spring model*/, false /*loop closure*/); - } plant_w_spr.Finalize(); auto context_w_spr = plant_w_spr.CreateDefaultContext(); @@ -158,11 +154,6 @@ int DoMain(int argc, char* argv[]) { // Note that we didn't add drift to yaw angle here because it requires // changing SimulatorDrift. - auto simulator_drift = - builder.AddSystem(plant_w_spr, drift_mean, drift_cov); - builder.Connect(state_receiver->get_output_port(0), - simulator_drift->get_input_port_state()); - // Create human high-level control Eigen::Vector2d global_target_position(gains.global_target_position_x, gains.global_target_position_y); @@ -197,7 +188,7 @@ int DoMain(int argc, char* argv[]) { // Create heading traj generator auto head_traj_gen = builder.AddSystem( plant_w_spr, context_w_spr.get()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), head_traj_gen->get_state_input_port()); builder.Connect(high_level_command->get_output_port_yaw(), head_traj_gen->get_yaw_input_port()); @@ -212,18 +203,15 @@ int DoMain(int argc, char* argv[]) { double double_support_duration = gains.ds_time; vector fsm_states; vector state_durations; - if (FLAGS_is_two_phase) { - fsm_states = {left_stance_state, right_stance_state}; - state_durations = {left_support_duration, right_support_duration}; - } else { - fsm_states = {left_stance_state, post_left_double_support_state, + + fsm_states = {left_stance_state, post_left_double_support_state, right_stance_state, post_right_double_support_state}; - state_durations = {left_support_duration, double_support_duration, + state_durations = {left_support_duration, double_support_duration, right_support_duration, double_support_duration}; - } + auto fsm = builder.AddSystem( plant_w_spr, fsm_states, state_durations); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), fsm->get_state_input_port()); // Create leafsystem that record the switching time of the FSM @@ -235,7 +223,7 @@ int DoMain(int argc, char* argv[]) { liftoff_event_time->set_name("liftoff_time"); builder.Connect(fsm->get_output_port(0), liftoff_event_time->get_input_port_fsm()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), liftoff_event_time->get_input_port_state()); std::vector double_support_states = {post_left_double_support_state, post_right_double_support_state}; @@ -245,7 +233,7 @@ int DoMain(int argc, char* argv[]) { touchdown_event_time->set_name("touchdown_time"); builder.Connect(fsm->get_output_port(0), touchdown_event_time->get_input_port_fsm()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), touchdown_event_time->get_input_port_state()); // Create CoM trajectory generator @@ -256,23 +244,18 @@ int DoMain(int argc, char* argv[]) { vector unordered_state_durations; vector&>>> contact_points_in_each_state; - if (FLAGS_is_two_phase) { - unordered_fsm_states = {left_stance_state, right_stance_state}; - unordered_state_durations = {left_support_duration, right_support_duration}; - contact_points_in_each_state.push_back({left_toe_mid}); - contact_points_in_each_state.push_back({right_toe_mid}); - } else { - unordered_fsm_states = {left_stance_state, right_stance_state, + + unordered_fsm_states = {left_stance_state, right_stance_state, post_left_double_support_state, post_right_double_support_state}; - unordered_state_durations = {left_support_duration, right_support_duration, + unordered_state_durations = {left_support_duration, right_support_duration, double_support_duration, double_support_duration}; - contact_points_in_each_state.push_back({left_toe_mid}); - contact_points_in_each_state.push_back({right_toe_mid}); - contact_points_in_each_state.push_back({left_toe_mid}); - contact_points_in_each_state.push_back({right_toe_mid}); - } + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + auto lipm_traj_generator = builder.AddSystem( plant_w_spr, context_w_spr.get(), desired_com_height, unordered_fsm_states, unordered_state_durations, @@ -281,7 +264,7 @@ int DoMain(int argc, char* argv[]) { lipm_traj_generator->get_input_port_fsm()); builder.Connect(touchdown_event_time->get_output_port_event_time(), lipm_traj_generator->get_input_port_touchdown_time()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), lipm_traj_generator->get_input_port_state()); // We can use the same desired_com_height for pelvis_traj_generator as we use @@ -297,7 +280,7 @@ int DoMain(int argc, char* argv[]) { pelvis_traj_generator->get_input_port_fsm()); builder.Connect(touchdown_event_time->get_output_port_event_time(), pelvis_traj_generator->get_input_port_touchdown_time()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), pelvis_traj_generator->get_input_port_state()); // Create velocity control by foot placement @@ -309,7 +292,7 @@ int DoMain(int argc, char* argv[]) { left_support_duration, 0, wrt_com_in_local_frame); builder.Connect(high_level_command->get_output_port_xy(), walking_speed_control->get_input_port_des_hor_vel()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), walking_speed_control->get_input_port_state()); builder.Connect(lipm_traj_generator->get_output_port_lipm_from_current(), walking_speed_control->get_input_port_com()); @@ -341,7 +324,7 @@ int DoMain(int argc, char* argv[]) { swing_ft_traj_generator->get_input_port_fsm()); builder.Connect(liftoff_event_time->get_output_port_event_time_of_interest(), swing_ft_traj_generator->get_input_port_fsm_switch_time()); - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), swing_ft_traj_generator->get_input_port_state()); builder.Connect(lipm_traj_generator->get_output_port_lipm_from_current(), swing_ft_traj_generator->get_input_port_com()); @@ -369,7 +352,7 @@ int DoMain(int argc, char* argv[]) { // Create Operational space control auto osc = builder.AddSystem( - plant_w_spr, plant_w_spr, context_w_spr.get(), context_w_spr.get(), true); + plant_w_spr, context_w_spr.get(), true); // Cost int n_v = plant_w_spr.num_velocities(); @@ -393,27 +376,26 @@ int DoMain(int argc, char* argv[]) { std::unique_ptr> right_fixed_knee_spring; std::unique_ptr> left_fixed_ankle_spring; std::unique_ptr> right_fixed_ankle_spring; - if (FLAGS_spring_model) { - auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_spr); - auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); - left_fixed_knee_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("knee_joint_left"), - vel_idx_map.at("knee_joint_leftdot"), 0); - right_fixed_knee_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("knee_joint_right"), - vel_idx_map.at("knee_joint_rightdot"), 0); - left_fixed_ankle_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_left"), - vel_idx_map.at("ankle_spring_joint_leftdot"), 0); - right_fixed_ankle_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_right"), - vel_idx_map.at("ankle_spring_joint_rightdot"), 0); - evaluators.add_evaluator(left_fixed_knee_spring.get()); - evaluators.add_evaluator(right_fixed_knee_spring.get()); - evaluators.add_evaluator(left_fixed_ankle_spring.get()); - evaluators.add_evaluator(right_fixed_ankle_spring.get()); - } - osc->AddKinematicConstraint(&evaluators); + auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_spr); + auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); + left_fixed_knee_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("knee_joint_left"), + vel_idx_map.at("knee_joint_leftdot"), 0); + right_fixed_knee_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("knee_joint_right"), + vel_idx_map.at("knee_joint_rightdot"), 0); + left_fixed_ankle_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("ankle_spring_joint_left"), + vel_idx_map.at("ankle_spring_joint_leftdot"), 0); + right_fixed_ankle_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("ankle_spring_joint_right"), + vel_idx_map.at("ankle_spring_joint_rightdot"), 0); + evaluators.add_evaluator(left_fixed_knee_spring.get()); + evaluators.add_evaluator(right_fixed_knee_spring.get()); + evaluators.add_evaluator(left_fixed_ankle_spring.get()); + evaluators.add_evaluator(right_fixed_ankle_spring.get()); + osc->AddKinematicConstraint( + std::unique_ptr>(&evaluators)); // Soft constraint // w_contact_relax shouldn't be too big, cause we want tracking error to be @@ -422,8 +404,6 @@ int DoMain(int argc, char* argv[]) { // Friction coefficient osc->SetContactFriction(gains.mu); // Add contact points (The position doesn't matter. It's not used in OSC) - // Note: it's important to express the contact point acceleration in the local - // frame to avoid bad numerics in the OSC const auto& pelvis = plant_w_spr.GetBodyByName("pelvis"); multibody::WorldYawViewFrame view_frame(pelvis); auto left_toe_evaluator = multibody::WorldPointEvaluator( @@ -438,28 +418,23 @@ int DoMain(int argc, char* argv[]) { auto right_heel_evaluator = multibody::WorldPointEvaluator( plant_w_spr, right_heel.first, right_heel.second, view_frame, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); - osc->AddStateAndContactPoint(left_stance_state, &left_toe_evaluator); - osc->AddStateAndContactPoint(left_stance_state, &left_heel_evaluator); - osc->AddStateAndContactPoint(right_stance_state, &right_toe_evaluator); - osc->AddStateAndContactPoint(right_stance_state, &right_heel_evaluator); - if (!FLAGS_is_two_phase) { - osc->AddStateAndContactPoint(post_left_double_support_state, - &left_toe_evaluator); - osc->AddStateAndContactPoint(post_left_double_support_state, - &left_heel_evaluator); - osc->AddStateAndContactPoint(post_left_double_support_state, - &right_toe_evaluator); - osc->AddStateAndContactPoint(post_left_double_support_state, - &right_heel_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &left_toe_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &left_heel_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &right_toe_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &right_heel_evaluator); - } + + osc->AddContactPoint( + "left_toe", + std::unique_ptr>(&left_toe_evaluator), + {left_stance_state, post_left_double_support_state, post_right_double_support_state}); + osc->AddContactPoint( + "left_heel", + std::unique_ptr>(&left_heel_evaluator), + {left_stance_state, post_left_double_support_state, post_right_double_support_state}); + osc->AddContactPoint( + "right_toe", + std::unique_ptr>(&right_toe_evaluator), + {right_stance_state, post_left_double_support_state, post_right_double_support_state}); + osc->AddContactPoint( + "right_heel", + std::unique_ptr>(&right_heel_evaluator), + {right_stance_state, post_left_double_support_state, post_right_double_support_state}); // Swing foot tracking std::vector swing_ft_gain_multiplier_breaks{ @@ -486,20 +461,21 @@ int DoMain(int argc, char* argv[]) { swing_ft_accel_gain_multiplier_breaks, swing_ft_accel_gain_multiplier_samples)); - TransTaskSpaceTrackingData swing_foot_data( + + auto swing_foot_data = std::make_unique( "swing_ft_data", gains.K_p_swing_foot, gains.K_d_swing_foot, gains.W_swing_foot, plant_w_spr, plant_w_spr); - swing_foot_data.AddStateAndPointToTrack(left_stance_state, "toe_right"); - swing_foot_data.AddStateAndPointToTrack(right_stance_state, "toe_left"); - ComTrackingData com_data("com_data", gains.K_p_swing_foot, - gains.K_d_swing_foot, gains.W_swing_foot, - plant_w_spr, plant_w_spr); - com_data.AddFiniteStateToTrack(left_stance_state); - com_data.AddFiniteStateToTrack(right_stance_state); + swing_foot_data->AddStateAndPointToTrack(left_stance_state, "toe_right"); + swing_foot_data->AddStateAndPointToTrack(right_stance_state, "toe_left"); + auto com_data = std::make_unique( + "com_data", gains.K_p_swing_foot, gains.K_d_swing_foot, + gains.W_swing_foot, plant_w_spr, plant_w_spr); + com_data->AddFiniteStateToTrack(left_stance_state); + com_data->AddFiniteStateToTrack(right_stance_state); auto swing_ft_traj_local = std::make_unique( "swing_ft_traj", gains.K_p_swing_foot, gains.K_d_swing_foot, - gains.W_swing_foot, plant_w_spr, plant_w_spr, &swing_foot_data, - &com_data); + gains.W_swing_foot, plant_w_spr, plant_w_spr, swing_foot_data.get(), + com_data.get()); auto pelvis_view_frame = std::make_shared>(plant_w_spr.GetBodyByName("pelvis")); swing_ft_traj_local->SetViewFrame(pelvis_view_frame); @@ -570,9 +546,6 @@ int DoMain(int argc, char* argv[]) { osc->AddTrackingData(std::move(swing_toe_traj_left)); osc->AddTrackingData(std::move(swing_toe_traj_right)); - auto hip_yaw_traj_gen = - builder.AddSystem(left_stance_state); - // Swing hip yaw joint tracking auto swing_hip_yaw_traj = std::make_unique( "swing_hip_yaw_traj", gains.K_p_hip_yaw, gains.K_d_hip_yaw, @@ -581,16 +554,7 @@ int DoMain(int argc, char* argv[]) { left_stance_state, "hip_yaw_right", "hip_yaw_rightdot"); swing_hip_yaw_traj->AddStateAndJointToTrack( right_stance_state, "hip_yaw_left", "hip_yaw_leftdot"); - - if (FLAGS_use_radio) { - builder.Connect(cassie_out_to_radio->get_output_port(), - hip_yaw_traj_gen->get_radio_input_port()); - builder.Connect(fsm->get_fsm_output_port(), - hip_yaw_traj_gen->get_fsm_input_port()); - osc->AddTrackingData(std::move(swing_hip_yaw_traj)); - } else { - osc->AddConstTrackingData(std::move(swing_hip_yaw_traj), VectorXd::Zero(1)); - } + osc->AddConstTrackingData(std::move(swing_hip_yaw_traj), VectorXd::Zero(1)); // Set double support duration for force blending osc->SetUpDoubleSupportPhaseBlending( @@ -603,7 +567,7 @@ int DoMain(int argc, char* argv[]) { // Build OSC problem osc->Build(); // Connect ports - builder.Connect(simulator_drift->get_output_port(0), + builder.Connect(state_receiver->get_output_port(0), osc->get_input_port_robot_output()); builder.Connect(fsm->get_output_port(0), osc->get_input_port_fsm()); if (use_pelvis_for_lipm_tracking) { @@ -625,10 +589,6 @@ int DoMain(int argc, char* argv[]) { builder.Connect(right_toe_angle_traj_gen->get_output_port(0), osc->get_input_port_tracking_data("right_toe_angle_traj")); builder.Connect(osc->get_output_port(0), command_sender->get_input_port(0)); - if (FLAGS_use_radio) { - builder.Connect(hip_yaw_traj_gen->get_hip_yaw_output_port(), - osc->get_input_port_tracking_data("swing_hip_yaw_traj")); - } if (FLAGS_publish_osc_data) { // Create osc debug sender. auto osc_debug_pub = @@ -641,11 +601,13 @@ int DoMain(int argc, char* argv[]) { // Create the diagram auto owned_diagram = builder.Build(); - owned_diagram->set_name("osc walking controller"); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name("osc walking controller"); // Run lcm-driven simulation + DrawAndSaveDiagramGraph(*shared_diagram); systems::LcmDrivenLoop loop( - &lcm_local, std::move(owned_diagram), state_receiver, FLAGS_channel_x, + &lcm_local, shared_diagram, state_receiver, FLAGS_channel_x, true); loop.Simulate(); diff --git a/examples/Cassie/run_osc_walking_controller_alip.cc b/examples/Cassie/run_osc_walking_controller_alip.cc index 6b64069eaf..f00522991b 100644 --- a/examples/Cassie/run_osc_walking_controller_alip.cc +++ b/examples/Cassie/run_osc_walking_controller_alip.cc @@ -3,18 +3,19 @@ #include "dairlib/lcmt_robot_input.hpp" #include "dairlib/lcmt_robot_output.hpp" #include "examples/Cassie/cassie_utils.h" -#include "examples/Cassie/systems/simulator_drift.h" -#include "examples/Cassie/osc/hip_yaw_traj_gen.h" #include "examples/Cassie/osc/heading_traj_generator.h" #include "examples/Cassie/osc/high_level_command.h" +#include "examples/Cassie/osc/hip_yaw_traj_gen.h" #include "examples/Cassie/osc/osc_walking_gains_alip.h" #include "examples/Cassie/osc/swing_toe_traj_generator.h" #include "examples/Cassie/systems/cassie_out_to_radio.h" +#include "examples/Cassie/systems/simulator_drift.h" #include "multibody/kinematic/fixed_joint_evaluator.h" #include "multibody/kinematic/kinematic_evaluator_set.h" #include "multibody/multibody_utils.h" -#include "systems/controllers/fsm_event_time.h" +#include "systems/controllers/alip_swing_ft_traj_gen.h" #include "systems/controllers/alip_traj_gen.h" +#include "systems/controllers/fsm_event_time.h" #include "systems/controllers/osc/com_tracking_data.h" #include "systems/controllers/osc/joint_space_tracking_data.h" #include "systems/controllers/osc/operational_space_control.h" @@ -22,10 +23,9 @@ #include "systems/controllers/osc/relative_translation_tracking_data.h" #include "systems/controllers/osc/rot_space_tracking_data.h" #include "systems/controllers/osc/trans_space_tracking_data.h" -#include "systems/controllers/alip_swing_ft_traj_gen.h" #include "systems/controllers/time_based_fsm.h" -#include "systems/framework/lcm_driven_loop.h" #include "systems/filters/floating_base_velocity_filter.h" +#include "systems/framework/lcm_driven_loop.h" #include "systems/robot_lcm_systems.h" #include "drake/common/yaml/yaml_io.h" @@ -49,10 +49,10 @@ using Eigen::VectorXd; using drake::multibody::Frame; using drake::systems::DiagramBuilder; using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; using drake::systems::lcm::LcmPublisherSystem; -using drake::systems::lcm::LcmSubscriberSystem; using drake::systems::lcm::LcmScopeSystem; -using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmSubscriberSystem; using multibody::WorldYawViewFrame; using systems::controllers::ComTrackingData; @@ -62,8 +62,8 @@ using systems::controllers::RotTaskSpaceTrackingData; using systems::controllers::TransTaskSpaceTrackingData; using multibody::FixedJointEvaluator; -using multibody::MakeNameToVelocitiesMap; using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; using drake::trajectories::PiecewisePolynomial; @@ -83,13 +83,8 @@ DEFINE_string(gains_filename, "examples/Cassie/osc/osc_walking_gains_alip.yaml", "Filepath containing gains"); DEFINE_bool(publish_osc_data, true, "whether to publish lcm messages for OscTrackData"); - -DEFINE_bool(is_two_phase, false, - "true: only right/left single support" - "false: both double and single support"); DEFINE_double(qp_time_limit, 0.002, "maximum qp solve time"); -DEFINE_bool(spring_model, true, ""); DEFINE_bool(publish_filtered_state, false, "whether to publish the low pass filtered state"); @@ -97,19 +92,14 @@ int DoMain(int argc, char* argv[]) { gflags::ParseCommandLineFlags(&argc, &argv, true); // Read-in the parameters - auto gains = drake::yaml::LoadYamlFile(FLAGS_gains_filename); + auto gains = + drake::yaml::LoadYamlFile(FLAGS_gains_filename); // Build Cassie MBP drake::multibody::MultibodyPlant plant_w_spr(0.0); - if (FLAGS_spring_model) { - AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, - "examples/Cassie/urdf/cassie_v2.urdf", - true /*spring model*/, false /*loop closure*/); - } else { - AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, - "examples/Cassie/urdf/cassie_fixed_springs.urdf", - false /*spring model*/, false /*loop closure*/); - } + AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, + "examples/Cassie/urdf/cassie_v2.urdf", + true /*spring model*/, false /*loop closure*/); plant_w_spr.Finalize(); auto context_w_spr = plant_w_spr.CreateDefaultContext(); @@ -128,7 +118,8 @@ int DoMain(int argc, char* argv[]) { auto right_heel = RightToeRear(plant_w_spr); // Get body frames and points - Vector3d center_of_pressure = left_heel.first + + Vector3d center_of_pressure = + left_heel.first + gains.contact_point_pos * (left_toe.first - left_heel.first); auto left_toe_mid = std::pair&>( center_of_pressure, plant_w_spr.GetFrameByName("toe_left")); @@ -142,17 +133,17 @@ int DoMain(int argc, char* argv[]) { // Create state receiver. auto state_receiver = builder.AddSystem(plant_w_spr); - auto pelvis_filt = - builder.AddSystem( - plant_w_spr, gains.pelvis_xyz_vel_filter_tau); + auto pelvis_filt = builder.AddSystem( + plant_w_spr, gains.pelvis_xyz_vel_filter_tau); builder.Connect(*state_receiver, *pelvis_filt); if (FLAGS_publish_filtered_state) { - auto [filtered_state_scope, filtered_state_sender]= - // AddToBuilder will add the systems to the diagram and connect their ports - LcmScopeSystem::AddToBuilder( - &builder, &lcm_local,pelvis_filt->get_output_port(), - "CASSIE_STATE_FB_FILTERED", 0); + auto [filtered_state_scope, filtered_state_sender] = + // AddToBuilder will add the systems to the diagram and connect their + // ports + LcmScopeSystem::AddToBuilder(&builder, &lcm_local, + pelvis_filt->get_output_port(), + "CASSIE_STATE_FB_FILTERED", 0); } // Create command sender. @@ -181,8 +172,7 @@ int DoMain(int argc, char* argv[]) { builder.Connect(pelvis_filt->get_output_port(0), simulator_drift->get_input_port_state()); - auto cassie_out_to_radio = - builder.AddSystem(); + auto cassie_out_to_radio = builder.AddSystem(); // Create human high-level control Eigen::Vector2d global_target_position(gains.global_target_position_x, @@ -229,15 +219,12 @@ int DoMain(int argc, char* argv[]) { double double_support_duration = gains.ds_time; vector fsm_states; vector state_durations; - if (FLAGS_is_two_phase) { - fsm_states = {left_stance_state, right_stance_state}; - state_durations = {left_support_duration, right_support_duration}; - } else { - fsm_states = {left_stance_state, post_left_double_support_state, - right_stance_state, post_right_double_support_state}; - state_durations = {left_support_duration, double_support_duration, - right_support_duration, double_support_duration}; - } + + fsm_states = {left_stance_state, post_left_double_support_state, + right_stance_state, post_right_double_support_state}; + state_durations = {left_support_duration, double_support_duration, + right_support_duration, double_support_duration}; + auto fsm = builder.AddSystem( plant_w_spr, fsm_states, state_durations); builder.Connect(simulator_drift->get_output_port(0), @@ -273,23 +260,18 @@ int DoMain(int argc, char* argv[]) { vector unordered_state_durations; vector&>>> contact_points_in_each_state; - if (FLAGS_is_two_phase) { - unordered_fsm_states = {left_stance_state, right_stance_state}; - unordered_state_durations = {left_support_duration, right_support_duration}; - contact_points_in_each_state.push_back({left_toe_mid}); - contact_points_in_each_state.push_back({right_toe_mid}); - } else { - unordered_fsm_states = {left_stance_state, right_stance_state, - post_left_double_support_state, - post_right_double_support_state}; - unordered_state_durations = {left_support_duration, right_support_duration, - double_support_duration, - double_support_duration}; - contact_points_in_each_state.push_back({left_toe_mid}); - contact_points_in_each_state.push_back({right_toe_mid}); - contact_points_in_each_state.push_back({left_toe_mid}); - contact_points_in_each_state.push_back({right_toe_mid}); - } + + unordered_fsm_states = {left_stance_state, right_stance_state, + post_left_double_support_state, + post_right_double_support_state}; + unordered_state_durations = {left_support_duration, right_support_duration, + double_support_duration, + double_support_duration}; + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + contact_points_in_each_state.push_back({left_toe_mid}); + contact_points_in_each_state.push_back({right_toe_mid}); + auto alip_traj_generator = builder.AddSystem( plant_w_spr, context_w_spr.get(), desired_com_height, unordered_fsm_states, unordered_state_durations, @@ -356,14 +338,15 @@ int DoMain(int argc, char* argv[]) { // Create Operational space control auto osc = builder.AddSystem( - plant_w_spr, plant_w_spr, context_w_spr.get(), context_w_spr.get(), true); + plant_w_spr, context_w_spr.get(), true); // Cost int n_v = plant_w_spr.num_velocities(); int n_u = plant_w_spr.num_actuators(); MatrixXd Q_accel = gains.w_accel * MatrixXd::Identity(n_v, n_v); osc->SetAccelerationCostWeights(Q_accel); - osc->SetInputSmoothingCostWeights(gains.w_input_reg * MatrixXd::Identity(n_u, n_u)); + osc->SetInputSmoothingCostWeights(gains.w_input_reg * + MatrixXd::Identity(n_u, n_u)); // Constraints in OSC multibody::KinematicEvaluatorSet evaluators(plant_w_spr); @@ -379,27 +362,26 @@ int DoMain(int argc, char* argv[]) { std::unique_ptr> right_fixed_knee_spring; std::unique_ptr> left_fixed_ankle_spring; std::unique_ptr> right_fixed_ankle_spring; - if (FLAGS_spring_model) { - auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_spr); - auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); - left_fixed_knee_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("knee_joint_left"), - vel_idx_map.at("knee_joint_leftdot"), 0); - right_fixed_knee_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("knee_joint_right"), - vel_idx_map.at("knee_joint_rightdot"), 0); - left_fixed_ankle_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_left"), - vel_idx_map.at("ankle_spring_joint_leftdot"), 0); - right_fixed_ankle_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_right"), - vel_idx_map.at("ankle_spring_joint_rightdot"), 0); - evaluators.add_evaluator(left_fixed_knee_spring.get()); - evaluators.add_evaluator(right_fixed_knee_spring.get()); - evaluators.add_evaluator(left_fixed_ankle_spring.get()); - evaluators.add_evaluator(right_fixed_ankle_spring.get()); - } - osc->AddKinematicConstraint(&evaluators); + auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_spr); + auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); + left_fixed_knee_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("knee_joint_left"), + vel_idx_map.at("knee_joint_leftdot"), 0); + right_fixed_knee_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("knee_joint_right"), + vel_idx_map.at("knee_joint_rightdot"), 0); + left_fixed_ankle_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("ankle_spring_joint_left"), + vel_idx_map.at("ankle_spring_joint_leftdot"), 0); + right_fixed_ankle_spring = std::make_unique>( + plant_w_spr, pos_idx_map.at("ankle_spring_joint_right"), + vel_idx_map.at("ankle_spring_joint_rightdot"), 0); + evaluators.add_evaluator(left_fixed_knee_spring.get()); + evaluators.add_evaluator(right_fixed_knee_spring.get()); + evaluators.add_evaluator(left_fixed_ankle_spring.get()); + evaluators.add_evaluator(right_fixed_ankle_spring.get()); + osc->AddKinematicConstraint( + std::unique_ptr>(&evaluators)); // Soft constraint // w_contact_relax shouldn't be too big, cause we want tracking error to be @@ -422,28 +404,27 @@ int DoMain(int argc, char* argv[]) { auto right_heel_evaluator = multibody::WorldPointEvaluator( plant_w_spr, right_heel.first, right_heel.second, view_frame, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); - osc->AddStateAndContactPoint(left_stance_state, &left_toe_evaluator); - osc->AddStateAndContactPoint(left_stance_state, &left_heel_evaluator); - osc->AddStateAndContactPoint(right_stance_state, &right_toe_evaluator); - osc->AddStateAndContactPoint(right_stance_state, &right_heel_evaluator); - if (!FLAGS_is_two_phase) { - osc->AddStateAndContactPoint(post_left_double_support_state, - &left_toe_evaluator); - osc->AddStateAndContactPoint(post_left_double_support_state, - &left_heel_evaluator); - osc->AddStateAndContactPoint(post_left_double_support_state, - &right_toe_evaluator); - osc->AddStateAndContactPoint(post_left_double_support_state, - &right_heel_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &left_toe_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &left_heel_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &right_toe_evaluator); - osc->AddStateAndContactPoint(post_right_double_support_state, - &right_heel_evaluator); - } + + osc->AddContactPoint("left_toe", + std::unique_ptr>( + &left_toe_evaluator), + {left_stance_state, post_left_double_support_state, + post_right_double_support_state}); + osc->AddContactPoint("left_heel", + std::unique_ptr>( + &left_heel_evaluator), + {left_stance_state, post_left_double_support_state, + post_right_double_support_state}); + osc->AddContactPoint("right_toe", + std::unique_ptr>( + &right_toe_evaluator), + {right_stance_state, post_left_double_support_state, + post_right_double_support_state}); + osc->AddContactPoint("right_heel", + std::unique_ptr>( + &right_heel_evaluator), + {right_stance_state, post_left_double_support_state, + post_right_double_support_state}); // Swing foot tracking std::vector swing_ft_gain_multiplier_breaks{ @@ -471,8 +452,7 @@ int DoMain(int argc, char* argv[]) { swing_ft_accel_gain_multiplier_breaks, swing_ft_accel_gain_multiplier_samples)); - - auto swing_foot_data = std::make_unique ( + auto swing_foot_data = std::make_unique( "swing_ft_data", gains.K_p_swing_foot, gains.K_d_swing_foot, gains.W_swing_foot, plant_w_spr, plant_w_spr); swing_foot_data->AddStateAndPointToTrack(left_stance_state, "toe_right"); @@ -480,12 +460,12 @@ int DoMain(int argc, char* argv[]) { auto vel_map = MakeNameToVelocitiesMap(plant_w_spr); - auto com_data = std::make_unique ("com_data", gains.K_p_swing_foot, - gains.K_d_swing_foot, gains.W_swing_foot, - plant_w_spr, plant_w_spr); + auto com_data = std::make_unique( + "com_data", gains.K_p_swing_foot, gains.K_d_swing_foot, + gains.W_swing_foot, plant_w_spr, plant_w_spr); com_data->AddFiniteStateToTrack(left_stance_state); com_data->AddFiniteStateToTrack(right_stance_state); - auto swing_ft_traj_local = std::make_unique ( + auto swing_ft_traj_local = std::make_unique( "swing_ft_traj", gains.K_p_swing_foot, gains.K_d_swing_foot, gains.W_swing_foot, plant_w_spr, plant_w_spr, swing_foot_data.get(), com_data.get()); @@ -499,21 +479,21 @@ int DoMain(int argc, char* argv[]) { swing_ft_accel_gain_multiplier_gain_multiplier); osc->AddTrackingData(std::move(swing_ft_traj_local)); - auto center_of_mass_traj = std::make_unique ("alip_com_traj", gains.K_p_com, - gains.K_d_com, gains.W_com, plant_w_spr, - plant_w_spr); + auto center_of_mass_traj = std::make_unique( + "alip_com_traj", gains.K_p_com, gains.K_d_com, gains.W_com, plant_w_spr, + plant_w_spr); // FiniteStatesToTrack cannot be empty center_of_mass_traj->AddFiniteStateToTrack(-1); osc->AddTrackingData(std::move(center_of_mass_traj)); // Pelvis rotation tracking (pitch and roll) - auto pelvis_balance_traj = std::make_unique ( + auto pelvis_balance_traj = std::make_unique( "pelvis_balance_traj", gains.K_p_pelvis_balance, gains.K_d_pelvis_balance, gains.W_pelvis_balance, plant_w_spr, plant_w_spr); pelvis_balance_traj->AddFrameToTrack("pelvis"); osc->AddTrackingData(std::move(pelvis_balance_traj)); // Pelvis rotation tracking (yaw) - auto pelvis_heading_traj = std::make_unique ( + auto pelvis_heading_traj = std::make_unique( "pelvis_heading_traj", gains.K_p_pelvis_heading, gains.K_d_pelvis_heading, gains.W_pelvis_heading, plant_w_spr, plant_w_spr); pelvis_heading_traj->AddFrameToTrack("pelvis"); @@ -521,10 +501,10 @@ int DoMain(int argc, char* argv[]) { gains.period_of_no_heading_control); // Swing toe joint tracking - auto swing_toe_traj_left = std::make_unique ( + auto swing_toe_traj_left = std::make_unique( "left_toe_angle_traj", gains.K_p_swing_toe, gains.K_d_swing_toe, gains.W_swing_toe, plant_w_spr, plant_w_spr); - auto swing_toe_traj_right = std::make_unique ( + auto swing_toe_traj_right = std::make_unique( "right_toe_angle_traj", gains.K_p_swing_toe, gains.K_d_swing_toe, gains.W_swing_toe, plant_w_spr, plant_w_spr); swing_toe_traj_right->AddStateAndJointToTrack(left_stance_state, "toe_right", @@ -534,18 +514,17 @@ int DoMain(int argc, char* argv[]) { osc->AddTrackingData(std::move(swing_toe_traj_left)); osc->AddTrackingData(std::move(swing_toe_traj_right)); - auto hip_yaw_traj_gen = builder.AddSystem(left_stance_state); // Swing hip yaw joint tracking - auto swing_hip_yaw_traj = std::make_unique ( + auto swing_hip_yaw_traj = std::make_unique( "swing_hip_yaw_traj", gains.K_p_hip_yaw, gains.K_d_hip_yaw, gains.W_hip_yaw, plant_w_spr, plant_w_spr); - swing_hip_yaw_traj->AddStateAndJointToTrack(left_stance_state, "hip_yaw_right", - "hip_yaw_rightdot"); - swing_hip_yaw_traj->AddStateAndJointToTrack(right_stance_state, "hip_yaw_left", - "hip_yaw_leftdot"); + swing_hip_yaw_traj->AddStateAndJointToTrack( + left_stance_state, "hip_yaw_right", "hip_yaw_rightdot"); + swing_hip_yaw_traj->AddStateAndJointToTrack( + right_stance_state, "hip_yaw_left", "hip_yaw_leftdot"); if (FLAGS_use_radio) { builder.Connect(cassie_out_to_radio->get_output_port(), @@ -565,13 +544,13 @@ int DoMain(int argc, char* argv[]) { osc->SetOsqpSolverOptionsFromYaml( "examples/Cassie/osc/solver_settings/osqp_options_walking.yaml"); - if (gains.W_com(0,0) == 0){ - osc->SetInputCostForJointAndFsmStateWeight( - "toe_left_motor", left_stance_state, 1.0); + if (gains.W_com(0, 0) == 0) { + osc->SetInputCostForJointAndFsmStateWeight("toe_left_motor", + left_stance_state, 1.0); osc->SetInputCostForJointAndFsmStateWeight( "toe_left_motor", post_right_double_support_state, 1.0); - osc->SetInputCostForJointAndFsmStateWeight( - "toe_right_motor", right_stance_state, 1.0); + osc->SetInputCostForJointAndFsmStateWeight("toe_right_motor", + right_stance_state, 1.0); osc->SetInputCostForJointAndFsmStateWeight( "toe_right_motor", post_left_double_support_state, 1.0); } @@ -582,7 +561,7 @@ int DoMain(int argc, char* argv[]) { osc->get_input_port_robot_output()); builder.Connect(fsm->get_output_port(0), osc->get_input_port_fsm()); builder.Connect(alip_traj_generator->get_output_port_com(), - osc->get_input_port_tracking_data("alip_com_traj")); + osc->get_input_port_tracking_data("alip_com_traj")); builder.Connect(swing_ft_traj_generator->get_output_port(0), osc->get_input_port_tracking_data("swing_ft_traj")); builder.Connect(head_traj_gen->get_output_port(0), @@ -604,7 +583,8 @@ int DoMain(int argc, char* argv[]) { builder.AddSystem(LcmPublisherSystem::Make( "OSC_DEBUG_WALKING", &lcm_local, TriggerTypeSet({TriggerType::kForced}))); - builder.Connect(osc->get_output_port_osc_debug(), osc_debug_pub->get_input_port()); + builder.Connect(osc->get_output_port_osc_debug(), + osc_debug_pub->get_input_port()); } // Create the diagram @@ -622,4 +602,4 @@ int DoMain(int argc, char* argv[]) { } // namespace dairlib -int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/Cassie/run_pd_controller.cc b/examples/Cassie/run_pd_controller.cc index c47c7c836f..6ab1bc3dac 100644 --- a/examples/Cassie/run_pd_controller.cc +++ b/examples/Cassie/run_pd_controller.cc @@ -1,11 +1,6 @@ +#include #include -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/systems/lcm/lcm_publisher_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" - #include "dairlib/lcmt_pd_config.hpp" #include "dairlib/lcmt_robot_input.hpp" #include "dairlib/lcmt_robot_output.hpp" @@ -14,6 +9,12 @@ #include "systems/controllers/pd_config_lcm.h" #include "systems/robot_lcm_systems.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_interface_system.h" +#include "drake/systems/lcm/lcm_publisher_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" + namespace dairlib { using drake::geometry::SceneGraph; @@ -64,8 +65,7 @@ int doMain(int argc, char* argv[]) { // Create config receiver. auto config_sub = builder.AddSystem( LcmSubscriberSystem::Make(channel_config, lcm)); - auto config_receiver = - builder.AddSystem(plant); + auto config_receiver = builder.AddSystem(plant); builder.Connect(config_sub->get_output_port(), config_receiver->get_input_port(0)); @@ -73,15 +73,13 @@ int doMain(int argc, char* argv[]) { auto command_pub = builder.AddSystem(LcmPublisherSystem::Make( channel_u, lcm, 1.0 / 1000.0)); - auto command_sender = - builder.AddSystem(plant); + auto command_sender = builder.AddSystem(plant); builder.Connect(command_sender->get_output_port(0), command_pub->get_input_port()); auto controller = builder.AddSystem( - plant.num_positions(), plant.num_velocities(), - plant.num_actuators()); + plant.num_positions(), plant.num_velocities(), plant.num_actuators()); builder.Connect(state_receiver->get_output_port(0), controller->get_input_port_output()); @@ -114,4 +112,5 @@ int doMain(int argc, char* argv[]) { } // namespace dairlib +// int main(int argc, char* argv[]) { return dairlib::doMain(argc, argv); } int main(int argc, char* argv[]) { return dairlib::doMain(argc, argv); } diff --git a/examples/Cassie/saved_trajectories/box_jump b/examples/Cassie/saved_trajectories/box_jump new file mode 100644 index 0000000000..b8b76f2ef6 Binary files /dev/null and b/examples/Cassie/saved_trajectories/box_jump differ diff --git a/examples/Cassie/saved_trajectories/down_jump b/examples/Cassie/saved_trajectories/down_jump new file mode 100644 index 0000000000..670b25fb3e Binary files /dev/null and b/examples/Cassie/saved_trajectories/down_jump differ diff --git a/examples/Cassie/saved_trajectories/gait_parameters/dircon_box_jump.yaml b/examples/Cassie/saved_trajectories/gait_parameters/dircon_box_jump.yaml new file mode 100644 index 0000000000..2108932f92 --- /dev/null +++ b/examples/Cassie/saved_trajectories/gait_parameters/dircon_box_jump.yaml @@ -0,0 +1,32 @@ +spring_urdf: examples/Cassie/urdf/cassie_v2.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf + +knot_points: 8 +delta_height: 0.5 +start_height: 0.8 +distance: 0.3 +min_stance_duration: 0.2 +max_stance_duration: 1.5 +min_flight_duration: 0.1 +max_flight_duration: 1.0 +actuator_limit: 1.0 +input_delta: 80 + +snopt_scale_option: 0 +tol: 1e-4 +dual_inf_tol: 1e-4 +constr_viol_tol: 1e-4 +compl_inf_tol: 1e-4 +acceptable_tol: 1e-3 +acceptable_iter: 10 +cost_scaling: 1e-3 +use_ipopt: false +ipopt_iter: 100 + +data_directory: examples/Cassie/saved_trajectories/ +load_filename: box_jump_5 +save_filename: box_jump_6 +use_springs: false +convert_to_springs: false +same_knotpoints: true + diff --git a/examples/Cassie/saved_trajectories/gait_parameters/dircon_down_jump.yaml b/examples/Cassie/saved_trajectories/gait_parameters/dircon_down_jump.yaml new file mode 100644 index 0000000000..f210958b54 --- /dev/null +++ b/examples/Cassie/saved_trajectories/gait_parameters/dircon_down_jump.yaml @@ -0,0 +1,32 @@ +spring_urdf: examples/Cassie/urdf/cassie_v2.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf + +knot_points: 8 +delta_height: -0.5 +start_height: 0.8 +distance: 0.6 +min_stance_duration: 0.2 +max_stance_duration: 1.0 +min_flight_duration: 0.1 +max_flight_duration: 1.0 +actuator_limit: 1.0 +input_delta: 75 + +snopt_scale_option: 0 +tol: 1e-5 +dual_inf_tol: 1e-4 +constr_viol_tol: 1e-4 +compl_inf_tol: 1e-4 +acceptable_tol: 1e-3 +acceptable_iter: 10 +cost_scaling: 1e-3 +use_ipopt: false +ipopt_iter: 100 + +data_directory: examples/Cassie/saved_trajectories/ +load_filename: down_jump_4 +save_filename: down_jump_4 +use_springs: false +convert_to_springs: false +same_knotpoints: true + diff --git a/examples/Cassie/saved_trajectories/gait_parameters/dircon_jump.yaml b/examples/Cassie/saved_trajectories/gait_parameters/dircon_jump.yaml new file mode 100644 index 0000000000..05546d850a --- /dev/null +++ b/examples/Cassie/saved_trajectories/gait_parameters/dircon_jump.yaml @@ -0,0 +1,32 @@ +spring_urdf: examples/Cassie/urdf/cassie_v2.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf + +knot_points: 8 +delta_height: 0.0 +start_height: 0.8 +distance: 0.3 +min_stance_duration: 0.2 +max_stance_duration: 1.0 +min_flight_duration: 0.1 +max_flight_duration: 1.0 +actuator_limit: 1.0 +input_delta: 75 + +snopt_scale_option: 0 +tol: 1e-4 +dual_inf_tol: 1e-4 +constr_viol_tol: 1e-4 +compl_inf_tol: 1e-4 +acceptable_tol: 1e-3 +acceptable_iter: 10 +cost_scaling: 1e-3 +use_ipopt: false +ipopt_iter: 100 + +data_directory: examples/Cassie/saved_trajectories/ +load_filename: jump +save_filename: jump +use_springs: false +convert_to_springs: false +same_knotpoints: true + diff --git a/examples/Cassie/saved_trajectories/gait_parameters/dircon_long_jump.yaml b/examples/Cassie/saved_trajectories/gait_parameters/dircon_long_jump.yaml new file mode 100644 index 0000000000..756344d1f1 --- /dev/null +++ b/examples/Cassie/saved_trajectories/gait_parameters/dircon_long_jump.yaml @@ -0,0 +1,32 @@ +spring_urdf: examples/Cassie/urdf/cassie_v2.urdf +fixed_spring_urdf: examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf + +knot_points: 8 +delta_height: 0.0 +start_height: 0.85 +distance: 0.7 +min_stance_duration: 0.2 +max_stance_duration: 1.0 +min_flight_duration: 0.1 +max_flight_duration: 1.0 +actuator_limit: 1.0 +input_delta: 80 + +snopt_scale_option: 0 +tol: 1e-4 +dual_inf_tol: 1e-4 +constr_viol_tol: 1e-4 +compl_inf_tol: 1e-4 +acceptable_tol: 1e-3 +acceptable_iter: 10 +cost_scaling: 1e-3 +use_ipopt: false +ipopt_iter: 100 + +data_directory: examples/Cassie/saved_trajectories/ +load_filename: long_jump_conservative +save_filename: long_jump +use_springs: false +convert_to_springs: false +same_knotpoints: true + diff --git a/examples/Cassie/saved_trajectories/jump b/examples/Cassie/saved_trajectories/jump new file mode 100644 index 0000000000..b95266ff18 Binary files /dev/null and b/examples/Cassie/saved_trajectories/jump differ diff --git a/examples/Cassie/saved_trajectories/long_jump b/examples/Cassie/saved_trajectories/long_jump new file mode 100644 index 0000000000..7168ed279e Binary files /dev/null and b/examples/Cassie/saved_trajectories/long_jump differ diff --git a/examples/Cassie/start_logging.py b/examples/Cassie/start_logging.py index 1193cd0bb4..88bdd39f2e 100644 --- a/examples/Cassie/start_logging.py +++ b/examples/Cassie/start_logging.py @@ -6,19 +6,26 @@ def main(): + sim = (os.getlogin() == 'brian') curr_date = date.today().strftime("%m_%d_%y") year = date.today().strftime("%Y") - logdir = f"{os.getenv('HOME')}/logs/{year}/{curr_date}" - dair = f"{os.getenv('HOME')}/workspace/dairlib/" - standing_gains = dair + "examples/Cassie/osc/osc_standing_gains.yaml" - walking_gains = dair + "examples/Cassie/osc/osc_walking_gains.yaml" - alip_walking_gains = dair + "examples/Cassie/osc/alip_osc_walking_gains.yaml" + + logdir = \ + f"{os.getenv('HOME')}/workspace/logs/cassie_simulation/{year}/{curr_date}" \ + if sim else f"{os.getenv('HOME')}/logs/{year}/{curr_date}" + + current_dair_dir = f"{os.getcwd()}/" + standing_gains = current_dair_dir + "examples/Cassie/osc/osc_standing_gains.yaml" + walking_gains = current_dair_dir + "examples/Cassie/osc/osc_walking_gains.yaml" + alip_gains = current_dair_dir + "examples/Cassie/osc/osc_walking_gains_alip.yaml" + running_gains = current_dair_dir + "examples/Cassie/osc_run/osc_running_gains.yaml" + jumping_gains = current_dair_dir + "examples/Cassie/osc_jump/osc_jumping_gains.yaml" if not os.path.isdir(logdir): os.mkdir(logdir) - git_diff = subprocess.check_output(['git', 'diff'], cwd=dair) - commit_tag = subprocess.check_output(['git', 'rev-parse', 'HEAD'], cwd=dair) + git_diff = subprocess.check_output(['git', 'diff'], cwd=current_dair_dir) + commit_tag = subprocess.check_output(['git', 'rev-parse', 'HEAD'], cwd=current_dair_dir) os.chdir(logdir) current_logs = sorted(glob.glob('lcmlog-*')) @@ -35,8 +42,9 @@ def main(): subprocess.run(['cp', standing_gains, 'standing_gains_%s.yaml' % log_num]) subprocess.run(['cp', walking_gains, 'walking_gains_%s.yaml' % log_num]) - subprocess.run(['cp', alip_walking_gains, - 'walking_gains_alip_%s.yaml' % log_num]) + subprocess.run(['cp', alip_gains, 'walking_gains_alip%s.yaml' % log_num]) + subprocess.run(['cp', running_gains, 'running_gains_%s.yaml' % log_num]) + subprocess.run(['cp', jumping_gains, 'jumping_gains_%s.yaml' % log_num]) subprocess.run(['lcm-logger', '-f', 'lcmlog-%s' % log_num]) diff --git a/examples/Cassie/systems/cassie_encoder.cc b/examples/Cassie/systems/cassie_encoder.cc index eee194243e..2cec041487 100644 --- a/examples/Cassie/systems/cassie_encoder.cc +++ b/examples/Cassie/systems/cassie_encoder.cc @@ -7,6 +7,7 @@ namespace dairlib { using Eigen::VectorXd; +using drake::systems::BasicVector; CassieEncoder::CassieEncoder( const drake::multibody::MultibodyPlant& plant) @@ -37,17 +38,17 @@ CassieEncoder::CassieEncoder( } this->DeclareVectorInputPort( "robot_state", - systems::BasicVector(num_positions_ + num_velocities_)); + BasicVector(num_positions_ + num_velocities_)); this->DeclareVectorOutputPort( "filtered_state", - systems::BasicVector(num_positions_ + num_velocities_), + BasicVector(num_positions_ + num_velocities_), &CassieEncoder::UpdateFilter); } void CassieEncoder::UpdateFilter(const drake::systems::Context& context, - systems::BasicVector* output) const { - const systems::BasicVector& x = - *this->template EvalVectorInput(context, 0); + BasicVector* output) const { + const BasicVector& x = + *this->template EvalVectorInput(context, 0); VectorXd q = x.value().head(num_positions_); VectorXd v = x.value().tail(num_velocities_); diff --git a/examples/Cassie/systems/cassie_encoder.h b/examples/Cassie/systems/cassie_encoder.h index a2e5a011ca..66749179e9 100644 --- a/examples/Cassie/systems/cassie_encoder.h +++ b/examples/Cassie/systems/cassie_encoder.h @@ -70,7 +70,7 @@ class CassieEncoder final : public drake::systems::LeafSystem { protected: void UpdateFilter(const drake::systems::Context& context, - systems::BasicVector* output) const; + drake::systems::BasicVector* output) const; private: struct DriveFilter { diff --git a/examples/Cassie/systems/input_supervisor.cc b/examples/Cassie/systems/input_supervisor.cc index 28f311dcb4..407d552833 100644 --- a/examples/Cassie/systems/input_supervisor.cc +++ b/examples/Cassie/systems/input_supervisor.cc @@ -105,7 +105,7 @@ InputSupervisor::InputSupervisor( K_ *= kEStopGain; // Create update for error flag - DeclarePerStepDiscreteUpdateEvent(&InputSupervisor::UpdateErrorFlag); + DeclareForcedDiscreteUpdateEvent(&InputSupervisor::UpdateErrorFlag); } void InputSupervisor::SetMotorTorques(const Context& context, diff --git a/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc b/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc index 84de52e132..9cb578a982 100644 --- a/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc +++ b/examples/Cassie/systems/sim_cassie_sensor_aggregator.cc @@ -7,6 +7,7 @@ namespace systems { using std::string; using drake::systems::Context; +using drake::systems::BasicVector; using drake::AbstractValue; using dairlib::systems::TimestampedVector; using Eigen::VectorXd; diff --git a/examples/Cassie/systems/sim_cassie_sensor_aggregator.h b/examples/Cassie/systems/sim_cassie_sensor_aggregator.h index f9eee8088c..74062fca25 100644 --- a/examples/Cassie/systems/sim_cassie_sensor_aggregator.h +++ b/examples/Cassie/systems/sim_cassie_sensor_aggregator.h @@ -14,8 +14,8 @@ #include "systems/framework/output_vector.h" #include "systems/framework/timestamped_vector.h" -#include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/multibody/plant/multibody_plant.h" -#include "external/drake/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/systems/framework/leaf_system.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/leaf_system.h" namespace dairlib { namespace systems { diff --git a/examples/Cassie/test/benchmark_dynamics.cc b/examples/Cassie/test/benchmark_dynamics.cc index b8f8a31c8b..380812205f 100644 --- a/examples/Cassie/test/benchmark_dynamics.cc +++ b/examples/Cassie/test/benchmark_dynamics.cc @@ -80,7 +80,7 @@ int do_main() { scene_graph.set_name("scene_graph"); multibody::Parser parser(&multibody_plant, &scene_graph); - parser.AddModelFromFile(dairlib::FindResourceOrThrow( + parser.AddModels(dairlib::FindResourceOrThrow( "examples/Cassie/urdf/cassie_v2.urdf")); multibody_plant.WeldFrames( diff --git a/examples/Cassie/test/input_supervisor_test.cc b/examples/Cassie/test/input_supervisor_test.cc deleted file mode 100644 index 4d53f216ba..0000000000 --- a/examples/Cassie/test/input_supervisor_test.cc +++ /dev/null @@ -1,95 +0,0 @@ -#include "examples/Cassie/systems/input_supervisor.h" - -#include -#include -#include - -#include "dairlib/lcmt_controller_switch.hpp" -#include "examples/Cassie/cassie_utils.h" - -#include "drake/multibody/plant/multibody_plant.h" - -namespace dairlib { -namespace systems { - -using Eigen::VectorXd; - -class InputSupervisorTest : public ::testing::Test { - protected: - InputSupervisorTest() - : plant_(drake::multibody::MultibodyPlant(0.0)) { - AddCassieMultibody(&plant_, nullptr, true /*floating base*/, - "examples/Cassie/urdf/cassie_v2.urdf", - true /*spring model*/, false /*loop closure*/); - plant_.Finalize(); - VectorXd input_limit = 20.0 * VectorXd::Ones(10); - supervisor_ = std::make_unique( - plant_, "DEFAULT_CONTROL_CHANNEL", 10.0, 0.01, input_limit, min_consecutive_failures); - context_ = supervisor_->CreateDefaultContext(); - status_output_ = std::make_unique(); - cassie_out_ = std::make_unique(); - motor_output_ = - std::make_unique>(plant_.num_actuators()); - command_input_ = - std::make_unique>(plant_.num_actuators()); - state_input_ = std::make_unique>( - plant_.num_positions(), plant_.num_velocities(), - plant_.num_actuators()); - - state_input_port_ = supervisor_->get_input_port_state().get_index(); - } - - int state_input_port_; - drake::multibody::MultibodyPlant plant_; - const int min_consecutive_failures = 5; - std::unique_ptr supervisor_; - std::unique_ptr status_output_; - std::unique_ptr cassie_out_; - std::unique_ptr> motor_output_; - std::unique_ptr> command_input_; - std::unique_ptr> state_input_; - std::unique_ptr> context_; -}; - -TEST_F(InputSupervisorTest, BlendEffortsTest) { - VectorXd prev_input = VectorXd::Zero(plant_.num_actuators()); - VectorXd desired_input = 10 * VectorXd::Ones(plant_.num_actuators()); - double blend_start_time = 0.5; - double timestamp = 1.0; - double blend_duration = 1.0; - std::unique_ptr> switch_msg = - std::make_unique>(); - std::unique_ptr> controller_failure = - std::make_unique>(); - cassie_out_->pelvis.radio.channel[15] = 0; - switch_msg->get_mutable_value().blend_duration = blend_duration; - switch_msg->get_mutable_value().utime = blend_start_time * 1e6; - command_input_->get_mutable_value() = desired_input; - command_input_->set_timestamp(timestamp); - supervisor_->get_input_port_state().FixValue(context_.get(), *state_input_); - supervisor_->get_input_port_cassie().FixValue(context_.get(), *cassie_out_); - supervisor_->get_input_port_safety_controller().FixValue(context_.get(), - *command_input_); - supervisor_->get_input_port_controller_switch().FixValue( - context_.get(), (drake::AbstractValue&)*switch_msg); - supervisor_->get_input_port_controller_error().FixValue( - context_.get(), (drake::AbstractValue&)*controller_failure); - supervisor_->get_input_port_command().FixValue(context_.get(), - *command_input_); - supervisor_->UpdateErrorFlag(*context_, - &context_->get_mutable_discrete_state()); - supervisor_->SetMotorTorques(*context_, motor_output_.get()); - - VectorXd output_from_supervisor = motor_output_->get_value(); - double alpha = (timestamp - blend_start_time) / blend_duration; - - EXPECT_EQ(output_from_supervisor, alpha * desired_input); -} - -} // namespace systems -} // namespace dairlib - -int main(int argc, char** argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/examples/Cassie/test/log_timing_test.cc b/examples/Cassie/test/log_timing_test.cc index be45d8857a..568870016a 100644 --- a/examples/Cassie/test/log_timing_test.cc +++ b/examples/Cassie/test/log_timing_test.cc @@ -1,7 +1,7 @@ #include #include #include -#include "lcm/lcm-cpp.hpp" +#include DEFINE_string(file, "", "Log file name."); DEFINE_int64(max_count, 5000, "Max number of messages to read."); diff --git a/examples/Cassie/test/min_deps_mbt_sim.cc b/examples/Cassie/test/min_deps_mbt_sim.cc index 7a50899086..5ae25c3f2b 100644 --- a/examples/Cassie/test/min_deps_mbt_sim.cc +++ b/examples/Cassie/test/min_deps_mbt_sim.cc @@ -49,7 +49,7 @@ int do_main(int argc, char* argv[]) { // NOTE: will need to change path as appropriate std::string full_name = "/home/posa/workspace/dairlib/examples/Cassie/urdf/cassie_v2.urdf"; Parser parser(&plant, &scene_graph); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.WeldFrames( plant.world_frame(), plant.GetFrameByName("pelvis"), @@ -60,7 +60,7 @@ int do_main(int argc, char* argv[]) { Eigen::VectorXd::Zero(plant.num_actuators())); builder.Connect(input_source->get_output_port(), - plant.get_actuation_input_port()); + plant.get_input_port_actuation()); builder.Connect( plant.get_geometry_poses_output_port(), diff --git a/examples/Cassie/urdf/cassie_fixed_spring_conservative.urdf b/examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf similarity index 97% rename from examples/Cassie/urdf/cassie_fixed_spring_conservative.urdf rename to examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf index 904add7e7b..0bc93d7fac 100644 --- a/examples/Cassie/urdf/cassie_fixed_spring_conservative.urdf +++ b/examples/Cassie/urdf/cassie_fixed_springs_conservative.urdf @@ -617,7 +617,7 @@ - + @@ -626,7 +626,7 @@ - + @@ -636,7 +636,7 @@ - + @@ -645,7 +645,7 @@ - + @@ -655,7 +655,7 @@ - + @@ -664,7 +664,7 @@ - + @@ -673,7 +673,7 @@ - + @@ -682,7 +682,7 @@ - + @@ -791,7 +791,7 @@ - + @@ -800,7 +800,7 @@ - + diff --git a/examples/Cassie/urdf/cassie_foot_crank.urdf b/examples/Cassie/urdf/cassie_foot_crank.urdf new file mode 100644 index 0000000000..b0e2111df1 --- /dev/null +++ b/examples/Cassie/urdf/cassie_foot_crank.urdf @@ -0,0 +1,134 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/Cassie/urdf/cassie_knee_linkage.urdf b/examples/Cassie/urdf/cassie_knee_linkage.urdf new file mode 100644 index 0000000000..c1618a1b76 --- /dev/null +++ b/examples/Cassie/urdf/cassie_knee_linkage.urdf @@ -0,0 +1,229 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/Cassie/urdf/cassie_left_leg.urdf b/examples/Cassie/urdf/cassie_left_leg.urdf new file mode 100644 index 0000000000..d235568aaf --- /dev/null +++ b/examples/Cassie/urdf/cassie_left_leg.urdf @@ -0,0 +1,335 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/Cassie/urdf/cassie_v2.urdf b/examples/Cassie/urdf/cassie_v2.urdf index 46284e004a..bb0f7ca0b5 100644 --- a/examples/Cassie/urdf/cassie_v2.urdf +++ b/examples/Cassie/urdf/cassie_v2.urdf @@ -614,8 +614,8 @@ - - + + @@ -623,8 +623,8 @@ - - + + @@ -633,7 +633,7 @@ - + @@ -642,7 +642,7 @@ - + @@ -652,7 +652,7 @@ - + @@ -661,7 +661,7 @@ - + @@ -670,7 +670,7 @@ - + @@ -679,7 +679,7 @@ - + @@ -713,8 +713,8 @@ - - + + @@ -722,8 +722,8 @@ - - + + @@ -784,7 +784,7 @@ - + @@ -793,7 +793,7 @@ - + diff --git a/examples/Cassie/urdf/cassie_v2_conservative.urdf b/examples/Cassie/urdf/cassie_v2_conservative.urdf index 7be788d201..b7120290d5 100644 --- a/examples/Cassie/urdf/cassie_v2_conservative.urdf +++ b/examples/Cassie/urdf/cassie_v2_conservative.urdf @@ -615,7 +615,7 @@ - + @@ -624,7 +624,7 @@ - + @@ -634,7 +634,7 @@ - + @@ -643,7 +643,7 @@ - + @@ -652,9 +652,8 @@ - - + @@ -662,9 +661,8 @@ - - + @@ -672,8 +670,8 @@ - - + + @@ -681,8 +679,8 @@ - - + + @@ -716,7 +714,7 @@ - + @@ -725,7 +723,7 @@ - + @@ -748,7 +746,7 @@ - + @@ -757,7 +755,7 @@ - + @@ -767,7 +765,7 @@ - + @@ -777,7 +775,7 @@ - + @@ -787,7 +785,7 @@ - + @@ -796,7 +794,7 @@ - + diff --git a/examples/Cassie/visualize_trajectory.cc b/examples/Cassie/visualize_trajectory.cc index ca9e4293f3..9efcbf549b 100644 --- a/examples/Cassie/visualize_trajectory.cc +++ b/examples/Cassie/visualize_trajectory.cc @@ -45,13 +45,34 @@ int DoMain() { // Drake system initialization stuff drake::systems::DiagramBuilder builder; SceneGraph& scene_graph = *builder.AddSystem(); + SceneGraph& scene_graph_w_spr = *builder.AddSystem(); scene_graph.set_name("scene_graph"); - MultibodyPlant plant(1e-5); + MultibodyPlant plant(1e-3); Parser parser(&plant, &scene_graph); - parser.AddModelFromFile( - FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf")); + const std::string& fixed_spring_urdf = + "examples/Cassie/urdf/cassie_fixed_springs.urdf"; + parser.AddModels(fixed_spring_urdf); plant.Finalize(); + MultibodyPlant plant_w_spr(1e-3); + const std::string& spring_urdf = "examples/Cassie/urdf/cassie_v2_shells.urdf"; + Parser parser_w_spr(&plant_w_spr, &scene_graph_w_spr); + + parser_w_spr.AddModels(spring_urdf); + plant_w_spr.Finalize(); + auto pos_spr_map = + multibody::CreateWithSpringsToWithoutSpringsMapPos(plant_w_spr, plant); + auto vel_spr_map = + multibody::CreateWithSpringsToWithoutSpringsMapVel(plant_w_spr, plant); + pos_spr_map.transposeInPlace(); + vel_spr_map.transposeInPlace(); + MatrixXd state_spr_map = + MatrixXd::Zero(plant_w_spr.num_positions() + plant_w_spr.num_velocities(), + plant.num_positions() + plant.num_velocities()); + state_spr_map.block(0, 0, 23, 19) = pos_spr_map; + state_spr_map.block(23, 19, 22, 18) = vel_spr_map; + auto vis_urdf = FLAGS_use_springs ? spring_urdf : fixed_spring_urdf; + int nq = plant.num_positions(); int nv = plant.num_positions(); int nx = nq + nv; @@ -66,15 +87,19 @@ int DoMain() { PiecewisePolynomial optimal_traj = saved_traj.ReconstructStateTrajectory(); + if (FLAGS_use_springs) { + optimal_traj = + saved_traj.ReconstructStateTrajectoryWithSprings(state_spr_map); + } std::vector time_vector = optimal_traj.get_segment_times(); if (FLAGS_mirror_traj) { auto mirrored_traj = saved_traj.ReconstructMirrorStateTrajectory(optimal_traj.end_time()); VectorXd x_offset = VectorXd::Zero(nx); - x_offset(pos_map["base_x"]) = - optimal_traj.value(optimal_traj.end_time())(pos_map["base_x"]) - - optimal_traj.value(optimal_traj.start_time())(pos_map["base_x"]); + x_offset(pos_map["pelvis_x"]) = + optimal_traj.value(optimal_traj.end_time())(pos_map["pelvis_x"]) - + optimal_traj.value(optimal_traj.start_time())(pos_map["pelvis_x"]); std::vector x_offset_rep(mirrored_traj.get_segment_times().size(), x_offset); PiecewisePolynomial x_offset_traj = @@ -82,9 +107,9 @@ int DoMain() { mirrored_traj.get_segment_times(), x_offset_rep); optimal_traj.ConcatenateInTime(mirrored_traj + x_offset_traj); - x_offset(pos_map["base_x"]) = - optimal_traj.value(optimal_traj.end_time())(pos_map["base_x"]) - - optimal_traj.value(optimal_traj.start_time())(pos_map["base_x"]); + x_offset(pos_map["pelvis_x"]) = + optimal_traj.value(optimal_traj.end_time())(pos_map["pelvis_x"]) - + optimal_traj.value(optimal_traj.start_time())(pos_map["pelvis_x"]); x_offset_rep = std::vector( optimal_traj.get_segment_times().size(), x_offset); @@ -109,27 +134,42 @@ int DoMain() { simulator.AdvanceTo(optimal_traj.end_time()); } while (FLAGS_visualize_mode == 1); } else if (FLAGS_visualize_mode == 2) { - MatrixXd poses = MatrixXd::Zero(nq, FLAGS_num_poses); + MatrixXd poses = MatrixXd::Zero(23, FLAGS_num_poses); for (int i = 0; i < FLAGS_num_poses; ++i) { poses.col(i) = optimal_traj.value( time_vector[i * time_vector.size() / FLAGS_num_poses]); + // poses(6, i) += 0.4; } + VectorXd alpha_scale = VectorXd::Ones(FLAGS_num_poses); if (FLAGS_use_transparency) { - VectorXd alpha_scale = VectorXd::LinSpaced(FLAGS_num_poses, 0.2, 1.0); - multibody::MultiposeVisualizer visualizer = - multibody::MultiposeVisualizer( - FindResourceOrThrow( - "examples/Cassie/urdf/cassie_fixed_springs.urdf"), - FLAGS_num_poses, alpha_scale.array().square()); - visualizer.DrawPoses(poses); - } else { - multibody::MultiposeVisualizer visualizer = - multibody::MultiposeVisualizer( - FindResourceOrThrow( - "examples/Cassie/urdf/cassie_fixed_springs.urdf"), - FLAGS_num_poses); - visualizer.DrawPoses(poses); + alpha_scale = VectorXd::LinSpaced(FLAGS_num_poses, 0.2, 1.0); } + multibody::MultiposeVisualizer visualizer = multibody::MultiposeVisualizer( + vis_urdf, FLAGS_num_poses, alpha_scale.array().square()); + auto ortho_camera = drake::geometry::Meshcat::OrthographicCamera(); + ortho_camera.top = 2; + ortho_camera.bottom = -0.1; + ortho_camera.left = -1; + ortho_camera.right = 4; + ortho_camera.near = 0; + ortho_camera.far = 500; + ortho_camera.zoom = 1; + auto perspective_camera = drake::geometry::Meshcat::PerspectiveCamera(); + perspective_camera.fov = 75; + perspective_camera.aspect = 1; + perspective_camera.near = 1; + perspective_camera.far = 1000; + perspective_camera.zoom = 1; + auto translation = Vector3d(); + translation << 0.45, 0, 0.25; + auto origin = drake::math::RigidTransform(translation); + auto box = drake::geometry::Box(0.5, 1.0, 0.5); + visualizer.GetMeshcat()->SetObject("box", box); + visualizer.GetMeshcat()->SetTransform("box", origin); +// visualizer.GetMeshcat()->SetCamera(perspective_camera); + visualizer.GetMeshcat()->SetCamera(ortho_camera); + visualizer.DrawPoses(poses); + while (true){} } return 0; diff --git a/examples/Cassie/visualizer.cc b/examples/Cassie/visualizer.cc index 6e6f9a935d..726eda240a 100644 --- a/examples/Cassie/visualizer.cc +++ b/examples/Cassie/visualizer.cc @@ -121,7 +121,6 @@ int do_main(int argc, char* argv[]) { auto meshcat = std::make_shared(); auto visualizer = &drake::geometry::MeshcatVisualizer::AddToBuilder( &builder, scene_graph, meshcat, std::move(params)); - // state_receiver->set_publish_period(1.0/30.0); // framerate auto diagram = builder.Build(); diff --git a/examples/PlanarWalker/run_gait_dircon.cc b/examples/PlanarWalker/run_gait_dircon.cc index 6c7213d78b..2655915fb3 100644 --- a/examples/PlanarWalker/run_gait_dircon.cc +++ b/examples/PlanarWalker/run_gait_dircon.cc @@ -230,8 +230,8 @@ int main(int argc, char* argv[]) { std::string full_name = dairlib::FindResourceOrThrow("examples/PlanarWalker/PlanarWalker.urdf"); - parser.AddModelFromFile(full_name); - parser_vis.AddModelFromFile(full_name); + parser.AddModels(full_name); + parser_vis.AddModels(full_name); plant->WeldFrames( plant->world_frame(), plant->GetFrameByName("base"), diff --git a/examples/acrobot/swingup.cc b/examples/acrobot/swingup.cc index d8c9bd34dc..055c062cd8 100644 --- a/examples/acrobot/swingup.cc +++ b/examples/acrobot/swingup.cc @@ -150,8 +150,8 @@ int main(int argc, char* argv[]) { std::string full_name = dairlib::FindResourceOrThrow("examples/acrobot/Acrobot.urdf"); - parser.AddModelFromFile(full_name); - parser_vis.AddModelFromFile(full_name); + parser.AddModels(full_name); + parser_vis.AddModels(full_name); plant->Finalize(); plant_vis->Finalize(); diff --git a/examples/franka/BUILD.bazel b/examples/franka/BUILD.bazel new file mode 100644 index 0000000000..b0b651bfd5 --- /dev/null +++ b/examples/franka/BUILD.bazel @@ -0,0 +1,287 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "urdfs", + data = glob([ + "urdf/**", + ]), +) + +cc_binary( + name = "full_diagram", + srcs = ["full_diagram.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//examples/franka/diagrams:franka_c3_controller_diagram", + "//examples/franka/diagrams:franka_osc_controller_diagram", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "@drake//:drake_shared_library", + ], +) + +cc_binary( + name = "franka_sim", + srcs = ["franka_sim.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//examples/franka/systems:franka_systems", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/framework:lcm_driven_loop", + "//systems/primitives:radio_parser", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_osc_controller", + srcs = ["franka_osc_controller.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//examples/franka/systems:franka_systems", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/primitives:radio_parser", + "//systems/trajectory_optimization:lcm_trajectory_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_c3_controller", + srcs = ["franka_c3_controller.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//examples/franka/systems:franka_systems", + "//systems:franka_kinematics", + "//systems/senders:c3_state_sender", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/controllers:c3_controller", + "//systems/controllers:lcs_factory_system", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/primitives:radio_parser", + "//systems/trajectory_optimization:c3_output_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_c3_controller_two_objects", + srcs = ["franka_c3_controller_two_objects.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//examples/franka/systems:franka_systems", + "//systems:franka_kinematics", + "//systems/senders:c3_state_sender", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/controllers:c3_controller", + "//systems/controllers:lcs_factory_system", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/primitives:radio_parser", + "//systems/trajectory_optimization:c3_output_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "forward_kinematics_for_lcs", + srcs = ["forward_kinematics_for_lcs.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//examples/franka/systems:franka_systems", + "//systems:franka_kinematics", + "//systems/senders:c3_state_sender", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/controllers:c3_controller", + "//systems/controllers:lcs_factory_system", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/primitives:radio_parser", + "//systems/trajectory_optimization:c3_output_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_visualizer", + srcs = ["franka_visualizer.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//multibody:utils", + "//multibody:visualization_utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/primitives", + "//systems/trajectory_optimization:lcm_trajectory_systems", + "//systems/visualization:lcm_visualization_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_bridge_driver_out", + srcs = ["franka_bridge_driver_out.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//systems:franka_state_translator", + "//multibody:utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:lcm_driven_loop", + "//systems/primitives", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_bridge_driver_in", + srcs = ["franka_bridge_driver_in.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + deps = [ + ":parameters", + "//common", + "//systems:franka_state_translator", + "//multibody:utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:lcm_driven_loop", + "//systems/primitives", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_ros_lcm_bridge", + srcs = ["franka_ros_lcm_bridge.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + tags = ["ros"], + deps = [ + ":parameters", + "//common", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/primitives", + "//systems/ros:franka_ros_channels", + "//systems/ros:franka_ros_lcm_conversions", + "//systems/ros:ros_pubsub_systems", + "@drake//:drake_shared_library", + "@gflags", + "@ros", + ], +) + +cc_binary( + name = "franka_lcm_ros_bridge", + srcs = ["franka_lcm_ros_bridge.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + ], + tags = ["ros"], + deps = [ + ":parameters", + "//common", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:lcm_driven_loop", + "//systems/ros:franka_ros_channels", + "//systems/ros:franka_ros_lcm_conversions", + "//systems/ros:ros_pubsub_systems", + "@drake//:drake_shared_library", + "@gflags", + "@ros", + ], +) + +cc_library( + name = "parameters", + hdrs = [ + "parameters/franka_c3_controller_params.h", + "parameters/franka_c3_scene_params.h", + "parameters/franka_lcm_channels.h", + "parameters/franka_osc_controller_params.h", + "parameters/franka_sim_params.h", + "parameters/franka_sim_scene_params.h", + ], + data = glob([ + "parameters/*.yaml", + "parameters/**/*.yaml", + ]), + deps = [ + "@drake//:drake_shared_library", + "//common/parameters:franka_drake_lcm_driver_channels", + ], +) diff --git a/examples/franka/README.md b/examples/franka/README.md new file mode 100644 index 0000000000..989a8dc943 --- /dev/null +++ b/examples/franka/README.md @@ -0,0 +1,77 @@ +## Experiment Instructions + +Central repo for C3 and its examples, including: +- https://arxiv.org/abs/2405.08731 + +This branch/repo is being constantly updated so examples may be unstable. + +## Simulated Robot + +1. Start the procman script containing a list of relevant processes +``` +bot-procman-sheriff -l franka_sim.pmd +``` + +2. In the procman window, start the operator processes (meshcat visualizer) using the script `script:start_operator_commands`. Scripts are located in the top bar of the procman window. + +3. The meshcat visualizer can be viewed by opening a browser and navigating to `localhost:7000` + +4. The examples with the C3 controller can be run using the script `script:c3_mpc`. Note, the task can be changed by changing the `scene_index` in the parameters. More details in [changing the scene](#changing-the-scene). This script spawns three processes: + - `bazel-bin/examples/franka/franka_sim`: Simulated environment which takes in torques commands from `franka_osc` and publishes the state of the system via LCM on various channels. + - `bazel-bin/examples/franka/franka_osc_controller`: Low-level task-space controller that tracks task-space trajectories it receives from the MPC + - `bazel-bin/examples/franka/franka_c3_controller`: Contact Implicit MPC controller that takes in the state of the system and publishes end effector trajectories to be tracked by the OSC. + +5. The simulator and controller can be stopped using the script `script:stop_controllers_and_simulators`. +6. A comparison using the sampling based MPC controllers from the [MJMPC controllers](https://github.com/google-deepmind/mujoco_mpc) can be run using `script:mjmpc_with_drake_sim`. This extracts out just the controller portion of the MJMPC code base and runs in on the identical task (scene 1) in the Drake simulator. Instructions to build and configure the standalone MJMPC controllers are a WIP. + +## Physical Robot + +Hardware instructions updated for Ubuntu 22.04. We are no longer using ROS or ROS2 and instead relying on [drake-franka-driver](https://github.com/RobotLocomotion/drake-franka-driver), which works via LCM. Much thanks to the Drake developers who provided this! + +### Installing `drake-franka-driver` + +``` +git clone https://github.com/RobotLocomotion/drake-franka-driver +cd drake-franka-driver +bazel build ... +``` + + +### Running Experiments + +1. Start the procman script containing a list of relevant processes. The primary differences from`franka_sim.pmd` script are the lcm_channels and the drake-franka-driver and corresponding translators to communicate with the Franka via LCM. + + - In the root of dairlib: ``` bot-procman-sheriff -l examples/franka/franka_hardware.pmd ``` + - In the root of drake-franka-driver: ```bot-procman-deputy franka_control``` + +2. In the procman window, start the operator processes (meshcat visualizer and xbox controller) using the script `script:start_operator_commands`. Scripts are located in the top bar of the procman window. + +3. The meshcat visualizer can be viewed by opening a browser and navigating to `localhost:7000` + +4. The processes, except the C3 controller, can be run using the script `script:start_experiment`. This spawns the following processes: + - `start_logging.py`: Starts a lcm-logger with an automatic naming convention for the log number. + - `record_video.py`: Streams all available webcams to a .mp4 file corresponding to the log number. + - `torque_driver`: `drake-franka-driver` in torque control mode. + - `franka_driver_`(in/out): communicates with `drake-franka-driver` to receive/publish franka state information and torque commands. This is just a translator between Drake's Franka Panda specific lcm messages and the standardized robot commands that we use. + - `bazel-bin/examples/franka/franka_osc_controller`: Low-level task-space controller that tracks task-space trajectories it receives from the MPC + + +5. For safety, start the C3 controller separately after verifying the rest of the processes have started successfully, by manually starting the `franka_c3` process. +6. Using the xbox controller, switch from tracking the teleop commands to the MPC plan by pressing "A". +7. Stop the experiment using `script:stop_experiment`. This also stops logging and recording. + + +## Changing the scene + +We currently have environment descriptions and gains for the following scenes: + +The scene can be changed by updating the `scene_index` parameter in BOTH `franka_sim_params.yaml` and `franka_c3_controller_params.yaml`. +The visualizer process must be restarted if changing the scene. + +| Scene Index | Description | +|-------------|-----------------------------------------------------------------------------------------------------------------------------------------------| +| 0 | No fixed environment features. For testing controlled sliding purely between the end effector and tray + optional objects. Gains still a WIP. | +| 1 | Primary RSS paper example with supports | +| 2 | Variation on RSS paper example with rotated supports | +| 3 | Additional rotating with wall task for RSS paper | + diff --git a/examples/franka/diagrams/BUILD.bazel b/examples/franka/diagrams/BUILD.bazel new file mode 100644 index 0000000000..bedf79b67d --- /dev/null +++ b/examples/franka/diagrams/BUILD.bazel @@ -0,0 +1,59 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "franka_c3_controller_diagram", + srcs = ["franka_c3_controller_diagram.cc"], + hdrs = ["franka_c3_controller_diagram.h"], + deps = [ + "//common", + "//examples/franka:parameters", + "//examples/franka/systems:franka_systems", + "//systems/senders:c3_state_sender", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/controllers:c3_controller", + "//systems/controllers:lcs_factory_system", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/primitives:radio_parser", + "//systems/trajectory_optimization:c3_output_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_library( + name = "franka_osc_controller_diagram", + srcs = ["franka_osc_controller_diagram.cc"], + hdrs = ["franka_osc_controller_diagram.h"], + deps = [ + "//common", + "//examples/franka:parameters", + "//examples/franka/systems:franka_systems", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers", + "//systems/controllers/osc:operational_space_control", + "//systems/primitives:radio_parser", + "//systems/trajectory_optimization:lcm_trajectory_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_library( + name = "franka_sim_diagram", + srcs = ["franka_sim_diagram.cc"], + hdrs = ["franka_sim_diagram.h"], + deps = [ + "//common", + "//examples/franka:parameters", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "@drake//:drake_shared_library", + "@gflags", + ], +) diff --git a/examples/franka/diagrams/franka_c3_controller_diagram.cc b/examples/franka/diagrams/franka_c3_controller_diagram.cc new file mode 100644 index 0000000000..686e820237 --- /dev/null +++ b/examples/franka/diagrams/franka_c3_controller_diagram.cc @@ -0,0 +1,339 @@ +// +// Created by yangwill on 2/19/24. +// + +#include "examples/franka/diagrams/franka_c3_controller_diagram.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/franka/parameters/franka_c3_controller_params.h" +#include "examples/franka/parameters/franka_c3_scene_params.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "systems/senders/c3_state_sender.h" +#include "examples/franka/systems/c3_trajectory_generator.h" +#include "systems/franka_kinematics.h" +#include "examples/franka/systems/plate_balancing_target.h" +#include "multibody/multibody_utils.h" +#include "solvers/lcs_factory.h" +#include "systems/controllers/c3/lcs_factory_system.h" +#include "systems/controllers/c3/c3_controller.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/c3_output_systems.h" + +namespace dairlib { + +namespace examples { +namespace controllers { + +using dairlib::solvers::LCSFactory; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; + +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; +using std::string; + +FrankaC3ControllerDiagram::FrankaC3ControllerDiagram( + const string& controller_settings, const C3Options c3_options, const string& lcm_channels, + drake::lcm::DrakeLcm* lcm, bool publish_c3_debug) { + this->set_name("FrankaC3Controller"); + DiagramBuilder builder; + + FrankaC3ControllerParams controller_params = + drake::yaml::LoadYamlFile(controller_settings); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels); + FrankaC3SceneParams scene_params = + drake::yaml::LoadYamlFile( + controller_params.c3_scene_file[controller_params.scene_index]); +// C3Options c3_options = drake::yaml::LoadYamlFile( +// controller_params.c3_options_file[controller_params.scene_index]); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(controller_params.osqp_settings_file)) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + plant_franka_ = new drake::multibody::MultibodyPlant(0.0); + Parser parser_franka(plant_franka_, nullptr); + parser_franka.AddModelsFromUrl(scene_params.franka_model); + drake::multibody::ModelInstanceIndex end_effector_index = + parser_franka.AddModels( + FindResourceOrThrow(scene_params.end_effector_model))[0]; + + RigidTransform X_WI = RigidTransform::Identity(); + plant_franka_->WeldFrames(plant_franka_->world_frame(), + plant_franka_->GetFrameByName("panda_link0"), X_WI); + + RigidTransform T_EE_W = + RigidTransform(drake::math::RotationMatrix(), + scene_params.tool_attachment_frame); + plant_franka_->WeldFrames( + plant_franka_->GetFrameByName("panda_link7"), + plant_franka_->GetFrameByName("plate", end_effector_index), T_EE_W); + + plant_franka_->Finalize(); + plant_franka_context_ = plant_franka_->CreateDefaultContext(); + + /// + plant_tray_ = new drake::multibody::MultibodyPlant(0.0); + Parser parser_tray(plant_tray_, nullptr); + parser_tray.AddModels(scene_params.object_models[0]); + plant_tray_->Finalize(); + plant_tray_context_ = plant_tray_->CreateDefaultContext(); + + drake::planning::RobotDiagramBuilder lcs_diagram_builder; + lcs_diagram_builder.parser().SetAutoRenaming(true); + lcs_diagram_builder.parser().AddModels(scene_params.end_effector_lcs_model); + + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = lcs_diagram_builder.parser().AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + lcs_diagram_builder.plant().WeldFrames( + lcs_diagram_builder.plant().world_frame(), + lcs_diagram_builder.plant().GetFrameByName("base", environment_model_indices[i]), + T_E_W); + } + for (int i = 0; i < scene_params.object_models.size(); ++i) { + lcs_diagram_builder.parser().AddModels(scene_params.object_models[i]); + } + lcs_diagram_builder.plant().WeldFrames( + lcs_diagram_builder.plant().world_frame(), + lcs_diagram_builder.plant().GetFrameByName("base_link"), X_WI); + lcs_diagram_builder.plant().Finalize(); + robot_diagram_for_lcs_ = lcs_diagram_builder.Build(); + plant_for_lcs_autodiff_ = drake::systems::System::ToAutoDiffXd( + robot_diagram_for_lcs_->plant()); + + robot_diagram_root_context_ = robot_diagram_for_lcs_->CreateDefaultContext(); + plant_for_lcs_autodiff_context_ = + plant_for_lcs_autodiff_->CreateDefaultContext(); + + /// + std::vector end_effector_contact_points = + robot_diagram_for_lcs_->plant().GetCollisionGeometriesForBody( + robot_diagram_for_lcs_->plant().GetBodyByName("plate")); + for (int i = 0; i < environment_model_indices.size(); ++i) { + std::vector + environment_support_contact_points = + robot_diagram_for_lcs_->plant().GetCollisionGeometriesForBody( + robot_diagram_for_lcs_->plant().GetBodyByName("base", + environment_model_indices[i])); + end_effector_contact_points.insert( + end_effector_contact_points.end(), + environment_support_contact_points.begin(), + environment_support_contact_points.end()); + } + std::vector tray_geoms = + robot_diagram_for_lcs_->plant().GetCollisionGeometriesForBody( + robot_diagram_for_lcs_->plant().GetBodyByName("tray")); + std::unordered_map> + contact_geoms; + contact_geoms["PLATE"] = end_effector_contact_points; + contact_geoms["TRAY"] = tray_geoms; + + std::vector> contact_pairs; + for (auto geom_id : contact_geoms["PLATE"]) { + contact_pairs.emplace_back(geom_id, contact_geoms["TRAY"][0]); + } + + auto franka_state_receiver = + builder.AddSystem(*plant_franka_); + auto tray_state_receiver = + builder.AddSystem(*plant_tray_); + auto reduced_order_model_receiver = + builder.AddSystem( + *plant_franka_, plant_franka_context_.get(), *plant_tray_, + plant_tray_context_.get(), scene_params.end_effector_name, + "tray", controller_params.include_end_effector_orientation); + + auto plate_balancing_target = + builder.AddSystem( + *plant_tray_, scene_params.end_effector_thickness, + controller_params.near_target_threshold); + plate_balancing_target->SetRemoteControlParameters( + controller_params.first_target[controller_params.scene_index], + controller_params.second_target[controller_params.scene_index], + controller_params.third_target[controller_params.scene_index], + controller_params.x_scale, controller_params.y_scale, + controller_params.z_scale); + std::vector input_sizes = {3, 7, 3, 6}; + auto target_state_mux = + builder.AddSystem(input_sizes); + auto end_effector_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(3)); + auto tray_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(6)); + builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), + target_state_mux->get_input_port(0)); + builder.Connect(plate_balancing_target->get_output_port_tray_target(), + target_state_mux->get_input_port(1)); + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(2)); + builder.Connect(tray_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(3)); + auto lcs_factory = builder.AddSystem( + robot_diagram_for_lcs_->plant(), + robot_diagram_for_lcs_->mutable_plant_context( + robot_diagram_root_context_.get()), + *plant_for_lcs_autodiff_, *plant_for_lcs_autodiff_context_, contact_pairs, + c3_options); + auto controller = builder.AddSystem( + robot_diagram_for_lcs_->plant(), c3_options); + auto c3_trajectory_generator = + builder.AddSystem( + robot_diagram_for_lcs_->plant(), c3_options); +// auto placeholder_trajectory = lcmt_timestamped_saved_traj(); + auto placeholder_solution = C3Output::C3Solution(); + placeholder_solution.x_sol_ = Eigen::MatrixXf::Zero(c3_options.g_x.size(), c3_options.N); + placeholder_solution.lambda_sol_ = Eigen::MatrixXf::Zero(c3_options.g_lambda.size(), c3_options.N); + placeholder_solution.u_sol_ = Eigen::MatrixXf::Zero(c3_options.g_u.size(), c3_options.N); + placeholder_solution.time_vector_ = Eigen::VectorXf::LinSpaced(c3_options.N, 0, 1); + auto discrete_time_delay = builder.AddSystem(1 / c3_options.publish_frequency, + 2, + drake::Value(placeholder_solution)); + std::vector state_names = { + "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", + "tray_qx", "tray_qy", "tray_qz", "tray_x", + "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", + "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", + "tray_vz", "tray_vz", "tray_vz", + }; + auto c3_state_sender = + builder.AddSystem(3 + 7 + 3 + 6, state_names); + c3_trajectory_generator->SetPublishEndEffectorOrientation( + controller_params.include_end_effector_orientation); + auto c3_output_sender = builder.AddSystem(); + auto passthrough = builder.AddSystem>(18); + + controller->SetOsqpSolverOptions(solver_options); + + // publisher connections + DRAKE_DEMAND(c3_options.publish_frequency > 0); + builder.Connect(controller->get_output_port_c3_solution(), + discrete_time_delay->get_input_port()); + + + builder.Connect(franka_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_franka_state()); + builder.Connect(target_state_mux->get_output_port(), + controller->get_input_port_target()); + builder.Connect(lcs_factory->get_output_port_lcs(), + controller->get_input_port_lcs()); + builder.Connect(tray_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_object_state()); + builder.Connect(tray_state_receiver->get_output_port(), + plate_balancing_target->get_input_port_tray_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + controller->get_input_port_lcs_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + lcs_factory->get_input_port_lcs_state()); + builder.Connect(passthrough->get_output_port(), + plate_balancing_target->get_input_port_radio()); + builder.Connect(discrete_time_delay->get_output_port(), + c3_trajectory_generator->get_input_port_c3_solution()); + builder.Connect(lcs_factory->get_output_port_lcs_contact_jacobian(), + c3_output_sender->get_input_port_lcs_contact_info()); + + builder.Connect(target_state_mux->get_output_port(), + c3_state_sender->get_input_port_target_state()); + builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), + c3_state_sender->get_input_port_actual_state()); + + builder.Connect(discrete_time_delay->get_output_port(), + c3_output_sender->get_input_port_c3_solution()); + builder.Connect(controller->get_output_port_c3_intermediates(), + c3_output_sender->get_input_port_c3_intermediates()); + + + if (publish_c3_debug){ + auto actor_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_actor_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto object_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_object_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_output_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_debug_output_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_target_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_target_state_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_actual_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_actual_state_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_forces_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_force_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + builder.Connect(c3_trajectory_generator->get_output_port_actor_trajectory(), + actor_trajectory_sender->get_input_port()); + builder.Connect(c3_trajectory_generator->get_output_port_object_trajectory(), + object_trajectory_sender->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_target_c3_state(), + c3_target_state_publisher->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), + c3_actual_state_publisher->get_input_port()); + builder.Connect(c3_output_sender->get_output_port_c3_debug(), + c3_output_publisher->get_input_port()); + builder.Connect(c3_output_sender->get_output_port_c3_force(), + c3_forces_publisher->get_input_port()); + } + + + + // Publisher connections + franka_state_port_ = builder.ExportInput(franka_state_receiver->get_input_port(), + "franka_state: lcmt_robot_output"); + tray_state_port_ = builder.ExportInput(tray_state_receiver->get_input_port(), + "tray_state: lcmt_object_state"); + radio_port_ = builder.ExportInput(passthrough->get_input_port(), "raw_radio"); + mpc_plan_port_ = builder.ExportOutput( + c3_trajectory_generator->get_output_port_actor_trajectory(), + "actor_trajectory"); + + builder.BuildInto(this); + this->set_name("FrankaC3Controller"); +} + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/franka/diagrams/franka_c3_controller_diagram.h b/examples/franka/diagrams/franka_c3_controller_diagram.h new file mode 100644 index 0000000000..4e0abb92c4 --- /dev/null +++ b/examples/franka/diagrams/franka_c3_controller_diagram.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "solvers/c3_options.h" + +namespace dairlib { +namespace examples { +namespace controllers { + +class FrankaC3ControllerDiagram : public drake::systems::Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaC3ControllerDiagram) + + /// @param[in] osc_gains_filename filepath containing the osc_running_gains. + /// @param[in] osqp_settings filepath containing the osqp settings. + FrankaC3ControllerDiagram(const std::string& controller_settings, const C3Options c3_options, + const std::string& lcm_channels, + drake::lcm::DrakeLcm* lcm, bool publish_c3_debug = false); + + /// @return the input port for the plant state. + const drake::systems::InputPort& get_input_port_robot_state() const { + return this->get_input_port(franka_state_port_); + } + + /// @return the input port for the plant state. + const drake::systems::InputPort& get_input_port_object_state() const { + return this->get_input_port(tray_state_port_); + } + + /// @return the input port for the cassie_out struct (containing radio + /// commands). + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + /// @return the output port for the lcmt_robot_input message. + const drake::systems::OutputPort& get_output_port_mpc_plan() const { + return this->get_output_port(mpc_plan_port_); + } + + private: + drake::multibody::MultibodyPlant* plant_franka_; + drake::multibody::MultibodyPlant* plant_tray_; + + // Storage for the diagram and its plant and scene graph. + // After Build(), the `builder_` is set to nullptr. + std::unique_ptr> robot_diagram_for_lcs_; + + std::unique_ptr> robot_diagram_root_context_; + std::unique_ptr> plant_for_lcs_diagram_context_; + std::unique_ptr> plant_for_lcs_autodiff_; + std::unique_ptr> plant_franka_context_; + std::unique_ptr> plant_tray_context_; + std::unique_ptr> plant_for_lcs_autodiff_context_; + + drake::systems::InputPortIndex franka_state_port_; + drake::systems::InputPortIndex tray_state_port_; + drake::systems::InputPortIndex radio_port_; + drake::systems::OutputPortIndex mpc_plan_port_; +}; + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/franka/diagrams/franka_osc_controller_diagram.cc b/examples/franka/diagrams/franka_osc_controller_diagram.cc new file mode 100644 index 0000000000..854581ab80 --- /dev/null +++ b/examples/franka/diagrams/franka_osc_controller_diagram.cc @@ -0,0 +1,269 @@ +// +// Created by yangwill on 2/19/24. +// + +#include "examples/franka/diagrams/franka_osc_controller_diagram.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_osc_controller_params.h" +#include "systems/controllers/osc/end_effector_force.h" +#include "systems/controllers/osc/end_effector_orientation.h" +#include "systems/controllers/osc/end_effector_position.h" +#include "lcm/lcm_trajectory.h" +#include "multibody/multibody_utils.h" +#include "systems/controllers/gravity_compensator.h" +#include "systems/controllers/osc/operational_space_control.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/lcm_trajectory_systems.h" + +namespace dairlib { + +namespace examples { +namespace controllers { + +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; + +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; +using std::string; + +FrankaOSCControllerDiagram::FrankaOSCControllerDiagram( + const string& controller_settings, const string& lcm_channels, + drake::lcm::DrakeLcm* lcm) { + DiagramBuilder builder; + + // load parameters + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaControllerParams controller_params = + drake::yaml::LoadYamlFile(controller_settings); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels); + OSCGains gains = drake::yaml::LoadYamlFile( + FindResourceOrThrow(controller_settings), {}, {}, yaml_options); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow( + "examples/franka/parameters/franka_osc_qp_settings.yaml")) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + plant_ = new drake::multibody::MultibodyPlant(0.0); + Parser parser(plant_, nullptr); + parser.AddModelsFromUrl(controller_params.franka_model); + + RigidTransform X_WI = RigidTransform::Identity(); + plant_->WeldFrames(plant_->world_frame(), + plant_->GetFrameByName("panda_link0"), X_WI); + + if (!controller_params.end_effector_name.empty()) { + drake::multibody::ModelInstanceIndex end_effector_index = parser.AddModels( + FindResourceOrThrow(controller_params.end_effector_model))[0]; + RigidTransform T_EE_W = + RigidTransform(drake::math::RotationMatrix(), + controller_params.tool_attachment_frame); + plant_->WeldFrames( + plant_->GetFrameByName("panda_link7"), + plant_->GetFrameByName(controller_params.end_effector_name, + end_effector_index), + T_EE_W); + } else { + std::cout << "OSC plant has been constructed with no end effector." + << std::endl; + } + + plant_->Finalize(); + plant_context_ = plant_->CreateDefaultContext(); + + auto state_receiver = + builder.AddSystem(*plant_); + auto end_effector_position_receiver = + builder.AddSystem( + "end_effector_position_target"); + auto end_effector_force_receiver = + builder.AddSystem( + "end_effector_force_target"); + auto end_effector_orientation_receiver = + builder.AddSystem( + "end_effector_orientation_target"); + auto franka_command_sender = + builder.AddSystem(*plant_); + auto osc_command_sender = + builder.AddSystem(*plant_); + auto end_effector_trajectory = + builder.AddSystem(*plant_, + plant_context_.get(), controller_params.neutral_position, false, + controller_params.end_effector_name); + auto passthrough = builder.AddSystem>(18); + end_effector_trajectory->SetRemoteControlParameters( + controller_params.neutral_position, controller_params.x_scale, + controller_params.y_scale, controller_params.z_scale); + auto end_effector_orientation_trajectory = + builder.AddSystem(); + end_effector_orientation_trajectory->SetTrackOrientation( + controller_params.track_end_effector_orientation); + auto end_effector_force_trajectory = + builder.AddSystem(); + auto osc = builder.AddSystem( + *plant_, plant_context_.get(), false); + if (controller_params.publish_debug_info) { + auto franka_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_input_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto osc_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto osc_debug_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_debug_channel, lcm, + TriggerTypeSet({TriggerType::kForced}))); + builder.Connect(osc->get_output_port_osc_debug(), + osc_debug_pub->get_input_port()); + builder.Connect(franka_command_sender->get_output_port(), + franka_command_pub->get_input_port()); + builder.Connect(osc_command_sender->get_output_port(), + osc_command_pub->get_input_port()); + } + + auto end_effector_position_tracking_data = + std::make_unique( + "end_effector_target", controller_params.K_p_end_effector, + controller_params.K_d_end_effector, controller_params.W_end_effector, + *plant_, *plant_); + end_effector_position_tracking_data->AddPointToTrack( + controller_params.end_effector_name); + const VectorXd& end_effector_acceleration_limits = + controller_params.end_effector_acceleration * Vector3d::Ones(); + end_effector_position_tracking_data->SetCmdAccelerationBounds( + -end_effector_acceleration_limits, end_effector_acceleration_limits); + auto mid_link_position_tracking_data_for_rel = + std::make_unique( + "panda_joint2_target", controller_params.K_p_mid_link, + controller_params.K_d_mid_link, controller_params.W_mid_link, *plant_, + *plant_); + mid_link_position_tracking_data_for_rel->AddJointToTrack("panda_joint2", + "panda_joint2dot"); + + auto end_effector_force_tracking_data = + std::make_unique( + "end_effector_force", controller_params.W_ee_lambda, *plant_, *plant_, + controller_params.end_effector_name, Vector3d::Zero()); + + auto end_effector_orientation_tracking_data = + std::make_unique( + "end_effector_orientation_target", + controller_params.K_p_end_effector_rot, + controller_params.K_d_end_effector_rot, + controller_params.W_end_effector_rot, *plant_, *plant_); + end_effector_orientation_tracking_data->AddFrameToTrack( + controller_params.end_effector_name); + Eigen::VectorXd orientation_target = Eigen::VectorXd::Zero(4); + orientation_target(0) = 1; + osc->AddTrackingData(std::move(end_effector_position_tracking_data)); + osc->AddConstTrackingData(std::move(mid_link_position_tracking_data_for_rel), + 1.6 * VectorXd::Ones(1)); + osc->AddTrackingData(std::move(end_effector_orientation_tracking_data)); + osc->AddForceTrackingData(std::move(end_effector_force_tracking_data)); + osc->SetAccelerationCostWeights(gains.W_acceleration); + osc->SetInputCostWeights(gains.W_input_regularization); + osc->SetInputSmoothingCostWeights(gains.W_input_smoothing_regularization); + if (controller_params.enforce_acceleration_constraints) { + osc->EnableAccelerationConstraints(); + } else { + osc->DisableAccelerationConstraints(); + } + osc->SetContactFriction(controller_params.mu); + osc->SetOsqpSolverOptions(solver_options); + + osc->Build(); + + if (controller_params.cancel_gravity_compensation) { + auto gravity_compensator = + builder.AddSystem(*plant_, + *plant_context_); + builder.Connect(osc->get_output_port_osc_command(), + gravity_compensator->get_input_port()); + builder.Connect(gravity_compensator->get_output_port(), + franka_command_sender->get_input_port()); + } else { + DRAKE_DEMAND(lcm_channels == + "examples/franka/parameters/lcm_channels_simulation.yaml"); + builder.Connect(osc->get_output_port_osc_command(), + franka_command_sender->get_input_port(0)); + } + + builder.Connect(passthrough->get_output_port(), + end_effector_trajectory->get_input_port_radio()); + builder.Connect(passthrough->get_output_port(), + end_effector_orientation_trajectory->get_input_port_radio()); + builder.Connect(passthrough->get_output_port(), + end_effector_force_trajectory->get_input_port_radio()); + + builder.Connect(osc->get_output_port_osc_command(), + osc_command_sender->get_input_port(0)); + builder.Connect(state_receiver->get_output_port(0), + osc->get_input_port_robot_output()); + builder.Connect(end_effector_position_receiver->get_output_port(0), + end_effector_trajectory->get_input_port_trajectory()); + builder.Connect( + end_effector_orientation_receiver->get_output_port(0), + end_effector_orientation_trajectory->get_input_port_trajectory()); + builder.Connect(end_effector_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_target")); + builder.Connect( + end_effector_orientation_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_orientation_target")); + builder.Connect(end_effector_force_receiver->get_output_port(0), + end_effector_force_trajectory->get_input_port_trajectory()); + builder.Connect(end_effector_force_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_force")); + + // Publisher connections + franka_state_port_ = builder.ExportInput(state_receiver->get_input_port(), + "lcmt_robot_output"); + end_effector_position_port_ = + builder.ExportInput(end_effector_position_receiver->get_input_port(), + "end_effector_position: lcmt_timestamped_trajectory"); + end_effector_orientation_port_ = builder.ExportInput( + end_effector_orientation_receiver->get_input_port(), + "end_effector_orientation: lcmt_timestamped_trajectory"); + end_effector_force_port_ = + builder.ExportInput(end_effector_force_receiver->get_input_port(), + "end_effector_force: lcmt_timestamped_trajectory"); + radio_port_ = builder.ExportInput(passthrough->get_input_port(), "raw_radio"); + builder.ExportOutput(osc->get_output_port_osc_command(), "robot_input"); + + builder.BuildInto(this); + this->set_name("FrankaOSCController"); +} + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/franka/diagrams/franka_osc_controller_diagram.h b/examples/franka/diagrams/franka_osc_controller_diagram.h new file mode 100644 index 0000000000..4c5a48e058 --- /dev/null +++ b/examples/franka/diagrams/franka_osc_controller_diagram.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/external_force_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" + +namespace dairlib { +namespace examples { +namespace controllers { + +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; +using systems::controllers::ExternalForceTrackingData; + +class FrankaOSCControllerDiagram : public drake::systems::Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaOSCControllerDiagram) + + /// @param[in] osc_gains_filename filepath containing the osc_running_gains. + /// @param[in] osqp_settings filepath containing the osqp settings. + FrankaOSCControllerDiagram(const std::string& controller_settings, + const std::string& lcm_channels, + drake::lcm::DrakeLcm* lcm); + + /// @return the input port for the plant state. + const drake::systems::InputPort& get_input_port_robot_state() const { + return this->get_input_port(franka_state_port_); + } + + const drake::systems::InputPort& get_input_port_end_effector_position() const { + return this->get_input_port(end_effector_position_port_); + } + + const drake::systems::InputPort& get_input_port_end_effector_orientation() const { + return this->get_input_port(end_effector_orientation_port_); + } + + const drake::systems::InputPort& get_input_port_end_effector_force() const { + return this->get_input_port(end_effector_force_port_); + } + + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + /// @return the output port for the lcmt_robot_input message. + const drake::systems::OutputPort& get_output_port_robot_input() const { + return this->get_output_port(robot_input_port_); + } + + private: + drake::multibody::MultibodyPlant* plant_; + std::unique_ptr> plant_context_; + + drake::systems::InputPortIndex franka_state_port_; + drake::systems::InputPortIndex end_effector_position_port_; + drake::systems::InputPortIndex end_effector_orientation_port_; + drake::systems::InputPortIndex end_effector_force_port_; + drake::systems::InputPortIndex radio_port_; + const drake::systems::OutputPortIndex robot_input_port_ = + drake::systems::OutputPortIndex(0); +}; + +} // namespace controllers +} // namespace examples +} // namespace dairlib diff --git a/examples/franka/diagrams/franka_sim_diagram.cc b/examples/franka/diagrams/franka_sim_diagram.cc new file mode 100644 index 0000000000..584acef837 --- /dev/null +++ b/examples/franka/diagrams/franka_sim_diagram.cc @@ -0,0 +1,141 @@ + +#include "franka_sim_diagram.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "common/find_resource.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "examples/franka/parameters/franka_sim_scene_params.h" +#include "systems/robot_lcm_systems.h" + +namespace dairlib { +namespace examples { +using drake::systems::lcm::LcmPublisherSystem; + +using drake::geometry::GeometrySet; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::systems::DiagramBuilder; + +FrankaSimDiagram::FrankaSimDiagram(std::unique_ptr> plant, + drake::lcm::DrakeLcm* lcm) { + // load parameters + DiagramBuilder builder; + scene_graph_ = builder.AddSystem(); + scene_graph_->set_name("scene_graph"); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile("examples/franka/parameters/lcm_channels_simulation.yaml"); + FrankaSimSceneParams scene_params = + drake::yaml::LoadYamlFile( + sim_params.sim_scene_file[sim_params.scene_index]); + /// Sim Start + plant_ = builder.AddSystem(std::move(plant)); + + auto parser = drake::multibody::Parser(plant_, scene_graph_); +// lcs_diagram_builder.parser() lcs_diagram_builder.parser()(&plant); + parser.SetAutoRenaming(true); + drake::multibody::ModelInstanceIndex franka_index = + parser.AddModelsFromUrl(sim_params.franka_model)[0]; + drake::multibody::ModelInstanceIndex c3_end_effector_index = + parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; + drake::multibody::ModelInstanceIndex tray_index = + parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; + + RigidTransform T_X_W = RigidTransform( + drake::math::RotationMatrix(), Eigen::VectorXd::Zero(3)); + RigidTransform T_EE_W = RigidTransform( + drake::math::RotationMatrix(), sim_params.tool_attachment_frame); + plant_->WeldFrames(plant_->world_frame(), plant_->GetFrameByName("panda_link0"), + T_X_W); + plant_->WeldFrames(plant_->GetFrameByName("panda_link7"), + plant_->GetFrameByName("plate", c3_end_effector_index), + T_EE_W); + + // we WANT to model collisions between link5 and the supports + const drake::geometry::GeometrySet& franka_geom_set = + plant->CollectRegisteredGeometries({&plant->GetBodyByName("panda_link0"), + &plant->GetBodyByName("panda_link1"), + &plant->GetBodyByName("panda_link2"), + &plant->GetBodyByName("panda_link3"), + &plant->GetBodyByName("panda_link4")}); + drake::geometry::GeometrySet support_geom_set; + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = parser.AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + plant->WeldFrames( + plant->world_frame(), + plant->GetFrameByName("base", environment_model_indices[i]), + T_E_W); + support_geom_set.Add(plant->GetCollisionGeometriesForBody(plant->GetBodyByName("base", + environment_model_indices[i]))); + } + plant->ExcludeCollisionGeometriesWithCollisionFilterGroupPair( + {"supports", support_geom_set}, {"franka", franka_geom_set}); + + const drake::geometry::GeometrySet& paddle_geom_set = + plant_->CollectRegisteredGeometries({ + &plant_->GetBodyByName("panda_link2"), + &plant_->GetBodyByName("panda_link3"), + &plant_->GetBodyByName("panda_link4"), + &plant_->GetBodyByName("panda_link5"), + &plant_->GetBodyByName("panda_link6"), + &plant_->GetBodyByName("panda_link8"), + }); + auto tray_collision_set = GeometrySet( + plant_->GetCollisionGeometriesForBody(plant_->GetBodyByName("tray"))); + plant_->ExcludeCollisionGeometriesWithCollisionFilterGroupPair( + {"paddle", paddle_geom_set}, {"tray", tray_collision_set}); + + plant_->Finalize(); + auto tray_state_sender = + builder.AddSystem(*plant_, tray_index); + auto franka_state_sender = + builder.AddSystem(*plant_, franka_index, false); + + // for lcm debugging + auto state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_state_channel, lcm, + drake::systems::TriggerTypeSet( + {drake::systems::TriggerType::kForced}))); + auto tray_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.tray_state_channel, lcm, + drake::systems::TriggerTypeSet( + {drake::systems::TriggerType::kForced}))); + builder.Connect(plant_->get_state_output_port(tray_index), + tray_state_sender->get_input_port_state()); + builder.Connect(plant_->get_state_output_port(franka_index), + franka_state_sender->get_input_port_state()); + builder.Connect(*franka_state_sender, *state_pub); + builder.Connect(*tray_state_sender, *tray_state_pub); + // end lcm debugging + + actuation_port_ = builder.ExportInput(plant_->get_actuation_input_port(), + "franka_sim: robot_efforts"); + tray_state_port_ = builder.ExportOutput(tray_state_sender->get_output_port(), + "franka_sim: tray_state"); + franka_state_port_ = builder.ExportOutput(franka_state_sender->get_output_port(), + "franka_sim: franka_state"); + + builder.BuildInto(this); + this->set_name("FrankaSim"); + +} +} // namespace examples +} // namespace dairlib \ No newline at end of file diff --git a/examples/franka/diagrams/franka_sim_diagram.h b/examples/franka/diagrams/franka_sim_diagram.h new file mode 100644 index 0000000000..daf0d8f235 --- /dev/null +++ b/examples/franka/diagrams/franka_sim_diagram.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/systems/framework/diagram.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/system.h" + +namespace dairlib { +namespace examples { + +class FrankaSimDiagram : public drake::systems::Diagram { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaSimDiagram) + + /// @param[in] urdf filepath containing the osc_running_gains. + FrankaSimDiagram( + std::unique_ptr> plant, + drake::lcm::DrakeLcm* lcm); + + /// @return the input port for the actuation command. + const drake::systems::InputPort& get_input_port_actuation() const { + return this->get_input_port(actuation_port_); + } + + /// @return the output port for the plant state as an OutputVector. + const drake::systems::OutputPort& get_output_port_tray_state() const { + return this->get_output_port(tray_state_port_); + } + + /// @return the output port for the plant state as an OutputVector. + const drake::systems::OutputPort& get_output_port_franka_state() + const { + return this->get_output_port(franka_state_port_); + } + + const drake::multibody::MultibodyPlant& get_plant() { return *plant_; } + + private: + drake::multibody::MultibodyPlant* plant_; + drake::geometry::SceneGraph* scene_graph_; + + drake::systems::InputPortIndex actuation_port_; + drake::systems::OutputPortIndex tray_state_port_; + drake::systems::OutputPortIndex franka_state_port_; +}; + +} // namespace examples +} // namespace dairlib diff --git a/examples/franka/diagrams/full_diagram.cc b/examples/franka/diagrams/full_diagram.cc new file mode 100644 index 0000000000..a988919a05 --- /dev/null +++ b/examples/franka/diagrams/full_diagram.cc @@ -0,0 +1,146 @@ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "examples/franka/diagrams/franka_c3_controller_diagram.h" +#include "examples/franka/diagrams/franka_osc_controller_diagram.h" +#include "examples/franka/diagrams/franka_sim_diagram.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "multibody/multibody_utils.h" +#include "systems/primitives/subvector_pass_through.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +namespace dairlib { + +using drake::geometry::GeometrySet; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::VectorXd; +using examples::controllers::FrankaC3ControllerDiagram; +using examples::controllers::FrankaOSCControllerDiagram; +using systems::RobotInputReceiver; +using systems::RobotOutputSender; +using systems::SubvectorPassThrough; + +int DoMain(int argc, char* argv[]) { + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile( + "examples/franka/parameters/lcm_channels_simulation.yaml"); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + + DiagramBuilder builder; + std::unique_ptr> sim_plant = + std::make_unique>(sim_params.dt); + + auto franka_sim = + builder.AddSystem(std::move(sim_plant), &lcm); + + /// OSC + auto osc_controller = builder.AddSystem( + "examples/franka/parameters/franka_osc_controller_params.yaml", + "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); + + /// C3 plant + auto c3_controller = builder.AddSystem( + "examples/franka/parameters/franka_c3_controller_params.yaml", + "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); + + /* -------------------------------------------------------------------------------------------*/ + auto passthrough = builder.AddSystem( + osc_controller->get_output_port_robot_input().size(), 0, + franka_sim->get_input_port_actuation().size()); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + + //// OSC connections + + // Diagram Connections + builder.Connect(osc_controller->get_output_port_robot_input(), + passthrough->get_input_port()); + builder.Connect(c3_controller->get_output_port_mpc_plan(), + osc_controller->get_input_port_end_effector_position()); + builder.Connect(c3_controller->get_output_port_mpc_plan(), + osc_controller->get_input_port_end_effector_orientation()); + builder.Connect(c3_controller->get_output_port_mpc_plan(), + osc_controller->get_input_port_end_effector_force()); + + builder.Connect(franka_sim->get_output_port_franka_state(), + osc_controller->get_input_port_robot_state()); + builder.Connect(franka_sim->get_output_port_franka_state(), + c3_controller->get_input_port_robot_state()); + builder.Connect(franka_sim->get_output_port_tray_state(), + c3_controller->get_input_port_object_state()); + + builder.Connect(radio_sub->get_output_port(), + c3_controller->get_input_port_radio()); + builder.Connect(radio_sub->get_output_port(), + osc_controller->get_input_port_radio()); + + builder.Connect(passthrough->get_output_port(), + franka_sim->get_input_port_actuation()); + + auto diagram = builder.Build(); + diagram->set_name("plate_balancing_full_diagram"); + DrawAndSaveDiagramGraph(*diagram); + + drake::systems::Simulator simulator(*diagram); + + simulator.set_publish_every_time_step(true); + simulator.set_publish_at_initialization(true); + simulator.set_target_realtime_rate(sim_params.realtime_rate); + + + auto diagram_context = diagram->CreateDefaultContext(); + auto& plant = franka_sim->get_plant(); + auto& plant_context = + diagram->GetMutableSubsystemContext(plant, diagram_context.get()); + int nq = plant.num_positions(); + int nv = plant.num_velocities(); + VectorXd q = VectorXd::Zero(nq); + q.head(7) = sim_params.q_init_franka; + + q.tail(7) = sim_params.q_init_tray[sim_params.scene_index]; + + plant.SetPositions(&plant_context, q); + + VectorXd v = VectorXd::Zero(nv); + plant.SetVelocities(&plant_context, v); + + simulator.Initialize(); + simulator.AdvanceTo(std::numeric_limits::infinity()); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/forward_kinematics_for_lcs.cc b/examples/franka/forward_kinematics_for_lcs.cc new file mode 100644 index 0000000000..b33493d765 --- /dev/null +++ b/examples/franka/forward_kinematics_for_lcs.cc @@ -0,0 +1,206 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/franka/parameters/franka_c3_controller_params.h" +#include "examples/franka/parameters/franka_c3_scene_params.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "systems/senders/c3_state_sender.h" +#include "examples/franka/systems/c3_trajectory_generator.h" +#include "systems/franka_kinematics.h" +#include "examples/franka/systems/plate_balancing_target.h" +#include "multibody/multibody_utils.h" +#include "solvers/lcs_factory.h" +#include "systems/controllers/c3/lcs_factory_system.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +namespace dairlib { + +using dairlib::solvers::LCSFactory; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; + +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; + +DEFINE_string(controller_settings, + "examples/franka/parameters/franka_c3_controller_params.yaml", + "Controller settings such as channels. Attempting to minimize " + "number of gflags"); +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_simulation.yaml", + "Filepath containing lcm channels"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + // load parameters + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + FLAGS_controller_settings); + FrankaC3SceneParams scene_params = + drake::yaml::LoadYamlFile( + controller_params.c3_scene_file[controller_params.scene_index]); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + + DiagramBuilder plant_builder; + + MultibodyPlant plant_franka(0.0); + Parser parser_franka(&plant_franka, nullptr); + parser_franka.AddModelsFromUrl(scene_params.franka_model); + drake::multibody::ModelInstanceIndex end_effector_index = + parser_franka.AddModels( + FindResourceOrThrow(scene_params.end_effector_model))[0]; + + RigidTransform X_WI = RigidTransform::Identity(); + plant_franka.WeldFrames(plant_franka.world_frame(), + plant_franka.GetFrameByName("panda_link0"), X_WI); + + RigidTransform T_EE_W = + RigidTransform(drake::math::RotationMatrix(), + scene_params.tool_attachment_frame); + plant_franka.WeldFrames( + plant_franka.GetFrameByName("panda_link7"), + plant_franka.GetFrameByName("plate", end_effector_index), T_EE_W); + + plant_franka.Finalize(); + auto franka_context = plant_franka.CreateDefaultContext(); + + /// + MultibodyPlant plant_tray(0.0); + Parser parser_tray(&plant_tray, nullptr); + parser_tray.AddModels(scene_params.object_models[0]); + plant_tray.Finalize(); + auto tray_context = plant_tray.CreateDefaultContext(); + + DiagramBuilder builder; + + auto tray_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.tray_state_channel, &lcm)); + auto franka_state_receiver = + builder.AddSystem(plant_franka); + auto tray_state_receiver = + builder.AddSystem(plant_tray); + auto reduced_order_model_receiver = + builder.AddSystem( + plant_franka, franka_context.get(), plant_tray, tray_context.get(), + scene_params.end_effector_name, "tray", + controller_params.include_end_effector_orientation); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + auto radio_to_vector = builder.AddSystem(); + + std::vector state_names = { + "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", + "tray_qx", "tray_qy", "tray_qz", "tray_x", + "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", + "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", + "tray_vz", "tray_vz", "tray_vz", + }; + auto c3_state_sender = + builder.AddSystem(3 + 7 + 3 + 6, state_names); + auto plate_balancing_target = + builder.AddSystem( + plant_tray, scene_params.end_effector_thickness, + controller_params.near_target_threshold); + plate_balancing_target->SetRemoteControlParameters( + controller_params.first_target[controller_params.scene_index], controller_params.second_target[controller_params.scene_index], + controller_params.third_target[controller_params.scene_index], controller_params.x_scale, + controller_params.y_scale, controller_params.z_scale); + std::vector input_sizes = {3, 7, 3, 6}; + auto target_state_mux = + builder.AddSystem(input_sizes); + auto end_effector_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(3)); + auto tray_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(6)); + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(2)); + builder.Connect(tray_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(3)); + + + auto c3_actual_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_actual_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_target_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_target_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + builder.Connect(*radio_sub, *radio_to_vector); + builder.Connect(tray_state_receiver->get_output_port(), + plate_balancing_target->get_input_port_tray_state()); + builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), + target_state_mux->get_input_port(0)); + builder.Connect(plate_balancing_target->get_output_port_tray_target(), + target_state_mux->get_input_port(1)); + builder.Connect(tray_state_sub->get_output_port(), + tray_state_receiver->get_input_port()); + builder.Connect(radio_to_vector->get_output_port(), + plate_balancing_target->get_input_port_radio()); + builder.Connect(franka_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_franka_state()); + builder.Connect(tray_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_object_state()); + builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), + c3_state_sender->get_input_port_actual_state()); + builder.Connect(target_state_mux->get_output_port(), + c3_state_sender->get_input_port_target_state()); + builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), + c3_actual_state_publisher->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_target_c3_state(), + c3_target_state_publisher->get_input_port()); + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_forward_kinematics")); + + // Run lcm-driven simulation + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, franka_state_receiver, + lcm_channel_params.franka_state_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + + LcmHandleSubscriptionsUntil( + &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; }); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_bridge_driver_in.cc b/examples/franka/franka_bridge_driver_in.cc new file mode 100644 index 0000000000..a1f49422e0 --- /dev/null +++ b/examples/franka/franka_bridge_driver_in.cc @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/parameters/franka_drake_lcm_driver_channels.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "systems/franka_state_translator.h" +#include "multibody/multibody_utils.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; + +using dairlib::systems::RobotInputReceiver; +using dairlib::systems::RobotOutputSender; +using dairlib::systems::SubvectorPassThrough; +using dairlib::systems::TimestampedVector; + +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_hardware.yaml", + "Filepath containing lcm channels"); +DEFINE_string(franka_driver_channels, + "common/parameters/franka_drake_lcm_driver_channels.yaml", + "Filepath containing drake franka driver channels"); + +namespace dairlib { + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + FrankaDrakeLcmDriverChannels franka_driver_channel_params = + drake::yaml::LoadYamlFile(FLAGS_franka_driver_channels); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + + DiagramBuilder builder; + + MultibodyPlant plant(0.0); + + Parser parser(&plant); + parser.AddModelsFromUrl(sim_params.franka_model); + Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); + RigidTransform R_X_W = RigidTransform( + drake::math::RotationMatrix(), franka_origin); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + R_X_W); + plant.Finalize(); + + auto pos_map = multibody::MakeNameToPositionsMap(plant); + auto vel_map = multibody::MakeNameToVelocitiesMap(plant); + auto act_map = multibody::MakeNameToActuatorsMap(plant); + + auto pos_names = multibody::ExtractOrderedNamesFromMap(pos_map); + auto vel_names = multibody::ExtractOrderedNamesFromMap(vel_map); + auto act_names = multibody::ExtractOrderedNamesFromMap(act_map); + + /* -------------------------------------------------------------------------------------------*/ + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + auto franka_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + franka_driver_channel_params.franka_command_channel, &lcm, + 1.0 / 1000.0)); + auto franka_command_translator = builder.AddSystem(); + + builder.Connect(*franka_command_translator, *franka_command_pub); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_bridge_driver_in")); + + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, franka_command_translator, + lcm_channel_params.franka_input_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + loop.Simulate(); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_bridge_driver_out.cc b/examples/franka/franka_bridge_driver_out.cc new file mode 100644 index 0000000000..f43deb43c8 --- /dev/null +++ b/examples/franka/franka_bridge_driver_out.cc @@ -0,0 +1,106 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/parameters/franka_drake_lcm_driver_channels.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "systems/franka_state_translator.h" +#include "multibody/multibody_utils.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; + +using dairlib::systems::RobotInputReceiver; +using dairlib::systems::RobotOutputSender; +using dairlib::systems::SubvectorPassThrough; +using dairlib::systems::TimestampedVector; + +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_hardware.yaml", + "Filepath containing lcm channels"); +DEFINE_string(franka_driver_channels, + "common/parameters/franka_drake_lcm_driver_channels.yaml", + "Filepath containing drake franka driver channels"); + +namespace dairlib { + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + FrankaDrakeLcmDriverChannels franka_driver_channel_params = + drake::yaml::LoadYamlFile(FLAGS_franka_driver_channels); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + + DiagramBuilder builder; + + MultibodyPlant plant(0.0); + + Parser parser(&plant); + parser.AddModelsFromUrl(sim_params.franka_model); + Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); + RigidTransform R_X_W = RigidTransform( + drake::math::RotationMatrix(), franka_origin); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + R_X_W); + plant.Finalize(); + + auto pos_map = multibody::MakeNameToPositionsMap(plant); + auto vel_map = multibody::MakeNameToVelocitiesMap(plant); + auto act_map = multibody::MakeNameToActuatorsMap(plant); + + auto pos_names = multibody::ExtractOrderedNamesFromMap(pos_map); + auto vel_names = multibody::ExtractOrderedNamesFromMap(vel_map); + auto act_names = multibody::ExtractOrderedNamesFromMap(act_map); + + /* -------------------------------------------------------------------------------------------*/ + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + + auto franka_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_state_channel, &lcm, + 1.0 / 1000.0)); + auto franka_state_translator = builder.AddSystem( + pos_names, vel_names, act_names); + + builder.Connect(*franka_state_translator, *franka_state_pub); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_bridge_driver_out")); + + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, franka_state_translator, + franka_driver_channel_params.franka_status_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + loop.Simulate(); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_c3_controller.cc b/examples/franka/franka_c3_controller.cc new file mode 100644 index 0000000000..b0a2025810 --- /dev/null +++ b/examples/franka/franka_c3_controller.cc @@ -0,0 +1,336 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "examples/franka/parameters/franka_c3_controller_params.h" +#include "examples/franka/parameters/franka_c3_scene_params.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "systems/senders/c3_state_sender.h" +#include "examples/franka/systems/c3_trajectory_generator.h" +#include "systems/franka_kinematics.h" +#include "examples/franka/systems/plate_balancing_target.h" +#include "multibody/multibody_utils.h" +#include "solvers/lcs_factory.h" +#include "systems/controllers/c3/lcs_factory_system.h" +#include "systems/controllers/c3/c3_controller.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/c3_output_systems.h" + +namespace dairlib { + +using dairlib::solvers::LCSFactory; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; + +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; + +DEFINE_string(controller_settings, + "examples/franka/parameters/franka_c3_controller_params.yaml", + "Controller settings such as channels. Attempting to minimize " + "number of gflags"); +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_simulation.yaml", + "Filepath containing lcm channels"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + // load parameters + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + FLAGS_controller_settings); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + C3Options c3_options = drake::yaml::LoadYamlFile( + controller_params.c3_options_file[controller_params.scene_index]); + FrankaC3SceneParams scene_params = + drake::yaml::LoadYamlFile( + controller_params.c3_scene_file[controller_params.scene_index]); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(controller_params.osqp_settings_file)) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + + MultibodyPlant plant_franka(0.0); + Parser parser_franka(&plant_franka, nullptr); + parser_franka.AddModelsFromUrl(scene_params.franka_model); + drake::multibody::ModelInstanceIndex end_effector_index = + parser_franka.AddModels( + FindResourceOrThrow(scene_params.end_effector_model))[0]; + + RigidTransform X_WI = RigidTransform::Identity(); + plant_franka.WeldFrames(plant_franka.world_frame(), + plant_franka.GetFrameByName("panda_link0"), X_WI); + + RigidTransform T_EE_W = + RigidTransform(drake::math::RotationMatrix(), + scene_params.tool_attachment_frame); + plant_franka.WeldFrames( + plant_franka.GetFrameByName("panda_link7"), + plant_franka.GetFrameByName("plate", end_effector_index), T_EE_W); + + plant_franka.Finalize(); + auto franka_context = plant_franka.CreateDefaultContext(); + + /// + MultibodyPlant plant_tray(0.0); + Parser parser_tray(&plant_tray, nullptr); + parser_tray.AddModels(scene_params.object_models[0]); + plant_tray.Finalize(); + auto tray_context = plant_tray.CreateDefaultContext(); + + /// + DiagramBuilder plant_builder; + auto [plant_for_lcs, scene_graph] = + AddMultibodyPlantSceneGraph(&plant_builder, 0.0); + Parser lcs_parser(&plant_for_lcs); + lcs_parser.SetAutoRenaming(true); + lcs_parser.AddModels(scene_params.end_effector_lcs_model); + + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = lcs_parser.AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + plant_for_lcs.WeldFrames( + plant_for_lcs.world_frame(), + plant_for_lcs.GetFrameByName("base", environment_model_indices[i]), + T_E_W); + } + for (int i = 0; i < scene_params.object_models.size(); ++i) { + lcs_parser.AddModels(scene_params.object_models[i]); + } + + plant_for_lcs.WeldFrames(plant_for_lcs.world_frame(), + plant_for_lcs.GetFrameByName("base_link"), X_WI); + plant_for_lcs.Finalize(); + std::unique_ptr> plant_for_lcs_autodiff = + drake::systems::System::ToAutoDiffXd(plant_for_lcs); + + auto plant_diagram = plant_builder.Build(); + std::unique_ptr> diagram_context = + plant_diagram->CreateDefaultContext(); + auto& plant_for_lcs_context = plant_diagram->GetMutableSubsystemContext( + plant_for_lcs, diagram_context.get()); + auto plate_context_ad = plant_for_lcs_autodiff->CreateDefaultContext(); + + /// + std::vector end_effector_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("plate")); + for (int i = 0; i < environment_model_indices.size(); ++i) { + std::vector + environment_support_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("base", + environment_model_indices[i])); + end_effector_contact_points.insert( + end_effector_contact_points.end(), + environment_support_contact_points.begin(), + environment_support_contact_points.end()); + } + std::vector tray_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("tray")); + std::unordered_map> + contact_geoms; + contact_geoms["PLATE"] = end_effector_contact_points; + contact_geoms["TRAY"] = tray_geoms; + + std::vector> contact_pairs; + for (auto geom_id : contact_geoms["PLATE"]) { + contact_pairs.emplace_back(geom_id, contact_geoms["TRAY"][0]); + } + + DiagramBuilder builder; + + auto tray_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.tray_state_channel, &lcm)); + auto franka_state_receiver = + builder.AddSystem(plant_franka); + auto tray_state_receiver = + builder.AddSystem(plant_tray); + auto reduced_order_model_receiver = + builder.AddSystem( + plant_franka, franka_context.get(), plant_tray, tray_context.get(), + scene_params.end_effector_name, "tray", + controller_params.include_end_effector_orientation); + auto actor_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_actor_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + auto object_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_object_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + auto c3_output_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_debug_output_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_target_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_target_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_actual_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_actual_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_forces_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_force_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + auto radio_to_vector = builder.AddSystem(); + + auto plate_balancing_target = + builder.AddSystem( + plant_tray, scene_params.end_effector_thickness, + controller_params.near_target_threshold); + plate_balancing_target->SetRemoteControlParameters( + controller_params.first_target[controller_params.scene_index], + controller_params.second_target[controller_params.scene_index], + controller_params.third_target[controller_params.scene_index], + controller_params.x_scale, controller_params.y_scale, + controller_params.z_scale); + std::vector input_sizes = {3, 7, 3, 6}; + auto target_state_mux = + builder.AddSystem(input_sizes); + auto end_effector_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(3)); + builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), + target_state_mux->get_input_port(0)); + builder.Connect(plate_balancing_target->get_output_port_tray_target(), + target_state_mux->get_input_port(1)); + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(2)); + builder.Connect(plate_balancing_target->get_output_port_tray_velocity_target(), + target_state_mux->get_input_port(3)); + auto lcs_factory = builder.AddSystem( + plant_for_lcs, plant_for_lcs_context, *plant_for_lcs_autodiff, + *plate_context_ad, contact_pairs, c3_options); + auto controller = + builder.AddSystem(plant_for_lcs, c3_options); + auto c3_trajectory_generator = + builder.AddSystem(plant_for_lcs, + c3_options); + std::vector state_names = { + "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", + "tray_qx", "tray_qy", "tray_qz", "tray_x", + "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", + "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", + "tray_vz", "tray_vz", "tray_vz", + }; + auto c3_state_sender = + builder.AddSystem(3 + 7 + 3 + 6, state_names); + c3_trajectory_generator->SetPublishEndEffectorOrientation( + controller_params.include_end_effector_orientation); + auto c3_output_sender = builder.AddSystem(); + controller->SetOsqpSolverOptions(solver_options); + + builder.Connect(*radio_sub, *radio_to_vector); + builder.Connect(franka_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_franka_state()); + builder.Connect(target_state_mux->get_output_port(), + controller->get_input_port_target()); + builder.Connect(lcs_factory->get_output_port_lcs(), + controller->get_input_port_lcs()); + builder.Connect(tray_state_sub->get_output_port(), + tray_state_receiver->get_input_port()); + builder.Connect(tray_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_object_state()); + builder.Connect(tray_state_receiver->get_output_port(), + plate_balancing_target->get_input_port_tray_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + controller->get_input_port_lcs_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + lcs_factory->get_input_port_lcs_state()); + builder.Connect(radio_to_vector->get_output_port(), + plate_balancing_target->get_input_port_radio()); + builder.Connect(controller->get_output_port_c3_solution(), + c3_trajectory_generator->get_input_port_c3_solution()); + builder.Connect(lcs_factory->get_output_port_lcs_contact_jacobian(), + c3_output_sender->get_input_port_lcs_contact_info()); + builder.Connect(c3_trajectory_generator->get_output_port_actor_trajectory(), + actor_trajectory_sender->get_input_port()); + builder.Connect(c3_trajectory_generator->get_output_port_object_trajectory(), + object_trajectory_sender->get_input_port()); + builder.Connect(target_state_mux->get_output_port(), + c3_state_sender->get_input_port_target_state()); + builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), + c3_state_sender->get_input_port_actual_state()); + builder.Connect(c3_state_sender->get_output_port_target_c3_state(), + c3_target_state_publisher->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), + c3_actual_state_publisher->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution(), + c3_output_sender->get_input_port_c3_solution()); + builder.Connect(controller->get_output_port_c3_intermediates(), + c3_output_sender->get_input_port_c3_intermediates()); + builder.Connect(c3_output_sender->get_output_port_c3_debug(), + c3_output_publisher->get_input_port()); + builder.Connect(c3_output_sender->get_output_port_c3_force(), + c3_forces_publisher->get_input_port()); + // builder.Connect(c3_output_sender->get_output_port_next_c3_input(), + // lcs_factory->get_input_port_lcs_input()); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_c3_controller")); + plant_diagram->set_name(("franka_c3_plant")); + // DrawAndSaveDiagramGraph(*plant_diagram); + + // Run lcm-driven simulation + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, franka_state_receiver, + lcm_channel_params.franka_state_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + // auto& controller_context = loop.get_diagram()->GetMutableSubsystemContext( + // *controller, &loop.get_diagram_mutable_context()); + // controller->get_input_port_target().FixValue(&controller_context, x_des); + LcmHandleSubscriptionsUntil( + &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; }); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_c3_controller_two_objects.cc b/examples/franka/franka_c3_controller_two_objects.cc new file mode 100644 index 0000000000..cb793450c3 --- /dev/null +++ b/examples/franka/franka_c3_controller_two_objects.cc @@ -0,0 +1,350 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/franka/parameters/franka_c3_controller_params.h" +#include "examples/franka/parameters/franka_c3_scene_params.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "systems/senders/c3_state_sender.h" +#include "examples/franka/systems/c3_trajectory_generator.h" +#include "systems/franka_kinematics.h" +#include "examples/franka/systems/plate_balancing_target.h" +#include "multibody/multibody_utils.h" +#include "solvers/lcs_factory.h" +#include "systems/controllers/c3/lcs_factory_system.h" +#include "systems/controllers/c3/c3_controller.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/c3_output_systems.h" + +namespace dairlib { + +using dairlib::solvers::LCSFactory; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; + +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; + +DEFINE_string(controller_settings, + "examples/franka/parameters/franka_c3_controller_params.yaml", + "Controller settings such as channels. Attempting to minimize " + "number of gflags"); +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_simulation.yaml", + "Filepath containing lcm channels"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + // load parameters + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + FLAGS_controller_settings); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + C3Options c3_options = drake::yaml::LoadYamlFile( + controller_params.c3_options_file[controller_params.scene_index]); + FrankaC3SceneParams scene_params = + drake::yaml::LoadYamlFile( + controller_params.c3_scene_file[controller_params.scene_index]); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(controller_params.osqp_settings_file)) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + DiagramBuilder plant_builder; + + MultibodyPlant plant_franka(0.0); + Parser parser_franka(&plant_franka, nullptr); + parser_franka.AddModelsFromUrl(scene_params.franka_model); + drake::multibody::ModelInstanceIndex end_effector_index = + parser_franka.AddModels( + FindResourceOrThrow(scene_params.end_effector_model))[0]; + + RigidTransform X_WI = RigidTransform::Identity(); + plant_franka.WeldFrames(plant_franka.world_frame(), + plant_franka.GetFrameByName("panda_link0"), X_WI); + + RigidTransform T_EE_W = + RigidTransform(drake::math::RotationMatrix(), + scene_params.tool_attachment_frame); + plant_franka.WeldFrames( + plant_franka.GetFrameByName("panda_link7"), + plant_franka.GetFrameByName("plate", end_effector_index), T_EE_W); + + plant_franka.Finalize(); + auto franka_context = plant_franka.CreateDefaultContext(); + + /// + MultibodyPlant plant_tray(0.0); + Parser parser_tray(&plant_tray, nullptr); + + auto tray_index = parser_tray.AddModels(scene_params.object_models[0])[0]; + auto object_index = parser_tray.AddModels(scene_params.object_models[1])[0]; + plant_tray.Finalize(); + auto tray_context = plant_tray.CreateDefaultContext(); + + /// + auto [plant_for_lcs, scene_graph] = + AddMultibodyPlantSceneGraph(&plant_builder, 0.0); + Parser lcs_parser(&plant_for_lcs); + lcs_parser.SetAutoRenaming(true); + lcs_parser.AddModels(scene_params.end_effector_lcs_model); + + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = lcs_parser.AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + plant_for_lcs.WeldFrames( + plant_for_lcs.world_frame(), + plant_for_lcs.GetFrameByName("base", environment_model_indices[i]), + T_E_W); + } + for (int i = 0; i < scene_params.object_models.size(); ++i) { + lcs_parser.AddModels(scene_params.object_models[i]); + } + + plant_for_lcs.WeldFrames(plant_for_lcs.world_frame(), + plant_for_lcs.GetFrameByName("base_link"), X_WI); + plant_for_lcs.Finalize(); + std::unique_ptr> plant_for_lcs_autodiff = + drake::systems::System::ToAutoDiffXd(plant_for_lcs); + + auto plant_diagram = plant_builder.Build(); + std::unique_ptr> diagram_context = + plant_diagram->CreateDefaultContext(); + auto& plant_for_lcs_context = plant_diagram->GetMutableSubsystemContext( + plant_for_lcs, diagram_context.get()); + auto plate_context_ad = plant_for_lcs_autodiff->CreateDefaultContext(); + + /// + std::vector end_effector_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("plate")); + for (int i = 0; i < environment_model_indices.size(); ++i) { + std::vector + environment_support_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("base", + environment_model_indices[i])); + end_effector_contact_points.insert( + end_effector_contact_points.end(), + environment_support_contact_points.begin(), + environment_support_contact_points.end()); + } + std::vector tray_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("tray")); + std::vector object_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("base")); + + std::vector> contact_pairs; + for (auto geom_id : end_effector_contact_points) { + contact_pairs.emplace_back(geom_id, tray_geoms[0]); + } + for (auto geom_id : object_geoms) { + contact_pairs.emplace_back(tray_geoms[0], geom_id); + } + + DiagramBuilder builder; + + auto tray_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.tray_state_channel, &lcm)); + auto object_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.object_state_channel, &lcm)); + auto franka_state_receiver = + builder.AddSystem(plant_franka); + auto tray_state_receiver = + builder.AddSystem(plant_tray, tray_index); + auto object_state_receiver = + builder.AddSystem(plant_tray, object_index); + auto reduced_order_model_receiver = + builder.AddSystem( + plant_franka, franka_context.get(), plant_tray, tray_context.get(), + scene_params.end_effector_name, "tray", + controller_params.include_end_effector_orientation); + auto actor_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_actor_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + auto object_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_object_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + auto c3_output_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_debug_output_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_target_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_target_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_actual_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_actual_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_forces_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_force_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + auto radio_to_vector = builder.AddSystem(); + + auto plate_balancing_target = + builder.AddSystem( + plant_tray, scene_params.end_effector_thickness, + controller_params.near_target_threshold); + plate_balancing_target->SetRemoteControlParameters( + controller_params.first_target[controller_params.scene_index], + controller_params.second_target[controller_params.scene_index], + controller_params.third_target[controller_params.scene_index], + controller_params.x_scale, controller_params.y_scale, + controller_params.z_scale); + std::vector input_sizes = {3, 7, 3, 6}; + auto target_state_mux = + builder.AddSystem(input_sizes); + auto end_effector_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(3)); + auto tray_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(6)); + builder.Connect(plate_balancing_target->get_output_port_end_effector_target(), + target_state_mux->get_input_port(0)); + builder.Connect(plate_balancing_target->get_output_port_tray_target(), + target_state_mux->get_input_port(1)); + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(2)); + builder.Connect(tray_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(3)); + auto lcs_factory = builder.AddSystem( + plant_for_lcs, plant_for_lcs_context, *plant_for_lcs_autodiff, + *plate_context_ad, contact_pairs, c3_options); + auto controller = + builder.AddSystem(plant_for_lcs, c3_options); + auto c3_trajectory_generator = + builder.AddSystem(plant_for_lcs, + c3_options); + std::vector state_names = { + "end_effector_x", "end_effector_y", "end_effector_z", "tray_qw", + "tray_qx", "tray_qy", "tray_qz", "tray_x", + "tray_y", "tray_z", "end_effector_vx", "end_effector_vy", + "end_effector_vz", "tray_wx", "tray_wy", "tray_wz", + "tray_vz", "tray_vz", "tray_vz", + }; + auto c3_state_sender = + builder.AddSystem(3 + 7 + 3 + 6, state_names); + c3_trajectory_generator->SetPublishEndEffectorOrientation( + controller_params.include_end_effector_orientation); + auto c3_output_sender = builder.AddSystem(); + controller->SetOsqpSolverOptions(solver_options); + + builder.Connect(*radio_sub, *radio_to_vector); + builder.Connect(franka_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_franka_state()); + builder.Connect(target_state_mux->get_output_port(), + controller->get_input_port_target()); + builder.Connect(lcs_factory->get_output_port_lcs(), + controller->get_input_port_lcs()); + builder.Connect(tray_state_sub->get_output_port(), + tray_state_receiver->get_input_port()); + builder.Connect(tray_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_object_state()); + builder.Connect(tray_state_receiver->get_output_port(), + plate_balancing_target->get_input_port_tray_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + controller->get_input_port_lcs_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + lcs_factory->get_input_port_lcs_state()); + builder.Connect(radio_to_vector->get_output_port(), + plate_balancing_target->get_input_port_radio()); + builder.Connect(controller->get_output_port_c3_solution(), + c3_trajectory_generator->get_input_port_c3_solution()); + builder.Connect(lcs_factory->get_output_port_lcs_contact_jacobian(), + c3_output_sender->get_input_port_lcs_contact_info()); + builder.Connect(c3_trajectory_generator->get_output_port_actor_trajectory(), + actor_trajectory_sender->get_input_port()); + builder.Connect(c3_trajectory_generator->get_output_port_object_trajectory(), + object_trajectory_sender->get_input_port()); + builder.Connect(target_state_mux->get_output_port(), + c3_state_sender->get_input_port_target_state()); + builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), + c3_state_sender->get_input_port_actual_state()); + builder.Connect(c3_state_sender->get_output_port_target_c3_state(), + c3_target_state_publisher->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), + c3_actual_state_publisher->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution(), + c3_output_sender->get_input_port_c3_solution()); + builder.Connect(controller->get_output_port_c3_intermediates(), + c3_output_sender->get_input_port_c3_intermediates()); + builder.Connect(c3_output_sender->get_output_port_c3_debug(), + c3_output_publisher->get_input_port()); + builder.Connect(c3_output_sender->get_output_port_c3_force(), + c3_forces_publisher->get_input_port()); + // builder.Connect(c3_output_sender->get_output_port_next_c3_input(), + // lcs_factory->get_input_port_lcs_input()); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_c3_controller")); + plant_diagram->set_name(("franka_c3_plant")); + // DrawAndSaveDiagramGraph(*plant_diagram); + + // Run lcm-driven simulation + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, franka_state_receiver, + lcm_channel_params.franka_state_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + // auto& controller_context = loop.get_diagram()->GetMutableSubsystemContext( + // *controller, &loop.get_diagram_mutable_context()); + // controller->get_input_port_target().FixValue(&controller_context, x_des); + LcmHandleSubscriptionsUntil( + &lcm, [&]() { return tray_state_sub->GetInternalMessageCount() > 1; }); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_hardware.pmd b/examples/franka/franka_hardware.pmd new file mode 100644 index 0000000000..b20047cf2c --- /dev/null +++ b/examples/franka/franka_hardware.pmd @@ -0,0 +1,81 @@ +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/franka/franka_visualizer --lcm_channels=examples/franka/parameters/lcm_channels_hardware.yaml"; + host = "localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/Cassie/cassie_xbox_remote"; + host = "localhost"; + } + cmd "logger" { + exec = "python3 start_logging.py hw"; + host = "localhost"; + } + cmd "record_video" { + exec = "python3 record_video.py"; + host = "localhost"; + } +} + +group "controllers (hardware)" { + cmd "franka_osc" { + exec = "bazel-bin/examples/franka/franka_osc_controller --lcm_channels=examples/franka/parameters/lcm_channels_hardware.yaml"; + host = "localhost"; + } + cmd "franka_c3" { + exec = "bazel-bin/examples/franka/franka_c3_controller --lcm_channels=examples/franka/parameters/lcm_channels_hardware.yaml"; + host = "localhost"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } +} + +group "drivers" { + cmd "franka_driver_out" { + exec = "bazel-bin/examples/franka/franka_bridge_driver_out"; + host = "localhost"; + } + cmd "franka_driver_in" { + exec = "bazel-bin/examples/franka/franka_bridge_driver_in"; + host = "localhost"; + } + cmd "position_driver" { + exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=position"; + host = "franka_control"; + } + cmd "torque_driver" { + exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=torque"; + host = "franka_control"; + } +} + +script "start_operator_commands" { + restart cmd "visualizer"; + restart cmd "xbox"; +} + +script "start_experiment" { + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "franka_osc"; + stop cmd "torque_driver"; + start cmd "record_video"; + start cmd "logger"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "franka_osc"; +} + +script "stop_experiment" { + stop group "drivers"; + stop group "controllers (hardware)"; + stop cmd "record_video"; + stop cmd "logger"; +} diff --git a/examples/franka/franka_lcm_ros_bridge.cc b/examples/franka/franka_lcm_ros_bridge.cc new file mode 100644 index 0000000000..0238c7253d --- /dev/null +++ b/examples/franka/franka_lcm_ros_bridge.cc @@ -0,0 +1,126 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "franka_msgs/FrankaState.h" +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "std_msgs/Float64MultiArray.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/ros/franka_ros_lcm_conversions.h" +#include "systems/ros/parameters/franka_ros_channels.h" +#include "systems/ros/ros_publisher_system.h" +#include "systems/ros/ros_subscriber_system.h" +#include "systems/system_utils.h" + +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; + +using dairlib::systems::LcmToRosTimestampedVector; +using dairlib::systems::RobotInputReceiver; +using dairlib::systems::RobotOutputSender; +using dairlib::systems::RosPublisherSystem; +using dairlib::systems::RosSubscriberSystem; +using dairlib::systems::RosToLcmObjectState; +using dairlib::systems::RosToLcmRobotState; +using dairlib::systems::SubvectorPassThrough; +using dairlib::systems::TimestampedVector; + +// Shutdown ROS gracefully and then exit +void SigintHandler(int sig) { + ros::shutdown(); + exit(sig); +} + +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_hardware.yaml", + "Filepath containing lcm channels"); +DEFINE_string(ros_channels, "systems/ros/parameters/franka_ros_channels.yaml", + "Filepath containing ROS channels"); + +namespace dairlib { + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + FrankaRosChannels ros_channel_params = + drake::yaml::LoadYamlFile(FLAGS_ros_channels); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + + ros::init(argc, argv, "run_lcm_to_ros"); + ros::NodeHandle node_handle; + + drake::lcm::DrakeLcm drake_lcm("udpm://239.255.76.67:7667?ttl=0"); + DiagramBuilder builder; + + MultibodyPlant plant(0.0); + + Parser parser(&plant); + parser.AddModelsFromUrl(sim_params.franka_model)[0]; + Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); + RigidTransform R_X_W = RigidTransform( + drake::math::RotationMatrix(), franka_origin); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + R_X_W); + plant.Finalize(); + + /* -------------------------------------------------------------------------------------------*/ + auto robot_input_receiver = builder.AddSystem(plant); + auto lcm_to_ros_robot_input = builder.AddSystem(7); + auto robot_input_ros_publisher = builder.AddSystem( + systems::RosPublisherSystem::Make( + ros_channel_params.franka_input_channel, &node_handle, + {drake::systems::TriggerType::kForced})); + + builder.Connect(robot_input_receiver->get_output_port(), + lcm_to_ros_robot_input->get_input_port()); + builder.Connect(lcm_to_ros_robot_input->get_output_port(), + robot_input_ros_publisher->get_input_port()); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_lcm_ros_bridge")); + + // figure out what the arguments to this mean + ros::AsyncSpinner spinner(1); + spinner.start(); + signal(SIGINT, SigintHandler); + + systems::LcmDrivenLoop loop( + &drake_lcm, shared_diagram, robot_input_receiver, + lcm_channel_params.franka_input_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + loop.Simulate(); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_osc_controller.cc b/examples/franka/franka_osc_controller.cc new file mode 100644 index 0000000000..4fed320e4a --- /dev/null +++ b/examples/franka/franka_osc_controller.cc @@ -0,0 +1,291 @@ + +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_osc_controller_params.h" +#include "systems/controllers/osc/end_effector_force.h" +#include "systems/controllers/osc/end_effector_orientation.h" +#include "systems/controllers/osc/end_effector_position.h" +#include "lcm/lcm_trajectory.h" +#include "multibody/multibody_utils.h" +#include "systems/controllers/gravity_compensator.h" +#include "systems/controllers/osc/external_force_tracking_data.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/operational_space_control.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/lcm_trajectory_systems.h" + +#include "drake/common/find_resource.h" +#include "drake/common/yaml/yaml_io.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_interface_system.h" +#include "drake/systems/lcm/lcm_publisher_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" + +namespace dairlib { + +using drake::math::RigidTransform; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; + +using systems::controllers::ExternalForceTrackingData; +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RelativeTranslationTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; + +DEFINE_string(osqp_settings, + "examples/franka/parameters/franka_osc_qp_settings.yaml", + "Filepath containing qp settings"); +DEFINE_string(controller_parameters, + "examples/franka/parameters/franka_osc_controller_params.yaml", + "Controller settings such as channels. Attempting to minimize " + "number of gflags"); +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_simulation.yaml", + "Filepath containing lcm channels"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + // load parameters + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaControllerParams controller_params = + drake::yaml::LoadYamlFile( + FLAGS_controller_parameters); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + OSCGains gains = drake::yaml::LoadYamlFile( + FindResourceOrThrow(FLAGS_controller_parameters), {}, {}, yaml_options); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(FLAGS_osqp_settings)) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + DiagramBuilder builder; + + drake::multibody::MultibodyPlant plant(0.0); + Parser parser(&plant, nullptr); + parser.AddModelsFromUrl(controller_params.franka_model); + + RigidTransform X_WI = RigidTransform::Identity(); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + X_WI); + + if (!controller_params.end_effector_name.empty()) { + drake::multibody::ModelInstanceIndex end_effector_index = parser.AddModels( + FindResourceOrThrow(controller_params.end_effector_model))[0]; + RigidTransform T_EE_W = + RigidTransform(drake::math::RotationMatrix(), + controller_params.tool_attachment_frame); + plant.WeldFrames(plant.GetFrameByName("panda_link7"), + plant.GetFrameByName(controller_params.end_effector_name, + end_effector_index), + T_EE_W); + } else { + std::cout << "OSC plant has been constructed with no end effector." + << std::endl; + } + + plant.Finalize(); + auto plant_context = plant.CreateDefaultContext(); + + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + auto state_receiver = builder.AddSystem(plant); + auto end_effector_trajectory_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_actor_channel, &lcm)); + auto end_effector_position_receiver = + builder.AddSystem( + "end_effector_position_target"); + auto end_effector_force_receiver = + builder.AddSystem( + "end_effector_force_target"); + auto end_effector_orientation_receiver = + builder.AddSystem( + "end_effector_orientation_target"); + auto franka_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_input_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto osc_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto franka_command_sender = + builder.AddSystem(plant); + auto osc_command_sender = + builder.AddSystem(plant); + auto end_effector_trajectory = + builder.AddSystem(plant, + plant_context.get(), controller_params.neutral_position, false, + controller_params.end_effector_name); + end_effector_trajectory->SetRemoteControlParameters( + controller_params.neutral_position, controller_params.x_scale, + controller_params.y_scale, controller_params.z_scale); + auto end_effector_orientation_trajectory = + builder.AddSystem(); + end_effector_orientation_trajectory->SetTrackOrientation( + controller_params.track_end_effector_orientation); + auto end_effector_force_trajectory = + builder.AddSystem(); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + auto osc = builder.AddSystem( + plant, plant_context.get(), false); + if (controller_params.publish_debug_info) { + auto osc_debug_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_debug_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + builder.Connect(osc->get_output_port_osc_debug(), + osc_debug_pub->get_input_port()); + } + + auto end_effector_position_tracking_data = + std::make_unique( + "end_effector_target", controller_params.K_p_end_effector, + controller_params.K_d_end_effector, controller_params.W_end_effector, + plant, plant); + end_effector_position_tracking_data->AddPointToTrack( + controller_params.end_effector_name); + const VectorXd& end_effector_acceleration_limits = + controller_params.end_effector_acceleration * Vector3d::Ones(); + end_effector_position_tracking_data->SetCmdAccelerationBounds( + -end_effector_acceleration_limits, end_effector_acceleration_limits); + auto mid_link_position_tracking_data_for_rel = + std::make_unique( + "panda_joint2_target", controller_params.K_p_mid_link, + controller_params.K_d_mid_link, controller_params.W_mid_link, plant, + plant); + mid_link_position_tracking_data_for_rel->AddJointToTrack("panda_joint2", + "panda_joint2dot"); + + auto end_effector_force_tracking_data = + std::make_unique( + "end_effector_force", controller_params.W_ee_lambda, plant, plant, + controller_params.end_effector_name, Vector3d::Zero()); + + auto end_effector_orientation_tracking_data = + std::make_unique( + "end_effector_orientation_target", + controller_params.K_p_end_effector_rot, + controller_params.K_d_end_effector_rot, + controller_params.W_end_effector_rot, plant, plant); + end_effector_orientation_tracking_data->AddFrameToTrack( + controller_params.end_effector_name); + Eigen::VectorXd orientation_target = Eigen::VectorXd::Zero(4); + orientation_target(0) = 1; + osc->AddTrackingData(std::move(end_effector_position_tracking_data)); + osc->AddConstTrackingData(std::move(mid_link_position_tracking_data_for_rel), + 1.6 * VectorXd::Ones(1)); + osc->AddTrackingData(std::move(end_effector_orientation_tracking_data)); + osc->AddForceTrackingData(std::move(end_effector_force_tracking_data)); + osc->SetAccelerationCostWeights(gains.W_acceleration); + osc->SetInputCostWeights(gains.W_input_regularization); + osc->SetInputSmoothingCostWeights(gains.W_input_smoothing_regularization); + if (controller_params.enforce_acceleration_constraints) { + osc->EnableAccelerationConstraints(); + } else { + osc->DisableAccelerationConstraints(); + } + osc->SetContactFriction(controller_params.mu); + osc->SetOsqpSolverOptions(solver_options); + + osc->Build(); + + if (controller_params.cancel_gravity_compensation) { + auto gravity_compensator = + builder.AddSystem(plant, + *plant_context); + builder.Connect(osc->get_output_port_osc_command(), + gravity_compensator->get_input_port()); + builder.Connect(gravity_compensator->get_output_port(), + franka_command_sender->get_input_port()); + } else { + if (FLAGS_lcm_channels == + "examples/franka/parameters/lcm_channels_hardware.yaml") { + std::cerr << "Using hardware lcm channels but not cancelling gravity " + "compensation. Please check the OSC settings" + << std::endl; + return -1; + } + builder.Connect(osc->get_output_port_osc_command(), + franka_command_sender->get_input_port(0)); + } + + builder.Connect(radio_sub->get_output_port(), + end_effector_trajectory->get_input_port_radio()); + builder.Connect(radio_sub->get_output_port(), + end_effector_orientation_trajectory->get_input_port_radio()); + builder.Connect(radio_sub->get_output_port(), + end_effector_force_trajectory->get_input_port_radio()); + builder.Connect(franka_command_sender->get_output_port(), + franka_command_pub->get_input_port()); + builder.Connect(osc_command_sender->get_output_port(), + osc_command_pub->get_input_port()); + builder.Connect(osc->get_output_port_osc_command(), + osc_command_sender->get_input_port(0)); + + builder.Connect(state_receiver->get_output_port(0), + osc->get_input_port_robot_output()); + builder.Connect(end_effector_trajectory_sub->get_output_port(), + end_effector_position_receiver->get_input_port_trajectory()); + builder.Connect(end_effector_trajectory_sub->get_output_port(), + end_effector_force_receiver->get_input_port_trajectory()); + builder.Connect( + end_effector_trajectory_sub->get_output_port(), + end_effector_orientation_receiver->get_input_port_trajectory()); + builder.Connect(end_effector_position_receiver->get_output_port(0), + end_effector_trajectory->get_input_port_trajectory()); + builder.Connect( + end_effector_orientation_receiver->get_output_port(0), + end_effector_orientation_trajectory->get_input_port_trajectory()); + builder.Connect(end_effector_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_target")); + builder.Connect( + end_effector_orientation_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_orientation_target")); + builder.Connect(end_effector_force_receiver->get_output_port(0), + end_effector_force_trajectory->get_input_port_trajectory()); + builder.Connect(end_effector_force_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_force")); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_osc_controller")); + // Run lcm-driven simulation + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, state_receiver, + lcm_channel_params.franka_state_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } \ No newline at end of file diff --git a/examples/franka/franka_ros_lcm_bridge.cc b/examples/franka/franka_ros_lcm_bridge.cc new file mode 100644 index 0000000000..7294abce28 --- /dev/null +++ b/examples/franka/franka_ros_lcm_bridge.cc @@ -0,0 +1,192 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/find_resource.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "franka_msgs/FrankaState.h" +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "nav_msgs/Odometry.h" +#include "systems/robot_lcm_systems.h" +#include "systems/ros/franka_ros_lcm_conversions.h" +#include "systems/ros/parameters/franka_ros_channels.h" +#include "systems/ros/ros_publisher_system.h" +#include "systems/ros/ros_subscriber_system.h" +#include "systems/system_utils.h" + +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; + +using dairlib::systems::LcmToRosTimestampedVector; +using dairlib::systems::RobotInputReceiver; +using dairlib::systems::RobotOutputSender; +using dairlib::systems::RosPublisherSystem; +using dairlib::systems::RosSubscriberSystem; +using dairlib::systems::RosToLcmObjectState; +using dairlib::systems::RosToLcmRobotState; +using dairlib::systems::SubvectorPassThrough; +using dairlib::systems::TimestampedVector; +// using dairlib::systems::ROSToRobotOutputLCM; + +// Shutdown ROS gracefully and then exit +void SigintHandler(int sig) { + ros::shutdown(); + exit(sig); +} + +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_hardware.yaml", + "Filepath containing lcm channels"); +DEFINE_string(ros_channels, "systems/ros/parameters/franka_ros_channels.yaml", + "Filepath containing ROS channels"); + +namespace dairlib { + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + FrankaRosChannels ros_channel_params = + drake::yaml::LoadYamlFile(FLAGS_ros_channels); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + + ros::init(argc, argv, "run_ros_to_lcm"); + ros::NodeHandle node_handle; + + drake::lcm::DrakeLcm drake_lcm("udpm://239.255.76.67:7667?ttl=0"); + DiagramBuilder builder; + + MultibodyPlant plant(0.0); + + Parser parser(&plant); + auto franka_index = + parser.AddModelsFromUrl(sim_params.franka_model)[0]; + auto tray_index = + parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; + Eigen::Vector3d franka_origin = Eigen::VectorXd::Zero(3); + RigidTransform R_X_W = RigidTransform( + drake::math::RotationMatrix(), franka_origin); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + R_X_W); + plant.Finalize(); + + /// convert franka joint states to lcm + auto franka_joint_state_ros_subscriber = + builder.AddSystem(RosSubscriberSystem::Make( + ros_channel_params.franka_state_channel, &node_handle)); + auto ros_to_lcm_robot_state = builder.AddSystem(RosToLcmRobotState::Make( + plant.num_positions(franka_index), plant.num_velocities(franka_index), + plant.num_actuators(franka_index))); + + // change this to output correctly (i.e. when ros subscriber gets new message) + auto robot_output_lcm_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_state_channel, &drake_lcm, + {drake::systems::TriggerType::kForced})); + /// connections + builder.Connect(franka_joint_state_ros_subscriber->get_output_port(), + ros_to_lcm_robot_state->get_input_port()); + builder.Connect(ros_to_lcm_robot_state->get_output_port(), + robot_output_lcm_publisher->get_input_port()); + + /* -------------------------------------------------------------------------------------------*/ + auto tray_object_state_ros_subscriber = + builder.AddSystem(RosSubscriberSystem::Make( + ros_channel_params.tray_state_channel, &node_handle)); + auto ros_to_lcm_object_state = builder.AddSystem( + RosToLcmObjectState::Make(plant, tray_index, "tray")); + + // change this to output correctly (i.e. when ros subscriber gets new message) + auto tray_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.tray_state_channel, &drake_lcm, + {drake::systems::TriggerType::kForced})); + /// connections + builder.Connect(tray_object_state_ros_subscriber->get_output_port(), + ros_to_lcm_object_state->get_input_port()); + builder.Connect(ros_to_lcm_object_state->get_output_port(), + tray_state_pub->get_input_port()); + + /* -------------------------------------------------------------------------------------------*/ + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("franka_ros_lcm_bridge")); + const auto& diagram = *shared_diagram; + DrawAndSaveDiagramGraph(diagram); + drake::systems::Simulator simulator(shared_diagram); + auto& diagram_context = simulator.get_mutable_context(); + + // figure out what the arguments to this mean + ros::AsyncSpinner spinner(1); + spinner.start(); + signal(SIGINT, SigintHandler); + + // Wait for the first message. + drake::log()->info("Waiting for first ROS message from Franka"); + int old_message_count = 0; + old_message_count = + franka_joint_state_ros_subscriber->WaitForMessage(old_message_count); + + // Initialize the context based on the first message. + const double t0 = franka_joint_state_ros_subscriber->message_time(); + diagram_context.SetTime(t0); + simulator.Initialize(); + + drake::log()->info("franka ros-lcm bridge started"); + + while (true) { + old_message_count = + franka_joint_state_ros_subscriber->WaitForMessage(old_message_count); + const double time = franka_joint_state_ros_subscriber->message_time(); + + // Check if we are very far ahead or behind + // (likely due to a restart of the driving clock) + if (time > simulator.get_context().get_time() + 1.0 || + time < simulator.get_context().get_time()) { + std::cout << "Dispatcher time is " << simulator.get_context().get_time() + << ", but stepping to " << time << std::endl; + std::cout << "Difference is too large, resetting dispatcher time." + << std::endl; + simulator.get_mutable_context().SetTime(time); + simulator.Initialize(); + } + + simulator.AdvanceTo(time); + + // Force-publish via the diagram + diagram.CalcForcedUnrestrictedUpdate(diagram_context, + &diagram_context.get_mutable_state()); + diagram.CalcForcedDiscreteVariableUpdate( + diagram_context, &diagram_context.get_mutable_discrete_state()); + diagram.ForcedPublish(diagram_context); + } + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/franka_sim.cc b/examples/franka/franka_sim.cc new file mode 100644 index 0000000000..f047338d8d --- /dev/null +++ b/examples/franka/franka_sim.cc @@ -0,0 +1,231 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "examples/franka/parameters/franka_sim_scene_params.h" +#include "examples/franka/systems/external_force_generator.h" +#include "multibody/multibody_utils.h" +#include "systems/primitives/radio_parser.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +namespace dairlib { + +using dairlib::systems::SubvectorPassThrough; +using drake::geometry::GeometrySet; +using drake::geometry::SceneGraph; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using drake::trajectories::PiecewisePolynomial; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; +using systems::AddActuationRecieverAndStateSenderLcm; + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_simulation.yaml", + "Filepath containing lcm channels"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + // load parameters + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + FrankaSimSceneParams scene_params = + drake::yaml::LoadYamlFile( + sim_params.sim_scene_file[sim_params.scene_index]); + + // load urdf and sphere + DiagramBuilder builder; + double sim_dt = sim_params.dt; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, sim_dt); + + Parser parser(&plant); + parser.SetAutoRenaming(true); + drake::multibody::ModelInstanceIndex franka_index = + parser.AddModelsFromUrl(sim_params.franka_model)[0]; + drake::multibody::ModelInstanceIndex end_effector_index = + parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; + drake::multibody::ModelInstanceIndex tray_index = + parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; + drake::multibody::ModelInstanceIndex object_index = + parser.AddModels(FindResourceOrThrow(sim_params.object_model))[0]; + multibody::AddFlatTerrain(&plant, &scene_graph, 1.0, 1.0); + + RigidTransform X_WI = RigidTransform::Identity(); + Vector3d franka_origin = Eigen::VectorXd::Zero(3); + + RigidTransform T_X_W = RigidTransform( + drake::math::RotationMatrix(), franka_origin); + RigidTransform T_EE_W = RigidTransform( + drake::math::RotationMatrix(), sim_params.tool_attachment_frame); + + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + T_X_W); + plant.WeldFrames(plant.GetFrameByName("panda_link7"), + plant.GetFrameByName("plate", end_effector_index), T_EE_W); + + // we WANT to model collisions between link5 and the supports + const drake::geometry::GeometrySet& franka_geom_set = + plant.CollectRegisteredGeometries({&plant.GetBodyByName("panda_link0"), + &plant.GetBodyByName("panda_link1"), + &plant.GetBodyByName("panda_link2"), + &plant.GetBodyByName("panda_link3"), + &plant.GetBodyByName("panda_link4")}); + + drake::geometry::GeometrySet support_geom_set; + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = parser.AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + plant.WeldFrames(plant.world_frame(), + plant.GetFrameByName("base", environment_model_indices[i]), + T_E_W); + support_geom_set.Add(plant.GetCollisionGeometriesForBody( + plant.GetBodyByName("base", environment_model_indices[i]))); + } + plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( + {"supports", support_geom_set}, {"franka", franka_geom_set}); + + const drake::geometry::GeometrySet& franka_only_geom_set = + plant.CollectRegisteredGeometries({ + &plant.GetBodyByName("panda_link2"), + &plant.GetBodyByName("panda_link3"), + &plant.GetBodyByName("panda_link4"), + &plant.GetBodyByName("panda_link5"), + &plant.GetBodyByName("panda_link6"), + &plant.GetBodyByName("panda_link7"), + &plant.GetBodyByName("panda_link8"), + }); + auto tray_collision_set = GeometrySet( + plant.GetCollisionGeometriesForBody(plant.GetBodyByName("tray"))); + plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( + {"franka", franka_only_geom_set}, {"tray", tray_collision_set}); + + plant.Finalize(); + /* -------------------------------------------------------------------------------------------*/ + + drake::lcm::DrakeLcm drake_lcm; + auto lcm = + builder.AddSystem(&drake_lcm); + AddActuationRecieverAndStateSenderLcm( + &builder, plant, lcm, lcm_channel_params.franka_input_channel, + lcm_channel_params.franka_state_channel, sim_params.franka_publish_rate, + franka_index, sim_params.publish_efforts, sim_params.actuator_delay); + auto tray_state_sender = + builder.AddSystem(plant, sim_params.publish_object_velocities, tray_index); + auto tray_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.tray_state_channel, lcm, + 1.0 / sim_params.tray_publish_rate)); + auto object_state_sender = + builder.AddSystem(plant, sim_params.publish_object_velocities, object_index); + auto object_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.object_state_channel, lcm, + 1.0 / sim_params.object_publish_rate)); + + builder.Connect(plant.get_state_output_port(tray_index), + tray_state_sender->get_input_port_state()); + builder.Connect(tray_state_sender->get_output_port(), + tray_state_pub->get_input_port()); + builder.Connect(plant.get_state_output_port(object_index), + object_state_sender->get_input_port_state()); + builder.Connect(object_state_sender->get_output_port(), + object_state_pub->get_input_port()); + + auto external_force_generator = builder.AddSystem( + plant.GetBodyByName("tray").index()); + external_force_generator->SetRemoteControlParameters( + sim_params.external_force_scaling[0], + sim_params.external_force_scaling[1], + sim_params.external_force_scaling[2]); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &drake_lcm)); + auto radio_to_vector = builder.AddSystem(); + builder.Connect(*radio_sub, *radio_to_vector); + builder.Connect(radio_to_vector->get_output_port(), + external_force_generator->get_input_port_radio()); + builder.Connect(external_force_generator->get_output_port_spatial_force(), + plant.get_applied_spatial_force_input_port()); + + int nq = plant.num_positions(); + int nv = plant.num_velocities(); + + if (sim_params.visualize_drake_sim) { + drake::visualization::AddDefaultVisualization(&builder); + } + + auto diagram = builder.Build(); + + drake::systems::Simulator simulator(*diagram); + + simulator.set_publish_every_time_step(false); + simulator.set_publish_at_initialization(false); + simulator.set_target_realtime_rate(sim_params.realtime_rate); + + auto& plant_context = diagram->GetMutableSubsystemContext( + plant, &simulator.get_mutable_context()); + + VectorXd q = VectorXd::Zero(nq); + + q.head(plant.num_positions(franka_index)) = sim_params.q_init_franka; + + q.segment(plant.num_positions(franka_index), + plant.num_positions(tray_index)) = + sim_params.q_init_tray[sim_params.scene_index]; + q.tail(plant.num_positions(object_index)) = + sim_params.q_init_object[sim_params.scene_index]; + + plant.SetPositions(&plant_context, q); + + VectorXd v = VectorXd::Zero(nv); + plant.SetVelocities(&plant_context, v); + + simulator.Initialize(); + simulator.AdvanceTo(std::numeric_limits::infinity()); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/franka_sim.pmd b/examples/franka/franka_sim.pmd new file mode 100644 index 0000000000..f12f036d8d --- /dev/null +++ b/examples/franka/franka_sim.pmd @@ -0,0 +1,69 @@ +group "simulations" { + cmd "franka_sim" { + exec = "bazel-bin/examples/franka/franka_sim"; + host = "localhost"; + } + cmd "franka_kinematics" { + exec = "bazel-bin/examples/franka/forward_kinematics_for_lcs"; + host = "localhost"; + } +} + +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/franka/franka_visualizer"; + host = "localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/Cassie/cassie_xbox_remote"; + host = "localhost"; + } + cmd "start_logging" { + exec = "python3 start_logging.py sim"; + host = "localhost"; + } +} + +group "controllers" { + cmd "franka_osc" { + exec = "bazel-bin/examples/franka/franka_osc_controller"; + host = "localhost"; + } + cmd "franka_c3" { + exec = "bazel-bin/examples/franka/franka_c3_controller"; + host = "localhost"; + } + cmd "mujoco_mpc" { + exec = "../mujoco_mpc/build/bin/standalone_controller"; + host = "localhost"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } +} + +script "c3_mpc" { + start cmd "franka_sim"; + start cmd "franka_osc"; + start cmd "franka_c3"; +} + +script "mjmpc_with_drake_sim" { + start cmd "franka_sim"; + start cmd "franka_osc"; + start cmd "mujoco_mpc"; + start cmd "franka_kinematics"; +} + +script "start_operator_commands" { + start cmd "visualizer"; +} + +script "stop_controllers_and_simulators" { + stop group "simulations"; + stop group "controllers"; +} diff --git a/examples/franka/franka_visualizer.cc b/examples/franka/franka_visualizer.cc new file mode 100644 index 0000000000..2d301b3982 --- /dev/null +++ b/examples/franka/franka_visualizer.cc @@ -0,0 +1,330 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "dairlib/lcmt_robot_output.hpp" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "examples/franka/parameters/franka_sim_scene_params.h" +#include "multibody/com_pose_system.h" +#include "multibody/multibody_utils.h" +#include "multibody/visualization_utils.h" +#include "systems/primitives/subvector_pass_through.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/lcm_trajectory_systems.h" +#include "systems/visualization/lcm_visualization_systems.h" + +#include "drake/common/find_resource.h" +#include "drake/common/yaml/yaml_io.h" +#include "drake/geometry/drake_visualizer.h" +#include "drake/geometry/meshcat_visualizer.h" +#include "drake/geometry/meshcat_visualizer_params.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_interface_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" +#include "drake/systems/rendering/multibody_position_to_geometry_pose.h" + +namespace dairlib { + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using dairlib::systems::ObjectStateReceiver; +using dairlib::systems::RobotOutputReceiver; +using dairlib::systems::SubvectorPassThrough; +using drake::geometry::DrakeVisualizer; +using drake::geometry::SceneGraph; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::multibody::MultibodyPlant; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::multibody::UnitInertia; +using drake::systems::Simulator; +using drake::systems::lcm::LcmSubscriberSystem; +using drake::systems::rendering::MultibodyPositionToGeometryPose; + +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; + +DEFINE_string(lcm_channels, + "examples/franka/parameters/lcm_channels_simulation.yaml", + "Filepath containing lcm channels"); + +int do_main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(FLAGS_lcm_channels); + FrankaSimSceneParams scene_params = + drake::yaml::LoadYamlFile( + sim_params.sim_scene_file[sim_params.scene_index]); + + drake::systems::DiagramBuilder builder; + + SceneGraph& scene_graph = *builder.AddSystem(); + scene_graph.set_name("scene_graph"); + + MultibodyPlant plant(0.0); + + Parser parser(&plant, &scene_graph); + parser.SetAutoRenaming(true); + drake::multibody::ModelInstanceIndex franka_index = + parser.AddModelsFromUrl(sim_params.franka_model)[0]; + drake::multibody::ModelInstanceIndex end_effector_index = + parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; + drake::multibody::ModelInstanceIndex tray_index = + parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; + drake::multibody::ModelInstanceIndex object_index = + parser.AddModels(FindResourceOrThrow(sim_params.object_model))[0]; + multibody::AddFlatTerrain(&plant, &scene_graph, 1.0, 1.0); + + RigidTransform X_WI = RigidTransform::Identity(); + Vector3d franka_origin = Eigen::VectorXd::Zero(3); + + RigidTransform R_X_W = RigidTransform( + drake::math::RotationMatrix(), franka_origin); + RigidTransform T_EE_W = RigidTransform( + drake::math::RotationMatrix(), sim_params.tool_attachment_frame); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + R_X_W); + plant.WeldFrames(plant.GetFrameByName("panda_link7"), + plant.GetFrameByName("plate", end_effector_index), T_EE_W); + + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = parser.AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + plant.WeldFrames(plant.world_frame(), + plant.GetFrameByName("base", environment_model_indices[i]), + T_E_W); + } + + plant.Finalize(); + + auto lcm = builder.AddSystem(); + + // Create state receiver. + auto franka_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.franka_state_channel, lcm)); + auto tray_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.tray_state_channel, lcm)); + auto object_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.object_state_channel, lcm)); + auto franka_state_receiver = + builder.AddSystem(plant, franka_index); + auto tray_state_receiver = + builder.AddSystem(plant, tray_index); + auto object_state_receiver = + builder.AddSystem(plant, object_index); + + auto franka_passthrough = builder.AddSystem( + franka_state_receiver->get_output_port(0).size(), 0, + plant.num_positions(franka_index)); + auto robot_time_passthrough = builder.AddSystem( + franka_state_receiver->get_output_port(0).size(), + franka_state_receiver->get_output_port(0).size() - 1, 1); + auto tray_passthrough = builder.AddSystem( + tray_state_receiver->get_output_port(0).size(), 0, + plant.num_positions(tray_index)); + auto object_passthrough = builder.AddSystem( + tray_state_receiver->get_output_port(0).size(), 0, + plant.num_positions(object_index)); + + std::vector input_sizes = {plant.num_positions(franka_index), + plant.num_positions(tray_index), + plant.num_positions(object_index)}; + auto mux = + builder.AddSystem>(input_sizes); + + auto trajectory_sub_actor = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_actor_channel, lcm)); + auto trajectory_sub_tray = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_object_channel, lcm)); + auto trajectory_sub_force = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.c3_force_channel, lcm)); + + auto c3_state_actual_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.c3_actual_state_channel, lcm)); + auto c3_state_target_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.c3_target_state_channel, lcm)); + auto to_pose = + builder.AddSystem>(plant); + + drake::geometry::MeshcatVisualizerParams params; + params.publish_period = 1.0 / sim_params.visualizer_publish_rate; + auto meshcat = std::make_shared(); + + meshcat->SetCameraPose(scene_params.camera_pose, + scene_params.camera_target); + + if (sim_params.visualize_workspace) { + double width = sim_params.world_x_limits[sim_params.scene_index][1] - + sim_params.world_x_limits[sim_params.scene_index][0]; + double depth = sim_params.world_y_limits[sim_params.scene_index][1] - + sim_params.world_y_limits[sim_params.scene_index][0]; + double height = sim_params.world_z_limits[sim_params.scene_index][1] - + sim_params.world_z_limits[sim_params.scene_index][0]; + Vector3d workspace_center = { + 0.5 * (sim_params.world_x_limits[sim_params.scene_index][1] + + sim_params.world_x_limits[sim_params.scene_index][0]), + 0.5 * (sim_params.world_y_limits[sim_params.scene_index][1] + + sim_params.world_y_limits[sim_params.scene_index][0]), + 0.5 * (sim_params.world_z_limits[sim_params.scene_index][1] + + sim_params.world_z_limits[sim_params.scene_index][0])}; + meshcat->SetObject("c3_state/workspace", + drake::geometry::Box(width, depth, height), + {1, 0, 0, 0.2}); + meshcat->SetTransform("c3_state/workspace", + RigidTransformd(workspace_center)); + } + if (sim_params.visualize_center_of_mass_plan) { + auto trajectory_drawer_actor = + builder.AddSystem( + meshcat, "end_effector_position_target"); + auto trajectory_drawer_object = + builder.AddSystem( + meshcat, "object_position_target"); + trajectory_drawer_actor->SetLineColor(drake::geometry::Rgba({1, 0, 0, 1})); + trajectory_drawer_object->SetLineColor(drake::geometry::Rgba({0, 0, 1, 1})); + trajectory_drawer_actor->SetNumSamples(40); + trajectory_drawer_object->SetNumSamples(40); + builder.Connect(trajectory_sub_actor->get_output_port(), + trajectory_drawer_actor->get_input_port_trajectory()); + builder.Connect(trajectory_sub_tray->get_output_port(), + trajectory_drawer_object->get_input_port_trajectory()); + } + + if (sim_params.visualize_pose_trace) { + auto object_pose_drawer = builder.AddSystem( + meshcat, + FindResourceOrThrow("examples/franka/urdf/tray.sdf"), + "object_position_target", "object_orientation_target"); + auto end_effector_pose_drawer = builder.AddSystem( + meshcat, FindResourceOrThrow(sim_params.end_effector_model), + "end_effector_position_target", "end_effector_orientation_target"); + + builder.Connect(trajectory_sub_tray->get_output_port(), + object_pose_drawer->get_input_port_trajectory()); + builder.Connect(trajectory_sub_actor->get_output_port(), + end_effector_pose_drawer->get_input_port_trajectory()); + } + + if (sim_params.visualize_c3_object_state || sim_params.visualize_c3_end_effector_state) { + auto c3_target_drawer = + builder.AddSystem(meshcat, sim_params.visualize_c3_object_state, sim_params.visualize_c3_end_effector_state); + builder.Connect(c3_state_actual_sub->get_output_port(), + c3_target_drawer->get_input_port_c3_state_actual()); + builder.Connect(c3_state_target_sub->get_output_port(), + c3_target_drawer->get_input_port_c3_state_target()); + builder.Connect(c3_state_target_sub->get_output_port(), + c3_target_drawer->get_input_port_c3_state_final_target()); + } + + if (sim_params.visualize_c3_forces) { + auto end_effector_force_drawer = builder.AddSystem( + meshcat, "end_effector_position_target", "end_effector_force_target", + "lcs_force_trajectory"); + builder.Connect( + trajectory_sub_actor->get_output_port(), + end_effector_force_drawer->get_input_port_actor_trajectory()); + builder.Connect( + trajectory_sub_force->get_output_port(), + end_effector_force_drawer->get_input_port_force_trajectory()); + builder.Connect(robot_time_passthrough->get_output_port(), + end_effector_force_drawer->get_input_port_robot_time()); + } + + builder.Connect(franka_passthrough->get_output_port(), + mux->get_input_port(0)); + builder.Connect(tray_passthrough->get_output_port(), mux->get_input_port(1)); + builder.Connect(object_passthrough->get_output_port(), + mux->get_input_port(2)); + builder.Connect(*mux, *to_pose); + builder.Connect( + to_pose->get_output_port(), + scene_graph.get_source_pose_port(plant.get_source_id().value())); + builder.Connect(*franka_state_receiver, *franka_passthrough); + builder.Connect(*franka_state_receiver, *robot_time_passthrough); + builder.Connect(*tray_state_receiver, *tray_passthrough); + builder.Connect(*object_state_receiver, *object_passthrough); + builder.Connect(*franka_state_sub, *franka_state_receiver); + builder.Connect(*tray_state_sub, *tray_state_receiver); + builder.Connect(*object_state_sub, *object_state_receiver); + + auto visualizer = &drake::geometry::MeshcatVisualizer::AddToBuilder( + &builder, scene_graph, meshcat, std::move(params)); + + auto diagram = builder.Build(); + diagram->set_name("franka_c3_visualizer"); + DrawAndSaveDiagramGraph(*diagram); + auto context = diagram->CreateDefaultContext(); + + auto& franka_state_sub_context = + diagram->GetMutableSubsystemContext(*franka_state_sub, context.get()); + auto& tray_state_sub_context = + diagram->GetMutableSubsystemContext(*tray_state_sub, context.get()); + auto& object_state_sub_context = + diagram->GetMutableSubsystemContext(*object_state_sub, context.get()); + franka_state_receiver->InitializeSubscriberPositions( + plant, franka_state_sub_context); + tray_state_receiver->InitializeSubscriberPositions(plant, + tray_state_sub_context); + object_state_receiver->InitializeSubscriberPositions( + plant, object_state_sub_context); + + /// Use the simulator to drive at a fixed rate + /// If set_publish_every_time_step is true, this publishes twice + /// Set realtime rate. Otherwise, runs as fast as possible + auto simulator = + std::make_unique>(*diagram, std::move(context)); + simulator->set_publish_every_time_step(false); + simulator->set_publish_at_initialization(false); + simulator->set_target_realtime_rate( + 1.0); // may need to change this to param.real_time_rate? + simulator->Initialize(); + + simulator->AdvanceTo(std::numeric_limits::infinity()); + // meshcat->get_mutable_recording().set_loop_mode(drake::geometry::MeshcatAnimation::LoopMode::kLoopRepeat); + // meshcat->StartRecording(); + + // simulator->AdvanceTo(18.0); + // meshcat->StopRecording(); + // meshcat->PublishRecording(); + // std::ofstream outfile("visualization.html"); + // outfile << meshcat->StaticHtml() < +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "examples/franka/diagrams/franka_c3_controller_diagram.h" +#include "examples/franka/diagrams/franka_osc_controller_diagram.h" +#include "examples/franka/parameters/franka_c3_controller_params.h" +#include "examples/franka/parameters/franka_lcm_channels.h" +#include "examples/franka/parameters/franka_sim_params.h" +#include "examples/franka/parameters/franka_sim_scene_params.h" +#include "multibody/multibody_utils.h" +#include "systems/primitives/radio_parser.h" +#include "systems/primitives/subvector_pass_through.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +namespace dairlib { + +using drake::geometry::GeometrySet; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::VectorXd; +using examples::controllers::FrankaC3ControllerDiagram; +using examples::controllers::FrankaOSCControllerDiagram; +using systems::RobotInputReceiver; +using systems::RobotOutputSender; +using systems::SubvectorPassThrough; + +int DoMain(int argc, char* argv[]) { + drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); + + drake::yaml::LoadYamlOptions yaml_options; + yaml_options.allow_yaml_with_no_cpp = true; + FrankaLcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile( + "examples/franka/parameters/lcm_channels_simulation.yaml"); + // load parameters + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_sim_params.yaml"); + FrankaSimSceneParams scene_params = + drake::yaml::LoadYamlFile( + sim_params.sim_scene_file[sim_params.scene_index]); + FrankaC3ControllerParams c3_params = + drake::yaml::LoadYamlFile( + "examples/franka/parameters/franka_c3_controller_params.yaml"); + C3Options c3_options = drake::yaml::LoadYamlFile( + c3_params.c3_options_file[c3_params.scene_index]); + + DiagramBuilder builder; + + /// Sim Start + double sim_dt = sim_params.dt; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, sim_dt); + + Parser parser(&plant); + parser.SetAutoRenaming(true); + drake::multibody::ModelInstanceIndex franka_index = + parser.AddModelsFromUrl(sim_params.franka_model)[0]; + drake::multibody::ModelInstanceIndex c3_end_effector_index = + parser.AddModels(FindResourceOrThrow(sim_params.end_effector_model))[0]; + drake::multibody::ModelInstanceIndex tray_index = + parser.AddModels(FindResourceOrThrow(sim_params.tray_model))[0]; + multibody::AddFlatTerrain(&plant, &scene_graph, 1.0, 1.0); + + RigidTransform T_X_W = RigidTransform( + drake::math::RotationMatrix(), Eigen::VectorXd::Zero(3)); + RigidTransform T_EE_W = RigidTransform( + drake::math::RotationMatrix(), sim_params.tool_attachment_frame); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"), + T_X_W); + plant.WeldFrames(plant.GetFrameByName("panda_link7"), + plant.GetFrameByName("plate", c3_end_effector_index), + T_EE_W); + + const drake::geometry::GeometrySet& franka_geom_set = + plant.CollectRegisteredGeometries({&plant.GetBodyByName("panda_link0"), + &plant.GetBodyByName("panda_link1"), + &plant.GetBodyByName("panda_link2"), + &plant.GetBodyByName("panda_link3"), + &plant.GetBodyByName("panda_link4")}); + + drake::geometry::GeometrySet support_geom_set; + std::vector environment_model_indices; + environment_model_indices.resize(scene_params.environment_models.size()); + for (int i = 0; i < scene_params.environment_models.size(); ++i) { + environment_model_indices[i] = parser.AddModels( + FindResourceOrThrow(scene_params.environment_models[i]))[0]; + RigidTransform T_E_W = + RigidTransform(drake::math::RollPitchYaw( + scene_params.environment_orientations[i]), + scene_params.environment_positions[i]); + plant.WeldFrames( + plant.world_frame(), + plant.GetFrameByName("base", environment_model_indices[i]), + T_E_W); + support_geom_set.Add(plant.GetCollisionGeometriesForBody(plant.GetBodyByName("base", + environment_model_indices[i]))); + } + plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( + {"supports", support_geom_set}, {"franka", franka_geom_set}); + + + const drake::geometry::GeometrySet& paddle_geom_set = + plant.CollectRegisteredGeometries({ + &plant.GetBodyByName("panda_link2"), + &plant.GetBodyByName("panda_link3"), + &plant.GetBodyByName("panda_link4"), + &plant.GetBodyByName("panda_link5"), + &plant.GetBodyByName("panda_link6"), + &plant.GetBodyByName("panda_link8"), + }); + auto tray_collision_set = GeometrySet( + plant.GetCollisionGeometriesForBody(plant.GetBodyByName("tray"))); + plant.ExcludeCollisionGeometriesWithCollisionFilterGroupPair( + {"paddle", paddle_geom_set}, {"tray", tray_collision_set}); + + plant.Finalize(); + + /// OSC + auto osc_controller = builder.AddSystem( + "examples/franka/parameters/franka_osc_controller_params.yaml", + "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); + + /// C3 plant + auto c3_controller = builder.AddSystem( + "examples/franka/parameters/franka_c3_controller_params.yaml", c3_options, + "examples/franka/parameters/lcm_channels_simulation.yaml", &lcm); + + /* -------------------------------------------------------------------------------------------*/ + auto passthrough = builder.AddSystem( + osc_controller->get_output_port_robot_input().size(), 0, + plant.get_actuation_input_port().size()); + auto tray_state_sender = + builder.AddSystem(plant, tray_index); + auto franka_state_sender = + builder.AddSystem(plant, franka_index, false); + auto state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_state_channel, &lcm, + drake::systems::TriggerTypeSet( + {drake::systems::TriggerType::kForced}))); + auto tray_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.tray_state_channel, &lcm, + drake::systems::TriggerTypeSet( + {drake::systems::TriggerType::kForced}))); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + + //// OSC connections + + auto radio_to_vector = builder.AddSystem(); + builder.Connect(radio_sub->get_output_port(), + radio_to_vector->get_input_port()); + // Diagram Connections + builder.Connect(osc_controller->get_output_port_robot_input(), + passthrough->get_input_port()); + builder.Connect(c3_controller->get_output_port_mpc_plan(), + osc_controller->get_input_port_end_effector_position()); + builder.Connect(c3_controller->get_output_port_mpc_plan(), + osc_controller->get_input_port_end_effector_orientation()); + builder.Connect(c3_controller->get_output_port_mpc_plan(), + osc_controller->get_input_port_end_effector_force()); + + builder.Connect(franka_state_sender->get_output_port(), + osc_controller->get_input_port_robot_state()); + builder.Connect(franka_state_sender->get_output_port(), + c3_controller->get_input_port_robot_state()); + builder.Connect(tray_state_sender->get_output_port(), + c3_controller->get_input_port_object_state()); + builder.Connect(radio_to_vector->get_output_port(), + c3_controller->get_input_port_radio()); + builder.Connect(radio_to_vector->get_output_port(), + osc_controller->get_input_port_radio()); + + builder.Connect(*franka_state_sender, *state_pub); + builder.Connect(tray_state_sender->get_output_port(), + tray_state_pub->get_input_port()); + builder.Connect(plant.get_state_output_port(franka_index), + franka_state_sender->get_input_port_state()); + builder.Connect(plant.get_state_output_port(tray_index), + tray_state_sender->get_input_port_state()); + builder.Connect(passthrough->get_output_port(), + plant.get_actuation_input_port()); + + int nq = plant.num_positions(); + int nv = plant.num_velocities(); + + if (sim_params.visualize_drake_sim) { + drake::visualization::AddDefaultVisualization(&builder); + } + + auto diagram = builder.Build(); + diagram->set_name("plate_balancing_full_diagram"); + DrawAndSaveDiagramGraph(*diagram); + + drake::systems::Simulator simulator(*diagram); + + simulator.set_publish_every_time_step(true); + simulator.set_publish_at_initialization(true); + simulator.set_target_realtime_rate(sim_params.realtime_rate); + + auto& plant_context = diagram->GetMutableSubsystemContext( + plant, &simulator.get_mutable_context()); + + VectorXd q = VectorXd::Zero(nq); + q.head(plant.num_positions(franka_index)) = sim_params.q_init_franka; + + q.tail(plant.num_positions(tray_index)) = + sim_params.q_init_tray[sim_params.scene_index]; + + plant.SetPositions(&plant_context, q); + + VectorXd v = VectorXd::Zero(nv); + plant.SetVelocities(&plant_context, v); + + simulator.Initialize(); + simulator.AdvanceTo(std::numeric_limits::infinity()); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/franka/parameters/c3_options/franka_c3_options_floating.yaml b/examples/franka/parameters/c3_options/franka_c3_options_floating.yaml new file mode 100644 index 0000000000..cf813acb07 --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_floating.yaml @@ -0,0 +1,54 @@ +admm_iter: 4 +rho: 0.1 +rho_scale: 1 +num_threads: 6 +delta_option: 1 +# options are 'MIQP' or 'QP' +projection_type: 'MIQP' +# options are 'stewart_and_trinkle' or 'anitescu +contact_model: 'stewart_and_trinkle' + +mu: 0.4 +mu_plate: 0.4 +dt: 0.05 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 3 +N: 5 + +# matrix scaling +w_Q: 20 +w_R: 0 +# Penalty on all decision variables, assuming scalar +w_G: 1 +# Penalty on all decision variables, assuming scalar +w_U: 0.5 + +# n_lambda = 2 * n_contacts + 2 * n_contacts * num_friction_directions +# size = n_x ( 7 + 3 + 6 + 3 ) + n_lambda (2 * 3 + 2 * 3 * 2) + n_u (3) = 40 for stewart and trinkle +# size = n_x ( 7 + 3 + 6 + 3 ) + n_lambda (2 * 3 * 2) + n_u (3) = 34 for anitescu +g_size: 49 +u_size: 49 +#g_size: 34 +#u_size: 34 +# State Tracking Error, assuming diagonal +q_vector: [100, 100, 500, 0, 0, 0, 0, 0, 0, 0, 10000, 10000, 10000, + 0, 0, 0, 5, 5, 10, 1, 1, 1, 5, 5, 5] +# Penalty on all decision variables +g_vector: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +#g_vector: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, +# 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +# Penalty on all decision variables +u_vector: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +#u_vector: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 1, +# 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +# Penalty on efforts, assuming diagonal +r_vector: [1, 1, 1, 0.1, 0.1, 0.1] + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml b/examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml new file mode 100644 index 0000000000..8150fd15dd --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml @@ -0,0 +1,71 @@ +admm_iter: 2 +rho: 0 # does not do anything +rho_scale: 3.9 +num_threads: 5 +delta_option: 1 +# options are 'MIQP' or 'QP' +projection_type: 'MIQP' +# options are 'stewart_and_trinkle' or 'anitescu' +#contact_model: 'stewart_and_trinkle' +contact_model: 'anitescu' +warm_start: true +use_predicted_x0: true +end_on_qp_step: false +solve_time_filter_alpha: 0.95 # \bar{dt} = (1 - alpha) dt + (alpha) * \bar{dt} +#publish_frequency : 0 +publish_frequency: 0 + +# Workspace Limits (specified as Linear Constraints on the end effector position) +workspace_limits: [[0.866, 0.5, 0.0, 0.3, 0.49], + [-0.5, 0.866, 0.0, -0.1782, -0.1682], + [0.0, 0.0, 1.0, 0.35, 0.7]] +workspace_margins: 0.05 + +u_horizontal_limits: [-10, 10] +u_vertical_limits: [0, 30] + +# LCS generation parameters +mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1] +dt: 0.075 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 7 +N: 5 +gamma: 1.0 # discount factor on MPC costs + +#matrix scaling +w_Q: 50 +w_R: 50 +#Penalty on all decision variables, assuming scalar +w_G: 0.15 +#Penalty on all decision variables, assuming scalar +w_U: 0.1 + +#State Tracking Error, assuming diagonal +q_vector: [150, 150, 150, 0, 1, 1, 0, 15000, 15000, 15000, + 5, 5, 15, 10, 10, 1, 5, 5, 5] +#Penalty on efforts, assuming diagonal +r_vector: [0.15, 0.15, 0.1] + +#Penalty on matching projected variables +g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma: [1, 1, 1, 1, 1, 1, 1] +g_lambda_n: [1, 1, 1, 1, 1, 1, 1] +g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +g_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] +g_u: [1, 1, 1] + +#Penalty on matching the QP variables +u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +u_gamma: [1, 1, 1, 1, 1, 1, 1] +u_lambda_n: [1, 1, 1, 1, 1, 1, 1] +u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +u_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] +u_u: [30, 30, 30] + + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_supports.yaml b/examples/franka/parameters/c3_options/franka_c3_options_supports.yaml new file mode 100644 index 0000000000..33c0fae0b2 --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_supports.yaml @@ -0,0 +1,73 @@ +admm_iter: 2 +rho: 0 # does not do anything +rho_scale: 4 +num_threads: 5 +delta_option: 1 +#options are 'MIQP' or 'QP' +projection_type: 'MIQP' +#options are 'stewart_and_trinkle' or 'anitescu' +#contact_model : 'stewart_and_trinkle' +contact_model: 'anitescu' +warm_start: true +use_predicted_x0: true +use_robust_formulation: false +end_on_qp_step: false +solve_time_filter_alpha: 0.95 +#set to 0 to publish as fast as possible +publish_frequency: 0 +#publish_frequency: 15 + +# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub] +workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], + [0.0, 1.0, 0.0, -0.1, 0.1], + [0.0, 0.0, 1.0, 0.35, 0.7]] + +workspace_margins: 0.05 + +u_horizontal_limits: [-10, 10] +u_vertical_limits: [0, 30] + +mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1] +dt: 0.075 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 7 +N: 5 +gamma: 1.0 # discount factor on MPC costs + +#matrix scaling +w_Q: 50 +w_R: 50 +#Penalty on all decision variables, assuming scalar +w_G: 0.15 +#Penalty on all decision variables, assuming scalar +w_U: 0.1 + +#State Tracking Error, assuming diagonal +q_vector: [150, 150, 150, 0, 1, 1, 0, 15000, 15000, 15000, + 5, 5, 15, 10, 10, 1, 5, 5, 5] +#Penalty on efforts, assuming diagonal +r_vector: [0.15, 0.15, 0.1] + +#Penalty on matching projected variables +g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma: [1, 1, 1, 1, 1, 1, 1] +g_lambda_n: [1, 1, 1, 1, 1, 1, 1] +g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +g_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] +g_u: [1, 1, 1] + +#Penalty on matching the QP variables +u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +u_gamma: [1, 1, 1, 1, 1, 1, 1] +u_lambda_n: [1, 1, 1, 1, 1, 1, 1] +u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +u_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] +u_u: [30, 30, 30] + + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_supports_st.yaml b/examples/franka/parameters/c3_options/franka_c3_options_supports_st.yaml new file mode 100644 index 0000000000..c504cd699e --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_supports_st.yaml @@ -0,0 +1,74 @@ +admm_iter: 5 +rho: 0 # does not do anything +rho_scale: 2 +num_threads: 5 +delta_option: 1 +#options are 'MIQP' or 'QP' +projection_type: 'MIQP' +#options are 'stewart_and_trinkle' or 'anitescu' +contact_model : 'stewart_and_trinkle' +#contact_model: 'anitescu' +warm_start: true +use_predicted_x0: true +use_robust_formulation: true +end_on_qp_step: false +solve_time_filter_alpha: 0.95 +#set to 0 to publish as fast as possible +publish_frequency : 0 +#publish_frequency: 15 + +# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub] +workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], + [0.0, 1.0, 0.0, -0.1, 0.1], + [0.0, 0.0, 1.0, 0.35, 0.7]] +#world_y_limits: [-0.1, 0.1] +#world_z_limits: [0.35, 0.7] +workspace_margins: 0.05 + +u_horizontal_limits: [-10, 10] +u_vertical_limits: [0, 30] + +mu: [0.6, 0.6, 0.6, 0.1, 0.1, 0.1, 0.1] +dt: 0.075 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 7 +N: 5 +gamma: 1.0 # discount factor on MPC costs + +#matrix scaling +w_Q: 50 +w_R: 50 +#Penalty on all decision variables, assuming scalar +w_G: 0.15 +#Penalty on all decision variables, assuming scalar +w_U: 0.1 + +#State Tracking Error, assuming diagonal +q_vector: [150, 150, 150, 0, 1, 1, 0, 15000, 15000, 15000, + 5, 5, 15, 10, 10, 1, 5, 5, 5] +#Penalty on efforts, assuming diagonal +r_vector: [0.15, 0.15, 0.1] + +#Penalty on matching projected variables +g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma: [1, 1, 1, 1, 1, 1, 1] +g_lambda_n: [50, 50, 50, 50, 50, 50, 50] +g_lambda_t: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] +g_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] +g_u: [1, 1, 1] + +#Penalty on matching the QP variables +u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +u_gamma: [10, 10, 10, 10, 10, 10, 10] +u_lambda_n: [300, 300, 300, 300, 300, 300, 300] +u_lambda_t: [300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300, 300] +u_lambda: [85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85] +u_u: [30, 30, 30] + + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_translation.yaml b/examples/franka/parameters/c3_options/franka_c3_options_translation.yaml new file mode 100644 index 0000000000..45c679466d --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_translation.yaml @@ -0,0 +1,72 @@ +admm_iter: 2 +rho: 0.0 +rho_scale: 4 +num_threads: 5 +delta_option: 1 +# options are 'MIQP' or 'QP' +projection_type: 'MIQP' +# options are 'stewart_and_trinkle' or 'anitescu' +#contact_model: 'anitescu' +contact_model: 'stewart_and_trinkle' +warm_start: false +use_predicted_x0: false +use_robust_formulation: false +end_on_qp_step: false +solve_time_filter_alpha: 0.95 +#publish_frequency : 0 +publish_frequency: 0 + +# Workspace Limits +workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], + [0.0, 1.0, 0.0, -0.2, 0.2], + [0.0, 0.0, 1.0, 0.3, 0.6]] +workspace_margins: 0.05 + +u_horizontal_limits: [-50, 50] +u_vertical_limits: [-50, 50] + +mu: [0.6, 0.6, 0.6] +dt: 0.05 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 3 +N: 5 +gamma: 1 + +# matrix scaling +w_Q: 50 +w_R: 25 +# Penalty on all decision variables, assuming scalar +w_G: 0.1 +# Penalty on all decision variables, assuming scalar +w_U: 0.5 + +# State Tracking Error, assuming diagonal +q_vector: [175, 175, 175, 1, 1, 1, 1, 15000, 15000, 15000, + 5, 5, 5, 10, 10, 10, 5, 5, 5] + +# Penalty on efforts, assuming diagonal +r_vector: [0.15, 0.15, 0.1] + +# Penalty on all decision variables +g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma: [50, 50, 50] +g_lambda_n: [50, 50, 50] +g_lambda_t: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] +g_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] +g_u: [1, 1, 1] + +# Penalty on all decision variables +u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +u_gamma: [100, 100, 100] +u_lambda_n: [100, 100, 100] +u_lambda_t: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] +u_lambda: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] +u_u: [30, 30, 30] + + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml b/examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml new file mode 100644 index 0000000000..a8271bfd81 --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml @@ -0,0 +1,71 @@ +admm_iter: 2 +rho: 0.0 +rho_scale: 4 +num_threads: 5 +delta_option: 1 +# options are 'MIQP' or 'QP' +projection_type: 'MIQP' +# options are 'stewart_and_trinkle' or 'anitescu' +contact_model: 'anitescu' +#contact_model: 'stewart_and_trinkle' +warm_start: true +use_predicted_x0: false +end_on_qp_step: false +solve_time_filter_alpha: 0.9 +#publish_frequency : 0 +publish_frequency: 25 + +# Workspace Limits +workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.6], + [0.0, 1.0, 0.0, -0.2, 0.2], + [0.0, 0.0, 1.0, 0.3, 0.6]] +workspace_margins: 0.05 + +u_horizontal_limits: [-10, 10] +u_vertical_limits: [0, 30] + +mu: [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] +dt: 0.05 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 6 +N: 5 +gamma: 1 + +# matrix scaling +w_Q: 50 +w_R: 1 +# Penalty on all decision variables, assuming scalar +w_G: 1 +# Penalty on all decision variables, assuming scalar +w_U: 0.5 + +# State Tracking Error, assuming diagonal +q_vector: [175, 175, 175, 10, 10, 10, 10, 5000, 5000, 5000, + 5, 5, 10, 1, 1, 1, 5, 5, 5] + +# Penalty on efforts, assuming diagonal +r_vector: [0.1, 0.1, 0.1] + +# Penalty on all decision variables +g_x: [1, 1, 1, 1, 1, 1, 1, 10, 10, 10, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma: [1, 1, 1] +g_lambda_n: [1, 1, 1] +g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +g_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] +g_u: [1, 1, 1] + +# Penalty on all decision variables +u_x: [1, 1, 1, 1, 1, 1, 1, 10, 10, 10, 1, 1, 1, 10, 10, 10, 10, 10, 10] +u_gamma: [1, 1, 1] +u_lambda_n: [1, 1, 1] +u_lambda_t: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +u_lambda: [50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50] +u_u: [10, 10, 10] + + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_options/franka_c3_options_wall.yaml b/examples/franka/parameters/c3_options/franka_c3_options_wall.yaml new file mode 100644 index 0000000000..fb5f43beb2 --- /dev/null +++ b/examples/franka/parameters/c3_options/franka_c3_options_wall.yaml @@ -0,0 +1,71 @@ +admm_iter: 3 +rho: 0 # does not do anything +rho_scale: 5 +num_threads: 4 +delta_option: 1 +#options are 'MIQP' or 'QP' +projection_type: 'MIQP' +#options are 'stewart_and_trinkle' or 'anitescu' +#contact_model : 'stewart_and_trinkle' +contact_model: 'anitescu' +warm_start: true +use_predicted_x0: true +end_on_qp_step: false +solve_time_filter_alpha: 0.95 +#set to 0 to publish as fast as possible +publish_frequency : 0 +#publish_frequency: 25 + +# End Effector Workspace Limits Specified as Linear Constraints [x, y, z, lb, ub] +workspace_limits: [[1.0, 0.0, 0.0, 0.4, 0.7], + [0.0, 1.0, 0.0, -0.2, 0.2], + [0.0, 0.0, 1.0, 0.4, 0.5]] +workspace_margins: 0.05 + +u_horizontal_limits: [-10, 10] +u_vertical_limits: [-0, 30] + +mu: [0.8, 0.8, 0.8, 1.0] +dt: 0.05 +solve_dt: 0.05 +num_friction_directions: 2 +num_contacts: 4 +N: 4 +gamma: 1.0 # discount factor on MPC costs + +#matrix scaling +w_Q: 50 +w_R: 75 +#Penalty on all decision variables, assuming scalar +w_G: 0.5 +#Penalty on all decision variables, assuming scalar +w_U: 0.5 + +#State Tracking Error, assuming diagonal +q_vector: [10, 25, 150, 1000, 1000, 1000, 1000, 25, 25, 15000, + 2.5, 5, 5, 1, 1, 500, 5, 5, 5] +#Penalty on efforts, assuming diagonal +r_vector: [2.0, 0.75, 0.05] + +#Penalty on matching projected variables +g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma: [1, 1, 1, 1] +g_lambda_n: [1, 1, 1, 1] +g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +g_lambda: [150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150] +g_u: [2.5, 2.5, 2.5] + +#Penalty on matching the QP variables +u_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +u_gamma: [1, 1, 1, 1] +u_lambda_n: [1, 1, 1, 1] +u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] +u_lambda: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] +u_u: [30, 30, 30] + + +# Parameters used in the QP projection method. As alpha -> 0, any error in +# complimentarity constraints also approaches 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1000 +penalize_changes_in_u_across_solves: true diff --git a/examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml b/examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml new file mode 100644 index 0000000000..069fb2bf64 --- /dev/null +++ b/examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml @@ -0,0 +1,14 @@ +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector.urdf +end_effector_name: plate +end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf +tool_attachment_frame: [0, 0, 0.107] +end_effector_thickness: 0.016 +object_models: [examples/franka/urdf/tray_lcs.sdf] +environment_models: [examples/franka/urdf/support_point_contact.urdf, + examples/franka/urdf/support_point_contact.urdf] +# orientation in RPY then position in XYZ +environment_orientations: [[0. , 0. , 0.5236], + [0. , 0. , 0.5236]] +environment_positions: [[0.6178, 0.3299, 0.45 ], + [0.7678, 0.0701, 0.45 ]] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/supports_scene.yaml b/examples/franka/parameters/c3_scenes/supports_scene.yaml new file mode 100644 index 0000000000..a0aaecdb2f --- /dev/null +++ b/examples/franka/parameters/c3_scenes/supports_scene.yaml @@ -0,0 +1,14 @@ +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector.urdf +end_effector_name: plate +end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf +tool_attachment_frame: [0, 0, 0.107] +end_effector_thickness: 0.016 +object_models: [examples/franka/urdf/tray_lcs.sdf] +environment_models: [examples/franka/urdf/support_point_contact.urdf, + examples/franka/urdf/support_point_contact.urdf] +# orientation in RPY then position in XYZ +environment_orientations: [[0.0, 0.0, 0.0], + [0.0, 0.0, 0.0]] +environment_positions: [[0.8, 0.15, 0.447], + [0.8, -0.15, 0.447]] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/tray_scene.yaml b/examples/franka/parameters/c3_scenes/tray_scene.yaml new file mode 100644 index 0000000000..4d8fae92ac --- /dev/null +++ b/examples/franka/parameters/c3_scenes/tray_scene.yaml @@ -0,0 +1,11 @@ +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector.urdf +end_effector_name: plate +end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf +tool_attachment_frame: [0, 0, 0.107] +end_effector_thickness: 0.016 +object_models: [examples/franka/urdf/tray_lcs.sdf] +environment_models: [] +# orientation in RPY then position in XYZ +environment_orientations: [] +environment_positions: [] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/two_object_scene.yaml b/examples/franka/parameters/c3_scenes/two_object_scene.yaml new file mode 100644 index 0000000000..9a394e55c8 --- /dev/null +++ b/examples/franka/parameters/c3_scenes/two_object_scene.yaml @@ -0,0 +1,11 @@ +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector.urdf +end_effector_name: plate +end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf +tool_attachment_frame: [0, 0, 0.107] +end_effector_thickness: 0.016 +object_models: [examples/franka/urdf/tray_lcs.sdf, examples/franka/urdf/cylinder_lcs.sdf] +environment_models: [] +# orientation in RPY then position in XYZ +environment_orientations: [] +environment_positions: [] \ No newline at end of file diff --git a/examples/franka/parameters/c3_scenes/wall_scene.yaml b/examples/franka/parameters/c3_scenes/wall_scene.yaml new file mode 100644 index 0000000000..39e02111a1 --- /dev/null +++ b/examples/franka/parameters/c3_scenes/wall_scene.yaml @@ -0,0 +1,11 @@ +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector.urdf +end_effector_name: plate +end_effector_lcs_model: examples/franka/urdf/plate_end_effector_translation.urdf +tool_attachment_frame: [0, 0, 0.107] +end_effector_thickness: 0.016 +object_models: [examples/franka/urdf/tray_lcs.sdf] +environment_models: [examples/franka/urdf/side_wall.urdf] +# orientation in RPY then position in XYZ +environment_orientations: [[0.0, 0.0, 0.0]] +environment_positions: [[0.6, 0.285, 0.35]] \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_controller_params.h b/examples/franka/parameters/franka_c3_controller_params.h new file mode 100644 index 0000000000..4b09b48ca6 --- /dev/null +++ b/examples/franka/parameters/franka_c3_controller_params.h @@ -0,0 +1,42 @@ +#pragma once + +#include "solvers/c3_options.h" + +#include "drake/common/yaml/yaml_read_archive.h" + +struct FrankaC3ControllerParams { + int scene_index; + std::vector c3_options_file; + std::vector c3_scene_file; + std::string osqp_settings_file; + + bool include_end_effector_orientation; + double target_frequency; + + double near_target_threshold; + std::vector first_target; + std::vector second_target; + std::vector third_target; + double x_scale; + double y_scale; + double z_scale; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(c3_options_file)); + a->Visit(DRAKE_NVP(c3_scene_file)); + a->Visit(DRAKE_NVP(osqp_settings_file)); + + a->Visit(DRAKE_NVP(include_end_effector_orientation)); + a->Visit(DRAKE_NVP(target_frequency)); + a->Visit(DRAKE_NVP(scene_index)); + + a->Visit(DRAKE_NVP(first_target)); + a->Visit(DRAKE_NVP(second_target)); + a->Visit(DRAKE_NVP(third_target)); + a->Visit(DRAKE_NVP(x_scale)); + a->Visit(DRAKE_NVP(y_scale)); + a->Visit(DRAKE_NVP(z_scale)); + a->Visit(DRAKE_NVP(near_target_threshold)); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_controller_params.yaml b/examples/franka/parameters/franka_c3_controller_params.yaml new file mode 100644 index 0000000000..40a59b106a --- /dev/null +++ b/examples/franka/parameters/franka_c3_controller_params.yaml @@ -0,0 +1,36 @@ +scene_index: 1 +c3_options_file: [examples/franka/parameters/c3_options/franka_c3_options_translation.yaml, + examples/franka/parameters/c3_options/franka_c3_options_supports.yaml, + examples/franka/parameters/c3_options/franka_c3_options_rotated_supports.yaml, + examples/franka/parameters/c3_options/franka_c3_options_wall.yaml, + examples/franka/parameters/c3_options/franka_c3_options_two_objects.yaml] +c3_scene_file: [examples/franka/parameters/c3_scenes/tray_scene.yaml, + examples/franka/parameters/c3_scenes/supports_scene.yaml, + examples/franka/parameters/c3_scenes/supports_rotated_scene.yaml, + examples/franka/parameters/c3_scenes/wall_scene.yaml, + examples/franka/parameters/c3_scenes/tray_scene.yaml] +osqp_settings_file: examples/franka/parameters/franka_c3_qp_settings.yaml + +include_end_effector_orientation: false +# Note: C3 is set to update much faster than this number. This is just to reduce preempting of the current plan +target_frequency: 0 #unused + +near_target_threshold: 0.05 +first_target: [[0.55, 0.0, 0.485], + [0.45, 0, 0.485], + [0.3897, 0.025, 0.49], + [0.55, 0.0, 0.485], + [0.45, 0.0, 0.5],] +second_target: [[0.55, 0.0, 0.485], + [0.45, 0, 0.6], + [0.3897, 0.025, 0.6], + [0.55, 0.0, 0.485], + [0.45, 0.0, 0.5],] +third_target: [[0.55, 0.0, 0.485], + [0.7, 0.00, 0.485], + [0.6062, 0.15, 0.49], + [0.55, 0.0, 0.485], + [0.45, 0.0, 0.5],] +x_scale: 0.1 +y_scale: 0.1 +z_scale: 0.1 diff --git a/examples/franka/parameters/franka_c3_qp_settings.yaml b/examples/franka/parameters/franka_c3_qp_settings.yaml new file mode 100644 index 0000000000..8a12cbe60e --- /dev/null +++ b/examples/franka/parameters/franka_c3_qp_settings.yaml @@ -0,0 +1,26 @@ +print_to_console: 0 +log_file_name: "" +int_options: + max_iter: 1000 + verbose: 0 + warm_start: 1 + scaling: 1 + adaptive_rho: 1 + adaptive_rho_interval: 0 + polish: 1 + polish_refine_iter: 1 + scaled_termination: 1 + check_termination: 100 +double_options: + rho: 0.1 + sigma: 1e-6 + eps_abs: 1e-5 + eps_rel: 1e-5 + eps_prim_inf: 1e-5 + eps_dual_inf: 1e-5 + alpha: 1.6 + delta: 1e-6 + adaptive_rho_tolerance: 5 + adaptive_rho_fraction: 0.4 + time_limit: 0 +string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_scene_params.h b/examples/franka/parameters/franka_c3_scene_params.h new file mode 100644 index 0000000000..7d4034db74 --- /dev/null +++ b/examples/franka/parameters/franka_c3_scene_params.h @@ -0,0 +1,30 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + +struct FrankaC3SceneParams { + std::string franka_model; + std::string end_effector_model; + std::string end_effector_name; + Eigen::Vector3d tool_attachment_frame; + double end_effector_thickness; + std::string end_effector_lcs_model; + std::vector object_models; + std::vector environment_models; + std::vector environment_orientations; + std::vector environment_positions; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(franka_model)); + a->Visit(DRAKE_NVP(end_effector_model)); + a->Visit(DRAKE_NVP(end_effector_name)); + a->Visit(DRAKE_NVP(tool_attachment_frame)); + a->Visit(DRAKE_NVP(end_effector_thickness)); + a->Visit(DRAKE_NVP(end_effector_lcs_model)); + a->Visit(DRAKE_NVP(object_models)); + a->Visit(DRAKE_NVP(environment_models)); + a->Visit(DRAKE_NVP(environment_orientations)); + a->Visit(DRAKE_NVP(environment_positions)); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_lcm_channels.h b/examples/franka/parameters/franka_lcm_channels.h new file mode 100644 index 0000000000..adb84c4e2b --- /dev/null +++ b/examples/franka/parameters/franka_lcm_channels.h @@ -0,0 +1,39 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + + +struct FrankaLcmChannels { + std::string franka_state_channel; + std::string tray_state_channel; + std::string object_state_channel; + std::string franka_input_channel; + std::string franka_input_echo; + std::string osc_channel; + std::string osc_debug_channel; + std::string c3_actor_channel; + std::string c3_object_channel; + std::string c3_force_channel; + std::string c3_debug_output_channel; + std::string c3_target_state_channel; + std::string c3_actual_state_channel; + std::string radio_channel; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(franka_state_channel)); + a->Visit(DRAKE_NVP(tray_state_channel)); + a->Visit(DRAKE_NVP(object_state_channel)); + a->Visit(DRAKE_NVP(franka_input_channel)); + a->Visit(DRAKE_NVP(franka_input_echo)); + a->Visit(DRAKE_NVP(osc_channel)); + a->Visit(DRAKE_NVP(osc_debug_channel)); + a->Visit(DRAKE_NVP(c3_actor_channel)); + a->Visit(DRAKE_NVP(c3_object_channel)); + a->Visit(DRAKE_NVP(c3_force_channel)); + a->Visit(DRAKE_NVP(c3_debug_output_channel)); + a->Visit(DRAKE_NVP(c3_target_state_channel)); + a->Visit(DRAKE_NVP(c3_actual_state_channel)); + a->Visit(DRAKE_NVP(radio_channel)); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_osc_controller_params.h b/examples/franka/parameters/franka_osc_controller_params.h new file mode 100644 index 0000000000..2c1b6215f4 --- /dev/null +++ b/examples/franka/parameters/franka_osc_controller_params.h @@ -0,0 +1,100 @@ +#pragma once + +#include "systems/controllers/osc/osc_gains.h" + +#include "drake/common/yaml/yaml_read_archive.h" + +struct FrankaControllerParams : OSCGains { + std::string franka_model; + std::string end_effector_model; + std::string end_effector_name; + + Eigen::VectorXd tool_attachment_frame; + double end_effector_acceleration; + bool track_end_effector_orientation; + bool cancel_gravity_compensation; + bool enforce_acceleration_constraints; + bool publish_debug_info; + + Eigen::VectorXd neutral_position; + double x_scale; + double y_scale; + double z_scale; + + double w_elbow; + double elbow_kp; + double elbow_kd; + + std::vector EndEffectorW; + std::vector EndEffectorKp; + std::vector EndEffectorKd; + std::vector EndEffectorRotW; + std::vector EndEffectorRotKp; + std::vector EndEffectorRotKd; + std::vector LambdaEndEffectorW; + + Eigen::MatrixXd W_end_effector; + Eigen::MatrixXd K_p_end_effector; + Eigen::MatrixXd K_d_end_effector; + Eigen::MatrixXd W_mid_link; + Eigen::MatrixXd K_p_mid_link; + Eigen::MatrixXd K_d_mid_link; + Eigen::MatrixXd W_end_effector_rot; + Eigen::MatrixXd K_p_end_effector_rot; + Eigen::MatrixXd K_d_end_effector_rot; + Eigen::MatrixXd W_ee_lambda; + + template + void Serialize(Archive* a) { + OSCGains::Serialize(a); + + a->Visit(DRAKE_NVP(franka_model)); + a->Visit(DRAKE_NVP(end_effector_model)); + a->Visit(DRAKE_NVP(end_effector_name)); + a->Visit(DRAKE_NVP(end_effector_acceleration)); + a->Visit(DRAKE_NVP(track_end_effector_orientation)); + a->Visit(DRAKE_NVP(cancel_gravity_compensation)); + a->Visit(DRAKE_NVP(enforce_acceleration_constraints)); + a->Visit(DRAKE_NVP(publish_debug_info)); + a->Visit(DRAKE_NVP(EndEffectorW)); + a->Visit(DRAKE_NVP(EndEffectorKp)); + a->Visit(DRAKE_NVP(EndEffectorKd)); + a->Visit(DRAKE_NVP(EndEffectorRotW)); + a->Visit(DRAKE_NVP(EndEffectorRotKp)); + a->Visit(DRAKE_NVP(EndEffectorRotKd)); + a->Visit(DRAKE_NVP(LambdaEndEffectorW)); + a->Visit(DRAKE_NVP(w_elbow)); + a->Visit(DRAKE_NVP(elbow_kp)); + a->Visit(DRAKE_NVP(elbow_kd)); + a->Visit(DRAKE_NVP(tool_attachment_frame)); + a->Visit(DRAKE_NVP(neutral_position)); + a->Visit(DRAKE_NVP(x_scale)); + a->Visit(DRAKE_NVP(y_scale)); + a->Visit(DRAKE_NVP(z_scale)); + + W_end_effector = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorW.data(), 3, 3); + K_p_end_effector = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorKp.data(), 3, 3); + K_d_end_effector = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorKd.data(), 3, 3); + W_mid_link = this->w_elbow * MatrixXd::Identity(1, 1); + K_p_mid_link = this->elbow_kp * MatrixXd::Identity(1, 1); + K_d_mid_link = this->elbow_kd * MatrixXd::Identity(1, 1); + W_end_effector_rot = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorRotW.data(), 3, 3); + K_p_end_effector_rot = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorRotKp.data(), 3, 3); + K_d_end_effector_rot = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorRotKd.data(), 3, 3); + W_ee_lambda = Eigen::Map< + Eigen::Matrix>( + this->LambdaEndEffectorW.data(), 3, 3); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_osc_controller_params.yaml b/examples/franka/parameters/franka_osc_controller_params.yaml new file mode 100644 index 0000000000..8fa4ae1673 --- /dev/null +++ b/examples/franka/parameters/franka_osc_controller_params.yaml @@ -0,0 +1,68 @@ +controller_frequency: 1000 + +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector_massless.urdf +end_effector_name: plate + +tool_attachment_frame: [0, 0, 0.107] + +neutral_position: [0.55, 0, 0.45] +x_scale: 0.1 +y_scale: 0.1 +z_scale: 0.1 + +w_input: 0.00 +w_input_reg: 0.0 +w_accel: 0.00001 +w_soft_constraint: 0.0 +w_lambda: 0.0 +impact_threshold: 0.000 +impact_tau: 0.000 +mu: 1.0 # unused +end_effector_acceleration: 10 +track_end_effector_orientation: false +cancel_gravity_compensation: false +enforce_acceleration_constraints: false +publish_debug_info: true + +W_accel: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] +W_input_reg: [ 1, 1, 1, 1, 1, 1, 10] +W_lambda_c_reg: [ 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01 ] +W_lambda_h_reg: [ 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001 ] + +w_elbow: 1 +elbow_kp: 200 +elbow_kd: 10 + +EndEffectorW: + [1, 0, 0, + 0, 1, 0, + 0, 0, 1] +EndEffectorKp: + [ 200, 0, 0, + 0, 200, 0, + 0, 0, 200 ] +EndEffectorKd: + [ 20, 0, 0, + 0, 20, 0, + 0, 0, 20 ] +EndEffectorRotW: + [ 10, 0, 0, + 0, 10, 0, + 0, 0, 10 ] +EndEffectorRotKp: + [ 800, 0, 0, + 0, 800, 0, + 0, 0, 800] +EndEffectorRotKd: + [ 40, 0, 0, + 0, 40, 0, + 0, 0, 40] +LambdaEndEffectorW: + [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] diff --git a/examples/franka/parameters/franka_osc_qp_settings.yaml b/examples/franka/parameters/franka_osc_qp_settings.yaml new file mode 100644 index 0000000000..e14875b7a1 --- /dev/null +++ b/examples/franka/parameters/franka_osc_qp_settings.yaml @@ -0,0 +1,26 @@ +print_to_console: 0 +log_file_name: "" +int_options: + max_iter: 1000 + verbose: 0 + warm_start: 1 + scaling: 1 + adaptive_rho: 1 + adaptive_rho_interval: 0 + polish: 1 + polish_refine_iter: 1 + scaled_termination: 1 + check_termination: 25 +double_options: + rho: 0.001 + sigma: 1e-6 + eps_abs: 1e-5 + eps_rel: 1e-5 + eps_prim_inf: 1e-5 + eps_dual_inf: 1e-5 + alpha: 1.6 + delta: 1e-6 + adaptive_rho_tolerance: 5 + adaptive_rho_fraction: 0.4 + time_limit: 0 +string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_qp_options.yaml b/examples/franka/parameters/franka_qp_options.yaml new file mode 100644 index 0000000000..9771db4a84 --- /dev/null +++ b/examples/franka/parameters/franka_qp_options.yaml @@ -0,0 +1,4 @@ +Q: 2 +R: 0.1 +G: 3 +U: 0 diff --git a/examples/franka/parameters/franka_sim_params.h b/examples/franka/parameters/franka_sim_params.h new file mode 100644 index 0000000000..0835d92e10 --- /dev/null +++ b/examples/franka/parameters/franka_sim_params.h @@ -0,0 +1,81 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + +struct FrankaSimParams { + std::vector sim_scene_file; + std::string franka_model; + std::string end_effector_model; + std::string tray_model; + std::string object_model; + + double dt; + double realtime_rate; + double actuator_delay; + double franka_publish_rate; + double tray_publish_rate; + double object_publish_rate; + double visualizer_publish_rate; + + int scene_index; + bool visualize_drake_sim; + bool publish_efforts; + bool publish_object_velocities; + + Eigen::VectorXd q_init_franka; + std::vector q_init_tray; + std::vector q_init_object; + Eigen::VectorXd tool_attachment_frame; + + std::vector world_x_limits; + std::vector world_y_limits; + std::vector world_z_limits; + + std::vector external_force_scaling; + + bool visualize_pose_trace; + bool visualize_center_of_mass_plan; + bool visualize_c3_forces; + bool visualize_c3_object_state; + bool visualize_c3_end_effector_state; + bool visualize_workspace; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(sim_scene_file)); + a->Visit(DRAKE_NVP(franka_model)); + a->Visit(DRAKE_NVP(end_effector_model)); + a->Visit(DRAKE_NVP(tray_model)); + a->Visit(DRAKE_NVP(object_model)); + + a->Visit(DRAKE_NVP(dt)); + a->Visit(DRAKE_NVP(realtime_rate)); + a->Visit(DRAKE_NVP(actuator_delay)); + a->Visit(DRAKE_NVP(franka_publish_rate)); + a->Visit(DRAKE_NVP(tray_publish_rate)); + a->Visit(DRAKE_NVP(object_publish_rate)); + a->Visit(DRAKE_NVP(visualizer_publish_rate)); + + a->Visit(DRAKE_NVP(scene_index)); + a->Visit(DRAKE_NVP(visualize_drake_sim)); + a->Visit(DRAKE_NVP(publish_efforts)); + a->Visit(DRAKE_NVP(publish_object_velocities)); + + a->Visit(DRAKE_NVP(q_init_franka)); + a->Visit(DRAKE_NVP(q_init_tray)); + a->Visit(DRAKE_NVP(q_init_object)); + a->Visit(DRAKE_NVP(tool_attachment_frame)); + + a->Visit(DRAKE_NVP(world_x_limits)); + a->Visit(DRAKE_NVP(world_y_limits)); + a->Visit(DRAKE_NVP(world_z_limits)); + a->Visit(DRAKE_NVP(external_force_scaling)); + + a->Visit(DRAKE_NVP(visualize_pose_trace)); + a->Visit(DRAKE_NVP(visualize_center_of_mass_plan)); + a->Visit(DRAKE_NVP(visualize_c3_forces)); + a->Visit(DRAKE_NVP(visualize_c3_object_state)); + a->Visit(DRAKE_NVP(visualize_c3_end_effector_state)); + a->Visit(DRAKE_NVP(visualize_workspace)); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/franka_sim_params.yaml b/examples/franka/parameters/franka_sim_params.yaml new file mode 100644 index 0000000000..b95a39c5b4 --- /dev/null +++ b/examples/franka/parameters/franka_sim_params.yaml @@ -0,0 +1,59 @@ +franka_model: package://drake_models/franka_description/urdf/panda_arm.urdf +end_effector_model: examples/franka/urdf/plate_end_effector.urdf +tray_model: examples/franka/urdf/tray.sdf # rigid contact model does not do sticking well +object_model: examples/franka/urdf/cylinder_object.urdf +franka_publish_rate: 1000 +tray_publish_rate: 1000 +object_publish_rate: 1000 +visualizer_publish_rate: 32 +actuator_delay: 0.000 + +scene_index: 1 +visualize_drake_sim: false +publish_efforts: true +publish_object_velocities: true + +tool_attachment_frame: [0, 0, 0.107] + +sim_scene_file: [examples/franka/parameters/sim_scenes/empty_scene.yaml, + examples/franka/parameters/sim_scenes/supports_scene.yaml, + examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml, + examples/franka/parameters/sim_scenes/wall_scene.yaml] + +# Workspace Limits +world_x_limits: [[0.4, 0.6], + [0.4, 0.6], + [0.45, 0.65], + [0.45, 0.65]] +world_y_limits: [[-0.15, 0.15], + [-0.1, 0.1], + [-0.05, 0.3], + [0.45, 0.65]] +world_z_limits: [[0.35, 0.7], + [0.35, 0.7], + [0.35, 0.7], + [0.45, 0.65]] + +external_force_scaling: [10.0, 10.0, 10.0] + +dt: 0.0005 +realtime_rate: 1.0 +q_init_franka: [-1.3, 1.6, 1.6, -2.1, 1.57, 1.62, -0.81] + +q_init_tray: [[1, 0, 0, 0, 0.55, 0.0, 0.465], + [1, 0, 0, 0, 0.7, 0.00, 0.485], + [1. , 0. , 0. , 0. , 0.5889, 0.14 , 0.485], + [0.8775826, 0, 0, 0.4794255, 0.55, 0.02, 0.46]] +# [1, 0, 0, 0, 0.55, 0.0, 0.46]] + +q_init_object: [[1, 0, 0, 0, 0.55, 0.0, 0.0], + [1, 0, 0, 0, 0.7, 0.00, 0.0], + [1. , 0. , 0. , 0. , -0.55, 0.0, 0.0 ], + [1. , 0. , 0. , 0. , -1.55, 0.0, 0.0 ]] + +visualize_workspace: false +visualize_pose_trace: true +visualize_center_of_mass_plan: false +visualize_c3_forces: true +visualize_c3_object_state: true +visualize_c3_end_effector_state: false diff --git a/examples/franka/parameters/franka_sim_scene_params.h b/examples/franka/parameters/franka_sim_scene_params.h new file mode 100644 index 0000000000..cc546c6b30 --- /dev/null +++ b/examples/franka/parameters/franka_sim_scene_params.h @@ -0,0 +1,25 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + +// Currently this scene only defines static environment obstacles +struct FrankaSimSceneParams { + std::vector environment_models; + std::vector environment_orientations; + std::vector environment_positions; + + Eigen::VectorXd camera_pose; + Eigen::VectorXd camera_target; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(environment_models)); + a->Visit(DRAKE_NVP(environment_orientations)); + a->Visit(DRAKE_NVP(environment_positions)); + DRAKE_DEMAND(environment_models.size() == environment_orientations.size()); + DRAKE_DEMAND(environment_models.size() == environment_positions.size()); + + a->Visit(DRAKE_NVP(camera_pose)); + a->Visit(DRAKE_NVP(camera_target)); + } +}; \ No newline at end of file diff --git a/examples/franka/parameters/lcm_channels_hardware.yaml b/examples/franka/parameters/lcm_channels_hardware.yaml new file mode 100644 index 0000000000..a1c12f8099 --- /dev/null +++ b/examples/franka/parameters/lcm_channels_hardware.yaml @@ -0,0 +1,17 @@ +franka_state_channel: FRANKA_STATE +tray_state_channel: TRAY_STATE +object_state_channel: BOX_STATE +franka_input_channel: FRANKA_INPUT +franka_input_echo: FRANKA_INPUT_ECHO + +osc_channel: OSC_FRANKA +osc_debug_channel: OSC_DEBUG_FRANKA + +c3_actor_channel: C3_TRAJECTORY_ACTOR +c3_object_channel: C3_TRAJECTORY_TRAY +c3_force_channel: C3_TRAJECTORY_FORCE +c3_debug_output_channel: C3_DEBUG +c3_target_state_channel: C3_TARGET +c3_actual_state_channel: C3_ACTUAL + +radio_channel: RADIO diff --git a/examples/franka/parameters/lcm_channels_simulation.yaml b/examples/franka/parameters/lcm_channels_simulation.yaml new file mode 100644 index 0000000000..97596064bf --- /dev/null +++ b/examples/franka/parameters/lcm_channels_simulation.yaml @@ -0,0 +1,17 @@ +franka_state_channel: FRANKA_STATE_SIMULATION # lcmt_robot_output +tray_state_channel: TRAY_STATE_SIMULATION # lcmt_object_state +object_state_channel: OBJECT_STATE_SIMULATION # lcmt_object_state +franka_input_channel: FRANKA_INPUT # lcmt_robot_input +franka_input_echo: FRANKA_INPUT_ECHO + +osc_channel: OSC_FRANKA +osc_debug_channel: OSC_DEBUG_FRANKA + +c3_actor_channel: C3_TRAJECTORY_ACTOR # lcmt_timestamped_saved_traj +c3_object_channel: C3_TRAJECTORY_TRAY # lcmt_timestamped_saved_traj +c3_force_channel: C3_FORCES +c3_debug_output_channel: C3_DEBUG +c3_target_state_channel: C3_TARGET # lcmt_c3_state +c3_actual_state_channel: C3_ACTUAL # lcmt_c3_state + +radio_channel: RADIO diff --git a/examples/franka/parameters/sim_scenes/empty_scene.yaml b/examples/franka/parameters/sim_scenes/empty_scene.yaml new file mode 100644 index 0000000000..b813f26d31 --- /dev/null +++ b/examples/franka/parameters/sim_scenes/empty_scene.yaml @@ -0,0 +1,7 @@ +environment_models: [] +# orientation in RPY then position in XYZ +environment_orientations: [] +environment_positions: [] + +camera_pose: [1.5, 0, 0.6] +camera_target: [0.5, 0, 0.5] \ No newline at end of file diff --git a/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml b/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml new file mode 100644 index 0000000000..6f16b5b21b --- /dev/null +++ b/examples/franka/parameters/sim_scenes/supports_rotated_scene.yaml @@ -0,0 +1,16 @@ +environment_models: [examples/franka/urdf/support.urdf, + examples/franka/urdf/support.urdf, + examples/franka/urdf/center_support.urdf] +# orientation in RPY then position in XYZ +environment_orientations: [[0. , 0. , 0.5236], + [0. , 0. , 0.5236], + [0. , 0. , 0.5236]] +environment_positions: [[0.6178, 0.3299, 0.45 ], + [0.7678, 0.0701, 0.45 ], + [0.5889, 0.14 , 0.45 ]] + +#camera_pose: [ 0.5, -0.9, 0.75 ] +#camera_target: [ 0.5, 0, 0.5 ] + +camera_pose: [ 0.5, 0.0, 1.5 ] +camera_target: [ 0.5, 0, 0.5 ] \ No newline at end of file diff --git a/examples/franka/parameters/sim_scenes/supports_scene.yaml b/examples/franka/parameters/sim_scenes/supports_scene.yaml new file mode 100644 index 0000000000..1f9c99e12a --- /dev/null +++ b/examples/franka/parameters/sim_scenes/supports_scene.yaml @@ -0,0 +1,13 @@ +environment_models: [examples/franka/urdf/support.urdf, + examples/franka/urdf/support.urdf, + examples/franka/urdf/center_support.urdf] +# orientation in RPY then position in XYZ +environment_orientations: [[0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0]] +environment_positions: [[0.8, 0.15, 0.447], + [0.8, -0.15, 0.447], + [0.7, 0.0, 0.447]] + +camera_pose: [0.5, -0.9, 0.75] +camera_target: [0.5, 0, 0.5] \ No newline at end of file diff --git a/examples/franka/parameters/sim_scenes/wall_scene.yaml b/examples/franka/parameters/sim_scenes/wall_scene.yaml new file mode 100644 index 0000000000..336dae7191 --- /dev/null +++ b/examples/franka/parameters/sim_scenes/wall_scene.yaml @@ -0,0 +1,10 @@ +camera_pose: [ 1.5, 0, 0.75 ] +camera_target: [ 0.5, 0, 0.5 ] +#camera_pose: [ 0.55, 0, 1.2 ] +#camera_target: [ 0.55, 0, 0.5 ] + +environment_models: [examples/franka/urdf/side_wall.urdf] +# orientation in RPY then position in XYZ +environment_orientations: [[0.0, 0.0, 0.0]] +environment_positions: [[0.6, 0.3, 0.35]] + diff --git a/examples/franka/start_logging.py b/examples/franka/start_logging.py new file mode 100644 index 0000000000..0717bbc03c --- /dev/null +++ b/examples/franka/start_logging.py @@ -0,0 +1,39 @@ +import subprocess +import os +import glob +import codecs +from datetime import date +import sys + +def main(log_type): + curr_date = date.today().strftime("%m_%d_%y") + year = date.today().strftime("%Y") + logdir = f"{os.getenv('HOME')}/logs/{year}/{curr_date}" + dair = f"{os.getenv('HOME')}/workspace/dairlib/" + + if not os.path.isdir(logdir): + os.mkdir(logdir) + + git_diff = subprocess.check_output(['git', 'diff'], cwd=dair) + commit_tag = subprocess.check_output(['git', 'rev-parse', 'HEAD'], cwd=dair) + + os.chdir(logdir) + current_logs = sorted(glob.glob(log_type + 'log-*')) + try: + last_log = int(current_logs[-1].split('-')[-1]) + log_num = f'{last_log+1:02}' + except: + log_num = '00' + + if log_type == 'hw': + with open('commit_tag%s' % log_num, 'w') as f: + f.write(str(commit_tag)) + f.write("\n\ngit diff:\n\n") + f.write(codecs.getdecoder("unicode_escape")(git_diff)[0]) + + subprocess.run(['lcm-logger', '-f', log_type + 'log-%s' % log_num]) + + +if __name__ == '__main__': + log_type = sys.argv[1] + main(log_type) diff --git a/examples/franka/systems/BUILD.bazel b/examples/franka/systems/BUILD.bazel new file mode 100644 index 0000000000..2f2e1e7fc1 --- /dev/null +++ b/examples/franka/systems/BUILD.bazel @@ -0,0 +1,59 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "franka_systems", + srcs = [], + deps = [ + ":c3_trajectory_generator", + ":external_force_generator", + ":plate_balancing_target", + ], +) + +cc_library( + name = "external_force_generator", + srcs = ["external_force_generator.cc"], + hdrs = ["external_force_generator.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "plate_balancing_target", + srcs = ["plate_balancing_target.cc"], + hdrs = ["plate_balancing_target.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "c3_trajectory_generator", + srcs = [ + "c3_trajectory_generator.cc", + ], + hdrs = [ + "c3_trajectory_generator.h", + ], + deps = [ + "//systems:franka_kinematics", + "//common:find_resource", + "//lcm:lcm_trajectory_saver", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//solvers:c3", + "//solvers:c3_output", + "//solvers:solver_options_io", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) \ No newline at end of file diff --git a/examples/franka/systems/c3_trajectory_generator.cc b/examples/franka/systems/c3_trajectory_generator.cc new file mode 100644 index 0000000000..024223162b --- /dev/null +++ b/examples/franka/systems/c3_trajectory_generator.cc @@ -0,0 +1,154 @@ +#include "examples/franka/systems/c3_trajectory_generator.h" + +#include + +#include "common/find_resource.h" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "systems/franka_kinematics_vector.h" +#include "multibody/multibody_utils.h" +#include "solvers/c3_output.h" +#include "solvers/lcs.h" + +namespace dairlib { + +using drake::multibody::ModelInstanceIndex; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using Eigen::MatrixXd; +using Eigen::MatrixXf; +using Eigen::VectorXd; +using solvers::LCS; +using systems::TimestampedVector; + +namespace systems { + +C3TrajectoryGenerator::C3TrajectoryGenerator( + const drake::multibody::MultibodyPlant& plant, C3Options c3_options) + : plant_(plant), c3_options_(std::move(c3_options)), N_(c3_options_.N) { + this->set_name("c3_trajectory_generator"); + + n_q_ = plant_.num_positions(); + n_v_ = plant_.num_velocities(); + n_x_ = n_q_ + n_v_; + if (c3_options_.contact_model == "stewart_and_trinkle") { + n_lambda_ = + 2 * c3_options_.num_contacts + + 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; + } else if (c3_options_.contact_model == "anitescu") { + n_lambda_ = + 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; + } + n_u_ = plant_.num_actuators(); + + c3_solution_port_ = + this->DeclareAbstractInputPort("c3_solution", + drake::Value()) + .get_index(); + + actor_trajectory_port_ = + this->DeclareAbstractOutputPort( + "c3_actor_trajectory_output", + dairlib::lcmt_timestamped_saved_traj(), + &C3TrajectoryGenerator::OutputActorTrajectory) + .get_index(); + object_trajectory_port_ = + this->DeclareAbstractOutputPort( + "c3_object_trajectory_output", + dairlib::lcmt_timestamped_saved_traj(), + &C3TrajectoryGenerator::OutputObjectTrajectory) + .get_index(); +} + +void C3TrajectoryGenerator::OutputActorTrajectory( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_traj) const { + const auto& c3_solution = + this->EvalInputValue(context, c3_solution_port_); + DRAKE_DEMAND(c3_solution->x_sol_.rows() == n_q_ + n_v_); + + MatrixXd knots = MatrixXd::Zero(6, N_); + knots.topRows(3) = c3_solution->x_sol_.topRows(3).cast(); + knots.bottomRows(3) = + c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); + LcmTrajectory::Trajectory end_effector_traj; + end_effector_traj.traj_name = "end_effector_position_target"; + end_effector_traj.datatypes = + std::vector(knots.rows(), "double"); + end_effector_traj.datapoints = knots; + end_effector_traj.time_vector = c3_solution->time_vector_.cast(); + LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, + "end_effector_position_target", + "end_effector_position_target", false); + + MatrixXd force_samples = c3_solution->u_sol_.cast(); + LcmTrajectory::Trajectory force_traj; + force_traj.traj_name = "end_effector_force_target"; + force_traj.datatypes = + std::vector(force_samples.rows(), "double"); + force_traj.datapoints = force_samples; + force_traj.time_vector = c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); + + if (publish_end_effector_orientation_) { + LcmTrajectory::Trajectory end_effector_orientation_traj; + // first 3 rows are rpy, last 3 rows are angular velocity + MatrixXd orientation_samples = MatrixXd::Zero(6, N_); + orientation_samples.topRows(3) = + c3_solution->x_sol_.topRows(6).bottomRows(3).cast(); + orientation_samples.bottomRows(3) = c3_solution->x_sol_.bottomRows(n_v_) + .topRows(6) + .bottomRows(3) + .cast(); + end_effector_orientation_traj.traj_name = "end_effector_orientation_target"; + end_effector_orientation_traj.datatypes = + std::vector(orientation_samples.rows(), "double"); + end_effector_orientation_traj.datapoints = orientation_samples; + end_effector_orientation_traj.time_vector = + c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(end_effector_orientation_traj.traj_name, + end_effector_orientation_traj); + } + + output_traj->saved_traj = lcm_traj.GenerateLcmObject(); + output_traj->utime = context.get_time() * 1e6; +} + +void C3TrajectoryGenerator::OutputObjectTrajectory( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_traj) const { + const auto& c3_solution = + this->EvalInputValue(context, c3_solution_port_); + DRAKE_DEMAND(c3_solution->x_sol_.rows() == n_q_ + n_v_); + MatrixXd knots = MatrixXd::Zero(6, N_); + knots.topRows(3) = c3_solution->x_sol_.middleRows(n_q_ - 3, 3).cast(); + knots.bottomRows(3) = + c3_solution->x_sol_.middleRows(n_q_ + n_v_ - 3, 3).cast(); + LcmTrajectory::Trajectory object_traj; + object_traj.traj_name = "object_position_target"; + object_traj.datatypes = std::vector(knots.rows(), "double"); + object_traj.datapoints = knots; + object_traj.time_vector = c3_solution->time_vector_.cast(); + LcmTrajectory lcm_traj({object_traj}, {"object_position_target"}, + "object_target", "object_target", false); + + LcmTrajectory::Trajectory object_orientation_traj; + // first 3 rows are rpy, last 3 rows are angular velocity + MatrixXd orientation_samples = MatrixXd::Zero(4, N_); + orientation_samples = + c3_solution->x_sol_.middleRows(n_q_ - 7, 4).cast(); + object_orientation_traj.traj_name = "object_orientation_target"; + object_orientation_traj.datatypes = + std::vector(orientation_samples.rows(), "double"); + object_orientation_traj.datapoints = orientation_samples; + object_orientation_traj.time_vector = + c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(object_orientation_traj.traj_name, + object_orientation_traj); + + output_traj->saved_traj = lcm_traj.GenerateLcmObject(); + output_traj->utime = context.get_time() * 1e6; +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/c3_trajectory_generator.h b/examples/franka/systems/c3_trajectory_generator.h new file mode 100644 index 0000000000..5ae42e5b8a --- /dev/null +++ b/examples/franka/systems/c3_trajectory_generator.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include + +#include + +#include "common/find_resource.h" +#include "dairlib/lcmt_saved_traj.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" +#include "solvers/c3_options.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Outputs a lcmt_timestamped_saved_traj +class C3TrajectoryGenerator : public drake::systems::LeafSystem { + public: + explicit C3TrajectoryGenerator( + const drake::multibody::MultibodyPlant& plant, + C3Options c3_options); + + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_port_); + } + + const drake::systems::OutputPort& get_output_port_actor_trajectory() + const { + return this->get_output_port(actor_trajectory_port_); + } + const drake::systems::OutputPort& get_output_port_object_trajectory() + const { + return this->get_output_port(object_trajectory_port_); + } + + void SetPublishEndEffectorOrientation(bool publish_end_effector_orientation) { + publish_end_effector_orientation_ = publish_end_effector_orientation; + } + + private: + void OutputActorTrajectory( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_traj) const; + + void OutputObjectTrajectory( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_traj) const; + + drake::systems::InputPortIndex c3_solution_port_; + drake::systems::OutputPortIndex actor_trajectory_port_; + drake::systems::OutputPortIndex object_trajectory_port_; + + const drake::multibody::MultibodyPlant& plant_; + C3Options c3_options_; + + bool publish_end_effector_orientation_ = false; + + // convenience for variable sizes + int n_q_; + int n_v_; + int n_x_; + int n_lambda_; + int n_u_; + + int N_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/examples/franka/systems/external_force_generator.cc b/examples/franka/systems/external_force_generator.cc new file mode 100644 index 0000000000..f3168085e6 --- /dev/null +++ b/examples/franka/systems/external_force_generator.cc @@ -0,0 +1,62 @@ +#include "external_force_generator.h" + +using Eigen::Vector3d; +using Eigen::VectorXd; + +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::EventStatus; + +namespace dairlib { + +ExternalForceGenerator::ExternalForceGenerator( + drake::multibody::BodyIndex body_index) + : body_index_(body_index) { + // Input/Output Setup + radio_port_ = + this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) + .get_index(); + + spatial_force_port_ = + this->DeclareAbstractOutputPort( + "object_spatial_force", + std::vector< + drake::multibody::ExternallyAppliedSpatialForce>(), + &ExternalForceGenerator::CalcSpatialForce) + .get_index(); +} + +void ExternalForceGenerator::SetRemoteControlParameters(double x_scale, + double y_scale, + double z_scale) { + x_scale_ = x_scale; + y_scale_ = y_scale; + z_scale_ = z_scale; +} + +void ExternalForceGenerator::CalcSpatialForce( + const drake::systems::Context& context, + std::vector>* + spatial_forces) const { + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + Vector3d force = VectorXd::Zero(3); + if (radio_out->value()[12]) { + force(0) = radio_out->value()[0] * x_scale_; + force(1) = radio_out->value()[1] * y_scale_; + force(2) = radio_out->value()[2] * z_scale_; + } + if (spatial_forces->empty()) { + auto spatial_force = + drake::multibody::ExternallyAppliedSpatialForce(); + spatial_force.body_index = body_index_; + spatial_force.F_Bq_W = + drake::multibody::SpatialForce(Vector3d::Zero(), force); + spatial_forces->push_back(spatial_force); + } else { + spatial_forces->at(0).F_Bq_W = + drake::multibody::SpatialForce(Vector3d::Zero(), force); + spatial_forces->at(0).body_index = body_index_; + } +} + +} // namespace dairlib diff --git a/examples/franka/systems/external_force_generator.h b/examples/franka/systems/external_force_generator.h new file mode 100644 index 0000000000..a94f023f4b --- /dev/null +++ b/examples/franka/systems/external_force_generator.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class ExternalForceGenerator : public drake::systems::LeafSystem { + public: + ExternalForceGenerator(drake::multibody::BodyIndex body_index); + + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + const drake::systems::OutputPort& get_output_port_spatial_force() + const { + return this->get_output_port(spatial_force_port_); + } + + void SetRemoteControlParameters(double x_scale, double y_scale, + double z_scale); + + private: + void CalcSpatialForce(const drake::systems::Context& context, + std::vector>* spatial_forces) const; + + drake::systems::InputPortIndex radio_port_; + drake::systems::OutputPortIndex spatial_force_port_; + drake::multibody::BodyIndex body_index_; + double x_scale_ = 0; + double y_scale_ = 0; + double z_scale_ = 0; +}; + +} // namespace dairlib diff --git a/examples/franka/systems/plate_balancing_target.cc b/examples/franka/systems/plate_balancing_target.cc new file mode 100644 index 0000000000..a503d18af1 --- /dev/null +++ b/examples/franka/systems/plate_balancing_target.cc @@ -0,0 +1,199 @@ +#include "plate_balancing_target.h" + +#include +#include + +#include "dairlib/lcmt_radio_out.hpp" + +using dairlib::systems::StateVector; +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::EventStatus; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + +PlateBalancingTargetGenerator::PlateBalancingTargetGenerator( + const MultibodyPlant& object_plant, double end_effector_thickness, + double target_threshold) + : end_effector_thickness_(end_effector_thickness), + target_threshold_(target_threshold) { + // Input/Output Setup + radio_port_ = + this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) + .get_index(); + tray_state_port_ = + this->DeclareVectorInputPort( + "x_object", StateVector(object_plant.num_positions(), + object_plant.num_velocities())) + .get_index(); + end_effector_target_port_ = + this->DeclareVectorOutputPort( + "end_effector_target", BasicVector(3), + &PlateBalancingTargetGenerator::CalcEndEffectorTarget) + .get_index(); + tray_target_port_ = this->DeclareVectorOutputPort( + "tray_target", BasicVector(7), + &PlateBalancingTargetGenerator::CalcTrayTarget) + .get_index(); + tray_velocity_target_port_ = this->DeclareVectorOutputPort( + "tray_velocity_target", BasicVector(6), + &PlateBalancingTargetGenerator::CalcTrayVelocityTarget) + .get_index(); + sequence_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); + within_target_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); + time_entered_target_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); + DeclareForcedDiscreteUpdateEvent( + &PlateBalancingTargetGenerator::DiscreteVariableUpdate); +} + +EventStatus PlateBalancingTargetGenerator::DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const { + const StateVector* tray_state = + (StateVector*)this->EvalVectorInput(context, tray_state_port_); + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + + // Ugly FSM + int current_sequence = context.get_discrete_state(sequence_index_)[0]; + int within_target = context.get_discrete_state(within_target_index_)[0]; + int time_entered_target = + context.get_discrete_state(time_entered_target_index_)[0]; + if (current_sequence == 0) { + if ((tray_state->GetPositions().tail(3) - first_target_).norm() < + target_threshold_) { + if (within_target == + 0) { // set the time of when the tray first hits the target + discrete_state->get_mutable_value(time_entered_target_index_)[0] = + context.get_time(); + } + discrete_state->get_mutable_value(within_target_index_)[0] = 1; + } + if (within_target == 1 && + (context.get_time() - time_entered_target) > 0.5) { + discrete_state->get_mutable_value(within_target_index_)[0] = 0; + discrete_state->get_mutable_value(sequence_index_)[0] = 1; + } + } else if (current_sequence == 1) { + if ((tray_state->GetPositions().tail(3) - second_target_).norm() < + target_threshold_) { + if (within_target == + 0) { // set the time of when the tray first hits the target + discrete_state->get_mutable_value(time_entered_target_index_)[0] = + context.get_time(); + } + discrete_state->get_mutable_value(within_target_index_)[0] = 1; + } + if (within_target == 1 && + (context.get_time() - time_entered_target) > delay_at_top_) { + discrete_state->get_mutable_value(within_target_index_)[0] = 0; + discrete_state->get_mutable_value(sequence_index_)[0] = 2; + } + } else if (current_sequence == 2) { + if ((tray_state->GetPositions().tail(3) - third_target_).norm() < + target_threshold_) { + discrete_state->get_mutable_value(sequence_index_)[0] = 3; + } + } + if (current_sequence == 3 && radio_out->value()[15] < 0) { + discrete_state->get_mutable_value(sequence_index_)[0] = 0; + } + return EventStatus::Succeeded(); +} + +void PlateBalancingTargetGenerator::SetRemoteControlParameters( + const Eigen::Vector3d& first_target, const Eigen::Vector3d& second_target, + const Eigen::Vector3d& third_target, double x_scale, double y_scale, + double z_scale) { + first_target_ = first_target; + second_target_ = second_target; + third_target_ = third_target; + x_scale_ = x_scale; + y_scale_ = y_scale; + z_scale_ = z_scale; +} + +void PlateBalancingTargetGenerator::CalcEndEffectorTarget( + const drake::systems::Context& context, + drake::systems::BasicVector* target) const { + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + + VectorXd end_effector_position = first_target_; + // Update target if remote trigger is active + if (context.get_discrete_state(sequence_index_)[0] == 1) { + end_effector_position = second_target_; // raise the tray once it is close + } else if (context.get_discrete_state(sequence_index_)[0] == 2 || + context.get_discrete_state(sequence_index_)[0] == 3) { + end_effector_position = third_target_; // put the tray back + } + end_effector_position[2] -= + end_effector_thickness_; // place end effector below tray + if (radio_out->value()[13] > 0) { + end_effector_position(0) += radio_out->value()[0] * x_scale_; + end_effector_position(1) += radio_out->value()[1] * y_scale_; + end_effector_position(2) += radio_out->value()[2] * z_scale_; + } + if (end_effector_position[0] > 0.6) { + end_effector_position[0] = 0.6; // keep it within the workspace + } + // end_effector_position(0) = 0.55; + // end_effector_position(1) = 0.1 * sin(4 * context.get_time()); + // end_effector_position(2) = 0.45 + 0.1 * cos(2 *context.get_time()) - + // end_effector_thickness_; end_effector_position(1) = 0.1 * (int) (2 * + // sin(0.5 * context.get_time())); end_effector_position(2) = 0.45 - + // end_effector_thickness_; + target->SetFromVector(end_effector_position); +} + +void PlateBalancingTargetGenerator::CalcTrayTarget( + const drake::systems::Context& context, + BasicVector* target) const { + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + VectorXd target_tray_state = VectorXd::Zero(7); + VectorXd tray_position = first_target_; + + if (context.get_discrete_state(sequence_index_)[0] == 1) { + tray_position = second_target_; + } else if (context.get_discrete_state(sequence_index_)[0] == 2 || + context.get_discrete_state(sequence_index_)[0] == 3) { + tray_position = third_target_; + } + if (radio_out->value()[13] > 0) { + tray_position(0) += radio_out->value()[0] * x_scale_; + tray_position(1) += radio_out->value()[1] * y_scale_; + tray_position(2) += radio_out->value()[2] * z_scale_; + } + // tray_position(0) = 0.55; + // tray_position(1) = 0.1 * sin(4 * context.get_time()); + // tray_position(2) = 0.45 + 0.1 * cos(2 *context.get_time()); + // tray_position(1) = 0.1 * (int) (2 * sin(0.5 * context.get_time())); + // tray_position(2) = 0.45; + target_tray_state << 1, 0, 0, 0, tray_position; // tray orientation is flat + target->SetFromVector(target_tray_state); +} + +void PlateBalancingTargetGenerator::CalcTrayVelocityTarget( + const drake::systems::Context& context, + BasicVector* target) const { + const StateVector* tray_state = + (StateVector*)this->EvalVectorInput(context, tray_state_port_); + Eigen::Quaterniond y_quat_des(1, 0, 0, 0); + const VectorX &q = tray_state->GetPositions(); + Eigen::Quaterniond y_quat(q(0), q(1), q(2), q(3)); + Eigen::AngleAxis angle_axis_diff(y_quat_des * y_quat.inverse()); + VectorXd angle_error = angle_axis_diff.angle() * angle_axis_diff.axis(); + + VectorXd target_tray_state = VectorXd::Zero(6); + + // tray_position(0) = 0.55; + // tray_position(1) = 0.1 * sin(4 * context.get_time()); + // tray_position(2) = 0.45 + 0.1 * cos(2 *context.get_time()); + // tray_position(1) = 0.1 * (int) (2 * sin(0.5 * context.get_time())); + // tray_position(2) = 0.45; + target_tray_state << angle_error, VectorXd::Zero(3); // tray orientation is flat + target->SetFromVector(target_tray_state); +} + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/examples/franka/systems/plate_balancing_target.h b/examples/franka/systems/plate_balancing_target.h new file mode 100644 index 0000000000..be73d052b1 --- /dev/null +++ b/examples/franka/systems/plate_balancing_target.h @@ -0,0 +1,81 @@ +#pragma once + +#include + +#include "systems/framework/state_vector.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +class PlateBalancingTargetGenerator + : public drake::systems::LeafSystem { + public: + PlateBalancingTargetGenerator( + const drake::multibody::MultibodyPlant& object_plant, + double end_effector_thickness, double target_threshold = 0.075); + + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + const drake::systems::InputPort& get_input_port_tray_state() const { + return this->get_input_port(tray_state_port_); + } + + const drake::systems::OutputPort& + get_output_port_end_effector_target() const { + return this->get_output_port(end_effector_target_port_); + } + + const drake::systems::OutputPort& get_output_port_tray_target() + const { + return this->get_output_port(tray_target_port_); + } + + const drake::systems::OutputPort& get_output_port_tray_velocity_target() + const { + return this->get_output_port(tray_velocity_target_port_); + } + + void SetRemoteControlParameters(const Eigen::Vector3d& first_target, + const Eigen::Vector3d& second_target, + const Eigen::Vector3d& third_target, + double x_scale, double y_scale, + double z_scale); + + private: + void CalcEndEffectorTarget(const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + void CalcTrayTarget(const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + void CalcTrayVelocityTarget(const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex radio_port_; + drake::systems::InputPortIndex tray_state_port_; + drake::systems::OutputPortIndex end_effector_target_port_; + drake::systems::OutputPortIndex tray_target_port_; + drake::systems::OutputPortIndex tray_velocity_target_port_; + + drake::systems::DiscreteStateIndex sequence_index_; + drake::systems::DiscreteStateIndex within_target_index_; + drake::systems::DiscreteStateIndex time_entered_target_index_; + double end_effector_thickness_; + Eigen::Vector3d first_target_; + Eigen::Vector3d second_target_; + Eigen::Vector3d third_target_; +// const double delay_at_top_ = 10.0; + const double delay_at_top_ = 3.0; + double target_threshold_; + double x_scale_; + double y_scale_; + double z_scale_; +}; + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/examples/franka/urdf/center_support.urdf b/examples/franka/urdf/center_support.urdf new file mode 100644 index 0000000000..90ace69be8 --- /dev/null +++ b/examples/franka/urdf/center_support.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + cod + + + + + + + + + + + cod + + + + + + + diff --git a/examples/franka/urdf/cylinder_lcs.urdf b/examples/franka/urdf/cylinder_lcs.urdf new file mode 100644 index 0000000000..31a4718b6c --- /dev/null +++ b/examples/franka/urdf/cylinder_lcs.urdf @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/cylinder_object.urdf b/examples/franka/urdf/cylinder_object.urdf new file mode 100644 index 0000000000..5832f2b11b --- /dev/null +++ b/examples/franka/urdf/cylinder_object.urdf @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector.urdf b/examples/franka/urdf/plate_end_effector.urdf new file mode 100644 index 0000000000..a0efe0cef0 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector_floating.urdf b/examples/franka/urdf/plate_end_effector_floating.urdf new file mode 100644 index 0000000000..efaa4bb17f --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_floating.urdf @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + 1 + + + + + 1 + + + + + 1 + + + + + 1 + + + + + 1 + + diff --git a/examples/franka/urdf/plate_end_effector_massless.urdf b/examples/franka/urdf/plate_end_effector_massless.urdf new file mode 100644 index 0000000000..b81d96de63 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_massless.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector_parameter_sweep.urdf b/examples/franka/urdf/plate_end_effector_parameter_sweep.urdf new file mode 100644 index 0000000000..18c5c1a817 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_parameter_sweep.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/plate_end_effector_translation.urdf b/examples/franka/urdf/plate_end_effector_translation.urdf new file mode 100644 index 0000000000..bb2ca2f4f8 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_translation.urdf @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + 1 + + + + + 1 + + diff --git a/examples/franka/urdf/plate_end_effector_translation_he.urdf b/examples/franka/urdf/plate_end_effector_translation_he.urdf new file mode 100644 index 0000000000..01d1b5a3d9 --- /dev/null +++ b/examples/franka/urdf/plate_end_effector_translation_he.urdf @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + 1 + + + + + 1 + + diff --git a/examples/franka/urdf/side_wall.urdf b/examples/franka/urdf/side_wall.urdf new file mode 100644 index 0000000000..4cb74faec7 --- /dev/null +++ b/examples/franka/urdf/side_wall.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + cod + + + + + + + + + + + cod + + + + + + + diff --git a/examples/franka/urdf/support.urdf b/examples/franka/urdf/support.urdf new file mode 100644 index 0000000000..0bfd702a4f --- /dev/null +++ b/examples/franka/urdf/support.urdf @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/support_point_contact.urdf b/examples/franka/urdf/support_point_contact.urdf new file mode 100644 index 0000000000..3e06df6c64 --- /dev/null +++ b/examples/franka/urdf/support_point_contact.urdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/franka/urdf/tray.sdf b/examples/franka/urdf/tray.sdf new file mode 100644 index 0000000000..b774f23a7b --- /dev/null +++ b/examples/franka/urdf/tray.sdf @@ -0,0 +1,49 @@ + + + + + + 0 0 0 0 0 0 + 1 + + + 0.013105 + 0 + 0 + 0.013105 + 0 + 0.026129 + + + + 0 0 0.0 0 0 0 + + + 0.2286 + 0.022 + + + + 0.1 0.1 0.1 0.6 + + + + 0 0 0.0 0 0 0 + + + 0.2286 + 0.022 + + + + + 3.0e7 + 0.15 + 10 + 0.4 + + + + + \ No newline at end of file diff --git a/examples/franka/urdf/tray_lcs.sdf b/examples/franka/urdf/tray_lcs.sdf new file mode 100644 index 0000000000..0f5862fe11 --- /dev/null +++ b/examples/franka/urdf/tray_lcs.sdf @@ -0,0 +1,43 @@ + + + + + + 0 0 0 0 0 0 + 1 + + + 0.013105 + 0 + 0 + 0.013105 + 0 + 0.026129 + + + + 0 0 0.0 0 0 0 + + + 0.2286 + 0.022 + + + + 0.1 0.1 0.1 0.8 + + + + 0 0 0.0 0 0 0 + + + 0.2286 + 0.022 + + + 0.15 0.3 0 0 0 0 + + + + \ No newline at end of file diff --git a/examples/franka/urdf/tray_transparent.sdf b/examples/franka/urdf/tray_transparent.sdf new file mode 100644 index 0000000000..c0947bf6fb --- /dev/null +++ b/examples/franka/urdf/tray_transparent.sdf @@ -0,0 +1,49 @@ + + + + + + 0 0 0 0 0 0 + 1 + + + 0.013105 + 0 + 0 + 0.013105 + 0 + 0.026129 + + + + 0 0 0.0 0 0 0 + + + 0.2276 + 0.022 + + + + 0.1 0.1 0.1 0.9 + + + + 0 0 0.0 0 0 0 + + + 0.2286 + 0.022 + + + + + 3.0e7 + 0.18 + 10 + 0.4 + + + + + \ No newline at end of file diff --git a/examples/impact_invariant_control/five_link_biped.urdf b/examples/impact_invariant_control/five_link_biped.urdf index e3260cc997..2ec29d6a6d 100644 --- a/examples/impact_invariant_control/five_link_biped.urdf +++ b/examples/impact_invariant_control/five_link_biped.urdf @@ -2,7 +2,7 @@ - + @@ -10,13 +10,17 @@ - + + + + + @@ -29,7 +33,7 @@ - + @@ -38,7 +42,7 @@ - + diff --git a/examples/impact_invariant_control/five_link_biped_sim.cc b/examples/impact_invariant_control/five_link_biped_sim.cc index e0ad093cd3..722312dff1 100644 --- a/examples/impact_invariant_control/five_link_biped_sim.cc +++ b/examples/impact_invariant_control/five_link_biped_sim.cc @@ -80,7 +80,7 @@ int do_main(int argc, char* argv[]) { Parser parser(&plant, &scene_graph); std::string full_name = FindResourceOrThrow( "examples/impact_invariant_control/five_link_biped.urdf"); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), drake::math::RigidTransform()); multibody::AddFlatTerrain(&plant, &scene_graph, .8, .8); // Add ground @@ -101,6 +101,7 @@ int do_main(int argc, char* argv[]) { builder.AddSystem(LcmSubscriberSystem::Make( FLAGS_channel_u, lcm)); auto input_receiver = builder.AddSystem(plant); + // connect input receiver auto passthrough = builder.AddSystem( input_receiver->get_output_port(0).size(), 0, @@ -116,7 +117,7 @@ int do_main(int argc, char* argv[]) { LcmPublisherSystem::Make( "CONTACT_RESULTS", lcm, 1.0 / FLAGS_publish_rate)); contact_results_publisher.set_name("contact_results_publisher"); - auto state_sender = builder.AddSystem(plant); + auto state_sender = builder.AddSystem(plant, true); // Contact results to lcm msg. builder.Connect(*input_sub, *input_receiver); @@ -129,10 +130,12 @@ int do_main(int argc, char* argv[]) { contact_results_publisher.get_input_port()); builder.Connect(plant.get_state_output_port(), state_sender->get_input_port_state()); + builder.Connect(passthrough->get_output_port(), + state_sender->get_input_port_effort()); builder.Connect(state_sender->get_output_port(0), state_pub->get_input_port()); builder.Connect( - plant.get_geometry_poses_output_port(), + plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.get_source_id().value())); builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()); diff --git a/examples/impact_invariant_control/five_link_biped_visualizer.cc b/examples/impact_invariant_control/five_link_biped_visualizer.cc index 805f265706..29f425fe27 100644 --- a/examples/impact_invariant_control/five_link_biped_visualizer.cc +++ b/examples/impact_invariant_control/five_link_biped_visualizer.cc @@ -14,6 +14,8 @@ #include "drake/systems/lcm/lcm_interface_system.h" #include "drake/systems/lcm/lcm_subscriber_system.h" #include "drake/systems/rendering/multibody_position_to_geometry_pose.h" +#include "drake/geometry/meshcat_visualizer.h" +#include "drake/geometry/meshcat_visualizer_params.h" namespace dairlib { @@ -53,7 +55,7 @@ int do_main(int argc, char* argv[]) { SceneGraph* scene_graph = builder.AddSystem(); scene_graph->set_name("scene_graph"); Parser parser(&plant, scene_graph); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), drake::math::RigidTransform()); plant.mutable_gravity_field().set_gravity_vector(-9.81 * @@ -118,6 +120,21 @@ int do_main(int argc, char* argv[]) { ball_to_pose->get_output_port(), scene_graph->get_source_pose_port(ball_plant->get_source_id().value())); } + drake::geometry::MeshcatVisualizerParams params; + params.publish_period = 1.0 / 60.0; + auto meshcat = std::make_shared(); + auto visualizer = &drake::geometry::MeshcatVisualizer::AddToBuilder( + &builder, *scene_graph, meshcat, std::move(params)); + + auto ortho_camera = drake::geometry::Meshcat::OrthographicCamera(); + ortho_camera.top = 2; + ortho_camera.bottom = -0.1; + ortho_camera.left = -1; + ortho_camera.right = 4; + ortho_camera.near = 0; + ortho_camera.far = 500; + ortho_camera.zoom = 1; + meshcat->SetCamera(ortho_camera); DrakeVisualizer::AddToBuilder(&builder, *scene_graph); diff --git a/examples/impact_invariant_control/impact_aware_time_based_fsm.h b/examples/impact_invariant_control/impact_aware_time_based_fsm.h index 579bfd6a97..301626d015 100644 --- a/examples/impact_invariant_control/impact_aware_time_based_fsm.h +++ b/examples/impact_invariant_control/impact_aware_time_based_fsm.h @@ -2,7 +2,7 @@ #include -#include "common/blending_utils.h" +#include "common/math_utils.h" #include "systems/controllers/time_based_fsm.h" #include "systems/framework/impact_info_vector.h" #include "systems/framework/output_vector.h" diff --git a/examples/impact_invariant_control/impact_invariant_examples.pmd b/examples/impact_invariant_control/impact_invariant_examples.pmd index 3c2036041a..f35fc12e7c 100644 --- a/examples/impact_invariant_control/impact_invariant_examples.pmd +++ b/examples/impact_invariant_control/impact_invariant_examples.pmd @@ -19,7 +19,7 @@ group "5.trajectory-optimization" { host = "localhost"; } cmd "visualize_jumping_trajectory" { - exec = "bazel-bin/examples/Cassie/visualize_trajectory --folder_path=\"examples/Cassie/saved_trajectories/\" --realtime_rate=0.1 --num_poses=20 --visualize_mode=2 --use_transparency=1 --trajectory_name=\"jumping_0.15h_0.3d\""; + exec = "bazel-bin/examples/Cassie/visualize_trajectory --folder_path=\"examples/Cassie/saved_trajectories/\" --realtime_rate=0.5 --num_poses=20 --visualize_mode=1 --use_transparency=1 --trajectory_name=\"jumping_0.15h_0.3d\""; host = "localhost"; } cmd "convert_traj_for_controller" { @@ -46,7 +46,7 @@ group "1.simulated-robot" { host = "localhost"; } cmd "five_link_biped_sim" { - exec = "bazel-bin/examples/impact_invariant_control/five_link_biped_sim --folder_path=\"/home/yangwill/Documents/research/projects/five_link_biped/walking/saved_trajs/\" --trajectory_name=\"rabbit_walking\" --sim_time=1.0 --target_realtime_rate=0.25"; + exec = "bazel-bin/examples/impact_invariant_control/five_link_biped_sim --folder_path=\"examples/impact_invariant_control/saved_trajectories/\" --trajectory_name=\"rabbit_walking\" --sim_time=1.0 --target_realtime_rate=0.25 --error=0.1"; host = "localhost"; } } @@ -70,55 +70,3 @@ group "2.controllers" { } -script "osc-jumping (drake)" { - stop cmd "osc_jumping_controller"; - stop cmd "mbp_sim" wait "stopped"; - start cmd "osc_jumping_controller" wait "running"; - start cmd "mbp_sim"; -} - -script "osc-jumping (mujoco)" { - stop cmd "osc_jumping_controller (mujoco)"; - stop cmd "dispatcher-robot-in"; - stop cmd "cassie-mujoco" wait "stopped"; - start cmd "osc_jumping_controller (mujoco)" wait "running"; - start cmd "dispatcher-robot-in"; - start cmd "cassie-mujoco"; -} - -script "osc_standing (mujoco)" { - start cmd "cassie-mujoco"; - start cmd "dispatcher-robot-in"; - start cmd "osc_standing_controller"; -} - -script "run-mujoco-lcm-pd-control" { - run_script "start-operator-MBP"; - start cmd "3.cassie-mujoco-fixed-base"; - start cmd "2.a.dispatcher-robot-out (lcm)"; - start cmd "3.dispatcher-robot-in"; - start cmd "0.pd-controller"; -} - -script "run-real-robot-pd-control" { - run_script "start-operator-real-robot"; - start cmd "0.dispatcher-robot-out-real-robot"; - start cmd "1.dispatcher-robot-in-real-robot"; - start cmd "2.pd-controller-real-robot"; -} - -script "run_gazebo" { - start cmd "2.b.dispatcher-robot-out (gazebo)"; - wait ms 500; - start cmd "0.launch-gazebo"; -} - -script "switch-to-standing" { - start cmd "osc_standing_controller"; - stop cmd "osc_walking_controller"; -} - -script "switch-to-walking" { - start cmd "osc_walking_controller"; - stop cmd "osc_standing_controller"; -} diff --git a/examples/impact_invariant_control/joint_space_walking_gains.yaml b/examples/impact_invariant_control/joint_space_walking_gains.yaml index 2bff46d1f6..eb2fe28632 100644 --- a/examples/impact_invariant_control/joint_space_walking_gains.yaml +++ b/examples/impact_invariant_control/joint_space_walking_gains.yaml @@ -2,7 +2,7 @@ w_input: 0 w_accel: 0.0 w_soft_constraint: 1000000 -impact_threshold: 0.025 +impact_threshold: 0.000 mu: 0.4 diff --git a/examples/impact_invariant_control/platform.urdf b/examples/impact_invariant_control/platform.urdf index 9930a690af..dc71d2af32 100644 --- a/examples/impact_invariant_control/platform.urdf +++ b/examples/impact_invariant_control/platform.urdf @@ -5,18 +5,18 @@ - + - - + + - + - + - + diff --git a/examples/impact_invariant_control/run_dircon_walking.cc b/examples/impact_invariant_control/run_dircon_walking.cc index b8b752ac8f..63f5016183 100644 --- a/examples/impact_invariant_control/run_dircon_walking.cc +++ b/examples/impact_invariant_control/run_dircon_walking.cc @@ -39,14 +39,13 @@ DEFINE_double(mu_kinetic, 0.7, "The dynamic coefficient of friction"); DEFINE_double(v_tol, 0.01, "The maximum slipping speed allowed during stiction (m/s)"); DEFINE_double(height, 0.75, "The jump height wrt to the torso COM (m)"); -DEFINE_int64(knot_points, 10, "Number of knot points per mode"); +DEFINE_int32(knot_points, 10, "Number of knot points per mode"); DEFINE_double(max_duration, 1.0, "Maximum trajectory duration (s)"); // Simulation parameters. -DEFINE_double(timestep, 1e-5, "The simulator time step (s)"); DEFINE_string(data_directory, - "examples/impact_invariant_control/saved_trajectories", + "examples/impact_invariant_control/saved_trajectories/", "Directory path to save decision vars to."); DEFINE_string(save_filename, "rabbit_walking", "Filename to save decision " @@ -92,14 +91,14 @@ drake::trajectories::PiecewisePolynomial run_traj_opt( const Body& left_lower_leg = plant->GetBodyByName("left_lower_leg"); const Body& right_lower_leg = plant->GetBodyByName("right_lower_leg"); - Vector3d pt; - pt << 0, 0, -0.4; + Vector3d foot_on_leg; + foot_on_leg << 0, 0, -0.4; bool isXZ = true; auto leftFootConstraint = - DirconPositionData(*plant, left_lower_leg, pt, isXZ); + DirconPositionData(*plant, left_lower_leg, foot_on_leg, isXZ); auto rightFootConstraint = - DirconPositionData(*plant, right_lower_leg, pt, isXZ); + DirconPositionData(*plant, right_lower_leg, foot_on_leg, isXZ); // Specifies that the foot has to be on the // ground with normal/friction specified by normal/mu @@ -244,6 +243,11 @@ drake::trajectories::PiecewisePolynomial run_traj_opt( trajopt->AddRunningCost(u.transpose() * R * u); trajopt->AddRunningCost(x.transpose() * Q * x); + double alpha = .2; + int num_poses = std::max((int)(timesteps.size() + 1), FLAGS_knot_points); + trajopt->CreateVisualizationCallback(FindResourceOrThrow( + "examples/impact_invariant_control/five_link_biped.urdf"), num_poses, alpha); + // Solve the traj optimization problem auto start = std::chrono::high_resolution_clock::now(); const auto result = Solve(prog, prog.initial_guess()); @@ -274,7 +278,7 @@ int doMain(int argc, char* argv[]) { Parser parser(&plant, &scene_graph); std::string full_name = FindResourceOrThrow( "examples/impact_invariant_control/five_link_biped.urdf"); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.mutable_gravity_field().set_gravity_vector(-FLAGS_gravity * Eigen::Vector3d::UnitZ()); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), @@ -282,13 +286,12 @@ int doMain(int argc, char* argv[]) { plant.Finalize(); - // print_state_map(&plant); - // Generate guesses for states, inputs, and contact forces std::srand(time(0)); // Initialize random number generator. int nx = plant.num_positions() + plant.num_velocities(); // Eigen::VectorXd x_0(nx); - Eigen::VectorXd x_0 = Eigen::VectorXd::Zero(nx); + Eigen::VectorXd x_0 = Eigen::VectorXd::Random(nx); +// x_0(0) = 1; Eigen::VectorXd init_l_vec(2); init_l_vec << 0, 15 * FLAGS_gravity; @@ -317,9 +320,9 @@ int doMain(int argc, char* argv[]) { } auto init_x_traj = - PiecewisePolynomial::ZeroOrderHold(init_time, init_x); + PiecewisePolynomial::FirstOrderHold(init_time, init_x); auto init_u_traj = - PiecewisePolynomial::ZeroOrderHold(init_time, init_u); + PiecewisePolynomial::FirstOrderHold(init_time, init_u); for (int j = 0; j < n_modes; ++j) { std::vector init_l_j; @@ -349,8 +352,6 @@ int doMain(int argc, char* argv[]) { drake::trajectories::PiecewisePolynomial optimal_traj = run_traj_opt(&plant, init_x_traj, init_u_traj, init_l_traj, init_lc_traj, init_vc_traj); - multibody::ConnectTrajectoryVisualizer(&plant, &builder, &scene_graph, - optimal_traj); return 0; } diff --git a/examples/impact_invariant_control/run_joint_space_walking_controller.cc b/examples/impact_invariant_control/run_joint_space_walking_controller.cc index 9832f2e2f5..763ec4845b 100644 --- a/examples/impact_invariant_control/run_joint_space_walking_controller.cc +++ b/examples/impact_invariant_control/run_joint_space_walking_controller.cc @@ -35,6 +35,7 @@ using drake::geometry::SceneGraph; using drake::multibody::Frame; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; +using drake::systems::Diagram; using drake::systems::DiagramBuilder; using drake::systems::TriggerType; using drake::systems::TriggerTypeSet; @@ -75,7 +76,7 @@ int DoMain(int argc, char* argv[]) { Parser parser(&plant, &scene_graph); std::string full_name = FindResourceOrThrow( "examples/impact_invariant_control/five_link_biped.urdf"); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), drake::math::RigidTransform()); plant.Finalize(); @@ -122,7 +123,7 @@ int DoMain(int argc, char* argv[]) { FLAGS_channel_u, &lcm, TriggerTypeSet({TriggerType::kForced}))); auto command_sender = builder.AddSystem(plant); auto osc = builder.AddSystem( - plant, plant, plant_context.get(), plant_context.get(), true); + plant, plant_context.get(), true); auto osc_debug_pub = builder.AddSystem(LcmPublisherSystem::Make( "OSC_DEBUG_WALKING", &lcm, TriggerTypeSet({TriggerType::kForced}))); @@ -146,8 +147,15 @@ int DoMain(int argc, char* argv[]) { plant, foot_contact_disp, plant.GetBodyByName("right_foot").body_frame(), Matrix3d::Identity(), Vector3d::Zero(), {0, 2}); - osc->AddStateAndContactPoint(0, &left_foot_evaluator); - osc->AddStateAndContactPoint(1, &right_foot_evaluator); + osc->AddContactPoint( + "left_foot", + std::unique_ptr>(&left_foot_evaluator), + {0}); + osc->AddContactPoint( + "right_foot", + std::unique_ptr>(&right_foot_evaluator), + {1}); + // Create maps for joints map pos_map_wo_spr = multibody::MakeNameToPositionsMap(plant); @@ -206,11 +214,12 @@ int DoMain(int argc, char* argv[]) { // Run lcm-driven simulation // Create the diagram auto owned_diagram = builder.Build(); - owned_diagram->set_name(("id_walking_controller")); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("id_walking_controller")); // Run lcm-driven simulation systems::LcmDrivenLoop loop( - &lcm, std::move(owned_diagram), state_receiver, FLAGS_channel_x, true); + &lcm, shared_diagram, state_receiver, FLAGS_channel_x, true); loop.Simulate(); return 0; diff --git a/examples/impact_invariant_control/saved_trajectories/rabbit_walking b/examples/impact_invariant_control/saved_trajectories/rabbit_walking index f9d9238e8f..889c10f9b4 100644 Binary files a/examples/impact_invariant_control/saved_trajectories/rabbit_walking and b/examples/impact_invariant_control/saved_trajectories/rabbit_walking differ diff --git a/examples/kuka_iiwa_arm/BUILD.bazel b/examples/kuka_iiwa_arm/BUILD.bazel deleted file mode 100644 index ace8be3a78..0000000000 --- a/examples/kuka_iiwa_arm/BUILD.bazel +++ /dev/null @@ -1,75 +0,0 @@ -load("@drake//tools/lint:lint.bzl", "add_lint_tests") - -package(default_visibility = ["//visibility:public"]) - -load( - "@drake//tools/skylark:drake_lcm.bzl", - "drake_lcm_cc_library", - "drake_lcm_java_library", - "drake_lcm_py_library", -) -load( - "@drake//tools/skylark:drake_py.bzl", - "drake_py_binary", - "drake_py_library", - "drake_py_unittest", -) - -cc_binary( - name = "kuka_simulation", - srcs = ["kuka_simulation.cc"], - data = [ - "@drake//manipulation/models/iiwa_description:models", - ], - deps = [ - ":kuka_torque_controller", - "//common", - "@drake//:drake_shared_library", - ], -) - -cc_binary( - name = "iiwa_oscillate", - srcs = ["iiwa_oscillate.cc"], - deps = [ - "//systems/controllers:endeffector_position_controller", - "//systems/controllers:endeffector_velocity_controller", - "//systems/controllers:safe_velocity_controller", - "@drake//:drake_shared_library", - ], -) - -cc_binary( - name = "iiwa_visualizer", - srcs = ["iiwa_visualizer.cc"], - data = [ - "@drake//examples/kuka_iiwa_arm:models", - "@drake//manipulation/models/iiwa_description:models", - ], - deprecation = "Attic/RigidBodyTree is deprecated.", - tags = ["manual"], - deps = [ - "//common", - "@drake//:drake_shared_library", - ], -) - -cc_library( - name = "kuka_torque_controller", - srcs = ["kuka_torque_controller.cc"], - hdrs = ["kuka_torque_controller.h"], - visibility = ["//visibility:public"], - deps = [ - "@drake//:drake_shared_library", - ], -) - -cc_binary( - name = "iiwa_controller_demo", - srcs = ["iiwa_controller_demo.cc"], - deps = [ - "//systems/controllers:endeffector_position_controller", - "//systems/controllers:endeffector_velocity_controller", - "@drake//:drake_shared_library", - ], -) diff --git a/examples/kuka_iiwa_arm/iiwa_controller_demo.cc b/examples/kuka_iiwa_arm/iiwa_controller_demo.cc deleted file mode 100644 index d9253b016a..0000000000 --- a/examples/kuka_iiwa_arm/iiwa_controller_demo.cc +++ /dev/null @@ -1,205 +0,0 @@ -// Kp and 'Rotational' Kp -#define K_P 1 -#define K_OMEGA 0.7 - -// Kd and 'Rotational' Kd -#define K_D 1 -#define K_R 0.3 - -#include "drake/common/trajectories/piecewise_polynomial.h" -#include "drake/manipulation/kuka_iiwa/iiwa_status_receiver.h" -#include "drake/manipulation/kuka_iiwa/iiwa_command_sender.h" -#include "drake/systems/lcm/lcm_publisher_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/primitives/trajectory_source.h" -#include "drake/systems/primitives/constant_vector_source.h" -#include "drake/lcm/drake_lcm.h" -#include "drake/lcmt_iiwa_command.hpp" -#include "drake/lcmt_iiwa_status.hpp" -#include "drake/multibody/plant/multibody_plant.h" -#include "drake/multibody/parsing/parser.h" - -#include "systems/controllers/endeffector_velocity_controller.h" -#include "systems/controllers/endeffector_position_controller.h" - -namespace dairlib { - -// This function creates a controller for a Kuka LBR Iiwa arm by connecting an -// EndEffectorPositionController to an EndEffectorVelocityController to control -// the individual joint torques as to move the endeffector -// to a desired position. -int do_main(int argc, char* argv[]) { - // Creating end effector trajectory - // TODO make this modular - const std::vector times {0.0, 25.0, 35.0, 45.0, 55.0, 65.0, - 75.0, 85.0, 95.0, 105.0, 115}; - - std::vector points(times.size()); - - Eigen::Vector3d AS, A0, A1, A2, A3, A4, A5, A6, A7, A8, A9; - - AS << -0.23, -0.2, 0.25; - A0 << -0.23, -0.2, 0.25; - - A1 << -0.23, -0.6, 0.25; - A2 << 0.23, -0.6, 0.25; - - A3 << 0.23, -0.2, 0.25; - - A4 << 0.23, -0.2, 0.25; - - A5 << 0.23, -0.6, 0.25; - - A6 << -0.23, -0.6, 0.25; - - A7 << -0.23, -0.2, 0.25; - - A8 << -0.23, -0.2, 0.25; - A9 << -0.23, -0.2, 0.25; - - points[0] = AS; - points[1] = A0; - points[2] = A1; - points[3] = A2; - points[4] = A3; - points[5] = A4; - points[6] = A5; - points[7] = A6; - points[8] = A7; - points[9] = A8; - points[10] = A9; - - auto ee_trajectory = drake::trajectories::PiecewisePolynomial< - double>::FirstOrderHold(times, points); - - // Creating end effector orientation trajectory - const std::vector orient_times {0, 115}; - std::vector orient_points(orient_times.size()); - Eigen::Vector4d start_o, end_o; - start_o << 0, 0, 1, 0; - end_o << 0, 0, 1, 0; - - orient_points[0] = start_o; - orient_points[1] = end_o; - - auto orientation_trajectory = drake::trajectories::PiecewisePolynomial< - double>::FirstOrderHold(orient_times, orient_points); - - // Initialize Kuka model URDF-- from Drake kuka simulation files - std::string kModelPath = "../drake/manipulation/models/iiwa_description" - "/iiwa7/iiwa7_no_collision.sdf"; - const std::string urdf_string = FindResourceOrThrow(kModelPath); - - // MultibodyPlants are created here, then passed by reference - // to the controller blocks for internal modelling. - const auto X_WI = drake::math::RigidTransform::Identity(); - std::unique_ptr> owned_plant = - std::make_unique>(0.0); - - drake::multibody::Parser plant_parser(owned_plant.get()); - plant_parser.AddModelFromFile(urdf_string, "iiwa"); - owned_plant->WeldFrames(owned_plant->world_frame(), - owned_plant->GetFrameByName("iiwa_link_0"), X_WI); - owned_plant->Finalize(); - - drake::systems::DiagramBuilder builder; - - auto lcm = builder.AddSystem(); - - // Adding status subscriber and receiver blocks - auto status_subscriber = builder.AddSystem( - drake::systems::lcm::LcmSubscriberSystem::Make( - "IIWA_STATUS", lcm)); - auto status_receiver = builder.AddSystem< - drake::manipulation::kuka_iiwa::IiwaStatusReceiver>(); - - // The coordinates for the end effector with respect to the last joint, - // used to determine location of end effector - Eigen::Vector3d eeContactFrame; - eeContactFrame << 0.0, 0, 0.09; - - const std::string link_7 = "iiwa_link_7"; - - // Adding position controller block - auto position_controller = builder.AddSystem< - systems::EndEffectorPositionController>( - *owned_plant, link_7, eeContactFrame, K_P, K_OMEGA); - - // Adding Velocity Controller block - auto velocity_controller = builder.AddSystem< - systems::EndEffectorVelocityController>( - *owned_plant, link_7, eeContactFrame, K_D, K_R); - - - // Adding linear position Trajectory Source - auto input_trajectory = builder.AddSystem( - ee_trajectory); - // Adding orientation Trajectory Source - auto input_orientation = builder.AddSystem( - orientation_trajectory); - - - // Adding command publisher and broadcaster blocks - auto command_sender = builder.AddSystem< - drake::manipulation::kuka_iiwa::IiwaCommandSender>(); - auto command_publisher = builder.AddSystem( - drake::systems::lcm::LcmPublisherSystem::Make( - "IIWA_COMMAND", lcm, 1.0/200.0)); - - // Torque Controller-- includes virtual springs and damping. - VectorXd ConstPositionCommand; - - // The virtual spring stiffness in Nm/rad. - ConstPositionCommand.resize(7); - ConstPositionCommand << 0, 0, 0, 0, 0, 0, 0; - - auto positionCommand = builder.AddSystem< - drake::systems::ConstantVectorSource>(ConstPositionCommand); - - builder.Connect(status_subscriber->get_output_port(), - status_receiver->get_input_port()); - builder.Connect(status_receiver->get_position_measured_output_port(), - velocity_controller->get_joint_pos_input_port()); - builder.Connect(status_receiver->get_velocity_estimated_output_port(), - velocity_controller->get_joint_vel_input_port()); - //Connecting q input from status receiver to position controller - builder.Connect(status_receiver->get_position_measured_output_port(), - position_controller->get_joint_pos_input_port()); - //Connecting x_desired input from trajectory to position controller - builder.Connect(input_trajectory->get_output_port(), - position_controller->get_endpoint_pos_input_port()); - //Connecting desired orientation to position controller - builder.Connect(input_orientation->get_output_port(), - position_controller->get_endpoint_orient_input_port()); - //Connecting position (twist) controller to trajectory/velocity controller; - builder.Connect(position_controller->get_endpoint_cmd_output_port(), - velocity_controller->get_endpoint_twist_input_port()); - - builder.Connect(velocity_controller->get_endpoint_torque_output_port(), - command_sender->get_torque_input_port()); - - builder.Connect(positionCommand->get_output_port(), - command_sender->get_position_input_port()); - builder.Connect(command_sender->get_output_port(), - command_publisher->get_input_port()); - - auto diagram = builder.Build(); - - drake::systems::Simulator simulator(*diagram); - - simulator.set_publish_every_time_step(false); - simulator.set_target_realtime_rate(1.0); - simulator.Initialize(); - simulator.AdvanceTo(ee_trajectory.end_time()); - return 0; -} - -} // Namespace dairlib - -int main(int argc, char* argv[]) { - return dairlib::do_main(argc, argv); -} diff --git a/examples/kuka_iiwa_arm/iiwa_oscillate.cc b/examples/kuka_iiwa_arm/iiwa_oscillate.cc deleted file mode 100644 index 9827cb2b64..0000000000 --- a/examples/kuka_iiwa_arm/iiwa_oscillate.cc +++ /dev/null @@ -1,134 +0,0 @@ -#define AMPLITUDE 1 -#define FREQUENCY 3 -#define MAX_VELOCITY 5 - -#include -#include -#include -#include - -#include "drake/systems/lcm/lcm_publisher_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" -#include "drake/systems/primitives/multiplexer.h" -#include "drake/systems/primitives/constant_vector_source.h" -#include "drake/systems/primitives/sine.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/lcmt_iiwa_command.hpp" -#include "drake/lcmt_iiwa_status.hpp" -#include "drake/manipulation/kuka_iiwa/iiwa_status_receiver.h" -#include "drake/manipulation/kuka_iiwa/iiwa_command_sender.h" -#include "drake/common/find_resource.h" -#include "drake/common/text_logging.h" - -#include "spdlog/spdlog.h" -#include "spdlog/sinks/stdout_color_sinks.h" -#include "spdlog/sinks/basic_file_sink.h" - -#include "systems/controllers/safe_velocity_controller.h" - -namespace dairlib { - -void setup_log(); - -int do_main(int argc, char* argv[]) { - drake::systems::DiagramBuilder builder; - auto lcm = builder.AddSystem(); - - // Adding status subscriber and receiver blocks - auto status_subscriber = builder.AddSystem( - drake::systems::lcm::LcmSubscriberSystem::Make( - "IIWA_STATUS", lcm)); - auto status_receiver = builder.AddSystem(); - - // Adding command publisher and broadcaster blocks - auto command_sender = builder.AddSystem(); - auto command_publisher = builder.AddSystem( - drake::systems::lcm::LcmPublisherSystem::Make( - "IIWA_COMMAND", lcm, 1.0/200.0)); - - Eigen::VectorXd zeros_low = Eigen::VectorXd::Zero(4); - auto zeros_low_source = builder.AddSystem(zeros_low); - - Eigen::VectorXd zeros_high = Eigen::VectorXd::Zero(2); - auto zeros_high_source = builder.AddSystem(zeros_high); - - auto sine_source = builder.AddSystem(AMPLITUDE, FREQUENCY, 0, 1, true); - //setup_log(); - - std::vector input_sizes = {4, 1, 2}; - auto mux = builder.AddSystem(input_sizes); - - const int num_iiwa_joints = 7; - auto velocity_controller = builder.AddSystem( - MAX_VELOCITY, num_iiwa_joints); - - Eigen::VectorXd zeros_seven = Eigen::VectorXd::Zero(7); - auto constant_position_src = builder.AddSystem(zeros_seven); - - builder.Connect(zeros_low_source->get_output_port(), - mux->get_input_port(0)); - builder.Connect(sine_source->get_output_port(0), - mux->get_input_port(1)); - builder.Connect(zeros_high_source->get_output_port(), - mux->get_input_port(2)); - - builder.Connect(status_subscriber->get_output_port(), - status_receiver->get_input_port()); - - builder.Connect(mux->get_output_port(0), - velocity_controller->get_joint_torques_input_port()); - - builder.Connect(status_receiver->get_velocity_estimated_output_port(), - velocity_controller->get_joint_velocities_input_port()); - - builder.Connect(velocity_controller->get_joint_torques_output_port(), - command_sender->get_torque_input_port()); - - // builder.Connect(status_receiver->get_position_measured_output_port(), - // command_sender->get_position_input_port()); - - builder.Connect(constant_position_src->get_output_port(), - command_sender->get_position_input_port()); - - builder.Connect(command_sender->get_output_port(), - command_publisher->get_input_port()); - - auto diagram = builder.Build(); - drake::systems::Simulator simulator(*diagram); - - simulator.set_publish_every_time_step(false); - simulator.set_publish_at_initialization(false); - simulator.set_target_realtime_rate(1.0); - simulator.Initialize(); - - simulator.AdvanceTo(20); - return 0; -} - -void setup_log() { - auto t = std::time(nullptr); - auto tm = *std::localtime(&t); - - std::ostringstream oss; - oss << std::put_time(&tm, "%d_%m_%Y-%H_%M_%S"); - auto time_str = oss.str(); - - auto file_str = "logs/test_config_" + time_str + ".txt"; - - auto file_sink = std::make_shared(file_str, true); - drake::log()->sinks().push_back(file_sink); - - drake::log()->info("Testing with amplitude {} and frequency {}", AMPLITUDE, FREQUENCY); - drake::log()->info("Max velocity {}", MAX_VELOCITY); - drake::log()->info("Has spdlog: {}", drake::logging::kHaveSpdlog); - -} - -} // Namespace dairlib - -int main(int argc, char* argv[]) { - return dairlib::do_main(argc, argv); -} diff --git a/examples/kuka_iiwa_arm/iiwa_visualizer.cc b/examples/kuka_iiwa_arm/iiwa_visualizer.cc deleted file mode 100644 index 083de5f4f4..0000000000 --- a/examples/kuka_iiwa_arm/iiwa_visualizer.cc +++ /dev/null @@ -1,87 +0,0 @@ -#include "drake/common/find_resource.h" -#include "drake/manipulation/kuka_iiwa/iiwa_status_receiver.h" -#include "drake/lcmt_iiwa_status.hpp" -#include "drake/multibody/rigid_body_plant/rigid_body_plant.h" -#include "drake/multibody/rigid_body_tree_construction.h" -#include "drake/multibody/parsers/urdf_parser.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" -#include "drake/systems/primitives/discrete_derivative.h" -#include "drake/multibody/rigid_body_plant/drake_visualizer.h" - -namespace dairlib { -using drake::systems::RigidBodyPlant; -using drake::systems::Simulator; -using drake::manipulation::kuka_iiwa::IiwaStatusReceiver; - -int doMain(int argc, char* argv[]) { - - drake::systems::DiagramBuilder builder; - // Adds a plant. - const char* kModelPath = - "drake/manipulation/models/iiwa_description/" - "urdf/iiwa14_polytope_collision.urdf"; - const std::string urdf = drake::FindResourceOrThrow(kModelPath); - int num_joints; - - - RigidBodyTree tree; - { - drake::parsers::urdf::AddModelInstanceFromUrdfFileToWorld( - urdf, drake::multibody::joints::kFixed, &tree); - drake::multibody::AddFlatTerrainToWorld(&tree, 100., 10.); - //plant = builder.AddPlant(std::move(tree)); - num_joints = tree.get_num_positions(); - } - - auto lcm = builder.AddSystem(); - const std::string channel_x = "IIWA_STATUS"; - - // Create state subscriber and state receiver. - auto state_sub = builder.AddSystem( - drake::systems::lcm::LcmSubscriberSystem::Make < - drake::lcmt_iiwa_status > (channel_x, lcm)); - auto state_receiver = builder.AddSystem(num_joints); - - // Create visualizer - auto visualizer = builder.AddSystem(tree, lcm); - visualizer->set_publish_period(1.0/30.0); - - // Get the state from the LCM message and add it to the Visualizer - auto desired_state_from_position = builder.AddSystem< - drake::systems::StateInterpolatorWithDiscreteDerivative>( - num_joints, 0.005); - desired_state_from_position->set_name("desired_state_from_position"); - - builder.Connect(state_sub->get_output_port(), - state_receiver->get_input_port()); - builder.Connect(state_receiver->get_position_measured_output_port(), - desired_state_from_position->get_input_port()); - builder.Connect(desired_state_from_position->get_output_port(), - visualizer->get_input_port(0)); - - auto diagram = builder.Build(); - auto context = diagram->CreateDefaultContext(); - - auto stepper = std::make_unique>(*diagram, - std::move(context)); - - stepper->set_publish_every_time_step(false); - stepper->set_publish_at_initialization(false); - stepper->set_target_realtime_rate(1.0); - stepper->Initialize(); - - drake::log()->info("visualizer started"); - - stepper->AdvanceTo(std::numeric_limits::infinity()); - - return 0; -} - -} // namespace dairlib - -int main(int argc, char* argv[]) { - return dairlib::doMain(argc, argv); -} diff --git a/examples/kuka_iiwa_arm/kuka_simulation.cc b/examples/kuka_iiwa_arm/kuka_simulation.cc deleted file mode 100644 index 39c31a5323..0000000000 --- a/examples/kuka_iiwa_arm/kuka_simulation.cc +++ /dev/null @@ -1,224 +0,0 @@ -/// @file -/// -/// Implements a simulation of the KUKA iiwa arm. Like the driver for the -/// physical arm, this simulation communicates over LCM using lcmt_iiwa_status -/// and lcmt_iiwa_command messages. It is intended to be a be a direct -/// replacement for the KUKA iiwa driver and the actual robot hardware. - -#include -#include -#include -#include - -#include "drake/common/find_resource.h" -#include "drake/geometry/drake_visualizer.h" -#include "drake/geometry/scene_graph.h" -#include "drake/manipulation/kuka_iiwa/iiwa_command_receiver.h" -#include "drake/manipulation/kuka_iiwa/iiwa_status_sender.h" -#include "drake/math/rigid_transform.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/tree/revolute_joint.h" -#include "drake/multibody/tree/multibody_tree.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/primitives/demultiplexer.h" -#include "drake/systems/primitives/discrete_derivative.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/lcm/lcm_interface_system.h" -#include "drake/systems/lcm/lcm_publisher_system.h" -#include "drake/systems/lcm/lcm_subscriber_system.h" -#include "drake/multibody/plant/multibody_plant.h" -#include "drake/lcmt_iiwa_command.hpp" -#include "drake/lcmt_iiwa_status.hpp" -#include "examples/kuka_iiwa_arm/kuka_torque_controller.h" - -namespace dairlib { -namespace examples { -namespace kuka_iiwa_arm { - - using Eigen::Vector3d; - using Eigen::VectorXd; - - using drake::geometry::DrakeVisualizer; - using drake::geometry::SceneGraph; - using drake::manipulation::kuka_iiwa::IiwaCommandReceiver; - using drake::manipulation::kuka_iiwa::IiwaStatusSender; - using drake::math::RigidTransform; - using drake::math::RollPitchYaw; - using drake::multibody::Joint; - using drake::multibody::ModelInstanceIndex; - using drake::multibody::MultibodyPlant; - using drake::multibody::RevoluteJoint; - using drake::multibody::SpatialInertia; - using drake::systems::StateInterpolatorWithDiscreteDerivative; - -int DoMain() { - - std::unique_ptr> owned_world_plant = - std::make_unique>(0.0001); - std::unique_ptr> owned_controller_plant = - std::make_unique>(0.0); - std::unique_ptr> owned_scene_graph = - std::make_unique>(); - - MultibodyPlant* world_plant = - owned_world_plant.get(); - MultibodyPlant* controller_plant = - owned_controller_plant.get(); - drake::geometry::SceneGraph* scene_graph = - owned_scene_graph.get(); - world_plant->RegisterAsSourceForSceneGraph(scene_graph); - - // Get the Iiwa model. TODO: grab this from pegged drake libraries - const char* kModelPath = - "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf"; - const std::string kuka_urdf = drake::FindResourceOrThrow(kModelPath); - const auto X_WI = RigidTransform::Identity(); - - // Add the Iiwa model to the world model - drake::multibody::Parser world_plant_parser(world_plant); - const drake::multibody::ModelInstanceIndex iiwa_model = - world_plant_parser.AddModelFromFile(kuka_urdf, "iiwa"); - world_plant->WeldFrames( - owned_world_plant->world_frame(), - owned_world_plant->GetFrameByName("iiwa_link_0", iiwa_model), X_WI); - - // Create and add a plant to the controller-specific model - drake::multibody::Parser controller_plant_parser(controller_plant); - const auto controller_iiwa_model = controller_plant_parser.AddModelFromFile( - kuka_urdf, "iiwa"); - owned_controller_plant->WeldFrames(owned_controller_plant->world_frame(), - owned_controller_plant->GetFrameByName("iiwa_link_0", controller_iiwa_model), - X_WI); - - // Finalize the plants to begin adding them to a system - owned_controller_plant->Finalize(); - world_plant->Finalize(); - - const int num_iiwa_positions = world_plant->num_positions(); - const int num_iiwa_velocities = world_plant->num_velocities(); - - drake::systems::DiagramBuilder builder; - builder.AddSystem(std::move(owned_world_plant)); - builder.AddSystem(std::move(owned_scene_graph)); - - auto lcm = builder.AddSystem(); - - // Create the command subscriber and status publisher. - auto command_sub = builder.AddSystem( - drake::systems::lcm::LcmSubscriberSystem::Make( - "IIWA_COMMAND", lcm)); - command_sub->set_name("command_subscriber"); - auto command_receiver = - builder.AddSystem(num_iiwa_positions); - command_receiver->set_name("command_receiver"); - - // LCM publisher system - const double kIiwaLcmStatusPeriod = 0.005; - auto iiwa_status_publisher = builder.AddSystem( - drake::systems::lcm::LcmPublisherSystem::Make( - "IIWA_STATUS", lcm, kIiwaLcmStatusPeriod /* publish period */)); - - // Torque Controller-- includes virtual springs and damping. - VectorXd stiffness, damping_ratio; - - // The virtual spring stiffness in Nm/rad. - stiffness.resize(num_iiwa_positions); - stiffness << 2, 2, 2, 2, 2, 2, 2; - - // A dimensionless damping ratio. See KukaTorqueController for details. - damping_ratio.resize(num_iiwa_positions); - damping_ratio.setConstant(1.0); - auto iiwa_controller = builder.AddSystem< - dairlib::systems::KukaTorqueController>( - std::move(owned_controller_plant), stiffness, damping_ratio); - - // Creating status sender - auto iiwa_status = builder.AddSystem(num_iiwa_positions); - - // Creating system to approximate desired state command from a discrete - // derivative of the position command input port. - auto desired_state_from_position = builder.AddSystem< - drake::systems::StateInterpolatorWithDiscreteDerivative>( - num_iiwa_positions, world_plant->time_step()); - - // Demuxing system state for status publisher - auto demux = builder.AddSystem>( - 2 * num_iiwa_positions, num_iiwa_positions); - - builder.Connect(command_sub->get_output_port(), - command_receiver->get_message_input_port()); - - // Connecting iiwa input ports - builder.Connect(iiwa_controller->get_output_port_control(), - world_plant->get_actuation_input_port(iiwa_model)); - - // Interpolating desired positions into desired velocities for controller. - builder.Connect(command_receiver->get_commanded_position_output_port(), - desired_state_from_position->get_input_port()); - - // Connecting iiwa controller input ports - builder.Connect(world_plant->get_state_output_port(iiwa_model), - iiwa_controller->get_input_port_estimated_state()); - builder.Connect(desired_state_from_position->get_output_port(), - iiwa_controller->get_input_port_desired_state()); - builder.Connect(command_receiver->get_commanded_torque_output_port(), - iiwa_controller->get_input_port_commanded_torque()); - - // Demux is for separating q and v from state output port. - builder.Connect(world_plant->get_state_output_port(iiwa_model), - demux->get_input_port(0)); - - // Connecting outputs to iiwa state broadcaster - builder.Connect(demux->get_output_port(0), - iiwa_status->get_position_measured_input_port()); - builder.Connect(demux->get_output_port(1), - iiwa_status->get_velocity_estimated_input_port()); - builder.Connect(command_receiver->get_output_port(0), - iiwa_status->get_position_commanded_input_port()); - builder.Connect(iiwa_controller->get_output_port_control(), - iiwa_status->get_torque_commanded_input_port()); - builder.Connect(iiwa_controller->get_output_port_control(), - iiwa_status->get_torque_measured_input_port()); - builder.Connect(world_plant->get_generalized_contact_forces_output_port(iiwa_model), - iiwa_status->get_torque_external_input_port()); - builder.Connect(iiwa_status->get_output_port(), - iiwa_status_publisher->get_input_port()); - - // Connecting world to scene graph components for visualization - builder.Connect( - world_plant->get_geometry_poses_output_port(), - scene_graph->get_source_pose_port(world_plant->get_source_id().value())); - builder.Connect(scene_graph->get_query_output_port(), - world_plant->get_geometry_query_input_port()); - - DrakeVisualizer::AddToBuilder(&builder, *scene_graph); - - auto diagram = builder.Build(); - drake::systems::Simulator simulator(*diagram); - - // Set the iiwa default joint configuration. - drake::VectorX q0_iiwa(num_iiwa_positions + num_iiwa_velocities); - q0_iiwa << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; - - drake::systems::Context& context = - diagram->GetMutableSubsystemContext(*world_plant, - &simulator.get_mutable_context()); - - drake::systems::BasicVector& state = - context.get_mutable_discrete_state(0); - state.SetFromVector(q0_iiwa); - - simulator.set_publish_every_time_step(false); - simulator.set_target_realtime_rate(1.0); - simulator.AdvanceTo(std::numeric_limits::infinity()); - return 0; - -} - -} // namespace kuka_iiwa_arm -} // namespace examples -} // namespace drake - -int main(int argc, char* argv[]) { - return dairlib::examples::kuka_iiwa_arm::DoMain(); -} diff --git a/examples/kuka_iiwa_arm/kuka_test.pmd b/examples/kuka_iiwa_arm/kuka_test.pmd deleted file mode 100644 index 9f18384e2a..0000000000 --- a/examples/kuka_iiwa_arm/kuka_test.pmd +++ /dev/null @@ -1,44 +0,0 @@ -group "0.operator" { - cmd "1.drake-director" { - exec = "bazel-bin/director/drake-director --script examples/kuka_iiwa_arm/signal_scope_panel.py"; - host = "localhost"; - } -} - -group "1.simulated-robot" { - cmd "1.simulator" { - exec = "bazel-bin/examples/kuka_iiwa_arm/kuka_simulation"; - host = "localhost"; - } - - cmd "1a. rbp simulator" { - exec = "../drake/bazel-bin/examples/kuka_iiwa_arm/kuka_simulation"; - host = "localhost"; - } - - cmd "2.position_broadcaster" { - exec = "bazel-bin/examples/kuka_iiwa_arm/iiwa_controller_demo"; - host = "localhost"; - } - - cmd "3.iiwa_oscillate" { - exec = "bazel-bin/examples/kuka_iiwa_arm/iiwa_oscillate"; - host = "localhost"; - } - - cmd "4.Visualizer" { - exec = "bazel-bin/examples/kuka_iiwa_arm/iiwa_visualizer"; - host = "localhost"; - } -} - -group "2.lcm-tools" { - cmd "0.lcm-spy" { - exec = "bazel-bin/lcmtypes/dair-lcm-spy"; - host = "localhost"; - } - cmd "1.signal-scope" { - exec = "bazel-bin/director/signal-scope"; - host = "localhost"; - } -} diff --git a/examples/kuka_iiwa_arm/kuka_torque_controller.cc b/examples/kuka_iiwa_arm/kuka_torque_controller.cc deleted file mode 100644 index a9b7687d40..0000000000 --- a/examples/kuka_iiwa_arm/kuka_torque_controller.cc +++ /dev/null @@ -1,175 +0,0 @@ -// A rewrite of Drake's kuka_torque_controller updated with a MultiBodyPlant. -#include - -#include -#include "drake/systems/controllers/inverse_dynamics.h" -#include "drake/systems/controllers/pid_controller.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/framework/leaf_system.h" -#include "drake/systems/primitives/adder.h" -#include "drake/systems/primitives/constant_vector_source.h" -#include "drake/systems/primitives/pass_through.h" -#include "drake/multibody/plant/multibody_plant.h" - -#include "examples/kuka_iiwa_arm/kuka_torque_controller.h" - -namespace dairlib { -namespace systems { - - using drake::systems::kVectorValued; - using drake::systems::Adder; - using drake::systems::BasicVector; - using drake::systems::Context; - using drake::systems::DiagramBuilder; - using drake::systems::LeafSystem; - using drake::systems::PassThrough; - using drake::systems::controllers::InverseDynamics; - using drake::systems::controllers::PidController; - using drake::multibody::MultibodyPlant; - using drake::VectorX; - -template -class StateDependentDamper : public LeafSystem { - public: - StateDependentDamper(const MultibodyPlant& plant, - const VectorX& stiffness, - const VectorX& damping_ratio) - : plant_(plant), stiffness_(stiffness), damping_ratio_(damping_ratio) { - const int num_q = plant_.num_positions(); - const int num_v = plant_.num_velocities(); - const int num_x = num_q + num_v; - - DRAKE_DEMAND(stiffness.size() == num_v); - DRAKE_DEMAND(damping_ratio.size() == num_v); - - this->DeclareInputPort("x", kVectorValued, num_x); - this->DeclareVectorOutputPort("u", BasicVector(num_v), - &StateDependentDamper::CalcTorque); - } - - private: - const MultibodyPlant& plant_; - const VectorX stiffness_; - const VectorX damping_ratio_; - - /** - * Computes joint level damping forces by computing the damping ratio for each - * joint independently as if all other joints were fixed. Note that the - * effective inertia of a joint, when all of the other joints are fixed, is - * given by the corresponding diagonal entry of the mass matrix. The critical - * damping gain for the i-th joint is given by 2*sqrt(M(i,i)*stiffness(i)). - */ - void CalcTorque(const Context& context, BasicVector* torque) const { - Eigen::VectorXd x = this->EvalVectorInput(context, 0)->get_value(); - Eigen::VectorXd q = x.head(plant_.num_positions()); - Eigen::VectorXd v = x.tail(plant_.num_velocities()); - - // Compute critical damping gains and scale by damping ratio. Use Eigen - // arrays (rather than matrices) for elementwise multiplication. - Eigen::MatrixXd H(plant_.num_positions(), plant_.num_positions()); - std::unique_ptr> plant_context = plant_.CreateDefaultContext(); - - plant_.CalcMassMatrix(*plant_context.get(), &H); - Eigen::ArrayXd temp = H.diagonal().array() * stiffness_.array(); - Eigen::ArrayXd damping_gains = 2 * temp.sqrt(); - damping_gains *= damping_ratio_.array(); - - // Compute damping torque. - torque->get_mutable_value() = -(damping_gains * v.array()).matrix(); - } -}; - - -template -void KukaTorqueController::SetUp(const VectorX& stiffness, - const VectorX& damping_ratio) { - DiagramBuilder builder; - const MultibodyPlant& plant = *robot_for_control_; - DRAKE_DEMAND(plant.num_positions() == stiffness.size()); - DRAKE_DEMAND(plant.num_positions() == damping_ratio.size()); - - const int dim = plant.num_positions(); - - /* - torque_in ---------------------------- - | - (q, v) ------>|Gravity Comp|------+---> torque_out - | /| - --->|Damping|---------- | - | | - --->| Virtual Spring |--- - (q*, v*) ------>|PID with kd=ki=0| - */ - - // Redirects estimated state input into PID and gravity compensation. - auto pass_through = builder.template AddSystem>(2 * dim); - - - // Add gravity compensator. - auto gravity_comp = builder.template AddSystem>( - &plant, InverseDynamics::kGravityCompensation); - - // Adds virtual springs. - Eigen::VectorXd kd(dim); - Eigen::VectorXd ki(dim); - Eigen::VectorXd kp(dim); - kd.setZero(); - ki.setZero(); - kp.setZero(); - auto spring = builder.template AddSystem>(kp, kd, ki); - - // Adds virtual damper. - auto damper = builder.template AddSystem>( - plant, stiffness, damping_ratio); - - // Adds an adder to sum the gravity compensation, spring, damper, and - // feedforward torque. - auto adder = builder.template AddSystem>(4, dim); - - // Connects the estimated state to the gravity compensator. - builder.Connect(pass_through->get_output_port(), - gravity_comp->get_input_port_estimated_state()); - - // Connects the estimated state to the spring. - builder.Connect(pass_through->get_output_port(), - spring->get_input_port_estimated_state()); - - // Connects the estimated state to the damper. - builder.Connect(pass_through->get_output_port(), - damper->get_input_port(0)); - - // Connects the gravity compensation, spring, and damper torques to the adder. - builder.Connect(gravity_comp->get_output_port_force(), adder->get_input_port(1)); - builder.Connect(spring->get_output_port(0), adder->get_input_port(2)); - builder.Connect(damper->get_output_port(0), adder->get_input_port(3)); - - // Exposes the estimated state port. - input_port_index_estimated_state_ = - builder.ExportInput(pass_through->get_input_port()); - - // Exposes the desired state port. - input_port_index_desired_state_ = - builder.ExportInput(spring->get_input_port_desired_state()); - - // Exposes the commanded torque port. - input_port_index_commanded_torque_ = - builder.ExportInput(adder->get_input_port(0)); - - // Exposes controller output. - output_port_index_control_ = builder.ExportOutput(adder->get_output_port()); - builder.BuildInto(this); -} - -template -KukaTorqueController::KukaTorqueController( - std::unique_ptr> plant, const VectorX& stiffness, - const VectorX& damping) { - robot_for_control_ = std::move(plant); - SetUp(stiffness, damping); -} - - -template class dairlib::systems::KukaTorqueController; - -} // namespace systems -} // namespace dairlib diff --git a/examples/kuka_iiwa_arm/kuka_torque_controller.h b/examples/kuka_iiwa_arm/kuka_torque_controller.h deleted file mode 100644 index d762465521..0000000000 --- a/examples/kuka_iiwa_arm/kuka_torque_controller.h +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include -#include - -#include "drake/common/drake_copyable.h" -#include "drake/multibody/plant/multibody_plant.h" -#include "drake/systems/controllers/state_feedback_controller_interface.h" -#include "drake/systems/framework/diagram.h" - -namespace dairlib { -namespace systems { - -/** - * Controller that take emulates the kuka_iiwa_arm when operated in torque - * control mode. The controller specifies a stiffness and damping ratio at each - * of the joints. Because the critical damping constant is a function of the - * configuration the damping is non-linear. See - * https://github.com/RobotLocomotion/drake-iiwa-driver/blob/master/kuka-driver/sunrise_1.11/DrakeFRITorqueDriver.java - * - * for details on the low-level controller. Note that the - * input_port_desired_state() method takes a full state for convenient wiring - * with other Systems, but ignores the velocity component. - */ - -using drake::VectorX; -using drake::multibody::MultibodyPlant; - -template -class KukaTorqueController - : public drake::systems::Diagram, - public drake::systems::controllers::StateFeedbackControllerInterface { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(KukaTorqueController) - KukaTorqueController(std::unique_ptr> plant, - const VectorX& stiffness, - const VectorX& damping); - - const drake::systems::InputPort& get_input_port_commanded_torque() const { - return drake::systems::Diagram::get_input_port( - input_port_index_commanded_torque_); - } - - const drake::systems::InputPort& get_input_port_estimated_state() const override { - return drake::systems::Diagram::get_input_port( - input_port_index_estimated_state_); - } - - const drake::systems::InputPort& get_input_port_desired_state() const override { - return drake::systems::Diagram::get_input_port(input_port_index_desired_state_); - } - - const drake::systems::OutputPort& get_output_port_control() const override { - return drake::systems::Diagram::get_output_port(output_port_index_control_); - } - - private: - void SetUp(const VectorX& stiffness, - const VectorX& damping_ratio); - std::unique_ptr> robot_for_control_{nullptr}; - int input_port_index_estimated_state_{-1}; - int input_port_index_desired_state_{-1}; - int input_port_index_commanded_torque_{-1}; - int output_port_index_control_{-1}; -}; - -} // namespace systems -} // namespace dair diff --git a/examples/kuka_iiwa_arm/signal_scope_panel.py b/examples/kuka_iiwa_arm/signal_scope_panel.py deleted file mode 100644 index c0508908c4..0000000000 --- a/examples/kuka_iiwa_arm/signal_scope_panel.py +++ /dev/null @@ -1,8 +0,0 @@ -import dairlib.lcmt_iiwa_command -import dairlib.lcmt_iiwa_status - -import director.openscope as scope -import subprocess - -view = applogic.getMainWindow() -applogic.addShortcut(view, 'Ctrl+I', scope.startSignalScope) diff --git a/examples/sampling_c3/BUILD.bazel b/examples/sampling_c3/BUILD.bazel new file mode 100644 index 0000000000..af48fccb44 --- /dev/null +++ b/examples/sampling_c3/BUILD.bazel @@ -0,0 +1,295 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "urdfs", + data = glob([ + "urdf/**", + ]), +) + +filegroup( + name = "all_example_params_yamls", + visibility = ["//visibility:public"], + data = ["//examples/sampling_c3/jacktoy:parameters"], +) + +filegroup( + name = "shared_params_yamls", + srcs = glob(["shared_parameters/*.yaml"]), + visibility = ["//visibility:public"], +) + +cc_binary( + name = "franka_sim", + srcs = ["franka_sim.cc"], + data = [ + ":urdfs", + "//examples/sampling_c3:all_example_params_yamls", + "@drake_models//:franka_description", + ], + deps = [ + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//examples/sampling_c3/parameter_headers:franka_sim_params", + "//common", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:lcm_driven_loop", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_osc_controller", + srcs = ["franka_osc_controller.cc"], + data = [ + ":urdfs", + "//examples/sampling_c3:all_example_params_yamls", + "@drake_models//:franka_description", + ], + deps = [ + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//examples/sampling_c3/parameter_headers:osc_params", + "//common", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers:gravity_compensator", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/trajectory_optimization:lcm_trajectory_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_joint_osc_controller", + srcs = ["franka_joint_osc_controller.cc"], + data = [ + ":urdfs", + "//examples/sampling_c3:all_example_params_yamls", + "@drake_models//:franka_description", + ], + deps = [ + "//examples/sampling_c3:joint_trajectory_generator", + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//examples/sampling_c3/parameter_headers:osc_params", + "//common", + "//lcm:lcm_trajectory_saver", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers:gravity_compensator", + "//systems/controllers/osc:operational_space_control", + "//systems/framework:lcm_driven_loop", + "//systems/trajectory_optimization:lcm_trajectory_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_library( + name = "joint_trajectory_generator", + srcs = ["joint_trajectory_generator.cc"], + hdrs = ["joint_trajectory_generator.h"], + deps = [ + "//systems/framework:vector", + ], +) + +cc_binary( + name = "franka_visualizer", + srcs = ["franka_visualizer.cc"], + data = [ + ":urdfs", + "//examples/sampling_c3:all_example_params_yamls", + "@drake_models//:franka_description", + ], + deps = [ + ":c3_mode_visualizer", + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_options", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//examples/sampling_c3/parameter_headers:visualizer_params", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:sampling_params", + "//common", + "//multibody:utils", + "//multibody:visualization_utils", + "//systems/senders:senders", + "//systems:franka_kinematics", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/primitives", + "//systems/trajectory_optimization:lcm_trajectory_systems", + "//systems/visualization:lcm_visualization_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_bridge_driver_out", + srcs = ["franka_bridge_driver_out.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + "//examples/sampling_c3:all_example_params_yamls", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//common/parameters:franka_drake_lcm_driver_channels", + "//common", + "//systems:franka_state_translator", + "//multibody:utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:lcm_driven_loop", + "//systems/primitives", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_bridge_driver_in", + srcs = ["franka_bridge_driver_in.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + "//examples/sampling_c3:all_example_params_yamls", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//common/parameters:franka_drake_lcm_driver_channels", + "//common", + "//systems:franka_state_translator", + "//multibody:utils", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/framework:lcm_driven_loop", + "//systems/primitives", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_binary( + name = "franka_sampling_c3_controller", + srcs = ["franka_sampling_c3_controller.cc"], + data = [ + ":urdfs", + "@drake_models//:franka_description", + "//examples/sampling_c3:all_example_params_yamls", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3:goal_generator", + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//common", + "//lcm:lcm_trajectory_saver", + "//systems:franka_kinematics", + "//systems/senders:sample_buffer_sender", + "//systems/senders:senders", + "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers:sampling_c3_controller", + "//systems/framework:lcm_driven_loop", + "//systems/trajectory_optimization:c3_output_systems", + "@drake//:drake_shared_library", + "@gflags", + ], +) + +cc_library( + name = "c3_mode_visualizer", + srcs = ["c3_mode_visualizer.cc"], + hdrs = ["c3_mode_visualizer.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//lcm:lcm_trajectory_saver", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "goal_generator", + srcs = ["goal_generator.cc"], + hdrs = ["goal_generator.h"], + deps = [ + "//examples/sampling_c3/parameter_headers:goal_params", + "//systems/framework:vector", + "//lcmtypes:lcmt_robot", + "//lcm:lcm_trajectory_saver", + "//common:math_utils", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "generate_samples", + srcs = ["generate_samples.cc"], + hdrs = ["generate_samples.h"], + deps = [ + "@drake//:drake_shared_library", + "//examples/sampling_c3/parameter_headers:sampling_c3_options", + "//examples/sampling_c3/parameter_headers:sampling_params", + "//multibody:utils", + "//multibody:geom_geom_collider", + "//common:math_utils", + "//common:update_context", + "//solvers:c3", + ], +) + +cc_library( + name = "reposition", + srcs = ["reposition.cc"], + hdrs = ["reposition.h"], + deps = [ + "//solvers:c3", + "//examples/sampling_c3/parameter_headers:sampling_c3_options", + "//examples/sampling_c3/parameter_headers:reposition_params", + "//common:math_utils", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "sampling_c3_utils", + srcs = ["sampling_c3_utils.cc"], + hdrs = ["sampling_c3_utils.h"], + deps = [ + "//common", + "//common:math_utils", + "@drake//:drake_shared_library", + ], +) + +py_binary( + name = "xbox_script", + srcs = ["xbox_script.py"], + data = [ + "@lcm//:lcm-python", + ], + main = "xbox_script.py", + deps = [ + "//lcmtypes:lcmtypes_robot_py", + ], +) diff --git a/examples/sampling_c3/README.md b/examples/sampling_c3/README.md new file mode 100644 index 0000000000..362d37e8d6 --- /dev/null +++ b/examples/sampling_c3/README.md @@ -0,0 +1,123 @@ +# Approximating Global Contact-Implicit MPC via Sampling and Local Complementarity +This is an implementation of our paper currently available on Arxiv. + +[[Project webpage](https://approximating-global-ci-mpc.github.io/)] [[Arxiv](https://arxiv.org/abs/2505.13350)] [[Supplemental video](https://youtu.be/rv9n8Uyvoh0)] + +## Abstract +> To achieve general-purpose dexterous manipulation, robots must rapidly devise and execute contact-rich behaviors. Existing model-based controllers are incapable of globally optimizing in real-time over the exponential number of possible contact sequences. Instead, recent progress in contact-implicit control has leveraged simpler models that, while still hybrid, make local approximations. However, the use of local models inherently limits the controller to only exploit nearby interactions, potentially requiring intervention to richly explore the space of possible contacts. We present a novel approach which leverages the strengths of local complementarity-based control in combination with low-dimensional, but global, sampling of possible end-effector locations. Our key insight is to consider a contact-free stage preceding a contact-rich stage at every control loop. Our algorithm, in parallel, samples end effector locations to which the contact-free stage can move the robot, then considers the cost predicted by contact-rich MPC local to each sampled location. The result is a globally-informed, contact-implicit controller capable of real-time dexterous manipulation. We demonstrate our controller on precise, non-prehensile manipulation of non-convex objects using a Franka Panda arm. + +## Bibtex +``` +@article{venkatesh2025approximating, + title={Approximating Global Contact-Implicit MPC via Sampling and Local Complementarity}, + author={Sharanya Venkatesh* and Bibit Bianchini* and Alp Aydinoglu and William Yang and Michael Posa}, + year={2025}, + journal={arXiv preprint arXiv:2505.13350}, + website={https://approximating-global-ci-mpc.github.io/} +} +``` + +## Simulation Experiments + +1. Start the procman script containing a list of relevant processes. There's a different one for each demo, currently the 3D jack or planar push T: +``` +bot-procman-sheriff -l examples/sampling_c3/jacktoy/franka_sim_jack.pmd # Jack example +bot-procman-sheriff -l examples/sampling_c3/push_t/franka_sim_t.pmd # T example +``` + +2. In the procman window, start the meshcat visualizer by starting the `visualizer` command under the `operator` group. The meshcat visualizer can be viewed by opening a browser and navigating to `localhost:7000`. + +3. The examples with the sampling C3 controller can be run using the script `script:start_experiment_no_logs`. Scripts are located in the top bar of the procman window. This script spawns three processes: + - `bazel-bin/examples/sampling_c3/franka_sim`: Simulated environment which takes in torques commands from `franka_osc` and publishes the state of the system via LCM on various channels. + - `bazel-bin/examples/sampling_c3/franka_osc_controller`: Low-level task-space controller that tracks task-space trajectories it receives from the MPC. + - `bazel-bin/examples/sampling_c3/franka_sampling_c3_controller`: Contact Implicit MPC controller that takes in the state of the system and publishes end effector trajectories to be tracked by the OSC. + +4. The simulator and controller can be stopped using the script `script:end_experiment`. + + +### MJPC Comparison +A comparison using the sampling based MPC controllers from the [MuJoCo MPC controllers](https://github.com/google-deepmind/mujoco_mpc) is currently implemented in `dairlib`'s `jack_mjpc` branch [here](https://github.com/DAIRLab/dairlib/tree/jack_mjpc). Bringing this code into `sampling_based_c3_public` is WIP. + +The MJPC comparison extracts out just the controller portion of the MJPC code base and runs in on the identical task in the Drake simulator. [Our MJPC fork](https://github.com/ebianchi/mujoco_mpc) needs to be installed for this to run properly. Instructions are WIP. + +## Hardware Experiments + +### Network Setup +We use a two computer setup with a Franka robot arm: + 1. Computer 1: Runs sampling C3 controller and [our fork of FoundationPose](https://github.com/dairlab/foundationpose) for object tracking. Requires a GPU to run FoundationPose. + - Connected via ethernet to Computer 2. + + 2. Computer 2: Runs our OSC, Franka drivers, and webcam recording. Requires a realtime kernel for communicating with the Franka. We rely on [drake-franka-driver](https://github.com/RobotLocomotion/drake-franka-driver), which works via LCM. Much thanks to the Drake developers who provided this! No ROS/ROS2 involved. + + 3. Ethernet connections: + - Computer 1 <--> Computer 2 with link-local network settings. + - Computer 2 <--> Franka control box with iPv4 manual network settings matching the Franka IP. + + 4. USB connections: + - RealSense for FoundationPose <--> Computer 1 + - USB webcams for experiment recording <--> Computer 2 + +### Franka Driver: Installing `drake-franka-driver` +``` +git clone https://github.com/RobotLocomotion/drake-franka-driver +cd drake-franka-driver +bazel build ... +``` + +### Object Pose Estimation via FoundationPose +[Our fork of FoundationPose](https://github.com/dairlab/foundationpose) handles converting the object pose estimate to world coordinates given access to a camera extrinsic calibration, and it publishes these poses over LCM. For camera calibration, see [the process documented here](https://github.com/ebianchi/ci_mpc_utils/tree/main?tab=readme-ov-file#camera-calibration). + +### Running Experiments + +1. Start the procman script containing a list of relevant processes. The primary differences from the simulation `franka_sim_jack.pmd`/`franka_sim_t.pmd` scripts are the lcm_channels and the drake-franka-driver and corresponding translators to communicate with the Franka via LCM. + + - In the root of dairlib on Computer 1, start the procman sheriff (can use `jack` or `t` examples): + + ``` + export LCM_DEFAULT_URL=udpm://239.255.76.67:7667?ttl=1 + bot-procman-sheriff -l examples/sampling_c3/jacktoy/franka_hardware_jack.pmd + ``` + + - In the root of dairlib on Computer 1, start the procman deputy: + + ``` + export LCM_DEFAULT_URL=udpm://239.255.76.67:7667?ttl=1 # required for python xbox script + bot-procman-deputy -n sampling_c3_localhost --lcmurl=udpm://239.255.76.67:7667?ttl=1 + ``` + + - In the root of drake-franka-driver on Computer 2, start the procman deputy for the Franka driver: + + ``` + bot-procman-deputy -n drake_franka_driver --lcmurl=udpm://239.255.76.67:7667?ttl=1 + ``` + + - In the root of dairlib on Computer 2, start the procman deputy for the OSC: + + ``` + bot-procman-deputy -n franka_control --lcmurl=udpm://239.255.76.67:7667?ttl=1 + ``` + +2. Start object tracking with FoundationPose. Can use `jack` or `t` system names. + ``` + bash docker/run_container.sh + cd FoundationPose + python run_live_demo.py --debug=0 --system=jack + ``` + +3. In the procman window, start the operator processes (meshcat visualizer and xbox controller) using the script `script:start_operator_commands`. Scripts are located in the top bar of the procman window. The meshcat visualizer can be viewed by opening a browser and navigating to `localhost:7000` + +4. Run the script `script:init_experiment` to slowly move the Franka to an initial configuration. + +5. Begin the experiment using the script `script:start_experiment`. This spawns the following processes: + - `start_logging.py`: Starts a lcm-logger with an automatic naming convention for the log number. + - `record_video.py`: Streams all available webcams to a .mp4 file corresponding to the log number. + - `torque_driver`: `drake-franka-driver` in torque control mode. + - `franka_driver_`(in/out): communicates with `drake-franka-driver` to receive/publish franka state information and torque commands. This is just a translator between Drake's Franka Panda specific lcm messages and the standardized robot commands that we use. + - `bazel-bin/examples/sampling_c3/franka_osc_controller`: Low-level task-space controller that tracks task-space trajectories it receives from the MPC. + - `bazel-bin/examples/sampling_c3/franka_sampling_c3_controller`: Contact Implicit MPC controller that takes in the state of the system and publishes end effector trajectories to be tracked by the OSC. + - `bazel-bin/examples/sampling_c3/xbox_script`: This gets started with `script:start_operator_commands` but gets restarted with `script:start_experiment` to ensure the experiment starts in teleop mode as a safety precaution. + +6. Using the xbox controller, switch from tracking the teleop commands to the MPC plan by pressing "A". Do this after checking that the plans look reasonable in the visualizer. +7. Stop the experiment using `script:stop_experiment`. This also stops logging and recording. + + diff --git a/examples/sampling_c3/c3_mode_visualizer.cc b/examples/sampling_c3/c3_mode_visualizer.cc new file mode 100644 index 0000000000..d282e6a12d --- /dev/null +++ b/examples/sampling_c3/c3_mode_visualizer.cc @@ -0,0 +1,88 @@ +#include "examples/sampling_c3/c3_mode_visualizer.h" + +#include "lcm/lcm_trajectory.h" +#include "systems/framework/timestamped_vector.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +using drake::systems::Context; +using systems::TimestampedVector; + +namespace systems { + +C3ModeVisualizer::C3ModeVisualizer() { + this->set_name("C3ModeVisualizer"); + + is_c3_mode_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj: is_c3_mode_input", + drake::Value{}) + .get_index(); + + // 19 is the hardcoded size of the current lcs state vector. Alternatively, + // pass in the plant and read the size from there. + curr_lcs_state_ = this->DeclareVectorInputPort( + "curr_lcs_state", TimestampedVector(19)).get_index(); + + // Output c3_mode indicator for visualization. + c3_mode_visualization_traj_port_ = + this->DeclareAbstractOutputPort( + "mode_visualization_traj", dairlib::lcmt_timestamped_saved_traj(), + &C3ModeVisualizer::OutputC3ModeVisualization) + .get_index(); +} + +void C3ModeVisualizer::OutputC3ModeVisualization( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* c3_mode_visualization_traj) const { + // NOTE: LcmTrajectory needs at least two knot points, so create a dummy + // second knot also at the desired C3 mode visualization location. + Eigen::MatrixXd knots = Eigen::MatrixXd::Zero(3, 2); + + // Only update trajectory if the C3 mode input port has received data. + if (this->EvalInputValue( + context, is_c3_mode_input_port_)->utime > 1e-3) { + // Evaluate input port to get the sample locations as an + // lcmt_timestamped_saved_traj. + const auto& is_c3_mode_lcmt = + this->EvalInputValue( + context, is_c3_mode_input_port_); + + const TimestampedVector* curr_lcs_state = + (TimestampedVector*)this->EvalVectorInput( + context, curr_lcs_state_); + + auto is_c3_mode_lcm_obj = LcmTrajectory(is_c3_mode_lcmt->saved_traj); + auto is_c3_mode_traj = is_c3_mode_lcm_obj.GetTrajectory("is_c3_mode"); + auto is_c3_mode = is_c3_mode_traj.datapoints; + + // If we are in C3 mode, visualize the current EE location. + if (is_c3_mode(0)) { + knots.col(0) = curr_lcs_state->get_data().head(3); + knots.col(1) = curr_lcs_state->get_data().head(3); + } + } + + Eigen::VectorXd timestamp = Eigen::VectorXd::Zero(2); + timestamp(0) = context.get_time(); + timestamp(1) = context.get_time() + 1e-3; + + LcmTrajectory::Trajectory c3_mode; + c3_mode.traj_name = "c3_mode_visualization"; + c3_mode.datatypes = std::vector(3, "double"); + c3_mode.datapoints = knots; + c3_mode.time_vector = timestamp.cast(); + + LcmTrajectory c3_mode_traj( + {c3_mode}, {"c3_mode_visualization"}, "c3_mode_visualization", + "c3_mode_visualization", false); + + // Output as lcmt_timestamped_saved_traj + c3_mode_visualization_traj->saved_traj = c3_mode_traj.GenerateLcmObject(); + c3_mode_visualization_traj->utime = context.get_time() * 1e6; +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/c3_mode_visualizer.h b/examples/sampling_c3/c3_mode_visualizer.h new file mode 100644 index 0000000000..0e4a53cf14 --- /dev/null +++ b/examples/sampling_c3/c3_mode_visualizer.h @@ -0,0 +1,49 @@ +#include + +#include +#include +#include "dairlib/lcmt_saved_traj.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// A Drake system in the visualizer diagram for outputting a trajectory +/// (lcmt_timestamped_saved_traj) based on the current C3 mode. When in C3 +/// mode, the trajectory follows the EE location (to highlight the EE in pink). +/// When not in C3 mode, the trajectory is at the origin (to hide the pink EE in +/// the visualizer). +class C3ModeVisualizer : public drake::systems::LeafSystem { + public: + C3ModeVisualizer(); + + // Input ports + const drake::systems::InputPort& get_input_port_is_c3_mode() const { + return this->get_input_port(is_c3_mode_input_port_); + } + + const drake::systems::InputPort& get_input_port_curr_lcs_state() + const { + return this->get_input_port(curr_lcs_state_); + } + + // Output port + const drake::systems::OutputPort& + get_output_port_c3_mode_visualization_traj() const { + return this->get_output_port(c3_mode_visualization_traj_port_); + } + + private: + void OutputC3ModeVisualization( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* c3_mode_visualization_traj) const; + + drake::systems::InputPortIndex is_c3_mode_input_port_; + drake::systems::InputPortIndex curr_lcs_state_; + drake::systems::OutputPortIndex c3_mode_visualization_traj_port_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/franka_bridge_driver_in.cc b/examples/sampling_c3/franka_bridge_driver_in.cc new file mode 100644 index 0000000000..7a7659d3ad --- /dev/null +++ b/examples/sampling_c3/franka_bridge_driver_in.cc @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/parameters/franka_drake_lcm_driver_channels.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "systems/franka_state_translator.h" +#include "multibody/multibody_utils.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +#include "drake/common/find_resource.h" + +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; + +using dairlib::systems::RobotInputReceiver; +using dairlib::systems::RobotOutputSender; +using dairlib::systems::SubvectorPassThrough; +using dairlib::systems::TimestampedVector; + + +DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=0", + "LCM URL with IP, port, and TTL settings"); +DEFINE_string(demo_name, "jacktoy", + "Demo within sampling_c3; used to find controller params file"); + +namespace dairlib { + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + std::string lcm_channels_file = controller_params.lcm_channels_hardware_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + FrankaDrakeLcmDriverChannels franka_driver_channel_params = + drake::yaml::LoadYamlFile( + controller_params.franka_driver_channels_file); + + DiagramBuilder builder; + + MultibodyPlant plant(0.0); + AddFrankaToPlant(&plant, nullptr, false, false); + plant.Finalize(); + + auto pos_map = multibody::MakeNameToPositionsMap(plant); + auto vel_map = multibody::MakeNameToVelocitiesMap(plant); + auto act_map = multibody::MakeNameToActuatorsMap(plant); + + auto pos_names = multibody::ExtractOrderedNamesFromMap(pos_map); + auto vel_names = multibody::ExtractOrderedNamesFromMap(vel_map); + auto act_names = multibody::ExtractOrderedNamesFromMap(act_map); + + /* -------------------------------------------------------------------------------------------*/ + drake::lcm::DrakeLcm lcm(FLAGS_lcm_url); + + auto franka_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + franka_driver_channel_params.franka_command_channel, &lcm, + 1.0 / 1000.0)); + auto franka_command_translator = + builder.AddSystem(); + + builder.Connect(*franka_command_translator, *franka_command_pub); + + auto owned_diagram = builder.Build(); + owned_diagram->set_name(("sampling_c3_franka_bridge_driver_in")); + + systems::LcmDrivenLoop loop( + &lcm, std::move(owned_diagram), franka_command_translator, + lcm_channel_params.franka_input_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + loop.Simulate(); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_bridge_driver_out.cc b/examples/sampling_c3/franka_bridge_driver_out.cc new file mode 100644 index 0000000000..2b8570d2c5 --- /dev/null +++ b/examples/sampling_c3/franka_bridge_driver_out.cc @@ -0,0 +1,108 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/parameters/franka_drake_lcm_driver_channels.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "systems/franka_state_translator.h" +#include "multibody/multibody_utils.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" + +#include "drake/common/find_resource.h" + +using drake::math::RigidTransform; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::Simulator; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; + +using dairlib::systems::RobotInputReceiver; +using dairlib::systems::RobotOutputSender; +using dairlib::systems::SubvectorPassThrough; +using dairlib::systems::TimestampedVector; + + +// NOTE: While most module's TTL is set to 0 by default, this one is set to 1 +// since it necessarily needs to communicate with the Franka. +DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=1", + "LCM URL with IP, port, and TTL settings"); +DEFINE_string( + demo_name, "jacktoy", + "Name for the demo, used for finding the sub-example-specific files."); + +namespace dairlib { + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + std::string lcm_channels_file = controller_params.lcm_channels_hardware_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + FrankaDrakeLcmDriverChannels franka_driver_channel_params = + drake::yaml::LoadYamlFile( + controller_params.franka_driver_channels_file); + + DiagramBuilder builder; + + MultibodyPlant plant(0.0); + + Parser parser(&plant); + AddFrankaToPlant(&plant, nullptr, false, false); + plant.Finalize(); + + auto pos_map = multibody::MakeNameToPositionsMap(plant); + auto vel_map = multibody::MakeNameToVelocitiesMap(plant); + auto act_map = multibody::MakeNameToActuatorsMap(plant); + + auto pos_names = multibody::ExtractOrderedNamesFromMap(pos_map); + auto vel_names = multibody::ExtractOrderedNamesFromMap(vel_map); + auto act_names = multibody::ExtractOrderedNamesFromMap(act_map); + + /* -------------------------------------------------------------------------*/ + drake::lcm::DrakeLcm lcm(FLAGS_lcm_url); + + auto franka_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_state_channel, &lcm, 1.0 / 1000.0)); + auto franka_state_translator = + builder.AddSystem( + pos_names, vel_names, act_names); + + builder.Connect(*franka_state_translator, *franka_state_pub); + + auto owned_diagram = builder.Build(); + owned_diagram->set_name(("sampling_c3_franka_bridge_driver_out")); + + systems::LcmDrivenLoop loop( + &lcm, std::move(owned_diagram), franka_state_translator, + franka_driver_channel_params.franka_status_channel, true); + DrawAndSaveDiagramGraph(*loop.get_diagram()); + loop.Simulate(); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_joint_osc_controller.cc b/examples/sampling_c3/franka_joint_osc_controller.cc new file mode 100644 index 0000000000..866bd7196b --- /dev/null +++ b/examples/sampling_c3/franka_joint_osc_controller.cc @@ -0,0 +1,194 @@ + +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "examples/sampling_c3/parameter_headers/osc_params.h" +#include "systems/controllers/osc/end_effector_force.h" +#include "systems/controllers/osc/end_effector_orientation.h" +#include "systems/controllers/osc/end_effector_position.h" +#include "joint_trajectory_generator.h" +#include "lcm/lcm_trajectory.h" +#include "multibody/multibody_utils.h" +#include "systems/controllers/gravity_compensator.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/operational_space_control.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/lcm_trajectory_systems.h" + +#include "drake/common/find_resource.h" +#include "drake/common/yaml/yaml_io.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_interface_system.h" +#include "drake/systems/lcm/lcm_publisher_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" + +namespace dairlib { + +using drake::math::RigidTransform; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; +using std::string; + +using systems::controllers::JointSpaceTrackingData; + + +DEFINE_bool(is_simulation, true, "True for simulation, false for hardware"); +DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=0", + "LCM URL with IP, port, and TTL settings"); +DEFINE_string(demo_name, "jacktoy", + "Demo within sampling_c3; used to find controller params file"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::lcm::DrakeLcm lcm(FLAGS_lcm_url); + + // Load parameters. + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + SamplingC3OSCParams osc_params = + drake::yaml::LoadYamlFile( + controller_params.osc_params_file); + std::string lcm_channels_file = FLAGS_is_simulation ? + controller_params.lcm_channels_simulation_file : + controller_params.lcm_channels_hardware_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(controller_params.osc_qp_settings_file)) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + // Create a Franka-only plant. + drake::multibody::MultibodyPlant plant(0.0); + AddFrankaToPlant(&plant); + plant.Finalize(); + auto plant_context = plant.CreateDefaultContext(); + + // Piece together the diagram. + DiagramBuilder builder; + + auto state_receiver = builder.AddSystem(plant); + auto franka_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_input_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto osc_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto franka_command_sender = + builder.AddSystem(plant); + auto osc_command_sender = + builder.AddSystem(plant); + auto osc = builder.AddSystem( + plant, plant_context.get(), false); + if (osc_params.publish_debug_info) { + auto osc_debug_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_debug_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + builder.Connect(osc->get_output_port_osc_debug(), + osc_debug_pub->get_input_port()); + } + // WARNING: Hard-coded initial joint configurations for the robot in the + // sampling c3 experiments. + VectorXd target_position = VectorXd::Zero(7); + target_position << 2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08; + auto joint_traj_generator = + builder.AddSystem(plant, target_position); + std::vector> joint_tracking_data_vec; + std::vector joint_names = { + "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7"}; + for (int joint_idx = 0; joint_idx < joint_names.size(); ++joint_idx) { + string joint_name = joint_names[joint_idx]; + MatrixXd W = 1.0 * MatrixXd::Identity(1, 1); + MatrixXd K_p = 100 * MatrixXd::Identity(1, 1); + MatrixXd K_d = 5 * MatrixXd::Identity(1, 1); + joint_tracking_data_vec.push_back(std::make_unique( + joint_name + "_traj", K_p, K_d, W, plant, plant)); + joint_tracking_data_vec[joint_idx]->AddJointToTrack(joint_name, + joint_name + "dot"); + + osc->AddTrackingData(std::move(joint_tracking_data_vec[joint_idx])); + + builder.Connect(joint_traj_generator->get_output_port_joint(joint_idx), + osc->get_input_port_tracking_data(joint_name + "_traj")); + } + + osc->SetAccelerationCostWeights(osc_params.W_acceleration); + osc->SetInputCostWeights(osc_params.W_input_regularization); + osc->SetInputSmoothingCostWeights(osc_params.W_input_smoothing_regularization); + + osc->SetContactFriction(osc_params.mu); + osc->SetOsqpSolverOptions(solver_options); + + osc->Build(); + + if (osc_params.cancel_gravity_compensation) { + if (FLAGS_is_simulation) { + std::cerr<<"Sim OSC needs cancel_gravity_compensation: false"<(plant, + *plant_context); + builder.Connect(osc->get_output_port_osc_command(), + gravity_compensator->get_input_port()); + builder.Connect(gravity_compensator->get_output_port(), + franka_command_sender->get_input_port()); + } else { + if (!FLAGS_is_simulation) { + std::cerr<<"HW OSC needs cancel_gravity_compensation: true"<get_output_port_osc_command(), + franka_command_sender->get_input_port(0)); + } + + builder.Connect(franka_command_sender->get_output_port(), + franka_command_pub->get_input_port()); + builder.Connect(osc_command_sender->get_output_port(), + osc_command_pub->get_input_port()); + builder.Connect(osc->get_output_port_osc_command(), + osc_command_sender->get_input_port(0)); + builder.Connect(state_receiver->get_output_port(0), + osc->get_input_port_robot_output()); + builder.Connect(state_receiver->get_output_port(0), + joint_traj_generator->get_input_port_robot_state()); + + auto owned_diagram = builder.Build(); + owned_diagram->set_name(("sampling_c3_franka_joint_osc_controller")); + DrawAndSaveDiagramGraph(*owned_diagram); + // Run lcm-driven simulation + systems::LcmDrivenLoop loop( + &lcm, std::move(owned_diagram), state_receiver, + lcm_channel_params.franka_state_channel, true); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_osc_controller.cc b/examples/sampling_c3/franka_osc_controller.cc new file mode 100644 index 0000000000..871d3dca53 --- /dev/null +++ b/examples/sampling_c3/franka_osc_controller.cc @@ -0,0 +1,277 @@ + +#include +#include +#include + +#include "common/eigen_utils.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "examples/sampling_c3/parameter_headers/osc_params.h" +#include "systems/controllers/osc/end_effector_force.h" +#include "systems/controllers/osc/end_effector_orientation.h" +#include "systems/controllers/osc/end_effector_position.h" +#include "lcm/lcm_trajectory.h" +#include "multibody/multibody_utils.h" +#include "systems/controllers/gravity_compensator.h" +#include "systems/controllers/osc/external_force_tracking_data.h" +#include "systems/controllers/osc/joint_space_tracking_data.h" +#include "systems/controllers/osc/operational_space_control.h" +#include "systems/controllers/osc/relative_translation_tracking_data.h" +#include "systems/controllers/osc/rot_space_tracking_data.h" +#include "systems/controllers/osc/trans_space_tracking_data.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/robot_lcm_systems.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/lcm_trajectory_systems.h" + +#include "drake/common/find_resource.h" +#include "drake/common/yaml/yaml_io.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_interface_system.h" +#include "drake/systems/lcm/lcm_publisher_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" + +namespace dairlib { + +using drake::math::RigidTransform; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; + +using systems::controllers::ExternalForceTrackingData; +using systems::controllers::JointSpaceTrackingData; +using systems::controllers::RelativeTranslationTrackingData; +using systems::controllers::RotTaskSpaceTrackingData; +using systems::controllers::TransTaskSpaceTrackingData; + +DEFINE_bool(is_simulation, true, "True for simulation, false for hardware"); +DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=0", + "LCM URL with IP, port, and TTL settings"); +DEFINE_string(demo_name, "jacktoy", + "Demo within sampling_c3; used to find controller params file"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::lcm::DrakeLcm lcm(FLAGS_lcm_url); + + // Load parameters. + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + SamplingC3OSCParams osc_params = + drake::yaml::LoadYamlFile( + controller_params.osc_params_file); + std::string lcm_channels_file = FLAGS_is_simulation ? + controller_params.lcm_channels_simulation_file : + controller_params.lcm_channels_hardware_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + drake::solvers::SolverOptions solver_options = + drake::yaml::LoadYamlFile( + FindResourceOrThrow(controller_params.osc_qp_settings_file)) + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + // Create a Franka-only plant. + drake::multibody::MultibodyPlant plant(0.0); + AddFrankaToPlant(&plant); + plant.Finalize(); + auto plant_context = plant.CreateDefaultContext(); + + // Piece together the diagram. + DiagramBuilder builder; + + auto state_receiver = builder.AddSystem(plant); + auto end_effector_trajectory_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.tracking_trajectory_actor_channel, &lcm)); + auto end_effector_position_receiver = + builder.AddSystem( + "end_effector_position_target"); + auto end_effector_force_receiver = + builder.AddSystem( + "end_effector_force_target"); + auto end_effector_orientation_receiver = + builder.AddSystem( + "end_effector_orientation_target"); + auto franka_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.franka_input_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto osc_command_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto franka_command_sender = + builder.AddSystem(plant); + auto osc_command_sender = + builder.AddSystem(plant); + auto end_effector_trajectory = + builder.AddSystem( + plant, plant_context.get(), osc_params.neutral_position, + osc_params.teleop_neutral_position, kEndEffectorName); + end_effector_trajectory->SetRemoteControlParameters( + osc_params.neutral_position, osc_params.x_scale, + osc_params.y_scale, osc_params.z_scale); + auto end_effector_orientation_trajectory = + builder.AddSystem(); + end_effector_orientation_trajectory->SetTrackOrientation( + osc_params.track_end_effector_orientation); + auto end_effector_force_trajectory = + builder.AddSystem(); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + auto osc = builder.AddSystem( + plant, plant_context.get(), false); + if (osc_params.publish_debug_info) { + auto osc_debug_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.osc_debug_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + builder.Connect(osc->get_output_port_osc_debug(), + osc_debug_pub->get_input_port()); + } + + auto end_effector_position_tracking_data = + std::make_unique( + "end_effector_target", osc_params.K_p_end_effector, + osc_params.K_d_end_effector, osc_params.W_end_effector, + plant, plant); + end_effector_position_tracking_data->AddPointToTrack(kEndEffectorName); + const VectorXd& end_effector_acceleration_limits = + osc_params.end_effector_acceleration * Vector3d::Ones(); + end_effector_position_tracking_data->SetCmdAccelerationBounds( + -end_effector_acceleration_limits, end_effector_acceleration_limits); + auto mid_link_position_tracking_data_for_rel = + std::make_unique( + "panda_joint2_target", osc_params.K_p_mid_link, + osc_params.K_d_mid_link, osc_params.W_mid_link, plant, + plant); + mid_link_position_tracking_data_for_rel->AddJointToTrack("panda_joint2", + "panda_joint2dot"); + + auto end_effector_force_tracking_data = + std::make_unique( + "end_effector_force", osc_params.W_ee_lambda, plant, plant, + kEndEffectorName, Vector3d::Zero()); + + auto end_effector_orientation_tracking_data = + std::make_unique( + "end_effector_orientation_target", + osc_params.K_p_end_effector_rot, + osc_params.K_d_end_effector_rot, + osc_params.W_end_effector_rot, plant, plant); + end_effector_orientation_tracking_data->AddFrameToTrack(kEndEffectorName); + Eigen::VectorXd orientation_target = Eigen::VectorXd::Zero(4); + orientation_target(0) = 1; + osc->AddTrackingData(std::move(end_effector_position_tracking_data)); + // Since the Franka has 7 joints to control a 6 DOF EE command, add an + // additional tracking objective for joint 2 at a good configuration for the + // sampling C3 experiments. 1.1 joint target empirically works well. + osc->AddConstTrackingData(std::move(mid_link_position_tracking_data_for_rel), + 1.1 * VectorXd::Ones(1)); + osc->AddTrackingData(std::move(end_effector_orientation_tracking_data)); + osc->AddForceTrackingData(std::move(end_effector_force_tracking_data)); + osc->SetAccelerationCostWeights(osc_params.W_acceleration); + osc->SetInputCostWeights(osc_params.W_input_regularization); + osc->SetInputSmoothingCostWeights(osc_params.W_input_smoothing_regularization); + if (osc_params.enforce_acceleration_constraints) { + osc->EnableAccelerationConstraints(); + } else { + osc->DisableAccelerationConstraints(); + } + osc->SetContactFriction(osc_params.mu); + osc->SetOsqpSolverOptions(solver_options); + + osc->Build(); + + if (osc_params.cancel_gravity_compensation) { + if (FLAGS_is_simulation) { + std::cerr<<"Sim OSC needs cancel_gravity_compensation: false"<(plant, + *plant_context); + builder.Connect(osc->get_output_port_osc_command(), + gravity_compensator->get_input_port()); + builder.Connect(gravity_compensator->get_output_port(), + franka_command_sender->get_input_port()); + } else { + if (!FLAGS_is_simulation) { + std::cerr<<"HW OSC needs cancel_gravity_compensation: true"<get_output_port_osc_command(), + franka_command_sender->get_input_port(0)); + } + + builder.Connect(radio_sub->get_output_port(0), + end_effector_trajectory->get_input_port_radio()); + builder.Connect(radio_sub->get_output_port(0), + end_effector_orientation_trajectory->get_input_port_radio()); + builder.Connect(radio_sub->get_output_port(0), + end_effector_force_trajectory->get_input_port_radio()); + builder.Connect(franka_command_sender->get_output_port(), + franka_command_pub->get_input_port()); + builder.Connect(osc_command_sender->get_output_port(), + osc_command_pub->get_input_port()); + builder.Connect(osc->get_output_port_osc_command(), + osc_command_sender->get_input_port(0)); + + builder.Connect(state_receiver->get_output_port(0), + osc->get_input_port_robot_output()); + builder.Connect(end_effector_trajectory_sub->get_output_port(), + end_effector_position_receiver->get_input_port_trajectory()); + builder.Connect(end_effector_trajectory_sub->get_output_port(), + end_effector_force_receiver->get_input_port_trajectory()); + builder.Connect( + end_effector_trajectory_sub->get_output_port(), + end_effector_orientation_receiver->get_input_port_trajectory()); + builder.Connect(end_effector_position_receiver->get_output_port(0), + end_effector_trajectory->get_input_port_trajectory()); + builder.Connect(state_receiver->get_output_port(0), + end_effector_trajectory->get_input_port_state()); + builder.Connect( + end_effector_orientation_receiver->get_output_port(0), + end_effector_orientation_trajectory->get_input_port_trajectory()); + builder.Connect(end_effector_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_target")); + builder.Connect( + end_effector_orientation_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_orientation_target")); + builder.Connect(end_effector_force_receiver->get_output_port(0), + end_effector_force_trajectory->get_input_port_trajectory()); + builder.Connect(end_effector_force_trajectory->get_output_port(0), + osc->get_input_port_tracking_data("end_effector_force")); + + auto owned_diagram = builder.Build(); + std::shared_ptr> shared_diagram = std::move(owned_diagram); + shared_diagram->set_name(("sampling_c3_franka_osc_controller")); + DrawAndSaveDiagramGraph(*shared_diagram); + // Run lcm-driven simulation + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, state_receiver, + lcm_channel_params.franka_state_channel, true); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_sampling_c3_controller.cc b/examples/sampling_c3/franka_sampling_c3_controller.cc new file mode 100644 index 0000000000..55db53192b --- /dev/null +++ b/examples/sampling_c3/franka_sampling_c3_controller.cc @@ -0,0 +1,540 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "goal_generator.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "multibody/multibody_utils.h" +#include "solvers/lcs_factory.h" +#include "systems/controllers/sampling_based_c3_controller.h" +#include "systems/framework/lcm_driven_loop.h" +#include "systems/franka_kinematics.h" +#include "systems/robot_lcm_systems.h" +#include "systems/senders/c3_state_sender.h" +#include "systems/senders/sample_buffer_sender.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/c3_output_systems.h" + +namespace dairlib { + +using dairlib::solvers::LCSFactory; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Diagram; +using drake::systems::DiagramBuilder; +using drake::systems::TriggerType; +using drake::systems::TriggerTypeSet; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using Eigen::MatrixXd; + +using Eigen::Vector3d; +using Eigen::VectorXd; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; +using std::vector; + + +DEFINE_bool(is_simulation, true, "True for simulation, false for hardware"); +DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=0", + "LCM URL with IP, port, and TTL settings"); +DEFINE_string(demo_name, "jacktoy", + "Demo within sampling_c3; used to find controller params file"); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + drake::lcm::DrakeLcm lcm(FLAGS_lcm_url); + + // Load parameters. + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + std::string lcm_channels_file = FLAGS_is_simulation ? + controller_params.lcm_channels_simulation_file : + controller_params.lcm_channels_hardware_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + + // Create a Franka-only plant. + MultibodyPlant plant_franka(0.0); + AddFrankaToPlant(&plant_franka); + plant_franka.Finalize(); + auto franka_context = plant_franka.CreateDefaultContext(); + + // Create an object-only plant. + MultibodyPlant plant_object(0.0); + AddObjectToPlant(&plant_object, nullptr, controller_params.object_model); + plant_object.Finalize(); + auto object_context = plant_object.CreateDefaultContext(); + + // Create the LCS plant containing a floating EE, object, and ground. + DiagramBuilder plant_lcs_builder; + auto [plant_lcs, scene_graph] = + AddMultibodyPlantSceneGraph(&plant_lcs_builder, 0.0); + AddLCSModelsToPlant(&plant_lcs, &scene_graph, controller_params.object_model, + controller_params.include_end_effector_orientation); + plant_lcs.Finalize(); + + std::unique_ptr> plant_lcs_autodiff = + drake::systems::System::ToAutoDiffXd(plant_lcs); + + auto plant_lcs_diagram = plant_lcs_builder.Build(); + std::unique_ptr> diagram_context = + plant_lcs_diagram->CreateDefaultContext(); + auto& plant_lcs_context = plant_lcs_diagram->GetMutableSubsystemContext( + plant_lcs, diagram_context.get()); + auto plant_lcs_context_ad = plant_lcs_autodiff->CreateDefaultContext(); + + // Build the contact pairs based on the demo. + std::vector>> contact_pairs; + std::vector> ee_contact_pairs; + std::vector> ground_object_contact_pairs; + std::unordered_map contact_geoms; + + // All demos include the end effector and ground. + drake::geometry::GeometryId ee_contact_points = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("end_effector_simple"))[0]; + drake::geometry::GeometryId ground_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("ground"))[0]; + contact_geoms["EE"] = ee_contact_points; + contact_geoms["GROUND"] = ground_geoms; + std::vector> ee_ground_contact{ + SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; + + if (FLAGS_demo_name == "jacktoy") { + drake::geometry::GeometryId capsule1_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_1"))[0]; + drake::geometry::GeometryId capsule2_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_2"))[0]; + drake::geometry::GeometryId capsule3_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_3"))[0]; + + drake::geometry::GeometryId capsule1_sphere1_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_1"))[1]; + drake::geometry::GeometryId capsule1_sphere2_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_1"))[2]; + drake::geometry::GeometryId capsule2_sphere1_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_2"))[1]; + drake::geometry::GeometryId capsule2_sphere2_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_2"))[2]; + drake::geometry::GeometryId capsule3_sphere1_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_3"))[1]; + drake::geometry::GeometryId capsule3_sphere2_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("capsule_3"))[2]; + + contact_geoms["CAPSULE_1"] = capsule1_geoms; + contact_geoms["CAPSULE_2"] = capsule2_geoms; + contact_geoms["CAPSULE_3"] = capsule3_geoms; + contact_geoms["CAPSULE_1_SPHERE_1"] = capsule1_sphere1_geoms; + contact_geoms["CAPSULE_1_SPHERE_2"] = capsule1_sphere2_geoms; + contact_geoms["CAPSULE_2_SPHERE_1"] = capsule2_sphere1_geoms; + contact_geoms["CAPSULE_2_SPHERE_2"] = capsule2_sphere2_geoms; + contact_geoms["CAPSULE_3_SPHERE_1"] = capsule3_sphere1_geoms; + contact_geoms["CAPSULE_3_SPHERE_2"] = capsule3_sphere2_geoms; + + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_1"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_2"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_3"])); + + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["CAPSULE_1_SPHERE_1"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["CAPSULE_1_SPHERE_2"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["CAPSULE_2_SPHERE_1"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["CAPSULE_2_SPHERE_2"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["CAPSULE_3_SPHERE_1"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["CAPSULE_3_SPHERE_2"], contact_geoms["GROUND"])); + } + else if (FLAGS_demo_name == "push_t") { + drake::geometry::GeometryId vertical_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[0]; + drake::geometry::GeometryId horizontal_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("horizontal_link"))[0]; + + drake::geometry::GeometryId top_left_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[1]; + drake::geometry::GeometryId top_right_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[2]; + drake::geometry::GeometryId bottom_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[3]; + + contact_geoms["VERTICAL_LINK"] = vertical_geoms; + contact_geoms["HORIZONTAL_LINK"] = horizontal_geoms; + contact_geoms["TOP_LEFT_SPHERE"] = top_left_sphere_geoms; + contact_geoms["TOP_RIGHT_SPHERE"] = top_right_sphere_geoms; + contact_geoms["BOTTOM_SPHERE"] = bottom_sphere_geoms; + + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["HORIZONTAL_LINK"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["VERTICAL_LINK"])); + + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["TOP_LEFT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["TOP_RIGHT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["BOTTOM_SPHERE"], contact_geoms["GROUND"])); + } + else { + throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); + } + // Order: EE-ground, EE-object, object-ground. + contact_pairs.push_back(ee_ground_contact); + contact_pairs.push_back(ee_contact_pairs); + contact_pairs.push_back(ground_object_contact_pairs); + + // Piece together the diagram. + DiagramBuilder builder; + + auto object_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.object_state_channel, &lcm)); + auto franka_state_receiver = + builder.AddSystem(plant_franka); + auto object_state_receiver = + builder.AddSystem(plant_object); + auto radio_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.radio_channel, &lcm)); + auto reduced_order_model_receiver = + builder.AddSystem( + plant_franka, franka_context.get(), plant_object, + object_context.get(), kEndEffectorName, + controller_params.object_body_name, + controller_params.include_end_effector_orientation); + + // Select the target generator based on the demo. + std::unique_ptr target_generator; + if (FLAGS_demo_name == "jacktoy") { + target_generator = + std::make_unique( + plant_object, controller_params.goal_params); + } else if (FLAGS_demo_name == "push_t") { + target_generator = + std::make_unique( + plant_object, controller_params.goal_params); + } else { + throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); + } + auto* control_target = builder.AddSystem(std::move(target_generator)); + + // Input sizes are EE position (3), object pose (7), EE velocity (3), object + // velocities (6). + std::vector input_sizes = {3, 7, 3, 6}; + auto target_state_mux = + builder.AddSystem(input_sizes); + auto final_target_state_mux = + builder.AddSystem(input_sizes); + auto end_effector_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(3)); + auto object_zero_velocity_source = + builder.AddSystem( + VectorXd::Zero(6)); + auto target_gen_info_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.target_generator_info_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + builder.Connect(control_target->get_output_port_end_effector_target(), + target_state_mux->get_input_port(0)); + builder.Connect(control_target->get_output_port_object_target(), + target_state_mux->get_input_port(1)); + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + target_state_mux->get_input_port(2)); + builder.Connect(control_target->get_output_port_object_velocity_target(), + target_state_mux->get_input_port(3)); + builder.Connect(control_target->get_output_port_target_gen_info(), + target_gen_info_publisher->get_input_port()); + builder.Connect(control_target->get_output_port_end_effector_target(), + final_target_state_mux->get_input_port(0)); + builder.Connect(control_target->get_output_port_object_final_target(), + final_target_state_mux->get_input_port(1)); + builder.Connect(end_effector_zero_velocity_source->get_output_port(), + final_target_state_mux->get_input_port(2)); + builder.Connect(object_zero_velocity_source->get_output_port(), + final_target_state_mux->get_input_port(3)); + + // Sampling C3 controller. + auto controller = builder.AddSystem( + plant_lcs, &plant_lcs_context, *plant_lcs_autodiff, + plant_lcs_context_ad.get(), contact_pairs, controller_params); + + // Systems for publishing the current and best planned trajectories. + auto actor_trajectory_sender_curr_plan = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_actor_curr_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto object_trajectory_sender_curr_plan = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_object_curr_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto actor_trajectory_sender_best_plan = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_actor_best_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto object_trajectory_sender_best_plan = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_object_best_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + // C3 senders. + auto c3_output_sender_curr_plan = + builder.AddNamedSystem("c3_output_sender_curr_plan", + std::make_unique()); + auto c3_output_publisher_curr_plan = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_debug_output_curr_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_forces_publisher_curr_plan = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_force_curr_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + auto c3_output_sender_best_plan = + builder.AddNamedSystem("c3_output_sender_best_plan", + std::make_unique()); + auto c3_output_publisher_best_plan = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_debug_output_best_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_forces_publisher_best_plan = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_force_best_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + // Systems for publishing the tracking output. + auto actor_c3_execution_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.c3_trajectory_exec_actor_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto actor_repos_execution_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.repos_trajectory_exec_actor_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto actor_tracking_trajectory_sender = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.tracking_trajectory_actor_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + // Sample-related senders/publishers. + auto sample_buffer_sender = builder.AddSystem( + controller_params.sampling_params.N_sample_buffer, + plant_lcs.num_positions()); + auto sample_buffer_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.sample_buffer_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto sample_locations_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.sample_locations_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto sample_costs_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.sample_costs_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + // Debugging publishers. + auto controller_debug_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.sampling_c3_debug_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto is_c3_mode_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.is_c3_mode_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + // Dynamically feasible plan publishers. + auto dynamically_feasible_curr_plan_actor_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.dynamically_feasible_curr_actor_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto dynamically_feasible_curr_plan_object_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.dynamically_feasible_curr_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto dynamically_feasible_best_plan_actor_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.dynamically_feasible_best_actor_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto dynamically_feasible_best_plan_object_publisher = builder.AddSystem( + LcmPublisherSystem::Make( + lcm_channel_params.dynamically_feasible_best_plan_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + std::vector state_names = { + "end_effector_x", "end_effector_y", "end_effector_z", + "object_qw", "object_qx", "object_qy", "object_qz", + "object_x", "object_y", "object_z", + "end_effector_vx", "end_effector_vy", "end_effector_vz", + "object_wx" , "object_wy", "object_wz", + "object_vz", "object_vz", "object_vz", + }; + // C3 state senders: actual, target, and final target. + auto c3_state_sender = builder.AddSystem( + plant_lcs.num_positions() + plant_lcs.num_velocities(), + state_names); + auto c3_target_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_target_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_actual_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_actual_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + auto c3_final_target_state_publisher = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.c3_final_target_state_channel, &lcm, + TriggerTypeSet({TriggerType::kForced}))); + + builder.Connect(franka_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_franka_state()); + builder.Connect(object_state_sub->get_output_port(), + object_state_receiver->get_input_port()); + builder.Connect(object_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_object_state()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + controller->get_input_port_lcs_state()); + builder.Connect(object_state_receiver->get_output_port(), + control_target->get_input_port_object_state()); + builder.Connect(target_state_mux->get_output_port(), + controller->get_input_port_target()); + builder.Connect(final_target_state_mux->get_output_port(), + controller->get_input_port_final_target()); + builder.Connect(radio_sub->get_output_port(), + controller->get_input_port_radio()); + builder.Connect(radio_sub->get_output_port(), + control_target->get_input_port_radio()); + builder.Connect(controller->get_output_port_c3_solution_curr_plan_actor(), + actor_trajectory_sender_curr_plan->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution_curr_plan_object(), + object_trajectory_sender_curr_plan->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution_best_plan_actor(), + actor_trajectory_sender_best_plan->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution_best_plan_object(), + object_trajectory_sender_best_plan->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution_curr_plan(), + c3_output_sender_curr_plan->get_input_port_c3_solution()); + builder.Connect(controller->get_output_port_c3_intermediates_curr_plan(), + c3_output_sender_curr_plan->get_input_port_c3_intermediates()); + builder.Connect(controller->get_output_port_lcs_contact_jacobian_curr_plan(), + c3_output_sender_curr_plan->get_input_port_lcs_contact_info()); + builder.Connect(c3_output_sender_curr_plan->get_output_port_c3_debug(), + c3_output_publisher_curr_plan->get_input_port()); + builder.Connect(c3_output_sender_curr_plan->get_output_port_c3_force(), + c3_forces_publisher_curr_plan->get_input_port()); + builder.Connect(controller->get_output_port_c3_solution_best_plan(), + c3_output_sender_best_plan->get_input_port_c3_solution()); + builder.Connect(controller->get_output_port_c3_intermediates_best_plan(), + c3_output_sender_best_plan->get_input_port_c3_intermediates()); + builder.Connect(controller->get_output_port_lcs_contact_jacobian_best_plan(), + c3_output_sender_best_plan->get_input_port_lcs_contact_info()); + builder.Connect(c3_output_sender_best_plan->get_output_port_c3_debug(), + c3_output_publisher_best_plan->get_input_port()); + builder.Connect(c3_output_sender_best_plan->get_output_port_c3_force(), + c3_forces_publisher_best_plan->get_input_port()); + builder.Connect(controller->get_output_port_dynamically_feasible_curr_plan_actor(), + dynamically_feasible_curr_plan_actor_publisher->get_input_port()); + builder.Connect(controller->get_output_port_dynamically_feasible_best_plan_actor(), + dynamically_feasible_best_plan_actor_publisher->get_input_port()); + builder.Connect(controller->get_output_port_dynamically_feasible_curr_plan_object(), + dynamically_feasible_curr_plan_object_publisher->get_input_port()); + builder.Connect(controller->get_output_port_dynamically_feasible_curr_plan_object(), + dynamically_feasible_best_plan_object_publisher->get_input_port()); + builder.Connect(target_state_mux->get_output_port(), + c3_state_sender->get_input_port_target_state()); + builder.Connect(final_target_state_mux->get_output_port(), + c3_state_sender->get_input_port_final_target_state()); + builder.Connect(reduced_order_model_receiver->get_output_port_lcs_state(), + c3_state_sender->get_input_port_actual_state()); + builder.Connect(c3_state_sender->get_output_port_target_c3_state(), + c3_target_state_publisher->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_final_target_c3_state(), + c3_final_target_state_publisher->get_input_port()); + builder.Connect(c3_state_sender->get_output_port_actual_c3_state(), + c3_actual_state_publisher->get_input_port()); + builder.Connect(controller->get_output_port_c3_traj_execute_actor(), + actor_c3_execution_trajectory_sender->get_input_port()); + builder.Connect(controller->get_output_port_repos_traj_execute_actor(), + actor_repos_execution_trajectory_sender->get_input_port()); + builder.Connect(controller->get_output_port_traj_execute_actor(), + actor_tracking_trajectory_sender->get_input_port()); + builder.Connect(controller->get_output_port_all_sample_locations(), + sample_locations_publisher->get_input_port()); + builder.Connect(controller->get_output_port_all_sample_costs(), + sample_costs_publisher->get_input_port()); + builder.Connect(controller->get_output_port_is_c3_mode(), + is_c3_mode_publisher->get_input_port()); + builder.Connect(controller->get_output_port_debug(), + controller_debug_publisher->get_input_port()); + builder.Connect(sample_buffer_sender->get_output_port_sample_buffer(), + sample_buffer_publisher->get_input_port()); + builder.Connect(controller->get_output_port_sample_buffer_configurations(), + sample_buffer_sender->get_input_port_samples()); + builder.Connect(controller->get_output_port_sample_buffer_costs(), + sample_buffer_sender->get_input_port_sample_costs()); + + auto owned_diagram = builder.Build(); + owned_diagram->set_name(("sampling_c3_controller_" + FLAGS_demo_name)); + plant_lcs_diagram->set_name(("sampling_c3_lcs_plant" + FLAGS_demo_name)); + DrawAndSaveDiagramGraph(*owned_diagram); + DrawAndSaveDiagramGraph(*plant_lcs_diagram); + + // Run lcm-driven simulation. The buffer size argument is needed to ensure + // the latest messages are used in the control loop. See + // https://github.com/DAIRLab/dairlib/pull/366 for more details. + int lcm_buffer_size = 200; + std::shared_ptr> shared_diagram = std::move(owned_diagram); + systems::LcmDrivenLoop loop( + &lcm, shared_diagram, franka_state_receiver, + lcm_channel_params.franka_state_channel, true, lcm_buffer_size); + LcmHandleSubscriptionsUntil( + &lcm, [&]() { return object_state_sub->GetInternalMessageCount() > 1; }); + loop.Simulate(); + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_sim.cc b/examples/sampling_c3/franka_sim.cc new file mode 100644 index 0000000000..8d92dc0d64 --- /dev/null +++ b/examples/sampling_c3/franka_sim.cc @@ -0,0 +1,139 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "examples/sampling_c3/parameter_headers/franka_sim_params.h" +#include "multibody/multibody_utils.h" +#include "systems/robot_lcm_systems.h" + +namespace dairlib { + +using dairlib::systems::SubvectorPassThrough; +using drake::geometry::GeometrySet; +using drake::geometry::SceneGraph; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::ModelInstanceIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::lcm::LcmPublisherSystem; +using drake::systems::lcm::LcmSubscriberSystem; +using drake::trajectories::PiecewisePolynomial; +using multibody::MakeNameToPositionsMap; +using multibody::MakeNameToVelocitiesMap; +using systems::AddActuationRecieverAndStateSenderLcm; + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + + +DEFINE_string(lcm_url, "udpm://239.255.76.67:7667?ttl=0", + "LCM URL with IP, port, and TTL settings"); +DEFINE_string(demo_name, "jacktoy", + "Name for the demo, used when building filepaths for output."); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + // Load parameters. + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + std::string lcm_channels_file = + controller_params.lcm_channels_simulation_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + FrankaSimParams sim_params = drake::yaml::LoadYamlFile( + controller_params.sim_params_file); + + // Build the simulation plant. + DiagramBuilder builder; + double sim_dt = sim_params.dt; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, sim_dt); + ModelInstanceIndex franka_index = AddFrankaToPlant(&plant, &scene_graph); + ModelInstanceIndex object_index = AddObjectToPlant(&plant, &scene_graph, + sim_params.object_model); + plant.Finalize(); + /* -------------------------------------------------------------------------------------------*/ + + drake::lcm::DrakeLcm drake_lcm(FLAGS_lcm_url); + auto lcm = + builder.AddSystem(&drake_lcm); + AddActuationRecieverAndStateSenderLcm( + &builder, plant, lcm, lcm_channel_params.franka_input_channel, + lcm_channel_params.franka_state_channel, sim_params.franka_publish_rate, + franka_index, sim_params.publish_efforts, sim_params.actuator_delay); + auto object_state_sender = + builder.AddSystem(plant, false, object_index); + auto object_state_pub = + builder.AddSystem(LcmPublisherSystem::Make( + lcm_channel_params.object_state_channel, lcm, + 1.0 / sim_params.object_publish_rate)); + + builder.Connect(plant.get_state_output_port(object_index), + object_state_sender->get_input_port_state()); + builder.Connect(object_state_sender->get_output_port(), + object_state_pub->get_input_port()); + + int nq = plant.num_positions(); + int nv = plant.num_velocities(); + + if (sim_params.visualize_drake_sim) { + drake::visualization::AddDefaultVisualization(&builder); + } + + auto diagram = builder.Build(); + + drake::systems::Simulator simulator(*diagram); + + simulator.set_publish_every_time_step(false); + simulator.set_publish_at_initialization(false); + simulator.set_target_realtime_rate(sim_params.realtime_rate); + + auto& plant_context = diagram->GetMutableSubsystemContext( + plant, &simulator.get_mutable_context()); + + VectorXd q = VectorXd::Zero(nq); + + q.head(plant.num_positions(franka_index)) = sim_params.q_init_franka; + q.tail(plant.num_positions(object_index)) = sim_params.q_init_object; + + plant.SetPositions(&plant_context, q); + + VectorXd v = VectorXd::Zero(nv); + plant.SetVelocities(&plant_context, v); + + simulator.Initialize(); + simulator.AdvanceTo(std::numeric_limits::infinity()); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/franka_visualizer.cc b/examples/sampling_c3/franka_visualizer.cc new file mode 100644 index 0000000000..9ab4b23f15 --- /dev/null +++ b/examples/sampling_c3/franka_visualizer.cc @@ -0,0 +1,544 @@ +#include +#include +#include +#include +#include + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "dairlib/lcmt_robot_output.hpp" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/c3_mode_visualizer.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/lcm_channels.h" +#include "examples/sampling_c3/parameter_headers/visualizer_params.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" +#include "examples/sampling_c3/parameter_headers/sampling_params.h" +#include "multibody/com_pose_system.h" +#include "multibody/multibody_utils.h" +#include "multibody/visualization_utils.h" +#include "systems/franka_kinematics.h" +#include "systems/primitives/subvector_pass_through.h" +#include "systems/robot_lcm_systems.h" +#include "systems/senders/sample_buffer_to_point_cloud.h" +#include "systems/system_utils.h" +#include "systems/trajectory_optimization/lcm_trajectory_systems.h" +#include "systems/visualization/lcm_visualization_systems.h" + +#include "drake/common/find_resource.h" +#include "drake/common/yaml/yaml_io.h" +#include "drake/geometry/drake_visualizer.h" +#include "drake/geometry/meshcat_point_cloud_visualizer.h" +#include "drake/geometry/meshcat_visualizer.h" +#include "drake/geometry/meshcat_visualizer_params.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/lcm/lcm_interface_system.h" +#include "drake/systems/lcm/lcm_subscriber_system.h" +#include "drake/systems/primitives/multiplexer.h" +#include "drake/systems/rendering/multibody_position_to_geometry_pose.h" + +namespace dairlib { + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using dairlib::systems::ObjectStateReceiver; +using dairlib::systems::RobotOutputReceiver; +using dairlib::systems::SubvectorPassThrough; +using drake::geometry::DrakeVisualizer; +using drake::geometry::MeshcatPointCloudVisualizer; +using drake::geometry::SceneGraph; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::multibody::ModelInstanceIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::multibody::UnitInertia; +using drake::systems::Simulator; +using drake::systems::lcm::LcmSubscriberSystem; +using drake::systems::rendering::MultibodyPositionToGeometryPose; + +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; + + +DEFINE_bool(is_simulation, true, "True for simulation, false for hardware"); +DEFINE_string(demo_name, "jacktoy", + "Name for the demo, used when building filepaths for output."); + +int do_main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + + // Load parameters. + std::string controller_params_path = "examples/sampling_c3/" + + FLAGS_demo_name + "/parameters/sampling_c3_controller_params.yaml"; + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + controller_params_path); + SamplingC3VisualizerParams vis_params = + drake::yaml::LoadYamlFile( + controller_params.vis_params_file); + SamplingC3Options sampling_c3_options = controller_params.sampling_c3_options; + SamplingParams sampling_params = controller_params.sampling_params; + std::string lcm_channels_file = FLAGS_is_simulation ? + controller_params.lcm_channels_simulation_file : + controller_params.lcm_channels_hardware_file; + SamplingC3LcmChannels lcm_channel_params = + drake::yaml::LoadYamlFile(lcm_channels_file); + + drake::systems::DiagramBuilder builder; + + SceneGraph& scene_graph = *builder.AddSystem(); + scene_graph.set_name("scene_graph"); + + // Build the visualizer plant. + MultibodyPlant plant(0.0); + ModelInstanceIndex franka_index = AddFrankaToPlant(&plant, &scene_graph); + ModelInstanceIndex object_index = AddObjectToPlant( + &plant, &scene_graph, vis_params.object_vis_model); + plant.Finalize(); + + // Create a Franka-only plant. + MultibodyPlant plant_franka(0.0); + AddFrankaToPlant(&plant_franka, nullptr, true, false); + plant_franka.Finalize(); + auto franka_context = plant_franka.CreateDefaultContext(); + + // Create an object-only plant. + MultibodyPlant plant_object(0.0); + AddObjectToPlant(&plant_object, nullptr, + vis_params.object_vis_model); + plant_object.Finalize(); + auto object_context = plant_object.CreateDefaultContext(); + + auto lcm = builder.AddSystem(); + auto franka_state_receiver = + builder.AddSystem(plant, franka_index); + auto object_state_receiver = + builder.AddSystem(plant, object_index); + auto franka_passthrough = builder.AddSystem( + franka_state_receiver->get_output_port(0).size(), 0, + plant.num_positions(franka_index)); + auto robot_time_passthrough = builder.AddSystem( + franka_state_receiver->get_output_port(0).size(), + franka_state_receiver->get_output_port(0).size() - 1, 1); + auto tray_passthrough = builder.AddSystem( + object_state_receiver->get_output_port(0).size(), 0, + plant.num_positions(object_index)); + + std::vector input_sizes = {plant.num_positions(franka_index), + plant.num_positions(object_index)}; + auto mux = + builder.AddSystem>(input_sizes); + auto reduced_order_model_receiver = + builder.AddSystem( + plant_franka, franka_context.get(), plant_object, + object_context.get(), kEndEffectorName, + controller_params.object_body_name, false); + builder.Connect(franka_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_franka_state()); + builder.Connect(object_state_receiver->get_output_port(), + reduced_order_model_receiver->get_input_port_object_state()); + + // LCM subscribers. + auto franka_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.franka_state_channel, lcm)); + auto object_state_sub = + builder.AddSystem(LcmSubscriberSystem::Make( + lcm_channel_params.object_state_channel, lcm)); + auto is_c3_mode_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.is_c3_mode_channel, lcm)); + + auto c3_execution_trajectory_sub_actor = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_trajectory_exec_actor_channel, lcm)); + auto repos_execution_trajectory_sub_actor = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.repos_trajectory_exec_actor_channel, lcm)); + + auto trajectory_sub_actor_curr = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_actor_curr_plan_channel, lcm)); + auto trajectory_sub_object_curr = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_object_curr_plan_channel, lcm)); + auto trajectory_sub_force_curr = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_force_curr_channel, lcm)); + auto dynamically_feasible_trajectory_sub_object_curr = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.dynamically_feasible_curr_plan_channel, lcm)); + auto dynamically_feasible_trajectory_sub_actor_curr = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.dynamically_feasible_curr_actor_plan_channel, + lcm)); + + auto trajectory_sub_actor_best = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_actor_best_plan_channel, lcm)); + auto trajectory_sub_object_best = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_object_best_plan_channel, lcm)); + auto trajectory_sub_force_best = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_force_best_channel, lcm)); + auto dynamically_feasible_trajectory_sub_object_best = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.dynamically_feasible_best_plan_channel, lcm)); + auto dynamically_feasible_trajectory_sub_actor_best = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.dynamically_feasible_best_actor_plan_channel, + lcm)); + + auto sample_location_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.sample_locations_channel, lcm)); + auto sample_buffer_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.sample_buffer_channel, lcm)); + auto sample_costs_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.sample_costs_channel, lcm)); + + auto c3_state_actual_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_actual_state_channel, lcm)); + auto c3_state_target_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_target_state_channel, lcm)); + auto c3_final_state_target_sub = builder.AddSystem( + LcmSubscriberSystem::Make( + lcm_channel_params.c3_final_target_state_channel, lcm)); + + auto to_pose = + builder.AddSystem>(plant); + + drake::geometry::MeshcatVisualizerParams params; + params.publish_period = 1.0 / vis_params.visualizer_publish_rate; + auto meshcat = std::make_shared(); + meshcat->SetCameraPose(vis_params.camera_pose, vis_params.camera_target); + + if (vis_params.visualize_c3_workspace) { + // Note: There are also robot radius limits which are not visualized. + double width = sampling_c3_options.workspace_limits[0][4] - + sampling_c3_options.workspace_limits[0][3]; // x + double depth = sampling_c3_options.workspace_limits[1][4] - + sampling_c3_options.workspace_limits[1][3]; // y + double height = sampling_c3_options.workspace_limits[2][4] - + sampling_c3_options.workspace_limits[2][3]; // z + Vector3d workspace_center = { + 0.5 * (sampling_c3_options.workspace_limits[0][4] + + sampling_c3_options.workspace_limits[0][3]), + 0.5 * (sampling_c3_options.workspace_limits[1][4] + + sampling_c3_options.workspace_limits[1][3]), + 0.5 * (sampling_c3_options.workspace_limits[2][4] + + sampling_c3_options.workspace_limits[2][3])}; + meshcat->SetObject("c3_workspace", + drake::geometry::Box(width, depth, height), + {0, 1, 0, 0.2}); + meshcat->SetTransform("c3_workspace", RigidTransformd(workspace_center)); + } + + if (vis_params.visualize_execution_plan) { + auto c3_exec_trajectory_drawer_actor = + builder.AddSystem( + meshcat, "end_effector_position_target", "c3_exec_"); + auto repos_trajectory_drawer_actor = + builder.AddSystem( + meshcat, "end_effector_position_target", "repos_exec_"); + c3_exec_trajectory_drawer_actor->SetLineColor( + drake::geometry::Rgba({1, 0.75, 0.79, 1})); + c3_exec_trajectory_drawer_actor->SetLineWidth(10000000); + repos_trajectory_drawer_actor->SetLineColor( + drake::geometry::Rgba({0, 0, 1, 1})); + repos_trajectory_drawer_actor->SetLineWidth(10000000); + c3_exec_trajectory_drawer_actor->SetNumSamples(sampling_c3_options.N); + repos_trajectory_drawer_actor->SetNumSamples(sampling_c3_options.N); + builder.Connect( + c3_execution_trajectory_sub_actor->get_output_port(), + c3_exec_trajectory_drawer_actor->get_input_port_trajectory()); + builder.Connect(repos_execution_trajectory_sub_actor->get_output_port(), + repos_trajectory_drawer_actor->get_input_port_trajectory()); + } + + if (vis_params.visualize_center_of_mass_plan_curr) { + auto trajectory_drawer_actor_curr = + builder.AddSystem( + meshcat, "end_effector_position_target", "curr_"); + auto trajectory_drawer_object_curr = + builder.AddSystem( + meshcat, "object_position_target", "curr_"); + trajectory_drawer_actor_curr->SetLineColor( + drake::geometry::Rgba({1, 0, 0, 1})); + trajectory_drawer_object_curr->SetLineColor( + drake::geometry::Rgba({1, 0, 0, 1})); + trajectory_drawer_actor_curr->SetNumSamples(sampling_c3_options.N); + trajectory_drawer_object_curr->SetNumSamples(sampling_c3_options.N); + builder.Connect(trajectory_sub_actor_curr->get_output_port(), + trajectory_drawer_actor_curr->get_input_port_trajectory()); + builder.Connect(trajectory_sub_object_curr->get_output_port(), + trajectory_drawer_object_curr->get_input_port_trajectory()); + } + + if (vis_params.visualize_center_of_mass_plan_best) { + auto trajectory_drawer_actor_best = + builder.AddSystem( + meshcat, "end_effector_position_target", "best_"); + auto trajectory_drawer_object_best = + builder.AddSystem( + meshcat, "object_position_target", "best_"); + trajectory_drawer_actor_best->SetLineColor( + drake::geometry::Rgba({0, 1, 0, 1})); + trajectory_drawer_object_best->SetLineColor( + drake::geometry::Rgba({0, 1, 0, 1})); + trajectory_drawer_actor_best->SetNumSamples(sampling_c3_options.N); + trajectory_drawer_object_best->SetNumSamples(sampling_c3_options.N); + builder.Connect(trajectory_sub_actor_best->get_output_port(), + trajectory_drawer_actor_best->get_input_port_trajectory()); + builder.Connect(trajectory_sub_object_best->get_output_port(), + trajectory_drawer_object_best->get_input_port_trajectory()); + } + + if (vis_params.visualize_c3_plan_curr) { + auto object_pose_drawer_curr = builder.AddSystem( + meshcat, FindResourceOrThrow(vis_params.object_vis_model), + "object_position_target", "object_orientation_target", + "plans/curr_planned", sampling_c3_options.N, true, + vis_params.c3_curr_object_color); + builder.Connect(trajectory_sub_object_curr->get_output_port(), + object_pose_drawer_curr->get_input_port_trajectory()); + + auto end_effector_pose_drawer_curr = + builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.ee_vis_model), + "end_effector_position_target", "end_effector_orientation_target", + "plans/curr_planned", sampling_c3_options.N, false, + vis_params.c3_curr_ee_color); + builder.Connect(trajectory_sub_actor_curr->get_output_port(), + end_effector_pose_drawer_curr->get_input_port_trajectory()); + + auto dynamically_feasible_object_pose_drawer_curr = + builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.object_vis_model), + "object_position_target", "object_orientation_target", + "plans/dynamically_feasible_curr_plan", + sampling_c3_options.N + 1, true, vis_params.df_curr_object_color); + builder.Connect( + dynamically_feasible_trajectory_sub_object_curr->get_output_port(), + dynamically_feasible_object_pose_drawer_curr + ->get_input_port_trajectory()); + + auto dynamically_feasible_actor_pose_drawer_curr_actor = + builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.ee_vis_model), + "ee_position_target", "end_effector_orientation_target", + "plans/dynamically_feasible_curr_plan", + sampling_c3_options.N + 1, false, vis_params.df_curr_ee_color); + builder.Connect( + dynamically_feasible_trajectory_sub_actor_curr->get_output_port(), + dynamically_feasible_actor_pose_drawer_curr_actor + ->get_input_port_trajectory()); + } + + if (vis_params.visualize_c3_plan_best) { + auto object_pose_drawer_best = builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.object_vis_model), + "object_position_target", "object_orientation_target", + "plans/best_planned", sampling_c3_options.N, true, + vis_params.c3_best_object_color); + builder.Connect(trajectory_sub_object_best->get_output_port(), + object_pose_drawer_best->get_input_port_trajectory()); + + auto end_effector_pose_drawer_best = + builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.ee_vis_model), + "end_effector_position_target", "end_effector_orientation_target", + "plans/best_planned", sampling_c3_options.N, false, + vis_params.c3_best_ee_color); + builder.Connect(trajectory_sub_actor_best->get_output_port(), + end_effector_pose_drawer_best->get_input_port_trajectory()); + + auto dynamically_feasible_object_pose_drawer_best = + builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.object_vis_model), + "object_position_target", "object_orientation_target", + "plans/dynamically_feasible_best_plan", sampling_c3_options.N + 1, + true, vis_params.df_best_object_color); + builder.Connect( + dynamically_feasible_trajectory_sub_object_best->get_output_port(), + dynamically_feasible_object_pose_drawer_best + ->get_input_port_trajectory()); + + auto dynamically_feasible_actor_pose_drawer_best_actor = + builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.ee_vis_model), + "ee_position_target", "end_effector_orientation_target", + "plans/dynamically_feasible_best_plan", + sampling_c3_options.N + 1, false, vis_params.df_best_ee_color); + builder.Connect( + dynamically_feasible_trajectory_sub_actor_best->get_output_port(), + dynamically_feasible_actor_pose_drawer_best_actor + ->get_input_port_trajectory()); + } + + if (vis_params.visualize_sample_locations) { + int from_buffer = 0; + if (sampling_params.consider_best_buffer_sample_when_leaving_c3) { + from_buffer = 1; + } + auto sample_locations_drawer = builder.AddSystem( + meshcat, + FindResourceOrThrow(vis_params.ee_vis_model), + "sample_locations", "unused_orientation_name", "samples", + std::max(sampling_params.num_additional_samples_c3 + from_buffer, + sampling_params.num_additional_samples_repos + 1) + + 1, + false, vis_params.sample_color); + + builder.Connect(sample_location_sub->get_output_port(), + sample_locations_drawer->get_input_port_trajectory()); + } + + if (vis_params.visualize_sample_buffer) { + auto sample_buffer_to_point_cloud_converter = + builder.AddSystem(); + auto sample_buffer_point_cloud_visualizer = + builder.AddSystem(meshcat, + "sample_buffer"); + sample_buffer_point_cloud_visualizer->set_point_size(0.02); + + builder.Connect(sample_buffer_sub->get_output_port(), + sample_buffer_to_point_cloud_converter + ->get_input_port_lcmt_sample_buffer()); + builder.Connect(sample_buffer_to_point_cloud_converter + ->get_output_port_sample_buffer_point_cloud(), + sample_buffer_point_cloud_visualizer->cloud_input_port()); + builder.Connect(sample_costs_sub->get_output_port(), + sample_buffer_to_point_cloud_converter + ->get_input_port_new_sample_costs()); + } + + if (vis_params.visualize_c3_state) { + auto c3_target_drawer = + builder.AddSystem(meshcat, true, true); + builder.Connect(c3_state_actual_sub->get_output_port(), + c3_target_drawer->get_input_port_c3_state_actual()); + builder.Connect(c3_state_target_sub->get_output_port(), + c3_target_drawer->get_input_port_c3_state_target()); + builder.Connect(c3_final_state_target_sub->get_output_port(), + c3_target_drawer->get_input_port_c3_state_final_target()); + } + + if (vis_params.visualize_c3_forces_curr) { + auto end_effector_force_drawer_curr = + builder.AddSystem( + meshcat, "end_effector_position_target", + "end_effector_force_target", "lcs_force_trajectory_curr", "curr_"); + builder.Connect( + trajectory_sub_actor_curr->get_output_port(), + end_effector_force_drawer_curr->get_input_port_actor_trajectory()); + builder.Connect( + trajectory_sub_force_curr->get_output_port(), + end_effector_force_drawer_curr->get_input_port_force_trajectory()); + builder.Connect( + robot_time_passthrough->get_output_port(), + end_effector_force_drawer_curr->get_input_port_robot_time()); + } + + if (vis_params.visualize_c3_forces_best) { + auto end_effector_force_drawer_best = + builder.AddSystem( + meshcat, "end_effector_position_target", + "end_effector_force_target", "lcs_force_trajectory_best", "best_"); + builder.Connect( + trajectory_sub_actor_best->get_output_port(), + end_effector_force_drawer_best->get_input_port_actor_trajectory()); + builder.Connect( + trajectory_sub_force_best->get_output_port(), + end_effector_force_drawer_best->get_input_port_force_trajectory()); + builder.Connect( + robot_time_passthrough->get_output_port(), + end_effector_force_drawer_best->get_input_port_robot_time()); + } + + if (vis_params.visualize_is_c3_mode) { + auto c3_mode_visualizer = builder.AddSystem(); + builder.Connect(is_c3_mode_sub->get_output_port(), + c3_mode_visualizer->get_input_port_is_c3_mode()); + builder.Connect(reduced_order_model_receiver->get_output_port(), + c3_mode_visualizer->get_input_port_curr_lcs_state()); + auto is_c3_mode_drawer = builder.AddSystem( + meshcat, FindResourceOrThrow(vis_params.ee_vis_model), + "c3_mode_visualization", "end_effector_orientation_target", "c3_mode", + 1, false, vis_params.is_c3_mode_color); + builder.Connect( + c3_mode_visualizer->get_output_port_c3_mode_visualization_traj(), + is_c3_mode_drawer->get_input_port_trajectory()); + } + + builder.Connect(franka_passthrough->get_output_port(), + mux->get_input_port(0)); + builder.Connect(tray_passthrough->get_output_port(), mux->get_input_port(1)); + builder.Connect(*mux, *to_pose); + builder.Connect( + to_pose->get_output_port(), + scene_graph.get_source_pose_port(plant.get_source_id().value())); + builder.Connect(*franka_state_receiver, *franka_passthrough); + builder.Connect(*franka_state_receiver, *robot_time_passthrough); + builder.Connect(*object_state_receiver, *tray_passthrough); + builder.Connect(*franka_state_sub, *franka_state_receiver); + builder.Connect(*object_state_sub, *object_state_receiver); + + auto visualizer = &drake::geometry::MeshcatVisualizer::AddToBuilder( + &builder, scene_graph, meshcat, std::move(params)); + + auto diagram = builder.Build(); + diagram->set_name(("sampling_c3_visualizer_" + FLAGS_demo_name)); + DrawAndSaveDiagramGraph(*diagram); + auto context = diagram->CreateDefaultContext(); + + auto& franka_state_sub_context = + diagram->GetMutableSubsystemContext(*franka_state_sub, context.get()); + auto& object_state_sub_context = + diagram->GetMutableSubsystemContext(*object_state_sub, context.get()); + franka_state_receiver->InitializeSubscriberPositions( + plant, franka_state_sub_context); + object_state_receiver->InitializeSubscriberPositions( + plant, object_state_sub_context); + + /// Use the simulator to drive at a fixed rate + /// If set_publish_every_time_step is true, this publishes twice + auto simulator = + std::make_unique>(*diagram, std::move(context)); + simulator->set_publish_every_time_step(false); + simulator->set_publish_at_initialization(false); + simulator->set_target_realtime_rate(1.0); + simulator->Initialize(); + + drake::log()->info("visualizer started"); + + simulator->AdvanceTo(std::numeric_limits::infinity()); + + return 0; +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { + return dairlib::do_main(argc, argv); +} diff --git a/examples/sampling_c3/generate_samples.cc b/examples/sampling_c3/generate_samples.cc new file mode 100644 index 0000000000..62af91711b --- /dev/null +++ b/examples/sampling_c3/generate_samples.cc @@ -0,0 +1,475 @@ +#include "generate_samples.h" +#include "multibody/geom_geom_collider.h" + +#include +#include + +using drake::AutoDiffVecXd; +using drake::AutoDiffXd; +using drake::SortedPair; +using drake::geometry::FrameId; +using drake::geometry::GeometryId; +using drake::geometry::Shape; +using drake::geometry::SignedDistancePair; +using drake::geometry::Sphere; +using drake::math::RigidTransform; +using drake::multibody::Body; +using Eigen::Vector3d; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + +// Public call for generating samples. +std::vector GenerateSampleStates( + const int& n_q, const int& n_v, const int& n_u, + const Eigen::VectorXd& x_lcs, const bool& is_doing_c3, + const SamplingParams& sampling_params, + const SamplingC3Options& sampling_c3_options, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms) { + // Determine number of samples based on mode. + int num_samples; + if (is_doing_c3) { + num_samples = sampling_params.num_additional_samples_c3; + } else { + num_samples = sampling_params.num_additional_samples_repos; + } + std::vector candidate_states(num_samples); + // Initialize all candidate states to be the same as the current LCS state. + // NOTE: A naive step might be to set the sample EE velocities to zero, but + // in practice this can cause undesired cost differences between the current + // location and the candidate states. Keeping the current EE velocity the + // same across all samples is an equalizer. + for (int i = 0; i < num_samples; i++) { + candidate_states[i] = x_lcs; + } + + // Split function calls based on sampling strategy. + SamplingStrategy strategy = sampling_params.sampling_strategy; + if (strategy == SamplingStrategy::kRadiallySymmetric) { + for (int i = 0; i < num_samples; i++) { + candidate_states[i].head(3) = RadiallySymmetricSampling( + n_q, n_v, x_lcs, num_samples, i, sampling_params.sampling_radius, + sampling_params.sampling_height); + if (sampling_params.filter_samples_for_safety && + !IsSampleInWorkspace(candidate_states[i], sampling_c3_options)) { + throw std::runtime_error( + "Error: Radially symmetric sample location is outside workspace."); + } + } + } else if (strategy == SamplingStrategy::kRandomOnCircle) { + for (int i = 0; i < num_samples; i++) { + do { + candidate_states[i].head(3) = RandomOnCircleSampling( + n_q, n_v, x_lcs, sampling_params.sampling_radius, + sampling_params.sampling_height); + } while (sampling_params.filter_samples_for_safety && + !IsSampleInWorkspace(candidate_states[i], sampling_c3_options)); + } + } else if (strategy == SamplingStrategy::kRandomOnSphere) { + for (int i = 0; i < num_samples; i++) { + do { + candidate_states[i].head(3) = RandomOnSphereSampling( + n_q, n_v, x_lcs, sampling_params.sampling_radius, + sampling_params.min_angle_from_vertical, + sampling_params.max_angle_from_vertical); + } while (sampling_params.filter_samples_for_safety && + !IsSampleInWorkspace(candidate_states[i], sampling_c3_options)); + } + } else if (strategy == SamplingStrategy::kFixed) { + if (num_samples > sampling_params.fixed_sample_locations.size()) { + throw std::runtime_error( + "Error: More fixed samples requested than provided."); + } + for (int i = 0; i < num_samples; i++) { + if (sampling_params.fixed_sample_locations.cols() != 3) { + throw std::runtime_error("Error: Fixed sample locations must be 3D."); + } + candidate_states[i].head(3) = FixedSample( + sampling_params.fixed_sample_locations.row(i)); + if (sampling_params.filter_samples_for_safety && + !IsSampleInWorkspace(candidate_states[i], sampling_c3_options)) { + throw std::runtime_error( + "Error: Fixed sample location is outside workspace."); + } + } + } else if (strategy == SamplingStrategy::kRandomOnPerimeter) { + for (int i = 0; i < num_samples; i++) { + do { + candidate_states[i].head(3) = PerimeterSampling( + n_q, n_v, n_u, x_lcs, plant, context, plant_ad, context_ad, + contact_geoms, sampling_params, sampling_c3_options); + } while (sampling_params.filter_samples_for_safety && + !IsSampleInWorkspace(candidate_states[i], sampling_c3_options)); + } + } else if (strategy == SamplingStrategy::kRandomOnShell) { + for (int i = 0; i < num_samples; i++) { + do { + candidate_states[i].head(3) = ShellSampling( + n_q, n_v, n_u, x_lcs, plant, context, plant_ad, context_ad, + contact_geoms, sampling_params, sampling_c3_options); + } while (sampling_params.filter_samples_for_safety && + !IsSampleInWorkspace(candidate_states[i], sampling_c3_options)); + } + } else { + throw std::runtime_error("Error: Sampling strategy not recognized."); + } + return candidate_states; +} + +// kRadiallySymmetric: Equally spaced on perimeter of circle of fixed radius +// and height. This generates angle offsets from world frame. +Eigen::Vector3d RadiallySymmetricSampling( + const int& n_q, const int& n_v, const Eigen::VectorXd& x_lcs, + const int& num_samples, const int& i, const double& sampling_radius, + const double& sampling_height) { + // Center the sampling circle on the current object location. + Vector3d object_xyz = x_lcs.segment(n_q - 3, 3); + double theta = (360 / static_cast(num_samples)) * (M_PI / 180); + + // Update the hypothetical state's EE location. + Eigen::Vector3d sample = Vector3d::Zero(); + sample[0] = object_xyz[0] + sampling_radius * cos((double)i * theta); + sample[1] = object_xyz[1] + sampling_radius * sin((double)i * theta); + sample[2] = sampling_height; + return sample; +} + +// kRandomOnCircle: Random on perimeter of circle of fixed radius and height. +Eigen::Vector3d RandomOnCircleSampling( + const int& n_q, const int& n_v, const Eigen::VectorXd& x_lcs, + const double& sampling_radius, const double& sampling_height) { + // Center the sampling circle on the current object location. + Vector3d object_xyz = x_lcs.segment(n_q - 3, 3); + + // Generate a random theta. + double theta = RandomUniform(0, 2*M_PI); + + // Update the hypothetical state's EE location. + Eigen::Vector3d sample = Vector3d::Zero(); + sample[0] = object_xyz[0] + sampling_radius * cos(theta); + sample[1] = object_xyz[1] + sampling_radius * sin(theta); + sample[2] = sampling_height; + return sample; +} + +// kRandomOnSphere: Random on surface of sphere of fixed radius within +// elevation angles. +Eigen::Vector3d RandomOnSphereSampling( + const int& n_q, const int& n_v, const Eigen::VectorXd& x_lcs, + const double& sampling_radius, const double& min_angle_from_vertical, + const double& max_angle_from_vertical) { + // Center the sampling circle on the current object location. + Vector3d object_xyz = x_lcs.segment(n_q - 3, 3); + + // Generate a random theta about and elevation angle from the vertical axis. + double theta = RandomUniform(0, 2*M_PI); + double elevation_theta = RandomUniform(min_angle_from_vertical, + max_angle_from_vertical); + + // Update the hypothetical state's EE location. + Eigen::Vector3d sample = Vector3d::Zero(); + sample[0] = object_xyz[0] + sampling_radius*cos(theta)*sin(elevation_theta); + sample[1] = object_xyz[1] + sampling_radius*sin(theta)*sin(elevation_theta); + sample[2] = object_xyz[2] + sampling_radius*cos(elevation_theta); + return sample; +} + +// kFixed +Eigen::Vector3d FixedSample(const Eigen::Vector3d& fixed_sample_location) { + return fixed_sample_location; +} + +// kRandomOnPerimeter: Random on (roughly) inflated perimeter of 2D slice of +// object on its z=0 body plane, (roughly) projected vertically to pre-specified +// sample height. The precise steps are: +// 1) Sample points in the body frame's (x, y, z=0) plane. These may not be +// flat in the world frame. +// 2) Project the sampled points vertically to a fixed world height. +// 3) Reject any that are not in collision with the object. +// 4) Project the colliding samples outward to a fixed clearance distance. +// These may not be flat in the world frame (especially for curved objects). +// 5) Reject any that are too close to the object (i.e. if the witness point on +// the object changes during the projection such that it was insufficient). +// 6) Reject any that are too far away from the desired sampling height. +// +// This procedure works best if: +// 1) The body z-axis is aligned (or nearly aligned) with the world z-axis. +// 2) The body origin lies roughly at the mid-height of the object, so a +// vertical projection results in an even perimeter around the geometry. +// 3) The side walls of the object are vertical. +// +// TODO: implement a more general perimeter strategy without requiring the +// above assumptions. +Eigen::Vector3d PerimeterSampling( + const int& n_q, const int& n_v, const int& n_u, + const Eigen::VectorXd& x_lcs, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + const SamplingParams& sampling_params, + const SamplingC3Options sampling_c3_options) +{ + Eigen::VectorXd candidate_state = VectorXd::Zero(n_q + n_v); + int min_distance_index = -1; + + // Try projecting colliding samples until one is near desired sampling height + // and maintains the desired clearance. + while (true) { + do { + // These are in body frame. + double x_sample = RandomUniform(sampling_params.grid_x_limits[0], + sampling_params.grid_x_limits[1]); + double y_sample = RandomUniform(sampling_params.grid_y_limits[0], + sampling_params.grid_y_limits[1]); + // WARNING: This assumes 1) the body's z-axis is roughly aligned with the + // world z-axis, and 2) the body origin is roughly at the mid-height of + // the object. + double z_sample = 0; + + // Convert to world frame using the current object state. + Eigen::Quaterniond quat_object(x_lcs(3), x_lcs(4), x_lcs(5), x_lcs(6)); + Eigen::Vector3d object_position = x_lcs.segment(7, 3); + candidate_state = x_lcs; + candidate_state.head(3) = + quat_object * Eigen::Vector3d(x_sample, y_sample, z_sample) + + object_position; + + // Project samples to specified sampling height in world frame. + candidate_state[2] = sampling_params.sampling_height; + } while (!IsSampleWithinDistanceOfSurface( + n_q, n_v, n_u, 0.0, candidate_state, plant, context, plant_ad, context_ad, + contact_geoms, sampling_c3_options, min_distance_index)); + + // Project the sample past the surface of the object with clearance. + Eigen::VectorXd projected_state = ProjectSampleOutsideObject( + candidate_state, min_distance_index, sampling_params, plant, *context, + contact_geoms); + + // Check the desired clearance is satisfied; otherwise try again. + UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, + projected_state); + if (IsSampleWithinDistanceOfSurface( + n_q, n_v, n_u, sampling_params.sample_projection_clearance, + projected_state, plant, context, plant_ad, context_ad, contact_geoms, + sampling_c3_options, min_distance_index)) { + continue; + } + // Check the projection is within a small epsilon of the sampling height; + // otherwise try again. + // WARNING: This assumes the walls of the object are roughly vertical. + double epsilon = 0.001; + if (projected_state[2] < sampling_params.sampling_height - epsilon || + projected_state[2] > sampling_params.sampling_height + epsilon) { + continue; + } + + // Undo the update context. + UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, x_lcs); + Eigen::Vector3d sample = projected_state.head(3); + return sample; + } +} + + +// kRandomOnShell: Random on inflated 3D shell surrounding the object. Makes a +// light assumption that the body origin is roughly centered on its geometry. +// +// TODO: this strategy is largely untested. +Eigen::Vector3d ShellSampling( + const int& n_q, const int& n_v, const int& n_u, + const Eigen::VectorXd& x_lcs, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + const SamplingParams& sampling_params, + const SamplingC3Options sampling_c3_options) +{ + Eigen::VectorXd candidate_state = VectorXd::Zero(n_q + n_v); + int min_distance_index = -1; + + // Try projecting colliding samples until one is above minimum EE height and + // maintains the desired clearance. + while (true) { + do { + // Center the sampling sphere on the current object location. + Vector3d object_xyz = x_lcs.segment(7, 3); + double x_samplec = object_xyz[0]; + double y_samplec = object_xyz[1]; + double z_samplec = object_xyz[2]; + + // Generate a random theta about and elevation angle from vertical axis. + double theta = RandomUniform(0, 2*M_PI); + double elevation_theta = RandomUniform( + sampling_params.min_angle_from_vertical, + sampling_params.max_angle_from_vertical); + + // Generate random sampling radius. + double sampling_radius = RandomUniform( + sampling_params.min_sampling_radius, + sampling_params.max_sampling_radius); + + // Update the hypothetical state's end effector location to the tested + // sample location. + candidate_state = x_lcs; + candidate_state[0] = + x_samplec + sampling_radius * cos(theta) * sin(elevation_theta); + candidate_state[1] = + y_samplec + sampling_radius * sin(theta) * sin(elevation_theta); + candidate_state[2] = z_samplec + sampling_radius * cos(elevation_theta); + } while (!IsSampleWithinDistanceOfSurface( + n_q, n_v, n_u, 0.0, candidate_state, plant, context, plant_ad, context_ad, + contact_geoms, sampling_c3_options, min_distance_index)); + + // Project the sample past the surface of the object with clearance. + Eigen::VectorXd projected_state = ProjectSampleOutsideObject( + candidate_state, min_distance_index, sampling_params, plant, *context, + contact_geoms); + + // Check the desired clearance is satisfied; otherwise try again. + UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, + projected_state); + if (IsSampleWithinDistanceOfSurface( + n_q, n_v, n_u, sampling_params.sample_projection_clearance, + projected_state, plant, context, plant_ad, context_ad, contact_geoms, + sampling_c3_options, min_distance_index)) { + continue; + } + // Check the projection is above the minimum EE height; otherwise try again. + if (projected_state[2] < sampling_c3_options.workspace_limits[2][3]) { + continue; + } + + // Undo the update context. + UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, x_lcs); + Eigen::Vector3d sample = projected_state.head(3); + return sample; + } +} + +bool IsSampleInWorkspace(const Eigen::VectorXd& candidate_state, + const SamplingC3Options& sampling_c3_options) { + double candidate_radius = + sqrt(std::pow(candidate_state[0], 2) + std::pow(candidate_state[1], 2)); + if (candidate_state[0] < sampling_c3_options.workspace_limits[0][3] // x min + || candidate_state[0] > sampling_c3_options.workspace_limits[0][4] // x max + || candidate_state[1] < sampling_c3_options.workspace_limits[1][3] // y min + || candidate_state[1] > sampling_c3_options.workspace_limits[1][4] // y max + || candidate_state[2] < sampling_c3_options.workspace_limits[2][3] // z min + || candidate_state[2] > sampling_c3_options.workspace_limits[2][4] // z max + || candidate_radius > sampling_c3_options.robot_radius_limits[1] // r min + || candidate_radius < sampling_c3_options.robot_radius_limits[0]) // r max + {return false;} + return true; +} + +double GetEERadiusFromPlant( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector< + std::vector>>& + contact_geoms) +{ + const auto& query_port = plant.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + const auto& inspector = query_object.inspector(); + + // Locate the EE and obtain its radius. The first set of contact geoms has + // the EE and ground. + GeometryId ee_geom_id = contact_geoms.at(0).at(0).first(); + const drake::geometry::Shape& shape = inspector.GetShape(ee_geom_id); + const auto* sphere = dynamic_cast(&shape); + if (sphere) { + return sphere->radius(); + } + throw std::runtime_error("End effector geometry is not a sphere!"); +} + +bool IsSampleWithinDistanceOfSurface( + const int& n_q, const int& n_v, const int& n_u, + const double& clearance_distance, + const Eigen::VectorXd& candidate_state, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + SamplingC3Options sampling_c3_options, + int& min_distance_index) +{ + // Update the context of the plant with the candidate state. + UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, + candidate_state); + + // Find the closest pair if there are multiple pairs + std::vector distances; + for (int i = 0; i < contact_geoms.at(1).size(); i++) { + SortedPair pair{(contact_geoms.at(1)).at(i)}; + multibody::GeomGeomCollider collider(plant, pair); + + auto [phi_i, J_i] = collider.EvalPolytope( + *context, sampling_c3_options.num_friction_directions); + distances.push_back(phi_i); + } + + // Find the minimum distance. + auto min_distance_it = std::min_element(distances.begin(), distances.end()); + // Modify a reference to the index of the closest pair of contact geoms so + // that the projection function need not recompute the closest pair. + min_distance_index = std::distance(distances.begin(), min_distance_it); + double min_distance = *min_distance_it; + + // Require that min_distance be at least 1 mm within the clearance distance. + return min_distance <= clearance_distance - 1e-3; +} + +Eigen::VectorXd ProjectSampleOutsideObject( + Eigen::VectorXd& candidate_state, int min_distance_index, + const SamplingParams& sampling_params, + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector< + std::vector>>& + contact_geoms) { + + // Compute the witness points between the penetrating sample and the object + // surface. + multibody::GeomGeomCollider collider( + plant, contact_geoms.at(1).at(min_distance_index)); + auto [p_world_contact_ee, p_world_contact_obj] = collider.CalcWitnessPoints( + context); + + // Get the EE radius to factor into the projection. + double ee_radius = GetEERadiusFromPlant(plant, context, contact_geoms); + + // Find vector in direction from EE to object witness points. + Eigen::Vector3d ee_to_obj = p_world_contact_obj - p_world_contact_ee; + Eigen::Vector3d ee_to_obj_normalized = ee_to_obj.normalized(); + // Add clearance to the object in the same direction. + Eigen::Vector3d p_world_contact_obj_clearance = + p_world_contact_obj + + (ee_radius + sampling_params.sample_projection_clearance) * + ee_to_obj_normalized; + candidate_state.head(3) = p_world_contact_obj_clearance; + return candidate_state; +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/generate_samples.h b/examples/sampling_c3/generate_samples.h new file mode 100644 index 0000000000..fae0086319 --- /dev/null +++ b/examples/sampling_c3/generate_samples.h @@ -0,0 +1,158 @@ +#include + +#include +#include +#include + +#include "common/math_utils.h" +#include "common/update_context.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" +#include "examples/sampling_c3/parameter_headers/sampling_params.h" +#include "multibody/geom_geom_collider.h" +#include "multibody/multibody_utils.h" +#include "solvers/c3_options.h" + +using Eigen::Vector3d; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + + +/// Public function +std::vector GenerateSampleStates( + const int& n_q, + const int& n_v, + const int& n_u, + const Eigen::VectorXd& x_lcs, + const bool& is_doing_c3, + const SamplingParams& sampling_params, + const SamplingC3Options& sampling_c3_options, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms +); + +/// Individual sampling strategies returning 3D EE position +Eigen::Vector3d RadiallySymmetricSampling( + const int& n_q, + const int& n_v, + const Eigen::VectorXd& x_lcs, + const int& num_samples, + const int& i, + const double& sampling_radius, + const double& sampling_height +); + +Eigen::Vector3d RandomOnCircleSampling( + const int& n_q, + const int& n_v, + const Eigen::VectorXd& x_lcs, + const double& sampling_radius, + const double& sampling_height +); + +Eigen::Vector3d RandomOnSphereSampling( + const int& n_q, + const int& n_v, + const Eigen::VectorXd& x_lcs, + const double& sampling_radius, + const double& min_angle_from_vertical, + const double& max_angle_from_vertical +); + +Eigen::Vector3d FixedSample( + const Eigen::Vector3d& fixed_sample_location +); + +Eigen::Vector3d PerimeterSampling( + const int& n_q, + const int& n_v, + const int& n_u, + const Eigen::VectorXd& x_lcs, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + const SamplingParams& sampling_params, + const SamplingC3Options sampling_c3_options +); + +Eigen::Vector3d ShellSampling( + const int& n_q, + const int& n_v, + const int& n_u, + const Eigen::VectorXd& x_lcs, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + const SamplingParams& sampling_params, + const SamplingC3Options sampling_c3_options +); + + +/// Helper functions + +/// Whether the candidate state's EE location is within the robot workspace. +bool IsSampleInWorkspace( + const Eigen::VectorXd& candidate_state, + const SamplingC3Options& sampling_c3_options +); + +/// Find and return the end effector radius from the plant. +/// WARNING: assumes EE-object pairs are first in contact_geoms. +double GetEERadiusFromPlant( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector< + std::vector>>& + contact_geoms +); + +/// Whether the candidate state's EE location is within a clearance distance of +/// the surface of the object. Can factor in the EE radius if a real clearance +/// distance between the EE and the object is desired; otherwise this is the +/// distance from the center of the EE to the surface of the object. +bool IsSampleWithinDistanceOfSurface( + const int& n_q, + const int& n_v, + const int& n_u, + const double& clearance_distance, + const Eigen::VectorXd& candidate_state, + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + SamplingC3Options sampling_c3_options, + int& min_distance_index +); + +/// Project an EE candidate location outside the object such that the clearance +/// distance in the sampling parameters is satisfied. +Eigen::VectorXd ProjectSampleOutsideObject( + Eigen::VectorXd& candidate_state, + int min_distance_index, + const SamplingParams& sampling_params, + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector< + std::vector>>& + contact_geoms +); + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/examples/sampling_c3/goal_generator.cc b/examples/sampling_c3/goal_generator.cc new file mode 100644 index 0000000000..3a1ac790a2 --- /dev/null +++ b/examples/sampling_c3/goal_generator.cc @@ -0,0 +1,300 @@ +#include "goal_generator.h" + +#include +#include + +#include "examples/sampling_c3/parameter_headers/goal_params.h" +#include "lcm/lcm_trajectory.h" + +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + +SamplingC3GoalGenerator::SamplingC3GoalGenerator( + const drake::multibody::MultibodyPlant& object_plant, + const SamplingC3GoalParams& goal_params, + const std::vector& nominal_orientations) : + goal_params_(goal_params), + nominal_orientations_(nominal_orientations) { + // INPUT PORTS + radio_port_ = this->DeclareAbstractInputPort( + "lcmt_radio_out", + drake::Value{}) + .get_index(); + object_state_port_ = this->DeclareVectorInputPort( + "x_object", + StateVector(object_plant.num_positions(), + object_plant.num_velocities())) + .get_index(); + + // OUTPUT PORTS + end_effector_target_port_ = this->DeclareVectorOutputPort( + "end_effector_target", + BasicVector(3), + &SamplingC3GoalGenerator::CalcEndEffectorTarget) + .get_index(); + object_target_port_ = this->DeclareVectorOutputPort( + "object_target", + BasicVector(7), + &SamplingC3GoalGenerator::CalcObjectTarget) + .get_index(); + object_velocity_target_port_ = this->DeclareVectorOutputPort( + "object_velocity_target", + BasicVector(6), + &SamplingC3GoalGenerator::CalcObjectVelocityTarget) + .get_index(); + object_final_target_port_ = this->DeclareVectorOutputPort( + "object_final_target", + BasicVector(7), + &SamplingC3GoalGenerator::OutputObjectFinalTarget) + .get_index(); + target_gen_info_port_ = this->DeclareAbstractOutputPort( + "target_generator_info", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3GoalGenerator::OutputGoalGeneratorInfo) + .get_index(); + + // Start with the fixed goal from the goal params. + target_final_object_position_ = goal_params_.fixed_target_position; + target_final_object_orientation_ = goal_params_.fixed_target_orientation; +} + +// Fixes the EE target to be a fixed offset above the object. +void SamplingC3GoalGenerator::CalcEndEffectorTarget( + const drake::systems::Context& context, + drake::systems::BasicVector* target) const { + const StateVector* object_state = + (StateVector*)this->EvalVectorInput(context, object_state_port_); + + VectorXd end_effector_position = object_state->GetPositions().tail(3); + end_effector_position[2] += goal_params_.ee_target_z_offset_above_object; + target->SetFromVector(end_effector_position); +} + +void SamplingC3GoalGenerator::CalcObjectTarget( + const drake::systems::Context& context, + BasicVector* target) const { + const StateVector* object_state = + (StateVector*)this->EvalVectorInput(context, object_state_port_); + Eigen::Vector3d obj_curr_position = object_state->GetPositions().tail(3); + VectorXd obj_curr_quat = object_state->GetPositions().head(4); + Eigen::Quaterniond curr_quat(obj_curr_quat(0), obj_curr_quat(1), + obj_curr_quat(2), obj_curr_quat(3)); + + // First, ignore lookahead and use the final goal. + VectorXd target_obj_position = target_final_object_position_; + Eigen::Quaterniond target_obj_orientation( + target_final_object_orientation_[0], target_final_object_orientation_[1], + target_final_object_orientation_[2], target_final_object_orientation_[3]); + + // Check if success has been met. Update goal if necessary. + double object_position_error = + (obj_curr_position - target_final_object_position_).norm(); + Eigen::AngleAxis angle_axis_diff(target_obj_orientation * + curr_quat.inverse()); + double object_angular_error = angle_axis_diff.angle(); + + if ((object_position_error < goal_params_.position_success_threshold) && + (object_angular_error < goal_params_.orientation_success_threshold)) { + std::cout << "\nMet pose goal!\n" << std::endl; + OnGoalReached(); + } + + // Apply lookahead. + std::tie(target_obj_orientation, target_obj_position) = + GenerateLineTrajectoryWithLookahead(curr_quat, obj_curr_position); + VectorXd target_obj_state = VectorXd::Zero(7); + target_obj_state << target_obj_orientation.w(), target_obj_orientation.x(), + target_obj_orientation.y(), target_obj_orientation.z(), target_obj_position; + target->SetFromVector(target_obj_state); +} + +// Command zero linear velocity, and command angular velocity that scales with +// orientation error, scaled by angle_err_to_vel_factor_. +void SamplingC3GoalGenerator::CalcObjectVelocityTarget( + const drake::systems::Context& context, + BasicVector* target) const { + const StateVector* object_state = + (StateVector*)this->EvalVectorInput(context, object_state_port_); + // Get the final and current orientation. + Eigen::Quaterniond y_quat_des( + target_final_object_orientation_[0], target_final_object_orientation_[1], + target_final_object_orientation_[2], target_final_object_orientation_[3]); + const VectorX& q = object_state->GetPositions().head(4); + Eigen::VectorXd normalized_q = q / q.norm(); + Eigen::Quaterniond y_quat(normalized_q(0), normalized_q(1), + normalized_q(2), normalized_q(3)); + + // Compute the orientation error, apply the lookahead angle, and convert the + // axis-angle error to an angular velocity command. + Eigen::AngleAxis angle_axis_diff(y_quat_des * y_quat.inverse()); + auto orientation_trajectory = + PiecewiseQuaternionSlerp({0, 1}, {y_quat, y_quat_des}); + double lookahead_fraction = + std::min(goal_params_.lookahead_angle / angle_axis_diff.angle(), 1.0); + Eigen::MatrixXd y_quat_lookahead = + orientation_trajectory.value(lookahead_fraction); + Eigen::Quaterniond y_quat_lookahead_quat( + y_quat_lookahead(0), y_quat_lookahead(1), + y_quat_lookahead(2), y_quat_lookahead(3)); + + Eigen::AngleAxis angle_axis_diff_to_lookahead(y_quat_lookahead_quat * + y_quat.inverse()); + VectorXd angle_error = angle_axis_diff_to_lookahead.angle() * + angle_axis_diff_to_lookahead.axis(); + angle_error *= goal_params_.angle_err_to_vel_factor; + + VectorXd target_obj_velocity = VectorXd::Zero(6); + target_obj_velocity << angle_error, VectorXd::Zero(3); + target->SetFromVector(target_obj_velocity); +} + +void SamplingC3GoalGenerator::OutputObjectFinalTarget( + const drake::systems::Context& context, + BasicVector* target) const { + VectorXd target_final_obj_state = VectorXd::Zero(7); + target_final_obj_state << target_final_object_orientation_, + target_final_object_position_; + target->SetFromVector(target_final_obj_state); +} + +// Randomly generates final position within the specified goal limits in x/y/r. +void SamplingC3GoalGenerator::SetRandomizedTargetFinalObjectPosition() const { + double x, y = 0; + while ((sqrt(x * x + y * y) > goal_params_.random_goal_radius_limits[1]) || + (sqrt(x * x + y * y) < goal_params_.random_goal_radius_limits[0])) { + x = RandomUniform(goal_params_.random_goal_x_limits[0], + goal_params_.random_goal_x_limits[1]); + y = RandomUniform(goal_params_.random_goal_y_limits[0], + goal_params_.random_goal_y_limits[1]); + } + + target_final_object_position_ << x, y, goal_params_.resting_object_height; +} + +// Randomly generates final orientation from the set of valid orientations plus +// an imposed random yaw. If no topples are required, the yaw is at least 90 +// degrees away from the current orientation. +void SamplingC3GoalGenerator::SetRandomizedTargetFinalObjectOrientation() const { + const auto& valid_orientations = GetNominalOrientations(); + std::uniform_int_distribution dis(0, valid_orientations.size() - 1); + std::mt19937 rng{std::random_device{}()}; + int random_index = dis(rng); + Eigen::Quaterniond quat_nominal = valid_orientations.at(random_index); + + // Add random yaw in world frame. Ensure at least 90 degrees away if no + // topple is required. + double min_yaw = 0; + double max_yaw = 2 * M_PI; + if (random_index == orientation_index_) { + min_yaw = M_PI / 2; + max_yaw = 3 * M_PI / 2; + quat_nominal = Eigen::Quaterniond( + target_final_object_orientation_[0], target_final_object_orientation_[1], + target_final_object_orientation_[2], target_final_object_orientation_[3]); + } + double yaw = RandomUniform(min_yaw, max_yaw); + Eigen::Quaterniond quat_world_yaw( + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Quaterniond quat_final = quat_world_yaw * quat_nominal; + target_final_object_orientation_ << quat_final.w(), quat_final.x(), + quat_final.y(), quat_final.z(); + orientation_index_ = random_index; +} + +void SamplingC3GoalGenerator::CycleThroughOrientationSequence() const { + const auto& nominal_orientations = GetNominalOrientations(); + int num_nominal_orientations = nominal_orientations.size(); + Eigen::Quaterniond next_quat = + nominal_orientations.at(goal_counter_ % num_nominal_orientations); + target_final_object_orientation_ << next_quat.w(), next_quat.x(), + next_quat.y(), next_quat.z(); + orientation_index_ = goal_counter_ % num_nominal_orientations; +} + +// Outputs the orientation index for debugging. +void SamplingC3GoalGenerator::OutputGoalGeneratorInfo( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* target) const { + Eigen::MatrixXd orientation_index_data = + orientation_index_ * Eigen::MatrixXd::Ones(1, 1); + Eigen::VectorXd timestamp = context.get_time() * Eigen::VectorXd::Ones(1); + + LcmTrajectory::Trajectory orientation_index_traj; + orientation_index_traj.traj_name = "orientation_index"; + orientation_index_traj.datatypes = std::vector(1, "int"); + orientation_index_traj.datapoints = orientation_index_data; + orientation_index_traj.time_vector = timestamp.cast(); + + LcmTrajectory orientation_index_lcm_traj( + {orientation_index_traj}, {"orientation_index"}, "orientation_index", + "orientation_index", false); + + target->saved_traj = orientation_index_lcm_traj.GenerateLcmObject(); + target->utime = context.get_time() * 1e6; +} + +void SamplingC3GoalGenerator::OnGoalReached() const { + // Reset the target object orientation and position. + if (goal_params_.goal_mode == GoalMode::kRandom) { + SetRandomizedTargetFinalObjectPosition(); + SetRandomizedTargetFinalObjectOrientation(); + } else if (goal_params_.goal_mode == GoalMode::kOrientationSequence) { + // Set the next orientation in the sequence. + CycleThroughOrientationSequence(); + } else { + std::cout << "You have only specified a single goal." << std::endl; + } + goal_counter_++; +} + +std::pair +SamplingC3GoalGenerator::GenerateLineTrajectoryWithLookahead( + const Eigen::Quaterniond& quat_curr_orientation, + const Eigen::Vector3d& obj_curr_position) const { + Eigen::Vector3d target_obj_position = Eigen::Vector3d::Zero(3); + Eigen::Quaterniond target_obj_orientation = Eigen::Quaterniond::Identity(); + + // First handle position lookahead. + Eigen::Vector3d start_point = obj_curr_position; + Eigen::Vector3d end_point = target_final_object_position_; + + Eigen::Vector3d distance_vector = end_point - start_point; + Eigen::Vector3d unit_vector = distance_vector.normalized(); + double step_size = std::min(goal_params_.lookahead_step_size, + distance_vector.norm()); + target_obj_position = start_point + step_size * unit_vector; + + // Second handle orientation lookahead. + Eigen::Quaterniond y_quat_des( + target_final_object_orientation_[0], target_final_object_orientation_[1], + target_final_object_orientation_[2], target_final_object_orientation_[3]); + Eigen::AngleAxis angle_axis_diff(y_quat_des * + quat_curr_orientation.inverse()); + double angle = angle_axis_diff.angle(); + Eigen::Vector3d axis = angle_axis_diff.axis(); + + // Enforce consistency near 180 degrees. + if ((axis.dot(last_rotation_axis_) < 0) && + (M_PI - angle < goal_params_.angle_hysteresis)) { + angle = 2 * M_PI - angle; + axis = -axis; + } + last_rotation_axis_ = axis; + + // Enforce the lookahead. + angle = std::min(angle, goal_params_.lookahead_angle); + + // Apply the rotation. + Eigen::AngleAxis angle_axis_relative(angle, axis); + Eigen::Quaterniond quat_relative = Eigen::Quaterniond(angle_axis_relative); + Eigen::Quaterniond y_quat_lookahead_quat = + quat_relative * quat_curr_orientation; + target_obj_orientation = y_quat_lookahead_quat; + + return std::make_pair(target_obj_orientation, target_obj_position); +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/goal_generator.h b/examples/sampling_c3/goal_generator.h new file mode 100644 index 0000000000..e70f0f4d9e --- /dev/null +++ b/examples/sampling_c3/goal_generator.h @@ -0,0 +1,158 @@ +#include +#include +#include +#include + +#include "common/math_utils.h" +#include "dairlib/lcmt_radio_out.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "systems/framework/state_vector.h" +#include "examples/sampling_c3/parameter_headers/goal_params.h" + +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/leaf_system.h" + +using drake::systems::BasicVector; +using drake::trajectories::PiecewiseQuaternionSlerp; + + +// Define the nominal orientations for the jack demo. +inline const Eigen::Quaterniond kQuatAllUp{ + 0.88047623921714, 0.279848142333121, -0.36470519963100, -0.115916895959295}; +inline const Eigen::Quaterniond kQuatRedDown{ + 0.88047623921714, 0.279848142333121, 0.36470519963100, 0.115916895959295}; +inline const Eigen::Quaterniond kQuatBlueUp{ + 0.70455634261098, -0.060003000646865, 0.455768038939282, -0.5406250962371}; +inline const Eigen::Quaterniond kQuatAllDown{ + 0.455768038939282, -0.54062509623716, 0.70455634261098, -0.0600030006468661}; +inline const Eigen::Quaterniond kQuatGreenUp{ + 0.364705199631001, 0.115916895959295, 0.88047623921714, 0.279848142333121}; +inline const Eigen::Quaterniond kQuatBlueDown{ + 0.0600030006468662, 0.70455634261098, 0.5406250962371, 0.45576803893928}; +inline const Eigen::Quaterniond kQuatRedUp{ + -0.27984814233312, 0.88047623921714, -0.115916895959295, 0.36470519963100}; +inline const Eigen::Quaterniond kQuatGreenDown{ + -0.82047323857028, 0.424708200277866, 0.17591989660616, 0.33985114297998}; +inline const std::vector kNominalOrientationsJack{ + kQuatAllUp, kQuatRedDown, kQuatBlueUp, kQuatAllDown, + kQuatGreenUp, kQuatBlueDown, kQuatRedUp, kQuatGreenDown}; + +// Define the nominal orientations for any planar demo. +inline const Eigen::Quaterniond kQUAT_FLAT{1.0, 0.0, 0.0, 0.0}; +inline const std::vector kNominalOrientationsPlanar{ + kQUAT_FLAT}; + + +namespace dairlib { +namespace systems { + +class SamplingC3GoalGenerator : public drake::systems::LeafSystem { + public: + SamplingC3GoalGenerator( + const drake::multibody::MultibodyPlant& object_plant, + const SamplingC3GoalParams& goal_params, + const std::vector& nominal_orientations); + + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + const drake::systems::InputPort& get_input_port_object_state() const { + return this->get_input_port(object_state_port_); + } + + const drake::systems::OutputPort& + get_output_port_end_effector_target() const { + return this->get_output_port(end_effector_target_port_); + } + + const drake::systems::OutputPort& get_output_port_object_target() + const { + return this->get_output_port(object_target_port_); + } + + const drake::systems::OutputPort& + get_output_port_object_velocity_target() const { + return this->get_output_port(object_velocity_target_port_); + } + + const drake::systems::OutputPort& + get_output_port_object_final_target() const { + return this->get_output_port(object_final_target_port_); + } + + const drake::systems::OutputPort& get_output_port_target_gen_info() + const { + return this->get_output_port(target_gen_info_port_); + } + + private: + const std::vector& GetNominalOrientations() const { + return nominal_orientations_; } + + void CalcEndEffectorTarget( + const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + void CalcObjectTarget( + const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + void CalcObjectVelocityTarget( + const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + void OutputObjectFinalTarget( + const drake::systems::Context& context, + drake::systems::BasicVector* target) const; + void OutputGoalGeneratorInfo( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* target) const; + void SetRandomizedTargetFinalObjectPosition() const; + void SetRandomizedTargetFinalObjectOrientation() const; + void CycleThroughOrientationSequence() const; + void OnGoalReached() const; + std::pair + GenerateLineTrajectoryWithLookahead( + const Eigen::Quaterniond& quat_curr_orientation, + const Eigen::Vector3d& obj_curr_position) const; + + // Input ports + drake::systems::InputPortIndex radio_port_; + drake::systems::InputPortIndex object_state_port_; + drake::systems::OutputPortIndex end_effector_target_port_; + drake::systems::OutputPortIndex object_target_port_; + drake::systems::OutputPortIndex object_velocity_target_port_; + drake::systems::OutputPortIndex object_final_target_port_; + drake::systems::OutputPortIndex target_gen_info_port_; + + const SamplingC3GoalParams goal_params_; + const std::vector nominal_orientations_; + mutable Eigen::VectorXd target_final_object_position_; + mutable Eigen::VectorXd target_final_object_orientation_; + mutable Eigen::Vector3d last_rotation_axis_ = Eigen::Vector3d::Zero(); + mutable int goal_counter_ = 1; + mutable int orientation_index_ = -1; +}; + +class SamplingC3GoalGeneratorJacktoy : public SamplingC3GoalGenerator { + public: + SamplingC3GoalGeneratorJacktoy( + const drake::multibody::MultibodyPlant& object_plant, + const SamplingC3GoalParams& goal_params) : + SamplingC3GoalGenerator( + object_plant, goal_params, kNominalOrientationsJack) {} +}; + + +// class TargetGeneratorPushT : public TargetGenerator { +class SamplingC3GoalGeneratorPushT : public SamplingC3GoalGenerator { + public: + SamplingC3GoalGeneratorPushT( + const drake::multibody::MultibodyPlant& object_plant, + const SamplingC3GoalParams& goal_params) : + SamplingC3GoalGenerator( + object_plant, goal_params, kNominalOrientationsPlanar) {} +}; + + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/jacktoy/BUILD.bazel b/examples/sampling_c3/jacktoy/BUILD.bazel new file mode 100644 index 0000000000..faec88ea69 --- /dev/null +++ b/examples/sampling_c3/jacktoy/BUILD.bazel @@ -0,0 +1,76 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "parameters", + data = glob([ + "parameters/*.yaml", + ]), +) + +cc_library( + name = "lcm_channels_jacktoy", + hdrs = [], + data = [ + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_sim_params_jacktoy", + hdrs = [], + data = [ + "parameters/sim_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:franka_sim_params", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "sampling_c3_controller_params_jacktoy", + hdrs = [], + data = [ + "parameters/sampling_c3_controller_params.yaml", + "parameters/sampling_params.yaml", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "goal_params_jacktoy", + hdrs = [], + data = [ + "parameters/goal_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:goal_params", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_drake_lcm_driver_channels_jacktoy", + hdrs = [], + data = [ + "//common/parameters:franka_drake_lcm_driver_channels", + ], + deps = [ + "//common/parameters:franka_drake_lcm_driver_channels", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) diff --git a/examples/sampling_c3/jacktoy/franka_hardware_jack.pmd b/examples/sampling_c3/jacktoy/franka_hardware_jack.pmd new file mode 100644 index 0000000000..e1151e8164 --- /dev/null +++ b/examples/sampling_c3/jacktoy/franka_hardware_jack.pmd @@ -0,0 +1,103 @@ +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=false --demo_name=jacktoy"; + host = "sampling_c3_localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "sampling_c3_localhost"; + } + cmd "logger" { + exec = "python3 examples/sampling_c3/start_logging.py hw jacktoy"; + host = "sampling_c3_localhost"; + } + cmd "record_video" { + exec = "python3 record_video.py"; + host = "franka_control"; + } +} + +group "controllers (hardware)" { + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=false --demo_name=jacktoy; + host = "sampling_c3_localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=false --demo_name=jacktoy --lcm_url=udpm://239.255.76.67:7667?ttl=1; + host = "franka_control"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "sampling_c3_localhost"; + } +} + +group "drivers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=false --demo_name=jacktoy"; + host = "franka_control"; + } + cmd "franka_driver_out" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_out --demo_name=jacktoy"; + host = "franka_control"; + } + cmd "franka_driver_in" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_in --demo_name=jacktoy"; + host = "franka_control"; + } + cmd "torque_driver" { + exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=torque"; + host = "drake_franka_driver"; + } +} + + +script "init_experiment" { + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 10000; + stop group "drivers"; +} + +script "start_experiment" { + restart cmd "xbox"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + stop cmd "franka_osc"; + start cmd "record_video"; + start cmd "logger"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 5000; + stop cmd "move_to_init"; + start cmd "franka_osc"; + start cmd "sampling_c3"; +} + +script "start_operator_commands" { + restart cmd "visualizer"; + restart cmd "xbox"; + restart cmd "lcm-spy"; +} + +script "stop_experiment" { + stop group "drivers"; + stop group "controllers (hardware)"; + stop cmd "record_video"; + stop cmd "logger"; +} diff --git a/examples/sampling_c3/jacktoy/franka_sim_jack.pmd b/examples/sampling_c3/jacktoy/franka_sim_jack.pmd new file mode 100644 index 0000000000..6da613d4c3 --- /dev/null +++ b/examples/sampling_c3/jacktoy/franka_sim_jack.pmd @@ -0,0 +1,71 @@ +group "simulations" { + cmd "franka_sim" { + exec = "bazel-bin/examples/sampling_c3/franka_sim --demo_name=jacktoy"; + host = "localhost"; + } +} + +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=true --demo_name=jacktoy"; + host = "localhost"; + } + cmd "drake-director" { + exec = "bazel-bin/director/drake-director"; + host = "localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "localhost"; + } + cmd "start_logging" { + exec = "python3 examples/sampling_c3/start_logging.py sim jacktoy"; + host = "localhost"; + } +} + +group "controllers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=true --demo_name=jacktoy"; + host = "localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=true --demo_name=jacktoy"; + host = "localhost"; + } + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=true --demo_name=jacktoy"; + host = "localhost"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } +} + +script "start_experiment_no_logs"{ + stop cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "start_experiment_with_logs"{ + restart cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "end_experiment"{ + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_sim"; + wait ms 10; + stop cmd "start_logging"; +} diff --git a/examples/sampling_c3/jacktoy/parameters/goal_params.yaml b/examples/sampling_c3/jacktoy/parameters/goal_params.yaml new file mode 100644 index 0000000000..3345078eb0 --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/goal_params.yaml @@ -0,0 +1,37 @@ +# Goal mode options (defined as GoalMode enum in goal_params.h): +# 0. kRandom: randomly generate a new goal. +# 1. kOrientationSequence: keep position goal the same, and cycle through a +# sequence of orientations. +# 2. kFixedGoal: keep the same goal. +goal_mode: 0 + +### Parameters used for multiple goal modes. +# Success thresholds for position and orientation. +position_success_threshold: 0.02 +orientation_success_threshold: 0.1 # 0.1 rad = 5.7 degrees + +resting_object_height: 0.031971466 # in world frame +ee_target_z_offset_above_object: 0.06 # defines EE goal w.r.t. object position + +# Lookahead parameters to define a sub-goal for C3. +lookahead_step_size: 0.15 # meters +lookahead_angle: 2 # rad + +# Apply hysteresis on the lookahead angle so the sub-goal orientation doesn't +# flip back and forth near the 180 degree error singularity. A lower number +# means the sub-goal orientation can switch more often; a higher number means +# the sub-goal orientation is more stable but possibly less optimal. +angle_hysteresis: 0.4 + +# Factor to convert an angular error to angular velocity command (0=disabled). +angle_err_to_vel_factor: 0 + +# Initial goal (and only goal for fixed goal mode). +fixed_target_position: [0.45, 0.2, 0.031971466] +fixed_target_orientation: [ 0.8804762392171493, 0.27984814233312133, + -0.3647051996310009, -0.11591689595929514] + +# Random-specific parameters. +random_goal_x_limits: [0.42, 0.5] +random_goal_y_limits: [0.02, 0.25] +random_goal_radius_limits: [0.48, 0.5] diff --git a/examples/sampling_c3/jacktoy/parameters/progress_params.yaml b/examples/sampling_c3/jacktoy/parameters/progress_params.yaml new file mode 100644 index 0000000000..d169fd27af --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/progress_params.yaml @@ -0,0 +1,64 @@ +# Ways of computing C3 costs after solving the MPC problem (defined as +# C3CostComputationType enum in c3_options.h): +# 0. kSimLCS: Simulate the LCS dynamics from the planned +# inputs. +# 1. kUseC3Plan: Use the C3 planned trajectory and inputs. +# 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned +# inputs only for the object; use the planned +# EE trajectory. +# 3. kSimImpedance: Try to emulate the real cost of the system +# associated not only applying the planned +# inputs, but also tracking the planned EE +# trajectory with an impedance controller. +# 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE +# states are replaced with the plan from C3 +# at the end. +# 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the +# object terms contribute to the final cost. +cost_type: 3 +cost_type_position: 2 + +# Progress metric to use to determine if C3 is making progress, all phrased as +# improvement requirements over a number of control loops (defined as +# ProgressMetric enum in progress_params.h) +# 0. kC3Cost: C3 cost. +# 1. kConfigCost: Current object configuration cost. +# 2. kPosOrRotCost: Current position or rotation error. +# 3. kConfigCostDrop: Drop in object configuration cost (this is the same as +# kConfigCost if the required drop is 0; a more +# aggressive drop cuts C3 off earlier). +track_c3_progress_via: 3 + +# Ignore orientation errors when object is beyond this threshold from the goal. +cost_switching_threshold_distance: 0.50 + +# Penalize repositioning distance. +travel_cost_per_meter: 0 + +# Number of control loops to wait before cutting off C3 due to unproductivity. +# Used for kC3Cost, kConfigCost, kPosOrRotCost. +num_control_loops_to_wait: 14 +num_control_loops_to_wait_position: 20 + +# kConfigCostDrop parameters. +progress_enforced_cost_drop: 0.018 +progress_enforced_over_n_loops: 12 + +### Hysteresis parameters for switching between C3 and repositioning modes. +finished_reposition_cost: 1000000000 + +hyst_c3_to_repos: 300000 +hyst_c3_to_repos_position: 300000 +hyst_repos_to_c3: 200000 +hyst_repos_to_c3_position: 200000 +hyst_repos_to_repos: 60000000000000 +hyst_repos_to_repos_position: 60000000000000 + +# Relative hysteresis is in terms of a fraction of the current cost. +use_relative_hysteresis: true +hyst_c3_to_repos_frac: 0.85 +hyst_c3_to_repos_frac_position: 0.6 +hyst_repos_to_c3_frac: 0.3 +hyst_repos_to_c3_frac_position: 0.5 +hyst_repos_to_repos_frac: 0.1 +hyst_repos_to_repos_frac_position: 0.1 diff --git a/examples/sampling_c3/jacktoy/parameters/reposition_params.yaml b/examples/sampling_c3/jacktoy/parameters/reposition_params.yaml new file mode 100644 index 0000000000..682779b193 --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/reposition_params.yaml @@ -0,0 +1,32 @@ +# Repositioning trajectory type options (defined as RepositioningTrajectoryType +# enum in reposition_params.h): +# 0. kSpline: move from current to target with spline that distorts +# around the object. +# 1. kSpherical: move away from object to a radius, along the spherical +# surface to outside target, then in to target. +# 2. kCircular: (planar) move away from object to a planar radius, +# along the circle to outside target, then in to target. +# 3. kPiecewiseLinear: move up to waypoint height, over to above target, then +# down. +traj_type: 1 + +### Parameters used for multiple repositioning trajectory types. +speed: 0.20 # m/s + +# Thresholds for switching to straight line trajectories. +use_straight_line_traj_under_spline: 0.12 # xyz meters for spline +use_straight_line_traj_within_angle: 0.3 # rad for circle/sphere +use_straight_line_traj_under_piecewise_linear: 0.008 # xy meters for PWL + +# Spline-specific parameters. +spline_width: 0.17 + +# Spherical-specific parameters. +sphere_radius: 0.12 + +# Circular-specific parameters. +circle_radius: 0.20 +circle_height: 0.00 + +# Piecewise-linear-specific parameters. +pwl_waypoint_height: 0.06 diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml new file mode 100644 index 0000000000..da0a74987f --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml @@ -0,0 +1,23 @@ +sampling_c3_options_file: examples/sampling_c3/jacktoy/parameters/sampling_c3_options.yaml +reposition_params_file: examples/sampling_c3/jacktoy/parameters/reposition_params.yaml +progress_params_file: examples/sampling_c3/jacktoy/parameters/progress_params.yaml +sampling_params_file: examples/sampling_c3/jacktoy/parameters/sampling_params.yaml +goal_params_file: examples/sampling_c3/jacktoy/parameters/goal_params.yaml + +sim_params_file: examples/sampling_c3/jacktoy/parameters/sim_params.yaml +vis_params_file: examples/sampling_c3/jacktoy/parameters/vis_params.yaml +osc_params_file: examples/sampling_c3/shared_parameters/osc_params.yaml +osqp_settings_file: examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +osc_qp_settings_file: examples/sampling_c3/shared_parameters/osc_qp_settings.yaml +franka_driver_channels_file: common/parameters/franka_drake_lcm_driver_channels.yaml +lcm_channels_hardware_file: examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml +lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml + +# Note: Use capsule_1 body name as representative of the pose of the entire +# jack model. This works because capsule_1's origin is the same as the jack's. +object_model: examples/sampling_c3/urdf/jack_control.sdf +object_body_name: capsule_1 + +include_end_effector_orientation: false + +control_loop_delay_ms: 0 diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_c3_options.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_c3_options.yaml new file mode 100644 index 0000000000..965151260d --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/sampling_c3_options.yaml @@ -0,0 +1,249 @@ +### C3 options +admm_iter: 3 +rho: 0 #This isn't used anywhere! +rho_scale: 3 +num_threads: 5 +num_outer_threads: 4 +delta_option: 1 +projection_type: 'MIQP' # 'MIQP' or 'QP' +contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. +warm_start: false +end_on_qp_step: false +use_robust_formulation: false +solve_time_filter_alpha: 0.95 +publish_frequency: 0 +penalize_changes_in_u_across_solves: false # Penalize (u-u_prev) instead of u. +num_friction_directions: 2 + +N: 5 +gamma: 1.0 # discount factor on MPC costs + +# As alpha -> 0, any complimentarity constraint error in QP projection -> 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1 + +### Limits +# Workspace limits specified as linear constraints [x, y, z, lb, ub] +workspace_limits: [[1, 0, 0, 0.15, 0.75], + [0, 1, 0, -0.6, 0.6], + [0, 0, 1, -0.01, 0.3]] +robot_radius_limits: [0.25, 0.75] +workspace_margins: 0.02 +u_horizontal_limits: [-50, 50] +u_vertical_limits: [-50, 50] + +### Whether to use a predicted EE location for each mode. +use_predicted_x0_c3: true +use_predicted_x0_repos: true +use_predicted_x0_reset_mechanism: true # Resets if prediction is too far. + +### Jack demo specific parameters: +# Contact pairs include a number of (ee-ground, ee-jack, jack-ground). +mu_per_pair_type: [0.4165, 1, 0.4615] # match URDFs with (2mu1*mu2)/(mu1+mu2) +resolve_contacts_to_lists: [[0, 1, 3], + [0, 3, 3], + [1, 1, 3], + [1, 3, 3], + [0, 1, 4], + [0, 3, 6], + [0, 2, 3]] +# Index into this list to choose number of contacts for C3 solve and cost. +num_contacts_index: 0 +num_contacts_index_for_cost: 5 + +planning_dt_position: 0.1 +planning_dt_pose: 0.05 + +### Cost parameters +### If list of lists, then index into them with num_contact_index(_for_cost). +# Set 4x4 portion of Q for quaternion based on current and desired quaternion. +use_quaternion_dependent_cost: true +q_quaternion_dependent_weight: 2500 +q_quaternion_dependent_regularizer_fraction: 0 + +Kp_for_ee_pd_rollout: 100 +Kd_for_ee_pd_rollout: 0.5 + +### Pose tracking cost parameters +# Matrix scaling +w_Q: 45 +w_R: 1 +w_G: 0.25 +w_U: 0.26 + +q_vector: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 200, 200, 120, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x: [800, 800, 800, 100, 100, 100, 100, 10, 10, 10, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +g_lambda_list: [ + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] +g_u: [30, 30, 30] + +# Penalty on matching the QP variables +u_x: [10, 10, 10, 50, 50, 50, 50, 50, 50, 50, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +# u_lambda_list is not used for the QP projection; overwritten by alpha*F. +u_lambda_list: [ + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u: [0.01, 0.01, 0.01] + +### Position tracking cost parameters +# Matrix scaling +w_Q_position: 45 +w_R_position: 1 +w_G_position: 0.25 +w_U_position: 0.26 + +q_vector_position: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 1500, 1500, 1500, # object position + 5, 5, 5, # EE linear velocity + 0.01, 0.01, 0.01, # object angular velocity + 1, 1, 1] # object linear velocity +r_vector_position: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x_position: [800, 800, 800, 10, 10, 10, 10, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +g_lambda_position_list: [ + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] +g_u_position: [30, 30, 30] + +# Penalty on matching the QP variables +u_x_position: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +# u_lambda_list is not used for the QP projection; overwritten by alpha*F. +u_lambda_position_list: [ + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u_position: [0.01, 0.01, 0.01] + + +### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but +### are overwritten by other sampling C3 parameters. +use_predicted_x0: false # instead: use_predicted_x0_c3, + # use_predicted_x0_repos, + # use_predicted_x0_reset_mechanism +dt: 0 # instead: planning_dt_pose, planning_dt_position +solve_dt: 0 # unused +mu: [] # instead based on indexing into mu_per_pair_type +num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists +# Instead for the below, index into their _list versions. +g_gamma: [] +g_lambda_n: [] +g_lambda_t: [] +g_lambda: [] +u_gamma: [] +u_lambda_n: [] +u_lambda_t: [] +u_lambda: [] diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml new file mode 100644 index 0000000000..4c0c606a56 --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml @@ -0,0 +1,45 @@ +# Sampling strategies (defined as SamplingStrategy enum in sampling_params.h): +# 0. kRadiallySymmetric: radially distributed samples on a planar circle. +# 1. kRandomOnCircle: random samples on a planar circle. +# 2. kRandomOnSphere: random samples on a spherical surface. +# 3. kFixed: fixed set of samples. +# 4. kRandomOnPerimeter: random samples on a perimeter offset past the object +# surface (roughly planar). +# 5. kRandomOnShell: random samples on a shell offset past the object +# surface. +sampling_strategy: 2 + +### Parameters relevant for all sampling strategies. +filter_samples_for_safety: true + +# Number of samples. Add 2 for repositioning (current location, previous +# repositioning target) and 1 for C3 (current location) to get total sample +# number solved in parallel for each control loop. +num_additional_samples_repos: 1 +num_additional_samples_c3: 2 + +# Sample buffer parameters. +consider_best_buffer_sample_when_leaving_c3: true +N_sample_buffer: 100 +pos_error_sample_retention: 0.005 +ang_error_sample_retention: 0.087 # 0.087 rad = 5 deg + +# Shared across multiple sampling strategies. +sampling_radius: 0.10 # kRadiallySymmetric, kRandomOnCircle, kRandomOnSphere +sampling_height: 0.04 # kRadiallySymmetric, kRandomOnCircle, kRandomOnPerimeter +sample_projection_clearance: 0.008 # kRandomOnPerimeter, kRandomOnShell +min_angle_from_vertical: 0.2 # kRandomOnSphere, kRandomOnShell +max_angle_from_vertical: 1.95 # kRandomOnSphere, kRandomOnShell + +# kFixed parameters. +fixed_sample_locations: [[0.45, 0.340, 0.04], # sample 1 + [0.27, 0.455, 0.04], # sample 2 + [0.27, 0.225, 0.04]] # sample 3 + +# kRandomOnPerimeter parameters. +grid_x_limits: [-0.16, 0.16] +grid_y_limits: [-0.16, 0.16] + +# kRandomOnShell parameters. +min_sampling_radius: 0.07 +max_sampling_radius: 0.10 diff --git a/examples/sampling_c3/jacktoy/parameters/sim_params.yaml b/examples/sampling_c3/jacktoy/parameters/sim_params.yaml new file mode 100644 index 0000000000..2f38315515 --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/sim_params.yaml @@ -0,0 +1,14 @@ +object_model: examples/sampling_c3/urdf/jack.sdf + +franka_publish_rate: 1000 +object_publish_rate: 10 +actuator_delay: 0.000 + +visualize_drake_sim: false +publish_efforts: true + +dt: 0.0001 +realtime_rate: 1 + +q_init_franka: [2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08] +q_init_object: [-0.3347435, -0.026718954, 0.8878204, 0.31518626, 0.4, 0.30, 0.03] diff --git a/examples/sampling_c3/jacktoy/parameters/vis_params.yaml b/examples/sampling_c3/jacktoy/parameters/vis_params.yaml new file mode 100644 index 0000000000..7102d4aaf2 --- /dev/null +++ b/examples/sampling_c3/jacktoy/parameters/vis_params.yaml @@ -0,0 +1,37 @@ +ee_vis_model: examples/sampling_c3/urdf/ee_visualization_model.urdf +object_vis_model: examples/sampling_c3/urdf/jack.sdf +visualizer_publish_rate: 30 + +camera_pose: [1.5, 0, 0.6] +camera_target: [0.5, 0, 0.5] + +visualize_c3_workspace: false +visualize_c3_state: true + +visualize_center_of_mass_plan_curr: false +visualize_c3_forces_curr: false +visualize_center_of_mass_plan_best: false +visualize_c3_forces_best: false + +visualize_is_c3_mode: true +visualize_sample_locations: true +visualize_sample_buffer: true + +# Note: Colors can either be empty ([]) or RGB values between 0 and 1. If left +# empty, the colors defined in the URDF/SDF files will be used. +is_c3_mode_color: [1.0, 0.43, 1.0] +sample_color: [1.0, 1.0, 0.0] + +visualize_execution_plan: false + +visualize_c3_plan_curr: false +c3_curr_object_color: [] +c3_curr_ee_color: [1.0, 1.0, 1.0] +df_curr_object_color: [] +df_curr_ee_color: [0.5, 1.0, 0.5] + +visualize_c3_plan_best: false +c3_best_object_color: [1.0, 0.64, 0.0] +c3_best_ee_color: [1.0, 0.64, 0.0] +df_best_object_color: [1.0, 0.64, 0.0] +df_best_ee_color: [1.0, 0.64, 0.0] diff --git a/examples/sampling_c3/joint_trajectory_generator.cc b/examples/sampling_c3/joint_trajectory_generator.cc new file mode 100644 index 0000000000..fbd40e2151 --- /dev/null +++ b/examples/sampling_c3/joint_trajectory_generator.cc @@ -0,0 +1,108 @@ +#include "joint_trajectory_generator.h" + +#include "systems/framework/output_vector.h" + +using Eigen::Map; +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::string; + +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteUpdateEvent; +using drake::systems::DiscreteValues; +using drake::systems::EventStatus; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +namespace dairlib { + +using systems::OutputVector; + +JointTrajectoryGenerator::JointTrajectoryGenerator( + const drake::multibody::MultibodyPlant& plant, + const Eigen::VectorXd& target_position) + : target_position_(target_position) { + // Input/Output Setup + PiecewisePolynomial pp = PiecewisePolynomial(); + + radio_port_ = + this->DeclareVectorInputPort("lcmt_radio_out", BasicVector(18)) + .get_index(); + state_port_ = + this->DeclareVectorInputPort( + "x_franka", OutputVector(plant.num_positions(), + plant.num_velocities(), plant.num_actuators())) + .get_index(); + + initial_position_index_ = + this->DeclareDiscreteState(VectorXd::Zero(plant.num_positions())); + initial_time_index_ = this->DeclareDiscreteState(VectorXd::Zero(1)); + + DeclareForcedDiscreteUpdateEvent( + &JointTrajectoryGenerator::DiscreteVariableUpdate); + + joint_trajectory_ports_.resize(plant.num_positions()); + for (int i = 0; i < plant.num_positions(); ++i) { + joint_trajectory_ports_[i] = + this->DeclareAbstractOutputPort( + "joint_traj_" + std::to_string(i), + []() { + return drake::AbstractValue::Make>( + PiecewisePolynomial(VectorXd::Zero(1))); + }, + [this, i](const Context& context, drake::AbstractValue* traj){ + this->CalcTraj(i, context, traj); + }) + .get_index(); + } +} + +drake::systems::EventStatus JointTrajectoryGenerator::DiscreteVariableUpdate( + const Context& context, + drake::systems::DiscreteValues* discrete_state) const { + auto initial_positions = + discrete_state->get_mutable_value(initial_position_index_); + auto initial_time = discrete_state->get_mutable_value(initial_time_index_); + const OutputVector* franka_output = + (OutputVector*)this->EvalVectorInput(context, state_port_); + if (initial_time[0] == 0.0) { // poll once + initial_positions = franka_output->GetPositions(); + initial_time[0] = context.get_time(); + } + return drake::systems::EventStatus::Succeeded(); +} + +void JointTrajectoryGenerator::CalcTraj( + int joint_index, const drake::systems::Context& context, + drake::AbstractValue* output) const { + auto output_value = &output->get_mutable_value>(); + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + output_value); + const OutputVector* franka_output = + (OutputVector*)this->EvalVectorInput(context, state_port_); + auto initial_positions = + context.get_discrete_state(initial_position_index_).value(); + auto initial_time = context.get_discrete_state(initial_time_index_).value(); + + const auto& radio_out = this->EvalVectorInput(context, radio_port_); + + std::vector breaks = {initial_time[0] + 1.0, initial_time[0] + 6.0}; + std::vector sampled_positions(2); + sampled_positions[0] = MatrixXd::Zero(1, 1); + sampled_positions[0] << initial_positions[joint_index]; + sampled_positions[1] = MatrixXd::Zero(1, 1); + sampled_positions[1] << target_position_[joint_index]; + if (context.get_time() >= 1.0) { // poll for a second + *casted_traj = + PiecewisePolynomial::CubicWithContinuousSecondDerivatives( + breaks, sampled_positions, MatrixXd::Zero(1, 1), + MatrixXd::Zero(1, 1)); + } else { + *casted_traj = PiecewisePolynomial( + franka_output->GetPositions().segment(joint_index, 1)); + } +} + +} // namespace dairlib diff --git a/examples/sampling_c3/joint_trajectory_generator.h b/examples/sampling_c3/joint_trajectory_generator.h new file mode 100644 index 0000000000..0d8b46412e --- /dev/null +++ b/examples/sampling_c3/joint_trajectory_generator.h @@ -0,0 +1,44 @@ +#pragma once + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class JointTrajectoryGenerator : public drake::systems::LeafSystem { + public: + JointTrajectoryGenerator( + const drake::multibody::MultibodyPlant& plant, + const Eigen::VectorXd& target_position); + + const drake::systems::InputPort& get_input_port_robot_state() const { + return this->get_input_port(state_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + const drake::systems::OutputPort& get_output_port_joint( + int joint_index) const { + return this->get_output_port(joint_trajectory_ports_[joint_index]); + } + + private: + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void CalcTraj(int joint_index, const drake::systems::Context& context, + drake::AbstractValue* traj) const; + + drake::systems::InputPortIndex state_port_; + drake::systems::InputPortIndex radio_port_; + std::vector joint_trajectory_ports_; + drake::systems::DiscreteStateIndex initial_position_index_; + drake::systems::DiscreteStateIndex initial_time_index_; + const Eigen::VectorXd target_position_; + + double default_speed = 0.1; // rad/s +}; + +} // namespace dairlib diff --git a/examples/sampling_c3/parameter_headers/BUILD.bazel b/examples/sampling_c3/parameter_headers/BUILD.bazel new file mode 100644 index 0000000000..8377beeb8f --- /dev/null +++ b/examples/sampling_c3/parameter_headers/BUILD.bazel @@ -0,0 +1,71 @@ +cc_library( + name = "sampling_c3_options", + hdrs = ["sampling_c3_options.h"], + visibility = ["//visibility:public"], # Allow all subpackages to use it + deps = ["//solvers:c3"] +) + +cc_library( + name = "franka_sim_params", + hdrs = ["franka_sim_params.h"], + visibility = ["//visibility:public"], +) + +cc_library( + name = "visualizer_params", + hdrs = ["visualizer_params.h"], + visibility = ["//visibility:public"], +) + +cc_library( + name = "lcm_channels", + hdrs = ["lcm_channels.h"], + visibility = ["//visibility:public"], +) + +cc_library( + name = "sampling_c3_controller_params", + hdrs = ["sampling_c3_controller_params.h"], + visibility = ["//visibility:public"], + deps = [ + "//examples/sampling_c3/parameter_headers:sampling_c3_options", + "//examples/sampling_c3/parameter_headers:reposition_params", + "//examples/sampling_c3/parameter_headers:progress_params", + "//examples/sampling_c3/parameter_headers:sampling_params", + "//examples/sampling_c3/parameter_headers:goal_params", + ], +) + +cc_library( + name = "osc_params", + hdrs = ["osc_params.h"], + visibility = ["//visibility:public"], +) + +cc_library( + name = "goal_params", + hdrs = ["goal_params.h"], + visibility = ["//visibility:public"], + deps = ["//common:file_utils"], +) + +cc_library( + name = "sampling_params", + hdrs = ["sampling_params.h"], + visibility = ["//visibility:public"], + deps = ["//common:file_utils"], +) + +cc_library( + name = "reposition_params", + hdrs = ["reposition_params.h"], + visibility = ["//visibility:public"], + deps = ["//common:file_utils"], +) + +cc_library( + name = "progress_params", + hdrs = ["progress_params.h"], + visibility = ["//visibility:public"], + deps = ["//common:file_utils"], +) diff --git a/examples/sampling_c3/parameter_headers/franka_sim_params.h b/examples/sampling_c3/parameter_headers/franka_sim_params.h new file mode 100644 index 0000000000..e65125e7bc --- /dev/null +++ b/examples/sampling_c3/parameter_headers/franka_sim_params.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include "drake/common/yaml/yaml_read_archive.h" + +struct FrankaSimParams { + std::string object_model; + double dt; + double realtime_rate; + double actuator_delay; + double franka_publish_rate; + double object_publish_rate; + bool visualize_drake_sim; + bool publish_efforts; + Eigen::VectorXd q_init_franka; + Eigen::VectorXd q_init_object; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(object_model)); + a->Visit(DRAKE_NVP(dt)); + a->Visit(DRAKE_NVP(realtime_rate)); + a->Visit(DRAKE_NVP(actuator_delay)); + a->Visit(DRAKE_NVP(franka_publish_rate)); + a->Visit(DRAKE_NVP(object_publish_rate)); + a->Visit(DRAKE_NVP(visualize_drake_sim)); + a->Visit(DRAKE_NVP(publish_efforts)); + a->Visit(DRAKE_NVP(q_init_franka)); + a->Visit(DRAKE_NVP(q_init_object)); + } +}; diff --git a/examples/sampling_c3/parameter_headers/goal_params.h b/examples/sampling_c3/parameter_headers/goal_params.h new file mode 100644 index 0000000000..656a38aff1 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/goal_params.h @@ -0,0 +1,71 @@ +#pragma once + +#include "common/file_utils.h" +#include "drake/common/yaml/yaml_read_archive.h" + + +/* Goal mode options: + 0. kRandom: randomly generate a new goal. + 1. kOrientationSequence: keep position goal the same, and cycle through a + sequence of orientations. + 2. kFixedGoal: keep the same goal. +*/ +enum GoalMode { + kRandom, + kOrientationSequence, + kFixedGoal +}; + +struct SamplingC3GoalParams { + GoalMode goal_mode; + + /// Parameters used for multiple goal modes. + /// Success thresholds for position and orientation. + double position_success_threshold; + double orientation_success_threshold; + + double resting_object_height; // in world frame + double ee_target_z_offset_above_object; // defines EE goal wrt object height + + /// Lookahead parameters to define a sub-goal for C3. + double lookahead_step_size; + double lookahead_angle; + + /// Apply hysteresis on the lookahead angle so the sub-goal orientation does + /// not flip back and forth near the 180 degree error singularity. A lower + /// number means the sub-goal orientation can switch more often; a higher + /// number means the sub-goal orientation is more stable but possibly less + /// optimal. + double angle_hysteresis; + + /// Factor to convert an angular error to angular velocity command (a value of + /// 0 disables this feature).) + double angle_err_to_vel_factor; + + /// Initial goal (and only goal for fixed goal mode). + Eigen::VectorXd fixed_target_position; + Eigen::VectorXd fixed_target_orientation; + + /// Random-specific parameters. + Eigen::VectorXd random_goal_x_limits; + Eigen::VectorXd random_goal_y_limits; + Eigen::VectorXd random_goal_radius_limits; + + template + void Serialize(Archive* a) { + ENUM_DESERIALIZE(a, goal_mode); + a->Visit(DRAKE_NVP(position_success_threshold)); + a->Visit(DRAKE_NVP(orientation_success_threshold)); + a->Visit(DRAKE_NVP(resting_object_height)); + a->Visit(DRAKE_NVP(ee_target_z_offset_above_object)); + a->Visit(DRAKE_NVP(lookahead_step_size)); + a->Visit(DRAKE_NVP(lookahead_angle)); + a->Visit(DRAKE_NVP(angle_hysteresis)); + a->Visit(DRAKE_NVP(angle_err_to_vel_factor)); + a->Visit(DRAKE_NVP(fixed_target_position)); + a->Visit(DRAKE_NVP(fixed_target_orientation)); + a->Visit(DRAKE_NVP(random_goal_x_limits)); + a->Visit(DRAKE_NVP(random_goal_y_limits)); + a->Visit(DRAKE_NVP(random_goal_radius_limits)); + } +}; diff --git a/examples/sampling_c3/parameter_headers/lcm_channels.h b/examples/sampling_c3/parameter_headers/lcm_channels.h new file mode 100644 index 0000000000..6413613956 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/lcm_channels.h @@ -0,0 +1,82 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + +struct SamplingC3LcmChannels { + std::string franka_state_channel; + std::string object_state_channel; + std::string franka_input_channel; + std::string osc_channel; + std::string osc_debug_channel; + + std::string c3_actor_curr_plan_channel; + std::string c3_object_curr_plan_channel; + std::string c3_force_curr_channel; + std::string c3_debug_output_curr_channel; + + std::string c3_actor_best_plan_channel; + std::string c3_object_best_plan_channel; + std::string c3_force_best_channel; + std::string c3_debug_output_best_channel; + + std::string c3_trajectory_exec_actor_channel; + std::string repos_trajectory_exec_actor_channel; + std::string tracking_trajectory_actor_channel; + std::string tracking_trajectory_object_channel; + + std::string c3_final_target_state_channel; + std::string c3_target_state_channel; + std::string c3_actual_state_channel; + std::string radio_channel; + + std::string sample_locations_channel; + std::string sample_buffer_channel; + std::string dynamically_feasible_curr_actor_plan_channel; + std::string dynamically_feasible_curr_plan_channel; + std::string dynamically_feasible_best_actor_plan_channel; + std::string dynamically_feasible_best_plan_channel; + std::string sample_costs_channel; + std::string sampling_c3_debug_channel; + std::string is_c3_mode_channel; + std::string target_generator_info_channel; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(franka_state_channel)); + a->Visit(DRAKE_NVP(object_state_channel)); + a->Visit(DRAKE_NVP(franka_input_channel)); + a->Visit(DRAKE_NVP(osc_channel)); + a->Visit(DRAKE_NVP(osc_debug_channel)); + + a->Visit(DRAKE_NVP(c3_actor_curr_plan_channel)); + a->Visit(DRAKE_NVP(c3_object_curr_plan_channel)); + a->Visit(DRAKE_NVP(c3_force_curr_channel)); + a->Visit(DRAKE_NVP(c3_debug_output_curr_channel)); + + a->Visit(DRAKE_NVP(c3_actor_best_plan_channel)); + a->Visit(DRAKE_NVP(c3_object_best_plan_channel)); + a->Visit(DRAKE_NVP(c3_force_best_channel)); + a->Visit(DRAKE_NVP(c3_debug_output_best_channel)); + + a->Visit(DRAKE_NVP(c3_trajectory_exec_actor_channel)); + a->Visit(DRAKE_NVP(repos_trajectory_exec_actor_channel)); + a->Visit(DRAKE_NVP(tracking_trajectory_actor_channel)); + a->Visit(DRAKE_NVP(tracking_trajectory_object_channel)); + + a->Visit(DRAKE_NVP(c3_final_target_state_channel)); + a->Visit(DRAKE_NVP(c3_target_state_channel)); + a->Visit(DRAKE_NVP(c3_actual_state_channel)); + a->Visit(DRAKE_NVP(radio_channel)); + + a->Visit(DRAKE_NVP(sample_locations_channel)); + a->Visit(DRAKE_NVP(sample_buffer_channel)); + a->Visit(DRAKE_NVP(dynamically_feasible_curr_actor_plan_channel)); + a->Visit(DRAKE_NVP(dynamically_feasible_curr_plan_channel)); + a->Visit(DRAKE_NVP(dynamically_feasible_best_actor_plan_channel)); + a->Visit(DRAKE_NVP(dynamically_feasible_best_plan_channel)); + a->Visit(DRAKE_NVP(sample_costs_channel)); + a->Visit(DRAKE_NVP(sampling_c3_debug_channel)); + a->Visit(DRAKE_NVP(is_c3_mode_channel)); + a->Visit(DRAKE_NVP(target_generator_info_channel)); + } +}; diff --git a/examples/sampling_c3/parameter_headers/osc_params.h b/examples/sampling_c3/parameter_headers/osc_params.h new file mode 100644 index 0000000000..187d4876f2 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/osc_params.h @@ -0,0 +1,106 @@ +#pragma once + +#include "systems/controllers/osc/osc_gains.h" + +#include "drake/common/yaml/yaml_read_archive.h" + +/// Extends OSCGains to include parameters specific to sampling C3 example. +struct SamplingC3OSCParams : OSCGains { + /// Teleop params: There are two teleoperation modes: + /// 1) locked EE neutral position where xbox controls perturb the EE about + /// this neutral position (corresponds to teleop_neutral_position = false). + /// 2) free EE neutral position where the EE can be moved freely in space + /// where xbox controls change the EE resting position (corresponds to + /// teleop_neutral_position = true). + Eigen::VectorXd neutral_position; + bool teleop_neutral_position; + double x_scale; + double y_scale; + double z_scale; + + double end_effector_acceleration; + bool track_end_effector_orientation; + bool cancel_gravity_compensation; + bool enforce_acceleration_constraints; + bool publish_debug_info; + + /// Joint 2 position tracking weight values. It is helpful to introduce a + /// target position for the elbow joint to avoid singularities. NOTE: the + /// target value is currently hardcoded in franka_osc_controller.cc. + double w_elbow; + double elbow_kp; + double elbow_kd; + /// End effector position tracking weight matrix values. + std::vector EndEffectorW; + std::vector EndEffectorKp; + std::vector EndEffectorKd; + /// End effector orientation tracking weight matrix values. + std::vector EndEffectorRotW; + std::vector EndEffectorRotKp; + std::vector EndEffectorRotKd; + /// End effector force tracking weight matrix values. + std::vector LambdaEndEffectorW; + + Eigen::MatrixXd W_mid_link; + Eigen::MatrixXd K_p_mid_link; + Eigen::MatrixXd K_d_mid_link; + Eigen::MatrixXd W_end_effector; + Eigen::MatrixXd K_p_end_effector; + Eigen::MatrixXd K_d_end_effector; + Eigen::MatrixXd W_end_effector_rot; + Eigen::MatrixXd K_p_end_effector_rot; + Eigen::MatrixXd K_d_end_effector_rot; + Eigen::MatrixXd W_ee_lambda; + + template + void Serialize(Archive* a) { + OSCGains::Serialize(a); + a->Visit(DRAKE_NVP(neutral_position)); + a->Visit(DRAKE_NVP(teleop_neutral_position)); + a->Visit(DRAKE_NVP(x_scale)); + a->Visit(DRAKE_NVP(y_scale)); + a->Visit(DRAKE_NVP(z_scale)); + a->Visit(DRAKE_NVP(end_effector_acceleration)); + a->Visit(DRAKE_NVP(track_end_effector_orientation)); + a->Visit(DRAKE_NVP(cancel_gravity_compensation)); + a->Visit(DRAKE_NVP(enforce_acceleration_constraints)); + a->Visit(DRAKE_NVP(publish_debug_info)); + a->Visit(DRAKE_NVP(w_elbow)); + a->Visit(DRAKE_NVP(elbow_kp)); + a->Visit(DRAKE_NVP(elbow_kd)); + a->Visit(DRAKE_NVP(EndEffectorW)); + a->Visit(DRAKE_NVP(EndEffectorKp)); + a->Visit(DRAKE_NVP(EndEffectorKd)); + a->Visit(DRAKE_NVP(EndEffectorRotW)); + a->Visit(DRAKE_NVP(EndEffectorRotKp)); + a->Visit(DRAKE_NVP(EndEffectorRotKd)); + a->Visit(DRAKE_NVP(LambdaEndEffectorW)); + + // Weight matrix for end effector position tracking. + W_end_effector = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorW.data(), 3, 3); + K_p_end_effector = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorKp.data(), 3, 3); + K_d_end_effector = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorKd.data(), 3, 3); + W_mid_link = this->w_elbow * MatrixXd::Identity(1, 1); + K_p_mid_link = this->elbow_kp * MatrixXd::Identity(1, 1); + K_d_mid_link = this->elbow_kd * MatrixXd::Identity(1, 1); + W_end_effector_rot = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorRotW.data(), 3, 3); + K_p_end_effector_rot = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorRotKp.data(), 3, 3); + K_d_end_effector_rot = Eigen::Map< + Eigen::Matrix>( + this->EndEffectorRotKd.data(), 3, 3); + // Weight matrix for ee force tracking. + W_ee_lambda = Eigen::Map< + Eigen::Matrix>( + this->LambdaEndEffectorW.data(), 3, 3); + } +}; diff --git a/examples/sampling_c3/parameter_headers/progress_params.h b/examples/sampling_c3/parameter_headers/progress_params.h new file mode 100644 index 0000000000..ed4cca9e2f --- /dev/null +++ b/examples/sampling_c3/parameter_headers/progress_params.h @@ -0,0 +1,77 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + +#include "common/file_utils.h" +#include "solvers/c3_options.h" + + +/* C3 progress metric options, all phrased as improvement requirements over a + number of control loops: + 0. kC3Cost: C3 cost. + 1. kConfigCost: Current object configuration cost. + 2. kPosOrRotCost: Current position or rotation error. + 3. kConfigCostDrop: Drop in object configuration cost (this is the same as + kConfigCost if the required drop is 0; a more aggressive + drop cuts C3 off earlier). +*/ +enum ProgressMetric { + kC3Cost, + kConfigCost, + kPosOrRotCost, + kConfigCostDrop +}; + + +struct SamplingC3ProgressParams { + C3CostComputationType cost_type; + C3CostComputationType cost_type_position; + int num_control_loops_to_wait; + int num_control_loops_to_wait_position; + ProgressMetric track_c3_progress_via; + double progress_enforced_cost_drop; + int progress_enforced_over_n_loops; + double cost_switching_threshold_distance; + double travel_cost_per_meter; + double hyst_c3_to_repos; + double hyst_c3_to_repos_position; + double finished_reposition_cost; + double hyst_repos_to_c3; + double hyst_repos_to_c3_position; + double hyst_repos_to_repos; + double hyst_repos_to_repos_position; + bool use_relative_hysteresis; + double hyst_c3_to_repos_frac; + double hyst_repos_to_c3_frac; + double hyst_repos_to_repos_frac; + double hyst_c3_to_repos_frac_position; + double hyst_repos_to_c3_frac_position; + double hyst_repos_to_repos_frac_position; + + template + void Serialize(Archive* a) { + ENUM_DESERIALIZE(a, cost_type); + ENUM_DESERIALIZE(a, cost_type_position); + ENUM_DESERIALIZE(a, track_c3_progress_via); + a->Visit(DRAKE_NVP(num_control_loops_to_wait)); + a->Visit(DRAKE_NVP(num_control_loops_to_wait_position)); + a->Visit(DRAKE_NVP(progress_enforced_cost_drop)); + a->Visit(DRAKE_NVP(progress_enforced_over_n_loops)); + a->Visit(DRAKE_NVP(cost_switching_threshold_distance)); + a->Visit(DRAKE_NVP(travel_cost_per_meter)); + a->Visit(DRAKE_NVP(hyst_c3_to_repos)); + a->Visit(DRAKE_NVP(hyst_c3_to_repos_position)); + a->Visit(DRAKE_NVP(finished_reposition_cost)); + a->Visit(DRAKE_NVP(hyst_repos_to_c3)); + a->Visit(DRAKE_NVP(hyst_repos_to_c3_position)); + a->Visit(DRAKE_NVP(hyst_repos_to_repos)); + a->Visit(DRAKE_NVP(hyst_repos_to_repos_position)); + a->Visit(DRAKE_NVP(use_relative_hysteresis)); + a->Visit(DRAKE_NVP(hyst_c3_to_repos_frac)); + a->Visit(DRAKE_NVP(hyst_repos_to_c3_frac)); + a->Visit(DRAKE_NVP(hyst_repos_to_repos_frac)); + a->Visit(DRAKE_NVP(hyst_c3_to_repos_frac_position)); + a->Visit(DRAKE_NVP(hyst_repos_to_c3_frac_position)); + a->Visit(DRAKE_NVP(hyst_repos_to_repos_frac_position)); + } +}; diff --git a/examples/sampling_c3/parameter_headers/reposition_params.h b/examples/sampling_c3/parameter_headers/reposition_params.h new file mode 100644 index 0000000000..9b942cfe63 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/reposition_params.h @@ -0,0 +1,62 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + +#include "common/file_utils.h" + + +/* Repositioning trajectory type options: + 0. kSpline: move from current to target with spline that distorts + around the object. + 1. kSpherical: move away from object to a radius, along the spherical + surface to outside target, then in to target. + 2. kCircular: (planar) move away from object to a planar radius, + along the circle to outside target, then in to target. + 3. kPiecewiseLinear: move up to waypoint height, over to above target, then + down. +*/ +enum RepositioningTrajectoryType { + kSpline, + kSpherical, + kCircular, + kPiecewiseLinear +}; + +struct SamplingC3RepositionParams { + RepositioningTrajectoryType traj_type; + + /// Parameters used for multiple repositioning trajectory types. + double speed; + + /// Thresholds for switching to straight line trajectories. + double use_straight_line_traj_under_spline; + double use_straight_line_traj_within_angle; + double use_straight_line_traj_under_piecewise_linear; + + /// Spline-specific parameters. + double spline_width; + + /// Sphere-specific parameters. + double sphere_radius; + + /// Circle-specific parameters. + double circle_radius; + double circle_height; + + /// Piecewise-linear-specific parameters. + double pwl_waypoint_height; + + template + void Serialize(Archive* a) { + ENUM_DESERIALIZE(a, traj_type); + a->Visit(DRAKE_NVP(speed)); + a->Visit(DRAKE_NVP(use_straight_line_traj_under_spline)); + a->Visit(DRAKE_NVP(use_straight_line_traj_within_angle)); + a->Visit(DRAKE_NVP(use_straight_line_traj_under_piecewise_linear)); + a->Visit(DRAKE_NVP(spline_width)); + a->Visit(DRAKE_NVP(sphere_radius)); + a->Visit(DRAKE_NVP(circle_radius)); + a->Visit(DRAKE_NVP(circle_height)); + a->Visit(DRAKE_NVP(pwl_waypoint_height)); + } +}; diff --git a/examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h b/examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h new file mode 100644 index 0000000000..8cd9bf9cb0 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h @@ -0,0 +1,75 @@ +#pragma once + +#include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" +#include "examples/sampling_c3/parameter_headers/reposition_params.h" +#include "examples/sampling_c3/parameter_headers/progress_params.h" +#include "examples/sampling_c3/parameter_headers/sampling_params.h" +#include "examples/sampling_c3/parameter_headers/goal_params.h" + +#include "drake/common/yaml/yaml_read_archive.h" +#include + + +struct SamplingC3ControllerParams { + std::string sampling_c3_options_file; // C3 mode params + std::string reposition_params_file; // repositioning mode params + std::string progress_params_file; // mode switching params + std::string sampling_params_file; // sampling params + std::string goal_params_file; // goal checking/setting params + + std::string sim_params_file; // simulation params + std::string vis_params_file; // visualization params + std::string osc_params_file; // OSC params + std::string osqp_settings_file; + std::string osc_qp_settings_file; + std::string franka_driver_channels_file; + std::string lcm_channels_hardware_file; + std::string lcm_channels_simulation_file; + + std::string object_model; + std::string object_body_name; + + double workspace_margin; + bool include_end_effector_orientation; + int control_loop_delay_ms; + + /// Store sub-parameter classes internally. + SamplingC3Options sampling_c3_options; + SamplingC3RepositionParams reposition_params; + SamplingC3ProgressParams progress_params; + SamplingParams sampling_params; + SamplingC3GoalParams goal_params; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(sampling_c3_options_file)); + a->Visit(DRAKE_NVP(reposition_params_file)); + a->Visit(DRAKE_NVP(progress_params_file)); + a->Visit(DRAKE_NVP(sampling_params_file)); + a->Visit(DRAKE_NVP(goal_params_file)); + a->Visit(DRAKE_NVP(sim_params_file)); + a->Visit(DRAKE_NVP(vis_params_file)); + a->Visit(DRAKE_NVP(osc_params_file)); + a->Visit(DRAKE_NVP(osqp_settings_file)); + a->Visit(DRAKE_NVP(osc_qp_settings_file)); + a->Visit(DRAKE_NVP(franka_driver_channels_file)); + a->Visit(DRAKE_NVP(lcm_channels_hardware_file)); + a->Visit(DRAKE_NVP(lcm_channels_simulation_file)); + a->Visit(DRAKE_NVP(object_model)); + a->Visit(DRAKE_NVP(object_body_name)); + a->Visit(DRAKE_NVP(include_end_effector_orientation)); + a->Visit(DRAKE_NVP(control_loop_delay_ms)); + + /// Store individual parameter classes internally. + sampling_c3_options = drake::yaml::LoadYamlFile( + sampling_c3_options_file); + reposition_params = drake::yaml::LoadYamlFile( + reposition_params_file); + progress_params = drake::yaml::LoadYamlFile( + progress_params_file); + sampling_params = drake::yaml::LoadYamlFile( + sampling_params_file); + goal_params = drake::yaml::LoadYamlFile( + goal_params_file); + } +}; diff --git a/examples/sampling_c3/parameter_headers/sampling_c3_options.h b/examples/sampling_c3/parameter_headers/sampling_c3_options.h new file mode 100644 index 0000000000..4a41f2ea2c --- /dev/null +++ b/examples/sampling_c3/parameter_headers/sampling_c3_options.h @@ -0,0 +1,298 @@ +#pragma once +#include + +#include "solvers/c3_options.h" + +#include "drake/common/yaml/yaml_read_archive.h" + +struct SamplingC3Options : C3Options { + int num_outer_threads; // for outer sampling loop. + + /// Additional radius-based workspace limit. + std::vector robot_radius_limits; + + /// Whether to use a predicted EE location for each mode. + bool use_predicted_x0_c3; + bool use_predicted_x0_repos; + bool use_predicted_x0_reset_mechanism; // Resets if prediction is too far. + + /// Contact pair parameters. + std::vector mu_per_pair_type; + std::vector> resolve_contacts_to_lists; + std::vector resolve_contacts_to; + std::vector resolve_contacts_to_for_cost; + int num_contacts_index; + int num_contacts_index_for_cost; + std::vector mu_for_cost; + int num_contacts_for_cost; + + double planning_dt_pose; + double planning_dt_position; + + /// Cost parameters. + bool use_quaternion_dependent_cost; + double q_quaternion_dependent_weight; + double q_quaternion_dependent_regularizer_fraction; + + double Kp_for_ee_pd_rollout; + double Kd_for_ee_pd_rollout; + + /// Cost parameters for pose tracking. + std::vector> g_gamma_list; + std::vector> g_lambda_n_list; + std::vector> g_lambda_t_list; + std::vector> g_lambda_list; + + std::vector> u_gamma_list; + std::vector> u_lambda_n_list; + std::vector> u_lambda_t_list; + std::vector> u_lambda_list; + + /// Cost parameters for position tracking. + double w_Q_position; + double w_R_position; + double w_G_position; + double w_U_position; + std::vector q_vector_position; + std::vector r_vector_position; + + std::vector g_x_position; + std::vector> g_gamma_position_list; + std::vector> g_lambda_n_position_list; + std::vector> g_lambda_t_position_list; + std::vector> g_lambda_position_list; + std::vector g_u_position; + + std::vector u_x_position; + std::vector> u_gamma_position_list; + std::vector> u_lambda_n_position_list; + std::vector> u_lambda_t_position_list; + std::vector> u_lambda_position_list; + std::vector u_u_position; + + C3Options c3_options_pose; + C3Options c3_options_position; + + template + void Serialize(Archive* a) { + C3Options::Serialize(a); + a->Visit(DRAKE_NVP(num_outer_threads)); + a->Visit(DRAKE_NVP(robot_radius_limits)); + a->Visit(DRAKE_NVP(use_predicted_x0_c3)); + a->Visit(DRAKE_NVP(use_predicted_x0_repos)); + a->Visit(DRAKE_NVP(use_predicted_x0_reset_mechanism)); + + a->Visit(DRAKE_NVP(mu_per_pair_type)); + a->Visit(DRAKE_NVP(resolve_contacts_to_lists)); + a->Visit(DRAKE_NVP(num_contacts_index)); + a->Visit(DRAKE_NVP(num_contacts_index_for_cost)); + + a->Visit(DRAKE_NVP(planning_dt_pose)); + a->Visit(DRAKE_NVP(planning_dt_position)); + + a->Visit(DRAKE_NVP(use_quaternion_dependent_cost)); + a->Visit(DRAKE_NVP(q_quaternion_dependent_weight)); + a->Visit(DRAKE_NVP(q_quaternion_dependent_regularizer_fraction)); + + a->Visit(DRAKE_NVP(Kp_for_ee_pd_rollout)); + a->Visit(DRAKE_NVP(Kd_for_ee_pd_rollout)); + + a->Visit(DRAKE_NVP(g_gamma_list)); + a->Visit(DRAKE_NVP(g_lambda_n_list)); + a->Visit(DRAKE_NVP(g_lambda_t_list)); + a->Visit(DRAKE_NVP(g_lambda_list)); + + a->Visit(DRAKE_NVP(u_gamma_list)); + a->Visit(DRAKE_NVP(u_lambda_n_list)); + a->Visit(DRAKE_NVP(u_lambda_t_list)); + a->Visit(DRAKE_NVP(u_lambda_list)); + + a->Visit(DRAKE_NVP(w_Q_position)); + a->Visit(DRAKE_NVP(w_R_position)); + a->Visit(DRAKE_NVP(w_G_position)); + a->Visit(DRAKE_NVP(w_U_position)); + a->Visit(DRAKE_NVP(q_vector_position)); + a->Visit(DRAKE_NVP(r_vector_position)); + + a->Visit(DRAKE_NVP(g_x_position)); + a->Visit(DRAKE_NVP(g_gamma_position_list)); + a->Visit(DRAKE_NVP(g_lambda_n_position_list)); + a->Visit(DRAKE_NVP(g_lambda_t_position_list)); + a->Visit(DRAKE_NVP(g_lambda_position_list)); + a->Visit(DRAKE_NVP(g_u_position)); + + a->Visit(DRAKE_NVP(u_x_position)); + a->Visit(DRAKE_NVP(u_gamma_position_list)); + a->Visit(DRAKE_NVP(u_lambda_n_position_list)); + a->Visit(DRAKE_NVP(u_lambda_t_position_list)); + a->Visit(DRAKE_NVP(u_lambda_position_list)); + a->Visit(DRAKE_NVP(u_u_position)); + + // Set a few parameters based on num_contacts_index, differentiating between + // for C3 solve and for C3 cost computation. + resolve_contacts_to = resolve_contacts_to_lists[num_contacts_index]; + resolve_contacts_to_for_cost = + resolve_contacts_to_lists[num_contacts_index_for_cost]; + num_contacts = std::accumulate( + resolve_contacts_to.begin(), resolve_contacts_to.end(), 0); + num_contacts_for_cost = std::accumulate( + resolve_contacts_to_for_cost.begin(), resolve_contacts_to_for_cost.end(), + 0); + mu.clear(); + for (size_t i = 0; i < mu_per_pair_type.size(); ++i) { + int repeat = resolve_contacts_to_lists[num_contacts_index][i]; + mu.insert(mu.end(), repeat, mu_per_pair_type[i]); + } + mu_for_cost.clear(); + for (size_t i = 0; i < mu_per_pair_type.size(); ++i) { + int repeat = resolve_contacts_to_lists[num_contacts_index_for_cost][i]; + mu_for_cost.insert(mu_for_cost.end(), repeat, mu_per_pair_type[i]); + } + + // Create C3 options for both pose and position tracking. + SetCommonC3Options(&c3_options_pose); + SetPoseTrackingOptions(&c3_options_pose); + SetCommonC3Options(&c3_options_position); + SetPositionTrackingOptions(&c3_options_position); + } + + C3Options GetC3Options(const bool& is_pose_tracking) const { + if (is_pose_tracking) { return c3_options_pose; } + return c3_options_position; + } + + private: + void PopulateCostMatricesFromVectors(C3Options* options) const { + std::vector g_vector = std::vector(); + g_vector.insert(g_vector.end(), options->g_x.begin(), options->g_x.end()); + if (options->contact_model == "stewart_and_trinkle") { + g_vector.insert(g_vector.end(), options->g_gamma.begin(), + options->g_gamma.end()); + g_vector.insert(g_vector.end(), options->g_lambda_n.begin(), + options->g_lambda_n.end()); + g_vector.insert(g_vector.end(), options->g_lambda_t.begin(), + options->g_lambda_t.end()); + } else { + g_vector.insert(g_vector.end(), options->g_lambda.begin(), + options->g_lambda.end()); + } + + g_vector.insert(g_vector.end(), options->g_u.begin(), options->g_u.end()); + + std::vector u_vector = std::vector(); + u_vector.insert(u_vector.end(), options->u_x.begin(), options->u_x.end()); + if (options->contact_model == "stewart_and_trinkle") { + u_vector.insert(u_vector.end(), options->u_gamma.begin(), + options->u_gamma.end()); + u_vector.insert(u_vector.end(), options->u_lambda_n.begin(), + options->u_lambda_n.end()); + u_vector.insert(u_vector.end(), options->u_lambda_t.begin(), + options->u_lambda_t.end()); + } else { + u_vector.insert(u_vector.end(), options->u_lambda.begin(), + options->u_lambda.end()); + } + u_vector.insert(u_vector.end(), options->u_u.begin(), options->u_u.end()); + + options->g_vector = g_vector; + options->u_vector = u_vector; + Eigen::VectorXd q = Eigen::Map( + options->q_vector.data(), options->q_vector.size()); + Eigen::VectorXd r = Eigen::Map( + options->r_vector.data(), options->r_vector.size()); + Eigen::VectorXd g = Eigen::Map( + options->g_vector.data(), options->g_vector.size()); + Eigen::VectorXd u = Eigen::Map( + options->u_vector.data(), options->u_vector.size()); + + options->Q = options->w_Q * q.asDiagonal(); + options->R = options->w_R * r.asDiagonal(); + options->G = options->w_G * g.asDiagonal(); + options->U = options->w_U * u.asDiagonal(); + } + + void SetCommonC3Options(C3Options* options) const { + options->admm_iter = admm_iter; + options->rho = rho; + options->rho_scale = rho_scale; + options->num_threads = num_threads; + options->delta_option = delta_option; + options->contact_model = contact_model; + options->projection_type = projection_type; + options->warm_start = warm_start; + options->use_predicted_x0 = false; // unused by sampling C3 + options->end_on_qp_step = end_on_qp_step; + options->use_robust_formulation = use_robust_formulation; + options->solve_time_filter_alpha = solve_time_filter_alpha; + options->publish_frequency = publish_frequency; + + options->workspace_limits = workspace_limits; + options->workspace_margins = workspace_margins; + options->u_horizontal_limits = u_horizontal_limits; + options->u_vertical_limits = u_vertical_limits; + + options->N = N; + options->gamma = gamma; + + options->solve_dt = 0; // unused in all of C3 + options->num_friction_directions = num_friction_directions; + + options->qp_projection_alpha = qp_projection_alpha; + options->qp_projection_scaling = qp_projection_scaling; + options->penalize_changes_in_u_across_solves = + penalize_changes_in_u_across_solves; + + options->mu = mu; + options->num_contacts = num_contacts; + } + + void SetPositionTrackingOptions(C3Options* options) const { + options->dt = planning_dt_position; + options->w_Q = w_Q_position; + options->w_R = w_R_position; + options->w_G = w_G_position; + options->w_U = w_U_position; + options->q_vector = q_vector_position; + options->r_vector = r_vector_position; + + options->g_x = g_x_position; + options->g_gamma = g_gamma_position_list[num_contacts_index]; + options->g_lambda_n = g_lambda_n_position_list[num_contacts_index]; + options->g_lambda_t = g_lambda_t_position_list[num_contacts_index]; + options->g_lambda = g_lambda_position_list[num_contacts_index]; + options->g_u = g_u_position; + + options->u_x = u_x_position; + options->u_gamma = u_gamma_position_list[num_contacts_index]; + options->u_lambda_n = u_lambda_n_position_list[num_contacts_index]; + options->u_lambda_t = u_lambda_t_position_list[num_contacts_index]; + options->u_lambda = u_lambda_position_list[num_contacts_index]; + options->u_u = u_u_position; + PopulateCostMatricesFromVectors(options); + } + + void SetPoseTrackingOptions(C3Options* options) const { + options->dt = planning_dt_pose; + options->w_Q = w_Q; + options->w_R = w_R; + options->w_G = w_G; + options->w_U = w_U; + options->q_vector = q_vector; + options->r_vector = r_vector; + + options->g_x = g_x; + options->g_gamma = g_gamma_list[num_contacts_index]; + options->g_lambda_n = g_lambda_n_list[num_contacts_index]; + options->g_lambda_t = g_lambda_t_list[num_contacts_index]; + options->g_lambda = g_lambda_list[num_contacts_index]; + options->g_u = g_u; + + options->u_x = u_x; + options->u_gamma = u_gamma_list[num_contacts_index]; + options->u_lambda_n = u_lambda_n_list[num_contacts_index]; + options->u_lambda_t = u_lambda_t_list[num_contacts_index]; + options->u_lambda = u_lambda_list[num_contacts_index]; + options->u_u = u_u; + PopulateCostMatricesFromVectors(options); + } +}; diff --git a/examples/sampling_c3/parameter_headers/sampling_params.h b/examples/sampling_c3/parameter_headers/sampling_params.h new file mode 100644 index 0000000000..708e274450 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/sampling_params.h @@ -0,0 +1,85 @@ +#pragma once + +#include "common/file_utils.h" +#include "drake/common/yaml/yaml_read_archive.h" + + +/* Sample generation options: + 0. kRadiallySymmetric: radially distributed samples on a planar circle. + 1. kRandomOnCircle: random samples on a planar circle. + 2. kRandomOnSphere: random samples on a spherical surface. + 3. kFixed: fixed set of samples. + 4. kRandomOnPerimeter: random samples on a perimeter offset past the object + surface (roughly planar). + 5. kRandomOnShell: random samples on a shell offset past the object + surface. +*/ +enum SamplingStrategy { + kRadiallySymmetric, + kRandomOnCircle, + kRandomOnSphere, + kFixed, + kRandomOnPerimeter, + kRandomOnShell +}; + +struct SamplingParams { + SamplingStrategy sampling_strategy; + + /// Parameters relevant for all sampling strategies. + bool filter_samples_for_safety; + + /// Number of samples. Add 2 for repositioning (current location, previous + /// repositioning target) and 1 for C3 (current location) to get total number + /// solved in parallel for each control loop. + int num_additional_samples_repos; + int num_additional_samples_c3; + + /// Sample buffer parameters. + bool consider_best_buffer_sample_when_leaving_c3; + int N_sample_buffer; + double pos_error_sample_retention; + double ang_error_sample_retention; + + /// Shared across multiple sampling strategies. + double sampling_radius; // kRadiallySymmetric, kRandomOnCircle, + // kRandomOnSphere + double sampling_height; // kRadiallySymmetric, kRandomOnCircle + // kRandomOnPerimeter + double sample_projection_clearance; // kRandomOnPerimeter, kRandomOnShell + double min_angle_from_vertical; // kRandomOnSphere, kRandomOnShell + double max_angle_from_vertical; // kRandomOnSphere, kRandomOnShell + + /// kFixed parameters. + Eigen::MatrixXd fixed_sample_locations; // n_samples rows, 3 columns + + /// kRandomOnPerimeter parameters. + std::vector grid_x_limits; + std::vector grid_y_limits; + + /// kRandomOnShell parameters. + double min_sampling_radius; + double max_sampling_radius; + + template + void Serialize(Archive* a) { + ENUM_DESERIALIZE(a, sampling_strategy); + a->Visit(DRAKE_NVP(filter_samples_for_safety)); + a->Visit(DRAKE_NVP(fixed_sample_locations)); + a->Visit(DRAKE_NVP(sampling_radius)); + a->Visit(DRAKE_NVP(min_angle_from_vertical)); + a->Visit(DRAKE_NVP(max_angle_from_vertical)); + a->Visit(DRAKE_NVP(sampling_height)); + a->Visit(DRAKE_NVP(grid_x_limits)); + a->Visit(DRAKE_NVP(grid_y_limits)); + a->Visit(DRAKE_NVP(sample_projection_clearance)); + a->Visit(DRAKE_NVP(min_sampling_radius)); + a->Visit(DRAKE_NVP(max_sampling_radius)); + a->Visit(DRAKE_NVP(num_additional_samples_repos)); + a->Visit(DRAKE_NVP(num_additional_samples_c3)); + a->Visit(DRAKE_NVP(consider_best_buffer_sample_when_leaving_c3)); + a->Visit(DRAKE_NVP(N_sample_buffer)); + a->Visit(DRAKE_NVP(pos_error_sample_retention)); + a->Visit(DRAKE_NVP(ang_error_sample_retention)); + } +}; diff --git a/examples/sampling_c3/parameter_headers/visualizer_params.h b/examples/sampling_c3/parameter_headers/visualizer_params.h new file mode 100644 index 0000000000..73c7d40656 --- /dev/null +++ b/examples/sampling_c3/parameter_headers/visualizer_params.h @@ -0,0 +1,73 @@ +#pragma once + +#include +#include "drake/common/yaml/yaml_read_archive.h" + +struct SamplingC3VisualizerParams { + std::string ee_vis_model; + std::string object_vis_model; + double visualizer_publish_rate; + + Eigen::VectorXd camera_pose; + Eigen::VectorXd camera_target; + + bool visualize_c3_workspace; + bool visualize_c3_state; + + bool visualize_center_of_mass_plan_curr; + bool visualize_c3_forces_curr; + bool visualize_center_of_mass_plan_best; + bool visualize_c3_forces_best; + + bool visualize_is_c3_mode; + bool visualize_sample_locations; + bool visualize_sample_buffer; + + Eigen::VectorXd is_c3_mode_color; + Eigen::VectorXd sample_color; + + bool visualize_execution_plan; + + bool visualize_c3_plan_curr; + Eigen::VectorXd c3_curr_object_color; + Eigen::VectorXd c3_curr_ee_color; + Eigen::VectorXd df_curr_object_color; + Eigen::VectorXd df_curr_ee_color; + + bool visualize_c3_plan_best; + Eigen::VectorXd c3_best_object_color; + Eigen::VectorXd c3_best_ee_color; + Eigen::VectorXd df_best_object_color; + Eigen::VectorXd df_best_ee_color; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(ee_vis_model)); + a->Visit(DRAKE_NVP(object_vis_model)); + a->Visit(DRAKE_NVP(visualizer_publish_rate)); + a->Visit(DRAKE_NVP(camera_pose)); + a->Visit(DRAKE_NVP(camera_target)); + a->Visit(DRAKE_NVP(visualize_c3_workspace)); + a->Visit(DRAKE_NVP(visualize_c3_state)); + a->Visit(DRAKE_NVP(visualize_center_of_mass_plan_curr)); + a->Visit(DRAKE_NVP(visualize_c3_forces_curr)); + a->Visit(DRAKE_NVP(visualize_center_of_mass_plan_best)); + a->Visit(DRAKE_NVP(visualize_c3_forces_best)); + a->Visit(DRAKE_NVP(visualize_is_c3_mode)); + a->Visit(DRAKE_NVP(visualize_sample_locations)); + a->Visit(DRAKE_NVP(visualize_sample_buffer)); + a->Visit(DRAKE_NVP(is_c3_mode_color)); + a->Visit(DRAKE_NVP(sample_color)); + a->Visit(DRAKE_NVP(visualize_execution_plan)); + a->Visit(DRAKE_NVP(visualize_c3_plan_curr)); + a->Visit(DRAKE_NVP(c3_curr_object_color)); + a->Visit(DRAKE_NVP(c3_curr_ee_color)); + a->Visit(DRAKE_NVP(df_curr_object_color)); + a->Visit(DRAKE_NVP(df_curr_ee_color)); + a->Visit(DRAKE_NVP(visualize_c3_plan_best)); + a->Visit(DRAKE_NVP(c3_best_object_color)); + a->Visit(DRAKE_NVP(c3_best_ee_color)); + a->Visit(DRAKE_NVP(df_best_object_color)); + a->Visit(DRAKE_NVP(df_best_ee_color)); +} +}; diff --git a/examples/sampling_c3/push_t/BUILD.bazel b/examples/sampling_c3/push_t/BUILD.bazel new file mode 100644 index 0000000000..bd09516749 --- /dev/null +++ b/examples/sampling_c3/push_t/BUILD.bazel @@ -0,0 +1,76 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "parameters", + data = glob([ + "parameters/*.yaml", + ]), +) + +cc_library( + name = "lcm_channels_push_t", + hdrs = [], + data = [ + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_sim_params_push_t", + hdrs = [], + data = [ + "parameters/sim_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:franka_sim_params", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "sampling_c3_controller_params_push_t", + hdrs = [], + data = [ + "parameters/sampling_c3_controller_params.yaml", + "parameters/sampling_params.yaml", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "goal_params_push_t", + hdrs = [], + data = [ + "parameters/goal_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:goal_params", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_drake_lcm_driver_channels_push_t", + hdrs = [], + data = [ + "//common/parameters:franka_drake_lcm_driver_channels", + ], + deps = [ + "//common/parameters:franka_drake_lcm_driver_channels", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) diff --git a/examples/sampling_c3/push_t/franka_hardware_t.pmd b/examples/sampling_c3/push_t/franka_hardware_t.pmd new file mode 100644 index 0000000000..9c953335da --- /dev/null +++ b/examples/sampling_c3/push_t/franka_hardware_t.pmd @@ -0,0 +1,103 @@ +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=false --demo_name=push_t"; + host = "sampling_c3_localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "sampling_c3_localhost"; + } + cmd "logger" { + exec = "python3 examples/sampling_c3/start_logging.py hw push_t"; + host = "sampling_c3_localhost"; + } + cmd "record_video" { + exec = "python3 record_video.py"; + host = "franka_control"; + } +} + +group "controllers (hardware)" { + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=false --demo_name=push_t; + host = "sampling_c3_localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=false --demo_name=push_t --lcm_url=udpm://239.255.76.67:7667?ttl=1; + host = "franka_control"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "sampling_c3_localhost"; + } +} + +group "drivers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=false --demo_name=push_t"; + host = "franka_control"; + } + cmd "franka_driver_out" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_out --demo_name=push_t"; + host = "franka_control"; + } + cmd "franka_driver_in" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_in --demo_name=push_t"; + host = "franka_control"; + } + cmd "torque_driver" { + exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=torque"; + host = "drake_franka_driver"; + } +} + + +script "init_experiment" { + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 10000; + stop group "drivers"; +} + +script "start_experiment" { + restart cmd "xbox"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + stop cmd "franka_osc"; + start cmd "record_video"; + start cmd "logger"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 5000; + stop cmd "move_to_init"; + start cmd "franka_osc"; + start cmd "sampling_c3"; +} + +script "start_operator_commands" { + restart cmd "visualizer"; + restart cmd "xbox"; + restart cmd "lcm-spy"; +} + +script "stop_experiment" { + stop group "drivers"; + stop group "controllers (hardware)"; + stop cmd "record_video"; + stop cmd "logger"; +} diff --git a/examples/sampling_c3/push_t/franka_sim_t.pmd b/examples/sampling_c3/push_t/franka_sim_t.pmd new file mode 100644 index 0000000000..420b50d58e --- /dev/null +++ b/examples/sampling_c3/push_t/franka_sim_t.pmd @@ -0,0 +1,71 @@ +group "simulations" { + cmd "franka_sim" { + exec = "bazel-bin/examples/sampling_c3/franka_sim --demo_name=push_t"; + host = "localhost"; + } +} + +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "drake-director" { + exec = "bazel-bin/director/drake-director"; + host = "localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "localhost"; + } + cmd "start_logging" { + exec = "python3 examples/sampling_c3/start_logging.py sim push_t"; + host = "localhost"; + } +} + +group "controllers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } +} + +script "start_experiment_no_logs"{ + stop cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "start_experiment_with_logs"{ + restart cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "end_experiment"{ + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_sim"; + wait ms 10; + stop cmd "start_logging"; +} diff --git a/examples/sampling_c3/push_t/parameters/goal_params.yaml b/examples/sampling_c3/push_t/parameters/goal_params.yaml new file mode 100644 index 0000000000..f4c7785eb5 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/goal_params.yaml @@ -0,0 +1,36 @@ +# Goal mode options (defined as GoalMode enum in goal_params.h): +# 0. kRandom: randomly generate a new goal. +# 1. kOrientationSequence: keep position goal the same, and cycle through a +# sequence of orientations. +# 2. kFixedGoal: keep the same goal. +goal_mode: 0 + +### Parameters used for multiple goal modes. +# Success thresholds for position and orientation. +position_success_threshold: 0.02 +orientation_success_threshold: 0.1 # 0.1 rad = 5.7 degrees + +resting_object_height: -0.009 # in world frame +ee_target_z_offset_above_object: 0.06 # defines EE goal w.r.t. object position + +# Lookahead parameters to define a sub-goal for C3. +lookahead_step_size: 0.15 # meters +lookahead_angle: 2 # rad + +# Apply hysteresis on the lookahead angle so the sub-goal orientation doesn't +# flip back and forth near the 180 degree error singularity. A lower number +# means the sub-goal orientation can switch more often; a higher number means +# the sub-goal orientation is more stable but possibly less optimal. +angle_hysteresis: 0.4 + +# Factor to convert an angular error to angular velocity command (0=disabled). +angle_err_to_vel_factor: 0 + +# Initial goal (and only goal for fixed goal mode). +fixed_target_position: [0.48199167, 0.18745379, -0.009] +fixed_target_orientation: [-0.9327733, 0, 0, 0.36046353] + +# Random-specific parameters. +random_goal_x_limits: [0.44, 0.52] +random_goal_y_limits: [0.02, 0.25] +random_goal_radius_limits: [0.5, 0.52] diff --git a/examples/sampling_c3/push_t/parameters/progress_params.yaml b/examples/sampling_c3/push_t/parameters/progress_params.yaml new file mode 100644 index 0000000000..48609fc7d6 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/progress_params.yaml @@ -0,0 +1,64 @@ +# Ways of computing C3 costs after solving the MPC problem (defined as +# C3CostComputationType enum in c3_options.h): +# 0. kSimLCS: Simulate the LCS dynamics from the planned +# inputs. +# 1. kUseC3Plan: Use the C3 planned trajectory and inputs. +# 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned +# inputs only for the object; use the planned +# EE trajectory. +# 3. kSimImpedance: Try to emulate the real cost of the system +# associated not only applying the planned +# inputs, but also tracking the planned EE +# trajectory with an impedance controller. +# 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE +# states are replaced with the plan from C3 +# at the end. +# 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the +# object terms contribute to the final cost. +cost_type: 5 +cost_type_position: 5 + +# Progress metric to use to determine if C3 is making progress, all phrased as +# improvement requirements over a number of control loops (defined as +# ProgressMetric enum in progress_params.h) +# 0. kC3Cost: C3 cost. +# 1. kConfigCost: Current object configuration cost. +# 2. kPosOrRotCost: Current position or rotation error. +# 3. kConfigCostDrop: Drop in object configuration cost (this is the same as +# kConfigCost if the required drop is 0; a more +# aggressive drop cuts C3 off earlier). +track_c3_progress_via: 3 + +# Ignore orientation errors when object is beyond this threshold from the goal. +cost_switching_threshold_distance: 0.50 + +# Penalize repositioning distance. +travel_cost_per_meter: 0 + +# Number of control loops to wait before cutting off C3 due to unproductivity. +# Used for kC3Cost, kConfigCost, kPosOrRotCost. +num_control_loops_to_wait: 5 +num_control_loops_to_wait_position: 5 + +# kConfigCostDrop parameters. +progress_enforced_cost_drop: 0.01 +progress_enforced_over_n_loops: 28 + +### Hysteresis parameters for switching between C3 and repositioning modes. +finished_reposition_cost: 1000000000 + +hyst_c3_to_repos: 300000 +hyst_c3_to_repos_position: 300000 +hyst_repos_to_c3: 200000 +hyst_repos_to_c3_position: 200000 +hyst_repos_to_repos: 60000000000000 +hyst_repos_to_repos_position: 60000000000000 + +# Relative hysteresis is in terms of a fraction of the current cost. +use_relative_hysteresis: true +hyst_c3_to_repos_frac: 0.4 +hyst_c3_to_repos_frac_position: 0.6 +hyst_repos_to_c3_frac: 0.9 +hyst_repos_to_c3_frac_position: 0.5 +hyst_repos_to_repos_frac: 0.3 +hyst_repos_to_repos_frac_position: 0.1 diff --git a/examples/sampling_c3/push_t/parameters/reposition_params.yaml b/examples/sampling_c3/push_t/parameters/reposition_params.yaml new file mode 100644 index 0000000000..faaf08b02b --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/reposition_params.yaml @@ -0,0 +1,32 @@ +# Repositioning trajectory type options (defined as RepositioningTrajectoryType +# enum in reposition_params.h): +# 0. kSpline: move from current to target with spline that distorts +# around the object. +# 1. kSpherical: move away from object to a radius, along the spherical +# surface to outside target, then in to target. +# 2. kCircular: (planar) move away from object to a planar radius, +# along the circle to outside target, then in to target. +# 3. kPiecewiseLinear: move up to waypoint height, over to above target, then +# down. +traj_type: 3 + +### Parameters used for multiple repositioning trajectory types. +speed: 0.18 # m/s + +# Thresholds for switching to straight line trajectories. +use_straight_line_traj_under_spline: 0.12 # xyz meters for spline +use_straight_line_traj_within_angle: 0.3 # rad for circle/sphere +use_straight_line_traj_under_piecewise_linear: 0.008 # xy meters for PWL + +# Spline-specific parameters. +spline_width: 0.17 + +# Spherical-specific parameters. +sphere_radius: 0.18 + +# Circular-specific parameters. +circle_radius: 0.20 +circle_height: 0.00 + +# Piecewise-linear-specific parameters. +pwl_waypoint_height: 0.06 diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml new file mode 100644 index 0000000000..7977849b5b --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml @@ -0,0 +1,23 @@ +sampling_c3_options_file: examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml +reposition_params_file: examples/sampling_c3/push_t/parameters/reposition_params.yaml +progress_params_file: examples/sampling_c3/push_t/parameters/progress_params.yaml +sampling_params_file: examples/sampling_c3/push_t/parameters/sampling_params.yaml +goal_params_file: examples/sampling_c3/push_t/parameters/goal_params.yaml + +sim_params_file: examples/sampling_c3/push_t/parameters/sim_params.yaml +vis_params_file: examples/sampling_c3/push_t/parameters/vis_params.yaml +osc_params_file: examples/sampling_c3/shared_parameters/osc_params.yaml +osqp_settings_file: examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +osc_qp_settings_file: examples/sampling_c3/shared_parameters/osc_qp_settings.yaml +franka_driver_channels_file: common/parameters/franka_drake_lcm_driver_channels.yaml +lcm_channels_hardware_file: examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml +lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml + +# Note: Use vertical_link body name as representative of the pose of the entire +# T model. This works because vertical_link's origin is the same as the T's. +object_model: examples/sampling_c3/urdf/push_t_control.sdf #TODO T_vertical.sdf +object_body_name: vertical_link + +include_end_effector_orientation: false + +control_loop_delay_ms: 0 diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml new file mode 100644 index 0000000000..d4c165d2dc --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml @@ -0,0 +1,165 @@ +### C3 options +admm_iter: 3 +rho: 0 #This isn't used anywhere! +rho_scale: 3 +num_threads: 5 +num_outer_threads: 4 +delta_option: 1 +projection_type: 'MIQP' # 'MIQP' or 'QP' +contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. +warm_start: false +end_on_qp_step: false +use_robust_formulation: false +solve_time_filter_alpha: 0.95 +publish_frequency: 0 +penalize_changes_in_u_across_solves: false # Penalize (u-u_prev) instead of u. +num_friction_directions: 2 + +N: 5 +gamma: 1.0 # discount factor on MPC costs + +# As alpha -> 0, any complimentarity constraint error in QP projection -> 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1 + +### Limits +# Workspace limits specified as linear constraints [x, y, z, lb, ub] +workspace_limits: [[1, 0, 0, 0.15, 0.75], + [0, 1, 0, -0.6, 0.6], + [0, 0, 1, -0.01, 0.3]] +robot_radius_limits: [0.25, 0.75] +workspace_margins: 0.02 +u_horizontal_limits: [-50, 50] +u_vertical_limits: [-50, 50] + +### Whether to use a predicted EE location for each mode. +use_predicted_x0_c3: true +use_predicted_x0_repos: true +use_predicted_x0_reset_mechanism: false # Resets if prediction is too far. + +### Push-T demo specific parameters: +# Contact pairs include a number of (ee-ground, ee-T, T-ground). +mu_per_pair_type: [0.4165, 1, 0.4615] # match URDFs with (2mu1*mu2)/(mu1+mu2) +resolve_contacts_to_lists: [[0, 1, 3], + [0, 2, 3]] + +# Index into this list to choose number of contacts for C3 solve and cost. +num_contacts_index: 0 +num_contacts_index_for_cost: 1 + +planning_dt_position: 0.1 +planning_dt_pose: 0.05 + +### Cost parameters +### If list of lists, then index into them with num_contact_index(_for_cost). +# Set 4x4 portion of Q for quaternion based on current and desired quaternion. +use_quaternion_dependent_cost: true +q_quaternion_dependent_weight: 1000 +q_quaternion_dependent_regularizer_fraction: 0 + +Kp_for_ee_pd_rollout: 100 +Kd_for_ee_pd_rollout: 0.5 + +### Pose tracking cost parameters +# Matrix scaling +w_Q: 45 +w_R: 1 +w_G: 0.25 +w_U: 0.26 + +q_vector: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 200, 200, 120, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x: [950, 950, 950, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +g_lambda_list: [ + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] +g_u: [30, 30, 30] + +# Penalty on matching the QP variables +u_x: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +# u_lambda_list is not used for the QP projection; overwritten by alpha*F. +u_lambda_list: [ + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u: [0.01, 0.01, 0.01] + +### Position tracking cost parameters +# Matrix scaling +w_Q_position: 45 +w_R_position: 1 +w_G_position: 0.25 +w_U_position: 0.26 + +q_vector_position: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 250, 250, 250, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector_position: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x_position: [900, 900, 900, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +g_lambda_position_list: [ + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] +g_u_position: [30, 30, 30] + +# Penalty on matching the QP variables +u_x_position: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +# u_lambda is not used for the QP projection; overwritten by alpha*F. +u_lambda_position_list: [ + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u_position: [0.01, 0.01, 0.01] + + +### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but +### are overwritten by other sampling C3 parameters. +use_predicted_x0: false # instead: use_predicted_x0_c3, + # use_predicted_x0_repos, + # use_predicted_x0_reset_mechanism +dt: 0 # instead: planning_dt_pose, planning_dt_position +solve_dt: 0 # unused +mu: [] # instead based on indexing into mu_per_pair_type +num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists +# Instead for the below, index into their _list versions. +g_gamma: [] +g_lambda_n: [] +g_lambda_t: [] +g_lambda: [] +u_gamma: [] +u_lambda_n: [] +u_lambda_t: [] +u_lambda: [] diff --git a/examples/sampling_c3/push_t/parameters/sampling_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_params.yaml new file mode 100644 index 0000000000..6c5d4e7c8d --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_params.yaml @@ -0,0 +1,44 @@ +# Sampling strategies (defined as SamplingStrategy enum in sampling_params.h): +# 0. kRadiallySymmetric: radially distributed samples on a planar circle. +# 1. kRandomOnCircle: random samples on a planar circle. +# 2. kRandomOnSphere: random samples on a spherical surface. +# 3. kFixed: fixed set of samples. +# 4. kRandomOnPerimeter: random samples on a perimeter offset past the object +# surface (roughly planar). +# 5. kRandomOnShell: random samples on a shell offset past the object +# surface. +sampling_strategy: 4 + +### Parameters relevant for all sampling strategies. +filter_samples_for_safety: true + +# Number of samples. Add 2 for repositioning (current location, previous +# repositioning target) and 1 for C3 (current location) to get total sample +# number solved in parallel for each control loop. +num_additional_samples_repos: 1 +num_additional_samples_c3: 2 + +# Sample buffer parameters. +consider_best_buffer_sample_when_leaving_c3: true +N_sample_buffer: 100 +pos_error_sample_retention: 0.003 +ang_error_sample_retention: 0.0349 # 0.0349 rad = 2 deg + +# Shared across multiple sampling strategies. +sampling_radius: 0.16 # kRadiallySymmetric, kRandomOnCircle, kRandomOnSphere +sampling_height: 0.005 # kRadiallySymmetric, kRandomOnCircle, kRandomOnPerimeter +sample_projection_clearance: 0.02 # kRandomOnPerimeter, kRandomOnShell +min_angle_from_vertical: 1.49 # kRandomOnSphere, kRandomOnShell +max_angle_from_vertical: 1.5 # kRandomOnSphere, kRandomOnShell + +# kFixed parameters. +fixed_sample_locations: [[0.45, 0.270, 0.270], # sample 1 + [0.34, 0.455, 0.225]] # sample 2 + +# kRandomOnPerimeter parameters. +grid_x_limits: [-0.12, 0.08] +grid_y_limits: [-0.08, 0.08] + +# kRandomOnShell parameters. +min_sampling_radius: 0.07 +max_sampling_radius: 0.10 diff --git a/examples/sampling_c3/push_t/parameters/sim_params.yaml b/examples/sampling_c3/push_t/parameters/sim_params.yaml new file mode 100644 index 0000000000..8ed1c5225e --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sim_params.yaml @@ -0,0 +1,14 @@ +object_model: examples/sampling_c3/urdf/push_t.sdf + +franka_publish_rate: 1000 +object_publish_rate: 10 +actuator_delay: 0.000 + +visualize_drake_sim: false +publish_efforts: true + +dt: 0.0001 +realtime_rate: 1 + +q_init_franka: [2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08] +q_init_object: [1.0, 0, 0, 0, 0.5, 0, -0.009] diff --git a/examples/sampling_c3/push_t/parameters/vis_params.yaml b/examples/sampling_c3/push_t/parameters/vis_params.yaml new file mode 100644 index 0000000000..87c9195fdf --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/vis_params.yaml @@ -0,0 +1,37 @@ +ee_vis_model: examples/sampling_c3/urdf/ee_visualization_model.urdf +object_vis_model: examples/sampling_c3/urdf/push_t.sdf +visualizer_publish_rate: 30 + +camera_pose: [1.5, 0, 0.6] +camera_target: [0.5, 0, 0.5] + +visualize_c3_workspace: false +visualize_c3_state: true + +visualize_center_of_mass_plan_curr: false +visualize_c3_forces_curr: false +visualize_center_of_mass_plan_best: false +visualize_c3_forces_best: false + +visualize_is_c3_mode: true +visualize_sample_locations: true +visualize_sample_buffer: true + +# Note: Colors can either be empty ([]) or RGB values between 0 and 1. If left +# empty, the colors defined in the URDF/SDF files will be used. +is_c3_mode_color: [1.0, 0.43, 1.0] +sample_color: [1.0, 1.0, 0.0] + +visualize_execution_plan: false + +visualize_c3_plan_curr: false +c3_curr_object_color: [] +c3_curr_ee_color: [0.8, 0.8, 0.8] +df_curr_object_color: [] +df_curr_ee_color: [0.5, 1.0, 0.5] + +visualize_c3_plan_best: false +c3_best_object_color: [1.0, 0.64, 0.0] +c3_best_ee_color: [1.0, 0.64, 0.0] +df_best_object_color: [1.0, 0.64, 0.0] +df_best_ee_color: [1.0, 0.64, 0.0] diff --git a/examples/sampling_c3/reposition.cc b/examples/sampling_c3/reposition.cc new file mode 100644 index 0000000000..7eba9608ed --- /dev/null +++ b/examples/sampling_c3/reposition.cc @@ -0,0 +1,470 @@ +#include "reposition.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" + +namespace dairlib { + +using drake::trajectories::PiecewisePolynomial; + +namespace systems { + +// TODO @bibit further cleanup could require +/- z workspace limits instead of +// sampling_c3_options +Eigen::MatrixXd Reposition( + const int& n_q, + const int& n_x, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params, + const SamplingC3Options& sampling_c3_options) { + + Eigen::MatrixXd knots = Eigen::MatrixXd::Zero(n_x, N); + + Eigen::Vector3d current_ee_location = x_lcs.head(3); + Eigen::Vector3d current_object_location = x_lcs.segment(n_q-3, 3); + Eigen::Vector3d curr_to_goal_vec = repos_target - current_ee_location; + + // Get two unit vectors in the plane of the arc between the current and goal + // ee locations. + Eigen::Vector3d v1 = + (current_ee_location - current_object_location).normalized(); + Eigen::Vector3d v2 = (repos_target - current_object_location).normalized(); + // NOTE: Need to clamp between not quite (-1, 1) to avoid NaNs. + double travel_angle = std::acos(std::clamp(v1.dot(v2), -0.9999, 0.9999)); + + // Compute EE position errors to repositioning target. + double travel_distance = curr_to_goal_vec.norm(); + double xy_travel_distance = curr_to_goal_vec.head(2).norm(); + + // Use a straight line trajectory if close to the target. + RepositioningTrajectoryType traj_type = reposition_params.traj_type; + if ((travel_distance < + reposition_params.use_straight_line_traj_under_spline && + traj_type == RepositioningTrajectoryType::kSpline) || + ((traj_type == RepositioningTrajectoryType::kSpherical || + traj_type == RepositioningTrajectoryType::kCircular) && + travel_angle < reposition_params.use_straight_line_traj_within_angle) || + (traj_type == RepositioningTrajectoryType::kPiecewiseLinear && + xy_travel_distance < + reposition_params.use_straight_line_traj_under_piecewise_linear)) { + RepositionStraightLine( + knots, n_q, n_x, N, x_lcs, repos_target, dt, is_doing_c3, + finished_reposition_flag, reposition_params); + } + else if (traj_type == RepositioningTrajectoryType::kSpline) { + RepositionSpline( + knots, n_q, N, x_lcs, repos_target, dt, is_doing_c3, + finished_reposition_flag, reposition_params, sampling_c3_options); + } + else if (traj_type == RepositioningTrajectoryType::kSpherical) { + RepositionSpherical( + knots, n_q, N, x_lcs, repos_target, dt, is_doing_c3, + finished_reposition_flag, reposition_params, sampling_c3_options); + } + else if (traj_type == RepositioningTrajectoryType::kCircular) { + RepositionCircular( + knots, n_q, N, x_lcs, repos_target, dt, is_doing_c3, + finished_reposition_flag, reposition_params); + } + else if (traj_type == RepositioningTrajectoryType::kPiecewiseLinear) { + RepositionPiecewiseLinear( + knots, N, x_lcs, repos_target, dt, is_doing_c3, finished_reposition_flag, + reposition_params); + } + + return knots; +} + +void RepositionStraightLine( + Eigen::MatrixXd& knots, const int& n_q, const int& n_x, const int& N, + const Eigen::VectorXd& x_lcs, const Eigen::Vector3d& repos_target, + const double& dt, const bool& is_doing_c3, bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params) { + Eigen::Vector3d current_ee_location = x_lcs.head(3); + Eigen::Vector3d curr_to_goal_vec = repos_target - current_ee_location; + double travel_distance = curr_to_goal_vec.norm(); + + Eigen::VectorXd times = Eigen::VectorXd::Zero(2); + times[0] = 0; + // Ensure the times used to define PiecewisePolynomial are increasing. + double total_travel_time = travel_distance / reposition_params.speed; + times[1] = std::max(total_travel_time, 0.0001); + + Eigen::MatrixXd points = Eigen::MatrixXd::Zero(n_x, 2); + points.col(0) = x_lcs; + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = repos_target; + next_lcs_state.segment(n_q, 3) = Eigen::Vector3d::Zero(); + points.col(1) = next_lcs_state; + auto trajectory = + PiecewisePolynomial::FirstOrderHold(times, points); + + for (int i = 0; i < N; i++) { + double t_line = std::min((i)*dt, total_travel_time); + knots.col(i) = trajectory.value(t_line); + + // If one step gets to the goal, set finished_reposition_flag. + if (i == 1 && t_line >= total_travel_time && !is_doing_c3) { + finished_reposition_flag = true; + } + } +} + +void RepositionSpline( + Eigen::MatrixXd& knots, const int& n_q, const int& N, + const Eigen::VectorXd& x_lcs, const Eigen::Vector3d& repos_target, + const double& dt, const bool& is_doing_c3, bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params, + const SamplingC3Options& sampling_c3_options) { + Eigen::Vector3d current_ee_location = x_lcs.head(3); + Eigen::Vector3d current_object_location = x_lcs.segment(n_q-3, 3); + Eigen::Vector3d curr_to_goal_vec = repos_target - current_ee_location; + double travel_distance = curr_to_goal_vec.norm(); + + // Compute spline waypoints. + Eigen::Vector3d p0 = current_ee_location; + Eigen::Vector3d p3 = repos_target; + Eigen::Vector3d p1 = + current_ee_location + 0.25 * curr_to_goal_vec - current_object_location; + p1 = current_object_location + + reposition_params.spline_width * p1 / p1.norm(); + Eigen::Vector3d p2 = + current_ee_location + 0.75 * curr_to_goal_vec - current_object_location; + p2 = current_object_location + + reposition_params.spline_width * p2 / p2.norm(); + + for (int i = 0; i < N; i++) { + double total_travel_time = travel_distance / reposition_params.speed; + double t_spline = (i)*dt / total_travel_time; + t_spline = std::min(1.0, t_spline); // Don't overshoot end of the spline. + + // Set finished_reposition_flag if only one step is required. + if (i == 1 && t_spline == 1 && !is_doing_c3) { + finished_reposition_flag = true; + std::cout<<"WARNING! Spline finished repositioning in 1 step."< M_PI_2) { + v4 = -v4; + travel_angle = 2 * M_PI - travel_angle; + } + // Prevent getting stuck if the EE is 180 degrees around from the target. + if (travel_angle > 0.9 * M_PI) { + Eigen::Vector3d almost_v4 = v1; + almost_v4(2) += 1; + v3 = v1.cross(almost_v4.normalized()).normalized(); + v4 = v3.cross(v1).normalized(); + } + + // The arc is defined by the equation: + // x = x_c + r*cos(theta)*v1 + r*sin(theta)*v4 + // ...where theta should be [0, travel_angle] and r*dtheta should be the + // desired travel distance. + double dtheta = reposition_params.speed*dt / reposition_params.sphere_radius; + double step_size = reposition_params.speed * dt; + + knots.col(0) = x_lcs; + int i = 1; + // Handle the first leg: straight line from current EE location to waypoint1. + double dist_to_wp1 = (current_ee_location - waypoint1).norm(); + while ((i * step_size < dist_to_wp1) && (i < N)) { + Eigen::Vector3d straight_line_point = + current_ee_location + + i * step_size / dist_to_wp1 * (waypoint1 - current_ee_location); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Handle the second leg: arc from waypoint1 to waypoint2. + int leg1_i = i; + double dtheta0 = (i * step_size - dist_to_wp1) / step_size * dtheta; + while ((dtheta0 + (i - leg1_i) * dtheta < travel_angle) && (i < N)) { + Eigen::Vector3d arc_point = + current_object_location + + reposition_params.sphere_radius * + (std::cos(dtheta0 + (i - leg1_i) * dtheta) * v1 + + std::sin(dtheta0 + (i - leg1_i) * dtheta) * v4); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = arc_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Handle the last leg: straight line from waypoint2 to goal EE location. + int leg2_i = i; + double dstep = + (dtheta0 + (i - leg1_i) * dtheta - travel_angle) / dtheta * step_size; + double dist_wp2_to_goal = (waypoint2 - repos_target).norm(); + while ((dstep + (i - leg2_i) * step_size < dist_wp2_to_goal) && (i < N)) { + Eigen::Vector3d straight_line_point = + waypoint2 + (dstep + (i - leg2_i) * step_size) / dist_wp2_to_goal * + (repos_target - waypoint2); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Enforce minimum z height for end effector. + for (int j = 0; j < i; j++) { + if (knots(2, j) < sampling_c3_options.workspace_limits[2][3]) { + knots(2, j) = sampling_c3_options.workspace_limits[2][3]; + } + } + + // Fill in the rest of the knots with the goal EE location. + for (int j = i; j < N; j++) { + Eigen::Vector3d x_lcs_goal = x_lcs; + x_lcs_goal.head(3) = repos_target; + knots.col(j) = x_lcs_goal; + if (j == 1 && !is_doing_c3) { + finished_reposition_flag = true; + } + } +} + +void RepositionCircular( + Eigen::MatrixXd& knots, const int& n_q, const int& N, + const Eigen::VectorXd& x_lcs, const Eigen::Vector3d& repos_target, + const double& dt, const bool& is_doing_c3, bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params) { + Eigen::Vector3d current_ee_location = x_lcs.head(3); + Eigen::Vector3d current_object_location = x_lcs.segment(n_q-3, 3); + + // Project positions to the plane of the circle. + Eigen::Vector3d current_object_projection = current_object_location; + current_object_projection(2) = reposition_params.circle_height; + Eigen::Vector3d curr_ee_projection = current_ee_location; + curr_ee_projection(2) = reposition_params.circle_height; + Eigen::Vector3d best_sample_projection = repos_target; + best_sample_projection(2) = reposition_params.circle_height; + + Eigen::Vector3d v1 = + (curr_ee_projection - current_object_projection).normalized(); + Eigen::Vector3d v2 = + (best_sample_projection - current_object_projection).normalized(); + + // Compute travel angle. + double travel_angle = std::acos(v1.dot(v2)); + + // Define the waypoints for the circular trajectory. + Eigen::Vector3d waypoint1 = current_object_projection + + reposition_params.circle_radius * v1; + Eigen::Vector3d waypoint2 = current_object_projection + + reposition_params.circle_radius * v2; + + // Compute tangent vector to the circle at waypoint1. + Eigen::Vector3d v3 = v1.cross(v2).normalized(); + Eigen::Vector3d v4 = v3.cross(v1).normalized(); + + if (travel_angle > M_PI) { + travel_angle = 2 * M_PI - travel_angle; + v4 = -v4; + } + + // The arc is defined by the equation: x = x_c + r*cos(theta) + r*sin(theta) + // the relation between the linear velocity and the angular velocity is + // given by v = r*omega, where v is the linear velocity and omega is the + // angular velocity. The angular velocity is given by omega = v/r. The + // angular velocity is given by omega = dtheta/dt. Therefore, dtheta = + // v/r*dt. This is the increment in the angle in the time dt. + double dtheta = reposition_params.speed*dt / reposition_params.circle_radius; + double step_size = reposition_params.speed * dt; + + knots.col(0) = x_lcs; + int i = 1; + // Handle the first leg: straight line from current EE location to + // waypoint1. + double dist_to_wp1 = (current_ee_location - waypoint1).norm(); + while ((i * step_size < dist_to_wp1) && (i < N)) { + Eigen::Vector3d straight_line_point = + current_ee_location + + i * step_size / dist_to_wp1 * (waypoint1 - current_ee_location); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Handle the second leg: arc from waypoint1 to waypoint2. + int leg1_i = i; + double dtheta0 = (i * step_size - dist_to_wp1) / step_size * dtheta; + while (((i - leg1_i) * dtheta < travel_angle) && (i < N)) { + Eigen::Vector3d arc_point = + current_object_projection + + reposition_params.circle_radius * + (std::cos(dtheta0 + (i - leg1_i) * dtheta) * v1 + + std::sin(dtheta0 + (i - leg1_i) * dtheta) * v4); + arc_point(2) = reposition_params.circle_height; + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = arc_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Handle the last leg: straight line from waypoint2 to goal EE location. + int leg2_i = i; + double dstep = + (dtheta0 + (i - leg1_i) * dtheta - travel_angle) / dtheta * step_size; + double dist_wp2_to_goal = (waypoint2 - repos_target).norm(); + while ((dstep + (i - leg2_i) * step_size < dist_wp2_to_goal) && (i < N)) { + Eigen::Vector3d straight_line_point = + waypoint2 + (dstep + (i - leg2_i) * step_size) / dist_wp2_to_goal * + (repos_target - waypoint2); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Fill in the rest of the knots with the goal EE location. + for (int j = i; j < N; j++) { + Eigen::Vector3d x_lcs_goal = x_lcs; + x_lcs_goal.head(3) = repos_target; + knots.col(j) = x_lcs_goal; + if (j == 1 && !is_doing_c3) { + finished_reposition_flag = true; + } + } +} + +// The piecewise linear trajectory uses three legs: go up from current location +// to a fixed repositioning height, move in a straight line to right above the +// sample, then move down to the sample. +void RepositionPiecewiseLinear( + Eigen::MatrixXd& knots, const int& N, const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, const double& dt, + const bool& is_doing_c3, bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params) { + Eigen::Vector3d current_ee_location = x_lcs.head(3); + + // Define the waypoints for the three-leg repositioning. + Eigen::Vector3d waypoint_above_ee = current_ee_location; + waypoint_above_ee(2) = reposition_params.pwl_waypoint_height; + Eigen::Vector3d waypoint_above_sample = repos_target; + waypoint_above_sample(2) = reposition_params.pwl_waypoint_height; + + knots.col(0) = x_lcs; + int i = 1; + double step_size = reposition_params.speed * dt; + + // First leg: straight line from current EE location to waypoint_above_ee. + double dist_to_wp1 = (current_ee_location - waypoint_above_ee).norm(); + while ((i * step_size < dist_to_wp1) && (i < N)) { + Eigen::Vector3d straight_line_point = + current_ee_location + i * step_size / dist_to_wp1 * + (waypoint_above_ee - current_ee_location); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Second leg: straight line from waypoint_above_ee to + // waypoint_above_sample. + int leg1_i = i; + double dstep0 = i * step_size - dist_to_wp1; + double dist_to_wp2 = (waypoint_above_ee - waypoint_above_sample).norm(); + while ((dstep0 + (i - leg1_i) * step_size < dist_to_wp2) && (i < N)) { + Eigen::Vector3d straight_line_point = + waypoint_above_ee + (dstep0 + (i - leg1_i) * step_size) / dist_to_wp2 * + (waypoint_above_sample - waypoint_above_ee); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Third leg: straight line from waypoint_above_sample to + // repos_target. Really this doesn't even get used because it enters + // the use_straight_line condition before then. + int leg2_i = i; + double dstep1 = (i - leg1_i) * step_size - dist_to_wp2; + double dist_to_goal = (waypoint_above_sample - repos_target).norm(); + while ((dstep1 + (i - leg2_i) * step_size < dist_to_goal) && (i < N)) { + Eigen::Vector3d straight_line_point = + waypoint_above_sample + + (dstep1 + (i - leg2_i) * step_size) / dist_to_goal * + (repos_target - waypoint_above_sample); + + Eigen::VectorXd next_lcs_state = x_lcs; + next_lcs_state.head(3) = straight_line_point; + knots.col(i) = next_lcs_state; + i++; + } + + // Fill in the rest of the knots with the goal EE location. + for (int j = i; j < N; j++) { + Eigen::Vector3d x_lcs_goal = x_lcs; + x_lcs_goal.head(3) = repos_target; + knots.col(j) = x_lcs_goal; + if (j == 1 && !is_doing_c3) { + finished_reposition_flag = true; + } + } +} + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/reposition.h b/examples/sampling_c3/reposition.h new file mode 100644 index 0000000000..37eff8aba8 --- /dev/null +++ b/examples/sampling_c3/reposition.h @@ -0,0 +1,82 @@ +#include + +#include "examples/sampling_c3/parameter_headers/reposition_params.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" + +namespace dairlib { +namespace systems { + +/// Public function for generating a set of repositioning knot points. If the +/// repositioning trajectory gets to the target within a single timestep, +/// finished_reposition_flag is set to true. +Eigen::MatrixXd Reposition( + const int& n_q, + const int& n_x, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params, + const SamplingC3Options& sampling_c3_options +); + +/// Individual repositioning functions for each type of trajectory. Each sets +/// the knot points and finished_reposition_flag appropriately. +void RepositionStraightLine( + Eigen::MatrixXd& knots, + const int& n_q, + const int& n_x, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params +); +void RepositionSpline( + Eigen::MatrixXd& knots, + const int& n_q, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params, + const SamplingC3Options& sampling_c3_options); +void RepositionSpherical( + Eigen::MatrixXd& knots, + const int& n_q, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params, + const SamplingC3Options& sampling_c3_options); +void RepositionCircular( + Eigen::MatrixXd& knots, + const int& n_q, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params); +void RepositionPiecewiseLinear( + Eigen::MatrixXd& knots, + const int& N, + const Eigen::VectorXd& x_lcs, + const Eigen::Vector3d& repos_target, + const double& dt, + const bool& is_doing_c3, + bool& finished_reposition_flag, + const SamplingC3RepositionParams& reposition_params); + +} // namespace systems +} // namespace dairlib diff --git a/examples/sampling_c3/sampling_c3_utils.cc b/examples/sampling_c3/sampling_c3_utils.cc new file mode 100644 index 0000000000..bf5eab66bb --- /dev/null +++ b/examples/sampling_c3/sampling_c3_utils.cc @@ -0,0 +1,89 @@ +#include "sampling_c3_utils.h" + +#include "common/find_resource.h" +#include "drake/multibody/parsing/parser.h" + +namespace dairlib { + +using drake::geometry::SceneGraph; +using drake::math::RigidTransform; +using drake::multibody::ModelInstanceIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; + + +ModelInstanceIndex AddFrankaToPlant(MultibodyPlant* plant, + SceneGraph* scene_graph, + const bool& include_ee, + const bool& include_ground_and_platform) { + Parser parser(plant, scene_graph); + parser.SetAutoRenaming(true); + + ModelInstanceIndex franka_index = parser.AddModelsFromUrl(kFrankaModel)[0]; + RigidTransform X_WI = RigidTransform::Identity(); + plant->WeldFrames(plant->world_frame(), + plant->GetFrameByName("panda_link0"), X_WI); + + if (include_ee) { + parser.AddModels(FindResourceOrThrow(kEndEffectorModel)); + RigidTransform T_EE_W = RigidTransform( + drake::math::RotationMatrix( + drake::math::RollPitchYaw(3.1415, 0, 0)), + kToolAttachmentFrame); + plant->WeldFrames(plant->GetFrameByName("panda_link7"), + plant->GetFrameByName("end_effector_flange"), T_EE_W); + } + + if (include_ground_and_platform) { + parser.AddModels(FindResourceOrThrow(kGroundModel)); + parser.AddModels(FindResourceOrThrow(kPlatformModel)); + + RigidTransform X_F_P = RigidTransform( + drake::math::RotationMatrix(), kFrankaToPlatformOffset); + RigidTransform X_F_G_franka = RigidTransform( + drake::math::RotationMatrix(), kFrankaToGroundOffset); + + plant->WeldFrames(plant->GetFrameByName("panda_link0"), + plant->GetFrameByName("ground"), X_F_G_franka); + plant->WeldFrames(plant->GetFrameByName("panda_link0"), + plant->GetFrameByName("platform"), X_F_P); + } + + return franka_index; +} + +drake::multibody::ModelInstanceIndex AddObjectToPlant( + drake::multibody::MultibodyPlant* plant, + drake::geometry::SceneGraph* scene_graph, + const std::string& object_model) { + Parser parser(plant, scene_graph); + parser.SetAutoRenaming(true); + return parser.AddModels(FindResourceOrThrow(object_model))[0]; +} + +void AddLCSModelsToPlant( + MultibodyPlant* plant, + SceneGraph* scene_graph, + const std::string& object_model, + const bool& include_end_effector_orientation) { + // Cannot currently handle end effector orientation (would just require new + // EE simple model with orientation DOFs). + DRAKE_DEMAND(!include_end_effector_orientation); + + Parser parser_lcs(plant); + parser_lcs.SetAutoRenaming(true); + parser_lcs.AddModels(kEndEffectorSimpleModel); + parser_lcs.AddModels(kGroundModel); + parser_lcs.AddModels(object_model); + + RigidTransform X_WI = RigidTransform::Identity(); + + RigidTransform X_W_G = RigidTransform( + drake::math::RotationMatrix(), kWorldToGroundOffset); + plant->WeldFrames(plant->world_frame(), + plant->GetFrameByName("base_link"), X_WI); + plant->WeldFrames(plant->world_frame(), + plant->GetFrameByName("ground"), X_W_G); +} + +} // namespace dairlib diff --git a/examples/sampling_c3/sampling_c3_utils.h b/examples/sampling_c3/sampling_c3_utils.h new file mode 100644 index 0000000000..3117661ee9 --- /dev/null +++ b/examples/sampling_c3/sampling_c3_utils.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +#include "drake/multibody/plant/multibody_plant.h" + +namespace dairlib { + +/// Constants for the Franka and end effector. +static constexpr const char* kFrankaModel = + "package://drake_models/franka_description/urdf/panda_arm.urdf"; +static constexpr const char* kEndEffectorModel = + "examples/sampling_c3/urdf/end_effector_full.urdf"; +static constexpr const char* kEndEffectorSimpleModel = + "examples/sampling_c3/urdf/end_effector_simple_model.urdf"; +static constexpr const char* kEndEffectorName = "end_effector_tip"; +static constexpr const char* kGroundModel = + "examples/sampling_c3/urdf/ground.urdf"; +static constexpr const char* kPlatformModel = + "examples/sampling_c3/urdf/platform.urdf"; + +/// This is the offset from the Panda's link7 frame to its flange where an end +/// effector can be attached. +static const Eigen::Vector3d kToolAttachmentFrame = {0, 0, 0.107}; + +static const Eigen::Vector3d kFrankaToGroundOffset = {0, 0, -0.029}; +static const Eigen::Vector3d kFrankaToPlatformOffset = {0, 0, -0.0145}; +static const Eigen::Vector3d kWorldToFrankaOffset = {0, 0, 0}; +static const Eigen::Vector3d kWorldToGroundOffset = kWorldToFrankaOffset + + kFrankaToGroundOffset; + +/// Add the Franka to a given multibody plant and scene graph. +/// @param plant a pointer to the MultibodyPlant +/// @param scene_graph a pointer to the SceneGraph--may be nullptr (or omitted) +/// @param include_ground_and_platform whether to include the ground and +/// platform in the plant. If false, only the Franka and end effector will be +/// added. +/// @return the ModelInstanceIndex of the Franka in the plant +drake::multibody::ModelInstanceIndex AddFrankaToPlant( + drake::multibody::MultibodyPlant* plant, + drake::geometry::SceneGraph* scene_graph = nullptr, + const bool& include_ee = true, + const bool& include_ground_and_platform = true); + +/// Add an object to a given multibody plant and scene graph. +/// @param plant a pointer to the MultibodyPlant +/// @param scene_graph a pointer to the SceneGraph--may be nullptr (or omitted) +/// @param object_model the model of the object to add to the plant +/// @return the ModelInstanceIndex of the object in the plant +drake::multibody::ModelInstanceIndex AddObjectToPlant( + drake::multibody::MultibodyPlant* plant, + drake::geometry::SceneGraph* scene_graph = nullptr, + const std::string& object_model = ""); + +/// Add LCS models to a given multibody plant and scene graph. +/// @param plant a pointer to the MultibodyPlant +/// @param scene_graph a pointer to the SceneGraph--may be nullptr (or omitted) +/// @param object_model the model of the object to add to the plant +/// @param include_end_effector_orientation whether to include the end effector +/// orientation as DOFs in the plant. True is currently unimplemented. +void AddLCSModelsToPlant( + drake::multibody::MultibodyPlant* plant, + drake::geometry::SceneGraph* scene_graph = nullptr, + const std::string& object_model = "", + const bool& include_end_effector_orientation = false); + +} // namespace dairlib diff --git a/examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml b/examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml new file mode 100644 index 0000000000..10d62aa082 --- /dev/null +++ b/examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml @@ -0,0 +1,43 @@ +# All messages are of type lcmt_timestamped_saved_traj unless otherwise noted. + +franka_state_channel: FRANKA_STATE # lcmt_robot_output +object_state_channel: OBJECT_STATE # lcmt_object_state +franka_input_channel: FRANKA_INPUT # lcmt_robot_input + +osc_channel: OSC_FRANKA # lcmt_robot_input +osc_debug_channel: OSC_DEBUG_FRANKA # lcmt_osc_output + +c3_actor_curr_plan_channel: C3_TRAJECTORY_ACTOR_CURR_PLAN +c3_object_curr_plan_channel: C3_TRAJECTORY_OBJECT_CURR_PLAN +c3_force_curr_channel: C3_FORCES_CURR # lcmt_c3_forces +c3_debug_output_curr_channel: C3_DEBUG_CURR # lcmt_c3_output + +c3_actor_best_plan_channel: C3_TRAJECTORY_ACTOR_BEST_PLAN +c3_object_best_plan_channel: C3_TRAJECTORY_OBJECT_BEST_PLAN +c3_force_best_channel: C3_FORCES_BEST # lcmt_c3_forces +c3_debug_output_best_channel: C3_DEBUG_BEST # lcmt_c3_output + +c3_trajectory_exec_actor_channel: C3_EXECUTION_TRAJECTORY_ACTOR +repos_trajectory_exec_actor_channel: REPOS_EXECUTION_TRAJECTORY_ACTOR + +tracking_trajectory_actor_channel: TRACKING_TRAJECTORY_ACTOR +tracking_trajectory_object_channel: TRACKING_TRAJECTORY_OBJECT + +c3_final_target_state_channel: C3_FINAL_TARGET # lcmt_c3_state +c3_target_state_channel: C3_TARGET # lcmt_c3_state +c3_actual_state_channel: C3_ACTUAL # lcmt_c3_state + +sample_locations_channel: SAMPLE_LOCATIONS +sample_costs_channel: SAMPLE_COSTS +sample_buffer_channel: SAMPLE_BUFFER # lcmt_sample_buffer + +dynamically_feasible_curr_actor_plan_channel: DYNAMICALLY_FEASIBLE_CURR_ACTOR_PLAN +dynamically_feasible_curr_plan_channel: DYNAMICALLY_FEASIBLE_CURR_PLAN +dynamically_feasible_best_actor_plan_channel: DYNAMICALLY_FEASIBLE_BEST_ACTOR_PLAN +dynamically_feasible_best_plan_channel: DYNAMICALLY_FEASIBLE_BEST_PLAN + +is_c3_mode_channel: IS_C3_MODE +target_generator_info_channel: TARGET_GEN_INFO +sampling_c3_debug_channel: SAMPLING_C3_DEBUG # lcmt_sampling_c3_debug + +radio_channel: SAMPLING_C3_RADIO # lcmt_radio_out diff --git a/examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml b/examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml new file mode 100644 index 0000000000..524747d8e9 --- /dev/null +++ b/examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml @@ -0,0 +1,43 @@ +# All messages are of type lcmt_timestamped_saved_traj unless otherwise noted. + +franka_state_channel: FRANKA_STATE_SIMULATION # lcmt_robot_output +object_state_channel: OBJECT_STATE_SIMULATION # lcmt_object_state +franka_input_channel: FRANKA_INPUT # lcmt_robot_input + +osc_channel: OSC_FRANKA # lcmt_robot_input +osc_debug_channel: OSC_DEBUG_FRANKA # lcmt_osc_output + +c3_actor_curr_plan_channel: C3_TRAJECTORY_ACTOR_CURR_PLAN +c3_object_curr_plan_channel: C3_TRAJECTORY_OBJECT_CURR_PLAN +c3_force_curr_channel: C3_FORCES_CURR # lcmt_c3_forces +c3_debug_output_curr_channel: C3_DEBUG_CURR # lcmt_c3_output + +c3_actor_best_plan_channel: C3_TRAJECTORY_ACTOR_BEST_PLAN +c3_object_best_plan_channel: C3_TRAJECTORY_OBJECT_BEST_PLAN +c3_force_best_channel: C3_FORCES_BEST # lcmt_c3_forces +c3_debug_output_best_channel: C3_DEBUG_BEST # lcmt_c3_output + +c3_trajectory_exec_actor_channel: C3_EXECUTION_TRAJECTORY_ACTOR +repos_trajectory_exec_actor_channel: REPOS_EXECUTION_TRAJECTORY_ACTOR + +tracking_trajectory_actor_channel: TRACKING_TRAJECTORY_ACTOR +tracking_trajectory_object_channel: TRACKING_TRAJECTORY_OBJECT + +c3_final_target_state_channel: C3_FINAL_TARGET # lcmt_c3_state +c3_target_state_channel: C3_TARGET # lcmt_c3_state +c3_actual_state_channel: C3_ACTUAL # lcmt_c3_state + +sample_locations_channel: SAMPLE_LOCATIONS +sample_costs_channel: SAMPLE_COSTS +sample_buffer_channel: SAMPLE_BUFFER # lcmt_sample_buffer + +dynamically_feasible_curr_actor_plan_channel: DYNAMICALLY_FEASIBLE_CURR_ACTOR_PLAN +dynamically_feasible_curr_plan_channel: DYNAMICALLY_FEASIBLE_CURR_PLAN +dynamically_feasible_best_actor_plan_channel: DYNAMICALLY_FEASIBLE_BEST_ACTOR_PLAN +dynamically_feasible_best_plan_channel: DYNAMICALLY_FEASIBLE_BEST_PLAN + +is_c3_mode_channel: IS_C3_MODE +target_generator_info_channel: TARGET_GEN_INFO +sampling_c3_debug_channel: SAMPLING_C3_DEBUG # lcmt_sampling_c3_debug + +radio_channel: SAMPLING_C3_RADIO # lcmt_radio_out diff --git a/examples/sampling_c3/shared_parameters/osc_params.yaml b/examples/sampling_c3/shared_parameters/osc_params.yaml new file mode 100644 index 0000000000..9682517970 --- /dev/null +++ b/examples/sampling_c3/shared_parameters/osc_params.yaml @@ -0,0 +1,77 @@ +### Parameters from OSCGains ### +controller_frequency: 1000 +w_input: 0.00 +w_input_reg: 0.0 +w_accel: 0.00001 +w_soft_constraint: 0.0 +w_lambda: 0.0 +impact_threshold: 0.000 +impact_tau: 0.000 +mu: 0.4615 + +W_accel: [ 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] +W_input_reg: [ 1, 1, 1, 1, 1, 1, 10] +W_lambda_c_reg: [ 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01, + 0.001, 0.001, 0.01 ] +W_lambda_h_reg: [ 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001 ] + + +### Additional parameters for SamplingC3OSCParams ### + +# Two teleoperation modes: 1) locked EE neutral position, where xbox controls +# perturb the EE; and 2) xbox controls move the EE to a new neutral position. +neutral_position: [0.4276197, 0.19295175, 0.12756625] +teleop_neutral_position: true + +# If using teleop_neutral_position, it's beneficial for the x/y/z_scales to be +# relatively small, i.e. 0.0002 works well (compared to 0.2 for the locked +# neutral position scheme). +x_scale: 0.0002 +y_scale: 0.0002 +z_scale: 0.0002 + +end_effector_acceleration: 10 +track_end_effector_orientation: false +cancel_gravity_compensation: false +enforce_acceleration_constraints: false +publish_debug_info: true + +w_elbow: 1 +elbow_kp: 200 +elbow_kd: 10 + +# This is the weight matrix for the end effector position tracking. +EndEffectorW: + [1, 0, 0, + 0, 1, 0, + 0, 0, 1] +EndEffectorKp: + [ 200, 0, 0, + 0, 200, 0, + 0, 0, 200 ] +EndEffectorKd: + [ 20, 0, 0, + 0, 20, 0, + 0, 0, 20 ] +EndEffectorRotW: + [ 10, 0, 0, + 0, 10, 0, + 0, 0, 10 ] +EndEffectorRotKp: + [ 800, 0, 0, + 0, 800, 0, + 0, 0, 800] +EndEffectorRotKd: + [ 40, 0, 0, + 0, 40, 0, + 0, 0, 40] +# This is the weight matrix for the end effector force tracking. +# The end effector forces in the OSC are called lambdas but are really tracking +# the forces at the end effector or the u_sol we get in c3. +LambdaEndEffectorW: + [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] diff --git a/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml b/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml new file mode 100644 index 0000000000..c34da54176 --- /dev/null +++ b/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml @@ -0,0 +1,26 @@ +print_to_console: 0 +log_file_name: "" +int_options: + max_iter: 1000 + verbose: 0 + warm_start: 1 + scaling: 1 + adaptive_rho: 1 + adaptive_rho_interval: 0 + polish: 1 + polish_refine_iter: 1 + scaled_termination: 1 + check_termination: 25 +double_options: + rho: 0.001 + sigma: 1e-6 + eps_abs: 1e-5 + eps_rel: 1e-5 + eps_prim_inf: 1e-5 + eps_dual_inf: 1e-5 + alpha: 1.6 + delta: 1e-6 + adaptive_rho_tolerance: 5 + adaptive_rho_fraction: 0.4 + time_limit: 0 +string_options: {} diff --git a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml new file mode 100644 index 0000000000..d4f5187dfe --- /dev/null +++ b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml @@ -0,0 +1,26 @@ +print_to_console: 0 +log_file_name: "" +int_options: + max_iter: 1000 + verbose: 0 + warm_start: 1 + scaling: 1 + adaptive_rho: 1 + adaptive_rho_interval: 0 + polish: 1 + polish_refine_iter: 1 + scaled_termination: 1 + check_termination: 100 +double_options: + rho: 0.1 + sigma: 1e-6 + eps_abs: 1e-5 + eps_rel: 1e-5 + eps_prim_inf: 1e-5 + eps_dual_inf: 1e-5 + alpha: 1.6 + delta: 1e-6 + adaptive_rho_tolerance: 5 + adaptive_rho_fraction: 0.4 + time_limit: 0 +string_options: {} diff --git a/examples/sampling_c3/start_logging.py b/examples/sampling_c3/start_logging.py new file mode 100644 index 0000000000..726b2e737d --- /dev/null +++ b/examples/sampling_c3/start_logging.py @@ -0,0 +1,102 @@ +import subprocess +import os +import os.path as op +import glob +import codecs +from datetime import date +import sys +import yaml + + +def main(log_type, demo_name): + curr_date = date.today().strftime("%m_%d_%y") + year = date.today().strftime("%Y") + logdir = f"/mnt/data2/bibit/logs/{year}/{curr_date}" + dair = op.abspath(op.join(op.dirname(__file__), "../../")) + + if not op.isdir(logdir): + os.mkdir(logdir) + + # Hardcoded sampling_c3_controller_params path. + sampling_c3_controller_params_path = op.join( + dair, "examples", "sampling_c3", demo_name, "parameters", + "sampling_c3_controller_params.yaml" + ) + + with open(sampling_c3_controller_params_path) as f: + controller_params = yaml.load(f, Loader=yaml.FullLoader) + + sampling_c3_params_path = op.join(dair, controller_params['sampling_c3_options_file']) + repos_params_path = op.join(dair, controller_params['reposition_params_file']) + progress_params_path = op.join(dair, controller_params['progress_params_file']) + sampling_params_path = op.join(dair, controller_params['sampling_params_file']) + goal_params_path = op.join(dair, controller_params['goal_params_file']) + sim_params_path = op.join(dair, controller_params['sim_params_file']) + osc_params_path = op.join(dair, controller_params['osc_params_file']) + osqp_params_path = op.join(dair, controller_params['osqp_settings_file']) + osc_qp_params_path = op.join(dair, controller_params['osc_qp_settings_file']) + + with open(sim_params_path) as f: + sim_params = yaml.load(f, Loader=yaml.FullLoader) + + # c3 gains, sampling c3 options, sampling, osc, sim, goal + + object_c3_urdf = op.join(dair, controller_params['object_model']) + object_sim_urdf = op.join(dair, sim_params['object_model']) + + # NOTE: must match kEndEffectorSimpleModel in sampling_c3_utils.h + ee_simple_model_urdf = op.join( + dair, "examples/sampling_c3/urdf/end_effector_simple_model.urdf") + + git_diff = subprocess.check_output(['git', 'diff'], cwd=dair) + commit_tag = subprocess.check_output(['git', 'rev-parse', 'HEAD'], cwd=dair) + + os.chdir(logdir) + + try: + directories = glob.glob(op.join(logdir, "*")) + directory_names = [op.basename(d) for d in directories if op.isdir(d)] + last_log = max([int(name) for name in directory_names if name.isdigit()]) + log_num = str(last_log+1).zfill(6) + except: + log_num = str(0).zfill(6) + + if log_type == 'hw': + with open('commit_tag%s' % log_num, 'w') as f: + f.write(str(commit_tag)) + f.write("\n\ngit diff:\n\n") + f.write(codecs.getdecoder("unicode_escape")(git_diff)[0]) + if not op.isdir(log_num): + os.mkdir(log_num) + + os.chdir(log_num) + logname = f'{log_type}log-{log_num}' + + # Parameter files. + subprocess.run(['cp',sampling_c3_controller_params_path, f'sampling_c3_controller_params_{log_num}.yaml']) + subprocess.run(['cp',sampling_c3_params_path, f'sampling_c3_params_{log_num}.yaml']) + subprocess.run(['cp',repos_params_path, f'repos_params_{log_num}.yaml']) + subprocess.run(['cp',progress_params_path, f'progress_params_{log_num}.yaml']) + subprocess.run(['cp',sampling_params_path, f'sampling_params_{log_num}.yaml']) + subprocess.run(['cp',goal_params_path, f'goal_params_{log_num}.yaml']) + subprocess.run(['cp',sim_params_path, f'sim_params_{log_num}.yaml']) + subprocess.run(['cp',osc_params_path, f'osc_params_{log_num}.yaml']) + subprocess.run(['cp',osqp_params_path, f'osqp_params_{log_num}.yaml']) + subprocess.run(['cp',osc_qp_params_path, f'osc_qp_params_{log_num}.yaml']) + + # URDFs/SDFs with original file extensions. + ee_simple_model_ext = op.splitext(ee_simple_model_urdf)[1] + object_c3_ext = op.splitext(object_c3_urdf)[1] + object_sim_ext = op.splitext(object_sim_urdf)[1] + subprocess.run(['cp', ee_simple_model_urdf, f'ee_simple_model_urdf_{log_num}{ee_simple_model_ext}']) + subprocess.run(['cp', object_c3_urdf, f'object_c3_urdf_{log_num}{object_c3_ext}']) + subprocess.run(['cp', object_sim_urdf, f'object_sim_urdf_{log_num}{object_sim_ext}']) + + # Begin logging. + subprocess.run(['/opt/lcm/1.4.0/bin/lcm-logger', '-f', logname]) + + +if __name__ == '__main__': + log_type = sys.argv[1] + demo_name = sys.argv[2] + main(log_type, demo_name) diff --git a/examples/sampling_c3/test/BUILD.bazel b/examples/sampling_c3/test/BUILD.bazel new file mode 100644 index 0000000000..2ad6b352bb --- /dev/null +++ b/examples/sampling_c3/test/BUILD.bazel @@ -0,0 +1,25 @@ +cc_binary( + name = "lcm_log_loader", + srcs = [ + "lcm_log_loader.cc", + ], + deps = [ + "@gflags", + "//common:find_resource", + "//systems/controllers:sampling_c3_controller", + "//lcm:lcm_trajectory_saver", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems:system_utils", + "//solvers:c3", + "//solvers:lcs", + "//solvers:c3_output", + "//solvers:solver_options_io", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "//examples/sampling_c3:sampling_c3_utils", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:franka_sim_params", + "//examples/sampling_c3/parameter_headers:sampling_c3_options", + ], +) diff --git a/examples/sampling_c3/test/cost_visualization.py b/examples/sampling_c3/test/cost_visualization.py new file mode 100644 index 0000000000..b140329796 --- /dev/null +++ b/examples/sampling_c3/test/cost_visualization.py @@ -0,0 +1,161 @@ +"""Script to visualize the costs of samples. Requires the drake_env virtual +environment located at workspace/drake_env, i.e.: + +source ~/workspace/drake_env/bin/activate + +Visualizes the costs and configurations exported by lcm_log_loader.cc when the +DO_SAMPLE_VISUALIZATIONS is #defined. That log loading script writes files to +the test/tmp directory. This script loads those files and visualizes the +information in meshcat. + +Run this script with the following command: +python examples/sampling_c3/test/cost_visualization.py jacktoy +""" +import argparse +import matplotlib.pyplot as plt +import numpy as np +import os.path as op + +from pydrake.common.eigen_geometry import Quaternion +from pydrake.geometry import MeshcatVisualizer, MeshcatVisualizerParams, \ + SceneGraph, StartMeshcat +from pydrake.math import RigidTransform, RotationMatrix +from pydrake.multibody.plant import MultibodyPlant +from pydrake.multibody.parsing import Parser +from pydrake.multibody.tree import FixedOffsetFrame +from pydrake.perception import BaseField, Fields, PointCloud +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.visualization._triad import AddFrameTriadIllustration + + +def costs_to_colors(costs): + """Returns (3, N) array of RGB colors based on costs.""" + # Try doing a map that will make the good samples more obvious. + normalized_costs = (costs - costs.min()) / (costs.max() - costs.min()) + mapped = np.arctan(normalized_costs * 100) / (np.pi/2) + mapped /= mapped.max() + colormap = plt.cm.RdYlGn.reversed() + rgba_colors = colormap(mapped) + return rgba_colors[:, :3].T * 255 + + +def visualize(demo_name): + + DAIRLIB_DIR = op.abspath(op.join(op.dirname(__file__), '../../../')) + print(f"DAIRLIB_DIR: {DAIRLIB_DIR}") + LOAD_DIR = op.join(DAIRLIB_DIR, f'examples/sampling_c3/{demo_name}/test/tmp') + + # Load the data exported by the LCM log loader. + x_lcs_samples = np.loadtxt(op.join(LOAD_DIR, 'x_lcs_samples.txt')) + costs = np.loadtxt(op.join(LOAD_DIR, 'costs.txt')) + x_lcs_desired = np.loadtxt(op.join(LOAD_DIR, 'x_lcs_desired.txt')) + p_world_to_franka = np.loadtxt(op.join(LOAD_DIR, 'p_world_to_franka.txt')) + p_franka_to_ground = np.loadtxt(op.join(LOAD_DIR, 'p_franka_to_ground.txt')) + + # Center the jack at the origin for faster panning/viewing in meshcat. + jack_xy = x_lcs_samples[0, 7:9].copy() + x_lcs_samples[:, :2] -= jack_xy + x_lcs_samples[:, 7:9] -= jack_xy + x_lcs_desired[:2] -= jack_xy + x_lcs_desired[7:9] -= jack_xy + + # Start building the scene for visualization in meshcat. + builder = DiagramBuilder() + plant = builder.AddSystem(MultibodyPlant(time_step=0.0)) + scene_graph = builder.AddSystem(SceneGraph()) + plant.RegisterAsSourceForSceneGraph(scene_graph) + + ee_urdf = op.join(DAIRLIB_DIR, 'examples/sampling_c3/urdf/end_effector_simple_model.urdf') + ground_urdf = op.join(DAIRLIB_DIR, 'examples/sampling_c3/urdf/ground.urdf') + if demo_name == "jacktoy": + jack_urdf = op.join(DAIRLIB_DIR, 'examples/sampling_c3/urdf/jack.sdf') + triad_body_name = "capsule_1" + quaternion_input = Quaternion(x_lcs_desired[3:7]/np.linalg.norm(x_lcs_desired[3:7])) + elif demo_name == "push_t": + jack_urdf = op.join(DAIRLIB_DIR, 'examples/sampling_c3/urdf/T_vertical.sdf') + triad_body_name = "vertical_link" + quaternion_input = Quaternion(x_lcs_desired[3:7]/np.linalg.norm(x_lcs_desired[3:7])) + elif demo_name == "ball_rolling": + jack_urdf = op.join(DAIRLIB_DIR, 'examples/sampling_c3/urdf/sphere.urdf') + triad_body_name = "sphere" + quaternion_input = Quaternion(x_lcs_desired[3:7]/np.linalg.norm(x_lcs_desired[3:7])) + elif demo_name == "box_topple": + jack_urdf = op.join(DAIRLIB_DIR, 'examples/sampling_c3/urdf/box.sdf') + triad_body_name = "box" + quaternion_input = Quaternion(x_lcs_desired[3:7]/np.linalg.norm(x_lcs_desired[3:7])) + else: + raise ValueError(f"Unknown demo_name: {demo_name}") + + # Add the models. + urdf_path = jack_urdf + Parser(plant).AddModels(ee_urdf) + Parser(plant).AddModels(jack_urdf) + Parser(plant).AddModels(ground_urdf) + + p_world_to_ground = p_world_to_franka + p_franka_to_ground + X_W_Ground = RigidTransform(RotationMatrix(), p_world_to_ground) + quaternion_input = Quaternion(x_lcs_desired[3:7]/np.linalg.norm(x_lcs_desired[3:7])) + X_WGoal = RigidTransform( + quaternion=quaternion_input, p=x_lcs_desired[7:10]) + plant.WeldFrames( + plant.world_frame(), plant.GetFrameByName("base_link"), RigidTransform()) + plant.WeldFrames( + plant.world_frame(), plant.GetFrameByName("ground"), X_W_Ground) + plant.AddFrame( + FixedOffsetFrame(name="goal", P=plant.world_frame(), X_PF=X_WGoal)) + + # Add some triads for the goal and current configuration. + AddFrameTriadIllustration( + plant=plant, scene_graph=scene_graph, body=plant.GetBodyByName(triad_body_name), + length=0.1, radius=0.005, opacity=1.0) + AddFrameTriadIllustration( + plant=plant, scene_graph=scene_graph, frame=plant.GetFrameByName("goal"), + length=0.1, radius=0.005, opacity=1.0) + plant.Finalize() + + builder.Connect(plant.get_geometry_pose_output_port(), + scene_graph.get_source_pose_port(plant.get_source_id())) + builder.Connect(scene_graph.get_query_output_port(), + plant.get_geometry_query_input_port()) + + # Add a point cloud to represent the samples and their associated costs. + fields = Fields(BaseField.kXYZs | BaseField.kRGBs) + point_cloud = PointCloud(new_size=costs.shape[0], fields=fields) + point_cloud.mutable_xyzs()[:] = x_lcs_samples[:, :3].T + point_cloud.mutable_rgbs()[:] = costs_to_colors(costs) + + # Add meshcat visualization. + meshcat = StartMeshcat() + meshcat.Delete() + visualizer = MeshcatVisualizer.AddToBuilder( + builder, scene_graph, meshcat, MeshcatVisualizerParams() + ) + print(f"Meshcat is running at: {meshcat.web_url()}") + + meshcat.SetObject(path="point_cloud", cloud=point_cloud, point_size=0.005) + + # Build the diagram. + diagram = builder.Build() + context = diagram.CreateDefaultContext() + + # Start a simulator. + sim = Simulator(diagram) + sim.set_publish_at_initialization(True) + sim.set_publish_every_time_step(True) + sim.Initialize() + + # Set the pose of the model. + plant_context = plant.GetMyMutableContextFromRoot(sim.get_mutable_context()) + plant.SetPositionsAndVelocities(plant_context, x_lcs_samples[0]) + sim.AdvanceTo(0) + + breakpoint() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Visualize costs of samples in meshcat.") + parser.add_argument("demo_name", choices=["jacktoy","ball_rolling", "box_topple", "push_t"], + help="Name of the demo to visualize") + args = parser.parse_args() + visualize(args.demo_name) diff --git a/examples/sampling_c3/test/lcm_log_loader.cc b/examples/sampling_c3/test/lcm_log_loader.cc new file mode 100644 index 0000000000..269d11995b --- /dev/null +++ b/examples/sampling_c3/test/lcm_log_loader.cc @@ -0,0 +1,1225 @@ +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "dairlib/lcmt_c3_state.hpp" +#include "dairlib/lcmt_object_state.hpp" +#include "dairlib/lcmt_radio_out.hpp" +#include "dairlib/lcmt_robot_output.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "examples/sampling_c3/sampling_c3_utils.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/franka_sim_params.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" +#include "solvers/c3_output.h" +#include "solvers/lcs_factory.h" +#include "systems/controllers/sampling_based_c3_controller.h" +#include "systems/framework/timestamped_vector.h" +#include "systems/system_utils.h" + +// To use this file, run as follows based on the demo name: +// bazel-bin/examples/sampling_c3/test/lcm_log_loader --demo_name=jacktoy +// /mnt/data2/sharanya/logs/2025/05_07_25/000000 1 +// bazel-bin/examples/sampling_c3/test/lcm_log_loader --demo_name=push_t +// /mnt/data2/sharanya/logs/2025/05_07_25/000001 1 +// bazel-bin/examples/sampling_c3/test/lcm_log_loader --demo_name=box_topple +// /mnt/data2/sharanya/logs/2025/05_07_25/000002 1 + +/////// WARNING!! ////// +// This version of the log loader will not work for logs collected post March +// 31st, 2025 due to the changes made to sampling_c3_controller_params.h as per +// this commit where the safe params were removed: +// https://github.com/DAIRLab/dairlib/commit/c381e5cd89500cfd3d702ada6d3c659fe8bbb43e#diff-a6f0508e6ca4588155b0f75ba718f66042ed9d05ee1b7727a963820c6fcf0cca +// This is because the log loader is trying to read a vector of c3_options files +// which is no longer the case. + +DEFINE_string(demo_name, "unkown", + "Demo within sampling_c3; used to find controller params file"); + +// Uncomment this line to output cost information for evenly spaced samples. +// #define DO_SAMPLE_VISUALIZATIONS + +// Sample grid locations. +#define N_VERTICAL 100 +#define N_HORIZONTAL 100 + +#define N_Q 10 +#define N_V 9 + +namespace dairlib { + +using dairlib::systems::TimestampedVector; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::math::RigidTransform; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::DiagramBuilder; +using solvers::LCSFactory; + +// Declare function that will generate samples around T location. +std::vector GenerateEvenlySpacedSamplesAroundT( + const Eigen::VectorXd& x_lcs, + const SamplingParams& sampling_params, const int& num_vertical, + const int& num_horizontal); + +// Declare function that will generate samples around jack location. +std::pair, std::vector> +GenerateEvenlySpacedSamplesAroundJack( + const Eigen::VectorXd& x_lcs, + const SamplingParams& sampling_params, const int& num_vertical, + const int& num_horizontal); + +// Declare helper function that will print a vector of Eigen::VectorXd in a +// Python-friendly format to define a numpy array. +void PythonFriendlyVectorOfVectorXdPrint( + char* name, std::vector some_vector); + +// Declare helper function that will write a vector of Eigen::VectorXd to a txt +// file such that it can be loaded from a python file via np.loadtxt. +void PythonFriendlyVectorOfVectorXdToFile( + char* name, std::vector some_vector); + +int DoMain(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + if (argc != 3) { + std::cerr << "Usage: " << argv[0] + << " --demo_name={DEMO} " + << std::endl; + return 1; + } + std::cout << "LOG LOADER FOR PUSH T EXAMPLE" << std::endl; + const std::string& log_folder = std::string(argv[1]); + const double& time_into_log = std::stod(std::string(argv[2])); + + // Turn the folder into a file path. + std::string log_number = + log_folder.substr(log_folder.find_last_of("/") + 1, 6); + std::string log_filepath = log_folder + "/simlog-" + log_number; + std::cout << "Parsing log at: " << log_filepath << std::endl; + + // Set the start time. + // This time is how many seconds into the log we want to start processing + // information. Convert to microseconds. + const int64_t time_into_log_in_microsecs = time_into_log * 1e6; + + std::cout << "time into log in seconds: " << time_into_log << std::endl; + std::cout << " in microseconds: " << time_into_log_in_microsecs << std::endl; + + // Load the recorded parameters: (1/6) Controller parameters. + // TODO @bibit these need to be loaded from the stored log copy, not from the + // filepath in the controller params + std::string sampling_c3_controller_params_path = log_filepath; + std::string to_replace = "simlog-"; + std::string sampling_c3_controller_params_path_replacement = + "sampling_c3_controller_params_"; + sampling_c3_controller_params_path.replace( + sampling_c3_controller_params_path.find(to_replace), to_replace.length(), + sampling_c3_controller_params_path_replacement); + SamplingC3ControllerParams controller_params = + drake::yaml::LoadYamlFile( + sampling_c3_controller_params_path + ".yaml"); + std::cout << "path of sampling_c3_controller_params loaded: " + << sampling_c3_controller_params_path << std::endl; + + // (2/6) C3 parameters. + std::string c3_gains_path = log_filepath; + std::string c3_gains_path_replacement = "c3_gains_"; + c3_gains_path.replace(c3_gains_path.find(to_replace), to_replace.length(), + c3_gains_path_replacement); + C3Options c3_options; + c3_options = drake::yaml::LoadYamlFile(c3_gains_path + ".yaml"); + + // Sampling c3 options. + // WARNING: ALL LOGS PRIOR TO MAY 15, 2025 WILL NOT HAVE A SAMPLING C3 + // OPTIONS. PLEASE USE AN OLDER VERSION OF THIS FILE TO LOAD LOGS COLLECTED + // PRIOR TO MAY 15, 2025. + std::string sampling_c3_options_path = log_filepath; + std::string sampling_c3_options_path_replacement = "sampling_c3_options_"; + sampling_c3_options_path.replace(sampling_c3_options_path.find(to_replace), + to_replace.length(), + sampling_c3_options_path_replacement); + SamplingC3Options sampling_c3_options = + drake::yaml::LoadYamlFile(sampling_c3_options_path + + ".yaml"); + // NOTE: can temporarily hard code many more ADMM iterations or other + // changes here, e.g.: + // c3_options.admm_iter = 8; + + // example of updating Q to test effect of changing cost weights on C3 solve + // std::vector new_q {0.01, 0.01, 0.01, + // 0.1, 0.1, 0.1, 0.1, + // 250, 250, 250, + // 2.5, 2.5, 2.5, + // 0.05, 0.05, 0.05, + // 0.05, 0.05, 0.05}; + // c3_options.q_vector_pose = new_q; + // Eigen::VectorXd q_pose = Eigen::Map( + // c3_options.q_vector_pose.data(), c3_options.q_vector_pose.size()); + // c3_options.Q_pose = c3_options.w_Q * q_pose.asDiagonal(); + // std::cout<<"Updated Q"<(sim_params_path + ".yaml"); + + // (4/6) Sampling parameters. + std::string sampling_params_path = log_filepath; + std::string sampling_params_path_replacement = "sampling_params_"; + sampling_params_path.replace(sampling_params_path.find(to_replace), + to_replace.length(), + sampling_params_path_replacement); + SamplingParams sampling_params = drake::yaml::LoadYamlFile( + sampling_params_path + ".yaml"); + // NOTE: hard code the number of additional samples to be 0, since this + // script is just to debug a single C3 solve. + sampling_params.num_additional_samples_c3 = 0; + sampling_params.num_additional_samples_repos = 0; + + // (5/6) Progress parameters. + std::string progress_params_path = log_filepath; + std::string progress_params_path_replacement = "progress_params_"; + progress_params_path.replace(progress_params_path.find(to_replace), + to_replace.length(), + progress_params_path_replacement); + SamplingC3ProgressParams progress_params = + drake::yaml::LoadYamlFile( + progress_params_path + ".yaml"); + + // (6/6) dummy reposition parameters (should not matter). + SamplingC3RepositionParams reposition_params; + + // Create an instance of the LCM log handler. + lcm::LCM lcm; + lcm::LogFile log_file(log_filepath, "r"); + // Keep track of the timestamp of the first message in the log. + int64_t u_init_time = 0; + const lcm::LogEvent* first_event = log_file.readNextEvent(); + if (first_event != nullptr) { + u_init_time = first_event->timestamp; + std::cout << "Initial event timestamp (s): " << u_init_time / 1e6 + << std::endl; + } else { + std::cerr << "Error: No events in the log!" << std::endl; + return 1; // Exit if no events are found + } + + // Now seek to the time we want to start processing the log. That is, + // time_into_log_in_microseconds offset from the first message in the log. + log_file.seekToTimestamp(time_into_log_in_microsecs + u_init_time); + + const lcm::LogEvent* event; + + // Read the log until the first C3_ACTUAL message after the start time. + // Prepare to grab all desired message information. + std::set channels_to_check = { + "C3_ACTUAL", + "C3_TARGET", + "C3_FINAL_TARGET", + "DYNAMICALLY_FEASIBLE_CURR_PLAN", + "DYNAMICALLY_FEASIBLE_CURR_ACTOR_PLAN", + "C3_DEBUG_CURR", + "IS_C3_MODE", + "SAMPLE_LOCATIONS", + "SAMPLE_COSTS", + "DYNAMICALLY_FEASIBLE_BEST_PLAN", + "DYNAMICALLY_FEASIBLE_BEST_ACTOR_PLAN"}; + + Eigen::VectorXd x_lcs_actual = Eigen::VectorXd::Zero(19); + Eigen::VectorXd x_lcs_desired = Eigen::VectorXd::Zero(19); + Eigen::VectorXd x_lcs_final_desired = Eigen::VectorXd::Zero(19); + Eigen::MatrixXd dyn_feas_curr_plan_obj_pos = + Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd dyn_feas_curr_plan_ee_pos = + Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd dyn_feas_curr_plan_obj_orientation = + Eigen::MatrixXd::Zero(4, c3_options.N + 1); + Eigen::MatrixXd dyn_feas_best_plan_obj_pos = + Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd dyn_feas_best_plan_ee_pos = + Eigen::MatrixXd::Zero(3, c3_options.N + 1); + Eigen::MatrixXd dyn_feas_best_plan_obj_orientation = + Eigen::MatrixXd::Zero(4, c3_options.N + 1); + + Eigen::MatrixXd u_sol = Eigen::MatrixXd::Zero(3, c3_options.N); + Eigen::MatrixXd x_sol = Eigen::MatrixXd::Zero(19, c3_options.N); + Eigen::MatrixXd lambda_sol = Eigen::MatrixXd::Zero(16, c3_options.N); + Eigen::MatrixXd w_sol = Eigen::MatrixXd::Zero(38, c3_options.N); + Eigen::MatrixXd delta_sol = Eigen::MatrixXd::Zero(38, c3_options.N); + + // Collect the sample locations + std::vector sample_locations_in_log; + std::vector sample_costs_in_log; + + bool is_c3_mode = false; + bool is_c3_mode_set = false; + + while ((event = log_file.readNextEvent()) != nullptr) { + // Offset the time stamp by the initial time for better readability. + int64_t adjusted_utimestamp = event->timestamp - u_init_time; + + if (event->channel == "C3_ACTUAL") { + if (event->timestamp > time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_c3_state message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received C3_ACTUAL message in seconds utime: " + << (message.utime) / 1e6 << " and event timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; i < 19; i++) { + x_lcs_actual(i) = static_cast(message.state[i]); + } + } else { + std::cerr << "Failed to decode C3_ACTUAL message" << std::endl; + } + } + } else if (event->channel == "C3_TARGET") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_c3_state message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received C3_TARGET message in seconds utime: " + << (message.utime) / 1e6 << " and event timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; i < 19; i++) { + x_lcs_desired(i) = static_cast(message.state[i]); + } + } else { + std::cerr << "Failed to decode C3_TARGET message" << std::endl; + } + } + } else if (event->channel == "C3_FINAL_TARGET") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_c3_state message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received C3_FINAL_TARGET message in seconds utime: " + << (message.utime) / 1e6 << " and event timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; i < 19; i++) { + x_lcs_final_desired(i) = static_cast(message.state[i]); + } + } else { + std::cerr << "Failed to decode C3_FINAL_TARGET message" << std::endl; + } + } + } else if (event->channel == "DYNAMICALLY_FEASIBLE_CURR_PLAN") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received DYNAMICALLY_FEASIBLE_CURR_PLAN message in " + << "seconds utime: " << (message.utime) / 1e6 + << " and event " << "timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < c3_options.N + 1; j++) { + dyn_feas_curr_plan_obj_orientation(i, j) = + message.saved_traj.trajectories[0].datapoints[i][j]; + } + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < c3_options.N + 1; j++) { + dyn_feas_curr_plan_obj_pos(i, j) = + message.saved_traj.trajectories[1].datapoints[i][j]; + } + } + } else { + std::cerr << "Failed to decode DYNAMICALLY_FEASIBLE_CURR_PLAN " + << "message" << std::endl; + } + } + } else if (event->channel == "DYNAMICALLY_FEASIBLE_CURR_ACTOR_PLAN") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received DYNAMICALLY_FEASIBLE_CURR_ACTOR_PLAN " + << "message in seconds utime: " << (message.utime) / 1e6 + << " and event timestamp " << adjusted_utimestamp / 1e6 + << std::endl; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < c3_options.N + 1; j++) { + dyn_feas_curr_plan_ee_pos(i, j) = + message.saved_traj.trajectories[0].datapoints[i][j]; + } + } + } else { + std::cerr << "Failed to decode DYNAMICALLY_FEASIBLE_CURR_ACTOR_PLAN" + << " message" << std::endl; + } + } + } else if (event->channel == "DYNAMICALLY_FEASIBLE_BEST_PLAN") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received DYNAMICALLY_FEASIBLE_BEST_PLAN " + << "message in seconds utime: " << (message.utime) / 1e6 + << " and event timestamp " << adjusted_utimestamp / 1e6 + << std::endl; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < c3_options.N + 1; j++) { + dyn_feas_best_plan_obj_orientation(i, j) = + message.saved_traj.trajectories[0].datapoints[i][j]; + } + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < c3_options.N + 1; j++) { + dyn_feas_best_plan_obj_pos(i, j) = + message.saved_traj.trajectories[1].datapoints[i][j]; + } + } + } else { + std::cerr << "Failed to decode DYNAMICALLY_FEASIBLE_BEST_PLAN" + << " message" << std::endl; + } + } + } else if (event->channel == "DYNAMICALLY_FEASIBLE_BEST_ACTOR_PLAN") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received DYNAMICALLY_FEASIBLE_BEST_ACTOR_PLAN " + << "message in seconds utime: " << (message.utime) / 1e6 + << " and event timestamp " << adjusted_utimestamp / 1e6 + << std::endl; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < c3_options.N + 1; j++) { + dyn_feas_best_plan_ee_pos(i, j) = + message.saved_traj.trajectories[0].datapoints[i][j]; + } + } + } else { + std::cerr << "Failed to decode DYNAMICALLY_FEASIBLE_BEST_ACTOR_PLAN" + << " message" << std::endl; + } + } + } else if (event->channel == "C3_DEBUG_CURR") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_c3_output message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received C3_DEBUG_CURR message in seconds utime: " + << (message.utime) / 1e6 << " and event timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < c3_options.N; j++) { + u_sol(i, j) = + static_cast(message.c3_solution.u_sol[i][j]); + } + } + for (int i = 0; i < 19; i++) { + for (int j = 0; j < c3_options.N; j++) { + x_sol(i, j) = + static_cast(message.c3_solution.x_sol[i][j]); + } + } + for (int i = 0; i < 16; i++) { + for (int j = 0; j < c3_options.N; j++) { + lambda_sol(i, j) = + static_cast(message.c3_solution.lambda_sol[i][j]); + } + } + for (int i = 0; i < 38; i++) { + for (int j = 0; j < c3_options.N; j++) { + w_sol(i, j) = + static_cast(message.c3_intermediates.w_sol[i][j]); + } + } + for (int i = 0; i < 38; i++) { + for (int j = 0; j < c3_options.N; j++) { + delta_sol(i, j) = + static_cast(message.c3_intermediates.delta_sol[i][j]); + } + } + } else { + std::cerr << "Failed to decode C3_DEBUG_CURR message" << std::endl; + } + } + } else if (event->channel == "IS_C3_MODE") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received IS_C3_MODE message in " + << "seconds utime: " << (message.utime) / 1e6 + << " and event " << "timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + is_c3_mode = message.saved_traj.trajectories[0].datapoints[0][0]; + is_c3_mode_set = true; + } else { + std::cerr << "Failed to decode IS_C3_MODE message" << std::endl; + } + } + } else if (event->channel == "SAMPLE_LOCATIONS") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received SAMPLE_LOCATIONS message in " + << "seconds utime: " << (message.utime) / 1e6 + << " and event " << "timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; + i < message.saved_traj.trajectories[0].datapoints[0].size(); + i++) { + Eigen::VectorXd sample_location = Eigen::VectorXd::Zero(3); + for (int j = 0; j < 3; j++) { + sample_location(j) = + message.saved_traj.trajectories[0].datapoints[j][i]; + } + sample_locations_in_log.push_back(sample_location); + } + } else { + std::cerr << "Failed to decode SAMPLE_LOCATIONS message" << std::endl; + } + } + } else if (event->channel == "SAMPLE_COSTS") { + if (event->timestamp >= time_into_log_in_microsecs + u_init_time) { + dairlib::lcmt_timestamped_saved_traj message; + if (message.decode(event->data, 0, event->datalen) > 0) { + std::cout << "Received SAMPLE_COSTS message in " + << "seconds utime: " << (message.utime) / 1e6 + << " and event " << "timestamp " + << adjusted_utimestamp / 1e6 << std::endl; + for (int i = 0; + i < message.saved_traj.trajectories[0].datapoints[0].size(); + i++) { + sample_costs_in_log.push_back(Eigen::VectorXd::Constant( + 1, message.saved_traj.trajectories[0].datapoints[0][i])); + } + } else { + std::cerr << "Failed to decode SAMPLE_COSTS message" << std::endl; + } + } + } + + // Break out of loop if we have one message for every desired channel. + if (channels_to_check.find(event->channel) != channels_to_check.end()) { + if ((x_lcs_actual != Eigen::VectorXd::Zero(19)) && + (x_lcs_desired != Eigen::VectorXd::Zero(19)) && + (x_lcs_final_desired != Eigen::VectorXd::Zero(19)) && + (dyn_feas_curr_plan_ee_pos != + Eigen::MatrixXd::Zero(3, c3_options.N + 1)) && + (dyn_feas_curr_plan_obj_pos != + Eigen::MatrixXd::Zero(3, c3_options.N + 1)) && + (dyn_feas_curr_plan_obj_orientation != + Eigen::MatrixXd::Zero(4, c3_options.N + 1)) && + (u_sol != Eigen::MatrixXd::Zero(3, c3_options.N)) && + (is_c3_mode_set) && (sample_locations_in_log.size() > 0) && + (sample_costs_in_log.size() > 0)) { + break; + } + } + } + + std::cout << "x_lcs_actual from log : " << x_lcs_actual.transpose() << "\n" + << std::endl; + std::cout << "Available sample locations from log and corresponding costs: " + << std::endl; + std::cout << "Number of sample locations in log: " + << sample_locations_in_log.size() << std::endl; + for (int i = 0; i < sample_locations_in_log.size(); i++) { + if (i < sample_costs_in_log.size()) { + std::cout << "sample " << i << " : " + << sample_locations_in_log[i].transpose() << " cost is " + << sample_costs_in_log[i] << std::endl; + if (i == 0) { + std::cout << "Dynamically feasible plan is: " << std::endl; + std::cout << "End effector position: " + << dyn_feas_curr_plan_ee_pos.transpose() << std::endl; + std::cout << "Object orientation: " + << dyn_feas_curr_plan_obj_orientation.transpose() + << std::endl; + std::cout << "Object position: " + << dyn_feas_curr_plan_obj_pos.transpose() << std::endl; + } + if (i == 1) { + std::cout << "Dynamically feasible best plan is: " << std::endl; + std::cout << "End effector position: " + << dyn_feas_best_plan_ee_pos.transpose() << std::endl; + std::cout << "Object orientation: " + << dyn_feas_best_plan_obj_orientation.transpose() + << std::endl; + std::cout << "Object position: " + << dyn_feas_best_plan_obj_pos.transpose() << std::endl; + } + } else { + std::cout << "sample " << i << " : " + << sample_locations_in_log[i].transpose() << " cost is " + << "N/A" << std::endl; + } + } + + std::cout << "\nFuture timestamps:" << std::endl; + int more_msgs_to_print = 5; + while (((event = log_file.readNextEvent()) != nullptr) && + (more_msgs_to_print > 0)) { + if (event->channel == "C3_ACTUAL") { + std::cout << "Received C3_ACTUAL message in seconds: " + << (event->timestamp - u_init_time) / 1e6 << std::endl; + more_msgs_to_print--; + } + } + std::cout << "" << std::endl; + + // Create the plant for the LCS. + DiagramBuilder plant_builder; + auto [plant_for_lcs, scene_graph] = + AddMultibodyPlantSceneGraph(&plant_builder, 0.0); + AddLCSModelsToPlant(&plant_for_lcs, &scene_graph, + controller_params.object_model, + controller_params.include_end_effector_orientation); + plant_for_lcs.Finalize(); + std::unique_ptr> plant_for_lcs_autodiff = + drake::systems::System::ToAutoDiffXd(plant_for_lcs); + + auto plant_diagram = plant_builder.Build(); + std::unique_ptr> diagram_context = + plant_diagram->CreateDefaultContext(); + auto& plant_for_lcs_context = plant_diagram->GetMutableSubsystemContext( + plant_for_lcs, diagram_context.get()); + auto plant_for_lcs_context_ad = + plant_for_lcs_autodiff->CreateDefaultContext(); + + std::vector>> contact_pairs; + + // Creating a map of contact geoms + std::unordered_map contact_geoms; + + // Change contact geoms based on the demo_name + if (FLAGS_demo_name == "push_t") { + drake::geometry::GeometryId ee_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("end_effector_simple"))[0]; + drake::geometry::GeometryId horizontal_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("horizontal_link"))[0]; + drake::geometry::GeometryId vertical_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("vertical_link"))[0]; + + drake::geometry::GeometryId corner_nxynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_nxynz"))[0]; + drake::geometry::GeometryId corner_nxnynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_nxnynz"))[0]; + drake::geometry::GeometryId corner_xynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_xynz"))[0]; + + drake::geometry::GeometryId ground_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("ground"))[0]; + + // Creating a map of contact geoms + contact_geoms["EE"] = ee_contact_points; + contact_geoms["horizontal_link"] = horizontal_geoms; + contact_geoms["vertical_link"] = vertical_geoms; + contact_geoms["corner_nxynz"] = corner_nxynz_geoms; + contact_geoms["corner_nxnynz"] = corner_nxnynz_geoms; + contact_geoms["corner_xynz"] = corner_xynz_geoms; + contact_geoms["GROUND"] = ground_geoms; + + std::vector> ee_contact_pairs; + + // TODO @bibit contact pair ordering needs to be (ee-ground, ee-jack, jack-ground) + // Creating a list of contact pairs for the end effector and the object to + // hand over to lcs factory in the controller to resolve + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["horizontal_link"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["vertical_link"])); + + // Creating a list of contact pairs for the object and the ground + SortedPair ground_contact_1{ + SortedPair(contact_geoms["corner_nxynz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_2{ + SortedPair(contact_geoms["corner_nxnynz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_3{ + SortedPair(contact_geoms["corner_xynz"], contact_geoms["GROUND"])}; + + contact_pairs.push_back(ee_contact_pairs); + + if (sampling_c3_options.num_contacts_index == 2 || + sampling_c3_options.num_contacts_index == 3) { + // If num_contacts_index is 2 or 3, we add an additional contact pair + // between the end effector and the ground. + std::vector> ee_ground_contact{ + SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; + contact_pairs.push_back(ee_ground_contact); + } + std::vector> ground_object_contact_pairs; + ground_object_contact_pairs.push_back(ground_contact_1); + ground_object_contact_pairs.push_back(ground_contact_2); + ground_object_contact_pairs.push_back(ground_contact_3); + contact_pairs.push_back(ground_object_contact_pairs); + } else if (FLAGS_demo_name == "box_topple") { + drake::geometry::GeometryId ee_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("end_effector_simple"))[0]; + drake::geometry::GeometryId box_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("box"))[0]; + + drake::geometry::GeometryId corner_xynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_xynz"))[0]; + drake::geometry::GeometryId corner_xnynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_xnynz"))[0]; + drake::geometry::GeometryId corner_nxynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_nxynz"))[0]; + drake::geometry::GeometryId corner_nxnynz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_nxnynz"))[0]; + drake::geometry::GeometryId corner_xyz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_xyz"))[0]; + drake::geometry::GeometryId corner_xnyz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_xnyz"))[0]; + drake::geometry::GeometryId corner_nxyz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_nxyz"))[0]; + drake::geometry::GeometryId corner_nxnyz_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("corner_nxnyz"))[0]; + + drake::geometry::GeometryId ground_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("ground"))[0]; + + // Creating a map of contact geoms + contact_geoms["EE"] = ee_contact_points; + contact_geoms["box"] = box_geoms; + contact_geoms["corner_xynz"] = corner_xynz_geoms; + contact_geoms["corner_xnynz"] = corner_xnynz_geoms; + contact_geoms["corner_nxynz"] = corner_nxynz_geoms; + contact_geoms["corner_nxnynz"] = corner_nxnynz_geoms; + contact_geoms["corner_xyz"] = corner_xyz_geoms; + contact_geoms["corner_xnyz"] = corner_xnyz_geoms; + contact_geoms["corner_nxyz"] = corner_nxyz_geoms; + contact_geoms["corner_nxnyz"] = corner_nxnyz_geoms; + contact_geoms["GROUND"] = ground_geoms; + + std::vector> ee_contact_pairs; + + // Creating a list of contact pairs for the end effector and the object to + // hand over to lcs factory in the controller to resolve + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["box"])); + + // Creating a list of contact pairs for the object and the ground + SortedPair ground_contact_1{ + SortedPair(contact_geoms["corner_xynz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_2{ + SortedPair(contact_geoms["corner_xnynz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_3{ + SortedPair(contact_geoms["corner_nxynz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_4{ + SortedPair(contact_geoms["corner_nxnynz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_5{ + SortedPair(contact_geoms["corner_xyz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_6{ + SortedPair(contact_geoms["corner_xnyz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_7{ + SortedPair(contact_geoms["corner_nxyz"], contact_geoms["GROUND"])}; + SortedPair ground_contact_8{ + SortedPair(contact_geoms["corner_nxnyz"], contact_geoms["GROUND"])}; + + contact_pairs.push_back(ee_contact_pairs); + + if (sampling_c3_options.num_contacts_index == 2 || + sampling_c3_options.num_contacts_index == 3) { + // If num_contacts_index is 2 or 3, we add an additional contact pair + // between the end effector and the ground. + std::vector> ee_ground_contact{ + SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; + contact_pairs.push_back(ee_ground_contact); + } + std::vector> ground_object_contact_pairs; + ground_object_contact_pairs.push_back(ground_contact_1); + ground_object_contact_pairs.push_back(ground_contact_2); + ground_object_contact_pairs.push_back(ground_contact_3); + ground_object_contact_pairs.push_back(ground_contact_4); + ground_object_contact_pairs.push_back(ground_contact_5); + ground_object_contact_pairs.push_back(ground_contact_6); + ground_object_contact_pairs.push_back(ground_contact_7); + ground_object_contact_pairs.push_back(ground_contact_8); + contact_pairs.push_back(ground_object_contact_pairs); + } else if (FLAGS_demo_name == "jacktoy") { + drake::geometry::GeometryId ee_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("end_effector_simple"))[0]; + drake::geometry::GeometryId capsule1_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_1"))[0]; + drake::geometry::GeometryId capsule2_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_2"))[0]; + drake::geometry::GeometryId capsule3_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_3"))[0]; + + drake::geometry::GeometryId capsule1_sphere1_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_1"))[1]; + drake::geometry::GeometryId capsule1_sphere2_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_1"))[2]; + drake::geometry::GeometryId capsule2_sphere1_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_2"))[1]; + drake::geometry::GeometryId capsule2_sphere2_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_2"))[2]; + drake::geometry::GeometryId capsule3_sphere1_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_3"))[1]; + drake::geometry::GeometryId capsule3_sphere2_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("capsule_3"))[2]; + + drake::geometry::GeometryId ground_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("ground"))[0]; + + // Creating a map of contact geoms + contact_geoms["EE"] = ee_contact_points; + contact_geoms["CAPSULE_1"] = capsule1_geoms; + contact_geoms["CAPSULE_2"] = capsule2_geoms; + contact_geoms["CAPSULE_3"] = capsule3_geoms; + contact_geoms["CAPSULE_1_SPHERE_1"] = capsule1_sphere1_geoms; + contact_geoms["CAPSULE_1_SPHERE_2"] = capsule1_sphere2_geoms; + contact_geoms["CAPSULE_2_SPHERE_1"] = capsule2_sphere1_geoms; + contact_geoms["CAPSULE_2_SPHERE_2"] = capsule2_sphere2_geoms; + contact_geoms["CAPSULE_3_SPHERE_1"] = capsule3_sphere1_geoms; + contact_geoms["CAPSULE_3_SPHERE_2"] = capsule3_sphere2_geoms; + contact_geoms["GROUND"] = ground_geoms; + + std::vector> ee_contact_pairs; + + // Creating a list of contact pairs for the end effector and the object to + // hand over to lcs factory in the controller to resolve + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_1"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_2"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_3"])); + + // Creating a list of contact pairs for the object and the ground + SortedPair ground_contact_1_1{SortedPair( + contact_geoms["CAPSULE_1_SPHERE_1"], contact_geoms["GROUND"])}; + SortedPair ground_contact_1_2{SortedPair( + contact_geoms["CAPSULE_1_SPHERE_2"], contact_geoms["GROUND"])}; + SortedPair ground_contact_2_1{SortedPair( + contact_geoms["CAPSULE_2_SPHERE_1"], contact_geoms["GROUND"])}; + SortedPair ground_contact_2_2{SortedPair( + contact_geoms["CAPSULE_2_SPHERE_2"], contact_geoms["GROUND"])}; + SortedPair ground_contact_3_1{SortedPair( + contact_geoms["CAPSULE_3_SPHERE_1"], contact_geoms["GROUND"])}; + SortedPair ground_contact_3_2{SortedPair( + contact_geoms["CAPSULE_3_SPHERE_2"], contact_geoms["GROUND"])}; + + contact_pairs.push_back(ee_contact_pairs); + + if (sampling_c3_options.num_contacts_index == 2 || + sampling_c3_options.num_contacts_index == 3) { + // If num_contacts_index is 2 or 3, we add an additional contact pair + // between the end effector and the ground. + std::vector> ee_ground_contact{ + SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; + contact_pairs.push_back(ee_ground_contact); + } + std::vector> ground_object_contact_pairs; + ground_object_contact_pairs.push_back(ground_contact_1_1); + ground_object_contact_pairs.push_back(ground_contact_1_2); + ground_object_contact_pairs.push_back(ground_contact_2_1); + ground_object_contact_pairs.push_back(ground_contact_2_2); + ground_object_contact_pairs.push_back(ground_contact_3_1); + ground_object_contact_pairs.push_back(ground_contact_3_2); + contact_pairs.push_back(ground_object_contact_pairs); + } else if (FLAGS_demo_name == "ball_rolling") { + drake::geometry::GeometryId ee_contact_points = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("end_effector_simple"))[0]; + drake::geometry::GeometryId object_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("sphere"))[0]; + drake::geometry::GeometryId ground_geoms = + plant_for_lcs.GetCollisionGeometriesForBody( + plant_for_lcs.GetBodyByName("ground"))[0]; + + // Creating a map of contact geoms + contact_geoms["EE"] = ee_contact_points; + contact_geoms["SPHERE"] = object_geoms; + contact_geoms["GROUND"] = ground_geoms; + + std::vector> ee_contact_pairs; + + // Creating a list of contact pairs for the end effector and the object to + // hand over to lcs factory in the controller to resolve + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["SPHERE"])); + // Creating a list of contact pairs for the object and the ground + std::vector> ground_contact{ + SortedPair(contact_geoms["SPHERE"], contact_geoms["GROUND"])}; + + // will have [[(ee,sphere)], [(sphere, ground)]] + contact_pairs.push_back(ee_contact_pairs); + + if (sampling_c3_options.num_contacts_index == 1) { + // If num_contacts_index is 2 or 3, we add an additional contact pair + // between the end effector and the ground. + std::vector> ee_ground_contact{ + SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; + contact_pairs.push_back(ee_ground_contact); + } + contact_pairs.push_back(ground_contact); + } + + plant_diagram->set_name(("sampling_c3_test_plant_" + FLAGS_demo_name)); + DrawAndSaveDiagramGraph(*plant_diagram); + + // Create the controller. The last bool argument is the verbose flag. + bool verbose = true; +#ifdef DO_SAMPLE_VISUALIZATIONS + verbose = false; + c3_options.use_predicted_x0_c3 = false; +#endif + DiagramBuilder builder; + auto controller = builder.AddSystem( + plant_for_lcs, &plant_for_lcs_context, *plant_for_lcs_autodiff, + plant_for_lcs_context_ad.get(), contact_pairs, controller_params, + verbose); + auto controller_context = controller->CreateDefaultContext(); + + auto owned_diagram = builder.Build(); + owned_diagram->set_name(("sampling_c3_controller_test_" + FLAGS_demo_name)); + DrawAndSaveDiagramGraph(*owned_diagram); + + // Fix static input ports. + controller->get_input_port_radio().FixValue( + controller_context.get(), drake::Value{}); + controller->get_input_port_target().FixValue(controller_context.get(), + x_lcs_desired); + controller->get_input_port_final_target().FixValue(controller_context.get(), + x_lcs_final_desired); + + std::vector x_lcs_actuals; + + // Testing sample location 0 and sample location 1 from the log from all + // sample locations. This was done to verify the anolmaly seen as a result of + // using cost type 3 in the push_T case. Eigen::VectorXd curr_sample_in_log = + // x_lcs_actual; curr_sample_in_log.head(3) = sample_locations_in_log[0]; + // x_lcs_actuals.push_back(curr_sample_in_log); + Eigen::VectorXd best_sample_in_log = x_lcs_actual; + best_sample_in_log.head(3) = sample_locations_in_log[1]; + x_lcs_actuals.push_back(best_sample_in_log); + +#ifdef DO_SAMPLE_VISUALIZATIONS + std::cout << "DO_SAMPLE_VISUALIZATIONS" << std::endl; + + std::vector x_lcs_samples; + std::vector angles; + + // Check the demo name and call the corresponding function directly. + if (FLAGS_demo_name == "push_t" || FLAGS_demo_name == "box_topple") { + // Directly call the GenerateEvenlySpacedSamplesAroundT function + x_lcs_samples = GenerateEvenlySpacedSamplesAroundT( + x_lcs_actual, sampling_params, N_VERTICAL, N_HORIZONTAL); + } else if (FLAGS_demo_name == "jacktoy" || + FLAGS_demo_name == "ball_rolling") { + // Directly call the GenerateEvenlySpacedSamplesAroundJack function + auto [samples, angle_data] = GenerateEvenlySpacedSamplesAroundJack( + x_lcs_actual, sampling_params, N_VERTICAL, N_HORIZONTAL); + x_lcs_samples = samples; + angles = angle_data; + } + + x_lcs_actuals.clear(); + x_lcs_actuals = x_lcs_samples; + + // Prepare to print out the costs. + Eigen::VectorXd costs; + + std::cout << "DO_SAMPLE_VISUALIZATIONS" << std::endl; + if (FLAGS_demo_name == "jacktoy" || FLAGS_demo_name == "ball_rolling") { + costs = Eigen::VectorXd::Zero(angles.size()); // If angles are available + } else if (FLAGS_demo_name == "push_t" || FLAGS_demo_name == "box_topple") { + costs = Eigen::VectorXd::Zero(x_lcs_samples.size()); // For the other case + } +#endif + + // Remember that doing this in a loop actually triggers the x_pred calculation + // in the controller. Ensure this is something you want to do. + for (int i = 0; i < x_lcs_actuals.size(); i++) { + Eigen::VectorXd x_lcs_actual_i = x_lcs_actuals[i]; + + // Set plant states, fix input port values. + plant_for_lcs.SetPositionsAndVelocities(&plant_for_lcs_context, + x_lcs_actual_i); + controller->get_input_port_lcs_state().FixValue( + controller_context.get(), TimestampedVector(x_lcs_actual_i)); + + // Do forced publish. + auto discrete_state = controller->AllocateDiscreteVariables(); + controller->CalcForcedDiscreteVariableUpdate(*controller_context, + discrete_state.get()); + auto sample_costs_inside = + controller->get_output_port_all_sample_costs() + .Eval>(*controller_context); + controller->ForcedPublish(*controller_context); + +#ifdef DO_SAMPLE_VISUALIZATIONS + // Get the cost of the sample by querying the sample costs output port. + auto sample_costs = controller->get_output_port_all_sample_costs() + .Eval>(*controller_context); + costs(i) = sample_costs[0]; + if (i % 10 == 0) { + std::cout << "Finished " << i << " of " << x_lcs_actuals.size() + << " samples." << std::endl; + } +#endif + } +#ifdef DO_SAMPLE_VISUALIZATIONS + // Write values like positions and costs to files which can be loaded with + // np.loadtxt() cleanly from a python script. If the outputs are small + // enough, can swap the file writing for printing to the console via + // PythonFriendlyVectorOfVectorXdPrint. + PythonFriendlyVectorOfVectorXdToFile("x_lcs_samples", x_lcs_samples); + PythonFriendlyVectorOfVectorXdToFile("costs", {costs}); + PythonFriendlyVectorOfVectorXdToFile("x_lcs_desired", {x_lcs_desired}); + PythonFriendlyVectorOfVectorXdToFile("p_world_to_franka", + {kWorldToFrankaOffset}); + PythonFriendlyVectorOfVectorXdToFile("p_franka_to_ground", + {kFrankaToGroundOffset}); + + std::cout << "\nee_urdf = op.join(DAIRLIB_DIR, '" + << kEndEffectorSimpleModel << "')" << std::endl; + std::cout << "jack_urdf = op.join(DAIRLIB_DIR, '" << sim_params.object_model + << "')" << std::endl; +#endif + + std::cout << "Finished ForcedPublish" << std::endl; + + // Example of getting the C3 solution by evaluating the output port. + // auto c3_solution = controller->get_output_port_c3_solution_curr_plan( + // ).Eval(*controller_context); + // std::cout << "\nc3_solution.x_sol_ = " << c3_solution.x_sol_ << std::endl; + // std::cout << "\nc3_solution.lambda_sol_ = " << c3_solution.lambda_sol_ + // << std::endl; + // std::cout << "\nc3_solution.u_sol_ = " << c3_solution.u_sol_ << std::endl; + // std::cout << "\nc3_solution.time_vector_ = " << c3_solution.time_vector_ + // << std::endl; + + return 0; +} + +/* Define function that will generate samples around T location. + + Args: + x_lcs_actual + sampling_params + num_vertical + num_horizontal + + Returns: + - A std::vector of x_lcs samples + - A std::vector of 2D (theta, elevation_theta) positions from which the + samples were derived. +*/ +std::vector GenerateEvenlySpacedSamplesAroundT( + const Eigen::VectorXd& x_lcs, + const SamplingParams& sampling_params, const int& num_vertical, + const int& num_horizontal) { + // Extract sampling parameters. + double sampling_height = 0.00; + + // the sampling region has an outer x and y limit. These are expressed in body + // frame. + double outer_x_limit = 0.16; + double outer_y_limit = 0.16; + + // Generate samples on a grid around the T + std::vector x_lcs_samples; + + // Compute the step size for the grid. + // num_vertical means how many rows of samples we want to take. That is x + // changes across each row. num_horizontal means how many columns of samples + // we want to take. That is y changes across each column. + double step_size_x = 2 * outer_x_limit / (num_vertical - 1); + double step_size_y = 2 * outer_y_limit / (num_horizontal - 1); + std::cout << "step_size_x: " << step_size_x << std::endl; + std::cout << "step_size_y: " << step_size_y << std::endl; + + std::vector unfiltered_samples_in_body_frame; + // Set the dummy to be the same as the actual lcs value. + Eigen::VectorXd x_lcs_sample = x_lcs; + std::cout << "x_lcs: " << x_lcs.transpose() << std::endl; + Eigen::Quaterniond quat_object(x_lcs(3), x_lcs(4), x_lcs(5), x_lcs(6)); + Eigen::Vector3d object_position = x_lcs.segment(7, 3); + + // Generate uniform samples on a grid. Replace the end effector values in the + // dummy with the sample values. + for (int i = 0; i < num_vertical; i++) { + for (int j = 0; j < num_horizontal; j++) { + x_lcs_sample(0) = -outer_x_limit + i * step_size_x; + x_lcs_sample(1) = -outer_y_limit + j * step_size_y; + x_lcs_sample(2) = sampling_height; + unfiltered_samples_in_body_frame.push_back(x_lcs_sample); + } + } + + // Define limits for sample validity. + double ee_radius = 0; // 0.0145; + double clearance = 0; // 0.002; // Maintain a clearance of 2mm from the + // T. + double x_bottom_lim_1 = 0.08 + ee_radius + clearance; + double x_top_lim_1 = -0.08 + ee_radius + clearance; + double x_top_lim_2 = -0.08 - 0.04 - ee_radius - clearance; + double y_lim_1 = 0.04 + ee_radius + clearance; + double y_lim_2 = 0.08 + ee_radius + clearance; + + // Currently setting filtering off. + std::vector filtered_samples_in_body_frame = + unfiltered_samples_in_body_frame; + + // Convert the samples to world frame. + std::vector filtered_samples_in_world_frame; + for (const auto& sample : filtered_samples_in_body_frame) { + Eigen::VectorXd sample_in_world_frame = sample; + // Convert end effector position to world frame. + sample_in_world_frame.head(3) = + quat_object * sample.head(3) + object_position; + filtered_samples_in_world_frame.push_back(sample_in_world_frame); + } + + return filtered_samples_in_world_frame; +} + +/* Define function that will generate samples around jack location. + + Args: + x_lcs_actual + sampling_params + num_vertical + num_horizontal + + Returns: + - A std::vector of x_lcs samples + - A std::vector of 2D (theta, elevation_theta) positions from which the + samples were derived. +*/ +std::pair, std::vector> +GenerateEvenlySpacedSamplesAroundJack( + const Eigen::VectorXd& x_lcs, + const SamplingParams& sampling_params, const int& num_vertical, + const int& num_horizontal) { + // Grab sampling parameters. + double sampling_radius = sampling_params.sampling_radius; + double min_angle_from_vertical = sampling_params.min_angle_from_vertical; + double max_angle_from_vertical = sampling_params.max_angle_from_vertical; + + // Pull out the q and v from the LCS state. The end effector location and + // velocity of this state will be changed for the sample. + Eigen::VectorXd q = x_lcs.head(N_Q); + Eigen::VectorXd v = x_lcs.tail(N_V); + + // Center the sampling circle on the current ball location. + Eigen::Vector3d object_xyz = q.tail(3); + double x_samplec = object_xyz[0]; + double y_samplec = object_xyz[1]; + double z_samplec = object_xyz[2]; + + // Prepare to store samples and (theta, elevation_theta) angles. + std::vector x_lcs_samples; + std::vector angles; + + // Double for loop over the number of vertical and horizontal samples. + for (int i = 0; i < num_vertical; i++) { + double elevation_theta = + min_angle_from_vertical + + i * (max_angle_from_vertical - min_angle_from_vertical) / + (num_vertical - 1); + for (int j = 0; j < num_horizontal; j++) { + double theta = j * 2 * M_PI / num_horizontal; + + // Update the hypothetical state's end effector location to the tested + // sample location. + Eigen::VectorXd q_ee = Eigen::VectorXd::Zero(3); + q_ee[0] = x_samplec + sampling_radius * cos(theta) * sin(elevation_theta); + q_ee[1] = y_samplec + sampling_radius * sin(theta) * sin(elevation_theta); + q_ee[2] = z_samplec + sampling_radius * cos(elevation_theta); + + Eigen::VectorXd candidate_state = Eigen::VectorXd::Zero(N_Q + N_V); + candidate_state << q_ee, x_lcs.segment(3, N_Q - 3), v; + + // Store the sample and the angle pair. + x_lcs_samples.push_back(candidate_state); + Eigen::Vector2d angle_pair; + angle_pair << theta, elevation_theta; + angles.push_back(angle_pair); + } + } + return std::make_pair(x_lcs_samples, angles); +} + +void PythonFriendlyVectorOfVectorXdPrint( + char* name, std::vector some_vector) { + std::cout << name << " = np.array("; + if (some_vector.size() > 1) { + std::cout << "["; + } + for (const auto& some_element : some_vector) { + std::cout << "\n\t["; + for (int i = 0; i < some_element.size(); i++) { + std::cout << some_element(i) << ", "; + } + std::cout << "], "; + } + if (some_vector.size() > 1) { + std::cout << "]"; + } + std::cout << ")" << std::endl; +} + +void PythonFriendlyVectorOfVectorXdToFile( + char* name, std::vector some_vector) { + std::string filename = + "/home/sharanya/workspace/dairlib/examples/sampling_c3/push_t/test"; + filename += "/tmp/"; + filename += name; + filename += ".txt"; + + // Remove the file if it already exists. + std::remove(filename.c_str()); + std::ofstream file(filename); + + if (!file) { + std::cerr << "Erorr: Could not create " << filename << std::endl; + } + std::cout << "Writing to " << filename << std::endl; + + for (const auto& some_element : some_vector) { + for (int i = 0; i < some_element.size(); i++) { + file << some_element(i) << " "; + } + file << "\n"; + } + file.close(); +} + +} // namespace dairlib + +int main(int argc, char* argv[]) { return dairlib::DoMain(argc, argv); } diff --git a/examples/sampling_c3/urdf/ee_visualization_model.urdf b/examples/sampling_c3/urdf/ee_visualization_model.urdf new file mode 100644 index 0000000000..b7ef70c665 --- /dev/null +++ b/examples/sampling_c3/urdf/ee_visualization_model.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/sampling_c3/urdf/end_effector_full.urdf b/examples/sampling_c3/urdf/end_effector_full.urdf new file mode 100644 index 0000000000..fc03c81b6e --- /dev/null +++ b/examples/sampling_c3/urdf/end_effector_full.urdf @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/sampling_c3/urdf/end_effector_simple_model.urdf b/examples/sampling_c3/urdf/end_effector_simple_model.urdf new file mode 100644 index 0000000000..ba138d6298 --- /dev/null +++ b/examples/sampling_c3/urdf/end_effector_simple_model.urdf @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + 1 + + + + + + 1 + + + + + + + diff --git a/examples/sampling_c3/urdf/ground.urdf b/examples/sampling_c3/urdf/ground.urdf new file mode 100644 index 0000000000..5c78d56cc5 --- /dev/null +++ b/examples/sampling_c3/urdf/ground.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/sampling_c3/urdf/jack.sdf b/examples/sampling_c3/urdf/jack.sdf new file mode 100644 index 0000000000..fe37103c5e --- /dev/null +++ b/examples/sampling_c3/urdf/jack.sdf @@ -0,0 +1,139 @@ + + + + + + 0 0 0 0 0 0 + 0.052 + + + 0.00007072 + 0 + 0 + 0.00007072 + 0 + 0.00000572 + + + + 0 0 0.0 0 0 0 + + + 0.025 + 0.125 + + + + 0.3 + + + + 0 0 0.0 0 0 0 + + + 0.025 + 0.125 + + + + 0 0 1 1 + + + + + + 0 0 0 0 1.5708 0 + 0.052 + + + 0.00007072 + 0 + 0 + 0.00007072 + 0 + 0.00000572 + + + + 0 0 0 0 1.5708 0 + + + 0.025 + 0.125 + + + + 0.3 + + + + 0 0 0 0 1.5708 0 + + + 0.025 + 0.125 + + + + 1 0 0 1 + + + + + + 0 0 0 1.5708 0 0 + 0.052 + + + 0.00007072 + 0 + 0 + 0.00007072 + 0 + 0.00000572 + + + + 0 0 0 1.5708 0 0 + + + 0.025 + 0.125 + + + + 0.3 + + + + 0 0 0 1.5708 0 0 + + + 0.025 + 0.125 + + + + 0 1 0 1 + + + + + + + capsule_1 + capsule_2 + 0 0 0 0 0 0 + + + + capsule_2 + capsule_3 + 0 0 0 0 0 0 + + + + diff --git a/examples/sampling_c3/urdf/jack_control.sdf b/examples/sampling_c3/urdf/jack_control.sdf new file mode 100644 index 0000000000..60ae41aaea --- /dev/null +++ b/examples/sampling_c3/urdf/jack_control.sdf @@ -0,0 +1,187 @@ + + + + + + 0 0 0 0 0 0 + 0.33 + + + 0.0004488 + 0 + 0 + 0.0004488 + 0 + 0.0000363 + + + + 0 0 0.0 0 0 0 + + + 0.025 + 0.125 + + + + 0.3 + + + + 0 0 0.0625 0 0 0 + + + 0.025 + + + + + 0 0 -0.0625 0 0 0 + + + 0.025 + + + + + 0 0 0.0 0 0 0 + + + 0.025 + 0.125 + + + + 0 0 1 1 + + + + + + 0 0 0 0 1.5708 0 + 0.33 + + + 0.0004488 + 0 + 0 + 0.0004488 + 0 + 0.0000363 + + + + 0 0 0 0 1.5708 0 + + + 0.025 + 0.125 + + + + 0.3 + + + + 0.0625 0 0 0 0 0 + + + 0.025 + + + + + -0.0625 0 0 0 0 0 + + + 0.025 + + + + + 0 0 0 0 1.5708 0 + + + 0.025 + 0.125 + + + + 1 0 0 1 + + + + + + 0 0 0 1.5708 0 0 + 0.33 + + + 0.0004488 + 0 + 0 + 0.0004488 + 0 + 0.0000363 + + + + 0 0 0 1.5708 0 0 + + + 0.025 + 0.125 + + + + 0.3 + + + + 0 0.0625 0 0 0 0 + + + 0.025 + + + + + 0 -0.0625 0 0 0 0 + + + 0.025 + + + + + 0 0 0 1.5708 0 0 + + + 0.025 + 0.125 + + + + 0 1 0 1 + + + + + + + capsule_1 + capsule_2 + 0 0 0 0 0 0 + + + + capsule_2 + capsule_3 + 0 0 0 0 0 0 + + + + diff --git a/examples/sampling_c3/urdf/platform.urdf b/examples/sampling_c3/urdf/platform.urdf new file mode 100644 index 0000000000..9c700d5f9a --- /dev/null +++ b/examples/sampling_c3/urdf/platform.urdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/examples/sampling_c3/urdf/push_t.sdf b/examples/sampling_c3/urdf/push_t.sdf new file mode 100644 index 0000000000..bc3104bcf5 --- /dev/null +++ b/examples/sampling_c3/urdf/push_t.sdf @@ -0,0 +1,93 @@ + + + + + + 0 0 0 0 0 0 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + + -0.10 0 0 0 0 1.5708 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + vertical_link + horizontal_link + 0 0 0 0 0 0 + + + + \ No newline at end of file diff --git a/examples/sampling_c3/urdf/push_t_control.sdf b/examples/sampling_c3/urdf/push_t_control.sdf new file mode 100644 index 0000000000..2c017fd56e --- /dev/null +++ b/examples/sampling_c3/urdf/push_t_control.sdf @@ -0,0 +1,117 @@ + + + + + + 0 0 0 0 0 0 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + -0.12 0.08 -0.02 0 0 0 + + + 0.001 + + + + + -0.12 -0.08 -0.02 0 0 0 + + + 0.001 + + + + + 0.08 0.0 -0.02 0 0 0 + + + 0.001 + + + + + + + + -0.10 0 0 0 0 1.5708 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + vertical_link + horizontal_link + 0 0 0 0 0 0 + + + + \ No newline at end of file diff --git a/examples/sampling_c3/xbox_script.py b/examples/sampling_c3/xbox_script.py new file mode 100644 index 0000000000..3b1888c3c5 --- /dev/null +++ b/examples/sampling_c3/xbox_script.py @@ -0,0 +1,95 @@ +"""Teleoperation controls for sampling C3 controller using an xbox controller. + +Button presses: +- A: Toggle teleoperation mode +- X: Toggle force tracking mode +- Y: Toggle force C3 mode +- Start: Print cost breakdown +- Back: Print current rotation and position cost + +Mapping to radio channels: +- Channel 0: x-direction teleop +- Channel 1: y-direction teleop +- Channel 2: z-direction teleop +- Channel 6: Back button (print current rotation and position cost) +- Channel 7: Start button (print cost breakdown) +- Channel 11: X button (force tracking mode) +- Channel 12: Y button (force C3 mode) +- Channel 14: A button (teleoperation mode) +""" + +import pygame +import dairlib.lcmt_radio_out +import lcm +import numpy as np + +def main(): + publisher = lcm.LCM() + + pygame.init() + + # Used to manage how fast the screen updates + clock = pygame.time.Clock() + + if (pygame.joystick.get_count() != 1): + raise RuntimeError("Please connect exactly one controller") + + joystick = pygame.joystick.Joystick(0) + joystick.init() + + done = False + i = 0 + latching_switch_a = 1 + latching_switch_x = 0 + latching_switch_y = 0 + latching_switch_start = 0 + latching_switch_back = 0 + print("Teleop Status: " + str(latching_switch_a)) + print("Force Tracking Status: " + str(not latching_switch_x)) + + + while not done: + # Get the name from the OS for the controller/joystick + name = joystick.get_name() + + for event in pygame.event.get(): + if event.type == pygame.JOYBUTTONDOWN: + # use this print statement to find the button number + # print(f"Button {event.button} pressed") + if event.button == 0: + latching_switch_a = not latching_switch_a + print("Teleop Status: " + str(latching_switch_a)) + if event.button == 2: + latching_switch_x = not latching_switch_x + print("Force Tracking Status: " + str(not latching_switch_x)) + if event.button == 3: + latching_switch_y = not latching_switch_y + print("Force C3 Mode Status: " + str(latching_switch_y)) + if event.button == 7: + latching_switch_start = not latching_switch_start + print("Print cost breakdown status: " + str(latching_switch_start)) + if event.button == 6: + latching_switch_back = not latching_switch_back + print("Print current rot and pos cost status: " + str(latching_switch_back)) + + # Send LCM message + radio_msg = dairlib.lcmt_radio_out() + radio_msg.channel[0] = -joystick.get_axis(1) + radio_msg.channel[1] = -joystick.get_axis(0) + radio_msg.channel[2] = -joystick.get_axis(4) + radio_msg.channel[6] = latching_switch_back + radio_msg.channel[7] = latching_switch_start + radio_msg.channel[11] = latching_switch_x + radio_msg.channel[12] = latching_switch_y + radio_msg.channel[14] = latching_switch_a + + publisher.publish("SAMPLING_C3_RADIO", radio_msg.encode()) + + # Limit to 20 frames per second + clock.tick(60) + i += 1 + + pygame.quit() + +if __name__ == '__main__': + main() diff --git a/examples/trifinger/lcm_control_demo.py b/examples/trifinger/lcm_control_demo.py index 6564c38c9f..817c31799f 100644 --- a/examples/trifinger/lcm_control_demo.py +++ b/examples/trifinger/lcm_control_demo.py @@ -47,9 +47,9 @@ def CalcControl(self, context, output): parser = Parser(plant) parser.package_map().Add("robot_properties_fingers", "examples/trifinger/robot_properties_fingers") -parser.AddModelFromFile(pydairlib.common.FindResourceOrThrow( +parser.AddModels(pydairlib.common.FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/urdf/trifinger_minimal_collision.urdf")) -parser.AddModelFromFile(pydairlib.common.FindResourceOrThrow( +parser.AddModels(pydairlib.common.FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/cube/cube_v2.urdf")) # Fix the base of the finger to the world diff --git a/examples/trifinger/simulate_trifinger.py b/examples/trifinger/simulate_trifinger.py index d20ec2e4b4..0a08c51563 100644 --- a/examples/trifinger/simulate_trifinger.py +++ b/examples/trifinger/simulate_trifinger.py @@ -3,32 +3,34 @@ from pydairlib.multibody import (addFlatTerrain, makeNameToPositionsMap) import pydairlib.common + # A demo controller system class TrifingerDemoController(LeafSystem): - def __init__(self, plant): - LeafSystem.__init__(self) - - self.plant = plant - - # Input is state, output is torque (control action) - self.DeclareVectorInputPort("x", plant.num_positions() + - plant.num_velocities()) - self.DeclareVectorOutputPort("u", plant.num_actuators(), self.CalcControl) - - def CalcControl(self, context, output): - x = self.EvalVectorInput(context, 0).get_value() - # q and v are [fingers; cube] - # cube position is [quat; xyz] and velocity [ang_vel; xyz] - q = x[0:self.plant.num_positions()] - v = x[self.plant.num_positions():] - # u = -.03*np.ones(self.plant.num_actuators()) - - # use a simple PD controller with constant setpoint - kp = 10 - kd = 2 - q_des = np.array([.1, 0, -1, .1, -.5, -1, .1, -.5, -1]) - u = kp*(q_des - q[0:9]) - kd * v[0:9] - output.SetFromVector(u) + def __init__(self, plant): + LeafSystem.__init__(self) + + self.plant = plant + + # Input is state, output is torque (control action) + self.DeclareVectorInputPort("x", plant.num_positions() + + plant.num_velocities()) + self.DeclareVectorOutputPort("u", plant.num_actuators(), self.CalcControl) + + def CalcControl(self, context, output): + x = self.EvalVectorInput(context, 0).get_value() + # q and v are [fingers; cube] + # cube position is [quat; xyz] and velocity [ang_vel; xyz] + q = x[0:self.plant.num_positions()] + v = x[self.plant.num_positions():] + # u = -.03*np.ones(self.plant.num_actuators()) + + # use a simple PD controller with constant setpoint + kp = 10 + kd = 2 + q_des = np.array([.1, 0, -1, .1, -.5, -1, .1, -.5, -1]) + u = kp * (q_des - q[0:9]) - kd * v[0:9] + output.SetFromVector(u) + # Load the URDF and the cube builder = DiagramBuilder() @@ -41,9 +43,9 @@ def CalcControl(self, context, output): parser = Parser(plant) parser.package_map().Add("robot_properties_fingers", "examples/trifinger/robot_properties_fingers") -parser.AddModelFromFile(pydairlib.common.FindResourceOrThrow( +parser.AddModels(pydairlib.common.FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/urdf/trifinger_minimal_collision.urdf")) -parser.AddModelFromFile(pydairlib.common.FindResourceOrThrow( +parser.AddModels(pydairlib.common.FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/cube/cube_v2.urdf")) # Fix the base of the finger to the world @@ -73,7 +75,6 @@ def CalcControl(self, context, output): builder.Connect(controller.get_output_port(0), mux.get_input_port(1)) builder.Connect(mux.get_output_port(0), logger.get_input_port(0)) - diagram = builder.Build() simulator = Simulator(diagram) @@ -109,4 +110,4 @@ def CalcControl(self, context, output): simulator.AdvanceTo(3) # numpy array of data (nq+nv+nu) x n_time -data = logger.FindLog(simulator.get_context()).data() \ No newline at end of file +data = logger.FindLog(simulator.get_context()).data() diff --git a/examples/trifinger/simulate_trifinger_lcm.py b/examples/trifinger/simulate_trifinger_lcm.py index e3f6f7329d..faf1e5970c 100644 --- a/examples/trifinger/simulate_trifinger_lcm.py +++ b/examples/trifinger/simulate_trifinger_lcm.py @@ -17,9 +17,9 @@ parser = Parser(plant) parser.package_map().Add("robot_properties_fingers", "examples/trifinger/robot_properties_fingers") -parser.AddModelFromFile(pydairlib.common.FindResourceOrThrow( +parser.AddModels(pydairlib.common.FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/urdf/trifinger_minimal_collision.urdf")) -parser.AddModelFromFile(pydairlib.common.FindResourceOrThrow( +parser.AddModels(pydairlib.common.FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/cube/cube_v2.urdf")) # Fix the base of the finger to the world diff --git a/install/bionic/Dockerfile b/install/bionic/Dockerfile deleted file mode 100644 index 63ff911d81..0000000000 --- a/install/bionic/Dockerfile +++ /dev/null @@ -1,18 +0,0 @@ -FROM ros:melodic-ros-base-bionic -WORKDIR /dairlib -ENV DAIRLIB_DOCKER_VERSION_25=25 -COPY . . -RUN apt-get update && apt-get install -y wget lsb-release pkg-config zip g++ zlib1g-dev unzip python -RUN set -eux \ - && apt-get install --no-install-recommends locales \ - && locale-gen en_US.UTF-8 -RUN if type sudo 2>/dev/null; then \ - echo "The sudo command already exists... Skipping."; \ - else \ - echo -e "#!/bin/sh\n\${@}" > /usr/sbin/sudo; \ - chmod +x /usr/sbin/sudo; \ - fi -RUN set -eux \ - && export DEBIAN_FRONTEND=noninteractive \ - && yes | install/install_prereqs_bionic.sh \ - && rm -rf /var/lib/apt/lists/* diff --git a/install/bionic/ros/Dockerfile b/install/bionic/ros/Dockerfile deleted file mode 100644 index 6d7d1abfce..0000000000 --- a/install/bionic/ros/Dockerfile +++ /dev/null @@ -1,19 +0,0 @@ -FROM ros:melodic-ros-base-bionic -WORKDIR /dairlib -ENV DAIRLIB_DOCKER_VERSION_25=25 -COPY . . -RUN apt-get update && apt-get install -y wget lsb-release pkg-config zip g++ zlib1g-dev unzip python -RUN apt-get install -y python-rosinstall-generator python-catkin-tools -RUN set -eux \ - && apt-get install --no-install-recommends locales \ - && locale-gen en_US.UTF-8 -RUN if type sudo 2>/dev/null; then \ - echo "The sudo command already exists... Skipping."; \ - else \ - echo -e "#!/bin/sh\n\${@}" > /usr/sbin/sudo; \ - chmod +x /usr/sbin/sudo; \ - fi -RUN set -eux \ - && export DEBIAN_FRONTEND=noninteractive \ - && yes | install/install_prereqs_bionic.sh \ - && rm -rf /var/lib/apt/lists/* diff --git a/install/focal/Dockerfile b/install/focal/Dockerfile deleted file mode 100644 index 8033b6125f..0000000000 --- a/install/focal/Dockerfile +++ /dev/null @@ -1,18 +0,0 @@ -FROM ros:noetic-ros-base-focal -WORKDIR /dairlib -ENV DAIRLIB_DOCKER_VERSION_25=25 -COPY . . -RUN apt-get update && apt-get install -y wget lsb-release pkg-config zip g++ zlib1g-dev unzip python clang-12 clang-format-12 -RUN set -eux \ - && apt-get install --no-install-recommends locales \ - && locale-gen en_US.UTF-8 -RUN if type sudo 2>/dev/null; then \ - echo "The sudo command already exists... Skipping."; \ - else \ - echo -e "#!/bin/sh\n\${@}" > /usr/sbin/sudo; \ - chmod +x /usr/sbin/sudo; \ - fi -RUN set -eux \ - && export DEBIAN_FRONTEND=noninteractive \ - && yes | install/install_prereqs_focal.sh \ - && rm -rf /var/lib/apt/lists/* diff --git a/install/focal/ros/Dockerfile b/install/focal/ros/Dockerfile deleted file mode 100644 index 57d8f7a8eb..0000000000 --- a/install/focal/ros/Dockerfile +++ /dev/null @@ -1,20 +0,0 @@ -FROM ros:noetic-ros-base-focal -WORKDIR /dairlib -ENV DAIRLIB_DOCKER_VERSION_26=26 -ENV ROS_PYTHON_VERSION=3 -COPY . . -RUN apt-get update && apt-get install -y wget lsb-release pkg-config zip g++ zlib1g-dev unzip python -RUN apt-get install -y python3-rosinstall-generator python3-catkin-tools python3-vcstool python3-osrf-pycommon python3-vcstool python3-empy clang-12 clang-format-12 -RUN set -eux \ - && apt-get install --no-install-recommends locales \ - && locale-gen en_US.UTF-8 -RUN if type sudo 2>/dev/null; then \ - echo "The sudo command already exists... Skipping."; \ - else \ - echo -e "#!/bin/sh\n\${@}" > /usr/sbin/sudo; \ - chmod +x /usr/sbin/sudo; \ - fi -RUN set -eux \ - && export DEBIAN_FRONTEND=noninteractive \ - && yes | install/install_prereqs_focal.sh \ - && rm -rf /var/lib/apt/lists/* diff --git a/install/install_prereqs_bionic.sh b/install/install_prereqs_bionic.sh deleted file mode 100755 index 64d1cbdaf9..0000000000 --- a/install/install_prereqs_bionic.sh +++ /dev/null @@ -1,37 +0,0 @@ -#!/bin/bash - -# look in WORKSPACE file for appropriate commit number -DRAKE_COMMIT=$(grep -oP '(?<=DRAKE_COMMIT = ")(.*)(?=")' $(dirname "$0")/../WORKSPACE) - -ubuntu_codename=$(lsb_release -sc) - -# Add drake apt repository for lcm and libbot2 -wget -O - https://drake-apt.csail.mit.edu/drake.asc | apt-key add -echo "deb [arch=amd64] https://drake-apt.csail.mit.edu/bionic bionic main" > /etc/apt/sources.list.d/drake.list -apt-get update - -echo "Using Drake commit '${DRAKE_COMMIT}'" -# Download and run drake install scripts -mkdir tmp -cd tmp -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/install_prereqs.sh" -mkdir source_distribution -cd source_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/install_prereqs.sh" -# skipping drake's "install_prereqs_user_environment.sh" -touch install_prereqs_user_environment.sh -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}.txt" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}-clang.txt" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}-test-only.txt" -cd .. -mkdir binary_distribution -cd binary_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/binary_distribution/install_prereqs.sh" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/binary_distribution/packages-${ubuntu_codename}.txt" -cd .. -chmod +x install_prereqs.sh -./install_prereqs.sh -cd .. -rm -rf tmp/ -# In addition to drake, install lcm and libbot2 -apt install lcm libbot2 \ No newline at end of file diff --git a/install/install_prereqs_focal.sh b/install/install_prereqs_focal.sh deleted file mode 100755 index 04fc306c16..0000000000 --- a/install/install_prereqs_focal.sh +++ /dev/null @@ -1,37 +0,0 @@ -#!/bin/bash - -# look in WORKSPACE file for appropriate commit number -DRAKE_COMMIT=$(grep -oP '(?<=DRAKE_COMMIT = ")(.*)(?=")' $(dirname "$0")/../WORKSPACE) - -ubuntu_codename=$(lsb_release -sc) - -# Add drake apt repository for lcm and libbot2 -wget -O - https://drake-apt.csail.mit.edu/drake.asc | apt-key add -echo "deb [arch=amd64] https://drake-apt.csail.mit.edu/focal focal main" > /etc/apt/sources.list.d/drake.list -apt-get update - -echo "Using Drake commit '${DRAKE_COMMIT}'" -# Download and run drake install scripts -mkdir tmp -cd tmp -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/install_prereqs.sh" -mkdir source_distribution -cd source_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/install_prereqs.sh" -# skipping drake's "install_prereqs_user_environment.sh" -touch install_prereqs_user_environment.sh -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}.txt" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}-clang.txt" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}-test-only.txt" -cd .. -mkdir binary_distribution -cd binary_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/binary_distribution/install_prereqs.sh" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/binary_distribution/packages-${ubuntu_codename}.txt" -cd .. -chmod +x install_prereqs.sh -./install_prereqs.sh -cd .. -rm -rf tmp/ -# In addition to drake, install lcm and libbot2 -apt install lcm libbot2 \ No newline at end of file diff --git a/install/install_prereqs_jammy.sh b/install/install_prereqs_jammy.sh deleted file mode 100755 index 987fe41795..0000000000 --- a/install/install_prereqs_jammy.sh +++ /dev/null @@ -1,37 +0,0 @@ -#!/bin/bash - -# look in WORKSPACE file for appropriate commit number -DRAKE_COMMIT=$(grep -oP '(?<=DRAKE_COMMIT = ")(.*)(?=")' $(dirname "$0")/../WORKSPACE) - -ubuntu_codename=$(lsb_release -sc) - -# Add drake apt repository for lcm and libbot2 -wget -qO- https://drake-apt.csail.mit.edu/drake.asc | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/drake.gpg >/dev/null -echo "deb [arch=amd64] https://drake-apt.csail.mit.edu/${ubuntu_codename} ${ubuntu_codename} main" | sudo tee /etc/apt/sources.list.d/drake.list >/dev/null -apt-get update - -echo "Using Drake commit '${DRAKE_COMMIT}'" -# Download and run drake install scripts -mkdir tmp -cd tmp -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/install_prereqs.sh" -mkdir source_distribution -cd source_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/install_prereqs.sh" -# skipping drake's "install_prereqs_user_environment.sh" -touch install_prereqs_user_environment.sh -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}.txt" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}-clang.txt" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/source_distribution/packages-${ubuntu_codename}-test-only.txt" -cd .. -mkdir binary_distribution -cd binary_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/binary_distribution/install_prereqs.sh" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/ubuntu/binary_distribution/packages-${ubuntu_codename}.txt" -cd .. -chmod +x install_prereqs.sh -./install_prereqs.sh -cd .. -rm -rf tmp/ -# In addition to drake, install lcm and libbot2 -apt install lcm libbot2 \ No newline at end of file diff --git a/install/install_prereqs_mac.sh b/install/install_prereqs_mac.sh deleted file mode 100755 index b447a4a7c9..0000000000 --- a/install/install_prereqs_mac.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -# look in WORKSPACE file for appropriate commit number -DRAKE_COMMIT=$(grep -oP '(?<=DRAKE_COMMIT = ")(.*)(?=")' $(dirname "$0")/../WORKSPACE) - -ubuntu_codename=$(lsb_release -sc) - -echo "Using Drake commit '${DRAKE_COMMIT}'" -# Download and run drake install scripts -mkdir tmp -cd tmp -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/install_prereqs.sh" -mkdir source_distribution -cd source_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/source_distribution/install_prereqs.sh" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/source_distribution/install_prereqs_user_environment.sh" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/source_distribution/Brewfile" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/source_distribution/requirements.txt" -cd .. -mkdir binary_distribution -cd binary_distribution -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/binary_distribution/install_prereqs.sh" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/binary_distribution/Brewfile" -wget "https://raw.githubusercontent.com/RobotLocomotion/drake/${DRAKE_COMMIT}/setup/mac/binary_distribution/requirements.txt" -cd .. -chmod +x install_prereqs.sh -./install_prereqs.sh -cd .. -rm -rf tmp/ -# In addition to drake, install lcm and libbot2 -# TODO: the line below is not tested, need a mac user to see if it will work! -brew install lcm libbot2 \ No newline at end of file diff --git a/install/install_prereqs_ubuntu.sh b/install/install_prereqs_ubuntu.sh new file mode 100755 index 0000000000..40361c5daa --- /dev/null +++ b/install/install_prereqs_ubuntu.sh @@ -0,0 +1,21 @@ + #!/bin/bash + +# look in WORKSPACE file for appropriate commit number +DRAKE_VERSION=$(grep -oP 'DRAKE_VERSION = "v?\K[^"]*' $(dirname "$0")/../MODULE.bazel) +echo ${DRAKE_VERSION} + +ubuntu_codename=$(lsb_release -sc) + +# Add drake apt repository for lcm and libbot2 +wget -qO- https://drake-apt.csail.mit.edu/drake.asc | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/drake.gpg >/dev/null +echo "deb [arch=amd64] https://drake-apt.csail.mit.edu/${ubuntu_codename} ${ubuntu_codename} main" | sudo tee /etc/apt/sources.list.d/drake.list >/dev/null + + +wget https://github.com/RobotLocomotion/drake/archive/v${DRAKE_VERSION}.tar.gz +tar -xzf v${DRAKE_VERSION}.tar.gz drake-$DRAKE_VERSION/setup/ +./drake-${DRAKE_VERSION}/setup/install_prereqs --developer -y --with-bazel +rm -rf ./drake-${DRAKE_VERSION}/ +rm v${DRAKE_VERSION}.tar.gz + +# In addition to drake, install lcm and libbot2 +sudo apt install lcm libbot2 \ No newline at end of file diff --git a/install/jammy/Dockerfile b/install/jammy/Dockerfile deleted file mode 100644 index dd06a27c86..0000000000 --- a/install/jammy/Dockerfile +++ /dev/null @@ -1,19 +0,0 @@ -FROM ubuntu:jammy -WORKDIR ./dairlib -ENV DAIRLIB_DOCKER_VERSION_25=25 -COPY . . -RUN apt-get update && apt-get install -y wget lsb-release pkg-config zip g++ zlib1g-dev unzip clang-12 clang-format-12 ca-certificates gnupg - -RUN set -eux \ - && apt-get install --no-install-recommends locales \ - && locale-gen en_US.UTF-8 -RUN if type sudo 2>/dev/null; then \ - echo "The sudo command already exists... Skipping."; \ - else \ - echo -e "#!/bin/sh\n\${@}" > /usr/sbin/sudo; \ - chmod +x /usr/sbin/sudo; \ - fi -RUN set -eux \ - && export DEBIAN_FRONTEND=noninteractive \ - && yes | install/install_prereqs_jammy.sh \ - && rm -rf /var/lib/apt/lists/* diff --git a/lcm/dircon_saved_trajectory.cc b/lcm/dircon_saved_trajectory.cc index e408f39458..d046c5bcd9 100644 --- a/lcm/dircon_saved_trajectory.cc +++ b/lcm/dircon_saved_trajectory.cc @@ -34,7 +34,6 @@ DirconTrajectory::DirconTrajectory( LcmTrajectory::Trajectory state_traj; LcmTrajectory::Trajectory state_derivative_traj; LcmTrajectory::Trajectory force_traj; - LcmTrajectory::Trajectory impulse_traj; state_traj.traj_name = "state_traj" + std::to_string(mode); state_traj.datapoints = x[mode]; @@ -66,15 +65,6 @@ DirconTrajectory::DirconTrajectory( Eigen::Map(dircon.GetForceSamplesByMode(result, mode).data(), num_forces, force_traj.time_vector.size()); - // Impulse vars - if (mode > 0) { - impulse_traj.traj_name = "impulse_vars" + std::to_string(mode); - impulse_traj.datatypes = impulse_names; - // Get start of mode to get time of impulse - impulse_traj.time_vector = state_breaks[mode].segment(0, 1); - impulse_traj.datapoints = result.GetSolution(dircon.impulse_vars(mode)); - } - // Collocation force vars if (state_breaks[mode].size() > 1) { LcmTrajectory::Trajectory collocation_force_traj; @@ -114,12 +104,29 @@ DirconTrajectory::DirconTrajectory( AddTrajectory(state_traj.traj_name, state_traj); AddTrajectory(state_derivative_traj.traj_name, state_derivative_traj); AddTrajectory(force_traj.traj_name, force_traj); - AddTrajectory(impulse_traj.traj_name, impulse_traj); x_.push_back(&state_traj); xdot_.push_back(&state_derivative_traj); lambda_.push_back(&force_traj); - impulse_.push_back(&impulse_traj); + // Impulse vars + LcmTrajectory::Trajectory impulse_traj; + if (mode > 0) { + impulse_traj.traj_name = "impulse_vars" + std::to_string(mode); + // Check to make sure an impact occurs + if (dircon.impulse_vars(mode - 1).size() == 0){ + impulse_traj.datatypes = impulse_names; + // Get start of mode to get time of impulse + impulse_traj.time_vector = state_breaks[mode].segment(0, 1); + impulse_traj.datapoints = VectorXd::Zero(impulse_names.size()); + }else{ + impulse_traj.datatypes = impulse_names; + // Get start of mode to get time of impulse + impulse_traj.time_vector = state_breaks[mode].segment(0, 1); + impulse_traj.datapoints = result.GetSolution(dircon.impulse_vars(mode - 1)); + } + AddTrajectory(impulse_traj.traj_name, impulse_traj); + impulse_.push_back(&impulse_traj); + } } // Input trajectory @@ -290,8 +297,9 @@ const { PiecewisePolynomial DirconTrajectory::ReconstructStateTrajectoryWithSprings( - Eigen::MatrixXd& spr_to_wo_spr_map) const { + Eigen::MatrixXd& wo_spr_to_spr_map) const { PiecewisePolynomial state_traj; + DRAKE_DEMAND(wo_spr_to_spr_map.cols() == x_[0]->datapoints.rows()); for (int mode = 0; mode < num_modes_; ++mode) { // Cannot form trajectory with only a single break @@ -299,8 +307,8 @@ DirconTrajectory::ReconstructStateTrajectoryWithSprings( continue; } state_traj.ConcatenateInTime(PiecewisePolynomial::CubicHermite( - x_[mode]->time_vector, spr_to_wo_spr_map * x_[mode]->datapoints, - spr_to_wo_spr_map * xdot_[mode]->datapoints)); + x_[mode]->time_vector, wo_spr_to_spr_map * state_map_ * x_[mode]->datapoints, + wo_spr_to_spr_map * state_map_ * xdot_[mode]->datapoints)); } return state_traj; } diff --git a/lcm/dircon_saved_trajectory.h b/lcm/dircon_saved_trajectory.h index aeab7bd59c..0faf4818ef 100644 --- a/lcm/dircon_saved_trajectory.h +++ b/lcm/dircon_saved_trajectory.h @@ -87,36 +87,36 @@ class DirconTrajectory : public LcmTrajectory { Eigen::MatrixXd GetStateSamples(int mode) const { DRAKE_DEMAND(mode >= 0); DRAKE_DEMAND(mode < num_modes_); - return state_map_ * x_[mode]->datapoints; + return state_map_ * x_.at(mode)->datapoints; } Eigen::MatrixXd GetStateDerivativeSamples(int mode) const { DRAKE_DEMAND(mode >= 0); DRAKE_DEMAND(mode < num_modes_); - return state_map_ * xdot_[mode]->datapoints; + return state_map_ * xdot_.at(mode)->datapoints; } Eigen::MatrixXd GetStateBreaks(int mode) const { DRAKE_DEMAND(mode >= 0); DRAKE_DEMAND(mode < num_modes_); - return x_[mode]->time_vector; + return x_.at(mode)->time_vector; } Eigen::MatrixXd GetInputSamples() const { return actuator_map_ * u_->datapoints; } Eigen::MatrixXd GetBreaks() const { return u_->time_vector; } Eigen::MatrixXd GetForceSamples(int mode) const { - return lambda_[mode]->datapoints; + return lambda_.at(mode)->datapoints; } Eigen::MatrixXd GetForceBreaks(int mode) const { - return lambda_[mode]->time_vector; + return lambda_.at(mode)->time_vector; } Eigen::MatrixXd GetImpulseSamples(int mode) const { - return impulse_[mode - 1]->datapoints; + return impulse_.at(mode - 1)->datapoints; } Eigen::MatrixXd GetCollocationForceSamples(int mode) const { - return lambda_c_[mode]->datapoints; + return lambda_c_.at(mode)->datapoints; } Eigen::MatrixXd GetCollocationForceBreaks(int mode) const { - return lambda_c_[mode]->time_vector; + return lambda_c_.at(mode)->time_vector; } Eigen::VectorXd GetDecisionVariables() const { return decision_vars_->datapoints; diff --git a/lcm/lcm_trajectory.cc b/lcm/lcm_trajectory.cc index dec3b50dd7..f8cf7fb806 100644 --- a/lcm/lcm_trajectory.cc +++ b/lcm/lcm_trajectory.cc @@ -38,7 +38,7 @@ std::string exec(const char* cmd) { return result; } -LcmTrajectory::Trajectory::Trajectory(string traj_name, +LcmTrajectory::Trajectory::Trajectory(const string& traj_name, const lcmt_trajectory_block& traj_block) { int num_points = traj_block.num_points; int num_datatypes = traj_block.num_datatypes; @@ -47,6 +47,7 @@ LcmTrajectory::Trajectory::Trajectory(string traj_name, this->time_vector = VectorXd::Map(traj_block.time_vec.data(), num_points); this->datapoints = MatrixXd(num_datatypes, num_points); + DRAKE_ASSERT(traj_block.datatypes.size() == traj_block.datapoints.size()); // Convert vector> to EigenMatrix for (int i = 0; i < num_datatypes; ++i) { this->datapoints.row(i) = @@ -63,6 +64,7 @@ LcmTrajectory::LcmTrajectory(const vector& trajectories, for (const string& traj_name : trajectory_names_) { trajectories_[traj_name] = trajectories[index++]; } + metadata_.git_dirty_flag = false; if (get_metadata) { std::cout << "NOTE: Using subprocesses to get LcmTrajectory metadata\n"; ConstructMetadataObject(name, description); @@ -183,6 +185,13 @@ void LcmTrajectory::AddTrajectory(const std::string& trajectory_name, trajectories_[trajectory_name] = trajectory; } +void LcmTrajectory::ClearTrajectories() { + if (this->trajectories_.size() > 0) { + trajectories_.clear(); + trajectory_names_.clear(); + } +} + void LcmTrajectory::ConstructMetadataObject(string name, string description) { std::time_t t = std::time(nullptr); // get time now diff --git a/lcm/lcm_trajectory.h b/lcm/lcm_trajectory.h index 69d955b326..538eaecc5d 100644 --- a/lcm/lcm_trajectory.h +++ b/lcm/lcm_trajectory.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -26,7 +27,8 @@ class LcmTrajectory { /// lcmt_trajectory_block is the lcmtype analog struct Trajectory { Trajectory() = default; - Trajectory(std::string traj_name, const lcmt_trajectory_block& traj_block); + Trajectory(const std::string& traj_name, + const lcmt_trajectory_block& traj_block); std::string traj_name; Eigen::VectorXd time_vector; @@ -39,9 +41,8 @@ class LcmTrajectory { LcmTrajectory() = default; LcmTrajectory(const std::vector& trajectories, const std::vector& trajectory_names, - const std::string& name, - const std::string& description, - bool get_metadata=true); + const std::string& name, const std::string& description, + bool get_metadata = false); explicit LcmTrajectory(const lcmt_saved_traj& traj); @@ -64,20 +65,33 @@ class LcmTrajectory { lcmt_metadata GetMetadata() const { return metadata_; } + bool HasTrajectory(const std::string& trajectory_name) const { + return trajectories_.count(trajectory_name) > 0; + } + const Trajectory& GetTrajectory(const std::string& trajectory_name) const { - return trajectories_.at(trajectory_name); + try { + return trajectories_.at(trajectory_name); + } catch (std::exception& e) { + std::cerr << "Trajectory: " << trajectory_name << " does not exist." + << std::endl; + throw std::out_of_range(""); + } } /// Add additional LcmTrajectory::Trajectory objects void AddTrajectory(const std::string& trajectory_name, const Trajectory& trajectory); + /// Remove all trajectories + void ClearTrajectories(); + /// Returns a vector of the names of the stored Trajectory objects const std::vector& GetTrajectoryNames() const { return trajectory_names_; } - lcmt_saved_traj GenerateLcmObject() const; + protected: /// Constructs a lcmt_metadata object with a specified name and description /// Other relevant metadata details such as datatime and git status are @@ -85,7 +99,6 @@ class LcmTrajectory { void ConstructMetadataObject(std::string name, std::string description); private: - lcmt_metadata metadata_; std::unordered_map trajectories_; std::vector trajectory_names_; diff --git a/lcm/test/lcm_trajectory_test.cc b/lcm/test/lcm_trajectory_test.cc index 95466c20ba..f6a91e4db7 100644 --- a/lcm/test/lcm_trajectory_test.cc +++ b/lcm/test/lcm_trajectory_test.cc @@ -70,7 +70,7 @@ class LcmTrajectoryTest : public ::testing::Test { trajectories_[1] = traj_2_; lcm_traj_ = LcmTrajectory(trajectories_, trajectory_names_, TEST_NAME, - TEST_DESCRIPTION); + TEST_DESCRIPTION, true); } LcmTrajectory::Trajectory traj_1_; diff --git a/lcmtypes/BUILD.bazel b/lcmtypes/BUILD.bazel index 3297db6fc0..528d97693c 100644 --- a/lcmtypes/BUILD.bazel +++ b/lcmtypes/BUILD.bazel @@ -1,8 +1,8 @@ load( - "@drake//tools/skylark:drake_lcm.bzl", - "drake_lcm_cc_library", - "drake_lcm_java_library", - "drake_lcm_py_library", + "@drake//tools/workspace/lcm:lcm.bzl", + "lcm_cc_library", + "lcm_java_library", + "lcm_py_library", ) load( "@drake//tools/skylark:drake_java.bzl", @@ -15,45 +15,39 @@ package(default_visibility = ["//visibility:public"]) #Lcm libraries #Lcm libraries -drake_lcm_cc_library( +lcm_cc_library( name = "lcmt_robot", lcm_package = "dairlib", lcm_srcs = glob(["*.lcm"]), ) #Lcm libraries -drake_lcm_cc_library( +lcm_cc_library( name = "lcmtypes_robot_archive", lcm_package = "archive.dairlib", lcm_srcs = glob(["archive/*.lcm"]), ) -drake_lcm_java_library( +lcm_java_library( name = "lcmtypes_robot_java", lcm_package = "dairlib", lcm_srcs = glob(["*.lcm"]), ) -drake_lcm_py_library( +lcm_py_library( name = "lcmtypes_robot_py", add_current_package_to_imports = True, # Use //:module_py instead. extra_srcs = ["__init__.py"], lcm_package = "dairlib", lcm_srcs = glob(["*.lcm"]), - deps = [ - "@drake//:module_py", - ], ) -drake_lcm_py_library( +lcm_py_library( name = "lcmtypes_robot_archive_py", add_current_package_to_imports = True, # Use //:module_py instead. extra_srcs = ["__init__.py"], lcm_package = "archive.dairlib", lcm_srcs = glob(["archive/*.lcm"]), - deps = [ - "@drake//:module_py", - ], ) drake_java_binary( @@ -63,6 +57,5 @@ drake_java_binary( runtime_deps = [ ":lcmtypes_robot_java", "@drake//lcmtypes:lcmtypes_drake_java", - "@optitrack_driver//lcmtypes:lcmtypes_optitrack", ], ) diff --git a/lcmtypes/lcmt_c3_forces.lcm b/lcmtypes/lcmt_c3_forces.lcm new file mode 100644 index 0000000000..f9122bd72a --- /dev/null +++ b/lcmtypes/lcmt_c3_forces.lcm @@ -0,0 +1,11 @@ +package dairlib; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_c3_forces +{ + int64_t utime; + + int32_t num_forces; + lcmt_force forces[num_forces]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_intermediates.lcm b/lcmtypes/lcmt_c3_intermediates.lcm new file mode 100644 index 0000000000..c709decfb9 --- /dev/null +++ b/lcmtypes/lcmt_c3_intermediates.lcm @@ -0,0 +1,12 @@ +package dairlib; + +struct lcmt_c3_intermediates +{ + int32_t num_points; + int32_t num_total_variables; + + float time_vec[num_points]; + float z_sol[num_total_variables][num_points]; + float delta_sol[num_total_variables][num_points]; + float w_sol[num_total_variables][num_points]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_output.lcm b/lcmtypes/lcmt_c3_output.lcm new file mode 100644 index 0000000000..b82b674801 --- /dev/null +++ b/lcmtypes/lcmt_c3_output.lcm @@ -0,0 +1,9 @@ +package dairlib; + +struct lcmt_c3_output +{ + int64_t utime; + + lcmt_c3_solution c3_solution; + lcmt_c3_intermediates c3_intermediates; +} diff --git a/lcmtypes/lcmt_c3_solution.lcm b/lcmtypes/lcmt_c3_solution.lcm new file mode 100644 index 0000000000..9648477032 --- /dev/null +++ b/lcmtypes/lcmt_c3_solution.lcm @@ -0,0 +1,14 @@ +package dairlib; + +struct lcmt_c3_solution +{ + int32_t num_points; + int32_t num_state_variables; + int32_t num_contact_variables; + int32_t num_input_variables; + + float time_vec[num_points]; + float x_sol[num_state_variables][num_points]; + float lambda_sol[num_contact_variables][num_points]; + float u_sol[num_input_variables][num_points]; +} diff --git a/lcmtypes/lcmt_c3_state.lcm b/lcmtypes/lcmt_c3_state.lcm new file mode 100644 index 0000000000..1d1f4a2101 --- /dev/null +++ b/lcmtypes/lcmt_c3_state.lcm @@ -0,0 +1,10 @@ +package dairlib; + +struct lcmt_c3_state +{ + int64_t utime; + int32_t num_states; + + float state [num_states]; + string state_names [num_states]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_force.lcm b/lcmtypes/lcmt_force.lcm new file mode 100644 index 0000000000..935c9635aa --- /dev/null +++ b/lcmtypes/lcmt_force.lcm @@ -0,0 +1,12 @@ +package dairlib; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_force +{ + int64_t utime; + + // These are all expressed in the world frame. + double contact_point[3]; + double contact_force[3]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_franka_state.lcm b/lcmtypes/lcmt_franka_state.lcm new file mode 100644 index 0000000000..2ef1575979 --- /dev/null +++ b/lcmtypes/lcmt_franka_state.lcm @@ -0,0 +1,55 @@ +package dairlib; + +// mimics structure of FrankaState ROS message type +// https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html +struct lcmt_franka_state +{ + double O_T_EE[16]; + double O_T_EE_d[16]; + double F_T_EE[16]; + double F_T_NE[16]; + double NE_T_EE[16]; + double EE_T_K[16]; + double m_ee; + double I_ee[9]; + double F_x_Cee[3]; + double m_load; + double I_load[9]; + double F_x_Cload[3]; + double m_total; + double I_total[9]; + double F_x_Ctotal[3]; + double elbow[2]; + double elbow_d[2]; + double elbow_c[2]; + double delbow_c[2]; + double ddelbow_c[2]; + double tau_J[7]; + double tau_J_d[7]; + double dtau_J[7]; + double q[7]; + double q_d[7]; + double dq[7]; + double dq_d[7]; + double ddq_d[7]; + double joint_contact[7]; + double cartesian_contact[6]; + double joint_collision[7]; + double cartesian_collision[6]; + double tau_ext_hat_filtered[7]; + double O_F_ext_hat_K[6]; + double K_F_ext_hat_K[6]; + double O_dP_EE_d[16]; + double O_T_EE_c[16]; + double O_dP_EE_c[6]; + double O_ddP_EE_c[6]; + double theta[7]; + double dtheta[7]; + string current_errors; + string last_motion_errors; + double control_command_success_rate; + int16_t robot_mode; + double franka_time; + double utime; + boolean valid; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_object_state.lcm b/lcmtypes/lcmt_object_state.lcm new file mode 100644 index 0000000000..aa5d1cffa2 --- /dev/null +++ b/lcmtypes/lcmt_object_state.lcm @@ -0,0 +1,16 @@ +package dairlib; + +struct lcmt_object_state +{ + int64_t utime; + string object_name; + + int32_t num_positions; + int32_t num_velocities; + + string position_names [num_positions]; + double position [num_positions]; + + string velocity_names [num_velocities]; + double velocity [num_velocities]; +} diff --git a/lcmtypes/lcmt_osc_qp_output.lcm b/lcmtypes/lcmt_osc_qp_output.lcm index 0f2163b1c3..ae7bacd0fb 100644 --- a/lcmtypes/lcmt_osc_qp_output.lcm +++ b/lcmtypes/lcmt_osc_qp_output.lcm @@ -5,12 +5,14 @@ struct lcmt_osc_qp_output int32_t u_dim; int32_t lambda_c_dim; int32_t lambda_h_dim; + int32_t lambda_e_dim; int32_t v_dim; int32_t epsilon_dim; double solve_time; double u_sol[u_dim]; double lambda_c_sol[lambda_c_dim]; double lambda_h_sol[lambda_h_dim]; + double lambda_e_sol[lambda_e_dim]; double dv_sol[v_dim]; double epsilon_sol[epsilon_dim]; } diff --git a/lcmtypes/lcmt_qp.lcm b/lcmtypes/lcmt_qp.lcm new file mode 100644 index 0000000000..3b59298f5c --- /dev/null +++ b/lcmtypes/lcmt_qp.lcm @@ -0,0 +1,17 @@ +package dairlib; + +struct lcmt_qp +{ + int32_t n_x; + int32_t n_ineq; + int32_t n_eq; + double Q[n_x][n_x]; + double w[n_x]; + double A_ineq[n_ineq][n_x]; + double ineq_lb[n_ineq]; + double ineq_ub[n_ineq]; + double A_eq[n_eq][n_x]; + double b_eq[n_eq]; + double x_lb[n_x]; + double x_ub[n_x]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_sample_buffer.lcm b/lcmtypes/lcmt_sample_buffer.lcm new file mode 100644 index 0000000000..d044b9a569 --- /dev/null +++ b/lcmtypes/lcmt_sample_buffer.lcm @@ -0,0 +1,13 @@ +package dairlib; + +struct lcmt_sample_buffer +{ + int64_t utime; + int32_t buffer_length; + int32_t num_configurations; + + int32_t num_in_buffer; + + float costs[buffer_length]; + float configurations[buffer_length][num_configurations]; +} diff --git a/lcmtypes/lcmt_sampling_c3_debug.lcm b/lcmtypes/lcmt_sampling_c3_debug.lcm new file mode 100644 index 0000000000..25c6d70bf8 --- /dev/null +++ b/lcmtypes/lcmt_sampling_c3_debug.lcm @@ -0,0 +1,46 @@ +package dairlib; + +/* lcmtype with debug information for the sampling controller. + + mode_switch_reason key: + 0: No mode switch + 1: Switch to C3 because lower cost + 2: Switch to C3 because reached repositioning target + 3: Switch to repositioning because found lower cost sample + 4: Switch to repositioning because unproductive in C3 + 5: Switch to C3 because xbox controller forced into C3 mode + + source_of_pursued_target key: + 0: No pursued target (i.e. in C3 mode) + 1: Previous repositioning target + 2: New sample + 3: Sample from buffer +*/ + +struct lcmt_sampling_c3_debug +{ + int64_t utime; + + boolean is_c3_mode; + + boolean is_teleop; + boolean is_force_tracking; + boolean is_forced_into_c3; + + boolean in_pose_tracking_mode; + + int32_t mode_switch_reason; + int32_t source_of_pursued_target; + + int32_t detected_goal_changes; + + int32_t best_progress_steps_ago; + + float lowest_cost; + float lowest_pos_and_rot_current_cost; + float lowest_position_error; + float lowest_orientation_error; + + float current_pos_error; + float current_rot_error; +} diff --git a/lcmtypes/lcmt_timestamped_saved_traj.lcm b/lcmtypes/lcmt_timestamped_saved_traj.lcm new file mode 100644 index 0000000000..7a49bd7c2b --- /dev/null +++ b/lcmtypes/lcmt_timestamped_saved_traj.lcm @@ -0,0 +1,10 @@ +package dairlib; + +/* lcmtype analog for LcmTrajectory WITH TIMESTAMP to enable using a + LcmSerializer to save/load trajectories +*/ + +struct lcmt_timestamped_saved_traj { + int64_t utime; + lcmt_saved_traj saved_traj; +} diff --git a/multibody/BUILD.bazel b/multibody/BUILD.bazel index 0d76a6d617..2257aa9060 100644 --- a/multibody/BUILD.bazel +++ b/multibody/BUILD.bazel @@ -100,7 +100,7 @@ cc_test( cc_library( name = "ball_urdf", - data = glob(["multibody/ball.urdf"]), + data = glob(["ball.urdf"]), ) cc_library( @@ -123,3 +123,16 @@ cc_binary( "@drake//:drake_shared_library", ], ) + +cc_library( + name = "hydroelastic_geom_collider", + srcs = ["hydroelastic_geom_collider.cc"], + hdrs = [ + "hydroelastic_geom_collider.h", + ], + deps = [ + ":utils", + "//common", + "@drake//:drake_shared_library", + ], +) diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index 60d79fe659..e34bdfbed6 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -1,17 +1,17 @@ #include "multibody/geom_geom_collider.h" -#include "multibody/geom_geom_collider.h" #include "drake/math/rotation_matrix.h" - -using Eigen::Vector3d; -using Eigen::Matrix; +#include using drake::EigenPtr; using drake::MatrixX; +using drake::VectorX; using drake::geometry::GeometryId; using drake::geometry::SignedDistancePair; using drake::multibody::JacobianWrtVariable; using drake::multibody::MultibodyPlant; using drake::systems::Context; +using Eigen::Matrix; +using Eigen::Vector3d; namespace dairlib { namespace multibody { @@ -22,8 +22,7 @@ GeomGeomCollider::GeomGeomCollider( const drake::SortedPair geometry_pair) : plant_(plant), geometry_id_A_(geometry_pair.first()), - geometry_id_B_(geometry_pair.second()) { -} + geometry_id_B_(geometry_pair.second()) {} template std::pair> GeomGeomCollider::Eval(const Context& context, @@ -35,22 +34,21 @@ template std::pair> GeomGeomCollider::EvalPolytope( const Context& context, int num_friction_directions, JacobianWrtVariable wrt) { - if (num_friction_directions == 1) { throw std::runtime_error( - "GeomGeomCollider cannot specificy 1 friction direction unless " - "using EvalPlanar."); + "GeomGeomCollider cannot specify 1 friction direction unless " + "using EvalPlanar."); } // Build friction basis - Matrix force_basis( - 2 * num_friction_directions + 1, 3); + Matrix force_basis(2 * num_friction_directions + 1, + 3); force_basis.row(0) << 1, 0, 0; for (int i = 0; i < num_friction_directions; i++) { double theta = (M_PI * i) / num_friction_directions; - force_basis.row(2*i + 1) = Vector3d(0, cos(theta), sin(theta)); - force_basis.row(2*i + 2) = -force_basis.row(2*i + 1); + force_basis.row(2 * i + 1) = Vector3d(0, cos(theta), sin(theta)); + force_basis.row(2 * i + 2) = -force_basis.row(2 * i + 1); } return DoEval(context, force_basis, wrt); } @@ -62,12 +60,41 @@ std::pair> GeomGeomCollider::EvalPlanar( return DoEval(context, planar_normal.transpose(), wrt, true); } - // } +template +std::pair, VectorX> +GeomGeomCollider::CalcWitnessPoints(const Context& context) { + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + + const SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints(geometry_id_A_, + geometry_id_B_); + const auto& inspector = query_object.inspector(); + const auto frame_A_id = inspector.GetFrameId(geometry_id_A_); + const auto frame_B_id = inspector.GetFrameId(geometry_id_B_); + const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame(); + const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame(); + + const Vector3d& p_ACa = + inspector.GetPoseInFrame(geometry_id_A_).template cast() * + signed_distance_pair.p_ACa; + const Vector3d& p_BCb = + inspector.GetPoseInFrame(geometry_id_B_).template cast() * + signed_distance_pair.p_BCb; + Vector3d p_WCa = Vector3d::Zero(); + Vector3d p_WCb = Vector3d::Zero(); + plant_.CalcPointsPositions(context, frameA, p_ACa, plant_.world_frame(), + &p_WCa); + plant_.CalcPointsPositions(context, frameB, p_BCb, plant_.world_frame(), + &p_WCb); + return std::pair, VectorX>(p_WCa, p_WCb); +} + template std::pair> GeomGeomCollider::DoEval( const Context& context, Matrix force_basis, JacobianWrtVariable wrt, bool planar) { - const auto& query_port = plant_.get_geometry_query_input_port(); const auto& query_object = query_port.template Eval>(context); @@ -84,27 +111,25 @@ std::pair> GeomGeomCollider::DoEval( const Vector3d& p_ACa = inspector.GetPoseInFrame(geometry_id_A_).template cast() * - signed_distance_pair.p_ACa; + signed_distance_pair.p_ACa; const Vector3d& p_BCb = inspector.GetPoseInFrame(geometry_id_B_).template cast() * - signed_distance_pair.p_BCb; - + signed_distance_pair.p_BCb; - int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() : - plant_.num_positions(); + int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() + : plant_.num_positions(); Matrix Jv_WCa(3, n_cols); Matrix Jv_WCb(3, n_cols); - plant_.CalcJacobianTranslationalVelocity(context, wrt, - frameA, p_ACa, plant_.world_frame(), - plant_.world_frame(), &Jv_WCa); - plant_.CalcJacobianTranslationalVelocity(context, wrt, - frameB, p_BCb, plant_.world_frame(), - plant_.world_frame(), &Jv_WCb); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameA, p_ACa, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCa); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameB, p_BCb, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCb); - const auto& R_WC = - drake::math::RotationMatrix::MakeFromOneVector( - signed_distance_pair.nhat_BA_W, 0); + auto R_WC = drake::math::RotationMatrix::MakeFromOneVector( + signed_distance_pair.nhat_BA_W, 0); // if this is a planar problem, then the basis has one row and encodes // the planar normal direction. @@ -113,10 +138,11 @@ std::pair> GeomGeomCollider::DoEval( // thus the somewhat awkward calculations here. if (planar) { Vector3d planar_normal = force_basis.row(0); + force_basis = Eigen::MatrixXd::Zero(3, 3); force_basis.resize(3, 3); - // First row is the contact normal, projected to the plane - force_basis.row(0) = signed_distance_pair.nhat_BA_W - + force_basis.row(0) = + signed_distance_pair.nhat_BA_W - planar_normal * planar_normal.dot(signed_distance_pair.nhat_BA_W); force_basis.row(0).normalize(); @@ -124,8 +150,9 @@ std::pair> GeomGeomCollider::DoEval( force_basis.row(1) = signed_distance_pair.nhat_BA_W.cross(planar_normal); force_basis.row(1).normalize(); force_basis.row(2) = -force_basis.row(1); + R_WC = drake::math::RotationMatrix::Identity(); } - // Standard case + // Standard case auto J = force_basis * R_WC.matrix().transpose() * (Jv_WCa - Jv_WCb); return std::pair>(signed_distance_pair.distance, J); } @@ -133,5 +160,4 @@ std::pair> GeomGeomCollider::DoEval( } // namespace multibody } // namespace dairlib - template class dairlib::multibody::GeomGeomCollider; \ No newline at end of file diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index 5e32b95290..9d3b107f5a 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -31,7 +31,6 @@ class GeomGeomCollider { drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); - /// Calculates the distance and contact frame Jacobian. /// Jacobian is ordered [J_n; J_t], and has shape //// (2*num_friction_directions + 1) x (nq or nv), depending @@ -48,12 +47,10 @@ class GeomGeomCollider { /// @param num_friction_directions /// @return A pair with std::pair> EvalPolytope( - const drake::systems::Context& context, - int num_friction_directions, + const drake::systems::Context& context, int num_friction_directions, drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); - /// Calculates the distance and contact frame Jacobian for a 2D planar problem /// Jacobian is ordered [J_n; +J_t; -J_t], and has shape 3 x (nq). /// J_t refers to the (contact_normal x planar_normal) direction @@ -66,6 +63,9 @@ class GeomGeomCollider { drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); + std::pair, drake::VectorX> CalcWitnessPoints( + const drake::systems::Context& context); + private: std::pair> DoEval( const drake::systems::Context& context, diff --git a/multibody/hydroelastic_geom_collider.cc b/multibody/hydroelastic_geom_collider.cc new file mode 100644 index 0000000000..62b6e8360e --- /dev/null +++ b/multibody/hydroelastic_geom_collider.cc @@ -0,0 +1,246 @@ +#include "multibody/hydroelastic_geom_collider.h" + +#include + +#include "drake/math/rotation_matrix.h" + +using drake::EigenPtr; +using drake::MatrixX; +using drake::VectorX; +using drake::geometry::GeometryId; +using drake::geometry::HydroelasticContactRepresentation; +using drake::geometry::SignedDistancePair; +using drake::multibody::JacobianWrtVariable; +using drake::multibody::MultibodyPlant; +using drake::systems::Context; +using Eigen::Matrix; +using Eigen::Vector3d; +using Eigen::VectorXd; + +namespace dairlib { +namespace multibody { + +template +HydroelasticGeomCollider::HydroelasticGeomCollider( + const drake::multibody::MultibodyPlant& plant, + const std::vector>& + contact_geoms) + : plant_(plant), contact_pairs_(contact_geoms) {} + +template +std::vector>> HydroelasticGeomCollider::Eval( + const Context& context, JacobianWrtVariable wrt) { + return DoEval(context, Eigen::Matrix3d::Identity(), wrt); +} + +template +std::vector>> HydroelasticGeomCollider::EvalPolytope( + const Context& context, int num_friction_directions, + JacobianWrtVariable wrt) { + if (num_friction_directions == 1) { + throw std::runtime_error( + "HydroelasticGeomCollider cannot specify 1 friction direction unless " + "using EvalPlanar."); + } + + // Build friction basis + Matrix force_basis(2 * num_friction_directions + 1, + 3); + force_basis.row(0) << 1, 0, 0; + + for (int i = 0; i < num_friction_directions; i++) { + double theta = (M_PI * i) / num_friction_directions; + force_basis.row(2 * i + 1) = Vector3d(0, cos(theta), sin(theta)); + force_basis.row(2 * i + 2) = -force_basis.row(2 * i + 1); + } + return DoEval(context, force_basis, wrt); +} + +template +std::vector>> HydroelasticGeomCollider::EvalPlanar( + const Context& context, const Vector3d& planar_normal, + JacobianWrtVariable wrt) { + return DoEval(context, planar_normal.transpose(), wrt, true); +} + +template +std::vector> HydroelasticGeomCollider::CalcWitnessPoints( + const Context& context) { + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + + auto geom_ids = contact_pairs_.front(); + auto geometry_id_A_ = geom_ids.first(); + auto geometry_id_B_ = geom_ids.second(); + + const SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints(geometry_id_A_, + geometry_id_B_); + const auto& inspector = query_object.inspector(); + const auto frame_A_id = inspector.GetFrameId(geometry_id_A_); + const auto frame_B_id = inspector.GetFrameId(geometry_id_B_); + const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame(); + const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame(); + + const Vector3d& p_ACa = + inspector.GetPoseInFrame(geometry_id_A_).template cast() * + signed_distance_pair.p_ACa; + const Vector3d& p_BCb = + inspector.GetPoseInFrame(geometry_id_B_).template cast() * + signed_distance_pair.p_BCb; + Vector3d p_WCa = Vector3d::Zero(); + Vector3d p_WCb = Vector3d::Zero(); + plant_.CalcPointsPositions(context, frameA, p_ACa, plant_.world_frame(), + &p_WCa); + plant_.CalcPointsPositions(context, frameB, p_BCb, plant_.world_frame(), + &p_WCb); + std::vector> witness_points; + witness_points.push_back(p_WCa); + return witness_points; +} + +template +std::vector>> HydroelasticGeomCollider::DoEval( + const Context& context, Matrix force_basis, + JacobianWrtVariable wrt, bool planar) { + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + const auto& inspector = query_object.inspector(); + + const auto& contact_surfaces = query_object.ComputeContactSurfaces( + HydroelasticContactRepresentation::kPolygon); +// auto contact_info = drake::multibody::internal::HydroelasticContactInfoAndBodySpatialForces( +// plant_.num_bodies()); +// plant_.CalcHydroelasticContactForces(context, &contact_info); + std::vector>> contact_data; + std::set> + geom_pairs_with_data; + for (auto& contact_surface : contact_surfaces) { + std::cout << "body M name: " + << plant_ + .GetBodyFromFrameId( + inspector.GetFrameId(contact_surface.id_M())) + ->name() + << std::endl; + std::cout << "body N name: " + << plant_ + .GetBodyFromFrameId( + inspector.GetFrameId(contact_surface.id_N())) + ->name() + << std::endl; + std::cout << "Has M gradient" << contact_surface.HasGradE_M() << std::endl; + std::cout << "Has N gradient" << contact_surface.HasGradE_N() << std::endl; + + std::cout << "Total area: " << contact_surface.total_area() << std::endl; + std::cout << "contact_surface.num_faces()" << contact_surface.num_faces() + << std::endl; + const SignedDistancePair contact_surface_signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints( + contact_surface.id_M(), contact_surface.id_N()); + + // const auto frame_B_id = inspector.GetFrameId(geometry_id_B_); + const auto& frameM = + plant_.GetBodyFromFrameId(inspector.GetFrameId(contact_surface.id_M())) + ->body_frame(); + const auto& frameN = + plant_.GetBodyFromFrameId(inspector.GetFrameId(contact_surface.id_N())) + ->body_frame(); + + const Vector3d& p_ACM = + inspector.GetPoseInFrame(contact_surface.id_M()).template cast() * + contact_surface_signed_distance_pair.p_ACa; + const Vector3d& p_BCN = + inspector.GetPoseInFrame(contact_surface.id_N()).template cast() * + contact_surface_signed_distance_pair.p_BCb; + + Vector3d p_WCa = Vector3d::Zero(); + Vector3d p_WCb = Vector3d::Zero(); + plant_.CalcPointsPositions(context, frameM, p_ACM, plant_.world_frame(), + &p_WCa); + plant_.CalcPointsPositions(context, frameN, p_BCN, plant_.world_frame(), + &p_WCb); + int face_with_area = 0; + VectorXd wrench_basis = VectorXd::Zero(6); + + for (int i = 0; i < contact_surface.num_faces(); ++i) { + if (contact_surface.area(i) > .0001) { + std::cout << "face centroid: " << contact_surface.centroid(i) + << std::endl; + if (contact_surface.HasGradE_N()) { + std::cout << "Grad N" << contact_surface.EvaluateGradE_N_W(i) + << std::endl; + } + face_with_area += 1; + VectorXd wrench = VectorXd::Zero(6); + wrench << contact_surface.face_normal(i), + (contact_surface.centroid(i) - p_WCb) + .cross(contact_surface.face_normal(i)); + wrench_basis += contact_surface.area(i) * wrench; + } + } + std::cout << "wrench basis: " << wrench_basis << std::endl; + std::cout << "faces with non-negligible area: " << face_with_area + << std::endl; + int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() + : plant_.num_positions(); +// Matrix Jv_WCa(3, n_cols); + Matrix Js_V_ABp_E(6, n_cols); + plant_.CalcJacobianSpatialVelocity(context, wrt, frameM, contact_surface_signed_distance_pair.p_ACa, + plant_.world_frame(), + plant_.world_frame(), &Js_V_ABp_E); + plant_.CalcJacobianSpatialVelocity(context, wrt, frameM, contact_surface_signed_distance_pair.p_ACa, + plant_.world_frame(), + plant_.world_frame(), &Js_V_ABp_E); + contact_data.push_back(std::pair>(contact_surface_signed_distance_pair.distance, Js_V_ABp_E)); + geom_pairs_with_data.insert( + drake::SortedPair(contact_surface.id_M(), + contact_surface.id_N())); + } + + for (auto &contact_pair : contact_pairs_){ + if (geom_pairs_with_data.find(contact_pair) == geom_pairs_with_data.end()){ + auto geometry_id_M = contact_pair.first(); + auto geometry_id_N = contact_pair.second(); + const SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints(geometry_id_M, + geometry_id_N); + const auto frame_A_id = inspector.GetFrameId(geometry_id_M); + const auto frame_B_id = inspector.GetFrameId(geometry_id_N); + const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame(); + const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame(); + + const Vector3d& p_ACa = + inspector.GetPoseInFrame(geometry_id_M).template cast() * + signed_distance_pair.p_ACa; + const Vector3d& p_BCb = + inspector.GetPoseInFrame(geometry_id_N).template cast() * + signed_distance_pair.p_BCb; + + int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() + : plant_.num_positions(); + Matrix Jv_WCa(3, n_cols); + Matrix Jv_WCb(3, n_cols); + + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameA, p_ACa, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCa); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameB, p_BCb, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCb); + + const auto& R_WC = drake::math::RotationMatrix::MakeFromOneVector( + signed_distance_pair.nhat_BA_W, 0); + + // Standard case + auto J = force_basis * R_WC.matrix().transpose() * (Jv_WCa - Jv_WCb); + contact_data.push_back(std::pair>(signed_distance_pair.distance, J)); + } + } +} + +} // namespace multibody +} // namespace dairlib + +template class dairlib::multibody::HydroelasticGeomCollider; \ No newline at end of file diff --git a/multibody/hydroelastic_geom_collider.h b/multibody/hydroelastic_geom_collider.h new file mode 100644 index 0000000000..4606fad155 --- /dev/null +++ b/multibody/hydroelastic_geom_collider.h @@ -0,0 +1,85 @@ +#pragma once + +#include "drake/common/sorted_pair.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace dairlib { +namespace multibody { + +/// A class for computing collider properties as hydroelastic contact patches +/// This class attempts to compute hydroelastic contact data when available and +/// defaults to a single point contact per geometry pair if hydroelastic is not +/// available +template +class HydroelasticGeomCollider { + public: + /// Default constructor + /// Specifies the MultibodyPlant object, as well as the two geometries + /// + /// @param plant + /// @param geometry_id_A + /// @param geometry_id_B + HydroelasticGeomCollider( + const drake::multibody::MultibodyPlant& plant, + const std::vector>& contact_geoms); + + /// Calculates the distance and contact frame Jacobian. + /// Jacobian is ordered [J_n; J_t], and has shape 3 x (nq or nv), depending + /// on the choice of JacobianWrtVariable. + /// @param context The context for the MultibodyPlant + /// @return A pair with + std::vector>> Eval( + const drake::systems::Context& context, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV); + + /// Calculates the distance and contact frame Jacobian. + /// Jacobian is ordered [J_n; J_t], and has shape + //// (2*num_friction_directions + 1) x (nq or nv), depending + /// on the choice of JacobianWrtVariable. + /// + /// Specifies the number of friction directions, used to + /// construct a polytope representation of friction with + /// (2 * num_friction_directions) faces. + /// Setting this to 0 is acceptable for frictionless contact + /// num_friction_directions != 1, as this would not be + /// well-defined in 3D. + /// + /// @param context The context for the MultibodyPlant + /// @param num_friction_directions + /// @return A pair with + std::vector>> EvalPolytope( + const drake::systems::Context& context, int num_friction_directions, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV); + + /// Calculates the distance and contact frame Jacobian for a 2D planar problem + /// Jacobian is ordered [J_n; +J_t; -J_t], and has shape 3 x (nq). + /// J_t refers to the (contact_normal x planar_normal) direction + /// @param context The context for the MultibodyPlant + /// @param planar_normal The normal vector to the planar system + /// @return A pair with + std::vector>> EvalPlanar( + const drake::systems::Context& context, + const Eigen::Vector3d& planar_normal, + drake::multibody::JacobianWrtVariable wrt = + drake::multibody::JacobianWrtVariable::kV); + + std::vector> CalcWitnessPoints( + const drake::systems::Context& context); + + private: + std::vector>> DoEval( + const drake::systems::Context& context, + Eigen::Matrix force_basis, + drake::multibody::JacobianWrtVariable wrt, bool planar = false); + + const drake::multibody::MultibodyPlant& plant_; + const std::vector>& + contact_pairs_; +}; + +} // namespace multibody +} // namespace dairlib + +extern template class dairlib::multibody::HydroelasticGeomCollider; diff --git a/multibody/kinematic/distance_evaluator.h b/multibody/kinematic/distance_evaluator.h index d34dcd1f4d..490cecc40d 100644 --- a/multibody/kinematic/distance_evaluator.h +++ b/multibody/kinematic/distance_evaluator.h @@ -14,7 +14,7 @@ namespace multibody { /// The one exception is Jdotv, since Drake does not currently support /// MultibodyPlant.CalcBiasSpatialAcceleration with non-world frames, template -class DistanceEvaluator : public KinematicEvaluator { +class DistanceEvaluator final : public KinematicEvaluator { public: /// Constructor for DistanceEvaluator /// @param plant diff --git a/multibody/kinematic/kinematic_evaluator_set.cc b/multibody/kinematic/kinematic_evaluator_set.cc index 5e1e8d0373..551ff7e646 100644 --- a/multibody/kinematic/kinematic_evaluator_set.cc +++ b/multibody/kinematic/kinematic_evaluator_set.cc @@ -1,5 +1,6 @@ #include "multibody/kinematic/kinematic_evaluator_set.h" +#include "drake/common/text_logging.h" #include "drake/math/autodiff_gradient.h" namespace dairlib { @@ -149,7 +150,7 @@ VectorX KinematicEvaluatorSet::EvalFullJacobianDotTimesV( } template -int KinematicEvaluatorSet::add_evaluator(KinematicEvaluator* e) { +int KinematicEvaluatorSet::add_evaluator(const KinematicEvaluator* e) { // Compare plants for equality by reference DRAKE_DEMAND(&plant_ == &e->plant()); diff --git a/multibody/kinematic/kinematic_evaluator_set.h b/multibody/kinematic/kinematic_evaluator_set.h index 62f65ee9ac..b7e4c00f1a 100644 --- a/multibody/kinematic/kinematic_evaluator_set.h +++ b/multibody/kinematic/kinematic_evaluator_set.h @@ -150,12 +150,12 @@ class KinematicEvaluatorSet { return *evaluators_.at(index); }; - const std::vector*>& get_evaluators() const { + const std::vector*>& get_evaluators() const { return evaluators_; }; /// Adds an evaluator to the end of the list, returning the associated index - int add_evaluator(KinematicEvaluator* e); + int add_evaluator(const KinematicEvaluator* e); /// Count the total number of active rows int count_active() const; @@ -174,7 +174,7 @@ class KinematicEvaluatorSet { private: const drake::multibody::MultibodyPlant& plant_; - std::vector*> evaluators_; + std::vector*> evaluators_; }; } // namespace multibody diff --git a/multibody/kinematic/test/kinematic_evaluator_caching_test.cc b/multibody/kinematic/test/kinematic_evaluator_caching_test.cc index 43cd3e62fe..333dec4bc5 100644 --- a/multibody/kinematic/test/kinematic_evaluator_caching_test.cc +++ b/multibody/kinematic/test/kinematic_evaluator_caching_test.cc @@ -309,7 +309,7 @@ int DoMain(int argc, char* argv[]) { drake::multibody::MultibodyPlant plant(0); drake::multibody::Parser parser(&plant); std::string full_name = FindResourceOrThrow(filename); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant.Finalize(); multibody::KinematicEvaluatorSet evaluators(plant); diff --git a/multibody/kinematic/test/kinematic_evaluator_test.cc b/multibody/kinematic/test/kinematic_evaluator_test.cc index 904993cd22..14ca6846da 100644 --- a/multibody/kinematic/test/kinematic_evaluator_test.cc +++ b/multibody/kinematic/test/kinematic_evaluator_test.cc @@ -36,7 +36,7 @@ class KinematicEvaluatorTest : public ::testing::Test { std::string full_name = dairlib::FindResourceOrThrow("examples/PlanarWalker/PlanarWalker.urdf"); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant_->WeldFrames(plant_->world_frame(), plant_->GetFrameByName("base"), drake::math::RigidTransform()); diff --git a/multibody/kinematic/world_point_evaluator.h b/multibody/kinematic/world_point_evaluator.h index 420aaf22da..22eee0083e 100644 --- a/multibody/kinematic/world_point_evaluator.h +++ b/multibody/kinematic/world_point_evaluator.h @@ -34,7 +34,7 @@ class WorldPointEvaluator : public KinematicEvaluator { std::vector active_directions = {0, 1, 2}); /// The same constructor as the above one except for the argument - /// `view_frame`. + /// `view_frame_`. /// `WorldPointEvaluator` computes position, Jacobian and JdotV in the world /// frame, and expresses them in `ViewFrame` (i.e. rotates the vectors and /// matrix). diff --git a/multibody/multibody_utils.cc b/multibody/multibody_utils.cc index 4c1156778f..8bd7d0a66d 100644 --- a/multibody/multibody_utils.cc +++ b/multibody/multibody_utils.cc @@ -4,6 +4,7 @@ #include #include "drake/common/drake_assert.h" +#include "drake/geometry/proximity_properties.h" #include "drake/math/autodiff_gradient.h" namespace dairlib { @@ -22,9 +23,9 @@ using drake::multibody::JointIndex; using drake::multibody::ModelInstanceIndex; using drake::multibody::MultibodyPlant; using drake::systems::Context; -using Eigen::VectorXd; -using Eigen::Vector3d; using Eigen::Vector2d; +using Eigen::Vector3d; +using Eigen::VectorXd; using std::map; using std::string; using std::vector; @@ -121,7 +122,8 @@ void SetInputsIfNew(const MultibodyPlant& plant, template void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, double mu_static, double mu_kinetic, - Eigen::Vector3d normal_W, bool show_ground) { + Eigen::Vector3d normal_W, double stiffness, + double dissipation_rate, bool show_ground) { if (!plant->geometry_source_is_registered()) { plant->RegisterAsSourceForSceneGraph(scene_graph); } @@ -133,8 +135,19 @@ void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, const drake::math::RigidTransformd X_WG( HalfSpace::MakePose(normal_W, point_W)); - plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace(), - "collision", friction); + if (stiffness != 0) { + drake::geometry::ProximityProperties props; + props.AddProperty("material", "point_contact_stiffness", stiffness); + props.AddProperty("material", "hunt_crossley_dissipation", + dissipation_rate); + props.AddProperty(drake::geometry::internal::kMaterialGroup, + drake::geometry::internal::kFriction, friction); + plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace(), + "collision", props); + } else { + plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace(), + "collision", friction); + } // Add visual for the ground. if (show_ground) { @@ -144,10 +157,11 @@ void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, } /// Get the ordered names from a NameTo___Map -vector ExtractOrderedNamesFromMap(const map& map) { +vector ExtractOrderedNamesFromMap(const map& map, + int index_start) { vector names(map.size()); for (const auto& entry : map) { - names[entry.second] = entry.first; + names[entry.second - index_start] = entry.first; } return names; } @@ -188,16 +202,12 @@ map MakeNameToPositionsMap(const MultibodyPlant& plant) { } } - // TODO: once RBT fully deprecated, this block can likely be removed, using - // default coordinate names from Drake. auto floating_bodies = plant.GetFloatingBaseBodies(); - DRAKE_THROW_UNLESS(floating_bodies.size() <= 1); for (auto body_index : floating_bodies) { const auto& body = plant.get_body(body_index); DRAKE_ASSERT(body.has_quaternion_dofs()); int start = body.floating_positions_start(); - // should be body.name() once RBT is deprecated - std::string name = "base"; + std::string name = body.name(); name_to_index_map[name + "_qw"] = start; name_to_index_map[name + "_qx"] = start + 1; name_to_index_map[name + "_qy"] = start + 2; @@ -219,6 +229,64 @@ map MakeNameToPositionsMap(const MultibodyPlant& plant) { return name_to_index_map; } +/// Construct a map between joint names and position indices +/// such that q(index) has the given name +/// -Only accurately includes joints with a single position and single velocity +/// -Others are included as "position[ind]"" +/// -Index mapping can also be used as a state mapping (assumes x = [q;v]) +template +map MakeNameToPositionsMap(const MultibodyPlant& plant, + ModelInstanceIndex model_instance) { + map name_to_index_map; + std::set index_set; + for (auto i : plant.GetJointIndices(model_instance)) { + const drake::multibody::Joint& joint = plant.get_joint(i); + auto name = joint.name(); + + if (joint.num_velocities() == 1 && joint.num_positions() == 1) { + std::vector index_vector{i}; + auto selectorMatrix = plant.MakeStateSelectorMatrix(index_vector); + // find index and add + int selector_index = -1; + for (int j = 0; j < selectorMatrix.cols(); ++j) { + if (selectorMatrix(0, j) == 1) { + if (selector_index == -1) { + selector_index = j; + } else { + throw std::logic_error("Unable to create selector map."); + } + } + } + if (selector_index == -1) { + std::logic_error("Unable to create selector map."); + } + + name_to_index_map[name] = selector_index; + index_set.insert(selector_index); + } + } + + if (plant.HasUniqueFreeBaseBody(model_instance)) { + const auto& body = plant.GetUniqueFreeBaseBodyOrThrow(model_instance); + DRAKE_ASSERT(body.has_quaternion_dofs()); + int start = body.floating_positions_start(); + std::string name = body.name(); + name_to_index_map[name + "_qw"] = start; + name_to_index_map[name + "_qx"] = start + 1; + name_to_index_map[name + "_qy"] = start + 2; + name_to_index_map[name + "_qz"] = start + 3; + name_to_index_map[name + "_x"] = start + 4; + name_to_index_map[name + "_y"] = start + 5; + name_to_index_map[name + "_z"] = start + 6; + for (int i = 0; i < 7; i++) { + index_set.insert(start + i); + } + } + // if index has not already been captured, throw an error + DRAKE_THROW_UNLESS(plant.num_positions(model_instance) == index_set.size()); + + return name_to_index_map; +} /// Construct a map between joint names and velocity indices /// such that v(index) has the given name @@ -233,8 +301,6 @@ map MakeNameToVelocitiesMap(const MultibodyPlant& plant) { for (JointIndex i(0); i < plant.num_joints(); ++i) { const drake::multibody::Joint& joint = plant.get_joint(i); - // TODO(posa): this "dot" should be removed, it's an anachronism from - // RBT auto name = joint.name() + "dot"; if (joint.num_velocities() == 1 && joint.num_positions() == 1) { @@ -261,12 +327,10 @@ map MakeNameToVelocitiesMap(const MultibodyPlant& plant) { } auto floating_bodies = plant.GetFloatingBaseBodies(); - // Remove throw once RBT deprecated - DRAKE_THROW_UNLESS(floating_bodies.size() <= 1); for (auto body_index : floating_bodies) { const auto& body = plant.get_body(body_index); - int start = body.floating_velocities_start() - plant.num_positions(); - std::string name = "base"; // should be body.name() once RBT is deprecated + int start = body.floating_velocities_start_in_v(); + std::string name = body.name(); name_to_index_map[name + "_wx"] = start; name_to_index_map[name + "_wy"] = start + 1; name_to_index_map[name + "_wz"] = start + 2; @@ -285,6 +349,67 @@ map MakeNameToVelocitiesMap(const MultibodyPlant& plant) { } } + // if index has not already been captured, throw an error + DRAKE_THROW_UNLESS(plant.num_velocities() == index_set.size()); + return name_to_index_map; +} + +/// Construct a map between joint names and velocity indices +/// such that v(index) has the given name +/// -Only accurately includes joints with a single position and single velocity +/// -Others are included as "state[ind]" +/// -Index mapping can also be used as a state mapping, AFTER +/// an offset of num_positions is applied (assumes x = [q;v]) +template +map MakeNameToVelocitiesMap(const MultibodyPlant& plant, + ModelInstanceIndex model_instance) { + map name_to_index_map; + std::set index_set; + + for (auto i : plant.GetJointIndices(model_instance)) { + const drake::multibody::Joint& joint = plant.get_joint(i); + auto name = joint.name() + "dot"; + + if (joint.num_velocities() == 1 && joint.num_positions() == 1) { + std::vector index_vector{i}; + auto selectorMatrix = plant.MakeStateSelectorMatrix(index_vector); + // find index and add + int selector_index = -1; + for (int j = 0; j < selectorMatrix.cols(); ++j) { + if (selectorMatrix(1, j) == 1) { + if (selector_index == -1) { + selector_index = j; + } else { + throw std::logic_error("Unable to create selector map."); + } + } + } + if (selector_index == -1) { + throw std::logic_error("Unable to create selector map."); + } + + name_to_index_map[name] = selector_index - plant.num_positions(); + index_set.insert(selector_index - plant.num_positions()); + } + } + + if (plant.HasUniqueFreeBaseBody(model_instance)) { + const auto& body = plant.GetUniqueFreeBaseBodyOrThrow(model_instance); + int start = body.floating_velocities_start_in_v(); + std::string name = body.name(); + name_to_index_map[name + "_wx"] = start; + name_to_index_map[name + "_wy"] = start + 1; + name_to_index_map[name + "_wz"] = start + 2; + name_to_index_map[name + "_vx"] = start + 3; + name_to_index_map[name + "_vy"] = start + 4; + name_to_index_map[name + "_vz"] = start + 5; + for (int i = 0; i < 6; i++) { + index_set.insert(start + i); + } + } + + // if index has not already been captured, throw an error + DRAKE_THROW_UNLESS(plant.num_velocities(model_instance) == index_set.size()); return name_to_index_map; } @@ -323,8 +448,7 @@ map MakeNameToActuatorsMap(const MultibodyPlant& plant) { } template -vector CreateStateNameVectorFromMap( - const MultibodyPlant& plant) { +vector CreateStateNameVectorFromMap(const MultibodyPlant& plant) { map pos_map = MakeNameToPositionsMap(plant); map vel_map = MakeNameToVelocitiesMap(plant); vector state_names(pos_map.size() + vel_map.size()); @@ -340,8 +464,7 @@ vector CreateStateNameVectorFromMap( } template -vector CreateActuatorNameVectorFromMap( - const MultibodyPlant& plant) { +vector CreateActuatorNameVectorFromMap(const MultibodyPlant& plant) { map act_map = MakeNameToActuatorsMap(plant); return ExtractOrderedNamesFromMap(act_map); } @@ -453,10 +576,9 @@ Vector3d ReExpressWorldVector3InBodyYawFrame(const MultibodyPlant& plant, const Vector3d& vec) { Vector3d pelvis_x = plant.GetBodyByName(body_name).EvalPoseInWorld(context).rotation().col(0); - double yaw = atan2(pelvis_x(1), pelvis_x(0)); - return Vector3d(cos(yaw) * vec(0) + sin(yaw)*vec(1), - -sin(yaw) * vec(0) + cos(yaw)*vec(1), - vec(2)); + double yaw = atan2(pelvis_x(1), pelvis_x(0)); + return Vector3d(cos(yaw) * vec(0) + sin(yaw) * vec(1), + -sin(yaw) * vec(0) + cos(yaw) * vec(1), vec(2)); } template @@ -466,9 +588,9 @@ Vector2d ReExpressWorldVector2InBodyYawFrame(const MultibodyPlant& plant, const Vector2d& vec) { Vector3d pelvis_x = plant.GetBodyByName(body_name).EvalPoseInWorld(context).rotation().col(0); - double yaw = atan2(pelvis_x(1), pelvis_x(0)); - return Vector2d(cos(yaw) * vec(0) + sin(yaw)*vec(1), - -sin(yaw) * vec(0) + cos(yaw)*vec(1)); + double yaw = atan2(pelvis_x(1), pelvis_x(0)); + return Vector2d(cos(yaw) * vec(0) + sin(yaw) * vec(1), + -sin(yaw) * vec(0) + cos(yaw) * vec(1)); } VectorXd MakeJointPositionOffsetFromMap( @@ -487,21 +609,21 @@ Eigen::MatrixXd WToQuatDotMap(const Eigen::Vector4d& q) { // clang-format off Eigen::MatrixXd ret(4,3); ret << -q(1), -q(2), -q(3), - q(0), q(3), -q(2), - -q(3), q(0), q(1), - q(2), -q(1), q(0); + q(0), q(3), -q(2), + -q(3), q(0), q(1), + q(2), -q(1), q(0); ret *= 0.5; // clang-format on return ret; } -Eigen::MatrixXd JwrtqdotToJwrtv( - const Eigen::VectorXd& q, const Eigen::MatrixXd& Jwrtqdot) { +Eigen::MatrixXd JwrtqdotToJwrtv(const Eigen::VectorXd& q, + const Eigen::MatrixXd& Jwrtqdot) { //[J_1:4, J_5:end] * [WToQuatDotMap, 0] = [J_1:4 * WToQuatDotMap, J_5:end] // [ 0 , I] DRAKE_DEMAND(Jwrtqdot.cols() == q.size()); - Eigen::MatrixXd ret(Jwrtqdot.rows(), q.size() -1); + Eigen::MatrixXd ret(Jwrtqdot.rows(), q.size() - 1); ret << Jwrtqdot.leftCols<4>() * WToQuatDotMap(q.head<4>()), Jwrtqdot.rightCols(q.size() - 4); return ret; @@ -517,8 +639,12 @@ template Vector3d ReExpressWorldVector3InBodyYawFrame(const MultibodyPlant& plant, const Context& context, const std::string& body_name, const Vector2d& vec); //NOLINT template map MakeNameToPositionsMap(const MultibodyPlant& plant); // NOLINT template map MakeNameToPositionsMap(const MultibodyPlant &plant); // NOLINT +template map MakeNameToPositionsMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT +template map MakeNameToPositionsMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT template map MakeNameToVelocitiesMap(const MultibodyPlant& plant); // NOLINT template map MakeNameToVelocitiesMap(const MultibodyPlant& plant); // NOLINT +template map MakeNameToVelocitiesMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT +template map MakeNameToVelocitiesMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT template map MakeNameToActuatorsMap(const MultibodyPlant& plant); // NOLINT template map MakeNameToActuatorsMap(const MultibodyPlant& plant); // NOLINT template vector CreateStateNameVectorFromMap(const MultibodyPlant& plant); // NOLINT @@ -527,7 +653,7 @@ template vector CreateActuatorNameVectorFromMap(const MultibodyPlant CreateActuatorNameVectorFromMap(const MultibodyPlant& plant); // NOLINT template Eigen::MatrixXd CreateWithSpringsToWithoutSpringsMapPos(const drake::multibody::MultibodyPlant& plant_w_spr, const drake::multibody::MultibodyPlant& plant_wo_spr); // NOLINT template Eigen::MatrixXd CreateWithSpringsToWithoutSpringsMapVel(const drake::multibody::MultibodyPlant& plant_w_spr, const drake::multibody::MultibodyPlant& plant_wo_spr); // NOLINT -template void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, double mu_static, double mu_kinetic, Eigen::Vector3d normal_W, bool show_ground); // NOLINT +template void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, double mu_static, double mu_kinetic, Eigen::Vector3d normal_W, double stiffness, double dissipation_rate, bool show_ground); // NOLINT template VectorX GetInput(const MultibodyPlant& plant, const Context& context); // NOLINT template VectorX GetInput(const MultibodyPlant& plant, const Context& context); // NOLINT template std::unique_ptr> CreateContext(const MultibodyPlant& plant, const Eigen::Ref& state, const Eigen::Ref& input); // NOLINT diff --git a/multibody/multibody_utils.h b/multibody/multibody_utils.h index 03ad94022a..4d829fe985 100644 --- a/multibody/multibody_utils.h +++ b/multibody/multibody_utils.h @@ -68,23 +68,37 @@ void AddFlatTerrain(drake::multibody::MultibodyPlant* plant, drake::geometry::SceneGraph* scene_graph, double mu_static, double mu_kinetic, Eigen::Vector3d normal_W = Eigen::Vector3d(0, 0, 1), + double stiffness = 0, double dissipation_rate = 0, bool show_ground = true); /// Get the ordered names from a NameTo___Map std::vector ExtractOrderedNamesFromMap( - const std::map& map); + const std::map& map, int index_start = 0); /// Given a MultibodyPlant, builds a map from position name to position index template std::map MakeNameToPositionsMap( const drake::multibody::MultibodyPlant& plant); -/// Given a MultiBodyTree, builds a map from velocity name to velocity index +/// Given a MultibodyPlant, builds a map from position name to position index +template +std::map MakeNameToPositionsMap( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index); + +/// Given a MultiBodyPlant, builds a map from velocity name to velocity index template std::map MakeNameToVelocitiesMap( const drake::multibody::MultibodyPlant& plant); -/// Given a MultiBodyTree, builds a map from actuator name to actuator index + +/// Given a MultiBodyPlant, builds a map from velocity name to velocity index +template +std::map MakeNameToVelocitiesMap( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index); + +/// Given a MultiBodyPlant, builds a map from actuator name to actuator index template std::map MakeNameToActuatorsMap( const drake::multibody::MultibodyPlant& plant); diff --git a/multibody/multipose_visualizer.cc b/multibody/multipose_visualizer.cc index e11da4ef05..cf1e021430 100644 --- a/multibody/multipose_visualizer.cc +++ b/multibody/multipose_visualizer.cc @@ -1,11 +1,9 @@ #include "multibody/multipose_visualizer.h" -#include "drake/geometry/drake_visualizer.h" #include "drake/geometry/scene_graph.h" #include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/lcm/lcm_interface_system.h" -using drake::geometry::DrakeVisualizer; +using drake::geometry::Meshcat; using drake::geometry::SceneGraph; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; @@ -19,17 +17,22 @@ namespace multibody { MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, string weld_frame_to_world) : MultiposeVisualizer(model_file, num_poses, - Eigen::VectorXd::Constant(num_poses, 1.0)) {} + Eigen::VectorXd::Constant(num_poses, 1.0), + weld_frame_to_world) {} MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, double alpha_scale, string weld_frame_to_world) : MultiposeVisualizer(model_file, num_poses, - Eigen::VectorXd::Constant(num_poses, alpha_scale)) {} + Eigen::VectorXd::Constant(num_poses, alpha_scale), + weld_frame_to_world) {} MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, const Eigen::VectorXd& alpha_scale, - string weld_frame_to_world) + string weld_frame_to_world, + std::shared_ptr meshcat, + const std::string& pose_trace_name, + const Eigen::VectorXd& rgb) : num_poses_(num_poses) { DRAKE_DEMAND(num_poses == alpha_scale.size()); DiagramBuilder builder; @@ -38,13 +41,11 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, std::tie(plant_, scene_graph) = drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.0); - auto lcm = builder.AddSystem(); - Parser parser(plant_, scene_graph); - + Parser parser(plant_, scene_graph, pose_trace_name + "_pose_trace"); + parser.SetAutoRenaming(true); // Add num_poses copies of the plant, giving each a unique name for (int i = 0; i < num_poses_; i++) { - auto index = - parser.AddModelFromFile(model_file, "model[" + std::to_string(i) + "]"); + auto index = parser.AddModels(model_file)[0]; model_indices_.push_back(index); if (!weld_frame_to_world.empty()) { plant_->WeldFrames( @@ -77,7 +78,12 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, double new_alpha = alpha_scale(i - 2) * phong.a(); new_alpha = std::max(new_alpha, 0.0); new_alpha = std::min(new_alpha, 1.0); - phong.set(phong.r(), phong.g(), phong.b(), new_alpha); + if (rgb.size() == 3) { + phong.set(rgb(0), rgb(1), rgb(2), new_alpha); + } + else { + phong.set(phong.r(), phong.g(), phong.b(), new_alpha); + } new_props.UpdateProperty("phong", "diffuse", phong); scene_graph->AssignRole(plant_->get_source_id().value(), geometry_id, @@ -88,21 +94,32 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, } } - DrakeVisualizer::AddToBuilder(&builder, *scene_graph, lcm); + if (meshcat == nullptr) { + meshcat_ = std::make_shared(); + } else { + meshcat_ = meshcat; + } + meshcat_visualizer_ = + &drake::geometry::MeshcatVisualizer::AddToBuilder( + &builder, *scene_graph, meshcat_); + diagram_ = builder.Build(); diagram_context_ = diagram_->CreateDefaultContext(); - DrakeVisualizer::DispatchLoadMessage(*scene_graph, lcm); } -void MultiposeVisualizer::DrawPoses(MatrixXd poses) { +void MultiposeVisualizer::DrawPoses(MatrixXd poses, std::optional time_in_recording) { // Set positions for individual instances auto& plant_context = diagram_->GetMutableSubsystemContext(*plant_, diagram_context_.get()); + for (int i = 0; i < num_poses_; i++) { plant_->SetPositions( &plant_context, model_indices_.at(i), poses.block(0, i, plant_->num_positions(model_indices_.at(i)), 1)); } + if (time_in_recording){ + diagram_context_->SetTime(time_in_recording.value()); + } // Publish diagram diagram_->ForcedPublish(*diagram_context_); diff --git a/multibody/multipose_visualizer.h b/multibody/multipose_visualizer.h index 3d4f28b0d7..07374c3d62 100644 --- a/multibody/multipose_visualizer.h +++ b/multibody/multipose_visualizer.h @@ -3,6 +3,7 @@ #include #include +#include "drake/geometry/meshcat_visualizer.h" #include "drake/geometry/scene_graph.h" #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" @@ -44,19 +45,33 @@ class MultiposeVisualizer { /// @param alpha_scale Vector, of same length as num_poses. Provideas variable /// scaling of the transparency alpha field of all bodies, indexed by pose /// @param weld_frame_to_world Welds the frame of the given name to the world - MultiposeVisualizer(std::string model_file, int num_poses, - const Eigen::VectorXd& alpha_scale, - std::string weld_frame_to_world = ""); + /// @param meshcat Pointer to meshcat visualizer for option to attach to an + /// existing meshcat instance + /// @param pose_trace_name Name of the pose trace to use in meshcat + /// @param rgb the RGB color to use for all bodies. If not provided, the + /// color will default to what is defined in the model file. + MultiposeVisualizer( + std::string model_file, int num_poses, const Eigen::VectorXd& alpha_scale, + std::string weld_frame_to_world = "", + std::shared_ptr meshcat = nullptr, + const std::string& pose_trace_name = "", + const Eigen::VectorXd& rgb = Eigen::VectorXd()); /// Draws the poses in the given (num_positions x num_poses) matrix /// Note: the matrix can have extra rows (e.g. velocities), which will be /// ignored. - void DrawPoses(Eigen::MatrixXd poses); + void DrawPoses(Eigen::MatrixXd poses, std::optional time_in_recording = std::nullopt); + + const std::shared_ptr GetMeshcat() { + return meshcat_; + } private: int num_poses_; drake::multibody::MultibodyPlant* plant_; std::unique_ptr> diagram_; + std::shared_ptr meshcat_; + drake::geometry::MeshcatVisualizer* meshcat_visualizer_; std::unique_ptr> diagram_context_; std::vector model_indices_; }; diff --git a/multibody/test/geom_geom_collider_test.cc b/multibody/test/geom_geom_collider_test.cc index bcee569344..dbb1d33f08 100644 --- a/multibody/test/geom_geom_collider_test.cc +++ b/multibody/test/geom_geom_collider_test.cc @@ -28,9 +28,9 @@ void GeomGeomColliderTest() { parser.package_map().Add("robot_properties_fingers", "examples/trifinger/robot_properties_fingers"); - parser.AddModelFromFile(FindResourceOrThrow("examples/trifinger/" + parser.AddModels(FindResourceOrThrow("examples/trifinger/" "robot_properties_fingers/urdf/trifinger_minimal_collision.urdf")); - parser.AddModelFromFile(FindResourceOrThrow( + parser.AddModels(FindResourceOrThrow( "examples/trifinger/robot_properties_fingers/cube/cube_v2.urdf")); auto X_WI = drake::math::RigidTransform::Identity(); @@ -74,12 +74,12 @@ void GeomGeomColliderTest() { q(q_map.at("finger_base_to_upper_joint_240")) = 0; q(q_map.at("finger_upper_to_middle_joint_240")) = -1; q(q_map.at("finger_middle_to_lower_joint_240")) = -1.5; - q(q_map.at("base_qw")) = 1; - q(q_map.at("base_qx")) = 0; - q(q_map.at("base_qz")) = 0; - q(q_map.at("base_x")) = 0; - q(q_map.at("base_y")) = 0; - q(q_map.at("base_z")) = .05; + q(q_map.at("cube_qw")) = 1; + q(q_map.at("cube_qx")) = 0; + q(q_map.at("cube_qz")) = 0; + q(q_map.at("cube_x")) = 0; + q(q_map.at("cube_y")) = 0; + q(q_map.at("cube_z")) = .05; plant.SetPositions(&context, q); diff --git a/multibody/test/multibody_utils_test.cc b/multibody/test/multibody_utils_test.cc index 8c9f4df502..021411465a 100644 --- a/multibody/test/multibody_utils_test.cc +++ b/multibody/test/multibody_utils_test.cc @@ -23,7 +23,7 @@ class MultibodyUtilsTest : public ::testing::Test { std::string full_name = FindResourceOrThrow( "examples/Cassie/urdf/cassie_v2.urdf"); Parser parser(&plant_, &scene_graph); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); plant_.Finalize(); } diff --git a/multibody/visualization_utils.cc b/multibody/visualization_utils.cc index b5d819fc0c..ab2b7d70c8 100644 --- a/multibody/visualization_utils.cc +++ b/multibody/visualization_utils.cc @@ -3,7 +3,6 @@ #include "common/find_resource.h" #include "multibody/com_pose_system.h" #include "systems/primitives/subvector_pass_through.h" -#include "drake/geometry/drake_visualizer.h" #include "drake/multibody/parsing/parser.h" #include "drake/systems/primitives/trajectory_source.h" @@ -26,7 +25,7 @@ std::unique_ptr> ConstructBallPlant( auto ball_plant = std::make_unique>(0.0); std::string full_name = FindResourceOrThrow("multibody/ball.urdf"); Parser parser(ball_plant.get(), scene_graph); - parser.AddModelFromFile(full_name); + parser.AddModels(full_name); ball_plant->Finalize(); return ball_plant; } @@ -37,11 +36,11 @@ void ConnectTrajectoryVisualizer( drake::geometry::SceneGraph* scene_graph, const Trajectory& trajectory) { auto empty_plant = std::make_unique>(0.0); - ConnectTrajectoryVisualizer(plant, builder, scene_graph, trajectory, - *empty_plant); + ConnectTrajectoryVisualizerWithCoM(plant, builder, scene_graph, trajectory, + *empty_plant); } -void ConnectTrajectoryVisualizer( +void ConnectTrajectoryVisualizerWithCoM( const MultibodyPlant* plant, drake::systems::DiagramBuilder* builder, drake::geometry::SceneGraph* scene_graph, @@ -81,7 +80,6 @@ void ConnectTrajectoryVisualizer( scene_graph->get_source_pose_port(ball_plant.get_source_id().value())); } - DrakeVisualizer::AddToBuilder(builder, *scene_graph); } } // namespace multibody diff --git a/multibody/visualization_utils.h b/multibody/visualization_utils.h index 31de4481c2..fba3f5f21a 100644 --- a/multibody/visualization_utils.h +++ b/multibody/visualization_utils.h @@ -27,7 +27,7 @@ void ConnectTrajectoryVisualizer( drake::systems::DiagramBuilder* builder, drake::geometry::SceneGraph* scene_graph, const drake::trajectories::Trajectory& trajectory); -void ConnectTrajectoryVisualizer( +void ConnectTrajectoryVisualizerWithCoM( const drake::multibody::MultibodyPlant* plant, drake::systems::DiagramBuilder* builder, drake::geometry::SceneGraph* scene_graph, diff --git a/record_video.py b/record_video.py new file mode 100644 index 0000000000..71a04be89a --- /dev/null +++ b/record_video.py @@ -0,0 +1,49 @@ +import subprocess +import sys +import os +import glob +import signal + +from datetime import date +def main(): + # get devices + dev_names = [] + cmd = 'v4l2-ctl --list-devices'.split(' ') + process = subprocess.check_output(cmd) + lines = process.decode("utf-8").split('\n') + for i, line in enumerate(lines): + if 'HD' in line: + dev_names.append(lines[i+1].split('\t')[1]) + + cmds = [] + + # determine log number + curr_date = date.today().strftime("%m_%d_%y") + year = date.today().strftime("%Y") + logdir = f"{os.getenv('HOME')}/Videos/franka_experiments/{year}/{curr_date}/" + os.makedirs(logdir, exist_ok=True) + current_logs = sorted(glob.glob(logdir + '*log_*')) + if current_logs: + last_log = int(current_logs[-1].split('_')[-2].split('.')[0]) + log_num = f'{last_log+1:02}' + else: + log_num = '00' + + # construct video writing commands + for i, name in enumerate(dev_names): + experiment_name = logdir + 'log_' + log_num + '_cam' + str(i) + '.mp4' + # print(experiment_name) + cmd = ('ffmpeg -y -f v4l2 -r 30 -i ' + name).split(' ') + cmd.append(experiment_name) + print(' '.join(cmd)) + cmds.append(cmd) + # import pdb; pdb.set_trace() + processes = [] + for cmd in cmds: + processes.append(subprocess.Popen(cmd)) + for process in processes: + process.communicate() + + +if __name__ == '__main__': + main() diff --git a/signalscope/BUILD.bazel b/signalscope/BUILD.bazel index a1fbac7032..8f66a1aea2 100644 --- a/signalscope/BUILD.bazel +++ b/signalscope/BUILD.bazel @@ -12,14 +12,14 @@ py_binary( srcs = ["signal-scope.py"], data = ["@signal_scope"], main = "signal-scope.py", + tags = ["manual"], deps = ["//lcmtypes:lcmtypes_robot_py"], - tags=["manual"] ) load("@drake//tools/skylark:drake_runfiles_binary.bzl", "drake_runfiles_binary") drake_runfiles_binary( name = "signal-scope", + tags = ["manual"], target = ":signal-scope-py", - tags = ["manual"] ) diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 638e9a6106..16520d6ff1 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -19,6 +19,64 @@ cc_library( ], ) +cc_library( + name = "c3", + srcs = [ + "c3.cc", + "c3_miqp.cc", + "c3_qp.cc", + ], + hdrs = [ + "c3.h", + "c3_miqp.h", + "c3_options.h", + "c3_qp.h", + ], + copts = [ + "-fopenmp", + ], + linkopts = [ + "-fopenmp", + ], + deps = [ + ":lcs", + "//solvers:fast_osqp_solver", + "@drake//:drake_shared_library", + "@gurobi//:gurobi_cxx", + ], +) + +cc_library( + name = "c3_output", + srcs = [ + "c3_output.cc", + ], + hdrs = [ + "c3_output.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "lcs", + srcs = [ + "lcs.cc", + "lcs_factory.cc", + ], + hdrs = [ + "lcs.h", + "lcs_factory.h", + ], + deps = [ + "//multibody:geom_geom_collider", + "//multibody/kinematic", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "constraint_factory", srcs = [ @@ -86,6 +144,22 @@ cc_library( ], ) +cc_library( + name = "cost_function_utils", + srcs = [ + "cost_function_utils.cc", + ], + hdrs = [ + "cost_function_utils.h", + ], + deps = [ + "nonlinear_cost", + "//systems/trajectory_optimization:dircon", + "//systems/trajectory_optimization/dircon", + "@drake//:drake_shared_library", + ], +) + cc_test( name = "cost_constraint_approximation_test", size = "small", @@ -96,3 +170,8 @@ cc_test( "@gtest//:main", ], ) + +filegroup( + name = "solver_params", + srcs = glob(["*.yaml"]), +) \ No newline at end of file diff --git a/solvers/c3.cc b/solvers/c3.cc new file mode 100644 index 0000000000..1432bb5e8a --- /dev/null +++ b/solvers/c3.cc @@ -0,0 +1,900 @@ +#include "solvers/c3.h" + +#include +#include + +#include +#include + +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/moby_lcp_solver.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +using drake::solvers::MathematicalProgram; +using drake::solvers::MathematicalProgramResult; +using drake::solvers::SolutionResult; +using drake::solvers::SolverOptions; + +using drake::solvers::OsqpSolver; +using drake::solvers::OsqpSolverDetails; +using drake::solvers::Solve; + +C3::CostMatrices::CostMatrices(const std::vector& Q, + const std::vector& R, + const std::vector& G, + const std::vector& U) { + this->Q = Q; + this->R = R; + this->G = G; + this->U = U; +} + +C3::C3(const LCS& lcs, const C3::CostMatrices& costs, + const vector& x_des, const C3Options& options) + : warm_start_(options.warm_start), + lcs_(lcs), + N_((lcs.A_).size()), + n_((lcs.A_)[0].cols()), + m_((lcs.D_)[0].cols()), + k_((lcs.B_)[0].cols()), + A_(lcs.A_), + B_(lcs.B_), + D_(lcs.D_), + d_(lcs.d_), + E_(lcs.E_), + F_(lcs.F_), + H_(lcs.H_), + c_(lcs.c_), + Q_(costs.Q), + R_(costs.R), + U_(costs.U), + G_(costs.G), + x_desired_(x_des), + options_(options), + h_is_zero_(lcs.H_[0].isZero(0)), + osqp_(OsqpSolver()), + prog_(MathematicalProgram()) { + if (warm_start_) { + warm_start_delta_.resize(options_.admm_iter + 1); + warm_start_binary_.resize(options_.admm_iter + 1); + warm_start_x_.resize(options_.admm_iter + 1); + warm_start_lambda_.resize(options_.admm_iter + 1); + warm_start_u_.resize(options_.admm_iter + 1); + for (size_t iter = 0; iter < options_.admm_iter + 1; ++iter) { + warm_start_delta_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_delta_[iter][i] = VectorXd::Zero(n_ + m_ + k_); + } + warm_start_binary_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_binary_[iter][i] = VectorXd::Zero(m_); + } + warm_start_x_[iter].resize(N_ + 1); + for (size_t i = 0; i < N_ + 1; i++) { + warm_start_x_[iter][i] = VectorXd::Zero(n_); + } + warm_start_lambda_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_lambda_[iter][i] = VectorXd::Zero(m_); + } + warm_start_u_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_u_[iter][i] = VectorXd::Zero(k_); + } + } + } + + auto Dn = lcs.D_.at(0).norm(); + auto An = lcs.A_.at(0).norm(); + AnDn_ = An / Dn; + + for (int i = 0; i < N_; ++i) { + D_.at(i) *= AnDn_; + E_.at(i) /= AnDn_; + c_.at(i) /= AnDn_; + H_.at(i) /= AnDn_; + } + + x_ = vector(); + u_ = vector(); + lambda_ = vector(); + + z_sol_ = std::make_unique>(); + x_sol_ = std::make_unique>(); + lambda_sol_ = std::make_unique>(); + u_sol_ = std::make_unique>(); + w_sol_ = std::make_unique>(); + delta_sol_ = std::make_unique>(); + for (int i = 0; i < N_; ++i) { + z_sol_->push_back(Eigen::VectorXd::Zero(n_ + m_ + k_)); + x_sol_->push_back(Eigen::VectorXd::Zero(n_)); + lambda_sol_->push_back(Eigen::VectorXd::Zero(m_)); + u_sol_->push_back(Eigen::VectorXd::Zero(k_)); + w_sol_->push_back(Eigen::VectorXd::Zero(n_ + m_ + k_)); + delta_sol_->push_back(Eigen::VectorXd::Zero(n_ + m_ + k_)); + } + + for (int i = 0; i < N_ + 1; i++) { + x_.push_back(prog_.NewContinuousVariables(n_, "x" + std::to_string(i))); + if (i < N_) { + u_.push_back(prog_.NewContinuousVariables(k_, "k" + std::to_string(i))); + lambda_.push_back( + prog_.NewContinuousVariables(m_, "lambda" + std::to_string(i))); + } + } + + MatrixXd LinEq(n_, 2 * n_ + m_ + k_); + LinEq.block(0, n_ + m_ + k_, n_, n_) = -1 * MatrixXd::Identity(n_, n_); + dynamics_constraints_.resize(N_); + target_cost_.resize(N_ + 1); + for (int i = 0; i < N_; i++) { + LinEq.block(0, 0, n_, n_) = A_.at(i); + LinEq.block(0, n_, n_, m_) = D_.at(i); + LinEq.block(0, n_ + m_, n_, k_) = B_.at(i); + + dynamics_constraints_[i] = + prog_ + .AddLinearEqualityConstraint( + LinEq, -lcs.d_.at(i), + {x_.at(i), lambda_.at(i), u_.at(i), x_.at(i + 1)}) + .evaluator() + .get(); + } + input_costs_.resize(N_); + for (int i = 0; i < N_ + 1; i++) { + target_cost_[i] = + prog_ + .AddQuadraticCost(2 * Q_.at(i), -2 * Q_.at(i) * x_desired_.at(i), + x_.at(i), 1) + .evaluator() + .get(); + if (i < N_) { + input_costs_[i] = + prog_.AddQuadraticCost(2 * R_.at(i), VectorXd::Zero(k_), u_.at(i), 1) + .evaluator(); + } + } +} + +void C3::UpdateCostMatrices(const C3::CostMatrices& costs) { + DRAKE_DEMAND(costs.Q.size() == N_ + 1); + DRAKE_DEMAND(costs.R.size() == N_); + DRAKE_DEMAND(costs.U.size() == N_); + DRAKE_DEMAND(costs.G.size() == N_); + DRAKE_DEMAND(costs.Q[0].rows() == n_); + DRAKE_DEMAND(costs.Q[0].cols() == n_); + DRAKE_DEMAND(costs.R[0].rows() == k_); + DRAKE_DEMAND(costs.R[0].cols() == k_); + DRAKE_DEMAND(costs.U[0].rows() == n_ + m_ + k_); + DRAKE_DEMAND(costs.U[0].cols() == n_ + m_ + k_); + DRAKE_DEMAND(costs.G[0].rows() == n_ + m_ + k_); + DRAKE_DEMAND(costs.G[0].cols() == n_ + m_ + k_); + Q_ = costs.Q; + R_ = costs.R; + U_ = costs.U; + G_ = costs.G; +} + +void C3::UpdateLCS(const LCS& lcs) { + DRAKE_DEMAND(lcs.A_.size() == N_); + DRAKE_DEMAND(lcs.A_[0].rows() == n_); + DRAKE_DEMAND(lcs.A_[0].cols() == n_); + DRAKE_DEMAND(lcs.D_[0].cols() == m_); + DRAKE_DEMAND(lcs.B_[0].cols() == k_); + // Update the stored LCS object. + lcs_ = lcs; + A_ = lcs.A_; + B_ = lcs.B_; + D_ = lcs.D_; + d_ = lcs.d_; + E_ = lcs.E_; + F_ = lcs.F_; + H_ = lcs.H_; + c_ = lcs.c_; + dt_ = lcs.dt_; + W_x_ = lcs.W_x_; + W_l_ = lcs.W_l_; + W_u_ = lcs.W_u_; + w_ = lcs.w_; + h_is_zero_ = H_[0].isZero(0); + + MatrixXd LinEq = MatrixXd::Zero(n_, 2 * n_ + m_ + k_); + LinEq.block(0, n_ + k_ + m_, n_, n_) = -1 * MatrixXd::Identity(n_, n_); + + auto Dn = D_.at(0).norm(); + auto An = A_.at(0).norm(); + AnDn_ = An / Dn; + + for (int i = 0; i < N_; ++i) { + D_.at(i) *= AnDn_; + E_.at(i) /= AnDn_; + c_.at(i) /= AnDn_; + H_.at(i) /= AnDn_; + } + + for (int i = 0; i < N_; i++) { + LinEq.block(0, 0, n_, n_) = A_.at(i); + LinEq.block(0, n_, n_, m_) = D_.at(i); + LinEq.block(0, n_ + m_, n_, k_) = B_.at(i); + + dynamics_constraints_[i]->UpdateCoefficients(LinEq, -lcs.d_.at(i)); + } +} + +void C3::UpdateCostLCS(const LCS& lcs_for_cost) { + DRAKE_DEMAND(lcs_for_cost.A_.size() == N_); + DRAKE_DEMAND(lcs_for_cost.A_[0].rows() == n_); + DRAKE_DEMAND(lcs_for_cost.A_[0].cols() == n_); + DRAKE_DEMAND(lcs_for_cost.B_[0].cols() == k_); + // Update the stored LCS object. + if (!lcs_for_cost_) { + lcs_for_cost_ = std::make_unique(lcs_for_cost); + }else{ + *lcs_for_cost_ = lcs_for_cost; + } +} + +void C3::UpdateTarget(const std::vector& x_des) { + x_desired_ = x_des; + for (int i = 0; i < N_ + 1; i++) { + target_cost_[i]->UpdateCoefficients(2 * Q_.at(i), + -2 * Q_.at(i) * x_desired_.at(i)); + } +} + +void C3::Solve(const VectorXd& x0, bool verbose) { + auto start = std::chrono::high_resolution_clock::now(); + + VectorXd delta_init = VectorXd::Zero(n_ + m_ + k_); + if (options_.delta_option == 1) { + delta_init.head(n_) = x0; + } + std::vector delta(N_, delta_init); + std::vector w(N_, VectorXd::Zero(n_ + m_ + k_)); + vector Gv = G_; + + for (int i = 0; i < N_; ++i) { + if (options_.penalize_changes_in_u_across_solves) { + // Penalize deviation from previous input solution: input cost is + // (u-u_prev)' * R * (u-u_prev). + input_costs_[i]->UpdateCoefficients(2 * R_.at(i), + -2 * R_.at(i) * u_sol_->at(i)); + } + else{ + // Penalize inputs: input cost is u' * R * u. + input_costs_[i]->UpdateCoefficients(2 * R_.at(i), + Eigen::VectorXd::Zero(k_)); + } + } + + for (int iter = 0; iter < options_.admm_iter; iter++) { + ADMMStep(x0, &delta, &w, &Gv, iter, verbose); + } + + vector WD(N_, VectorXd::Zero(n_ + m_ + k_)); + for (int i = 0; i < N_; i++) { + WD.at(i) = delta.at(i) - w.at(i); + } + + vector zfin = SolveQP(x0, Gv, WD, options_.admm_iter, true); + + if (verbose) { + std::cout << "x0: " << x0.transpose() << std::endl; + std::cout << "Final ADMM Iteration: " << options_.admm_iter << std::endl; + // Make matrix versions of variables for more compact printing. + Eigen::MatrixXd verbose_delta = Eigen::MatrixXd::Zero(n_ + m_ + k_, N_); + Eigen::MatrixXd verbose_w = Eigen::MatrixXd::Zero(n_ + m_ + k_, N_); + Eigen::MatrixXd verbose_zfin = Eigen::MatrixXd::Zero(n_ + m_ + k_, N_); + Eigen::MatrixXd verbose_zsol = Eigen::MatrixXd::Zero(n_ + m_ + k_, N_); + for(int i = 0; i < N_; i++) { + verbose_delta.col(i) = delta[i]; + verbose_w.col(i) = w[i]; + verbose_zfin.col(i) = zfin_[i]; + verbose_zsol.col(i) = z_sol_->at(i); + } + std::cout<<"zfin: \n"<at(0).segment(0, n_) = x0; + x_sol_->at(0) = x0; + for (int i = 1; i < N_; ++i) { + z_sol_->at(i).segment(0, n_) = + A_.at(i - 1) * x_sol_->at(i - 1) + B_.at(i - 1) * u_sol_->at(i - 1) + + D_.at(i - 1) * lambda_sol_->at(i - 1) + d_.at(i - 1); + } + } + + for (int i = 0; i < N_; ++i) { + lambda_sol_->at(i) *= AnDn_; + z_sol_->at(i).segment(n_, m_) *= AnDn_; + } + + zfin_ = zfin; + auto finish = std::chrono::high_resolution_clock::now(); + auto elapsed = finish - start; + solve_time_ = + std::chrono::duration_cast(elapsed).count() / + 1e6; +} + +// This function relies on the previously computed zfin_ from Solve. +std::pair> C3::CalcCost( + C3CostComputationType cost_type, + double Kp_for_ee_pd_rollout, + double Kd_for_ee_pd_rollout, + bool force_tracking_disabled, + bool print_cost_breakdown, + bool verbose) const { + vector UU(N_, VectorXd::Zero(k_)); + std::vector XX(N_+1, VectorXd::Zero(n_)); + + // Simulate the dynamics from the planned inputs. + if (cost_type == C3CostComputationType::kSimLCS) { + XX[0] = zfin_[0].segment(0, n_); + for (int i = 0; i < N_; i++) { + UU[i] = zfin_[i].segment(n_ + m_, k_); + if (lcs_for_cost_) { + XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); + } + else { + XX[i+1] = lcs_.Simulate(XX[i], UU[i]); + } + } + } + + // Use the C3 plan. + else if (cost_type == C3CostComputationType::kUseC3Plan) { + for (int i = 0; i < N_; i++) { + UU[i] = zfin_[i].segment(n_ + m_, k_); + XX[i] = zfin_[i].segment(0, n_); + if (i == N_-1) { + if (lcs_for_cost_) { + XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); + } + else { + XX[i+1] = lcs_.Simulate(XX[i], UU[i]); + } + } + } + } + + // Simulate the dynamics from the planned inputs only for the object; use the + // planned EE trajectory. + else if (cost_type == C3CostComputationType::kSimLCSReplaceC3EEPlan) { + // Simulate the object trajectory. + XX[0] = zfin_[0].segment(0, n_); + for (int i = 0; i < N_; i++) { + UU[i] = zfin_[i].segment(n_ + m_, k_); + if (lcs_for_cost_) { + XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); + } + else { + XX[i+1] = lcs_.Simulate(XX[i], UU[i]); + } + } + // Replace ee traj with those from zfin_. + for (int i = 0; i < N_; i++) { + XX[i].segment(0,3) = zfin_[i].segment(0,3); + if (i == N_-1) { + if (lcs_for_cost_) { + XX[i+1].segment(0,3) = + lcs_for_cost_->Simulate(XX[i], UU[i]).segment(0,3); + } + else { + XX[i+1].segment(0,3) = lcs_.Simulate(XX[i], UU[i]).segment(0,3); + } + } + } + } + + // Try to emulate the real cost of the system associated not only applying the + // planned u but also the u associated with tracking the position plan over + // time. + else if (cost_type == C3CostComputationType::kSimImpedance) { + std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, + Kd_for_ee_pd_rollout, + force_tracking_disabled, + verbose); + } + + // The same as the previous cost type except the EE states are replaced with + // the plan from C3 at the end. + else if (cost_type == C3CostComputationType::kSimImpedanceReplaceC3EEPlan) { + std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, + Kd_for_ee_pd_rollout, + force_tracking_disabled, + verbose); + + // Replace the end effector position and velocity plans with the ones from + // the C3 plan. + for(int i = 0; i < N_; i++) { + XX[i].segment(0,3) = zfin_[i].segment(0,3); + XX[i].segment(10,3) = zfin_[i].segment(10,3); + if (i == N_-1) { + XX[i+1].segment(0,3) = zfin_[i].segment(0,3); + XX[i+1].segment(10,3) = zfin_[i].segment(10,3); + } + } + } + + // The same as the previous cost type except only object terms contribute to + // the final cost. + else if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { + std::tie(XX, UU) = SimulatePDControl(Kp_for_ee_pd_rollout, + Kd_for_ee_pd_rollout, + force_tracking_disabled, + verbose); + } + + // Declare Q_eff and R_eff as the Q and R to use for cost computation. + std::vector Q_eff = Q_; + std::vector R_eff = R_; + + // Calculate the cost over the N+1 time steps. + double cost = 0; + + //used only for verbose mode printouts + double error_contrib_ee_pos = 0; + double error_contrib_obj_orientation = 0; + double error_contrib_obj_pos = 0; + double error_contrib_ee_vel = 0; + double error_contrib_obj_ang_vel = 0; + double error_contrib_obj_vel = 0; + + double cost_contrib_ee_pos = 0; + double cost_contrib_obj_orientation = 0; + double cost_contrib_obj_pos = 0; + double cost_contrib_ee_vel = 0; + double cost_contrib_obj_ang_vel = 0; + double cost_contrib_obj_vel = 0; + + // Calculate the error and cost contributions for each state. + for (int i = 0; i < N_; i++) { + //ee_pos + error_contrib_ee_pos += + (XX[i].segment(0,3) - x_desired_[i].segment(0,3)).transpose() * + (XX[i].segment(0,3) - x_desired_[i].segment(0,3)); + cost_contrib_ee_pos += + (XX[i].segment(0,3) - x_desired_[i].segment(0,3)).transpose() * + Q_eff.at(i).block(0,0,3,3)*(XX[i].segment(0,3) - + x_desired_[i].segment(0,3)); + //obj_orientation + error_contrib_obj_orientation += + (XX[i].segment(3,4) - x_desired_[i].segment(3,4)).transpose() * + (XX[i].segment(3,4) - x_desired_[i].segment(3,4)); + cost_contrib_obj_orientation += + (XX[i].segment(3,4) - x_desired_[i].segment(3,4)).transpose() * + Q_eff.at(i).block(3,3,4,4) * + (XX[i].segment(3,4) - x_desired_[i].segment(3,4)); + //obj_pos + error_contrib_obj_pos += + (XX[i].segment(7,3) - x_desired_[i].segment(7,3)).transpose() * + (XX[i].segment(7,3) - x_desired_[i].segment(7,3)); + cost_contrib_obj_pos += + (XX[i].segment(7,3) - x_desired_[i].segment(7,3)).transpose() * + Q_eff.at(i).block(7,7,3,3)*(XX[i].segment(7,3) - + x_desired_[i].segment(7,3)); + //ee_vel + error_contrib_ee_vel += + (XX[i].segment(10,3) - x_desired_[i].segment(10,3)).transpose() * + (XX[i].segment(10,3) - x_desired_[i].segment(10,3)); + cost_contrib_ee_vel += + (XX[i].segment(10,3) - x_desired_[i].segment(10,3)).transpose() * + Q_eff.at(i).block(10,10,3,3) * (XX[i].segment(10,3) - + x_desired_[i].segment(10,3)); + //obj_ang_vel + error_contrib_obj_ang_vel += + (XX[i].segment(13,3) - x_desired_[i].segment(13,3)).transpose() * + (XX[i].segment(13,3) - x_desired_[i].segment(13,3)); + cost_contrib_obj_ang_vel += + (XX[i].segment(13,3) - x_desired_[i].segment(13,3)).transpose() * + Q_eff.at(i).block(13,13,3,3)*(XX[i].segment(13,3) - + x_desired_[i].segment(13,3)); + //obj_vel + error_contrib_obj_vel += + (XX[i].segment(16,3) - x_desired_[i].segment(16,3)).transpose() * + (XX[i].segment(16,3) - x_desired_[i].segment(16,3)); + cost_contrib_obj_vel += + (XX[i].segment(16,3) - x_desired_[i].segment(16,3)).transpose() * + Q_eff.at(i).block(16,16,3,3)*(XX[i].segment(16,3) - + x_desired_[i].segment(16,3)); + + cost = cost + (XX[i] - x_desired_[i]).transpose() * + Q_eff.at(i)*(XX[i] - x_desired_[i]) + + UU[i].transpose()*R_eff.at(i)*UU[i]; + } + + // Handle the N_th state. + cost = cost + (XX[N_] - x_desired_[N_]).transpose()*Q_eff.at(N_)*( + XX[N_] - x_desired_[N_]); + + error_contrib_ee_pos += + (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)).transpose() * + (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)); + error_contrib_obj_orientation += + (XX[N_].segment(3,4) - x_desired_[N_].segment(3,4)).transpose() * + (XX[N_].segment(3,4) - x_desired_[N_].segment(3,4)); + error_contrib_obj_pos += + (XX[N_].segment(7,3) - x_desired_[N_].segment(7,3)).transpose() * + (XX[N_].segment(7,3) - x_desired_[N_].segment(7,3)); + error_contrib_ee_vel += + (XX[N_].segment(10,3) - x_desired_[N_].segment(10,3)).transpose() * + (XX[N_].segment(10,3) - x_desired_[N_].segment(10,3)); + error_contrib_obj_ang_vel += + (XX[N_].segment(13,3) - x_desired_[N_].segment(13,3)).transpose() * + (XX[N_].segment(13,3) - x_desired_[N_].segment(13,3)); + error_contrib_obj_vel += + (XX[N_].segment(16,3) - x_desired_[N_].segment(16,3)).transpose() * + (XX[N_].segment(16,3) - x_desired_[N_].segment(16,3)); + + cost_contrib_ee_pos += + (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)).transpose() * + Q_eff.at(N_).block(0,0,3,3) * + (XX[N_].segment(0,3) - x_desired_[N_].segment(0,3)); + cost_contrib_obj_orientation += + (XX[N_].segment(3,4) - x_desired_[N_].segment(3,4)).transpose() * + Q_eff.at(N_).block(3,3,4,4)*(XX[N_].segment(3,4) - + x_desired_[N_].segment(3,4)); + cost_contrib_obj_pos += + (XX[N_].segment(7,3) - x_desired_[N_].segment(7,3)).transpose() * + Q_eff.at(N_).block(7,7,3,3)*(XX[N_].segment(7,3) - + x_desired_[N_].segment(7,3)); + cost_contrib_ee_vel += + (XX[N_].segment(10,3) - x_desired_[N_].segment(10,3)).transpose() * + Q_eff.at(N_).block(10,10,3,3)*(XX[N_].segment(10,3) - + x_desired_[N_].segment(10,3)); + cost_contrib_obj_ang_vel += + (XX[N_].segment(13,3) - x_desired_[N_].segment(13,3)).transpose() * + Q_eff.at(N_).block(13,13,3,3)*(XX[N_].segment(13,3) - + x_desired_[N_].segment(13,3)); + cost_contrib_obj_vel += + (XX[N_].segment(16,3) - x_desired_[N_].segment(16,3)).transpose() * + Q_eff.at(N_).block(16,16,3,3)*(XX[N_].segment(16,3) - + x_desired_[N_].segment(16,3)); + + if (cost_type == C3CostComputationType::kSimImpedanceObjectCostOnly) { + cost = cost_contrib_obj_pos + cost_contrib_obj_orientation + + cost_contrib_obj_vel + cost_contrib_obj_ang_vel; + } + + if (verbose || print_cost_breakdown) { + std::cout<<"Error breakdown"<> ret (cost, XX); + return ret; +} + +std::pair, std::vector> + C3::SimulatePDControl( + double Kp_for_ee_pd_rollout, double Kd_for_ee_pd_rollout, + bool force_tracking_disabled, bool verbose) const + { + if (verbose) { + std::cout<<"\nSIMULATING PD CONTROL"< UU(N_, VectorXd::Zero(k_)); + std::vector XX(N_+1, VectorXd::Zero(n_)); + for (int i = 0; i < N_; i++) { + UU[i] = zfin_[i].segment(n_ + m_, k_); + XX[i] = zfin_[i].segment(0, n_); + if (i == N_-1) { + if (lcs_for_cost_) { + XX[i+1] = lcs_for_cost_->Simulate(XX[i], UU[i]); + } + else{ + XX[i+1] = lcs_.Simulate(XX[i], UU[i]); + } + } + } + + // Set the PD gains for the emulated tracking controller. + Eigen::MatrixXd Kp = Kp_for_ee_pd_rollout*Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd Kd = Kd_for_ee_pd_rollout*Eigen::MatrixXd::Identity(3,3); + + // Obtain modified solutions for the PD controller. + std::vector UU_new(N_, VectorXd::Zero(k_)); + std::vector XX_new(N_+1, VectorXd::Zero(n_)); + + XX_new[0] = zfin_[0].segment(0, n_); + // This will just be the original u from zfin_[0] for the first time step. + // if the radio input is true, then the u will only emulate position + // tracking using the PD controller. + for (int i = 0; i < N_; i++) { + if (force_tracking_disabled) { + UU_new[i] = Kp*(XX[i].segment(0, 3) - XX_new[i].segment(0, 3)) + + Kd*(XX[i].segment(10, 3) - XX_new[i].segment(10, 3)); + } + else{ + UU_new[i] = UU[i] + + Kp*(XX[i].segment(0, 3) - XX_new[i].segment(0, 3)) + + Kd*(XX[i].segment(10, 3) - XX_new[i].segment(10, 3)); + } + if (lcs_for_cost_) { + + if (verbose) { + std::cout<<"simulated step "<Simulate(XX_new[i], UU_new[i], verbose); + } + else{ + XX_new[i+1] = lcs_.Simulate(XX_new[i], UU_new[i]); + } + } + return {XX_new, UU_new}; +} + +void C3::ADMMStep(const VectorXd& x0, vector* delta, + vector* w, vector* Gv, + int admm_iteration, bool verbose) { + vector WD(N_, VectorXd::Zero(n_ + m_ + k_)); + + for (int i = 0; i < N_; i++) { + WD.at(i) = delta->at(i) - w->at(i); + } + + vector z = SolveQP(x0, *Gv, WD, admm_iteration, true); + if (verbose) { + std::cout << "SolveQP Iteration: " << admm_iteration << std::endl; + Eigen::MatrixXd verbose_z = Eigen::MatrixXd::Zero(n_ + m_ + k_, N_); + for(int i = 0; i < N_; i++) { + verbose_z.col(i) = z[i]; + } + std::cout<<"z: \n"< ZW(N_, VectorXd::Zero(n_ + m_ + k_)); + for (int i = 0; i < N_; i++) { + ZW[i] = w->at(i) + z[i]; + } + + if (U_[0].isZero(0)) { + *delta = SolveProjection(*Gv, ZW, admm_iteration); + + } else { + *delta = SolveProjection(U_, ZW, admm_iteration); + } + if (verbose) { + std::cout << "ADMM Iteration: " << admm_iteration << std::endl; + Eigen::MatrixXd verbose_delta = Eigen::MatrixXd::Zero(n_ + m_ + k_, N_); + for(int i = 0; i < N_; i++) { + verbose_delta.col(i) = delta->at(i); + } + std::cout<<"delta: \n"<at(i) = w->at(i) + z[i] - delta->at(i); + w->at(i) = w->at(i) / options_.rho_scale; + Gv->at(i) = Gv->at(i) * options_.rho_scale; + } +} + +vector C3::SolveQP(const VectorXd& x0, const vector& G, + const vector& WD, int admm_iteration, + bool is_final_solve) { + for (auto& constraint : constraints_) { + prog_.RemoveConstraint(constraint); + } + constraints_.clear(); + constraints_.push_back(prog_.AddLinearConstraint(x_[0] == x0)); + + if (h_is_zero_ == 1) { // No dependence on u, so just simulate passive system + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd lambda0; + LCPSolver.SolveLcpLemkeRegularized(F_[0], E_[0] * x0 + c_[0], &lambda0); + constraints_.push_back(prog_.AddLinearConstraint(lambda_[0] == lambda0)); + } + + for (auto& cost : costs_) { + prog_.RemoveCost(cost); + } + costs_.clear(); + + for (int i = 0; i < N_ + 1; i++) { + if (i < N_) { + costs_.push_back(prog_.AddQuadraticCost( + 2 * G.at(i).block(0, 0, n_, n_), + -2 * G.at(i).block(0, 0, n_, n_) * WD.at(i).segment(0, n_), x_.at(i), + 1)); + costs_.push_back(prog_.AddQuadraticCost( + 2 * G.at(i).block(n_, n_, m_, m_), + -2 * G.at(i).block(n_, n_, m_, m_) * WD.at(i).segment(n_, m_), + lambda_.at(i), 1)); + costs_.push_back( + prog_.AddQuadraticCost(2 * G.at(i).block(n_ + m_, n_ + m_, k_, k_), + -2 * G.at(i).block(n_ + m_, n_ + m_, k_, k_) * + WD.at(i).segment(n_ + m_, k_), + u_.at(i), 1)); + } + } + + // /// initialize decision variables to warm start + if (warm_start_) { + int index = solve_time_ / dt_; + double weight = (solve_time_ - index * dt_) / dt_; + for (int i = 0; i < N_ - 1; i++) { + prog_.SetInitialGuess(x_[i], + (1 - weight) * warm_start_x_[admm_iteration][i] + + weight * warm_start_x_[admm_iteration][i + 1]); + prog_.SetInitialGuess( + lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration][i] + + weight * warm_start_lambda_[admm_iteration][i + 1]); + prog_.SetInitialGuess(u_[i], + (1 - weight) * warm_start_u_[admm_iteration][i] + + weight * warm_start_u_[admm_iteration][i + 1]); + } + prog_.SetInitialGuess(x_[0], x0); + prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); + } + + MathematicalProgramResult result = osqp_.Solve(prog_); + + if (result.is_success()) { + for (int i = 0; i < N_; i++) { + if (is_final_solve) { + x_sol_->at(i) = result.GetSolution(x_[i]); + lambda_sol_->at(i) = result.GetSolution(lambda_[i]); + u_sol_->at(i) = result.GetSolution(u_[i]); + } + z_sol_->at(i).segment(0, n_) = result.GetSolution(x_[i]); + z_sol_->at(i).segment(n_, m_) = result.GetSolution(lambda_[i]); + z_sol_->at(i).segment(n_ + m_, k_) = result.GetSolution(u_[i]); + + if (warm_start_) { + // update warm start parameters + warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); + warm_start_lambda_[admm_iteration][i] = result.GetSolution(lambda_[i]); + warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]); + } + } + if (warm_start_) { + warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]); + } + } + else { + std::cout<<"CAUTION: C3 QP solve did not succeed" << std::endl; + } + + return *z_sol_; +} + +vector C3::SolveProjection(const vector& U, + vector& WZ, int admm_iteration) { + vector deltaProj(N_, VectorXd::Zero(n_ + m_ + k_)); + int i; + + if (options_.num_threads > 0) { + omp_set_dynamic(0); // Explicitly disable dynamic teams + omp_set_num_threads(options_.num_threads); // Set number of threads + // TODO: A newer version of OpenMP likely uses omp_set_max_active_levels + // instead of omp_set_nested. Consider updating this after testing. + omp_set_nested(1); // Enable nested parallelism (important for + // sampling_c3_controller) + } + +#pragma omp parallel for num_threads(options_.num_threads) + for (i = 0; i < N_; i++) { + if (options_.use_robust_formulation && + admm_iteration == + (options_.admm_iter - 1)) { // only on the last iteration + if (warm_start_) { + if (i == N_ - 1) { + deltaProj[i] = SolveRobustSingleProjection( + U[i], WZ[i], E_[i], F_[i], H_[i], c_[i], W_x_, W_l_, W_u_, w_, + admm_iteration, -1); + } else { + deltaProj[i] = SolveRobustSingleProjection( + U[i], WZ[i], E_[i], F_[i], H_[i], c_[i], W_x_, W_l_, W_u_, w_, + admm_iteration, i + 1); + } + } else { + deltaProj[i] = SolveRobustSingleProjection( + U[i], WZ[i], E_[i], F_[i], H_[i], c_[i], W_x_, W_l_, W_u_, w_, + admm_iteration, -1); + } + } else { + if (warm_start_) { + if (i == N_ - 1) { + deltaProj[i] = SolveSingleProjection(U[i], WZ[i], E_[i], F_[i], H_[i], + c_[i], admm_iteration, -1); + } else { + deltaProj[i] = SolveSingleProjection(U[i], WZ[i], E_[i], F_[i], H_[i], + c_[i], admm_iteration, i + 1); + } + } else { + deltaProj[i] = SolveSingleProjection(U[i], WZ[i], E_[i], F_[i], H_[i], + c_[i], admm_iteration, -1); + } + } + } + + return deltaProj; +} + +void C3::AddLinearConstraint(Eigen::RowVectorXd& A, double lower_bound, + double upper_bound, int constraint) { + if (constraint == 1) { + for (int i = 1; i < N_; i++) { + user_constraints_.push_back( + prog_.AddLinearConstraint(A, lower_bound, upper_bound, x_.at(i))); + } + } + + if (constraint == 2) { + for (int i = 0; i < N_; i++) { + user_constraints_.push_back( + prog_.AddLinearConstraint(A, lower_bound, upper_bound, u_.at(i))); + } + } + + if (constraint == 3) { + for (int i = 0; i < N_; i++) { + user_constraints_.push_back(prog_.AddLinearConstraint( + A, lower_bound, upper_bound, lambda_.at(i))); + } + } +} + +void C3::RemoveConstraints() { + for (auto& userconstraint : user_constraints_) { + prog_.RemoveConstraint(userconstraint); + } + user_constraints_.clear(); +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3.h b/solvers/c3.h new file mode 100644 index 0000000000..922da99fa5 --- /dev/null +++ b/solvers/c3.h @@ -0,0 +1,248 @@ +#pragma once + +#include + +#include + +#include "solvers/c3_options.h" +#include "solvers/fast_osqp_solver.h" +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +namespace dairlib { +namespace solvers { + +class C3 { + public: + struct CostMatrices { + CostMatrices() = default; + CostMatrices(const std::vector& Q, + const std::vector& R, + const std::vector& G, + const std::vector& U); + std::vector Q; + std::vector R; + std::vector G; + std::vector U; + }; + /// @param LCS LCS parameters + /// @param costs Cost Matrices + /// @param x_des Desired goal state + /// @param options C3 options + C3(const LCS& LCS, const CostMatrices& costs, + const std::vector& x_des, const C3Options& options); + + virtual ~C3() = default; + + /// Solve the MPC problem. + /// @param x0 The initial state of the system + /// @param verbose Whether to print additional information + /// @return void + void Solve(const Eigen::VectorXd& x0, bool verbose = false); + + /// Compute the MPC cost, using previously solved MPC solution. + /// @param cost_type The method of computing the cost + /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control + /// used for some of the cost types + /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control + /// used for some of the cost types + /// @param force_tracking_disabled Whether to simulate EE PD control with + /// feedforward u from the MPC solution + /// @param print_cost_breakdown Whether to print the cost breakdown + /// @param verbose Whether to print additional information + /// @return The cost and its associated state trajectory + std::pair> CalcCost( + C3CostComputationType cost_type = kSimLCSReplaceC3EEPlan, + double Kp_for_ee_pd_rollout = 0.0, double Kd_for_ee_pd_rollout = 0.0, + bool force_tracking_disabled = false, bool print_cost_breakdown = false, + bool verbose = false) const; + + /// Helper function to simulate the dynamics with PD control on the EE + /// location and velocity plans, and the control input plans. Used for cost + /// types that simulate the impedance control. + /// @param Kp_for_ee_pd_rollout Proportional gain for simulated EE PD control + /// @param Kd_for_ee_pd_rollout Derivative gain for simulated EE PD control + /// @param force_tracking_disabled Whether to simulate EE PD control with + /// feedforward u from the MPC solution + /// @param verbose Whether to print additional information + /// @return the simulated state and input trajectories + std::pair, std::vector> + SimulatePDControl(double Kp_for_ee_pd_rollout = 0.0, + double Kd_for_ee_pd_rollout = 0.0, + bool force_tracking_disabled = false, + bool verbose = false) const; + + /// Solve a single ADMM step. + /// @param x0 The initial state of the system + /// @param delta The copy variables from the previous step + /// @param w The scaled dual variables from the previous step + /// @param G A pointer to the G variables from previous step + /// @param admm_iteration ADMM iteration for accurate warm starting + /// @param verbose Whether to print additional information + /// @return solution is saved in C3 object + void ADMMStep(const Eigen::VectorXd& x0, std::vector* delta, + std::vector* w, + std::vector* G, int admm_iteration, + bool verbose = false); + + /// Solve a single QP. + /// @param x0 The initial state of the system + /// @param G A pointer to the G variables from previous step + /// @param WD A pointer to the (w - delta) variables + /// @param admm_iteration ADMM iteration for accurate warm starting + /// @param is_final_solve Indicating final admm iteration in case of any + /// polishing steps + /// @return z MPC solution + std::vector SolveQP(const Eigen::VectorXd& x0, + const std::vector& G, + const std::vector& WD, + int admm_iteration, + bool is_final_solve = false); + + /// Solve the projection problem for all timesteps. + /// @param U Matrix for consensus cost + /// @param WZ (z + w) variables + /// @param admm_iteration ADMM iteration for accurate warm starting + std::vector SolveProjection( + const std::vector& U, std::vector& WZ, + int admm_iteration); + + /// allow users to add constraints (adds for all timesteps) + /// @param A, lower_bound, upper_bound lower_bound <= A^T x <= upper_bound + /// @param constraint inputconstraint, stateconstraint, forceconstraint + void AddLinearConstraint(Eigen::RowVectorXd& A, double lower_bound, + double upper_bound, int constraint); + + /// remove all constraints + void RemoveConstraints(); + + /// Solve a projection step for a single knot point k. + /// @param U Matrix for consensus cost + /// @param delta_c A pointer to the copy of (z + w) variables + /// @param E, F, H, c LCS contact parameters + /// @param admm_iteration ADMM iteration for accurate warm starting + /// @param warm_start_index knot point index for warm starting + /// @return delta_k + virtual Eigen::VectorXd SolveSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const int admm_iteration, const int& warm_start_index) = 0; + + /// Solve a robust (friction cone) projection step for a single knot point k. + /// @param U Matrix for consensus cost + /// @param delta_c A pointer to the copy of (z + w) variables + /// @param E, F, H, c LCS contact parameters + /// @param W_x, W_l, W_u, w Linearization of J_t v_{k+1} wrt x_k, lambda_k, + /// u_k + /// @param admm_iteration ADMM iteration for accurate warm starting + /// @param warm_start_index knot point index for warm starting + /// @return delta_k + virtual Eigen::VectorXd SolveRobustSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index) = 0; + + /// Solve a robust (friction cone) projection step for a single knot point + void SetOsqpSolverOptions(const drake::solvers::SolverOptions& options) { + prog_.SetSolverOptions(options); + } + + std::vector GetFullSolution() { return *z_sol_; } + std::vector GetStateSolution() { return *x_sol_; } + std::vector GetForceSolution() { return *lambda_sol_; } + std::vector GetInputSolution() { return *u_sol_; } + std::vector GetDualDeltaSolution() { return *delta_sol_; } + std::vector GetDualWSolution() { return *w_sol_; } + + public: + void UpdateCostMatrices(const C3::CostMatrices& costs); + void UpdateLCS(const LCS& lcs); + + /// Update the LCS used for cost computation. This can differ from the LCS + /// used for the solve, e.g. if more contacts are used for cost than for + /// computing the plan. NOTE: This does not update the internal cost + /// matrices used for the solve (Q_, R_, G_, U_). Those are updated via + /// UpdateCostMatrices. + void UpdateCostLCS(const LCS& lcs_for_cost); + void UpdateTarget(const std::vector& x_des); + + protected: + std::vector> warm_start_delta_; + std::vector> warm_start_binary_; + std::vector> warm_start_x_; + std::vector> warm_start_lambda_; + std::vector> warm_start_u_; + bool warm_start_; + const int N_; + const int n_; // n_x + const int m_; // n_lambda + const int k_; // n_u + const C3Options options_; + + private: + // TODO: storing the LCS as a class variable makes the LCS matrices + // redundant. Could consider removing LCS matrices as class variables. + mutable LCS lcs_; + std::unique_ptr lcs_for_cost_; + std::vector A_; + std::vector B_; + std::vector D_; + std::vector d_; + std::vector E_; + std::vector F_; + std::vector H_; + std::vector c_; + Eigen::MatrixXd W_x_; + Eigen::MatrixXd W_l_; + Eigen::MatrixXd W_u_; + Eigen::VectorXd w_; + double AnDn_ = 1.0; + std::vector Q_; + std::vector R_; + std::vector U_; + std::vector G_; + std::vector x_desired_; + double dt_ = 0; + double solve_time_ = 0; + bool h_is_zero_; + + /// MathematicalProgram for QP step + drake::solvers::OsqpSolver osqp_; + drake::solvers::MathematicalProgram prog_; + /// Decision variables for QP step + std::vector x_; + std::vector u_; + std::vector lambda_; + /// QP step constraints + std::vector dynamics_constraints_; + // initial state constraint + std::vector> + constraints_; + // workspace and input limit constraints + std::vector> + user_constraints_; + /// QP step costs + std::vector target_cost_; + std::vector> costs_; + std::vector> input_costs_; + + // Solutions + mutable std::vector zfin_; + std::unique_ptr> x_sol_; + std::unique_ptr> lambda_sol_; + std::unique_ptr> u_sol_; + + std::unique_ptr> z_sol_; + std::unique_ptr> delta_sol_; + std::unique_ptr> w_sol_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_miqp.cc b/solvers/c3_miqp.cc new file mode 100644 index 0000000000..e8448cbd0d --- /dev/null +++ b/solvers/c3_miqp.cc @@ -0,0 +1,297 @@ +#include "solvers/c3_miqp.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +C3MIQP::C3MIQP(const LCS& LCS, const CostMatrices& costs, + const vector& xdesired, const C3Options& options) + : C3(LCS, costs, xdesired, options), env_(true) { + // Create an environment + env_.set("LogToConsole", "0"); + env_.set("OutputFlag", "0"); + env_.set("Threads", "5"); + env_.start(); +} + +VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, + const VectorXd& delta_c, + const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, + const int admm_iteration, + const int& warm_start_index) { + // set up linear term in cost + VectorXd cost_lin = -2 * delta_c.transpose() * U; + + // set up for constraints (Ex + F \lambda + Hu + c >= 0) + MatrixXd Mcons1(m_, n_ + m_ + k_); + Mcons1 << E, F, H; + + // set up for constraints (\lambda >= 0) + MatrixXd MM1 = MatrixXd::Zero(m_, n_); + MatrixXd MM2 = MatrixXd::Identity(m_, m_); + MatrixXd MM3 = MatrixXd::Zero(m_, k_); + MatrixXd Mcons2(m_, n_ + m_ + k_); + Mcons2 << MM1, MM2, MM3; + + GRBModel model = GRBModel(env_); + + GRBVar delta_k[n_ + m_ + k_]; + GRBVar binary[m_]; + + for (int i = 0; i < m_; i++) { + binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); + if (warm_start_index != -1) { + binary[i].set(GRB_DoubleAttr_Start, + warm_start_binary_[admm_iteration][warm_start_index](i)); + } + } + + for (int i = 0; i < n_ + m_ + k_; i++) { + delta_k[i] = model.addVar(-100.0, 100.0, 0.0, GRB_CONTINUOUS); + if (warm_start_index != -1) { + delta_k[i].set(GRB_DoubleAttr_Start, + warm_start_delta_[admm_iteration][warm_start_index](i)); + } + } + + GRBQuadExpr obj = 0; + + for (int i = 0; i < n_ + m_ + k_; i++) { + obj.addTerm(cost_lin(i), delta_k[i]); + obj.addTerm(U(i, i), delta_k[i], delta_k[i]); + } + + model.setObjective(obj, GRB_MINIMIZE); + + // initial state constraint +// if (warm_start_index == 0){ +// for (int i = 0; i < n_; ++i){ +// model.addConstr(delta_k[i] == delta_c[i]); +// } +// } + + int M = 100000; // big M variable + double coeff[n_ + m_ + k_]; + double coeff2[n_ + m_ + k_]; + + for (int i = 0; i < m_; i++) { + GRBLinExpr lambda_expr = 0; + + /// convert VectorXd to double + for (int j = 0; j < n_ + m_ + k_; j++) { + coeff[j] = Mcons2(i, j); + } + + lambda_expr.addTerms(coeff, delta_k, n_ + m_ + k_); + model.addConstr(lambda_expr >= 0); + model.addConstr(lambda_expr <= M * (1 - binary[i])); + + GRBLinExpr activation_expr = 0; + + /// convert VectorXd to double + for (int j = 0; j < n_ + m_ + k_; j++) { + coeff2[j] = Mcons1(i, j); + } + + activation_expr.addTerms(coeff2, delta_k, n_ + m_ + k_); + model.addConstr(activation_expr + c(i) >= 0); + model.addConstr(activation_expr + c(i) <= M * binary[i]); + } + + model.optimize(); + + VectorXd delta_kc(n_ + m_ + k_); + VectorXd binaryc(m_); + for (int i = 0; i < n_ + m_ + k_; i++) { + delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X); + } + for (int i = 0; i < m_; i++) { + binaryc(i) = binary[i].get(GRB_DoubleAttr_X); + } + + if (warm_start_index != -1) { + warm_start_delta_[admm_iteration][warm_start_index] = delta_kc; + warm_start_binary_[admm_iteration][warm_start_index] = binaryc; + } + return delta_kc; +} + +VectorXd C3MIQP::SolveRobustSingleProjection( + const MatrixXd& U, const VectorXd& delta_c, const MatrixXd& E, + const MatrixXd& F, const MatrixXd& H, const VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index) { + // set up linear term in cost + VectorXd cost_lin = -2 * delta_c.transpose() * U; + + // set up for constraints (Ex + F \lambda + Hu + c >= 0) + MatrixXd Mcons1(m_, n_ + m_ + k_); + Mcons1 << E, F, H; + + // set up for constraints (\lambda >= 0) + MatrixXd MM1 = MatrixXd::Zero(m_, n_); + MatrixXd MM2 = MatrixXd::Identity(m_, m_); + MatrixXd MM3 = MatrixXd::Zero(m_, k_); + MatrixXd Mcons2(m_, n_ + m_ + k_); + Mcons2 << MM1, MM2, MM3; + + GRBModel model = GRBModel(env_); + + GRBVar delta_k[n_ + m_ + k_]; + GRBVar binary[m_]; + GRBVar reduced_friction_cone_binary[m_]; + + for (int i = 0; i < m_; i++) { + binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); + if (warm_start_index != -1) { + binary[i].set(GRB_DoubleAttr_Start, + warm_start_binary_[admm_iteration][warm_start_index](i)); + } + } + + for (int i = 0; i < n_ + m_ + k_; i++) { + delta_k[i] = model.addVar(-100.0, 100.0, 0.0, GRB_CONTINUOUS); + if (warm_start_index != -1) { + delta_k[i].set(GRB_DoubleAttr_Start, + warm_start_delta_[admm_iteration][warm_start_index](i)); + } + } + + GRBQuadExpr obj = 0; + + for (int i = 0; i < n_ + m_ + k_; i++) { + obj.addTerm(cost_lin(i), delta_k[i]); + obj.addTerm(U(i, i), delta_k[i], delta_k[i]); + } + + // initial state constraint + if (warm_start_index == 0){ + for (int i = 0; i < n_; ++i){ + model.addConstr(delta_k[i] == delta_c[i]); + } + } + + + model.setObjective(obj, GRB_MINIMIZE); + + int M = 1000; // big M variable + double coeff[n_ + m_ + k_]; + double coeff2[n_ + m_ + k_]; + + for (int i = 0; i < m_; i++) { + GRBLinExpr lambda_expr = 0; + + /// convert VectorXd to double + for (int j = 0; j < n_ + m_ + k_; j++) { + coeff[j] = Mcons2(i, j); + } + + lambda_expr.addTerms(coeff, delta_k, n_ + m_ + k_); + model.addConstr(lambda_expr >= 0); + model.addConstr(lambda_expr <= M * (1 - binary[i])); + + GRBLinExpr activation_expr = 0; + + /// convert VectorXd to double + for (int j = 0; j < n_ + m_ + k_; j++) { + coeff2[j] = Mcons1(i, j); + } + + activation_expr.addTerms(coeff2, delta_k, n_ + m_ + k_); + model.addConstr(activation_expr + c(i) >= 0); + model.addConstr(activation_expr + c(i) <= M * binary[i]); + } + + double* tangential_velocity_coeffs; + MatrixXd P_t(m_, n_ + m_ + k_); + int constraint_rows = m_; + P_t << W_x, W_l, W_u; +// std::cout << "W_x: " << W_x << std::endl; +// std::cout << "W_l: " << W_l << std::endl; +// std::cout << "W_u: " << W_u << std::endl; + // stewart and trinkle +// P_t << E, F, H; +// MatrixXd P_t2 = P_t.bottomRows(((m_ / 6) * 4) / 7 * 3); +// P_t = P_t2; +// constraint_rows = P_t2.rows(); + + for (int i = 0; i < constraint_rows; ++i) { + reduced_friction_cone_binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); + GRBLinExpr tangential_velocity_expr = 0; + tangential_velocity_coeffs = P_t.row(i).data(); + tangential_velocity_expr.addTerms(tangential_velocity_coeffs, delta_k, + n_ + m_ + k_); + + /// Constraint explanation: + /// Adding a binary decision to either be: + /// - inside a conservative (mu_l) friction cone (stick) + /// or + /// - on the boundary of the original friction cone (mu) (slip) + /// where mu_l < mu + + /// The active constraint for the sticking condition is: + /// lambda_1 - lambda_2 <= mu_l / mu * (lambda_1 + lambda_2) + + /// The active constraint for the friction cone boundary is: + /// lambda_2 == 0 + /// where lambda_2 is the friction force in the sliding direction + /// tangential velocity expr is the tangential velocity in the SAME direction as the friction force + if (i % 2 == 1) { + model.addConstr(delta_k[n_ + i - 1] - delta_k[n_ + i] <= + (0.5 / 0.6) * (delta_k[n_ + i] + delta_k[n_ + i - 1]) + + M * (reduced_friction_cone_binary[i])); + model.addConstr(tangential_velocity_expr <= + M * (reduced_friction_cone_binary[i])); + model.addConstr(delta_k[n_ + i] <= + M * (1 - reduced_friction_cone_binary[i])); + } else { + model.addConstr(delta_k[n_ + i + 1] - delta_k[n_ + i] <= + (0.5 / 0.6) * (delta_k[n_ + i] + delta_k[n_ + i + 1]) + + M * (reduced_friction_cone_binary[i])); + model.addConstr(tangential_velocity_expr <= + M * (reduced_friction_cone_binary[i])); + model.addConstr(delta_k[n_ + i] <= + M * (1 - reduced_friction_cone_binary[i])); + } + /// if i % 2 == 0: + /// lambda_1 = 0 + /// else: + /// lambda_2 = 0 +// model.addConstr(delta_k[n_ + i] <= +// M * (1 - reduced_friction_cone_binary[i])); + } + + model.optimize(); + + VectorXd delta_kc(n_ + m_ + k_); + VectorXd binaryc(m_); + for (int i = 0; i < n_ + m_ + k_; i++) { + delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X); + } + for (int i = 0; i < m_; i++) { + binaryc(i) = binary[i].get(GRB_DoubleAttr_X); + } + + if (warm_start_index != -1) { + warm_start_delta_[admm_iteration][warm_start_index] = delta_kc; + warm_start_binary_[admm_iteration][warm_start_index] = binaryc; + } + + return delta_kc; +} + +std::vector C3MIQP::GetWarmStartDelta() const { + return warm_start_delta_[0]; +} + +std::vector C3MIQP::GetWarmStartBinary() const { + return warm_start_binary_[0]; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_miqp.h b/solvers/c3_miqp.h new file mode 100644 index 0000000000..0a80f70148 --- /dev/null +++ b/solvers/c3_miqp.h @@ -0,0 +1,47 @@ +#pragma once + +#include + +#include + +#include "gurobi_c++.h" +#include "solvers/c3.h" +#include "solvers/lcs.h" + +namespace dairlib { +namespace solvers { + +class C3MIQP final : public C3 { + public: + /// Default constructor for time-varying LCS + C3MIQP(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, const C3Options& options); + + ~C3MIQP() override = default; + + /// Virtual projection method + Eigen::VectorXd SolveSingleProjection(const Eigen::MatrixXd& U, + const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, + const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, + const Eigen::VectorXd& c, + const int admm_iteration, + const int& warm_start_index = -1) override; + /// Robust projection method + Eigen::VectorXd SolveRobustSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index = -1) override; + std::vector GetWarmStartDelta() const; + std::vector GetWarmStartBinary() const; + + private: + GRBEnv env_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_options.h b/solvers/c3_options.h new file mode 100644 index 0000000000..e79fd6db6f --- /dev/null +++ b/solvers/c3_options.h @@ -0,0 +1,190 @@ +#pragma once +#include + +#include "drake/common/yaml/yaml_read_archive.h" + + +/* Ways of computing C3 costs after solving the MPC problem: + 0. kSimLCS: Simulate the LCS dynamics from the planned + inputs. + 1. kUseC3Plan: Use the C3 planned trajectory and inputs. + 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned + inputs only for the object; use the planned + EE trajectory. + 3. kSimImpedance: Try to emulate the real cost of the system + associated not only applying the planned + inputs, but also tracking the planned EE + trajectory with an impedance controller. + 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE + states are replaced with the plan from C3 at + the end. + 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the + object terms contribute to the final cost. +*/ +enum C3CostComputationType { + kSimLCS, + kUseC3Plan, + kSimLCSReplaceC3EEPlan, + kSimImpedance, + kSimImpedanceReplaceC3EEPlan, + kSimImpedanceObjectCostOnly, +}; + +struct C3Options { + // Hyperparameters + int admm_iter; // total number of ADMM iterations + float rho; // initial value of the rho parameter + float rho_scale; // scaling of rho parameter (/rho = rho_scale * /rho) + int num_threads; // 0 is dynamic, greater than 0 for a fixed count + int delta_option; // different options for delta update + std::string projection_type; + std::string contact_model; + bool warm_start; + bool use_predicted_x0; + bool end_on_qp_step; + bool use_robust_formulation; + double solve_time_filter_alpha; + double publish_frequency; + + std::vector u_horizontal_limits; + std::vector u_vertical_limits; + std::vector workspace_limits; + double workspace_margins; + + int N; + double gamma; + + double w_Q; + double w_R; + double w_G; + double w_U; + + std::vector q_vector; + std::vector r_vector; + + std::vector g_vector; + std::vector g_x; + std::vector g_gamma; + std::vector g_lambda_n; + std::vector g_lambda_t; + std::vector g_lambda; + std::vector g_u; + + std::vector u_vector; + std::vector u_x; + std::vector u_gamma; + std::vector u_lambda_n; + std::vector u_lambda_t; + std::vector u_lambda; + std::vector u_u; + + double qp_projection_alpha; + double qp_projection_scaling; + bool penalize_changes_in_u_across_solves; + std::vector mu; + double dt; + double solve_dt; // unused + int num_friction_directions; + int num_contacts; + Eigen::MatrixXd Q; + Eigen::MatrixXd R; + Eigen::MatrixXd G; + Eigen::MatrixXd U; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(admm_iter)); + a->Visit(DRAKE_NVP(rho)); + a->Visit(DRAKE_NVP(rho_scale)); + a->Visit(DRAKE_NVP(num_threads)); + a->Visit(DRAKE_NVP(delta_option)); + a->Visit(DRAKE_NVP(contact_model)); + a->Visit(DRAKE_NVP(projection_type)); + if (projection_type == "QP") { + DRAKE_DEMAND(contact_model == "anitescu"); + } + a->Visit(DRAKE_NVP(warm_start)); + a->Visit(DRAKE_NVP(use_predicted_x0)); + a->Visit(DRAKE_NVP(end_on_qp_step)); + a->Visit(DRAKE_NVP(use_robust_formulation)); + a->Visit(DRAKE_NVP(solve_time_filter_alpha)); + a->Visit(DRAKE_NVP(publish_frequency)); + + a->Visit(DRAKE_NVP(workspace_limits)); + a->Visit(DRAKE_NVP(u_horizontal_limits)); + a->Visit(DRAKE_NVP(u_vertical_limits)); + a->Visit(DRAKE_NVP(workspace_margins)); + + a->Visit(DRAKE_NVP(mu)); + a->Visit(DRAKE_NVP(dt)); + a->Visit(DRAKE_NVP(solve_dt)); + a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(num_contacts)); + + a->Visit(DRAKE_NVP(N)); + a->Visit(DRAKE_NVP(gamma)); + a->Visit(DRAKE_NVP(w_Q)); + a->Visit(DRAKE_NVP(w_R)); + a->Visit(DRAKE_NVP(w_G)); + a->Visit(DRAKE_NVP(w_U)); + a->Visit(DRAKE_NVP(q_vector)); + a->Visit(DRAKE_NVP(r_vector)); + a->Visit(DRAKE_NVP(g_x)); + a->Visit(DRAKE_NVP(g_gamma)); + a->Visit(DRAKE_NVP(g_lambda_n)); + a->Visit(DRAKE_NVP(g_lambda_t)); + a->Visit(DRAKE_NVP(g_lambda)); + a->Visit(DRAKE_NVP(g_u)); + a->Visit(DRAKE_NVP(u_x)); + a->Visit(DRAKE_NVP(u_gamma)); + a->Visit(DRAKE_NVP(u_lambda_n)); + a->Visit(DRAKE_NVP(u_lambda_t)); + a->Visit(DRAKE_NVP(u_lambda)); + a->Visit(DRAKE_NVP(u_u)); + + a->Visit(DRAKE_NVP(qp_projection_alpha)); + a->Visit(DRAKE_NVP(qp_projection_scaling)); + a->Visit(DRAKE_NVP(penalize_changes_in_u_across_solves)); + + g_vector = std::vector(); + g_vector.insert(g_vector.end(), g_x.begin(), g_x.end()); + if (contact_model == "stewart_and_trinkle") { + g_vector.insert(g_vector.end(), g_gamma.begin(), g_gamma.end()); + g_vector.insert(g_vector.end(), g_lambda_n.begin(), g_lambda_n.end()); + g_vector.insert(g_vector.end(), g_lambda_t.begin(), g_lambda_t.end()); + } else { + g_vector.insert(g_vector.end(), g_lambda.begin(), g_lambda.end()); + } + + g_vector.insert(g_vector.end(), g_u.begin(), g_u.end()); + u_vector = std::vector(); + u_vector.insert(u_vector.end(), u_x.begin(), u_x.end()); + if (contact_model == "stewart_and_trinkle") { + u_vector.insert(u_vector.end(), u_gamma.begin(), u_gamma.end()); + u_vector.insert(u_vector.end(), u_lambda_n.begin(), u_lambda_n.end()); + u_vector.insert(u_vector.end(), u_lambda_t.begin(), u_lambda_t.end()); + } else { + u_vector.insert(u_vector.end(), u_lambda.begin(), u_lambda.end()); + } + u_vector.insert(u_vector.end(), u_u.begin(), u_u.end()); + + Eigen::VectorXd q = Eigen::Map( + this->q_vector.data(), this->q_vector.size()); + Eigen::VectorXd r = Eigen::Map( + this->r_vector.data(), this->r_vector.size()); + Eigen::VectorXd g = Eigen::Map( + this->g_vector.data(), this->g_vector.size()); + Eigen::VectorXd u = Eigen::Map( + this->u_vector.data(), this->u_vector.size()); + + DRAKE_DEMAND(g_lambda.size() == num_contacts * num_friction_directions*2); + DRAKE_DEMAND(u_lambda.size() == num_contacts * num_friction_directions*2); + DRAKE_DEMAND(mu.size() == num_contacts); + DRAKE_DEMAND(g.size() == u.size()); + + Q = w_Q * q.asDiagonal(); + R = w_R * r.asDiagonal(); + G = w_G * g.asDiagonal(); + U = w_U * u.asDiagonal(); + } +}; \ No newline at end of file diff --git a/solvers/c3_output.cc b/solvers/c3_output.cc new file mode 100644 index 0000000000..2f68be4b3d --- /dev/null +++ b/solvers/c3_output.cc @@ -0,0 +1,81 @@ +#include "c3_output.h" + +using Eigen::VectorXd; +using Eigen::VectorXf; +using std::vector; + +namespace dairlib { + +C3Output::C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates) + : c3_solution_(c3_sol), c3_intermediates_(c3_intermediates) {} + +lcmt_c3_output C3Output::GenerateLcmObject(double time) const { + lcmt_c3_output c3_output; + lcmt_c3_solution c3_solution; + lcmt_c3_intermediates c3_intermediates; + c3_output.utime = static_cast(1e6 * time); + + c3_solution.num_points = c3_solution_.time_vector_.size(); + int knot_points = c3_solution.num_points; + c3_solution.num_state_variables = c3_solution_.x_sol_.rows(); + c3_solution.num_contact_variables = c3_solution_.lambda_sol_.rows(); + c3_solution.num_input_variables = c3_solution_.u_sol_.rows(); + c3_solution.time_vec.reserve(knot_points); + c3_solution.x_sol = vector>(c3_solution.num_state_variables, + vector(knot_points)); + c3_solution.lambda_sol = vector>( + c3_solution.num_contact_variables, vector(knot_points)); + c3_solution.u_sol = vector>(c3_solution.num_input_variables, + vector(knot_points)); + + c3_solution.time_vec = vector( + c3_solution_.time_vector_.data(), + c3_solution_.time_vector_.data() + c3_solution_.time_vector_.size()); + + c3_intermediates.num_total_variables = c3_intermediates_.delta_.rows(); + c3_intermediates.num_points = c3_solution.num_points; + c3_intermediates.time_vec.reserve(c3_intermediates.num_points); + c3_intermediates.z_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.delta_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.w_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.time_vec = c3_solution.time_vec; + + for (int i = 0; i < c3_solution.num_state_variables; ++i) { + VectorXf temp_row = c3_solution_.x_sol_.row(i); + memcpy(c3_solution.x_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_solution.num_contact_variables; ++i) { + VectorXf temp_row = c3_solution_.lambda_sol_.row(i); + memcpy(c3_solution.lambda_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_solution.num_input_variables; ++i) { + VectorXf temp_row = c3_solution_.u_sol_.row(i); + memcpy(c3_solution.u_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_intermediates.num_total_variables; ++i) { + VectorXf temp_z_row = c3_intermediates_.z_.row(i); + VectorXf temp_delta_row = c3_intermediates_.delta_.row(i); + VectorXf temp_w_row = c3_intermediates_.w_.row(i); + memcpy(c3_intermediates.z_sol[i].data(), temp_z_row.data(), + sizeof(float) * knot_points); + memcpy(c3_intermediates.delta_sol[i].data(), temp_delta_row.data(), + sizeof(float) * knot_points); + memcpy(c3_intermediates.w_sol[i].data(), temp_w_row.data(), + sizeof(float) * knot_points); + } + + c3_output.c3_solution = c3_solution; + // Not assigning to reduce space +// c3_output.c3_intermediates = lcmt_c3_intermediates(); + c3_output.c3_intermediates = c3_intermediates; + + return c3_output; +} + +} // namespace dairlib diff --git a/solvers/c3_output.h b/solvers/c3_output.h new file mode 100644 index 0000000000..6e4f574211 --- /dev/null +++ b/solvers/c3_output.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "dairlib/lcmt_c3_output.hpp" + +namespace dairlib { + +/// Used for outputting c3 solutions and intermediate variables for debugging purposes + +class C3Output { + public: + struct C3Solution { + C3Solution() = default; + + // Shape is (variable_vector_size, knot_points) + Eigen::VectorXf time_vector_; + Eigen::MatrixXf x_sol_; + Eigen::MatrixXf lambda_sol_; + Eigen::MatrixXf u_sol_; + }; + + struct C3Intermediates { + C3Intermediates() = default; + + // Shape is (variable_vector_size, knot_points) + Eigen::VectorXf time_vector_; + Eigen::MatrixXf z_; + Eigen::MatrixXf delta_; + Eigen::MatrixXf w_; + }; + + C3Output() = default; + C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates); + + explicit C3Output(const lcmt_c3_output& traj); + + virtual ~C3Output() = default; + + lcmt_c3_output GenerateLcmObject(double time) const; + + private: + C3Solution c3_solution_; + C3Intermediates c3_intermediates_; +}; + +} // namespace dairlib diff --git a/solvers/c3_qp.cc b/solvers/c3_qp.cc new file mode 100644 index 0000000000..686ec1e6df --- /dev/null +++ b/solvers/c3_qp.cc @@ -0,0 +1,114 @@ +#include "solvers/c3_qp.h" + +#include + +#include + +#include "solvers/c3_options.h" +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +using drake::solvers::MathematicalProgram; +using drake::solvers::MathematicalProgramResult; +using drake::solvers::SolutionResult; +using drake::solvers::SolverOptions; + +using drake::solvers::OsqpSolver; +using drake::solvers::OsqpSolverDetails; +using drake::solvers::Solve; + +C3QP::C3QP(const LCS& LCS, const CostMatrices& costs, + const vector& xdesired, const C3Options& options) + : C3(LCS, costs, xdesired, options) {} + +VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, + const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, + const int admm_iteration, + const int& warm_start_index) { + drake::solvers::MathematicalProgram prog; + drake::solvers::SolverOptions solver_options; + drake::solvers::OsqpSolver osqp_; + + auto xn_ = prog.NewContinuousVariables(n_, "x"); + auto ln_ = prog.NewContinuousVariables(m_, "lambda"); + auto un_ = prog.NewContinuousVariables(k_, "u"); + + // As alpha -> 0, the complimentarity condition is enforced more strictly. + double alpha = options_.qp_projection_alpha; + double scaling = options_.qp_projection_scaling; + + MatrixXd EFH = MatrixXd::Zero(m_, n_ + m_ + k_); + EFH.block(0, 0, m_, n_) = E / scaling; + EFH.block(0, n_, m_, m_) = F / scaling; + EFH.block(0, n_ + m_, m_, k_) = H / scaling; + + prog.AddLinearConstraint( + EFH, -c / scaling, + Eigen::VectorXd::Constant(m_, std::numeric_limits::infinity()), + {xn_, ln_, un_}); + + prog.AddLinearConstraint( + MatrixXd::Identity(m_, m_), VectorXd::Zero(m_), + Eigen::VectorXd::Constant(m_, std::numeric_limits::infinity()), + ln_); + + MatrixXd New_U = U; + New_U.block(n_, n_, m_, m_) = alpha * F; + + VectorXd cost_linear = -delta_c.transpose() * New_U; + + prog.AddQuadraticCost(New_U, cost_linear, {xn_, ln_, un_}, 1); + + prog.AddQuadraticCost((1 - alpha) * F, VectorXd::Zero(m_), ln_, 1); + + solver_options.SetOption(OsqpSolver::id(), "max_iter", 500); + solver_options.SetOption(OsqpSolver::id(), "verbose", 0); + solver_options.SetOption(OsqpSolver::id(), "polish", 1); + solver_options.SetOption(OsqpSolver::id(), "polish_refine_iter", 1); + solver_options.SetOption(OsqpSolver::id(), "rho", 1e-4); + solver_options.SetOption(OsqpSolver::id(), "scaled_termination", 1); + prog.SetSolverOptions(solver_options); + + MathematicalProgramResult result = osqp_.Solve(prog); + + VectorXd xSol = result.GetSolution(xn_); + VectorXd lamSol = result.GetSolution(ln_); + VectorXd uSol = result.GetSolution(un_); + + VectorXd delta_kc = VectorXd::Zero(n_ + m_ + k_); + delta_kc.segment(0, n_) = xSol; + delta_kc.segment(n_, m_) = lamSol; + delta_kc.segment(n_ + m_, k_) = uSol; + + return delta_kc; +} + +VectorXd C3QP::SolveRobustSingleProjection(const MatrixXd& U, const VectorXd& delta_c, const MatrixXd& E, + const MatrixXd& F, const MatrixXd& H, const VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index) { + return delta_c; +} + +std::vector C3QP::GetWarmStartDelta() const { + return warm_start_delta_[0]; +} + +std::vector C3QP::GetWarmStartBinary() const { + return warm_start_binary_[0]; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_qp.h b/solvers/c3_qp.h new file mode 100644 index 0000000000..0c058e5355 --- /dev/null +++ b/solvers/c3_qp.h @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include + +#include "solvers/c3.h" +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/moby_lcp_solver.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +#include "solvers/c3_options.h" +#include "solvers/fast_osqp_solver.h" + + +namespace dairlib { +namespace solvers { + +class C3QP final : public C3 { + public: + /// Default constructor for time-varying LCS + C3QP(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, + const C3Options& options); + + ~C3QP() override = default; + + /// Virtual projection method + Eigen::VectorXd SolveSingleProjection(const Eigen::MatrixXd& U, + const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, + const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, + const Eigen::VectorXd& c, + const int admm_iteration, + const int& warm_start_index = -1) override; + /// Robust projection method + Eigen::VectorXd SolveRobustSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const Eigen::MatrixXd& W_x, const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, const Eigen::VectorXd& w, + const int admm_iteration, const int& warm_start_index = -1) override; + std::vector GetWarmStartDelta() const; + std::vector GetWarmStartBinary() const; + +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/cost_function_utils.cc b/solvers/cost_function_utils.cc new file mode 100644 index 0000000000..7c8e4a41d5 --- /dev/null +++ b/solvers/cost_function_utils.cc @@ -0,0 +1,104 @@ +#include "cost_function_utils.h" +namespace dairlib { + +namespace solvers { + +using Eigen::VectorXd; +using drake::AutoDiffXd; +using drake::VectorX; +using drake::solvers::VectorXDecisionVariable; + +template +void AddPositiveWorkCost( + dairlib::systems::trajectory_optimization::Dircon& trajopt, + const drake::multibody::MultibodyPlant& plant, + double W) { + for (int mode = 0; mode < trajopt.num_modes(); ++mode) { + int mode_start = trajopt.get_mode_start(mode); + int mode_end = trajopt.get_mode_start(mode) + trajopt.mode_length(mode) - 1; + + for (int i = mode_start; i < mode_end; ++i) { + int n_vars = 1 + 2 * plant.num_velocities() + 2 * plant.num_actuators(); + int n_v = plant.num_velocities(); + int n_u = plant.num_actuators(); + drake::solvers::VectorXDecisionVariable variables(n_vars); + variables(0) = trajopt.time_step(i)(0); + variables.segment(1, n_v) = + trajopt.state_vars(mode, i - mode_start).tail(n_v); + variables.segment(1 + n_v, n_v) = + trajopt.state_vars(mode, i + 1 - mode_start).tail(n_v); + variables.segment(1 + 2 * n_v, n_u) = + trajopt.input_vars(mode, i - mode_start); + variables.segment(1 + 2 * n_v + n_u, n_u) = + trajopt.input_vars(mode, i + 1 - mode_start); + trajopt.prog().AddCost( + std::make_shared>(n_v, plant.num_actuators(), + plant.MakeActuationMatrix(), W, + "pos_work_cost_" + std::to_string(i)), + variables); + } + } +} + +template +PositiveMechanicalWork::PositiveMechanicalWork( + int n_v, int n_u, const Eigen::MatrixXd& B, const double W, + const std::string& description) + : solvers::NonlinearCost((1 + 2 * n_v + 2 * n_u), description), + W_(W), + B_t_(B.transpose()), + n_v_(n_v), + n_u_(n_u) { + // Checks for the provided decision vars + DRAKE_DEMAND(B.rows() == n_v_); + DRAKE_DEMAND(B.cols() == n_u_); +} + +template +T sigmoid(T val) { + return exp(val) / (1 + exp(val)); +} + +template +void PositiveMechanicalWork::EvaluateCost( + const Eigen::Ref>& x, drake::VectorX* y) const { + int start_idx = 0; + auto h = x.segment(start_idx, 1); + start_idx += 1; + auto v_i = x.segment(start_idx, n_v_); + start_idx += n_v_; + auto v_ip1 = x.segment(start_idx, n_v_); + start_idx += n_v_; + auto u_i = x.segment(start_idx, n_u_); + start_idx += n_u_; + auto u_ip1 = x.segment(start_idx, n_u_); + + T cost = T(); + VectorX actuated_velocities_i = B_t_ * v_i; + VectorX actuated_velocities_ip1 = B_t_ * v_ip1; + T zero = T(); + for (int joint_idx = 0; joint_idx < n_u_; ++joint_idx) { + cost += std::max(actuated_velocities_i(joint_idx) * u_i(joint_idx), zero); + cost += std::max(actuated_velocities_ip1(joint_idx) * u_ip1(joint_idx), zero); + // cost += sigmoid(actuated_velocities_i(joint_idx) * u_i(joint_idx)); + // cost += sigmoid(actuated_velocities_ip1(joint_idx) *u_ip1(joint_idx)); + } + cost = 0.5 * h(0) * cost; + cost *= W_; + VectorX cost_vec(1); + cost_vec << cost; + *y = cost_vec; +} + +template void AddPositiveWorkCost( + dairlib::systems::trajectory_optimization::Dircon& trajopt, + const drake::multibody::MultibodyPlant& plant, double W); +//template void AddPositiveWorkCost( +// dairlib::systems::trajectory_optimization::Dircon& trajopt, +// const drake::multibody::MultibodyPlant& plant); + +} // namespace solvers +} // namespace dairlib + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::dairlib::solvers::PositiveMechanicalWork) \ No newline at end of file diff --git a/solvers/cost_function_utils.h b/solvers/cost_function_utils.h new file mode 100644 index 0000000000..1001c3722c --- /dev/null +++ b/solvers/cost_function_utils.h @@ -0,0 +1,55 @@ +#pragma once + +#include "solvers/nonlinear_cost.h" +#include "systems/trajectory_optimization/dircon/dircon.h" + +#include "drake/math/autodiff.h" +#include "drake/math/autodiff_gradient.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/solvers/decision_variable.h" +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/mathematical_program_result.h" + +namespace dairlib { + +namespace solvers { + +template +void AddPositiveWorkCost( + dairlib::systems::trajectory_optimization::Dircon& trajopt, + const drake::multibody::MultibodyPlant& plant, double W); + +template +class PositiveMechanicalWork : public NonlinearCost { + public: + PositiveMechanicalWork(int n_v, int n_u, const Eigen::MatrixXd& B, + double W, const std::string& description); + + private: + void EvaluateCost(const Eigen::Ref>& x, + drake::VectorX* y) const override; + double W_; // + const Eigen::MatrixXd B_t_; + int n_h_ = 2; + int n_v_; + int n_u_; +}; + +template +class ElectricalLoss : public NonlinearCost { + public: + ElectricalLoss(int n_v, int n_u, const Eigen::MatrixXd& B, + double W, const std::string& description); + + private: + void EvaluateCost(const Eigen::Ref>& x, + drake::VectorX* y) const override; + double W_; // + const Eigen::MatrixXd B_t_; + int n_h_ = 2; + int n_v_; + int n_u_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/default_osc_osqp_settings.yaml b/solvers/default_osc_osqp_settings.yaml new file mode 100644 index 0000000000..8cd6e9af8d --- /dev/null +++ b/solvers/default_osc_osqp_settings.yaml @@ -0,0 +1,21 @@ +rho: 0.001 +sigma: 1e-6 +max_iter: 1000 +eps_abs: 1e-7 +eps_rel: 1e-7 +eps_prim_inf: 1e-5 +eps_dual_inf: 1e-5 +alpha: 1.6 +delta: 1e-6 +polish: 1 +polish_refine_iter: 3 +verbose: 0 +scaled_termination: 1 +check_termination: 25 +warm_start: 1 +scaling: 1 +adaptive_rho: 1 +adaptive_rho_interval: 0 +adaptive_rho_tolerance: 5 +adaptive_rho_fraction: 0.4 +time_limit: 0 diff --git a/solvers/fast_osqp_solver.cc b/solvers/fast_osqp_solver.cc index 475dce35f7..62341fadc5 100644 --- a/solvers/fast_osqp_solver.cc +++ b/solvers/fast_osqp_solver.cc @@ -319,7 +319,7 @@ void SetFastOsqpSolverSettings(const SolverOptions& solver_options, SetFastOsqpSolverSetting(options_int, "scaling", &(settings->scaling)); SetFastOsqpSolverSetting(options_int, "adaptive_rho", &(settings->adaptive_rho)); - SetFastOsqpSolverSetting(options_double, "adaptive_rho_interval", + SetFastOsqpSolverSetting(options_int, "adaptive_rho_interval", &(settings->adaptive_rho_interval)); SetFastOsqpSolverSetting(options_double, "adaptive_rho_tolerance", &(settings->adaptive_rho_tolerance)); @@ -450,6 +450,7 @@ void FastOsqpSolver::DoSolve(const MathematicalProgram& prog, osqp_update_bounds(workspace_, l_.data(), u_.data()); osqp_update_P_A(workspace_, P_csc_->x, OSQP_NULL, P_csc_->nzmax, A_csc_->x, OSQP_NULL, A_csc_->nzmax); + osqp_update_warm_start(workspace_, osqp_settings_->warm_start); // If any step fails, it will set the solution_result and skip other steps. std::optional solution_result; @@ -458,6 +459,7 @@ void FastOsqpSolver::DoSolve(const MathematicalProgram& prog, if (!solution_result) { DRAKE_THROW_UNLESS(workspace_ != nullptr); const c_int osqp_solve_err = osqp_solve(workspace_); + DisableWarmStart(); // will only be re-enabled if the solve was successful if (osqp_solve_err != 0) { solution_result = SolutionResult::kInvalidInput; } diff --git a/solvers/fast_osqp_solver.h b/solvers/fast_osqp_solver.h index 53ac6d51a6..ec7d3d0789 100644 --- a/solvers/fast_osqp_solver.h +++ b/solvers/fast_osqp_solver.h @@ -68,7 +68,6 @@ class FastOsqpSolver final : public drake::solvers::SolverBase { void DisableWarmStart() const { osqp_settings_->warm_start = false; warm_start_ = false; - is_init_ = false; } /// Solver will automatically reenable warm starting after a successful solve void EnableWarmStart() const { diff --git a/solvers/fast_osqp_solver_common.cc b/solvers/fast_osqp_solver_common.cc index 40e688d7bd..70c06cad9a 100644 --- a/solvers/fast_osqp_solver_common.cc +++ b/solvers/fast_osqp_solver_common.cc @@ -18,8 +18,8 @@ namespace dairlib { namespace solvers { FastOsqpSolver::FastOsqpSolver() - : SolverBase(id(), &is_available, &is_enabled, &ProgramAttributesSatisfied) { -} + : SolverBase(id(), &is_available, &is_enabled, + &ProgramAttributesSatisfied) {} FastOsqpSolver::~FastOsqpSolver() = default; @@ -32,7 +32,7 @@ bool FastOsqpSolver::is_enabled() { return true; } namespace { bool CheckAttributes(const MathematicalProgram& prog, - std::string* explanation) { + std::string* explanation) { static const drake::never_destroyed solver_capabilities( std::initializer_list{ ProgramAttribute::kLinearCost, ProgramAttribute::kQuadraticCost, @@ -61,7 +61,8 @@ bool CheckAttributes(const MathematicalProgram& prog, } const drake::solvers::Binding* nonconvex_quadratic_cost = - FindNonconvexQuadraticCost(prog.quadratic_costs()); + drake::solvers::internal::FindNonconvexQuadraticCost( + prog.quadratic_costs()); if (nonconvex_quadratic_cost != nullptr) { if (explanation) { *explanation = diff --git a/solvers/lcs.cc b/solvers/lcs.cc new file mode 100644 index 0000000000..cc1458459f --- /dev/null +++ b/solvers/lcs.cc @@ -0,0 +1,122 @@ +#include "solvers/lcs.h" + +#include + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/moby_lcp_solver.h" +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +namespace dairlib { +namespace solvers { + +LCS::LCS(const vector& A, const vector& B, + const vector& D, const vector& d, + const vector& E, const vector& F, + const vector& H, const vector& c, double dt) + : A_(A), + B_(B), + D_(D), + d_(d), + E_(E), + F_(F), + H_(H), + c_(c), + N_(A_.size()), + dt_(dt), + n_(A_[0].rows()), + m_(D_[0].cols()), + k_(H_[0].cols()) {} + +LCS::LCS(const MatrixXd& A, const MatrixXd& B, const MatrixXd& D, + const VectorXd& d, const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, const int& N, double dt) + : LCS(vector(N, A), vector(N, B), + vector(N, D), vector(N, d), + vector(N, E), vector(N, F), + vector(N, H), vector(N, c), dt) {} + +LCS::LCS(const LCS& lcs) + : N_(lcs.N_), dt_(lcs.dt_), n_(lcs.n_), m_(lcs.m_), k_(lcs.k_) { + A_.resize(N_); + B_.resize(N_); + D_.resize(N_); + d_.resize(N_); + E_.resize(N_); + F_.resize(N_); + H_.resize(N_); + c_.resize(N_); + for (int i = 0; i < lcs.N_; ++i) { + A_.at(i) = lcs.A_.at(i); + B_.at(i) = lcs.B_.at(i); + D_.at(i) = lcs.D_.at(i); + d_.at(i) = lcs.d_.at(i); + E_.at(i) = lcs.E_.at(i); + F_.at(i) = lcs.F_.at(i); + H_.at(i) = lcs.H_.at(i); + c_.at(i) = lcs.c_.at(i); + } + W_x_ = lcs.W_x_; + W_l_ = lcs.W_l_; + W_u_ = lcs.W_u_; + w_ = lcs.w_; + has_tangent_linearization_ = lcs.has_tangent_linearization_; +} + +LCS& LCS::operator=(const LCS& lcs) { + N_ = lcs.N_; + dt_ = lcs.dt_; + n_ = lcs.n_; + m_ = lcs.m_; + k_ = lcs.k_; + A_.resize(N_); + B_.resize(N_); + D_.resize(N_); + d_.resize(N_); + E_.resize(N_); + F_.resize(N_); + H_.resize(N_); + c_.resize(N_); + for (int i = 0; i < lcs.N_; ++i) { + A_.at(i) = lcs.A_.at(i); + B_.at(i) = lcs.B_.at(i); + D_.at(i) = lcs.D_.at(i); + d_.at(i) = lcs.d_.at(i); + E_.at(i) = lcs.E_.at(i); + F_.at(i) = lcs.F_.at(i); + H_.at(i) = lcs.H_.at(i); + c_.at(i) = lcs.c_.at(i); + } + W_x_ = lcs.W_x_; + W_l_ = lcs.W_l_; + W_u_ = lcs.W_u_; + w_ = lcs.w_; + has_tangent_linearization_ = lcs.has_tangent_linearization_; + return *this; +} + +const VectorXd LCS::Simulate(const VectorXd& x_init, const VectorXd& input, + bool verbose) { + VectorXd x_final; + // calculate force + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd force; + + auto flag = LCPSolver.SolveLcpLemkeRegularized( + F_[0], E_[0] * x_init + c_[0] + H_[0] * input, &force); + + if (flag == 0) { + std::cout << "LCP failed: returning x_init" << std::endl; + return x_init; + } + // update + x_final = A_[0] * x_init + B_[0] * input + D_[0] * force + d_[0]; + if (verbose) { + std::cout<<"\tLCP simulated force is "< +#include + +#include + +#include "drake/common/sorted_pair.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace dairlib { +namespace solvers { +class LCS { + public: + /// Constructor for time-varying LCS + /// @param A, B, D, d Dynamics constraints x_{k+1} = A_k x_k + B_k u_k + D_k + /// \lambda_k + d_k + /// @param E, F, H, c Complementarity constraints 0 <= \lambda_k \perp E_k + /// x_k + F_k \lambda_k + H_k u_k + c_k + LCS(const std::vector& A, + const std::vector& B, + const std::vector& D, + const std::vector& d, + const std::vector& E, + const std::vector& F, + const std::vector& H, + const std::vector& c, double dt); + + /// Constructor for time-invariant LCS + LCS(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, + const Eigen::MatrixXd& D, const Eigen::VectorXd& d, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int& N, + double dt); + + LCS(const LCS& other); + LCS& operator=(const LCS&); + LCS(LCS&&) = default; + LCS& operator=(LCS&&) = default; + + void SetTangentGapLinearization(const Eigen::MatrixXd& W_x, + const Eigen::MatrixXd& W_l, + const Eigen::MatrixXd& W_u, + const Eigen::VectorXd& w) { + W_x_ = W_x; + W_l_ = W_l; + W_u_ = W_u; + w_ = w; + has_tangent_linearization_ = true; + } + + /// Simulate the system for one-step + /// @param x_init Initial x value + /// @param input Input value + const Eigen::VectorXd Simulate(const Eigen::VectorXd& x_init, + const Eigen::VectorXd& input, + bool verbose = false); + + public: + std::vector A_; + std::vector B_; + std::vector D_; + std::vector d_; + std::vector E_; + std::vector F_; + std::vector H_; + std::vector c_; + Eigen::MatrixXd W_x_; + Eigen::MatrixXd W_l_; + Eigen::MatrixXd W_u_; + Eigen::VectorXd w_; + bool has_tangent_linearization_ = false; + Eigen::MatrixXd J_c_; + int N_; + double dt_; + + int n_; + int m_; + int k_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs_factory.cc b/solvers/lcs_factory.cc new file mode 100644 index 0000000000..e3cf2ee035 --- /dev/null +++ b/solvers/lcs_factory.cc @@ -0,0 +1,548 @@ +#include "solvers/lcs_factory.h" + +#include + +#include "multibody/geom_geom_collider.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" + +#include "drake/math/autodiff_gradient.h" +#include "drake/solvers/moby_lcp_solver.h" + +namespace dairlib { +namespace solvers { + +using std::set; +using std::vector; + +using drake::AutoDiffVecXd; +using drake::AutoDiffXd; +using drake::MatrixX; +using drake::SortedPair; +using drake::VectorX; +using drake::geometry::GeometryId; +using drake::math::ExtractGradient; +using drake::math::ExtractValue; +using drake::multibody::MultibodyPlant; +using drake::systems::Context; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +LCS LCSFactory::LinearizePlantToLCS( + const MultibodyPlant& plant, const Context& context, + const MultibodyPlant& plant_ad, + const Context& context_ad, + const vector>& contact_geoms, + int num_friction_directions, const std::vector& mu, double dt, + int N, ContactModel contact_model) { + int n_x = plant_ad.num_positions() + plant_ad.num_velocities(); + int n_u = plant_ad.num_actuators(); + + int n_contacts = contact_geoms.size(); + + DRAKE_DEMAND(plant_ad.num_velocities() == plant.num_velocities()); + DRAKE_DEMAND(plant_ad.num_positions() == plant.num_positions()); + DRAKE_DEMAND(mu.size() == n_contacts); + int n_v = plant.num_velocities(); + int n_q = plant.num_positions(); + + AutoDiffVecXd C(n_v); + plant_ad.CalcBiasTerm(context_ad, &C); + VectorXd u_dyn = plant.get_actuation_input_port().Eval(context); + + auto B_dyn_ad = plant_ad.MakeActuationMatrix(); + AutoDiffVecXd Bu = + B_dyn_ad * plant_ad.get_actuation_input_port().Eval(context_ad); + + AutoDiffVecXd tau_g = plant_ad.CalcGravityGeneralizedForces(context_ad); + + drake::multibody::MultibodyForces f_app(plant_ad); + plant_ad.CalcForceElementsContribution(context_ad, &f_app); + + MatrixX M(n_v, n_v); + plant_ad.CalcMassMatrix(context_ad, &M); + + // If this ldlt is slow, there are alternate formulations which avoid it + AutoDiffVecXd vdot_no_contact = + M.ldlt().solve(tau_g + Bu + f_app.generalized_forces() - C); + // Constant term in dynamics, d_vv = d + A x_0 + B u_0 + VectorXd d_vv = ExtractValue(vdot_no_contact); + // Derivatives w.r.t. x and u, AB + MatrixXd AB_v = ExtractGradient(vdot_no_contact); + VectorXd x_dvv(n_q + n_v + n_u); + x_dvv << plant.GetPositions(context), plant.GetVelocities(context), u_dyn; + VectorXd x_dvvcomp = AB_v * x_dvv; + VectorXd d_v = d_vv - x_dvvcomp; + + /////////// + AutoDiffVecXd x_ad = plant_ad.GetPositionsAndVelocities(context_ad); + AutoDiffVecXd qdot_no_contact(n_q); + AutoDiffVecXd vel_ad = x_ad.tail(n_v); + + plant_ad.MapVelocityToQDot(context_ad, vel_ad, &qdot_no_contact); + MatrixXd AB_q = ExtractGradient(qdot_no_contact); + MatrixXd d_q = ExtractValue(qdot_no_contact); + Eigen::SparseMatrix Nqt; + Nqt = plant.MakeVelocityToQDotMap(context); + MatrixXd qdotNv = MatrixXd(Nqt); + + Eigen::SparseMatrix NqI; + NqI = plant.MakeQDotToVelocityMap(context); + MatrixXd vNqdot = MatrixXd(NqI); + + VectorXd phi(n_contacts); + MatrixXd J_n(n_contacts, n_v); + MatrixXd J_t(2 * n_contacts * num_friction_directions, n_v); + + for (int i = 0; i < n_contacts; i++) { + multibody::GeomGeomCollider collider( + plant, + contact_geoms[i]); + if (num_friction_directions == 1) { + Eigen::Vector3d planar_normal; + planar_normal << 0, 1, 0; + auto [phi_i, J_i] = collider.EvalPlanar(context, planar_normal); + phi(i) = phi_i; + J_n.row(i) = J_i.row(0); + J_t.block(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } else { + auto [phi_i, J_i] = + collider.EvalPolytope(context, num_friction_directions); + phi(i) = phi_i; + J_n.row(i) = J_i.row(0); + J_t.block(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } + + // J_i is 3 x n_v + // row (0) is contact normal + // rows (1-num_friction directions) are the contact tangents + } + + auto M_ldlt = ExtractValue(M).ldlt(); + MatrixXd MinvJ_n_T = M_ldlt.solve(J_n.transpose()); + MatrixXd MinvJ_t_T = M_ldlt.solve(J_t.transpose()); + + MatrixXd A = MatrixXd::Zero(n_x, n_x); + MatrixXd B = MatrixXd::Zero(n_x, n_u); + VectorXd d = VectorXd::Zero(n_x); + + MatrixXd AB_v_q = AB_v.block(0, 0, n_v, n_q); + MatrixXd AB_v_v = AB_v.block(0, n_q, n_v, n_v); + MatrixXd AB_v_u = AB_v.block(0, n_x, n_v, n_u); + MatrixXd M_double = MatrixXd::Zero(n_v, n_v); + plant.CalcMassMatrix(context, &M_double); + + A.block(0, 0, n_q, n_q) = + MatrixXd::Identity(n_q, n_q) + dt * dt * qdotNv * AB_v_q; + A.block(0, n_q, n_q, n_v) = dt * qdotNv + dt * dt * qdotNv * AB_v_v; + A.block(n_q, 0, n_v, n_q) = dt * AB_v_q; + A.block(n_q, n_q, n_v, n_v) = dt * AB_v_v + MatrixXd::Identity(n_v, n_v); + + B.block(0, 0, n_q, n_u) = dt * dt * qdotNv * AB_v_u; + B.block(n_q, 0, n_v, n_u) = dt * AB_v_u; + + d.head(n_q) = dt * dt * qdotNv * d_v; + d.tail(n_v) = dt * d_v; + + MatrixXd E_t = + MatrixXd::Zero(n_contacts, 2 * n_contacts * num_friction_directions); + for (int i = 0; i < n_contacts; i++) { + E_t.block(i, i * (2 * num_friction_directions), 1, + 2 * num_friction_directions) = + MatrixXd::Ones(1, 2 * num_friction_directions); + } + + int n_lambda = 0; + if (contact_model == ContactModel::kStewartAndTrinkle) { + n_lambda = 2 * n_contacts + 2 * n_contacts * num_friction_directions; + } else { + n_lambda = 2 * n_contacts * num_friction_directions; + } + + // Matrices with contact variables + MatrixXd D = MatrixXd::Zero(n_x, n_lambda); + MatrixXd E = MatrixXd::Zero(n_lambda, n_x); + MatrixXd F = MatrixXd::Zero(n_lambda, n_lambda); + MatrixXd H = MatrixXd::Zero(n_lambda, n_u); + VectorXd c = VectorXd::Zero(n_lambda); + + MatrixXd W_x = MatrixXd::Zero(n_lambda, n_x); + MatrixXd W_l = MatrixXd::Zero(n_lambda, n_lambda); + MatrixXd W_u = MatrixXd::Zero(n_lambda, n_u); + MatrixXd w = VectorXd::Zero(n_lambda); + + if (contact_model == ContactModel::kStewartAndTrinkle) { + D.block(0, 2 * n_contacts, n_q, 2 * n_contacts * num_friction_directions) = + dt * dt * qdotNv * MinvJ_t_T; + D.block(n_q, 2 * n_contacts, n_v, + 2 * n_contacts * num_friction_directions) = dt * MinvJ_t_T; + D.block(0, n_contacts, n_q, n_contacts) = dt * dt * qdotNv * MinvJ_n_T; + + D.block(n_q, n_contacts, n_v, n_contacts) = dt * MinvJ_n_T; + // Complementarity condition for gamma: mu lambda^n + E.block(n_contacts, 0, n_contacts, n_q) = + dt * dt * J_n * AB_v_q + J_n * vNqdot; + E.block(2 * n_contacts, 0, 2 * n_contacts * num_friction_directions, n_q) = + dt * J_t * AB_v_q; + E.block(n_contacts, n_q, n_contacts, n_v) = + dt * J_n + dt * dt * J_n * AB_v_v; + E.block(2 * n_contacts, n_q, 2 * n_contacts * num_friction_directions, + n_v) = J_t + dt * J_t * AB_v_v; + + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + // Complementarity condition for gamma: mu lambda^n + F.block(0, n_contacts, n_contacts, n_contacts) = mu_vec.asDiagonal(); + + // Complementarity condition for gamma: lambda^t + F.block(0, 2 * n_contacts, n_contacts, + 2 * n_contacts * num_friction_directions) = -E_t; + + // Complementarity condition for lambda_n: dt J_n (lambda^n component of + // v_{k+1}) + F.block(n_contacts, n_contacts, n_contacts, n_contacts) = + dt * dt * J_n * MinvJ_n_T; + // Complementarity condition for lambda_n: dt J_n (lambda^t component of + // v_{k+1}) + F.block(n_contacts, 2 * n_contacts, n_contacts, + 2 * n_contacts * num_friction_directions) = + dt * dt * J_n * MinvJ_t_T; + // Complementarity condition for lambda_t: dt J_t (gamma component of + // v_{k+1}) + F.block(2 * n_contacts, 0, 2 * n_contacts * num_friction_directions, + n_contacts) = E_t.transpose(); + // Complementarity condition for lambda_t: dt J_t (lambda^n component of + // v_{k+1}) + F.block(2 * n_contacts, n_contacts, + 2 * n_contacts * num_friction_directions, n_contacts) = + dt * J_t * MinvJ_n_T; + // Complementarity condition for lambda_t: dt J_t (lambda^t component of + // v_{k+1}) + F.block(2 * n_contacts, 2 * n_contacts, + 2 * n_contacts * num_friction_directions, + 2 * n_contacts * num_friction_directions) = dt * J_t * MinvJ_t_T; + + H.block(n_contacts, 0, n_contacts, n_u) = dt * dt * J_n * AB_v_u; + H.block(2 * n_contacts, 0, 2 * n_contacts * num_friction_directions, n_u) = + dt * J_t * AB_v_u; + + c.segment(n_contacts, n_contacts) = + phi + dt * dt * J_n * d_v - J_n * vNqdot * plant.GetPositions(context); + c.segment(2 * n_contacts, 2 * n_contacts * num_friction_directions) = + J_t * dt * d_v; + } else if (contact_model == ContactModel::kAnitescu) { + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + VectorXd anitescu_mu_vec = VectorXd::Zero(n_lambda); + for (int i = 0; i < mu_vec.rows(); i++) { + anitescu_mu_vec.segment((2 * num_friction_directions) * i, + 2 * num_friction_directions) = + mu_vec(i) * VectorXd::Ones(2 * num_friction_directions); + } + MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); + // Constructing friction bases + MatrixXd J_c = E_t.transpose() * J_n + anitescu_mu_matrix * J_t; + + MatrixXd MinvJ_c_T = M_ldlt.solve(J_c.transpose()); + + D.block(0, 0, n_q, n_lambda) = dt * dt * qdotNv * MinvJ_c_T; + D.block(n_q, 0, n_v, n_lambda) = dt * MinvJ_c_T; + + // q component of complementarity constraint + E.block(0, 0, n_lambda, n_q) = + dt * J_c * AB_v_q + E_t.transpose() * J_n * vNqdot / dt; + E.block(0, n_q, n_lambda, n_v) = J_c + dt * J_c * AB_v_v; + + // lambda component of complementarity constraint + F = dt * J_c * MinvJ_c_T; + + // u component of complementarity constraint + H = dt * J_c * AB_v_u; + // constant component of complementarity constraint + c = E_t.transpose() * phi / dt + dt * J_c * d_v - + E_t.transpose() * J_n * vNqdot * plant.GetPositions(context) / dt; + + // Anitescu model needs an explicit formulation for the tangential + // components in order to appropriately activate the robust constraint + // (TODO): yangwill do another pass to verify this formulation + W_x.block(0, 0, n_lambda, n_q) = J_t * AB_v_q; + W_x.block(0, n_q, n_lambda, n_v) = J_t + J_t * AB_v_v; + W_l = J_t * (MinvJ_c_T); + W_u = J_t * (AB_v_u); + w = J_t * (d_v); + } + + LCS system(A, B, D, d, E, F, H, c, N, dt); + system.SetTangentGapLinearization(W_x, W_l, W_u, w); + return system; +} + +std::pair> +LCSFactory::ComputeContactJacobian( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, + dairlib::solvers::ContactModel contact_model) { + int n_contacts = contact_geoms.size(); + + int n_v = plant.num_velocities(); + + VectorXd phi(n_contacts); + MatrixXd J_n(n_contacts, n_v); + MatrixXd J_t(2 * n_contacts * num_friction_directions, n_v); + std::vector contact_points; + for (int i = 0; i < n_contacts; i++) { + multibody::GeomGeomCollider collider(plant, contact_geoms[i]); + auto [phi_i, J_i] = collider.EvalPolytope(context, num_friction_directions); + auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context); + // TODO(yangwill): think about if we want to push back both witness points + contact_points.push_back(p_WCa); + phi(i) = phi_i; + J_n.row(i) = J_i.row(0); + J_t.block(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } + + if (contact_model == ContactModel::kStewartAndTrinkle) { + MatrixXd J_c = MatrixXd::Zero( + n_contacts + 2 * n_contacts * num_friction_directions, n_v); + J_c << J_n, J_t; + return std::make_pair(J_c, contact_points); + } else if (contact_model == ContactModel::kAnitescu) { + MatrixXd E_t = + MatrixXd::Zero(n_contacts, 2 * n_contacts * num_friction_directions); + for (int i = 0; i < n_contacts; i++) { + E_t.block(i, i * (2 * num_friction_directions), 1, + 2 * num_friction_directions) = + MatrixXd::Ones(1, 2 * num_friction_directions); + } + int n_contact_vars = 2 * n_contacts * num_friction_directions; + + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + VectorXd anitescu_mu_vec = VectorXd::Zero(n_contact_vars); + for (int i = 0; i < mu_vec.rows(); i++) { + double cur = mu_vec(i); + anitescu_mu_vec(4 * i) = cur; + anitescu_mu_vec(4 * i + 1) = cur; + anitescu_mu_vec(4 * i + 2) = cur; + anitescu_mu_vec(4 * i + 3) = cur; + } + MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); + MatrixXd J_c = E_t.transpose() * J_n + anitescu_mu_matrix * J_t; + return std::make_pair(J_c, contact_points); + } else { + std::cerr << ("Unknown projection type") << std::endl; + DRAKE_THROW_UNLESS(false); + } +} + +LCS LCSFactory::FixSomeModes(const LCS& other, set active_lambda_inds, + set inactive_lambda_inds) { + vector remaining_inds; + + // Assumes constant number of contacts per index + int n_lambda = other.F_[0].rows(); + + // Need to solve for lambda_active in terms of remaining elements + // Build temporary [F1, F2] by eliminating rows for inactive + for (int i = 0; i < n_lambda; i++) { + // active/inactive must be exclusive + DRAKE_ASSERT(!active_lambda_inds.count(i) || + !inactive_lambda_inds.count(i)); + + // In C++20, could use contains instead of count + if (!active_lambda_inds.count(i) && !inactive_lambda_inds.count(i)) { + remaining_inds.push_back(i); + } + } + + int n_remaining = remaining_inds.size(); + int n_active = active_lambda_inds.size(); + + vector A, B, D, E, F, H; + vector d, c; + + // Build selection matrices: + // S_a selects active indices + // S_r selects remaining indices + + MatrixXd S_a = MatrixXd::Zero(n_active, n_lambda); + MatrixXd S_r = MatrixXd::Zero(n_remaining, n_lambda); + + for (int i = 0; i < n_remaining; i++) { + S_r(i, remaining_inds[i]) = 1; + } + { + int i = 0; + for (auto ind_j : active_lambda_inds) { + S_a(i, ind_j) = 1; + i++; + } + } + + for (int k = 0; k < other.N_; k++) { + Eigen::BDCSVD svd; + svd.setThreshold(1e-5); + svd.compute(S_a * other.F_[k] * S_a.transpose(), + Eigen::ComputeFullU | Eigen::ComputeFullV); + + // F_active likely to be low-rank due to friction, but that should be OK + // MatrixXd res = svd.solve(F_ar); + + // Build new complementarity constraints + // F_a_inv = pinv(S_a * F * S_a^T) + // 0 <= \lambda_k \perp E_k x_k + F_k \lambda_k + H_k u_k + c_k + // 0 = S_a *(E x + F S_a^T \lambda_a + F S_r^T \lambda_r + H_k u_k + c_k) + // \lambda_a = -F_a_inv * (S_a F S_r^T * lambda_r + S_a E x + S_a H u + S_a + // c) + // + // 0 <= \lambda_r \perp S_r (I - F S_a^T F_a_inv S_a) E x + ... + // S_r (I - F S_a^T F_a_inv S_a) F S_r^T \lambda_r + + // ... S_r (I - F S_a^T F_a_inv S_a) H u + ... S_r (I - + // F S_a^T F_a_inv S_a) c + // + // Calling L = S_r (I - F S_a^T F_a_inv S_a)S_r * other.D_[k] + // E_k = L E + // F_k = L F S_r^t + // H_k = L H + // c_k = L c + // std::cout << S_r << std::endl << std::endl; + // std::cout << other.F_[k] << std::endl << std::endl; + // std::cout << other.F_[k] << std::endl << std::endl; + // auto tmp = S_r * (MatrixXd::Identity(n_lambda, n_lambda) - + // other.F_[k] * S_a.transpose()); + MatrixXd L = S_r * (MatrixXd::Identity(n_lambda, n_lambda) - + other.F_[k] * S_a.transpose() * svd.solve(S_a)); + MatrixXd E_k = L * other.E_[k]; + MatrixXd F_k = L * other.F_[k] * S_r.transpose(); + MatrixXd H_k = L * other.H_[k]; + MatrixXd c_k = L * other.c_[k]; + + // Similarly, + // A_k = A - D * S_a^T * F_a_inv * S_a * E + // B_k = B - D * S_a^T * F_a_inv * S_a * H + // D_k = D * S_r^T - D * S_a^T * F_a_inv * S_a F S_r^T + // d_k = d - D * S_a^T F_a_inv * S_a * c + // + // Calling P = D * S_a^T * F_a_inv * S_a + // + // A_k = A - P E + // B_k = B - P H + // D_k = S_r D - P S_r^T + // d_k = d - P c + MatrixXd P = other.D_[k] * S_a.transpose() * svd.solve(S_a); + MatrixXd A_k = other.A_[k] - P * other.E_[k]; + MatrixXd B_k = other.B_[k] - P * other.H_[k]; + MatrixXd D_k = other.D_[k] * S_r.transpose() - P * S_r.transpose(); + MatrixXd d_k = other.d_[k] - P * other.c_[k]; + E.push_back(E_k); + F.push_back(F_k); + H.push_back(H_k); + c.push_back(c_k); + A.push_back(A_k); + B.push_back(B_k); + D.push_back(D_k); + d.push_back(d_k); + } + return LCS(A, B, D, d, E, F, H, c, other.dt_); +} + +vector> LCSFactory::PreProcessor( + const MultibodyPlant& plant, const Context& context, + const vector>>& contact_geoms, + const vector& resolve_contacts_to_list, + int num_friction_directions, bool verbose) { + + int n_contacts = std::accumulate( + resolve_contacts_to_list.begin(), resolve_contacts_to_list.end(), 0); + std::vector> resolved_contacts; + resolved_contacts.reserve(n_contacts); + + for (int i = 0; i < contact_geoms.size(); i++) { + DRAKE_DEMAND(contact_geoms[i].size() >= resolve_contacts_to_list[i]); + + const auto& candidates = contact_geoms[i]; + const int num_to_select = resolve_contacts_to_list[i]; + + if (verbose && candidates.size() > 1) { + std::cout << "Contact pair " << i << " : choosing between:" << std::endl; + } + + std::vector distances; + distances.reserve(candidates.size()); + + for (const auto& pair : candidates) { + multibody::GeomGeomCollider collider(plant, pair); + auto [phi_i, J_i] = collider.EvalPolytope(context, + num_friction_directions); + distances.push_back(phi_i); + if (verbose) { + PrintVerboseContactInfo(plant, context, pair, phi_i); + } + } + + for (int j = 0; j < num_to_select; ++j) { + auto min_it = std::min_element(distances.begin(), distances.end()); + int min_index = std::distance(distances.begin(), min_it); + resolved_contacts.push_back(candidates[min_index]); + distances[min_index] = std::numeric_limits::infinity(); + + if (verbose && candidates.size() > 1) { + std::cout << " --> Chose option " << min_index << std::endl; + } + } + } + DRAKE_DEMAND(resolved_contacts.size() == n_contacts); + return resolved_contacts; +} + +void LCSFactory::PrintVerboseContactInfo(const MultibodyPlant& plant, + const Context& context, + const SortedPair& pair, + const double phi_i) { + const auto& query_port = plant.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + const auto& inspector = query_object.inspector(); + + // Get the witness points on each geometry. + const SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints( + pair.first(), pair.second()); + + const Eigen::Vector3d& p_ACa = + inspector.GetPoseInFrame(pair.first()).template cast() * + signed_distance_pair.p_ACa; + const Eigen::Vector3d& p_BCb = + inspector.GetPoseInFrame(pair.second()).template cast() * + signed_distance_pair.p_BCb; + + // Represent the witness points as points in world frame. + RigidTransform T_body1_contact = RigidTransform(p_ACa); + const FrameId f1_id = inspector.GetFrameId(pair.first()); + const Body* body1 = plant.GetBodyFromFrameId(f1_id); + RigidTransform T_world_body1 = body1->EvalPoseInWorld(context); + Eigen::Vector3d p_world_contact_a = T_world_body1 * + T_body1_contact.translation(); + + RigidTransform T_body2_contact = RigidTransform(p_BCb); + const FrameId f2_id = inspector.GetFrameId(pair.second()); + const Body* body2 = plant.GetBodyFromFrameId(f2_id); + RigidTransform T_world_body2 = body2->EvalPoseInWorld(context); + Eigen::Vector3d p_world_contact_b = T_world_body2 * + T_body2_contact.translation(); + + std::cout << "Contact pair: (" << inspector.GetName(pair.first()) + << ", " << inspector.GetName(pair.second()) + << ") with phi = " << phi_i << " between world points [" + << p_world_contact_a.transpose() << "], [" + << p_world_contact_b.transpose() << "]" << std::endl; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs_factory.h b/solvers/lcs_factory.h new file mode 100644 index 0000000000..3038f72393 --- /dev/null +++ b/solvers/lcs_factory.h @@ -0,0 +1,110 @@ +#pragma once + +#include + +#include "solvers/lcs.h" +#include "multibody/geom_geom_collider.h" +#include "drake/geometry/query_object.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/math/rigid_transform.h" + +using std::vector; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::multibody::MultibodyPlant; +using drake::multibody::Body; +using drake::math::RigidTransform; +using drake::geometry::FrameId; +using drake::systems::Context; +using drake::geometry::SignedDistancePair; +namespace dairlib { +namespace solvers { + +enum class ContactModel { + kStewartAndTrinkle, /// Stewart and Trinkle timestepping contact + kAnitescu /// Anitescu convex contact +}; + +class LCSFactory { + public: + /// Build a time-invariant LCS, linearizing a MultibodyPlant + /// Contacts are specified by the pairs in contact_geoms. Each element + /// in the contact_geoms vector defines a collision. + /// This method also uses two copies of the Context, one for double and one + /// for AutoDiff. Given that Contexts can be expensive to create, this is + /// preferred to extracting the double-version from the AutoDiff. + /// + /// TODO: add variant allowing for different frictional properties per + /// contact + /// + /// @param plant The standard MultibodyPlant + /// @param context The context about which to linearize (double) + /// @param plant_ad An AutoDiffXd templated plant for gradient calculation + /// @param context The context about which to linearize (AutoDiffXd) + /// @param contact_geoms + /// @param num_friction faces + /// @param mu + /// @oaram dt The timestep for discretization + /// @param N + static LCS LinearizePlantToLCS( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + const drake::systems::Context& context_ad, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, double dt, + int N, ContactModel = ContactModel::kStewartAndTrinkle); + + static std::pair> ComputeContactJacobian( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, + ContactModel = ContactModel::kStewartAndTrinkle); + + /// Create an LCS by fixing some modes from another LCS + /// Ignores generated inequalities that correspond to these modes, but + /// these could be returned via pointer if useful + /// + /// @param active_lambda_inds The indices for lambda thta must be non-zero + /// @param inactive_lambda_inds The indices for lambda that must be 0 + static LCS FixSomeModes(const LCS& other, std::set active_lambda_inds, + std::set inactive_lambda_inds); + + /// Optionally preprocess contact pairs to select the closest contacts + /// @param plant The MultibodyPlant + /// @param context The plant context + /// @param contact_geoms The contact geometries + /// @param resolve_contacts_to_list The number of contacts to resolve to + /// @param num_friction_directions The number of friction directions + /// @param verbose Whether to print verbose information + /// @return The closest contacts + static std::vector> + PreProcessor( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>>& + contact_geoms, + const std::vector& resolve_contacts_to_list, + int num_friction_directions, + bool verbose = false); + + private: + /// This function is for debugging purposes. This is largely a copy of the + /// EvalPolytope function in the GeomGeomCollider class with more verbosity. + /// @param plant The MultibodyPlant + /// @param context The plant context + /// @param pair The contact pair + /// @param phi_i The signed distance + static void PrintVerboseContactInfo( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const drake::SortedPair& pair, + const double phi_i); +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/osqp_options_default.yaml b/solvers/osqp_options_default.yaml index 105d21597a..40d3e70c95 100644 --- a/solvers/osqp_options_default.yaml +++ b/solvers/osqp_options_default.yaml @@ -2,7 +2,6 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 200 - linsys_solver: 0 verbose: 0 warm_start: 1 polish: 1 @@ -11,6 +10,7 @@ int_options: polish_refine_iter: 3 scaling: 10 adaptive_rho: 1 + adaptive_rho_interval: 0 double_options: rho: 0.0001 @@ -21,7 +21,6 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 1.0 diff --git a/solvers/osqp_settings_yaml.h b/solvers/osqp_settings_yaml.h new file mode 100644 index 0000000000..a4074222a2 --- /dev/null +++ b/solvers/osqp_settings_yaml.h @@ -0,0 +1,56 @@ +#pragma once + +#include "yaml-cpp/yaml.h" + +#include "drake/common/yaml/yaml_read_archive.h" + +namespace dairlib { +namespace solvers { +struct OSQPSettingsYaml { + double rho; + double sigma; + int max_iter; + double eps_abs; + double eps_rel; + double eps_prim_inf; + double eps_dual_inf; + double alpha; + double delta; + int polish; + int polish_refine_iter; + int verbose; + int scaled_termination; + int check_termination; + int warm_start; + int scaling; + int adaptive_rho; + int adaptive_rho_interval; + double adaptive_rho_tolerance; + double adaptive_rho_fraction; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(rho)); + a->Visit(DRAKE_NVP(sigma)); + a->Visit(DRAKE_NVP(max_iter)); + a->Visit(DRAKE_NVP(eps_abs)); + a->Visit(DRAKE_NVP(eps_rel)); + a->Visit(DRAKE_NVP(eps_prim_inf)); + a->Visit(DRAKE_NVP(eps_dual_inf)); + a->Visit(DRAKE_NVP(alpha)); + a->Visit(DRAKE_NVP(delta)); + a->Visit(DRAKE_NVP(polish)); + a->Visit(DRAKE_NVP(polish_refine_iter)); + a->Visit(DRAKE_NVP(verbose)); + a->Visit(DRAKE_NVP(scaled_termination)); + a->Visit(DRAKE_NVP(check_termination)); + a->Visit(DRAKE_NVP(warm_start)); + a->Visit(DRAKE_NVP(scaling)); + a->Visit(DRAKE_NVP(adaptive_rho)); + a->Visit(DRAKE_NVP(adaptive_rho_interval)); + a->Visit(DRAKE_NVP(adaptive_rho_tolerance)); + a->Visit(DRAKE_NVP(adaptive_rho_fraction)); + } +}; +} // namespace solvers +} // namespace dairlib \ No newline at end of file diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index bbc897c752..9c26e30887 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -33,3 +33,44 @@ cc_library( "@drake//:drake_shared_library", ], ) + +cc_library( + name = "franka_kinematics", + srcs = ["franka_kinematics.cc"], + hdrs = ["franka_kinematics.h"], + deps = [ + ":franka_kinematics_vector", + "//common", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/primitives", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_kinematics_vector", + srcs = ["franka_kinematics_vector.cc"], + hdrs = ["franka_kinematics_vector.h"], + deps = [ + "//common", + "//multibody:utils", + "//systems/primitives", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_state_translator", + srcs = [ + "franka_state_translator.cc", + ], + hdrs = [ + "franka_state_translator.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) \ No newline at end of file diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index d760c8b3b7..897301842e 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -2,17 +2,25 @@ load("@drake//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = ["//visibility:public"]) - cc_library( name = "controllers_all", srcs = [], deps = [ ":affine_controller", + ":cassie_out_to_radio", ":controller_failure_aggregator", ":fsm_event_time", ], ) +cc_library( + name = "c3_systems", + deps = [ + ":c3_controller", + ":lcs_factory_system", + ], +) + cc_library( name = "control_utils", srcs = [ @@ -40,6 +48,97 @@ cc_library( ], ) +cc_library( + name = "cassie_out_to_radio", + srcs = ["cassie_out_to_radio.cc"], + hdrs = ["cassie_out_to_radio.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "lcs_factory_system", + srcs = ["c3/lcs_factory_system.cc"], + hdrs = ["c3/lcs_factory_system.h"], + deps = [ + "//multibody:utils", + "//solvers:c3", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "c3_controller", + srcs = ["c3/c3_controller.cc"], + hdrs = ["c3/c3_controller.h"], + data = [ + "//solvers:solver_params", + ], + deps = [ + "//lcm:lcm_trajectory_saver", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//solvers:c3", + "//solvers:c3_output", + "//solvers:solver_options_io", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "sampling_c3_controller", + srcs = ["sampling_based_c3_controller.cc"], + hdrs = ["sampling_based_c3_controller.h"], + data = [ + "//solvers:solver_params", + ], + deps = [ + "//lcm:lcm_trajectory_saver", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//solvers:c3", + "//solvers:c3_output", + "//solvers:solver_options_io", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "//examples/sampling_c3:generate_samples", + "//examples/sampling_c3:reposition", + "//common:quaternion_error_hessian", + "//solvers:lcs", + "//examples/sampling_c3/parameter_headers:sampling_c3_controller_params", + "//examples/sampling_c3/parameter_headers:sampling_c3_options", + "//examples/sampling_c3/parameter_headers:sampling_params", + "//examples/sampling_c3/parameter_headers:reposition_params", + "//examples/sampling_c3/parameter_headers:progress_params", + "//common:update_context", + ], + copts = [ + "-fopenmp", + ], + linkopts = [ + "-lgomp", + ], +) + +cc_library( + name = "gravity_compensator", + srcs = [ + "gravity_compensator.cc", + ], + hdrs = [ + "gravity_compensator.h", + ], + deps = [ + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "linear_controller", srcs = [ @@ -89,6 +188,7 @@ cc_library( deps = [ ":affine_controller", ":constrained_lqr_controller", + ":gravity_compensator", ":linear_controller", ], ) diff --git a/systems/controllers/alip_swing_ft_traj_gen.cc b/systems/controllers/alip_swing_ft_traj_gen.cc index 516909f8ed..45c9472f00 100644 --- a/systems/controllers/alip_swing_ft_traj_gen.cc +++ b/systems/controllers/alip_swing_ft_traj_gen.cc @@ -1,13 +1,14 @@ #include "systems/controllers/alip_swing_ft_traj_gen.h" -#include #include +#include #include #include -#include "drake/math/roll_pitch_yaw.h" -#include "systems/controllers/control_utils.h" #include "multibody/multibody_utils.h" +#include "systems/controllers/control_utils.h" + +#include "drake/math/roll_pitch_yaw.h" using std::string; @@ -17,6 +18,7 @@ using Eigen::Vector3d; using Eigen::Vector4d; using Eigen::VectorXd; +using drake::math::RollPitchYaw; using drake::multibody::Frame; using drake::multibody::JacobianWrtVariable; using drake::systems::BasicVector; @@ -24,7 +26,6 @@ using drake::systems::Context; using drake::systems::DiscreteUpdateEvent; using drake::systems::DiscreteValues; using drake::systems::EventStatus; -using drake::math::RollPitchYaw; using drake::trajectories::ExponentialPlusPiecewisePolynomial; using drake::trajectories::PiecewisePolynomial; @@ -37,7 +38,7 @@ AlipSwingFootTrajGenerator::AlipSwingFootTrajGenerator( std::vector left_right_support_fsm_states, std::vector left_right_support_durations, std::vector&>> - left_right_foot, + left_right_foot, std::string floating_base_body_name, double double_support_duration, double mid_foot_height, double desired_final_foot_height, double desired_final_vertical_foot_velocity, @@ -64,10 +65,10 @@ AlipSwingFootTrajGenerator::AlipSwingFootTrajGenerator( // Input/Output Setup state_port_ = this->DeclareVectorInputPort( - "x, u, t", OutputVector(plant.num_positions(), - plant.num_velocities(), - plant.num_actuators())) - .get_index(); + "x, u, t", OutputVector(plant.num_positions(), + plant.num_velocities(), + plant.num_actuators())) + .get_index(); fsm_port_ = this->DeclareVectorInputPort("fsm", BasicVector(1)).get_index(); liftoff_time_port_ = @@ -75,14 +76,14 @@ AlipSwingFootTrajGenerator::AlipSwingFootTrajGenerator( .get_index(); PiecewisePolynomial pp(VectorXd::Zero(0)); - alip_state_port_ = this->DeclareAbstractInputPort( - "alip x, y, Lx, Ly", - drake::Value>(pp)) - .get_index(); - vdes_port_ = - this->DeclareVectorInputPort("desired horizontal walking speed", - BasicVector(2)) + alip_state_port_ = + this->DeclareAbstractInputPort( + "alip x, y, Lx, Ly", + drake::Value>(pp)) .get_index(); + vdes_port_ = this->DeclareVectorInputPort("desired horizontal walking speed", + BasicVector(2)) + .get_index(); // Provide an instance to allocate the memory first (for the output) drake::trajectories::Trajectory& traj_instance = pp; this->DeclareAbstractOutputPort("swing_foot_xyz", traj_instance, @@ -123,13 +124,13 @@ EventStatus AlipSwingFootTrajGenerator::DiscreteVariableUpdate( (OutputVector*)this->EvalVectorInput(context, state_port_); auto prev_fsm_state = discrete_state->get_mutable_vector(prev_fsm_state_idx_) - .get_mutable_value(); + .get_mutable_value(); // Find fsm_state in left_right_support_fsm_states - bool is_single_support_phase = (find(left_right_support_fsm_states_.begin(), - left_right_support_fsm_states_.end(), - fsm_state) - != left_right_support_fsm_states_.end()); + bool is_single_support_phase = + (find(left_right_support_fsm_states_.begin(), + left_right_support_fsm_states_.end(), + fsm_state) != left_right_support_fsm_states_.end()); // when entering a new state which is in left_right_support_fsm_states if (fsm_state != prev_fsm_state(0) && is_single_support_phase) { @@ -137,8 +138,9 @@ EventStatus AlipSwingFootTrajGenerator::DiscreteVariableUpdate( VectorXd q = robot_output->GetPositions(); multibody::SetPositionsIfNew(plant_, q, context_); - auto swing_foot_pos_at_liftoff = discrete_state->get_mutable_vector( - liftoff_swing_foot_pos_idx_).get_mutable_value(); + auto swing_foot_pos_at_liftoff = + discrete_state->get_mutable_vector(liftoff_swing_foot_pos_idx_) + .get_mutable_value(); auto swing_foot = swing_foot_map_.at(fsm_state); plant_.CalcPointsPositions(*context_, swing_foot.second, swing_foot.first, @@ -147,7 +149,7 @@ EventStatus AlipSwingFootTrajGenerator::DiscreteVariableUpdate( swing_foot_pos_at_liftoff = multibody::ReExpressWorldVector3InBodyYawFrame( plant_, *context_, "pelvis", swing_foot_pos_at_liftoff - - plant_.CalcCenterOfMassPositionInWorld(*context_)); + plant_.CalcCenterOfMassPositionInWorld(*context_)); } return EventStatus::Succeeded(); @@ -157,9 +159,7 @@ void AlipSwingFootTrajGenerator::CalcFootStepAndStanceFootHeight( const Context& context, const OutputVector* robot_output, const double end_time_of_this_interval, Vector2d* x_fs, double* stance_foot_height) const { - - int fsm_state = - this->EvalVectorInput(context, fsm_port_)->get_value()(0); + int fsm_state = this->EvalVectorInput(context, fsm_port_)->get_value()(0); VectorXd q = robot_output->GetPositions(); multibody::SetPositionsIfNew(plant_, q, context_); @@ -171,17 +171,14 @@ void AlipSwingFootTrajGenerator::CalcFootStepAndStanceFootHeight( world_, &stance_foot_pos); Vector3d pelvis_x = - plant_.EvalBodyPoseInWorld(*context_, pelvis_) - .rotation().col(0); + plant_.EvalBodyPoseInWorld(*context_, pelvis_).rotation().col(0); double approx_pelvis_yaw = atan2(pelvis_x(1), pelvis_x(0)); Vector3d CoM_curr = plant_.CalcCenterOfMassPositionInWorld(*context_); Vector2d vdes_xy = this->EvalVectorInput(context, vdes_port_)->get_value(); - double L_y_des = vdes_xy(0) * - (CoM_curr(2) - stance_foot_pos(2)) * m_; - double L_x_offset = -vdes_xy(1) * - (CoM_curr(2) - stance_foot_pos(2)) * m_; + double L_y_des = vdes_xy(0) * (CoM_curr(2) - stance_foot_pos(2)) * m_; + double L_x_offset = -vdes_xy(1) * (CoM_curr(2) - stance_foot_pos(2)) * m_; // com and angular momentum prediction const drake::AbstractValue* alip_traj_p = @@ -197,25 +194,23 @@ void AlipSwingFootTrajGenerator::CalcFootStepAndStanceFootHeight( double omega = sqrt(9.81 / H); double T = duration_map_.at(fsm_state) + double_support_duration_; double L_x_n = m_ * H * footstep_offset_ * - (omega * sinh(omega * T) / (1 + cosh(omega * T))); - + (omega * sinh(omega * T) / (1 + cosh(omega * T))); Vector2d L_i = multibody::ReExpressWorldVector2InBodyYawFrame( - plant_, *context_,"pelvis", alip_pred.tail<2>()); - Vector2d L_f = is_right_support ? - Vector2d(L_x_offset + L_x_n, L_y_des) : - Vector2d(L_x_offset - L_x_n, L_y_des); - double p_x_ft_to_com = ( L_f(1) - cosh(omega*T) * L_i(1) ) / - (m_ * H * omega * sinh(omega*T)); - double p_y_ft_to_com = -( L_f(0) - cosh(omega*T) * L_i(0) ) / - (m_ * H * omega * sinh(omega*T)); + plant_, *context_, "pelvis", alip_pred.tail<2>()); + Vector2d L_f = is_right_support ? Vector2d(L_x_offset + L_x_n, L_y_des) + : Vector2d(L_x_offset - L_x_n, L_y_des); + double p_x_ft_to_com = + (L_f(1) - cosh(omega * T) * L_i(1)) / (m_ * H * omega * sinh(omega * T)); + double p_y_ft_to_com = + -(L_f(0) - cosh(omega * T) * L_i(0)) / (m_ * H * omega * sinh(omega * T)); *x_fs = Vector2d(-p_x_ft_to_com, -p_y_ft_to_com); /// Imposing guards // Impose half-plane guard Vector2d stance_foot_wrt_com_in_local_frame = multibody::ReExpressWorldVector2InBodyYawFrame( - plant_, *context_,"pelvis", (stance_foot_pos - CoM_curr).head<2>()); + plant_, *context_, "pelvis", (stance_foot_pos - CoM_curr).head<2>()); if (is_right_support) { double line_pos = std::max(center_line_offset_, stance_foot_wrt_com_in_local_frame(1)); @@ -235,7 +230,8 @@ void AlipSwingFootTrajGenerator::CalcFootStepAndStanceFootHeight( *stance_foot_height = stance_foot_pos(2) - CoM_curr(2); } -PiecewisePolynomial AlipSwingFootTrajGenerator::CreateSplineForSwingFoot( +PiecewisePolynomial +AlipSwingFootTrajGenerator::CreateSplineForSwingFoot( const double start_time_of_this_interval, const double end_time_of_this_interval, const double stance_duration, const Vector3d& init_swing_foot_pos, const Vector2d& x_fs, @@ -281,8 +277,8 @@ void AlipSwingFootTrajGenerator::CalcTrajs( drake::trajectories::Trajectory* traj) const { // Cast traj for polymorphism auto* pp_traj = - (PiecewisePolynomial*)dynamic_cast*>( - traj); + (PiecewisePolynomial*)dynamic_cast*>( + traj); // Get discrete states const auto swing_foot_pos_at_liftoff = @@ -292,8 +288,7 @@ void AlipSwingFootTrajGenerator::CalcTrajs( this->EvalVectorInput(context, liftoff_time_port_)->get_value(); // Read in finite state machine - int fsm_state = - this->EvalVectorInput(context, fsm_port_)->get_value()(0); + int fsm_state = this->EvalVectorInput(context, fsm_port_)->get_value()(0); // Find fsm_state in left_right_support_fsm_states auto it = find(left_right_support_fsm_states_.begin(), diff --git a/systems/controllers/alip_swing_ft_traj_gen.h b/systems/controllers/alip_swing_ft_traj_gen.h index d154f9d702..6c04b9b763 100644 --- a/systems/controllers/alip_swing_ft_traj_gen.h +++ b/systems/controllers/alip_swing_ft_traj_gen.h @@ -107,7 +107,7 @@ class AlipSwingFootTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const drake::multibody::Body& pelvis_; std::vector left_right_support_fsm_states_; diff --git a/systems/controllers/alip_traj_gen.cc b/systems/controllers/alip_traj_gen.cc index 3cdde31eb8..ab3dcfb2eb 100644 --- a/systems/controllers/alip_traj_gen.cc +++ b/systems/controllers/alip_traj_gen.cc @@ -3,8 +3,8 @@ // #include "alip_traj_gen.h" -#include +#include #include #include @@ -37,8 +37,8 @@ ALIPTrajGenerator::ALIPTrajGenerator( const vector& unordered_state_durations, const vector&>>>& - contact_points_in_each_state, const Eigen::MatrixXd& Q, - const Eigen::MatrixXd& R) + contact_points_in_each_state, + const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R) : plant_(plant), context_(context), desired_com_height_(desired_com_height), @@ -46,18 +46,18 @@ ALIPTrajGenerator::ALIPTrajGenerator( unordered_state_durations_(unordered_state_durations), contact_points_in_each_state_(contact_points_in_each_state), world_(plant_.world_frame()) { - DRAKE_DEMAND(unordered_fsm_states.size() == unordered_state_durations.size()); - DRAKE_DEMAND(unordered_fsm_states.size() == contact_points_in_each_state.size()); + DRAKE_DEMAND(unordered_fsm_states.size() == + contact_points_in_each_state.size()); this->set_name("ALIP_traj"); // Input/Output Setup state_port_ = this->DeclareVectorInputPort( - "x, u, t", OutputVector(plant.num_positions(), - plant.num_velocities(), - plant.num_actuators())) - .get_index(); + "x, u, t", OutputVector(plant.num_positions(), + plant.num_velocities(), + plant.num_actuators())) + .get_index(); fsm_port_ = this->DeclareVectorInputPort("fsm", BasicVector(1)).get_index(); touchdown_time_port_ = @@ -67,47 +67,45 @@ ALIPTrajGenerator::ALIPTrajGenerator( // Provide an instance to allocate the memory first (for the output) ExponentialPlusPiecewisePolynomial exp; drake::trajectories::Trajectory& traj_inst = exp; - output_port_com_ = - this->DeclareAbstractOutputPort("alip_com_prediction", traj_inst, - &ALIPTrajGenerator::CalcComTrajFromCurrent) - .get_index(); - output_port_alip_state_ = - this->DeclareAbstractOutputPort("alip x, y, Lx, Ly prediction", - traj_inst, - &ALIPTrajGenerator::CalcAlipTrajFromCurrent) - .get_index(); + output_port_com_ = this->DeclareAbstractOutputPort( + "alip_com_prediction", traj_inst, + &ALIPTrajGenerator::CalcComTrajFromCurrent) + .get_index(); + output_port_alip_state_ = this->DeclareAbstractOutputPort( + "alip x, y, Lx, Ly prediction", traj_inst, + &ALIPTrajGenerator::CalcAlipTrajFromCurrent) + .get_index(); m_ = plant_.CalcTotalMass(*context); MatrixXd A = CalcA(desired_com_height); - MatrixXd B = -MatrixXd::Identity(4,2); - MatrixXd C = MatrixXd::Identity(4,4); - MatrixXd G = MatrixXd::Identity(4,4); + MatrixXd B = -MatrixXd::Identity(4, 2); + MatrixXd C = MatrixXd::Identity(4, 4); + MatrixXd G = MatrixXd::Identity(4, 4); S2SKalmanFilterData filter_data = {A, B, C, Q, R, G}; S2SKalmanFilter filter = S2SKalmanFilter(filter_data); - std::pair model_filter = {filter, filter_data}; - alip_filter_idx_ = this->DeclareAbstractState( - drake::Value>(model_filter)); + std::pair model_filter = {filter, + filter_data}; + alip_filter_idx_ = this->DeclareAbstractState( + drake::Value>( + model_filter)); prev_foot_idx_ = this->DeclareDiscreteState(Vector2d::Zero()); prev_fsm_idx_ = this->DeclareDiscreteState(-1 * VectorXd::Ones(1)); - com_z_idx_ = this->DeclareDiscreteState( - desired_com_height * VectorXd::Ones(1)); + com_z_idx_ = + this->DeclareDiscreteState(desired_com_height * VectorXd::Ones(1)); this->DeclarePerStepUnrestrictedUpdateEvent( &ALIPTrajGenerator::UnrestrictedUpdate); } drake::systems::EventStatus ALIPTrajGenerator::UnrestrictedUpdate( - const drake::systems::Context &context, - drake::systems::State *state) const { - + const drake::systems::Context& context, + drake::systems::State* state) const { int prev_fsm = state->get_discrete_state(prev_fsm_idx_).value()(0); - auto& [filter, filter_data] = - state->get_mutable_abstract_state>(alip_filter_idx_); + auto& [filter, filter_data] = state->get_mutable_abstract_state< + std::pair>(alip_filter_idx_); // Read in current state const OutputVector* robot_output = @@ -122,8 +120,8 @@ drake::systems::EventStatus ALIPTrajGenerator::UnrestrictedUpdate( // calculate current estimate of ALIP state Vector3d CoM, L, stance_foot_pos; - CalcAlipState(robot_output->GetState(), mode_index, - &CoM, &L, &stance_foot_pos); + CalcAlipState(robot_output->GetState(), mode_index, &CoM, &L, + &stance_foot_pos); Vector4d x_alip; x_alip.head(2) = CoM.head(2) - stance_foot_pos.head(2); @@ -135,25 +133,30 @@ drake::systems::EventStatus ALIPTrajGenerator::UnrestrictedUpdate( filter.Update(filter_data, Vector2d::Zero(), x_alip, robot_output->get_timestamp()); } else { - Vector2d prev_stance_pos = state->get_discrete_state(prev_foot_idx_).value(); - filter.Update(filter_data, stance_foot_pos.head<2>() - prev_stance_pos, x_alip, - robot_output->get_timestamp()); - state->get_mutable_discrete_state(prev_fsm_idx_).get_mutable_value() << fsm_state; + Vector2d prev_stance_pos = + state->get_discrete_state(prev_foot_idx_).value(); + filter.Update(filter_data, stance_foot_pos.head<2>() - prev_stance_pos, + x_alip, robot_output->get_timestamp()); + state->get_mutable_discrete_state(prev_fsm_idx_).get_mutable_value() + << fsm_state; } - state->get_mutable_discrete_state(com_z_idx_).get_mutable_value() << CoM.tail(1) - stance_foot_pos.tail(1); - state->get_mutable_discrete_state(prev_foot_idx_).get_mutable_value() << stance_foot_pos.head<2>(); + state->get_mutable_discrete_state(com_z_idx_).get_mutable_value() + << CoM.tail(1) - stance_foot_pos.tail(1); + state->get_mutable_discrete_state(prev_foot_idx_).get_mutable_value() + << stance_foot_pos.head<2>(); return EventStatus::Succeeded(); } -ExponentialPlusPiecewisePolynomial ALIPTrajGenerator::ConstructAlipComTraj( - const Vector3d& CoM, const Vector3d& stance_foot_pos, const Vector4d& x_alip, - double start_time, double end_time_of_this_fsm_state) const { - +ExponentialPlusPiecewisePolynomial +ALIPTrajGenerator::ConstructAlipComTraj( + const Vector3d& CoM, const Vector3d& stance_foot_pos, + const Vector4d& x_alip, double start_time, + double end_time_of_this_fsm_state) const { double CoM_wrt_foot_z = (CoM(2) - stance_foot_pos(2)); DRAKE_DEMAND(CoM_wrt_foot_z > 0); // create a 3D one-segment polynomial for ExponentialPlusPiecewisePolynomial - Vector2d T_waypoint_com {start_time, end_time_of_this_fsm_state}; + Vector2d T_waypoint_com{start_time, end_time_of_this_fsm_state}; MatrixXd Y = MatrixXd::Zero(3, 2); Y.col(0).head(2) = stance_foot_pos.head(2); Y.col(1).head(2) = stance_foot_pos.head(2); @@ -163,10 +166,9 @@ ExponentialPlusPiecewisePolynomial ALIPTrajGenerator::ConstructAlipComTr double max_height_diff_per_step = 0.05; double final_height = std::clamp( desired_com_height_ + stance_foot_pos(2), - CoM(2) - max_height_diff_per_step, - CoM(2) + max_height_diff_per_step); + CoM(2) - max_height_diff_per_step, CoM(2) + max_height_diff_per_step); // double final_height = desired_com_height_ + stance_foot_pos(2); - Y(2,0) = final_height; + Y(2, 0) = final_height; Y(2, 1) = final_height; Vector3d Y_dot_start = Vector3d::Zero(); @@ -174,18 +176,15 @@ ExponentialPlusPiecewisePolynomial ALIPTrajGenerator::ConstructAlipComTr PiecewisePolynomial pp_part = PiecewisePolynomial::CubicWithContinuousSecondDerivatives( - T_waypoint_com, Y, - Y_dot_start, Y_dot_end); + T_waypoint_com, Y, Y_dot_start, Y_dot_end); - MatrixXd K = MatrixXd::Zero(3,4); - K.topLeftCorner(2,2) = MatrixXd::Identity(2,2); + MatrixXd K = MatrixXd::Zero(3, 4); + K.topLeftCorner(2, 2) = MatrixXd::Identity(2, 2); auto A = CalcA(CoM_wrt_foot_z); - return ExponentialPlusPiecewisePolynomial( - K, A, x_alip, pp_part); + return ExponentialPlusPiecewisePolynomial(K, A, x_alip, pp_part); } - Eigen::MatrixXd ALIPTrajGenerator::CalcA(double com_z) const { // Dynamics of ALIP: (eqn 6) https://arxiv.org/pdf/2109.14862.pdf const double g = 9.81; @@ -203,27 +202,25 @@ Eigen::MatrixXd ALIPTrajGenerator::CalcA(double com_z) const { return A; } -ExponentialPlusPiecewisePolynomial ALIPTrajGenerator::ConstructAlipStateTraj( +ExponentialPlusPiecewisePolynomial +ALIPTrajGenerator::ConstructAlipStateTraj( const Eigen::Vector4d& x_alip, double com_z, double start_time, double end_time_of_this_fsm_state) const { - Vector2d breaks = {start_time, end_time_of_this_fsm_state}; PiecewisePolynomial pp_part = PiecewisePolynomial::CubicWithContinuousSecondDerivatives( - breaks, MatrixXd::Zero(4,2), - Vector4d::Zero(), Vector4d::Zero()); - MatrixXd K = MatrixXd::Identity(4,4); + breaks, MatrixXd::Zero(4, 2), Vector4d::Zero(), Vector4d::Zero()); + MatrixXd K = MatrixXd::Identity(4, 4); - return ExponentialPlusPiecewisePolynomial( - K, CalcA(com_z), x_alip, pp_part); + return ExponentialPlusPiecewisePolynomial(K, CalcA(com_z), x_alip, + pp_part); } void ALIPTrajGenerator::CalcAlipState( - const Eigen::VectorXd &x, int mode_index, + const Eigen::VectorXd& x, int mode_index, const drake::EigenPtr& CoM_p, const drake::EigenPtr& L_p, const drake::EigenPtr& stance_pos_p) const { - multibody::SetPositionsAndVelocitiesIfNew(plant_, x, context_); Vector3d CoM = plant_.CalcCenterOfMassPositionInWorld(*context_); @@ -237,17 +234,18 @@ void ALIPTrajGenerator::CalcAlipState( } stance_foot_pos /= contact_points_in_each_state_[mode_index].size(); - Vector3d L = plant_.CalcSpatialMomentumInWorldAboutPoint( - *context_, stance_foot_pos).rotational(); + Vector3d L = + plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, stance_foot_pos) + .rotational(); *CoM_p = CoM; *L_p = L; *stance_pos_p = stance_foot_pos; } -void ALIPTrajGenerator::CalcComTrajFromCurrent(const drake::systems::Context< - double> &context, drake::trajectories::Trajectory *traj) const { - +void ALIPTrajGenerator::CalcComTrajFromCurrent( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { // Read in current state const OutputVector* robot_output = (OutputVector*)this->EvalVectorInput(context, state_port_); @@ -265,18 +263,18 @@ void ALIPTrajGenerator::CalcComTrajFromCurrent(const drake::systems::Context< double timestamp = robot_output->get_timestamp(); double start_time = timestamp; double end_time = prev_event_time(0) + unordered_state_durations_[mode_index]; - start_time = std::clamp(start_time, - -std::numeric_limits::infinity(), - end_time - 0.001); + start_time = std::clamp( + start_time, -std::numeric_limits::infinity(), end_time - 0.001); Vector3d CoM, L, stance_foot_pos; - CalcAlipState( - robot_output->GetState(), mode_index, - &CoM, &L, &stance_foot_pos); + CalcAlipState(robot_output->GetState(), mode_index, &CoM, &L, + &stance_foot_pos); Vector4d x_alip = - context.get_abstract_state>(alip_filter_idx_).first.x(); + context + .get_abstract_state>( + alip_filter_idx_) + .first.x(); // Assign traj auto exp_pp_traj = (ExponentialPlusPiecewisePolynomial*)dynamic_cast< @@ -285,8 +283,9 @@ void ALIPTrajGenerator::CalcComTrajFromCurrent(const drake::systems::Context< ConstructAlipComTraj(CoM, stance_foot_pos, x_alip, start_time, end_time); } -void ALIPTrajGenerator::CalcAlipTrajFromCurrent(const drake::systems::Context< - double> &context, drake::trajectories::Trajectory *traj) const { +void ALIPTrajGenerator::CalcAlipTrajFromCurrent( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { // Read in current state const OutputVector* robot_output = (OutputVector*)this->EvalVectorInput(context, state_port_); @@ -304,27 +303,25 @@ void ALIPTrajGenerator::CalcAlipTrajFromCurrent(const drake::systems::Context< double timestamp = robot_output->get_timestamp(); double start_time = timestamp; double end_time = prev_event_time(0) + unordered_state_durations_[mode_index]; - start_time = std::clamp(start_time, - -std::numeric_limits::infinity(), - end_time - 0.001); + start_time = std::clamp( + start_time, -std::numeric_limits::infinity(), end_time - 0.001); // Assign traj auto exp_pp_traj = (ExponentialPlusPiecewisePolynomial*)dynamic_cast< ExponentialPlusPiecewisePolynomial*>(traj); Vector4d x_alip = - context.get_abstract_state>( - alip_filter_idx_).first.x(); + context + .get_abstract_state>( + alip_filter_idx_) + .first.x(); double com_z = context.get_discrete_state(com_z_idx_).value()(0); - *exp_pp_traj = - ConstructAlipStateTraj(x_alip, com_z, start_time, end_time); + *exp_pp_traj = ConstructAlipStateTraj(x_alip, com_z, start_time, end_time); } int ALIPTrajGenerator::GetModeIdx(int fsm_state) const { - auto it = find(unordered_fsm_states_.begin(), - unordered_fsm_states_.end(), + auto it = find(unordered_fsm_states_.begin(), unordered_fsm_states_.end(), fsm_state); int mode_index = std::distance(unordered_fsm_states_.begin(), it); DRAKE_DEMAND(it != unordered_fsm_states_.end()); @@ -333,4 +330,3 @@ int ALIPTrajGenerator::GetModeIdx(int fsm_state) const { } // namespace systems } // namespace dairlib - diff --git a/systems/controllers/alip_traj_gen.h b/systems/controllers/alip_traj_gen.h index 5a4c76ae88..49a8edb0a0 100644 --- a/systems/controllers/alip_traj_gen.h +++ b/systems/controllers/alip_traj_gen.h @@ -119,7 +119,7 @@ class ALIPTrajGenerator : public drake::systems::LeafSystem { const std::vector&>>>& contact_points_in_each_state_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; double m_; diff --git a/systems/controllers/c3/c3_controller.cc b/systems/controllers/c3/c3_controller.cc new file mode 100644 index 0000000000..8585005e73 --- /dev/null +++ b/systems/controllers/c3/c3_controller.cc @@ -0,0 +1,279 @@ +#include "c3_controller.h" +#include + +#include "solvers/c3_miqp.h" +#include "solvers/c3_qp.h" +#include "solvers/lcs_factory.h" + +namespace dairlib { + +using drake::multibody::ModelInstanceIndex; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using Eigen::MatrixXd; +using Eigen::MatrixXf; +using Eigen::VectorXd; +using Eigen::VectorXf; +using solvers::C3; +using solvers::C3MIQP; +using solvers::C3QP; +using solvers::LCS; +using solvers::LCSFactory; +using std::vector; +using systems::TimestampedVector; + +namespace systems { + +C3Controller::C3Controller( + const drake::multibody::MultibodyPlant& plant, C3Options c3_options) + : plant_(plant), + c3_options_(std::move(c3_options)), + G_(std::vector(c3_options_.N, c3_options_.G)), + U_(std::vector(c3_options_.N, c3_options_.U)), + N_(c3_options_.N) { + this->set_name("c3_controller"); + + double discount_factor = 1; + for (int i = 0; i < N_; ++i) { + Q_.push_back(discount_factor * c3_options_.Q); + R_.push_back(discount_factor * c3_options_.R); + discount_factor *= c3_options_.gamma; + } + Q_.push_back(discount_factor * c3_options_.Q); + DRAKE_DEMAND(Q_.size() == N_ + 1); + DRAKE_DEMAND(R_.size() == N_); + + n_q_ = plant_.num_positions(); + n_v_ = plant_.num_velocities(); + n_u_ = plant_.num_actuators(); + n_x_ = n_q_ + n_v_; + dt_ = c3_options_.dt; + solve_time_filter_constant_ = c3_options_.solve_time_filter_alpha; + if (c3_options_.contact_model == "stewart_and_trinkle") { + n_lambda_ = + 2 * c3_options_.num_contacts + + 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; + } else if (c3_options_.contact_model == "anitescu") { + n_lambda_ = + 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; + } + VectorXd zeros = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); + + n_u_ = plant_.num_actuators(); + + // Creates placeholder lcs to construct base C3 problem + // Placeholder LCS will have correct size as it's already determined by the + // contact model + auto lcs_placeholder = CreatePlaceholderLCS(); + auto x_desired_placeholder = + std::vector(N_ + 1, VectorXd::Zero(n_x_)); + if (c3_options_.projection_type == "MIQP") { + c3_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options_); + + } else if (c3_options_.projection_type == "QP") { + c3_ = std::make_unique(lcs_placeholder, + C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options_); + + } else { + std::cerr << ("Unknown projection type") << std::endl; + DRAKE_THROW_UNLESS(false); + } + + c3_->SetOsqpSolverOptions(solver_options_); + + // Set actor bounds, + // TODO(yangwill): move this out of here because it is task specific + for (int i = 0; i < c3_options_.workspace_limits.size(); ++i) { + Eigen::RowVectorXd A = VectorXd::Zero(n_x_); + A.segment(0, 3) = c3_options_.workspace_limits[i].segment(0, 3); + c3_->AddLinearConstraint(A, c3_options_.workspace_limits[i][3], + c3_options_.workspace_limits[i][4], 1); + } + for (int i : vector({0, 1})) { + Eigen::RowVectorXd A = VectorXd::Zero(n_u_); + A(i) = 1.0; + c3_->AddLinearConstraint(A, c3_options_.u_horizontal_limits[0], + c3_options_.u_horizontal_limits[1], 2); + } + for (int i : vector({2})) { + Eigen::RowVectorXd A = VectorXd::Zero(n_u_); + A(i) = 1.0; + c3_->AddLinearConstraint(A, c3_options_.u_vertical_limits[0], + c3_options_.u_vertical_limits[1], 2); + } + + lcs_state_input_port_ = + this->DeclareVectorInputPort("x_lcs", TimestampedVector(n_x_)) + .get_index(); + lcs_input_port_ = + this->DeclareAbstractInputPort("lcs", drake::Value(lcs_placeholder)) + .get_index(); + + target_input_port_ = + this->DeclareVectorInputPort("x_lcs_des", n_x_).get_index(); + + auto c3_solution = C3Output::C3Solution(); + c3_solution.x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); + c3_solution.lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); + c3_solution.u_sol_ = MatrixXf::Zero(n_u_, N_); + c3_solution.time_vector_ = VectorXf::Zero(N_); + auto c3_intermediates = C3Output::C3Intermediates(); + c3_intermediates.z_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); + c3_intermediates.delta_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); + c3_intermediates.w_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); + c3_intermediates.time_vector_ = VectorXf::Zero(N_); + c3_solution_port_ = + this->DeclareAbstractOutputPort("c3_solution", c3_solution, + &C3Controller::OutputC3Solution) + .get_index(); + c3_intermediates_port_ = + this->DeclareAbstractOutputPort("c3_intermediates", c3_intermediates, + &C3Controller::OutputC3Intermediates) + .get_index(); + + plan_start_time_index_ = DeclareDiscreteState(1); + x_pred_index_ = DeclareDiscreteState(n_x_); + filtered_solve_time_index_ = DeclareDiscreteState(1); + + if (c3_options_.publish_frequency > 0) { + DeclarePeriodicDiscreteUpdateEvent(1 / c3_options_.publish_frequency, 0.0, + &C3Controller::ComputePlan); + } else { + DeclareForcedDiscreteUpdateEvent(&C3Controller::ComputePlan); + } +} + +LCS C3Controller::CreatePlaceholderLCS() const { + MatrixXd A = MatrixXd::Ones(n_x_, n_x_); + MatrixXd B = MatrixXd::Zero(n_x_, n_u_); + VectorXd d = VectorXd::Zero(n_x_); + MatrixXd D = MatrixXd::Ones(n_x_, n_lambda_); + MatrixXd E = MatrixXd::Zero(n_lambda_, n_x_); + MatrixXd F = MatrixXd::Zero(n_lambda_, n_lambda_); + MatrixXd H = MatrixXd::Zero(n_lambda_, n_u_); + VectorXd c = VectorXd::Zero(n_lambda_); + return LCS(A, B, D, d, E, F, H, c, c3_options_.N, c3_options_.dt); +} + +drake::systems::EventStatus C3Controller::ComputePlan( + const Context& context, + DiscreteValues* discrete_state) const { + auto start = std::chrono::high_resolution_clock::now(); + const BasicVector& x_des = + *this->template EvalVectorInput(context, target_input_port_); + const TimestampedVector* lcs_x = + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); + + auto& lcs = + this->EvalAbstractInput(context, lcs_input_port_)->get_value(); + drake::VectorX x_lcs = lcs_x->get_data(); + auto& x_pred = context.get_discrete_state(x_pred_index_).value(); + auto mutable_x_pred = discrete_state->get_mutable_value(x_pred_index_); + auto mutable_solve_time = + discrete_state->get_mutable_value(filtered_solve_time_index_); + + if (x_lcs.segment(n_q_, 3).norm() > 0.01 && c3_options_.use_predicted_x0 && + !x_pred.isZero()) { + x_lcs[0] = std::clamp(x_pred[0], x_lcs[0] - 10 * dt_ * dt_, + x_lcs[0] + 10 * dt_ * dt_); + x_lcs[1] = std::clamp(x_pred[1], x_lcs[1] - 10 * dt_ * dt_, + x_lcs[1] + 10 * dt_ * dt_); + x_lcs[2] = std::clamp(x_pred[2], x_lcs[2] - 10 * dt_ * dt_, + x_lcs[2] + 10 * dt_ * dt_); + x_lcs[n_q_ + 0] = std::clamp(x_pred[n_q_ + 0], x_lcs[n_q_ + 0] - 10 * dt_, + x_lcs[n_q_ + 0] + 10 * dt_); + x_lcs[n_q_ + 1] = std::clamp(x_pred[n_q_ + 1], x_lcs[n_q_ + 1] - 10 * dt_, + x_lcs[n_q_ + 1] + 10 * dt_); + x_lcs[n_q_ + 2] = std::clamp(x_pred[n_q_ + 2], x_lcs[n_q_ + 2] - 10 * dt_, + x_lcs[n_q_ + 2] + 10 * dt_); + } + + discrete_state->get_mutable_value(plan_start_time_index_)[0] = + lcs_x->get_timestamp(); + + std::vector x_desired = + std::vector(N_ + 1, x_des.value()); + + // Force Checking of Workspace Limits + for (int i = 0; i < c3_options_.workspace_limits.size(); ++i) { + DRAKE_DEMAND(lcs_x->get_data().segment(0, 3).transpose() * + c3_options_.workspace_limits[i].segment(0, 3) > + c3_options_.workspace_limits[i][3] - + c3_options_.workspace_margins); + DRAKE_DEMAND(lcs_x->get_data().segment(0, 3).transpose() * + c3_options_.workspace_limits[i].segment(0, 3) < + c3_options_.workspace_limits[i][4] + + c3_options_.workspace_margins); + } + + c3_->UpdateLCS(lcs); + c3_->UpdateTarget(x_desired); + c3_->Solve(x_lcs); + + auto finish = std::chrono::high_resolution_clock::now(); + auto elapsed = finish - start; + double solve_time = + std::chrono::duration_cast(elapsed).count() / + 1e6; + mutable_solve_time[0] = (1 - solve_time_filter_constant_) * solve_time + + solve_time_filter_constant_ * mutable_solve_time[0]; + + if (c3_options_.publish_frequency > 0) { + solve_time = 1.0 / c3_options_.publish_frequency; + mutable_solve_time[0] = solve_time; + } + + auto z_sol = c3_->GetFullSolution(); + if (mutable_solve_time[0] < (N_ - 1) * dt_) { + int index = mutable_solve_time[0] / dt_; + double weight = (mutable_solve_time[0] - index * dt_) / dt_; + mutable_x_pred = (1 - weight) * z_sol[index].segment(0, n_x_) + + weight * z_sol[index + 1].segment(0, n_x_); + } else { + mutable_x_pred = z_sol[N_ - 1].segment(0, n_x_); + } + + return drake::systems::EventStatus::Succeeded(); +} + +void C3Controller::OutputC3Solution( + const drake::systems::Context& context, + C3Output::C3Solution* c3_solution) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + double solve_time = context.get_discrete_state(filtered_solve_time_index_)[0]; + + auto z_sol = c3_->GetFullSolution(); + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = solve_time + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } +} + +void C3Controller::OutputC3Intermediates( + const drake::systems::Context& context, + C3Output::C3Intermediates* c3_intermediates) const { + double solve_time = context.get_discrete_state(filtered_solve_time_index_)[0]; + double t = context.get_discrete_state(plan_start_time_index_)[0] + solve_time; + auto z = c3_->GetFullSolution(); + auto delta = c3_->GetDualDeltaSolution(); + auto w = c3_->GetDualWSolution(); + + for (int i = 0; i < N_; i++) { + c3_intermediates->time_vector_(i) = solve_time + t + i * dt_; + c3_intermediates->z_.col(i) = z[i].cast(); + c3_intermediates->delta_.col(i) = delta[i].cast(); + c3_intermediates->w_.col(i) = w[i].cast(); + } +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/c3/c3_controller.h b/systems/controllers/c3/c3_controller.h new file mode 100644 index 0000000000..e8131583eb --- /dev/null +++ b/systems/controllers/c3/c3_controller.h @@ -0,0 +1,104 @@ +#pragma once + +#include +#include + +#include + +#include "common/find_resource.h" +#include "dairlib/lcmt_saved_traj.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" +#include "solvers/c3.h" +#include "solvers/c3_options.h" +#include "solvers/c3_output.h" +#include "solvers/lcs.h" +#include "solvers/solver_options_io.h" +#include "systems/framework/timestamped_vector.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +class C3Controller : public drake::systems::LeafSystem { + public: + explicit C3Controller(const drake::multibody::MultibodyPlant& plant, + C3Options c3_options); + + const drake::systems::InputPort& get_input_port_target() const { + return this->get_input_port(target_input_port_); + } + + const drake::systems::InputPort& get_input_port_lcs_state() const { + return this->get_input_port(lcs_state_input_port_); + } + + const drake::systems::InputPort& get_input_port_lcs() const { + return this->get_input_port(lcs_input_port_); + } + + const drake::systems::OutputPort& get_output_port_c3_solution() + const { + return this->get_output_port(c3_solution_port_); + } + const drake::systems::OutputPort& get_output_port_c3_intermediates() + const { + return this->get_output_port(c3_intermediates_port_); + } + + void SetOsqpSolverOptions(const drake::solvers::SolverOptions& options) { + solver_options_ = options; + c3_->SetOsqpSolverOptions(solver_options_); + } + + private: + solvers::LCS CreatePlaceholderLCS() const; + + drake::systems::EventStatus ComputePlan( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void OutputC3Solution(const drake::systems::Context& context, + C3Output::C3Solution* c3_solution) const; + + void OutputC3Intermediates(const drake::systems::Context& context, + C3Output::C3Intermediates* c3_intermediates) const; + + drake::systems::InputPortIndex target_input_port_; + drake::systems::InputPortIndex lcs_state_input_port_; + drake::systems::InputPortIndex lcs_input_port_; + drake::systems::OutputPortIndex c3_solution_port_; + drake::systems::OutputPortIndex c3_intermediates_port_; + + const drake::multibody::MultibodyPlant& plant_; + + C3Options c3_options_; + drake::solvers::SolverOptions solver_options_ = + drake::yaml::LoadYamlFile( + "solvers/osqp_options_default.yaml") + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + // convenience for variable sizes + int n_q_; + int n_v_; + int n_x_; + int n_lambda_; + int n_u_; + double dt_; + + mutable std::unique_ptr c3_; + + double solve_time_filter_constant_; + drake::systems::DiscreteStateIndex plan_start_time_index_; + drake::systems::DiscreteStateIndex x_pred_index_; + drake::systems::DiscreteStateIndex filtered_solve_time_index_; + std::vector Q_; + std::vector R_; + std::vector G_; + std::vector U_; + int N_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/c3/lcs_factory_system.cc b/systems/controllers/c3/lcs_factory_system.cc new file mode 100644 index 0000000000..fc4cc16a70 --- /dev/null +++ b/systems/controllers/c3/lcs_factory_system.cc @@ -0,0 +1,149 @@ +#include "lcs_factory_system.h" + +#include + +#include "multibody/multibody_utils.h" +#include "solvers/lcs.h" +#include "solvers/lcs_factory.h" +#include "systems/framework/timestamped_vector.h" + +namespace dairlib { + +using drake::multibody::ModelInstanceIndex; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using Eigen::MatrixXd; +using Eigen::MatrixXf; +using Eigen::VectorXd; +using solvers::LCS; +using solvers::LCSFactory; +using systems::TimestampedVector; + +namespace systems { + +LCSFactorySystem::LCSFactorySystem( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad, + const std::vector> + contact_geoms, + C3Options c3_options) + : plant_(plant), + context_(context), + plant_ad_(plant_ad), + context_ad_(context_ad), + contact_pairs_(contact_geoms), + c3_options_(std::move(c3_options)), + N_(c3_options_.N) { + this->set_name("lcs_factory_system"); + + n_q_ = plant_.num_positions(); + n_v_ = plant_.num_velocities(); + n_x_ = n_q_ + n_v_; + dt_ = c3_options_.dt; + if (c3_options_.contact_model == "stewart_and_trinkle") { + n_lambda_ = + 2 * c3_options_.num_contacts + + 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; + } else if (c3_options_.contact_model == "anitescu") { + n_lambda_ = + 2 * c3_options_.num_friction_directions * c3_options_.num_contacts; + } + n_u_ = plant_.num_actuators(); + + lcs_state_input_port_ = + this->DeclareVectorInputPort("x_lcs", TimestampedVector(n_x_)) + .get_index(); + +// lcs_inputs_input_port_ = +// this->DeclareVectorInputPort("u_lcs", BasicVector(n_u_)) +// .get_index(); + + MatrixXd A = MatrixXd::Zero(n_x_, n_x_); + MatrixXd B = MatrixXd::Zero(n_x_, n_u_); + VectorXd d = VectorXd::Zero(n_x_); + MatrixXd D = MatrixXd::Zero(n_x_, n_lambda_); + MatrixXd E = MatrixXd::Zero(n_lambda_, n_x_); + MatrixXd F = MatrixXd::Zero(n_lambda_, n_lambda_); + MatrixXd H = MatrixXd::Zero(n_lambda_, n_u_); + VectorXd c = VectorXd::Zero(n_lambda_); + lcs_port_ = this->DeclareAbstractOutputPort( + "lcs", LCS(A, B, D, d, E, F, H, c, N_, dt_), + &LCSFactorySystem::OutputLCS) + .get_index(); + + lcs_contact_jacobian_port_ = this->DeclareAbstractOutputPort( + "J_lcs, p_lcs", std::pair(Eigen::MatrixXd(n_x_, n_lambda_), std::vector()), + &LCSFactorySystem::OutputLCSContactJacobian) + .get_index(); +} + +void LCSFactorySystem::OutputLCS(const drake::systems::Context& context, + LCS* output_lcs) const { + const TimestampedVector* lcs_x = + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); +// const auto lcs_u = +// (BasicVector*)this->EvalVectorInput(context, +// lcs_inputs_input_port_); + DRAKE_DEMAND(lcs_x->get_data().size() == n_x_); +// DRAKE_DEMAND(lcs_u->get_value().size() == n_u_); + VectorXd q_v_u = + VectorXd::Zero(plant_.num_positions() + plant_.num_velocities() + + plant_.num_actuators()); +// q_v_u << lcs_x->get_data(), lcs_u->get_value(); + q_v_u << lcs_x->get_data(), VectorXd::Zero(n_u_); + drake::AutoDiffVecXd q_v_u_ad = drake::math::InitializeAutoDiff(q_v_u); + + plant_.SetPositionsAndVelocities(&context_, q_v_u.head(n_x_)); + multibody::SetInputsIfNew(plant_, q_v_u.tail(n_u_), &context_); + multibody::SetInputsIfNew(plant_ad_, q_v_u_ad.tail(n_u_), + &context_ad_); + solvers::ContactModel contact_model; + if (c3_options_.contact_model == "stewart_and_trinkle") { + contact_model = solvers::ContactModel::kStewartAndTrinkle; + } else if (c3_options_.contact_model == "anitescu") { + contact_model = solvers::ContactModel::kAnitescu; + } else { + throw std::runtime_error("unknown or unsupported contact model"); + } + + *output_lcs = LCSFactory::LinearizePlantToLCS( + plant_, context_, plant_ad_, context_ad_, contact_pairs_, + c3_options_.num_friction_directions, c3_options_.mu, c3_options_.dt, + c3_options_.N, contact_model); +} + +void LCSFactorySystem::OutputLCSContactJacobian(const drake::systems::Context& context, + std::pair>* output) const { + const TimestampedVector* lcs_x = + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); + + VectorXd q_v_u = + VectorXd::Zero(plant_.num_positions() + plant_.num_velocities() + + plant_.num_actuators()); + // u is irrelevant in pure geometric/kinematic calculation + q_v_u << lcs_x->get_data(), VectorXd::Zero(n_u_); + + plant_.SetPositionsAndVelocities(&context_, q_v_u.head(n_x_)); + multibody::SetInputsIfNew(plant_, q_v_u.tail(n_u_), &context_); + solvers::ContactModel contact_model; + if (c3_options_.contact_model == "stewart_and_trinkle") { + contact_model = solvers::ContactModel::kStewartAndTrinkle; + } else if (c3_options_.contact_model == "anitescu") { + contact_model = solvers::ContactModel::kAnitescu; + } else { + throw std::runtime_error("unknown or unsupported contact model"); + } + + std::vector contact_points; + *output = LCSFactory::ComputeContactJacobian( + plant_, context_, contact_pairs_, + c3_options_.num_friction_directions, c3_options_.mu, contact_model); +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/c3/lcs_factory_system.h b/systems/controllers/c3/lcs_factory_system.h new file mode 100644 index 0000000000..a267d308e0 --- /dev/null +++ b/systems/controllers/c3/lcs_factory_system.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include + +#include + +#include "solvers/c3_options.h" +#include "solvers/lcs.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Outputs a lcmt_timestamped_saved_traj +class LCSFactorySystem : public drake::systems::LeafSystem { + public: + explicit LCSFactorySystem( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context& context_ad, + const std::vector> contact_geoms, + C3Options c3_options); + + const drake::systems::InputPort& get_input_port_lcs_state() const { + return this->get_input_port(lcs_state_input_port_); + } + +// const drake::systems::InputPort& get_input_port_lcs_input() const { +// return this->get_input_port(lcs_inputs_input_port_); +// } + + const drake::systems::OutputPort& get_output_port_lcs() const { + return this->get_output_port(lcs_port_); + } + + const drake::systems::OutputPort& get_output_port_lcs_contact_jacobian() const { + return this->get_output_port(lcs_contact_jacobian_port_); + } + + private: + void OutputLCS(const drake::systems::Context& context, + solvers::LCS* output_traj) const; + void OutputLCSContactJacobian(const drake::systems::Context& context, + std::pair>*) const; + + drake::systems::InputPortIndex lcs_state_input_port_; +// drake::systems::InputPortIndex lcs_inputs_input_port_; + drake::systems::OutputPortIndex lcs_port_; + drake::systems::OutputPortIndex lcs_contact_jacobian_port_; + + const drake::multibody::MultibodyPlant& plant_; + drake::systems::Context& context_; + const drake::multibody::MultibodyPlant& plant_ad_; + drake::systems::Context& context_ad_; + const std::vector> + contact_pairs_; + + C3Options c3_options_; + + // convenience for variable sizes + int n_q_; + int n_v_; + int n_x_; + int n_lambda_; + int n_u_; + int N_; + double dt_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/cassie_out_to_radio.cc b/systems/controllers/cassie_out_to_radio.cc new file mode 100644 index 0000000000..91ef4e2e6a --- /dev/null +++ b/systems/controllers/cassie_out_to_radio.cc @@ -0,0 +1,28 @@ +#include "systems/controllers/cassie_out_to_radio.h" + +namespace dairlib { +namespace systems { + +using drake::systems::kUseDefaultName; + +CassieOutToRadio::CassieOutToRadio(){ + cassie_out_input_port_ = + this->DeclareAbstractInputPort("lcmt_cassie_out", + drake::Value{}) + .get_index(); + radio_output_port_ = + this->DeclareAbstractOutputPort("lcmt_radio_out", + &CassieOutToRadio::CalcRadioOut) + .get_index(); +} + +void CassieOutToRadio::CalcRadioOut( + const drake::systems::Context& context, + dairlib::lcmt_radio_out* output) const { + const dairlib::lcmt_cassie_out* cassie_output = this->EvalInputValue( + context, cassie_out_input_port_); + *output = cassie_output->pelvis.radio; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/cassie_out_to_radio.h b/systems/controllers/cassie_out_to_radio.h new file mode 100644 index 0000000000..cf97652bee --- /dev/null +++ b/systems/controllers/cassie_out_to_radio.h @@ -0,0 +1,36 @@ +#pragma once +#include "dairlib/lcmt_cassie_out.hpp" +#include "dairlib/lcmt_radio_out.hpp" +#include + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +class CassieOutToRadio : public drake::systems::LeafSystem { + public: + CassieOutToRadio(); + + const drake::systems::InputPort& get_input_port() const { + return drake::systems::LeafSystem::get_input_port(0); + } + + const drake::systems::OutputPort& get_output_port() const { + return drake::systems::LeafSystem::get_output_port(0); + } + + protected: + void CalcRadioOut( + const drake::systems::Context& context, + dairlib::lcmt_radio_out* output) const; + + private: + bool is_abstract() const { return false;} + + int cassie_out_input_port_; + int radio_output_port_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/endeffector_position_controller.cc b/systems/controllers/endeffector_position_controller.cc index 9b15a51528..1ec1c8f5da 100644 --- a/systems/controllers/endeffector_position_controller.cc +++ b/systems/controllers/endeffector_position_controller.cc @@ -4,6 +4,19 @@ namespace dairlib{ namespace systems{ +using Eigen::VectorXd; +using Eigen::MatrixXd; +using Eigen::Quaternion; +using Eigen::Quaterniond; +using Eigen::AngleAxis; +using Eigen::AngleAxisd; +using drake::systems::LeafSystem; +using drake::systems::Context; +using drake::multibody::MultibodyPlant; +using drake::multibody::Frame; +using drake::systems::BasicVector; +using drake::VectorX; + EndEffectorPositionController::EndEffectorPositionController( const MultibodyPlant& plant, std::string ee_frame_name, Eigen::Vector3d ee_contact_frame, double k_p, double k_omega) diff --git a/systems/controllers/endeffector_position_controller.h b/systems/controllers/endeffector_position_controller.h index b17297567b..2fa4855d16 100644 --- a/systems/controllers/endeffector_position_controller.h +++ b/systems/controllers/endeffector_position_controller.h @@ -10,17 +10,6 @@ #include #include -using Eigen::VectorXd; -using Eigen::MatrixXd; -using Eigen::Quaternion; -using Eigen::Quaterniond; -using Eigen::AngleAxis; -using Eigen::AngleAxisd; -using drake::systems::LeafSystem; -using drake::systems::Context; -using drake::multibody::MultibodyPlant; -using drake::multibody::Frame; - namespace dairlib{ namespace systems{ @@ -30,10 +19,10 @@ namespace systems{ // outputs appropriate velocity (twist) as 6-vector: first three indices // are desired angular velocity, next three are desired linear velocity. -class EndEffectorPositionController : public LeafSystem { + class EndEffectorPositionController : public drake::systems::LeafSystem { public: // Constructor - EndEffectorPositionController(const MultibodyPlant& plant, + EndEffectorPositionController(const drake::multibody::MultibodyPlant& plant, std::string ee_frame_name, Eigen::Vector3d ee_contact_frame, double k_p, double k_omega); @@ -55,13 +44,13 @@ class EndEffectorPositionController : public LeafSystem { private: // Callback method called when declaring output port of the system. // Twist combines linear and angular velocities. - void CalcOutputTwist(const Context &context, - BasicVector* output) const; + void CalcOutputTwist(const drake::systems::Context &context, + drake::systems::BasicVector* output) const; - const MultibodyPlant& plant_; - const Frame& plant_world_frame_; + const drake::multibody::MultibodyPlant& plant_; + const drake::multibody::Frame& plant_world_frame_; Eigen::Vector3d ee_contact_frame_; - const Frame& ee_joint_frame_; + const drake::multibody::Frame& ee_joint_frame_; double k_p_; double k_omega_; int joint_position_measured_port_; diff --git a/systems/controllers/endeffector_velocity_controller.cc b/systems/controllers/endeffector_velocity_controller.cc index 188c55d867..3a3d9b81ba 100644 --- a/systems/controllers/endeffector_velocity_controller.cc +++ b/systems/controllers/endeffector_velocity_controller.cc @@ -1,6 +1,14 @@ #include "systems/controllers/endeffector_velocity_controller.h" #include +using Eigen::VectorXd; +using Eigen::MatrixXd; +using drake::systems::LeafSystem; +using drake::systems::Context; +using drake::systems::BasicVector; +using drake::multibody::MultibodyPlant; +using drake::multibody::Frame; + namespace dairlib{ namespace systems{ diff --git a/systems/controllers/endeffector_velocity_controller.h b/systems/controllers/endeffector_velocity_controller.h index 7a68cad445..7b49f30a7a 100644 --- a/systems/controllers/endeffector_velocity_controller.h +++ b/systems/controllers/endeffector_velocity_controller.h @@ -8,12 +8,6 @@ #include -using Eigen::VectorXd; -using Eigen::MatrixXd; -using drake::systems::LeafSystem; -using drake::systems::Context; -using drake::multibody::MultibodyPlant; -using drake::multibody::Frame; namespace dairlib{ namespace systems{ @@ -23,10 +17,10 @@ namespace systems{ // In this case, KukaIiwaVelocityController will take in desired velocities, // q, q_dot, and output an appropriate torque // \Tau = jacobian.transpose x K x desired_accelerations -class EndEffectorVelocityController : public LeafSystem { + class EndEffectorVelocityController : public drake::systems::LeafSystem { public: // Constructor - EndEffectorVelocityController(const MultibodyPlant& plant, + EndEffectorVelocityController(const drake::multibody::MultibodyPlant& plant, std::string ee_frame_name, Eigen::Vector3d ee_contact_frame, double k_d, double k_r); @@ -49,12 +43,12 @@ class EndEffectorVelocityController : public LeafSystem { // The callback called when declaring the output port of the system. // The 'output' vector is set in place and then passed out. // Think a simulink system. - void CalcOutputTorques(const Context& context, - BasicVector* output) const; + void CalcOutputTorques(const drake::systems::Context& context, + drake::systems::BasicVector* output) const; - const MultibodyPlant& plant_; + const drake::multibody::MultibodyPlant& plant_; int num_joints_; - const Frame& ee_joint_frame_; + const drake::multibody::Frame& ee_joint_frame_; Eigen::Vector3d ee_contact_frame_; int joint_position_measured_port_; int joint_velocity_measured_port_; diff --git a/systems/controllers/gravity_compensator.cc b/systems/controllers/gravity_compensator.cc new file mode 100644 index 0000000000..93fc4370bc --- /dev/null +++ b/systems/controllers/gravity_compensator.cc @@ -0,0 +1,45 @@ +#include "gravity_compensator.h" + +#include "systems/framework/timestamped_vector.h" + +using drake::multibody::MultibodyPlant; +using drake::systems::Context; +using drake::systems::LeafSystem; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + + +GravityCompensationRemover::GravityCompensationRemover( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context) + : plant_(plant), context_(context){ + + num_actuators_ = plant_.num_actuators(); + this->DeclareVectorInputPort("u, t", + TimestampedVector(num_actuators_)); + this->DeclareVectorOutputPort("u, t", + TimestampedVector(num_actuators_), + &GravityCompensationRemover::CancelGravityCompensation); +} + +void GravityCompensationRemover::CancelGravityCompensation(const drake::systems::Context& context, + TimestampedVector* output) const { + const TimestampedVector* tau = + (TimestampedVector*)this->EvalVectorInput(context, 0); + VectorXd tau_g = plant_.CalcGravityGeneralizedForces(context_); + + VectorXd compensated_tau = VectorXd::Zero(num_actuators_); + for (int i = 0; i < num_actuators_; i++){ + compensated_tau(i) = tau->GetAtIndex(i) + tau_g(i); + } + + output->SetDataVector(compensated_tau); + output->set_timestamp(tau->get_timestamp()); +} + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/gravity_compensator.h b/systems/controllers/gravity_compensator.h new file mode 100644 index 0000000000..9d7bcb8cf1 --- /dev/null +++ b/systems/controllers/gravity_compensator.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + + +#include +#include + +#include "systems/framework/output_vector.h" +#include "drake/systems/framework/leaf_system.h" + +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/context.h" + +#include +#include +#include + +using drake::multibody::MultibodyPlant; +using drake::systems::Context; +using drake::systems::LeafSystem; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +namespace dairlib { +namespace systems { + +class GravityCompensationRemover : public LeafSystem { + public: + GravityCompensationRemover( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context); + + private: + // adds gravity compensation to output + void CancelGravityCompensation(const drake::systems::Context& context, + TimestampedVector* output) const; + + // constructor variables + const MultibodyPlant& plant_; + drake::systems::Context& context_; + int num_actuators_; +}; + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/lipm_traj_gen.cc b/systems/controllers/lipm_traj_gen.cc index 33f6b6b129..a3f7163d0f 100644 --- a/systems/controllers/lipm_traj_gen.cc +++ b/systems/controllers/lipm_traj_gen.cc @@ -2,11 +2,9 @@ #include -#include #include #include - using std::cout; using std::endl; using std::string; diff --git a/systems/controllers/lipm_traj_gen.h b/systems/controllers/lipm_traj_gen.h index 43638019d2..20785f2eb5 100644 --- a/systems/controllers/lipm_traj_gen.h +++ b/systems/controllers/lipm_traj_gen.h @@ -105,7 +105,7 @@ class LIPMTrajGenerator : public drake::systems::LeafSystem { const std::vector&>>>& contact_points_in_each_state_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; bool use_com_; diff --git a/systems/controllers/osc/BUILD.bazel b/systems/controllers/osc/BUILD.bazel index deb1f3fc9d..9c6f20abd4 100644 --- a/systems/controllers/osc/BUILD.bazel +++ b/systems/controllers/osc/BUILD.bazel @@ -2,6 +2,16 @@ load("@drake//tools/lint:lint.bzl", "add_lint_tests") package(default_visibility = ["//visibility:public"]) +cc_library( + name = "inverse_dynamics_qp", + srcs = ["inverse_dynamics_qp.cc"], + hdrs = ["inverse_dynamics_qp.h"], + deps = [ + "//multibody/kinematic", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "operational_space_control", srcs = [ @@ -11,6 +21,7 @@ cc_library( "operational_space_control.h", ], deps = [ + ":inverse_dynamics_qp", ":osc_gains", ":osc_tracking_datas", "//common:eigen_utils", @@ -24,6 +35,9 @@ cc_library( "//systems/controllers:controller_failure_aggregator", "//systems/framework:vector", "@drake//:drake_shared_library", + ":end_effector_force", + ":end_effector_orientation", + ":end_effector_position", ], ) @@ -31,6 +45,7 @@ cc_library( name = "osc_tracking_datas", deps = [ ":com_tracking_data", + ":external_force_tracking_data", ":joint_space_tracking_data", ":osc_tracking_data", ":relative_transform_tracking_data", @@ -125,6 +140,18 @@ cc_library( ], ) +cc_library( + name = "external_force_tracking_data", + srcs = ["external_force_tracking_data.cc"], + hdrs = ["external_force_tracking_data.h"], + deps = [ + ":osc_tracking_data", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "osc_gains", hdrs = ["osc_gains.h"], @@ -132,3 +159,42 @@ cc_library( "@drake//:drake_shared_library", ], ) + +cc_library( + name = "end_effector_position", + srcs = ["end_effector_position.cc"], + hdrs = ["end_effector_position.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "end_effector_force", + srcs = ["end_effector_force.cc"], + hdrs = ["end_effector_force.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) + +cc_library( + name = "end_effector_orientation", + srcs = ["end_effector_orientation.cc"], + hdrs = ["end_effector_orientation.h"], + deps = [ + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "@lcm", + ], +) \ No newline at end of file diff --git a/systems/controllers/osc/end_effector_force.cc b/systems/controllers/osc/end_effector_force.cc new file mode 100644 index 0000000000..ac03669cd4 --- /dev/null +++ b/systems/controllers/osc/end_effector_force.cc @@ -0,0 +1,87 @@ +#include "end_effector_force.h" + +#include "dairlib/lcmt_radio_out.hpp" +#include "multibody/multibody_utils.h" + +using Eigen::Map; +using Eigen::Vector2d; +using Eigen::Vector3d; +using Eigen::VectorXd; +using std::string; + +using dairlib::systems::OutputVector; +using drake::multibody::RigidBodyFrame; +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteUpdateEvent; +using drake::systems::DiscreteValues; +using drake::systems::EventStatus; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +namespace dairlib { + +EndEffectorForceTrajectoryGenerator::EndEffectorForceTrajectoryGenerator() { + PiecewisePolynomial pp = PiecewisePolynomial(); + + trajectory_port_ = + this->DeclareAbstractInputPort( + "trajectory", + drake::Value>(pp)) + .get_index(); + radio_port_ = this->DeclareAbstractInputPort("lcmt_radio_out", + drake::Value{}).get_index(); + controller_switch_index_ = this->DeclareDiscreteState(VectorXd::Ones(1)); + DeclareForcedDiscreteUpdateEvent( + &EndEffectorForceTrajectoryGenerator::DiscreteVariableUpdate); + PiecewisePolynomial empty_pp_traj(Vector3d::Zero()); + Trajectory& traj_inst = empty_pp_traj; + this->DeclareAbstractOutputPort( + "end_effector_force_trajectory", traj_inst, + &EndEffectorForceTrajectoryGenerator::CalcTraj); +} + +EventStatus EndEffectorForceTrajectoryGenerator::DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const { + const auto& radio_out = this->EvalInputValue( + context, radio_port_); + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + bool using_c3 = context.get_discrete_state(controller_switch_index_)[0]; + if (!using_c3 && radio_out->channel[14] == 0) { + if (!trajectory_input.value(0).isZero() && + (context.get_time() - trajectory_input.start_time()) < 0.04) { + discrete_state->get_mutable_value(controller_switch_index_)[0] = 1; + } + } + return EventStatus::Succeeded(); +} + +void EndEffectorForceTrajectoryGenerator::CalcTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + // // Read in finite state machine + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + const auto& radio_out = this->EvalInputValue( + context, radio_port_); + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (radio_out->channel[11] || radio_out->channel[14] || + trajectory_input.value(0).isZero()) { + *casted_traj = + drake::trajectories::PiecewisePolynomial(Vector3d::Zero()); + } else { + if (context.get_discrete_state(controller_switch_index_)[0]) { + *casted_traj = *(PiecewisePolynomial*)dynamic_cast< + const PiecewisePolynomial*>(&trajectory_input); + } + } +} + +} // namespace dairlib diff --git a/systems/controllers/osc/end_effector_force.h b/systems/controllers/osc/end_effector_force.h new file mode 100644 index 0000000000..0c4468d228 --- /dev/null +++ b/systems/controllers/osc/end_effector_force.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include "systems/framework/output_vector.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class EndEffectorForceTrajectoryGenerator + : public drake::systems::LeafSystem { + public: + EndEffectorForceTrajectoryGenerator(); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + private: + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void CalcTraj(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::DiscreteStateIndex controller_switch_index_; + + drake::systems::InputPortIndex trajectory_port_; + drake::systems::InputPortIndex radio_port_; +}; + +} // namespace dairlib diff --git a/systems/controllers/osc/end_effector_orientation.cc b/systems/controllers/osc/end_effector_orientation.cc new file mode 100644 index 0000000000..0c49fbc631 --- /dev/null +++ b/systems/controllers/osc/end_effector_orientation.cc @@ -0,0 +1,57 @@ +#include "end_effector_orientation.h" + +#include "dairlib/lcmt_radio_out.hpp" + +using drake::systems::BasicVector; +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::string; + +using drake::systems::Context; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; + +namespace dairlib { + +EndEffectorOrientationTrajectoryGenerator::EndEffectorOrientationTrajectoryGenerator() { + auto pp = drake::trajectories::PiecewiseQuaternionSlerp(); + + trajectory_port_ = + this->DeclareAbstractInputPort( + "trajectory", + drake::Value>(pp)) + .get_index(); + radio_port_ = this->DeclareAbstractInputPort("lcmt_radio_out", + drake::Value{}).get_index(); + PiecewiseQuaternionSlerp empty_slerp_traj; + Trajectory& traj_inst = empty_slerp_traj; + this->DeclareAbstractOutputPort("end_effector_orientation", traj_inst, + &EndEffectorOrientationTrajectoryGenerator::CalcTraj) + .get_index(); +} + +void EndEffectorOrientationTrajectoryGenerator::CalcTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + const auto& radio_out = this->EvalInputValue( + context, radio_port_); + auto* casted_traj = (PiecewiseQuaternionSlerp*)dynamic_cast< + PiecewiseQuaternionSlerp*>(traj); + if (radio_out->channel[14] and track_orientation_) { + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + *casted_traj = *(PiecewiseQuaternionSlerp*)dynamic_cast< + const PiecewiseQuaternionSlerp*>(&trajectory_input); + } else { + PiecewiseQuaternionSlerp result; + Eigen::VectorXd neutral_quaternion = VectorXd::Zero(4); + neutral_quaternion(0) = 1; + result = drake::trajectories::PiecewiseQuaternionSlerp( + {0, 1}, + {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)}); + *casted_traj = result; + } +} + +} // namespace dairlib diff --git a/systems/controllers/osc/end_effector_orientation.h b/systems/controllers/osc/end_effector_orientation.h new file mode 100644 index 0000000000..16c06d1378 --- /dev/null +++ b/systems/controllers/osc/end_effector_orientation.h @@ -0,0 +1,38 @@ +#pragma once + +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class EndEffectorOrientationTrajectoryGenerator + : public drake::systems::LeafSystem { + public: + EndEffectorOrientationTrajectoryGenerator(); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + void SetTrackOrientation(bool track_orientation) { + track_orientation_ = track_orientation; + } + + private: + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void CalcTraj(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + bool track_orientation_; + + drake::systems::InputPortIndex trajectory_port_; + drake::systems::InputPortIndex radio_port_; +}; + +} // namespace dairlib diff --git a/systems/controllers/osc/end_effector_position.cc b/systems/controllers/osc/end_effector_position.cc new file mode 100644 index 0000000000..b11a9f86b4 --- /dev/null +++ b/systems/controllers/osc/end_effector_position.cc @@ -0,0 +1,157 @@ +/**This system is used by the osc diagram to read an end effector trajectory + * from an lcm message and pass it onto the OSC class. It allows for teleop to + * track the current end effector position or a neutral position based on the + * teleop_neutral_pose flag. */ +#include "end_effector_position.h" +#include +#include "dairlib/lcmt_radio_out.hpp" +#include "multibody/multibody_utils.h" + +using Eigen::Map; +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::string; + +using dairlib::systems::OutputVector; +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteUpdateEvent; +using drake::systems::DiscreteValues; +using drake::systems::EventStatus; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +namespace dairlib { + +EndEffectorPositionTrajectoryGenerator::EndEffectorPositionTrajectoryGenerator( + const MultibodyPlant& plant, Context* context, + const Eigen::VectorXd& neutral_pose, bool teleop_neutral_pose, + const std::string& end_effector_name) + : plant_(plant), context_(context), end_effector_name_(end_effector_name) { + // Input/Output Setup + state_port_ = this->DeclareVectorInputPort( + "x", OutputVector(plant_.num_positions(), + plant_.num_velocities(), + plant_.num_actuators())) + .get_index(); + PiecewisePolynomial pp = PiecewisePolynomial(); + + trajectory_port_ = + this->DeclareAbstractInputPort( + "trajectory", + drake::Value>(pp)) + .get_index(); + radio_port_ = this->DeclareAbstractInputPort("lcmt_radio_out", + drake::Value{}).get_index(); + + PiecewisePolynomial empty_pp_traj(neutral_pose); + Trajectory& traj_inst = empty_pp_traj; + if (teleop_neutral_pose) { + this->DeclareAbstractOutputPort( + "end_effector_trajectory", traj_inst, + &EndEffectorPositionTrajectoryGenerator::CalcPoseShiftingTraj); + } + else { + this->DeclareAbstractOutputPort( + "end_effector_trajectory", traj_inst, + &EndEffectorPositionTrajectoryGenerator::CalcNeutralPoseBasedTraj); + } +} + +void EndEffectorPositionTrajectoryGenerator::SetRemoteControlParameters( + const Eigen::Vector3d& neutral_pose, double x_scale, double y_scale, + double z_scale) { + neutral_pose_ = neutral_pose; + shifting_pose_ = neutral_pose; + x_scale_ = x_scale; + y_scale_ = y_scale; + z_scale_ = z_scale; +} + +void EndEffectorPositionTrajectoryGenerator::CalcNeutralPoseBasedTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + const auto& radio_out = this->EvalInputValue( + context, radio_port_); + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (radio_out->channel[14]) { + PiecewisePolynomial result; + + // Compute the target position based on an offset from neutral pose. + VectorXd y_0 = neutral_pose_; + y_0(0) += radio_out->channel[0] * x_scale_; + y_0(1) += radio_out->channel[1] * y_scale_; + y_0(2) += radio_out->channel[2] * z_scale_; + + result = drake::trajectories::PiecewisePolynomial(y_0); + *casted_traj = result; + } else { + if (trajectory_input.value(0).isZero()) { + } else { + *casted_traj = *(PiecewisePolynomial*)dynamic_cast< + const PiecewisePolynomial*>(&trajectory_input); + } + } +} + +void EndEffectorPositionTrajectoryGenerator::CalcPoseShiftingTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + const auto& trajectory_input = + this->EvalAbstractInput(context, trajectory_port_) + ->get_value>(); + const auto& radio_out = this->EvalInputValue( + context, radio_port_); + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (radio_out->channel[14]) { + if (!was_in_teleop_mode_) { + // Update the shifting position to the current position -- this means + // every time teleop mode is newly entered, the robot will stay where it + // is instead of snapping to the neutral position elsewhere. + const OutputVector* franka_state = + (OutputVector*)this->EvalVectorInput(context, state_port_); + VectorXd q_franka = franka_state->GetPositions(); + multibody::SetPositionsIfNew(plant_, q_franka, + context_); + auto end_effector_pose = plant_.EvalBodyPoseInWorld( + *context_, plant_.GetBodyByName(end_effector_name_)); + shifting_pose_ = end_effector_pose.translation(); + } + was_in_teleop_mode_ = true; + + PiecewisePolynomial result; + + // Compute the target position by stepping the held pose. + if (std::abs(radio_out->channel[0]) > 0.01) { + shifting_pose_(0) += radio_out->channel[0] * x_scale_; + } + if (std::abs(radio_out->channel[1]) > 0.01) { + shifting_pose_(1) += radio_out->channel[1] * y_scale_; + } + if (std::abs(radio_out->channel[2]) > 0.01) { + shifting_pose_(2) += radio_out->channel[2] * z_scale_; + } + VectorXd y_0 = shifting_pose_; + + result = drake::trajectories::PiecewisePolynomial(y_0); + *casted_traj = result; + } + else { + was_in_teleop_mode_ = false; + if (trajectory_input.value(0).isZero()) { + } else { + *casted_traj = *(PiecewisePolynomial*)dynamic_cast< + const PiecewisePolynomial*>(&trajectory_input); + } + } +} + +} // namespace dairlib diff --git a/systems/controllers/osc/end_effector_position.h b/systems/controllers/osc/end_effector_position.h new file mode 100644 index 0000000000..aa129c08f4 --- /dev/null +++ b/systems/controllers/osc/end_effector_position.h @@ -0,0 +1,67 @@ +/**This system is used by the osc diagram to read an end effector trajectory + * from an lcm message and pass it onto the OSC class. It allows for teleop to + * track the current end effector position or a neutral position based on the + * teleop_neutral_pose flag. */ +#pragma once + +#include +#include "systems/framework/output_vector.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +class EndEffectorPositionTrajectoryGenerator + : public drake::systems::LeafSystem { + public: + EndEffectorPositionTrajectoryGenerator( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + const Eigen::VectorXd& neutral_pose, + bool teleop_neutral_pose, + const std::string& end_effector_name); + + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_port_); + } + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_port_); + } + const drake::systems::InputPort& get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + + void SetRemoteControlParameters(const Eigen::Vector3d& neutral_pose, + double x_scale, double y_scale, + double z_scale); + + private: + drake::systems::EventStatus DiscreteVariableUpdate( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + void CalcNeutralPoseBasedTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + void CalcPoseShiftingTraj( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::InputPortIndex state_port_; + drake::systems::InputPortIndex trajectory_port_; + drake::systems::InputPortIndex radio_port_; + + const drake::multibody::MultibodyPlant& plant_; + drake::systems::Context* context_; + std::string end_effector_name_; + Eigen::Vector3d neutral_pose_ = {0.55, 0, 0.40}; + mutable Eigen::Vector3d shifting_pose_ = {0.55, 0, 0.40}; + mutable bool was_in_teleop_mode_ = false; + double x_scale_; + double y_scale_; + double z_scale_; +}; + +} // namespace dairlib diff --git a/systems/controllers/osc/external_force_tracking_data.cc b/systems/controllers/osc/external_force_tracking_data.cc new file mode 100644 index 0000000000..bdea677c65 --- /dev/null +++ b/systems/controllers/osc/external_force_tracking_data.cc @@ -0,0 +1,43 @@ +#include "external_force_tracking_data.h" + +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::VectorXd; +using std::string; +using std::vector; + +using drake::multibody::JacobianWrtVariable; +using drake::multibody::MultibodyPlant; +using drake::systems::Context; + +namespace dairlib::systems::controllers { + +ExternalForceTrackingData::ExternalForceTrackingData( + const string& name, const MatrixXd& W, + const MultibodyPlant& plant_w_spr, + const MultibodyPlant& plant_wo_spr, const std::string& body_name, + const Vector3d& pt_on_body) + : name_(name), + plant_w_spr_(plant_w_spr), + plant_wo_spr_(plant_wo_spr), + world_w_spr_(plant_w_spr_.world_frame()), + world_wo_spr_(plant_wo_spr_.world_frame()), + body_frame_w_spr_(&plant_w_spr_.GetBodyByName(body_name).body_frame()), + body_frame_wo_spr_(&plant_wo_spr_.GetBodyByName(body_name).body_frame()), + pt_on_body_(pt_on_body), + W_(W) { + lambda_des_ = Vector3d::Zero(); +} + +void ExternalForceTrackingData::Update( + const Eigen::VectorXd& x_w_spr, + const drake::systems::Context& context_w_spr, + const Eigen::VectorXd& x_wo_spr, + const drake::systems::Context& context_wo_spr, + const drake::trajectories::Trajectory& traj, double t) { + DRAKE_DEMAND(traj.rows() == 3); + lambda_des_ = traj.value(t); +} + +} // namespace dairlib::systems::controllers \ No newline at end of file diff --git a/systems/controllers/osc/external_force_tracking_data.h b/systems/controllers/osc/external_force_tracking_data.h new file mode 100644 index 0000000000..d27996ae55 --- /dev/null +++ b/systems/controllers/osc/external_force_tracking_data.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +namespace dairlib { +namespace systems { +namespace controllers { + +/// ExternalForceTrackingData +/// Force tracking objective. Used to track desired external forces. Requires +/// contact points on the MultibodyPlant where contact forces enter the dynamics +class ExternalForceTrackingData { + public: + ExternalForceTrackingData( + const std::string& name, const Eigen::MatrixXd& W, + const drake::multibody::MultibodyPlant& plant_w_spr, + const drake::multibody::MultibodyPlant& plant_wo_spr, + const std::string& body_name, const Eigen::Vector3d& pt_on_body); + + const Eigen::MatrixXd& GetWeight() const { return W_; } + const Eigen::VectorXd& GetLambdaDes() const { return lambda_des_; } + const std::string& GetName() const { return name_; }; + const Eigen::Vector3d& GetPointOnBody() const { return pt_on_body_; }; + const drake::multibody::Frame& GetBodyFrame() const { + return *body_frame_wo_spr_; + }; + + const drake::multibody::MultibodyPlant& plant_w_spr() const { + return plant_w_spr_; + }; + const drake::multibody::MultibodyPlant& plant_wo_spr() const { + return plant_wo_spr_; + }; + void Update(const Eigen::VectorXd& x_w_spr, + const drake::systems::Context& context_w_spr, + const Eigen::VectorXd& x_wo_spr, + const drake::systems::Context& context_wo_spr, + const drake::trajectories::Trajectory& traj, double t); + + protected: + private: + std::string name_; + + const drake::multibody::MultibodyPlant& plant_w_spr_; + const drake::multibody::MultibodyPlant& plant_wo_spr_; + // World frames + const drake::multibody::RigidBodyFrame& world_w_spr_; + const drake::multibody::RigidBodyFrame& world_wo_spr_; + + const drake::multibody::RigidBodyFrame* body_frame_w_spr_; + const drake::multibody::RigidBodyFrame* body_frame_wo_spr_; + const Eigen::Vector3d pt_on_body_; + + Eigen::VectorXd lambda_des_; + Eigen::MatrixXd W_; +}; + +} // namespace controllers +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/osc/inverse_dynamics_qp.cc b/systems/controllers/osc/inverse_dynamics_qp.cc new file mode 100644 index 0000000000..54771032f3 --- /dev/null +++ b/systems/controllers/osc/inverse_dynamics_qp.cc @@ -0,0 +1,232 @@ +#include "inverse_dynamics_qp.h" + +#include "multibody/multibody_utils.h" + +namespace dairlib { +namespace systems { +namespace controllers { + +using std::string; +using std::unique_ptr; +using std::vector; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +using multibody::KinematicEvaluator; +using multibody::KinematicEvaluatorSet; +using multibody::SetPositionsAndVelocitiesIfNew; +using multibody::WorldPointEvaluator; + +using drake::multibody::MultibodyPlant; +using drake::solvers::MathematicalProgram; +using drake::solvers::VariableRefList; +using drake::solvers::VectorXDecisionVariable; +using drake::systems::Context; + +InverseDynamicsQp::InverseDynamicsQp(const MultibodyPlant& plant, + Context* context) + : plant_(plant), + context_(context), + nq_(plant.num_positions()), + nv_(plant.num_velocities()), + nu_(plant.num_actuated_dofs()) {} + +void InverseDynamicsQp::AddHolonomicConstraint( + unique_ptr> eval) { + DRAKE_DEMAND(&eval->plant() == &plant_); + DRAKE_DEMAND(not built_); + DRAKE_DEMAND(holonomic_constraints_ == nullptr); + DRAKE_DEMAND(nh_ == 0); + + holonomic_constraints_ = std::move(eval); + nh_ = holonomic_constraints_->count_full(); +} + +void InverseDynamicsQp::AddContactConstraint( + const string& name, unique_ptr> eval, + double friction_coefficient) { + DRAKE_DEMAND(not built_); + DRAKE_DEMAND(friction_coefficient >= 0); + DRAKE_DEMAND(contact_constraint_evaluators_.count(name) == 0); + DRAKE_DEMAND(&eval->plant() == &plant_); + + mu_map_.insert({name, friction_coefficient}); + lambda_c_start_.insert({name, nc_}); + Jc_active_start_.insert({name, nc_active_}); + contact_constraint_evaluators_.insert({name, std::move(eval)}); + + nc_ += contact_constraint_evaluators_.at(name)->num_full(); + nc_active_ += contact_constraint_evaluators_.at(name)->num_active(); +} + +void InverseDynamicsQp::AddExternalForce( + const string& name, unique_ptr> eval) { + DRAKE_DEMAND(not built_); + DRAKE_DEMAND(&eval->plant() == &plant_); + DRAKE_DEMAND(external_force_evaluators_.count(name) == 0); + + external_force_evaluators_.insert({name, std::move(eval)}); + lambda_e_start_and_size_.insert( + {name, {ne_, external_force_evaluators_.at(name)->num_full()}}); + ne_ += external_force_evaluators_.at(name)->num_full(); +} + +void InverseDynamicsQp::Build() { + DRAKE_DEMAND(not built_); + + dv_ = prog_.NewContinuousVariables(nv_, "dv"); + u_ = prog_.NewContinuousVariables(nu_, "u"); + lambda_h_ = prog_.NewContinuousVariables(nh_, "lambda_holonomic"); + lambda_c_ = prog_.NewContinuousVariables(nc_, "lambda_contact"); + lambda_e_ = prog_.NewContinuousVariables(ne_, "lambda_external"); + epsilon_ = prog_.NewContinuousVariables(nc_active_, "soft_constraint_slack"); + + dynamics_c_ = + prog_ + .AddLinearEqualityConstraint( + MatrixXd::Zero(nv_, nv_ + nu_ + nh_ + nc_ + ne_), + VectorXd::Zero(nv_), {dv_, u_, lambda_h_, lambda_c_, lambda_e_}) + .evaluator(); + + holonomic_c_ = prog_ + .AddLinearEqualityConstraint(MatrixXd::Zero(nh_, nv_), + VectorXd::Zero(nh_), dv_) + .evaluator(); + + contact_c_ = prog_ + .AddLinearEqualityConstraint( + MatrixXd::Zero(nc_active_, nv_ + nc_active_), + VectorXd::Zero(nc_active_), {dv_, epsilon_}) + .evaluator(); + + for (const auto& [cname, eval] : contact_constraint_evaluators_) { + double mu = mu_map_.at(cname); + MatrixXd A = MatrixXd(5, 3); + A << -1, 0, mu, 0, -1, mu, 1, 0, mu, 0, 1, mu, 0, 0, 1; + lambda_c_friction_cone_.insert( + {cname, + prog_ + .AddLinearConstraint( + A, VectorXd::Zero(5), + VectorXd::Constant(5, std::numeric_limits::infinity()), + lambda_c_.segment(lambda_c_start_.at(cname), 3)) + .evaluator()}); + } + + if (with_input_constraints_) { + VectorXd u_min(nu_); + VectorXd u_max(nu_); + for (drake::multibody::JointActuatorIndex i(0); i < nu_; ++i) { + u_min[i] = -plant_.get_joint_actuator(i).effort_limit(); + u_max[i] = plant_.get_joint_actuator(i).effort_limit(); + } + input_limit_c_ = + prog_.AddBoundingBoxConstraint(u_min, u_max, u_).evaluator(); + } + + if (with_acceleration_constraints_) { + VectorXd ddq_min = VectorXd::Zero(nq_); + VectorXd ddq_max = VectorXd::Zero(nq_); + for (drake::multibody::JointIndex i(0); i < nq_; ++i) { + if (plant_.get_joint(i).acceleration_lower_limits().size() != 0) { + ddq_min[i] = plant_.get_joint(i).acceleration_lower_limits()[0]; + ddq_max[i] = plant_.get_joint(i).acceleration_upper_limits()[0]; + } + } + if (ddq_max.isZero()) { + throw std::runtime_error( + "Attempting to set acceleration limits when acceleration limits have " + "not been defined for the plant."); + } + acceleration_limit_c_ = + prog_.AddBoundingBoxConstraint(ddq_min, ddq_max, dv_).evaluator(); + } + built_ = true; +} + +void InverseDynamicsQp::AddQuadraticCost(const string& name, const MatrixXd& Q, + const VectorXd& b, + const VariableRefList& vars) { + DRAKE_DEMAND(built_); + DRAKE_DEMAND(all_costs_.count(name) == 0); + all_costs_.insert({name, prog_.AddQuadraticCost(Q, b, vars).evaluator()}); +} + +void InverseDynamicsQp::AddQuadraticCost(const string& name, const MatrixXd& Q, + const VectorXd& b, + const VectorXDecisionVariable& vars) { + DRAKE_DEMAND(built_); + DRAKE_DEMAND(all_costs_.count(name) == 0); + all_costs_.insert({name, prog_.AddQuadraticCost(Q, b, vars).evaluator()}); +} + +void InverseDynamicsQp::UpdateDynamics( + const VectorXd& x, const vector& active_contact_constraints, + const vector& active_external_forces) { + SetPositionsAndVelocitiesIfNew(plant_, x, context_); + + MatrixXd M(nv_, nv_); + VectorXd bias(nv_); + MatrixXd B = plant_.MakeActuationMatrix(); + VectorXd grav = plant_.CalcGravityGeneralizedForces(*context_); + + plant_.CalcMassMatrix(*context_, &M); + plant_.CalcBiasTerm(*context_, &bias); + + if (with_gravity_compensation_) { + bias = bias - grav; + } + + MatrixXd Jh = MatrixXd::Zero(nh_, nv_); + VectorXd Jh_dot_v = VectorXd::Zero(nh_); + + if (holonomic_constraints_ != nullptr) { + Jh = holonomic_constraints_->EvalFullJacobian(*context_); + Jh_dot_v = holonomic_constraints_->EvalFullJacobianDotTimesV(*context_); + } + + MatrixXd Jc_active = MatrixXd::Zero(nc_active_, nv_); + VectorXd Jc_active_dot_v = VectorXd::Zero(nc_active_); + MatrixXd Jc = MatrixXd::Zero(nc_, nv_); + MatrixXd Je = MatrixXd::Zero(ne_, nv_); + + for (const auto& c : active_contact_constraints) { + DRAKE_DEMAND(contact_constraint_evaluators_.count(c) > 0); + const auto& evaluator = contact_constraint_evaluators_.at(c); + Jc.block(lambda_c_start_.at(c), 0, 3, nv_) = + evaluator->EvalFullJacobian(*context_); + int start = Jc_active_start_.at(c); + for (int i = 0; i < evaluator->num_active(); ++i) { + Jc_active.row(start + i) = + Jc.row(lambda_c_start_.at(c) + evaluator->active_inds().at(i)); + Jc_active_dot_v.segment(start, evaluator->num_active()) = + evaluator->EvalActiveJacobianDotTimesV(*context_); + } + } + for (const auto& e : active_external_forces) { + const auto& [start, size] = lambda_e_start_and_size_.at(e); + Je.block(start, 0, size, nv_) = + external_force_evaluators_.at(e)->EvalFullJacobian(*context_); + } + + MatrixXd A_dyn = MatrixXd::Zero(nv_, nv_ + nu_ + nh_ + nc_ + ne_); + A_dyn.block(0, 0, nv_, nv_) = M; + A_dyn.block(0, nv_, nv_, nu_) = -B; + A_dyn.block(0, nv_ + nu_, nv_, nh_) = -Jh.transpose(); + A_dyn.block(0, nv_ + nu_ + nh_, nv_, nc_) = -Jc.transpose(); + A_dyn.block(0, nv_ + nu_ + nh_ + nc_, nv_, ne_) = -Je.transpose(); + + MatrixXd A_c = MatrixXd::Zero(nc_active_, nv_ + nc_active_); + A_c.block(0, 0, nc_active_, nv_) = Jc_active; + A_c.block(0, nv_, nc_active_, nc_active_) = + MatrixXd::Identity(nc_active_, nc_active_); + + dynamics_c_->UpdateCoefficients(A_dyn, -bias); + holonomic_c_->UpdateCoefficients(Jh, -Jh_dot_v); + contact_c_->UpdateCoefficients(A_c, -Jc_active_dot_v); +} + +} // namespace controllers +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/osc/inverse_dynamics_qp.h b/systems/controllers/osc/inverse_dynamics_qp.h new file mode 100644 index 0000000000..4741e5d544 --- /dev/null +++ b/systems/controllers/osc/inverse_dynamics_qp.h @@ -0,0 +1,341 @@ +#pragma once +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" + +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/solvers/mathematical_program.h" + +namespace dairlib { +namespace systems { +namespace controllers { + +using CostMap = + std::unordered_map>; + +using FrictionConeMap = + std::unordered_map>; + +using ContactMap = std::unordered_map< + std::string, std::unique_ptr>>; + +/*! + * Wrapper class for handling kinematics and dynamics for a quadratic program + * which computes dynamically consistent accelerations, inputs, + * and constraint forces (including contacts) which minimize some combined + * cost on these variables. + * + * Designed to be used as a back-end for operational space control, but + * applicable to other model-based instantaneous QP controllers. + * + * Constructs a QP of the form + * + * minimize C₁(v̇) + C₂(u), C₃(λₕ) + C₄(λ_c) + C₄(λₑ) + * subject to Jₕv̇ + J̇ₕ = 0 + * J_cv̇ + J̇_c = 0 + * Mv̇ + c = Bu + Jₕᵀλₕ + J_cᵀλ_c + Jₑᵀλₑ + * λ_c ∈ FrictionCone + * + * Where + * + * Cᵢ are arbitrary user-defined convex quadratic costs, + * v̇ are generalized accelerations, + * u are actuation efforts, + * λₕ are constraint forces for general holonomic constraints such linkages, + * λ_c are constraint forces for contact constraints + * λₑ are external forces, such as contact forces, which do not constrain the + * robot's motion (like force on the end effector of a robot arm) + * + */ +class InverseDynamicsQp { + public: + InverseDynamicsQp(const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context); + + /*! + * @brief Adds the set of holonomic constraints given by eval to the QP. + */ + void AddHolonomicConstraint( + std::unique_ptr> eval); + + /*! + * Adds a contact constraint + * @param name name of the contact constraint + * @param eval WorldPointEvaluator describing the contact point + * @param friction_coefficient friction coefficient of the friction cone for + * the associated contact force + */ + void AddContactConstraint( + const std::string& name, + std::unique_ptr> eval, + double friction_coefficient); + + /*! + * Adds an external force to the dynamics. + * + * Note: By default, no constraints are imposed on the external forces. If + * external forces are declared without corresponding constraints/costs + * from the upstream controller, then unexpected behavior may occur. + * (i.e. Unconstrained forces effectively act as input variables.) + * + * @param name name of the external force + * @param eval WorldPointEvaluator for the associated jacobian + */ + void AddExternalForce( + const std::string& name, + std::unique_ptr> eval); + + /*! + * Adds the quadratic cost 1/2 xᵀQx + bᵀx to the underlying QP + * + * Must be called AFTER build + * + * @param name name of the cost (must be unique) + * @param Q PSD cost hessian + * @param b linear term + * @param vars decision variables representing x + */ + void AddQuadraticCost(const std::string& name, const Eigen::MatrixXd& Q, + const Eigen::VectorXd& b, + const drake::solvers::VectorXDecisionVariable& vars); + + /*! + * See above + */ + void AddQuadraticCost(const std::string& name, const Eigen::MatrixXd& Q, + const Eigen::VectorXd& b, + const drake::solvers::VariableRefList& vars); + + /*! + * Builds the underlying QP + */ + void Build(); + + /*! + * @return the total dimension of the (stacked) contact forces. + * This is greater than or equal to the number of active rows in the + * contact constraint (see kinematic_evaluator.h). + */ + [[nodiscard]] int nc() const { return nc_; } + + /*! + * @return the total dimension of the holonomic constraint forces. + * Equal to the number of rows in the holonomic constraint. + */ + [[nodiscard]] int nh() const { return nh_; } + + /*! + * @return the total dimension of the external forces. + * This is equal to the number of rows in the external force constraint. + */ + [[nodiscard]] int ne() const { return ne_; } + + /*! + * @return the total number of active rows in the contact constraint for all + * contacts. Potentially less than nc() to avoid redundant constraints. + */ + [[nodiscard]] int nc_active() const { return nc_active_; } + + /* + * N.B. To avoid overhead in the OSC loop, we don't check the if the QP is + * built before these are called, but the decision variables are empty + * until then! + */ + [[nodiscard]] const drake::solvers::VectorXDecisionVariable& dv() const { + return dv_; + } + [[nodiscard]] const drake::solvers::VectorXDecisionVariable& u() const { + return u_; + } + [[nodiscard]] const drake::solvers::VectorXDecisionVariable& lambda_h() + const { + return lambda_h_; + } + [[nodiscard]] const drake::solvers::VectorXDecisionVariable& lambda_c() + const { + return lambda_c_; + } + [[nodiscard]] const drake::solvers::VectorXDecisionVariable& lambda_e() + const { + return lambda_e_; + } + [[nodiscard]] const drake::solvers::VectorXDecisionVariable& epsilon() const { + return epsilon_; + } + + /*! + * @return a const-reference to the underlying MathematicalProgram + */ + [[nodiscard]] const drake::solvers::MathematicalProgram& get_prog() const { + return prog_; + } + + /*! + * @return a mutable reference to the underlying MathematicalProgram + */ + [[nodiscard]] drake::solvers::MathematicalProgram& get_mutable_prog() { + return prog_; + } + + /*! + * @brief gets a const reference WorldPointEvaluator for the contact with + * the given name + */ + [[nodiscard]] const multibody::WorldPointEvaluator& + get_contact_evaluator(const std::string& name) const { + return *contact_constraint_evaluators_.at(name); + } + + /*! + * @brief gets a const reference to the KinematicEvaluatorSet associated + * with the holonomic constraints + */ + [[nodiscard]] const multibody::KinematicEvaluatorSet& + get_holonomic_evaluators() const { + return *holonomic_constraints_; + } + + /*! + * @brief updates the coefficients of the cost with the given name to + * 1/2 xᵀQx + bᵀx + c + */ + void UpdateCost(const std::string& name, const Eigen::MatrixXd& Q, + const Eigen::VectorXd& b, double c = 0) { + all_costs_.at(name)->UpdateCoefficients(Q, b, c, true); + }; + + /*! + * updates the dynamics and contact constraints to reflect a given + * state and contact configuration. + * @param x the robot state [q, v] + * @param active_contact_constraints the names of all contact constraints + * currently in contact + * @param active_external_forces the names of all the external forces + * currently being applied + */ + void UpdateDynamics( + const Eigen::VectorXd& x, + const std::vector& active_contact_constraints, + const std::vector& active_external_forces); + + /*! + * @brief gets the drake QuadraticCost evaluator associated with a given cost + */ + [[nodiscard]] const drake::solvers::QuadraticCost& get_cost_evaluator( + const std::string& name) const { + return *all_costs_.at(name); + } + + /*! + * @brief checks if this QP has a cost with the given name + */ + bool has_cost_named(const std::string& name) { + return all_costs_.count(name) > 0; + } + + /*! + * @brief utility functions to enable/disable gravity compensation + */ + void DisableGravityCompensation() { with_gravity_compensation_ = false; } + void EnableGravityCompensation() { with_gravity_compensation_ = true; } + bool HasGravityCompensation() { return with_gravity_compensation_; } + + /*! + * @brief utility functions to enable/disable input constraints + */ + void DisableInputConstraints() { with_input_constraints_ = false; } + void EnableInputConstraints() { with_input_constraints_ = true; } + bool HasInputConstraints() { return with_input_constraints_; } + + /*! + * @brief utility functions to enable/disable acceleration constraints + */ + void DisableAccelerationConstraints() { + with_acceleration_constraints_ = false; + } + void EnableAccelerationConstraints() { + with_acceleration_constraints_ = true; + } + bool HasAccelerationConstraints() { return with_acceleration_constraints_; } + + private: + // Multibody Dynamics + const drake::multibody::MultibodyPlant& plant_; + drake::systems::Context* context_; + + // Holonomic constraints are bilateral constraints that are always active + std::unique_ptr> + holonomic_constraints_ = nullptr; + + // Contact constraints are unilateral constraints with an associated + // contact force which obeys the friction cone + ContactMap contact_constraint_evaluators_{}; + std::unordered_map lambda_c_start_; + std::unordered_map Jc_active_start_; + std::unordered_map mu_map_; + + // External forces are reaction forces we want to "track," but which do not + // constrain the robot's motion + std::unordered_map< + std::string, std::unique_ptr>> + external_force_evaluators_{}; + + std::unordered_map> lambda_e_start_and_size_; + + // Size of position, velocity, and input of the plant + int nq_; + int nv_; + int nu_; + + // Size of constraints and external forces + int nh_ = 0; + int ne_ = 0; + int nc_ = 0; + int nc_active_ = 0; + + // Mathematical Program + drake::solvers::MathematicalProgram prog_; + + // Decision Variables + drake::solvers::VectorXDecisionVariable u_{}; + drake::solvers::VectorXDecisionVariable dv_{}; + drake::solvers::VectorXDecisionVariable lambda_h_{}; + drake::solvers::VectorXDecisionVariable lambda_c_{}; + drake::solvers::VectorXDecisionVariable lambda_e_{}; + drake::solvers::VectorXDecisionVariable epsilon_{}; + + // Costs + CostMap all_costs_; + + // Friction Cone Constraints + FrictionConeMap lambda_c_friction_cone_; + FrictionConeMap lambda_e_friction_cone_; + + // Dynamics + std::shared_ptr dynamics_c_; + std::shared_ptr holonomic_c_; + std::shared_ptr contact_c_; + + // input limits + std::shared_ptr input_limit_c_; + + // acceleration limits + std::shared_ptr acceleration_limit_c_; + + // Bookkeeping + bool built_ = false; + + // Flag to indicate if gravity compensation is enabled. Default is true. + bool with_gravity_compensation_ = true; + + // Flag to indicate if input constraints are enabled. Default is true. + bool with_input_constraints_ = true; + + // Flag to indicate if acceleration constraints are enabled. Default is false. + bool with_acceleration_constraints_ = false; +}; + +} // namespace controllers +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/controllers/osc/operational_space_control.cc b/systems/controllers/osc/operational_space_control.cc index 766b987b72..6d5c2a7247 100644 --- a/systems/controllers/osc/operational_space_control.cc +++ b/systems/controllers/osc/operational_space_control.cc @@ -48,33 +48,24 @@ using multibody::SetVelocitiesIfNew; using multibody::WorldPointEvaluator; OperationalSpaceControl::OperationalSpaceControl( - const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr, - drake::systems::Context* context_w_spr, - drake::systems::Context* context_wo_spr, + const MultibodyPlant& plant, + drake::systems::Context* context, bool used_with_finite_state_machine) - : plant_w_spr_(plant_w_spr), - plant_wo_spr_(plant_wo_spr), - context_w_spr_(context_w_spr), - context_wo_spr_(context_wo_spr), - world_w_spr_(plant_w_spr_.world_frame()), - world_wo_spr_(plant_wo_spr_.world_frame()), - used_with_finite_state_machine_(used_with_finite_state_machine) { + : plant_(plant), + context_(context), + used_with_finite_state_machine_(used_with_finite_state_machine), + id_qp_(plant_, context_) { this->set_name("OSC"); - n_q_ = plant_wo_spr.num_positions(); - n_v_ = plant_wo_spr.num_velocities(); - n_u_ = plant_wo_spr.num_actuators(); - - int n_q_w_spr = plant_w_spr.num_positions(); - int n_v_w_spr = plant_w_spr.num_velocities(); - int n_u_w_spr = plant_w_spr.num_actuators(); + n_q_ = plant.num_positions(); + n_v_ = plant.num_velocities(); + n_u_ = plant.num_actuated_dofs(); // Input/Output Setup - state_port_ = - this->DeclareVectorInputPort( - "x, u, t", OutputVector(n_q_w_spr, n_v_w_spr, n_u_w_spr)) - .get_index(); + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(n_q_, n_v_, n_u_)) + .get_index(); + if (used_with_finite_state_machine) { fsm_port_ = this->DeclareVectorInputPort("fsm", BasicVector(1)).get_index(); @@ -92,10 +83,10 @@ OperationalSpaceControl::OperationalSpaceControl( prev_event_time_idx_ = this->DeclareDiscreteState(VectorXd::Zero(1)); } - osc_output_port_ = this->DeclareVectorOutputPort( - "u, t", TimestampedVector(n_u_w_spr), - &OperationalSpaceControl::CalcOptimalInput) - .get_index(); + osc_output_port_ = + this->DeclareVectorOutputPort("u, t", TimestampedVector(n_u_), + &OperationalSpaceControl::CalcOptimalInput) + .get_index(); osc_debug_port_ = this->DeclareAbstractOutputPort( "lcmt_osc_debug", &OperationalSpaceControl::AssignOscLcmOutput) @@ -106,28 +97,12 @@ OperationalSpaceControl::OperationalSpaceControl( &OperationalSpaceControl::CheckTracking) .get_index(); - const std::map& vel_map_wo_spr = - multibody::MakeNameToVelocitiesMap(plant_wo_spr); - - // Initialize the mapping from spring to no spring - map_position_from_spring_to_no_spring_ = - CreateWithSpringsToWithoutSpringsMapPos(plant_w_spr, plant_wo_spr); - map_velocity_from_spring_to_no_spring_ = - CreateWithSpringsToWithoutSpringsMapVel(plant_w_spr, plant_wo_spr); - - // Get input limits - VectorXd u_min(n_u_); - VectorXd u_max(n_u_); - for (JointActuatorIndex i(0); i < n_u_; ++i) { - u_min[i] = -plant_wo_spr_.get_joint_actuator(i).effort_limit(); - u_max[i] = plant_wo_spr_.get_joint_actuator(i).effort_limit(); - } - u_min_ = u_min; - u_max_ = u_max; + const std::map& vel_map = + multibody::MakeNameToVelocitiesMap(plant); n_revolute_joints_ = 0; - for (JointIndex i(0); i < plant_wo_spr_.num_joints(); ++i) { - const drake::multibody::Joint& joint = plant_wo_spr_.get_joint(i); + for (JointIndex i(0); i < plant_.num_joints(); ++i) { + const drake::multibody::Joint& joint = plant_.get_joint(i); if (joint.type_name() == "revolute") { n_revolute_joints_ += 1; } @@ -135,13 +110,13 @@ OperationalSpaceControl::OperationalSpaceControl( VectorXd q_min(n_revolute_joints_); VectorXd q_max(n_revolute_joints_); int floating_base_offset = n_v_ - n_revolute_joints_; - for (JointIndex i(0); i < plant_wo_spr_.num_joints(); ++i) { - const drake::multibody::Joint& joint = plant_wo_spr_.get_joint(i); + for (JointIndex i(0); i < plant_.num_joints(); ++i) { + const drake::multibody::Joint& joint = plant_.get_joint(i); if (joint.type_name() == "revolute") { - q_min(vel_map_wo_spr.at(joint.name() + "dot") - floating_base_offset) = - plant_wo_spr.get_joint(i).position_lower_limits()[0]; - q_max(vel_map_wo_spr.at(joint.name() + "dot") - floating_base_offset) = - plant_wo_spr.get_joint(i).position_upper_limits()[0]; + q_min(vel_map.at(joint.name() + "dot") - floating_base_offset) = + plant_.get_joint(i).position_lower_limits()[0]; + q_max(vel_map.at(joint.name() + "dot") - floating_base_offset) = + plant_.get_joint(i).position_upper_limits()[0]; } if (joint.type_name() == "prismatic" && (joint.position_lower_limits()[0] != @@ -155,9 +130,6 @@ OperationalSpaceControl::OperationalSpaceControl( } q_min_ = q_min; q_max_ = q_max; - - // Check if the model is floating based - is_quaternion_ = multibody::HasQuaternion(plant_w_spr); } // Optional features @@ -177,45 +149,33 @@ void OperationalSpaceControl::SetInputCostForJointAndFsmStateWeight( if (W_input_.size() == 0) { W_input_ = Eigen::MatrixXd::Zero(n_u_, n_u_); } - int idx = MakeNameToActuatorsMap(plant_wo_spr_).at(joint_u_name); + int idx = MakeNameToActuatorsMap(plant_).at(joint_u_name); fsm_to_w_input_map_[fsm] = std::pair{idx, w}; } // Constraint methods void OperationalSpaceControl::AddContactPoint( - const WorldPointEvaluator* evaluator) { - single_contact_mode_ = true; - AddStateAndContactPoint(-1, evaluator); -} - -void OperationalSpaceControl::AddStateAndContactPoint( - int state, const WorldPointEvaluator* evaluator) { - DRAKE_DEMAND(&evaluator->plant() == &plant_wo_spr_); - - // Find the new contact in all_contacts_ - auto it_c = std::find(all_contacts_.begin(), all_contacts_.end(), evaluator); - int contact_idx = std::distance(all_contacts_.begin(), it_c); - // Add to contact list if the new contact doesn't exist in the list - if (it_c == all_contacts_.cend()) { - all_contacts_.push_back(evaluator); + const std::string& name, + std::unique_ptr> evaluator, + std::vector fsm_states) { + if (fsm_states.empty()) { + fsm_states.push_back(-1); + } + for (const auto& i : fsm_states) { + if (contact_names_map_.count(i) > 0) { + contact_names_map_.at(i).push_back(name); + } else { + contact_names_map_.insert({i, {name}}); + } } - // Find the finite state machine state in contact_indices_map_ - auto map_iterator = contact_indices_map_.find(state); - if (map_iterator == contact_indices_map_.end()) { - // state doesn't exist in the map - contact_indices_map_[state] = {contact_idx}; - } else { - // Add contact_idx to the existing set (note that std::set removes - // duplicates automatically) - map_iterator->second.insert(contact_idx); - } + DRAKE_DEMAND(mu_ > 0); + id_qp_.AddContactConstraint(name, std::move(evaluator), mu_); } void OperationalSpaceControl::AddKinematicConstraint( - const multibody::KinematicEvaluatorSet* evaluators) { - DRAKE_DEMAND(&evaluators->plant() == &plant_wo_spr_); - kinematic_evaluators_ = evaluators; + std::unique_ptr> evaluator) { + id_qp_.AddHolonomicConstraint(std::move(evaluator)); } // Tracking data methods @@ -223,8 +183,6 @@ void OperationalSpaceControl::AddTrackingData( std::unique_ptr tracking_data, double t_lb, double t_ub) { tracking_data_vec_->push_back(std::move(tracking_data)); fixed_position_vec_.emplace_back(VectorXd::Zero(0)); - t_s_vec_.push_back(t_lb); - t_e_vec_.push_back(t_ub); // Construct input ports and add element to traj_name_to_port_index_map_ if // the port for the traj is not created yet @@ -240,13 +198,40 @@ void OperationalSpaceControl::AddTrackingData( traj_name_to_port_index_map_[traj_name] = port_index; } } + +// Tracking data methods +void OperationalSpaceControl::AddForceTrackingData( + std::unique_ptr tracking_data) { + force_tracking_data_vec_->push_back(std::move(tracking_data)); + + // Declare point where external force is applied in the world frame to the OSC + // backend + auto evaluator = std::make_unique>( + plant_, force_tracking_data_vec_->back()->GetPointOnBody(), + force_tracking_data_vec_->back()->GetBodyFrame()); + id_qp_.AddExternalForce(force_tracking_data_vec_->back()->GetName(), + std::move(evaluator)); + + // Construct input ports and add element to traj_name_to_port_index_map_ if + // the port for the traj is not created yet + string traj_name = force_tracking_data_vec_->back()->GetName(); + if (traj_name_to_port_index_map_.find(traj_name) == + traj_name_to_port_index_map_.end()) { + PiecewisePolynomial pp = PiecewisePolynomial(); + int port_index = + this->DeclareAbstractInputPort( + traj_name, + drake::Value>(pp)) + .get_index(); + traj_name_to_port_index_map_[traj_name] = port_index; + } +} + void OperationalSpaceControl::AddConstTrackingData( std::unique_ptr tracking_data, const VectorXd& v, double t_lb, double t_ub) { tracking_data_vec_->push_back(std::move(tracking_data)); fixed_position_vec_.push_back(v); - t_s_vec_.push_back(t_lb); - t_e_vec_.push_back(t_ub); } // Osc checkers and constructor @@ -264,12 +249,9 @@ void OperationalSpaceControl::CheckCostSettings() { } } void OperationalSpaceControl::CheckConstraintSettings() { - if (!all_contacts_.empty()) { + if (!contact_names_map_.empty()) { DRAKE_DEMAND(mu_ != -1); } - if (single_contact_mode_) { - DRAKE_DEMAND(contact_indices_map_.size() == 1); - } } void OperationalSpaceControl::Build() { @@ -278,212 +260,108 @@ void OperationalSpaceControl::Build() { CheckConstraintSettings(); for (auto& tracking_data : *tracking_data_vec_) { tracking_data->CheckOscTrackingData(); - DRAKE_DEMAND(&tracking_data->plant_w_spr() == &plant_w_spr_); - DRAKE_DEMAND(&tracking_data->plant_wo_spr() == &plant_wo_spr_); + DRAKE_DEMAND(&tracking_data->plant_w_spr() == &plant_); } // Construct QP - prog_ = std::make_unique(); - - // Size of decision variable - n_h_ = (kinematic_evaluators_ == nullptr) - ? 0 - : kinematic_evaluators_->count_full(); - n_c_ = kSpaceDim * all_contacts_.size(); - n_c_active_ = 0; - for (auto evaluator : all_contacts_) { - n_c_active_ += evaluator->num_active(); - } - - // Record the contact dimension per state - for (auto contact_map : contact_indices_map_) { - int active_contact_dim = 0; - for (unsigned int i = 0; i < all_contacts_.size(); i++) { - if (contact_map.second.find(i) != contact_map.second.end()) { - active_contact_dim += - all_contacts_[i]->EvalFullJacobian(*context_wo_spr_).rows(); - } - } - active_contact_dim_[contact_map.first] = active_contact_dim; - } + id_qp_.Build(); // Initialize solution dv_sol_ = std::make_unique(n_v_); u_sol_ = std::make_unique(n_u_); - lambda_c_sol_ = std::make_unique(n_c_); - lambda_h_sol_ = std::make_unique(n_h_); - epsilon_sol_ = std::make_unique(n_c_active_); + lambda_c_sol_ = std::make_unique(id_qp_.nc()); + lambda_h_sol_ = std::make_unique(id_qp_.nh()); + lambda_e_sol_ = std::make_unique(id_qp_.ne()); + epsilon_sol_ = std::make_unique(id_qp_.nc_active()); u_prev_ = std::make_unique(n_u_); dv_sol_->setZero(); u_sol_->setZero(); lambda_c_sol_->setZero(); lambda_h_sol_->setZero(); + lambda_e_sol_->setZero(); u_prev_->setZero(); - // Add decision variables - dv_ = prog_->NewContinuousVariables(n_v_, "dv"); - u_ = prog_->NewContinuousVariables(n_u_, "u"); - lambda_c_ = prog_->NewContinuousVariables(n_c_, "lambda_contact"); - lambda_h_ = prog_->NewContinuousVariables(n_h_, "lambda_holonomic"); - epsilon_ = prog_->NewContinuousVariables(n_c_active_, "epsilon"); - - // Add constraints - // 1. Dynamics constraint - dynamics_constraint_ = - prog_ - ->AddLinearEqualityConstraint( - MatrixXd::Zero(n_v_, n_v_ + n_c_ + n_h_ + n_u_), - VectorXd::Zero(n_v_), {dv_, lambda_c_, lambda_h_, u_}) - .evaluator() - .get(); - // 2. Holonomic constraint - holonomic_constraint_ = - prog_ - ->AddLinearEqualityConstraint(MatrixXd::Zero(n_h_, n_v_), - VectorXd::Zero(n_h_), dv_) - .evaluator() - .get(); - // 3. Contact constraint - if (!all_contacts_.empty()) { - if (w_soft_constraint_ <= 0) { - contact_constraints_ = - prog_ - ->AddLinearEqualityConstraint(MatrixXd::Zero(n_c_active_, n_v_), - VectorXd::Zero(n_c_active_), dv_) - .evaluator() - .get(); - } else { - // Relaxed version: - contact_constraints_ = - prog_ - ->AddLinearEqualityConstraint( - MatrixXd::Zero(n_c_active_, n_v_ + n_c_active_), - VectorXd::Zero(n_c_active_), {dv_, epsilon_}) - .evaluator() - .get(); - } - } - if (!all_contacts_.empty()) { - MatrixXd A = MatrixXd(5, kSpaceDim); - A << -1, 0, mu_, 0, -1, mu_, 1, 0, mu_, 0, 1, mu_, 0, 0, 1; - - for (unsigned int j = 0; j < all_contacts_.size(); j++) { - friction_constraints_.push_back( - prog_ - ->AddLinearConstraint( - A, VectorXd::Zero(5), - Eigen::VectorXd::Constant( - 5, std::numeric_limits::infinity()), - lambda_c_.segment(kSpaceDim * j, 3)) - .evaluator() - .get()); - } - } - - // 5. Input constraint - if (with_input_constraints_) { - prog_->AddLinearConstraint(MatrixXd::Identity(n_u_, n_u_), u_min_, u_max_, - u_); - } - // No joint position constraint in this implementation - // Add costs // 1. input cost if (W_input_.size() > 0) { - input_cost_ = prog_->AddQuadraticCost(W_input_, VectorXd::Zero(n_u_), u_) - .evaluator() - .get(); + id_qp_.AddQuadraticCost("input_cost", W_input_, VectorXd::Zero(n_u_), + id_qp_.u()); } // 2. acceleration cost if (W_joint_accel_.size() > 0) { DRAKE_DEMAND(W_joint_accel_.rows() == n_v_); - accel_cost_ = - prog_->AddQuadraticCost(W_joint_accel_, VectorXd::Zero(n_v_), dv_) - .evaluator() - .get(); + id_qp_.AddQuadraticCost("acceleration_cost", W_joint_accel_, + VectorXd::Zero(n_v_), id_qp_.dv()); } if (W_input_smoothing_.size() > 0) { - input_smoothing_cost_ = - prog_->AddQuadraticCost(W_input_smoothing_, VectorXd::Zero(n_u_), u_) - .evaluator() - .get(); + id_qp_.AddQuadraticCost("input_smoothing_cost", W_input_smoothing_, + VectorXd::Zero(n_u_), id_qp_.u()); } // 3. contact force cost if (W_lambda_c_reg_.size() > 0) { - DRAKE_DEMAND(W_lambda_c_reg_.rows() == n_c_); - lambda_c_cost_ = - prog_ - ->AddQuadraticCost(W_lambda_c_reg_, VectorXd::Zero(n_c_), lambda_c_) - .evaluator() - .get(); + int nc = id_qp_.lambda_c().rows(); + DRAKE_DEMAND(W_lambda_c_reg_.rows() == nc); + id_qp_.AddQuadraticCost("lambda_c_cost", W_lambda_c_reg_, + VectorXd::Zero(nc), id_qp_.lambda_c()); } // 3. constraint force cost if (W_lambda_h_reg_.size() > 0) { - DRAKE_DEMAND(W_lambda_h_reg_.rows() == n_h_); - lambda_h_cost_ = - prog_ - ->AddQuadraticCost(W_lambda_h_reg_, VectorXd::Zero(n_h_), lambda_h_) - .evaluator() - .get(); + int nh = id_qp_.lambda_h().rows(); + DRAKE_DEMAND(W_lambda_h_reg_.rows() == nh); + id_qp_.AddQuadraticCost("lambda_h_cost", W_lambda_h_reg_, + VectorXd::Zero(nh), id_qp_.lambda_h()); } // 4. Soft constraint cost if (w_soft_constraint_ > 0) { - soft_constraint_cost_ = - prog_ - ->AddQuadraticCost(w_soft_constraint_ * - MatrixXd::Identity(n_c_active_, n_c_active_), - VectorXd::Zero(n_c_active_), epsilon_) - .evaluator() - .get(); + int nca = id_qp_.nc_active(); + id_qp_.AddQuadraticCost("soft_constraint_cost", + w_soft_constraint_ * MatrixXd::Identity(nca, nca), + VectorXd::Zero(nca), id_qp_.epsilon()); } // 4. Tracking cost - for (unsigned int i = 0; i < tracking_data_vec_->size(); i++) { - tracking_costs_.push_back(prog_ - ->AddQuadraticCost(MatrixXd::Zero(n_v_, n_v_), - VectorXd::Zero(n_v_), dv_) - .evaluator() - .get()); + for (const auto& data : *tracking_data_vec_) { + id_qp_.AddQuadraticCost(data->GetName(), MatrixXd::Zero(n_v_, n_v_), + VectorXd::Zero(n_v_), id_qp_.dv()); + } + + // 5. Track external force cost + int ne = id_qp_.lambda_e().rows(); + for (const auto& data : *force_tracking_data_vec_) { + id_qp_.AddQuadraticCost(data->GetName(), MatrixXd::Zero(ne, ne), + VectorXd::Zero(ne), id_qp_.lambda_e()); } - // 5. Joint Limit cost + // 6. Joint Limit cost // TODO(yangwill) discuss best way to implement joint limit cost if (w_joint_limit_ > 0) { K_joint_pos_ = w_joint_limit_ * W_joint_accel_.bottomRightCorner( n_revolute_joints_, n_revolute_joints_); - joint_limit_cost_ = prog_ - ->AddLinearCost(VectorXd::Zero(n_revolute_joints_), - 0, dv_.tail(n_revolute_joints_)) - .evaluator() - .get(); + id_qp_.AddQuadraticCost( + "joint_limit_cost", + MatrixXd::Zero(n_revolute_joints_, n_revolute_joints_), + VectorXd::Zero(n_revolute_joints_), + id_qp_.dv().tail(n_revolute_joints_)); } - // (Testing) 6. contact force blending + // (Testing) 7. contact force blending if (ds_duration_ > 0) { - epsilon_blend_ = - prog_->NewContinuousVariables(n_c_ / kSpaceDim, "epsilon_blend"); + int nc = id_qp_.nc(); + const auto& lambda = id_qp_.lambda_c(); blend_constraint_ = - prog_ - ->AddLinearEqualityConstraint( - MatrixXd::Zero(1, 2 * n_c_ / kSpaceDim), VectorXd::Zero(1), - {lambda_c_.segment(kSpaceDim * 0 + 2, 1), - lambda_c_.segment(kSpaceDim * 1 + 2, 1), - lambda_c_.segment(kSpaceDim * 2 + 2, 1), - lambda_c_.segment(kSpaceDim * 3 + 2, 1), epsilon_blend_}) + id_qp_.get_mutable_prog() + .AddLinearEqualityConstraint(MatrixXd::Zero(1, nc / kSpaceDim), + VectorXd::Zero(1), + {lambda.segment(kSpaceDim * 0 + 2, 1), + lambda.segment(kSpaceDim * 1 + 2, 1), + lambda.segment(kSpaceDim * 2 + 2, 1), + lambda.segment(kSpaceDim * 3 + 2, 1)}) .evaluator() .get(); - /// Soft constraint version - // DRAKE_DEMAND(w_blend_constraint_ > 0); - // prog_->AddQuadraticCost( - // w_blend_constraint_ * - // MatrixXd::Identity(n_c_ / kSpaceDim, n_c_ / kSpaceDim), - // VectorXd::Zero(n_c_ / kSpaceDim), epsilon_blend_); - /// hard constraint version - prog_->AddBoundingBoxConstraint(0, 0, epsilon_blend_); } solver_ = std::make_unique(); - prog_->SetSolverOptions(solver_options_); + id_qp_.get_mutable_prog().SetSolverOptions(solver_options_); } drake::systems::EventStatus OperationalSpaceControl::DiscreteVariableUpdate( @@ -504,6 +382,10 @@ drake::systems::EventStatus OperationalSpaceControl::DiscreteVariableUpdate( discrete_state->get_mutable_vector(prev_event_time_idx_).get_mutable_value() << timestamp; + + if (solver_->IsInitialized()) { + solver_->DisableWarmStart(); + } } return drake::systems::EventStatus::Succeeded(); } @@ -512,55 +394,30 @@ VectorXd OperationalSpaceControl::SolveQp( const VectorXd& x_w_spr, const VectorXd& x_wo_spr, const drake::systems::Context& context, double t, int fsm_state, double t_since_last_state_switch, double alpha, int next_fsm_state) const { - // Get active contact indices - std::set active_contact_set = {}; - if (single_contact_mode_) { - active_contact_set = contact_indices_map_.at(-1); - } else { - auto map_iterator = contact_indices_map_.find(fsm_state); - if (map_iterator != contact_indices_map_.end()) { - active_contact_set = map_iterator->second; - } else { - static const drake::logging::Warn log_once(const_cast( - (std::to_string(fsm_state) + - " is not a valid finite state machine state in OSC. This can happen " - "if there are modes with no active contacts.") - .c_str())); - } - } - // Update context - SetPositionsIfNew( - plant_w_spr_, x_w_spr.head(plant_w_spr_.num_positions()), context_w_spr_); - SetVelocitiesIfNew(plant_w_spr_, - x_w_spr.tail(plant_w_spr_.num_velocities()), - context_w_spr_); - SetPositionsIfNew(plant_wo_spr_, - x_wo_spr.head(plant_wo_spr_.num_positions()), - context_wo_spr_); - SetVelocitiesIfNew(plant_wo_spr_, - x_wo_spr.tail(plant_wo_spr_.num_velocities()), - context_wo_spr_); - - // Get M, f_cg, B matrices of the manipulator equation - MatrixXd B = plant_wo_spr_.MakeActuationMatrix(); - MatrixXd M(n_v_, n_v_); - plant_wo_spr_.CalcMassMatrix(*context_wo_spr_, &M); - VectorXd bias(n_v_); - plant_wo_spr_.CalcBiasTerm(*context_wo_spr_, &bias); - drake::multibody::MultibodyForces f_app(plant_wo_spr_); - plant_wo_spr_.CalcForceElementsContribution(*context_wo_spr_, &f_app); - VectorXd grav = plant_wo_spr_.CalcGravityGeneralizedForces(*context_wo_spr_); - bias = bias - grav; - // TODO (yangwill): Characterize damping in cassie model - // std::cout << f_app.generalized_forces().transpose() << std::endl; - // bias = bias - f_app.generalized_forces(); + SetPositionsIfNew(plant_, x_w_spr.head(plant_.num_positions()), + context_); + SetVelocitiesIfNew(plant_, x_w_spr.tail(plant_.num_velocities()), + context_); + + const auto active_contact_names = contact_names_map_.count(fsm_state) > 0 + ? contact_names_map_.at(fsm_state) + : std::vector(); + std::vector active_external_forces = {}; + for (const auto& force_tracking_data : *force_tracking_data_vec_) { + active_external_forces.push_back(force_tracking_data->GetName()); + } + id_qp_.UpdateDynamics(x_w_spr, active_contact_names, active_external_forces); // Invariant Impacts // Only update when near an impact bool near_impact = alpha != 0; VectorXd v_proj = VectorXd::Zero(n_v_); + if (near_impact) { + MatrixXd M(n_v_, n_v_); + plant_.CalcMassMatrix(*context_, &M); + UpdateImpactInvariantProjection(x_w_spr, x_wo_spr, context, t, t_since_last_state_switch, fsm_state, next_fsm_state, M); @@ -568,116 +425,18 @@ VectorXd OperationalSpaceControl::SolveQp( v_proj = alpha * M_Jt_ * ii_lambda_sol_ + 1e-13 * VectorXd::Ones(n_v_); } - // Get J and JdotV for holonomic constraint - MatrixXd J_h(n_h_, n_v_); - VectorXd JdotV_h(n_h_); - if (kinematic_evaluators_ != nullptr) { - J_h = kinematic_evaluators_->EvalFullJacobian(*context_wo_spr_); - JdotV_h = - kinematic_evaluators_->EvalFullJacobianDotTimesV(*context_wo_spr_); - } - - // Get J for external forces in equations of motion - MatrixXd J_c = MatrixXd::Zero(n_c_, n_v_); - for (unsigned int i = 0; i < all_contacts_.size(); i++) { - if (active_contact_set.find(i) != active_contact_set.end()) { - J_c.block(kSpaceDim * i, 0, kSpaceDim, n_v_) = - all_contacts_[i]->EvalFullJacobian(*context_wo_spr_); - } - } - - // Get J and JdotV for contact constraint - MatrixXd J_c_active = MatrixXd::Zero(n_c_active_, n_v_); - VectorXd JdotV_c_active = VectorXd::Zero(n_c_active_); - int row_idx = 0; - for (unsigned int i = 0; i < all_contacts_.size(); i++) { - auto contact_i = all_contacts_[i]; - if (active_contact_set.find(i) != active_contact_set.end()) { - // We don't call EvalActiveJacobian() because it'll repeat the computation - // of the Jacobian. (J_c_active is just a stack of slices of J_c) - for (int j = 0; j < contact_i->num_active(); j++) { - J_c_active.row(row_idx + j) = - J_c.row(kSpaceDim * i + contact_i->active_inds().at(j)); - } - JdotV_c_active.segment(row_idx, contact_i->num_active()) = - contact_i->EvalActiveJacobianDotTimesV(*context_wo_spr_); - } - row_idx += contact_i->num_active(); - } - - // Update constraints - // 1. Dynamics constraint - /// M*dv + bias == J_c^T*lambda_c + J_h^T*lambda_h + B*u - /// -> M*dv - J_c^T*lambda_c - J_h^T*lambda_h - B*u == - bias - /// -> [M, -J_c^T, -J_h^T, -B]*[dv, lambda_c, lambda_h, u]^T = - bias - MatrixXd A_dyn = MatrixXd::Zero(n_v_, n_v_ + n_c_ + n_h_ + n_u_); - A_dyn.block(0, 0, n_v_, n_v_) = M; - A_dyn.block(0, n_v_, n_v_, n_c_) = -J_c.transpose(); - A_dyn.block(0, n_v_ + n_c_, n_v_, n_h_) = -J_h.transpose(); - A_dyn.block(0, n_v_ + n_c_ + n_h_, n_v_, n_u_) = -B; - dynamics_constraint_->UpdateCoefficients(A_dyn, -bias); - // 2. Holonomic constraint - /// JdotV_h + J_h*dv == 0 - /// -> J_h*dv == -JdotV_h - if (n_h_ > 0) { - holonomic_constraint_->UpdateCoefficients(J_h, -JdotV_h); - } - // 3. Contact constraint - if (!all_contacts_.empty()) { - if (w_soft_constraint_ <= 0) { - /// JdotV_c_active + J_c_active*dv == 0 - /// -> J_c_active*dv == -JdotV_c_active - contact_constraints_->UpdateCoefficients(J_c_active, -JdotV_c_active); - } else { - // Relaxed version: - /// JdotV_c_active + J_c_active*dv == -epsilon - /// -> J_c_active*dv + I*epsilon == -JdotV_c_active - /// -> [J_c_active, I]* [dv, epsilon]^T == -JdotV_c_active - MatrixXd A_c = MatrixXd::Zero(n_c_active_, n_v_ + n_c_active_); - A_c.block(0, 0, n_c_active_, n_v_) = J_c_active; - A_c.block(0, n_v_, n_c_active_, n_c_active_) = - MatrixXd::Identity(n_c_active_, n_c_active_); - contact_constraints_->UpdateCoefficients(A_c, -JdotV_c_active); - } - } - // 4. Friction constraint (approximated friction cone) - /// For i = active contact indices - /// mu_*lambda_c(3*i+2) >= lambda_c(3*i+0) - /// -mu_*lambda_c(3*i+2) <= lambda_c(3*i+0) - /// mu_*lambda_c(3*i+2) >= lambda_c(3*i+1) - /// -mu_*lambda_c(3*i+2) <= lambda_c(3*i+1) - /// lambda_c(3*i+2) >= 0 - /// -> - /// mu_*lambda_c(3*i+2) - lambda_c(3*i+0) >= 0 - /// mu_*lambda_c(3*i+2) + lambda_c(3*i+0) >= 0 - /// mu_*lambda_c(3*i+2) - lambda_c(3*i+1) >= 0 - /// mu_*lambda_c(3*i+2) + lambda_c(3*i+1) >= 0 - /// lambda_c(3*i+2) >= 0 - if (!all_contacts_.empty()) { - for (unsigned int i = 0; i < all_contacts_.size(); i++) { - if (active_contact_set.find(i) != active_contact_set.end()) { - friction_constraints_.at(i)->UpdateLowerBound(VectorXd::Zero(5)); - } else { - friction_constraints_.at(i)->UpdateLowerBound( - VectorXd::Constant(5, -std::numeric_limits::infinity())); - } - } - } - // Update costs // 4. Tracking cost for (unsigned int i = 0; i < tracking_data_vec_->size(); i++) { auto tracking_data = tracking_data_vec_->at(i).get(); - if (tracking_data->IsActive(fsm_state) && - t_since_last_state_switch >= t_s_vec_.at(i) && - t_since_last_state_switch <= t_e_vec_.at(i)) { + if (tracking_data->IsActive(fsm_state)) { // Check whether or not it is a constant trajectory, and update // TrackingData if (fixed_position_vec_.at(i).size() != 0) { // Create constant trajectory and update tracking_data->Update( - x_w_spr, *context_w_spr_, x_wo_spr, *context_wo_spr_, + x_w_spr, *context_, x_wo_spr, *context_, PiecewisePolynomial(fixed_position_vec_.at(i)), t, t_since_last_state_switch, fsm_state, v_proj); } else { @@ -690,8 +449,7 @@ VectorXd OperationalSpaceControl::SolveQp( const auto& traj = input_traj->get_value>(); // Update - tracking_data->Update(x_w_spr, *context_w_spr_, x_wo_spr, - *context_wo_spr_, traj, t, + tracking_data->Update(x_w_spr, *context_, x_wo_spr, *context_, traj, t, t_since_last_state_switch, fsm_state, v_proj); } @@ -701,56 +459,71 @@ VectorXd OperationalSpaceControl::SolveQp( const VectorXd& JdotV_t = tracking_data->GetJdotTimesV(); const VectorXd constant_term = (JdotV_t - ddy_t); - tracking_costs_.at(i)->UpdateCoefficients( - 2 * J_t.transpose() * W * J_t, - 2 * J_t.transpose() * W * (JdotV_t - ddy_t), - constant_term.transpose() * W * constant_term, true); + id_qp_.UpdateCost(tracking_data->GetName(), 2 * J_t.transpose() * W * J_t, + 2 * J_t.transpose() * W * (JdotV_t - ddy_t), + constant_term.transpose() * W * constant_term); } else { - tracking_costs_.at(i)->UpdateCoefficients(MatrixXd::Zero(n_v_, n_v_), - VectorXd::Zero(n_v_)); + id_qp_.UpdateCost(tracking_data->GetName(), MatrixXd::Zero(n_v_, n_v_), + VectorXd::Zero(n_v_)); } } + // Update tracking cost for external forces + for (auto& force_tracking_data : *force_tracking_data_vec_) { + int port_index = + traj_name_to_port_index_map_.at(force_tracking_data->GetName()); + const drake::AbstractValue* input_traj = + this->EvalAbstractInput(context, port_index); + DRAKE_DEMAND(input_traj != nullptr); + const auto& traj = + input_traj->get_value>(); + force_tracking_data->Update(x_w_spr, *context_, x_wo_spr, *context_, traj, + t); + const MatrixXd W = force_tracking_data->GetWeight(); + const VectorXd lambda_des = force_tracking_data->GetLambdaDes(); + id_qp_.UpdateCost(force_tracking_data->GetName(), 2 * W, + -2 * W * lambda_des, + lambda_des.transpose() * W * lambda_des); + } + // Add joint limit constraints if (w_joint_limit_ > 0) { VectorXd w_joint_limit = - K_joint_pos_ * (x_wo_spr.head(plant_wo_spr_.num_positions()) - .tail(n_revolute_joints_) - - q_max_) - .cwiseMax(0) + - K_joint_pos_ * (x_wo_spr.head(plant_wo_spr_.num_positions()) - .tail(n_revolute_joints_) - - q_min_) - .cwiseMin(0); - joint_limit_cost_->UpdateCoefficients(w_joint_limit, 0); - } - + K_joint_pos_ * + (x_wo_spr.head(plant_.num_positions()).tail(n_revolute_joints_) - + q_max_) + .cwiseMax(0) + + K_joint_pos_ * + (x_wo_spr.head(plant_.num_positions()).tail(n_revolute_joints_) - + q_min_) + .cwiseMin(0); + id_qp_.UpdateCost("joint_limit_cost", + MatrixXd::Zero(n_revolute_joints_, n_revolute_joints_), + w_joint_limit); + } + + // TODO (@Brian-Acosta) test double support blending as a force cost // (Testing) 6. blend contact forces during double support phase if (ds_duration_ > 0) { - MatrixXd A = MatrixXd::Zero(1, 2 * n_c_ / kSpaceDim); + int nc = id_qp_.nc(); + MatrixXd A = MatrixXd::Zero(1, nc / kSpaceDim); if (std::find(ds_states_.begin(), ds_states_.end(), fsm_state) != ds_states_.end()) { + double s = std::clamp(t_since_last_state_switch / ds_duration_, 0.0, 1.0); double alpha_left = 0; double alpha_right = 0; if (prev_distinct_fsm_state_ == right_support_state_) { // We want left foot force to gradually increase - alpha_left = -1; - alpha_right = t_since_last_state_switch / - (ds_duration_ - t_since_last_state_switch); - + alpha_left = std::clamp(1.0 - s, 0.0, 1.0); + alpha_right = -s; } else if (prev_distinct_fsm_state_ == left_support_state_) { - alpha_left = t_since_last_state_switch / - (ds_duration_ - t_since_last_state_switch); - alpha_right = -1; + alpha_left = -s; + alpha_right = std::clamp(1.0 - s, 0.0, 1.0); } A(0, 0) = alpha_left / 2; A(0, 1) = alpha_left / 2; A(0, 2) = alpha_right / 2; A(0, 3) = alpha_right / 2; - A(0, 4) = 1; - A(0, 5) = 1; - A(0, 6) = 1; - A(0, 7) = 1; } blend_constraint_->UpdateCoefficients(A, VectorXd::Zero(1)); } @@ -763,43 +536,46 @@ VectorXd OperationalSpaceControl::SolveQp( double w = fsm_to_w_input_map_.at(fsm_state).second; W(j, j) += w; } - input_cost_->UpdateCoefficients(W, VectorXd::Zero(n_u_)); + id_qp_.UpdateCost("input_cost", W, VectorXd::Zero(n_u_)); } // (Testing) 7. Cost for staying close to the previous input if (W_input_smoothing_.size() > 0 && u_prev_) { - input_smoothing_cost_->UpdateCoefficients( - W_input_smoothing_, -W_input_smoothing_ * *u_prev_, + id_qp_.UpdateCost( + "input_smoothing_cost", W_input_smoothing_, + -W_input_smoothing_ * *u_prev_, 0.5 * u_prev_->transpose() * W_input_smoothing_ * *u_prev_); } if (W_lambda_c_reg_.size() > 0) { - lambda_c_cost_->UpdateCoefficients((1 + alpha) * W_lambda_c_reg_, - VectorXd::Zero(n_c_)); + id_qp_.UpdateCost("lambda_c_cost", (1 + alpha) * W_lambda_c_reg_, + VectorXd::Zero(id_qp_.nc())); } if (W_lambda_h_reg_.size() > 0) { - lambda_h_cost_->UpdateCoefficients((1 + alpha) * W_lambda_h_reg_, - VectorXd::Zero(n_h_)); + id_qp_.UpdateCost("lambda_h_reg", (1 + alpha) * W_lambda_h_reg_, + VectorXd::Zero(id_qp_.nh())); } if (!solver_->IsInitialized()) { - solver_->InitializeSolver(*prog_, solver_options_); + solver_->InitializeSolver(id_qp_.get_prog(), solver_options_); } // Solve the QP MathematicalProgramResult result; - result = solver_->Solve(*prog_); + result = solver_->Solve(id_qp_.get_prog()); solve_time_ = result.get_solver_details().run_time; if (result.is_success()) { // Extract solutions - *dv_sol_ = result.GetSolution(dv_); - *u_sol_ = result.GetSolution(u_); - *lambda_c_sol_ = result.GetSolution(lambda_c_); - *lambda_h_sol_ = result.GetSolution(lambda_h_); - *epsilon_sol_ = result.GetSolution(epsilon_); + *dv_sol_ = result.GetSolution(id_qp_.dv()); + *u_sol_ = result.GetSolution(id_qp_.u()); + *lambda_c_sol_ = result.GetSolution(id_qp_.lambda_c()); + *lambda_h_sol_ = result.GetSolution(id_qp_.lambda_h()); + *lambda_e_sol_ = result.GetSolution(id_qp_.lambda_e()); + *epsilon_sol_ = result.GetSolution(id_qp_.epsilon()); } else { *u_prev_ = 0.99 * *u_sol_ + VectorXd::Random(n_u_); + solver_->DisableWarmStart(); } for (auto& tracking_data : *tracking_data_vec_) { @@ -811,31 +587,35 @@ VectorXd OperationalSpaceControl::SolveQp( return *u_sol_; } +// TODO (@Yangwill) test that this is equivalent to the previous impact +// invariant implementation void OperationalSpaceControl::UpdateImpactInvariantProjection( const VectorXd& x_w_spr, const VectorXd& x_wo_spr, const Context& context, double t, double t_since_last_state_switch, int fsm_state, int next_fsm_state, const MatrixXd& M) const { - auto map_iterator = contact_indices_map_.find(next_fsm_state); - if (map_iterator == contact_indices_map_.end()) { - ii_lambda_sol_ = VectorXd::Zero(n_c_ + n_h_); - M_Jt_ = MatrixXd::Zero(n_v_, n_c_ + n_h_); + auto map_iterator = contact_names_map_.find(next_fsm_state); + + if (map_iterator == contact_names_map_.end()) { + ii_lambda_sol_ = VectorXd::Zero(id_qp_.nc() + id_qp_.nh()); + M_Jt_ = MatrixXd::Zero(n_v_, id_qp_.nc() + id_qp_.nh()); return; } - std::set next_contact_set = map_iterator->second; - int active_constraint_dim = active_contact_dim_.at(next_fsm_state) + n_h_; + + std::vector next_contact_set = map_iterator->second; + + int active_constraint_dim = kSpaceDim * next_contact_set.size() + id_qp_.nh(); + MatrixXd J_next = MatrixXd::Zero(active_constraint_dim, n_v_); int row_start = 0; - for (unsigned int i = 0; i < all_contacts_.size(); i++) { - if (next_contact_set.find(i) != next_contact_set.end()) { - J_next.block(row_start, 0, kSpaceDim, n_v_) = - all_contacts_[i]->EvalFullJacobian(*context_wo_spr_); - row_start += kSpaceDim; - } + for (const auto& cname : next_contact_set) { + J_next.block(row_start, 0, kSpaceDim, n_v_) = + id_qp_.get_contact_evaluator(cname).EvalFullJacobian(*context_); + row_start += kSpaceDim; } // Holonomic constraints - if (n_h_ > 0) { - J_next.block(row_start, 0, n_h_, n_v_) = - kinematic_evaluators_->EvalFullJacobian(*context_wo_spr_); + if (id_qp_.nh() > 0) { + J_next.block(row_start, 0, id_qp_.nh(), n_v_) = + id_qp_.get_holonomic_evaluators().EvalFullJacobian(*context_); } M_Jt_ = M.llt().solve(J_next.transpose()); @@ -850,7 +630,7 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( if (fixed_position_vec_.at(i).size() != 0) { // Create constant trajectory and update tracking_data->Update( - x_w_spr, *context_w_spr_, x_wo_spr, *context_wo_spr_, + x_w_spr, *context_, x_wo_spr, *context_, PiecewisePolynomial(fixed_position_vec_.at(i)), t, t_since_last_state_switch, fsm_state, v_proj); } else { @@ -861,8 +641,7 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( EvalAbstractInput(context, port_index); const auto& traj = input_traj->get_value>(); - tracking_data->Update(x_w_spr, *context_w_spr_, x_wo_spr, - *context_wo_spr_, traj, t, + tracking_data->Update(x_w_spr, *context_, x_wo_spr, *context_, traj, t, t_since_last_state_switch, fsm_state, v_proj); } } @@ -882,21 +661,22 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( } } - // int n_holonomic_constraints = n_h_; - MatrixXd A_constrained = MatrixXd::Zero(active_constraint_dim + n_h_, - active_constraint_dim + n_h_); + // int n_holonomic_constraints = id_qp_.nh(); + MatrixXd A_constrained = MatrixXd::Zero(active_constraint_dim + id_qp_.nh(), + active_constraint_dim + id_qp_.nh()); A_constrained.block(0, 0, active_constraint_dim, active_constraint_dim) = A.transpose() * A; - VectorXd b_constrained = VectorXd::Zero(active_constraint_dim + n_h_); + VectorXd b_constrained = VectorXd::Zero(active_constraint_dim + id_qp_.nh()); VectorXd Ab = A.transpose() * ydot_err_vec; - if (n_h_ > 0) { - MatrixXd J_h = kinematic_evaluators_->EvalFullJacobian(*context_wo_spr_); + if (id_qp_.nh() > 0) { + MatrixXd J_h = + id_qp_.get_holonomic_evaluators().EvalFullJacobian(*context_); MatrixXd C = J_h * M_Jt_; VectorXd d = J_h * x_w_spr.tail(n_v_); - A_constrained.block(active_constraint_dim, 0, n_h_, active_constraint_dim) = - C; - A_constrained.block(0, active_constraint_dim, active_constraint_dim, n_h_) = - C.transpose(); + A_constrained.block(active_constraint_dim, 0, id_qp_.nh(), + active_constraint_dim) = C; + A_constrained.block(0, active_constraint_dim, active_constraint_dim, + id_qp_.nh()) = C.transpose(); b_constrained << Ab, d; } else { b_constrained << Ab; @@ -919,70 +699,30 @@ void OperationalSpaceControl::AssignOscLcmOutput( fsm_state = fsm_output->get_value()(0); } - double time_since_last_state_switch = - used_with_finite_state_machine_ - ? state->get_timestamp() - - context.get_discrete_state(prev_event_time_idx_).get_value()(0) - : state->get_timestamp(); - output->utime = state->get_timestamp() * 1e6; output->fsm_state = fsm_state; - VectorXd y_accel_cost = VectorXd::Zero(1); - VectorXd y_input_cost = VectorXd::Zero(1); - VectorXd y_input_smoothing_cost = VectorXd::Zero(1); - VectorXd y_lambda_c_cost = VectorXd::Zero(1); - VectorXd y_lambda_h_cost = VectorXd::Zero(1); - VectorXd y_soft_constraint_cost = VectorXd::Zero(1); - VectorXd y_joint_limit_cost = VectorXd::Zero(1); - if (accel_cost_) { - accel_cost_->Eval(*dv_sol_, &y_accel_cost); - } - if (input_cost_) { - input_cost_->Eval(*u_sol_, &y_input_cost); - } - if (input_smoothing_cost_) { - input_smoothing_cost_->Eval(*u_sol_, &y_input_smoothing_cost); - } - if (lambda_c_cost_) { - lambda_c_cost_->Eval(*lambda_c_sol_, &y_lambda_c_cost); - } - if (lambda_h_cost_) { - lambda_h_cost_->Eval(*lambda_h_sol_, &y_lambda_h_cost); - } - if (soft_constraint_cost_) { - soft_constraint_cost_->Eval(*epsilon_sol_, &y_soft_constraint_cost); - } - // if (joint_limit_cost_) { - // joint_limit_cost_->Eval(*dv_sol_, &y_joint_limit_cost); - // } - double acceleration_cost = (accel_cost_ != nullptr) ? y_accel_cost[0] : 0; - double input_cost = (input_cost_ != nullptr) ? y_input_cost[0] : 0; - double input_smoothing_cost = - (input_smoothing_cost_ != nullptr) ? y_input_smoothing_cost[0] : 0; - double soft_constraint_cost = - (soft_constraint_cost_ != nullptr) ? y_soft_constraint_cost[0] : 0; - double lambda_c_cost = (lambda_c_cost_ != nullptr) ? y_lambda_c_cost[0] : 0; - double lambda_h_cost = (lambda_h_cost_ != nullptr) ? y_lambda_h_cost[0] : 0; - // double joint_limit_cost = - // (joint_limit_cost_ != nullptr) ? y_joint_limit_cost[0] : 0; - - total_cost += input_cost + acceleration_cost + soft_constraint_cost + - input_smoothing_cost + lambda_h_cost + lambda_c_cost; - output->regularization_costs.clear(); - output->regularization_cost_names.clear(); - output->regularization_costs.push_back(input_cost); - output->regularization_cost_names.emplace_back("input_cost"); - output->regularization_costs.push_back(acceleration_cost); - output->regularization_cost_names.emplace_back("acceleration_cost"); - output->regularization_costs.push_back(soft_constraint_cost); - output->regularization_cost_names.emplace_back("soft_constraint_cost"); - output->regularization_costs.push_back(input_smoothing_cost); - output->regularization_cost_names.emplace_back("input_smoothing_cost"); - output->regularization_costs.push_back(lambda_c_cost); - output->regularization_cost_names.emplace_back("lambda_c_cost"); - output->regularization_costs.push_back(lambda_h_cost); - output->regularization_cost_names.emplace_back("lambda_h_cost"); + const std::vector> + potential_regularization_cost_names_and_vars{ + {"input_cost", *u_sol_}, + {"acceleration_cost", *dv_sol_}, + {"soft_constraint_cost", *epsilon_sol_}, + {"input_smoothing_cost", *u_sol_}, + {"lambda_c_cost", *lambda_c_sol_}, + {"lambda_h_cost", *lambda_h_sol_}}; + + output->regularization_cost_names.clear(); + output->regularization_costs.clear(); + for (const auto& [name, sol] : potential_regularization_cost_names_and_vars) { + VectorXd y = VectorXd::Zero(1); + if (id_qp_.has_cost_named(name)) { + id_qp_.get_cost_evaluator(name).Eval(sol, &y); + } + output->regularization_cost_names.emplace_back(name); + output->regularization_costs.emplace_back(y(0)); + total_cost += y(0); + } + output->num_regularization_costs = output->regularization_costs.size(); output->tracking_data_names.clear(); output->tracking_data.clear(); @@ -991,13 +731,15 @@ void OperationalSpaceControl::AssignOscLcmOutput( lcmt_osc_qp_output qp_output; qp_output.solve_time = solve_time_; qp_output.u_dim = n_u_; - qp_output.lambda_c_dim = n_c_; - qp_output.lambda_h_dim = n_h_; + qp_output.lambda_c_dim = id_qp_.nc(); + qp_output.lambda_h_dim = id_qp_.nh(); + qp_output.lambda_e_dim = id_qp_.ne(); qp_output.v_dim = n_v_; - qp_output.epsilon_dim = n_c_active_; + qp_output.epsilon_dim = id_qp_.nc_active(); qp_output.u_sol = CopyVectorXdToStdVector(*u_sol_); qp_output.lambda_c_sol = CopyVectorXdToStdVector(*lambda_c_sol_); qp_output.lambda_h_sol = CopyVectorXdToStdVector(*lambda_h_sol_); + qp_output.lambda_e_sol = CopyVectorXdToStdVector(*lambda_e_sol_); qp_output.dv_sol = CopyVectorXdToStdVector(*dv_sol_); qp_output.epsilon_sol = CopyVectorXdToStdVector(*epsilon_sol_); output->qp_output = qp_output; @@ -1024,9 +766,7 @@ void OperationalSpaceControl::AssignOscLcmOutput( osc_output.yddot_command = std::vector(osc_output.ydot_dim); osc_output.yddot_command_sol = std::vector(osc_output.ydot_dim); - if (tracking_data->IsActive(fsm_state) && - time_since_last_state_switch >= t_s_vec_.at(i) && - time_since_last_state_switch <= t_e_vec_.at(i)) { + if (tracking_data->IsActive(fsm_state)) { osc_output.y = CopyVectorXdToStdVector(tracking_data->GetY()); osc_output.y_des = CopyVectorXdToStdVector(tracking_data->GetYDes()); osc_output.error_y = CopyVectorXdToStdVector(tracking_data->GetErrorY()); @@ -1043,7 +783,8 @@ void OperationalSpaceControl::AssignOscLcmOutput( CopyVectorXdToStdVector(tracking_data->GetYddotCommandSol()); VectorXd y_tracking_cost = VectorXd::Zero(1); - tracking_costs_[i]->Eval(*dv_sol_, &y_tracking_cost); + id_qp_.get_cost_evaluator(tracking_data->GetName()) + .Eval(*dv_sol_, &y_tracking_cost); total_cost += y_tracking_cost[0]; output->tracking_costs.push_back(y_tracking_cost[0]); output->tracking_data.push_back(osc_output); @@ -1060,23 +801,16 @@ void OperationalSpaceControl::CalcOptimalInput( const drake::systems::Context& context, systems::TimestampedVector* control) const { // Read in current state and time - auto robot_output = - (OutputVector*)this->EvalVectorInput(context, state_port_); + auto robot_output = dynamic_cast*>( + EvalVectorInput(context, state_port_)); - VectorXd q_w_spr = robot_output->GetPositions(); - VectorXd v_w_spr = robot_output->GetVelocities(); - VectorXd x_w_spr(plant_w_spr_.num_positions() + - plant_w_spr_.num_velocities()); - x_w_spr << q_w_spr, v_w_spr; + VectorXd x_w_spr = robot_output->GetState(); + VectorXd x_wo_spr = x_w_spr; double timestamp = robot_output->get_timestamp(); double current_time = timestamp; - VectorXd x_wo_spr(n_q_ + n_v_); - x_wo_spr << map_position_from_spring_to_no_spring_ * q_w_spr, - map_velocity_from_spring_to_no_spring_ * v_w_spr; - VectorXd u_sol(n_u_); if (used_with_finite_state_machine_) { // Read in finite state machine @@ -1119,12 +853,13 @@ void OperationalSpaceControl::CheckTracking( output->set_timestamp(robot_output->get_timestamp()); output->get_mutable_value()(0) = 0.0; VectorXd y_soft_constraint_cost = VectorXd::Zero(1); - if (soft_constraint_cost_ != nullptr) { - soft_constraint_cost_->Eval(*epsilon_sol_, &y_soft_constraint_cost); + if (id_qp_.has_cost_named("soft_constraint_cost")) { + id_qp_.get_cost_evaluator("soft_constraint_cost") + .Eval(*epsilon_sol_, &y_soft_constraint_cost); } if (y_soft_constraint_cost[0] > 1e5 || isnan(y_soft_constraint_cost[0])) { output->get_mutable_value()(0) = 1.0; } } -} // namespace dairlib::systems::controllers +} // namespace dairlib::systems::controllers \ No newline at end of file diff --git a/systems/controllers/osc/operational_space_control.h b/systems/controllers/osc/operational_space_control.h index e407aa1a60..cff23824c8 100644 --- a/systems/controllers/osc/operational_space_control.h +++ b/systems/controllers/osc/operational_space_control.h @@ -17,6 +17,8 @@ #include "solvers/fast_osqp_solver.h" #include "solvers/solver_options_io.h" #include "systems/controllers/control_utils.h" +#include "systems/controllers/osc/external_force_tracking_data.h" +#include "systems/controllers/osc/inverse_dynamics_qp.h" #include "systems/controllers/osc/osc_tracking_data.h" #include "systems/framework/impact_info_vector.h" #include "systems/framework/output_vector.h" @@ -24,7 +26,6 @@ #include "drake/common/trajectories/exponential_plus_piecewise_polynomial.h" #include "drake/common/trajectories/piecewise_polynomial.h" #include "drake/common/yaml/yaml_io.h" -#include "drake/solvers/mathematical_program.h" #include "drake/solvers/osqp_solver.h" #include "drake/solvers/solve.h" #include "drake/systems/framework/diagram.h" @@ -97,12 +98,9 @@ namespace dairlib::systems::controllers { class OperationalSpaceControl : public drake::systems::LeafSystem { public: - OperationalSpaceControl( - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr, - drake::systems::Context* context_w_spr, - drake::systems::Context* context_wo_spr, - bool used_with_finite_state_machine = true); + OperationalSpaceControl(const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + bool used_with_finite_state_machine = true); /***** Input/output ports *****/ @@ -224,14 +222,17 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { void SetJointLimitWeight(const double w) { w_joint_limit_ = w; } // Constraint methods - void DisableAcutationConstraint() { with_input_constraints_ = false; } void SetContactFriction(double mu) { mu_ = mu; } - void AddContactPoint(const multibody::WorldPointEvaluator* evaluator); - void AddStateAndContactPoint( - int state, const multibody::WorldPointEvaluator* evaluator); + void AddContactPoint( + const std::string& name, + std::unique_ptr> evaluator, + std::vector fsm_states = {}); + void AddKinematicConstraint( - const multibody::KinematicEvaluatorSet* evaluators); + std::unique_ptr> + evaluator); + // Tracking data methods /// The third argument is used to set a period in which OSC does not track the /// desired traj (the period starts when the finite state machine switches to @@ -239,6 +240,8 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { void AddTrackingData(std::unique_ptr tracking_data, double t_lb = 0, double t_ub = std::numeric_limits::infinity()); + void AddForceTrackingData( + std::unique_ptr tracking_data); void AddConstTrackingData( std::unique_ptr tracking_data, const Eigen::VectorXd& v, double t_lb = 0, double t_ub = std::numeric_limits::infinity()); @@ -261,12 +264,41 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { SetOsqpSolverOptions( drake::yaml::LoadYamlFile( FindResourceOrThrow(yaml_string)) - .GetAsSolverOptions(drake::solvers::OsqpSolver::id()) - ); + .GetAsSolverOptions(drake::solvers::OsqpSolver::id())); }; // OSC LeafSystem builder void Build(); + /*! + * @brief utility functions to enable/disable gravity compensation that will + * be executed by OSC backend. + */ + void EnableGravityCompensation() { id_qp_.EnableGravityCompensation(); } + void DisableGravityCompensation() { id_qp_.DisableGravityCompensation(); } + bool HasGravityCompensation() { return id_qp_.HasGravityCompensation(); } + + /*! + * @brief utility functions to enable/disable input constraints that will be + * executed by OSC backend. + */ + void EnableInputConstraints() { id_qp_.EnableInputConstraints(); } + void DisableInputConstraints() { id_qp_.DisableInputConstraints(); } + bool HasInputConstraints() { return id_qp_.HasInputConstraints(); } + + /*! + * @brief utility functions to enable/disable acceleration constraints that + * will be executed by OSC backend. + */ + void EnableAccelerationConstraints() { + id_qp_.EnableAccelerationConstraints(); + } + void DisableAccelerationConstraints() { + id_qp_.DisableAccelerationConstraints(); + } + bool HasAccelerationConstraints() { + return id_qp_.HasAccelerationConstraints(); + } + private: // Osc checkers and constructor-related methods void CheckCostSettings(); @@ -332,24 +364,14 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { int prev_fsm_state_idx_; int prev_event_time_idx_; - // Map position/velocity from model with spring to without spring - Eigen::MatrixXd map_position_from_spring_to_no_spring_; - Eigen::MatrixXd map_velocity_from_spring_to_no_spring_; - // Map from (non-const) trajectory names to input port indices std::map traj_name_to_port_index_map_; // MBP's. - const drake::multibody::MultibodyPlant& plant_w_spr_; - const drake::multibody::MultibodyPlant& plant_wo_spr_; + const drake::multibody::MultibodyPlant& plant_; // MBP context's - drake::systems::Context* context_w_spr_; - drake::systems::Context* context_wo_spr_; - - // World frames - const drake::multibody::BodyFrame& world_w_spr_; - const drake::multibody::BodyFrame& world_wo_spr_; + drake::systems::Context* context_; // Size of position, velocity and input of the MBP without spring int n_q_; @@ -357,18 +379,6 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { int n_u_; int n_revolute_joints_; - // Size of holonomic constraint and total/active contact constraints - int n_h_; - int n_c_; - int n_c_active_; - - // Manually specified holonomic constraints (only valid for plants_wo_springs) - const multibody::KinematicEvaluatorSet* kinematic_evaluators_ = nullptr; - - // robot input limits - Eigen::VectorXd u_min_; - Eigen::VectorXd u_max_; - // robot joint limits Eigen::VectorXd q_min_; Eigen::VectorXd q_max_; @@ -377,12 +387,17 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { Eigen::MatrixXd K_joint_pos_; Eigen::MatrixXd K_joint_vel_; + // robot joint acceleration limits + Eigen::VectorXd ddq_min_; + Eigen::VectorXd ddq_max_; + + // robot input limits + Eigen::VectorXd u_min_; + Eigen::VectorXd u_max_; + // flag indicating whether using osc with finite state machine or not bool used_with_finite_state_machine_; - // floating base model flag - bool is_quaternion_; - // Solver std::unique_ptr solver_; drake::solvers::SolverOptions solver_options_ = @@ -391,41 +406,20 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); // MathematicalProgram - std::unique_ptr prog_; - - // Decision variables - drake::solvers::VectorXDecisionVariable dv_; - drake::solvers::VectorXDecisionVariable u_; - drake::solvers::VectorXDecisionVariable lambda_c_; - drake::solvers::VectorXDecisionVariable lambda_h_; - drake::solvers::VectorXDecisionVariable epsilon_; - // Cost and constraints - drake::solvers::LinearEqualityConstraint* dynamics_constraint_; - drake::solvers::LinearEqualityConstraint* holonomic_constraint_; - drake::solvers::LinearEqualityConstraint* contact_constraints_; - std::vector friction_constraints_; - - std::vector tracking_costs_; - drake::solvers::QuadraticCost* accel_cost_ = nullptr; - drake::solvers::LinearCost* joint_limit_cost_ = nullptr; - drake::solvers::QuadraticCost* input_cost_ = nullptr; - drake::solvers::QuadraticCost* input_smoothing_cost_ = nullptr; - drake::solvers::QuadraticCost* lambda_c_cost_ = nullptr; - drake::solvers::QuadraticCost* lambda_h_cost_ = nullptr; - drake::solvers::QuadraticCost* soft_constraint_cost_ = nullptr; + mutable InverseDynamicsQp id_qp_; // OSC solution std::unique_ptr dv_sol_; std::unique_ptr u_sol_; std::unique_ptr lambda_c_sol_; std::unique_ptr lambda_h_sol_; + std::unique_ptr lambda_e_sol_; std::unique_ptr epsilon_sol_; std::unique_ptr u_prev_; - mutable double solve_time_; + mutable double solve_time_{}; mutable Eigen::VectorXd ii_lambda_sol_; mutable Eigen::MatrixXd M_Jt_; - std::map active_contact_dim_ = {}; // OSC cost members /// Using u cost would push the robot away from the fixed point, so the user @@ -441,42 +435,34 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { std::map> fsm_to_w_input_map_; // each pair is (joint index, weight) - // OSC constraint members - bool with_input_constraints_ = true; // Soft contact penalty coefficient and friction cone coefficient double mu_ = -1; // Friction coefficients double w_soft_constraint_ = -1; // Map finite state machine state to its active contact indices - std::map> contact_indices_map_ = {}; - // All contacts (used in contact constraints) - std::vector*> all_contacts_ = {}; - // single_contact_mode_ is true if there is only 1 contact mode in OSC - bool single_contact_mode_ = false; + std::map> contact_names_map_ = {}; // OSC tracking data (stored as a pointer because of caching) std::unique_ptr>> tracking_data_vec_ = std::make_unique>>(); + std::unique_ptr>> + force_tracking_data_vec_ = std::make_unique< + std::vector>>(); + // Fixed position of constant trajectories std::vector fixed_position_vec_; - // Set a period during which we apply control (Unit: seconds) - // Let t be the elapsed time since fsm switched to a new state. - // We only apply the control when t_s <= t <= t_e - std::vector t_s_vec_; - std::vector t_e_vec_; - // Optional feature -- contact force blend double ds_duration_ = -1; - int left_support_state_; - int right_support_state_; - std::vector ds_states_; + int left_support_state_{}; + int right_support_state_{}; + std::vector ds_states_{}; double w_blend_constraint_ = 0.1; // for soft constraint mutable double prev_distinct_fsm_state_ = -1; - drake::solvers::LinearEqualityConstraint* blend_constraint_; - drake::solvers::VectorXDecisionVariable epsilon_blend_; + drake::solvers::LinearEqualityConstraint* blend_constraint_ = nullptr; + drake::solvers::VectorXDecisionVariable epsilon_blend_{}; }; -} // namespace dairlib::systems::controllers +} // namespace dairlib::systems::controllers \ No newline at end of file diff --git a/systems/controllers/osc/options_tracking_data.cc b/systems/controllers/osc/options_tracking_data.cc index 9f9023f5ab..1d01660ea5 100644 --- a/systems/controllers/osc/options_tracking_data.cc +++ b/systems/controllers/osc/options_tracking_data.cc @@ -195,7 +195,7 @@ void OptionsTrackingData::SetTimerVaryingFeedForwardAccelMultiplier( ff_accel_multiplier_traj_ = ff_accel_multiplier_traj; } -void OptionsTrackingData::SetCmdAccelerationBounds(Eigen::VectorXd& lb, Eigen::VectorXd& ub){ +void OptionsTrackingData::SetCmdAccelerationBounds(const Eigen::VectorXd& lb, const Eigen::VectorXd& ub){ DRAKE_DEMAND(lb.size() == n_ydot_); DRAKE_DEMAND(ub.size() == n_ydot_); yddot_cmd_lb_ = lb; diff --git a/systems/controllers/osc/options_tracking_data.h b/systems/controllers/osc/options_tracking_data.h index eff035991c..21485e226d 100644 --- a/systems/controllers/osc/options_tracking_data.h +++ b/systems/controllers/osc/options_tracking_data.h @@ -42,7 +42,7 @@ class OptionsTrackingData : public OscTrackingData { std::shared_ptr> ff_accel_multiplier_traj); - void SetCmdAccelerationBounds(Eigen::VectorXd& lb, Eigen::VectorXd& ub); + void SetCmdAccelerationBounds(const Eigen::VectorXd& lb, const Eigen::VectorXd& ub); void SetViewFrame(std::shared_ptr> view_frame) { view_frame_ = view_frame; diff --git a/systems/controllers/osc/osc_gains.h b/systems/controllers/osc/osc_gains.h index 059b05e05a..252e4d92ff 100644 --- a/systems/controllers/osc/osc_gains.h +++ b/systems/controllers/osc/osc_gains.h @@ -23,6 +23,7 @@ struct OSCGains { MatrixXd W_acceleration; MatrixXd W_input_regularization; + MatrixXd W_input_smoothing_regularization; MatrixXd W_lambda_c_regularization; MatrixXd W_lambda_h_regularization; @@ -54,9 +55,10 @@ struct OSCGains { Eigen::VectorXd w_lambda_h_regularization = Eigen::Map( this->W_lambda_h_reg.data(), this->W_lambda_h_reg.size()); - W_acceleration = w_acceleration.asDiagonal(); - W_input_regularization = w_input_regularization.asDiagonal(); - W_lambda_c_regularization = w_lambda_c_regularization.asDiagonal(); - W_lambda_h_regularization = w_lambda_h_regularization.asDiagonal(); + W_acceleration = w_accel * w_acceleration.asDiagonal(); + W_input_regularization = w_input * w_input_regularization.asDiagonal(); + W_input_smoothing_regularization = w_input_reg * w_input_regularization.asDiagonal(); + W_lambda_c_regularization = w_lambda * w_lambda_c_regularization.asDiagonal(); + W_lambda_h_regularization = w_lambda * w_lambda_h_regularization.asDiagonal(); } }; \ No newline at end of file diff --git a/systems/controllers/osc/osc_tracking_data.h b/systems/controllers/osc/osc_tracking_data.h index 94bcb81883..cf7e42d9f2 100644 --- a/systems/controllers/osc/osc_tracking_data.h +++ b/systems/controllers/osc/osc_tracking_data.h @@ -169,8 +169,8 @@ class OscTrackingData { const drake::multibody::MultibodyPlant& plant_wo_spr_; // World frames - const drake::multibody::BodyFrame& world_w_spr_; - const drake::multibody::BodyFrame& world_wo_spr_; + const drake::multibody::RigidBodyFrame& world_w_spr_; + const drake::multibody::RigidBodyFrame& world_wo_spr_; private: void UpdateDesired(const drake::trajectories::Trajectory& traj, diff --git a/systems/controllers/osc/rot_space_tracking_data.cc b/systems/controllers/osc/rot_space_tracking_data.cc index 60e0174287..efb353ea72 100644 --- a/systems/controllers/osc/rot_space_tracking_data.cc +++ b/systems/controllers/osc/rot_space_tracking_data.cc @@ -80,22 +80,12 @@ void RotTaskSpaceTrackingData::UpdateYdot( } void RotTaskSpaceTrackingData::UpdateYdotError(const Eigen::VectorXd& v_proj) { - // Transform qdot to w - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond dy_quat_des(ydot_des_(0), ydot_des_(1), ydot_des_(2), - ydot_des_(3)); - Vector3d w_des_ = 2 * (dy_quat_des * y_quat_des.conjugate()).vec(); - // Because we transform the error here rather than in the parent - // options_tracking_data, and because J_y is already transformed in the view - // frame, we need to undo the transformation on J_y + DRAKE_DEMAND(ydot_des_.size() == 3); error_ydot_ = - w_des_ - ydot_ - view_frame_rot_T_.transpose() * GetJ() * v_proj; + ydot_des_ - ydot_; if (with_view_frame_) { error_ydot_ = view_frame_rot_T_ * error_ydot_; } - - ydot_des_ = - w_des_; // Overwrite 4d quat_dot with 3d omega. Need this for osc logging } void RotTaskSpaceTrackingData::UpdateJ(const VectorXd& x_wo_spr, @@ -120,12 +110,8 @@ void RotTaskSpaceTrackingData::UpdateJdotV( } void RotTaskSpaceTrackingData::UpdateYddotDes(double, double) { - // Convert ddq into angular acceleration - // See https://physics.stackexchange.com/q/460311 - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond yddot_quat_des(yddot_des_(0), yddot_des_(1), yddot_des_(2), - yddot_des_(3)); - yddot_des_converted_ = 2 * (yddot_quat_des * y_quat_des.conjugate()).vec(); + DRAKE_DEMAND(yddot_des_.size() == 3); + yddot_des_converted_ = yddot_des_; if (!idx_zero_feedforward_accel_.empty()) { std::cerr << "RotTaskSpaceTrackingData does not support zero feedforward " "acceleration"; diff --git a/systems/controllers/osc/rot_space_tracking_data.h b/systems/controllers/osc/rot_space_tracking_data.h index 713ea5c6c0..6b8b9aebb4 100644 --- a/systems/controllers/osc/rot_space_tracking_data.h +++ b/systems/controllers/osc/rot_space_tracking_data.h @@ -36,9 +36,9 @@ class RotTaskSpaceTrackingData final : public OptionsTrackingData { const Eigen::Isometry3d& frame_pose = Eigen::Isometry3d::Identity()); protected: - std::unordered_map*> + std::unordered_map*> body_frames_w_spr_; - std::unordered_map*> + std::unordered_map*> body_frames_wo_spr_; private: diff --git a/systems/controllers/osc/trans_space_tracking_data.h b/systems/controllers/osc/trans_space_tracking_data.h index f4067686f5..909f150803 100644 --- a/systems/controllers/osc/trans_space_tracking_data.h +++ b/systems/controllers/osc/trans_space_tracking_data.h @@ -33,9 +33,9 @@ class TransTaskSpaceTrackingData final : public OptionsTrackingData { const Eigen::Vector3d& pt_on_body = Eigen::Vector3d::Zero()); protected: - std::unordered_map*> + std::unordered_map*> body_frames_w_spr_; - std::unordered_map*> + std::unordered_map*> body_frames_wo_spr_; private: diff --git a/systems/controllers/safe_velocity_controller.cc b/systems/controllers/safe_velocity_controller.cc index 3c4ab58b46..27ff6528d9 100644 --- a/systems/controllers/safe_velocity_controller.cc +++ b/systems/controllers/safe_velocity_controller.cc @@ -1,5 +1,11 @@ #include "systems/controllers/safe_velocity_controller.h" +#include "drake/common/text_logging.h" + +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::VectorX; + namespace dairlib{ namespace systems{ diff --git a/systems/controllers/safe_velocity_controller.h b/systems/controllers/safe_velocity_controller.h index 28d4d18853..b35e1d8093 100644 --- a/systems/controllers/safe_velocity_controller.h +++ b/systems/controllers/safe_velocity_controller.h @@ -7,14 +7,10 @@ #include "drake/systems/framework/event_status.h" #include "drake/systems/framework/discrete_values.h" -using Eigen::VectorXd; -using drake::systems::LeafSystem; -using drake::systems::Context; - namespace dairlib{ namespace systems{ -class SafeVelocityController : public LeafSystem { + class SafeVelocityController : public drake::systems::LeafSystem { public: SafeVelocityController(double max_velocity, int num_joints); @@ -29,11 +25,11 @@ class SafeVelocityController : public LeafSystem { } private: - void CalcOutputTorques(const Context &context, - BasicVector* output) const; + void CalcOutputTorques(const drake::systems::Context &context, + drake::systems::BasicVector* output) const; drake::systems::EventStatus CheckTerminate( - const Context &context, + const drake::systems::Context &context, drake::systems::DiscreteValues* next_state) const; diff --git a/systems/controllers/sampling_based_c3_controller.cc b/systems/controllers/sampling_based_c3_controller.cc new file mode 100644 index 0000000000..1267e1658c --- /dev/null +++ b/systems/controllers/sampling_based_c3_controller.cc @@ -0,0 +1,2171 @@ +#include "sampling_based_c3_controller.h" + +#include +#include +#include +#include + +#include +#include + +#include "common/quaternion_error_hessian.h" +#include "dairlib/lcmt_radio_out.hpp" +#include "drake/multibody/plant/multibody_plant.h" +#include "examples/sampling_c3/generate_samples.h" +#include "examples/sampling_c3/reposition.h" +#include "multibody/multibody_utils.h" +#include "solvers/c3_miqp.h" +#include "solvers/c3_options.h" +#include "solvers/c3_qp.h" +#include "solvers/lcs.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" + +namespace dairlib { + +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::multibody::ModelInstanceIndex; +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using drake::trajectories::PiecewisePolynomial; +using Eigen::MatrixXd; +using Eigen::MatrixXf; +using Eigen::Vector3d; +using Eigen::VectorXd; +using Eigen::VectorXf; +using solvers::C3; +using solvers::C3MIQP; +using solvers::C3QP; +using solvers::LCS; +using solvers::LCSFactory; +using std::vector; +using systems::TimestampedVector; + +namespace systems { + +SamplingC3Controller::SamplingC3Controller( + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + SamplingC3ControllerParams controller_params, + bool verbose) + : plant_(plant), + context_(context), + plant_ad_(plant_ad), + context_ad_(context_ad), + contact_pairs_(contact_geoms), + controller_params_(std::move(controller_params)), + sampling_c3_options_(controller_params_.sampling_c3_options), + sampling_params_(controller_params_.sampling_params), + reposition_params_(controller_params_.reposition_params), + progress_params_(controller_params_.progress_params), + G_(std::vector(sampling_c3_options_.N, sampling_c3_options_.G)), + U_(std::vector(sampling_c3_options_.N, sampling_c3_options_.U)), + N_(sampling_c3_options_.N), + verbose_(verbose) { + this->set_name("sampling_c3_controller"); + + // Build C3Options from SamplingC3Options. + C3Options c3_options = sampling_c3_options_.GetC3Options( + crossed_cost_switching_threshold_); + + // Initialize Q_ and R_ to proper size. Values don't matter because the + // values get rewritten at the beginning of every control loop. + double discount_factor = 1; + for (int i = 0; i < N_; ++i) { + Q_.push_back(discount_factor * c3_options.Q); + R_.push_back(discount_factor * c3_options.R); + discount_factor *= c3_options.gamma; + } + Q_.push_back(discount_factor * c3_options.Q); + DRAKE_DEMAND(Q_.size() == N_ + 1); + DRAKE_DEMAND(R_.size() == N_); + + n_q_ = plant_.num_positions(); + n_v_ = plant_.num_velocities(); + n_u_ = plant_.num_actuators(); + n_x_ = n_q_ + n_v_; + + solve_time_filter_constant_ = sampling_c3_options_.solve_time_filter_alpha; + + if (sampling_c3_options_.contact_model == "stewart_and_trinkle") { + contact_model_ = solvers::ContactModel::kStewartAndTrinkle; + n_lambda_ = + 2 * sampling_c3_options_.num_contacts + + 2 * sampling_c3_options_.num_friction_directions * + sampling_c3_options_.num_contacts; + } else if (sampling_c3_options_.contact_model == "anitescu") { + contact_model_ = solvers::ContactModel::kAnitescu; + n_lambda_ = 2 * sampling_c3_options_.num_friction_directions * + sampling_c3_options_.num_contacts; + } else { + std::cerr << ("Unknown or unsupported contact model: " + + sampling_c3_options_.contact_model) << std::endl; + DRAKE_THROW_UNLESS(false); + } + + // Placeholder LCS will have correct size as it's already determined by the + // contact model. + auto lcs_placeholder = CreatePlaceholderLCS(); + auto x_desired_placeholder = + std::vector(N_ + 1, VectorXd::Zero(n_x_)); + if (sampling_c3_options_.projection_type == "MIQP") { + c3_curr_plan_ = std::make_unique( + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, + c3_options); + c3_best_plan_ = std::make_unique( + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, + c3_options); + c3_buffer_plan_ = std::make_unique( + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); + } else if (sampling_c3_options_.projection_type == "QP") { + c3_curr_plan_ = std::make_unique( + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), + x_desired_placeholder, c3_options); + c3_best_plan_ = std::make_unique( + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, + c3_options); + c3_buffer_plan_ = std::make_unique( + lcs_placeholder, C3::CostMatrices(Q_, R_, G_, U_), x_desired_placeholder, + c3_options); + } else { + std::cerr << ("Unknown projection type") << std::endl; + DRAKE_THROW_UNLESS(false); + } + + // Input ports. + radio_port_ = + this->DeclareAbstractInputPort("lcmt_radio_out", + drake::Value{}) + .get_index(); + lcs_state_input_port_ = + this->DeclareVectorInputPort("x_lcs", TimestampedVector(n_x_)) + .get_index(); + target_input_port_ = + this->DeclareVectorInputPort("x_lcs_des", n_x_).get_index(); + final_target_input_port_ = + this->DeclareVectorInputPort("x_lcs_final_des", n_x_).get_index(); + + // Output ports. + auto c3_solution = C3Output::C3Solution(); + c3_solution.x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); + c3_solution.lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); + c3_solution.u_sol_ = MatrixXf::Zero(n_u_, N_); + c3_solution.time_vector_ = VectorXf::Zero(N_); + auto c3_intermediates = C3Output::C3Intermediates(); + c3_intermediates.z_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); + c3_intermediates.w_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); + c3_intermediates.delta_ = MatrixXf::Zero(n_x_ + n_lambda_ + n_u_, N_); + c3_intermediates.time_vector_ = VectorXf::Zero(N_); + auto lcs_contact_jacobian = std::pair(Eigen::MatrixXd(n_x_, n_lambda_), + std::vector()); + + // Since the num_additional_samples_repos means the additional samples + // to generate in addition to the prev_repositioning_target_, add 1. + // Additionally add 1 to C3 if considering samples from the buffer. + int from_buffer = 0; + if (sampling_params_.consider_best_buffer_sample_when_leaving_c3) { + from_buffer = 1; + } + max_num_samples_ = + std::max(sampling_params_.num_additional_samples_repos + 1, + sampling_params_.num_additional_samples_c3 + from_buffer); + // The +1 here is to account for the current location. + all_sample_locations_ = + vector(max_num_samples_ + 1, Vector3d::Zero()); + LcmTrajectory lcm_traj = LcmTrajectory(); + + // Current location plan output ports. + // This output port is being kept so it can go into a C3outputSender which is + // what we use to grab downstream forces for visualization. + c3_solution_curr_plan_port_ = + this->DeclareAbstractOutputPort( + "c3_solution_curr_plan", c3_solution, + &SamplingC3Controller::OutputC3SolutionCurrPlan) + .get_index(); + c3_solution_curr_plan_actor_port_ = + this->DeclareAbstractOutputPort( + "c3_solution_curr_plan_actor", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputC3SolutionCurrPlanActor) + .get_index(); + c3_solution_curr_plan_object_port_ = + this->DeclareAbstractOutputPort( + "c3_solution_curr_plan_object", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputC3SolutionCurrPlanObject) + .get_index(); + c3_intermediates_curr_plan_port_ = + this->DeclareAbstractOutputPort( + "c3_intermediates_curr_plan", c3_intermediates, + &SamplingC3Controller::OutputC3IntermediatesCurrPlan) + .get_index(); + lcs_contact_jacobian_curr_plan_port_ = + this->DeclareAbstractOutputPort( + "J_lcs_curr_plan, p_lcs_curr_plan", lcs_contact_jacobian, + &SamplingC3Controller::OutputLCSContactJacobianCurrPlan) + .get_index(); + + // Best sample plan output ports. + // This output port is being kept so it can go into a C3outputSender which is + // what we use to grab downstream forces for visualization. + c3_solution_best_plan_port_ = + this->DeclareAbstractOutputPort( + "c3_solution_best_plan", c3_solution, + &SamplingC3Controller::OutputC3SolutionBestPlan) + .get_index(); + c3_solution_best_plan_actor_port_ = + this->DeclareAbstractOutputPort( + "c3_solution_best_plan_actor", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputC3SolutionBestPlanActor) + .get_index(); + c3_solution_best_plan_object_port_ = + this->DeclareAbstractOutputPort( + "c3_solution_best_plan_object", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputC3SolutionBestPlanObject) + .get_index(); + c3_intermediates_best_plan_port_ = + this->DeclareAbstractOutputPort( + "c3_intermediates_best_plan", c3_intermediates, + &SamplingC3Controller::OutputC3IntermediatesBestPlan) + .get_index(); + lcs_contact_jacobian_best_plan_port_ = + this->DeclareAbstractOutputPort( + "J_lcs_best_plan, p_lcs_best_plan", lcs_contact_jacobian, + &SamplingC3Controller::OutputLCSContactJacobianBestPlan) + .get_index(); + + // Execution trajectory output ports. + c3_traj_execute_actor_port_ = + this->DeclareAbstractOutputPort( + "c3_traj_execute_actor", dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputC3TrajExecuteActor) + .get_index(); + repos_traj_execute_actor_port_ = + this->DeclareAbstractOutputPort( + "repos_traj_execute_actor", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputReposTrajExecuteActor) + .get_index(); + traj_execute_actor_port_ = + this->DeclareAbstractOutputPort( + "traj_execute_actor", dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputTrajExecuteActor) + .get_index(); + is_c3_mode_port_ = + this->DeclareAbstractOutputPort( + "is_c3_mode", dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputIsC3Mode) + .get_index(); + + // Output ports for dynamically feasible plans used for cost computation and + // visualization. + dynamically_feasible_curr_plan_actor_port_ = + this->DeclareAbstractOutputPort( + "dynamically_feasible_curr_plan_actor", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanActor) + .get_index(); + dynamically_feasible_curr_plan_object_port_ = + this->DeclareAbstractOutputPort( + "dynamically_feasible_curr_plan_object", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanObject) + .get_index(); + dynamically_feasible_best_plan_actor_port_ = + this->DeclareAbstractOutputPort( + "dynamically_feasible_best_plan_actor", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor) + .get_index(); + dynamically_feasible_best_plan_object_port_ = + this->DeclareAbstractOutputPort( + "dynamically_feasible_best_plan_object", + dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputDynamicallyFeasibleBestPlanObject) + .get_index(); + + // Sample location related output ports. + // This port will output all samples except the current location. + // all_sample_locations_port_ does not include the current location. So + // index 0 is the first sample. + all_sample_locations_port_ = + this->DeclareAbstractOutputPort( + "all_sample_locations", dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputAllSampleLocations) + .get_index(); + // all_sample_costs_port_ does include the current location. So index 0 is + // the current location cost. + all_sample_costs_port_ = + this->DeclareAbstractOutputPort( + "all_sample_costs", dairlib::lcmt_timestamped_saved_traj(), + &SamplingC3Controller::OutputAllSampleCosts) + .get_index(); + + // A debug output port to publish information about the internals of the + // sampling-based controller. + debug_lcmt_port_ = + this->DeclareAbstractOutputPort("sampling_c3_debug", + dairlib::lcmt_sampling_c3_debug(), + &SamplingC3Controller::OutputDebug) + .get_index(); + + // Sample buffer related ouput ports. + sample_buffer_ = MatrixXd::Zero(sampling_params_.N_sample_buffer, n_q_); + sample_costs_buffer_ = -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); + sample_buffer_configurations_port_ = + this->DeclareAbstractOutputPort( + "sample_buffer_configurations", sample_buffer_, + &SamplingC3Controller::OutputSampleBufferConfigurations) + .get_index(); + + sample_buffer_costs_port_ = + this->DeclareAbstractOutputPort( + "sample_buffer_costs", sample_costs_buffer_, + &SamplingC3Controller::OutputSampleBufferCosts) + .get_index(); + + plan_start_time_index_ = DeclareDiscreteState(1); + x_pred_curr_plan_ = VectorXd::Zero(n_x_); + x_from_last_control_loop_ = VectorXd::Zero(n_x_); + x_pred_from_last_control_loop_ = VectorXd::Zero(n_x_); + x_final_target_ = VectorXd::Zero(n_x_); + + ResetProgressMetrics(); + + DeclareForcedDiscreteUpdateEvent(&SamplingC3Controller::ComputePlan); + + // Set parallelization settings. + omp_set_dynamic(0); // Explicitly disable dynamic teams. + omp_set_nested(1); // Enable nested threading. + if (sampling_c3_options_.num_outer_threads == 0) { + // Interpret setting number of threads to zero as a request to use all + // machine's threads. + num_threads_to_use_ = omp_get_max_threads(); + } else { + num_threads_to_use_ = sampling_c3_options_.num_outer_threads; + } + + if (verbose_) { + std::cout << "Initial filtered_solve_time_: " << filtered_solve_time_ + << std::endl; + } +} + +LCS SamplingC3Controller::CreatePlaceholderLCS() const { + MatrixXd A = MatrixXd::Ones(n_x_, n_x_); + MatrixXd B = MatrixXd::Zero(n_x_, n_u_); + VectorXd d = VectorXd::Zero(n_x_); + MatrixXd D = MatrixXd::Ones(n_x_, n_lambda_); + MatrixXd E = MatrixXd::Zero(n_lambda_, n_x_); + MatrixXd F = MatrixXd::Zero(n_lambda_, n_lambda_); + MatrixXd H = MatrixXd::Zero(n_lambda_, n_u_); + VectorXd c = VectorXd::Zero(n_lambda_); + return LCS(A, B, D, d, E, F, H, c, sampling_c3_options_.N, + sampling_c3_options_.planning_dt_position); +} + +drake::systems::EventStatus SamplingC3Controller::ComputePlan( + const Context& context, + DiscreteValues* discrete_state) const { + auto start = std::chrono::high_resolution_clock::now(); + + // Evaluate input ports. + const auto& radio_out = + this->EvalInputValue(context, radio_port_); + // Not sure why x_lcs_des is a vector while lcs_x_curr is a timestamped + // vector. + const BasicVector& x_lcs_des = + *this->template EvalVectorInput(context, target_input_port_); + const BasicVector& x_lcs_final_des = + *this->template EvalVectorInput(context, + final_target_input_port_); + const TimestampedVector* lcs_x_curr = + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); + + // Store the current LCS state. + drake::VectorX x_lcs_curr = lcs_x_curr->get_data(); + + if (verbose_) { + std::cout << "x_lcs_curr: " << x_lcs_curr.transpose() << std::endl; + std::cout << "x_lcs_des: " << x_lcs_des.get_value().transpose() + << std::endl; + std::cout << "x_lcs_final_des: " << x_lcs_final_des.get_value().transpose() + << std::endl; + std::cout << "x_pred_curr_plan_: " << x_pred_curr_plan_.transpose() + << std::endl; + } + + // Use a predicted EE state, if desired. + bool is_teleop = radio_out->channel[14]; + ResolvePredictedEEState(is_teleop, x_lcs_curr); + + discrete_state->get_mutable_value(plan_start_time_index_)[0] = + lcs_x_curr->get_timestamp(); + + // Check for workspace limit violations; if any, the controller stops. + CheckForWorkspaceLimitViolations(lcs_x_curr); + + // Compute the current position and orientation errors. + current_position_error_ = + (x_lcs_curr.segment(n_q_-3, 3) - + x_lcs_final_des.get_value().segment(n_q_-3, 3)).norm(); + Eigen::Quaterniond curr_quat( + x_lcs_curr[n_q_-7], x_lcs_curr[n_q_-6], + x_lcs_curr[n_q_-5], x_lcs_curr[n_q_-4]); + Eigen::Quaterniond des_quat( + x_lcs_final_des.get_value()[n_q_-7], x_lcs_final_des.get_value()[n_q_-6], + x_lcs_final_des.get_value()[n_q_-5], x_lcs_final_des.get_value()[n_q_-4]); + Eigen::AngleAxis angle_axis_diff(des_quat * curr_quat.inverse()); + current_orientation_error_ = angle_axis_diff.angle(); + + // Detect if the final target has changed, in which case return to caring only + // about position until the switching threshold has been crossed again. + // Exclude the EE goal from the comparison, since that always changes to be + // above the current object location. + if (!x_final_target_.segment(3, n_x_-3).isApprox( + x_lcs_final_des.value().segment(3, n_x_-3), 1e-5)) { + std::cout << "Detected goal change!" << std::endl; + if (verbose_) { + std::cout << " Last goal: " << x_final_target_.transpose() << std::endl; + std::cout << " New goal: " << x_lcs_final_des.value().transpose() + << std::endl; + std::cout << " --> Error: " + << (x_final_target_.segment(3, n_x_ - 3) - + x_lcs_final_des.value().segment(3, n_x_ - 3)) + .norm() + << std::endl; + } + crossed_cost_switching_threshold_ = false; + x_final_target_ = x_lcs_final_des.value(); + detected_goal_changes_++; + + // Reset the sample buffer now that the costs have changed. + sample_buffer_ = MatrixXd::Zero(sampling_params_.N_sample_buffer, n_q_); + sample_costs_buffer_ = + -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); + } + + // If the object is close to desired XY location, track its full pose. + if (!crossed_cost_switching_threshold_) { + if ((x_lcs_curr.segment(n_q_-3,2)-x_lcs_final_des.value().segment(n_q_-3,2)) + .norm() < progress_params_.cost_switching_threshold_distance) { + crossed_cost_switching_threshold_ = true; + std::cout << "Crossed cost switching threshold." << std::endl; + + // Reset the sample buffer and metrics now that the costs have changed. + sample_buffer_ = MatrixXd::Zero(sampling_params_.N_sample_buffer, n_q_); + sample_costs_buffer_ = + -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); + if (is_doing_c3_) { ResetProgressMetrics(); } + } + } + + // Build C3Options from SamplingC3Options based on the + // crossed_cost_switching_threshold_ flag. + C3Options c3_options = sampling_c3_options_.GetC3Options( + crossed_cost_switching_threshold_); + + // Update the cost matrices: Q_, R_, G_, and U_. + UpdateCostMatrices(x_lcs_curr, x_lcs_des, c3_options); + + // Generate states, differing from the current state only by EE sample + // locations. + std::vector candidate_states = + GenerateSampleStates(n_q_, n_v_, n_u_, x_lcs_curr, is_doing_c3_, + sampling_params_, sampling_c3_options_, plant_, + context_, plant_ad_, context_ad_, contact_pairs_); + + // Add the previous best repositioning target to the candidate states at the + // index 1 always. (Index 0 will become the current state.) + if (!is_doing_c3_) { + Eigen::VectorXd repositioning_target_state = x_lcs_curr; + repositioning_target_state.head(3) = prev_repositioning_target_; + candidate_states.insert(candidate_states.begin(), + repositioning_target_state); + } + // Insert the current location at the beginning of the candidate states. + candidate_states.insert(candidate_states.begin(), x_lcs_curr); + int num_total_samples = candidate_states.size(); + + if (verbose_) { + std::cout << "num_total_samples: " << num_total_samples << std::endl; + } + + // Update the set of sample locations under consideration. + all_sample_locations_.clear(); + for (int i = 0; i < num_total_samples; i++) { + all_sample_locations_.push_back(candidate_states[i].head(3)); + } + + // Make LCS objects for each sample. + auto lcs_pair = SamplingC3Controller::CreateLCSObjectsForSamples( + candidate_states, x_lcs_curr, c3_options, c3_options); + std::vector lcs_candidates = lcs_pair.first; + std::vector lcs_candidates_for_cost = lcs_pair.second; + + // Prepare variables that will get used or filled in by parallelization. + all_sample_costs_ = std::vector(num_total_samples, -1); + all_sample_dynamically_feasible_plans_ = + std::vector>( + num_total_samples, + std::vector(N_ + 1, VectorXd::Zero(n_x_))); + std::vector> c3_objects( + num_total_samples, nullptr); + bool force_tracking_disabled = radio_out->channel[11]; + C3CostComputationType cost_type = progress_params_.cost_type; + if (!crossed_cost_switching_threshold_) { + cost_type = progress_params_.cost_type_position; + } + +// Parallelize over computing C3 costs for each sample. +#pragma omp parallel for num_threads(num_threads_to_use_) + for (int i = 0; i < num_total_samples; i++) { + bool print_cost_breakdown = radio_out->channel[7] && + (i == SampleIndex::kCurrentLocation); + + // Get the candidate state and its LCS representation. + Eigen::VectorXd test_state = candidate_states.at(i); + solvers::LCS test_system = lcs_candidates.at(i); + + // Set up C3 with proper projection type and post-solve cost matrices. + std::shared_ptr test_c3_object; + std::vector x_desired = + std::vector(N_ + 1, x_lcs_des.value()); + if (c3_options.projection_type == "MIQP") { + test_c3_object = std::make_shared( + test_system, C3::CostMatrices(Q_, R_, G_, U_), x_desired, c3_options); + } else if (c3_options.projection_type == "QP") { + test_c3_object = std::make_shared( + test_system, C3::CostMatrices(Q_, R_, G_, U_), x_desired, c3_options); + } // Unknown projection types are rejected in the initialization. + test_c3_object->UpdateCostLCS(lcs_candidates_for_cost.at(i)); + + // Workspace limits. + for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { + Eigen::RowVectorXd A = VectorXd::Zero(n_x_); + A.segment(0, 3) = sampling_c3_options_.workspace_limits[i].segment(0, 3); + // TODO @bibit: For planar examples, we may want the z constraint to be an + // equality constraint. This would require two different sets of workspace + // limits: one for the C3 solve, and one for the controller for safety + // checks. + test_c3_object->AddLinearConstraint( + A, c3_options.workspace_limits[i][3], c3_options.workspace_limits[i][4], + 1); + } + // Input limits. + for (int i : vector({0, 1})) { + Eigen::RowVectorXd A = VectorXd::Zero(n_u_); + A(i) = 1.0; + test_c3_object->AddLinearConstraint( + A, c3_options.u_horizontal_limits[0], c3_options.u_horizontal_limits[1], + 2); + } + for (int i : vector({2})) { + Eigen::RowVectorXd A = VectorXd::Zero(n_u_); + A(i) = 1.0; + test_c3_object->AddLinearConstraint( + A, c3_options.u_vertical_limits[0], c3_options.u_vertical_limits[1], 2); + } + + // Solve C3, store resulting object and cost. + test_c3_object->SetOsqpSolverOptions(solver_options_); + test_c3_object->Solve(test_state, verbose_); + std::pair> cost_trajectory_pair = + test_c3_object->CalcCost( + cost_type, sampling_c3_options_.Kp_for_ee_pd_rollout, + sampling_c3_options_.Kd_for_ee_pd_rollout, force_tracking_disabled, + print_cost_breakdown, verbose_); + + double c3_cost = cost_trajectory_pair.first; + all_sample_dynamically_feasible_plans_.at(i) = cost_trajectory_pair.second; + +#pragma omp critical + { + c3_objects.at(i) = test_c3_object; + } + // Add travel cost (just looking at xy displacement, not also z). + double xy_travel_distance = (test_state.head(2)-x_lcs_curr.head(2)).norm(); + all_sample_costs_[i] = + c3_cost + progress_params_.travel_cost_per_meter * xy_travel_distance; + + // Add additional costs based on repositioning progress. + if ((i == SampleIndex::kCurrentReposTarget) && finished_reposition_flag_) { + all_sample_costs_[i] += progress_params_.finished_reposition_cost; + finished_reposition_flag_ = false; + } + } + // End of parallelization + + // Update the sample buffer. Do this before switching modes since 1) if in + // repositioning mode, don't add the repositioning target over and over again, + // and 2) since the best sample in the buffer may be the best sample overall + // and could be considered as a repositioning target. + MaintainSampleBuffer(x_lcs_curr); + + // Augment the considered samples with the best from the buffer, if eligible. + AugmentSamplesWithBuffer(c3_objects); + + // Set up hysteresis values based on if the cost switching threshold has been + // crossed. + double hyst_c3_to_repos = progress_params_.hyst_c3_to_repos; + double hyst_repos_to_c3 = progress_params_.hyst_repos_to_c3; + double hyst_repos_to_repos = progress_params_.hyst_repos_to_repos; + double hyst_c3_to_repos_frac = progress_params_.hyst_c3_to_repos_frac; + double hyst_repos_to_c3_frac = progress_params_.hyst_repos_to_c3_frac; + double hyst_repos_to_repos_frac = progress_params_.hyst_repos_to_repos_frac; + if (!crossed_cost_switching_threshold_) { + hyst_c3_to_repos = progress_params_.hyst_c3_to_repos_position; + hyst_repos_to_c3 = progress_params_.hyst_repos_to_c3_position; + hyst_repos_to_repos = progress_params_.hyst_repos_to_repos_position; + hyst_c3_to_repos_frac = progress_params_.hyst_c3_to_repos_frac_position; + hyst_repos_to_c3_frac = progress_params_.hyst_repos_to_c3_frac_position; + hyst_repos_to_repos_frac = + progress_params_.hyst_repos_to_repos_frac_position; + } + + // Review the cost results to determine the best sample. + bool force_c3_mode = radio_out->channel[12]; + double best_other_cost; + if (num_total_samples > 1) { + std::vector additional_sample_cost_vector = std::vector( + all_sample_costs_.begin() + 1, all_sample_costs_.end()); + best_other_cost = *std::min_element(additional_sample_cost_vector.begin(), + additional_sample_cost_vector.end()); + std::vector::iterator it = + std::min_element(std::begin(additional_sample_cost_vector), + std::end(additional_sample_cost_vector)); + best_sample_index_ = (SampleIndex)( + std::distance(std::begin(additional_sample_cost_vector), it) + 1); + } else { + force_c3_mode = true; + } + + if (verbose_) { + std::cout << "All sample costs before hystereses: " << std::endl; + for (int i = 0; i < num_total_samples; i++) { + std::cout << "Sample " << i << " cost: " << all_sample_costs_[i] + << std::endl; + } + std::cout << "In C3 mode? " << is_doing_c3_ << std::endl; + } + + // Determine whether to do C3 or reposition. + mode_switch_reason_ = ModeSwitchReason::kNoSwitch; + double curr_cost = all_sample_costs_[SampleIndex::kCurrentLocation]; + double repos_target_cost =all_sample_costs_[SampleIndex::kCurrentReposTarget]; + if (is_doing_c3_ == true) { // Currently doing C3. + pursued_target_source_ = PursuedTargetSource::kNoTarget; + + // Keep track of progress while in C3 mode. + bool met_minimum_progress = true; // Reset by below function. + bool print_current_pos_and_rot_cost = radio_out->channel[6]; + KeepTrackOfC3ModeProgress( + x_lcs_curr, x_lcs_final_des, met_minimum_progress, + print_current_pos_and_rot_cost); + + // Switch to repositioning if progress was insufficient. + if (!met_minimum_progress && !force_c3_mode && + (sampling_params_.num_additional_samples_c3 > 0)) { + is_doing_c3_ = false; + mode_switch_reason_ = ModeSwitchReason::kToReposUnproductive; + std::cout << "Repositioning after not making progress in C3" << std::endl; + } + + // Switch to repositioning if one of the other samples is better, with + // hysteresis. + else if ( + ((!progress_params_.use_relative_hysteresis && + curr_cost > best_other_cost + hyst_c3_to_repos) || + (progress_params_.use_relative_hysteresis && + curr_cost > best_other_cost + hyst_c3_to_repos_frac*curr_cost)) && + !force_c3_mode) + { + is_doing_c3_ = false; + mode_switch_reason_ = ModeSwitchReason::kToReposCost; + std::cout << "Repositioning because found good sample" << std::endl; + } + + // Reset progress metrics if switching to repositioning. + if (!is_doing_c3_) { + finished_reposition_flag_ = false; + ResetProgressMetrics(); + + // Determine the source of the repositioning target. + if (best_sample_index_ > sampling_params_.num_additional_samples_c3) { + pursued_target_source_ = PursuedTargetSource::kFromBuffer; + // Remove the sample from the buffer. + sample_buffer_.row(num_in_buffer_ - 1) = VectorXd::Zero(n_q_); + sample_costs_buffer_[num_in_buffer_ - 1] = -1; + num_in_buffer_--; + } else { + pursued_target_source_ = PursuedTargetSource::kNewSample; + } + } + } else { // Currently repositioning. + // First, apply hysteresis between repositioning targets. + if (best_sample_index_ == SampleIndex::kCurrentReposTarget) { + pursued_target_source_ = PursuedTargetSource::kPrevious; + } else { + // This means there is a lower cost sample other than the current + // repositioning target. If the lowest cost sample is not at least the + // hysteresis amount better than the current repositioning target, then + // continue pursuing the previous repositioning target. + if ( + (repos_target_cost < best_other_cost + hyst_repos_to_repos + && !progress_params_.use_relative_hysteresis) || + (repos_target_cost < best_other_cost + hyst_repos_to_repos_frac* + repos_target_cost + && progress_params_.use_relative_hysteresis)) + { + best_sample_index_ = SampleIndex::kCurrentReposTarget; + best_other_cost = repos_target_cost; + finished_reposition_flag_ = false; + pursued_target_source_ = PursuedTargetSource::kPrevious; + } + // Controller will switch to pursuing a new sample from its previous + // repositioning target only if the cost of switching to that new sample + // (with repos_to_repos hysteresis) is less than switching to C3 from + // current location (with repos_to_c3 hysteresis), so add the + // repos_to_repos hysteresis value here before the comparison to the + // current location C3 cost with repos_to_c3 hysteresis afterwards. + else { + pursued_target_source_ = PursuedTargetSource::kNewSample; + if (!progress_params_.use_relative_hysteresis) { + best_other_cost += hyst_repos_to_repos; + } else { + best_other_cost += hyst_repos_to_repos_frac*repos_target_cost; + } + } + } + + // Switch to C3 if forced by xbox controller. + if (force_c3_mode) { + std::cout << "Forcing into C3 mode" << std::endl; + is_doing_c3_ = true; + mode_switch_reason_ = ModeSwitchReason::kToC3Xbox; + pursued_target_source_ = PursuedTargetSource::kNoTarget; + } + // Switch to C3 if the current sample is better, with hysteresis. + else if ( + (!progress_params_.use_relative_hysteresis && + best_other_cost > curr_cost + hyst_repos_to_c3) || + (progress_params_.use_relative_hysteresis && + best_other_cost > curr_cost + hyst_repos_to_c3_frac*best_other_cost)) + { + is_doing_c3_ = true; + finished_reposition_flag_ = false; + if (repos_target_cost > progress_params_.finished_reposition_cost) { + mode_switch_reason_ = ModeSwitchReason::kToC3ReachedReposTarget; + std::cout << "Switching to C3 because reached repositioning target" + << std::endl; + } else { + mode_switch_reason_ = ModeSwitchReason::kToC3Cost; + std::cout << "Switching to C3 because lower in cost" << std::endl; + } + pursued_target_source_ = PursuedTargetSource::kNoTarget; + } + } + + if (verbose_) { + std::cout << "All sample costs before hystereses: " << std::endl; + for (int i = 0; i < num_total_samples; i++) { + std::cout << "Sample " << i << " cost: " << all_sample_costs_[i] + << std::endl; + } + std::cout << "In C3 mode after considering switch? " << is_doing_c3_ + << std::endl; + } + + // Update C3 objects and intermediates for current and best samples. + c3_curr_plan_ = c3_objects.at(SampleIndex::kCurrentLocation); + c3_best_plan_ = c3_objects.at(best_sample_index_); + // TODO If doing warmstarting, will need to save z, delta, and w vectors. + + // Update the execution trajectories. + double t = context.get_discrete_state(plan_start_time_index_)[0]; + UpdateC3ExecutionTrajectory(x_lcs_curr, t); + UpdateRepositioningExecutionTrajectory(x_lcs_curr, t); + + if (verbose_) { + std::cout << "x_pred_curr_plan_ after updating: " + << x_pred_curr_plan_.transpose() << std::endl; + std::vector zs = c3_curr_plan_->GetFullSolution(); + for (int i = 0; i < N_; i++) { + std::cout << "z[" << i << "]: " << zs[i].transpose() << std::endl; + } + solvers::LCS verbose_lcs = lcs_candidates.at(SampleIndex::kCurrentLocation); + Eigen::MatrixXd E = verbose_lcs.E_[0]; + Eigen::MatrixXd F = verbose_lcs.F_[0]; + Eigen::MatrixXd H = verbose_lcs.H_[0]; + Eigen::VectorXd c = verbose_lcs.c_[0]; + std::cout << "\nRight side of complementarity: " << std::endl; + for (int i = 0; i < N_; i++) { + Eigen::VectorXd x = zs[i].head(n_x_); + Eigen::VectorXd lambda = zs[i].segment(n_x_, n_lambda_); + Eigen::VectorXd u = zs[i].tail(n_u_); + std::cout << "\t" << i << ": " + << (E * x + F * lambda + H * u + c).transpose() << std::endl; + } + std::cout << "\nComplementarity violation: " << std::endl; + for (int i = 0; i < N_; i++) { + Eigen::VectorXd x = zs[i].head(n_x_); + Eigen::VectorXd lambda = zs[i].segment(n_x_, n_lambda_); + Eigen::VectorXd u = zs[i].tail(n_u_); + std::cout << "\t" << i << ": " + << lambda.dot(E * x + F * lambda + H * u + c) << std::endl; + } + + std::cout << "Dynamically feasible ee current plan: " << std::endl; + for (int i = 0; i < N_ + 1; i++) { + std::cout << all_sample_dynamically_feasible_plans_ + .at(SampleIndex::kCurrentLocation)[i] + .segment(0, 3) + .transpose() + << std::endl; + } + + std::cout << "Dynamically feasible object current plan: " << std::endl; + for (int i = 0; i < N_ + 1; i++) { + std::cout << all_sample_dynamically_feasible_plans_ + .at(SampleIndex::kCurrentLocation)[i] + .segment(n_q_-7, 7) + .transpose() + << std::endl; + } + } + + // Add delay. + std::this_thread::sleep_for( + std::chrono::milliseconds(controller_params_.control_loop_delay_ms)); + + // End of control loop cleanup. + auto finish = std::chrono::high_resolution_clock::now(); + auto elapsed = finish - start; + double solve_time = + std::chrono::duration_cast(elapsed).count()/1e6; + filtered_solve_time_ = (1 - solve_time_filter_constant_) * solve_time + + (solve_time_filter_constant_)*filtered_solve_time_; + + if (verbose_) { + std::cout << "At end of loop solve_time: " << solve_time << std::endl; + std::cout << "At end of loop filtered_solve_time_: " << filtered_solve_time_ + << std::endl; + } + + return drake::systems::EventStatus::Succeeded(); +} + +// Use a predicted EE state, if opted by controller settings. The predicted +// location is clamped to a reasonable distance from the current EE location. +// Optionally, there is a reset mechanism to prevent using a predicted EE +// location if the last state is closer to the current state than the +// prediction. +void SamplingC3Controller::ResolvePredictedEEState( + const bool& is_teleop, drake::VectorX& x_lcs_curr) const { + // Store the current actual state before applying prediction in preparation + // for next control loop. + x_from_last_control_loop_ = x_lcs_curr; + + // Detect if prediction is requested in current mode. + bool in_c3_mode_and_predict = + sampling_c3_options_.use_predicted_x0_c3 && is_doing_c3_; + bool in_repos_mode_and_predict = + sampling_c3_options_.use_predicted_x0_repos && !is_doing_c3_; + + if (!x_pred_curr_plan_.isZero() && !is_teleop && + (in_c3_mode_and_predict || in_repos_mode_and_predict)) { + // Consider the current, last, and predicted states. + Eigen::Vector3d curr_ee = x_lcs_curr.head(3); + Eigen::Vector3d last_ee = x_from_last_control_loop_.head(3); + Eigen::Vector3d pred_ee = x_pred_from_last_control_loop_.head(3); + + if (((curr_ee - last_ee).norm() < (curr_ee - pred_ee).norm()) && + (curr_ee - pred_ee).norm() > 0.01 && + !x_pred_from_last_control_loop_.isZero() && + sampling_c3_options_.use_predicted_x0_reset_mechanism) { + // Skip using the predicted state. + if (verbose_) { + std::cout << "RESET x_pred in mode: C3 " << in_c3_mode_and_predict + << ", or Repositioning " << in_repos_mode_and_predict + << std::endl; + std::cout << "curr_ee-last_ee is " << (curr_ee - last_ee).norm() + << " and curr_ee-pred_ee is " << (curr_ee - pred_ee).norm() + << std::endl; + std::cout << "x_lcs_curr without clamping: " << x_lcs_curr.transpose() + << std::endl; + } + } else { + // Do the clamping. + ClampEndEffectorAcceleration(x_lcs_curr); + if (verbose_) { + std::cout << "x_lcs_curr after clamping in mode: C3 " + << in_c3_mode_and_predict << ", or Repositioning " + << in_repos_mode_and_predict << " --> " + << x_lcs_curr.transpose() << std::endl; + } + } + } + + // Store the predicted actual state in preparation for next control loop. + x_pred_from_last_control_loop_ = x_lcs_curr; +} + +// Clamp end effector acceleration if using predicted state. +void SamplingC3Controller::ClampEndEffectorAcceleration( + drake::VectorX& x_lcs_curr) const { + // Use fixed approximate loop time for acceleration capping heuristic. + float approx_loop_dt = std::min(0.1, filtered_solve_time_); + float nominal_accel = 10; + for (int i = 0; i < 3; i++) { + x_lcs_curr[i] = std::clamp( + x_pred_curr_plan_[i], + x_lcs_curr[i] - nominal_accel * approx_loop_dt * approx_loop_dt, + x_lcs_curr[i] + nominal_accel * approx_loop_dt * approx_loop_dt); + x_lcs_curr[n_q_ + i] = std::clamp( + x_pred_curr_plan_[n_q_ + i], + x_lcs_curr[n_q_ + i] - nominal_accel * approx_loop_dt, + x_lcs_curr[n_q_ + i] + nominal_accel * approx_loop_dt); + } +} + +// Check for workspace limit violations. If violated, the controller errors and +// stops. +void SamplingC3Controller::CheckForWorkspaceLimitViolations( + const TimestampedVector* lcs_x_curr) const { + // xyz checks + for (int i = 0; i < sampling_c3_options_.workspace_limits.size(); ++i) { + DRAKE_DEMAND(lcs_x_curr->get_data().segment(0, 3).transpose() * + sampling_c3_options_.workspace_limits[i].segment(0, 3) > + sampling_c3_options_.workspace_limits[i][3] - + sampling_c3_options_.workspace_margins); + DRAKE_DEMAND(lcs_x_curr->get_data().segment(0, 3).transpose() * + sampling_c3_options_.workspace_limits[i].segment(0, 3) < + sampling_c3_options_.workspace_limits[i][4] + + sampling_c3_options_.workspace_margins); + } + // radius checks + DRAKE_DEMAND(std::pow(lcs_x_curr->get_data()[0], 2) + + std::pow(lcs_x_curr->get_data()[1], 2) > + std::pow(sampling_c3_options_.robot_radius_limits[0] + + sampling_c3_options_.workspace_margins, 2)); + DRAKE_DEMAND(std::pow(lcs_x_curr->get_data()[0], 2) + + std::pow(lcs_x_curr->get_data()[1], 2) < + std::pow(sampling_c3_options_.robot_radius_limits[1] - + sampling_c3_options_.workspace_margins, 2)); +} + +// Update the cost matrices (Q_, R_, G_, U_) in preparation for C3 solves. +// Handle quaternion-dependent cost if enabled. +void SamplingC3Controller::UpdateCostMatrices( + const drake::VectorX& x_lcs_curr, + const BasicVector& x_lcs_des, const C3Options& c3_options) const { + Q_.clear(); + R_.clear(); + G_.clear(); + U_.clear(); + double discount_factor = 1; + + dt_ = c3_options.dt; + for (int i = 0; i < N_ + 1; ++i) { + Q_.push_back(discount_factor * c3_options.Q); + discount_factor *= c3_options.gamma; + if (i < N_) { + R_.push_back(discount_factor * c3_options.R); + G_.push_back(c3_options.G); + U_.push_back(c3_options.U); + } + } + + if (sampling_c3_options_.use_quaternion_dependent_cost && + crossed_cost_switching_threshold_) { + Eigen::VectorXd quat = x_lcs_curr.segment(3, 4); + Eigen::VectorXd quat_desired = x_lcs_des.get_value().segment(3, 4); + Eigen::MatrixXd Q_quaternion_dependent_cost = + hessian_of_squared_quaternion_angle_difference(quat, quat_desired); + + // Get the eigenvalues of the hessian to regularize so the Q matrix is + // always PSD. + double min_eigval = + Q_quaternion_dependent_cost.eigenvalues().real().minCoeff(); + Eigen::MatrixXd Q_quaternion_dependent_regularizer_part_1 = + std::max(0.0, -min_eigval) * Eigen::MatrixXd::Identity(4, 4); + + Eigen::MatrixXd Q_quaternion_dependent_regularizer_part_2 = + quat_desired * quat_desired.transpose(); + DRAKE_DEMAND((Q_quaternion_dependent_cost.rows() == + Q_quaternion_dependent_cost.cols()) && + (Q_quaternion_dependent_cost.rows() == + Q_quaternion_dependent_regularizer_part_2.rows()) && + (Q_quaternion_dependent_cost.rows() == + Q_quaternion_dependent_regularizer_part_2.cols()) && + (Q_quaternion_dependent_cost.rows() == 4)); + double discount_factor = 1; + for (int i = 0; i < N_ + 1; ++i) { + Q_[i].block(3, 3, 4, 4) = + discount_factor * sampling_c3_options_.q_quaternion_dependent_weight * + (Q_quaternion_dependent_cost + + Q_quaternion_dependent_regularizer_part_1 + + sampling_c3_options_.q_quaternion_dependent_regularizer_fraction * + Q_quaternion_dependent_regularizer_part_2); + discount_factor *= sampling_c3_options_.gamma; + } + } + + if (verbose_) { + std::cout << "Q_[0] with gamma " << sampling_c3_options_.gamma << ":" + << std::endl; + std::cout << Q_[0] << std::endl; + std::cout << "R_[0] with gamma " << sampling_c3_options_.gamma << ":" + << std::endl; + std::cout << R_[0] << std::endl; + } +} + +// Create LCS objects (for the C3 solve and also for the C3 cost calculation) +// for each sample. +std::pair, std::vector> +SamplingC3Controller::CreateLCSObjectsForSamples( + const std::vector& candidate_states, + const drake::VectorX& x_lcs_curr, const C3Options& c3_options, + const C3Options& c3_options_curr_location) const { + std::vector lcs_candidates; + std::vector lcs_candidates_for_cost; + + int num_total_samples = candidate_states.size(); + for (int i = 0; i < num_total_samples; i++) { + // Context needs to be updated to create the LCS objects. + UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, + candidate_states[i]); + + // Resolve the contact pairs and create the LCS. + vector> resolved_contact_pairs = + LCSFactory::PreProcessor( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to, + c3_options.num_friction_directions, verbose_); + solvers::LCS lcs_object_sample = solvers::LCSFactory::LinearizePlantToLCS( + plant_, *context_, plant_ad_, *context_ad_, resolved_contact_pairs, + c3_options.num_friction_directions, c3_options.mu, dt_, N_, + contact_model_); + lcs_candidates.push_back(lcs_object_sample); + + // Create different LCS objects for cost calculation. + vector> resolved_contact_pairs_for_cost_simulation; + resolved_contact_pairs_for_cost_simulation = LCSFactory::PreProcessor( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to_for_cost, + sampling_c3_options_.num_friction_directions, verbose_); + solvers::LCS lcs_object_sample_for_cost_simulation = + solvers::LCSFactory::LinearizePlantToLCS( + plant_, *context_, plant_ad_, *context_ad_, + resolved_contact_pairs_for_cost_simulation, + sampling_c3_options_.num_friction_directions, + sampling_c3_options_.mu_for_cost, dt_, N_, contact_model_); + lcs_candidates_for_cost.push_back(lcs_object_sample_for_cost_simulation); + } + + // Reset the context to the current lcs state. + UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, + x_lcs_curr); + + if (verbose_) { + // Print the LCS matrices for verbose inspection. + solvers::LCS verbose_lcs = lcs_candidates.at( + SampleIndex::kCurrentLocation); + std::cout << "A: " << std::endl; + std::cout << verbose_lcs.A_[0] << std::endl; + std::cout << "B: " << std::endl; + std::cout << verbose_lcs.B_[0] << std::endl; + std::cout << "D: " << std::endl; + std::cout << verbose_lcs.D_[0] << std::endl; + std::cout << "d: " << std::endl; + std::cout << verbose_lcs.d_[0] << std::endl; + std::cout << "E: " << std::endl; + std::cout << verbose_lcs.E_[0] << std::endl; + std::cout << "F: " << std::endl; + std::cout << verbose_lcs.F_[0] << std::endl; + std::cout << "H: " << std::endl; + std::cout << verbose_lcs.H_[0] << std::endl; + std::cout << "c: " << std::endl; + std::cout << verbose_lcs.c_[0] << std::endl; + } + + return std::make_pair(lcs_candidates, lcs_candidates_for_cost); +} + +void SamplingC3Controller::UpdateC3ExecutionTrajectory( + const VectorXd& x_lcs, const double& t_context) const { + // Get the input and full state solution from the plan. + vector u_sol = c3_curr_plan_->GetInputSolution(); + vector x_sol = c3_curr_plan_->GetStateSolution(); + + // Setting up matrices to set up LCMTrajectory object. + Eigen::MatrixXd knots = Eigen::MatrixXd::Zero(n_x_, N_); + Eigen::VectorXd timestamps = Eigen::VectorXd::Zero(N_); + + // Set up matrices for LCMTrajectory object. + for (int i = 0; i < N_; i++) { + knots.col(i) = x_sol[i]; + timestamps[i] = t_context + filtered_solve_time_ + (i)*dt_; + } + + // Update predicted next state if in this mode. + if (is_doing_c3_) { + if (filtered_solve_time_ < (N_ - 1) * dt_) { + int last_passed_index = filtered_solve_time_ / dt_; + double fraction_to_next_index = + (filtered_solve_time_ / dt_) - (double)last_passed_index; + x_pred_curr_plan_ = + knots.col(last_passed_index) + + fraction_to_next_index * + (knots.col(last_passed_index + 1) - knots.col(last_passed_index)); + } else { + x_pred_curr_plan_ = knots.col(N_ - 1); + } + } + + // Add end effector position target to LCM Trajectory. + LcmTrajectory::Trajectory ee_traj; + ee_traj.traj_name = "end_effector_position_target"; + ee_traj.datatypes = std::vector(3, "double"); + ee_traj.datapoints = knots(Eigen::seqN(0, 3), Eigen::seqN(0, N_)); + ee_traj.time_vector = timestamps.cast(); + c3_execution_lcm_traj_.ClearTrajectories(); + c3_execution_lcm_traj_.AddTrajectory(ee_traj.traj_name, ee_traj); + + // Add end effector force target to LCM Trajectory. + // In c3 mode, the end effector forces should match the solved c3 inputs. + Eigen::MatrixXd force_samples = Eigen::MatrixXd::Zero(3, N_); + for (int i = 0; i < N_; i++) { + force_samples.col(i) = u_sol[i]; + } + LcmTrajectory::Trajectory force_traj; + force_traj.traj_name = "end_effector_force_target"; + force_traj.datatypes = + std::vector(force_samples.rows(), "double"); + force_traj.datapoints = force_samples; + force_traj.time_vector = timestamps.cast(); + c3_execution_lcm_traj_.AddTrajectory(force_traj.traj_name, force_traj); + + // No need to add object position and orientation since these are outputs sent + // in the C3 plans. +} + +// Compute repositioning trajectory. +void SamplingC3Controller::UpdateRepositioningExecutionTrajectory( + const VectorXd& x_lcs, const double& t_context) const { + // Get the best sample location. + Eigen::Vector3d best_sample_location = + all_sample_locations_[best_sample_index_]; + // Update the previous repositioning target for reference in next loop. + prev_repositioning_target_ = best_sample_location; + + // Generate knot points according to the repositioning strategy. + Eigen::MatrixXd knots = Reposition( + n_q_, n_x_, N_, x_lcs, best_sample_location, dt_, is_doing_c3_, + finished_reposition_flag_, reposition_params_, sampling_c3_options_); + + // Update predicted next state if in this mode. + if (!is_doing_c3_) { + if (filtered_solve_time_ < (N_ - 1) * dt_) { + int last_passed_index = filtered_solve_time_ / dt_; + double fraction_to_next_index = + (filtered_solve_time_ / dt_) - (double)last_passed_index; + x_pred_curr_plan_ = + knots.col(last_passed_index) + + fraction_to_next_index * + (knots.col(last_passed_index + 1) - knots.col(last_passed_index)); + } else { + x_pred_curr_plan_ = knots.col(N_ - 1); + } + } + + // Set up the trajectory. + Eigen::VectorXd timestamps = Eigen::VectorXd::Zero(N_); + for (int i = 0; i < N_; i++) { + timestamps[i] = t_context + filtered_solve_time_ + (i)*dt_; + } + + LcmTrajectory::Trajectory ee_traj; + ee_traj.traj_name = "end_effector_position_target"; + ee_traj.datatypes = std::vector(3, "double"); + ee_traj.datapoints = knots(Eigen::seqN(0, 3), Eigen::seqN(0, N_)); + ee_traj.time_vector = timestamps.cast(); + repos_execution_lcm_traj_.ClearTrajectories(); + repos_execution_lcm_traj_.AddTrajectory(ee_traj.traj_name, ee_traj); + + // In repositioning mode, the end effector should not exert forces. + MatrixXd force_samples = MatrixXd::Zero(3, N_); + LcmTrajectory::Trajectory force_traj; + force_traj.traj_name = "end_effector_force_target"; + force_traj.datatypes = + std::vector(force_samples.rows(), "double"); + force_traj.datapoints = force_samples; + force_traj.time_vector = timestamps.cast(); + repos_execution_lcm_traj_.AddTrajectory(force_traj.traj_name, force_traj); + + // No need to add object position and orientation. +} + +// Maintain the sample buffer: prune outdated samples and add new. +void SamplingC3Controller::MaintainSampleBuffer(const VectorXd& x_lcs) const { + // Determine if samples are outdated by comparing to the current object + // position and orientation. + Vector3d object_pos = x_lcs.segment(n_q_-3, 3); + Eigen::Vector4d object_quat = x_lcs.segment(3, 4).normalized(); + + MatrixXd buffer_xyzs = + sample_buffer_.block(0, 7, sampling_params_.N_sample_buffer, 3); + MatrixXd buffer_quats = + sample_buffer_.block(0, 3, sampling_params_.N_sample_buffer, 4); + + // First, remove outdated samples that have moved too much from current object + // configuration. + VectorXd quat_dots = (buffer_quats * object_quat).array().abs(); + VectorXd angles = (2.0 * quat_dots.array().acos()); + Eigen::Array mask_satisfies_rot = + (angles.array() < sampling_params_.ang_error_sample_retention); + + MatrixXd pos_deltas = buffer_xyzs.rowwise() - object_pos.transpose(); + VectorXd distances = pos_deltas.rowwise().norm(); + Eigen::Array mask_satisfies_pos = + (distances.array() < sampling_params_.pos_error_sample_retention); + + MatrixXd retained_samples = + MatrixXd::Zero(sampling_params_.N_sample_buffer, n_q_); + VectorXd retained_costs = + -1 * VectorXd::Ones(sampling_params_.N_sample_buffer); + int retained_count = 0; + for (int i = 0; i < sampling_params_.N_sample_buffer; i++) { + if (mask_satisfies_rot[i] && mask_satisfies_pos[i]) { + retained_samples.row(retained_count) = sample_buffer_.row(i); + retained_costs[retained_count] = sample_costs_buffer_[i]; + retained_count++; + } else if (sample_costs_buffer_[i] < 0) { + break; + } + } + sample_buffer_ = retained_samples; + sample_costs_buffer_ = retained_costs; + + // Second, in preparation for adding new samples stored in + // all_sample_locations_ (excluding the current location), if the buffer is + // going to overflow, get rid of the oldest samples first. NOTE: Step 4 + // moves the lowest cost sample in the buffer to the end, so the best sample + // is usually excluded from this cut. + int num_to_add = all_sample_locations_.size() - 1; + if (!is_doing_c3_) { + num_to_add--; + } + if (retained_count + num_to_add > sampling_params_.N_sample_buffer) { + int shift_by = + retained_count + num_to_add - sampling_params_.N_sample_buffer; + retained_count -= shift_by; + sample_buffer_.block(0, 0, retained_count, n_q_) = + sample_buffer_.block(shift_by, 0, retained_count, n_q_); + sample_costs_buffer_.segment(0, retained_count) = + sample_costs_buffer_.segment(shift_by, retained_count); + } + + // Third, add the new samples stored in all_sample_locations_ and + // all_sample_costs_. Don't add the current location (so the sample buffer + // contains more broadly sampled locations) or a currently pursued + // repositioning target. + int buffer_count = retained_count; + for (int i = retained_count; + i < retained_count + all_sample_locations_.size(); i++) { + DRAKE_DEMAND(buffer_count < sampling_params_.N_sample_buffer); + if ((i == retained_count) || (!is_doing_c3_ && i == retained_count + 1)) { + // Skip the current location. + // Skip the repositioning target if in repositioning mode. + } else { + VectorXd new_config = x_lcs.segment(0, n_q_); + new_config.segment(0, 3) = all_sample_locations_[i - retained_count]; + // Ensure a normalized quaternion is written to the buffer. + new_config.segment(3, 4) = object_quat; + sample_buffer_.row(buffer_count) = new_config; + sample_costs_buffer_[buffer_count] = + all_sample_costs_[i - retained_count]; + buffer_count++; + } + } + num_in_buffer_ = buffer_count; + + // Lastly, ensure the lowest cost sample is at the end of the buffer. + VectorXd eligible_costs = sample_costs_buffer_.head(num_in_buffer_); + int lowest_cost_index; + double lowest_buffer_cost = eligible_costs.minCoeff(&lowest_cost_index); + VectorXd lowest_cost_sample = sample_buffer_.row(lowest_cost_index); + sample_buffer_.row(lowest_cost_index) = + sample_buffer_.row(num_in_buffer_ - 1); + sample_costs_buffer_[lowest_cost_index] = + sample_costs_buffer_[num_in_buffer_ - 1]; + sample_buffer_.row(num_in_buffer_ - 1) = lowest_cost_sample; + sample_costs_buffer_[num_in_buffer_ - 1] = lowest_buffer_cost; + + DRAKE_DEMAND(sample_buffer_.rows() == sampling_params_.N_sample_buffer); + DRAKE_DEMAND(sample_buffer_.cols() == n_q_); + DRAKE_DEMAND(sample_costs_buffer_.size() == sampling_params_.N_sample_buffer); +} + +// If eligible, augment the current control loop's considered samples with the +// best one from the buffer. +void SamplingC3Controller::AugmentSamplesWithBuffer( + std::vector>& c3_objects) const { + // Add the best from the buffer to the samples, but only if in C3 mode and + // only if the best in the buffer is distinct from the current set of samples. + if ((is_doing_c3_) && + (sampling_params_.consider_best_buffer_sample_when_leaving_c3)) { + double lowest_buffer_cost = sample_costs_buffer_[num_in_buffer_ - 1]; + Vector3d best_buffer_ee_sample = + sample_buffer_.row(num_in_buffer_ - 1).head(3); + + double lowest_new_cost = + *std::min_element(all_sample_costs_.begin(), all_sample_costs_.end()); + std::vector::iterator it = std::min_element( + std::begin(all_sample_costs_), std::end(all_sample_costs_)); + int lowest_new_cost_index = + (SampleIndex)(std::distance(std::begin(all_sample_costs_), it)); + Vector3d best_new_ee_sample = all_sample_locations_[lowest_new_cost_index]; + + // Initialize the buffer plan with something. + if (dynamically_feasible_buffer_plan_.size() != N_ + 1) { + c3_buffer_plan_ = c3_objects[lowest_new_cost_index]; + dynamically_feasible_buffer_plan_ = + all_sample_dynamically_feasible_plans_[lowest_new_cost_index]; + } + // If the best in the buffer is from the current set of samples, store the + // associated C3 object and dynamically feasible plan, but don't add to the + // set of samples to consider for repositioning. + else if ((abs(lowest_buffer_cost - lowest_new_cost) < 1e-5) && + ((best_buffer_ee_sample - best_new_ee_sample).norm() < 1e-5)) { + c3_buffer_plan_ = c3_objects[lowest_new_cost_index]; + dynamically_feasible_buffer_plan_ = + all_sample_dynamically_feasible_plans_[lowest_new_cost_index]; + } + // If the best in the buffer is distinct from the current set of samples, + // consider it for repositioning by adding it to the set of samples, costs, + // etc. + else { + all_sample_costs_.push_back(lowest_buffer_cost); + all_sample_locations_.push_back(best_buffer_ee_sample); + c3_objects.push_back(c3_buffer_plan_); + all_sample_dynamically_feasible_plans_.push_back( + dynamically_feasible_buffer_plan_); + } + } +} + +// Keep track of the progress made in the current C3 mode. +void SamplingC3Controller::KeepTrackOfC3ModeProgress( + const drake::VectorX& x_lcs_curr, + const BasicVector& x_lcs_final_des, + bool& met_minimum_progress, + const bool& print_current_pos_and_rot_cost) const { + bool updated_cost = false; + bool updated_config_cost = false; + bool updated_pos_or_rot = false; + double cost_progress_fraction = -INFINITY; // Negative means progress. + + Eigen::MatrixXd Q_pos_and_rot = Q_[0].block(n_q_-7, n_q_-7, 7, 7); + Eigen::VectorXd pos_and_rot_error_vec = + x_lcs_curr.segment(n_q_-7, 7) - + x_lcs_final_des.get_value().segment(n_q_-7, 7); + double curr_pos_and_rot_cost = pos_and_rot_error_vec.transpose() * + Q_pos_and_rot * pos_and_rot_error_vec; + + // Check for progress along different metrics. + if ((all_sample_costs_[SampleIndex::kCurrentLocation] < lowest_cost_) || + (lowest_cost_ == -1.0)) { + lowest_cost_ = all_sample_costs_[SampleIndex::kCurrentLocation]; + updated_cost = true; + } + if ((curr_pos_and_rot_cost < lowest_pos_and_rot_current_cost_) || + (lowest_pos_and_rot_current_cost_ == -1.0)) { + lowest_pos_and_rot_current_cost_ = curr_pos_and_rot_cost; + updated_config_cost = true; + } + if ((current_position_error_ < lowest_position_error_) || + (lowest_position_error_ == -1.0)) { + lowest_position_error_ = current_position_error_; + updated_pos_or_rot = true; + } + if ((current_orientation_error_ < lowest_orientation_error_) || + (lowest_orientation_error_ == -1.0)) { + lowest_orientation_error_ = current_orientation_error_; + updated_pos_or_rot = true; + } + + // One of the progress metrics requires a history of object configuration + // costs. Maintain this history and check for progress. + object_config_cost_history_.push(curr_pos_and_rot_cost); + int max_history_length = + progress_params_.progress_enforced_over_n_loops; + if (object_config_cost_history_.size() > max_history_length) { + object_config_cost_history_.pop(); + } + // Check for progress if the history is full. + if (object_config_cost_history_.size() == max_history_length) { + // Note: front() is the oldest cost, back() is the most recent. + cost_progress_fraction = // Negative means progress. + ((object_config_cost_history_.back()-object_config_cost_history_.front()) + / object_config_cost_history_.front()); + } + + if (print_current_pos_and_rot_cost) { + std::cout << "Current rot and pos cost: " << curr_pos_and_rot_cost + << std::endl; + } + + // Keep track of how many control loops have passed since the best seen + // progress metric in this mode. + ProgressMetric progress_metric = progress_params_.track_c3_progress_via; + if (((progress_metric == ProgressMetric::kC3Cost) && updated_cost) + || ((progress_metric == ProgressMetric::kConfigCost) && updated_config_cost) + || ((progress_metric == ProgressMetric::kPosOrRotCost) && + updated_pos_or_rot)) { + best_progress_steps_ago_ = 0; + } else { + best_progress_steps_ago_++; + } + + // Detect if progress was sufficient according to progress metric. + int num_control_loops_to_wait = progress_params_.num_control_loops_to_wait; + if (!crossed_cost_switching_threshold_) { + num_control_loops_to_wait = + progress_params_.num_control_loops_to_wait_position; + } + if (progress_metric == ProgressMetric::kConfigCostDrop) { + if (cost_progress_fraction > -progress_params_.progress_enforced_cost_drop) + { met_minimum_progress = false; } + } + else if (best_progress_steps_ago_ > num_control_loops_to_wait) + { met_minimum_progress = false; } +} + +// Reset the metrics used to track progress in C3 mode. +void SamplingC3Controller::ResetProgressMetrics() const { + lowest_cost_ = -1.0; + lowest_pos_and_rot_current_cost_ = -1.0; + lowest_position_error_ = -1.0; + lowest_orientation_error_ = -1.0; + best_progress_steps_ago_ = 0; + // Clear the stored history of object configuration costs. + while (!object_config_cost_history_.empty()) { + object_config_cost_history_.pop(); + } +} + +// Output port handlers for current location +void SamplingC3Controller::OutputC3SolutionCurrPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + + auto c3_solution = std::make_unique(); + c3_solution->x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); + c3_solution->lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); + c3_solution->u_sol_ = MatrixXf::Zero(n_u_, N_); + c3_solution->time_vector_ = VectorXf::Zero(N_); + + auto z_sol = c3_curr_plan_->GetFullSolution(); + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } + + MatrixXd knots = MatrixXd::Zero(6, N_); + knots.topRows(3) = c3_solution->x_sol_.topRows(3).cast(); + knots.bottomRows(3) = + c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); + + LcmTrajectory::Trajectory end_effector_traj; + end_effector_traj.traj_name = "end_effector_position_target"; + end_effector_traj.datatypes = + std::vector(knots.rows(), "double"); + end_effector_traj.datapoints = knots; + end_effector_traj.time_vector = c3_solution->time_vector_.cast(); + LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, + "end_effector_position_target", + "end_effector_position_target", false); + + MatrixXd force_samples = c3_solution->u_sol_.cast(); + LcmTrajectory::Trajectory force_traj; + force_traj.traj_name = "end_effector_force_target"; + force_traj.datatypes = + std::vector(force_samples.rows(), "double"); + force_traj.datapoints = force_samples; + force_traj.time_vector = c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); + + output->saved_traj = lcm_traj.GenerateLcmObject(); + output->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputC3SolutionCurrPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + + auto c3_solution = std::make_unique(); + c3_solution->x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); + c3_solution->lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); + c3_solution->u_sol_ = MatrixXf::Zero(n_u_, N_); + c3_solution->time_vector_ = VectorXf::Zero(N_); + auto z_sol = c3_curr_plan_->GetFullSolution(); + + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } + + MatrixXd knots = MatrixXd::Zero(6, N_); + knots.topRows(3) = c3_solution->x_sol_.middleRows(n_q_ - 3, 3).cast(); + knots.bottomRows(3) = + c3_solution->x_sol_.middleRows(n_q_ + n_v_ - 3, 3).cast(); + LcmTrajectory::Trajectory object_traj; + object_traj.traj_name = "object_position_target"; + object_traj.datatypes = std::vector(knots.rows(), "double"); + object_traj.datapoints = knots; + object_traj.time_vector = c3_solution->time_vector_.cast(); + LcmTrajectory lcm_traj({object_traj}, {"object_position_target"}, + "object_target", "object_target", false); + + LcmTrajectory::Trajectory object_orientation_traj; + // first 3 rows are rpy, last 3 rows are angular velocity + MatrixXd orientation_samples = MatrixXd::Zero(4, N_); + orientation_samples = + c3_solution->x_sol_.middleRows(n_q_ - 7, 4).cast(); + object_orientation_traj.traj_name = "object_orientation_target"; + object_orientation_traj.datatypes = + std::vector(orientation_samples.rows(), "double"); + object_orientation_traj.datapoints = orientation_samples; + object_orientation_traj.time_vector = + c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(object_orientation_traj.traj_name, + object_orientation_traj); + + output->saved_traj = lcm_traj.GenerateLcmObject(); + output->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputC3SolutionCurrPlan( + const drake::systems::Context& context, + C3Output::C3Solution* c3_solution) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + + auto z_sol = c3_curr_plan_->GetFullSolution(); + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } +} + +void SamplingC3Controller::OutputC3IntermediatesCurrPlan( + const drake::systems::Context& context, + C3Output::C3Intermediates* c3_intermediates) const { + double t = context.get_discrete_state(plan_start_time_index_)[0] + + filtered_solve_time_; + auto z_sol_curr_plan = c3_curr_plan_->GetFullSolution(); + auto delta_curr_plan = c3_curr_plan_->GetDualDeltaSolution(); + auto w_curr_plan = c3_curr_plan_->GetDualWSolution(); + + for (int i = 0; i < N_; i++) { + c3_intermediates->time_vector_(i) = t + i * dt_; + c3_intermediates->z_.col(i) = z_sol_curr_plan[i].cast(); + c3_intermediates->w_.col(i) = w_curr_plan[i].cast(); + c3_intermediates->delta_.col(i) = delta_curr_plan[i].cast(); + } +} + +void SamplingC3Controller::OutputLCSContactJacobianCurrPlan( + const drake::systems::Context& context, + std::pair>* + lcs_contact_jacobian) const { + const TimestampedVector* lcs_x = + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); + + UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, + lcs_x->get_data()); + + C3Options c3_options = sampling_c3_options_.GetC3Options( + crossed_cost_switching_threshold_); + + // Preprocessing the contact pairs + vector> resolved_contact_pairs; + resolved_contact_pairs = LCSFactory::PreProcessor( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to, + c3_options.num_friction_directions, verbose_); + + // print size of resolved_contact_pairs + *lcs_contact_jacobian = LCSFactory::ComputeContactJacobian( + plant_, *context_, resolved_contact_pairs, + c3_options.num_friction_directions, c3_options.mu, contact_model_); +} + +// Output port handlers for best sample location +void SamplingC3Controller::OutputC3SolutionBestPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + + auto z_sol = c3_best_plan_->GetFullSolution(); + auto c3_solution = std::make_unique(); + c3_solution->x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); + c3_solution->lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); + c3_solution->u_sol_ = MatrixXf::Zero(n_u_, N_); + c3_solution->time_vector_ = VectorXf::Zero(N_); + + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } + + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } + + MatrixXd knots = MatrixXd::Zero(6, N_); + knots.topRows(3) = c3_solution->x_sol_.topRows(3).cast(); + knots.bottomRows(3) = + c3_solution->x_sol_.bottomRows(n_v_).topRows(3).cast(); + + LcmTrajectory::Trajectory end_effector_traj; + end_effector_traj.traj_name = "end_effector_position_target"; + end_effector_traj.datatypes = + std::vector(knots.rows(), "double"); + end_effector_traj.datapoints = knots; + end_effector_traj.time_vector = c3_solution->time_vector_.cast(); + LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, + "end_effector_position_target", + "end_effector_position_target", false); + + MatrixXd force_samples = c3_solution->u_sol_.cast(); + LcmTrajectory::Trajectory force_traj; + force_traj.traj_name = "end_effector_force_target"; + force_traj.datatypes = + std::vector(force_samples.rows(), "double"); + force_traj.datapoints = force_samples; + force_traj.time_vector = c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); + + output->saved_traj = lcm_traj.GenerateLcmObject(); + output->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputC3SolutionBestPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + + auto z_sol = c3_best_plan_->GetFullSolution(); + auto c3_solution = std::make_unique(); + c3_solution->x_sol_ = MatrixXf::Zero(n_q_ + n_v_, N_); + c3_solution->lambda_sol_ = MatrixXf::Zero(n_lambda_, N_); + c3_solution->u_sol_ = MatrixXf::Zero(n_u_, N_); + c3_solution->time_vector_ = VectorXf::Zero(N_); + + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } + + MatrixXd knots = MatrixXd::Zero(6, N_); + knots.topRows(3) = c3_solution->x_sol_.middleRows(n_q_ - 3, 3).cast(); + knots.bottomRows(3) = + c3_solution->x_sol_.middleRows(n_q_ + n_v_ - 3, 3).cast(); + LcmTrajectory::Trajectory object_traj; + object_traj.traj_name = "object_position_target"; + object_traj.datatypes = std::vector(knots.rows(), "double"); + object_traj.datapoints = knots; + object_traj.time_vector = c3_solution->time_vector_.cast(); + LcmTrajectory lcm_traj({object_traj}, {"object_position_target"}, + "object_target", "object_target", false); + + LcmTrajectory::Trajectory object_orientation_traj; + // first 3 rows are rpy, last 3 rows are angular velocity + MatrixXd orientation_samples = MatrixXd::Zero(4, N_); + orientation_samples = + c3_solution->x_sol_.middleRows(n_q_ - 7, 4).cast(); + object_orientation_traj.traj_name = "object_orientation_target"; + object_orientation_traj.datatypes = + std::vector(orientation_samples.rows(), "double"); + object_orientation_traj.datapoints = orientation_samples; + object_orientation_traj.time_vector = + c3_solution->time_vector_.cast(); + lcm_traj.AddTrajectory(object_orientation_traj.traj_name, + object_orientation_traj); + + output->saved_traj = lcm_traj.GenerateLcmObject(); + output->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputC3SolutionBestPlan( + const drake::systems::Context& context, + C3Output::C3Solution* c3_solution) const { + double t = context.get_discrete_state(plan_start_time_index_)[0]; + + auto z_sol = c3_best_plan_->GetFullSolution(); + for (int i = 0; i < N_; i++) { + c3_solution->time_vector_(i) = filtered_solve_time_ + t + i * dt_; + c3_solution->x_sol_.col(i) = z_sol[i].segment(0, n_x_).cast(); + c3_solution->lambda_sol_.col(i) = + z_sol[i].segment(n_x_, n_lambda_).cast(); + c3_solution->u_sol_.col(i) = + z_sol[i].segment(n_x_ + n_lambda_, n_u_).cast(); + } +} + +void SamplingC3Controller::OutputC3IntermediatesBestPlan( + const drake::systems::Context& context, + C3Output::C3Intermediates* c3_intermediates) const { + double t = context.get_discrete_state(plan_start_time_index_)[0] + + filtered_solve_time_; + auto z_sol_best_plan = c3_best_plan_->GetFullSolution(); + auto delta_best_plan = c3_best_plan_->GetDualDeltaSolution(); + auto w_best_plan = c3_best_plan_->GetDualWSolution(); + + for (int i = 0; i < N_; i++) { + c3_intermediates->time_vector_(i) = t + i * dt_; + c3_intermediates->z_.col(i) = z_sol_best_plan[i].cast(); + c3_intermediates->w_.col(i) = w_best_plan[i].cast(); + c3_intermediates->delta_.col(i) = delta_best_plan[i].cast(); + } +} + +void SamplingC3Controller::OutputLCSContactJacobianBestPlan( + const drake::systems::Context& context, + std::pair>* + lcs_contact_jacobian) const { + const TimestampedVector* lcs_x = + (TimestampedVector*)this->EvalVectorInput(context, + lcs_state_input_port_); + + // Linearize about state with end effector in sample location. + VectorXd x_sample = lcs_x->get_data(); + x_sample.head(3) = all_sample_locations_[best_sample_index_]; + UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, + x_sample); + + C3Options c3_options = sampling_c3_options_.GetC3Options( + crossed_cost_switching_threshold_); + + // Preprocess the contact pairs. + vector> resolved_contact_pairs; + resolved_contact_pairs = LCSFactory::PreProcessor( + plant_, *context_, contact_pairs_, + sampling_c3_options_.resolve_contacts_to, + c3_options.num_friction_directions, verbose_); + *lcs_contact_jacobian = LCSFactory::ComputeContactJacobian( + plant_, *context_, resolved_contact_pairs, + c3_options.num_friction_directions, c3_options.mu, contact_model_); + + // Revert the context. + UpdateContext(n_q_, n_v_, n_u_, plant_, context_, plant_ad_, context_ad_, + lcs_x->get_data()); +} + +// Output port handlers for executing C3 and repositioning ports +void SamplingC3Controller::OutputC3TrajExecuteActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_c3_execution_lcm_traj) const { + // Returned trajectory includes EE positions and feed-forward forces. + LcmTrajectory::Trajectory end_effector_traj = + c3_execution_lcm_traj_.GetTrajectory("end_effector_position_target"); + DRAKE_DEMAND(end_effector_traj.datapoints.rows() == 3); + LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, + "end_effector_position_target", + "end_effector_position_target", false); + + LcmTrajectory::Trajectory force_traj = + c3_execution_lcm_traj_.GetTrajectory("end_effector_force_target"); + lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); + + output_c3_execution_lcm_traj->saved_traj = lcm_traj.GenerateLcmObject(); + output_c3_execution_lcm_traj->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputReposTrajExecuteActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_repos_execution_lcm_traj) + const { + // Returned trajectory includes EE positions and feed-forward forces. + LcmTrajectory::Trajectory end_effector_traj = + repos_execution_lcm_traj_.GetTrajectory("end_effector_position_target"); + DRAKE_DEMAND(end_effector_traj.datapoints.rows() == 3); + LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, + "end_effector_position_target", + "end_effector_position_target", false); + + LcmTrajectory::Trajectory force_traj = + repos_execution_lcm_traj_.GetTrajectory("end_effector_force_target"); + lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); + + output_repos_execution_lcm_traj->saved_traj = lcm_traj.GenerateLcmObject(); + output_repos_execution_lcm_traj->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputTrajExecuteActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_execution_lcm_traj) const { + LcmTrajectory execution_lcm_traj; + if (is_doing_c3_) { + execution_lcm_traj = c3_execution_lcm_traj_; + } else { + execution_lcm_traj = repos_execution_lcm_traj_; + } + + // Returned trajectory includes EE positions and feed-forward forces. + LcmTrajectory::Trajectory end_effector_traj = + execution_lcm_traj.GetTrajectory("end_effector_position_target"); + DRAKE_DEMAND(end_effector_traj.datapoints.rows() == 3); + LcmTrajectory lcm_traj({end_effector_traj}, {"end_effector_position_target"}, + "end_effector_position_target", + "end_effector_position_target", false); + + MatrixXd force_samples = MatrixXd::Zero(3, N_); + LcmTrajectory::Trajectory force_traj = + execution_lcm_traj.GetTrajectory("end_effector_force_target"); + lcm_traj.AddTrajectory(force_traj.traj_name, force_traj); + + output_execution_lcm_traj->saved_traj = lcm_traj.GenerateLcmObject(); + output_execution_lcm_traj->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputIsC3Mode( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const { + Eigen::VectorXd vec = VectorXd::Constant(1, is_doing_c3_); + Eigen::MatrixXd c3_mode_data = Eigen::MatrixXd::Zero(1, 1); + Eigen::VectorXd timestamp = Eigen::VectorXd::Zero(1); + + // Read the boolean value into the matrix. + c3_mode_data(0, 0) = vec(0); + + LcmTrajectory::Trajectory c3_mode; + c3_mode.traj_name = "is_c3_mode"; + c3_mode.datatypes = std::vector(1, "bool"); + c3_mode.datapoints = c3_mode_data; + c3_mode.time_vector = timestamp.cast(); + LcmTrajectory c3_mode_traj({c3_mode}, {"is_c3_mode"}, "is_c3_mode", + "is_c3_mode", false); + + output->saved_traj = c3_mode_traj.GenerateLcmObject(); + output->utime = context.get_time() * 1e6; +} + +// Output port handler for Dynamically feasible trajectory used for cost +// computation. This will directy output an lcmt_timestamped_saved_traj +// object with the dynamically feasible trajectory. +void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_curr_plan_actor) + const { + std::vector dynamically_feasible_traj = + std::vector(N_ + 1, VectorXd::Zero(n_x_)); + for (int i = 0; i < N_ + 1; i++) { + dynamically_feasible_traj[i] << + all_sample_dynamically_feasible_plans_.at( + SampleIndex::kCurrentLocation)[i]; + } + + Eigen::MatrixXd knots = + Eigen::MatrixXd::Zero(3, dynamically_feasible_traj.size()); + Eigen::VectorXd timestamps = + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + for (int i = 0; i < dynamically_feasible_traj.size(); i++) { + knots.col(i) = dynamically_feasible_traj[i].head(3); + timestamps(i) = i; + } + + LcmTrajectory::Trajectory ee_traj; + Eigen::MatrixXd position_samples = Eigen::MatrixXd::Zero(3, N_ + 1); + position_samples = knots.bottomRows(3); + ee_traj.traj_name = "ee_position_target"; + ee_traj.datatypes = + std::vector(position_samples.rows(), "double"); + ee_traj.datapoints = position_samples; + ee_traj.time_vector = timestamps.cast(); + + LcmTrajectory ee_traj_lcm({ee_traj}, {"ee_position_target"}, + "ee_position_target", "ee_position_target", false); + + dynamically_feasible_curr_plan_actor->saved_traj = + ee_traj_lcm.GenerateLcmObject(); + dynamically_feasible_curr_plan_actor->utime = context.get_time() * 1e6; +} + +// Output port handler for Dynamically feasible trajectory used for cost +// computation. +void SamplingC3Controller::OutputDynamicallyFeasibleCurrPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_curr_plan_object) + const { + std::vector dynamically_feasible_traj = + std::vector(N_ + 1, VectorXd::Zero(n_x_)); + for (int i = 0; i < N_ + 1; i++) { + dynamically_feasible_traj[i] << + all_sample_dynamically_feasible_plans_.at( + SampleIndex::kCurrentLocation)[i]; + } + + Eigen::MatrixXd knots = + Eigen::MatrixXd::Zero(7, dynamically_feasible_traj.size()); + Eigen::VectorXd timestamps = + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + for (int i = 0; i < dynamically_feasible_traj.size(); i++) { + knots.col(i) = dynamically_feasible_traj[i].segment(n_q_-7, 7); + timestamps(i) = i; + } + + LcmTrajectory::Trajectory object_traj; + Eigen::MatrixXd position_samples = Eigen::MatrixXd::Zero(3, 6); + position_samples = knots.bottomRows(3); + object_traj.traj_name = "object_position_target"; + object_traj.datatypes = + std::vector(position_samples.rows(), "double"); + object_traj.datapoints = position_samples; + object_traj.time_vector = timestamps.cast(); + LcmTrajectory lcm_traj({object_traj}, {"object_position_target"}, + "object_target", "object_target", false); + + LcmTrajectory::Trajectory object_orientation_traj; + Eigen::MatrixXd orientation_samples = Eigen::MatrixXd::Zero(4, 6); + orientation_samples = knots.topRows(4); + object_orientation_traj.traj_name = "object_orientation_target"; + object_orientation_traj.datatypes = + std::vector(orientation_samples.rows(), "double"); + object_orientation_traj.datapoints = orientation_samples; + object_orientation_traj.time_vector = timestamps.cast(); + lcm_traj.AddTrajectory(object_orientation_traj.traj_name, + object_orientation_traj); + + dynamically_feasible_curr_plan_object->saved_traj = + lcm_traj.GenerateLcmObject(); + dynamically_feasible_curr_plan_object->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_best_plan) + const { + std::vector dynamically_feasible_traj = + std::vector(N_ + 1, VectorXd::Zero(n_x_)); + for (int i = 0; i < N_ + 1; i++) { + dynamically_feasible_traj[i] << + all_sample_dynamically_feasible_plans_.at(best_sample_index_)[i]; + } + + Eigen::MatrixXd knots = + Eigen::MatrixXd::Zero(3, dynamically_feasible_traj.size()); + Eigen::VectorXd timestamps = + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + for (int i = 0; i < dynamically_feasible_traj.size(); i++) { + knots.col(i) = dynamically_feasible_traj[i].head(3); + timestamps(i) = i; + } + + LcmTrajectory::Trajectory ee_traj; + Eigen::MatrixXd position_samples = Eigen::MatrixXd::Zero(3, 6); + position_samples = knots.bottomRows(3); + ee_traj.traj_name = "ee_position_target"; + ee_traj.datatypes = + std::vector(position_samples.rows(), "double"); + ee_traj.datapoints = position_samples; + ee_traj.time_vector = timestamps.cast(); + + LcmTrajectory ee_traj_lcm({ee_traj}, {"ee_position_target"}, + "ee_position_target", "ee_position_target", false); + + dynamically_feasible_best_plan->saved_traj = ee_traj_lcm.GenerateLcmObject(); + dynamically_feasible_best_plan->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputDynamicallyFeasibleBestPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* dynamically_feasible_best_plan) + const { + std::vector dynamically_feasible_traj = + std::vector(N_ + 1, VectorXd::Zero(n_x_)); + for (int i = 0; i < N_ + 1; i++) { + dynamically_feasible_traj[i] << + all_sample_dynamically_feasible_plans_.at(best_sample_index_)[i]; + } + + Eigen::MatrixXd knots = + Eigen::MatrixXd::Zero(7, dynamically_feasible_traj.size()); + Eigen::VectorXd timestamps = + Eigen::VectorXd::Zero(dynamically_feasible_traj.size()); + for (int i = 0; i < dynamically_feasible_traj.size(); i++) { + knots.col(i) = dynamically_feasible_traj[i].segment(n_q_-7, 7); + timestamps(i) = i; + } + + LcmTrajectory::Trajectory object_traj; + Eigen::MatrixXd position_samples = Eigen::MatrixXd::Zero(3, 6); + position_samples = knots.bottomRows(3); + object_traj.traj_name = "object_position_target"; + object_traj.datatypes = + std::vector(position_samples.rows(), "double"); + object_traj.datapoints = position_samples; + object_traj.time_vector = timestamps.cast(); + LcmTrajectory lcm_traj({object_traj}, {"object_position_target"}, + "object_target", "object_target", false); + + LcmTrajectory::Trajectory object_orientation_traj; + Eigen::MatrixXd orientation_samples = Eigen::MatrixXd::Zero(4, 6); + orientation_samples = knots.topRows(4); + object_orientation_traj.traj_name = "object_orientation_target"; + object_orientation_traj.datatypes = + std::vector(orientation_samples.rows(), "double"); + object_orientation_traj.datapoints = orientation_samples; + object_orientation_traj.time_vector = timestamps.cast(); + lcm_traj.AddTrajectory(object_orientation_traj.traj_name, + object_orientation_traj); + + dynamically_feasible_best_plan->saved_traj = lcm_traj.GenerateLcmObject(); + dynamically_feasible_best_plan->utime = context.get_time() * 1e6; +} + +// Output port handlers for sample-related ports +void SamplingC3Controller::OutputAllSampleLocations( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_all_sample_locations) const { + std::vector sample_locations = std::vector( + all_sample_locations_.begin(), all_sample_locations_.end()); + // Pad with zeros to make sure the size is max_num_samples_ + 1 for the + // visualizer. + while (sample_locations.size() < max_num_samples_ + 1) { + sample_locations.push_back(Vector3d::Zero()); + } + + Eigen::MatrixXd sample_datapoints = + Eigen::MatrixXd::Zero(3, sample_locations.size()); + Eigen::VectorXd timestamps = Eigen::VectorXd::Zero(sample_locations.size()); + for (int i = 0; i < sample_locations.size(); i++) { + sample_datapoints.col(i) = sample_locations[i]; + timestamps(i) = i; + } + + LcmTrajectory::Trajectory sample_positions; + sample_positions.traj_name = "sample_locations"; + sample_positions.datatypes = std::vector(3, "double"); + sample_positions.datapoints = sample_datapoints; + sample_positions.time_vector = timestamps.cast(); + LcmTrajectory sample_traj({sample_positions}, {"sample_locations"}, + "sample_locations", "sample_locations", false); + + output_all_sample_locations->saved_traj = sample_traj.GenerateLcmObject(); + output_all_sample_locations->utime = context.get_time() * 1e6; +} + +void SamplingC3Controller::OutputAllSampleCosts( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output_all_sample_costs) const { + Eigen::MatrixXd cost_datapoints = + Eigen::MatrixXd::Zero(1, all_sample_costs_.size()); + Eigen::VectorXd timestamps = Eigen::VectorXd::Zero(all_sample_costs_.size()); + + for (int i = 0; i < all_sample_costs_.size(); i++) { + cost_datapoints(0, i) = all_sample_costs_[i]; + timestamps(i) = i * dt_; // dummy timestamp for the sample costs + } + + LcmTrajectory::Trajectory sample_costs_traj; + sample_costs_traj.traj_name = "sample_costs"; + sample_costs_traj.datatypes = std::vector(1, "double"); + sample_costs_traj.datapoints = cost_datapoints; + sample_costs_traj.time_vector = timestamps.cast(); + LcmTrajectory cost_traj({sample_costs_traj}, {"sample_costs"}, "sample_costs", + "sample_costs", false); + + output_all_sample_costs->saved_traj = cost_traj.GenerateLcmObject(); + output_all_sample_costs->utime = context.get_time() * 1e6; + + if (verbose_) { + std::cout << "All sample costs as per output port: " << std::endl; + for (int i = 0; i < all_sample_costs_.size(); i++) { + std::cout << all_sample_costs_[i] << std::endl; + } + } +} + +void SamplingC3Controller::OutputDebug( + const drake::systems::Context& context, + dairlib::lcmt_sampling_c3_debug* debug_msg) const { + debug_msg->utime = context.get_time() * 1e6; + debug_msg->is_c3_mode = is_doing_c3_; + + // Redundant radio things included in debug message for convenience. + const auto& radio_out = + this->EvalInputValue(context, radio_port_); + debug_msg->is_teleop = radio_out->channel[14]; // 14 = teleop + debug_msg->is_force_tracking = !radio_out->channel[11]; // 11 = force tracking + // disabled + debug_msg->is_forced_into_c3 = radio_out->channel[12]; // 12 = forced into C3 + debug_msg->in_pose_tracking_mode = crossed_cost_switching_threshold_; + debug_msg->mode_switch_reason = mode_switch_reason_; + debug_msg->source_of_pursued_target = pursued_target_source_; + debug_msg->detected_goal_changes = detected_goal_changes_; + debug_msg->best_progress_steps_ago = best_progress_steps_ago_; + debug_msg->lowest_cost = lowest_cost_; + debug_msg->lowest_pos_and_rot_current_cost = lowest_pos_and_rot_current_cost_; + debug_msg->lowest_position_error = lowest_position_error_; + debug_msg->lowest_orientation_error = lowest_orientation_error_; + debug_msg->current_pos_error = current_position_error_; + debug_msg->current_rot_error = current_orientation_error_; +} + +void SamplingC3Controller::OutputSampleBufferConfigurations( + const drake::systems::Context& context, + Eigen::MatrixXd* sample_buffer_configurations) const { + *sample_buffer_configurations = sample_buffer_; +} + +void SamplingC3Controller::OutputSampleBufferCosts( + const drake::systems::Context& context, + Eigen::VectorXd* sample_buffer_costs) const { + *sample_buffer_costs = sample_costs_buffer_; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/sampling_based_c3_controller.h b/systems/controllers/sampling_based_c3_controller.h new file mode 100644 index 0000000000..ce470ae077 --- /dev/null +++ b/systems/controllers/sampling_based_c3_controller.h @@ -0,0 +1,462 @@ +#pragma once + +#include +#include +#include + +#include + +#include "common/find_resource.h" +#include "common/update_context.h" +#include "dairlib/lcmt_sampling_c3_debug.hpp" +#include "dairlib/lcmt_saved_traj.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "examples/sampling_c3/parameter_headers/sampling_c3_controller_params.h" +#include "examples/sampling_c3/parameter_headers/sampling_c3_options.h" +#include "examples/sampling_c3/parameter_headers/sampling_params.h" +#include "examples/sampling_c3/parameter_headers/reposition_params.h" +#include "examples/sampling_c3/parameter_headers/progress_params.h" +#include "lcm/lcm_trajectory.h" +#include "solvers/c3.h" +#include "solvers/c3_options.h" +#include "solvers/c3_output.h" +#include "solvers/lcs.h" +#include "solvers/lcs_factory.h" +#include "solvers/solver_options_io.h" +#include "systems/framework/timestamped_vector.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +using drake::AutoDiffVecXd; +using drake::AutoDiffXd; +using drake::MatrixX; +using drake::SortedPair; +using drake::VectorX; +using drake::geometry::GeometryId; +using drake::math::ExtractGradient; +using drake::math::ExtractValue; +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using systems::TimestampedVector; + +namespace systems { + + +enum SampleIndex { + kCurrentLocation, + kCurrentReposTarget // Only represents current reposition target when in + // reposition mode. + // Could expand this enum if want to reference more samples. +}; + +enum ModeSwitchReason { + kNoSwitch, + kToC3Cost, + kToC3ReachedReposTarget, + kToReposCost, + kToReposUnproductive, + kToC3Xbox +}; + +enum PursuedTargetSource { + kNoTarget, + kPrevious, + kNewSample, + kFromBuffer +}; + +class SamplingC3Controller : public drake::systems::LeafSystem { + public: + explicit SamplingC3Controller( + drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + drake::multibody::MultibodyPlant& plant_ad, + drake::systems::Context* context_ad, + const std::vector< + std::vector>>& + contact_geoms, + SamplingC3ControllerParams controller_params, bool verbose = false); + + // Input ports + const drake::systems::InputPort& + get_input_port_target() const { + return this->get_input_port(target_input_port_); + } + const drake::systems::InputPort& + get_input_port_final_target() const { + return this->get_input_port(final_target_input_port_); + } + const drake::systems::InputPort& + get_input_port_radio() const { + return this->get_input_port(radio_port_); + } + const drake::systems::InputPort& + get_input_port_lcs_state() const { + return this->get_input_port(lcs_state_input_port_); + } + + // Output ports + const drake::systems::OutputPort& + get_output_port_c3_solution_curr_plan() const { + return this->get_output_port(c3_solution_curr_plan_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_solution_curr_plan_actor() const { + return this->get_output_port(c3_solution_curr_plan_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_solution_curr_plan_object() const { + return this->get_output_port(c3_solution_curr_plan_object_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_intermediates_curr_plan() const { + return this->get_output_port(c3_intermediates_curr_plan_port_); + } + const drake::systems::OutputPort& + get_output_port_lcs_contact_jacobian_curr_plan() const { + return this->get_output_port(lcs_contact_jacobian_curr_plan_port_); + } + const drake::systems::OutputPort& + get_output_port_dynamically_feasible_curr_plan_actor() const { + return this->get_output_port(dynamically_feasible_curr_plan_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_dynamically_feasible_curr_plan_object() const { + return this->get_output_port(dynamically_feasible_curr_plan_object_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_solution_best_plan() const { + return this->get_output_port(c3_solution_best_plan_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_solution_best_plan_actor() const { + return this->get_output_port(c3_solution_best_plan_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_solution_best_plan_object() const { + return this->get_output_port(c3_solution_best_plan_object_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_intermediates_best_plan() const { + return this->get_output_port(c3_intermediates_best_plan_port_); + } + const drake::systems::OutputPort& + get_output_port_lcs_contact_jacobian_best_plan() const { + return this->get_output_port(lcs_contact_jacobian_best_plan_port_); + } + const drake::systems::OutputPort& + get_output_port_dynamically_feasible_best_plan_actor() const { + return this->get_output_port(dynamically_feasible_best_plan_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_dynamically_feasible_best_plan_object() const { + return this->get_output_port(dynamically_feasible_best_plan_object_port_); + } + const drake::systems::OutputPort& + get_output_port_c3_traj_execute_actor() const { + return this->get_output_port(c3_traj_execute_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_repos_traj_execute_actor() const { + return this->get_output_port(repos_traj_execute_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_traj_execute_actor() const { + return this->get_output_port(traj_execute_actor_port_); + } + const drake::systems::OutputPort& + get_output_port_is_c3_mode() const { + return this->get_output_port(is_c3_mode_port_); + } + const drake::systems::OutputPort& + get_output_port_all_sample_locations() const { + return this->get_output_port(all_sample_locations_port_); + } + const drake::systems::OutputPort& + get_output_port_all_sample_costs() const { + return this->get_output_port(all_sample_costs_port_); + } + const drake::systems::OutputPort& + get_output_port_debug() const { + return this->get_output_port(debug_lcmt_port_); + } + const drake::systems::OutputPort& + get_output_port_sample_buffer_configurations() const { + return this->get_output_port(sample_buffer_configurations_port_); + } + const drake::systems::OutputPort& + get_output_port_sample_buffer_costs() const { + return this->get_output_port(sample_buffer_costs_port_); + } + + private: + /// Function for computing one control loop + drake::systems::EventStatus ComputePlan( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + /// Helper functions + solvers::LCS CreatePlaceholderLCS() const; + + void ResolvePredictedEEState( + const bool& is_teleop, drake::VectorX& x_lcs_curr) const; + + void ClampEndEffectorAcceleration(drake::VectorX& x_lcs_curr) const; + + void CheckForWorkspaceLimitViolations( + const TimestampedVector* lcs_x_curr) const; + + void UpdateCostMatrices( + const drake::VectorX& x_lcs_curr, + const BasicVector& x_lcs_des, const C3Options& c3_options) const; + + std::pair, std::vector> + CreateLCSObjectsForSamples( + const std::vector& candidate_states, + const drake::VectorX& x_lcs_curr, const C3Options& c3_options, + const C3Options& c3_options_curr_location) const; + + void UpdateC3ExecutionTrajectory(const Eigen::VectorXd& x_lcs, + const double& t_context) const; + + void UpdateRepositioningExecutionTrajectory(const Eigen::VectorXd& x_lcs, + const double& t_context) const; + + void MaintainSampleBuffer(const Eigen::VectorXd& x_lcs) const; + + void AugmentSamplesWithBuffer( + std::vector>& c3_objects) const; + + void KeepTrackOfC3ModeProgress( + const drake::VectorX& x_lcs_curr, + const BasicVector& x_lcs_final_des, + bool& reset_progress_cost_buffer, + const bool& print_current_pos_and_rot_cost) const; + + void ResetProgressMetrics() const; + + /// Output port functions + void OutputC3SolutionCurrPlan( + const drake::systems::Context& context, + C3Output::C3Solution* c3_solution) const; + void OutputC3SolutionCurrPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const; + void OutputC3SolutionCurrPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const; + void OutputC3IntermediatesCurrPlan( + const drake::systems::Context& context, + C3Output::C3Intermediates* c3_intermediates) const; + void OutputLCSContactJacobianCurrPlan( + const drake::systems::Context& context, + std::pair>* + lcs_contact_jacobian) const; + void OutputC3SolutionBestPlan( + const drake::systems::Context& context, + C3Output::C3Solution* c3_solution) const; + void OutputC3SolutionBestPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const; + void OutputC3SolutionBestPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* output) const; + void OutputC3IntermediatesBestPlan( + const drake::systems::Context& context, + C3Output::C3Intermediates* c3_intermediates) const; + void OutputLCSContactJacobianBestPlan( + const drake::systems::Context& context, + std::pair>* + lcs_contact_jacobian) const; + void OutputDynamicallyFeasibleCurrPlanActor( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* + dynamically_feasible_curr_plan_actor) const; + void OutputDynamicallyFeasibleCurrPlanObject( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* + dynamically_feasible_curr_plan_object) const; + void OutputDynamicallyFeasibleBestPlanActor( + const drake::systems::Context& context, + lcmt_timestamped_saved_traj* dynamically_feasible_best_plan_actor) const; + void OutputDynamicallyFeasibleBestPlanObject( + const drake::systems::Context& context, + lcmt_timestamped_saved_traj* dynamically_feasible_best_plan_object) const; + void OutputAllSampleLocations( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* all_sample_locations) const; + void OutputAllSampleCosts( + const drake::systems::Context& context, + lcmt_timestamped_saved_traj* output_all_sample_costs) const; + void OutputC3TrajExecuteActor( + const drake::systems::Context& context, + lcmt_timestamped_saved_traj* c3_execution_lcm_traj) const; + void OutputReposTrajExecuteActor( + const drake::systems::Context& context, + lcmt_timestamped_saved_traj* repos_execution_lcm_traj) const; + void OutputTrajExecuteActor( + const drake::systems::Context& context, + lcmt_timestamped_saved_traj* execution_lcm_traj) const; + void OutputIsC3Mode( + const drake::systems::Context& context, + dairlib::lcmt_timestamped_saved_traj* is_c3_mode) const; + void OutputDebug( + const drake::systems::Context& context, + dairlib::lcmt_sampling_c3_debug* debug_msg) const; + void OutputSampleBufferConfigurations( + const drake::systems::Context& context, + Eigen::MatrixXd* sample_buffer_configurations) const; + void OutputSampleBufferCosts( + const drake::systems::Context& context, + Eigen::VectorXd* sample_buffer_costs) const; + + drake::systems::InputPortIndex radio_port_; + drake::systems::InputPortIndex final_target_input_port_; + drake::systems::InputPortIndex target_input_port_; + drake::systems::InputPortIndex lcs_state_input_port_; + // Current sample output port indices + drake::systems::OutputPortIndex c3_solution_curr_plan_port_; + drake::systems::OutputPortIndex c3_solution_curr_plan_actor_port_; + drake::systems::OutputPortIndex c3_solution_curr_plan_object_port_; + drake::systems::OutputPortIndex c3_intermediates_curr_plan_port_; + drake::systems::OutputPortIndex lcs_contact_jacobian_curr_plan_port_; + // Best sample output port indices + drake::systems::OutputPortIndex c3_solution_best_plan_port_; + drake::systems::OutputPortIndex c3_solution_best_plan_actor_port_; + drake::systems::OutputPortIndex c3_solution_best_plan_object_port_; + drake::systems::OutputPortIndex c3_intermediates_best_plan_port_; + drake::systems::OutputPortIndex lcs_contact_jacobian_best_plan_port_; + // Execution trajectory output port indices + drake::systems::OutputPortIndex c3_traj_execute_actor_port_; + drake::systems::OutputPortIndex repos_traj_execute_actor_port_; + drake::systems::OutputPortIndex traj_execute_actor_port_; + drake::systems::OutputPortIndex is_c3_mode_port_; + // Dynamically feasible plan output port indices + drake::systems::OutputPortIndex dynamically_feasible_curr_plan_actor_port_; + drake::systems::OutputPortIndex dynamically_feasible_curr_plan_object_port_; + drake::systems::OutputPortIndex dynamically_feasible_best_plan_actor_port_; + drake::systems::OutputPortIndex dynamically_feasible_best_plan_object_port_; + // Sample related output port indices + drake::systems::OutputPortIndex all_sample_locations_port_; + drake::systems::OutputPortIndex all_sample_costs_port_; + drake::systems::OutputPortIndex debug_lcmt_port_; + drake::systems::OutputPortIndex sample_buffer_configurations_port_; + drake::systems::OutputPortIndex sample_buffer_costs_port_; + + // This plant_ has been made 'not const' so that the context can be updated. + drake::multibody::MultibodyPlant& plant_; + drake::systems::Context* context_; + drake::multibody::MultibodyPlant& plant_ad_; + drake::systems::Context* context_ad_; + const std::vector< + std::vector>>& + contact_pairs_; + solvers::ContactModel contact_model_; + + SamplingC3ControllerParams controller_params_; + SamplingC3Options sampling_c3_options_; + SamplingParams sampling_params_; + SamplingC3RepositionParams reposition_params_; + SamplingC3ProgressParams progress_params_; + drake::solvers::SolverOptions solver_options_ = + drake::yaml::LoadYamlFile( + "solvers/osqp_options_default.yaml") + .GetAsSolverOptions(drake::solvers::OsqpSolver::id()); + + const bool verbose_; + int n_q_; + int n_v_; + int n_x_; + int n_lambda_; + int n_u_; + int max_num_samples_; + int N_; + + double solve_time_filter_constant_; + drake::systems::DiscreteStateIndex plan_start_time_index_; + + /// TODO: There are many mutable class variables, which is not best practice + /// in the Drake systems framework. These could be converted to discrete + /// state variables. + mutable double dt_ = 0.1; + + mutable std::vector Q_; + mutable std::vector R_; + mutable std::vector G_; + mutable std::vector U_; + + // Keep track of current C3 execution's best seen cost. + mutable int best_progress_steps_ago_; + mutable double lowest_cost_; + mutable double lowest_pos_and_rot_current_cost_; + mutable double lowest_position_error_; + mutable double lowest_orientation_error_; + mutable double current_position_error_; + mutable double current_orientation_error_; + mutable std::queue object_config_cost_history_; + + mutable double filtered_solve_time_ = 0; + + // Predictions for the end effector location. + mutable Eigen::VectorXd x_pred_curr_plan_; + mutable Eigen::VectorXd x_from_last_control_loop_; + mutable Eigen::VectorXd x_pred_from_last_control_loop_; + + // C3 solution for current location. + mutable std::shared_ptr c3_curr_plan_; + // TODO: these are currently assigned values but go unused -- may be useful if + // implementing warm start. + mutable std::vector z_sol_curr_plan_; + mutable std::vector delta_curr_plan_; + mutable std::vector w_curr_plan_; + + // C3 solution for best sample location. + mutable std::shared_ptr c3_best_plan_; + // TODO: these are currently assigned values but go unused -- may be useful if + // implementing warm start. + mutable std::vector z_sol_best_plan_; + mutable std::vector delta_best_plan_; + mutable std::vector w_best_plan_; + + // C3 solution for best sample in buffer. + mutable std::shared_ptr c3_buffer_plan_; + mutable std::vector dynamically_feasible_buffer_plan_; + + // LCS trajectories for C3 or repositioning modes. + mutable LcmTrajectory c3_execution_lcm_traj_; + mutable LcmTrajectory repos_execution_lcm_traj_; + + // Samples and associated costs computed in current control loop. + mutable std::vector all_sample_locations_; + mutable std::vector> + all_sample_dynamically_feasible_plans_; + mutable Eigen::Vector3d prev_repositioning_target_ = Eigen::Vector3d::Zero(); + mutable std::vector all_sample_costs_; + + // To detect if the final goal has been updated. + mutable Eigen::VectorXd x_final_target_; + mutable int detected_goal_changes_ = -1; + + // Sample buffer-related variables. + mutable int num_in_buffer_ = 0; + mutable Eigen::MatrixXd sample_buffer_; // (N_sample_buffer x n_q) + mutable Eigen::VectorXd sample_costs_buffer_; + + // Miscellaneous sample related variables. + mutable bool is_doing_c3_ = true; + mutable bool finished_reposition_flag_ = false; + // Crossing a threshold as the object gets closer to the goal means the + // controller goes from caring only about object position to caring about full + // pose. + mutable bool crossed_cost_switching_threshold_ = false; + mutable int num_threads_to_use_; + + mutable SampleIndex best_sample_index_ = kCurrentLocation; + mutable ModeSwitchReason mode_switch_reason_ = kNoSwitch; + mutable PursuedTargetSource pursued_target_source_ = kNoTarget; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/controllers/swing_ft_traj_gen.h b/systems/controllers/swing_ft_traj_gen.h index 80bf1e7a45..3c4b9e4a9f 100644 --- a/systems/controllers/swing_ft_traj_gen.h +++ b/systems/controllers/swing_ft_traj_gen.h @@ -110,7 +110,7 @@ class SwingFootTrajGenerator : public drake::systems::LeafSystem { const drake::multibody::MultibodyPlant& plant_; drake::systems::Context* context_; - const drake::multibody::BodyFrame& world_; + const drake::multibody::RigidBodyFrame& world_; const drake::multibody::Body& pelvis_; std::vector left_right_support_fsm_states_; diff --git a/systems/filters/floating_base_velocity_filter.h b/systems/filters/floating_base_velocity_filter.h index ac5551d3d0..916e526776 100644 --- a/systems/filters/floating_base_velocity_filter.h +++ b/systems/filters/floating_base_velocity_filter.h @@ -3,7 +3,7 @@ #include "drake/multibody/plant/multibody_plant.h" -/// Specialization of OutputVectorFilter for floating base linear velocity +/// Specialization of Timestamped Low Pass Filter for floting base velocity /// estimates namespace dairlib::systems { @@ -11,7 +11,7 @@ class FloatingBaseVelocityFilter : public OutputVectorFilter { public: FloatingBaseVelocityFilter( const drake::multibody::MultibodyPlant& plant, - const std::vector& tau) + const std::vector tau) : OutputVectorFilter(plant, tau, std::vector{plant.num_positions() + 3, plant.num_positions() + 4, diff --git a/systems/filters/output_vector_filter.cc b/systems/filters/output_vector_filter.cc index 79189ccf4a..35ec3f4cd6 100644 --- a/systems/filters/output_vector_filter.cc +++ b/systems/filters/output_vector_filter.cc @@ -11,24 +11,22 @@ namespace dairlib::systems { OutputVectorFilter::OutputVectorFilter( const drake::multibody::MultibodyPlant& plant, - const std::vector& tau, std::optional> filter_idxs) + const std::vector tau, std::optional> filter_idxs) : n_y_filt_(tau.size()), tau_(tau) { int n_y = plant.num_positions() + plant.num_velocities() + plant.num_actuators() + 3; - // Check user input DRAKE_DEMAND(filter_idxs.has_value() || tau.size() == n_y); if (filter_idxs.has_value()) { DRAKE_DEMAND(filter_idxs->size() == tau.size()); } - // Set the indices to be filtered if provided - if (filter_idxs.has_value()) { - filter_idxs_ = filter_idxs.value(); - } else { + if (!filter_idxs.has_value()) { std::vector idxs(n_y); std::iota(idxs.begin(), idxs.end(), 0); filter_idxs_ = idxs; + } else { + filter_idxs_ = filter_idxs.value(); } OutputVector model_vector( @@ -37,10 +35,10 @@ OutputVectorFilter::OutputVectorFilter( this->DeclareVectorInputPort("x", model_vector); this->DeclareVectorOutputPort("y", model_vector, &OutputVectorFilter::CopyFilterValues); - this->DeclarePerStepDiscreteUpdateEvent( + this->DeclareForcedDiscreteUpdateEvent( &OutputVectorFilter::DiscreteVariableUpdate); - prev_val_idx_ = this->DeclareDiscreteState(VectorXd::Zero(n_y_filt_)); + prev_val_idx_ = this->DeclareDiscreteState(VectorXd::Zero(n_y)); prev_time_idx_ = this->DeclareDiscreteState(VectorXd::Zero(1)); } @@ -52,34 +50,32 @@ drake::systems::EventStatus OutputVectorFilter::DiscreteVariableUpdate( double dt = y_t->get_timestamp() - discrete_state->get_value(prev_time_idx_)[0]; - VectorXd y = VectorXd::Zero(n_y_filt_); + VectorXd y = y_t->get_data(); VectorXd y_prev = discrete_state->get_value(prev_val_idx_); for (int i = 0; i < n_y_filt_; i++) { double alpha = dt / (dt + tau_.at(i)); - y(i) = - alpha * y_t->GetState()(filter_idxs_.at(i)) + (1.0 - alpha) * y_prev(i); + y(filter_idxs_.at(i)) = alpha * y(filter_idxs_.at(i)) + + (1.0 - alpha) * y_prev(filter_idxs_.at(i)); } discrete_state->get_mutable_value(prev_time_idx_) << y_t->get_timestamp() * VectorXd::Ones(1); discrete_state->get_mutable_value(prev_val_idx_) << y; + return drake::systems::EventStatus::Succeeded(); } void OutputVectorFilter::CopyFilterValues( const drake::systems::Context& context, dairlib::systems::OutputVector* y) const { - // Copy over y from the input port auto y_curr = (OutputVector*)EvalVectorInput(context, 0); + y->SetDataVector(y_curr->get_data()); y->set_timestamp(y_curr->get_timestamp()); - - // Replace y[i] with filtered value for all i in filtered_idxs_ - // but don't filter the first value if (context.get_discrete_state(prev_time_idx_).get_value()[0] >= .001) { VectorXd y_filt = context.get_discrete_state(prev_val_idx_).get_value(); - for (int i = 0; i < n_y_filt_; i++) { - y->get_mutable_value()[filter_idxs_.at(i)] = y_filt(i); + for (auto& i : filter_idxs_) { + y->get_mutable_value()[i] = y_filt(i); } } } diff --git a/systems/filters/output_vector_filter.h b/systems/filters/output_vector_filter.h index 5b0be3bf18..69349d99dc 100644 --- a/systems/filters/output_vector_filter.h +++ b/systems/filters/output_vector_filter.h @@ -1,19 +1,19 @@ #pragma once -#include "systems/framework/output_vector.h" - -#include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/leaf_system.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "systems/framework/output_vector.h" -/// OutputVectorFilter implements a first order vector-valued linear +/// TimestampedLowpassFilter implements a first order vector-valued linear /// lowpass filter. Since the control loops we use generally do not have -/// a fixed sampling rate, we implement this filter using timestamps +/// a fixed sampling rate, we implement this filter using timestamped vectors /// to achieve the desired time constant / cutoff frequency -using drake::systems::Context; -using drake::systems::LeafSystem; -using Eigen::MatrixXd; + using Eigen::VectorXd; +using Eigen::MatrixXd; +using drake::systems::LeafSystem; +using drake::systems::Context; namespace dairlib::systems { @@ -26,13 +26,14 @@ class OutputVectorFilter : public LeafSystem { public: // For cutoff frequency w_c (in rad/s), tau = 1/w_c, // For cutoff frequency f in Hz, tau = 1/(2*pi*f) - OutputVectorFilter(const drake::multibody::MultibodyPlant& plant, - const std::vector& tau, - std::optional> filter_idxs); + OutputVectorFilter( + const drake::multibody::MultibodyPlant& plant, + const std::vector tau, + std::optional> filter_idxs); private: - void CopyFilterValues(const drake::systems::Context& context, - OutputVector* y) const; + void CopyFilterValues(const drake::systems::Context &context, + OutputVector *y) const; drake::systems::EventStatus DiscreteVariableUpdate( const drake::systems::Context& context, @@ -40,8 +41,8 @@ class OutputVectorFilter : public LeafSystem { std::vector filter_idxs_; const int n_y_filt_; - const std::vector& tau_; + const std::vector tau_; // time constant int prev_val_idx_; int prev_time_idx_; }; -} // namespace dairlib::systems +} diff --git a/systems/framework/BUILD.bazel b/systems/framework/BUILD.bazel index a762800015..2b9b27766a 100644 --- a/systems/framework/BUILD.bazel +++ b/systems/framework/BUILD.bazel @@ -9,11 +9,13 @@ cc_library( srcs = [ "impact_info_vector.cc", "output_vector.cc", + "state_vector.cc", "timestamped_vector.cc", ], hdrs = [ "impact_info_vector.h", "output_vector.h", + "state_vector.h", "timestamped_vector.h", ], deps = [ diff --git a/systems/framework/geared_motor.cc b/systems/framework/geared_motor.cc index 4ec0d7b731..ddba8e082e 100644 --- a/systems/framework/geared_motor.cc +++ b/systems/framework/geared_motor.cc @@ -8,6 +8,7 @@ namespace systems { using drake::multibody::JointActuator; using drake::multibody::MultibodyPlant; using drake::systems::kUseDefaultName; +using drake::systems::BasicVector; GearedMotor::GearedMotor(const MultibodyPlant& plant, const std::unordered_map& max_motor_speeds) @@ -61,4 +62,4 @@ void GearedMotor::CalcTorqueOutput( } } // namespace systems -} // namespace dairlib +} // namespace dairlib \ No newline at end of file diff --git a/systems/framework/geared_motor.h b/systems/framework/geared_motor.h index 5f765482c3..dc03255a7b 100644 --- a/systems/framework/geared_motor.h +++ b/systems/framework/geared_motor.h @@ -39,7 +39,7 @@ class GearedMotor final : public drake::systems::LeafSystem { protected: void CalcTorqueOutput(const drake::systems::Context& context, - systems::BasicVector* output) const; + drake::systems::BasicVector* output) const; private: bool is_abstract() const { return false; } diff --git a/systems/framework/lcm_driven_loop.h b/systems/framework/lcm_driven_loop.h index a691e104ff..7d42e49a9d 100644 --- a/systems/framework/lcm_driven_loop.h +++ b/systems/framework/lcm_driven_loop.h @@ -3,6 +3,7 @@ #include #include #include +#include #include "dairlib/lcmt_controller_switch.hpp" #include "dairlib/lcmt_robot_output.hpp" @@ -52,6 +53,71 @@ namespace systems { /// Note that we implement the class only in the header file because we don't /// know what MessageTypes are beforehand. +/** + * Subscribes to and stores a copy of the most recent message on a given + * channel, for some @p Message type. All copies of a given Subscriber share + * the same underlying data. This class does NOT provide any mutex behavior + * for multi-threaded locking; it should only be used in cases where the + * governing DrakeLcmInterface::HandleSubscriptions is called from the same + * thread that owns all copies of this object. + */ +template +class Subscriber final { + public: + // Intentionally copyable so that it can be returned and stored by-value. + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Subscriber); + + /** + * Subscribes to the (non-empty) @p channel on the given (non-null) + * @p lcm instance. The `lcm` pointer is only used during construction; it + * is not retained by this object. When a undecodable message is received, + * @p on_error handler is invoked; when `on_error` is not provided, an + * exception will be thrown instead. + */ + Subscriber(drake::lcm::DrakeLcmInterface* lcm, const std::string& channel, + std::function on_error = {}) { + subscription_ = drake::lcm::Subscribe( + lcm, channel, + [data = data_](const Message& message) { + data->message = message; + data->count++; + }, + std::move(on_error)); + if (subscription_) { + subscription_->set_unsubscribe_on_delete(true); + } + } + + /** + * Returns the most recently received message, or a value-initialized (zeros) + * message otherwise. + */ + const Message& message() const { return data_->message; } + Message& message() { return data_->message; } + + /** Returns the total number of received messages. */ + int64_t count() const { return data_->count; } + int64_t& count() { return data_->count; } + + /** Clears all data (sets the message and count to all zeros). */ + void clear() { + data_->message = {}; + data_->count = 0; + } + + struct Data { + Message message{}; + int64_t count{0}; + }; + // Share a single copy of our (mutable) message storage, for all Subscribers + // to view or modify *and* for our subscription closure to write into. This + // will not be destroyed until all Subscribers are gone AND the subscription + // closure has been destroyed. + std::shared_ptr data_{std::make_shared()}; + // Keep our subscription active as long as a copy of this Subscriber remains. + std::shared_ptr subscription_; +}; + // We set a default value for SwitchMessageType so that we can generalize this // to both single and multi inputs. template > diagram, + std::shared_ptr> diagram, const drake::systems::LeafSystem* lcm_parser, - const std::string& input_channel, bool is_forced_publish) - : LcmDrivenLoop(drake_lcm, std::move(diagram), lcm_parser, + const std::string& input_channel, bool is_forced_publish, + int queue_capacity = 1) + : LcmDrivenLoop(drake_lcm, diagram, lcm_parser, std::vector(1, input_channel), input_channel, - "", is_forced_publish){}; + "", is_forced_publish, queue_capacity){}; /// Constructor for multi-input LcmDrivenLoop /// @param drake_lcm DrakeLcm @@ -85,11 +152,12 @@ class LcmDrivenLoop { /// @param switch_channel The name of the switch channel /// @param is_forced_publish A flag which enables publishing via diagram. LcmDrivenLoop(drake::lcm::DrakeLcm* drake_lcm, - std::unique_ptr> diagram, + std::shared_ptr> diagram, const drake::systems::LeafSystem* lcm_parser, std::vector input_channels, const std::string& active_channel, const std::string& switch_channel, bool is_forced_publish, + int queue_capacity = 1, const std::string& backup_drive_channel = "") : drake_lcm_(drake_lcm), lcm_parser_(lcm_parser), @@ -100,14 +168,14 @@ class LcmDrivenLoop { } diagram_ptr_ = diagram.get(); simulator_ = - std::make_unique>(std::move(diagram)); + std::make_unique>(*diagram); simulator_->set_publish_at_initialization(false); // Create subscriber for the switch (in the case of multi-input) DRAKE_DEMAND(!input_channels.empty()); if (input_channels.size() > 1) { DRAKE_DEMAND(!switch_channel.empty()); - switch_sub_ = std::make_unique>( + switch_sub_ = std::make_unique>( drake_lcm_, switch_channel); } @@ -115,7 +183,9 @@ class LcmDrivenLoop { for (const auto& name : input_channels) { std::cout << "Constructing subscriber for " << name << std::endl; name_to_input_sub_map_.insert(std::make_pair( - name, drake::lcm::Subscriber(drake_lcm_, name))); + name, Subscriber(drake_lcm_, name))); + name_to_input_sub_map_.at(name).subscription_->set_queue_capacity( + queue_capacity); } // Make sure input_channels contains active_channel, and then set initial @@ -132,7 +202,7 @@ class LcmDrivenLoop { active_channel_ = active_channel; if (!backup_drive_channel.empty()) { - state_sub_ = std::make_unique>( + state_sub_ = std::make_unique>( drake_lcm_, backup_drive_channel); } }; @@ -144,9 +214,9 @@ class LcmDrivenLoop { /// @param is_forced_publish A flag which enables publishing via diagram. /// The use case is that the user only need the time from lcm message. LcmDrivenLoop(drake::lcm::DrakeLcm* drake_lcm, - std::unique_ptr> diagram, + std::shared_ptr> diagram, const std::string& input_channel, bool is_forced_publish) - : LcmDrivenLoop(drake_lcm, std::move(diagram), nullptr, + : LcmDrivenLoop(drake_lcm, diagram, nullptr, std::vector(1, input_channel), input_channel, "", is_forced_publish){}; @@ -225,6 +295,11 @@ class LcmDrivenLoop { is_new_state_message; }); + // Pump drake's LCM subscribers to empty their internal queues until all + // LCM buffers are up-to-date. + // Addresses https://github.com/RobotLocomotion/drake/issues/15234 + while (drake_lcm_->HandleSubscriptions(0) > 0); + // Update the diagram context when there is new input message if (is_new_input_message || too_long_between_input_messages_) { // Write the InputMessageType message into the context if lcm_parser is @@ -262,8 +337,8 @@ class LcmDrivenLoop { simulator_->AdvanceTo(time); diagram_ptr_->CalcForcedUnrestrictedUpdate( diagram_context, &diagram_context.get_mutable_state()); - diagram_ptr_->CalcForcedDiscreteVariableUpdate(diagram_context, - &diagram_context.get_mutable_discrete_state()); + diagram_ptr_->CalcForcedDiscreteVariableUpdate( + diagram_context, &diagram_context.get_mutable_discrete_state()); if (is_forced_publish_) { // Force-publish via the diagram diagram_ptr_->ForcedPublish(diagram_context); @@ -318,15 +393,13 @@ class LcmDrivenLoop { drake::lcm::DrakeLcm* drake_lcm_; drake::systems::Diagram* diagram_ptr_; const drake::systems::LeafSystem* lcm_parser_; - std::unique_ptr> simulator_; + std::shared_ptr> simulator_; std::string diagram_name_ = "diagram"; std::string active_channel_; - std::unique_ptr> switch_sub_ = - nullptr; - std::map> - name_to_input_sub_map_; - std::unique_ptr> state_sub_; + std::unique_ptr> switch_sub_ = nullptr; + std::map> name_to_input_sub_map_; + std::unique_ptr> state_sub_; bool is_forced_publish_; bool too_long_between_input_messages_ = false; diff --git a/systems/framework/state_vector.cc b/systems/framework/state_vector.cc new file mode 100644 index 0000000000..60ae8e4de5 --- /dev/null +++ b/systems/framework/state_vector.cc @@ -0,0 +1,6 @@ +#include "systems/framework/state_vector.h" + +#include "drake/common/default_scalars.h" + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::dairlib::systems::StateVector) diff --git a/systems/framework/state_vector.h b/systems/framework/state_vector.h new file mode 100644 index 0000000000..506933dd12 --- /dev/null +++ b/systems/framework/state_vector.h @@ -0,0 +1,131 @@ +#pragma once + +#include "systems/framework/timestamped_vector.h" +#include +#include + + +namespace dairlib { +namespace systems { + +using drake::VectorX; +using std::string; +using std::vector; + +/// StateVector stores the object state as a TimestampedVector +/// * positions +/// * velocities +/// Similar to OutputVector but only the state variables +template +class StateVector : public TimestampedVector { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(StateVector) + + StateVector() = default; + + /// Initializes with the given @p size using the drake::dummy_value, which + /// is NaN when T = double. + explicit StateVector(int num_positions, int num_velocities) + : TimestampedVector(num_positions + num_velocities), + num_positions_(num_positions), + num_velocities_(num_velocities), + position_start_(0){} + + /// Constructs a StateVector with the specified positions and velocities. + explicit StateVector(const VectorX& positions, + const VectorX& velocities) + : StateVector(positions.size(), velocities.size()) { + this->SetPositions(positions); + this->SetVelocities(velocities); + } + + void SetPositions(VectorX positions) { + this->get_mutable_data().segment(position_start_, + num_positions_) = positions; + } + + void SetVelocities(VectorX velocities) { + this->get_mutable_data().segment(position_start_ + num_positions_, + num_velocities_) = velocities; + } + + void SetPositionAtIndex(int index, T value) { + this->SetAtIndex(position_start_ + index, value); + } + + void SetVelocityAtIndex(int index, T value) { + this->SetAtIndex(position_start_ + num_positions_ + index, value); + } + + void SetState(VectorX state) { + this->get_mutable_data().segment(position_start_, + num_positions_ + num_velocities_) = state; + } + + /// Returns a const state vector + const VectorX GetState() const { + return this->get_data().segment(position_start_, + num_positions_ + num_velocities_); + } + + /// Returns a const positions vector + const VectorX GetPositions() const { + return this->get_data().segment(position_start_, num_positions_); + } + + /// Returns a const velocities vector + const VectorX GetVelocities() const { + return this->get_data().segment(position_start_ + num_positions_, + num_velocities_); + } + + /// Returns a mutable state vector + Eigen::Map> GetMutableState() { + auto data = this->get_mutable_data().segment(position_start_, + num_positions_ + num_velocities_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable positions vector + Eigen::Map> GetMutablePositions() { + auto data = this->get_mutable_data().segment(position_start_, num_positions_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable velocities vector + Eigen::Map> GetMutableVelocities() { + auto data = this->get_mutable_data().segment( + position_start_ + num_positions_, num_velocities_); + return Eigen::Map>(&data(0), data.size()); + } + + T GetPositionAtIndex(int index) const { + return this->GetAtIndex(position_start_ + index); + } + + T GetVelocityAtIndex(int index) const { + return this->GetAtIndex(position_start_ + num_positions_ + index); + } + + void SetName(int index, string name) { + position_names_[index] = name; + } + + string GetName(int index) { + return position_names_[index]; + } + + protected: + virtual StateVector* DoClone() const { + return new StateVector(num_positions_, num_velocities_); + } + + private: + const int num_positions_; + const int num_velocities_; + const int position_start_; + vector position_names_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/framework/timestamped_vector.h b/systems/framework/timestamped_vector.h index ef93cec31b..471a9dde22 100644 --- a/systems/framework/timestamped_vector.h +++ b/systems/framework/timestamped_vector.h @@ -5,31 +5,29 @@ namespace dairlib { namespace systems { -using drake::systems::BasicVector; -using drake::VectorX; - -/// TimestampedVector wraps a BasicVector along with a timestamp field -/// The primary purpose of this is to pass-through a message (e.g. LCM) timestamp -/// Uses a length N+1 BasicVector to store a vector of length N and a timestamp -/// The timestamp is stored as the final element (Nth) +/// TimestampedVector wraps a drake::systems::BasicVector along with a timestamp field +/// The primary purpose of this is to pass-through a message (e.g. LCM) +/// timestamp Uses a length N+1 drake::systems::BasicVector to store a vector of length N and a +/// timestamp The timestamp is stored as the final element (Nth) template -class TimestampedVector : public drake::systems::BasicVector { -public: +class TimestampedVector : public drake::systems::BasicVector { + public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(TimestampedVector) /// Constructs an empty TimestampedVector. TimestampedVector() = default; - /// Initializes with the given @p size using the drake::dummy_value, which /// is NaN when T = double. explicit TimestampedVector(int data_size) - : BasicVector(data_size + 1), timestep_index_(data_size) {} + : drake::systems::BasicVector(data_size + 1), + timestep_index_(data_size), + data_size_(data_size) {} /// Constructs a TimestampedVector with the specified @p data. - explicit TimestampedVector(const VectorX& data) + explicit TimestampedVector(const drake::VectorX& data) : TimestampedVector(data.size()) { - VectorX data_timestamped = VectorX(data.size() + 1); + drake::VectorX data_timestamped = drake::VectorX(data.size() + 1); data_timestamped.head(data.size()) = data; data_timestamped(data.size()) = 0; this->SetFromVector(data_timestamped); @@ -48,10 +46,10 @@ class TimestampedVector : public drake::systems::BasicVector { this->SetAtIndex(timestep_index_, timestamp); } - T get_timestamp() const {return this->GetAtIndex(timestep_index_);} + T get_timestamp() const { return this->GetAtIndex(timestep_index_); } - /// Copies the entire vector to a new TimestampedVector, with the same concrete - /// implementation type. + /// Copies the entire vector to a new TimestampedVector, with the same + /// concrete implementation type. /// /// Uses the Non-Virtual Interface idiom because smart pointers do not have /// type covariance. @@ -62,42 +60,44 @@ class TimestampedVector : public drake::systems::BasicVector { } /// Returns the vector without the timestamp - VectorX CopyVectorNoTimestamp() const { + drake::VectorX CopyVectorNoTimestamp() const { return this->CopyToVector().head(timestep_index_); } /// Returns a mutable vector of the data values (without timestamp) - Eigen::Map> get_mutable_data() { + Eigen::Map> get_mutable_data() { auto data = this->get_mutable_value().head(timestep_index_); - return Eigen::Map>(&data(0), data.size()); + return Eigen::Map>(&data(0), data.size()); } /// Returns the entire vector as a const Eigen::VectorBlock. - const VectorX get_data() const { + const drake::VectorX get_data() const { return this->get_value().head(timestep_index_); } - - //sets the data part of the vector (without timestamp) - void SetDataVector(const Eigen::Ref>& value) { + // sets the data part of the vector (without timestamp) + void SetDataVector(const Eigen::Ref>& value) { this->get_mutable_data() = value; } + int data_size() const { return data_size_; } + protected: /// Returns a new TimestampedVector containing a copy of the entire vector. /// Caller must take ownership, and may rely on the NVI wrapper to initialize /// the clone elementwise. /// - /// Subclasses of TimestampedVector must override DoClone to return their covariant - /// type. + /// Subclasses of TimestampedVector must override DoClone to return their + /// covariant type. /// virtual TimestampedVector* DoClone() const { return new TimestampedVector(timestep_index_); } - private: - const int timestep_index_; - }; + private: + const int timestep_index_; + const int data_size_; +}; } // namespace systems } // namespace dairlib diff --git a/systems/franka_kinematics.cc b/systems/franka_kinematics.cc new file mode 100644 index 0000000000..f5070a6d9f --- /dev/null +++ b/systems/franka_kinematics.cc @@ -0,0 +1,118 @@ +#include "systems/franka_kinematics.h" + +#include "common/find_resource.h" +#include "multibody/multibody_utils.h" + +namespace dairlib { + +using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; +using drake::systems::Context; +using Eigen::VectorXd; +using systems::OutputVector; +using systems::StateVector; +using systems::TimestampedVector; + +namespace systems { + +FrankaKinematics::FrankaKinematics(const MultibodyPlant& franka_plant, + Context* franka_context, + const MultibodyPlant& object_plant, + Context* object_context, + const std::string& end_effector_name, + const std::string& object_name, + bool include_end_effector_orientation) + : franka_plant_(franka_plant), + franka_context_(franka_context), + object_plant_(object_plant), + object_context_(object_context), + world_(franka_plant_.world_frame()), + end_effector_name_(end_effector_name), + object_name_(object_name), + include_end_effector_orientation_(include_end_effector_orientation) { + this->set_name("franka_kinematics"); + franka_state_port_ = + this->DeclareVectorInputPort( + "x_franka", OutputVector(franka_plant.num_positions(), + franka_plant.num_velocities(), + franka_plant.num_actuators())) + .get_index(); + + object_state_port_ = + this->DeclareVectorInputPort( + "x_object", StateVector(object_plant.num_positions(), + object_plant.num_velocities())) + .get_index(); + num_end_effector_positions_ = 3 + include_end_effector_orientation_ * 3; + num_object_positions_ = 7; + num_end_effector_velocities_ = 3 + include_end_effector_orientation_ * 3; + num_object_velocities_ = 6; + lcs_state_port_ = + this->DeclareVectorOutputPort( + "x_lcs", + FrankaKinematicsVector( + num_end_effector_positions_, num_object_positions_, + num_end_effector_velocities_, num_object_velocities_), + &FrankaKinematics::ComputeLCSState) + .get_index(); +} + +void FrankaKinematics::ComputeLCSState( + const drake::systems::Context& context, + FrankaKinematicsVector* lcs_state) const { + const OutputVector* franka_output = + (OutputVector*)this->EvalVectorInput(context, franka_state_port_); + const StateVector* object_output = + (StateVector*)this->EvalVectorInput(context, object_state_port_); + + VectorXd q_franka = franka_output->GetPositions(); + VectorXd v_franka = franka_output->GetVelocities(); + VectorXd q_object = object_output->GetPositions(); + VectorXd v_object = object_output->GetVelocities(); + multibody::SetPositionsIfNew(franka_plant_, q_franka, + franka_context_); + multibody::SetVelocitiesIfNew(franka_plant_, v_franka, + franka_context_); + multibody::SetPositionsIfNew(object_plant_, q_object, + object_context_); + multibody::SetVelocitiesIfNew(object_plant_, v_object, + object_context_); + + auto end_effector_pose = franka_plant_.EvalBodyPoseInWorld( + *franka_context_, franka_plant_.GetBodyByName(end_effector_name_)); + auto object_pose = object_plant_.EvalBodyPoseInWorld( + *object_context_, object_plant_.GetBodyByName(object_name_)); + auto end_effector_spatial_velocity = + franka_plant_.EvalBodySpatialVelocityInWorld( + *franka_context_, franka_plant_.GetBodyByName(end_effector_name_)); + auto end_effector_rotation_rpy = + end_effector_pose.rotation().ToRollPitchYaw().vector(); + VectorXd end_effector_positions = VectorXd::Zero(num_end_effector_positions_); + VectorXd end_effector_velocities = + VectorXd::Zero(num_end_effector_velocities_); + + if (num_end_effector_positions_ > 3) { + end_effector_positions << end_effector_pose.translation(), + end_effector_rotation_rpy; + } else { + end_effector_positions << end_effector_pose.translation(); + } + if (num_end_effector_velocities_ > 3) { + end_effector_velocities << end_effector_spatial_velocity.rotational(), + end_effector_spatial_velocity.translational(); + } else { + end_effector_velocities << end_effector_spatial_velocity.translational(); + } + + VectorXd object_position = q_object; + object_position << q_object.head(4), object_pose.translation(); + + lcs_state->SetEndEffectorPositions(end_effector_positions); + lcs_state->SetObjectPositions(object_position); + lcs_state->SetEndEffectorVelocities(end_effector_velocities); + lcs_state->SetObjectVelocities(v_object); + lcs_state->set_timestamp(franka_output->get_timestamp()); +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/franka_kinematics.h b/systems/franka_kinematics.h new file mode 100644 index 0000000000..b3a916d10d --- /dev/null +++ b/systems/franka_kinematics.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include "systems/franka_kinematics_vector.h" + +#include "drake/systems/framework/leaf_system.h" +#include "systems/framework/timestamped_vector.h" +#include "systems/framework/output_vector.h" +#include "systems/framework/state_vector.h" + +namespace dairlib { +namespace systems { + +/// Outputs a lcmt_timestamped_saved_traj +class FrankaKinematics : public drake::systems::LeafSystem { + public: + explicit FrankaKinematics(const drake::multibody::MultibodyPlant& franka_plant, + drake::systems::Context* franka_context, + const drake::multibody::MultibodyPlant& object_plant, + drake::systems::Context* object_context, + const std::string& end_effector_name, + const std::string& object_name, + bool include_end_effector_orientation); + + const drake::systems::InputPort& get_input_port_object_state() const { + return this->get_input_port(object_state_port_); + } + + const drake::systems::InputPort& get_input_port_franka_state() const { + return this->get_input_port(franka_state_port_); + } + + const drake::systems::OutputPort& get_output_port_lcs_state() const { + return this->get_output_port(lcs_state_port_); + } + + private: + void ComputeLCSState( + const drake::systems::Context& context, + FrankaKinematicsVector* output_traj) const; + + drake::systems::InputPortIndex franka_state_port_; + drake::systems::InputPortIndex object_state_port_; + drake::systems::OutputPortIndex lcs_state_port_; + + int num_end_effector_positions_; + int num_object_positions_; + int num_end_effector_velocities_; + int num_object_velocities_; + + const drake::multibody::MultibodyPlant& franka_plant_; + drake::systems::Context* franka_context_; + const drake::multibody::MultibodyPlant& object_plant_; + drake::systems::Context* object_context_; + const drake::multibody::Frame& world_; + std::string end_effector_name_; + std::string object_name_; + const bool include_end_effector_orientation_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/franka_kinematics_vector.cc b/systems/franka_kinematics_vector.cc new file mode 100644 index 0000000000..2b7318e9f0 --- /dev/null +++ b/systems/franka_kinematics_vector.cc @@ -0,0 +1,6 @@ +#include "systems/franka_kinematics_vector.h" + +#include "drake/common/default_scalars.h" + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::dairlib::systems::FrankaKinematicsVector) diff --git a/systems/franka_kinematics_vector.h b/systems/franka_kinematics_vector.h new file mode 100644 index 0000000000..a6ae261b56 --- /dev/null +++ b/systems/franka_kinematics_vector.h @@ -0,0 +1,177 @@ +#pragma once + +#include +#include + +#include "systems/framework/timestamped_vector.h" + +namespace dairlib { +namespace systems { + +/// FrankaKinematicsVector stores the robot output as a TimestampedVector +/// * positions +/// * velocities +template +class FrankaKinematicsVector : public TimestampedVector { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FrankaKinematicsVector) + + FrankaKinematicsVector() = default; + + explicit FrankaKinematicsVector(int num_end_effector_positions, + int num_object_positions, + int num_end_effector_velocities, + int num_object_velocities) + : TimestampedVector(num_end_effector_positions + num_object_positions + + num_end_effector_velocities + + num_object_velocities), + num_end_effector_positions_(num_end_effector_positions), + num_object_positions_(num_object_positions), + num_end_effector_velocities_(num_end_effector_velocities), + num_object_velocities_(num_object_velocities), + end_effector_positions_start_(0), + object_positions_start_(num_end_effector_positions_), + end_effector_velocities_start_(num_end_effector_positions_ + + num_object_positions_), + object_velocities_start_(num_end_effector_positions_ + + num_object_positions_ + + num_end_effector_velocities_), + num_positions_(num_end_effector_positions_ + num_object_positions_), + num_velocities_(num_end_effector_velocities_ + num_object_velocities_) { + } + + /// Constructs a OutputVector with the specified positions and velocities. + explicit FrankaKinematicsVector( + const drake::VectorX& end_effector_positions, + const drake::VectorX& object_positions, + const drake::VectorX& end_effector_velocities, + const drake::VectorX& object_velocities) + : FrankaKinematicsVector( + end_effector_positions.size(), object_positions.size(), + end_effector_velocities.size(), object_velocities.size()) { + this->SetEndEffectorPositions(end_effector_positions); + this->SetObjectPositions(object_positions); + this->SetEndEffectorVelocities(end_effector_velocities); + this->SetObjectVelocities(object_velocities); + } + + void SetEndEffectorPositions(drake::VectorX positions) { + DRAKE_DEMAND(positions.size() == num_end_effector_positions_); + this->get_mutable_data().segment(end_effector_positions_start_, + num_end_effector_positions_) = positions; + } + + void SetObjectPositions(drake::VectorX positions) { + DRAKE_DEMAND(positions.size() == num_object_positions_); + this->get_mutable_data().segment(object_positions_start_, + num_object_positions_) = positions; + } + + void SetEndEffectorVelocities(drake::VectorX velocities) { + DRAKE_DEMAND(velocities.size() == num_end_effector_velocities_); + this->get_mutable_data().segment(end_effector_velocities_start_, + num_end_effector_velocities_) = velocities; + } + + void SetObjectVelocities(drake::VectorX velocities) { + DRAKE_DEMAND(velocities.size() == num_object_velocities_); + this->get_mutable_data().segment(object_velocities_start_, + num_object_velocities_) = velocities; + } + + void SetState(drake::VectorX state) { + DRAKE_DEMAND(state.size() == this->data_size()); + this->get_mutable_data().segment(end_effector_positions_start_, + this->data_size()) = state; + } + + /// Returns a const state vector + const drake::VectorX GetState() const { + return this->get_data().segment(end_effector_positions_start_, + this->data_size()); + } + + /// Returns a const positions vector for the end effector + const drake::VectorX GetEndEffectorPositions() const { + return this->get_data().segment(end_effector_positions_start_, + num_end_effector_positions_); + } + + /// Returns a const positions vector for the object + const drake::VectorX GetObjectPositions() const { + return this->get_data().segment(object_positions_start_, + num_object_positions_); + } + + /// Returns a const positions vector for the end effector + const drake::VectorX GetEndEffectorVelocities() const { + return this->get_data().segment(end_effector_velocities_start_, + num_end_effector_velocities_); + } + + /// Returns a const positions vector for the object + const drake::VectorX GetObjectVelocities() const { + return this->get_data().segment(object_velocities_start_, + num_object_velocities_); + } + + /// Returns a const velocities vector + const drake::VectorX GetVelocities() const { + return this->get_data().segment( + end_effector_velocities_start_, + num_end_effector_velocities_ + num_object_velocities_); + } + + /// Returns a const positions vector + const drake::VectorX GetPositions() const { + return this->get_data().segment( + end_effector_positions_start_, + num_end_effector_positions_ + num_object_positions_); + } + + /// Returns a mutable positions vector + Eigen::Map> GetMutablePositions() { + auto data = this->get_mutable_data().segment( + end_effector_positions_start_, + num_end_effector_positions_ + num_object_positions_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable velocities vector + Eigen::Map> GetMutableVelocities() { + auto data = this->get_mutable_data().segment( + end_effector_velocities_start_, + num_end_effector_velocities_ + num_object_velocities_); + return Eigen::Map>(&data(0), data.size()); + } + + /// Returns a mutable state vector + Eigen::Map> GetMutableState() { + auto data = this->get_mutable_data().segment(end_effector_positions_start_, + this->data_size()); + return Eigen::Map>(&data(0), data.size()); + } + + protected: + virtual FrankaKinematicsVector* DoClone() const { + return new FrankaKinematicsVector( + num_end_effector_positions_, num_object_positions_, + num_end_effector_velocities_, num_object_velocities_); + } + + private: + const int num_end_effector_positions_; + const int num_object_positions_; + const int num_end_effector_velocities_; + const int num_object_velocities_; + const int end_effector_positions_start_; + const int object_positions_start_; + const int end_effector_velocities_start_; + const int object_velocities_start_; + + const int num_positions_; + const int num_velocities_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/franka_state_translator.cc b/systems/franka_state_translator.cc new file mode 100644 index 0000000000..cd6af41d77 --- /dev/null +++ b/systems/franka_state_translator.cc @@ -0,0 +1,82 @@ +#include "franka_state_translator.h" + +#include + +#include "lcmtypes/drake/lcmt_panda_status.hpp" +#include "systems/framework/output_vector.h" + +namespace dairlib { + +using drake::systems::BasicVector; +using drake::systems::Context; +using systems::TimestampedVector; + +namespace systems { + +FrankaStateOutTranslator::FrankaStateOutTranslator( + std::vector joint_position_names, + std::vector joint_velocity_names, + std::vector joint_actuator_names) { + this->set_name("franka_state_translator"); + + panda_status_ = + this->DeclareAbstractInputPort("franka_state", + drake::Value()) + .get_index(); + DRAKE_DEMAND(joint_position_names.size() == kNumFrankaJoints); + DRAKE_DEMAND(joint_velocity_names.size() == kNumFrankaJoints); + DRAKE_DEMAND(joint_actuator_names.size() == kNumFrankaJoints); + auto default_robot_output_msg = dairlib::lcmt_robot_output(); + default_robot_output_msg.position_names = joint_position_names; + default_robot_output_msg.velocity_names = joint_velocity_names; + default_robot_output_msg.effort_names = joint_actuator_names; + state_output_ = this->DeclareAbstractOutputPort( + "lcmt_robot_output", default_robot_output_msg, + &FrankaStateOutTranslator::OutputFrankaState) + .get_index(); +} + +void FrankaStateOutTranslator::OutputFrankaState( + const drake::systems::Context& context, + dairlib::lcmt_robot_output* output) const { + const auto& panda_status = + this->EvalInputValue(context, panda_status_); + output->utime = panda_status->utime; + output->position = panda_status->joint_position; + output->num_positions = panda_status->num_joints; + output->num_velocities = panda_status->num_joints; + output->num_efforts = panda_status->num_joints; +// output->position_names = std::vector(output->num_positions, "double"); + output->velocity = panda_status->joint_velocity; + output->effort = panda_status->joint_torque; +} + +FrankaEffortsInTranslator::FrankaEffortsInTranslator() { + this->set_name("franka_command_translator"); + + robot_input_ = + this->DeclareAbstractInputPort("franka_state", + drake::Value()) + .get_index(); + + franka_command_output_ = + this->DeclareAbstractOutputPort( + "lcmt_panda_command", + &FrankaEffortsInTranslator::OutputFrankaCommand) + .get_index(); +} + +void FrankaEffortsInTranslator::OutputFrankaCommand( + const drake::systems::Context& context, + drake::lcmt_panda_command* output) const { + const auto& robot_input = + this->EvalInputValue(context, robot_input_); + DRAKE_DEMAND(robot_input->efforts.size() == kNumFrankaJoints); + output->utime = robot_input->utime; + output->num_joint_torque = robot_input->efforts.size(); + output->joint_torque = robot_input->efforts; + output->control_mode_expected = drake::lcmt_panda_status::CONTROL_MODE_TORQUE; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/franka_state_translator.h b/systems/franka_state_translator.h new file mode 100644 index 0000000000..f1a3bc8c39 --- /dev/null +++ b/systems/franka_state_translator.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include + +#include "common/find_resource.h" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +const static int kNumFrankaJoints = 7; + +class FrankaStateOutTranslator : public drake::systems::LeafSystem { + public: + explicit FrankaStateOutTranslator(std::vector joint_position_names, + std::vector joint_velocity_names, + std::vector joint_actuator_names); + + const drake::systems::InputPort& get_input_port_panda_status() const { + return this->get_input_port(panda_status_); + } + const drake::systems::OutputPort& get_output_port_robot_state() + const { + return this->get_output_port(state_output_); + } + + private: + void OutputFrankaState(const drake::systems::Context& context, + dairlib::lcmt_robot_output* output) const; + + drake::systems::InputPortIndex panda_status_; + drake::systems::OutputPortIndex state_output_; +}; + +class FrankaEffortsInTranslator : public drake::systems::LeafSystem { + public: + explicit FrankaEffortsInTranslator(); + + const drake::systems::InputPort& get_input_port_efforts() const { + return this->get_input_port(robot_input_); + } + const drake::systems::OutputPort& get_output_port_panda_command() + const { + return this->get_output_port(franka_command_output_); + } + + private: + void OutputFrankaCommand(const drake::systems::Context& context, + drake::lcmt_panda_command* output) const; + + drake::systems::InputPortIndex robot_input_; + drake::systems::OutputPortIndex franka_command_output_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/primitives/BUILD.bazel b/systems/primitives/BUILD.bazel index e54463d314..6a1eb747b7 100644 --- a/systems/primitives/BUILD.bazel +++ b/systems/primitives/BUILD.bazel @@ -37,6 +37,21 @@ cc_library( ], ) +cc_library( + name = "radio_parser", + srcs = [ + "radio_parser.cc", + ], + hdrs = [ + "radio_parser.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "gaussian_noise_pass_through", srcs = ["gaussian_noise_pass_through.cc"], diff --git a/systems/primitives/radio_parser.cc b/systems/primitives/radio_parser.cc new file mode 100644 index 0000000000..9f26acc05e --- /dev/null +++ b/systems/primitives/radio_parser.cc @@ -0,0 +1,49 @@ +#include "systems/primitives/radio_parser.h" + +namespace dairlib { +namespace systems { + +using drake::systems::BasicVector; +RadioParser::RadioParser() { + data_input_port_ = + this->DeclareVectorInputPort("raw_radio", BasicVector(2 + 16)) + .get_index(); + this->DeclareAbstractOutputPort("radio_out", dairlib::lcmt_radio_out(), + &RadioParser::CalcRadioOutput); +} + +void RadioParser::CalcRadioOutput( + const drake::systems::Context& context, + dairlib::lcmt_radio_out* output) const { + const BasicVector& data = + *this->template EvalVectorInput(context, data_input_port_); + output->radioReceiverSignalGood = data[0]; + output->receiverMedullaSignalGood = data[1]; + for (int i = 0; i < 16; ++i) { + output->channel[i] = data[2 + i]; + } +} + +RadioToVector::RadioToVector() { + radio_port_ = this->DeclareAbstractInputPort( + "radio_in", drake::Value()) + .get_index(); + this->DeclareVectorOutputPort("raw_radio", BasicVector(2 + 16), + &RadioToVector::ConvertToVector); +} + +void RadioToVector::ConvertToVector( + const drake::systems::Context& context, + drake::systems::BasicVector* output) const { + const auto& radio_out = + this->EvalInputValue(context, radio_port_); + output->SetZero(); + for (int i = 0; i < 16; ++i) { + output->get_mutable_value()[i] = radio_out->channel[i]; + } + output->get_mutable_value()[16 + 0] = radio_out->radioReceiverSignalGood; + output->get_mutable_value()[16 + 1] = radio_out->receiverMedullaSignalGood; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/primitives/radio_parser.h b/systems/primitives/radio_parser.h new file mode 100644 index 0000000000..f6886d00aa --- /dev/null +++ b/systems/primitives/radio_parser.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +class RadioParser : public drake::systems::LeafSystem { + public: + RadioParser(); + + const drake::systems::InputPort& get_input_port() const { + return drake::systems::LeafSystem::get_input_port(0); + } + + const drake::systems::OutputPort& get_output_port() const { + return drake::systems::LeafSystem::get_output_port(0); + } + + protected: + void CalcRadioOutput(const drake::systems::Context& context, + dairlib::lcmt_radio_out* output) const; + + private: + bool is_abstract() const { return false; } + int data_input_port_; +}; + +class RadioToVector : public drake::systems::LeafSystem { + public: + RadioToVector(); + + const drake::systems::InputPort& get_input_port() const { + return drake::systems::LeafSystem::get_input_port(0); + } + + const drake::systems::OutputPort& get_output_port() const { + return drake::systems::LeafSystem::get_output_port(0); + } + + protected: + void ConvertToVector(const drake::systems::Context& context, + drake::systems::BasicVector* output) const; + + private: + bool is_abstract() const { return false; } + int radio_port_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/primitives/vector_aggregator.h b/systems/primitives/vector_aggregator.h index 279094e79d..78819e10d9 100644 --- a/systems/primitives/vector_aggregator.h +++ b/systems/primitives/vector_aggregator.h @@ -65,7 +65,7 @@ class VectorAggregator : public drake::systems::LeafSystem { private: void DoPublish(const drake::systems::Context& context, const std::vector*>& - events) const override { + events) const { const TimestampedVector* input = dynamic_cast*>( EvalVectorInput(context, 0)); diff --git a/systems/robot_lcm_systems.cc b/systems/robot_lcm_systems.cc index b812225a48..c6b252cedb 100644 --- a/systems/robot_lcm_systems.cc +++ b/systems/robot_lcm_systems.cc @@ -1,7 +1,7 @@ -#include - #include "robot_lcm_systems.h" +#include "dairlib/lcmt_robot_input.hpp" +#include "dairlib/lcmt_robot_output.hpp" #include "multibody/multibody_utils.h" #include "drake/multibody/plant/multibody_plant.h" @@ -9,19 +9,16 @@ #include "drake/systems/lcm/lcm_subscriber_system.h" #include "drake/systems/primitives/discrete_time_delay.h" -#include "dairlib/lcmt_robot_input.hpp" -#include "dairlib/lcmt_robot_output.hpp" - namespace dairlib { namespace systems { using drake::multibody::JointActuatorIndex; using drake::multibody::JointIndex; using drake::multibody::MultibodyPlant; +using drake::systems::BasicVector; using drake::systems::Context; using drake::systems::lcm::LcmPublisherSystem; using drake::systems::lcm::LcmSubscriberSystem; -using drake::systems::LeafSystem; using Eigen::VectorXd; using std::string; using systems::OutputVector; @@ -36,6 +33,11 @@ RobotOutputReceiver::RobotOutputReceiver( num_efforts_ = plant.num_actuators(); position_index_map_ = multibody::MakeNameToPositionsMap(plant); velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + model_instance_ = + drake::multibody::ModelInstanceIndex(DEFAULT_MODEL_INSTANCE_INDEX); + + positions_start_idx_ = 0; + velocities_start_idx_ = 0; effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); this->DeclareAbstractInputPort("lcmt_robot_output", drake::Value{}); @@ -46,6 +48,35 @@ RobotOutputReceiver::RobotOutputReceiver( &RobotOutputReceiver::CopyOutput); } +RobotOutputReceiver::RobotOutputReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance) { + model_instance_ = model_instance; + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + num_efforts_ = plant.num_actuators(); + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + this->DeclareAbstractInputPort("lcmt_robot_output", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, u, t", + OutputVector(plant.num_positions(model_instance), + plant.num_velocities(model_instance), + plant.num_actuators()), + &RobotOutputReceiver::CopyOutput); +} + void RobotOutputReceiver::CopyOutput(const Context& context, OutputVector* output) const { const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); @@ -55,12 +86,12 @@ void RobotOutputReceiver::CopyOutput(const Context& context, VectorXd positions = VectorXd::Zero(num_positions_); for (int i = 0; i < state_msg.num_positions; i++) { int j = position_index_map_.at(state_msg.position_names[i]); - positions(j) = state_msg.position[i]; + positions(j - positions_start_idx_) = state_msg.position[i]; } VectorXd velocities = VectorXd::Zero(num_velocities_); for (int i = 0; i < state_msg.num_velocities; i++) { int j = velocity_index_map_.at(state_msg.velocity_names[i]); - velocities(j) = state_msg.velocity[i]; + velocities(j - velocities_start_idx_) = state_msg.velocity[i]; } VectorXd efforts = VectorXd::Zero(num_efforts_); for (int i = 0; i < state_msg.num_efforts; i++) { @@ -84,16 +115,18 @@ void RobotOutputReceiver::CopyOutput(const Context& context, void RobotOutputReceiver::InitializeSubscriberPositions( const MultibodyPlant& plant, - drake::systems::Context &context) const { + drake::systems::Context& context) const { auto& state_msg = context.get_mutable_abstract_state(0); // using the time from the context state_msg.utime = context.get_time() * 1e6; std::vector ordered_position_names = - multibody::ExtractOrderedNamesFromMap(position_index_map_); + multibody::ExtractOrderedNamesFromMap(position_index_map_, + positions_start_idx_); std::vector ordered_velocity_names = - multibody::ExtractOrderedNamesFromMap(velocity_index_map_); + multibody::ExtractOrderedNamesFromMap(velocity_index_map_, + velocities_start_idx_); std::vector ordered_effort_names = multibody::ExtractOrderedNamesFromMap(effort_index_map_); @@ -110,10 +143,17 @@ void RobotOutputReceiver::InitializeSubscriberPositions( } // Set quaternion w = 1, assumes drake quaternion ordering of wxyz - for (const auto& body_idx : plant.GetFloatingBaseBodies()) { - const auto& body = plant.get_body(body_idx); - if (body.has_quaternion_dofs()) { - state_msg.position[body.floating_positions_start()] = 1; + if (model_instance_ != + drake::multibody::ModelInstanceIndex(DEFAULT_MODEL_INSTANCE_INDEX)) { + if (plant.HasUniqueFreeBaseBody(model_instance_)) { + state_msg.position.at(0) = 1; + } + } else { + for (const auto& body_idx : plant.GetFloatingBaseBodies()) { + const auto& body = plant.get_body(body_idx); + if (body.has_quaternion_dofs()) { + state_msg.position.at(body.floating_positions_start()) = 1; + } } } @@ -125,7 +165,6 @@ void RobotOutputReceiver::InitializeSubscriberPositions( /*--------------------------------------------------------------------------*/ // methods implementation for RobotOutputSender. - RobotOutputSender::RobotOutputSender( const drake::multibody::MultibodyPlant& plant, const bool publish_efforts, const bool publish_imu) @@ -138,6 +177,9 @@ RobotOutputSender::RobotOutputSender( velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap(position_index_map_); ordered_velocity_names_ = @@ -164,6 +206,54 @@ RobotOutputSender::RobotOutputSender( &RobotOutputSender::Output); } +RobotOutputSender::RobotOutputSender( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance, + const bool publish_efforts, const bool publish_imu) + : publish_efforts_(publish_efforts), publish_imu_(publish_imu) { + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + num_efforts_ = plant.num_actuators(); + + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap( + position_index_map_, positions_start_idx_); + ordered_velocity_names_ = multibody::ExtractOrderedNamesFromMap( + velocity_index_map_, velocities_start_idx_); + ordered_effort_names_ = + multibody::ExtractOrderedNamesFromMap(effort_index_map_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + if (publish_efforts_) { + effort_input_port_ = + this->DeclareVectorInputPort("u", BasicVector(num_efforts_)) + .get_index(); + } + if (publish_imu_) { + imu_input_port_ = + this->DeclareVectorInputPort("imu_acceleration", BasicVector(3)) + .get_index(); + } + + this->DeclareAbstractOutputPort("lcmt_robot_output", + &RobotOutputSender::Output); +} + /// Populate a state message with all states void RobotOutputSender::Output(const Context& context, dairlib::lcmt_robot_output* state_msg) const { @@ -213,6 +303,221 @@ void RobotOutputSender::Output(const Context& context, } } +ObjectStateReceiver::ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant) { + num_positions_ = plant.num_positions(); + num_velocities_ = plant.num_velocities(); + position_index_map_ = multibody::MakeNameToPositionsMap(plant); + velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + model_instance_ = + drake::multibody::ModelInstanceIndex(DEFAULT_MODEL_INSTANCE_INDEX); + + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + this->DeclareAbstractInputPort("lcmt_object_state", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, t", + StateVector(plant.num_positions(), plant.num_velocities()), + &ObjectStateReceiver::CopyOutput); +} + +ObjectStateReceiver::ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance) { + model_instance_ = model_instance; + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + for (const auto& entry : plant.GetJointIndices(model_instance)) { + // If joint.num_positions() == 0, then it is a fixed joint. + // Skip it and fix positions_start_idx_ to be the non fixed joint. + if (plant.get_joint(entry).num_positions() != 0) { + positions_start_idx_ = plant.get_joint(entry).position_start(); + velocities_start_idx_ = plant.get_joint(entry).velocity_start(); + break; + } + } + this->DeclareAbstractInputPort("lcmt_object_state", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, t", + StateVector(plant.num_positions(model_instance), + plant.num_velocities(model_instance)), + &ObjectStateReceiver::CopyOutput); +} + +void ObjectStateReceiver::CopyOutput(const Context& context, + StateVector* output) const { + const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); + DRAKE_ASSERT(input != nullptr); + const auto& state_msg = input->get_value(); + + VectorXd positions = VectorXd::Zero(num_positions_); + for (int i = 0; i < state_msg.num_positions; i++) { + int j = position_index_map_.at(state_msg.position_names[i]); + positions(j - positions_start_idx_) = state_msg.position[i]; + } + VectorXd velocities = VectorXd::Zero(num_velocities_); + for (int i = 0; i < state_msg.num_velocities; i++) { + int j = velocity_index_map_.at(state_msg.velocity_names[i]); + velocities(j - velocities_start_idx_) = state_msg.velocity[i]; + } + + output->SetPositions(positions); + output->SetVelocities(velocities); + output->set_timestamp(state_msg.utime * 1.0e-6); +} + +void ObjectStateReceiver::InitializeSubscriberPositions( + const MultibodyPlant& plant, + drake::systems::Context& context) const { + auto& state_msg = context.get_mutable_abstract_state(0); + + // using the time from the context + state_msg.utime = context.get_time() * 1e6; + + std::vector ordered_position_names = + multibody::ExtractOrderedNamesFromMap(position_index_map_, + positions_start_idx_); + std::vector ordered_velocity_names = + multibody::ExtractOrderedNamesFromMap(velocity_index_map_, + velocities_start_idx_); + + state_msg.num_positions = num_positions_; + state_msg.num_velocities = num_velocities_; + state_msg.position_names.resize(num_positions_); + state_msg.velocity_names.resize(num_velocities_); + state_msg.position.resize(num_positions_); + state_msg.velocity.resize(num_positions_); + + for (int i = 0; i < num_positions_; i++) { + state_msg.position_names[i] = ordered_position_names[i]; + state_msg.position[i] = 0; + } + + // Set quaternion w = 1, assumes drake quaternion ordering of wxyz + if (model_instance_ != + drake::multibody::ModelInstanceIndex(DEFAULT_MODEL_INSTANCE_INDEX)) { + if (plant.HasUniqueFreeBaseBody(model_instance_)) { + state_msg.position.at(0) = 1; + } + } else { + for (const auto& body_idx : plant.GetFloatingBaseBodies()) { + const auto& body = plant.get_body(body_idx); + if (body.has_quaternion_dofs()) { + state_msg.position.at(body.floating_positions_start()) = 1; + } + } + } + + for (int i = 0; i < num_velocities_; i++) { + state_msg.velocity[i] = 0; + state_msg.velocity_names[i] = ordered_velocity_names[i]; + } +} + +/*--------------------------------------------------------------------------*/ +// methods implementation for RobotOutputSender. +ObjectStateSender::ObjectStateSender( + const drake::multibody::MultibodyPlant& plant) { + num_positions_ = plant.num_positions(); + num_velocities_ = plant.num_velocities(); + + position_index_map_ = multibody::MakeNameToPositionsMap(plant); + velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + + model_instance_ = + drake::multibody::ModelInstanceIndex(DEFAULT_MODEL_INSTANCE_INDEX); + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + + ordered_position_names_ = + multibody::ExtractOrderedNamesFromMap(position_index_map_); + ordered_velocity_names_ = + multibody::ExtractOrderedNamesFromMap(velocity_index_map_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + + this->DeclareAbstractOutputPort("lcmt_object_state", + &ObjectStateSender::Output); +} + +ObjectStateSender::ObjectStateSender( + const drake::multibody::MultibodyPlant& plant, + bool publish_velocities, + drake::multibody::ModelInstanceIndex model_instance) + : publish_velocities_(publish_velocities), model_instance_(model_instance) { + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + for (const auto& entry : plant.GetJointIndices(model_instance)) { + // If joint.num_positions() == 0, then it is a fixed joint. + // Skip it and fix positions_start_idx_ to be the non fixed joint. + if (plant.get_joint(entry).num_positions() != 0) { + positions_start_idx_ = plant.get_joint(entry).position_start(); + velocities_start_idx_ = plant.get_joint(entry).velocity_start(); + break; + } + } + + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap( + position_index_map_, positions_start_idx_); + ordered_velocity_names_ = multibody::ExtractOrderedNamesFromMap( + velocity_index_map_, velocities_start_idx_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + this->DeclareAbstractOutputPort("lcmt_object_state", + &ObjectStateSender::Output); +} + +/// Populate a state message with all states +void ObjectStateSender::Output(const Context& context, + dairlib::lcmt_object_state* state_msg) const { + const auto state = this->EvalVectorInput(context, state_input_port_); + + // using the time from the context + state_msg->utime = context.get_time() * 1e6; + + state_msg->num_positions = num_positions_; + state_msg->num_velocities = num_velocities_; + state_msg->position_names.resize(num_positions_); + state_msg->velocity_names.resize(num_velocities_); + state_msg->position.resize(num_positions_); + state_msg->velocity.resize(num_velocities_); + + for (int i = 0; i < num_positions_; i++) { + state_msg->position_names[i] = ordered_position_names_[i]; + if (std::isnan(state->GetAtIndex(i))) { + state_msg->position[i] = 0; + } else { + state_msg->position[i] = state->GetAtIndex(i); + } + } + for (int i = 0; i < num_velocities_; i++) { + state_msg->velocity[i] = 0; + if (publish_velocities_) { + state_msg->velocity[i] = state->GetAtIndex(num_positions_ + i); + } + state_msg->velocity_names[i] = ordered_velocity_names_[i]; + } +} + /*--------------------------------------------------------------------------*/ // methods implementation for RobotInputReceiver. @@ -222,9 +527,9 @@ RobotInputReceiver::RobotInputReceiver( actuator_index_map_ = multibody::MakeNameToActuatorsMap(plant); this->DeclareAbstractInputPort("lcmt_robot_input", drake::Value{}); - this->DeclareVectorOutputPort("u, t", - TimestampedVector(num_actuators_), - &RobotInputReceiver::CopyInputOut); + this->DeclareVectorOutputPort( + "u, t", TimestampedVector(num_actuators_), + &RobotInputReceiver::CopyInputOut, {all_sources_ticket()}); } void RobotInputReceiver::CopyInputOut(const Context& context, @@ -284,13 +589,10 @@ void RobotCommandSender::OutputCommand( SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( drake::systems::DiagramBuilder* builder, const MultibodyPlant& plant, - drake::systems::lcm::LcmInterfaceSystem* lcm, - std::string actuator_channel, - std::string state_channel, - double publish_rate, - bool publish_efforts, - double actuator_delay) { - + drake::systems::lcm::LcmInterfaceSystem* lcm, std::string actuator_channel, + std::string state_channel, double publish_rate, + drake::multibody::ModelInstanceIndex model_instance_index, + bool publish_efforts, double actuator_delay) { // Create LCM input for actuators auto input_sub = builder->AddSystem(LcmSubscriberSystem::Make( @@ -307,31 +609,32 @@ SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( builder->AddSystem(LcmPublisherSystem::Make( state_channel, lcm, 1.0 / publish_rate)); auto state_sender = builder->AddSystem( - plant, publish_efforts); - builder->Connect(plant.get_state_output_port(), + plant, model_instance_index, publish_efforts); + + builder->Connect(plant.get_state_output_port(model_instance_index), state_sender->get_input_port_state()); // Add delay, if used, and associated connections if (actuator_delay > 0) { - auto discrete_time_delay = - builder->AddSystem( - 1.0 / publish_rate, actuator_delay * publish_rate, - plant.num_actuators()); - builder->Connect(*passthrough, *discrete_time_delay); + auto discrete_time_delay = + builder->AddSystem( + 1.0 / publish_rate, actuator_delay * publish_rate, + plant.num_actuators()); + builder->Connect(*passthrough, *discrete_time_delay); + builder->Connect(discrete_time_delay->get_output_port(), + plant.get_actuation_input_port()); + + if (publish_efforts) { builder->Connect(discrete_time_delay->get_output_port(), - plant.get_actuation_input_port()); - - if (publish_efforts) { - builder->Connect(discrete_time_delay->get_output_port(), - state_sender->get_input_port_effort()); - } + state_sender->get_input_port_effort()); + } } else { + builder->Connect(passthrough->get_output_port(), + plant.get_actuation_input_port()); + if (publish_efforts) { builder->Connect(passthrough->get_output_port(), - plant.get_actuation_input_port()); - if (publish_efforts) { - builder->Connect(passthrough->get_output_port(), - state_sender->get_input_port_effort()); - } + state_sender->get_input_port_effort()); + } } builder->Connect(*state_sender, *state_pub); @@ -340,4 +643,4 @@ SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( } } // namespace systems -} // namespace dairlib +} // namespace dairlib \ No newline at end of file diff --git a/systems/robot_lcm_systems.h b/systems/robot_lcm_systems.h index 3e1a88c0f6..5f0f57b3cf 100644 --- a/systems/robot_lcm_systems.h +++ b/systems/robot_lcm_systems.h @@ -4,9 +4,12 @@ #include #include +#include + #include "dairlib/lcmt_robot_input.hpp" #include "dairlib/lcmt_robot_output.hpp" #include "systems/framework/output_vector.h" +#include "systems/framework/state_vector.h" #include "systems/framework/timestamped_vector.h" #include "systems/primitives/subvector_pass_through.h" @@ -17,11 +20,12 @@ namespace dairlib { namespace systems { +constexpr int DEFAULT_MODEL_INSTANCE_INDEX = 88888888; + /// @file This file contains classes dealing with sending/receiving -/// LCM messages related to a robot. The classes in this file are based on -/// acrobot_lcm.h +/// LCM messages related to a robot. -/// Receives the output of an LcmSubsriberSystem that subsribes to the +/// Receives the output of an LcmSubscriberSystem that subscribes to the /// Robot output channel with LCM type lcmt_robot_output, and outputs the /// robot states as a OutputVector. class RobotOutputReceiver : public drake::systems::LeafSystem { @@ -29,6 +33,10 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { explicit RobotOutputReceiver( const drake::multibody::MultibodyPlant& plant); + explicit RobotOutputReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance); + /// Convenience function to initialize an lcmt_robot_output subscriber with /// positions and velocities which are all zero except for the quaternion /// positions, which are all 1, 0, 0, 0 @@ -41,6 +49,9 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { private: void CopyOutput(const drake::systems::Context& context, OutputVector* output) const; + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; int num_positions_; int num_velocities_; int num_efforts_; @@ -52,6 +63,11 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { /// Converts a OutputVector object to LCM type lcmt_robot_output class RobotOutputSender : public drake::systems::LeafSystem { public: + explicit RobotOutputSender( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index, + const bool publish_efforts = false, const bool publish_imu = false); + explicit RobotOutputSender( const drake::multibody::MultibodyPlant& plant, const bool publish_efforts = false, const bool publish_imu = false); @@ -73,6 +89,8 @@ class RobotOutputSender : public drake::systems::LeafSystem { void Output(const drake::systems::Context& context, dairlib::lcmt_robot_output* output) const; + int positions_start_idx_; + int velocities_start_idx_; int num_positions_; int num_velocities_; int num_efforts_; @@ -89,7 +107,76 @@ class RobotOutputSender : public drake::systems::LeafSystem { bool publish_imu_; }; -/// Receives the output of an LcmSubsriberSystem that subsribes to the +/// @file This file contains classes dealing with sending/receiving +/// LCM messages related to a object. + +/// Receives the output of an LcmSubscriberSystem that subscribes to the +/// object state channel with LCM type lcmt_object_state, and outputs the +/// object state as a StateVector. +class ObjectStateReceiver : public drake::systems::LeafSystem { + public: + explicit ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant); + + explicit ObjectStateReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance); + + /// Convenience function to initialize an lcmt_object_state subscriber with + /// positions and velocities which are all zero except for the quaternion + /// positions, which are all 1, 0, 0, 0 + /// @param context The context of a + /// drake::LcmSubscriberSystem + void InitializeSubscriberPositions( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context& context) const; + + private: + void CopyOutput(const drake::systems::Context& context, + StateVector* output) const; + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; + int num_positions_; + int num_velocities_; + std::map position_index_map_; + std::map velocity_index_map_; +}; + +/// Converts a StateVector object to LCM type lcmt_robot_output +class ObjectStateSender : public drake::systems::LeafSystem { + public: + explicit ObjectStateSender( + const drake::multibody::MultibodyPlant& plant, + bool publish_velocities = true, + drake::multibody::ModelInstanceIndex model_instance_index = + drake::multibody::default_model_instance()); + + explicit ObjectStateSender( + const drake::multibody::MultibodyPlant& plant); + + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_input_port_); + } + + private: + void Output(const drake::systems::Context& context, + dairlib::lcmt_object_state* output) const; + + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; + int num_positions_; + int num_velocities_; + std::vector ordered_position_names_; + std::vector ordered_velocity_names_; + std::map position_index_map_; + std::map velocity_index_map_; + drake::systems::InputPortIndex state_input_port_; + bool publish_velocities_; +}; + +/// Receives the output of an LcmSubscriberSystem that subscribes to the /// robot input channel with LCM type lcmt_robot_input and outputs the /// robot inputs as a TimestampedVector. class RobotInputReceiver : public drake::systems::LeafSystem { @@ -122,7 +209,6 @@ class RobotCommandSender : public drake::systems::LeafSystem { std::map actuator_index_map_; }; - /// /// Convenience method to add and connect leaf systems for controlling /// a MultibodyPlant via LCM. Makes two primary connections: @@ -148,11 +234,10 @@ class RobotCommandSender : public drake::systems::LeafSystem { SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( drake::systems::DiagramBuilder* builder, const drake::multibody::MultibodyPlant& plant, - drake::systems::lcm::LcmInterfaceSystem* lcm, - std::string actuator_channel, - std::string state_channel, - double publish_rate, - bool publish_efforts = true, - double actuator_delay = 0); + drake::systems::lcm::LcmInterfaceSystem* lcm, std::string actuator_channel, + std::string state_channel, double publish_rate, + drake::multibody::ModelInstanceIndex model_instance_index, + bool publish_efforts = true, double actuator_delay = 0); + } // namespace systems -} // namespace dairlib +} // namespace dairlib \ No newline at end of file diff --git a/systems/ros/BUILD.bazel b/systems/ros/BUILD.bazel index 17dc86e94c..2785cd834f 100644 --- a/systems/ros/BUILD.bazel +++ b/systems/ros/BUILD.bazel @@ -1,4 +1,5 @@ # -*- python -*- +package(default_visibility = ["//visibility:public"]) cc_library( name = "ros_pubsub_systems", @@ -13,6 +14,29 @@ cc_library( ], ) +cc_library( + name = "franka_ros_lcm_conversions", + srcs = ["franka_ros_lcm_conversions.cc"], + hdrs = ["franka_ros_lcm_conversions.h"], + tags = ["ros"], + deps = [ + "//common", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/primitives", + "@drake//:drake_shared_library", + "@ros", + ], +) + +cc_library( + name = "franka_ros_channels", + hdrs = ["parameters/franka_ros_channels.h"], + deps = [ + "@drake//:drake_shared_library", + ], +) + cc_binary( name = "test_ros_publisher_system", srcs = ["test/test_ros_publisher_system.cc"], diff --git a/systems/ros/franka_msgs/Errors.msg b/systems/ros/franka_msgs/Errors.msg new file mode 100644 index 0000000000..a50780fb72 --- /dev/null +++ b/systems/ros/franka_msgs/Errors.msg @@ -0,0 +1,36 @@ +bool joint_position_limits_violation +bool cartesian_position_limits_violation +bool self_collision_avoidance_violation +bool joint_velocity_violation +bool cartesian_velocity_violation +bool force_control_safety_violation +bool joint_reflex +bool cartesian_reflex +bool max_goal_pose_deviation_violation +bool max_path_pose_deviation_violation +bool cartesian_velocity_profile_safety_violation +bool joint_position_motion_generator_start_pose_invalid +bool joint_motion_generator_position_limits_violation +bool joint_motion_generator_velocity_limits_violation +bool joint_motion_generator_velocity_discontinuity +bool joint_motion_generator_acceleration_discontinuity +bool cartesian_position_motion_generator_start_pose_invalid +bool cartesian_motion_generator_elbow_limit_violation +bool cartesian_motion_generator_velocity_limits_violation +bool cartesian_motion_generator_velocity_discontinuity +bool cartesian_motion_generator_acceleration_discontinuity +bool cartesian_motion_generator_elbow_sign_inconsistent +bool cartesian_motion_generator_start_elbow_invalid +bool cartesian_motion_generator_joint_position_limits_violation +bool cartesian_motion_generator_joint_velocity_limits_violation +bool cartesian_motion_generator_joint_velocity_discontinuity +bool cartesian_motion_generator_joint_acceleration_discontinuity +bool cartesian_position_motion_generator_invalid_frame +bool force_controller_desired_force_tolerance_violation +bool controller_torque_discontinuity +bool start_elbow_sign_inconsistent +bool communication_constraints_violation +bool power_limit_violation +bool joint_p2p_insufficient_torque_for_planning +bool tau_j_range_violation +bool instability_detected diff --git a/systems/ros/franka_msgs/FrankaState.msg b/systems/ros/franka_msgs/FrankaState.msg new file mode 100644 index 0000000000..37e6e23c6e --- /dev/null +++ b/systems/ros/franka_msgs/FrankaState.msg @@ -0,0 +1,54 @@ +std_msgs/Header header +float64[6] cartesian_collision +float64[6] cartesian_contact +float64[7] q +float64[7] q_d +float64[7] dq +float64[7] dq_d +float64[7] ddq_d +float64[7] theta +float64[7] dtheta +float64[7] tau_J +float64[7] dtau_J +float64[7] tau_J_d +float64[6] K_F_ext_hat_K +float64[2] elbow +float64[2] elbow_d +float64[2] elbow_c +float64[2] delbow_c +float64[2] ddelbow_c +float64[7] joint_collision +float64[7] joint_contact +float64[6] O_F_ext_hat_K +float64[6] O_dP_EE_d +float64[6] O_dP_EE_c +float64[6] O_ddP_EE_c +float64[7] tau_ext_hat_filtered +float64 m_ee +float64[3] F_x_Cee +float64[9] I_ee +float64 m_load +float64[3] F_x_Cload +float64[9] I_load +float64 m_total +float64[3] F_x_Ctotal +float64[9] I_total +float64[16] O_T_EE +float64[16] O_T_EE_d +float64[16] O_T_EE_c +float64[16] F_T_EE +float64[16] F_T_NE +float64[16] NE_T_EE +float64[16] EE_T_K +float64 time +float64 control_command_success_rate +uint8 ROBOT_MODE_OTHER=0 +uint8 ROBOT_MODE_IDLE=1 +uint8 ROBOT_MODE_MOVE=2 +uint8 ROBOT_MODE_GUIDING=3 +uint8 ROBOT_MODE_REFLEX=4 +uint8 ROBOT_MODE_USER_STOPPED=5 +uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6 +uint8 robot_mode +franka_msgs/Errors current_errors +franka_msgs/Errors last_motion_errors diff --git a/systems/ros/franka_ros_lcm_conversions.cc b/systems/ros/franka_ros_lcm_conversions.cc new file mode 100644 index 0000000000..67f1c96646 --- /dev/null +++ b/systems/ros/franka_ros_lcm_conversions.cc @@ -0,0 +1,331 @@ +#include "systems/ros/franka_ros_lcm_conversions.h" + +#include + +#include "franka_msgs/FrankaState.h" +#include "multibody/multibody_utils.h" +#include "nav_msgs/Path.h" +#include "nav_msgs/Odometry.h" +#include "sensor_msgs/JointState.h" + +namespace dairlib { +namespace systems { + +using Eigen::VectorXd; + +// LcmToRosTimestampedVector implementation +LcmToRosTimestampedVector::LcmToRosTimestampedVector(int vector_size) + : vector_size_(vector_size) { + this->DeclareVectorInputPort("u, t", TimestampedVector(vector_size_)); + this->DeclareAbstractOutputPort("ROS Float64MultiArray", + &LcmToRosTimestampedVector::ConvertToROS); +}; + +void LcmToRosTimestampedVector::ConvertToROS( + const drake::systems::Context& context, + std_msgs::Float64MultiArray* output) const { + const TimestampedVector* input = + (TimestampedVector*)this->EvalVectorInput(context, 0); + + // note: this function currently appends the timestamp to the end of the + // output + output->data.clear(); + // std::cout << "time: " << input->get_timestamp() << std::endl; + for (int i = 0; i < vector_size_; i++) { + // std::cout << "value: " << i << " " << input->GetAtIndex(i) << + // std::endl; + if (std::isnan(input->GetAtIndex(i))) { + output->data.push_back(0); + } else { + output->data.push_back(input->GetAtIndex(i)); + } + } + output->data.push_back(input->get_timestamp()); +} + +/***************************************************************************************/ +// ROSToRobotOuput implementation + +RosToLcmRobotState::RosToLcmRobotState(int num_positions, int num_velocities, + int num_efforts) + : num_positions_(num_positions), + num_velocities_(num_velocities), + num_efforts_(num_efforts) { + this->DeclareAbstractInputPort("Franka JointState topic", + drake::Value()); + this->DeclareAbstractOutputPort("lcmt_robot_output", + &RosToLcmRobotState::ConvertToLCM); +} + +void RosToLcmRobotState::ConvertToLCM( + const drake::systems::Context& context, + dairlib::lcmt_robot_output* output) const { + const drake::AbstractValue* const input = this->EvalAbstractInput(context, 0); + DRAKE_ASSERT(input != nullptr); + const auto& msg = input->get_value(); + + output->num_positions = num_positions_; + output->num_velocities = num_velocities_; + output->num_efforts = num_efforts_; + output->position_names.resize(num_positions_); + output->velocity_names.resize(num_velocities_); + output->effort_names.resize(num_efforts_); + output->position_names = position_names_; + output->velocity_names = velocity_names_; + output->effort_names = effort_names_; + output->position.resize(num_positions_); + output->velocity.resize(num_velocities_); + output->effort.resize(num_efforts_); + + // yet to receive message from rostopic + if (msg.position.empty()) { + for (int i = 0; i < num_positions_; i++) { + output->position[i] = nan(""); + } + for (int i = 0; i < num_velocities_; i++) { + output->velocity[i] = nan(""); + } + for (int i = 0; i < num_efforts_; i++) { + output->effort[i] = nan(""); + } + for (int i = 0; i < 3; i++) { + output->imu_accel[i] = nan(""); + } + } + // input should be order as "positions, velocities, effort, timestamp" + else { + for (int i = 0; i < num_franka_joints_; i++) { + output->position[i] = msg.position[i]; + } + // hard coded to add 7 zeros to the end of position + for (int i = num_franka_joints_; i < num_positions_; i++) { + output->position[i] = default_ball_position_[i - num_franka_joints_]; + } + + for (int i = 0; i < num_velocities_; i++) { + output->velocity[i] = msg.velocity[i]; + } + // hard coded to add 6 zeros to the end of velocity + for (int i = num_franka_joints_; i < num_velocities_; i++) { + output->velocity[i] = 0; + } + + for (int i = 0; i < num_efforts_; i++) { + output->effort[i] = msg.effort[i]; + } + for (int i = 0; i < 3; i++) { + output->imu_accel[i] = 0; + } + } + output->utime = context.get_time() * 1e6; +} + +/***************************************************************************************/ +// RosToLcmFrankaState implementation + +RosToLcmFrankaState::RosToLcmFrankaState() { + this->DeclareAbstractInputPort("ROS Float64MultiArray", + drake::Value()); + this->DeclareAbstractOutputPort("lcmt_franka_state", + &RosToLcmFrankaState::ConvertToLCM); +} + +void RosToLcmFrankaState::ConvertToLCM( + const drake::systems::Context& context, + dairlib::lcmt_franka_state* franka_state) const { + const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); + const auto& msg = input->get_value(); + + if (msg.O_T_EE.empty()) { + franka_state->valid = false; + } else { + for (size_t i = 0; i < msg.O_T_EE.size(); i++) { + franka_state->O_T_EE[i] = msg.O_T_EE[i]; + } + for (size_t i = 0; i < msg.O_T_EE_d.size(); i++) { + franka_state->O_T_EE_d[i] = msg.O_T_EE_d[i]; + } + for (size_t i = 0; i < msg.F_T_EE.size(); i++) { + franka_state->F_T_EE[i] = msg.F_T_EE[i]; + } + for (size_t i = 0; i < msg.F_T_NE.size(); i++) { + franka_state->F_T_NE[i] = msg.F_T_NE[i]; + } + for (size_t i = 0; i < msg.NE_T_EE.size(); i++) { + franka_state->NE_T_EE[i] = msg.NE_T_EE[i]; + } + for (size_t i = 0; i < msg.EE_T_K.size(); i++) { + franka_state->EE_T_K[i] = msg.EE_T_K[i]; + } + franka_state->m_ee = msg.m_ee; + for (size_t i = 0; i < msg.I_ee.size(); i++) { + franka_state->I_ee[i] = msg.I_ee[i]; + } + for (size_t i = 0; i < msg.F_x_Cee.size(); i++) { + franka_state->F_x_Cee[i] = msg.F_x_Cee[i]; + } + franka_state->m_load = msg.m_load; + for (size_t i = 0; i < msg.I_load.size(); i++) { + franka_state->I_load[i] = msg.I_load[i]; + } + for (size_t i = 0; i < msg.F_x_Cload.size(); i++) { + franka_state->F_x_Cload[i] = msg.F_x_Cload[i]; + } + franka_state->m_total = msg.m_total; + for (size_t i = 0; i < msg.I_total.size(); i++) { + franka_state->I_total[i] = msg.I_total[i]; + } + for (size_t i = 0; i < msg.F_x_Ctotal.size(); i++) { + franka_state->F_x_Ctotal[i] = msg.F_x_Ctotal[i]; + } + for (size_t i = 0; i < msg.elbow.size(); i++) { + franka_state->elbow[i] = msg.elbow[i]; + } + for (size_t i = 0; i < msg.elbow_d.size(); i++) { + franka_state->elbow_d[i] = msg.elbow_d[i]; + } + for (size_t i = 0; i < msg.elbow_c.size(); i++) { + franka_state->elbow_c[i] = msg.elbow_c[i]; + } + for (size_t i = 0; i < msg.delbow_c.size(); i++) { + franka_state->delbow_c[i] = msg.delbow_c[i]; + } + for (size_t i = 0; i < msg.ddelbow_c.size(); i++) { + franka_state->ddelbow_c[i] = msg.ddelbow_c[i]; + } + for (size_t i = 0; i < msg.tau_J.size(); i++) { + franka_state->tau_J[i] = msg.tau_J[i]; + } + for (size_t i = 0; i < msg.tau_J_d.size(); i++) { + franka_state->tau_J_d[i] = msg.tau_J_d[i]; + } + for (size_t i = 0; i < msg.dtau_J.size(); i++) { + franka_state->dtau_J[i] = msg.dtau_J[i]; + } + for (size_t i = 0; i < msg.q.size(); i++) { + franka_state->q[i] = msg.q[i]; + } + for (size_t i = 0; i < msg.q_d.size(); i++) { + franka_state->q_d[i] = msg.q_d[i]; + } + for (size_t i = 0; i < msg.dq.size(); i++) { + franka_state->dq[i] = msg.dq[i]; + } + for (size_t i = 0; i < msg.dq_d.size(); i++) { + franka_state->dq_d[i] = msg.dq_d[i]; + } + for (size_t i = 0; i < msg.ddq_d.size(); i++) { + franka_state->ddq_d[i] = msg.ddq_d[i]; + } + for (size_t i = 0; i < msg.joint_contact.size(); i++) { + franka_state->joint_contact[i] = msg.joint_contact[i]; + } + for (size_t i = 0; i < msg.cartesian_contact.size(); i++) { + franka_state->cartesian_contact[i] = msg.cartesian_contact[i]; + } + for (size_t i = 0; i < msg.joint_collision.size(); i++) { + franka_state->joint_collision[i] = msg.joint_collision[i]; + } + for (size_t i = 0; i < msg.cartesian_collision.size(); i++) { + franka_state->cartesian_collision[i] = msg.cartesian_collision[i]; + } + for (size_t i = 0; i < msg.tau_ext_hat_filtered.size(); i++) { + franka_state->tau_ext_hat_filtered[i] = msg.tau_ext_hat_filtered[i]; + } + for (size_t i = 0; i < msg.O_F_ext_hat_K.size(); i++) { + franka_state->O_F_ext_hat_K[i] = msg.O_F_ext_hat_K[i]; + } + for (size_t i = 0; i < msg.K_F_ext_hat_K.size(); i++) { + franka_state->K_F_ext_hat_K[i] = msg.K_F_ext_hat_K[i]; + } + for (size_t i = 0; i < msg.O_dP_EE_d.size(); i++) { + franka_state->O_dP_EE_d[i] = msg.O_dP_EE_d[i]; + } + for (size_t i = 0; i < msg.O_T_EE_c.size(); i++) { + franka_state->O_T_EE_c[i] = msg.O_T_EE_c[i]; + } + for (size_t i = 0; i < msg.O_dP_EE_c.size(); i++) { + franka_state->O_dP_EE_c[i] = msg.O_dP_EE_c[i]; + } + for (size_t i = 0; i < msg.O_ddP_EE_c.size(); i++) { + franka_state->O_ddP_EE_c[i] = msg.O_ddP_EE_c[i]; + } + for (size_t i = 0; i < msg.theta.size(); i++) { + franka_state->theta[i] = msg.theta[i]; + } + for (size_t i = 0; i < msg.dtheta.size(); i++) { + franka_state->dtheta[i] = msg.dtheta[i]; + } + + std::stringstream ss_current_errors; + ss_current_errors << msg.current_errors; + franka_state->current_errors = ss_current_errors.str(); + + std::stringstream ss_last_motion_errors; + ss_last_motion_errors << msg.last_motion_errors; + franka_state->last_motion_errors = ss_last_motion_errors.str(); + + franka_state->control_command_success_rate = + msg.control_command_success_rate; + franka_state->robot_mode = msg.robot_mode; + // time from franka robot + franka_state->franka_time = msg.time; + franka_state->valid = true; + } + // time for drake loop + franka_state->utime = context.get_time() * 1e6; +} + +/***************************************************************************************/ +RosToLcmObjectState::RosToLcmObjectState( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance, + const std::string& object_name) { + auto positions_start_idx = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + auto velocities_start_idx = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + + dairlib::lcmt_object_state object_state = dairlib::lcmt_object_state(); + object_state.num_positions = plant.num_positions(model_instance); + object_state.num_velocities = plant.num_velocities(model_instance); + object_state.object_name = object_name; + object_state.position = std::vector(object_state.num_positions); + object_state.position_names = multibody::ExtractOrderedNamesFromMap( + multibody::MakeNameToPositionsMap(plant, model_instance), + positions_start_idx); + object_state.velocity = std::vector(object_state.num_velocities); + object_state.velocity_names = multibody::ExtractOrderedNamesFromMap( + multibody::MakeNameToVelocitiesMap(plant, model_instance), + velocities_start_idx); + object_state.position[0] = 1; + this->DeclareAbstractOutputPort(object_name + "_state", object_state, + &RosToLcmObjectState::ConvertToLCM); + this->DeclareAbstractInputPort("ROS PoseWithCovariance", + drake::Value()); +} + +void RosToLcmObjectState::ConvertToLCM( + const drake::systems::Context& context, + dairlib::lcmt_object_state* object_state) const { + const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); + const auto& msg = input->get_value(); + + if (msg.pose.pose.position.x == 0) { + // do nothing when there is no message, just keep the most recent message1 + } else { + object_state->position[0] = msg.pose.pose.orientation.w; + object_state->position[1] = msg.pose.pose.orientation.x; + object_state->position[2] = msg.pose.pose.orientation.y; + object_state->position[3] = msg.pose.pose.orientation.z; + object_state->position[4] = msg.pose.pose.position.x; + object_state->position[5] = msg.pose.pose.position.y; + object_state->position[6] = msg.pose.pose.position.z; + } + object_state->utime = context.get_time() * 1e6; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/ros/franka_ros_lcm_conversions.h b/systems/ros/franka_ros_lcm_conversions.h new file mode 100644 index 0000000000..791960ebb7 --- /dev/null +++ b/systems/ros/franka_ros_lcm_conversions.h @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include + +#include "dairlib/lcmt_estimated_object_state.hpp" +#include "dairlib/lcmt_franka_state.hpp" +#include "dairlib/lcmt_robot_output.hpp" +#include "std_msgs/Float64MultiArray.h" +#include "systems/framework/output_vector.h" +#include "systems/framework/timestamped_vector.h" + +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/lcm/lcm_interface_system.h" + +namespace dairlib { +namespace systems { + +class LcmToRosTimestampedVector : public drake::systems::LeafSystem { + public: + static std::unique_ptr Make(int vector_size) { + return std::make_unique(vector_size); + } + + explicit LcmToRosTimestampedVector(int vector_size); + + private: + void ConvertToROS(const drake::systems::Context& context, + std_msgs::Float64MultiArray* output) const; + int vector_size_; +}; + +// NOTE: this class appends 7 zeros to the position and 6 zeros +// to the velocity fields. This was done since this class was hard +// coded for the C3 Franka experiments. +class RosToLcmRobotState : public drake::systems::LeafSystem { + public: + static std::unique_ptr Make(int num_positions, + int num_velocities, + int num_efforts) { + return std::make_unique(num_positions, num_velocities, + num_efforts); + } + + explicit RosToLcmRobotState(int num_positions, int num_velocities, + int num_efforts); + + private: + void ConvertToLCM(const drake::systems::Context& context, + dairlib::lcmt_robot_output* output) const; + int num_positions_; + int num_velocities_; + int num_efforts_; + const int num_franka_joints_{7}; + + const std::vector default_ball_position_{ + 1, 0, 0, 0, 0.5, 0, 0.0315 - 0.0301}; + + const std::vector position_names_{ + "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7", "base_qw", + "base_qx", "base_qy", "base_qz", "base_x", + "base_y", "base_z"}; + + const std::vector velocity_names_{ + "panda_joint1dot", "panda_joint2dot", "panda_joint3dot", + "panda_joint4dot", "panda_joint5dot", "panda_joint6dot", + "panda_joint7dot", "base_wx", "base_wy", + "base_wz", "base_vx", "base_vy", + "base_vz"}; + + const std::vector effort_names_{ + "panda_motor1", "panda_motor2", "panda_motor3", "panda_motor4", + "panda_motor5", "panda_motor6", "panda_motor7", + }; +}; + +/// More specific translator from Franka state to LCM. Similar to +/// lcmt_cassie_out.lcm +class RosToLcmFrankaState : public drake::systems::LeafSystem { + public: + static std::unique_ptr Make() { + return std::make_unique(); + } + + explicit RosToLcmFrankaState(); + + private: + void ConvertToLCM(const drake::systems::Context& context, + dairlib::lcmt_franka_state* franka_state) const; +}; + +class RosToLcmObjectState : public drake::systems::LeafSystem { + public: + static std::unique_ptr Make( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance, + const std::string& object_name) { + return std::make_unique(plant, model_instance, + object_name); + } + + explicit RosToLcmObjectState( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance, + const std::string& object_name); + + private: + void ConvertToLCM(const drake::systems::Context& context, + dairlib::lcmt_object_state* franka_state) const; +}; +} // namespace systems +} // namespace dairlib diff --git a/systems/ros/parameters/franka_ros_channels.h b/systems/ros/parameters/franka_ros_channels.h new file mode 100644 index 0000000000..d295204f92 --- /dev/null +++ b/systems/ros/parameters/franka_ros_channels.h @@ -0,0 +1,19 @@ +#pragma once + +#include "drake/common/yaml/yaml_read_archive.h" + + +struct FrankaRosChannels { + std::string franka_state_channel; + std::string tray_state_channel; + std::string box_state_channel; + std::string franka_input_channel; + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(franka_state_channel)); + a->Visit(DRAKE_NVP(tray_state_channel)); + a->Visit(DRAKE_NVP(box_state_channel)); + a->Visit(DRAKE_NVP(franka_input_channel)); + } +}; \ No newline at end of file diff --git a/systems/ros/parameters/franka_ros_channels.yaml b/systems/ros/parameters/franka_ros_channels.yaml new file mode 100644 index 0000000000..b60e08b698 --- /dev/null +++ b/systems/ros/parameters/franka_ros_channels.yaml @@ -0,0 +1,5 @@ +franka_state_channel: /franka/joint_states +tray_state_channel: /tagslam/odom/body_tray +box_state_channel: /franka/box_object_state +franka_input_channel: /franka/franka_input + diff --git a/systems/ros/ros_publisher_system.h b/systems/ros/ros_publisher_system.h index b49f3aa9a4..f3b21ee6fc 100644 --- a/systems/ros/ros_publisher_system.h +++ b/systems/ros/ros_publisher_system.h @@ -129,12 +129,7 @@ class RosPublisherSystem : public drake::systems::LeafSystem { if (publish_triggers.find(TriggerType::kPerStep) != publish_triggers.end()) { - this->DeclarePerStepEvent( - drake::systems::PublishEvent([this]( - const drake::systems::Context& context, - const drake::systems::PublishEvent&) { - this->PublishToRosTopic(context); - })); + this->DeclarePerStepPublishEvent(&RosPublisherSystem::PublishToRosTopic); } } diff --git a/systems/ros/ros_subscriber_system.h b/systems/ros/ros_subscriber_system.h index fb05650af1..9e37985919 100644 --- a/systems/ros/ros_subscriber_system.h +++ b/systems/ros/ros_subscriber_system.h @@ -1,15 +1,16 @@ #pragma once +#include #include #include #include #include +#include "ros/ros.h" + #include "drake/common/drake_copyable.h" #include "drake/systems/framework/leaf_system.h" -#include "ros/ros.h" - namespace dairlib { namespace systems { @@ -82,12 +83,16 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { message_state_index); set_name(make_name(topic_)); + start_ = std::chrono::steady_clock::now(); + time_ = 0; } ~RosSubscriberSystem() override{}; const std::string& get_topic_name() const { return topic_; } + double message_time() const { return time_; } + /// Returns the default name for a system that publishes @p topic. static std::string make_name(const std::string& topic) { return "RosSubscriberSystem(" + topic + ")"; @@ -187,6 +192,8 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { received_message_ = message; received_message_count_++; received_message_condition_variable_.notify_all(); + time_ = + (duration_cast(std::chrono::steady_clock::now() - start_)).count()/1.0e6; } // The topic on which to receive ROS messages. @@ -198,6 +205,9 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { // A condition variable that's signaled every time the handler is called. mutable std::condition_variable received_message_condition_variable_; + mutable double time_; + mutable std::chrono::time_point start_; + // The most recently received ROS message. RosMessage received_message_{}; diff --git a/systems/senders/BUILD.bazel b/systems/senders/BUILD.bazel new file mode 100644 index 0000000000..294107a22a --- /dev/null +++ b/systems/senders/BUILD.bazel @@ -0,0 +1,62 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "senders", + srcs = [], + deps = [ + ":sample_buffer_sender", + ":c3_state_sender", + ":sample_buffer_to_point_cloud", + ], +) + +cc_library( + name = "sample_buffer_to_point_cloud", + srcs = [ + "sample_buffer_to_point_cloud.cc", + ], + hdrs = [ + "sample_buffer_to_point_cloud.h", + ], + deps = [ + "//common:find_resource", + "//lcmtypes:lcmt_robot", + "//multibody:utils", + "//systems/framework:vector", + "@drake//:drake_shared_library", + "//lcm:lcm_trajectory_saver", + "@lcm", + ], +) + +cc_library( + name = "sample_buffer_sender", + srcs = [ + "sample_buffer_sender.cc", + ], + hdrs = [ + "sample_buffer_sender.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//common", + "//common:find_resource", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "c3_state_sender", + srcs = [ + "c3_state_sender.cc", + ], + hdrs = [ + "c3_state_sender.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) diff --git a/systems/senders/c3_state_sender.cc b/systems/senders/c3_state_sender.cc new file mode 100644 index 0000000000..ed296d9164 --- /dev/null +++ b/systems/senders/c3_state_sender.cc @@ -0,0 +1,86 @@ +#include "systems/senders/c3_state_sender.h" +#include "systems/framework/timestamped_vector.h" + +namespace dairlib { + +using drake::systems::BasicVector; +using systems::TimestampedVector; +using drake::systems::Context; + +namespace systems { + +C3StateSender::C3StateSender(int state_size, + std::vector state_names) { + this->set_name("c3_state_sender"); + + n_x_ = state_size; + + /// Final target state is the final C3 goal state. C3 objects do not use this + /// Final target state for computing plans/errors/costs, but it is useful to + /// have for visualization and/or debugging. + final_target_state_ = this->DeclareVectorInputPort( + "final_target_state", BasicVector(state_size)).get_index(); + + /// Target state is the intermediate C3 goal state that C3 uses for computing + /// errors. It is useful to truncate the final target state (yielding this + /// intermediate Target state) to bound the errors seen by C3. + target_state_ = this->DeclareVectorInputPort( + "target_state", BasicVector(state_size)).get_index(); + + /// Actual state is the current C3 state that C3 uses as an initial condition. + actual_state_ = this->DeclareVectorInputPort( + "actual_state", TimestampedVector(state_size)).get_index(); + + lcmt_c3_state default_c3_state = dairlib::lcmt_c3_state(); + default_c3_state.num_states = n_x_; + default_c3_state.utime = 0; + default_c3_state.state = std::vector(n_x_); + default_c3_state.state_names = state_names; + final_target_c3_state_ = this->DeclareAbstractOutputPort( + "c3_final_target_output", default_c3_state, + &C3StateSender::OutputFinalTargetState) + .get_index(); + target_c3_state_ = this->DeclareAbstractOutputPort( + "c3_target_output", default_c3_state, + &C3StateSender::OutputTargetState) + .get_index(); + actual_c3_state_ = this->DeclareAbstractOutputPort( + "c3_actual_output", default_c3_state, + &C3StateSender::OutputActualState) + .get_index(); +} + +void C3StateSender::OutputFinalTargetState( + const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const { + const auto final_target_state = this->EvalVectorInput(context, final_target_state_); + DRAKE_DEMAND(final_target_state->size() == n_x_); + output->utime = context.get_time() * 1e6; + for (int i = 0; i < n_x_; ++i) { + output->state[i] = static_cast(final_target_state->GetAtIndex(i)); + } +} + +void C3StateSender::OutputTargetState( + const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const { + const auto target_state = this->EvalVectorInput(context, target_state_); + DRAKE_DEMAND(target_state->size() == n_x_); + output->utime = context.get_time() * 1e6; + for (int i = 0; i < n_x_; ++i) { + output->state[i] = static_cast(target_state->GetAtIndex(i)); + } +} + +void C3StateSender::OutputActualState( + const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const { + const auto actual_state = (TimestampedVector*)this->EvalVectorInput(context, actual_state_); + DRAKE_DEMAND(actual_state->get_data().size() == n_x_); + output->utime = context.get_time() * 1e6; + for (int i = 0; i < n_x_; ++i) { + output->state[i] = static_cast(actual_state->GetAtIndex(i)); + } +} +} // namespace systems +} // namespace dairlib diff --git a/systems/senders/c3_state_sender.h b/systems/senders/c3_state_sender.h new file mode 100644 index 0000000000..ee92fd81aa --- /dev/null +++ b/systems/senders/c3_state_sender.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include + +#include "common/find_resource.h" +#include "dairlib/lcmt_c3_state.hpp" + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Outputs a lcmt_timestamped_saved_traj +class C3StateSender : public drake::systems::LeafSystem { + public: + explicit C3StateSender(int state_size, std::vector state_names); + + const drake::systems::InputPort& get_input_port_target_state() const { + return this->get_input_port(target_state_); + } + + const drake::systems::InputPort& get_input_port_final_target_state() const { + return this->get_input_port(final_target_state_); + } + + const drake::systems::InputPort& get_input_port_actual_state() const { + return this->get_input_port(actual_state_); + } + + const drake::systems::OutputPort& get_output_port_final_target_c3_state() + const { + return this->get_output_port(final_target_c3_state_); + } + + const drake::systems::OutputPort& get_output_port_target_c3_state() + const { + return this->get_output_port(target_c3_state_); + } + const drake::systems::OutputPort& get_output_port_actual_c3_state() + const { + return this->get_output_port(actual_c3_state_); + } + + private: + void OutputFinalTargetState(const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const; + + void OutputTargetState(const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const; + + void OutputActualState(const drake::systems::Context& context, + dairlib::lcmt_c3_state* output) const; + + drake::systems::InputPortIndex final_target_state_; + drake::systems::InputPortIndex target_state_; + drake::systems::InputPortIndex actual_state_; + drake::systems::OutputPortIndex final_target_c3_state_; + drake::systems::OutputPortIndex target_c3_state_; + drake::systems::OutputPortIndex actual_c3_state_; + + int n_x_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/senders/sample_buffer_sender.cc b/systems/senders/sample_buffer_sender.cc new file mode 100644 index 0000000000..21403de630 --- /dev/null +++ b/systems/senders/sample_buffer_sender.cc @@ -0,0 +1,77 @@ +#include "sample_buffer_sender.h" + +#include +#include + +#include "common/eigen_utils.h" + +namespace dairlib { +namespace systems { + +using drake::systems::Context; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +SampleBufferSender::SampleBufferSender(int buffer_size, int n_config) + : buffer_size_(buffer_size), n_config_(n_config) { + this->set_name("sample_buffer_sender"); + + MatrixXd sample_buffer = MatrixXd::Zero(buffer_size_, n_config_); + VectorXd cost_buffer = VectorXd::Zero(buffer_size_); + samples_port_ = + this->DeclareAbstractInputPort("sample_buffer_configurations", + drake::Value{sample_buffer}) + .get_index(); + sample_costs_port_ = + this->DeclareAbstractInputPort("sample_buffer_costs", + drake::Value{cost_buffer}) + .get_index(); + + lcm_sample_buffer_output_port_ = + this->DeclareAbstractOutputPort( + "lcmt_sample_buffer", dairlib::lcmt_sample_buffer(), + &SampleBufferSender::OutputSampleBufferLcm) + .get_index(); +} + +void SampleBufferSender::OutputSampleBufferLcm( + const drake::systems::Context& context, + dairlib::lcmt_sample_buffer* output) const { + // Evaluate input ports to get the sample configurations and costs. + const auto& buffer_configurations = + this->EvalInputValue(context, samples_port_); + const auto& buffer_costs = + this->EvalInputValue(context, sample_costs_port_); + + DRAKE_ASSERT(buffer_configurations->rows() == buffer_size_); + DRAKE_ASSERT(buffer_configurations->cols() == n_config_); + DRAKE_ASSERT(buffer_costs->size() == buffer_size_); + + // Count the number of active samples in the buffer. + int n_in_buffer = std::count_if (buffer_costs->begin(), buffer_costs->end(), + [](double cost) { return cost >= 0; }); + + // Convert the Eigen matrices to std::vectors. + std::vector cost_data(buffer_costs->data(), + buffer_costs->data() + buffer_size_); + std::vector> config_data(buffer_size_, + std::vector(n_config_, 0)); + for (int i = 0; i < buffer_size_; i++) { + for (int j = 0; j < n_config_; j++) { + config_data[i][j] = buffer_configurations->row(i)(j); + } + } + + // Set the fields of the LCM message. + output->utime = context.get_time() * 1e6; + output->buffer_length = buffer_size_; + output->num_configurations = n_config_; + output->num_in_buffer = n_in_buffer; + + output->costs.reserve(buffer_size_); + output->costs = cost_data; + output->configurations = config_data; +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/senders/sample_buffer_sender.h b/systems/senders/sample_buffer_sender.h new file mode 100644 index 0000000000..4bf5672451 --- /dev/null +++ b/systems/senders/sample_buffer_sender.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +#include "dairlib/lcmt_sample_buffer.hpp" + +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/lcm/lcm_interface_system.h" + +namespace dairlib { +namespace systems { + +/// Converts sample costs and configurations to LCM type lcmt_sample_buffer +class SampleBufferSender : public drake::systems::LeafSystem { + public: + SampleBufferSender(int buffer_size, int n_config); + + const drake::systems::InputPort& get_input_port_sample_costs() const { + return this->get_input_port(sample_costs_port_); + } + + const drake::systems::InputPort& get_input_port_samples() const { + return this->get_input_port(samples_port_); + } + + const drake::systems::OutputPort& get_output_port_sample_buffer() + const { + return this->get_output_port(lcm_sample_buffer_output_port_); + } + + private: + void OutputSampleBufferLcm(const drake::systems::Context& context, + dairlib::lcmt_sample_buffer* output) const; + + int buffer_size_; + int n_config_; + + drake::systems::InputPortIndex samples_port_; + drake::systems::InputPortIndex sample_costs_port_; + drake::systems::OutputPortIndex lcm_sample_buffer_output_port_; +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/senders/sample_buffer_to_point_cloud.cc b/systems/senders/sample_buffer_to_point_cloud.cc new file mode 100644 index 0000000000..66349f1895 --- /dev/null +++ b/systems/senders/sample_buffer_to_point_cloud.cc @@ -0,0 +1,131 @@ +#include "systems/senders/sample_buffer_to_point_cloud.h" + +#include "dairlib/lcmt_sample_buffer.hpp" +#include "dairlib/lcmt_saved_traj.hpp" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" + +#include "drake/perception/point_cloud.h" +#include "drake/perception/point_cloud_flags.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +using drake::perception::PointCloud; +using drake::systems::Context; + +namespace systems { + +PointCloudFromSampleBuffer::PointCloudFromSampleBuffer() { + this->set_name("PointCloudFromSampleBuffer"); + + lcmt_sample_buffer_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_sample_buffer", drake::Value{}) + .get_index(); + + new_sample_costs_input_port_ = + this->DeclareAbstractInputPort( + "new_sample_costs", + drake::Value{}) + .get_index(); + + point_cloud_output_port_ = + this->DeclareAbstractOutputPort( + "sample_buffer_point_cloud", PointCloud(), + &PointCloudFromSampleBuffer::OutputSampleBufferAsPointCloud) + .get_index(); + + color_floats_ = colorFloatMap(); + RGBs_ = RGBMap(); +} + +void PointCloudFromSampleBuffer::OutputSampleBufferAsPointCloud( + const drake::systems::Context& context, + PointCloud* sample_buffer_point_cloud) const { + if (!sample_buffer_point_cloud->HasExactFields( + drake::perception::pc_flags::kXYZs | + drake::perception::pc_flags::kRGBs)) { + sample_buffer_point_cloud->SetFields(drake::perception::pc_flags::kXYZs | + drake::perception::pc_flags::kRGBs); + } + + // Evaluate input ports to get the sample buffer contents and current location + // C3 cost. + const auto& sample_buffer_lcmt = + this->EvalInputValue( + context, lcmt_sample_buffer_input_port_); + const auto& new_sample_costs_traj_lcmt = + this->EvalInputValue( + context, new_sample_costs_input_port_); + + Eigen::Matrix3Xf ee_samples; + Eigen::Matrix3Xi rgbs; + + // Check if it's too early to output or if there are no samples in the buffer. + if ((sample_buffer_lcmt->utime < 1e-3) || + (sample_buffer_lcmt->num_in_buffer < 1)) { + ee_samples = Eigen::Matrix3Xf::Zero(3, 1); + rgbs = Eigen::Matrix3Xi::Zero(3, 1); + } else { + // Extract the end effector locations out of the configurations and the + // samples' associated costs. + int n_in_buffer = sample_buffer_lcmt->num_in_buffer; + ee_samples = Eigen::Matrix3Xf::Zero(3, n_in_buffer); + Eigen::VectorXf costs = Eigen::VectorXf::Zero(n_in_buffer); + + for (int sample_i = 0; sample_i < n_in_buffer; sample_i++) { + costs[sample_i] = sample_buffer_lcmt->costs[sample_i]; + std::vector configuration_i = + sample_buffer_lcmt->configurations[sample_i]; + for (int ee_i = 0; ee_i < 3; ee_i++) { + ee_samples(ee_i, sample_i) = configuration_i[ee_i]; + } + } + + // Normalize the costs to be between 0 and 1. + if (costs.maxCoeff() == costs.minCoeff()) { + rgbs = Eigen::Matrix3Xi::Zero(3, n_in_buffer); + } else { + Eigen::VectorXf normalized_costs = (costs.array() - costs.minCoeff()) / + (costs.maxCoeff() - costs.minCoeff()); + + // Get the current sample cost. + auto lcm_traj = LcmTrajectory(new_sample_costs_traj_lcmt->saved_traj); + auto lcm_sample_costs_traj = lcm_traj.GetTrajectory("sample_costs"); + Eigen::MatrixXd sample_costs = lcm_sample_costs_traj.datapoints.row(0); + double current_cost = sample_costs(0); + + double normalized_current_cost = (current_cost - costs.minCoeff()) / + (costs.maxCoeff() - costs.minCoeff()); + + // Make the current cost the centered color. + Eigen::VectorXf scaled_costs = + normalized_costs.array() - normalized_current_cost + 0.5; + + // Apply the color map. + Eigen::MatrixXf differences = + (scaled_costs.replicate(1, n_colors_) - + color_floats_.transpose().replicate(n_in_buffer, 1)) + .cwiseAbs(); + Eigen::VectorXi closest_color_indices(n_in_buffer); + for (int i = 0; i < n_in_buffer; i++) { + int idx; + differences.row(i).minCoeff(&idx); + closest_color_indices[i] = idx; + } + rgbs = Eigen::Matrix3Xi::Zero(3, n_in_buffer); + for (int i = 0; i < n_in_buffer; i++) { + rgbs.col(i) = RGBs_.row(closest_color_indices(i)); + } + } + } + + // Output as a point cloud where the colors reflect costs. + sample_buffer_point_cloud->resize(ee_samples.cols()); + sample_buffer_point_cloud->mutable_xyzs() = ee_samples; + sample_buffer_point_cloud->mutable_rgbs() = rgbs.cast(); +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/senders/sample_buffer_to_point_cloud.h b/systems/senders/sample_buffer_to_point_cloud.h new file mode 100644 index 0000000000..9f64bf567a --- /dev/null +++ b/systems/senders/sample_buffer_to_point_cloud.h @@ -0,0 +1,118 @@ +#include + +#include + +#include +#include + +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { + +using drake::perception::PointCloud; + +namespace systems { + +class PointCloudFromSampleBuffer : public drake::systems::LeafSystem { + // This system reads the sample_buffer over lcm and outputs a Drake PointCloud + // for visualization. + public: + PointCloudFromSampleBuffer(); + + // Input ports + const drake::systems::InputPort& get_input_port_lcmt_sample_buffer() + const { + return this->get_input_port(lcmt_sample_buffer_input_port_); + } + const drake::systems::InputPort& get_input_port_new_sample_costs() + const { + return this->get_input_port(new_sample_costs_input_port_); + } + + // Output port + const drake::systems::OutputPort& + get_output_port_sample_buffer_point_cloud() const { + return this->get_output_port(point_cloud_output_port_); + } + + private: + void OutputSampleBufferAsPointCloud( + const drake::systems::Context& context, + PointCloud* sample_buffer_point_cloud) const; + + drake::systems::InputPortIndex new_sample_costs_input_port_; + drake::systems::InputPortIndex lcmt_sample_buffer_input_port_; + drake::systems::OutputPortIndex point_cloud_output_port_; + + Eigen::VectorXf color_floats_; + Eigen::MatrixXi RGBs_; + + const int n_colors_ = 100; + + // Generates samples of a matplotlib colormap RdYlGn.reversed() for coloring + // the point cloud. + inline Eigen::VectorXf colorFloatMap() { + Eigen::VectorXf color_floats(n_colors_); + color_floats << 0.0, 0.010101010101010102, 0.020202020202020204, + 0.030303030303030304, 0.04040404040404041, 0.050505050505050504, + 0.06060606060606061, 0.0707070707070707, 0.08080808080808081, + 0.09090909090909091, 0.10101010101010101, 0.1111111111111111, + 0.12121212121212122, 0.13131313131313133, 0.1414141414141414, + 0.15151515151515152, 0.16161616161616163, 0.1717171717171717, + 0.18181818181818182, 0.1919191919191919, 0.20202020202020202, + 0.21212121212121213, 0.2222222222222222, 0.23232323232323232, + 0.24242424242424243, 0.25252525252525254, 0.26262626262626265, + 0.2727272727272727, 0.2828282828282828, 0.29292929292929293, + 0.30303030303030304, 0.31313131313131315, 0.32323232323232326, + 0.3333333333333333, 0.3434343434343434, 0.35353535353535354, + 0.36363636363636365, 0.37373737373737376, 0.3838383838383838, + 0.3939393939393939, 0.40404040404040403, 0.41414141414141414, + 0.42424242424242425, 0.43434343434343436, 0.4444444444444444, + 0.45454545454545453, 0.46464646464646464, 0.47474747474747475, + 0.48484848484848486, 0.494949494949495, 0.5050505050505051, + 0.5151515151515151, 0.5252525252525253, 0.5353535353535354, + 0.5454545454545454, 0.5555555555555556, 0.5656565656565656, + 0.5757575757575758, 0.5858585858585859, 0.5959595959595959, + 0.6060606060606061, 0.6161616161616161, 0.6262626262626263, + 0.6363636363636364, 0.6464646464646465, 0.6565656565656566, + 0.6666666666666666, 0.6767676767676768, 0.6868686868686869, + 0.696969696969697, 0.7070707070707071, 0.7171717171717171, + 0.7272727272727273, 0.7373737373737373, 0.7474747474747475, + 0.7575757575757576, 0.7676767676767676, 0.7777777777777778, + 0.7878787878787878, 0.797979797979798, 0.8080808080808081, + 0.8181818181818182, 0.8282828282828283, 0.8383838383838383, + 0.8484848484848485, 0.8585858585858586, 0.8686868686868687, + 0.8787878787878788, 0.8888888888888888, 0.898989898989899, + 0.9090909090909091, 0.9191919191919192, 0.9292929292929293, + 0.9393939393939394, 0.9494949494949495, 0.9595959595959596, + 0.9696969696969697, 0.9797979797979798, 0.98989898989899, 1.0; + return color_floats; + } + inline Eigen::MatrixXi RGBMap() { + Eigen::MatrixXi mat(n_colors_, 3); + mat << 0, 104, 55, 2, 108, 57, 5, 113, 60, 7, 117, 62, 10, 123, 65, 12, 127, + 67, 15, 132, 70, 18, 138, 73, 20, 142, 75, 23, 147, 78, 25, 151, 80, 33, + 156, 82, 42, 160, 84, 48, 163, 86, 57, 167, 88, 63, 170, 89, 72, 174, + 92, 78, 177, 93, 87, 182, 95, 96, 186, 98, 102, 189, 99, 110, 192, 100, + 115, 194, 100, 122, 198, 101, 130, 201, 102, 135, 203, 103, 142, 207, + 103, 147, 209, 104, 155, 212, 105, 160, 214, 105, 167, 217, 107, 173, + 220, 111, 177, 222, 113, 183, 224, 117, 187, 226, 120, 193, 229, 123, + 199, 231, 127, 203, 233, 130, 209, 236, 134, 213, 237, 136, 218, 240, + 141, 223, 242, 147, 226, 243, 151, 230, 245, 157, 233, 246, 161, 238, + 248, 168, 241, 249, 172, 245, 251, 178, 250, 253, 184, 253, 254, 188, + 255, 253, 188, 255, 251, 184, 255, 247, 178, 255, 243, 172, 255, 241, + 168, 254, 237, 161, 254, 235, 157, 254, 231, 151, 254, 229, 147, 254, + 225, 141, 254, 220, 136, 254, 216, 132, 254, 210, 127, 254, 206, 124, + 254, 200, 119, 253, 195, 114, 253, 191, 111, 253, 185, 106, 253, 181, + 103, 253, 175, 98, 252, 168, 94, 251, 163, 92, 250, 155, 88, 250, 150, + 86, 249, 142, 82, 248, 137, 80, 247, 129, 76, 246, 122, 73, 245, 117, + 71, 244, 109, 67, 242, 104, 65, 238, 97, 62, 235, 90, 58, 233, 85, 56, + 229, 78, 53, 227, 73, 51, 224, 66, 47, 221, 61, 45, 218, 54, 42, 214, + 47, 39, 210, 43, 39, 204, 38, 39, 200, 34, 39, 194, 28, 39, 189, 23, 38, + 185, 19, 38, 179, 13, 38, 175, 9, 38, 169, 4, 38, 165, 0, 38; + return mat; + } +}; + +} // namespace systems +} // namespace dairlib diff --git a/systems/system_utils.cc b/systems/system_utils.cc index 3ed7c3d19e..783d82db8c 100644 --- a/systems/system_utils.cc +++ b/systems/system_utils.cc @@ -8,7 +8,12 @@ namespace dairlib { void DrawAndSaveDiagramGraph(const drake::systems::Diagram& diagram, std::string path) { // Default path - if (path.empty()) path = "../" + diagram.get_name(); + if (path.empty()) { + path = "../diagrams/" + diagram.get_name(); + } + + // Create the directory if it does not exist + std::system("mkdir -p ../diagrams"); // Save Graphviz string to a file std::ofstream out(path); @@ -19,7 +24,7 @@ void DrawAndSaveDiagramGraph(const drake::systems::Diagram& diagram, // The command is `dot -Tps input_file -o output_file` std::regex r(" "); path = std::regex_replace(path, r, "\\ "); - std::string cmd = "dot -Tps " + path + " -o " + path + ".ps"; + std::string cmd = "dot -Tsvg " + path + " -o " + path + ".svg"; (void) std::system(cmd.c_str()); // Remove Graphviz string file diff --git a/systems/trajectory_optimization/BUILD.bazel b/systems/trajectory_optimization/BUILD.bazel index 0926507e40..a8bcc809ff 100644 --- a/systems/trajectory_optimization/BUILD.bazel +++ b/systems/trajectory_optimization/BUILD.bazel @@ -45,6 +45,39 @@ cc_library( ], ) +cc_library( + name = "lcm_trajectory_systems", + srcs = [ + "lcm_trajectory_systems.cc", + ], + hdrs = [ + "lcm_trajectory_systems.h", + ], + deps = [ + "//common:eigen_utils", + "//common:find_resource", + "//lcm:lcm_trajectory_saver", + "//multibody:multipose_visualizer", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "c3_output_systems", + srcs = [ + "c3_output_systems.cc", + ], + hdrs = [ + "c3_output_systems.h", + ], + deps = [ + "//common:eigen_utils", + "//lcmtypes:lcmt_robot", + "//solvers:c3_output", + "@drake//:drake_shared_library", + ], +) + cc_binary( name = "passive_constrained_pendulum_dircon", srcs = ["test/passive_constrained_pendulum_dircon.cc"], diff --git a/systems/trajectory_optimization/c3_output_systems.cc b/systems/trajectory_optimization/c3_output_systems.cc new file mode 100644 index 0000000000..61691633e3 --- /dev/null +++ b/systems/trajectory_optimization/c3_output_systems.cc @@ -0,0 +1,111 @@ +#include "c3_output_systems.h" + +#include "common/eigen_utils.h" +#include "dairlib/lcmt_force.hpp" +#include "solvers/c3_output.h" + +namespace dairlib { +namespace systems { + +using drake::systems::Context; +using drake::systems::DiscreteValues; +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::VectorXd; + +C3OutputSender::C3OutputSender() { + c3_solution_port_ = + this->DeclareAbstractInputPort("c3_solution", + drake::Value{}) + .get_index(); + c3_intermediates_port_ = + this->DeclareAbstractInputPort("c3_intermediates", + drake::Value{}) + .get_index(); + lcs_contact_info_port_ = + this->DeclareAbstractInputPort("J_lcs, p_lcs", drake::Value>>()) + .get_index(); + + this->set_name("c3_output_sender"); + lcm_c3_output_port_ = this->DeclareAbstractOutputPort( + "lcmt_c3_output", dairlib::lcmt_c3_output(), + &C3OutputSender::OutputC3Lcm) + .get_index(); + lcs_forces_output_port_ = this->DeclareAbstractOutputPort( + "lcmt_c3_force", dairlib::lcmt_c3_forces(), + &C3OutputSender::OutputC3Forces) + .get_index(); +} + +void C3OutputSender::OutputC3Lcm(const drake::systems::Context& context, + dairlib::lcmt_c3_output* output) const { + const auto& c3_solution = + this->EvalInputValue(context, c3_solution_port_); + const auto& c3_intermediates = + this->EvalInputValue(context, + c3_intermediates_port_); + + C3Output c3_output = C3Output(*c3_solution, *c3_intermediates); + *output = c3_output.GenerateLcmObject(context.get_time()); +} + +void C3OutputSender::OutputC3Forces( + const drake::systems::Context& context, + dairlib::lcmt_c3_forces* c3_forces_output) const { + const auto& c3_solution = + this->EvalInputValue(context, c3_solution_port_); + const auto& contact_info = + this->EvalInputValue>>(context, lcs_contact_info_port_); + MatrixXd J_c = contact_info->first; + int contact_force_start = c3_solution->lambda_sol_.rows() - J_c.rows(); + bool using_stewart_and_trinkle_model = contact_force_start > 0; + + auto contact_points = contact_info->second; + int forces_per_contact = contact_info->first.rows() / contact_points.size(); + + c3_forces_output->num_forces = forces_per_contact * contact_points.size(); + c3_forces_output->forces.resize(c3_forces_output->num_forces); + + int contact_var_start; + int force_index; + for (int contact_index = 0; contact_index < contact_points.size(); + ++contact_index) { + contact_var_start = contact_force_start + forces_per_contact * contact_index; + force_index = forces_per_contact * contact_index; + for (int i = 0; i < forces_per_contact; ++i) { + int contact_jacobian_row = force_index + i; // index for anitescu model + int contact_var_index = contact_var_start + i; + if (using_stewart_and_trinkle_model){ // index for stweart and trinkle model + if (i == 0){ + contact_jacobian_row = contact_index; + contact_var_index = contact_force_start + contact_index; + } else { + contact_jacobian_row = contact_points.size() + (forces_per_contact - 1) * contact_index + i - 1; + contact_var_index = contact_force_start + contact_points.size() + (forces_per_contact - 1) * contact_index + i - 1; + } + } + auto force = lcmt_force(); + force.contact_point[0] = contact_points.at(contact_index)[0]; + force.contact_point[1] = contact_points.at(contact_index)[1]; + force.contact_point[2] = contact_points.at(contact_index)[2]; + // TODO(yangwill): find a cleaner way to figure out the equivalent forces + // VISUALIZING FORCES FOR THE FIRST KNOT POINT + // 6, 7, 8 are the indices for the x,y,z components of the tray + // expressed in the world frame + force.contact_force[0] = + c3_solution->lambda_sol_(contact_var_index, 0) * + J_c.row(contact_jacobian_row)(6); + force.contact_force[1] = + c3_solution->lambda_sol_(contact_var_index, 0) * + J_c.row(contact_jacobian_row)(7); + force.contact_force[2] = + c3_solution->lambda_sol_(contact_var_index, 0) * + J_c.row(contact_jacobian_row)(8); + c3_forces_output->forces[force_index + i] = force; + } + } + c3_forces_output->utime = context.get_time() * 1e6; +} + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/trajectory_optimization/c3_output_systems.h b/systems/trajectory_optimization/c3_output_systems.h new file mode 100644 index 0000000000..1975721240 --- /dev/null +++ b/systems/trajectory_optimization/c3_output_systems.h @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#include "dairlib/lcmt_c3_forces.hpp" +#include "dairlib/lcmt_c3_output.hpp" + +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/lcm/lcm_interface_system.h" + +namespace dairlib { +namespace systems { + +/// Converts a OutputVector object to LCM type lcmt_robot_output +class C3OutputSender : public drake::systems::LeafSystem { + public: + C3OutputSender(); + + const drake::systems::InputPort& get_input_port_c3_solution() const { + return this->get_input_port(c3_solution_port_); + } + + const drake::systems::InputPort& get_input_port_c3_intermediates() + const { + return this->get_input_port(c3_intermediates_port_); + } + + const drake::systems::InputPort& get_input_port_lcs_contact_info() + const { + return this->get_input_port(lcs_contact_info_port_); + } + + const drake::systems::OutputPort& get_output_port_c3_debug() const { + return this->get_output_port(lcm_c3_output_port_); + } + + // LCS contact force, not actor input forces + const drake::systems::OutputPort& get_output_port_c3_force() const { + return this->get_output_port(lcs_forces_output_port_); + } + +// // LCS actor input force +// const drake::systems::OutputPort& get_output_port_next_c3_input() const { +// return this->get_output_port(lcs_inputs_output_port_); +// } + + private: + void OutputC3Lcm(const drake::systems::Context& context, + dairlib::lcmt_c3_output* output) const; + + void OutputC3Forces(const drake::systems::Context& context, + dairlib::lcmt_c3_forces* c3_forces_output) const; + +// void OutputNextC3Input(const drake::systems::Context& context, +// drake::systems::BasicVector* u_next) const; + + drake::systems::InputPortIndex c3_solution_port_; + drake::systems::InputPortIndex c3_intermediates_port_; + drake::systems::InputPortIndex lcs_contact_info_port_; + drake::systems::OutputPortIndex lcm_c3_output_port_; + drake::systems::OutputPortIndex lcs_forces_output_port_; +// drake::systems::OutputPortIndex lcs_inputs_output_port_; +}; + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/trajectory_optimization/dircon/dircon.h b/systems/trajectory_optimization/dircon/dircon.h index 5b450cf4c0..7b8ccc8280 100644 --- a/systems/trajectory_optimization/dircon/dircon.h +++ b/systems/trajectory_optimization/dircon/dircon.h @@ -203,6 +203,8 @@ class Dircon return mode_sequence_.mode(mode); } + const int get_mode_start(int index) { return mode_start_[index]; } + const drake::systems::Context& get_context(int mode, int knotpoint_index) { return *contexts_.at(mode).at(knotpoint_index); } diff --git a/systems/trajectory_optimization/dircon/test/passive_constrained_pendulum_dircon.cc b/systems/trajectory_optimization/dircon/test/passive_constrained_pendulum_dircon.cc index e7f519360b..a6c6349469 100644 --- a/systems/trajectory_optimization/dircon/test/passive_constrained_pendulum_dircon.cc +++ b/systems/trajectory_optimization/dircon/test/passive_constrained_pendulum_dircon.cc @@ -66,10 +66,10 @@ void runDircon() { Parser parser(&plant_double); Parser parser_vis(&plant_vis, &scene_graph); - parser.AddModelFromFile(urdf_path); + parser.AddModels(urdf_path); plant_double.Finalize(); - parser_vis.AddModelFromFile(urdf_path); + parser_vis.AddModels(urdf_path); plant_vis.Finalize(); std::unique_ptr> plant_pointer; diff --git a/systems/trajectory_optimization/dircon_opt_constraints.cc b/systems/trajectory_optimization/dircon_opt_constraints.cc index 4d0f6866df..f382e347f7 100644 --- a/systems/trajectory_optimization/dircon_opt_constraints.cc +++ b/systems/trajectory_optimization/dircon_opt_constraints.cc @@ -53,10 +53,13 @@ DirconDynamicConstraint::DirconDynamicConstraint( // is located at the first four element of the generalized position if (is_quaternion) { map positions_map = multibody::MakeNameToPositionsMap(plant); - DRAKE_DEMAND(positions_map.at("base_qw") == 0); - DRAKE_DEMAND(positions_map.at("base_qx") == 1); - DRAKE_DEMAND(positions_map.at("base_qy") == 2); - DRAKE_DEMAND(positions_map.at("base_qz") == 3); + auto floating_bodies = plant.GetFloatingBaseBodies(); + const auto& body = plant.get_body(*floating_bodies.begin()); + std::string name = body.name(); + DRAKE_DEMAND(positions_map.at(name + "_qw") == 0); + DRAKE_DEMAND(positions_map.at(name + "_qx") == 1); + DRAKE_DEMAND(positions_map.at(name + "_qy") == 2); + DRAKE_DEMAND(positions_map.at(name + "_qz") == 3); } } diff --git a/systems/trajectory_optimization/lcm_trajectory_systems.cc b/systems/trajectory_optimization/lcm_trajectory_systems.cc new file mode 100644 index 0000000000..6b097023dd --- /dev/null +++ b/systems/trajectory_optimization/lcm_trajectory_systems.cc @@ -0,0 +1,133 @@ +#include "lcm_trajectory_systems.h" + +#include + + +#include "common/eigen_utils.h" +#include "common/find_resource.h" +#include "dairlib/lcmt_timestamped_saved_traj.hpp" + +#include "drake/common/schema/rotation.h" +#include "drake/geometry/rgba.h" + +namespace dairlib { +namespace systems { + +using drake::geometry::Rgba; +using drake::math::RigidTransformd; +using drake::math::RotationMatrixd; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::VectorXd; + +LcmTrajectoryReceiver::LcmTrajectoryReceiver(std::string trajectory_name) + : trajectory_name_(std::move(trajectory_name)) { + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + PiecewisePolynomial empty_pp_traj(Eigen::VectorXd(0)); + Trajectory& traj_inst = empty_pp_traj; + this->set_name(trajectory_name_); + trajectory_output_port_ = + this->DeclareAbstractOutputPort(trajectory_name_, traj_inst, + &LcmTrajectoryReceiver::OutputTrajectory) + .get_index(); +} + +void LcmTrajectoryReceiver::OutputTrajectory( + const drake::systems::Context& context, + Trajectory* traj) const { + auto* casted_traj = + (PiecewisePolynomial*)dynamic_cast*>( + traj); + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime > 1e-3) { + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); + *casted_traj = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints); + if (trajectory_block.datapoints.rows() == 3) { + *casted_traj = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints); + } else { + *casted_traj = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints.topRows(trajectory_block.datapoints.rows() / 2)); +// *casted_traj = PiecewisePolynomial::CubicHermite( +// trajectory_block.time_vector, trajectory_block.datapoints.topRows(3), +// trajectory_block.datapoints.bottomRows(3)); + } + } else { + *casted_traj = PiecewisePolynomial(Vector3d::Zero()); + } +} + +LcmOrientationTrajectoryReceiver::LcmOrientationTrajectoryReceiver( + std::string trajectory_name) + : trajectory_name_(std::move(trajectory_name)) { + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + PiecewiseQuaternionSlerp empty_slerp_traj; + Trajectory& traj_inst = empty_slerp_traj; + this->set_name(trajectory_name_); + trajectory_output_port_ = + this->DeclareAbstractOutputPort( + trajectory_name_, traj_inst, + &LcmOrientationTrajectoryReceiver::OutputTrajectory) + .get_index(); +} + +void LcmOrientationTrajectoryReceiver::OutputTrajectory( + const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const { + auto* casted_traj = (PiecewiseQuaternionSlerp*)dynamic_cast< + PiecewiseQuaternionSlerp*>(traj); + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime > 1e-3) { + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + try { + lcm_traj.GetTrajectory(trajectory_name_); + } catch (std::exception& e) { + std::cerr << "Make sure the planner is sending orientation" << std::endl; + throw std::out_of_range(""); + } + const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); + + std::vector> quaternion_datapoints; + for (int i = 0; i < trajectory_block.datapoints.cols(); ++i) { + quaternion_datapoints.push_back( + drake::math::RollPitchYaw(trajectory_block.datapoints.col(i)) + .ToQuaternion()); + } + *casted_traj = PiecewiseQuaternionSlerp( + CopyVectorXdToStdVector(trajectory_block.time_vector), + quaternion_datapoints); + } else { + *casted_traj = drake::trajectories::PiecewiseQuaternionSlerp( + {0, 1}, + {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)}); + } +} + +} // namespace systems +} // namespace dairlib diff --git a/systems/trajectory_optimization/lcm_trajectory_systems.h b/systems/trajectory_optimization/lcm_trajectory_systems.h new file mode 100644 index 0000000000..bc46dc988e --- /dev/null +++ b/systems/trajectory_optimization/lcm_trajectory_systems.h @@ -0,0 +1,69 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "dairlib/lcmt_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" +#include "multibody/multipose_visualizer.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and outputs it as a drake PiecewisePolynomial. +class LcmTrajectoryReceiver : public drake::systems::LeafSystem { + public: + explicit LcmTrajectoryReceiver(std::string trajectory_name); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + const drake::systems::OutputPort& get_output_port_trajectory() const { + return this->get_output_port(trajectory_output_port_); + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::InputPortIndex trajectory_input_port_; + drake::systems::OutputPortIndex trajectory_output_port_; + const std::string trajectory_name_; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and outputs it as a drake PiecewisePolynomial. +class LcmOrientationTrajectoryReceiver + : public drake::systems::LeafSystem { + public: + explicit LcmOrientationTrajectoryReceiver(std::string trajectory_name); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + const drake::systems::OutputPort& get_output_port_trajectory() const { + return this->get_output_port(trajectory_output_port_); + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + drake::systems::InputPortIndex trajectory_input_port_; + drake::systems::OutputPortIndex trajectory_output_port_; + const std::string trajectory_name_; +}; + + +} // namespace systems +} // namespace dairlib diff --git a/systems/trajectory_optimization/test/passive_constrained_pendulum_dircon.cc b/systems/trajectory_optimization/test/passive_constrained_pendulum_dircon.cc index e5d3dcebc0..b5df5cfa93 100644 --- a/systems/trajectory_optimization/test/passive_constrained_pendulum_dircon.cc +++ b/systems/trajectory_optimization/test/passive_constrained_pendulum_dircon.cc @@ -56,7 +56,7 @@ void runDircon() { SceneGraph& scene_graph = *builder.AddSystem(); Parser parser(&plant, &scene_graph); - parser.AddModelFromFile(sdf_path); + parser.AddModels(sdf_path); plant.WeldFrames( plant.world_frame(), plant.GetFrameByName("base_link"), diff --git a/systems/visualization/BUILD.bazel b/systems/visualization/BUILD.bazel new file mode 100644 index 0000000000..34d0ffcff7 --- /dev/null +++ b/systems/visualization/BUILD.bazel @@ -0,0 +1,32 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "lcm_visualization_systems", + srcs = [ + "lcm_visualization_systems.cc", + ], + hdrs = [ + "lcm_visualization_systems.h", + ], + deps = [ + "//common:eigen_utils", + "//common:find_resource", + "//lcm:lcm_trajectory_saver", + "//multibody:multipose_visualizer", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "meshcat_dynamic_camera", + srcs = ["meshcat_dynamic_camera.cc"], + hdrs = ["meshcat_dynamic_camera.h"], + deps = [ + "//common", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) diff --git a/systems/visualization/lcm_visualization_systems.cc b/systems/visualization/lcm_visualization_systems.cc new file mode 100644 index 0000000000..95ada3f671 --- /dev/null +++ b/systems/visualization/lcm_visualization_systems.cc @@ -0,0 +1,517 @@ +#include "lcm_visualization_systems.h" + +#include +#include +#include + +#include "common/eigen_utils.h" + +#include "drake/common/schema/rotation.h" +#include "drake/geometry/rgba.h" + +namespace dairlib { +namespace systems { + +using drake::geometry::Rgba; +using drake::math::RigidTransformd; +using drake::math::RotationMatrixd; +using drake::systems::Context; +using drake::systems::DiscreteValues; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; +using Eigen::MatrixXd; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::VectorXd; + +LcmTrajectoryDrawer::LcmTrajectoryDrawer( + const std::shared_ptr& meshcat, + std::string trajectory_name, const std::string& system_name) + : meshcat_(meshcat), + trajectory_name_(std::move(trajectory_name)), + system_name_(std::move(system_name)) { + this->set_name("LcmTrajectoryDrawer: " + system_name_ + trajectory_name_); + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + DeclarePerStepDiscreteUpdateEvent(&LcmTrajectoryDrawer::DrawTrajectory); +} + +drake::systems::EventStatus LcmTrajectoryDrawer::DrawTrajectory( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + const auto& trajectory_block = lcm_traj.GetTrajectory(trajectory_name_); + MatrixXd line_points = MatrixXd::Zero(3, N_); + VectorXd breaks = + VectorXd::LinSpaced(N_, trajectory_block.time_vector[0], + trajectory_block.time_vector.tail(1)[0]); + if (trajectory_block.datapoints.rows() == 3) { + auto trajectory = PiecewisePolynomial::FirstOrderHold( + trajectory_block.time_vector, trajectory_block.datapoints); + for (int i = 0; i < line_points.cols(); ++i) { + line_points.col(i) = trajectory.value(breaks(i)); + } + } else { + auto trajectory = PiecewisePolynomial::CubicHermite( + trajectory_block.time_vector, trajectory_block.datapoints.topRows(3), + trajectory_block.datapoints.bottomRows(3)); + for (int i = 0; i < line_points.cols(); ++i) { + line_points.col(i) = trajectory.value(breaks(i)); + } + } + + DRAKE_DEMAND(line_points.rows() == 3); + meshcat_->SetLine("/trajectories/" + system_name_ + trajectory_name_, + line_points, line_width_, rgba_); + return drake::systems::EventStatus::Succeeded(); +} + +LcmPoseDrawer::LcmPoseDrawer( + const std::shared_ptr& meshcat, + const std::string& model_file, + const std::string& translation_trajectory_name, + const std::string& orientation_trajectory_name, + const std::string& system_name, + int num_poses, + bool add_transparency, + const Eigen::VectorXd& rgb) + : meshcat_(meshcat), + translation_trajectory_name_(translation_trajectory_name), + orientation_trajectory_name_(orientation_trajectory_name), + N_(num_poses) { + this->set_name("LcmPoseDrawer: " + system_name + translation_trajectory_name); + + Eigen::VectorXd alpha_scale; + if (add_transparency) { + alpha_scale = 1.0 * VectorXd::LinSpaced(N_, 0.5, 0.1); + } else { + alpha_scale = 1.0 * VectorXd::Ones(N_); + } + + multipose_visualizer_ = std::make_unique( + model_file, N_, alpha_scale, "", meshcat, system_name, rgb); + trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj", + drake::Value{}) + .get_index(); + + DeclarePerStepDiscreteUpdateEvent(&LcmPoseDrawer::DrawTrajectory); +} + +drake::systems::EventStatus LcmPoseDrawer::DrawTrajectory( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& lcmt_traj = + this->EvalInputValue( + context, trajectory_input_port_); + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + MatrixXd object_poses = MatrixXd::Zero(7, N_); + + const auto& lcm_translation_traj = + lcm_traj.GetTrajectory(translation_trajectory_name_); + auto translation_trajectory = PiecewisePolynomial::CubicHermite( + lcm_translation_traj.time_vector, + lcm_translation_traj.datapoints.topRows(3), + lcm_translation_traj.datapoints.bottomRows(3)); + auto orientation_trajectory = PiecewiseQuaternionSlerp( + {0, 1}, {Eigen::Quaterniond(1, 0, 0, 0), Eigen::Quaterniond(1, 0, 0, 0)}); + + if (lcm_traj.HasTrajectory(orientation_trajectory_name_)) { + const auto& lcm_orientation_traj = + lcm_traj.GetTrajectory(orientation_trajectory_name_); + std::vector> quaternion_datapoints; + for (int i = 0; i < lcm_orientation_traj.datapoints.cols(); ++i) { + VectorXd orientation_sample = lcm_orientation_traj.datapoints.col(i); + if (orientation_sample.isZero()) { + quaternion_datapoints.push_back(Quaterniond(1, 0, 0, 0)); + } else { + quaternion_datapoints.push_back( + Quaterniond(orientation_sample[0], orientation_sample[1], + orientation_sample[2], orientation_sample[3])); + } + } + orientation_trajectory = PiecewiseQuaternionSlerp( + CopyVectorXdToStdVector(lcm_orientation_traj.time_vector), + quaternion_datapoints); + } + + // ASSUMING orientation and translation trajectories have the same breaks. + // This recreates the trajectory using the knot points and then evaluates the + // trajectory at equal intervals based on the parameters. If the num_poses is + // equal to the number of knot points, then the poses will be the same as the + // knot points. + VectorXd translation_breaks = + VectorXd::LinSpaced(N_, lcm_translation_traj.time_vector[0], + lcm_translation_traj.time_vector.tail(1)[0]); + for (int i = 0; i < object_poses.cols(); ++i) { + object_poses.col(i) << orientation_trajectory.value( + translation_breaks(i)), + translation_trajectory.value(translation_breaks(i)); + } + + multipose_visualizer_->DrawPoses(object_poses); + + return drake::systems::EventStatus::Succeeded(); +} + +LcmForceDrawer::LcmForceDrawer( + const std::shared_ptr& meshcat, + std::string actor_trajectory_name, std::string force_trajectory_name, + std::string lcs_force_trajectory_name, const std::string& system_name) + : meshcat_(meshcat), + actor_trajectory_name_(std::move(actor_trajectory_name)), + force_trajectory_name_(std::move(force_trajectory_name)), + lcs_force_trajectory_name_(std::move(lcs_force_trajectory_name)) { + this->set_name("LcmForceDrawer: " + system_name + force_trajectory_name_); + actor_trajectory_input_port_ = + this->DeclareAbstractInputPort( + "lcmt_timestamped_saved_traj: actor", + drake::Value{}) + .get_index(); + + robot_time_input_port_ = + this->DeclareVectorInputPort("t_robot", 1).get_index(); + + force_trajectory_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_forces", + drake::Value{}) + .get_index(); + + meshcat_->SetProperty(force_path_, "visible", true, 0); + + actor_last_update_time_index_ = this->DeclareDiscreteState(1); + forces_last_update_time_index_ = this->DeclareDiscreteState(1); + meshcat_->SetObject(force_path_ + "/u_lcs/arrow/cylinder", cylinder_, + actor_force_color_); + meshcat_->SetObject(force_path_ + "/u_lcs/arrow/head", arrowhead_, + actor_force_color_); + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", false); + + DeclarePerStepDiscreteUpdateEvent(&LcmForceDrawer::DrawForce); + DeclarePerStepDiscreteUpdateEvent(&LcmForceDrawer::DrawForces); +} + +drake::systems::EventStatus LcmForceDrawer::DrawForce( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, actor_trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& lcmt_traj = + this->EvalInputValue( + context, actor_trajectory_input_port_); + + // Don't needlessly update + if (discrete_state->get_value(actor_last_update_time_index_)[0] == + lcmt_traj->utime * 1e-6) { + return drake::systems::EventStatus::Succeeded(); + } + discrete_state->get_mutable_value(actor_last_update_time_index_)[0] = + lcmt_traj->utime * 1e-6; + const auto& robot_time_vec = + this->EvalVectorInput(context, robot_time_input_port_); + double robot_time = robot_time_vec->GetAtIndex(0); + + auto lcm_traj = LcmTrajectory(lcmt_traj->saved_traj); + const auto& force_trajectory_block = + lcm_traj.GetTrajectory(force_trajectory_name_); + const auto& actor_trajectory_block = + lcm_traj.GetTrajectory(actor_trajectory_name_); + auto force_trajectory = PiecewisePolynomial::FirstOrderHold( + force_trajectory_block.time_vector, force_trajectory_block.datapoints); + VectorXd pose; + if (actor_trajectory_block.datapoints.rows() == 3) { + auto trajectory = PiecewisePolynomial::FirstOrderHold( + actor_trajectory_block.time_vector, actor_trajectory_block.datapoints); + pose = trajectory.value(robot_time); + } else { + auto trajectory = PiecewisePolynomial::CubicHermite( + actor_trajectory_block.time_vector, + actor_trajectory_block.datapoints.topRows(3), + actor_trajectory_block.datapoints.bottomRows(3)); + pose = trajectory.value(robot_time); + } + + auto force = force_trajectory.value(robot_time); + const std::string& force_path_root = force_path_ + "/u_lcs/"; + meshcat_->SetTransform(force_path_root, RigidTransformd(pose), context.get_time()); + const std::string& force_arrow_path = force_path_root + "arrow"; + + auto force_norm = force.norm(); + // Stretch the cylinder in z. + if (force_norm >= 0.01) { + meshcat_->SetTransform( + force_arrow_path, + RigidTransformd(RotationMatrixd::MakeFromOneVector(force, 2)), context.get_time()); + const double height = force_norm / newtons_per_meter_; + meshcat_->SetProperty(force_arrow_path + "/cylinder", "position", + {0, 0, 0.5 * height}, context.get_time()); + // Note: Meshcat does not fully support non-uniform scaling (see #18095). + // We get away with it here since there is no rotation on this frame and + // no children in the kinematic tree. + meshcat_->SetProperty(force_arrow_path + "/cylinder", "scale", + {1, 1, height}, context.get_time()); + // Translate the arrowheads. + const double arrowhead_height = radius_ * 2.0; + meshcat_->SetTransform( + force_arrow_path + "/head", + RigidTransformd(RotationMatrixd::MakeXRotation(M_PI), + Vector3d{0, 0, height + arrowhead_height}), context.get_time()); + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", true, context.get_time()); + } else { + meshcat_->SetProperty(force_path_ + "/u_lcs", "visible", false, context.get_time()); + } + return drake::systems::EventStatus::Succeeded(); +} + +drake::systems::EventStatus LcmForceDrawer::DrawForces( + const Context& context, + DiscreteValues* discrete_state) const { + if (this->EvalInputValue( + context, force_trajectory_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + const auto& c3_forces = this->EvalInputValue( + context, force_trajectory_input_port_); + + // Don't needlessly update + if (discrete_state->get_value(forces_last_update_time_index_)[0] == + c3_forces->utime * 1e-6) { + return drake::systems::EventStatus::Succeeded(); + } + discrete_state->get_mutable_value(forces_last_update_time_index_)[0] = + c3_forces->utime * 1e-6; + + for (int i = 0; i < c3_forces->num_forces; ++i) { + const VectorXd force = Eigen::Map( + c3_forces->forces[i].contact_force, 3); + auto force_norm = force.norm(); + const std::string& force_path_root = + force_path_ + "/lcs_force_" + std::to_string(i) + "/"; + if (force_norm >= 0.01) { + if (!meshcat_->HasPath(force_path_root + "arrow/")) { + meshcat_->SetObject(force_path_root + "arrow/cylinder", cylinder_, + contact_force_color_); + meshcat_->SetObject(force_path_root + "arrow/head", arrowhead_, + contact_force_color_); + } + + const VectorXd pose = Eigen::Map( + c3_forces->forces[i].contact_point, 3); + + meshcat_->SetTransform(force_path_root, RigidTransformd(pose), context.get_time()); + // Stretch the cylinder in z. + const std::string& force_arrow_path = force_path_root + "arrow"; + meshcat_->SetTransform( + force_arrow_path, + RigidTransformd(RotationMatrixd::MakeFromOneVector(force, 2)), context.get_time()); + const double height = force_norm / newtons_per_meter_; + meshcat_->SetProperty(force_arrow_path + "/cylinder", "position", + {0, 0, 0.5 * height}, context.get_time()); + // Note: Meshcat does not fully support non-uniform scaling (see + // #18095). We get away with it here since there is no rotation on this + // frame and no children in the kinematic tree. + meshcat_->SetProperty(force_arrow_path + "/cylinder", "scale", + {1, 1, height}, context.get_time()); + // Translate the arrowheads. + const double arrowhead_height = radius_ * 2.0; + meshcat_->SetTransform( + force_arrow_path + "/head", + RigidTransformd(RotationMatrixd::MakeXRotation(M_PI), + Vector3d{0, 0, height + arrowhead_height}), context.get_time()); + meshcat_->SetProperty(force_path_root, "visible", true, context.get_time()); + } else { + meshcat_->SetProperty(force_path_root, "visible", false, context.get_time()); + } + } + return drake::systems::EventStatus::Succeeded(); +} + +LcmC3TargetDrawer::LcmC3TargetDrawer( + const std::shared_ptr& meshcat, bool draw_tray, + bool draw_ee) + : meshcat_(meshcat), draw_tray_(draw_tray), draw_ee_(draw_ee) { + this->set_name("LcmC3TargetDrawer"); + c3_state_final_target_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_state: final_target", + drake::Value{}) + .get_index(); + c3_state_target_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_state: target", + drake::Value{}) + .get_index(); + + c3_state_actual_input_port_ = + this->DeclareAbstractInputPort("lcmt_c3_state: actual", + drake::Value{}) + .get_index(); + last_update_time_index_ = this->DeclareDiscreteState(1); + + meshcat_->SetProperty(c3_state_path_, "visible", true, 0); + + // TODO(yangwill): Clean up all this visualization, move to separate + // visualization directory1 + meshcat_->SetObject(c3_final_target_object_path_ + "/x-axis", cylinder_for_tray_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_final_target_object_path_ + "/y-axis", cylinder_for_tray_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_final_target_object_path_ + "/z-axis", cylinder_for_tray_, + {0, 0, 1, 1}); + meshcat_->SetObject(c3_target_object_path_ + "/x-axis", cylinder_for_tray_, + {1, 0, 0, 0.3}); + meshcat_->SetObject(c3_target_object_path_ + "/y-axis", cylinder_for_tray_, + {0, 1, 0, 0.3}); + meshcat_->SetObject(c3_target_object_path_ + "/z-axis", cylinder_for_tray_, + {0, 0, 1, 0.3}); + meshcat_->SetObject(c3_actual_object_path_ + "/x-axis", cylinder_for_tray_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_actual_object_path_ + "/y-axis", cylinder_for_tray_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_actual_object_path_ + "/z-axis", cylinder_for_tray_, + {0, 0, 1, 1}); + if (draw_ee_){ + meshcat_->SetObject(c3_target_ee_path_ + "/x-axis", cylinder_for_ee_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_target_ee_path_ + "/y-axis", cylinder_for_ee_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_target_ee_path_ + "/z-axis", cylinder_for_ee_, + {0, 0, 1, 1}); + meshcat_->SetObject(c3_actual_ee_path_ + "/x-axis", cylinder_for_ee_, + {1, 0, 0, 1}); + meshcat_->SetObject(c3_actual_ee_path_ + "/y-axis", cylinder_for_ee_, + {0, 1, 0, 1}); + meshcat_->SetObject(c3_actual_ee_path_ + "/z-axis", cylinder_for_ee_, + {0, 0, 1, 1}); + } + auto x_axis_transform = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitY()), + Vector3d{0.05, 0.0, 0.0}); + auto y_axis_transform = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitX()), + Vector3d{0.0, 0.05, 0.0}); + auto z_axis_transform = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitZ()), + Vector3d{0.0, 0.0, 0.05}); + auto x_axis_transform_ee = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitY()), + 0.5 * Vector3d{0.05, 0.0, 0.0}); + auto y_axis_transform_ee = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitX()), + 0.5 * Vector3d{0.0, 0.05, 0.0}); + auto z_axis_transform_ee = + RigidTransformd(Eigen::AngleAxis(0.5 * M_PI, Vector3d::UnitZ()), + 0.5 * Vector3d{0.0, 0.0, 0.05}); + meshcat_->SetTransform(c3_final_target_object_path_ + "/x-axis", x_axis_transform); + meshcat_->SetTransform(c3_final_target_object_path_ + "/y-axis", y_axis_transform); + meshcat_->SetTransform(c3_final_target_object_path_ + "/z-axis", z_axis_transform); + meshcat_->SetTransform(c3_target_object_path_ + "/x-axis", x_axis_transform); + meshcat_->SetTransform(c3_target_object_path_ + "/y-axis", y_axis_transform); + meshcat_->SetTransform(c3_target_object_path_ + "/z-axis", z_axis_transform); + meshcat_->SetTransform(c3_actual_object_path_ + "/x-axis", x_axis_transform); + meshcat_->SetTransform(c3_actual_object_path_ + "/y-axis", y_axis_transform); + meshcat_->SetTransform(c3_actual_object_path_ + "/z-axis", z_axis_transform); + if (draw_ee_){ + meshcat_->SetTransform(c3_target_ee_path_ + "/x-axis", x_axis_transform_ee); + meshcat_->SetTransform(c3_target_ee_path_ + "/y-axis", y_axis_transform_ee); + meshcat_->SetTransform(c3_target_ee_path_ + "/z-axis", z_axis_transform_ee); + meshcat_->SetTransform(c3_actual_ee_path_ + "/x-axis", x_axis_transform_ee); + meshcat_->SetTransform(c3_actual_ee_path_ + "/y-axis", y_axis_transform_ee); + meshcat_->SetTransform(c3_actual_ee_path_ + "/z-axis", z_axis_transform_ee); + } + + DeclarePerStepDiscreteUpdateEvent(&LcmC3TargetDrawer::DrawC3State); +} + +drake::systems::EventStatus LcmC3TargetDrawer::DrawC3State( + const Context& context, + DiscreteValues* discrete_state) const { + // Guarding the final state input port because it is not always connected, + // e.g. examples/franka. + const auto* c3_final_target = this->EvalInputValue( + context, c3_state_final_target_input_port_); + if (!c3_final_target || c3_final_target->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + if (this->EvalInputValue(context, + c3_state_target_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + if (this->EvalInputValue(context, + c3_state_actual_input_port_) + ->utime < 1e-3) { + return drake::systems::EventStatus::Succeeded(); + } + if (discrete_state->get_value(last_update_time_index_)[0] >= + context.get_time()) { + // no need to update if simulation has not advanced + return drake::systems::EventStatus::Succeeded(); + } + discrete_state->get_mutable_value(last_update_time_index_)[0] = + context.get_time(); + const auto& c3_target = this->EvalInputValue( + context, c3_state_target_input_port_); + const auto& c3_actual = this->EvalInputValue( + context, c3_state_actual_input_port_); + if (draw_tray_) { + meshcat_->SetTransform( + c3_final_target_object_path_, + RigidTransformd( + Eigen::Quaterniond(c3_final_target->state[3], + c3_final_target->state[4], + c3_final_target->state[5], + c3_final_target->state[6]), + Vector3d{c3_final_target->state[7], c3_final_target->state[8], + c3_final_target->state[9]})); + meshcat_->SetTransform( + c3_target_object_path_, + RigidTransformd( + Eigen::Quaterniond(c3_target->state[3], c3_target->state[4], + c3_target->state[5], c3_target->state[6]), + Vector3d{c3_target->state[7], c3_target->state[8], + c3_target->state[9]}), context.get_time()); + meshcat_->SetTransform( + c3_actual_object_path_, + RigidTransformd( + Eigen::Quaterniond(c3_actual->state[3], c3_actual->state[4], + c3_actual->state[5], c3_actual->state[6]), + Vector3d{c3_actual->state[7], c3_actual->state[8], + c3_actual->state[9]}), context.get_time()); + } + if (draw_ee_) { + meshcat_->SetTransform( + c3_target_ee_path_, + RigidTransformd(Vector3d{c3_target->state[0], c3_target->state[1], + c3_target->state[2]})); + meshcat_->SetTransform( + c3_actual_ee_path_, + RigidTransformd(Vector3d{c3_actual->state[0], c3_actual->state[1], + c3_actual->state[2]})); + } + return drake::systems::EventStatus::Succeeded(); +} + +} // namespace systems +} // namespace dairlib \ No newline at end of file diff --git a/systems/visualization/lcm_visualization_systems.h b/systems/visualization/lcm_visualization_systems.h new file mode 100644 index 0000000000..0d9773581d --- /dev/null +++ b/systems/visualization/lcm_visualization_systems.h @@ -0,0 +1,194 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "dairlib/lcmt_saved_traj.hpp" +#include "lcm/lcm_trajectory.h" +#include "multibody/multipose_visualizer.h" + +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/systems/framework/leaf_system.h" + +namespace dairlib { +namespace systems { + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class LcmTrajectoryDrawer : public drake::systems::LeafSystem { + public: + explicit LcmTrajectoryDrawer(const std::shared_ptr&, + std::string trajectory_name, + const std::string& system_name = ""); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + void SetLineColor(drake::geometry::Rgba rgba) { rgba_ = rgba; } + void SetLineWidth(double line_width) { line_width_ = line_width; } + + void SetNumSamples(int N) { + DRAKE_DEMAND(N > 1); + N_ = N; + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::EventStatus DrawTrajectory( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex trajectory_input_port_; + std::shared_ptr meshcat_; + const std::string system_name_; + const std::string trajectory_name_; + drake::geometry::Rgba rgba_ = drake::geometry::Rgba(0.1, 0.1, 0.1, 1.0); + double line_width_ = 100; + int N_ = 5; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws the object pose through meshcat. +class LcmPoseDrawer : public drake::systems::LeafSystem { + public: + explicit LcmPoseDrawer(const std::shared_ptr&, + const std::string& model_file, + const std::string& translation_trajectory_name, + const std::string& orientation_trajectory_name, + const std::string& system_name = "", + int num_poses = 5, + bool add_transparency = true, + const Eigen::VectorXd& rgb = Eigen::VectorXd()); + + const drake::systems::InputPort& get_input_port_trajectory() const { + return this->get_input_port(trajectory_input_port_); + } + + private: + void OutputTrajectory(const drake::systems::Context& context, + drake::trajectories::Trajectory* traj) const; + + drake::systems::EventStatus DrawTrajectory( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex trajectory_input_port_; + std::shared_ptr meshcat_; + const std::string translation_trajectory_name_; + const std::string orientation_trajectory_name_; + std::unique_ptr multipose_visualizer_; + const int N_; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class LcmForceDrawer : public drake::systems::LeafSystem { + public: + explicit LcmForceDrawer(const std::shared_ptr&, + std::string force_trajectory_name, + std::string actor_trajectory_name, + std::string lcs_force_trajectory_name, + const std::string& system_name = ""); + + const drake::systems::InputPort& get_input_port_actor_trajectory() + const { + return this->get_input_port(actor_trajectory_input_port_); + } + + const drake::systems::InputPort& get_input_port_robot_time() const { + return this->get_input_port(robot_time_input_port_); + } + + const drake::systems::InputPort& get_input_port_force_trajectory() + const { + return this->get_input_port(force_trajectory_input_port_); + } + + private: + drake::systems::EventStatus DrawForce( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + drake::systems::EventStatus DrawForces( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex actor_trajectory_input_port_; + drake::systems::InputPortIndex robot_time_input_port_; + drake::systems::InputPortIndex force_trajectory_input_port_; + + drake::systems::DiscreteStateIndex actor_last_update_time_index_; + drake::systems::DiscreteStateIndex forces_last_update_time_index_; + std::shared_ptr meshcat_; + const drake::geometry::Cylinder cylinder_ = + drake::geometry::Cylinder(0.002, 1.0); + const drake::geometry::MeshcatCone arrowhead_ = + drake::geometry::MeshcatCone(0.004, 0.004, 0.004); + const std::string force_path_ = "c3_forces"; + const std::string actor_trajectory_name_; + const std::string force_trajectory_name_; + const std::string lcs_force_trajectory_name_; + drake::geometry::Rgba actor_force_color_ = drake::geometry::Rgba(1, 0, 1, 1.0); + drake::geometry::Rgba contact_force_color_ = drake::geometry::Rgba(0.949, 0.757, 0.0, 1.0); + const double radius_ = 0.002; + const double newtons_per_meter_ = 40; +}; + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class LcmC3TargetDrawer : public drake::systems::LeafSystem { + public: + explicit LcmC3TargetDrawer(const std::shared_ptr&, + bool draw_tray = true, bool draw_ee = false); + + const drake::systems::InputPort& get_input_port_c3_state_final_target() + const { + return this->get_input_port(c3_state_final_target_input_port_); + } + + const drake::systems::InputPort& get_input_port_c3_state_target() + const { + return this->get_input_port(c3_state_target_input_port_); + } + + const drake::systems::InputPort& get_input_port_c3_state_actual() + const { + return this->get_input_port(c3_state_actual_input_port_); + } + + private: + drake::systems::EventStatus DrawC3State( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + std::shared_ptr meshcat_; + + drake::systems::InputPortIndex c3_state_final_target_input_port_; + drake::systems::InputPortIndex c3_state_target_input_port_; + drake::systems::InputPortIndex c3_state_actual_input_port_; + + bool draw_tray_; + bool draw_ee_; + + drake::systems::DiscreteStateIndex last_update_time_index_; + + const drake::geometry::Cylinder cylinder_for_tray_ = + drake::geometry::Cylinder(0.005, 0.1); + const drake::geometry::Cylinder cylinder_for_ee_ = + drake::geometry::Cylinder(0.0025, 0.05); + const std::string c3_state_path_ = "c3_state"; + const std::string c3_final_target_object_path_ = "c3_state/c3_final_target_object"; + const std::string c3_target_object_path_ = "c3_state/c3_target_object"; + const std::string c3_actual_object_path_ = "c3_state/c3_actual_object"; + const std::string c3_target_ee_path_ = "c3_state/c3_target_ee"; + const std::string c3_actual_ee_path_ = "c3_state/c3_actual_ee"; +}; +} // namespace systems +} // namespace dairlib diff --git a/systems/visualization/meshcat_dynamic_camera.cc b/systems/visualization/meshcat_dynamic_camera.cc new file mode 100644 index 0000000000..12570f6d18 --- /dev/null +++ b/systems/visualization/meshcat_dynamic_camera.cc @@ -0,0 +1,45 @@ +#include "systems/visualization/meshcat_dynamic_camera.h" + +#include + +#include "systems/framework/output_vector.h" + +namespace dairlib { +using systems::OutputVector; + +MeshcatDynamicCamera::MeshcatDynamicCamera( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* plant_context, + const std::shared_ptr& meshcat, + const drake::multibody::RigidBodyFrame& body_frame_to_track) + : plant_(plant), + plant_context_(plant_context), + body_frame_to_track_(body_frame_to_track), + meshcat_(meshcat) { + state_port_ = this->DeclareVectorInputPort( + "x, u, t", OutputVector(plant.num_positions(), + plant.num_velocities(), + plant.num_actuators())) + .get_index(); + DeclarePerStepDiscreteUpdateEvent(&MeshcatDynamicCamera::UpdateMeshcat); +} + +drake::systems::EventStatus MeshcatDynamicCamera::UpdateMeshcat( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const { + const OutputVector* robot_output = + (OutputVector*)this->EvalVectorInput(context, state_port_); + plant_.SetPositions(plant_context_, robot_output->GetPositions()); + Eigen::VectorXd q = robot_output->GetPositions(); + Eigen::VectorXd body_pos_in_world = Eigen::VectorXd::Zero(3); + plant_.CalcPointsPositions(*plant_context_, body_frame_to_track_, + Eigen::VectorXd::Zero(3), plant_.world_frame(), + &body_pos_in_world); + body_pos_in_world[2] = 0.8; + Eigen::VectorXd camera_pos_offset = Eigen::VectorXd::Zero(3); + camera_pos_offset << 0.1, 3.0, 0.5; + meshcat_->SetCameraPose(body_pos_in_world + camera_pos_offset, body_pos_in_world); + return drake::systems::EventStatus::Succeeded(); +} + +} // namespace dairlib \ No newline at end of file diff --git a/systems/visualization/meshcat_dynamic_camera.h b/systems/visualization/meshcat_dynamic_camera.h new file mode 100644 index 0000000000..8daeef7db5 --- /dev/null +++ b/systems/visualization/meshcat_dynamic_camera.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include + +#include "drake/multibody/plant/multibody_plant.h" + +namespace dairlib { + +/// Receives the output of an MPC planner as a lcmt_timestamped_saved_traj, +/// and draws it through meshcat. +class MeshcatDynamicCamera : public drake::systems::LeafSystem { + public: + explicit MeshcatDynamicCamera( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* plant_context, + const std::shared_ptr&, + const drake::multibody::RigidBodyFrame& body_frame_to_track); + + const drake::systems::InputPort& get_input_port_state() const { + return this->get_input_port(state_port_); + } + + private: + drake::systems::EventStatus UpdateMeshcat( + const drake::systems::Context& context, + drake::systems::DiscreteValues* discrete_state) const; + + drake::systems::InputPortIndex state_port_; + // Kinematic calculations + const drake::multibody::MultibodyPlant& plant_; + drake::systems::Context* plant_context_; + const drake::multibody::RigidBodyFrame& body_frame_to_track_; + + std::shared_ptr meshcat_; +}; + +} // namespace dairlib \ No newline at end of file diff --git a/tools/workspace/drake_models/BUILD.bazel b/tools/workspace/drake_models/BUILD.bazel new file mode 100644 index 0000000000..5dd2afc188 --- /dev/null +++ b/tools/workspace/drake_models/BUILD.bazel @@ -0,0 +1,9 @@ +load("@drake//doc:defs.bzl", "enumerate_filegroup") +load("@drake//tools/lint:lint.bzl", "add_lint_tests") + +enumerate_filegroup( + name = "inventory.txt", + data = ["@drake_models"], +) + +add_lint_tests() diff --git a/tools/workspace/drake_models/package.BUILD.bazel b/tools/workspace/drake_models/package.BUILD.bazel new file mode 100644 index 0000000000..245f642e99 --- /dev/null +++ b/tools/workspace/drake_models/package.BUILD.bazel @@ -0,0 +1,57 @@ +# -*- bazel -*- + +package(default_visibility = ["//visibility:public"]) + +# Glob for all files in this package. We do one big glob (instead of several +# smaller ones) so Bazel's loading is as efficient as possible. +_SRCS = glob(["**/*"]) + +# All files in this repository are public. Since everything here is excluded +# from Stable API policy, there's no point in trying to set visibility for the +# stable parts vs internal-use-only parts. +exports_files(_SRCS) + +# Provide a filegroup with all files. +filegroup( + name = "drake_models", + srcs = _SRCS, +) + +# Provide one filegroup per subdirectory (so Bazel users can depend on a +# smaller set of runfiles, for efficiency). +[ + filegroup( + name = top_dir, + srcs = [ + src + for src in _SRCS + if src.startswith(top_dir + "/") + ], + ) + for top_dir in depset([ + src.split("/", 1)[0] + for src in _SRCS + if "/" in src + ]).to_list() +] + +# Nominally, the `@drake_models` external is fetched via `github_archive()`, +# which creates a json metadata files to explain what it downloaded. Drake's +# install rules use that file to pin the lazy-download version of the models. +# +# However, in case a developer is using a local checkout of `@drake_models`, +# the json file will not exist. In that case, we need to generate a stub file +# to take its place, so that our Bazel install rules can still find it. We'll +# fill it with dummy data. To guard against shipping a Drake release with the +# dummy data, the package_map_remote_test checks the content of the json file. +glob(["drake_repository_metadata.json"], allow_empty = True) or genrule( + name = "_gen_dummy_metadata", + outs = ["drake_repository_metadata.json"], + cmd = "echo '{}' > $@".format( + json.encode(dict( + urls = [], + sha256 = "", + strip_prefix = "", + )), + ), +) diff --git a/tools/workspace/drake_models/repository.bzl b/tools/workspace/drake_models/repository.bzl new file mode 100644 index 0000000000..1a991b5c2a --- /dev/null +++ b/tools/workspace/drake_models/repository.bzl @@ -0,0 +1,16 @@ +load("@drake//tools/workspace:github.bzl", "github_archive") + +def drake_models_repository( + name, + mirrors = {"github": [ + "https://github.com/{repository}/archive/refs/tags/{tag_name}.tar.gz", # noqa + "https://github.com/{repository}/archive/{commit_sha}.tar.gz", + ]}): + github_archive( + name = name, + repository = "RobotLocomotion/models", + commit = "c81f2458cf6d19a20a27e1495e7f07202536e845", + sha256 = "1107e8314e49102a247f2e87666cba8bd1e76527112ce01d849e299cf8010d94", # noqa + build_file = ":package.BUILD.bazel", + mirrors = mirrors, + ) diff --git a/tools/workspace/gurobi/BUILD.bazel b/tools/workspace/gurobi/BUILD.bazel new file mode 100644 index 0000000000..a0f3ced5ee --- /dev/null +++ b/tools/workspace/gurobi/BUILD.bazel @@ -0,0 +1,13 @@ +load("@bazel_skylib//lib:selects.bzl", "selects") + +selects.config_setting_group( + name = "enabled", + match_any = [ + ":enabled_via_flag", + ], +) + +config_setting( + name = "enabled_via_flag", + flag_values = {"@drake//tools/flags:with_gurobi": "True"}, +) diff --git a/tools/workspace/gurobi/package-darwin.BUILD.bazel.in b/tools/workspace/gurobi/package-darwin.BUILD.bazel.in new file mode 100644 index 0000000000..82cdc278b6 --- /dev/null +++ b/tools/workspace/gurobi/package-darwin.BUILD.bazel.in @@ -0,0 +1,55 @@ +# -*- bazel -*- + +load("@drake//tools/install:install.bzl", "install") +load("@drake//tools/skylark:cc.bzl", "cc_library") + +licenses(["by_exception_only"]) # Gurobi + +# This rule is only built if a glob() call fails. +genrule( + name = "error-message", + outs = ["error-message.h"], + cmd = "echo 'error: Gurobi 10.0 is not installed at {gurobi_home}' && false", # noqa + visibility = ["//visibility:private"], +) + +GUROBI_C_HDRS = glob([ + "gurobi-distro/include/gurobi_c.h", +], allow_empty = True) or [":error-message.h"] + +GUROBI_CXX_HDRS = glob([ + "gurobi-distro/include/gurobi_c.h", + "gurobi-distro/include/gurobi_c++.h", +], allow_empty = True) or [":error-message.h"] + +cc_library( + name = "gurobi_c", + hdrs = GUROBI_C_HDRS, + includes = ["gurobi-distro/include"], + linkopts = [ + "-L{gurobi_home}/lib", + "-lgurobi100", + "-Wl,-rpath,{gurobi_home}/lib", + ], + visibility = ["//visibility:public"], +) + +cc_library( + name = "gurobi_cxx", + hdrs = GUROBI_CXX_HDRS, + includes = ["gurobi-distro/include"], + linkopts = [ + "-L{gurobi_home}/lib", + "-lgurobi100", + "-lgurobi_stdc++", + "-Wl,-rpath,{gurobi_home}/lib", + ], + visibility = ["//visibility:public"], +) + +# For macOS, the Drake install step does not need any additional actions to +# install Gurobi, since Gurobi was already installed system-wide in /Library. +install( + name = "install", + visibility = ["//visibility:public"], +) diff --git a/tools/workspace/gurobi/package-linux.BUILD.bazel.in b/tools/workspace/gurobi/package-linux.BUILD.bazel.in new file mode 100644 index 0000000000..33c9f7c29f --- /dev/null +++ b/tools/workspace/gurobi/package-linux.BUILD.bazel.in @@ -0,0 +1,82 @@ +# -*- bazel -*- + +load("@drake//tools/install:install.bzl", "install", "install_files") +load("@drake//tools/skylark:cc.bzl", "cc_library") + +licenses(["by_exception_only"]) # Gurobi + +# This rule is only built if a glob() call fails. +genrule( + name = "error-message", + outs = ["error-message.h"], + cmd = "echo 'error: Gurobi 10.0 is not installed at {gurobi_home}, export GUROBI_HOME to the correct value' && false", # noqa + visibility = ["//visibility:private"], +) + +GUROBI_C_HDRS = glob([ + "gurobi-distro/include/gurobi_c.h", +], allow_empty = True) or [":error-message.h"] + +GUROBI_CXX_HDRS = glob([ + "gurobi-distro/include/gurobi_c.h", + "gurobi-distro/include/gurobi_c++.h", +], allow_empty = True) or [":error-message.h"] + +# In the Gurobi package, libgurobi100.so is a symlink to libgurobi.so.10.0.*. +# However, if we use libgurobi.so.10.0.* in srcs, executables that link this +# library will be unable to find it at runtime in the Bazel sandbox, +# because the NEEDED statements in the executable will not square with the +# RPATH statements. I don't really know why this happens, but I suspect it +# might be a Bazel bug. + +GUROBI_C_SRCS = glob([ + "gurobi-distro/lib/libgurobi100.so", +], allow_empty = True) or [":error-message.h"] + +GUROBI_CXX_SRCS = glob([ + "gurobi-distro/lib/libgurobi100.so", + "gurobi-distro/lib/libgurobi_g++5.2.a", +], allow_empty = True) or [":error-message.h"] + +GUROBI_INSTALL_LIBRARIES = glob([ + "gurobi-distro/lib/libgurobi.so.10.0.*", + "gurobi-distro/lib/libgurobi100.so", +], allow_empty = True) or [":error-message.h"] + +GUROBI_DOCS = glob([ + "gurobi-distro/EULA.pdf", +], allow_empty = True) or [":error-message.h"] + +cc_library( + name = "gurobi_c", + srcs = GUROBI_C_SRCS, + hdrs = GUROBI_C_HDRS, + includes = ["gurobi-distro/include"], + linkopts = ["-pthread"], + visibility = ["//visibility:public"], +) + +cc_library( + name = "gurobi_cxx", + srcs = GUROBI_CXX_SRCS, + hdrs = GUROBI_CXX_HDRS, + includes = ["gurobi-distro/include"], + linkopts = ["-pthread"], + visibility = ["//visibility:public"], +) + +install_files( + name = "install_libraries", + dest = ".", + files = GUROBI_INSTALL_LIBRARIES, + strip_prefix = ["gurobi-distro"], + visibility = ["//visibility:private"], +) + +install( + name = "install", + docs = GUROBI_DOCS, + doc_strip_prefix = ["gurobi-distro"], + visibility = ["//visibility:public"], + deps = [":install_libraries"], +) diff --git a/tools/workspace/gurobi/repository.bzl b/tools/workspace/gurobi/repository.bzl new file mode 100644 index 0000000000..4e9907cecf --- /dev/null +++ b/tools/workspace/gurobi/repository.bzl @@ -0,0 +1,89 @@ +# This is a Bazel repository_rule for the Gurobi solver. See +# https://www.bazel.io/versions/master/docs/skylark/repository_rules.html + +# Finds the "latest" f'{path}/{prefix}*/{subdir}', where "latest" is determined +# by converting the part that matched the '*' to an integer and taking the +# match with the highest value. +def _find_latest(repo_ctx, path, prefix, subdir): + best_dir = None + best_version = None + for d in repo_ctx.path(path).readdir(): + if d.basename.startswith(prefix): + full_dir = d.get_child(subdir) + if full_dir.exists: + version = int(d.basename[len(prefix):]) + if best_version == None or version > best_version: + best_version = version + best_dir = str(full_dir) + + return best_dir or (path + "/" + prefix + "-notfound/" + subdir) + +# Ubuntu only: GUROBI_HOME should be the linux64 directory in the Gurobi 10.0 +# release. +# +def _gurobi_impl(repo_ctx): + os_name = repo_ctx.os.name + if os_name == "mac os x": + os_name = "darwin" + + if os_name == "darwin": + # Gurobi must be installed into its standard location. + gurobi_home = _find_latest( + repo_ctx, + "/Library", + "gurobi100", + "macos_universal2", + ) + repo_ctx.symlink(gurobi_home, "gurobi-distro") + elif os_name == "linux": + # The default directory for the downloaded gurobi is + # /opt/gurobi100*/linux64. If the user does not use the default + # directory, the he/she should set GUROBI_HOME environment variable to + # the gurobi file location. + gurobi_home = repo_ctx.getenv("GUROBI_HOME", "") + repo_ctx.symlink( + gurobi_home or _find_latest( + repo_ctx, + "/opt", + "gurobi100", + "linux64", + ), + "gurobi-distro", + ) + else: + # Defer error reporting to the BUILD file. + repo_ctx.symlink("/gurobi-notfound", "gurobi-distro") + os_name = "linux" + + # Emit the generated BUILD.bazel file. + repo_ctx.template( + "BUILD.bazel", + Label("@//tools/workspace/gurobi:" + + "package-{}.BUILD.bazel.in".format(os_name)), + substitutions = { + "{gurobi_home}": gurobi_home, + }, + executable = False, + ) + + # Capture whether or not Gurobi tests can run in parallel. + license_unlimited_int = repo_ctx.getenv("DRAKE_GUROBI_LICENSE_UNLIMITED", "0") # noqa: E501 + license_unlimited = bool(int(license_unlimited_int) == 1) + repo_ctx.file( + "defs.bzl", + content = "DRAKE_GUROBI_LICENSE_UNLIMITED = {}" + .format(license_unlimited), + executable = False, + ) + +gurobi_repository = repository_rule( + local = True, + implementation = _gurobi_impl, +) + +def _gurobi_extension_impl(module_ctx): + gurobi_repository(name = "my_gurobi") + +gurobi_extension = module_extension( + implementation = _gurobi_extension_impl, +) diff --git a/tools/workspace/osqp/package.BUILD.bazel b/tools/workspace/osqp/package.BUILD.bazel index 9d40ac32b7..782deb213d 100644 --- a/tools/workspace/osqp/package.BUILD.bazel +++ b/tools/workspace/osqp/package.BUILD.bazel @@ -1,13 +1,13 @@ # -*- bazel -*- -load( - "@drake//tools/workspace:cmake_configure_file.bzl", - "cmake_configure_file", -) load( "@drake//tools/install:install.bzl", "install", ) +load( + "@drake//tools/workspace:cmake_configure_file.bzl", + "cmake_configure_file", +) licenses(["notice"]) # Apache-2.0 diff --git a/tools/workspace/pydrake/repository.bzl b/tools/workspace/pydrake/repository.bzl index 78c121a0f3..4866b0d5f6 100644 --- a/tools/workspace/pydrake/repository.bzl +++ b/tools/workspace/pydrake/repository.bzl @@ -1,29 +1,29 @@ -load("@drake//tools/workspace:pypi_wheel.bzl", "setup_pypi_wheel") - -def _impl(repository_ctx): - mirrors = { - "pypi_wheel": [ - "https://www.seas.upenn.edu/~posa/files/{package}-{version}-{tag}.whl", # noqa - ], - } - - result = setup_pypi_wheel( - repository_ctx, - package = "drake", - version = "0.0.2022.2.2", - pypi_tag = "cp38-cp38-manylinux_2_27_x86_64", - blake2_256 = "0", # noqa - sha256 = "6ffd53c9889eaa3a2ce27784ee9a21f65f849bcfab47d1958c234bd218c2d163", # noqa - mirrors = mirrors, - data = "glob([\"pydrake/share/drake/**\"])", - ) - if result.error != None: - fail("Unable to complete setup for @{} repository: {}".format( - # (forced line break) - repository_ctx.name, - result.error, - )) - -pydrake_repository = repository_rule( - implementation = _impl, -) +#load("@drake//tools/workspace:pypi_wheel.bzl", "setup_pypi_wheel") +# +#def _impl(repository_ctx): +# mirrors = { +# "pypi_wheel": [ +# "https://www.seas.upenn.edu/~posa/files/{package}-{version}-{tag}.whl", # noqa +# ], +# } +# +# result = setup_pypi_wheel( +# repository_ctx, +# package = "drake", +# version = "0.0.2022.2.2", +# pypi_tag = "cp38-cp38-manylinux_2_27_x86_64", +# blake2_256 = "0", # noqa +# sha256 = "6ffd53c9889eaa3a2ce27784ee9a21f65f849bcfab47d1958c234bd218c2d163", # noqa +# mirrors = mirrors, +# data = "glob([\"pydrake/share/drake/**\"])", +# ) +# if result.error != None: +# fail("Unable to complete setup for @{} repository: {}".format( +# # (forced line break) +# repository_ctx.name, +# result.error, +# )) +# +#pydrake_repository = repository_rule( +# implementation = _impl, +#) diff --git a/tools/workspace/ros/.gitignore b/tools/workspace/ros/.gitignore deleted file mode 100644 index 1e059677cd..0000000000 --- a/tools/workspace/ros/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/bundle_ws/ diff --git a/tools/workspace/ros/BUILD.bazel b/tools/workspace/ros/BUILD.bazel deleted file mode 100644 index 5dbef1a94d..0000000000 --- a/tools/workspace/ros/BUILD.bazel +++ /dev/null @@ -1,30 +0,0 @@ -# -*- python -*- - -# Demo programs - -cc_binary( - name = "main", - srcs = ["test/main.cpp"], - tags = ["ros"], - deps = [ - "@ros", - ], -) - -py_binary( - name = "main_python", - srcs = ["test/main_python.py"], - tags = ["ros"], - deps = [ - "@ros//:ros_python", - ], -) - -py_binary( - name = "genmsg_test", - srcs = ["test/genmsg_test.py"], - tags = ["ros"], - deps = [ - "@genpy_repo//:genpy", - ], -) diff --git a/tools/workspace/ros/LICENSE_ros-bazel b/tools/workspace/ros/LICENSE_ros-bazel deleted file mode 100644 index 1fce6447d9..0000000000 --- a/tools/workspace/ros/LICENSE_ros-bazel +++ /dev/null @@ -1,24 +0,0 @@ -Original versions of //tools/workspace/ros are from -https://github.com/nicolov/ros-bazel - -MIT License - -Copyright (c) 2017 Nicolo Valigi - -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/tools/workspace/ros/bazel/genmsg.BUILD b/tools/workspace/ros/bazel/genmsg.BUILD deleted file mode 100644 index a752a0acb6..0000000000 --- a/tools/workspace/ros/bazel/genmsg.BUILD +++ /dev/null @@ -1,7 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -py_library( - name = "genmsg", - srcs = glob(["src/**/*.py"]), - imports = ["src"], -) diff --git a/tools/workspace/ros/bazel/genpy.BUILD b/tools/workspace/ros/bazel/genpy.BUILD deleted file mode 100644 index c431e52130..0000000000 --- a/tools/workspace/ros/bazel/genpy.BUILD +++ /dev/null @@ -1,18 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -py_library( - name = "genpy", - srcs = glob(["src/**/*.py"]), - imports = ["src"], - deps = [ - "@genmsg_repo//:genmsg", - ], -) - -py_binary( - name = "genmsg_py", - srcs = ["scripts/genmsg_py.py"], - deps = [ - ":genpy", - ], -) diff --git a/tools/workspace/ros/bazel/message_generation.bzl b/tools/workspace/ros/bazel/message_generation.bzl deleted file mode 100644 index 66af366203..0000000000 --- a/tools/workspace/ros/bazel/message_generation.bzl +++ /dev/null @@ -1,134 +0,0 @@ -# Reference: -# https://docs.bazel.build/versions/master/skylark/cookbook.html -# https://github.com/RobotLocomotion/drake/blob/eefddbee62439156b6faaf3b0cecdd0c57e704d7/tools/lcm.bzl - -load("//tools/workspace/ros/bazel:path_utils.bzl", "basename", "dirname", "join_paths") - -def _genmsg_outs(srcs, ros_package_name, extension): - """ Given a list of *.msg files, return the expected paths - to the generated code with that extension. """ - - (extension in [".py", ".h"] or - fail("Unknown extension %s" % extension)) - - msg_names = [] - for item in srcs: - if not item.endswith(".msg"): - fail("%s does not end in .msg" % item) - item_name = basename(item)[:-len(".msg")] - - if extension == ".py": - item_name = "_" + item_name - - msg_names.append(item_name) - - outs = [ - join_paths(ros_package_name, "msg", msg_name + extension) - for msg_name in msg_names - ] - - if extension == ".py": - outs += [ - join_paths(ros_package_name, "msg", "__init__.py"), - join_paths(ros_package_name, "__init__.py"), - ] - - return outs - -def _genpy_impl(ctx): - """Implementation for the genpy rule. Shells out to the scripts - shipped with genpy.""" - - srcpath = ctx.files.srcs[0].dirname - outpath = ctx.outputs.outs[0].dirname - - # Generate __init__.py for package - ctx.actions.write( - output = ctx.outputs.outs[-1], - content = "", - ) - - # Generate the actual messages - ctx.actions.run( - inputs = ctx.files.srcs, - outputs = ctx.outputs.outs[:-2], - executable = ctx.executable._gen_script, - arguments = [ - "-o", - outpath, - "-p", - ctx.attr.ros_package_name, - # Include path for the current package - "-I", - "%s:%s" % (ctx.attr.ros_package_name, srcpath), - # TODO: include paths of dependent packages - ] + [ - f.path - for f in ctx.files.srcs - ], - ) - - # Generate __init__.py for msg module - # NOTE: it looks at the .py files in its output path, so it also - # needs to depend on the previous step. - ctx.actions.run( - inputs = ctx.files.srcs + ctx.outputs.outs[:-2], - outputs = [ctx.outputs.outs[-2]], - executable = ctx.executable._gen_script, - arguments = [ - "--initpy", - "-o", - outpath, - "-p", - ctx.attr.ros_package_name, - ], - ) - - return struct() - -_genpy = rule( - implementation = _genpy_impl, - output_to_genfiles = True, - attrs = { - "srcs": attr.label_list(allow_files = True), - "ros_package_name": attr.string(), - "_gen_script": attr.label( - default = Label("@genpy_repo//:genmsg_py"), - executable = True, - cfg = "host", - ), - "outs": attr.output_list(), - }, -) - -def generate_messages( - srcs = None, - ros_package_name = None): - """ Wraps all message generation functionality. Uses the _genpy - and _gencpp to shell out to the code generation scripts, then wraps - the resulting files into Python and C++ libraries. - We use macros to hide some of the book-keeping of input & output - files. """ - - if not srcs: - fail("srcs is required (*.msg files).") - if not ros_package_name: - fail("ros_package_name is required.") - - outs = _genmsg_outs(srcs, ros_package_name, ".py") - - _genpy( - name = "lkfjaklsjfklasd", - srcs = srcs, - ros_package_name = ros_package_name, - outs = outs, - ) - - native.py_library( - name = "msgs_py", - srcs = outs, - imports = ["."], - deps = [ - "@genpy_repo//:genpy", - ], - ) diff --git a/tools/workspace/ros/bazel/path_utils.bzl b/tools/workspace/ros/bazel/path_utils.bzl deleted file mode 100644 index ae697cfcb9..0000000000 --- a/tools/workspace/ros/bazel/path_utils.bzl +++ /dev/null @@ -1,39 +0,0 @@ -# Lifted from: -# https://github.com/RobotLocomotion/drake/blob/eefddbee62439156b6faaf3b0cecdd0c57e704d7/tools/pathutils.bzl - -def basename(path): - """Return the file name portion of a file path.""" - return path.split("/")[-1] - -def dirname(path): - """Return the directory portion of a file path.""" - if path == "/": - return "/" - - parts = path.split("/") - - if len(parts) > 1: - return "/".join(parts[:-1]) - - return "." - -def join_paths(*args): - """Join paths without duplicating separators. - This is roughly equivalent to Python's `os.path.join`. - Args: - *args (:obj:`list` of :obj:`str`): Path components to be joined. - Returns: - :obj:`str`: The concatenation of the input path components. - """ - result = "" - - for part in args: - if part.endswith("/"): - part = part[-1] - - if part == "" or part == ".": - continue - - result += part + "/" - - return result[:-1] diff --git a/tools/workspace/ros/bazel/pypi.bzl b/tools/workspace/ros/bazel/pypi.bzl deleted file mode 100644 index 85483b4517..0000000000 --- a/tools/workspace/ros/bazel/pypi.bzl +++ /dev/null @@ -1,54 +0,0 @@ -_BUILD_FILE = """ -py_library( - name = 'libraries', - srcs = glob( - include = ['bin/**/*.py', 'site-packages/**/*.py'], - ), - data = glob( - include = ['bin/**/*', 'site-packages/**/*'], - exclude = ['**/*.py', '**/*.pyc'] - ), - imports=['site-packages'], - visibility = ['//visibility:public'] -) -""" - -def pip_package_impl(ctx): - getpip = ctx.path(ctx.attr._getpip) - path = ctx.path("site-packages") - - command = ["python", str(getpip)] - command += list(ctx.attr.packages) - command += ["--target", str(path)] - command += ["--install-option", "--install-scripts=%s" % ctx.path("bin")] - command += ["--no-cache-dir"] - - print(command) - result = ctx.execute(command) - if result.return_code != 0: - fail("Failed to execute %s.\n%s\n%s" % ( - " ".join(command), - result.stdout, - result.stderr, - )) - ctx.file("BUILD", _BUILD_FILE, False) - -pip_requirements = repository_rule( - pip_package_impl, - attrs = { - "packages": attr.string_list(), - "_getpip": attr.label( - default = Label("@pip//file:get-pip.py"), - allow_single_file = True, - executable = True, - cfg = "host", - ), - }, -) - -def pip(): - native.http_file( - name = "pip", - url = "https://bootstrap.pypa.io/get-pip.py", - sha256 = "19dae841a150c86e2a09d475b5eb0602861f2a5b7761ec268049a662dbd2bd0c", - ) diff --git a/tools/workspace/ros/compile_ros_workspace.sh b/tools/workspace/ros/compile_ros_workspace.sh deleted file mode 100755 index 1bbf8a0b4c..0000000000 --- a/tools/workspace/ros/compile_ros_workspace.sh +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env bash - -# In addition to your base ROS install, -# you must sudo apt-get install python3-vcstool - -# Tested on ubuntu 20.04 and 18.04 -BASE_DIR="$PWD" - -cd $(dirname "$BASH_SOURCE") - -set -e - -PACKAGES="roscpp rospy" -# You can add any published ros packages you need to this list. -# Local ROS packages should be their own bazel local_repository - -rm -rf bundle_ws -mkdir bundle_ws -pushd bundle_ws -mkdir src - -DISTRO=$(lsb_release -c -s) - -if [ $DISTRO == "bionic" ] -then - rosinstall_generator \ - --deps \ - --tar \ - --flat \ - $PACKAGES > ws.rosinstall - wstool init -j1 src ws.rosinstall -elif [ $DISTRO == "focal" ] -then - rosinstall_generator \ - --rosdistro noetic \ - --deps \ - --tar \ - --flat \ - $PACKAGES > ws.rosinstall - vcs import src < ws.rosinstall -else - echo "${DISTRO} not supported for ROS with dairlib!" - exit 1 -fi - -catkin config \ - --install \ - --source-space src \ - --build-space build \ - --devel-space devel \ - --log-space log \ - --install-space install \ - --isolate-devel \ - --no-extend - -catkin build -DPYTHON_EXECUTABLE=/usr/bin/python3 - -cd $BASE_DIR \ No newline at end of file diff --git a/tools/workspace/ros/ros.bazel b/tools/workspace/ros/ros.bazel deleted file mode 100644 index f7381abecb..0000000000 --- a/tools/workspace/ros/ros.bazel +++ /dev/null @@ -1,18 +0,0 @@ -# -*- python -*- - -package(default_visibility = ["//visibility:public"]) - -# Prebuilt C++ and Python ROS libraries - -cc_library( - name='ros', - srcs=glob(['lib/*.so']), - hdrs=glob(['include/**/*.h', 'include/**/*.hpp']), - strip_include_prefix='include' -) - -py_library( - name='ros_python', - srcs=glob(['lib/python2.7/dist-packages/**/*.py']), - imports=['lib/python2.7/dist-packages/'] -) diff --git a/tools/workspace/ros/test/genmsg_test.py b/tools/workspace/ros/test/genmsg_test.py deleted file mode 100644 index 93f8ee82cd..0000000000 --- a/tools/workspace/ros/test/genmsg_test.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -import os -import genpy - -import genpy.generator - -def main(): - pass - -if __name__ == '__main__': - main() diff --git a/tools/workspace/ros/test/main.cpp b/tools/workspace/ros/test/main.cpp deleted file mode 100644 index eef5db64fc..0000000000 --- a/tools/workspace/ros/test/main.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include -#include - -#include - -int main(int argc, char **argv) { - ros::init(argc, argv, "talker"); - - ros::NodeHandle n; - ros::Publisher chatter_pub = n.advertise("chatter", 1000); - - ros::Rate loop_rate(10); - - int count = 0; - while (ros::ok()) { - std_msgs::String msg; - - std::stringstream ss; - ss << "hello world " << count; - msg.data = ss.str(); - - ROS_INFO("%s", msg.data.c_str()); - - chatter_pub.publish(msg); - - ros::spinOnce(); - - loop_rate.sleep(); - ++count; - } - - return 0; -} \ No newline at end of file diff --git a/tools/workspace/ros/test/main_python.py b/tools/workspace/ros/test/main_python.py deleted file mode 100644 index 2dcf5c205c..0000000000 --- a/tools/workspace/ros/test/main_python.py +++ /dev/null @@ -1,24 +0,0 @@ -#!/usr/bin/env python - -""" -Simple node for testing the Python ROS libraries. -""" - -import rospy -from std_msgs.msg import String - -def talker(): - pub = rospy.Publisher('chatter', String, queue_size=10) - rospy.init_node('talker', anonymous=True) - rate = rospy.Rate(10) # 10hz - while not rospy.is_shutdown(): - hello_str = "hello world %s" % rospy.get_time() - rospy.loginfo(hello_str) - pub.publish(hello_str) - rate.sleep() - -if __name__ == '__main__': - try: - talker() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/tools/workspace/ros/test/test_msgs/BUILD b/tools/workspace/ros/test/test_msgs/BUILD deleted file mode 100644 index 1648daea73..0000000000 --- a/tools/workspace/ros/test/test_msgs/BUILD +++ /dev/null @@ -1,16 +0,0 @@ -load("//tools/workspace/ros/bazel:message_generation.bzl", "generate_messages") - -# Implicitly creates a msgs_py target in the current scope -generate_messages( - srcs = glob(["msg/*.msg"]), - ros_package_name = "test_msgs", -) - -py_binary( - name = "main", - srcs = ["scripts/main.py"], - tags = ["ros"], - deps = [ - ":msgs_py", - ], -) diff --git a/tools/workspace/ros/test/test_msgs/msg/Message1.msg b/tools/workspace/ros/test/test_msgs/msg/Message1.msg deleted file mode 100644 index 00f26208cb..0000000000 --- a/tools/workspace/ros/test/test_msgs/msg/Message1.msg +++ /dev/null @@ -1 +0,0 @@ -uint64 auint64 \ No newline at end of file diff --git a/tools/workspace/ros/test/test_msgs/msg/Message2.msg b/tools/workspace/ros/test/test_msgs/msg/Message2.msg deleted file mode 100644 index 861f1b534d..0000000000 --- a/tools/workspace/ros/test/test_msgs/msg/Message2.msg +++ /dev/null @@ -1,2 +0,0 @@ -string astr -Message1 amsg1 \ No newline at end of file diff --git a/tools/workspace/ros/test/test_msgs/scripts/main.py b/tools/workspace/ros/test/test_msgs/scripts/main.py deleted file mode 100644 index 5eb5c396fc..0000000000 --- a/tools/workspace/ros/test/test_msgs/scripts/main.py +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env python - -from test_msgs.msg import ( - Message1, - Message2, -) - - -def main(): - m1 = Message1() - print(m1) - - m2 = Message2() - print(m2) - - -if __name__ == '__main__': - main() diff --git a/tools/workspace/signal_scope/BUILD.bazel b/tools/workspace/signal_scope/BUILD.bazel deleted file mode 100644 index eb9fa5a75d..0000000000 --- a/tools/workspace/signal_scope/BUILD.bazel +++ /dev/null @@ -1,2 +0,0 @@ -# -*- mode: python -*- -# vi: set ft=python : diff --git a/tools/workspace/signal_scope/README b/tools/workspace/signal_scope/README deleted file mode 100644 index dbdda24f04..0000000000 --- a/tools/workspace/signal_scope/README +++ /dev/null @@ -1,2 +0,0 @@ -This directory and it's files are modified and pared down from those in -Drake's //tools/workspace/drake_visualizer as of b909617dd0156d61b3dd7943bc86a00f7effaa3a \ No newline at end of file diff --git a/tools/workspace/signal_scope/repository.bzl b/tools/workspace/signal_scope/repository.bzl deleted file mode 100644 index 127a43c03f..0000000000 --- a/tools/workspace/signal_scope/repository.bzl +++ /dev/null @@ -1,53 +0,0 @@ -# -*- mode: python -*- -# vi: set ft=python : - -load("@drake//tools/workspace:os.bzl", "determine_os") - -def _impl(repository_ctx): - os_result = determine_os(repository_ctx) - if os_result.error != None: - fail(os_result.error) - - if os_result.is_macos: - urls = ["https://www.seas.upenn.edu/~posa/files/signal-scope/signal-scope-mac.zip"] - prefix = "signal-scope-mac" - sha256 = "c34b4dd4d85f9575b51cc2e03236420d8517f5fdb232bffacc09bf35ee0bd473" - elif os_result.ubuntu_release == "16.04": - urls = ["https://www.seas.upenn.edu/~posa/files/signal-scope/signal-scope-16.04.zip"] - sha256 = "d51cc26c5130cdf51ee10602d2a32032219bfdf25f96bfaa1f7dbd9d9623e1cf" - prefix = "signal-scope-16.04" - elif os_result.ubuntu_release == "18.04": - urls = ["https://www.seas.upenn.edu/~posa/files/signal-scope/signal-scope-18.04.zip"] - sha256 = "b9dd567bde8f4a1043ae9f13194b7a2cf9c8c716f8e7373f11a98a770c404d54" - prefix = "signal-scope-18.04" - elif os_result.ubuntu_release == "20.04": - urls = ["https://www.seas.upenn.edu/~posa/files/signal-scope/signal-scope-20.04.zip"] - sha256 = "6bb42fdec9767984d21cd07ab0eb13e11189c3a1b5931a5fabef4c23de247fa6" - prefix = "signal-scope-20.04" - else: - fail("Operating system is NOT supported", attr = os_result) - - root_path = repository_ctx.path("") - - repository_ctx.download_and_extract(urls, "", sha256 = sha256, stripPrefix = prefix) - - file_content = """# -*- python -*- - -# DO NOT EDIT: generated signal_scope_repository() - -filegroup( - name = "signal_scope", - srcs = glob(["**"]), - visibility = ["//visibility:public"], -) -""" - - repository_ctx.file( - "BUILD.bazel", - content = file_content, - executable = False, - ) - -signal_scope_repository = repository_rule( - implementation = _impl, -)