├── .github ├── actions │ ├── focal │ │ └── action.yml │ └── jammy │ │ └── action.yml └── workflows │ └── ci.yml ├── .gitignore ├── CMakeLists.txt ├── README.md ├── config ├── CMakeLists.txt └── default.yaml ├── focal.dockerfile ├── jammy.dockerfile ├── lcmtypes ├── CMakeLists.txt ├── lcmt_plan_status.lcm └── lcmt_plan_status_constants.lcm ├── scripts ├── build_test.sh └── install_cppzmq.sh ├── src ├── CMakeLists.txt ├── examples │ ├── lcm_client_example.py │ ├── simple_test.py │ └── zmq_client_example.py ├── iiwa_plan_manager.cc ├── iiwa_plan_manager.h ├── import_all_test.py ├── plan_manager_system │ ├── CMakeLists.txt │ ├── iiwa_plan_manager_hardware_interface.cc │ ├── iiwa_plan_manager_hardware_interface.h │ ├── iiwa_plan_manager_system.cc │ └── iiwa_plan_manager_system.h ├── plan_runner_client │ ├── __init__.py │ ├── calc_plan_msg.py │ └── zmq_client.py ├── plans │ ├── CMakeLists.txt │ ├── iiwa_plan_factory.cc │ ├── iiwa_plan_factory.h │ ├── joint_space_trajectory_plan.cc │ ├── joint_space_trajectory_plan.h │ ├── plan_base.cc │ ├── plan_base.h │ ├── plan_factory_base.cc │ ├── plan_factory_base.h │ ├── task_space_trajectory_plan.cc │ └── task_space_trajectory_plan.h ├── run_plan_manager.cc ├── run_plan_manager_system.cc ├── run_zmq_example.cc ├── state_machine │ ├── CMakeLists.txt │ ├── plan_manager_state_machine.cc │ ├── plan_manager_state_machine.h │ ├── state_error.cc │ ├── state_error.h │ ├── state_idle.cc │ ├── state_idle.h │ ├── state_init.cc │ ├── state_init.h │ ├── state_running.cc │ └── state_running.h └── utils │ └── lcm_delay_analysis.py └── third_party ├── CMakeLists.txt └── googletest ├── LICENSE ├── gtest-all.cc └── gtest └── gtest.h /.github/actions/focal/action.yml: -------------------------------------------------------------------------------- 1 | # action.yml 2 | name: 'run focal test' 3 | description: 'runs tests on ubuntu 20.04.' 4 | 5 | runs: 6 | using: 'docker' 7 | image: '../../../focal.dockerfile' 8 | entrypoint: './scripts/build_test.sh' 9 | -------------------------------------------------------------------------------- /.github/actions/jammy/action.yml: -------------------------------------------------------------------------------- 1 | # action.yml 2 | name: 'run test' 3 | description: 'runs tests.' 4 | 5 | runs: 6 | using: 'docker' 7 | image: '../../../jammy.dockerfile' 8 | entrypoint: './scripts/build_test.sh' 9 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | on: 3 | push: 4 | branches: 5 | - main 6 | pull_request: 7 | schedule: 8 | - cron: "0 6 * * *" # 6am everyday. 9 | 10 | jobs: 11 | focal: 12 | runs-on: ubuntu-latest 13 | steps: 14 | # To use this repository's private action, 15 | # you must check out the repository 16 | - name: Checkout 17 | uses: actions/checkout@v3 18 | - name: build and test action step 19 | uses: ./.github/actions/focal 20 | id: build_test 21 | jammy: 22 | runs-on: ubuntu-latest 23 | steps: 24 | # To use this repository's private action, 25 | # you must check out the repository 26 | - name: Checkout 27 | uses: actions/checkout@v3 28 | - name: build and test action step 29 | uses: ./.github/actions/jammy 30 | id: build_test 31 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2017, Massachusetts Institute of Technology. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # * Neither the name of the copyright holder nor the names of its contributors 15 | # may be used to endorse or promote products derived from this software 16 | # without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | 30 | .idea 31 | .DS_Store 32 | .pyc 33 | cmake-build-debug 34 | cmake-build-release 35 | /build/ 36 | sandbox/ 37 | Testing 38 | __pycache__ 39 | 40 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # -*- mode: cmake -*- 2 | # vi: set ft=cmake : 3 | 4 | # Copyright (c) 2017, Massachusetts Institute of Technology. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # * Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # * Neither the name of the copyright holder nor the names of its contributors 18 | # may be used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | cmake_minimum_required(VERSION 3.10.2) 34 | project(robot-plan-runner) 35 | 36 | # N.B. This is a temporary flag. It only really applies to Linux, as Mac 37 | # does not need X11. 38 | option(RUN_X11_TESTS "Run tests that require X11" OFF) 39 | 40 | include(CTest) 41 | 42 | set(UNIX_DISTRIBUTION_ID) 43 | set(UNIX_DISTRIBUTION_CODENAME) 44 | 45 | if(NOT APPLE) 46 | find_program(LSB_RELEASE_EXECUTABLE NAMES lsb_release) 47 | execute_process(COMMAND "${LSB_RELEASE_EXECUTABLE}" --id --short 48 | RESULT_VARIABLE LSB_RELEASE_ID_SHORT_RESULT_VARIABLE 49 | OUTPUT_VARIABLE LSB_RELEASE_ID_SHORT_OUTPUT_VARIABLE 50 | OUTPUT_STRIP_TRAILING_WHITESPACE 51 | ) 52 | 53 | if(LSB_RELEASE_ID_SHORT_RESULT_VARIABLE EQUAL 0) 54 | set(UNIX_DISTRIBUTION_ID "${LSB_RELEASE_ID_SHORT_OUTPUT_VARIABLE}") 55 | endif() 56 | 57 | if(NOT UNIX_DISTRIBUTION_ID STREQUAL Ubuntu) 58 | message(FATAL_ERROR 59 | "Distribution ${UNIX_DISTRIBUTION_ID} is NOT supported" 60 | ) 61 | endif() 62 | 63 | execute_process(COMMAND "${LSB_RELEASE_EXECUTABLE}" --codename --short 64 | RESULT_VARIABLE LSB_RELEASE_CODENAME_SHORT_RESULT_VARIABLE 65 | OUTPUT_VARIABLE LSB_RELEASE_CODENAME_SHORT_OUTPUT_VARIABLE 66 | OUTPUT_STRIP_TRAILING_WHITESPACE 67 | ) 68 | 69 | if(LSB_RELEASE_CODENAME_SHORT_RESULT_VARIABLE EQUAL 0) 70 | set(UNIX_DISTRIBUTION_CODENAME 71 | "${LSB_RELEASE_CODENAME_SHORT_OUTPUT_VARIABLE}" 72 | ) 73 | endif() 74 | 75 | if(NOT UNIX_DISTRIBUTION_CODENAME MATCHES "^(focal|jammy)$") 76 | message(FATAL_ERROR 77 | "Release ${UNIX_DISTRIBUTION_CODENAME} is NOT supported. Please use " 78 | "Ubuntu 18.04 (Bionic) or 20.04 (Focal)." 79 | ) 80 | endif() 81 | endif() 82 | 83 | if(APPLE) 84 | set(FIND_PYTHON_EXECUTABLE_PATHS /usr/local/bin) 85 | set(FIND_PYTHON_INTERP_VERSION 3.11) 86 | elseif(UNIX_DISTRIBUTION_CODENAME STREQUAL focal) 87 | set(FIND_PYTHON_EXECUTABLE_PATHS /usr/bin) 88 | set(FIND_PYTHON_INTERP_VERSION 3.8) 89 | else() 90 | # Jammy 91 | set(FIND_PYTHON_EXECUTABLE_PATHS /usr/bin) 92 | set(FIND_PYTHON_INTERP_VERSION 3.10) 93 | endif() 94 | find_program(PYTHON_EXECUTABLE NAMES python3 95 | PATHS "${FIND_PYTHON_EXECUTABLE_PATHS}" 96 | NO_DEFAULT_PATH 97 | ) 98 | find_package(PythonInterp ${FIND_PYTHON_INTERP_VERSION} MODULE REQUIRED) 99 | 100 | execute_process(COMMAND ${PYTHON_EXECUTABLE}-config --exec-prefix 101 | OUTPUT_VARIABLE PYTHON_EXEC_PREFIX 102 | OUTPUT_STRIP_TRAILING_WHITESPACE 103 | ) 104 | list(APPEND CMAKE_PREFIX_PATH "${PYTHON_EXEC_PREFIX}") 105 | find_package(PythonLibs ${FIND_PYTHON_INTERP_VERSION} MODULE REQUIRED) 106 | 107 | find_package(drake CONFIG REQUIRED) 108 | find_package(Threads REQUIRED) 109 | find_package(cppzmq REQUIRED) 110 | find_package(yaml-cpp REQUIRED yaml-cpp>0.5) 111 | 112 | get_filename_component(PYTHONPATH 113 | "${drake_DIR}/../../python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages" 114 | REALPATH 115 | ) 116 | 117 | include(ExternalProject) 118 | 119 | set(LCM_INCLUDE_PATH ${CMAKE_BINARY_DIR}/robot-plan-runner-lcmtypes) 120 | 121 | ExternalProject_Add(robot-plan-runner-lcmtypes 122 | SOURCE_DIR "${PROJECT_SOURCE_DIR}/lcmtypes" 123 | CMAKE_ARGS 124 | -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} 125 | -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} 126 | -DCMAKE_CXX_FLAGS:STRING=${CMAKE_CXX_FLAGS} 127 | -DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS} 128 | -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_INSTALL_PREFIX} 129 | -DCMAKE_PREFIX_PATH:PATH=${CMAKE_PREFIX_PATH} 130 | -DCMAKE_VERBOSE_MAKEFILE:BOOL=${CMAKE_VERBOSE_MAKEFILE} 131 | BINARY_DIR ${LCM_INCLUDE_PATH} 132 | BUILD_ALWAYS ON 133 | INSTALL_COMMAND : 134 | ) 135 | 136 | set(CMAKE_CXX_EXTENSIONS OFF) 137 | set(CMAKE_CXX_STANDARD 17) 138 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 139 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 140 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) 141 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) 142 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) 143 | 144 | add_subdirectory(src) 145 | add_subdirectory(config) 146 | add_subdirectory(third_party) 147 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot Plan Runner 2 | ![ci_badge](https://github.com/robotlocomotion/robot-plan-runner/actions/workflows/ci.yml/badge.svg) 3 | 4 | --- 5 | This repo requires cppzmq which can be installed by running 6 | ``` 7 | cd robot-plan-runner 8 | sudo ./scripts/install_cppzmq.sh 9 | ``` 10 | 11 | This repo uses the [CMake `find_package(drake)` mechanism](https://github.com/RobotLocomotion/drake-external-examples/tree/master/drake_cmake_installed) to find an 12 | installed instance of Drake. 13 | 14 | To include Drake path as a flag in CMake use `-DCMAKE_PREFIX_PATH`. For example, 15 | 16 | ``` 17 | cd robot-plan-runner 18 | mkdir build && cd build 19 | cmake -DCMAKE_PREFIX_PATH=/opt/drake .. 20 | make -j4 21 | ``` 22 | 23 | To use lcm types defined within plan runner, add the python path: 24 | `export PYTHONPATH={ROBOT_PLAN_RUNNER_DIR}/build/robot-plan-runner-lcmtypes:${PYTHONPATH}` 25 | 26 | To use the client as an external module, add the python path: 27 | `export PYTHONPATH={ROBOT_PLAN_RUNNER_DIR}/src:${PYTHONPATH}` 28 | 29 | 30 | An incomplete design documentation can be found [here](https://slides.com/pang/deck-36762e). 31 | 32 | 33 | ## Continuous Integration 34 | - CI is implemented by a custom Github Action that builds a docker image 35 | using `./jammy.dockerfile` and runs the entrypoint script `. 36 | /sciprts/buld_test.sh`. 37 | 38 | -------------------------------------------------------------------------------- /config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copy the default yaml file into the build directory so that the user doesn't 2 | # always have to provide config files through argv. 3 | file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/default.yaml 4 | DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) 5 | -------------------------------------------------------------------------------- /config/default.yaml: -------------------------------------------------------------------------------- 1 | # Communication Parameters 2 | lcm_status_channel: "IIWA_STATUS" 3 | lcm_command_channel: "IIWA_COMMAND" 4 | lcm_plan_channel: "ROBOT_PLAN" 5 | zmq_socket_plan: "5555" 6 | zmq_socket_abort: "5556" 7 | zmq_socket_status: "5557" # pub-sub for feedback status and result. 8 | status_channel_name: "PLAN_STATUS" 9 | staus_update_period_seconds: 0.01 10 | 11 | # Robot Parameters 12 | robot_sdf_path: "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf" 13 | robot_baselink_name: "iiwa_link_0" 14 | robot_ee_body_name: "iiwa_link_7" 15 | robot_nominal_joint: [0, 0.6, 0, -1.75, 0, 1.0, 0] # from manipulation station. 16 | 17 | # Control Parameters 18 | control_period: 0.005 19 | q_threshold: 0.05 # norm between current q and commanded q. 20 | 21 | # Print Parameters. 22 | print_period_seconds: 10. 23 | -------------------------------------------------------------------------------- /focal.dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | 3 | # Install curl and useful transport 4 | ENV DEBIAN_FRONTEND=noninteractive 5 | RUN apt-get update && yes "Y" \ 6 | | apt-get install --no-install-recommends curl apt-transport-https sudo \ 7 | ca-certificates libgtest-dev libgflags-dev python3.8-dev python-is-python3 \ 8 | libyaml-cpp-dev \ 9 | && rm -rf /var/lib/apt/lists/* \ 10 | && apt-get clean all 11 | 12 | # install drake. 13 | ENV DRAKE_URL=https://github.com/RobotLocomotion/drake/releases/download/v1.19.0/drake-20230713-focal.tar.gz 14 | RUN curl -L -o drake.tar.gz $DRAKE_URL 15 | RUN tar -xzf drake.tar.gz -C /opt && rm drake.tar.gz 16 | RUN apt-get update \ 17 | && yes "Y" | bash /opt/drake/share/drake/setup/install_prereqs \ 18 | && rm -rf /var/lib/apt/lists/* \ 19 | && apt-get clean all 20 | 21 | ## install cppzmq 22 | COPY scripts/install_cppzmq.sh /install_dependencies.sh 23 | RUN /bin/bash /install_dependencies.sh 24 | 25 | # put drake on the python path. 26 | ENV PYTHONPATH /opt/drake/lib/python3.8/site-packages:$PYTHONPATH 27 | -------------------------------------------------------------------------------- /jammy.dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | # Install curl and useful transport 4 | ENV DEBIAN_FRONTEND=noninteractive 5 | RUN apt-get update && yes "Y" \ 6 | | apt-get install --no-install-recommends curl apt-transport-https sudo \ 7 | ca-certificates libgtest-dev libgflags-dev python3.10-dev python-is-python3 \ 8 | libyaml-cpp-dev \ 9 | && rm -rf /var/lib/apt/lists/* \ 10 | && apt-get clean all 11 | 12 | # install drake. 13 | ENV DRAKE_URL=https://github.com/RobotLocomotion/drake/releases/download/v1.19.0/drake-20230713-jammy.tar.gz 14 | RUN curl -L -o drake.tar.gz $DRAKE_URL 15 | RUN tar -xzf drake.tar.gz -C /opt && rm drake.tar.gz 16 | RUN apt-get update \ 17 | && yes "Y" | bash /opt/drake/share/drake/setup/install_prereqs \ 18 | && rm -rf /var/lib/apt/lists/* \ 19 | && apt-get clean all 20 | 21 | ## install cppzmq 22 | COPY scripts/install_cppzmq.sh /install_dependencies.sh 23 | RUN /bin/bash /install_dependencies.sh 24 | 25 | # put drake on the python path. 26 | ENV PYTHONPATH /opt/drake/lib/python3.10/site-packages:$PYTHONPATH 27 | -------------------------------------------------------------------------------- /lcmtypes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(robot-plan-runner-lcmtypes) 3 | 4 | find_package(lcm REQUIRED) 5 | find_package(PythonInterp REQUIRED) 6 | include(${LCM_USE_FILE}) 7 | 8 | lcm_wrap_types( 9 | CPP_HEADERS lcm_cpp_headers CPP11 10 | PYTHON_SOURCES lcm_python_sources 11 | lcmt_plan_status.lcm lcmt_plan_status_constants.lcm) 12 | 13 | lcm_add_library(${PROJECT_NAME} CPP ${lcm_cpp_headers}) 14 | add_custom_target(GenLcmtypes ALL DEPENDS ${PROJECT_NAME}.sources) 15 | -------------------------------------------------------------------------------- /lcmtypes/lcmt_plan_status.lcm: -------------------------------------------------------------------------------- 1 | package robot_plan_runner; 2 | 3 | struct lcmt_plan_status 4 | { 5 | int64_t utime; // signature of plan. 6 | int32_t status; 7 | } 8 | -------------------------------------------------------------------------------- /lcmtypes/lcmt_plan_status_constants.lcm: -------------------------------------------------------------------------------- 1 | package robot_plan_runner; 2 | 3 | struct lcmt_plan_status_constants 4 | { 5 | const int32_t RUNNING=0, DISCARDED=1, ERROR=2, FINISHED=3; 6 | } 7 | -------------------------------------------------------------------------------- /scripts/build_test.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | set -euxo pipefail 3 | 4 | # build plan runner 5 | mkdir /robot-plan-runner-build && cd /robot-plan-runner-build 6 | cmake -DCMAKE_PREFIX_PATH=/opt/drake /github/workspace 7 | make -j 8 | 9 | # run tests 10 | ctest -V . 11 | -------------------------------------------------------------------------------- /scripts/install_cppzmq.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | set -euxo pipefail 3 | 4 | if [[ "${EUID}" -ne 0 ]]; then 5 | echo 'ERROR: This script must be run as root' >&2 6 | exit 1 7 | fi 8 | 9 | # install libzmq 10 | apt-get update 11 | apt-get install -y libzmq3-dev 12 | 13 | # install cppzmq 14 | curl -o cppzmq.tar.gz -L https://github.com/zeromq/cppzmq/archive/refs/tags/v4.7.1.tar.gz 15 | tar -xzf cppzmq.tar.gz -C /opt && rm cppzmq.tar.gz 16 | 17 | pushd /opt/cppzmq-4.7.1 18 | mkdir build 19 | pushd build 20 | cmake -DCPPZMQ_BUILD_TESTS=OFF .. 21 | make install 22 | 23 | popd 24 | popd 25 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(plans) 2 | add_subdirectory(state_machine) 3 | add_subdirectory(plan_manager_system) 4 | 5 | 6 | add_library(iiwa_plan_manager iiwa_plan_manager.h iiwa_plan_manager.cc) 7 | target_link_libraries(iiwa_plan_manager 8 | plan_manager_state_machine cppzmq iiwa_plan_factory Threads::Threads) 9 | target_include_directories(iiwa_plan_manager PUBLIC ${LCM_INCLUDE_PATH}) 10 | add_dependencies(iiwa_plan_manager robot-plan-runner-lcmtypes) 11 | 12 | add_executable(run_zmq_example run_zmq_example.cc) 13 | target_link_libraries(run_zmq_example cppzmq iiwa_plan_factory yaml-cpp) 14 | 15 | add_executable(run_plan_manager run_plan_manager.cc) 16 | target_link_libraries(run_plan_manager iiwa_plan_manager yaml-cpp) 17 | 18 | add_executable(run_plan_manager_system run_plan_manager_system.cc) 19 | target_link_libraries(run_plan_manager_system 20 | iiwa_plan_manager_hardware_interface yaml-cpp) 21 | 22 | add_test(NAME import_all_test COMMAND 23 | "${PYTHON_EXECUTABLE}" "${CMAKE_CURRENT_SOURCE_DIR}/import_all_test.py" 24 | ) 25 | set_tests_properties(import_all_test PROPERTIES 26 | ENVIRONMENT "PYTHONPATH=${PYTHONPATH}:${LCM_INCLUDE_PATH}" 27 | ) 28 | 29 | # Add sanbox folder if it exists. 30 | if(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/sandbox") 31 | add_subdirectory(sandbox) 32 | endif() 33 | -------------------------------------------------------------------------------- /src/examples/lcm_client_example.py: -------------------------------------------------------------------------------- 1 | import lcm 2 | import numpy as np 3 | 4 | from plan_runner_client.zmq_client import IiwaPositionGetter 5 | from plan_runner_client.calc_plan_msg import ( 6 | calc_joint_space_plan_msg, 7 | calc_task_space_plan_msg 8 | ) 9 | 10 | # This script shows how to send plan messages over LCM to 11 | # IiwaPlanManagerSystem, the drake systems wrapper of IiwaPlanManager. 12 | 13 | lc = lcm.LCM() 14 | 15 | # This is imported just to query for current joint angles. 16 | iiwa_position_getter = IiwaPositionGetter() 17 | 18 | #%% Tests joint space plan. 19 | t_knots = np.array([0, 10]) 20 | 21 | # This assumes that the simulation is running and lcm messages are being 22 | # published in IIWA_STATUS. 23 | q0 = iiwa_position_getter.get_iiwa_position_measured() 24 | if (q0[0] == None): 25 | raise RuntimeError("No messages were detected in IIWA_STATUS. " + 26 | "Is the simulation runnning?") 27 | q_knots1 = np.zeros((2, 7)) 28 | q_knots1[:, ] = q0 29 | q_knots1[1, 0] += 1 30 | 31 | q_knots2 = np.zeros((2, 7)) 32 | q_knots2[0] = q_knots1[1] 33 | q_knots2[1] = q_knots1[0] 34 | 35 | #%% send msg 1 via lcm. 36 | lc.publish("ROBOT_PLAN", calc_joint_space_plan_msg(t_knots, q_knots1).encode()) 37 | 38 | #%% send msg 2 via lcm. 39 | lc.publish("ROBOT_PLAN", calc_joint_space_plan_msg(t_knots, q_knots2).encode()) 40 | -------------------------------------------------------------------------------- /src/examples/simple_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import zmq 3 | import lcm 4 | import time, copy 5 | 6 | from drake import lcmt_robot_state 7 | from drake import lcmt_robot_plan 8 | 9 | from plan_runner_client.zmq_client import PlanManagerZmqClient, SchunkManager 10 | from plan_runner_client.calc_plan_msg import ( 11 | calc_task_space_plan_msg, 12 | calc_joint_space_plan_msg 13 | ) 14 | 15 | from pydrake.math import RigidTransform, RollPitchYaw 16 | from pydrake.common.eigen_geometry import Quaternion 17 | from pydrake.trajectories import PiecewisePolynomial 18 | 19 | 20 | def normalized(x): 21 | return x / np.linalg.norm(x) 22 | 23 | 24 | zmq_client = PlanManagerZmqClient() 25 | schunk = SchunkManager() 26 | frame_E = zmq_client.plant.GetFrameByName('iiwa_link_7') 27 | X_ET = RigidTransform(RollPitchYaw(0, -np.pi / 2, 0), np.array([0, 0, 0.255])) 28 | 29 | # Step 1. Check that the correct Cartesian position is valid and kinematics 30 | # are working correctly. 31 | q_now = zmq_client.get_current_joint_angles() 32 | X_WE = zmq_client.get_current_ee_pose(frame_E) 33 | print("Current joint angles:") 34 | print(q_now) 35 | print("Current end-effector position:") 36 | print(X_WE) 37 | time.sleep(1.0) 38 | 39 | schunk.send_schunk_position_command(25) 40 | schunk.wait_for_command_to_finish() 41 | time.sleep(5.0) 42 | schunk.send_schunk_position_command(50) 43 | schunk.wait_for_command_to_finish() 44 | time.sleep(5.0) 45 | schunk.send_schunk_position_command(0) 46 | schunk.wait_for_command_to_finish() 47 | time.sleep(5.0) 48 | 49 | # Step 2. Test every joint. 50 | print("Testing joint-space plan by joint flexing.") 51 | duration = 30 52 | t_knots = np.linspace(0, duration, 22) 53 | q_knots = np.zeros((22, 7)) 54 | 55 | q_knots[0, :] = zmq_client.get_current_joint_angles() 56 | for i in range(7): 57 | q_knots[3 * i + 1, :] = q_knots[3 * i, :] 58 | q_knots[3 * i + 1, i] += 0.2 59 | q_knots[3 * i + 2, :] = q_knots[3 * i + 1, :] 60 | q_knots[3 * i + 2, i] -= 0.4 61 | q_knots[3 * i + 3, :] = q_knots[3 * i + 2, :] 62 | q_knots[3 * i + 3, i] += 0.2 63 | 64 | plan_msg1 = calc_joint_space_plan_msg(t_knots, q_knots) 65 | zmq_client.send_plan(plan_msg1) 66 | time.sleep(1.0) 67 | zmq_client.wait_for_plan_to_finish() 68 | time.sleep(2.0) 69 | 70 | # Step 3. Go to basic position for task-space demo. 71 | duration = 2 72 | t_knots = np.linspace(0, duration, 2) 73 | q_knots = np.zeros((2, 7)) 74 | 75 | q_knots[0, :] = zmq_client.get_current_joint_angles() 76 | q_knots[1, :] = [0, 0.3, 0.0, -1.75, 0.0, 1.0, 0.0] 77 | 78 | plan_msg2 = calc_joint_space_plan_msg(t_knots, q_knots) 79 | zmq_client.send_plan(plan_msg2) 80 | time.sleep(1.0) 81 | zmq_client.wait_for_plan_to_finish() 82 | 83 | time.sleep(2.0) 84 | 85 | # Step 4. Go to basic position for task-space demo. 86 | duration = 2 87 | t_knots = np.linspace(0, duration, 2) 88 | 89 | X_WT_lst = [] 90 | X_WT_lst.append(zmq_client.get_current_ee_pose(frame_E)) 91 | X_WT_lst.append(RigidTransform(RollPitchYaw([0, -np.pi, 0]), 92 | np.array([0.5, 0.0, 0.5]))) 93 | 94 | plan_msg2 = calc_task_space_plan_msg(RigidTransform(), X_WT_lst, t_knots) 95 | zmq_client.send_plan(plan_msg2) 96 | zmq_client.wait_for_plan_to_finish() 97 | 98 | # Step 4. Test Cartesian flexing. 99 | duration = 40 100 | div = 5 101 | t_knots = np.linspace(0, duration, 1 + div * 6) 102 | q_knots = np.zeros((1 + div * 6, 6)) 103 | 104 | q_knots[0, :] = np.array([0, -np.pi, 0, 0.5, 0.0, 0.5]) 105 | 106 | for i in range(0, 3): 107 | q_knots[div * i + 1, :] = q_knots[div * i, :] 108 | q_knots[div * i + 1, i] += 0.3 109 | q_knots[div * i + 2, :] = q_knots[div * i + 1, :] 110 | q_knots[div * i + 3, :] = q_knots[div * i + 2, :] 111 | q_knots[div * i + 3, i] -= 0.6 112 | q_knots[div * i + 4, :] = q_knots[div * i + 3, :] 113 | q_knots[div * i + 5, :] = q_knots[div * i + 4, :] 114 | q_knots[div * i + 5, i] += 0.3 115 | 116 | for i in range(3, 6): 117 | q_knots[div * i + 1, :] = q_knots[div * i, :] 118 | q_knots[div * i + 1, i] += 0.1 119 | q_knots[div * i + 2, :] = q_knots[div * i + 1, :] 120 | q_knots[div * i + 3, :] = q_knots[div * i + 2, :] 121 | q_knots[div * i + 3, i] -= 0.2 122 | q_knots[div * i + 4, :] = q_knots[div * i + 3, :] 123 | q_knots[div * i + 5, :] = q_knots[div * i + 4, :] 124 | q_knots[div * i + 5, i] += 0.1 125 | 126 | X_WT_lst = [] 127 | for i in range(1 + div * 6): 128 | X_WT_lst.append(RigidTransform( 129 | RollPitchYaw(q_knots[i, 0:3]), q_knots[i, 3:6])) 130 | 131 | plan_msg3 = calc_task_space_plan_msg(RigidTransform(), X_WT_lst, t_knots) 132 | zmq_client.send_plan(plan_msg3) 133 | time.sleep(1.0) 134 | zmq_client.wait_for_plan_to_finish() 135 | -------------------------------------------------------------------------------- /src/examples/zmq_client_example.py: -------------------------------------------------------------------------------- 1 | import time 2 | import argparse 3 | 4 | import numpy as np 5 | from pydrake.all import RigidTransform 6 | 7 | from plan_runner_client.calc_plan_msg import ( 8 | calc_joint_space_plan_msg, 9 | calc_task_space_plan_msg 10 | ) 11 | from plan_runner_client.zmq_client import PlanManagerZmqClient 12 | 13 | zmq_client = PlanManagerZmqClient() 14 | 15 | t_knots = np.array([0, 10]) 16 | q0 = zmq_client.get_current_joint_angles() 17 | 18 | if len(q0) == 0: 19 | raise RuntimeError("No messages were detected in IIWA_STATUS. " + 20 | "Is the simulation runnning?") 21 | 22 | q_knots1 = np.zeros((2, 7)) 23 | q_knots1[:, ] = q0 24 | q_knots1[1, 0] += 1 25 | 26 | q_knots2 = np.zeros((2, 7)) 27 | q_knots2[0] = q_knots1[1] 28 | q_knots2[1] = q_knots1[0] 29 | duration = 5.0 30 | 31 | 32 | def run_joint_space_plan(): 33 | """Test joint space plan.""" 34 | plan_msg = calc_joint_space_plan_msg([0, duration], q_knots1) 35 | zmq_client.send_plan(plan_msg) 36 | time.sleep(1.0) 37 | zmq_client.wait_for_plan_to_finish() 38 | 39 | 40 | def run_joint_space_plan_abort(): 41 | """Test joint space plan with abort.""" 42 | plan_msg = calc_joint_space_plan_msg([0, duration], q_knots1) 43 | zmq_client.send_plan(plan_msg) 44 | time.sleep(3.0) 45 | # Calling wait for plan to finish immediately after calling abort should 46 | # return FINISHED. 47 | zmq_client.abort() 48 | zmq_client.wait_for_plan_to_finish() 49 | 50 | 51 | def run_task_space_plan(): 52 | """Test task space plan.""" 53 | frame_E = zmq_client.plant.GetFrameByName('iiwa_link_7') 54 | X_ET = RigidTransform() 55 | X_ET.set_translation([0.1, 0, 0]) 56 | X_WE0 = zmq_client.get_current_ee_pose(frame_E) 57 | X_WT0 = X_WE0.multiply(X_ET) 58 | X_WT1 = RigidTransform(X_WT0.rotation(), 59 | X_WT0.translation() + np.array([0, 0.2, 0])) 60 | plan_msg = calc_task_space_plan_msg(X_ET, [X_WT0, X_WT1], [0, 5]) 61 | zmq_client.send_plan(plan_msg) 62 | 63 | 64 | def run_joint_space_plan_loop(): 65 | """ 66 | Send random joint space plan, randomly interrupt, return to home, 67 | and start over. 68 | """ 69 | # TODO: support WaitForServer, which blocks until the server is in state 70 | # IDLE. 71 | while True: 72 | print("plan sent") 73 | plan_msg = calc_joint_space_plan_msg([0, duration], q_knots1) 74 | zmq_client.send_plan(plan_msg) 75 | print("pretending to do some work") 76 | stop_duration = np.random.rand() * duration 77 | time.sleep(stop_duration) 78 | zmq_client.abort() 79 | print("plan aborted after t = {}s".format(stop_duration)) 80 | 81 | # # get current robot position. 82 | q_now = zmq_client.iiwa_position_getter.get_iiwa_position_measured() 83 | q_knots_return = np.vstack([q_now, q0]) 84 | plan_msg = calc_joint_space_plan_msg([0, stop_duration], q_knots_return) 85 | zmq_client.send_plan(plan_msg) 86 | print("Returning to q0.") 87 | zmq_client.wait_for_plan_to_finish() 88 | print("returned to q0.") 89 | print("------------------------------------------------------") 90 | 91 | 92 | def generate_doc_string(test_lst): 93 | doc_string = "Input an integer corresponding to the test.\n" 94 | for i in range(len(test_lst)): 95 | doc_string += str(i) + ": " 96 | doc_string += "[" + test_lst[i].__name__ + "] : " 97 | doc_string += test_lst[i].__doc__ 98 | doc_string += "\n" 99 | return doc_string 100 | 101 | 102 | if __name__ == "__main__": 103 | # New tests should be added in the list. 104 | test_lst = [ 105 | run_joint_space_plan, 106 | run_joint_space_plan_abort, 107 | run_task_space_plan, 108 | run_joint_space_plan_loop, 109 | ] 110 | 111 | parser = argparse.ArgumentParser( 112 | formatter_class=argparse.RawTextHelpFormatter) 113 | parser.add_argument("test_type", type=int, 114 | help=generate_doc_string(test_lst)) 115 | 116 | args = parser.parse_args() 117 | test_lst[args.test_type]() 118 | -------------------------------------------------------------------------------- /src/iiwa_plan_manager.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "drake_lcmtypes/drake/lcmt_iiwa_command.hpp" 5 | #include "drake_lcmtypes/drake/lcmt_robot_plan.hpp" 6 | #include "drake_lcmtypes/drake/lcmt_robot_state.hpp" 7 | 8 | #include "iiwa_plan_manager.h" 9 | #include "plans/plan_base.h" 10 | #include "robot_plan_runner/lcmt_plan_status.hpp" 11 | #include "robot_plan_runner/lcmt_plan_status_constants.hpp" 12 | 13 | using Eigen::VectorXd; 14 | using robot_plan_runner::lcmt_plan_status; 15 | using robot_plan_runner::lcmt_plan_status_constants; 16 | using std::cout; 17 | using std::endl; 18 | 19 | IiwaPlanManager::IiwaPlanManager(YAML::Node config) 20 | : config_(std::move(config)), 21 | control_period_seconds_(config["control_period"].as()) { 22 | double t_now_seconds = 23 | std::chrono::duration_cast( 24 | std::chrono::high_resolution_clock::now().time_since_epoch()) 25 | .count(); 26 | state_machine_ = 27 | std::make_unique(t_now_seconds, config_); 28 | plan_factory_ = std::make_unique(config_); 29 | 30 | // TODO(terry-suh): make another method here to check for errors in the config 31 | // file. are all the required fields there? 32 | } 33 | 34 | IiwaPlanManager::~IiwaPlanManager() { 35 | // Wait for all threads to terminate. 36 | for (auto &a : threads_) { 37 | if (a.second.joinable()) { 38 | a.second.join(); 39 | } 40 | } 41 | } 42 | 43 | void IiwaPlanManager::Run() { 44 | threads_["status_command"] = 45 | std::thread(&IiwaPlanManager::CalcCommandFromStatus, this); 46 | threads_["print_status"] = 47 | std::thread(&IiwaPlanManager::PrintStateMachineStatus, this); 48 | threads_["receive_plans"] = 49 | std::thread(&IiwaPlanManager::ReceivePlanAndPublishPlanStatus, this); 50 | threads_["cancel_plans"] = std::thread(&IiwaPlanManager::AbortPlans, this); 51 | } 52 | 53 | void IiwaPlanManager::CalcCommandFromStatus() { 54 | lcm_status_command_ = std::make_unique(); 55 | auto sub = lcm_status_command_->subscribe( 56 | config_["lcm_status_channel"].as(), 57 | &IiwaPlanManager::HandleIiwaStatus, this); 58 | sub->setQueueCapacity(1); 59 | while (true) { 60 | // >0 if a message was handled, 61 | // 0 if the function timed out, 62 | // <0 if an error occured. 63 | if (lcm_status_command_->handleTimeout(10) < 0) { 64 | break; 65 | } 66 | } 67 | } 68 | 69 | [[noreturn]] void IiwaPlanManager::PrintStateMachineStatus() const { 70 | TimePoint current_start_time = std::chrono::high_resolution_clock::now(); 71 | TimePoint next_start_time(current_start_time); 72 | const auto interval = std::chrono::milliseconds( 73 | static_cast(config_["print_period_seconds"].as() * 1000)); 74 | while (true) { 75 | current_start_time = std::chrono::high_resolution_clock::now(); 76 | next_start_time = current_start_time + interval; 77 | double t_now_seconds = std::chrono::duration_cast( 78 | current_start_time.time_since_epoch()) 79 | .count(); 80 | state_machine_->PrintCurrentState(t_now_seconds); 81 | std::this_thread::sleep_until(next_start_time); 82 | } 83 | } 84 | 85 | // Constructs a vector of bytes consisting of the channel name and the 86 | // encoded LCM message separated by a space: 87 | // (channel_name, ' ', encoded LCM message) 88 | std::vector PrependLcmMsgWithChannel(const std::string &channel_name, 89 | const lcmt_plan_status &msg) { 90 | const int data_len = msg.getEncodedSize(); 91 | const int channel_len = channel_name.size(); 92 | 93 | std::vector msg_full_bytes(data_len + channel_len + 1); 94 | for (size_t i = 0; i < channel_len; i++) { 95 | msg_full_bytes[i] = channel_name[i]; 96 | } 97 | msg_full_bytes[channel_len] = ' '; 98 | msg.encode(msg_full_bytes.data(), channel_len + 1, data_len); 99 | 100 | return msg_full_bytes; 101 | } 102 | 103 | [[noreturn]] void IiwaPlanManager::ReceivePlanAndPublishPlanStatus() { 104 | const std::string addr_prefix("tcp://*:"); 105 | zmq::socket_t plan_server(zmq_ctx_, zmq::socket_type::rep); 106 | plan_server.bind(addr_prefix + config_["zmq_socket_plan"].as()); 107 | 108 | zmq::socket_t status_publisher(zmq_ctx_, zmq::socket_type::pub); 109 | status_publisher.bind(addr_prefix + 110 | config_["zmq_socket_status"].as()); 111 | 112 | const auto channel_name_string = 113 | config_["status_channel_name"].as(); 114 | 115 | const auto status_update_period = std::chrono::milliseconds(static_cast( 116 | config_["staus_update_period_seconds"].as() * 1000)); 117 | 118 | while (true) { 119 | zmq::message_t plan_msg; 120 | // Blocks until a plan msg is received. 121 | auto res = plan_server.recv(plan_msg, zmq::recv_flags::none); 122 | 123 | drake::lcmt_robot_plan plan_lcm_msg; 124 | plan_lcm_msg.decode(plan_msg.data(), 0, plan_msg.size()); 125 | auto plan = plan_factory_->MakePlan(plan_lcm_msg); 126 | 127 | { 128 | std::lock_guard lock(mutex_state_machine_); 129 | state_machine_->QueueNewPlan(std::move(plan)); 130 | } 131 | 132 | // TODO: have QueueNewPlan return whether the plan is successfully added 133 | // or just discarded (due to state machine being in a state other than 134 | // IDLE). 135 | res = plan_server.send(zmq::str_buffer("plan_received"), 136 | zmq::send_flags::none); 137 | 138 | // Handle received plan. 139 | lcmt_plan_status msg_plan_status{}; 140 | msg_plan_status.utime = plan_lcm_msg.utime; 141 | TimePoint current_start_time = std::chrono::high_resolution_clock::now(); 142 | TimePoint next_start_time(current_start_time); 143 | while (true) { 144 | current_start_time = std::chrono::high_resolution_clock::now(); 145 | next_start_time = current_start_time + status_update_period; 146 | 147 | const auto current_state = state_machine_->get_state_type(); 148 | switch (current_state) { 149 | case PlanManagerStateTypes::kStateRunning: { 150 | msg_plan_status.status = lcmt_plan_status_constants::RUNNING; 151 | break; 152 | } 153 | case PlanManagerStateTypes::kStateIdle: { 154 | // TODO: aborted plans and successfully finished plans are not 155 | // distinguished, both are designated "FINISHED". 156 | // On the other hand, if the client calls abort, it should know that 157 | // the plan doesn't finish normally? 158 | msg_plan_status.status = lcmt_plan_status_constants::FINISHED; 159 | break; 160 | } 161 | case PlanManagerStateTypes::kStateError: { 162 | msg_plan_status.status = lcmt_plan_status_constants::ERROR; 163 | break; 164 | } 165 | case PlanManagerStateTypes::kStateInit: { 166 | msg_plan_status.status = lcmt_plan_status_constants::DISCARDED; 167 | break; 168 | } 169 | } 170 | auto reply_msg = 171 | PrependLcmMsgWithChannel(channel_name_string, msg_plan_status); 172 | zmq::message_t reply(reply_msg.begin(), reply_msg.end()); 173 | status_publisher.send(reply, zmq::send_flags::none); 174 | 175 | if (current_state != PlanManagerStateTypes::kStateRunning) { 176 | break; 177 | } 178 | std::this_thread::sleep_until(next_start_time); 179 | } 180 | } 181 | } 182 | 183 | void IiwaPlanManager::HandleIiwaStatus( 184 | const lcm::ReceiveBuffer *, const std::string &channel, 185 | const drake::lcmt_iiwa_status *status_msg) { 186 | const PlanBase *plan; 187 | auto t_now = std::chrono::high_resolution_clock::now(); 188 | const int num_joints = (*status_msg).num_joints; 189 | State s(Eigen::Map( 190 | (*status_msg).joint_position_measured.data(), num_joints), 191 | Eigen::Map( 192 | (*status_msg).joint_velocity_estimated.data(), num_joints), 193 | Eigen::Map((*status_msg).joint_torque_external.data(), 194 | num_joints)); 195 | Command c; 196 | bool command_has_error; 197 | { 198 | // Lock state machine. 199 | std::lock_guard lock(mutex_state_machine_); 200 | 201 | /* Get current plan. 202 | * Init: return nullptr. throws if state_machine.plans_ is not empty. 203 | * Idle: Ditto. 204 | * Error: Ditto. 205 | * Running: 206 | * - If this is the first control tick of a new plan: 207 | * - set state_machine.current_plan_start_time_seconds_ to the 208 | * current time, return state_machine.plans_.front(). 209 | * - If the current plan has ended (by comparing its uptime 210 | * against its duration): 211 | * - set state_machine.current_plan_start_time_seconds_ to the 212 | * current time (not useful). 213 | * - remove the current plan from state_machine.plans_ 214 | * 215 | * - If state_machine.plans_ is empty: 216 | * - set state_machine.current_plan_start_time_seconds_ to nullptr. 217 | * - change state to IDLE. 218 | */ 219 | plan = state_machine_->GetCurrentPlan(t_now, *status_msg); 220 | 221 | /* 222 | * ReceiveNewStatusMsg. 223 | * Init: 224 | * - set state_machine.iiwa_position_command_idle_ to 225 | * status_msg.joint_position_measured. 226 | * - change state to IDLE. 227 | * Idle: 228 | * - do nothing. 229 | * Running: 230 | * - do nothing. 231 | * Error: 232 | * - do nothing. 233 | */ 234 | state_machine_->ReceiveNewStatusMsg(*status_msg); 235 | 236 | // Compute command. 237 | if (plan) { 238 | const double t_plan = state_machine_->GetCurrentPlanUpTime(t_now); 239 | plan->Step(s, control_period_seconds_, t_plan, &c); 240 | } else if (state_machine_->get_state_type() == 241 | PlanManagerStateTypes::kStateIdle) { 242 | c.q_cmd = state_machine_->get_iiwa_position_command_idle(); 243 | c.tau_cmd = Eigen::VectorXd::Zero(num_joints); 244 | } else { 245 | // No commands are sent in state INIT or ERROR. 246 | return; 247 | } 248 | 249 | // Check command for error. 250 | command_has_error = state_machine_->CommandHasError(s, c); 251 | } 252 | 253 | if (!command_has_error) { 254 | drake::lcmt_iiwa_command cmd_msg; 255 | cmd_msg.num_joints = num_joints; 256 | cmd_msg.num_torques = num_joints; 257 | cmd_msg.utime = status_msg->utime; 258 | for (int i = 0; i < num_joints; i++) { 259 | cmd_msg.joint_position.push_back(c.q_cmd[i]); 260 | cmd_msg.joint_torque.push_back(c.tau_cmd[i]); 261 | } 262 | lcm_status_command_->publish( 263 | config_["lcm_command_channel"].as(), &cmd_msg); 264 | state_machine_->SetIiwaPositionCommandIdle(c.q_cmd); 265 | } 266 | } 267 | 268 | [[noreturn]] void IiwaPlanManager::AbortPlans() { 269 | zmq::socket_t abort_server(zmq_ctx_, zmq::socket_type::rep); 270 | abort_server.bind("tcp://*:" + config_["zmq_socket_abort"].as()); 271 | 272 | zmq::message_t msg; 273 | while (true) { 274 | auto res = abort_server.recv(msg, zmq::recv_flags::none); 275 | { 276 | std::lock_guard lock(mutex_state_machine_); 277 | state_machine_->AbortAllPlans(); 278 | } 279 | res = abort_server.send(zmq::str_buffer("plans_aborted"), 280 | zmq::send_flags::none); 281 | } 282 | } 283 | -------------------------------------------------------------------------------- /src/iiwa_plan_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "drake_lcmtypes/drake/lcmt_iiwa_status.hpp" 9 | #include "lcm/lcm-cpp.hpp" 10 | 11 | #include "plans/iiwa_plan_factory.h" 12 | #include "state_machine/plan_manager_state_machine.h" 13 | 14 | class IiwaPlanManager { 15 | public: 16 | IiwaPlanManager(YAML::Node config); 17 | ~IiwaPlanManager(); 18 | void Run(); 19 | 20 | private: 21 | const double control_period_seconds_; 22 | mutable std::mutex mutex_state_machine_; 23 | std::unique_ptr state_machine_; 24 | std::unordered_map threads_; 25 | std::unique_ptr plan_factory_; 26 | 27 | const YAML::Node config_; 28 | // This context is used in multiple threads. zmq context should be 29 | // thread-safe, according to their documentation... 30 | zmq::context_t zmq_ctx_; 31 | 32 | // Iiwa status + command thread. 33 | std::unique_ptr lcm_status_command_; 34 | void CalcCommandFromStatus(); 35 | void HandleIiwaStatus(const lcm::ReceiveBuffer *rbuf, 36 | const std::string &channel, 37 | const drake::lcmt_iiwa_status *status_msg); 38 | 39 | // Printing thread. 40 | [[noreturn]] void PrintStateMachineStatus() const; 41 | 42 | // Robot plans thread. 43 | [[noreturn]] void ReceivePlanAndPublishPlanStatus(); 44 | 45 | // Cancel plans thread. 46 | [[noreturn]] void AbortPlans(); 47 | }; 48 | -------------------------------------------------------------------------------- /src/import_all_test.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020, Toyota Research Institute. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # * Neither the name of the copyright holder nor the names of its contributors 15 | # may be used to endorse or promote products derived from this software 16 | # without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """ 31 | Provides an example of importing all modules available in pydrake. 32 | """ 33 | 34 | import pydrake.all 35 | from robot_plan_runner import lcmt_plan_status, lcmt_plan_status_constants 36 | -------------------------------------------------------------------------------- /src/plan_manager_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(iiwa_plan_manager_system 2 | iiwa_plan_manager_system.h iiwa_plan_manager_system.cc) 3 | target_link_libraries(iiwa_plan_manager_system 4 | plan_manager_state_machine iiwa_plan_factory) 5 | target_include_directories(iiwa_plan_manager_system 6 | PUBLIC ${PROJECT_SOURCE_DIR}/src) 7 | 8 | add_library(iiwa_plan_manager_hardware_interface 9 | iiwa_plan_manager_hardware_interface.h 10 | iiwa_plan_manager_hardware_interface.cc) 11 | target_link_libraries(iiwa_plan_manager_hardware_interface 12 | iiwa_plan_manager_system) 13 | -------------------------------------------------------------------------------- /src/plan_manager_system/iiwa_plan_manager_hardware_interface.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "drake/systems/analysis/simulator.h" 5 | #include "drake/systems/lcm/lcm_interface_system.h" 6 | 7 | #include "iiwa_plan_manager_hardware_interface.h" 8 | 9 | IiwaPlanManagerHardwareInterface::IiwaPlanManagerHardwareInterface( 10 | const YAML::Node &config) { 11 | owned_lcm_ = std::make_unique(); 12 | 13 | drake::systems::DiagramBuilder builder; 14 | 15 | // PlanManagerSystem. 16 | plan_manager_ = builder.template AddSystem(config); 17 | diagram_ = builder.Build(); 18 | 19 | // A standalone subscriber to IIWA_STATUS, used for lcm-driven loop. 20 | status_sub_ = 21 | std::make_unique>( 22 | owned_lcm_.get(), config["lcm_status_channel"].as()); 23 | 24 | // A standalone subscriber to ROBOT_PLAN. 25 | plan_sub_ = 26 | std::make_unique>( 27 | owned_lcm_.get(), config["lcm_plan_channel"].as()); 28 | } 29 | 30 | [[noreturn]] void IiwaPlanManagerHardwareInterface::Run() { 31 | drake::systems::Simulator sim(*diagram_); 32 | auto& context_diagram = sim.get_mutable_context(); 33 | auto& context_manager = 34 | plan_manager_->GetMyMutableContextFromRoot(&context_diagram); 35 | 36 | // Wait for the first IIWA_STATUS message. 37 | drake::log()->info("Waiting for first lcmt_iiwa_status"); 38 | drake::lcm::LcmHandleSubscriptionsUntil( 39 | owned_lcm_.get(), [&]() { return status_sub_->count() > 0; }); 40 | auto &status_value = plan_manager_->get_iiwa_status_input_port().FixValue( 41 | &context_manager, status_sub_->message()); 42 | auto &plan_value = plan_manager_->get_robot_plan_input_port().FixValue( 43 | &context_manager, drake::lcmt_robot_plan()); 44 | const double t_start = status_sub_->message().utime * 1e-6; 45 | 46 | drake::log()->info("Controller started"); 47 | while (true) { 48 | status_sub_->clear(); 49 | // Wait for an IIWA_STATUS message. 50 | drake::lcm::LcmHandleSubscriptionsUntil( 51 | owned_lcm_.get(), [&]() { return status_sub_->count() > 0; }); 52 | 53 | // Update diagram context. 54 | status_value.GetMutableData()->set_value(status_sub_->message()); 55 | if (plan_sub_->count() > 0) { 56 | plan_value.GetMutableData()->set_value(plan_sub_->message()); 57 | plan_sub_->clear(); 58 | } 59 | 60 | // Let Simulator handle other non-time-critical events. 61 | const double t = status_sub_->message().utime * 1e-6 - t_start; 62 | sim.AdvanceTo(t); 63 | 64 | // Compute command message and publish. 65 | const auto& iiwa_cmd_msg = plan_manager_->get_iiwa_command_output_port() 66 | .Eval(context_manager); 67 | drake::lcm::Publish(owned_lcm_.get(), "IIWA_COMMAND", iiwa_cmd_msg); 68 | } 69 | } 70 | 71 | void IiwaPlanManagerHardwareInterface::SaveGraphvizStringToFile( 72 | const std::string &file_name) { 73 | if (diagram_) { 74 | std::ofstream out(file_name); 75 | out << diagram_->GetGraphvizString(); 76 | out.close(); 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /src/plan_manager_system/iiwa_plan_manager_hardware_interface.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "drake/lcm/drake_lcm.h" 4 | #include "drake/systems/framework/diagram_builder.h" 5 | 6 | #include "plan_manager_system/iiwa_plan_manager_system.h" 7 | 8 | class IiwaPlanManagerHardwareInterface { 9 | public: 10 | explicit IiwaPlanManagerHardwareInterface(const YAML::Node &config); 11 | 12 | [[noreturn]] void Run(); 13 | 14 | /* 15 | * Saves the graphviz string which describes this system to a file. 16 | */ 17 | void SaveGraphvizStringToFile( 18 | const std::string &file_name = "system_graphviz_string.txt"); 19 | 20 | private: 21 | std::unique_ptr> diagram_; 22 | std::unique_ptr owned_lcm_; 23 | std::unique_ptr> status_sub_; 24 | std::unique_ptr> plan_sub_; 25 | IiwaPlanManagerSystem *plan_manager_{nullptr}; 26 | }; 27 | -------------------------------------------------------------------------------- /src/plan_manager_system/iiwa_plan_manager_system.cc: -------------------------------------------------------------------------------- 1 | #include "drake/common/value.h" 2 | #include 3 | 4 | #include "plan_manager_system/iiwa_plan_manager_system.h" 5 | 6 | using drake::lcmt_iiwa_command; 7 | using drake::lcmt_iiwa_status; 8 | using drake::lcmt_robot_plan; 9 | using drake::systems::BasicVector; 10 | using Eigen::VectorXd; 11 | 12 | IiwaPlanManagerSystem::IiwaPlanManagerSystem(YAML::Node config) 13 | : config_(std::move(config)), 14 | control_period_seconds_(config["control_period"].as()) { 15 | // Initialize state machine, plan factory. 16 | state_machine_ = std::make_unique(0, config_); 17 | plan_factory_ = std::make_unique(config_); 18 | 19 | // Printing events. 20 | set_name("IiwaPlanManagerSystem"); 21 | DeclarePeriodicPublishEvent(1.0, 0, 22 | &IiwaPlanManagerSystem::PrintCurrentState); 23 | 24 | // Input ports. 25 | input_port_iiwa_status_idx_ = 26 | DeclareAbstractInputPort("lcmt_iiwa_status", 27 | *drake::AbstractValue::Make(lcmt_iiwa_status())) 28 | .get_index(); 29 | input_port_robot_plan_idx_ = 30 | DeclareAbstractInputPort("lcmt_robot_plan", 31 | *drake::AbstractValue::Make(lcmt_robot_plan())) 32 | .get_index(); 33 | 34 | // Output port. 35 | // The rate at which this output port is evaluated is driven by the 36 | // downstream system. In lcm interface, the update rate is defined in the 37 | // downstream LcmPublisherSystem. In simulation without lcm, an 38 | // abstract-valued ZeroOrderHold is needed to enforce the update rate. 39 | output_port_iiwa_command_idx_ = 40 | DeclareAbstractOutputPort("lcmt_iiwa_command", 41 | &IiwaPlanManagerSystem::CalcIiwaCommand) 42 | .get_index(); 43 | } 44 | void IiwaPlanManagerSystem::CalcIiwaCommand( 45 | const drake::systems::Context &context, 46 | drake::lcmt_iiwa_command *cmd) const { 47 | const auto &msg_iiwa_status = 48 | get_iiwa_status_input_port().Eval(context); 49 | const auto &msg_robot_plan = 50 | get_robot_plan_input_port().Eval(context); 51 | auto &msg_iiwa_command = *cmd; 52 | // Handle new robot plan messages. 53 | if (msg_robot_plan.num_states > 0 and 54 | msg_robot_plan.utime != last_robot_plan_utime_) { 55 | // has new message. 56 | auto plan = plan_factory_->MakePlan(msg_robot_plan); 57 | state_machine_->QueueNewPlan(std::move(plan)); 58 | last_robot_plan_utime_ = msg_robot_plan.utime; 59 | } 60 | 61 | // Handle new iiwa status messages. 62 | if (msg_iiwa_status.num_joints == 0) { 63 | return; 64 | } 65 | 66 | const double t_now = context.get_time(); 67 | auto plan = state_machine_->GetCurrentPlan(t_now, msg_iiwa_status); 68 | state_machine_->ReceiveNewStatusMsg(msg_iiwa_status); 69 | 70 | State s( 71 | Eigen::Map(msg_iiwa_status.joint_position_measured.data(), 72 | msg_iiwa_status.num_joints), 73 | Eigen::Map( 74 | msg_iiwa_status.joint_velocity_estimated.data(), 75 | msg_iiwa_status.num_joints), 76 | Eigen::Map(msg_iiwa_status.joint_torque_external.data(), 77 | msg_iiwa_status.num_joints)); 78 | Command c; 79 | 80 | if (plan) { 81 | const double t_plan = state_machine_->GetCurrentPlanUpTime(t_now); 82 | plan->Step(s, control_period_seconds_, t_plan, &c); 83 | } else if (state_machine_->get_state_type() == 84 | PlanManagerStateTypes::kStateIdle) { 85 | c.q_cmd = state_machine_->get_iiwa_position_command_idle(); 86 | c.tau_cmd = Eigen::VectorXd::Zero(msg_iiwa_status.num_joints); 87 | } else { 88 | // In state INIT or ERROR. 89 | // Send an empty iiwa command message with utime = -1. 90 | // Behavior with mock_station_simulation: 91 | // throws with error message: 92 | // IiwaCommandReceiver expected num_joints = 7, but received 0. 93 | // TODO: confirm the behavior of drake-iiwa-driver (real robot). 94 | // What I think will happen: 95 | // throws if msg_iiwa_command.num_joints != 7, unless msg_iiwa_command 96 | // .utime == -1, in which case no command is sent to the robot. 97 | msg_iiwa_command = lcmt_iiwa_command(); 98 | msg_iiwa_command.utime = -1; 99 | return; 100 | } 101 | 102 | // Check command for error. 103 | if (!state_machine_->CommandHasError(s, c)) { 104 | const int num_joints = msg_iiwa_status.num_joints; 105 | msg_iiwa_command.num_joints = num_joints; 106 | msg_iiwa_command.num_torques = num_joints; 107 | msg_iiwa_command.utime = msg_iiwa_status.utime; 108 | msg_iiwa_command.joint_position.resize(num_joints); 109 | msg_iiwa_command.joint_torque.resize(num_joints); 110 | for (int i = 0; i < num_joints; i++) { 111 | msg_iiwa_command.joint_position[i] = c.q_cmd[i]; 112 | msg_iiwa_command.joint_torque[i] = c.tau_cmd[i]; 113 | } 114 | } 115 | } 116 | 117 | void IiwaPlanManagerSystem::PrintCurrentState( 118 | const drake::systems::Context &context) const { 119 | state_machine_->PrintCurrentState(context.get_time()); 120 | } 121 | -------------------------------------------------------------------------------- /src/plan_manager_system/iiwa_plan_manager_system.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "drake/systems/framework/leaf_system.h" 6 | #include "drake_lcmtypes/drake/lcmt_iiwa_command.hpp" 7 | #include "drake_lcmtypes/drake/lcmt_iiwa_status.hpp" 8 | #include "drake_lcmtypes/drake/lcmt_robot_plan.hpp" 9 | 10 | #include "plans/iiwa_plan_factory.h" 11 | #include "state_machine/plan_manager_state_machine.h" 12 | 13 | 14 | class IiwaPlanManagerSystem : public drake::systems::LeafSystem { 15 | public: 16 | DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IiwaPlanManagerSystem); 17 | IiwaPlanManagerSystem(YAML::Node config); 18 | 19 | const drake::systems::InputPort &get_iiwa_status_input_port() const { 20 | return get_input_port(input_port_iiwa_status_idx_); 21 | } 22 | 23 | const drake::systems::InputPort &get_robot_plan_input_port() const { 24 | return get_input_port(input_port_robot_plan_idx_); 25 | } 26 | 27 | 28 | const drake::systems::OutputPort & 29 | get_iiwa_command_output_port() const { 30 | return get_output_port(output_port_iiwa_command_idx_); 31 | } 32 | 33 | private: 34 | void CalcIiwaCommand(const drake::systems::Context &context, 35 | drake::lcmt_iiwa_command *cmd) const; 36 | 37 | void PrintCurrentState(const drake::systems::Context &context) const; 38 | 39 | const double control_period_seconds_{}; 40 | std::unique_ptr plan_factory_; 41 | std::unique_ptr state_machine_; 42 | // signature of the last robot plan. 43 | // TODO: use the hash of the plan (concatenated q_knots as strings?) as the 44 | // signature. 45 | mutable long last_robot_plan_utime_{-1}; 46 | 47 | // input port and state indices. 48 | drake::systems::InputPortIndex input_port_iiwa_status_idx_; 49 | drake::systems::InputPortIndex input_port_robot_plan_idx_; 50 | drake::systems::OutputPortIndex output_port_iiwa_command_idx_; 51 | 52 | const YAML::Node config_; 53 | }; 54 | -------------------------------------------------------------------------------- /src/plan_runner_client/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotLocomotion/robot-plan-runner/52e3ffaded10b23a0f8ca0fd4221d94a6da6c9dd/src/plan_runner_client/__init__.py -------------------------------------------------------------------------------- /src/plan_runner_client/calc_plan_msg.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import numpy as np 4 | from drake import lcmt_robot_state 5 | from drake import lcmt_robot_plan 6 | 7 | 8 | def calc_joint_space_plan_msg(t_knots, q_knots): 9 | n_knots, n_q = q_knots.shape 10 | msg_plan = lcmt_robot_plan() 11 | 12 | # It is important that the utime of each plan is unique. The drake 13 | # systems version of PlanManager uses utime to tell if one plan is 14 | # different from another. 15 | msg_plan.utime = round(time.time() * 1000) 16 | 17 | joint_names = ["iiwa_joint_{}".format(i) for i in range(n_q)] 18 | 19 | msg_plan.num_states = n_knots 20 | for i, q_i in enumerate(q_knots): 21 | msg_state = lcmt_robot_state() 22 | msg_state.utime = int(1e6 * t_knots[i]) 23 | msg_state.num_joints = n_q 24 | msg_state.joint_name = joint_names 25 | msg_state.joint_position = q_i 26 | 27 | msg_plan.plan.append(msg_state) 28 | 29 | return msg_plan 30 | 31 | 32 | def calc_task_space_plan_msg(X_ET, X_WT_list, t_knots): 33 | n_knots = len(t_knots) 34 | msg_plan = lcmt_robot_plan() 35 | msg_plan.utime = round(time.time() * 1000) 36 | msg_plan.num_states = n_knots + 1 37 | joint_names = ["qw", "qx", "qy", "qz", "px", "py", "pz"] 38 | n_q = len(joint_names) 39 | 40 | # TODO: use our own lcm types. 41 | 42 | # The first n_knots msg_states are for the trajectory. 43 | for i, X_WTi in enumerate(X_WT_list): 44 | msg_state = lcmt_robot_state() 45 | msg_state.utime = int(1e6 * t_knots[i]) 46 | msg_state.num_joints = n_q 47 | msg_state.joint_name = joint_names 48 | msg_state.joint_position = np.hstack( 49 | [X_WTi.rotation().ToQuaternion().wxyz(), X_WTi.translation()]) 50 | 51 | msg_plan.plan.append(msg_state) 52 | 53 | # The last state in msg_plan encodes the offset between tool frame ( 54 | # T) and EE frame E. 55 | msg_state_ET = lcmt_robot_state() 56 | msg_state_ET.utime = 0 57 | msg_state_ET.num_joints = 7 58 | msg_state_ET.joint_name = joint_names 59 | msg_state_ET.joint_position = np.hstack( 60 | [X_ET.rotation().ToQuaternion().wxyz(), X_ET.translation()]) 61 | msg_plan.plan.append(msg_state_ET) 62 | 63 | return msg_plan 64 | -------------------------------------------------------------------------------- /src/plan_runner_client/zmq_client.py: -------------------------------------------------------------------------------- 1 | import time 2 | import threading 3 | import sys 4 | import copy 5 | import logging 6 | 7 | import zmq 8 | import lcm 9 | 10 | import numpy as np 11 | 12 | from pydrake.all import MultibodyPlant, Parser, RigidTransform 13 | from pydrake.common import FindResourceOrThrow 14 | from drake import ( 15 | lcmt_iiwa_status, lcmt_robot_plan, 16 | lcmt_schunk_wsg_status, lcmt_schunk_wsg_command) 17 | 18 | from robot_plan_runner import lcmt_plan_status, lcmt_plan_status_constants 19 | 20 | 21 | def build_iiwa7_plant(): 22 | plant = MultibodyPlant(1e-3) 23 | parser = Parser(plant=plant) 24 | 25 | iiwa_drake_path = ( 26 | "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf") 27 | iiwa_path = FindResourceOrThrow(iiwa_drake_path) 28 | robot_model = parser.AddModelFromFile(iiwa_path) 29 | 30 | # weld robot to world frame. 31 | plant.WeldFrames(frame_on_parent_P=plant.world_frame(), 32 | frame_on_child_C=plant.GetFrameByName("iiwa_link_0"), 33 | X_PC=RigidTransform.Identity()) 34 | plant.Finalize() 35 | 36 | return plant 37 | 38 | 39 | class IiwaPositionGetter: 40 | def __init__(self): 41 | self.lc = lcm.LCM() 42 | sub = self.lc.subscribe("IIWA_STATUS", self.sub_callback) 43 | sub.set_queue_capacity(1) 44 | self.iiwa_position_measured = None 45 | self.msg_lock = threading.Lock() 46 | self.t1 = threading.Thread(target=self.update_iiwa_position_measured) 47 | self.t1.start() 48 | 49 | # TODO: Set up logger (should really do it elsewhere...) 50 | self.logger = logging.getLogger() 51 | self.logger.setLevel(logging.DEBUG) 52 | handler = logging.StreamHandler(sys.stdout) 53 | handler.setLevel(logging.DEBUG) 54 | formatter = logging.Formatter('%(asctime)s - %(levelname)s - %(message)s') 55 | handler.setFormatter(formatter) 56 | self.logger.addHandler(handler) 57 | 58 | # Wait for IIWA_STATUS. 59 | self.logger.info("Waiting for iiwa Status...") 60 | while True: 61 | self.msg_lock.acquire() 62 | if self.iiwa_position_measured is not None: 63 | self.msg_lock.release() 64 | break 65 | 66 | self.msg_lock.release() 67 | time.sleep(0.005) # check at 200Hz 68 | self.logger.info("Received!") 69 | 70 | def sub_callback(self, channel, data): 71 | iiwa_status_msg = lcmt_iiwa_status.decode(data) 72 | self.msg_lock.acquire() 73 | self.iiwa_position_measured = iiwa_status_msg.joint_position_measured 74 | self.msg_lock.release() 75 | 76 | def update_iiwa_position_measured(self): 77 | while True: 78 | self.lc.handle() 79 | 80 | def get_iiwa_position_measured(self): 81 | iiwa_position_measured = np.array([]) 82 | self.msg_lock.acquire() 83 | if self.iiwa_position_measured: 84 | iiwa_position_measured = np.array(self.iiwa_position_measured) 85 | self.msg_lock.release() 86 | return iiwa_position_measured 87 | 88 | 89 | class PlanManagerZmqClient: 90 | def __init__(self): 91 | self.context = zmq.Context() 92 | self.plan_client = self.context.socket(zmq.REQ) 93 | # TODO: load sockets from config.yml. 94 | self.plan_client.connect("tcp://localhost:5555") 95 | 96 | self.status_subscriber = self.context.socket(zmq.SUB) 97 | self.channel_name = "PLAN_STATUS" 98 | self.status_subscriber.setsockopt_string(zmq.SUBSCRIBE, 99 | self.channel_name) 100 | self.last_plan_msg = lcmt_robot_plan() 101 | self.last_plan_msg.utime = sys.maxsize # 2**63 - 1 102 | self.plan_msg_lock = threading.Lock() 103 | # Setting zmq.CONFLATE to true should keep only the last message, 104 | # but in practice zmq seems to only discard messages older than a 105 | # finite amount of time (a fraction of a second is what I observed). 106 | self.status_subscriber.setsockopt(zmq.CONFLATE, True) 107 | self.status_subscriber.connect("tcp://localhost:5557") 108 | self.last_status_msg = None 109 | self.status_msg_lock = threading.Lock() 110 | self.t1 = threading.Thread(target=self.subscribe_to_plan_status, daemon=True) 111 | self.t1.start() 112 | 113 | self.abort_client = self.context.socket(zmq.REQ) 114 | self.abort_client.connect("tcp://localhost:5556") 115 | 116 | # subscribe to IIWA_STATUS. 117 | self.iiwa_position_getter = IiwaPositionGetter() 118 | 119 | # iiwa7 plant. 120 | self.plant = build_iiwa7_plant() 121 | 122 | # names of the enum in lcmt_plan_status_constants 123 | self.plan_stats_dict = { 124 | 0: "RUNNING", 1: "DISCARDED", 2: "ERROR", 3: "FINISHED"} 125 | 126 | def subscribe_to_plan_status(self): 127 | while True: 128 | msg = self.status_subscriber.recv() 129 | lcm_msg_bytes = msg[len(self.channel_name) + 1:] 130 | lcm_msg = lcmt_plan_status.decode(lcm_msg_bytes) 131 | self.status_msg_lock.acquire() 132 | self.last_status_msg = lcm_msg 133 | self.status_msg_lock.release() 134 | 135 | def get_plan_status(self): 136 | self.status_msg_lock.acquire() 137 | status_msg = copy.deepcopy(self.last_status_msg) 138 | self.status_msg_lock.release() 139 | return status_msg 140 | 141 | def get_current_ee_pose(self, frame_E): 142 | context = self.plant.CreateDefaultContext() 143 | q = self.iiwa_position_getter.get_iiwa_position_measured() 144 | self.plant.SetPositions(context, q) 145 | return self.plant.CalcRelativeTransform( 146 | context, self.plant.world_frame(), frame_E) 147 | 148 | def get_current_joint_angles(self): 149 | return self.iiwa_position_getter.get_iiwa_position_measured() 150 | 151 | def send_plan(self, plan_msg): 152 | self.plan_msg_lock.acquire() 153 | self.last_plan_msg = copy.deepcopy(plan_msg) 154 | self.plan_msg_lock.release() 155 | self.plan_client.send(plan_msg.encode()) 156 | msg = self.plan_client.recv() 157 | assert msg == b'plan_received' 158 | print("plan received by server.") 159 | 160 | def wait_for_plan_to_finish(self): 161 | # TODO: add timeout. 162 | while True: 163 | status_msg = self.get_plan_status() 164 | self.plan_msg_lock.acquire() 165 | is_same_plan = ( 166 | self.last_plan_msg.utime == status_msg.utime) 167 | is_plan_finished = ( 168 | status_msg.status == 169 | lcmt_plan_status_constants.FINISHED) 170 | is_plan_error = ( 171 | status_msg.status == 172 | lcmt_plan_status_constants.ERROR) 173 | self.plan_msg_lock.release() 174 | 175 | if is_same_plan and (is_plan_finished or is_plan_error): 176 | break 177 | time.sleep(0.01) 178 | print("Final status:", self.plan_stats_dict[status_msg.status]) 179 | 180 | def abort(self): 181 | self.abort_client.send(b"abort") 182 | s = self.abort_client.recv_string() 183 | print(s) 184 | 185 | 186 | class SchunkManager: 187 | def __init__(self, force_limit=40.0): 188 | self.lc_sub = lcm.LCM() 189 | sub = self.lc_sub.subscribe("SCHUNK_WSG_STATUS", self.sub_callback) 190 | sub.set_queue_capacity(1) 191 | self.schunk_position_measured = None 192 | self.force_limit = force_limit 193 | 194 | # status receiving thread. 195 | self.msg_lock = threading.Lock() 196 | self.t_recv = threading.Thread(target=self.update_schunk_position_measured) 197 | self.t_recv.start() 198 | 199 | # command publishing thread. 200 | self.lc_pub = lcm.LCM() 201 | self.publish_lock = threading.Lock() 202 | self.t_pub = threading.Thread(target=self.publish_schunk_position_cmd) 203 | self.schunk_position_commanded = None 204 | self.t_pub.start() 205 | 206 | def sub_callback(self, channel, data): 207 | schunk_status_msg = lcmt_schunk_wsg_status.decode(data) 208 | self.msg_lock.acquire() 209 | self.schunk_position_measured = schunk_status_msg.actual_position_mm 210 | self.msg_lock.release() 211 | 212 | def update_schunk_position_measured(self): 213 | while True: 214 | self.lc_sub.handle() 215 | 216 | def publish_schunk_position_cmd(self, utime: int = 0): 217 | while True: 218 | if self.schunk_position_commanded is None: 219 | time.sleep(1 / 20) 220 | continue 221 | msg = lcmt_schunk_wsg_command() 222 | msg.utime = utime 223 | self.publish_lock.acquire() 224 | msg.target_position_mm = self.schunk_position_commanded 225 | self.publish_lock.release() 226 | msg.force = self.force_limit 227 | self.lc_pub.publish("SCHUNK_WSG_COMMAND", msg.encode()) 228 | 229 | # publish at 20Hz. 230 | time.sleep(1 / 20) 231 | 232 | def get_schunk_position_measured(self): 233 | self.msg_lock.acquire() 234 | p = np.array(self.schunk_position_measured) 235 | self.msg_lock.release() 236 | return p 237 | 238 | def send_schunk_position_command(self, command_mm): 239 | self.publish_lock.acquire() 240 | self.schunk_position_commanded = command_mm 241 | self.publish_lock.release() 242 | 243 | def wait_for_command_to_finish(self): 244 | # NOTE(terry-suh): Note that when doing a grasp, the commanded position 245 | # typically penetrates the object, and therefore the actual position 246 | # will never reach the desired position, causing this method to hang 247 | # indefinitely. In this case, it is more advisable to simply wait for 248 | # a set time using time.sleep instead of calling this method. 249 | time_now = time.time() 250 | while True: 251 | reached_goal = ( 252 | np.abs(self.get_schunk_position_measured() - 253 | self.schunk_position_commanded) < 0.5) 254 | if reached_goal: 255 | print("Schunk command is successfully executed.") 256 | break 257 | if (time.time() - time_now) > 60.0: 258 | print("Timeout. 60 seconds elapsed but Schunk failed to reach.") 259 | break 260 | -------------------------------------------------------------------------------- /src/plans/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(robot_plans 2 | plan_base.h plan_base.cc 3 | joint_space_trajectory_plan.h joint_space_trajectory_plan.cc 4 | task_space_trajectory_plan.h task_space_trajectory_plan.cc) 5 | target_link_libraries(robot_plans drake::drake) 6 | target_include_directories(robot_plans PUBLIC ${PROJECT_SOURCE_DIR}/src) 7 | 8 | add_library(iiwa_plan_factory iiwa_plan_factory.h iiwa_plan_factory.cc 9 | plan_factory_base.cc plan_factory_base.h) 10 | target_link_libraries(iiwa_plan_factory robot_plans) 11 | target_include_directories(iiwa_plan_factory PUBLIC ${PROJECT_SOURCE_DIR}/src) 12 | -------------------------------------------------------------------------------- /src/plans/iiwa_plan_factory.cc: -------------------------------------------------------------------------------- 1 | #include "iiwa_plan_factory.h" 2 | 3 | #include 4 | 5 | #include "drake/common/find_resource.h" 6 | #include "drake/common/trajectories/piecewise_polynomial.h" 7 | #include "drake/common/trajectories/piecewise_quaternion.h" 8 | #include "drake/multibody/parsing/parser.h" 9 | 10 | #include "joint_space_trajectory_plan.h" 11 | #include "task_space_trajectory_plan.h" 12 | 13 | using drake::trajectories::PiecewisePolynomial; 14 | using drake::trajectories::PiecewiseQuaternionSlerp; 15 | using Eigen::MatrixXd; 16 | using Eigen::Quaterniond; 17 | using Eigen::VectorXd; 18 | using std::cout; 19 | using std::endl; 20 | using std::vector; 21 | 22 | IiwaPlanFactory::IiwaPlanFactory(const YAML::Node &config) : config_(config) { 23 | plant_ = std::make_unique>(1e-3); 24 | auto parser = drake::multibody::Parser(plant_.get()); 25 | 26 | const auto &iiwa_path = config_["robot_sdf_path"].as(); 27 | parser.AddModelFromFile(drake::FindResourceOrThrow(iiwa_path)); 28 | plant_->WeldFrames( 29 | plant_->world_frame(), 30 | plant_->GetFrameByName(config_["robot_baselink_name"].as())); 31 | 32 | plant_->Finalize(); 33 | } 34 | 35 | std::unique_ptr 36 | IiwaPlanFactory::MakePlan(const drake::lcmt_robot_plan &msg_plan) const { 37 | // TODO: replace this with a better test and use an lcm type with an 38 | // enum for plan types? 39 | if (msg_plan.plan.at(0).joint_name.at(0) == "iiwa_joint_0") { 40 | return MakeJointSpaceTrajectoryPlan(msg_plan); 41 | } else if (msg_plan.plan.at(0).joint_name.at(0) == "qw") { 42 | return MakeTaskSpaceTrajectoryPlan(msg_plan); 43 | } 44 | throw std::runtime_error("error in plan lcm message."); 45 | } 46 | 47 | std::unique_ptr IiwaPlanFactory::MakeJointSpaceTrajectoryPlan( 48 | const drake::lcmt_robot_plan &msg_plan) const { 49 | int n_knots = msg_plan.num_states; 50 | int n_q = msg_plan.plan.at(0).num_joints; 51 | MatrixXd q_knots(n_q, n_knots); 52 | VectorXd t_knots(n_knots); 53 | 54 | for (int t = 0; t < n_knots; t++) { 55 | t_knots[t] = static_cast(msg_plan.plan.at(t).utime) / 1e6; 56 | for (int i = 0; i < n_q; i++) { 57 | q_knots(i, t) = msg_plan.plan.at(t).joint_position.at(i); 58 | } 59 | } 60 | 61 | auto q_traj = 62 | PiecewisePolynomial::CubicWithContinuousSecondDerivatives( 63 | t_knots, q_knots, VectorXd::Zero(n_q), VectorXd::Zero(n_q)); 64 | return std::make_unique(std::move(q_traj), 65 | plant_.get()); 66 | } 67 | 68 | std::unique_ptr IiwaPlanFactory::MakeTaskSpaceTrajectoryPlan( 69 | const drake::lcmt_robot_plan &msg_plan) const { 70 | int n_knots = msg_plan.num_states - 1; 71 | 72 | // X_ET. 73 | const auto& q_xyz_ET = msg_plan.plan[n_knots].joint_position; 74 | const auto Q_ET = Quaterniond(q_xyz_ET[0], q_xyz_ET[1], q_xyz_ET[2], 75 | q_xyz_ET[3]); 76 | const auto p_EoTo_E = Eigen::Vector3d(q_xyz_ET[4], q_xyz_ET[5], q_xyz_ET[6]); 77 | const auto X_ET = drake::math::RigidTransform(Q_ET, p_EoTo_E); 78 | 79 | // trajectory. 80 | vector t_knots(n_knots); 81 | vector quat_knots(n_knots); 82 | vector xyz_knots(n_knots, MatrixXd(3, 1)); 83 | 84 | for (int t = 0; t < n_knots; t++) { 85 | // Store time 86 | t_knots[t] = static_cast(msg_plan.plan.at(t).utime) / 1e6; 87 | // Store quaternions 88 | const auto& q_xyz = msg_plan.plan.at(t).joint_position; 89 | quat_knots[t] = Quaterniond(q_xyz[0], q_xyz[1], q_xyz[2], q_xyz[3]); 90 | 91 | // Store xyz positions. 92 | // TODO(terry-suh): Change this to a better implementation once Taskspace 93 | // gets their own LCM messages. 94 | for (int i = 0; i < 3; i++) { 95 | xyz_knots[t](i) = q_xyz[4 + i]; 96 | } 97 | } 98 | 99 | auto quat_traj = PiecewiseQuaternionSlerp(t_knots, quat_knots); 100 | 101 | // Use first order hold instead of trajectories. We want to avoid smoothing 102 | // non-smooth trajectories if the user commands so. 103 | // TODO(terry-suh): Should this be an option in the config? 104 | auto xyz_traj = 105 | PiecewisePolynomial::FirstOrderHold(t_knots, xyz_knots); 106 | 107 | // Get EE frame. 108 | const auto &frame_E = 109 | plant_->GetFrameByName(config_["robot_ee_body_name"].as()); 110 | 111 | vector nominal_joint_vector = 112 | config_["robot_nominal_joint"].as>(); 113 | 114 | Eigen::VectorXd nominal_joint= Eigen::Map( 115 | nominal_joint_vector.data(), nominal_joint_vector.size()); 116 | 117 | 118 | return std::make_unique( 119 | std::move(quat_traj), std::move(xyz_traj), X_ET, plant_.get(), frame_E, 120 | config_["control_period"].as(), nominal_joint); 121 | } 122 | -------------------------------------------------------------------------------- /src/plans/iiwa_plan_factory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "plans/plan_factory_base.h" 5 | 6 | class IiwaPlanFactory : public PlanFactory { 7 | public: 8 | IiwaPlanFactory(const YAML::Node &config); 9 | 10 | ~IiwaPlanFactory() override = default; 11 | 12 | [[nodiscard]] std::unique_ptr 13 | MakePlan(const drake::lcmt_robot_plan &msg_plan) const override; 14 | 15 | private: 16 | const YAML::Node &config_; 17 | [[nodiscard]] std::unique_ptr 18 | MakeJointSpaceTrajectoryPlan(const drake::lcmt_robot_plan &msg_plan) const; 19 | [[nodiscard]] std::unique_ptr 20 | MakeTaskSpaceTrajectoryPlan(const drake::lcmt_robot_plan &msg_plan) const; 21 | }; 22 | -------------------------------------------------------------------------------- /src/plans/joint_space_trajectory_plan.cc: -------------------------------------------------------------------------------- 1 | #include "joint_space_trajectory_plan.h" 2 | 3 | void JointSpaceTrajectoryPlan::Step(const State &, double control_period, 4 | double t, Command *cmd) const { 5 | cmd->q_cmd = q_traj_.value(t); 6 | cmd->tau_cmd = Eigen::VectorXd::Zero(q_traj_.rows()); 7 | } 8 | -------------------------------------------------------------------------------- /src/plans/joint_space_trajectory_plan.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "drake/common/trajectories/piecewise_polynomial.h" 4 | #include "plans/plan_base.h" 5 | 6 | class JointSpaceTrajectoryPlan : public PlanBase { 7 | public: 8 | JointSpaceTrajectoryPlan( 9 | drake::trajectories::PiecewisePolynomial q_traj, 10 | const drake::multibody::MultibodyPlant *plant) 11 | : PlanBase(plant), q_traj_(std::move(q_traj)){}; 12 | 13 | ~JointSpaceTrajectoryPlan() override = default; 14 | 15 | void Step(const State &state, double control_period, double t, 16 | Command *cmd) const override; 17 | 18 | [[nodiscard]] double duration() const override { 19 | return q_traj_.end_time() - q_traj_.start_time(); 20 | }; 21 | 22 | private: 23 | const drake::trajectories::PiecewisePolynomial q_traj_; 24 | }; 25 | -------------------------------------------------------------------------------- /src/plans/plan_base.cc: -------------------------------------------------------------------------------- 1 | #include "plan_base.h" -------------------------------------------------------------------------------- /src/plans/plan_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "drake/multibody/plant/multibody_plant.h" 7 | #include 8 | 9 | // Description of a single contact. 10 | struct ContactInfo { 11 | Eigen::Vector3d contact_force; // in world frame. 12 | Eigen::Vector3d contact_point; // in world frame. 13 | drake::multibody::BodyIndex contact_link_idx; // tied to a specific MBP. 14 | std::optional contact_normal; // in world frame? 15 | }; 16 | 17 | struct State { 18 | State() = default; 19 | State(const Eigen::Ref &q, 20 | const Eigen::Ref &v, 21 | const Eigen::Ref &tau_ext) 22 | : q(q), v(v), tau_ext(tau_ext){}; 23 | Eigen::VectorXd q; 24 | Eigen::VectorXd v; 25 | Eigen::VectorXd tau_ext; 26 | std::optional> contact_results; 27 | }; 28 | 29 | struct Command { 30 | Command() = default; 31 | Command(const Eigen::Ref &q_cmd, 32 | const Eigen::Ref &tau_cmd) 33 | : q_cmd(q_cmd), tau_cmd(tau_cmd){}; 34 | Eigen::VectorXd q_cmd; 35 | Eigen::VectorXd tau_cmd; 36 | }; 37 | 38 | class PlanBase { 39 | public: 40 | explicit PlanBase(const drake::multibody::MultibodyPlant *plant) 41 | : plant_(plant){}; 42 | 43 | virtual ~PlanBase() = default; 44 | 45 | virtual void Step(const State &state, double control_period, double t, 46 | Command *cmd) const = 0; 47 | 48 | [[nodiscard]] virtual double duration() const = 0; 49 | 50 | protected: 51 | drake::multibody::MultibodyPlant const *const plant_{nullptr}; 52 | }; 53 | -------------------------------------------------------------------------------- /src/plans/plan_factory_base.cc: -------------------------------------------------------------------------------- 1 | #include "plan_factory_base.h" 2 | -------------------------------------------------------------------------------- /src/plans/plan_factory_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "drake/multibody/plant/multibody_plant.h" 4 | #include "drake_lcmtypes/drake/lcmt_robot_plan.hpp" 5 | #include "drake_lcmtypes/drake/lcmt_robot_state.hpp" 6 | 7 | #include "plans/plan_base.h" 8 | 9 | class PlanFactory { 10 | public: 11 | virtual ~PlanFactory() = default; 12 | 13 | [[nodiscard]] virtual std::unique_ptr 14 | 15 | MakePlan(const drake::lcmt_robot_plan &msg_plan) const = 0; 16 | 17 | [[nodiscard]] const drake::multibody::MultibodyPlant & 18 | get_plant() const { 19 | return *plant_; 20 | }; 21 | 22 | protected: 23 | std::unique_ptr> plant_{nullptr}; 24 | }; 25 | -------------------------------------------------------------------------------- /src/plans/task_space_trajectory_plan.cc: -------------------------------------------------------------------------------- 1 | #include "task_space_trajectory_plan.h" 2 | 3 | #include 4 | 5 | using drake::MatrixX; 6 | using drake::Vector3; 7 | using drake::Vector6; 8 | using drake::multibody::ComputePoseDiffInCommonFrame; 9 | using drake::multibody::DifferentialInverseKinematicsResult; 10 | using drake::multibody::DifferentialInverseKinematicsStatus; 11 | using drake::multibody::DoDifferentialInverseKinematics; 12 | 13 | using std::cout; 14 | using std::endl; 15 | 16 | void TaskSpaceTrajectoryPlan::Step(const State &state, double control_period, 17 | double t, Command *cmd) const { 18 | 19 | // 1. Update diffik mbp with the current status of the robot. 20 | plant_->SetPositions(plant_context_.get(), state.q); 21 | 22 | // 2. Ask diffik to solve for desired position. 23 | const drake::math::RigidTransformd X_WT_desired(quat_traj_.orientation(t), 24 | xyz_traj_.value(t)); 25 | const auto &frame_W = plant_->world_frame(); 26 | const auto X_WE = 27 | plant_->CalcRelativeTransform(*plant_context_, frame_W, frame_E_); 28 | const auto X_WT = X_WE * X_ET_; 29 | 30 | const Vector6 V_WT_desired = 31 | ComputePoseDiffInCommonFrame(X_WT, X_WT_desired) / 32 | params_->get_time_step(); 33 | 34 | MatrixX J_WT(6, plant_->num_velocities()); 35 | plant_->CalcJacobianSpatialVelocity( 36 | *plant_context_, drake::multibody::JacobianWrtVariable::kV, frame_E_, 37 | X_ET_.translation(), frame_W, frame_W, &J_WT); 38 | 39 | DifferentialInverseKinematicsResult result = DoDifferentialInverseKinematics( 40 | state.q, state.v, V_WT_desired, J_WT, *params_); 41 | 42 | // 3. Check for errors and integrate. 43 | if (result.status != DifferentialInverseKinematicsStatus::kSolutionFound) { 44 | // Set the command to NAN so that state machine will detect downstream and 45 | // go to error state. 46 | cmd->q_cmd = NAN * Eigen::VectorXd::Zero(7); 47 | // TODO(terry-suh): how do I tell the use that the state machine went to 48 | // error because of this precise reason? Printing the error message here 49 | // seems like a good start, but we'll need to handle this better. 50 | std::cout << "DoDifferentialKinematics Failed to find a solution." 51 | << std::endl; 52 | } else { 53 | cmd->q_cmd = state.q + control_period * result.joint_velocities.value(); 54 | cmd->tau_cmd = Eigen::VectorXd::Zero(7); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/plans/task_space_trajectory_plan.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "drake/common/trajectories/piecewise_polynomial.h" 6 | #include "drake/common/trajectories/piecewise_quaternion.h" 7 | #include "drake/multibody/inverse_kinematics/differential_inverse_kinematics_integrator.h" 8 | #include "drake/math/rigid_transform.h" 9 | #include "drake/multibody/plant/multibody_plant.h" 10 | #include "drake/multibody/tree/multibody_tree.h" 11 | #include "drake/systems/framework/context.h" 12 | 13 | #include "plans/plan_base.h" 14 | 15 | class TaskSpaceTrajectoryPlan : public PlanBase { 16 | public: 17 | TaskSpaceTrajectoryPlan( 18 | drake::trajectories::PiecewiseQuaternionSlerp quat_traj, 19 | drake::trajectories::PiecewisePolynomial xyz_traj, 20 | drake::math::RigidTransformd X_ET, 21 | const drake::multibody::MultibodyPlant *plant, 22 | const drake::multibody::Frame &frame_E, 23 | double control_time_step, Eigen::VectorXd nominal_joint_position) 24 | : PlanBase(plant), quat_traj_(std::move(quat_traj)), 25 | X_ET_(std::move(X_ET)), xyz_traj_(std::move(xyz_traj)), 26 | frame_E_(frame_E), nominal_joint_position_(nominal_joint_position) { 27 | 28 | params_ = std::make_unique< 29 | drake::multibody::DifferentialInverseKinematicsParameters>( 30 | plant_->num_positions(), plant_->num_velocities()); 31 | params_->set_time_step(control_time_step); 32 | params_->set_nominal_joint_position(nominal_joint_position); 33 | plant_context_ = plant_->CreateDefaultContext(); 34 | } 35 | 36 | ~TaskSpaceTrajectoryPlan() override = default; 37 | 38 | void Step(const State &state, double control_period, double t, 39 | Command *cmd) const override; 40 | 41 | [[nodiscard]] double duration() const override { 42 | return xyz_traj_.end_time() - xyz_traj_.start_time(); 43 | }; 44 | 45 | private: 46 | const drake::trajectories::PiecewiseQuaternionSlerp quat_traj_; 47 | const drake::trajectories::PiecewisePolynomial xyz_traj_; 48 | const drake::math::RigidTransformd X_ET_; 49 | const Eigen::VectorXd nominal_joint_position_; 50 | 51 | // frame of end-effector body + offset. 52 | const drake::multibody::Frame &frame_E_; 53 | 54 | std::unique_ptr> plant_context_; 55 | std::unique_ptr< 56 | drake::multibody::DifferentialInverseKinematicsParameters> 57 | params_; 58 | }; 59 | -------------------------------------------------------------------------------- /src/run_plan_manager.cc: -------------------------------------------------------------------------------- 1 | #include "iiwa_plan_manager.h" 2 | #include 3 | 4 | int main(int argc, char **argv) { 5 | std::string filename; 6 | if (argc < 2) { 7 | filename = "../config/default.yaml"; 8 | } else { 9 | filename = argv[1]; 10 | } 11 | 12 | YAML::Node config; 13 | config = YAML::LoadFile(filename); 14 | 15 | IiwaPlanManager pm(config); 16 | pm.Run(); 17 | 18 | return 0; 19 | } -------------------------------------------------------------------------------- /src/run_plan_manager_system.cc: -------------------------------------------------------------------------------- 1 | #include "plan_manager_system/iiwa_plan_manager_hardware_interface.h" 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) { 6 | std::string filename; 7 | if (argc < 2) { 8 | filename = "../config/default.yaml"; 9 | } else { 10 | filename = argv[1]; 11 | } 12 | 13 | YAML::Node config; 14 | config = YAML::LoadFile(filename); 15 | 16 | auto plan_manger = IiwaPlanManagerHardwareInterface(config); 17 | plan_manger.SaveGraphvizStringToFile(); 18 | plan_manger.Run(); 19 | 20 | return EXIT_FAILURE; 21 | } 22 | -------------------------------------------------------------------------------- /src/run_zmq_example.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "drake_lcmtypes/drake/lcmt_robot_plan.hpp" 5 | #include "drake_lcmtypes/drake/lcmt_robot_state.hpp" 6 | #include 7 | #include 8 | 9 | #include "plans/iiwa_plan_factory.h" 10 | 11 | using std::cout; 12 | using std::endl; 13 | 14 | void PrintRobotState(const drake::lcmt_robot_state &state_msg) { 15 | cout << "t: " << static_cast(state_msg.utime) / 1e6 << endl; 16 | cout << "num joints: " << state_msg.num_joints << endl; 17 | for (int i = 0; i < state_msg.num_joints; i++) { 18 | cout << state_msg.joint_name[i] << " " << state_msg.joint_position[i] 19 | << endl; 20 | } 21 | cout << "---------------------------------------" << endl; 22 | } 23 | 24 | int main() { 25 | zmq::context_t ctx; 26 | zmq::socket_t sock(ctx, zmq::socket_type::push); 27 | zmq::socket_t socket(ctx, ZMQ_REP); 28 | socket.bind("tcp://*:5555"); 29 | 30 | std::string config_filename = "../config/default.yaml"; 31 | YAML::Node config; 32 | config = YAML::Load(config_filename); 33 | 34 | IiwaPlanFactory plan_factory(config); 35 | State s; 36 | Command c; 37 | 38 | drake::lcmt_robot_plan plan_lcm_msg; 39 | while (true) { 40 | zmq::message_t plan_msg; 41 | auto res = socket.recv(plan_msg, zmq::recv_flags::none); 42 | if (!res.has_value()) { 43 | throw std::runtime_error("Receiving plan message failed."); 44 | } 45 | 46 | cout << "received msg" << endl; 47 | plan_lcm_msg.decode(plan_msg.data(), 0, plan_msg.size()); 48 | cout << "plan num states " << plan_lcm_msg.num_states << endl; 49 | for (const auto &state_msg : plan_lcm_msg.plan) { 50 | PrintRobotState(state_msg); 51 | } 52 | 53 | cout << "compare strings " 54 | << (plan_lcm_msg.plan[0].joint_name[0] == "iiwa_joint_0") << endl; 55 | 56 | auto plan = plan_factory.MakePlan(plan_lcm_msg); 57 | plan->Step(s, 0.005, 0.876, &c); 58 | cout << "q_cmd " << c.q_cmd.transpose() << endl; 59 | cout << "tau_cmd " << c.tau_cmd.transpose() << endl; 60 | 61 | std::string reply_msg("success"); 62 | zmq::message_t reply(reply_msg.size()); 63 | memcpy(reply.data(), reply_msg.data(), reply_msg.size()); 64 | socket.send(reply, zmq::send_flags::none); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /src/state_machine/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(plan_manager_state_machine 2 | plan_manager_state_machine.h plan_manager_state_machine.cc 3 | state_init.h state_init.cc 4 | state_idle.h state_idle.cc 5 | state_running.h state_running.cc 6 | state_error.h state_error.cc) 7 | target_link_libraries(plan_manager_state_machine robot_plans) 8 | target_include_directories(plan_manager_state_machine PUBLIC 9 | ${PROJECT_SOURCE_DIR}/src) 10 | 11 | -------------------------------------------------------------------------------- /src/state_machine/plan_manager_state_machine.cc: -------------------------------------------------------------------------------- 1 | #include "state_machine/plan_manager_state_machine.h" 2 | #include "state_machine/state_error.h" 3 | #include "state_machine/state_idle.h" 4 | #include "state_machine/state_init.h" 5 | 6 | #include 7 | 8 | using std::cout; 9 | using std::endl; 10 | using std::string; 11 | 12 | PlanManagerStateMachine::PlanManagerStateMachine( 13 | double state_machine_start_time_seconds, const YAML::Node &config) 14 | : state_machine_start_time_seconds_(state_machine_start_time_seconds), 15 | config_(config) { 16 | // Initialize to state INIT. 17 | state_ = StateInit::Instance(); 18 | } 19 | 20 | void PlanManagerStateMachine::SetIiwaPositionCommandIdle( 21 | const Eigen::Ref &q_cmd) { 22 | if (iiwa_position_command_idle_) { 23 | DRAKE_THROW_UNLESS(q_cmd.size() == iiwa_position_command_idle_->size()); 24 | *iiwa_position_command_idle_ = q_cmd; 25 | } else { 26 | iiwa_position_command_idle_ = std::make_unique(q_cmd); 27 | } 28 | } 29 | 30 | const PlanBase *PlanManagerStateBase::GetCurrentPlan( 31 | PlanManagerStateMachine *state_machine, double t_now, 32 | const drake::lcmt_iiwa_status &msg_iiwa_status) const { 33 | DRAKE_THROW_UNLESS(state_machine->num_plans() == 0); 34 | return nullptr; 35 | } 36 | 37 | double PlanManagerStateBase::GetCurrentPlanUpTime( 38 | const PlanManagerStateMachine *state_machine, double t_now) const { 39 | string error_msg = "GetCurrentPlanUpTime should not be called in state "; 40 | error_msg += get_state_name(); 41 | error_msg += "."; 42 | throw std::runtime_error(error_msg); 43 | } 44 | 45 | void PlanManagerStateBase::QueueNewPlan(PlanManagerStateMachine *state_machine, 46 | std::unique_ptr plan) { 47 | string error_msg = "QueueNewPlan should not be called in state "; 48 | error_msg += get_state_name(); 49 | error_msg += "."; 50 | throw std::runtime_error(error_msg); 51 | } 52 | 53 | bool PlanManagerStateBase::CommandHasError( 54 | const State &state, const Command &cmd, 55 | PlanManagerStateMachine *state_machine, const double q_threshold) { 56 | bool is_nan = 57 | cmd.q_cmd.array().isNaN().sum() or cmd.tau_cmd.array().isNaN().sum(); 58 | 59 | bool is_too_far_away = (state.q - cmd.q_cmd).norm() > q_threshold; 60 | 61 | bool is_error = is_nan or is_too_far_away; 62 | if (is_error) { 63 | while (!state_machine->plans_.empty()) { 64 | // Delete all plans. 65 | state_machine->plans_.pop(); 66 | } 67 | ChangeState(state_machine, StateError::Instance()); 68 | } 69 | return is_error; 70 | } 71 | 72 | bool PlanManagerStateBase::has_received_status_msg() const { 73 | string error_msg = "has_received_status_msg should not be called in state "; 74 | error_msg += get_state_name(); 75 | error_msg += "."; 76 | throw std::runtime_error(error_msg); 77 | } 78 | 79 | void PlanManagerStateBase::AbortAllPlans( 80 | PlanManagerStateMachine *state_machine) { 81 | ChangeState(state_machine, StateIdle::Instance()); 82 | auto &plans_queue = state_machine->get_mutable_plans_queue(); 83 | while (!plans_queue.empty()) { 84 | plans_queue.pop(); 85 | } 86 | state_machine->reset_current_plan_start_time(); 87 | spdlog::flush_on(spdlog::level::info); 88 | spdlog::info("All plans have been aborted."); 89 | } 90 | -------------------------------------------------------------------------------- /src/state_machine/plan_manager_state_machine.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "drake/multibody/plant/multibody_plant.h" 9 | #include "drake_lcmtypes/drake/lcmt_iiwa_status.hpp" 10 | 11 | #include "plans/plan_base.h" 12 | 13 | // TODO: this is not used right now. 14 | enum PlanManagerStateTypes { 15 | kStateInit, 16 | kStateIdle, 17 | kStateRunning, 18 | kStateError 19 | }; 20 | using TimePoint = std::chrono::time_point; 21 | using DoubleSeconds = std::chrono::duration>; 22 | class PlanManagerStateBase; 23 | 24 | class PlanManagerStateMachine { 25 | public: 26 | explicit PlanManagerStateMachine(double state_machine_start_time_seconds, 27 | const YAML::Node &config); 28 | // State-dependent methods. 29 | // TODO: separate the logic that schedules plans into another function. 30 | [[nodiscard]] const PlanBase * 31 | GetCurrentPlan(const TimePoint &t_now, 32 | const drake::lcmt_iiwa_status &msg_iiwa_status); 33 | [[nodiscard]] const PlanBase * 34 | GetCurrentPlan(double t_now_seconds, 35 | const drake::lcmt_iiwa_status &msg_iiwa_status); 36 | 37 | // Returns in seconds how long the current plan has been active. 38 | [[nodiscard]] double GetCurrentPlanUpTime(const TimePoint &t_now) const; 39 | [[nodiscard]] double GetCurrentPlanUpTime(double t_now_seconds) const; 40 | 41 | // Print information about the currently active state. 42 | void PrintCurrentState(double t_now_seconds) const; 43 | 44 | // Tries to add a plan to the queue of plans to be executed. Note that the 45 | // maximum size of this queue is 1 in the current implementation. 46 | void QueueNewPlan(std::unique_ptr plan); 47 | 48 | // Checks a command computed by the plan.Step() function for errors. 49 | // Currently checks for: 50 | // 1. Nans 51 | // 2. If cmd.q_cmd and state.q is too far away with a hard-coded threshold. 52 | // This threshold is decided by a parameter in the config file. 53 | bool CommandHasError(const State &state, const Command &cmd); 54 | 55 | // Empties the plans_ queue and sets the state to IDLE. 56 | void AbortAllPlans(); 57 | 58 | // Returns true if an IIWA_STATUS message has been received. 59 | [[nodiscard]] bool has_received_status_msg() const; 60 | 61 | // Called when a new IIWA_STATUS message is received. If the current state is 62 | // INIT, the state is changed to IDLE. If the current state is IDLE, 63 | // RUNNING or ERROR, this function does nothing. 64 | void ReceiveNewStatusMsg(const drake::lcmt_iiwa_status &msg_iiwa_status); 65 | 66 | [[nodiscard]] PlanManagerStateTypes get_state_type() const; 67 | 68 | // Other methods. 69 | [[nodiscard]] size_t num_plans() const { return plans_.size(); } 70 | 71 | std::queue> &get_mutable_plans_queue() { 72 | return plans_; 73 | }; 74 | 75 | [[nodiscard]] const std::queue> & 76 | get_plans_queue() const { 77 | return plans_; 78 | }; 79 | 80 | // Stores lcmt_iiwa_status.joint_position_measured in 81 | // iiwa_position_command_idle_. 82 | void 83 | SetIiwaPositionCommandIdle(const Eigen::Ref &q_cmd); 84 | 85 | [[nodiscard]] const Eigen::VectorXd &get_iiwa_position_command_idle() const { 86 | return *iiwa_position_command_idle_; 87 | }; 88 | 89 | void reset_iiwa_position_command_idle() { 90 | iiwa_position_command_idle_.reset(); 91 | } 92 | 93 | [[nodiscard]] bool is_iiwa_position_command_idle_set() const { 94 | return iiwa_position_command_idle_ != nullptr; 95 | } 96 | 97 | [[nodiscard]] double get_state_machine_up_time(double t_now_seconds) const; 98 | 99 | // TODO: "time" methods should probably be private. Access by states can be 100 | // enabled by forwarding in PlanManagerStateBase. 101 | void set_current_plan_start_time(double t_now_seconds); 102 | 103 | void reset_current_plan_start_time() { 104 | current_plan_start_time_seconds_.reset(); 105 | }; 106 | 107 | [[nodiscard]] const double *get_current_plan_start_time() const { 108 | return current_plan_start_time_seconds_.get(); 109 | }; 110 | 111 | private: 112 | friend class PlanManagerStateBase; 113 | inline void ChangeState(PlanManagerStateBase *new_state); 114 | PlanManagerStateBase *state_{nullptr}; 115 | std::queue> plans_; 116 | 117 | const YAML::Node &config_; 118 | 119 | // In PlanManagerStateMachine, this stores TimePoint::time_since_epoch() in 120 | // seconds as a double. 121 | // In Drake systems, this stores context.get_time(). 122 | std::unique_ptr current_plan_start_time_seconds_{nullptr}; 123 | 124 | const double state_machine_start_time_seconds_; 125 | 126 | // The iiwa command to send in state IDLE. 127 | std::unique_ptr iiwa_position_command_idle_{nullptr}; 128 | }; 129 | 130 | class PlanManagerStateBase { 131 | public: 132 | // Virtual functions. 133 | [[nodiscard]] virtual double 134 | GetCurrentPlanUpTime(const PlanManagerStateMachine *state_machine, 135 | double t_now) const; 136 | 137 | [[nodiscard]] virtual bool has_received_status_msg() const; 138 | 139 | virtual void QueueNewPlan(PlanManagerStateMachine *state_machine, 140 | std::unique_ptr plan); 141 | 142 | virtual bool CommandHasError(const State &state, const Command &cmd, 143 | PlanManagerStateMachine *state_machine, 144 | const double q_threshold); 145 | 146 | virtual void AbortAllPlans(PlanManagerStateMachine *state_machine); 147 | 148 | virtual const PlanBase * 149 | GetCurrentPlan(PlanManagerStateMachine *state_machine, double t_now_seconds, 150 | const drake::lcmt_iiwa_status &msg_iiwa_status) const; 151 | 152 | // Pure virtual functions. 153 | [[nodiscard]] virtual PlanManagerStateTypes get_state_type() const = 0; 154 | 155 | virtual void 156 | ReceiveNewStatusMsg(PlanManagerStateMachine *state_machine, 157 | const drake::lcmt_iiwa_status &msg_iiwa_status) const = 0; 158 | virtual void PrintCurrentState(const PlanManagerStateMachine *state_machine, 159 | double t_now_seconds) const = 0; 160 | 161 | // Other functions. 162 | [[nodiscard]] const std::string &get_state_name() const { 163 | return state_name_; 164 | }; 165 | 166 | protected: 167 | explicit PlanManagerStateBase(std::string state_name) 168 | : state_name_(std::move(state_name)){}; 169 | 170 | static void ChangeState(PlanManagerStateMachine *state_machine, 171 | PlanManagerStateBase *new_state) { 172 | std::stringstream ss; 173 | ss << "[" << state_machine->state_->get_state_name() << "]" << "---->" 174 | << "[" << new_state->get_state_name() << "]"; 175 | spdlog::info(ss.str()); 176 | state_machine->ChangeState(new_state); 177 | }; 178 | 179 | private: 180 | const std::string state_name_; 181 | }; 182 | 183 | inline bool PlanManagerStateMachine::has_received_status_msg() const { 184 | return state_->has_received_status_msg(); 185 | } 186 | 187 | inline void PlanManagerStateMachine::ReceiveNewStatusMsg( 188 | const drake::lcmt_iiwa_status &msg_iiwa_status) { 189 | state_->ReceiveNewStatusMsg(this, msg_iiwa_status); 190 | } 191 | 192 | inline PlanManagerStateTypes PlanManagerStateMachine::get_state_type() const { 193 | return state_->get_state_type(); 194 | } 195 | 196 | inline void 197 | PlanManagerStateMachine::QueueNewPlan(std::unique_ptr plan) { 198 | state_->QueueNewPlan(this, std::move(plan)); 199 | } 200 | 201 | inline void 202 | PlanManagerStateMachine::ChangeState(PlanManagerStateBase *new_state) { 203 | state_ = new_state; 204 | } 205 | 206 | inline double 207 | PlanManagerStateMachine::GetCurrentPlanUpTime(const TimePoint &t_now) const { 208 | double t_now_double = 209 | std::chrono::duration_cast(t_now.time_since_epoch()) 210 | .count(); 211 | return state_->GetCurrentPlanUpTime(this, t_now_double); 212 | } 213 | 214 | inline double 215 | PlanManagerStateMachine::GetCurrentPlanUpTime(double t_now) const { 216 | return state_->GetCurrentPlanUpTime(this, t_now); 217 | } 218 | 219 | inline const PlanBase *PlanManagerStateMachine::GetCurrentPlan( 220 | const TimePoint &t_now, const drake::lcmt_iiwa_status &msg_iiwa_status) { 221 | double t_now_double = 222 | std::chrono::duration_cast(t_now.time_since_epoch()) 223 | .count(); 224 | return state_->GetCurrentPlan(this, t_now_double, msg_iiwa_status); 225 | } 226 | 227 | inline double 228 | PlanManagerStateMachine::get_state_machine_up_time(double t_now_seconds) const { 229 | return t_now_seconds - state_machine_start_time_seconds_; 230 | } 231 | 232 | inline const PlanBase *PlanManagerStateMachine::GetCurrentPlan( 233 | double t_now_seconds, const drake::lcmt_iiwa_status &msg_iiwa_status) { 234 | return state_->GetCurrentPlan(this, t_now_seconds, msg_iiwa_status); 235 | } 236 | 237 | inline void 238 | PlanManagerStateMachine::PrintCurrentState(double t_now_seconds) const { 239 | state_->PrintCurrentState(this, t_now_seconds); 240 | } 241 | 242 | inline void 243 | PlanManagerStateMachine::set_current_plan_start_time(double t_now_seconds) { 244 | current_plan_start_time_seconds_ = std::make_unique(t_now_seconds); 245 | } 246 | 247 | inline bool PlanManagerStateMachine::CommandHasError(const State &state, 248 | const Command &cmd) { 249 | return state_->CommandHasError(state, cmd, this, 250 | config_["q_threshold"].as()); 251 | } 252 | 253 | inline void PlanManagerStateMachine::AbortAllPlans() { 254 | state_->AbortAllPlans(this); 255 | } 256 | -------------------------------------------------------------------------------- /src/state_machine/state_error.cc: -------------------------------------------------------------------------------- 1 | #include "state_machine/state_error.h" 2 | 3 | #include 4 | 5 | using std::cout; 6 | using std::endl; 7 | 8 | PlanManagerStateBase *StateError::instance_ = nullptr; 9 | PlanManagerStateBase *StateError::Instance() { 10 | if (!instance_) { 11 | instance_ = new StateError(); 12 | } 13 | return instance_; 14 | } 15 | 16 | bool StateError::CommandHasError(const State &state, const Command &cmd, 17 | PlanManagerStateMachine *state_machine, 18 | const double q_threshold) { 19 | std::string error_msg = "CommandHasError should not be called in state "; 20 | error_msg += get_state_name(); 21 | error_msg += "."; 22 | throw std::runtime_error(error_msg); 23 | } 24 | 25 | void StateError::PrintCurrentState(const PlanManagerStateMachine *state_machine, 26 | double t_now_seconds) const { 27 | std::string msg; 28 | msg += ("[ERROR]: "); 29 | msg += "Number of plans: "; 30 | msg += std::to_string(state_machine->num_plans()); 31 | msg += ". t = "; 32 | msg += 33 | std::to_string(state_machine->get_state_machine_up_time(t_now_seconds)); 34 | spdlog::critical(msg); 35 | } 36 | 37 | void StateError::QueueNewPlan(PlanManagerStateMachine *state_machine, 38 | std::unique_ptr plan) { 39 | std::string msg("[ERROR]: received plan is discarded."); 40 | msg += "Number of plans: "; 41 | msg += std::to_string(state_machine->num_plans()); 42 | msg += "."; 43 | spdlog::critical(msg); 44 | } 45 | -------------------------------------------------------------------------------- /src/state_machine/state_error.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "state_machine/plan_manager_state_machine.h" 3 | 4 | class StateError : public PlanManagerStateBase { 5 | public: 6 | static PlanManagerStateBase *Instance(); 7 | 8 | void QueueNewPlan(PlanManagerStateMachine *state_machine, 9 | std::unique_ptr plan) override; 10 | 11 | bool CommandHasError(const State &state, const Command &cmd, 12 | PlanManagerStateMachine *state_machine, 13 | const double q_threshold) override; 14 | 15 | void PrintCurrentState(const PlanManagerStateMachine *state_machine, 16 | double t_now_seconds) const override; 17 | 18 | [[nodiscard]] PlanManagerStateTypes get_state_type() const override { 19 | return PlanManagerStateTypes::kStateError; 20 | }; 21 | 22 | void ReceiveNewStatusMsg( 23 | PlanManagerStateMachine *state_machine, 24 | const drake::lcmt_iiwa_status &msg_iiwa_status) const override{}; 25 | 26 | void AbortAllPlans(PlanManagerStateMachine *state_machine) override { 27 | throw std::runtime_error( 28 | "AbortAllPlans should not be called in state " + get_state_name()); 29 | }; 30 | 31 | private: 32 | StateError() : PlanManagerStateBase("ERROR"){}; 33 | static PlanManagerStateBase *instance_; 34 | }; 35 | -------------------------------------------------------------------------------- /src/state_machine/state_idle.cc: -------------------------------------------------------------------------------- 1 | #include "state_idle.h" 2 | #include "state_running.h" 3 | 4 | #include 5 | 6 | using std::cout; 7 | using std::endl; 8 | 9 | PlanManagerStateBase *StateIdle::instance_ = nullptr; 10 | PlanManagerStateBase *StateIdle::Instance() { 11 | if (!instance_) { 12 | instance_ = new StateIdle(); 13 | } 14 | return instance_; 15 | } 16 | 17 | void StateIdle::QueueNewPlan(PlanManagerStateMachine *state_machine, 18 | std::unique_ptr plan) { 19 | auto &plan_queue = state_machine->get_mutable_plans_queue(); 20 | plan_queue.push(std::move(plan)); 21 | ChangeState(state_machine, StateRunning::Instance()); 22 | } 23 | 24 | void StateIdle::PrintCurrentState(const PlanManagerStateMachine *state_machine, 25 | double t_now_seconds) const { 26 | std::string msg; 27 | msg += ("[IDLE]: waiting for new plans. "); 28 | msg += "Number of plans: "; 29 | msg += std::to_string(state_machine->num_plans()); 30 | msg += ". t = "; 31 | msg += 32 | std::to_string(state_machine->get_state_machine_up_time(t_now_seconds)); 33 | spdlog::info(msg); 34 | } 35 | 36 | -------------------------------------------------------------------------------- /src/state_machine/state_idle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "state_machine/plan_manager_state_machine.h" 3 | 4 | class StateIdle : public PlanManagerStateBase { 5 | public: 6 | static PlanManagerStateBase *Instance(); 7 | 8 | [[nodiscard]] bool has_received_status_msg() const override { return true; }; 9 | 10 | void ReceiveNewStatusMsg(PlanManagerStateMachine *state_machine, 11 | const drake::lcmt_iiwa_status &msg_iiwa_status) 12 | const override {}; 13 | 14 | void QueueNewPlan(PlanManagerStateMachine *state_machine, 15 | std::unique_ptr plan) override; 16 | 17 | void PrintCurrentState(const PlanManagerStateMachine *manager, 18 | double t_now_seconds) const override; 19 | 20 | [[nodiscard]] PlanManagerStateTypes get_state_type() const override { 21 | return PlanManagerStateTypes::kStateIdle; 22 | }; 23 | private: 24 | StateIdle() : PlanManagerStateBase("IDLE") {}; 25 | static PlanManagerStateBase *instance_; 26 | }; -------------------------------------------------------------------------------- /src/state_machine/state_init.cc: -------------------------------------------------------------------------------- 1 | #include "state_init.h" 2 | #include "state_idle.h" 3 | 4 | #include 5 | 6 | using std::cout; 7 | using std::endl; 8 | 9 | PlanManagerStateBase *StateInit::instance_ = nullptr; 10 | PlanManagerStateBase *StateInit::Instance() { 11 | if (!instance_) { 12 | instance_ = new StateInit(); 13 | } 14 | return instance_; 15 | } 16 | 17 | void StateInit::ReceiveNewStatusMsg( 18 | PlanManagerStateMachine *state_machine, 19 | const drake::lcmt_iiwa_status &msg_iiwa_status) const { 20 | state_machine->SetIiwaPositionCommandIdle(Eigen::Map( 21 | msg_iiwa_status.joint_position_commanded.data(), 22 | msg_iiwa_status.num_joints)); 23 | ChangeState(state_machine, StateIdle::Instance()); 24 | } 25 | 26 | void StateInit::QueueNewPlan(PlanManagerStateMachine *state_machine, 27 | std::unique_ptr plan) { 28 | spdlog::warn("[INIT]: no robot status message received yet. " 29 | "Received plan is discarded."); 30 | } 31 | 32 | bool StateInit::CommandHasError(const State &state, const Command &cmd, 33 | PlanManagerStateMachine *state_machine, 34 | const double q_threshold) { 35 | std::string error_msg = "CommandHasError should not be called in state "; 36 | error_msg += get_state_name(); 37 | error_msg += "."; 38 | throw std::runtime_error(error_msg); 39 | } 40 | 41 | void StateInit::PrintCurrentState(const PlanManagerStateMachine *state_machine, 42 | double t_now_seconds) const { 43 | std::string msg; 44 | msg += ("[INIT]: waiting for IIWA_STATUS. "); 45 | msg += "Number of plans: "; 46 | msg += std::to_string(state_machine->num_plans()); 47 | msg += ". t = "; 48 | msg += 49 | std::to_string(state_machine->get_state_machine_up_time(t_now_seconds)); 50 | spdlog::info(msg); 51 | } 52 | -------------------------------------------------------------------------------- /src/state_machine/state_init.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "state_machine/plan_manager_state_machine.h" 3 | 4 | class StateInit : public PlanManagerStateBase { 5 | public: 6 | static PlanManagerStateBase *Instance(); 7 | 8 | [[nodiscard]] bool has_received_status_msg() const override { return false; }; 9 | 10 | void ReceiveNewStatusMsg( 11 | PlanManagerStateMachine *state_machine, 12 | const drake::lcmt_iiwa_status &msg_iiwa_status) const override; 13 | 14 | void QueueNewPlan(PlanManagerStateMachine *state_machine, 15 | std::unique_ptr plan) override; 16 | 17 | bool CommandHasError(const State &state, const Command &cmd, 18 | PlanManagerStateMachine *state_machine, 19 | const double q_threshold) override; 20 | 21 | void PrintCurrentState(const PlanManagerStateMachine *manager, 22 | double t_now_seconds) const override; 23 | 24 | void AbortAllPlans(PlanManagerStateMachine *state_machine) override { 25 | throw std::runtime_error( 26 | "AbortAllPlans should not be called in state " + get_state_name()); 27 | }; 28 | 29 | [[nodiscard]] PlanManagerStateTypes get_state_type() const override { 30 | return PlanManagerStateTypes::kStateInit; 31 | }; 32 | 33 | private: 34 | StateInit() : PlanManagerStateBase("INIT"){}; 35 | static PlanManagerStateBase *instance_; 36 | }; 37 | -------------------------------------------------------------------------------- /src/state_machine/state_running.cc: -------------------------------------------------------------------------------- 1 | #include "state_running.h" 2 | #include "state_error.h" 3 | #include "state_idle.h" 4 | 5 | #include 6 | #include 7 | 8 | using std::cout; 9 | using std::endl; 10 | 11 | PlanManagerStateBase *StateRunning::instance_ = nullptr; 12 | PlanManagerStateBase *StateRunning::Instance() { 13 | if (!instance_) { 14 | instance_ = new StateRunning(); 15 | } 16 | return instance_; 17 | } 18 | 19 | const PlanBase *StateRunning::GetCurrentPlan( 20 | PlanManagerStateMachine *state_machine, double t_now, 21 | const drake::lcmt_iiwa_status &msg_iiwa_status) const { 22 | auto &plans = state_machine->get_mutable_plans_queue(); 23 | DRAKE_THROW_UNLESS(!plans.empty()); 24 | const double *t_start = state_machine->get_current_plan_start_time(); 25 | if (t_start == nullptr) { 26 | // current_plan_start_time_seconds_ is nullptr, meaning that there is no 27 | // active plan. 28 | state_machine->set_current_plan_start_time(t_now); 29 | } else { 30 | const double t_elapsed_seconds = state_machine->GetCurrentPlanUpTime(t_now); 31 | if (t_elapsed_seconds > plans.front()->duration() + 1.0) { 32 | // current plan has expired, switching to the next plan. 33 | // TODO: but we're moving towards not having a queue of plans in 34 | // state_machine? 35 | state_machine->set_current_plan_start_time(t_now); 36 | plans.pop(); 37 | } 38 | } 39 | if (plans.empty()) { 40 | state_machine->reset_current_plan_start_time(); 41 | ChangeState(state_machine, StateIdle::Instance()); 42 | return nullptr; 43 | } 44 | return plans.front().get(); 45 | } 46 | 47 | double 48 | StateRunning::GetCurrentPlanUpTime(const PlanManagerStateMachine *state_machine, 49 | double t_now) const { 50 | const double *t_start = state_machine->get_current_plan_start_time(); 51 | DRAKE_THROW_UNLESS(t_start != nullptr); 52 | return t_now - *t_start; 53 | } 54 | 55 | void StateRunning::QueueNewPlan(PlanManagerStateMachine *state_machine, 56 | std::unique_ptr plan) { 57 | // TODO: what is the desired behavior when receiving a new plan while a plan 58 | // is still being executed? Discard or queue? 59 | // Discard is probably the better option? The client should be blocked 60 | // while a plan is executing. A vector of plans can be sent over LCM to 61 | // be queued when state is IDLE. 62 | spdlog::warn("[Running]: another plan is running. " 63 | "Received plan is discarded."); 64 | } 65 | 66 | void StateRunning::PrintCurrentState( 67 | const PlanManagerStateMachine *state_machine, double t_now_seconds) const { 68 | std::string msg; 69 | msg += ("[RUNNING]: executing a plan. "); 70 | msg += "Number of plans: "; 71 | msg += std::to_string(state_machine->num_plans()); 72 | msg += ". t = "; 73 | msg += 74 | std::to_string(state_machine->get_state_machine_up_time(t_now_seconds)); 75 | spdlog::info(msg); 76 | } 77 | -------------------------------------------------------------------------------- /src/state_machine/state_running.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "state_machine/plan_manager_state_machine.h" 3 | 4 | class StateRunning : public PlanManagerStateBase { 5 | public: 6 | static PlanManagerStateBase *Instance(); 7 | 8 | const PlanBase * 9 | GetCurrentPlan(PlanManagerStateMachine *state_machine, double t_now, 10 | const drake::lcmt_iiwa_status &msg_iiwa_status) const override; 11 | 12 | double GetCurrentPlanUpTime(const PlanManagerStateMachine *state_machine, 13 | double t_now) const override; 14 | 15 | [[nodiscard]] bool has_received_status_msg() const override { return true; }; 16 | 17 | void ReceiveNewStatusMsg( 18 | PlanManagerStateMachine *state_machine, 19 | const drake::lcmt_iiwa_status &msg_iiwa_status) const override{}; 20 | 21 | void QueueNewPlan(PlanManagerStateMachine *state_machine, 22 | std::unique_ptr plan) override; 23 | 24 | void PrintCurrentState(const PlanManagerStateMachine *state_machine, 25 | double t_now_seconds) const override; 26 | 27 | [[nodiscard]] PlanManagerStateTypes get_state_type() const override { 28 | return PlanManagerStateTypes::kStateRunning; 29 | }; 30 | 31 | private: 32 | StateRunning() : PlanManagerStateBase("RUNNING"){}; 33 | static PlanManagerStateBase *instance_; 34 | }; 35 | -------------------------------------------------------------------------------- /src/utils/lcm_delay_analysis.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import os 3 | 4 | import lcm 5 | import numpy as np 6 | 7 | import matplotlib.pyplot as plt 8 | from drake import lcmt_iiwa_status, lcmt_iiwa_command 9 | 10 | #%% 11 | # log_path = os.path.join("/Users", "pangtao", "PycharmProjects", 12 | # "lcmlog-2021-04-26.01") 13 | log_path = os.path.join("/Users", "pangtao", "PycharmProjects", 14 | "lcmlog-2021-05-15.02") 15 | lcm_log = lcm.EventLog(log_path) 16 | 17 | #%% 18 | iiwa_status_list = [] 19 | iiwa_cmd_list = [] 20 | 21 | # extract time stamps and utimes of status and command messages from log file. 22 | for event in lcm_log: 23 | if event.channel == "IIWA_STATUS": 24 | msg = lcmt_iiwa_status.decode(event.data) 25 | iiwa_status_list.append((event.timestamp, msg.utime)) 26 | elif event.channel == "IIWA_COMMAND": 27 | msg = lcmt_iiwa_command.decode(event.data) 28 | iiwa_cmd_list.append((event.timestamp, msg.utime)) 29 | 30 | 31 | #%% match time stamps 32 | # make sure iiwa_cmd_list[0].utime >= iiwa_status_list[0].utime 33 | while iiwa_cmd_list[0][1] < iiwa_status_list[0][1]: 34 | iiwa_cmd_list.pop(0) 35 | 36 | # make sure iiwa_cmd_list[-1].utime == iiwa_status_list[-1].utime 37 | i_start_status = 0 38 | i_end_status = 0 39 | for i, status_times in enumerate(iiwa_status_list): 40 | if status_times[1] == iiwa_cmd_list[-1][1]: 41 | i_end_status = i 42 | break 43 | 44 | iiwa_status_list = iiwa_status_list[i_start_status:i_end_status + 1] 45 | 46 | print("number of iiwa command msgs: ", len(iiwa_cmd_list)) 47 | print("number of iiwa status msgs: ", len(iiwa_status_list)) 48 | 49 | 50 | #%% time difference between adjacent messages 51 | def get_dt(time_list): 52 | n = len(time_list) 53 | dt_list = [] 54 | nonuniform_count = 0 55 | for i in range(n - 1): 56 | t = time_list[i][1] 57 | t_next = time_list[i+1][1] 58 | d_utime = t_next - t 59 | if d_utime != 5000: 60 | # print(i, d_utime) 61 | nonuniform_count += 1 62 | dt_list.append((time_list[i+1][0] - time_list[i][0])/1000.) # in milliseconds 63 | print("Nonuniform messages count: {} / {}".format(nonuniform_count, n)) 64 | return np.array(dt_list) 65 | 66 | dt_status = get_dt(iiwa_status_list) 67 | dt_cmd = get_dt(iiwa_cmd_list) 68 | 69 | plt.figure(dpi=150) 70 | plt.hist(dt_cmd, label='cmd', bins=20) 71 | plt.hist(dt_status, label='status', bins=20) 72 | plt.ylabel('num messages') 73 | plt.yscale('log') 74 | plt.xlabel('ms') 75 | plt.legend() 76 | plt.title("time difference between adjacent messages\n" + log_path) 77 | plt.show() 78 | 79 | #%% time difference between cmd and status messages that have the same utime field. 80 | dt = [] 81 | idx_cmd = 0 82 | idx_status = 0 83 | n_cmd = len(iiwa_cmd_list) 84 | n_status = len(iiwa_status_list) 85 | 86 | while idx_cmd < n_cmd and idx_status < n_status: 87 | utime_cmd = iiwa_cmd_list[idx_cmd][1] 88 | utime_status = iiwa_status_list[idx_status][1] 89 | 90 | if utime_cmd == utime_status: 91 | dt.append((iiwa_cmd_list[idx_cmd][0] - 92 | iiwa_status_list[idx_status][0]) / 1000.) 93 | idx_cmd += 1 94 | idx_status += 1 95 | elif utime_cmd < utime_status: 96 | idx_cmd += 1 97 | elif utime_cmd > utime_status: 98 | idx_status += 1 99 | 100 | 101 | #%% 102 | dt = np.array(dt) 103 | plt.figure(dpi=200) 104 | plt.hist(dt, bins=20) 105 | plt.xlabel('ms') 106 | plt.title("time difference between cmd and status message\n" + log_path) 107 | plt.yscale('log') 108 | plt.ylabel('num message pairs') 109 | plt.grid(True) 110 | plt.show() 111 | 112 | print("dt mean:", np.mean(dt), "dt std", np.std(dt)) 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /third_party/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # -*- mode: cmake -*- 2 | # vi: set ft=cmake : 3 | 4 | # Copyright (c) 2019, Massachusetts Institute of Technology. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright notice, this 11 | # list of conditions and the following disclaimer. 12 | # 13 | # * Redistributions in binary form must reproduce the above copyright notice, 14 | # this list of conditions and the following disclaimer in the documentation 15 | # and/or other materials provided with the distribution. 16 | # 17 | # * Neither the name of the copyright holder nor the names of its contributors 18 | # may be used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | find_package(Threads) 34 | 35 | add_library(gtest STATIC googletest/gtest-all.cc googletest/gtest/gtest.h) 36 | target_include_directories(gtest PUBLIC googletest) 37 | target_link_libraries(gtest Threads::Threads) 38 | -------------------------------------------------------------------------------- /third_party/googletest/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2008, Google Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of Google Inc. nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | --------------------------------------------------------------------------------