├── .clang-format ├── .clang-format-check.sh ├── .clang-format-common.sh ├── .clang-format-fix.sh ├── .github └── workflows │ ├── ci.yaml │ ├── clang-format.yaml │ ├── config │ ├── PushCartHandForce.yaml │ ├── PushCartVelMode.yaml │ └── PushCartWaypoint.yaml │ └── scripts │ └── checkSimulationResults.py ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cnoid ├── CMakeLists.txt ├── model │ ├── CMakeLists.txt │ ├── Cart.body │ ├── CartHighFriction.body │ ├── FloorCheckeredPattern.body │ ├── LMC_materials.yaml │ └── texture │ │ └── CheckeredPattern.png └── project │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LMC_JVRC1_Cart.in.cnoid │ └── LMC_JVRC1_CartHighFriction.in.cnoid ├── depends.rosinstall ├── description ├── CMakeLists.txt ├── aliases │ └── .gitignore └── urdf │ └── Cart.urdf ├── doc ├── CMakeLists.txt └── Doxyfile.extra.in ├── etc ├── LocomanipController.in.yaml └── mc_rtc.in.yaml ├── include └── LocomanipController │ ├── CentroidalManager.h │ ├── FootTypes.h │ ├── HandTypes.h │ ├── LocomanipController.h │ ├── ManipManager.h │ ├── ManipPhase.h │ ├── MathUtils.h │ ├── State.h │ ├── centroidal │ └── CentroidalManagerPreviewControlExtZmp.h │ └── states │ ├── ConfigManipState.h │ ├── GuiManipState.h │ ├── InitialState.h │ └── TeleopState.h ├── launch └── display.launch ├── mujoco ├── CMakeLists.txt └── model │ ├── CMakeLists.txt │ └── Cart.xml ├── package.xml ├── rviz └── display.rviz ├── src ├── CMakeLists.txt ├── CentroidalManager.cpp ├── HandTypes.cpp ├── LocomanipController.cpp ├── ManipManager.cpp ├── ManipPhase.cpp ├── State.cpp ├── centroidal │ └── CentroidalManagerPreviewControlExtZmp.cpp ├── lib.cpp └── states │ ├── CMakeLists.txt │ ├── ConfigManipState.cpp │ ├── GuiManipState.cpp │ ├── InitialState.cpp │ ├── TeleopState.cpp │ └── data │ └── states.json └── tests └── CMakeLists.txt /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | AccessModifierOffset: -2 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignEscapedNewlinesLeft: true 8 | AlignOperands: true 9 | AlignTrailingComments: false 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: Empty 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: false 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: true 21 | BinPackParameters: false 22 | BreakBeforeBinaryOperators: NonAssignment 23 | BreakBeforeBraces: Allman 24 | BreakBeforeInheritanceComma: false 25 | BreakBeforeTernaryOperators: true 26 | BreakConstructorInitializersBeforeComma: false 27 | BreakConstructorInitializers: BeforeColon 28 | BreakAfterJavaFieldAnnotations: false 29 | BreakStringLiterals: true 30 | ColumnLimit: 120 31 | CommentPragmas: '^ IWYU pragma:' 32 | CompactNamespaces: false 33 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 34 | ConstructorInitializerIndentWidth: 0 35 | ContinuationIndentWidth: 4 36 | Cpp11BracedListStyle: true 37 | DerivePointerAlignment: false 38 | DisableFormat: false 39 | ExperimentalAutoDetectBinPacking: false 40 | FixNamespaceComments: true 41 | ForEachMacros: 42 | - foreach 43 | - Q_FOREACH 44 | - BOOST_FOREACH 45 | IncludeBlocks: Preserve 46 | IncludeCategories: 47 | - Regex: '^( $tmpfile 11 | if ! [[ -z `diff $tmpfile $f` ]]; then 12 | echo "Wrong formatting in $f" 13 | out=1 14 | fi 15 | done 16 | rm -f $tmpfile 17 | if [[ $out -eq 1 ]]; then 18 | echo "You can run ./.clang-format-fix.sh to fix the issues locally, then commit/push again" 19 | fi 20 | exit $out 21 | -------------------------------------------------------------------------------- /.clang-format-common.sh: -------------------------------------------------------------------------------- 1 | # This script is meant to be sourced from other scripts 2 | 3 | # Check for clang-format, prefer 10 if available 4 | if [[ -x "$(command -v clang-format-10)" ]]; then 5 | clang_format=clang-format-10 6 | elif [[ -x "$(command -v clang-format)" ]]; then 7 | clang_format=clang-format 8 | else 9 | echo "clang-format or clang-format-10 must be installed" 10 | exit 1 11 | fi 12 | 13 | # Find all source files in the project minus those that are auto-generated or we do not maintain 14 | src_files=`find src include tests -name '*.cpp' -or -name '*.h' -or -name '*.hpp'` 15 | -------------------------------------------------------------------------------- /.clang-format-fix.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | readonly this_dir=`cd $(dirname $0); pwd` 4 | cd $this_dir 5 | source .clang-format-common.sh 6 | 7 | for f in ${src_files}; do 8 | $clang_format -style=file -i $f 9 | done 10 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: CI of LocomanipController 2 | 3 | on: 4 | push: 5 | branches: 6 | - '**' 7 | pull_request: 8 | branches: 9 | - '**' 10 | schedule: 11 | - cron: '0 0 * * 0' 12 | 13 | jobs: 14 | build-and-test: 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | os: [ubuntu-20.04] 19 | catkin-build: [catkin, standalone] 20 | build-type: [RelWithDebInfo, Debug] 21 | mc-rtc-version: [head, stable] 22 | motion-type: [PushCartWaypoint] 23 | exclude: 24 | - build-type: Debug 25 | mc-rtc-version: stable 26 | # Some packages have not been released onto the stable mirror yet 27 | - catkin-build: standalone 28 | mc-rtc-version: stable 29 | include: 30 | - os: ubuntu-20.04 31 | catkin-build: catkin 32 | build-type: RelWithDebInfo 33 | mc-rtc-version: head 34 | motion-type: PushCartVelMode 35 | - os: ubuntu-20.04 36 | catkin-build: standalone 37 | build-type: RelWithDebInfo 38 | mc-rtc-version: head 39 | motion-type: PushCartVelMode 40 | - os: ubuntu-20.04 41 | catkin-build: catkin 42 | build-type: RelWithDebInfo 43 | mc-rtc-version: head 44 | motion-type: PushCartHandForce 45 | - os: ubuntu-20.04 46 | catkin-build: standalone 47 | build-type: RelWithDebInfo 48 | mc-rtc-version: head 49 | motion-type: PushCartHandForce 50 | runs-on: ${{ matrix.os }} 51 | env: 52 | RESULTS_POSTFIX: ${{ matrix.motion-type }}-${{ matrix.catkin-build }} 53 | steps: 54 | - name: Setup environment variables 55 | run: | 56 | set -e 57 | set -x 58 | if [ "${{ matrix.os }}" == "ubuntu-20.04" ] && \ 59 | [ "${{ matrix.build-type }}" == "RelWithDebInfo" ] && \ 60 | [ "${{ matrix.mc-rtc-version }}" == "head" ] 61 | then 62 | echo "RUN_SIMULATION_STEPS=true" >> $GITHUB_ENV 63 | else 64 | echo "RUN_SIMULATION_STEPS=false" >> $GITHUB_ENV 65 | fi 66 | if [ "${{ matrix.os }}" == "ubuntu-20.04" ] && \ 67 | [ "${{ matrix.catkin-build }}" == "catkin" ] && \ 68 | [ "${{ matrix.build-type }}" == "RelWithDebInfo" ] && \ 69 | [ "${{ matrix.mc-rtc-version }}" == "head" ] && \ 70 | [ "${{ matrix.motion-type }}" == "PushCartWaypoint" ] && \ 71 | [ "${{ github.repository_owner }}" == "isri-aist" ] && \ 72 | [ "${{ github.ref }}" == "refs/heads/master" ] 73 | then 74 | echo "UPLOAD_DOCUMENTATION=true" >> $GITHUB_ENV 75 | else 76 | echo "UPLOAD_DOCUMENTATION=false" >> $GITHUB_ENV 77 | fi 78 | if [ "${{ matrix.catkin-build }}" == "catkin" ] 79 | then 80 | echo "CI_DIR=${GITHUB_WORKSPACE}/catkin_ws/src/${{ github.repository }}/.github/workflows" >> $GITHUB_ENV 81 | else 82 | echo "CI_DIR=${GITHUB_WORKSPACE}/.github/workflows" >> $GITHUB_ENV 83 | fi 84 | - name: Install dependencies 85 | uses: jrl-umi3218/github-actions/install-dependencies@master 86 | with: 87 | ubuntu: | 88 | apt-mirrors: 89 | mc-rtc: 90 | cloudsmith: mc-rtc/${{ matrix.mc-rtc-version }} 91 | apt: libmc-rtc-dev mc-rtc-utils mc-state-observation doxygen graphviz jvrc-choreonoid choreonoid libcnoid-dev 92 | ros: | 93 | apt: ros-base mc-rtc-plugin eigen-conversions 94 | - name: Install standalone dependencies 95 | if: matrix.catkin-build == 'standalone' 96 | uses: jrl-umi3218/github-actions/install-dependencies@master 97 | with: 98 | build-type: ${{ matrix.build-type }} 99 | ubuntu: | 100 | apt: libforcecontrolcollection-dev libtrajectorycollection-dev libcentroidalcontrolcollection-dev 101 | github: | 102 | - path: isri-aist/BaselineWalkingController 103 | - path: isri-aist/CnoidRosUtils 104 | - name: Checkout repository code 105 | if: matrix.catkin-build == 'standalone' 106 | uses: actions/checkout@v3 107 | with: 108 | submodules: recursive 109 | - name: Standalone build 110 | if: matrix.catkin-build == 'standalone' 111 | uses: jrl-umi3218/github-actions/build-cmake-project@master 112 | with: 113 | build-type: ${{ matrix.build-type }} 114 | - name: Catkin build 115 | if: matrix.catkin-build == 'catkin' 116 | uses: jrl-umi3218/github-actions/build-catkin-project@master 117 | with: 118 | build-type: ${{ matrix.build-type }} 119 | cmake-args: -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} -DENABLE_QLD=ON 120 | catkin-test-args: --no-deps 121 | build-packages: locomanip_controller 122 | test-packages: locomanip_controller 123 | - name: Run simulation 124 | if: env.RUN_SIMULATION_STEPS == 'true' 125 | # https://github.com/jrl-umi3218/lipm_walking_controller/blob/b564d655388ae6a6725c504e5c74a62192e58c7c/.github/workflows/build.yml#L64-L92 126 | run: | 127 | set -e 128 | set -x 129 | sudo apt-get install -y -qq xvfb ffmpeg mesa-utils fluxbox xserver-xorg xserver-xorg-core xserver-xorg-video-all libwayland-egl1-mesa 130 | set +x 131 | if [ "${{ matrix.catkin-build }}" == "catkin" ] 132 | then 133 | . ${GITHUB_WORKSPACE}/catkin_ws/devel/setup.bash 134 | fi 135 | set -x 136 | export DISPLAY=":1" 137 | Xvfb ${DISPLAY} -screen 0 1920x1080x24 & 138 | sleep 10s 139 | fluxbox 2> /dev/null & 140 | mkdir -p ${HOME}/.config/mc_rtc/controllers 141 | if [ "${{ matrix.catkin-build }}" == "catkin" ] 142 | then 143 | cp ${GITHUB_WORKSPACE}/catkin_ws/src/${{ github.repository }}/etc/mc_rtc.yaml ${HOME}/.config/mc_rtc 144 | else 145 | echo "Enabled: LocomanipController" > ${HOME}/.config/mc_rtc/mc_rtc.yaml 146 | fi 147 | cp ${{ env.CI_DIR }}/config/${{ matrix.motion-type }}.yaml ${HOME}/.config/mc_rtc/controllers/LocomanipController.yaml 148 | ffmpeg -y -f x11grab -s 1920x1080 -r 30 -i ${DISPLAY} -qscale 0 -vcodec huffyuv /tmp/video.avi > /dev/null 2>&1 < /dev/null & 149 | FFMPEG_PID=$! 150 | /usr/share/hrpsys/samples/JVRC1/clear-omninames.sh 151 | roscore > /dev/null 2>&1 < /dev/null & 152 | ROSCORE_PID=$! 153 | sleep 1s 154 | if [ "${{ matrix.catkin-build }}" == "catkin" ]; then 155 | roscd locomanip_controller/cnoid/project 156 | else 157 | cd /usr/share/hrpsys/samples/JVRC1 158 | fi 159 | if [ "${{ matrix.motion-type }}" == "PushCartWaypoint" ]; then 160 | CNOID_FILE=LMC_JVRC1_Cart.cnoid 161 | elif [ "${{ matrix.motion-type }}" == "PushCartVelMode" ]; then 162 | CNOID_FILE=LMC_JVRC1_Cart.cnoid 163 | else # if [ "${{ matrix.motion-type }}" == "PushCartHandForce" ]; then 164 | CNOID_FILE=LMC_JVRC1_CartHighFriction.cnoid 165 | fi 166 | choreonoid ${CNOID_FILE} --test-mode --start-simulation & 167 | CNOID_PID=$! 168 | if [ "${{ matrix.motion-type }}" == "PushCartWaypoint" ]; then 169 | SLEEP_DURATION="150s" 170 | elif [ "${{ matrix.motion-type }}" == "PushCartVelMode" ]; then 171 | SLEEP_DURATION="100s" 172 | else # if [ "${{ matrix.motion-type }}" == "PushCartHandForce" ]; then 173 | SLEEP_DURATION="100s" 174 | fi 175 | sleep ${SLEEP_DURATION} 176 | kill -2 ${ROSCORE_PID} 177 | kill -2 ${CNOID_PID} 178 | kill -2 ${FFMPEG_PID} 179 | sleep 1s 180 | kill -9 ${CNOID_PID} || true 181 | sleep 10s 182 | mkdir -p /tmp/results 183 | ffmpeg -nostats -i /tmp/video.avi /tmp/results/LMC-video-${RESULTS_POSTFIX}.mp4 184 | LOG_FILENAME=LMC-log-${RESULTS_POSTFIX} 185 | mv `readlink -f /tmp/mc-control-LocomanipController-latest.bin` /tmp/${LOG_FILENAME}.bin 186 | tar czf /tmp/results/${LOG_FILENAME}.tar.gz -C /tmp ${LOG_FILENAME}.bin 187 | - name: Upload simulation data 188 | if: env.RUN_SIMULATION_STEPS == 'true' 189 | uses: actions/upload-artifact@v4 190 | with: 191 | name: LMC-ci-results-${{ env.RESULTS_POSTFIX }} 192 | path: /tmp/results 193 | - name: Check simulation results 194 | if: env.RUN_SIMULATION_STEPS == 'true' 195 | run: | 196 | set -e 197 | set -x 198 | if [ "${{ matrix.motion-type }}" == "PushCartWaypoint" ]; then 199 | EXPECTED_OBJ_POS="1.18 0.494 0.0" 200 | elif [ "${{ matrix.motion-type }}" == "PushCartVelMode" ]; then 201 | EXPECTED_OBJ_POS="1.33 -0.073 0.0" 202 | else # if [ "${{ matrix.motion-type }}" == "PushCartHandForce" ]; then 203 | EXPECTED_OBJ_POS="2.0 0.0 0.0" 204 | fi 205 | LOG_FILENAME=LMC-log-${RESULTS_POSTFIX} 206 | python3 ${{ env.CI_DIR }}/scripts/checkSimulationResults.py /tmp/${LOG_FILENAME}.bin --expected-obj-pos ${EXPECTED_OBJ_POS} 207 | - name: Upload documentation 208 | if: env.UPLOAD_DOCUMENTATION == 'true' 209 | run: | 210 | set -e 211 | set -x 212 | cd ${GITHUB_WORKSPACE}/catkin_ws/src/${{ github.repository }} 213 | git config --global user.name "Masaki Murooka" 214 | git config --global user.email "m-murooka@aist.go.jp" 215 | git remote set-url origin "https://mmurooka:${{ secrets.CI_TOKEN }}@github.com/isri-aist/LocomanipController" 216 | git fetch --depth=1 origin gh-pages:gh-pages 217 | git clean -dfx 218 | rm -rf cmake/ 219 | git checkout --quiet gh-pages 220 | rm -rf doxygen/ 221 | cp -r ${GITHUB_WORKSPACE}/catkin_ws/build/locomanip_controller/doc/html/ doxygen 222 | git add doxygen 223 | git_status=`git status -s` 224 | if test -n "$git_status"; then 225 | git commit --quiet -m "Update Doxygen HTML files from commit ${{ github.sha }}" 226 | git push origin gh-pages 227 | else 228 | echo "Github pages documentation is already up-to-date." 229 | fi 230 | -------------------------------------------------------------------------------- /.github/workflows/clang-format.yaml: -------------------------------------------------------------------------------- 1 | name: Check clang-format 2 | 3 | on: 4 | push: 5 | branches: 6 | - '**' 7 | pull_request: 8 | branches: 9 | - '**' 10 | 11 | jobs: 12 | clang-format: 13 | runs-on: ubuntu-20.04 14 | steps: 15 | - name: Checkout repository code 16 | uses: actions/checkout@v3 17 | - name: Install clang-format-10 18 | run: | 19 | sudo apt-get -y -qq update 20 | sudo apt-get -y -qq install clang-format-10 21 | - name: Run clang-format-check 22 | run: | 23 | ./.clang-format-check.sh 24 | -------------------------------------------------------------------------------- /.github/workflows/config/PushCartHandForce.yaml: -------------------------------------------------------------------------------- 1 | transitions: 2 | - [LMC::Initial_, OK, LMC::ConfigManip_, Auto] 3 | 4 | states: 5 | LMC::Initial_: 6 | base: LMC::Initial 7 | configs: 8 | autoStartTime: 2.0 9 | 10 | LMC::ConfigManip_: 11 | base: LMC::ConfigManip 12 | configs: 13 | preUpdateObj: true 14 | preWalk: true 15 | preHandWrenches: 16 | Left: 17 | # -25.0 * np.array([0.0, -1 * np.sin(np.deg2rad(15)), np.cos(np.deg2rad(15))]) 18 | force: [0.0, 6.4704761275630185, -24.148145657226706] 19 | couple: [0.0, 0.0, 0.0] 20 | Right: 21 | # -25.0 * np.array([0.0, np.sin(np.deg2rad(15)), np.cos(np.deg2rad(15))]) 22 | force: [0.0, -6.4704761275630185, -24.148145657226706] 23 | couple: [0.0, 0.0, 0.0] 24 | postHandWrenches: 25 | Left: 26 | force: [0.0, 0.0, 0.0] 27 | couple: [0.0, 0.0, 0.0] 28 | Right: 29 | force: [0.0, 0.0, 0.0] 30 | couple: [0.0, 0.0, 0.0] 31 | waypointList: 32 | - startTime: 2.0 33 | duration: 10.0 34 | relPose: 35 | translation: [1.0, 0.0, 0.0] 36 | 37 | ManipManager: 38 | objPoseTopic: /cnoid/object/pose 39 | objVelTopic: /cnoid/object/vel 40 | -------------------------------------------------------------------------------- /.github/workflows/config/PushCartVelMode.yaml: -------------------------------------------------------------------------------- 1 | transitions: 2 | - [LMC::Initial_, OK, LMC::ConfigManip_, Auto] 3 | 4 | states: 5 | LMC::Initial_: 6 | base: LMC::Initial 7 | configs: 8 | autoStartTime: 2.0 9 | 10 | LMC::ConfigManip_: 11 | base: LMC::ConfigManip 12 | configs: 13 | preUpdateObj: true 14 | preWalk: true 15 | velocityMode: 16 | velocity: [0.1, 0.0, -0.20943951023931956] 17 | duration: 10.0 # [sec] 18 | 19 | ManipManager: 20 | objPoseTopic: /cnoid/object/pose 21 | objVelTopic: /cnoid/object/vel 22 | -------------------------------------------------------------------------------- /.github/workflows/config/PushCartWaypoint.yaml: -------------------------------------------------------------------------------- 1 | transitions: 2 | - [LMC::Initial_, OK, LMC::Main_, Auto] 3 | 4 | states: 5 | LMC::Initial_: 6 | base: LMC::Initial 7 | configs: 8 | autoStartTime: 2.0 9 | 10 | LMC::ConfigManip_: 11 | base: LMC::ConfigManip 12 | configs: 13 | preUpdateObj: true 14 | preWalk: true 15 | waypointList: 16 | - startTime: 2.0 17 | duration: 10.0 18 | relPose: 19 | translation: [1.0, 0.0, 0.0] 20 | - duration: 10.0 21 | relPose: 22 | rotation: [0.0, 0.0, -0.5235987755982988] 23 | - duration: 10.0 24 | relPose: 25 | translation: [-1.0, 0.0, 0.0] 26 | 27 | LMC::Main_: 28 | base: Parallel 29 | states: [LMC::GuiManip_, LMC::ConfigManip_] 30 | 31 | ManipManager: 32 | objPoseTopic: /cnoid/object/pose 33 | objVelTopic: /cnoid/object/vel 34 | -------------------------------------------------------------------------------- /.github/workflows/scripts/checkSimulationResults.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import sys 4 | import argparse 5 | import numpy as np 6 | import eigen as e 7 | import mc_log_ui 8 | 9 | exit_status = 0 10 | 11 | # Parse arguments 12 | parser = argparse.ArgumentParser(description="Check simulation results.") 13 | parser.add_argument("log-filename", type=str, help="log filename") 14 | parser.add_argument("--tilting-angle-thre", type=float, default=30.0, 15 | help="tilting angle threshold [deg]") 16 | parser.add_argument("--expected-obj-pos", nargs=3, type=float, default=None, 17 | help="expected object position [m]") 18 | parser.add_argument("--obj-pos-thre", nargs=3, type=float, default=[0.25, 0.25, 0.25], 19 | help="object position threshold [m]") 20 | args = parser.parse_args() 21 | 22 | # Load log file 23 | args.log_filename = sys.argv[1] 24 | print("[checkSimulationResults.py] Load {}".format(args.log_filename)) 25 | log = mc_log_ui.read_log(args.log_filename) 26 | 27 | # Check tilting angle 28 | robot_quat_data_list = np.array([log["FloatingBase_orientation_w"], 29 | log["FloatingBase_orientation_x"], 30 | log["FloatingBase_orientation_y"], 31 | log["FloatingBase_orientation_z"]]).T 32 | obj_quat_data_list = np.array([log["ManipManager_objPose_measured_qw"], 33 | log["ManipManager_objPose_measured_qx"], 34 | log["ManipManager_objPose_measured_qy"], 35 | log["ManipManager_objPose_measured_qz"]]).T 36 | 37 | tilting_angle_list = [] 38 | for quat_data in robot_quat_data_list: 39 | # Inverse is required because the left-hand system is used in mc_rtc 40 | quat = e.Quaterniond(*quat_data).inverse() 41 | rot = quat.toRotationMatrix() 42 | z_axis = e.Vector3d(rot.block(0, 2, 3, 1)) 43 | tilting_angle_list.append(np.rad2deg(np.arccos(z_axis.dot(e.Vector3d.UnitZ())))) # [deg] 44 | for quat_data in obj_quat_data_list: 45 | # Inverse is required because the left-hand system is used in mc_rtc 46 | quat = e.Quaterniond(*quat_data).inverse() 47 | rot = quat.toRotationMatrix() 48 | z_axis = e.Vector3d(rot.block(0, 2, 3, 1)) 49 | tilting_angle_list.append(np.rad2deg(np.arccos(z_axis.dot(e.Vector3d.UnitZ())))) # [deg] 50 | tilting_angle_list = np.array(tilting_angle_list) 51 | max_tilting_angle = np.max(np.abs(tilting_angle_list)) 52 | 53 | if max_tilting_angle <= args.tilting_angle_thre: 54 | print(u"[success][checkSimulationResults.py] max_tilting_angle is below the threshold: {:.1f} <= \u00b1 {:.1f} [deg]".format( 55 | max_tilting_angle, args.tilting_angle_thre)) 56 | else: 57 | print(u"[error][checkSimulationResults.py] max_tilting_angle exceeds the threshold: {:.1f} > \u00b1 {:.1f} [deg]".format( 58 | max_tilting_angle, args.tilting_angle_thre)) 59 | exit_status = 1 60 | 61 | # Check last position 62 | last_obj_pos = np.array([log["ManipManager_objPose_measured_tx"][-1], 63 | log["ManipManager_objPose_measured_ty"][-1], 64 | log["ManipManager_objPose_measured_tz"][-1]]) 65 | 66 | if args.expected_obj_pos is not None: 67 | if (np.abs(last_obj_pos - np.array(args.expected_obj_pos)) < np.array(args.obj_pos_thre)).all(): 68 | with np.printoptions(precision=2): 69 | print(u"[success][checkSimulationResults.py] last_obj_pos is within the expected range: {} <= {} \u00b1 {} [m]".format( 70 | last_obj_pos, np.array(args.expected_obj_pos), np.array(args.obj_pos_thre))) 71 | else: 72 | with np.printoptions(precision=2): 73 | print(u"[error][checkSimulationResults.py] last_obj_pos is outside the expected range: {} > {} \u00b1 {} [m]".format( 74 | last_obj_pos, np.array(args.expected_obj_pos), np.array(args.obj_pos_thre))) 75 | exit_status = 1 76 | 77 | sys.exit(exit_status) 78 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | 4 | GPATH 5 | GRTAGS 6 | GSYMS 7 | GTAGS 8 | 9 | etc/mc_rtc.yaml 10 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | 3 | set(PROJECT_NAME locomanip_controller) 4 | set(PROJECT_GENERATED_HEADERS_SKIP_DEPRECATED ON) 5 | set(PROJECT_GENERATED_HEADERS_SKIP_CONFIG ON) 6 | set(PROJECT_GENERATED_HEADERS_SKIP_WARNING ON) 7 | set(PROJECT_URL https://github.com/isri-aist/LocomanipController) 8 | set(PROJECT_DESCRIPTION "") 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(PROJECT_USE_CMAKE_EXPORT TRUE) 11 | set(CXX_DISABLE_WERROR ON) 12 | set(CMAKE_COLOR_DIAGNOSTICS ON) 13 | option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) 14 | 15 | include(cmake/base.cmake) 16 | project(locomanip_controller LANGUAGES CXX) 17 | 18 | OPTION(ENABLE_CNOID "Install Choreonoid files" ON) 19 | OPTION(ENABLE_MUJOCO "Install MuJoCo files" OFF) 20 | 21 | # mc_rtc 22 | add_project_dependency(mc_rtc REQUIRED) 23 | if(NOT TARGET mc_rtc::mc_rtc_ros) 24 | message(FATAL_ERROR "mc_rtc ROS plugin is required for this controller") 25 | endif() 26 | 27 | if(DEFINED CATKIN_DEVEL_PREFIX) 28 | set(DOXYGEN_HTML_OUTPUT html) 29 | if(ENABLE_CNOID) 30 | set(CNOID_ROS_UTILS cnoid_ros_utils) 31 | else() 32 | set(CNOID_ROS_UTILS "") 33 | endif() 34 | find_package(catkin REQUIRED COMPONENTS 35 | baseline_walking_controller 36 | ${CNOID_ROS_UTILS} 37 | ) 38 | 39 | catkin_package( 40 | CATKIN_DEPENDS 41 | baseline_walking_controller 42 | DEPENDS EIGEN3 43 | INCLUDE_DIRS include 44 | LIBRARIES LocomanipController 45 | ) 46 | 47 | include_directories(include ${catkin_INCLUDE_DIRS}) 48 | link_directories(${catkin_LIBRARY_DIRS}) 49 | 50 | set(BWC_STATES_LIBRARIES "${baseline_walking_controller_DEVEL_PREFIX}/lib/baseline_walking_controller/states") 51 | set(BWC_STATES_FILES "${BWC_STATES_LIBRARIES}/data") 52 | set(STATES_INSTALL_PREFIX "${CATKIN_DEVEL_PREFIX}/lib") 53 | set(CONFIG_OUT "${CATKIN_DEVEL_PREFIX}/lib/mc_controller/etc/LocomanipController.yaml") 54 | set(CONFIG_DIR_STANDALONE "${MC_CONTROLLER_RUNTIME_INSTALL_PREFIX}/LocomanipController") 55 | set(CONFIG_DIR_CATKIN "${CATKIN_DEVEL_PREFIX}/lib/mc_controller/LocomanipController") 56 | if(EXISTS "${CONFIG_DIR_STANDALONE}" AND IS_DIRECTORY "${CONFIG_DIR_STANDALONE}") 57 | execute_process(COMMAND cmake -E create_symlink 58 | "${CONFIG_DIR_STANDALONE}" "${CONFIG_DIR_CATKIN}" 59 | ) 60 | endif() 61 | 62 | configure_file(etc/mc_rtc.in.yaml ${PROJECT_SOURCE_DIR}/etc/mc_rtc.yaml @ONLY) 63 | else() 64 | set(DOXYGEN_HTML_OUTPUT doxygen-html) 65 | set(STATES_INSTALL_PREFIX ${MC_CONTROLLER_INSTALL_PREFIX}) 66 | set(CONFIG_OUT "${CMAKE_CURRENT_BINARY_DIR}/etc/LocomanipController.yaml") 67 | add_project_dependency(baseline_walking_controller REQUIRED) 68 | if(ENABLE_CNOID) 69 | add_project_dependency(CnoidRosUtils REQUIRED) 70 | endif() 71 | endif() 72 | 73 | configure_file(etc/LocomanipController.in.yaml ${CONFIG_OUT} @ONLY) 74 | install(FILES ${CONFIG_OUT} DESTINATION ${MC_RTC_LIBDIR}/mc_controller/etc/) 75 | 76 | add_subdirectory(src) 77 | 78 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/LocomanipController DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} FILES_MATCHING PATTERN "*.h") 79 | 80 | add_subdirectory(description) 81 | 82 | if(ENABLE_CNOID) 83 | add_subdirectory(cnoid) 84 | endif() 85 | 86 | if(ENABLE_MUJOCO) 87 | add_subdirectory(mujoco) 88 | endif() 89 | 90 | if(BUILD_TESTING OR CATKIN_ENABLE_TESTING) 91 | add_subdirectory(tests) 92 | endif() 93 | 94 | if(INSTALL_DOCUMENTATION) 95 | add_subdirectory(doc) 96 | endif() 97 | 98 | set(PACKAGE_EXTRA_MACROS "set(LOCOMANIP_CONTROLLER_STATES_LIBRARIES \"${MC_CONTROLLER_INSTALL_PREFIX}/locomanip_controller/states\") 99 | set(LOCOMANIP_CONTROLLER_STATES_FILES \"\${LOCOMANIP_CONTROLLER_STATES_LIBRARIES}/data\")") 100 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2022, AIST-CNRS JRL 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # [LocomanipController](https://github.com/isri-aist/LocomanipController) 2 | Humanoid loco-manipulation controller 3 | 4 | [![CI](https://github.com/isri-aist/LocomanipController/actions/workflows/ci.yaml/badge.svg)](https://github.com/isri-aist/LocomanipController/actions/workflows/ci.yaml) 5 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/LocomanipController/) 6 | [![LICENSE](https://img.shields.io/github/license/isri-aist/LocomanipController)](https://github.com/isri-aist/LocomanipController/blob/master/LICENSE) 7 | 8 | https://user-images.githubusercontent.com/6636600/213716066-9f17af4c-4c75-44f7-be30-eb9ba92f4103.mp4 9 | 10 | https://user-images.githubusercontent.com/6636600/209670856-4ebf4456-9cf0-483e-8f41-db9ce55c9795.mp4 11 | 12 | ## Features 13 | - Completely open source! (controller framework: mc_rtc, simulator: Choreonoid, sample robot model: JVRC1) 14 | - Accepts commands for trajectory and velocity of a loco-manipulation object. 15 | - Considers object manipulation forces on the hands in the balance control during loco-manipulation. 16 | - Automated management with CI: Dynamics simulation is run on CI to verify loco-manipulation. 17 | 18 | ## Technical details 19 | This controller is a simple extension of [BaselineWalkingController](https://github.com/isri-aist/BaselineWalkingController) for loco-manipulation. 20 | For more information on walking control, see [BaselineWalkingController](https://github.com/isri-aist/BaselineWalkingController). 21 | For more information on the balance control in loco-manipulation considering object manipulation forces, please see the following paper: 22 | - M Murooka, et al. Humanoid loco-Manipulations pattern generation and stabilization control. RA-Letters, 2021. [(available here)](https://hal.science/hal-03425667) 23 | 24 | ## Install 25 | 26 | ### Requirements 27 | - Compiler supporting C++17 28 | - Tested with `Ubuntu 20.04 / ROS Noetic` and `Ubuntu 18.04 / ROS Melodic` 29 | 30 | ### Dependencies 31 | This package depends on 32 | - [mc_rtc](https://jrl-umi3218.github.io/mc_rtc) 33 | 34 | This package also depends on the following packages. However, manual installation is unnecessary when this package is installed using `wstool` as described in [Controller installation](#controller-installation). 35 | - [QpSolverCollection](https://github.com/isri-aist/QpSolverCollection) 36 | - [ForceControlCollection](https://github.com/isri-aist/ForceControlCollection) 37 | - [TrajectoryCollection](https://github.com/isri-aist/TrajectoryCollection) 38 | - [NMPC](https://github.com/isri-aist/NMPC) 39 | - [CentroidalControlCollection](https://github.com/isri-aist/CentroidalControlCollection) 40 | - [BaselineWalkingController](https://github.com/isri-aist/BaselineWalkingController) 41 | - [CnoidRosUtils](https://github.com/isri-aist/CnoidRosUtils) (Only for Choreonoid simulation) 42 | 43 | ### Preparation 44 | 1. (Skip if ROS is already installed.) Install ROS. See [here](http://wiki.ros.org/ROS/Installation) for details. 45 | ```bash 46 | $ export ROS_DISTRO=melodic 47 | $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 48 | $ wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 49 | $ sudo apt-get update 50 | $ sudo apt-get install ros-${ROS_DISTRO}-ros-base python-catkin-tools python-rosdep 51 | ``` 52 | 53 | 2. (Skip if mc_rtc is already installed.) Install mc_rtc. See [here](https://jrl-umi3218.github.io/mc_rtc/tutorials/introduction/installation-guide.html) for details. 54 | ```bash 55 | $ curl -1sLf 'https://dl.cloudsmith.io/public/mc-rtc/stable/setup.deb.sh' | sudo -E bash 56 | $ sudo apt-get install libmc-rtc-dev mc-rtc-utils mc-state-observation jvrc-choreonoid libcnoid-dev ros-${ROS_DISTRO}-mc-rtc-plugin ros-${ROS_DISTRO}-mc-rtc-rviz-panel libeigen-qld-dev 57 | ``` 58 | 59 | ### Controller installation 60 | 1. Setup catkin workspace. 61 | ```bash 62 | $ mkdir -p ~/ros/ws_lmc/src 63 | $ cd ~/ros/ws_lmc 64 | $ wstool init src 65 | $ wstool set -t src isri-aist/LocomanipController https://github.com/isri-aist/LocomanipController --git -y 66 | $ wstool update -t src isri-aist/LocomanipController 67 | $ wstool merge -t src src/isri-aist/LocomanipController/depends.rosinstall 68 | $ wstool update -t src 69 | ``` 70 | 71 | 2. Install dependent packages. 72 | ```bash 73 | $ source /opt/ros/${ROS_DISTRO}/setup.bash 74 | $ rosdep install -y -r --from-paths src --ignore-src 75 | ``` 76 | 77 | 3. Build a package. 78 | ```bash 79 | $ catkin build locomanip_controller -DCMAKE_BUILD_TYPE=RelWithDebInfo -DENABLE_QLD=ON --catkin-make-args all tests 80 | ``` 81 | 82 | 4. Setup controller 83 | ```bash 84 | $ mkdir -p ~/.config/mc_rtc/controllers 85 | $ cp ~/ros/ws_lmc/src/isri-aist/LocomanipController/etc/mc_rtc.yaml ~/.config/mc_rtc/mc_rtc.yaml 86 | ``` 87 | 88 | 5. Setup motion configuration file (optional) 89 | ```bash 90 | $ roscd locomanip_controller 91 | $ cp .github/workflows/config/PushCartWaypoint.yaml ~/.config/mc_rtc/controllers/LocomanipController.yaml 92 | ``` 93 | 94 | ### Simulation execution 95 | ```bash 96 | # Terminal 1 97 | $ source ~/ros/ws_lmc/devel/setup.bash 98 | $ roscore 99 | # Terminal 2 100 | $ source ~/ros/ws_lmc/devel/setup.bash 101 | $ roscd locomanip_controller/cnoid/project/ 102 | $ /usr/share/hrpsys/samples/JVRC1/clear-omninames.sh 103 | $ choreonoid LMC_JVRC1_Cart.cnoid --start-simulation 104 | # Terminal 3 105 | $ source ~/ros/ws_lmc/devel/setup.bash 106 | $ roslaunch locomanip_controller display.launch 107 | ``` 108 | -------------------------------------------------------------------------------- /cnoid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(PkgConfig) 2 | 3 | # openhrp3.1 4 | if(NOT DEFINED OPENHRP3_1_PREFIX) 5 | pkg_check_modules(OPENHRP3_1 REQUIRED openhrp3.1) 6 | endif() 7 | message("-- OPENHRP3_1_PREFIX: ${OPENHRP3_1_PREFIX}") 8 | 9 | # hrpsys-base 10 | if(NOT DEFINED HRPSYSBASE_PREFIX) 11 | pkg_check_modules(HRPSYSBASE REQUIRED hrpsys-base) 12 | endif() 13 | message("-- HRPSYSBASE_PREFIX: ${HRPSYSBASE_PREFIX}") 14 | 15 | pkg_check_modules(CNOID REQUIRED choreonoid) 16 | execute_process(COMMAND ${PKG_CONFIG_EXECUTABLE} --variable=sharedir choreonoid 17 | OUTPUT_VARIABLE CNOID_SHARE_SUBDIR 18 | OUTPUT_STRIP_TRAILING_WHITESPACE) 19 | message("-- CNOID_SHARE_SUBDIR: ${CNOID_SHARE_SUBDIR}") 20 | 21 | add_subdirectory(model) 22 | add_subdirectory(project) 23 | -------------------------------------------------------------------------------- /cnoid/model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(MODEL_FILE_LIST 2 | texture/CheckeredPattern.png 3 | Cart.body 4 | CartHighFriction.body 5 | FloorCheckeredPattern.body 6 | LMC_materials.yaml 7 | ) 8 | 9 | foreach(MODEL_FILE ${MODEL_FILE_LIST}) 10 | configure_file(${MODEL_FILE} ${CMAKE_CURRENT_BINARY_DIR}/model/environment/LmcCnoid/${MODEL_FILE} COPYONLY) 11 | endforeach() 12 | 13 | install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/model/environment/LmcCnoid DESTINATION ${CNOID_SHARE_SUBDIR}/model/environment) 14 | -------------------------------------------------------------------------------- /cnoid/model/Cart.body: -------------------------------------------------------------------------------- 1 | format: ChoreonoidBody 2 | formatVersion: 1.0 3 | angleUnit: degree 4 | name: Cart 5 | rootLink: Root 6 | links: 7 | - 8 | name: Root 9 | jointType: free 10 | - 11 | name: Body 12 | parent: Root 13 | jointType: fixed 14 | material: Cart 15 | translation: [ 0, 0, 0 ] 16 | centerOfMass: [ 0.4, 0, 0.3 ] 17 | mass: 10.0 18 | inertia: [ 19 | 1.0, 0, 0, 20 | 0, 1.0, 0, 21 | 0, 0, 1.0 ] 22 | elements: 23 | - 24 | type: Shape 25 | translation: [ 0.425, 0, 0.325 ] 26 | geometry: { type: Box, size: [ 0.85, 0.8, 0.65 ] } 27 | appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} 28 | - 29 | type: Shape 30 | translation: [ -0.35, 0, 1.0 ] 31 | rotation: [0, 0, 1, 0] 32 | geometry: { type: Cylinder, radius: 0.015, height: 0.8 } 33 | appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} 34 | - 35 | type: Shape 36 | translation: [ -0.175, 0.38, 0.825 ] 37 | rotation: [0.707107, 0, 0.707107, 90] 38 | geometry: { type: Cylinder, radius: 0.015, height: 0.52 } 39 | appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} 40 | - 41 | type: Shape 42 | translation: [ -0.175, -0.38, 0.825 ] 43 | rotation: [0.707107, 0, 0.707107, 90] 44 | geometry: { type: Cylinder, radius: 0.015, height: 0.52 } 45 | appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} 46 | -------------------------------------------------------------------------------- /cnoid/model/CartHighFriction.body: -------------------------------------------------------------------------------- 1 | format: ChoreonoidBody 2 | formatVersion: 1.0 3 | angleUnit: degree 4 | name: CartHighFriction 5 | rootLink: Root 6 | links: 7 | - 8 | name: Root 9 | jointType: free 10 | - 11 | name: Body 12 | parent: Root 13 | jointType: fixed 14 | material: CartHighFriction 15 | translation: [ 0, 0, 0 ] 16 | centerOfMass: [ 0.4, 0, 0.3 ] 17 | mass: 10.0 18 | inertia: [ 19 | 1.0, 0, 0, 20 | 0, 1.0, 0, 21 | 0, 0, 1.0 ] 22 | elements: 23 | - 24 | type: Shape 25 | translation: [ 0.425, 0, 0.325 ] 26 | geometry: { type: Box, size: [ 0.85, 0.8, 0.65 ] } 27 | appearance: { material: { diffuseColor: [ 0.5, 0.2, 0.3 ] }} 28 | - 29 | type: Shape 30 | translation: [ -0.35, 0, 1.0 ] 31 | rotation: [0, 0, 1, 0] 32 | geometry: { type: Cylinder, radius: 0.015, height: 0.8 } 33 | appearance: { material: { diffuseColor: [ 0.5, 0.2, 0.3 ] }} 34 | - 35 | type: Shape 36 | translation: [ -0.175, 0.38, 0.825 ] 37 | rotation: [0.707107, 0, 0.707107, 90] 38 | geometry: { type: Cylinder, radius: 0.015, height: 0.52 } 39 | appearance: { material: { diffuseColor: [ 0.5, 0.2, 0.3 ] }} 40 | - 41 | type: Shape 42 | translation: [ -0.175, -0.38, 0.825 ] 43 | rotation: [0.707107, 0, 0.707107, 90] 44 | geometry: { type: Cylinder, radius: 0.015, height: 0.52 } 45 | appearance: { material: { diffuseColor: [ 0.5, 0.2, 0.3 ] }} 46 | -------------------------------------------------------------------------------- /cnoid/model/FloorCheckeredPattern.body: -------------------------------------------------------------------------------- 1 | format: ChoreonoidBody 2 | formatVersion: 1.0 3 | name: FloorCheckeredPattern 4 | links: 5 | - 6 | name: Floor 7 | translation: [ 0, 0, -0.1 ] 8 | jointType: fixed 9 | material: Floor 10 | AMOR: true 11 | elements: 12 | Shape: 13 | geometry: { type: Box, size: [ 100.0, 100.0, 0.2 ] } 14 | appearance: 15 | texture: 16 | url: "texture/CheckeredPattern.png" 17 | repeatS: true 18 | repeatT: true 19 | textureTransform: 20 | scale: [ 100.0, 100.0 ] 21 | -------------------------------------------------------------------------------- /cnoid/model/LMC_materials.yaml: -------------------------------------------------------------------------------- 1 | materials: 2 | - 3 | name: Default 4 | roughness: 1.0 5 | - 6 | name: Floor 7 | roughness: 1.0 8 | - 9 | name: Cart 10 | roughness: 1.0 11 | - 12 | name: CartHighFriction 13 | roughness: 1.0 14 | 15 | contactMaterials: 16 | - 17 | materials: [ Default, Default ] 18 | friction: 0.5 19 | - 20 | materials: [ Default, Floor ] 21 | friction: 0.5 22 | - 23 | materials: [ Cart, Floor ] 24 | friction: 0.1 25 | - 26 | materials: [ CartHighFriction, Floor ] 27 | friction: 0.5 28 | -------------------------------------------------------------------------------- /cnoid/model/texture/CheckeredPattern.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/LocomanipController/ee25576904f3e225c6004afdd836914fc75c2ef8/cnoid/model/texture/CheckeredPattern.png -------------------------------------------------------------------------------- /cnoid/project/.gitignore: -------------------------------------------------------------------------------- 1 | rtc.conf.choreonoid 2 | robot.conf 3 | RobotHardware.conf 4 | PDcontroller.conf.choreonoid 5 | PDgains_sim.dat 6 | 7 | *.cnoid 8 | !*.in.cnoid 9 | -------------------------------------------------------------------------------- /cnoid/project/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(LMC_CNOID_CONFIG_FILE_LIST 2 | rtc.conf.choreonoid 3 | robot.conf 4 | RobotHardware.conf 5 | PDcontroller.conf.choreonoid 6 | PDgains_sim.dat 7 | ) 8 | if(DEFINED CATKIN_DEVEL_PREFIX) 9 | foreach(CNOID_CONFIG_FILE IN LISTS LMC_CNOID_CONFIG_FILE_LIST) 10 | execute_process(COMMAND cmake -E create_symlink 11 | "${HRPSYSBASE_PREFIX}/share/hrpsys/samples/JVRC1/${CNOID_CONFIG_FILE}" 12 | "${CMAKE_CURRENT_SOURCE_DIR}/${CNOID_CONFIG_FILE}" 13 | ) 14 | endforeach() 15 | endif() 16 | 17 | file(GLOB LMC_PROJECT_FILE_LIST ${CMAKE_CURRENT_SOURCE_DIR}/*.in.cnoid) 18 | 19 | if(DEFINED CATKIN_DEVEL_PREFIX) 20 | set(LMC_MODEL_DIR ${PROJECT_SOURCE_DIR}/cnoid/model) 21 | set(CNOID_PROJECT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) 22 | foreach(PROJECT_FILE_IN IN LISTS LMC_PROJECT_FILE_LIST) 23 | get_filename_component(PROJECT_FILE_IN ${PROJECT_FILE_IN} NAME) 24 | string(REPLACE ".in." "." PROJECT_FILE_OUT "${PROJECT_FILE_IN}") 25 | configure_file(${PROJECT_FILE_IN} 26 | ${CNOID_PROJECT_DIR}/${PROJECT_FILE_OUT} 27 | @ONLY) 28 | endforeach() 29 | endif() 30 | 31 | set(CNOID_PROJECT_DIR ${CMAKE_CURRENT_BINARY_DIR}) 32 | set(LMC_MODEL_DIR ${CNOID_SHARE_SUBDIR}/model/environment/LmcCnoid) 33 | foreach(PROJECT_FILE_IN IN LISTS LMC_PROJECT_FILE_LIST) 34 | get_filename_component(PROJECT_FILE_IN ${PROJECT_FILE_IN} NAME) 35 | string(REPLACE ".in." "." PROJECT_FILE_OUT "${PROJECT_FILE_IN}") 36 | configure_file(${PROJECT_FILE_IN} 37 | ${CNOID_PROJECT_DIR}/${PROJECT_FILE_OUT} 38 | @ONLY) 39 | install(FILES ${CNOID_PROJECT_DIR}/${PROJECT_FILE_OUT} DESTINATION ${HRPSYSBASE_PREFIX}/share/hrpsys/samples/JVRC1/) 40 | endforeach() 41 | -------------------------------------------------------------------------------- /cnoid/project/LMC_JVRC1_Cart.in.cnoid: -------------------------------------------------------------------------------- 1 | items: 2 | id: 0 3 | name: "Root" 4 | plugin: Base 5 | class: RootItem 6 | children: 7 | - 8 | id: 1 9 | name: "ModelLoader" 10 | plugin: Base 11 | class: ExtCommandItem 12 | data: 13 | command: openhrp-model-loader 14 | executeOnLoading: true 15 | waitingTimeAfterStarted: 0.1 16 | - 17 | id: 2 18 | name: "World" 19 | plugin: Body 20 | class: WorldItem 21 | data: 22 | collisionDetection: false 23 | collisionDetector: AISTCollisionDetector 24 | materialTableFile: "@LMC_MODEL_DIR@/LMC_materials.yaml" 25 | children: 26 | - 27 | id: 3 28 | name: "JVRC1" 29 | plugin: Body 30 | class: BodyItem 31 | data: 32 | modelFile: "@OPENHRP3_1_PREFIX@/share/OpenHRP-3.1/robot/JVRC1/main.wrl" 33 | currentBaseLink: "PELVIS" 34 | rootPosition: [ -0.0164712421, -0.00366884734, 0.823007651 ] 35 | rootAttitude: [ 36 | 0.999520541, 0.000990316288, -0.030946845, 37 | -0.00104419639, 0.999997967, -0.00172494057, 38 | 0.0309450738, 0.00175642811, 0.999519543 ] 39 | jointDisplacements: [ 40 | -21.761510, -0.462606, -0.048014, 42.554320, 0.121295, -19.017386, -21.649153, 1.310354, -0.046925, 42.222635, 41 | -1.255809, -18.793188, 0.015642, 7.603837, 0.166100, -0.000401, 0.009855, 0.010886, -2.647695, -9.070266, 42 | 0.126165, -29.262043, 0.004927, 0.075287, 0.012834, 0.126624, 0.177674, -0.126108, -0.160657, -0.126108, 43 | -0.160657, -2.660128, 9.131171, -0.108346, -29.278659, -0.004870, -0.065776, -0.012548, -0.127025, -0.173262, 44 | 0.118258, 0.155100, 0.118258, 0.155100 ] 45 | jointPositions: [ 46 | -0.38, -0.01, 0, 0.72, -0.01, -0.33, 47 | -0.38, 0.02, 0, 0.72, -0.02, -0.33, 48 | 0, 0.13, 0, 0, 0, 0, 49 | -0.052, -0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 50 | -0.052, 0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0] 51 | initialRootPosition: [ -0.0164712421, -0.00366884734, 0.823007651 ] 52 | initialRootAttitude: [ 53 | 0.999520541, 0.000990316287, -0.030946845, 54 | -0.00104419639, 0.999997967, -0.00172494057, 55 | 0.0309450738, 0.00175642811, 0.999519543 ] 56 | initialJointPositions: [ 57 | -0.38, -0.01, 0, 0.72, -0.01, -0.33, 58 | -0.38, 0.02, 0, 0.72, -0.02, -0.33, 59 | 0, 0.13, 0, 0, 0, 0, 60 | -0.052, -0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 61 | -0.052, 0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0] 62 | zmp: [ 0, 0, 0 ] 63 | collisionDetection: true 64 | selfCollisionDetection: false 65 | isEditable: true 66 | children: 67 | - 68 | id: 4 69 | name: "BodyRTC" 70 | plugin: OpenRTM 71 | class: BodyRTCItem 72 | data: 73 | isNoDelayMode: true 74 | controllerOptions: "" 75 | moduleName: "PDcontroller" 76 | confFileName: "Virtual-JVRC1-RTC.conf" 77 | configurationMode: "Use Configuration File" 78 | autoConnect: true 79 | instanceName: "JVRC1Controller(Robot)0" 80 | bodyPeriodicRate: 0.005 81 | baseDirectory: "RTC directory" 82 | - 83 | id: 5 84 | name: "SensorVisualizer" 85 | plugin: Body 86 | class: SensorVisualizer 87 | data: 88 | subItems: 89 | - 90 | class: ForceSensorVisualizer 91 | name: ForceSensor 92 | visualRatio: 0.002 93 | - 94 | class: PointCloudVisualizer 95 | name: dcamera 96 | translation: [ 0, 0, 0 ] 97 | rotation: [ 1, 0, 0, 0 ] 98 | renderingMode: Point 99 | pointSize: 0 100 | voxelSize: 0.01 101 | isEditable: false 102 | - 103 | class: RangeSensorVisualizer 104 | name: ranger 105 | translation: [ 0, 0, 0 ] 106 | rotation: [ 1, 0, 0, 0 ] 107 | renderingMode: Point 108 | pointSize: 0 109 | voxelSize: 0.01 110 | isEditable: false 111 | - 112 | class: CameraImageVisualizer 113 | name: rcamera 114 | - 115 | class: CameraImageVisualizer 116 | name: lcamera 117 | - 118 | class: CameraImageVisualizer 119 | name: dcamera_Image 120 | - 121 | id: 6 122 | name: "FloorCheckeredPattern" 123 | plugin: Body 124 | class: BodyItem 125 | data: 126 | modelFile: "@LMC_MODEL_DIR@/FloorCheckeredPattern.body" 127 | currentBaseLink: "" 128 | rootPosition: [ 0, 0, -0.1 ] 129 | rootAttitude: [ 130 | 1, 0, 0, 131 | 0, 1, 0, 132 | 0, 0, 1 ] 133 | jointPositions: [ ] 134 | initialRootPosition: [ 0, 0, -0.1 ] 135 | initialRootAttitude: [ 136 | 1, 0, 0, 137 | 0, 1, 0, 138 | 0, 0, 1 ] 139 | zmp: [ 0, 0, 0 ] 140 | collisionDetection: true 141 | selfCollisionDetection: false 142 | isEditable: true 143 | - 144 | id: 7 145 | name: "Cart" 146 | plugin: Body 147 | class: BodyItem 148 | data: 149 | modelFile: "@LMC_MODEL_DIR@/Cart.body" 150 | currentBaseLink: "Root" 151 | rootPosition: [ 1.0, 0, 0 ] 152 | rootAttitude: [ 153 | 1, 0, 0, 154 | 0, 1, 0, 155 | 0, 0, 1 ] 156 | jointPositions: [ ] 157 | initialRootPosition: [ 1.0, 0, 0 ] 158 | initialRootAttitude: [ 159 | 1, 0, 0, 160 | 0, 1, 0, 161 | 0, 0, 1 ] 162 | zmp: [ 0, 0, 0 ] 163 | collisionDetection: true 164 | selfCollisionDetection: false 165 | isEditable: true 166 | children: 167 | - 168 | id: 8 169 | name: "CnoidRosUtils::PosePublisher" 170 | plugin: CnoidRosUtils::Plugin 171 | class: CnoidRosUtils::PosePublisherItem 172 | data: 173 | Link name: "" 174 | Frame id: "" 175 | Pose topic name (only for topic output): "/cnoid/object/pose" 176 | Velocity topic name (only for topic output): "/cnoid/object/vel" 177 | TF frame id (only for TF output): "" 178 | Publish rate: 30 179 | Output TF: false 180 | - 181 | id: 9 182 | name: "AISTSimulator" 183 | plugin: Body 184 | class: AISTSimulatorItem 185 | data: 186 | realtimeSync: true 187 | recording: "full" 188 | timeRangeMode: "Unlimited" 189 | timeLength: 500 190 | allLinkPositionOutputMode: true 191 | deviceStateOutput: true 192 | controllerThreads: true 193 | recordCollisionData: false 194 | controllerOptions: "" 195 | dynamicsMode: "Forward dynamics" 196 | integrationMode: "Runge Kutta" 197 | gravity: [ 0, 0, -9.80665 ] 198 | staticFriction: 0.5 199 | dynamicFriction: 0.5 200 | cullingThresh: 0.01 201 | contactCullingDepth: 0.05 202 | errorCriterion: 0.001 203 | maxNumIterations: 100 204 | contactCorrectionDepth: 0.0002 205 | contactCorrectionVelocityRatio: 1 206 | kinematicWalking: false 207 | 2Dmode: false 208 | oldAccelSensorMode: false 209 | - 210 | id: 10 211 | name: "sim_mc.py" 212 | plugin: PythonSimScript 213 | class: PythonSimScriptItem 214 | data: 215 | timing: After init. 216 | delay: 0 217 | simulationOnly: true 218 | backgroundExecution: true 219 | file: "@HRPSYSBASE_PREFIX@/share/hrpsys/samples/JVRC1/sim_mc.py" 220 | views: 221 | - 222 | id: 0 223 | plugin: Base 224 | class: ItemPropertyView 225 | mounted: true 226 | - 227 | id: 1 228 | plugin: Base 229 | class: ItemTreeView 230 | mounted: true 231 | state: 232 | selected: [ 3 ] 233 | checked: [ 3, [ 5, "ForceSensor" ], 6, 7, 10 ] 234 | expanded: [ 2, 3, 4, 7, 9 ] 235 | - 236 | id: 2 237 | plugin: Base 238 | class: MessageView 239 | mounted: true 240 | - 241 | id: 3 242 | name: "Multi SE3 Seq" 243 | plugin: Base 244 | class: MultiSE3SeqGraphView 245 | state: 246 | mode: view 247 | editMode: freeLine 248 | original: true 249 | velocity: false 250 | acceleration: false 251 | limits: true 252 | grid: true 253 | gridWidth: 0.2 254 | gridHeight: 0.2 255 | lineWidth: 1 256 | rulers: false 257 | sync: true 258 | controlPointStep: 1 259 | controlPointOffset: 0 260 | controlPointHeighlight: false 261 | scrollMode: continuous 262 | lower: -10 263 | upper: 10 264 | visibleElements: [ 0, 1, 2 ] 265 | - 266 | id: 4 267 | name: "Multi Value Seq" 268 | plugin: Base 269 | class: MultiValueSeqGraphView 270 | state: 271 | mode: view 272 | editMode: freeLine 273 | original: true 274 | velocity: false 275 | acceleration: false 276 | limits: true 277 | grid: true 278 | gridWidth: 0.2 279 | gridHeight: 0.2 280 | lineWidth: 1 281 | rulers: false 282 | sync: true 283 | controlPointStep: 1 284 | controlPointOffset: 0 285 | controlPointHeighlight: false 286 | scrollMode: continuous 287 | lower: -10 288 | upper: 10 289 | - 290 | id: 5 291 | plugin: Base 292 | class: SceneView 293 | mounted: true 294 | state: 295 | editMode: false 296 | viewpointControlMode: thirdPerson 297 | collisionLines: true 298 | polygonMode: fill 299 | restrictCameraRoll: true 300 | verticalAxis: Z 301 | lightingMode: full 302 | cullingMode: enabled 303 | defaultHeadLight: true 304 | defaultHeadLightIntensity: 0.75 305 | headLightLightingFromBack: false 306 | worldLight: true 307 | worldLightIntensity: 0.5 308 | worldLightAmbient: 0.3 309 | additionalLights: true 310 | fog: true 311 | floorGrid: false 312 | floorGridSpan: 10 313 | floorGridInterval: 0.5 314 | xzGridSpan: 10 315 | xzGridInterval: 0.5 316 | xzGrid: false 317 | yzGridSpan: 10 318 | yzGridInterval: 0.5 319 | texture: true 320 | lineWidth: 1 321 | pointSize: 1 322 | normalVisualization: false 323 | normalLength: 0.01 324 | lightweightViewChange: false 325 | coordinateAxes: true 326 | fpsTestIteration: 1 327 | upsideDown: false 328 | cameras: 329 | - 330 | camera: [ System, Perspective ] 331 | isCurrent: true 332 | fieldOfView: 0.6978 333 | near: 0.01 334 | far: 10000 335 | eye: [ 1.43584974, -4.44576726, 2.81673074 ] 336 | direction: [ -0.00962550675, 0.891516332, -0.452886276 ] 337 | up: [ -0.00488942904, 0.452859881, 0.891568293 ] 338 | - 339 | camera: [ System, Orthographic ] 340 | orthoHeight: 20 341 | near: 0.01 342 | far: 10000 343 | backgroundColor: [ 1, 1, 1 ] 344 | gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] 345 | xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] 346 | yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] 347 | dedicatedItemTreeViewChecks: false 348 | - 349 | id: 6 350 | name: "Task" 351 | plugin: Base 352 | class: TaskView 353 | state: 354 | layoutMode: horizontal 355 | isAutoMode: false 356 | - 357 | id: 7 358 | name: "Text Editor" 359 | plugin: Base 360 | class: TextEditView 361 | - 362 | id: 8 363 | plugin: Body 364 | class: BodyLinkView 365 | state: 366 | showRotationMatrix: false 367 | - 368 | id: 9 369 | name: "Body State" 370 | plugin: Body 371 | class: BodyStateView 372 | mounted: true 373 | - 374 | id: 10 375 | plugin: Body 376 | class: JointDisplacementView 377 | state: 378 | showSelectedJoints: false 379 | showJointIDs: true 380 | showJointNames: true 381 | overlapJointNames: false 382 | showSliders: true 383 | showDials: false 384 | showPhases: false 385 | currentBodyItem: 3 386 | - 387 | id: 11 388 | name: "Joint Trajectories" 389 | plugin: Body 390 | class: JointGraphView 391 | state: 392 | mode: view 393 | editMode: freeLine 394 | original: true 395 | velocity: false 396 | acceleration: false 397 | limits: true 398 | grid: true 399 | gridWidth: 0.2 400 | gridHeight: 0.2 401 | lineWidth: 1 402 | rulers: false 403 | sync: true 404 | controlPointStep: 1 405 | controlPointOffset: 0 406 | controlPointHeighlight: false 407 | scrollMode: continuous 408 | lower: -10 409 | upper: 10 410 | - 411 | id: 12 412 | name: "Joint State" 413 | plugin: Body 414 | class: JointStateView 415 | mounted: true 416 | - 417 | id: 13 418 | plugin: Body 419 | class: LinkSelectionView 420 | state: 421 | listingMode: "Link List" 422 | currentBodyItem: 3 423 | bodyItems: 424 | - 425 | id: 3 426 | selectedLinks: [ 0 ] 427 | - 428 | id: 7 429 | selectedLinks: [ 1 ] 430 | - 431 | id: 14 432 | name: "Nameserver" 433 | plugin: Corba 434 | class: NameServerView 435 | - 436 | id: 15 437 | plugin: OpenRTM 438 | class: RTSNameServerView 439 | state: 440 | NameServers: 441 | - 442 | isDefaultNameServer: false 443 | host: "localhost" 444 | port: 2809 445 | - 446 | id: 16 447 | plugin: Python 448 | class: PythonConsoleView 449 | mounted: true 450 | toolbars: 451 | "BodyMotionGenerationBar": 452 | autoGenerationForNewBody: true 453 | balancer: false 454 | autoGeneration: false 455 | timeScaleRatio: 1 456 | preInitialDuration: 1 457 | postFinalDuration: 1 458 | onlyTimeBarRange: false 459 | makeNewBodyItem: true 460 | stealthyStepMode: true 461 | stealthyHeightRatioThresh: 2 462 | flatLiftingHeight: 0.005 463 | flatLandingHeight: 0.005 464 | impactReductionHeight: 0.005 465 | impactReductionTime: 0.04 466 | autoZmp: true 467 | minZmpTransitionTime: 0.1 468 | zmpCenteringTimeThresh: 0.03 469 | zmpTimeMarginBeforeLiftingSpin: 0 470 | zmpMaxDistanceFromCenter: 0.02 471 | allLinkPositions: false 472 | lipSyncMix: false 473 | "TimeBar": 474 | minTime: 0 475 | maxTime: 60 476 | frameRate: 1000 477 | playbackFrameRate: 50 478 | idleLoopDrivenMode: false 479 | currentTime: 0 480 | speedScale: 1 481 | syncToOngoingUpdates: false 482 | autoExpansion: true 483 | "BodyBar": 484 | current: 3 485 | "LeggedBodyBar": 486 | stanceWidth: 0.15 487 | "KinematicsBar": 488 | mode: IK 489 | enablePositionDragger: true 490 | penetrationBlock: true 491 | collisionLinkHighlight: false 492 | snapDistance: 0.025 493 | penetrationBlockDepth: 0.0005 494 | lazyCollisionDetectionMode: true 495 | Body: 496 | "BodyMotionEngine": 497 | updateJointVelocities: false 498 | "BodySelectionManager": 499 | currentBodyItem: 3 500 | currentLink: "PELVIS" 501 | "EditableSceneBody": 502 | editableSceneBodies: 503 | - 504 | bodyItem: 3 505 | showCenterOfMass: false 506 | showPpcom: false 507 | showZmp: false 508 | - 509 | bodyItem: 6 510 | showCenterOfMass: false 511 | showPpcom: false 512 | showZmp: false 513 | - 514 | bodyItem: 7 515 | showCenterOfMass: false 516 | showPpcom: false 517 | showZmp: false 518 | staticModelEditing: false 519 | "KinematicFaultChecker": 520 | checkJointPositions: true 521 | angleMargin: 0 522 | translationMargin: 0 523 | checkJointVelocities: true 524 | velocityLimitRatio: 100 525 | targetJoints: all 526 | checkSelfCollisions: true 527 | onlyTimeBarRange: false 528 | "SplineFilterDialog": 529 | isInputFrameRateSpecified: false 530 | inputFrameRate: 200 531 | isOutputFrameRateSpecified: false 532 | outputFrameRate: 200 533 | OpenRTM: 534 | "deleteUnmanagedRTCsOnStartingSimulation": true 535 | Python: 536 | "moduleSearchPath": 537 | - @HRPSYSBASE_PREFIX@/lib/python2.7/dist-packages/hrpsys 538 | viewAreas: 539 | - 540 | type: embedded 541 | tabs: true 542 | contents: 543 | type: splitter 544 | orientation: vertical 545 | sizes: [ 1094, 252 ] 546 | children: 547 | - 548 | type: splitter 549 | orientation: horizontal 550 | sizes: [ 489, 1959 ] 551 | children: 552 | - 553 | type: splitter 554 | orientation: vertical 555 | sizes: [ 499, 589 ] 556 | children: 557 | - 558 | type: pane 559 | views: [ 1, 12 ] 560 | current: 1 561 | - 562 | type: pane 563 | views: [ 0 ] 564 | current: 0 565 | - 566 | type: pane 567 | views: [ 5 ] 568 | current: 5 569 | - 570 | type: pane 571 | views: [ 2, 16, 9 ] 572 | current: 2 573 | layoutOfToolBars: 574 | rows: 575 | - 576 | - { name: "FileBar", x: 0, priority: 2 } 577 | - { name: "SimulationBar", x: 0, priority: 1 } 578 | - { name: "TimeBar", x: 47, priority: 0 } 579 | - { name: "SceneBar", x: 1640, priority: 3 } 580 | -------------------------------------------------------------------------------- /cnoid/project/LMC_JVRC1_CartHighFriction.in.cnoid: -------------------------------------------------------------------------------- 1 | items: 2 | id: 0 3 | name: "Root" 4 | plugin: Base 5 | class: RootItem 6 | children: 7 | - 8 | id: 1 9 | name: "ModelLoader" 10 | plugin: Base 11 | class: ExtCommandItem 12 | data: 13 | command: openhrp-model-loader 14 | executeOnLoading: true 15 | waitingTimeAfterStarted: 0.1 16 | - 17 | id: 2 18 | name: "World" 19 | plugin: Body 20 | class: WorldItem 21 | data: 22 | collisionDetection: false 23 | collisionDetector: AISTCollisionDetector 24 | materialTableFile: "@LMC_MODEL_DIR@/LMC_materials.yaml" 25 | children: 26 | - 27 | id: 3 28 | name: "JVRC1" 29 | plugin: Body 30 | class: BodyItem 31 | data: 32 | modelFile: "@OPENHRP3_1_PREFIX@/share/OpenHRP-3.1/robot/JVRC1/main.wrl" 33 | currentBaseLink: "PELVIS" 34 | rootPosition: [ -0.0164712421, -0.00366884734, 0.823007651 ] 35 | rootAttitude: [ 36 | 0.999520541, 0.000990316288, -0.030946845, 37 | -0.00104419639, 0.999997967, -0.00172494057, 38 | 0.0309450738, 0.00175642811, 0.999519543 ] 39 | jointDisplacements: [ 40 | -21.761510, -0.462606, -0.048014, 42.554320, 0.121295, -19.017386, -21.649153, 1.310354, -0.046925, 42.222635, 41 | -1.255809, -18.793188, 0.015642, 7.603837, 0.166100, -0.000401, 0.009855, 0.010886, -2.647695, -9.070266, 42 | 0.126165, -29.262043, 0.004927, 0.075287, 0.012834, 0.126624, 0.177674, -0.126108, -0.160657, -0.126108, 43 | -0.160657, -2.660128, 9.131171, -0.108346, -29.278659, -0.004870, -0.065776, -0.012548, -0.127025, -0.173262, 44 | 0.118258, 0.155100, 0.118258, 0.155100 ] 45 | jointPositions: [ 46 | -0.38, -0.01, 0, 0.72, -0.01, -0.33, 47 | -0.38, 0.02, 0, 0.72, -0.02, -0.33, 48 | 0, 0.13, 0, 0, 0, 0, 49 | -0.052, -0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 50 | -0.052, 0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0] 51 | initialRootPosition: [ -0.0164712421, -0.00366884734, 0.823007651 ] 52 | initialRootAttitude: [ 53 | 0.999520541, 0.000990316287, -0.030946845, 54 | -0.00104419639, 0.999997967, -0.00172494057, 55 | 0.0309450738, 0.00175642811, 0.999519543 ] 56 | initialJointPositions: [ 57 | -0.38, -0.01, 0, 0.72, -0.01, -0.33, 58 | -0.38, 0.02, 0, 0.72, -0.02, -0.33, 59 | 0, 0.13, 0, 0, 0, 0, 60 | -0.052, -0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 61 | -0.052, 0.17, 0, -0.52, 0, 0, 0, 0, 0, 0, 0, 0, 0] 62 | zmp: [ 0, 0, 0 ] 63 | collisionDetection: true 64 | selfCollisionDetection: false 65 | isEditable: true 66 | children: 67 | - 68 | id: 4 69 | name: "BodyRTC" 70 | plugin: OpenRTM 71 | class: BodyRTCItem 72 | data: 73 | isNoDelayMode: true 74 | controllerOptions: "" 75 | moduleName: "PDcontroller" 76 | confFileName: "Virtual-JVRC1-RTC.conf" 77 | configurationMode: "Use Configuration File" 78 | autoConnect: true 79 | instanceName: "JVRC1Controller(Robot)0" 80 | bodyPeriodicRate: 0.005 81 | baseDirectory: "RTC directory" 82 | - 83 | id: 5 84 | name: "SensorVisualizer" 85 | plugin: Body 86 | class: SensorVisualizer 87 | data: 88 | subItems: 89 | - 90 | class: ForceSensorVisualizer 91 | name: ForceSensor 92 | visualRatio: 0.002 93 | - 94 | class: PointCloudVisualizer 95 | name: dcamera 96 | translation: [ 0, 0, 0 ] 97 | rotation: [ 1, 0, 0, 0 ] 98 | renderingMode: Point 99 | pointSize: 0 100 | voxelSize: 0.01 101 | isEditable: false 102 | - 103 | class: RangeSensorVisualizer 104 | name: ranger 105 | translation: [ 0, 0, 0 ] 106 | rotation: [ 1, 0, 0, 0 ] 107 | renderingMode: Point 108 | pointSize: 0 109 | voxelSize: 0.01 110 | isEditable: false 111 | - 112 | class: CameraImageVisualizer 113 | name: rcamera 114 | - 115 | class: CameraImageVisualizer 116 | name: lcamera 117 | - 118 | class: CameraImageVisualizer 119 | name: dcamera_Image 120 | - 121 | id: 6 122 | name: "FloorCheckeredPattern" 123 | plugin: Body 124 | class: BodyItem 125 | data: 126 | modelFile: "@LMC_MODEL_DIR@/FloorCheckeredPattern.body" 127 | currentBaseLink: "" 128 | rootPosition: [ 0, 0, -0.1 ] 129 | rootAttitude: [ 130 | 1, 0, 0, 131 | 0, 1, 0, 132 | 0, 0, 1 ] 133 | jointPositions: [ ] 134 | initialRootPosition: [ 0, 0, -0.1 ] 135 | initialRootAttitude: [ 136 | 1, 0, 0, 137 | 0, 1, 0, 138 | 0, 0, 1 ] 139 | zmp: [ 0, 0, 0 ] 140 | collisionDetection: true 141 | selfCollisionDetection: false 142 | isEditable: true 143 | - 144 | id: 7 145 | name: "CartHighFriction" 146 | plugin: Body 147 | class: BodyItem 148 | data: 149 | modelFile: "@LMC_MODEL_DIR@/CartHighFriction.body" 150 | currentBaseLink: "Root" 151 | rootPosition: [ 1.0, 0, 0 ] 152 | rootAttitude: [ 153 | 1, 0, 0, 154 | 0, 1, 0, 155 | 0, 0, 1 ] 156 | jointPositions: [ ] 157 | initialRootPosition: [ 1.0, 0, 0 ] 158 | initialRootAttitude: [ 159 | 1, 0, 0, 160 | 0, 1, 0, 161 | 0, 0, 1 ] 162 | zmp: [ 0, 0, 0 ] 163 | collisionDetection: true 164 | selfCollisionDetection: false 165 | isEditable: true 166 | children: 167 | - 168 | id: 8 169 | name: "CnoidRosUtils::PosePublisher" 170 | plugin: CnoidRosUtils::Plugin 171 | class: CnoidRosUtils::PosePublisherItem 172 | data: 173 | Link name: "" 174 | Frame id: "" 175 | Pose topic name (only for topic output): "/cnoid/object/pose" 176 | Velocity topic name (only for topic output): "/cnoid/object/vel" 177 | TF frame id (only for TF output): "" 178 | Publish rate: 30 179 | Output TF: false 180 | - 181 | id: 9 182 | name: "AISTSimulator" 183 | plugin: Body 184 | class: AISTSimulatorItem 185 | data: 186 | realtimeSync: true 187 | recording: "full" 188 | timeRangeMode: "Unlimited" 189 | timeLength: 500 190 | allLinkPositionOutputMode: true 191 | deviceStateOutput: true 192 | controllerThreads: true 193 | recordCollisionData: false 194 | controllerOptions: "" 195 | dynamicsMode: "Forward dynamics" 196 | integrationMode: "Runge Kutta" 197 | gravity: [ 0, 0, -9.80665 ] 198 | staticFriction: 0.5 199 | dynamicFriction: 0.5 200 | cullingThresh: 0.01 201 | contactCullingDepth: 0.05 202 | errorCriterion: 0.001 203 | maxNumIterations: 100 204 | contactCorrectionDepth: 0.0002 205 | contactCorrectionVelocityRatio: 1 206 | kinematicWalking: false 207 | 2Dmode: false 208 | oldAccelSensorMode: false 209 | - 210 | id: 10 211 | name: "sim_mc.py" 212 | plugin: PythonSimScript 213 | class: PythonSimScriptItem 214 | data: 215 | timing: After init. 216 | delay: 0 217 | simulationOnly: true 218 | backgroundExecution: true 219 | file: "@HRPSYSBASE_PREFIX@/share/hrpsys/samples/JVRC1/sim_mc.py" 220 | views: 221 | - 222 | id: 0 223 | plugin: Base 224 | class: ItemPropertyView 225 | mounted: true 226 | - 227 | id: 1 228 | plugin: Base 229 | class: ItemTreeView 230 | mounted: true 231 | state: 232 | selected: [ 3 ] 233 | checked: [ 3, [ 5, "ForceSensor" ], 6, 7, 10 ] 234 | expanded: [ 2, 3, 4, 7, 9 ] 235 | - 236 | id: 2 237 | plugin: Base 238 | class: MessageView 239 | mounted: true 240 | - 241 | id: 3 242 | name: "Multi SE3 Seq" 243 | plugin: Base 244 | class: MultiSE3SeqGraphView 245 | state: 246 | mode: view 247 | editMode: freeLine 248 | original: true 249 | velocity: false 250 | acceleration: false 251 | limits: true 252 | grid: true 253 | gridWidth: 0.2 254 | gridHeight: 0.2 255 | lineWidth: 1 256 | rulers: false 257 | sync: true 258 | controlPointStep: 1 259 | controlPointOffset: 0 260 | controlPointHeighlight: false 261 | scrollMode: continuous 262 | lower: -10 263 | upper: 10 264 | visibleElements: [ 0, 1, 2 ] 265 | - 266 | id: 4 267 | name: "Multi Value Seq" 268 | plugin: Base 269 | class: MultiValueSeqGraphView 270 | state: 271 | mode: view 272 | editMode: freeLine 273 | original: true 274 | velocity: false 275 | acceleration: false 276 | limits: true 277 | grid: true 278 | gridWidth: 0.2 279 | gridHeight: 0.2 280 | lineWidth: 1 281 | rulers: false 282 | sync: true 283 | controlPointStep: 1 284 | controlPointOffset: 0 285 | controlPointHeighlight: false 286 | scrollMode: continuous 287 | lower: -10 288 | upper: 10 289 | - 290 | id: 5 291 | plugin: Base 292 | class: SceneView 293 | mounted: true 294 | state: 295 | editMode: false 296 | viewpointControlMode: thirdPerson 297 | collisionLines: true 298 | polygonMode: fill 299 | restrictCameraRoll: true 300 | verticalAxis: Z 301 | lightingMode: full 302 | cullingMode: enabled 303 | defaultHeadLight: true 304 | defaultHeadLightIntensity: 0.75 305 | headLightLightingFromBack: false 306 | worldLight: true 307 | worldLightIntensity: 0.5 308 | worldLightAmbient: 0.3 309 | additionalLights: true 310 | fog: true 311 | floorGrid: false 312 | floorGridSpan: 10 313 | floorGridInterval: 0.5 314 | xzGridSpan: 10 315 | xzGridInterval: 0.5 316 | xzGrid: false 317 | yzGridSpan: 10 318 | yzGridInterval: 0.5 319 | texture: true 320 | lineWidth: 1 321 | pointSize: 1 322 | normalVisualization: false 323 | normalLength: 0.01 324 | lightweightViewChange: false 325 | coordinateAxes: true 326 | fpsTestIteration: 1 327 | upsideDown: false 328 | cameras: 329 | - 330 | camera: [ System, Perspective ] 331 | isCurrent: true 332 | fieldOfView: 0.6978 333 | near: 0.01 334 | far: 10000 335 | eye: [ 1.43584974, -4.44576726, 2.81673074 ] 336 | direction: [ -0.00962550675, 0.891516332, -0.452886276 ] 337 | up: [ -0.00488942904, 0.452859881, 0.891568293 ] 338 | - 339 | camera: [ System, Orthographic ] 340 | orthoHeight: 20 341 | near: 0.01 342 | far: 10000 343 | backgroundColor: [ 1, 1, 1 ] 344 | gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] 345 | xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] 346 | yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] 347 | dedicatedItemTreeViewChecks: false 348 | - 349 | id: 6 350 | name: "Task" 351 | plugin: Base 352 | class: TaskView 353 | state: 354 | layoutMode: horizontal 355 | isAutoMode: false 356 | - 357 | id: 7 358 | name: "Text Editor" 359 | plugin: Base 360 | class: TextEditView 361 | - 362 | id: 8 363 | plugin: Body 364 | class: BodyLinkView 365 | state: 366 | showRotationMatrix: false 367 | - 368 | id: 9 369 | name: "Body State" 370 | plugin: Body 371 | class: BodyStateView 372 | mounted: true 373 | - 374 | id: 10 375 | plugin: Body 376 | class: JointDisplacementView 377 | state: 378 | showSelectedJoints: false 379 | showJointIDs: true 380 | showJointNames: true 381 | overlapJointNames: false 382 | showSliders: true 383 | showDials: false 384 | showPhases: false 385 | currentBodyItem: 3 386 | - 387 | id: 11 388 | name: "Joint Trajectories" 389 | plugin: Body 390 | class: JointGraphView 391 | state: 392 | mode: view 393 | editMode: freeLine 394 | original: true 395 | velocity: false 396 | acceleration: false 397 | limits: true 398 | grid: true 399 | gridWidth: 0.2 400 | gridHeight: 0.2 401 | lineWidth: 1 402 | rulers: false 403 | sync: true 404 | controlPointStep: 1 405 | controlPointOffset: 0 406 | controlPointHeighlight: false 407 | scrollMode: continuous 408 | lower: -10 409 | upper: 10 410 | - 411 | id: 12 412 | name: "Joint State" 413 | plugin: Body 414 | class: JointStateView 415 | mounted: true 416 | - 417 | id: 13 418 | plugin: Body 419 | class: LinkSelectionView 420 | state: 421 | listingMode: "Link List" 422 | currentBodyItem: 3 423 | bodyItems: 424 | - 425 | id: 3 426 | selectedLinks: [ 0 ] 427 | - 428 | id: 7 429 | selectedLinks: [ 1 ] 430 | - 431 | id: 14 432 | name: "Nameserver" 433 | plugin: Corba 434 | class: NameServerView 435 | - 436 | id: 15 437 | plugin: OpenRTM 438 | class: RTSNameServerView 439 | state: 440 | NameServers: 441 | - 442 | isDefaultNameServer: false 443 | host: "localhost" 444 | port: 2809 445 | - 446 | id: 16 447 | plugin: Python 448 | class: PythonConsoleView 449 | mounted: true 450 | toolbars: 451 | "BodyMotionGenerationBar": 452 | autoGenerationForNewBody: true 453 | balancer: false 454 | autoGeneration: false 455 | timeScaleRatio: 1 456 | preInitialDuration: 1 457 | postFinalDuration: 1 458 | onlyTimeBarRange: false 459 | makeNewBodyItem: true 460 | stealthyStepMode: true 461 | stealthyHeightRatioThresh: 2 462 | flatLiftingHeight: 0.005 463 | flatLandingHeight: 0.005 464 | impactReductionHeight: 0.005 465 | impactReductionTime: 0.04 466 | autoZmp: true 467 | minZmpTransitionTime: 0.1 468 | zmpCenteringTimeThresh: 0.03 469 | zmpTimeMarginBeforeLiftingSpin: 0 470 | zmpMaxDistanceFromCenter: 0.02 471 | allLinkPositions: false 472 | lipSyncMix: false 473 | "TimeBar": 474 | minTime: 0 475 | maxTime: 60 476 | frameRate: 1000 477 | playbackFrameRate: 50 478 | idleLoopDrivenMode: false 479 | currentTime: 0 480 | speedScale: 1 481 | syncToOngoingUpdates: false 482 | autoExpansion: true 483 | "BodyBar": 484 | current: 3 485 | "LeggedBodyBar": 486 | stanceWidth: 0.15 487 | "KinematicsBar": 488 | mode: IK 489 | enablePositionDragger: true 490 | penetrationBlock: true 491 | collisionLinkHighlight: false 492 | snapDistance: 0.025 493 | penetrationBlockDepth: 0.0005 494 | lazyCollisionDetectionMode: true 495 | Body: 496 | "BodyMotionEngine": 497 | updateJointVelocities: false 498 | "BodySelectionManager": 499 | currentBodyItem: 3 500 | currentLink: "PELVIS" 501 | "EditableSceneBody": 502 | editableSceneBodies: 503 | - 504 | bodyItem: 3 505 | showCenterOfMass: false 506 | showPpcom: false 507 | showZmp: false 508 | - 509 | bodyItem: 6 510 | showCenterOfMass: false 511 | showPpcom: false 512 | showZmp: false 513 | - 514 | bodyItem: 7 515 | showCenterOfMass: false 516 | showPpcom: false 517 | showZmp: false 518 | staticModelEditing: false 519 | "KinematicFaultChecker": 520 | checkJointPositions: true 521 | angleMargin: 0 522 | translationMargin: 0 523 | checkJointVelocities: true 524 | velocityLimitRatio: 100 525 | targetJoints: all 526 | checkSelfCollisions: true 527 | onlyTimeBarRange: false 528 | "SplineFilterDialog": 529 | isInputFrameRateSpecified: false 530 | inputFrameRate: 200 531 | isOutputFrameRateSpecified: false 532 | outputFrameRate: 200 533 | OpenRTM: 534 | "deleteUnmanagedRTCsOnStartingSimulation": true 535 | Python: 536 | "moduleSearchPath": 537 | - @HRPSYSBASE_PREFIX@/lib/python2.7/dist-packages/hrpsys 538 | viewAreas: 539 | - 540 | type: embedded 541 | tabs: true 542 | contents: 543 | type: splitter 544 | orientation: vertical 545 | sizes: [ 1094, 252 ] 546 | children: 547 | - 548 | type: splitter 549 | orientation: horizontal 550 | sizes: [ 489, 1959 ] 551 | children: 552 | - 553 | type: splitter 554 | orientation: vertical 555 | sizes: [ 499, 589 ] 556 | children: 557 | - 558 | type: pane 559 | views: [ 1, 12 ] 560 | current: 1 561 | - 562 | type: pane 563 | views: [ 0 ] 564 | current: 0 565 | - 566 | type: pane 567 | views: [ 5 ] 568 | current: 5 569 | - 570 | type: pane 571 | views: [ 2, 16, 9 ] 572 | current: 2 573 | layoutOfToolBars: 574 | rows: 575 | - 576 | - { name: "FileBar", x: 0, priority: 2 } 577 | - { name: "SimulationBar", x: 0, priority: 1 } 578 | - { name: "TimeBar", x: 47, priority: 0 } 579 | - { name: "SceneBar", x: 1640, priority: 3 } 580 | -------------------------------------------------------------------------------- /depends.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: isri-aist/BaselineWalkingController 3 | uri: https://github.com/isri-aist/BaselineWalkingController 4 | - git: 5 | local-name: isri-aist/CentroidalControlCollection 6 | uri: https://github.com/isri-aist/CentroidalControlCollection 7 | - git: 8 | local-name: isri-aist/CnoidRosUtils 9 | uri: https://github.com/isri-aist/CnoidRosUtils 10 | - git: 11 | local-name: isri-aist/ForceControlCollection 12 | uri: https://github.com/isri-aist/ForceControlCollection 13 | - git: 14 | local-name: isri-aist/NMPC 15 | uri: https://github.com/isri-aist/NMPC 16 | - git: 17 | local-name: isri-aist/QpSolverCollection 18 | uri: https://github.com/isri-aist/QpSolverCollection 19 | - git: 20 | local-name: isri-aist/TrajectoryCollection 21 | uri: https://github.com/isri-aist/TrajectoryCollection 22 | -------------------------------------------------------------------------------- /description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copied from https://github.com/jrl-umi3218/mc_rtc/blob/b2fe81b4f418c8251d85d3ceee974c0ba7e0610a/src/mc_robots/CMakeLists.txt#L28-L45 2 | macro(append_aliases PATH VAR NAME) 3 | file(GLOB URDF_FILES ${PATH}/urdf/*.urdf) 4 | foreach(URDF ${URDF_FILES}) 5 | get_filename_component(URDF_NAME "${URDF}" NAME_WE) 6 | # Robot should be env (fixed base) or object (floating base) instead of ${NAME} because arbitrary name robot cannot be added 7 | list(APPEND ${VAR} "${NAME}/${URDF_NAME}: [object, \"${PATH}\", ${URDF_NAME}]") 8 | endforeach() 9 | string(REPLACE ";" "\n" ${VAR} "${${VAR}}") 10 | endmacro() 11 | 12 | append_aliases(${CMAKE_CURRENT_SOURCE_DIR} LMC_ALIASES LMC) 13 | 14 | set(LMC_ROBOT_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/aliases/lmc_aliases.yml") 15 | file(WRITE ${LMC_ROBOT_MODULE_PATH} "${LMC_ALIASES}") 16 | message("-- LMC_ROBOT_MODULE_PATH: ${LMC_ROBOT_MODULE_PATH}") 17 | 18 | install(FILES "${LMC_ROBOT_MODULE_PATH}" DESTINATION "${MC_ROBOTS_RUNTIME_INSTALL_PREFIX}/aliases/") 19 | -------------------------------------------------------------------------------- /description/aliases/.gitignore: -------------------------------------------------------------------------------- 1 | lmc_aliases.yml 2 | -------------------------------------------------------------------------------- /description/urdf/Cart.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Doxygen REQUIRED) 2 | 3 | if(DOXYGEN_FOUND) 4 | set(DOXYFILE_PATH ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 5 | 6 | add_custom_target(LocomanipController_doc ALL 7 | ${DOXYGEN_EXECUTABLE} ${DOXYFILE_PATH} 8 | DEPENDS ${DOXYFILE_PATH} 9 | COMMENT "Generating Doxygen documentation" 10 | ) 11 | endif() 12 | -------------------------------------------------------------------------------- /etc/LocomanipController.in.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | # If true, the FSM transitions are managed by an external tool 3 | Managed: false 4 | # If true and the FSM is self-managed, transitions should be triggered 5 | StepByStep: true 6 | # Change idle behaviour, if true the state is kept until transition, 7 | # otherwise the FSM holds the last state until transition 8 | IdleKeepState: true 9 | # Where to look for state libraries 10 | StatesLibraries: 11 | - "@MC_STATES_DEFAULT_INSTALL_PREFIX@" 12 | - "@BWC_STATES_LIBRARIES@" 13 | - "@STATES_INSTALL_PREFIX@/@PROJECT_NAME@/states" 14 | # Where to look for state files 15 | StatesFiles: 16 | - "@MC_STATES_DEFAULT_INSTALL_PREFIX@/data" 17 | - "@BWC_STATES_FILES@" 18 | - "@STATES_INSTALL_PREFIX@/@PROJECT_NAME@/states/data" 19 | # If true, state factory will be more verbose 20 | VerboseStateFactory: false 21 | # General constraints, always on 22 | constraints: 23 | - type: contact 24 | - type: kinematics 25 | damper: [0.1, 0.01, 0.5] 26 | - type: compoundJoint 27 | # Collision constraint 28 | collisions: 29 | - type: collision 30 | useMinimal: true 31 | # Initial set of contacts 32 | contacts: [] 33 | 34 | # Implement some additional text states 35 | states: 36 | LMC::Initial_: 37 | base: LMC::Initial 38 | configs: 39 | gripperCommands: 40 | - name: l_gripper 41 | opening: 1.0 42 | - name: r_gripper 43 | opening: 1.0 44 | 45 | LMC::GuiManip_: 46 | base: LMC::GuiManip 47 | 48 | LMC::Teleop_: 49 | base: LMC::Teleop 50 | configs: 51 | twistTopicName: /cmd_vel 52 | 53 | LMC::Main_: 54 | base: Parallel 55 | states: [LMC::GuiManip_, LMC::Teleop_] 56 | 57 | # Transitions map 58 | transitions: 59 | - [LMC::Initial_, OK, LMC::Main_, Auto] 60 | # Initial state 61 | init: LMC::Initial_ 62 | 63 | ObserverPipelines: 64 | name: MainObserverPipeline 65 | gui: true 66 | observers: 67 | - type: Encoder 68 | - type: Attitude 69 | config: 70 | KalmanFilter: 71 | gyr_cov: 1e-6 72 | - type: KinematicInertial 73 | config: 74 | anchorFrame: 75 | maxAnchorFrameDiscontinuity: 0.05 # [m] 76 | 77 | controllerName: LMC 78 | 79 | PostureTask: 80 | stiffness: 10 81 | 82 | CoMTask: 83 | type: com 84 | stiffness: [1000.0, 1000.0, 1000.0] 85 | weight: 1000.0 86 | 87 | BaseOrientationTask: 88 | type: orientation 89 | stiffness: 300.0 90 | weight: 500.0 91 | 92 | FootTaskList: 93 | - type: firstOrderImpedance 94 | foot: Left 95 | frame: LeftFootCenter 96 | cutoffPeriod: 0.01 97 | stiffness: 1000.0 98 | weight: 1000.0 99 | - type: firstOrderImpedance 100 | foot: Right 101 | frame: RightFootCenter 102 | cutoffPeriod: 0.01 103 | stiffness: 1000.0 104 | weight: 1000.0 105 | 106 | HandTaskList: 107 | - type: impedance 108 | hand: Left 109 | frame: LeftGripper 110 | stiffness: 1000.0 111 | weight: 1000.0 112 | - type: impedance 113 | hand: Right 114 | frame: RightGripper 115 | stiffness: 1000.0 116 | weight: 1000.0 117 | 118 | FootManager: 119 | name: FootManager 120 | footstepDuration: 0.8 # [sec] 121 | doubleSupportRatio: 0.125 # [] 122 | deltaTransLimit: [0.15, 0.1, 12.5] # (x [m], y [m], theta [deg]) 123 | midToFootTranss: 124 | Left: 125 | translation: [0, 0.105, 0] # [m] 126 | Right: 127 | translation: [0, -0.105, 0] # [m] 128 | zmpHorizon: 3.0 # [sec] 129 | zmpOffset: [0, -0.02, 0] # (positive for x-forward, y-outside, z-upward) [m] 130 | defaultSwingTrajType: IndHorizontalVertical 131 | overwriteLandingPose: false 132 | stopSwingTrajForTouchDownFoot: true 133 | keepPoseForTouchDownFoot: false 134 | enableWrenchDistForTouchDownFoot: true 135 | enableArmSwing: false 136 | fricCoeff: 0.5 137 | touchDownRemainingDuration: 0.2 # [sec] 138 | touchDownPosError: 0.02 # [m] 139 | touchDownForceZ: 100 # [N] 140 | impedanceGains: 141 | SingleSupport: 142 | damper: 143 | linear: [300, 300, 300] 144 | angular: [100, 100, 100] 145 | spring: 146 | linear: [2250, 2250, 2250] 147 | angular: [0, 0, 2000] 148 | wrench: 149 | linear: [0, 0, 0] 150 | angular: [1, 1, 0] 151 | DoubleSupport: 152 | damper: 153 | linear: [300, 300, 1e4] 154 | angular: [100, 100, 100] 155 | spring: 156 | linear: [2250, 2250, 0] 157 | angular: [0, 0, 2000] 158 | wrench: 159 | linear: [0, 0, 1] 160 | angular: [1, 1, 0] 161 | Swing: 162 | damper: 163 | linear: [300, 300, 300] 164 | angular: [40, 40, 40] 165 | spring: 166 | linear: [2250, 2250, 2250] 167 | angular: [400, 400, 400] 168 | wrench: 169 | linear: [0, 0, 0] 170 | angular: [0, 0, 0] 171 | VelMode: 172 | footstepQueueSize: 3 173 | enableOnlineFootstepUpdate: false 174 | SwingTraj: 175 | CubicSplineSimple: 176 | withdrawDurationRatio: 0.25 177 | withdrawOffset: [0, 0, 0.123] # [m] 178 | approachDurationRatio: 0.25 179 | approachOffset: [0, 0, 0.987] # [m] 180 | swingOffset: [0, 0, 0.05] # [m] 181 | IndHorizontalVertical: 182 | withdrawDurationRatio: 0.25 183 | approachDurationRatio: 0.25 184 | verticalTopDurationRatio: 0.5 185 | verticalTopOffset: [0, 0, 0.05] # [m] 186 | tiltAngleWithdraw: 0 # [deg] 187 | tiltAngleApproach: 0 # [deg] 188 | tiltAngleWithdrawDurationRatio: 0.25 189 | tiltAngleApproachDurationRatio: 0.25 190 | tiltCenterWithdrawDurationRatio: 0.25 191 | tiltCenterApproachDurationRatio: 0.25 192 | tiltDistThre: 0.1 # [m] 193 | tiltForwardAngleThre: 100 # [deg] 194 | 195 | ManipManager: 196 | name: ManipManager 197 | objPoseInterpolator: BangBang 198 | objHorizon: 3.0 # [sec] 199 | objPoseTopic: /object/pose 200 | objVelTopic: /object/vel 201 | handTaskStiffness: 1000.0 202 | handTaskStiffnessInterpDuration: 4.0 # [sec] 203 | preReachDuration: 2.0 # [ec] 204 | reachDuration: 1.5 # [sec] 205 | reachHandDistThre: 0.5 # [m] 206 | objToHandTranss: 207 | Left: 208 | translation: [-0.35, 0.3, 1.0] # [m] 209 | # e.Quaterniond(e.AngleAxisd(np.pi, e.Vector3d.UnitX()).toRotationMatrix() * e.AngleAxisd(np.pi/2, e.Vector3d.UnitZ()).toRotationMatrix() * e.AngleAxisd(np.deg2rad(75), e.Vector3d.UnitX()).toRotationMatrix()).inverse() 210 | rotation: [-0.430459, -0.560986, 0.560986, -0.430459] 211 | Right: 212 | translation: [-0.35, -0.3, 1.0] # [m] 213 | # e.Quaterniond(e.AngleAxisd(np.pi, e.Vector3d.UnitX()).toRotationMatrix() * e.AngleAxisd(-np.pi/2, e.Vector3d.UnitZ()).toRotationMatrix() * e.AngleAxisd(np.deg2rad(-75), e.Vector3d.UnitX()).toRotationMatrix()).inverse() 214 | rotation: [0.430459, -0.560986, -0.560986, -0.430459] 215 | preReachTranss: 216 | Left: 217 | translation: [0, 0, -0.15] # [m] 218 | Right: 219 | translation: [0, 0, -0.15] # [m] 220 | impedanceGain: 221 | mass: 222 | angular: [2.0, 2.0, 2.0] 223 | linear: [10.0, 10.0, 10.0] 224 | damper: 225 | angular: [200.0, 200.0, 200.0] 226 | linear: [1000.0, 1000.0, 1000.0] 227 | spring: 228 | angular: [200.0, 200.0, 200.0] 229 | linear: [1000.0, 1000.0, 1000.0] 230 | wrench: 231 | angular: [0.0, 0.0, 0.0] 232 | linear: [1.0, 1.0, 1.0] 233 | graspCommands: 234 | - name: l_gripper 235 | opening: 0.0 236 | - name: r_gripper 237 | opening: 0.0 238 | ungraspCommands: 239 | - name: l_gripper 240 | opening: 1.0 241 | - name: r_gripper 242 | opening: 1.0 243 | objToFootMidTrans: 244 | translation: [-0.65, 0, 0] # [m] 245 | footstepDuration: 1.0 # [sec] 246 | doubleSupportRatio: 0.2 # [sec] 247 | handForceArrowScale: 0.02 248 | VelMode: 249 | nonholonomicObjectMotion: true 250 | 251 | CentroidalManager: 252 | name: CentroidalManager 253 | useActualStateForMpc: false 254 | enableZmpFeedback: true 255 | enableComZFeedback: true 256 | dcmGainP: 2.0 # It must be greater than 1 to be stable 257 | zmpVelGain: 0.02 258 | comZGainP: 2000.0 259 | comZGainD: 500.0 260 | refComZ: 0.825 # [m] 261 | useTargetPoseForControlRobotAnchorFrame: true 262 | useActualComForWrenchDist: false 263 | wrenchDistConfig: 264 | wrenchWeight: 265 | linear: [1.0, 1.0, 1.0] 266 | angular: [1.0, 1.0, 1.0] 267 | regularWeight: 1e-8 268 | ridgeForceMinMax: [3, 1000] # [N] 269 | 270 | # PreviewControlExtZmp 271 | method: PreviewControlExtZmp 272 | horizonDuration: 2.0 # [sec] 273 | horizonDt: 0.005 # [sec] 274 | 275 | 276 | # OverwriteConfigKeys: [NoSensors] 277 | 278 | OverwriteConfigList: 279 | NoSensors: 280 | FootManager: 281 | impedanceGains: 282 | SingleSupport: 283 | damper: 284 | linear: [300, 300, 300] 285 | angular: [40, 40, 40] 286 | spring: 287 | linear: [2250, 2250, 2250] 288 | angular: [400, 400, 400] 289 | wrench: 290 | linear: [0, 0, 0] 291 | angular: [0, 0, 0] 292 | DoubleSupport: 293 | damper: 294 | linear: [300, 300, 300] 295 | angular: [40, 40, 40] 296 | spring: 297 | linear: [2250, 2250, 2250] 298 | angular: [400, 400, 400] 299 | wrench: 300 | linear: [0, 0, 0] 301 | angular: [0, 0, 0] 302 | Swing: 303 | damper: 304 | linear: [300, 300, 300] 305 | angular: [40, 40, 40] 306 | spring: 307 | linear: [2250, 2250, 2250] 308 | angular: [400, 400, 400] 309 | wrench: 310 | linear: [0, 0, 0] 311 | angular: [0, 0, 0] 312 | 313 | ManipManager: 314 | impedanceGain: 315 | wrench: 316 | angular: [0.0, 0.0, 0.0] 317 | linear: [0.0, 0.0, 0.0] 318 | 319 | CentroidalManager: 320 | useActualStateForMpc: false 321 | enableZmpFeedback: false 322 | useActualComForWrenchDist: false 323 | 324 | robots: 325 | # Environment models 326 | ground: 327 | module: env/ground 328 | 329 | obj: 330 | module: LMC/Cart 331 | init_pos: 332 | translation: [0.0, 0.0, 0.0] 333 | 334 | # Robot-specific configurations 335 | jvrc1: 336 | PostureTask: 337 | target: 338 | R_SHOULDER_P: [0.3490658503988659] # 20 339 | R_SHOULDER_R: [-0.4363323129985824] # -25 340 | R_ELBOW_P: [-1.5707963267948966] # -90 341 | L_SHOULDER_P: [0.3490658503988659] # 20 342 | L_SHOULDER_R: [0.4363323129985824] # 25 343 | L_ELBOW_P: [-1.5707963267948966] # -90 344 | jointWeights: 345 | WAIST_R: 100 346 | WAIST_P: 100 347 | WAIST_Y: 100 348 | 349 | CoMTask: 350 | activeJoints: [ 351 | "Root", 352 | "R_HIP_Y", "R_HIP_R", "R_HIP_P", "R_KNEE", "R_ANKLE_P", "R_ANKLE_R", 353 | "L_HIP_Y", "L_HIP_R", "L_HIP_P", "L_KNEE", "L_ANKLE_P", "L_ANKLE_R"] 354 | 355 | BaseOrientationTask: 356 | frame: WAIST_R_S 357 | -------------------------------------------------------------------------------- /etc/mc_rtc.in.yaml: -------------------------------------------------------------------------------- 1 | MainRobot: JVRC1 2 | 3 | Enabled: LocomanipController 4 | ControllerModulePaths: ["@CATKIN_DEVEL_PREFIX@/lib/mc_controller/"] 5 | RobotModulePaths: ["@PROJECT_SOURCE_DIR@/description"] 6 | 7 | Timestep: 0.005 8 | -------------------------------------------------------------------------------- /include/LocomanipController/CentroidalManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | class LocomanipController; 8 | 9 | /** \brief Centroidal manager. 10 | 11 | Centroidal manager calculates the centroidal targets from the specified reference ZMP trajectory and sensor 12 | measurements. 13 | */ 14 | class CentroidalManager : virtual public BWC::CentroidalManager 15 | { 16 | public: 17 | /** \brief Constructor. 18 | \param ctlPtr pointer to controller 19 | \param mcRtcConfig mc_rtc configuration 20 | */ 21 | CentroidalManager(LocomanipController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig = {}); 22 | 23 | protected: 24 | /** \brief Const accessor to the controller. */ 25 | const LocomanipController & ctl() const; 26 | 27 | /** \brief Accessor to the controller. */ 28 | LocomanipController & ctl(); 29 | }; 30 | } // namespace LMC 31 | -------------------------------------------------------------------------------- /include/LocomanipController/FootTypes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | using BWC::Foot; 8 | 9 | namespace Feet 10 | { 11 | using BWC::Feet::Both; 12 | } 13 | 14 | using BWC::strToFoot; 15 | 16 | using BWC::opposite; 17 | 18 | using BWC::sign; 19 | 20 | using BWC::SupportPhase; 21 | 22 | using BWC::Footstep; 23 | } // namespace LMC 24 | -------------------------------------------------------------------------------- /include/LocomanipController/HandTypes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | /** \brief Hand. */ 8 | enum class Hand 9 | { 10 | //! Left hand 11 | Left = 0, 12 | 13 | //! Right hand 14 | Right 15 | }; 16 | 17 | namespace Hands 18 | { 19 | //! Both hands 20 | const std::set Both = {Hand::Left, Hand::Right}; 21 | } // namespace Hands 22 | 23 | /** \brief Convert string to hand. */ 24 | Hand strToHand(const std::string & handStr); 25 | 26 | /** \brief Get the opposite hand. */ 27 | Hand opposite(const Hand & hand); 28 | 29 | /** \brief Get the sign of hand. 30 | 31 | Positive for left hand, negative for right hand. 32 | */ 33 | int sign(const Hand & hand); 34 | } // namespace LMC 35 | 36 | namespace std 37 | { 38 | /** \brief Convert hand to string. */ 39 | std::string to_string(const LMC::Hand & hand); 40 | } // namespace std 41 | -------------------------------------------------------------------------------- /include/LocomanipController/LocomanipController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace mc_tasks 8 | { 9 | namespace force 10 | { 11 | struct ImpedanceTask; 12 | } // namespace force 13 | } // namespace mc_tasks 14 | 15 | namespace LMC 16 | { 17 | class ManipManager; 18 | 19 | /** \brief Humanoid loco-manipulation controller. */ 20 | struct LocomanipController : public BWC::BaselineWalkingController 21 | { 22 | public: 23 | /** \brief Constructor. 24 | \param rm robot module 25 | \param dt control timestep 26 | \param _config controller configuration 27 | \param allowEmptyManager whether to allow the managers to be empty (assuming initialized in the parent class) 28 | */ 29 | LocomanipController(mc_rbdyn::RobotModulePtr rm, 30 | double dt, 31 | const mc_rtc::Configuration & config, 32 | bool allowEmptyManager = false); 33 | 34 | /** \brief Reset a controller. 35 | 36 | This method is called when starting the controller. 37 | */ 38 | void reset(const mc_control::ControllerResetData & resetData) override; 39 | 40 | /** \brief Run a controller. 41 | 42 | This method is called every control period. 43 | */ 44 | bool run() override; 45 | 46 | /** \brief Stop a controller. 47 | 48 | This method is called when stopping the controller. 49 | */ 50 | void stop() override; 51 | 52 | /** \brief Accessor to the control object. */ 53 | inline mc_rbdyn::Robot & obj() 54 | { 55 | return robot("obj"); 56 | } 57 | 58 | /** \brief Const accessor to the control object. */ 59 | inline const mc_rbdyn::Robot & obj() const 60 | { 61 | return robot("obj"); 62 | } 63 | 64 | /** \brief Accessor to the real object. */ 65 | inline mc_rbdyn::Robot & realObj() 66 | { 67 | return realRobot("obj"); 68 | } 69 | 70 | /** \brief Const accessor to the real object. */ 71 | inline const mc_rbdyn::Robot & realObj() const 72 | { 73 | return realRobot("obj"); 74 | } 75 | 76 | public: 77 | //! Hand tasks 78 | std::unordered_map> handTasks_; 79 | 80 | //! Manipulation manager 81 | std::shared_ptr manipManager_; 82 | }; 83 | } // namespace LMC 84 | -------------------------------------------------------------------------------- /include/LocomanipController/ManipManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | namespace LMC 23 | { 24 | class LocomanipController; 25 | 26 | namespace ManipPhase 27 | { 28 | class Base; 29 | } 30 | 31 | /** \brief Waypoint of object trajectory. */ 32 | struct Waypoint 33 | { 34 | /** \brief Constructor. 35 | \param _startTime start time [sec] 36 | \param _endTime end time [sec] 37 | \param _pose object pose 38 | \param _config additional configuration 39 | */ 40 | Waypoint(double _startTime, 41 | double _endTime, 42 | const sva::PTransformd & _pose, 43 | const mc_rtc::Configuration & _config = {}) 44 | : startTime(_startTime), endTime(_endTime), pose(_pose), config(_config){}; 45 | 46 | //! Start time [sec] 47 | double startTime; 48 | 49 | //! End time [sec] 50 | double endTime; 51 | 52 | //! Object pose 53 | sva::PTransformd pose; 54 | 55 | //! Additional configuration 56 | mc_rtc::Configuration config; 57 | }; 58 | 59 | /** \brief Manipulation manager. 60 | 61 | Manipulation manager sets object pose and hand poses. 62 | */ 63 | class ManipManager 64 | { 65 | friend class ManipPhase::Base; 66 | 67 | public: 68 | /** \brief Configuration. */ 69 | struct Configuration 70 | { 71 | //! Name 72 | std::string name = "ManipManager"; 73 | 74 | //! Type of object pose interpolator ("BangBang" or "Cubic") 75 | std::string objPoseInterpolator = "BangBang"; 76 | 77 | //! Horizon of object trajectory [sec] 78 | double objHorizon = 2.0; 79 | 80 | //! Object pose topic name (not subscribe if empty) 81 | std::string objPoseTopic; 82 | 83 | //! Object velocity topic name (not subscribe if empty) 84 | std::string objVelTopic; 85 | 86 | //! Stiffness of hand task 87 | double handTaskStiffness = 1000.0; 88 | 89 | //! Pre-reach duration [sec] 90 | double preReachDuration = 2.0; 91 | 92 | //! Reach duration [sec] 93 | double reachDuration = 1.0; 94 | 95 | //! Distance threshold of reaching hand [m] 96 | double reachHandDistThre = 0.5; 97 | 98 | //! Transformations from object to hand 99 | std::unordered_map objToHandTranss = { 100 | {Hand::Left, sva::PTransformd(sva::RotY(-1 * mc_rtc::constants::PI / 2), Eigen::Vector3d(0, 0.4, 0))}, 101 | {Hand::Right, sva::PTransformd(sva::RotY(-1 * mc_rtc::constants::PI / 2), Eigen::Vector3d(0, -0.4, 0))}}; 102 | 103 | //! Transformations for pre-reach 104 | std::unordered_map preReachTranss = { 105 | {Hand::Left, sva::PTransformd(Eigen::Vector3d(0, 0, 0.1))}, 106 | {Hand::Right, sva::PTransformd(Eigen::Vector3d(0, 0, 0.1))}}; 107 | 108 | //! Impedance gain for hand tasks 109 | mc_tasks::force::ImpedanceGains impGain = mc_tasks::force::ImpedanceGains::Default(); 110 | 111 | //! Grasp gripper commands 112 | std::vector graspCommands; 113 | 114 | //! Ungrasp gripper commands 115 | std::vector ungraspCommands; 116 | 117 | //! Transformation from object to foot midpose 118 | sva::PTransformd objToFootMidTrans = sva::PTransformd(Eigen::Vector3d(0, 0, -0.6)); 119 | 120 | //! Duration of one footstep. [sec] 121 | double footstepDuration = 1.6; 122 | 123 | //! Duration ratio of double support phase 124 | double doubleSupportRatio = 0.35; 125 | 126 | //! Scale of hand force arrow (zero for no visualization) 127 | double handForceArrowScale = 0.02; 128 | 129 | /** \brief Load mc_rtc configuration. 130 | \param mcRtcConfig mc_rtc configuration 131 | */ 132 | void load(const mc_rtc::Configuration & mcRtcConfig); 133 | }; 134 | 135 | /** \brief Velocity mode data. 136 | 137 | In the velocity mode, the object is moved at the specified velocity. 138 | */ 139 | class VelModeData 140 | { 141 | public: 142 | /** \brief Configuration. */ 143 | struct Configuration 144 | { 145 | //! Whether the object moves nonholonomic, like a wheel. 146 | bool nonholonomicObjectMotion = true; 147 | 148 | /** \brief Load mc_rtc configuration. 149 | \param mcRtcConfig mc_rtc configuration 150 | */ 151 | void load(const mc_rtc::Configuration & mcRtcConfig); 152 | }; 153 | 154 | public: 155 | /** \brief Constructor. */ 156 | VelModeData() {} 157 | 158 | /** \brief Reset. 159 | \param enabled whether the velocity mode is enabled 160 | \param currentObjPose current object pose 161 | */ 162 | void reset(bool enabled, const sva::PTransformd & currentObjPose); 163 | 164 | public: 165 | //! Configuration 166 | Configuration config_; 167 | 168 | //! Whether the velocity mode is enabled 169 | bool enabled_ = false; 170 | 171 | //! Relative target velocity of object in the velocity mode (x [m/s], y [m/s], theta [rad/s]) 172 | Eigen::Vector3d targetVel_ = Eigen::Vector3d::Zero(); 173 | 174 | //! Pointer to the front footstep in the footstep queue 175 | const Footstep * frontFootstep_ = nullptr; 176 | 177 | //! Pose of the front waypoint in the waypoint queue 178 | sva::PTransformd frontWaypointPose_ = sva::PTransformd::Identity(); 179 | 180 | //! Object transformation in one footstep duration 181 | Eigen::Vector3d objDeltaTrans_ = Eigen::Vector3d::Zero(); 182 | }; 183 | 184 | public: 185 | /** \brief Constructor. 186 | \param ctlPtr pointer to controller 187 | \param mcRtcConfig mc_rtc configuration 188 | */ 189 | ManipManager(LocomanipController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig = {}); 190 | 191 | /** \brief Reset. 192 | 193 | This method should be called once when controller is reset. 194 | */ 195 | void reset(); 196 | 197 | /** \brief Update. 198 | 199 | This method should be called once every control cycle. 200 | */ 201 | virtual void update(); 202 | 203 | /** \brief Stop. 204 | 205 | This method should be called once when stopping the controller. 206 | */ 207 | void stop(); 208 | 209 | /** \brief Const accessor to the configuration. */ 210 | inline const Configuration & config() const noexcept 211 | { 212 | return config_; 213 | } 214 | 215 | /** \brief Const accessor to the velocity mode data. */ 216 | inline const VelModeData & velModeData() const noexcept 217 | { 218 | return velModeData_; 219 | } 220 | 221 | /** \brief Add entries to the GUI. */ 222 | void addToGUI(mc_rtc::gui::StateBuilder & gui); 223 | 224 | /** \brief Remove entries from the GUI. */ 225 | void removeFromGUI(mc_rtc::gui::StateBuilder & gui); 226 | 227 | /** \brief Add entries to the logger. */ 228 | void addToLogger(mc_rtc::Logger & logger); 229 | 230 | /** \brief Remove entries from the logger. */ 231 | void removeFromLogger(mc_rtc::Logger & logger); 232 | 233 | /** \brief Get hand surface name. */ 234 | const std::string & surfaceName(const Hand & hand) const; 235 | 236 | /** \brief Append a target waypoint to the queue. 237 | \param newWaypoint waypoint to append 238 | \return whether newWaypoint is appended 239 | 240 | A waypoint pose is an object pose that "does not" include an offset pose. 241 | */ 242 | bool appendWaypoint(const Waypoint & newWaypoint); 243 | 244 | /** \brief Clear waypoint queue. 245 | 246 | The object is stopped at the timing when the ongoing foot swing ends. 247 | */ 248 | void clearWaypointQueue(); 249 | 250 | /** \brief Reach hand to object. */ 251 | void reachHandToObj(); 252 | 253 | /** \brief Release hand from object. */ 254 | void releaseHandFromObj(); 255 | 256 | /** \brief Calculate reference object pose. 257 | \param t time 258 | 259 | The returned reference object pose "does not" include an offset pose. 260 | */ 261 | inline sva::PTransformd calcRefObjPose(double t) const 262 | { 263 | return (*objPoseFunc_)(t); 264 | } 265 | 266 | /** \brief Calculate reference object velocity. 267 | \param t time 268 | */ 269 | inline sva::MotionVecd calcRefObjVel(double t) const 270 | { 271 | return objPoseFunc_->derivative(t, 1); 272 | } 273 | 274 | /** \brief Access waypoint queue. 275 | 276 | A waypoint pose is an object pose that "does not" include an offset pose. 277 | */ 278 | inline const std::deque & waypointQueue() const noexcept 279 | { 280 | return waypointQueue_; 281 | } 282 | 283 | /** \brief Const accessor to the object pose offset. */ 284 | inline const sva::PTransformd & objPoseOffset() const noexcept 285 | { 286 | return objPoseOffset_; 287 | } 288 | 289 | /** \brief Set object pose offset with interpolation. 290 | \param newObjPoseOffset object pose offset to set 291 | \param interpDuration interpolation duration [sec] 292 | \return whether newObjPoseOffset is successfully set 293 | */ 294 | bool setObjPoseOffset(const sva::PTransformd & newObjPoseOffset, double interpDuration); 295 | 296 | /** \brief Whether the object pose offset is being interpolated. */ 297 | inline bool interpolatingObjPoseOffset() const 298 | { 299 | return static_cast(objPoseOffsetFunc_); 300 | }; 301 | 302 | /** \brief Get manipulation phase. */ 303 | inline const std::shared_ptr & manipPhase(const Hand & hand) const 304 | { 305 | return manipPhases_.at(hand); 306 | } 307 | 308 | /** \brief Set reference hand wrench. 309 | \param hand hand 310 | \param wrench reference hand wrench in the hand frame 311 | \param startTime time to start interpolation 312 | \param interpDuration duration to interpolate 313 | */ 314 | void setRefHandWrench(const Hand & hand, const sva::ForceVecd & wrench, double startTime, double interpDuration); 315 | 316 | /** \brief Calculate reference hand wrench. 317 | \param hand hand 318 | \param t time 319 | \return reference hand wrench in the hand frame 320 | */ 321 | inline sva::ForceVecd calcRefHandWrench(const Hand & hand, double t) const 322 | { 323 | return (*handWrenchFuncs_.at(hand))(t); 324 | } 325 | 326 | /** \brief Whether the reference hand wrenches are being interpolated. */ 327 | bool interpolatingRefHandWrench() const; 328 | 329 | /** \brief Require sending footstep command following an object. */ 330 | void requireFootstepFollowingObj(); 331 | 332 | /** \brief Start velocity mode. 333 | \return whether it is successfully started 334 | */ 335 | bool startVelMode(); 336 | 337 | /** \brief End velocity mode. 338 | \return whether it is successfully ended 339 | */ 340 | bool endVelMode(); 341 | 342 | /** \brief Set the relative target velocity 343 | \param targetVel relative target velocity of object in the velocity mode (x [m/s], y [m/s], theta [rad/s]) 344 | */ 345 | void setRelativeVel(const Eigen::Vector3d & targetVel); 346 | 347 | /** \brief Whether the velocity mode (i.e., moving the object at the relative target velocity) is enabled. */ 348 | inline bool velModeEnabled() const 349 | { 350 | return velModeData_.enabled_; 351 | } 352 | 353 | protected: 354 | /** \brief Const accessor to the controller. */ 355 | inline const LocomanipController & ctl() const 356 | { 357 | return *ctlPtr_; 358 | } 359 | 360 | /** \brief Accessor to the controller. */ 361 | inline LocomanipController & ctl() 362 | { 363 | return *ctlPtr_; 364 | } 365 | 366 | /** \brief Update object trajectory. */ 367 | virtual void updateObjTraj(); 368 | 369 | /** \brief Update hand tasks. */ 370 | virtual void updateHandTraj(); 371 | 372 | /** \brief Update footstep. */ 373 | virtual void updateFootstep(); 374 | 375 | /** \brief Update object and footstep for velocity mode. */ 376 | void updateForVelMode(); 377 | 378 | /** \brief Make a footstep. 379 | \param foot foot 380 | \param footMidpose middle pose of both feet 381 | \param startTime time to start the footstep 382 | \param swingTrajConfig configuration for swing trajectory 383 | */ 384 | Footstep makeFootstep(const Foot & foot, 385 | const sva::PTransformd & footMidpose, 386 | double startTime, 387 | const mc_rtc::Configuration & swingTrajConfig = {}) const; 388 | 389 | /** \brief ROS callback of object pose topic. */ 390 | void objPoseCallback(const geometry_msgs::PoseStamped::ConstPtr & poseStMsg); 391 | 392 | /** \brief ROS callback of object velocity topic. */ 393 | void objVelCallback(const geometry_msgs::TwistStamped::ConstPtr & twistStMsg); 394 | 395 | protected: 396 | //! Maximum time of interpolation endpoint 397 | const double interpMaxTime_ = 1e10; 398 | 399 | //! Configuration 400 | Configuration config_; 401 | 402 | //! Velocity mode data 403 | VelModeData velModeData_; 404 | 405 | //! Pointer to controller 406 | LocomanipController * ctlPtr_ = nullptr; 407 | 408 | //! Waypoint queue 409 | std::deque waypointQueue_; 410 | 411 | //! Last waypoint pose 412 | sva::PTransformd lastWaypointPose_ = sva::PTransformd::Identity(); 413 | 414 | //! Object pose function 415 | std::shared_ptr> objPoseFunc_; 416 | 417 | //! Object pose offset 418 | sva::PTransformd objPoseOffset_ = sva::PTransformd::Identity(); 419 | 420 | //! Object pose offset function 421 | std::shared_ptr> objPoseOffsetFunc_; 422 | 423 | //! Manipulation phases 424 | std::unordered_map> manipPhases_; 425 | 426 | //! Hand wrench functions 427 | std::unordered_map>> handWrenchFuncs_; 428 | 429 | //! Whether to require updating impedance gains 430 | bool requireImpGainUpdate_ = true; 431 | 432 | //! Whether to require sending footstep command following an object 433 | bool requireFootstepFollowingObj_ = false; 434 | 435 | //! ROS variables 436 | //! @{ 437 | std::shared_ptr nh_; 438 | ros::CallbackQueue callbackQueue_; 439 | ros::Subscriber objPoseSub_; 440 | ros::Subscriber objVelSub_; 441 | //! @} 442 | }; 443 | } // namespace LMC 444 | -------------------------------------------------------------------------------- /include/LocomanipController/ManipPhase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace LMC 8 | { 9 | class LocomanipController; 10 | class ManipManager; 11 | 12 | /** \brief Manipulation phase label. */ 13 | enum class ManipPhaseLabel 14 | { 15 | //! Free phase 16 | Free = 0, 17 | 18 | //! Pre-reach phase 19 | PreReach, 20 | 21 | //! Reach phase 22 | Reach, 23 | 24 | //! Grasp phase 25 | Grasp, 26 | 27 | //! Hold phase 28 | Hold, 29 | 30 | //! Ungrasp phase 31 | Ungrasp, 32 | 33 | //! Release phase 34 | Release 35 | }; 36 | 37 | namespace ManipPhase 38 | { 39 | /** \brief Base of manipulation phase. */ 40 | class Base 41 | { 42 | public: 43 | /** \brief Constructor. 44 | \param label manipulation phase label 45 | \param hand hand 46 | \param manipManager manipulation manager 47 | */ 48 | Base(const ManipPhaseLabel & label, const Hand & hand, ManipManager * manipManager) 49 | : label_(label), hand_(hand), manipManager_(manipManager) 50 | { 51 | } 52 | 53 | /** \brief Get label. */ 54 | virtual ManipPhaseLabel label() const 55 | { 56 | return label_; 57 | } 58 | 59 | /** \brief Run manipulation phase. */ 60 | virtual void run() {} 61 | 62 | /** \brief Get whether manipulation phase is completed. */ 63 | virtual bool complete() const 64 | { 65 | return false; 66 | } 67 | 68 | /** \brief Make next manipulation phase. */ 69 | virtual std::shared_ptr makeNextManipPhase() const 70 | { 71 | return nullptr; 72 | } 73 | 74 | protected: 75 | /** \brief Const accessor to the controller. */ 76 | const LocomanipController & ctl() const; 77 | 78 | /** \brief Accessor to the controller. */ 79 | LocomanipController & ctl(); 80 | 81 | protected: 82 | //! Manipulation phase label 83 | ManipPhaseLabel label_; 84 | 85 | //! Hand 86 | Hand hand_; 87 | 88 | //! Manipulation manager 89 | ManipManager * manipManager_; 90 | }; 91 | 92 | /** \brief Manipulation free phase. */ 93 | class Free : public Base 94 | { 95 | public: 96 | /** \brief Constructor. 97 | \param hand hand 98 | \param manipManager manipulation manager 99 | */ 100 | Free(const Hand & hand, ManipManager * manipManager); 101 | }; 102 | 103 | /** \brief Manipulation pre-reach phase. */ 104 | class PreReach : public Base 105 | { 106 | public: 107 | /** \brief Constructor. 108 | \param hand hand 109 | \param manipManager manipulation manager 110 | */ 111 | PreReach(const Hand & hand, ManipManager * manipManager); 112 | 113 | /** \brief Run manipulation phase. */ 114 | virtual void run() override; 115 | 116 | /** \brief Get whether manipulation phase is completed. */ 117 | virtual bool complete() const override; 118 | 119 | /** \brief Make next manipulation phase. */ 120 | virtual std::shared_ptr makeNextManipPhase() const override; 121 | 122 | protected: 123 | //! Phase end time [sec] 124 | double endTime_ = 0; 125 | }; 126 | 127 | /** \brief Manipulation reach phase. */ 128 | class Reach : public Base 129 | { 130 | public: 131 | /** \brief Constructor. 132 | \param hand hand 133 | \param manipManager manipulation manager 134 | */ 135 | Reach(const Hand & hand, ManipManager * manipManager); 136 | 137 | /** \brief Run manipulation phase. */ 138 | virtual void run() override; 139 | 140 | /** \brief Get whether manipulation phase is completed. */ 141 | virtual bool complete() const override; 142 | 143 | /** \brief Make next manipulation phase. */ 144 | virtual std::shared_ptr makeNextManipPhase() const override; 145 | 146 | protected: 147 | //! Function to interpolate reaching ratio 148 | std::shared_ptr> reachingRatioFunc_; 149 | }; 150 | 151 | /** \brief Manipulation grasp phase. */ 152 | class Grasp : public Base 153 | { 154 | public: 155 | /** \brief Constructor. 156 | \param hand hand 157 | \param manipManager manipulation manager 158 | */ 159 | Grasp(const Hand & hand, ManipManager * manipManager); 160 | 161 | /** \brief Run manipulation phase. */ 162 | virtual void run() override; 163 | 164 | /** \brief Get whether manipulation phase is completed. */ 165 | virtual bool complete() const override; 166 | 167 | /** \brief Make next manipulation phase. */ 168 | virtual std::shared_ptr makeNextManipPhase() const override; 169 | }; 170 | 171 | /** \brief Manipulation hold phase. */ 172 | class Hold : public Base 173 | { 174 | public: 175 | /** \brief Constructor. 176 | \param hand hand 177 | \param manipManager manipulation manager 178 | */ 179 | Hold(const Hand & hand, ManipManager * manipManager); 180 | 181 | /** \brief Run manipulation phase. */ 182 | virtual void run() override; 183 | }; 184 | 185 | /** \brief Manipulation ungrasp phase. */ 186 | class Ungrasp : public Base 187 | { 188 | public: 189 | /** \brief Constructor. 190 | \param hand hand 191 | \param manipManager manipulation manager 192 | */ 193 | Ungrasp(const Hand & hand, ManipManager * manipManager); 194 | 195 | /** \brief Run manipulation phase. */ 196 | virtual void run() override; 197 | 198 | /** \brief Get whether manipulation phase is completed. */ 199 | virtual bool complete() const override; 200 | 201 | /** \brief Make next manipulation phase. */ 202 | virtual std::shared_ptr makeNextManipPhase() const override; 203 | }; 204 | 205 | /** \brief Manipulation release phase. */ 206 | class Release : public Base 207 | { 208 | public: 209 | /** \brief Constructor. 210 | \param hand hand 211 | \param manipManager manipulation manager 212 | */ 213 | Release(const Hand & hand, ManipManager * manipManager); 214 | 215 | /** \brief Run manipulation phase. */ 216 | virtual void run() override; 217 | 218 | /** \brief Get whether manipulation phase is completed. */ 219 | virtual bool complete() const override; 220 | 221 | /** \brief Make next manipulation phase. */ 222 | virtual std::shared_ptr makeNextManipPhase() const override; 223 | 224 | protected: 225 | //! Function to interpolate reaching ratio 226 | std::shared_ptr> reachingRatioFunc_; 227 | }; 228 | } // namespace ManipPhase 229 | } // namespace LMC 230 | 231 | namespace std 232 | { 233 | /** \brief Convert manipulation phase label to string. */ 234 | std::string to_string(const LMC::ManipPhaseLabel & label); 235 | } // namespace std 236 | -------------------------------------------------------------------------------- /include/LocomanipController/MathUtils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | using BWC::projGround; 8 | } // namespace LMC 9 | -------------------------------------------------------------------------------- /include/LocomanipController/State.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | class LocomanipController; 8 | 9 | /** \brief FSM State with utility functions. */ 10 | struct State : mc_control::fsm::State 11 | { 12 | public: 13 | /** \brief Start. */ 14 | void start(mc_control::fsm::Controller & _ctl) override; 15 | 16 | protected: 17 | /** \brief Const accessor to the controller. */ 18 | inline const LocomanipController & ctl() const 19 | { 20 | return *ctlPtr_; 21 | } 22 | 23 | /** \brief Accessor to the controller. */ 24 | inline LocomanipController & ctl() 25 | { 26 | return *ctlPtr_; 27 | } 28 | 29 | protected: 30 | //! Pointer to controller 31 | LocomanipController * ctlPtr_ = nullptr; 32 | }; 33 | } // namespace LMC 34 | -------------------------------------------------------------------------------- /include/LocomanipController/centroidal/CentroidalManagerPreviewControlExtZmp.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace LMC 7 | { 8 | /** \brief Centroidal manager with preview control. 9 | 10 | Centroidal manager calculates the centroidal targets from the specified reference ZMP trajectory and sensor 11 | measurements. 12 | */ 13 | class CentroidalManagerPreviewControlExtZmp : public CentroidalManager, BWC::CentroidalManagerPreviewControlZmp 14 | { 15 | public: 16 | /** \brief Data of ext-ZMP (i.e., ZMP with external forces). 17 | 18 | 19 | The definition of ext-ZMP is given in the equation (9) of the paper: 20 | M Murooka, et al. Humanoid loco-Manipulations pattern generation and stabilization control. RA-Letters, 2021 21 | */ 22 | struct ExtZmpData 23 | { 24 | //! Scale 25 | double scale = 1.0; 26 | 27 | //! Offset 28 | Eigen::Vector2d offset = Eigen::Vector2d::Zero(); 29 | 30 | /** \brief Convert conventional ZMP to ext-ZMP. 31 | \param zmp ZMP 32 | */ 33 | inline Eigen::Vector2d apply(const Eigen::Vector2d & zmp) const 34 | { 35 | return scale * zmp - offset; 36 | } 37 | 38 | /** \brief Convert ext-ZMP to conventional ZMP. 39 | \param extZmp ext-ZMP 40 | */ 41 | inline Eigen::Vector2d applyInv(const Eigen::Vector2d & extZmp) const 42 | { 43 | return (extZmp + offset) / scale; 44 | } 45 | }; 46 | 47 | public: 48 | /** \brief Constructor. 49 | \param ctlPtr pointer to controller 50 | \param mcRtcConfig mc_rtc configuration 51 | */ 52 | CentroidalManagerPreviewControlExtZmp(LocomanipController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig = {}); 53 | 54 | /** \brief Add entries to the logger. */ 55 | virtual void addToLogger(mc_rtc::Logger & logger) override; 56 | 57 | protected: 58 | /** \brief Run MPC to plan centroidal trajectory. 59 | 60 | This method calculates plannedZmp_ and plannedForceZ_ from mpcCom_ and mpcComVel_. 61 | */ 62 | virtual void runMpc() override; 63 | 64 | /** \brief Calculate planned CoM acceleration. 65 | 66 | This method is overridden to support extended CoM-ZMP models (e.g., manipulation forces) in inherited classes. 67 | */ 68 | virtual Eigen::Vector3d calcPlannedComAccel() const override; 69 | 70 | /** \brief Calculate reference data of MPC. */ 71 | virtual Eigen::Vector2d calcRefData(double t) const override; 72 | 73 | /** \brief Calculate data of ext-ZMP. */ 74 | ExtZmpData calcExtZmpData(double t) const; 75 | 76 | protected: 77 | //! Data of ext-ZMP 78 | ExtZmpData extZmpData_; 79 | }; 80 | } // namespace LMC 81 | -------------------------------------------------------------------------------- /include/LocomanipController/states/ConfigManipState.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | /** \brief FSM state to send manipulation commands from configuration. */ 8 | struct ConfigManipState : State 9 | { 10 | public: 11 | /** \brief Start. */ 12 | void start(mc_control::fsm::Controller & ctl) override; 13 | 14 | /** \brief Run. */ 15 | bool run(mc_control::fsm::Controller & ctl) override; 16 | 17 | /** \brief Teardown. */ 18 | void teardown(mc_control::fsm::Controller & ctl) override; 19 | 20 | protected: 21 | //! Phase 22 | int phase_ = 0; 23 | 24 | //! End time of velocity mode [sec] 25 | double velModeEndTime_ = 0.0; 26 | }; 27 | } // namespace LMC 28 | -------------------------------------------------------------------------------- /include/LocomanipController/states/GuiManipState.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | /** \brief FSM state to send manipulation command from GUI. */ 8 | struct GuiManipState : State 9 | { 10 | public: 11 | /** \brief Start. */ 12 | void start(mc_control::fsm::Controller & ctl) override; 13 | 14 | /** \brief Run. */ 15 | bool run(mc_control::fsm::Controller & ctl) override; 16 | 17 | /** \brief Teardown. */ 18 | void teardown(mc_control::fsm::Controller & ctl) override; 19 | 20 | protected: 21 | //! Entry keys of GUI form 22 | //! @{ 23 | const std::unordered_map walkToObjConfigKeys_ = { 24 | {"x", "goal x relative to object [m]"}, 25 | {"y", "goal y relative to object [m]"}, 26 | {"yaw", "goal yaw relative to object [deg]"}}; 27 | const std::unordered_map moveObjConfigKeys_ = {{"x", "relative goal x [m]"}, 28 | {"yaw", "relative goal yaw [deg]"}, 29 | {"startTime", "start time from now [sec]"}, 30 | {"endTime", "end time from now [sec]"}, 31 | {"footstep", "whether to enable footstep"}}; 32 | const std::unordered_map updateObjConfigKeys_ = { 33 | {"target", "target object pose"}, 34 | {"interpDuration", "interpolation duration [sec]"}}; 35 | const std::unordered_map poseOffsetConfigKeys_ = { 36 | {"xyz", "xyz position [m]"}, 37 | {"rpy", "rpy orientation [deg]"}, 38 | {"interpDuration", "interpolation duration [sec]"}}; 39 | //! @} 40 | }; 41 | } // namespace LMC 42 | -------------------------------------------------------------------------------- /include/LocomanipController/states/InitialState.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace LMC 6 | { 7 | /** \brief FSM state to initialize. */ 8 | struct InitialState : State 9 | { 10 | public: 11 | /** \brief Start. */ 12 | void start(mc_control::fsm::Controller & ctl) override; 13 | 14 | /** \brief Run. */ 15 | bool run(mc_control::fsm::Controller & ctl) override; 16 | 17 | /** \brief Teardown. */ 18 | void teardown(mc_control::fsm::Controller & ctl) override; 19 | 20 | protected: 21 | /** \brief Check whether state is completed. */ 22 | bool complete() const; 23 | 24 | protected: 25 | //! Phase 26 | int phase_ = 0; 27 | 28 | //! Function to interpolate task stiffness 29 | std::shared_ptr> stiffnessRatioFunc_; 30 | 31 | //! Stiffness of CoM task 32 | Eigen::Vector3d comTaskStiffness_ = Eigen::Vector3d::Zero(); 33 | 34 | //! Stiffness of base link orientation task 35 | Eigen::Vector3d baseOriTaskStiffness_ = Eigen::Vector3d::Zero(); 36 | 37 | //! Stiffness of foot tasks 38 | std::unordered_map footTasksStiffness_; 39 | }; 40 | } // namespace LMC 41 | -------------------------------------------------------------------------------- /include/LocomanipController/states/TeleopState.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace LMC 10 | { 11 | /** \brief FSM state to walk with teleoperation. */ 12 | struct TeleopState : State 13 | { 14 | public: 15 | /** \brief Start. */ 16 | void start(mc_control::fsm::Controller & ctl) override; 17 | 18 | /** \brief Run. */ 19 | bool run(mc_control::fsm::Controller & ctl) override; 20 | 21 | /** \brief Teardown. */ 22 | void teardown(mc_control::fsm::Controller & ctl) override; 23 | 24 | protected: 25 | /** \brief ROS callback of twist topic. */ 26 | void twistCallback(const geometry_msgs::Twist::ConstPtr & twistMsg); 27 | 28 | protected: 29 | //! Relative target velocity of foot midpose (x [m/s], y [m/s], theta [rad/s]) 30 | Eigen::Vector3d targetVel_ = Eigen::Vector3d::Zero(); 31 | 32 | //! Scale to convert twist message to target velocity (x, y, theta) 33 | Eigen::Vector3d velScale_ = Eigen::Vector3d::Ones(); 34 | 35 | //! ROS variables 36 | //! @{ 37 | std::unique_ptr nh_; 38 | ros::CallbackQueue callbackQueue_; 39 | ros::Subscriber twistSub_; 40 | //! @} 41 | }; 42 | } // namespace LMC 43 | -------------------------------------------------------------------------------- /launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /mujoco/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # mc_mujoco 2 | if(NOT DEFINED MC_MUJOCO_MODEL_DIR) 3 | find_package(mc_mujoco REQUIRED) 4 | set(MC_MUJOCO_MODEL_DIR ${MC_MUJOCO_USER_DESTINATION}) 5 | endif() 6 | message("-- MC_MUJOCO_MODEL_DIR: ${MC_MUJOCO_MODEL_DIR}") 7 | 8 | add_subdirectory(model) 9 | -------------------------------------------------------------------------------- /mujoco/model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | file(GLOB MODEL_NAME_LIST ${CMAKE_CURRENT_SOURCE_DIR}/*.xml) 2 | 3 | foreach(MODEL_NAME IN LISTS MODEL_NAME_LIST) 4 | get_filename_component(MODEL_NAME ${MODEL_NAME} NAME_WE) 5 | file(WRITE 6 | ${CMAKE_CURRENT_BINARY_DIR}/${MODEL_NAME}.yaml 7 | "xmlModelPath: ${MC_MUJOCO_MODEL_DIR}/${MODEL_NAME}.xml" 8 | ) 9 | if(DEFINED CATKIN_DEVEL_PREFIX) 10 | configure_file(${MODEL_NAME}.xml 11 | ${MC_MUJOCO_MODEL_DIR}/${MODEL_NAME}.xml 12 | COPYONLY 13 | ) 14 | configure_file(${CMAKE_CURRENT_BINARY_DIR}/${MODEL_NAME}.yaml 15 | ${MC_MUJOCO_MODEL_DIR}/${MODEL_NAME}.yaml 16 | COPYONLY 17 | ) 18 | else() 19 | install(FILES 20 | ${MODEL_NAME}.xml 21 | ${CMAKE_CURRENT_BINARY_DIR}/${MODEL_NAME}.yaml 22 | DESTINATION 23 | ${MC_MUJOCO_MODEL_DIR} 24 | ) 25 | endif() 26 | endforeach() 27 | -------------------------------------------------------------------------------- /mujoco/model/Cart.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 13 | 15 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | locomanip_controller 3 | 0.1.0 4 | 5 | Humanoid loco-manipulation controller 6 | 7 | Masaki Murooka 8 | BSD 9 | 10 | http://ros.org/wiki/locomanip_controller 11 | Masaki Murooka 12 | 13 | catkin 14 | 15 | baseline_walking_controller 16 | 17 | eigen 18 | 19 | rviz 20 | rospy 21 | cnoid_ros_utils 22 | 23 | doxygen 24 | 25 | -------------------------------------------------------------------------------- /rviz/display.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /Surface1/Namespaces1 9 | Splitter Ratio: 0.4228571355342865 10 | Tree Height: 441 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5899999737739563 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | - Class: mc_rtc_rviz_panel/mc_rtc panel 30 | Name: mc_rtc panel 31 | Preferences: 32 | PromptSaveOnExit: false 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 1 39 | Class: rviz/Axes 40 | Enabled: false 41 | Length: 0.20000000298023224 42 | Name: Axes 43 | Radius: 0.03999999910593033 44 | Reference Frame: 45 | Show Trail: false 46 | Value: false 47 | - Alpha: 0.5 48 | Cell Size: 1 49 | Class: rviz/Grid 50 | Color: 0; 0; 0 51 | Enabled: true 52 | Line Style: 53 | Line Width: 0.029999999329447746 54 | Value: Lines 55 | Name: Grid 56 | Normal Cell Count: 0 57 | Offset: 58 | X: 0 59 | Y: 0 60 | Z: 0 61 | Plane: XY 62 | Plane Cell Count: 100 63 | Reference Frame: 64 | Value: true 65 | - Alpha: 0.5 66 | Class: rviz/RobotModel 67 | Collision Enabled: false 68 | Enabled: true 69 | Links: 70 | All Links Enabled: true 71 | Expand Joint Details: false 72 | Expand Link Details: false 73 | Expand Tree: false 74 | L_ANKLE_P_S: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | L_ANKLE_R_S: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | L_ELBOW_P_S: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | L_ELBOW_Y_S: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | L_HIP_P_S: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | L_HIP_R_S: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | L_HIP_Y_S: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | L_KNEE_S: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | L_LINDEX_S: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | L_LLITTLE_S: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | L_LTHUMB_S: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | Value: true 127 | L_SHOULDER_P_S: 128 | Alpha: 1 129 | Show Axes: false 130 | Show Trail: false 131 | Value: true 132 | L_SHOULDER_R_S: 133 | Alpha: 1 134 | Show Axes: false 135 | Show Trail: false 136 | L_SHOULDER_Y_S: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | L_UINDEX_S: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | L_ULITTLE_S: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | Value: true 151 | L_UTHUMB_S: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | Value: true 156 | L_WRIST_R_S: 157 | Alpha: 1 158 | Show Axes: false 159 | Show Trail: false 160 | L_WRIST_Y_S: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | Link Tree Style: Links in Alphabetic Order 166 | NECK_P_S: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | Value: true 171 | NECK_R_S: 172 | Alpha: 1 173 | Show Axes: false 174 | Show Trail: false 175 | NECK_Y_S: 176 | Alpha: 1 177 | Show Axes: false 178 | Show Trail: false 179 | Value: true 180 | PELVIS_S: 181 | Alpha: 1 182 | Show Axes: false 183 | Show Trail: false 184 | Value: true 185 | R_ANKLE_P_S: 186 | Alpha: 1 187 | Show Axes: false 188 | Show Trail: false 189 | Value: true 190 | R_ANKLE_R_S: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | R_ELBOW_P_S: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | Value: true 199 | R_ELBOW_Y_S: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | Value: true 204 | R_HIP_P_S: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | R_HIP_R_S: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | Value: true 213 | R_HIP_Y_S: 214 | Alpha: 1 215 | Show Axes: false 216 | Show Trail: false 217 | Value: true 218 | R_KNEE_S: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | R_LINDEX_S: 224 | Alpha: 1 225 | Show Axes: false 226 | Show Trail: false 227 | Value: true 228 | R_LLITTLE_S: 229 | Alpha: 1 230 | Show Axes: false 231 | Show Trail: false 232 | Value: true 233 | R_LTHUMB_S: 234 | Alpha: 1 235 | Show Axes: false 236 | Show Trail: false 237 | Value: true 238 | R_SHOULDER_P_S: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | Value: true 243 | R_SHOULDER_R_S: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | R_SHOULDER_Y_S: 248 | Alpha: 1 249 | Show Axes: false 250 | Show Trail: false 251 | Value: true 252 | R_UINDEX_S: 253 | Alpha: 1 254 | Show Axes: false 255 | Show Trail: false 256 | Value: true 257 | R_ULITTLE_S: 258 | Alpha: 1 259 | Show Axes: false 260 | Show Trail: false 261 | Value: true 262 | R_UTHUMB_S: 263 | Alpha: 1 264 | Show Axes: false 265 | Show Trail: false 266 | Value: true 267 | R_WRIST_R_S: 268 | Alpha: 1 269 | Show Axes: false 270 | Show Trail: false 271 | R_WRIST_Y_S: 272 | Alpha: 1 273 | Show Axes: false 274 | Show Trail: false 275 | Value: true 276 | WAIST_P_S: 277 | Alpha: 1 278 | Show Axes: false 279 | Show Trail: false 280 | WAIST_R_S: 281 | Alpha: 1 282 | Show Axes: false 283 | Show Trail: false 284 | Value: true 285 | WAIST_Y_S: 286 | Alpha: 1 287 | Show Axes: false 288 | Show Trail: false 289 | Value: true 290 | base_link: 291 | Alpha: 1 292 | Show Axes: false 293 | Show Trail: false 294 | dcamera: 295 | Alpha: 1 296 | Show Axes: false 297 | Show Trail: false 298 | gsensor: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | gyrometer: 303 | Alpha: 1 304 | Show Axes: false 305 | Show Trail: false 306 | l_ankle: 307 | Alpha: 1 308 | Show Axes: false 309 | Show Trail: false 310 | l_wrist: 311 | Alpha: 1 312 | Show Axes: false 313 | Show Trail: false 314 | lcamera: 315 | Alpha: 1 316 | Show Axes: false 317 | Show Trail: false 318 | lfsensor: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | lhsensor: 323 | Alpha: 1 324 | Show Axes: false 325 | Show Trail: false 326 | r_ankle: 327 | Alpha: 1 328 | Show Axes: false 329 | Show Trail: false 330 | r_wrist: 331 | Alpha: 1 332 | Show Axes: false 333 | Show Trail: false 334 | ranger: 335 | Alpha: 1 336 | Show Axes: false 337 | Show Trail: false 338 | rcamera: 339 | Alpha: 1 340 | Show Axes: false 341 | Show Trail: false 342 | rfsensor: 343 | Alpha: 1 344 | Show Axes: false 345 | Show Trail: false 346 | rhsensor: 347 | Alpha: 1 348 | Show Axes: false 349 | Show Trail: false 350 | Name: ControlRobotModel 351 | Robot Description: /control/robot_description 352 | TF Prefix: control 353 | Update Interval: 0 354 | Value: true 355 | Visual Enabled: true 356 | - Alpha: 1 357 | Class: rviz/RobotModel 358 | Collision Enabled: false 359 | Enabled: true 360 | Links: 361 | All Links Enabled: true 362 | Expand Joint Details: false 363 | Expand Link Details: false 364 | Expand Tree: false 365 | L_ANKLE_P_S: 366 | Alpha: 1 367 | Show Axes: false 368 | Show Trail: false 369 | Value: true 370 | L_ANKLE_R_S: 371 | Alpha: 1 372 | Show Axes: false 373 | Show Trail: false 374 | L_ELBOW_P_S: 375 | Alpha: 1 376 | Show Axes: false 377 | Show Trail: false 378 | Value: true 379 | L_ELBOW_Y_S: 380 | Alpha: 1 381 | Show Axes: false 382 | Show Trail: false 383 | Value: true 384 | L_HIP_P_S: 385 | Alpha: 1 386 | Show Axes: false 387 | Show Trail: false 388 | L_HIP_R_S: 389 | Alpha: 1 390 | Show Axes: false 391 | Show Trail: false 392 | Value: true 393 | L_HIP_Y_S: 394 | Alpha: 1 395 | Show Axes: false 396 | Show Trail: false 397 | Value: true 398 | L_KNEE_S: 399 | Alpha: 1 400 | Show Axes: false 401 | Show Trail: false 402 | Value: true 403 | L_LINDEX_S: 404 | Alpha: 1 405 | Show Axes: false 406 | Show Trail: false 407 | Value: true 408 | L_LLITTLE_S: 409 | Alpha: 1 410 | Show Axes: false 411 | Show Trail: false 412 | Value: true 413 | L_LTHUMB_S: 414 | Alpha: 1 415 | Show Axes: false 416 | Show Trail: false 417 | Value: true 418 | L_SHOULDER_P_S: 419 | Alpha: 1 420 | Show Axes: false 421 | Show Trail: false 422 | Value: true 423 | L_SHOULDER_R_S: 424 | Alpha: 1 425 | Show Axes: false 426 | Show Trail: false 427 | L_SHOULDER_Y_S: 428 | Alpha: 1 429 | Show Axes: false 430 | Show Trail: false 431 | Value: true 432 | L_UINDEX_S: 433 | Alpha: 1 434 | Show Axes: false 435 | Show Trail: false 436 | Value: true 437 | L_ULITTLE_S: 438 | Alpha: 1 439 | Show Axes: false 440 | Show Trail: false 441 | Value: true 442 | L_UTHUMB_S: 443 | Alpha: 1 444 | Show Axes: false 445 | Show Trail: false 446 | Value: true 447 | L_WRIST_R_S: 448 | Alpha: 1 449 | Show Axes: false 450 | Show Trail: false 451 | L_WRIST_Y_S: 452 | Alpha: 1 453 | Show Axes: false 454 | Show Trail: false 455 | Value: true 456 | Link Tree Style: Links in Alphabetic Order 457 | NECK_P_S: 458 | Alpha: 1 459 | Show Axes: false 460 | Show Trail: false 461 | Value: true 462 | NECK_R_S: 463 | Alpha: 1 464 | Show Axes: false 465 | Show Trail: false 466 | NECK_Y_S: 467 | Alpha: 1 468 | Show Axes: false 469 | Show Trail: false 470 | Value: true 471 | PELVIS_S: 472 | Alpha: 1 473 | Show Axes: false 474 | Show Trail: false 475 | Value: true 476 | R_ANKLE_P_S: 477 | Alpha: 1 478 | Show Axes: false 479 | Show Trail: false 480 | Value: true 481 | R_ANKLE_R_S: 482 | Alpha: 1 483 | Show Axes: false 484 | Show Trail: false 485 | R_ELBOW_P_S: 486 | Alpha: 1 487 | Show Axes: false 488 | Show Trail: false 489 | Value: true 490 | R_ELBOW_Y_S: 491 | Alpha: 1 492 | Show Axes: false 493 | Show Trail: false 494 | Value: true 495 | R_HIP_P_S: 496 | Alpha: 1 497 | Show Axes: false 498 | Show Trail: false 499 | R_HIP_R_S: 500 | Alpha: 1 501 | Show Axes: false 502 | Show Trail: false 503 | Value: true 504 | R_HIP_Y_S: 505 | Alpha: 1 506 | Show Axes: false 507 | Show Trail: false 508 | Value: true 509 | R_KNEE_S: 510 | Alpha: 1 511 | Show Axes: false 512 | Show Trail: false 513 | Value: true 514 | R_LINDEX_S: 515 | Alpha: 1 516 | Show Axes: false 517 | Show Trail: false 518 | Value: true 519 | R_LLITTLE_S: 520 | Alpha: 1 521 | Show Axes: false 522 | Show Trail: false 523 | Value: true 524 | R_LTHUMB_S: 525 | Alpha: 1 526 | Show Axes: false 527 | Show Trail: false 528 | Value: true 529 | R_SHOULDER_P_S: 530 | Alpha: 1 531 | Show Axes: false 532 | Show Trail: false 533 | Value: true 534 | R_SHOULDER_R_S: 535 | Alpha: 1 536 | Show Axes: false 537 | Show Trail: false 538 | R_SHOULDER_Y_S: 539 | Alpha: 1 540 | Show Axes: false 541 | Show Trail: false 542 | Value: true 543 | R_UINDEX_S: 544 | Alpha: 1 545 | Show Axes: false 546 | Show Trail: false 547 | Value: true 548 | R_ULITTLE_S: 549 | Alpha: 1 550 | Show Axes: false 551 | Show Trail: false 552 | Value: true 553 | R_UTHUMB_S: 554 | Alpha: 1 555 | Show Axes: false 556 | Show Trail: false 557 | Value: true 558 | R_WRIST_R_S: 559 | Alpha: 1 560 | Show Axes: false 561 | Show Trail: false 562 | R_WRIST_Y_S: 563 | Alpha: 1 564 | Show Axes: false 565 | Show Trail: false 566 | Value: true 567 | WAIST_P_S: 568 | Alpha: 1 569 | Show Axes: false 570 | Show Trail: false 571 | WAIST_R_S: 572 | Alpha: 1 573 | Show Axes: false 574 | Show Trail: false 575 | Value: true 576 | WAIST_Y_S: 577 | Alpha: 1 578 | Show Axes: false 579 | Show Trail: false 580 | Value: true 581 | base_link: 582 | Alpha: 1 583 | Show Axes: false 584 | Show Trail: false 585 | dcamera: 586 | Alpha: 1 587 | Show Axes: false 588 | Show Trail: false 589 | gsensor: 590 | Alpha: 1 591 | Show Axes: false 592 | Show Trail: false 593 | gyrometer: 594 | Alpha: 1 595 | Show Axes: false 596 | Show Trail: false 597 | l_ankle: 598 | Alpha: 1 599 | Show Axes: false 600 | Show Trail: false 601 | l_wrist: 602 | Alpha: 1 603 | Show Axes: false 604 | Show Trail: false 605 | lcamera: 606 | Alpha: 1 607 | Show Axes: false 608 | Show Trail: false 609 | lfsensor: 610 | Alpha: 1 611 | Show Axes: false 612 | Show Trail: false 613 | lhsensor: 614 | Alpha: 1 615 | Show Axes: false 616 | Show Trail: false 617 | r_ankle: 618 | Alpha: 1 619 | Show Axes: false 620 | Show Trail: false 621 | r_wrist: 622 | Alpha: 1 623 | Show Axes: false 624 | Show Trail: false 625 | ranger: 626 | Alpha: 1 627 | Show Axes: false 628 | Show Trail: false 629 | rcamera: 630 | Alpha: 1 631 | Show Axes: false 632 | Show Trail: false 633 | rfsensor: 634 | Alpha: 1 635 | Show Axes: false 636 | Show Trail: false 637 | rhsensor: 638 | Alpha: 1 639 | Show Axes: false 640 | Show Trail: false 641 | Name: RealRobotModel 642 | Robot Description: /real/robot_description 643 | TF Prefix: real 644 | Update Interval: 0 645 | Value: true 646 | Visual Enabled: true 647 | - Alpha: 0.5 648 | Class: rviz/RobotModel 649 | Collision Enabled: false 650 | Enabled: true 651 | Links: 652 | All Links Enabled: true 653 | Body: 654 | Alpha: 1 655 | Show Axes: false 656 | Show Trail: false 657 | Value: true 658 | Expand Joint Details: false 659 | Expand Link Details: false 660 | Expand Tree: false 661 | Link Tree Style: Links in Alphabetic Order 662 | Root: 663 | Alpha: 1 664 | Show Axes: false 665 | Show Trail: false 666 | Name: ControlObjectModel 667 | Robot Description: control/env_2/robot_description 668 | TF Prefix: control/env_2 669 | Update Interval: 0 670 | Value: true 671 | Visual Enabled: true 672 | - Alpha: 1 673 | Class: rviz/RobotModel 674 | Collision Enabled: false 675 | Enabled: true 676 | Links: 677 | All Links Enabled: true 678 | Body: 679 | Alpha: 1 680 | Show Axes: false 681 | Show Trail: false 682 | Value: true 683 | Expand Joint Details: false 684 | Expand Link Details: false 685 | Expand Tree: false 686 | Link Tree Style: Links in Alphabetic Order 687 | Root: 688 | Alpha: 1 689 | Show Axes: false 690 | Show Trail: false 691 | Name: RealObjectModel 692 | Robot Description: real/env_2/robot_description 693 | TF Prefix: real/env_2 694 | Update Interval: 0 695 | Value: true 696 | Visual Enabled: true 697 | - Class: rviz/InteractiveMarkers 698 | Enable Transparency: true 699 | Enabled: true 700 | Name: mc_rtc GUI interactive markers 701 | Show Axes: false 702 | Show Descriptions: true 703 | Show Visual Aids: false 704 | Update Topic: /mc_rtc_rviz_interactive_markers/update 705 | Value: true 706 | - Class: rviz/MarkerArray 707 | Enabled: true 708 | Marker Topic: /mc_rtc_rviz 709 | Name: mc_rtc GUI markers 710 | Namespaces: 711 | {} 712 | Queue Size: 100 713 | Value: true 714 | - Class: rviz/TF 715 | Enabled: false 716 | Frame Timeout: 1000 717 | Frames: 718 | All Enabled: false 719 | Marker Alpha: 1 720 | Marker Scale: 1 721 | Name: TF 722 | Show Arrows: true 723 | Show Axes: true 724 | Show Names: true 725 | Tree: 726 | {} 727 | Update Interval: 0 728 | Value: false 729 | - Class: rviz/MarkerArray 730 | Enabled: false 731 | Marker Topic: /surfaces 732 | Name: Surface 733 | Namespaces: 734 | {} 735 | Queue Size: 100 736 | Value: false 737 | Enabled: true 738 | Global Options: 739 | Background Color: 255; 255; 255 740 | Default Light: true 741 | Fixed Frame: robot_map 742 | Frame Rate: 30 743 | Name: root 744 | Tools: 745 | - Class: rviz/Interact 746 | Hide Inactive Objects: true 747 | - Class: rviz/MoveCamera 748 | - Class: rviz/Select 749 | - Class: rviz/FocusCamera 750 | - Class: rviz/Measure 751 | - Class: rviz/SetInitialPose 752 | Theta std deviation: 0.2617993950843811 753 | Topic: /initialpose 754 | X std deviation: 0.5 755 | Y std deviation: 0.5 756 | - Class: rviz/SetGoal 757 | Topic: /move_base_simple/goal 758 | - Class: rviz/PublishPoint 759 | Single click: true 760 | Topic: /clicked_point 761 | Value: true 762 | Views: 763 | Current: 764 | Class: rviz/Orbit 765 | Distance: 3.376565456390381 766 | Enable Stereo Rendering: 767 | Stereo Eye Separation: 0.05999999865889549 768 | Stereo Focal Distance: 1 769 | Swap Stereo Eyes: false 770 | Value: false 771 | Field of View: 0.7853981852531433 772 | Focal Point: 773 | X: 0.35975292325019836 774 | Y: 0.29052281379699707 775 | Z: 0.7271809577941895 776 | Focal Shape Fixed Size: false 777 | Focal Shape Size: 0.05000000074505806 778 | Invert Z Axis: false 779 | Name: Current View 780 | Near Clip Distance: 0.009999999776482582 781 | Pitch: 0.4048004150390625 782 | Target Frame: 783 | Yaw: 4.873887538909912 784 | Saved: ~ 785 | Window Geometry: 786 | Displays: 787 | collapsed: false 788 | Height: 1492 789 | Hide Left Dock: false 790 | Hide Right Dock: true 791 | QMainWindow State: 000000ff00000000fd0000000400000000000003e50000027cfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000004bb000001df00000185000000b1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012e000004e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000004e90000013500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000055800000264fc0100000002fc0000000000000558000001c400fffffffa000000010200000002fb000000100044006900730070006c006100790073010000003a000002640000018400fffffffb00000018006d0063005f007200740063002000700061006e0065006c0100000000ffffffff000000d800fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000058fc0100000002fb0000000800540069006d00650000000000000007800000065900fffffffb0000000800540069006d0065010000000000000450000000000000000000000558000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 792 | Selection: 793 | collapsed: false 794 | Time: 795 | collapsed: false 796 | Tool Properties: 797 | collapsed: false 798 | Views: 799 | collapsed: true 800 | Width: 1368 801 | X: 1512 802 | Y: 54 803 | mc_rtc panel: 804 | collapsed: false 805 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(CONTROLLER_NAME LocomanipController) 2 | 3 | add_library(${CONTROLLER_NAME} SHARED 4 | LocomanipController.cpp 5 | HandTypes.cpp 6 | ManipPhase.cpp 7 | ManipManager.cpp 8 | CentroidalManager.cpp 9 | State.cpp 10 | centroidal/CentroidalManagerPreviewControlExtZmp.cpp 11 | ) 12 | target_link_libraries(${CONTROLLER_NAME} PUBLIC 13 | mc_rtc::mc_rtc_utils 14 | mc_rtc::mc_rbdyn 15 | mc_rtc::mc_control_fsm 16 | mc_rtc::mc_rtc_ros 17 | ) 18 | 19 | if(DEFINED CATKIN_DEVEL_PREFIX) 20 | target_link_libraries(${CONTROLLER_NAME} PUBLIC ${catkin_LIBRARIES}) 21 | else() 22 | target_link_libraries(${CONTROLLER_NAME} PUBLIC baseline_walking_controller::BaselineWalkingController) 23 | endif() 24 | 25 | install(TARGETS ${CONTROLLER_NAME} DESTINATION ${MC_RTC_LIBDIR} EXPORT ${TARGETS_EXPORT_NAME}) 26 | 27 | add_controller(${CONTROLLER_NAME}_controller lib.cpp "") 28 | set_target_properties(${CONTROLLER_NAME}_controller PROPERTIES OUTPUT_NAME "${CONTROLLER_NAME}") 29 | target_link_libraries(${CONTROLLER_NAME}_controller PUBLIC ${CONTROLLER_NAME}) 30 | 31 | add_subdirectory(states) 32 | -------------------------------------------------------------------------------- /src/CentroidalManager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace LMC; 5 | 6 | CentroidalManager::CentroidalManager(LocomanipController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig) 7 | : BWC::CentroidalManager(ctlPtr, mcRtcConfig) 8 | { 9 | } 10 | 11 | const LocomanipController & CentroidalManager::ctl() const 12 | { 13 | return *static_cast(ctlPtr_); 14 | } 15 | 16 | LocomanipController & CentroidalManager::ctl() 17 | { 18 | return *static_cast(ctlPtr_); 19 | } 20 | -------------------------------------------------------------------------------- /src/HandTypes.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace LMC; 6 | 7 | Hand LMC::strToHand(const std::string & handStr) 8 | { 9 | if(handStr == "Left") 10 | { 11 | return Hand::Left; 12 | } 13 | else if(handStr == "Right") 14 | { 15 | return Hand::Right; 16 | } 17 | else 18 | { 19 | mc_rtc::log::error_and_throw("[strToHand] Unsupported Hand name: {}", handStr); 20 | } 21 | } 22 | 23 | Hand LMC::opposite(const Hand & hand) 24 | { 25 | if(hand == Hand::Left) 26 | { 27 | return Hand::Right; 28 | } 29 | else // if(handStr == "Right") 30 | { 31 | return Hand::Left; 32 | } 33 | } 34 | 35 | int LMC::sign(const Hand & hand) 36 | { 37 | if(hand == Hand::Left) 38 | { 39 | return 1; 40 | } 41 | else // if(handStr == "Right") 42 | { 43 | return -1; 44 | } 45 | } 46 | 47 | std::string std::to_string(const Hand & hand) 48 | { 49 | if(hand == Hand::Left) 50 | { 51 | return std::string("Left"); 52 | } 53 | else if(hand == Hand::Right) 54 | { 55 | return std::string("Right"); 56 | } 57 | else 58 | { 59 | mc_rtc::log::error_and_throw("[to_string] Unsupported hand: {}", std::to_string(static_cast(hand))); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/LocomanipController.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace LMC; 11 | 12 | LocomanipController::LocomanipController(mc_rbdyn::RobotModulePtr rm, 13 | double dt, 14 | const mc_rtc::Configuration & _config, 15 | bool allowEmptyManager) 16 | : BWC::BaselineWalkingController(rm, dt, _config, true) 17 | { 18 | // Setup tasks 19 | if(config().has("HandTaskList")) 20 | { 21 | for(const auto & handTaskConfig : config()("HandTaskList")) 22 | { 23 | Hand hand = strToHand(handTaskConfig("hand")); 24 | handTasks_.emplace(hand, 25 | mc_tasks::MetaTaskLoader::load(solver(), handTaskConfig)); 26 | handTasks_.at(hand)->name("HandTask_" + std::to_string(hand)); 27 | } 28 | } 29 | else 30 | { 31 | mc_rtc::log::warning("[LocomanipController] HandTaskList configuration is missing."); 32 | } 33 | 34 | // Setup managers 35 | if(!centroidalManager_) 36 | { 37 | if(config().has("CentroidalManager")) 38 | { 39 | std::string centroidalManagerMethod = config()("CentroidalManager")("method", std::string("")); 40 | if(centroidalManagerMethod == "PreviewControlExtZmp") 41 | { 42 | centroidalManager_ = 43 | std::make_shared(this, config()("CentroidalManager")); 44 | } 45 | else 46 | { 47 | if(!allowEmptyManager) 48 | { 49 | mc_rtc::log::error_and_throw("[LocomanipController] Invalid centroidalManagerMethod: {}.", 50 | centroidalManagerMethod); 51 | } 52 | } 53 | } 54 | else 55 | { 56 | mc_rtc::log::warning("[LocomanipController] CentroidalManager configuration is missing."); 57 | } 58 | } 59 | if(config().has("ManipManager")) 60 | { 61 | manipManager_ = std::make_shared(this, config()("ManipManager")); 62 | } 63 | else 64 | { 65 | mc_rtc::log::warning("[LocomanipController] ManipManager configuration is missing."); 66 | } 67 | 68 | mc_rtc::log::success("[LocomanipController] Constructed."); 69 | } 70 | 71 | void LocomanipController::reset(const mc_control::ControllerResetData & resetData) 72 | { 73 | BaselineWalkingController::reset(resetData); 74 | 75 | mc_rtc::log::success("[LocomanipController] Reset."); 76 | } 77 | 78 | bool LocomanipController::run() 79 | { 80 | t_ += dt(); 81 | 82 | if(enableManagerUpdate_) 83 | { 84 | // Update managers 85 | footManager_->update(); 86 | manipManager_->update(); 87 | centroidalManager_->update(); 88 | } 89 | 90 | return mc_control::fsm::Controller::run(); 91 | } 92 | 93 | void LocomanipController::stop() 94 | { 95 | // Clean up tasks 96 | for(const auto & hand : Hands::Both) 97 | { 98 | solver().removeTask(handTasks_.at(hand)); 99 | } 100 | 101 | // Clean up managers 102 | manipManager_->stop(); 103 | 104 | BaselineWalkingController::stop(); 105 | } 106 | -------------------------------------------------------------------------------- /src/ManipPhase.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace LMC; 8 | using namespace LMC::ManipPhase; 9 | 10 | const LocomanipController & Base::ctl() const 11 | { 12 | return manipManager_->ctl(); 13 | } 14 | 15 | LocomanipController & Base::ctl() 16 | { 17 | return manipManager_->ctl(); 18 | } 19 | 20 | Free::Free(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::Free, hand, manipManager) 21 | { 22 | ctl().solver().removeTask(ctl().handTasks_.at(hand_)); 23 | } 24 | 25 | PreReach::PreReach(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::PreReach, hand, manipManager) 26 | { 27 | // Add hand task 28 | ctl().handTasks_.at(hand_)->reset(); 29 | ctl().solver().addTask(ctl().handTasks_.at(hand_)); 30 | 31 | // Set the end time of stiffness interpolation of hand task 32 | endTime_ = ctl().t() + manipManager_->config().preReachDuration; 33 | } 34 | 35 | void PreReach::run() 36 | { 37 | // Set stiffness of hand task 38 | double remainingDuration = std::max(endTime_ - ctl().t(), 1e-6); 39 | // This line differs from the optimal trajectory formulation, but it empirically gives a smooth IK solution 40 | remainingDuration *= 0.5; 41 | double stiffnessMax = manipManager_->config().handTaskStiffness; 42 | double dampingMax = 2 * std::sqrt(stiffnessMax); 43 | double stiffness = std::min(6.0 / std::pow(remainingDuration, 2), stiffnessMax); 44 | double damping = std::min(4.0 / remainingDuration, dampingMax); 45 | ctl().handTasks_.at(hand_)->setGains(stiffness, damping); 46 | 47 | // Set target pose of hand task 48 | ctl().handTasks_.at(hand_)->targetPose(manipManager_->config().preReachTranss.at(hand_) 49 | * manipManager_->config().objToHandTranss.at(hand_) * ctl().obj().posW()); 50 | } 51 | 52 | bool PreReach::complete() const 53 | { 54 | return endTime_ <= ctl().t(); 55 | } 56 | 57 | std::shared_ptr PreReach::makeNextManipPhase() const 58 | { 59 | return std::make_shared(hand_, manipManager_); 60 | } 61 | 62 | Reach::Reach(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::Reach, hand, manipManager) 63 | { 64 | // Set stiffness of hand task 65 | ctl().handTasks_.at(hand_)->stiffness(manipManager_->config().handTaskStiffness); 66 | 67 | // Setup reaching interpolation 68 | reachingRatioFunc_ = std::make_shared>( 69 | std::map{{ctl().t(), 0.0}, {ctl().t() + manipManager_->config().reachDuration, 1.0}}); 70 | } 71 | 72 | void Reach::run() 73 | { 74 | // Set target pose of hand task 75 | if(reachingRatioFunc_) 76 | { 77 | double reachingRatio = 0.0; 78 | if(ctl().t() < reachingRatioFunc_->endTime()) 79 | { 80 | reachingRatio = (*reachingRatioFunc_)(ctl().t()); 81 | } 82 | else 83 | { 84 | reachingRatio = (*reachingRatioFunc_)(reachingRatioFunc_->endTime()); 85 | reachingRatioFunc_.reset(); 86 | } 87 | ctl().handTasks_.at(hand_)->targetPose( 88 | sva::interpolate(manipManager_->config().preReachTranss.at(hand_), sva::PTransformd::Identity(), reachingRatio) 89 | * manipManager_->config().objToHandTranss.at(hand_) * ctl().obj().posW()); 90 | } 91 | } 92 | 93 | bool Reach::complete() const 94 | { 95 | return !reachingRatioFunc_; 96 | } 97 | 98 | std::shared_ptr Reach::makeNextManipPhase() const 99 | { 100 | if(manipManager_->config().graspCommands.empty()) 101 | { 102 | return std::make_shared(hand_, manipManager_); 103 | } 104 | else 105 | { 106 | return std::make_shared(hand_, manipManager_); 107 | } 108 | } 109 | 110 | Grasp::Grasp(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::Grasp, hand, manipManager) 111 | { 112 | // Send gripper commands 113 | for(const auto & gripperCommandConfig : manipManager_->config().graspCommands) 114 | { 115 | ctl().robot().gripper(gripperCommandConfig("name")).configure(gripperCommandConfig); 116 | } 117 | } 118 | 119 | void Grasp::run() 120 | { 121 | ctl().handTasks_.at(hand_)->targetPose(manipManager_->config().objToHandTranss.at(hand_) * ctl().obj().posW()); 122 | } 123 | 124 | bool Grasp::complete() const 125 | { 126 | for(const auto & gripperCommandConfig : manipManager_->config().graspCommands) 127 | { 128 | if(!ctl().robot().gripper(gripperCommandConfig("name")).complete()) 129 | { 130 | return false; 131 | } 132 | } 133 | return true; 134 | } 135 | 136 | std::shared_ptr Grasp::makeNextManipPhase() const 137 | { 138 | return std::make_shared(hand_, manipManager_); 139 | } 140 | 141 | Hold::Hold(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::Hold, hand, manipManager) {} 142 | 143 | void Hold::run() 144 | { 145 | ctl().handTasks_.at(hand_)->targetPose(manipManager_->config().objToHandTranss.at(hand_) * ctl().obj().posW()); 146 | } 147 | 148 | Ungrasp::Ungrasp(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::Ungrasp, hand, manipManager) 149 | { 150 | // Send gripper commands 151 | for(const auto & gripperCommandConfig : manipManager_->config().ungraspCommands) 152 | { 153 | ctl().robot().gripper(gripperCommandConfig("name")).configure(gripperCommandConfig); 154 | } 155 | } 156 | 157 | void Ungrasp::run() 158 | { 159 | ctl().handTasks_.at(hand_)->targetPose(manipManager_->config().objToHandTranss.at(hand_) * ctl().obj().posW()); 160 | } 161 | 162 | bool Ungrasp::complete() const 163 | { 164 | for(const auto & gripperCommandConfig : manipManager_->config().ungraspCommands) 165 | { 166 | if(!ctl().robot().gripper(gripperCommandConfig("name")).complete()) 167 | { 168 | return false; 169 | } 170 | } 171 | return true; 172 | } 173 | 174 | std::shared_ptr Ungrasp::makeNextManipPhase() const 175 | { 176 | return std::make_shared(hand_, manipManager_); 177 | } 178 | 179 | Release::Release(const Hand & hand, ManipManager * manipManager) : Base(ManipPhaseLabel::Release, hand, manipManager) 180 | { 181 | // Setup reaching interpolation 182 | reachingRatioFunc_ = std::make_shared>( 183 | std::map{{ctl().t(), 1.0}, {ctl().t() + manipManager_->config().reachDuration, 0.0}}); 184 | } 185 | 186 | void Release::run() 187 | { 188 | // Set target pose of hand task 189 | if(reachingRatioFunc_) 190 | { 191 | double reachingRatio = 0.0; 192 | if(ctl().t() < reachingRatioFunc_->endTime()) 193 | { 194 | reachingRatio = (*reachingRatioFunc_)(ctl().t()); 195 | } 196 | else 197 | { 198 | reachingRatio = (*reachingRatioFunc_)(reachingRatioFunc_->endTime()); 199 | reachingRatioFunc_.reset(); 200 | } 201 | ctl().handTasks_.at(hand_)->targetPose( 202 | sva::interpolate(manipManager_->config().preReachTranss.at(hand_), sva::PTransformd::Identity(), reachingRatio) 203 | * manipManager_->config().objToHandTranss.at(hand_) * ctl().obj().posW()); 204 | } 205 | } 206 | 207 | bool Release::complete() const 208 | { 209 | return !reachingRatioFunc_; 210 | } 211 | 212 | std::shared_ptr Release::makeNextManipPhase() const 213 | { 214 | return std::make_shared(hand_, manipManager_); 215 | } 216 | 217 | std::string std::to_string(const ManipPhaseLabel & label) 218 | { 219 | if(label == ManipPhaseLabel::Free) 220 | { 221 | return std::string("Free"); 222 | } 223 | else if(label == ManipPhaseLabel::PreReach) 224 | { 225 | return std::string("PreReach"); 226 | } 227 | else if(label == ManipPhaseLabel::Reach) 228 | { 229 | return std::string("Reach"); 230 | } 231 | else if(label == ManipPhaseLabel::Grasp) 232 | { 233 | return std::string("Grasp"); 234 | } 235 | else if(label == ManipPhaseLabel::Hold) 236 | { 237 | return std::string("Hold"); 238 | } 239 | else if(label == ManipPhaseLabel::Ungrasp) 240 | { 241 | return std::string("Ungrasp"); 242 | } 243 | else if(label == ManipPhaseLabel::Release) 244 | { 245 | return std::string("Release"); 246 | } 247 | else 248 | { 249 | mc_rtc::log::error_and_throw("[to_string] Unsupported manipulation phase label: {}", 250 | std::to_string(static_cast(label))); 251 | } 252 | } 253 | -------------------------------------------------------------------------------- /src/State.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace LMC; 5 | 6 | void State::start(mc_control::fsm::Controller & _ctl) 7 | { 8 | ctlPtr_ = &static_cast(_ctl); 9 | 10 | // Setup PostureTask 11 | // Note that the target of PostureTask is updated to the current position automatically when the state switches 12 | // https://github.com/jrl-umi3218/mc_rtc/blob/b2fe81b4f418c8251d85d3ceee974c0ba7e0610a/src/mc_control/fsm/Executor.cpp#L203 13 | if(ctl().config().has("PostureTask")) 14 | { 15 | auto postureTask = ctl().getPostureTask(ctl().robot().name()); 16 | postureTask->load(ctl().solver(), ctl().config()("PostureTask")); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /src/centroidal/CentroidalManagerPreviewControlExtZmp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace LMC; 13 | 14 | CentroidalManagerPreviewControlExtZmp::CentroidalManagerPreviewControlExtZmp(LocomanipController * ctlPtr, 15 | const mc_rtc::Configuration & mcRtcConfig) 16 | : BWC::CentroidalManager(ctlPtr, mcRtcConfig), LMC::CentroidalManager(ctlPtr, mcRtcConfig), 17 | BWC::CentroidalManagerPreviewControlZmp(ctlPtr, mcRtcConfig) 18 | { 19 | } 20 | 21 | void CentroidalManagerPreviewControlExtZmp::addToLogger(mc_rtc::Logger & logger) 22 | { 23 | CentroidalManagerPreviewControlZmp::addToLogger(logger); 24 | 25 | logger.addLogEntry(config_.name + "_ExtZmp_scale", this, [this]() { return extZmpData_.scale; }); 26 | logger.addLogEntry(config_.name + "_ExtZmp_offset", this, [this]() { return extZmpData_.offset; }); 27 | } 28 | 29 | void CentroidalManagerPreviewControlExtZmp::runMpc() 30 | { 31 | extZmpData_ = calcExtZmpData(ctl().t()); 32 | 33 | // Add hand forces effects 34 | plannedZmp_.head<2>() = extZmpData_.apply(plannedZmp_.head<2>()); 35 | 36 | CentroidalManagerPreviewControlZmp::runMpc(); 37 | 38 | // Remove hand forces effects 39 | plannedZmp_.head<2>() = extZmpData_.applyInv(plannedZmp_.head<2>()); 40 | } 41 | 42 | Eigen::Vector3d CentroidalManagerPreviewControlExtZmp::calcPlannedComAccel() const 43 | { 44 | // Replace plannedZmp_ with plannedExtZmp 45 | Eigen::Vector3d plannedExtZmp; 46 | plannedExtZmp << extZmpData_.apply(plannedZmp_.head<2>()), plannedZmp_.z(); 47 | 48 | Eigen::Vector3d plannedComAccel; 49 | plannedComAccel << plannedForceZ_ / (robotMass_ * (mpcCom_.z() - refZmp_.z())) 50 | * (mpcCom_.head<2>() - plannedExtZmp.head<2>()), 51 | plannedForceZ_ / robotMass_; 52 | plannedComAccel.z() -= CCC::constants::g; 53 | return plannedComAccel; 54 | } 55 | 56 | Eigen::Vector2d CentroidalManagerPreviewControlExtZmp::calcRefData(double t) const 57 | { 58 | Eigen::Vector2d refZmp = CentroidalManagerPreviewControlZmp::calcRefData(t); 59 | ExtZmpData extZmpData = calcExtZmpData(t); 60 | // Add hand forces effects 61 | return extZmpData.apply(refZmp); 62 | } 63 | 64 | CentroidalManagerPreviewControlExtZmp::ExtZmpData CentroidalManagerPreviewControlExtZmp::calcExtZmpData(double t) const 65 | { 66 | ExtZmpData extZmpData; 67 | extZmpData.scale = 0.0; 68 | extZmpData.offset.setZero(); 69 | 70 | Eigen::Vector3d refZmp = ctl().footManager_->calcRefZmp(t); 71 | for(const auto & hand : Hands::Both) 72 | { 73 | if(ctl().manipManager_->manipPhase(hand)->label() != ManipPhaseLabel::Hold) 74 | { 75 | continue; 76 | } 77 | 78 | // Assume that objPoseOffset is constant 79 | sva::PTransformd objPose = ctl().manipManager_->objPoseOffset() * ctl().manipManager_->calcRefObjPose(t); 80 | sva::PTransformd handPose = ctl().manipManager_->config().objToHandTranss.at(hand) * objPose; 81 | // Represent the hand wrench in the frame whose position is same with the hand frame and orientation is same with 82 | // the world frame 83 | sva::ForceVecd handWrenchLocal = ctl().manipManager_->calcRefHandWrench(hand, t); 84 | sva::PTransformd handRotTrans(Eigen::Matrix3d(handPose.rotation())); 85 | sva::ForceVecd handWrench = handRotTrans.transMul(handWrenchLocal); 86 | 87 | const auto & pos = handPose.translation(); 88 | const auto & force = handWrench.force(); 89 | const auto & moment = handWrench.moment(); 90 | 91 | // Equation (3) in the paper: 92 | // M Murooka, et al. Humanoid loco-Manipulations pattern generation and stabilization control. RA-Letters, 2021 93 | extZmpData.scale -= force.z(); 94 | extZmpData.offset.x() += (pos.z() - refZmp.z()) * force.x() - pos.x() * force.z() + moment.y(); 95 | extZmpData.offset.y() += (pos.z() - refZmp.z()) * force.y() - pos.y() * force.z() - moment.x(); 96 | } 97 | 98 | // Ignore the effect of CoM Z acceleration 99 | double mg = robotMass_ * mc_rtc::constants::gravity.z(); 100 | extZmpData.scale /= mg; 101 | extZmpData.scale += 1.0; 102 | extZmpData.offset /= mg; 103 | 104 | return extZmpData; 105 | } 106 | -------------------------------------------------------------------------------- /src/lib.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | CONTROLLER_CONSTRUCTOR("LocomanipController", LMC::LocomanipController) 5 | -------------------------------------------------------------------------------- /src/states/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_fsm_state(InitialState InitialState.cpp) 2 | target_link_libraries(InitialState PUBLIC 3 | ${CONTROLLER_NAME}) 4 | 5 | add_fsm_state(ConfigManipState ConfigManipState.cpp) 6 | target_link_libraries(ConfigManipState PUBLIC 7 | ${CONTROLLER_NAME}) 8 | 9 | add_fsm_state(GuiManipState GuiManipState.cpp) 10 | target_link_libraries(GuiManipState PUBLIC 11 | ${CONTROLLER_NAME}) 12 | 13 | add_fsm_state(TeleopState TeleopState.cpp) 14 | target_link_libraries(TeleopState PUBLIC 15 | ${CONTROLLER_NAME}) 16 | 17 | add_fsm_data_directory(data) 18 | -------------------------------------------------------------------------------- /src/states/ConfigManipState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace LMC; 9 | 10 | void ConfigManipState::start(mc_control::fsm::Controller & _ctl) 11 | { 12 | State::start(_ctl); 13 | 14 | phase_ = 0; 15 | 16 | output("OK"); 17 | } 18 | 19 | bool ConfigManipState::run(mc_control::fsm::Controller &) 20 | { 21 | if(phase_ == 0) 22 | { 23 | if(config_.has("configs") && config_("configs")("preUpdateObj", false)) 24 | { 25 | ctl().manipManager_->appendWaypoint( 26 | Waypoint(ctl().t(), ctl().t() + 1.0, ctl().manipManager_->objPoseOffset().inv() * ctl().realObj().posW())); 27 | phase_ = 1; 28 | } 29 | else 30 | { 31 | phase_ = 2; 32 | } 33 | } 34 | else if(phase_ == 1) 35 | { 36 | if(ctl().manipManager_->waypointQueue().empty()) 37 | { 38 | phase_ = 2; 39 | } 40 | } 41 | else if(phase_ == 2) 42 | { 43 | if(config_.has("configs") && config_("configs")("preWalk", false)) 44 | { 45 | auto convertTo2d = [](const sva::PTransformd & pose) -> Eigen::Vector3d { 46 | return Eigen::Vector3d(pose.translation().x(), pose.translation().y(), 47 | mc_rbdyn::rpyFromMat(pose.rotation()).z()); 48 | }; 49 | const sva::PTransformd & initialFootMidpose = projGround(sva::interpolate( 50 | ctl().footManager_->targetFootPose(Foot::Left), ctl().footManager_->targetFootPose(Foot::Right), 0.5)); 51 | sva::PTransformd objToFootMidTrans = 52 | config_("configs")("objToFootMidTrans", ctl().manipManager_->config().objToFootMidTrans); 53 | ctl().footManager_->walkToRelativePose( 54 | convertTo2d(objToFootMidTrans * ctl().manipManager_->calcRefObjPose(ctl().t()) * initialFootMidpose.inv())); 55 | phase_ = 3; 56 | } 57 | else 58 | { 59 | phase_ = 4; 60 | } 61 | } 62 | else if(phase_ == 3) 63 | { 64 | if(ctl().footManager_->footstepQueue().empty()) 65 | { 66 | phase_ = 4; 67 | } 68 | } 69 | else if(phase_ == 4) 70 | { 71 | bool isReached = (ctl().manipManager_->manipPhase(Hand::Left)->label() == ManipPhaseLabel::Hold 72 | || ctl().manipManager_->manipPhase(Hand::Right)->label() == ManipPhaseLabel::Hold); 73 | if(config_.has("configs") && config_("configs")("reach", !isReached)) 74 | { 75 | ctl().manipManager_->reachHandToObj(); 76 | phase_ = 5; 77 | } 78 | else 79 | { 80 | phase_ = 6; 81 | } 82 | } 83 | else if(phase_ == 5) 84 | { 85 | if(ctl().manipManager_->manipPhase(Hand::Left)->label() == ManipPhaseLabel::Hold 86 | && ctl().manipManager_->manipPhase(Hand::Right)->label() == ManipPhaseLabel::Hold) 87 | { 88 | phase_ = 6; 89 | } 90 | } 91 | else if(phase_ == 6) 92 | { 93 | if(config_.has("configs") && config_("configs").has("preHandWrenches")) 94 | { 95 | for(const auto & handWrenchConfigKV : 96 | static_cast>(config_("configs")("preHandWrenches"))) 97 | { 98 | ctl().manipManager_->setRefHandWrench(strToHand(handWrenchConfigKV.first), handWrenchConfigKV.second, 99 | ctl().t() + 1.0, 1.0); 100 | } 101 | phase_ = 7; 102 | } 103 | else 104 | { 105 | phase_ = 8; 106 | } 107 | } 108 | else if(phase_ == 7) 109 | { 110 | if(!ctl().manipManager_->interpolatingRefHandWrench()) 111 | { 112 | phase_ = 8; 113 | } 114 | } 115 | else if(phase_ == 8) 116 | { 117 | if(config_.has("configs") && config_("configs").has("preObjPoseOffset")) 118 | { 119 | ctl().manipManager_->setObjPoseOffset(config_("configs")("preObjPoseOffset"), 1.0); 120 | phase_ = 9; 121 | } 122 | else 123 | { 124 | phase_ = 10; 125 | } 126 | } 127 | else if(phase_ == 9) 128 | { 129 | if(!ctl().manipManager_->interpolatingObjPoseOffset()) 130 | { 131 | phase_ = 10; 132 | } 133 | } 134 | else if(phase_ == 10) 135 | { 136 | if(config_.has("configs") && config_("configs").has("waypointList")) 137 | { 138 | double startTime = ctl().t(); 139 | sva::PTransformd pose = ctl().manipManager_->calcRefObjPose(ctl().t()); 140 | 141 | for(const auto & waypointConfig : config_("configs")("waypointList")) 142 | { 143 | if(waypointConfig.has("startTime")) 144 | { 145 | startTime = ctl().t() + static_cast(waypointConfig("startTime")); 146 | } 147 | double endTime; 148 | if(waypointConfig.has("endTime")) 149 | { 150 | endTime = ctl().t() + static_cast(waypointConfig("endTime")); 151 | } 152 | else if(waypointConfig.has("duration")) 153 | { 154 | endTime = startTime + static_cast(waypointConfig("duration")); 155 | } 156 | if(waypointConfig.has("pose")) 157 | { 158 | pose = waypointConfig("pose"); 159 | } 160 | else if(waypointConfig.has("relPose")) 161 | { 162 | pose = static_cast(waypointConfig("relPose")) * pose; 163 | } 164 | ctl().manipManager_->appendWaypoint( 165 | Waypoint(startTime, endTime, pose, waypointConfig("config", mc_rtc::Configuration()))); 166 | 167 | startTime = endTime; 168 | } 169 | 170 | if(config_("configs")("footstep", true)) 171 | { 172 | ctl().manipManager_->requireFootstepFollowingObj(); 173 | } 174 | 175 | phase_ = 11; 176 | } 177 | else if(config_.has("configs") && config_("configs").has("velocityMode")) 178 | { 179 | ctl().manipManager_->startVelMode(); 180 | ctl().manipManager_->setRelativeVel(config_("configs")("velocityMode")("velocity")); 181 | velModeEndTime_ = ctl().t() + static_cast(config_("configs")("velocityMode")("duration")); 182 | 183 | phase_ = 11; 184 | } 185 | else 186 | { 187 | phase_ = 12; 188 | } 189 | } 190 | else if(phase_ == 11) 191 | { 192 | if(config_.has("configs") && config_("configs").has("velocityMode")) 193 | { 194 | if(ctl().t() > velModeEndTime_ - 1.0 && ctl().manipManager_->velModeEnabled()) 195 | { 196 | ctl().manipManager_->setRelativeVel(Eigen::Vector3d::Zero()); 197 | } 198 | if(ctl().t() > velModeEndTime_ && ctl().manipManager_->velModeEnabled()) 199 | { 200 | ctl().manipManager_->endVelMode(); 201 | } 202 | } 203 | 204 | if(ctl().manipManager_->waypointQueue().empty() && ctl().footManager_->footstepQueue().empty() 205 | && !ctl().manipManager_->velModeEnabled()) 206 | { 207 | phase_ = 12; 208 | } 209 | } 210 | else if(phase_ == 12) 211 | { 212 | if(config_.has("configs") && config_("configs").has("postObjPoseOffset")) 213 | { 214 | ctl().manipManager_->setObjPoseOffset(config_("configs")("postObjPoseOffset"), 1.0); 215 | phase_ = 13; 216 | } 217 | else 218 | { 219 | phase_ = 14; 220 | } 221 | } 222 | else if(phase_ == 13) 223 | { 224 | if(!ctl().manipManager_->interpolatingObjPoseOffset()) 225 | { 226 | phase_ = 14; 227 | } 228 | } 229 | else if(phase_ == 14) 230 | { 231 | if(config_.has("configs") && config_("configs").has("postHandWrenches")) 232 | { 233 | for(const auto & handWrenchConfigKV : 234 | static_cast>(config_("configs")("postHandWrenches"))) 235 | { 236 | ctl().manipManager_->setRefHandWrench(strToHand(handWrenchConfigKV.first), handWrenchConfigKV.second, 237 | ctl().t() + 1.0, 1.0); 238 | } 239 | phase_ = 15; 240 | } 241 | else 242 | { 243 | phase_ = 16; 244 | } 245 | } 246 | else if(phase_ == 15) 247 | { 248 | if(!ctl().manipManager_->interpolatingRefHandWrench()) 249 | { 250 | phase_ = 16; 251 | } 252 | } 253 | else if(phase_ == 16) 254 | { 255 | if(config_.has("configs") && config_("configs")("release", true)) 256 | { 257 | ctl().manipManager_->releaseHandFromObj(); 258 | phase_ = 17; 259 | } 260 | else 261 | { 262 | phase_ = 18; 263 | } 264 | } 265 | else if(phase_ == 17) 266 | { 267 | if(ctl().manipManager_->manipPhase(Hand::Left)->label() == ManipPhaseLabel::Free 268 | && ctl().manipManager_->manipPhase(Hand::Right)->label() == ManipPhaseLabel::Free) 269 | { 270 | phase_ = 18; 271 | } 272 | } 273 | 274 | return phase_ == 18; 275 | } 276 | 277 | void ConfigManipState::teardown(mc_control::fsm::Controller &) {} 278 | 279 | EXPORT_SINGLE_STATE("LMC::ConfigManip", ConfigManipState) 280 | -------------------------------------------------------------------------------- /src/states/GuiManipState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace LMC; 13 | 14 | void GuiManipState::start(mc_control::fsm::Controller & _ctl) 15 | { 16 | State::start(_ctl); 17 | 18 | // Setup GUI 19 | ctl().gui()->addElement({ctl().name(), "GuiManip"}, 20 | mc_rtc::gui::Button("Reach", [this]() { ctl().manipManager_->reachHandToObj(); }), 21 | mc_rtc::gui::Button("Release", [this]() { ctl().manipManager_->releaseHandFromObj(); }), 22 | mc_rtc::gui::Button("StopManip", [this]() { ctl().manipManager_->clearWaypointQueue(); })); 23 | ctl().gui()->addElement( 24 | {ctl().name(), "GuiManip", "WalkToObj"}, 25 | mc_rtc::gui::Form( 26 | "WalkToObj", 27 | [this](const mc_rtc::Configuration & config) { 28 | auto convertTo2d = [](const sva::PTransformd & pose) -> Eigen::Vector3d { 29 | return Eigen::Vector3d(pose.translation().x(), pose.translation().y(), 30 | mc_rbdyn::rpyFromMat(pose.rotation()).z()); 31 | }; 32 | auto convertTo3d = [](const Eigen::Vector3d & trans) -> sva::PTransformd { 33 | return sva::PTransformd(sva::RotZ(trans.z()), Eigen::Vector3d(trans.x(), trans.y(), 0)); 34 | }; 35 | const sva::PTransformd & initialFootMidpose = projGround(sva::interpolate( 36 | ctl().footManager_->targetFootPose(Foot::Left), ctl().footManager_->targetFootPose(Foot::Right), 0.5)); 37 | sva::PTransformd objToFootMidTrans = 38 | convertTo3d(Eigen::Vector3d(config(walkToObjConfigKeys_.at("x")), config(walkToObjConfigKeys_.at("y")), 39 | mc_rtc::constants::toRad(config(walkToObjConfigKeys_.at("yaw"))))); 40 | ctl().footManager_->walkToRelativePose(convertTo2d( 41 | objToFootMidTrans * ctl().manipManager_->calcRefObjPose(ctl().t()) * initialFootMidpose.inv())); 42 | }, 43 | mc_rtc::gui::FormNumberInput(walkToObjConfigKeys_.at("x"), true, 44 | ctl().manipManager_->config().objToFootMidTrans.translation().x()), 45 | mc_rtc::gui::FormNumberInput(walkToObjConfigKeys_.at("y"), true, 46 | ctl().manipManager_->config().objToFootMidTrans.translation().y()), 47 | mc_rtc::gui::FormNumberInput( 48 | walkToObjConfigKeys_.at("yaw"), true, 49 | mc_rtc::constants::toDeg( 50 | mc_rbdyn::rpyFromMat(ctl().manipManager_->config().objToFootMidTrans.rotation()).z())))); 51 | ctl().gui()->addElement( 52 | {ctl().name(), "GuiManip", "MoveObj"}, 53 | mc_rtc::gui::Form( 54 | "MoveObj", 55 | [this](const mc_rtc::Configuration & config) { 56 | if(!(ctl().manipManager_->manipPhase(Hand::Left)->label() == ManipPhaseLabel::Hold 57 | || ctl().manipManager_->manipPhase(Hand::Right)->label() == ManipPhaseLabel::Hold)) 58 | { 59 | mc_rtc::log::error("[GuiManipState] \"MoveObj\" command is available only when the manipulation " 60 | "phase is Hold. Left: {}, Right: {}", 61 | std::to_string(ctl().manipManager_->manipPhase(Hand::Left)->label()), 62 | std::to_string(ctl().manipManager_->manipPhase(Hand::Right)->label())); 63 | return; 64 | } 65 | if(!ctl().manipManager_->waypointQueue().empty()) 66 | { 67 | mc_rtc::log::error( 68 | "[GuiManipState] \"MoveObj\" command is available only when the waypoint queue is empty: {}", 69 | ctl().manipManager_->waypointQueue().size()); 70 | return; 71 | } 72 | if(!ctl().footManager_->footstepQueue().empty()) 73 | { 74 | mc_rtc::log::error( 75 | "[GuiManipState] \"MoveObj\" command is available only when the footstep queue is empty: {}", 76 | ctl().footManager_->footstepQueue().size()); 77 | return; 78 | } 79 | double startTime = ctl().t() + static_cast(config(moveObjConfigKeys_.at("startTime"))); 80 | double endTime = ctl().t() + static_cast(config(moveObjConfigKeys_.at("endTime"))); 81 | sva::PTransformd pose = 82 | sva::PTransformd(sva::RotZ(mc_rtc::constants::toRad(config(moveObjConfigKeys_.at("yaw")))), 83 | Eigen::Vector3d(config(moveObjConfigKeys_.at("x")), 0.0, 0.0)) 84 | * ctl().manipManager_->calcRefObjPose(ctl().t()); 85 | ctl().manipManager_->appendWaypoint(Waypoint(startTime, endTime, pose)); 86 | if(config(moveObjConfigKeys_.at("footstep"))) 87 | { 88 | ctl().manipManager_->requireFootstepFollowingObj(); 89 | } 90 | }, 91 | mc_rtc::gui::FormNumberInput(moveObjConfigKeys_.at("x"), true, 0.0), 92 | mc_rtc::gui::FormNumberInput(moveObjConfigKeys_.at("yaw"), true, 0.0), 93 | mc_rtc::gui::FormNumberInput(moveObjConfigKeys_.at("startTime"), true, 2.0), 94 | mc_rtc::gui::FormNumberInput(moveObjConfigKeys_.at("endTime"), true, 12.0), 95 | mc_rtc::gui::FormCheckbox(moveObjConfigKeys_.at("footstep"), true, true))); 96 | ctl().gui()->addElement( 97 | {ctl().name(), "GuiManip", "UpdateObj"}, 98 | mc_rtc::gui::Form( 99 | "UpdateObj", 100 | [this](const mc_rtc::Configuration & config) { 101 | if(ctl().manipManager_->waypointQueue().size() > 0) 102 | { 103 | mc_rtc::log::error("[GuiManipState] \"UpdateObj\" command is available only when the waypoint " 104 | "queue is empty: {}", 105 | ctl().manipManager_->waypointQueue().size()); 106 | return; 107 | } 108 | double startTime = ctl().t(); 109 | double endTime = ctl().t() + static_cast(config(updateObjConfigKeys_.at("interpDuration"))); 110 | sva::PTransformd pose; 111 | if(config(updateObjConfigKeys_.at("target")) == "real") 112 | { 113 | pose = ctl().manipManager_->objPoseOffset().inv() * ctl().realObj().posW(); 114 | } 115 | else if(config(updateObjConfigKeys_.at("target")) == "nominal") 116 | { 117 | const sva::PTransformd & footMidpose = 118 | projGround(sva::interpolate(ctl().footManager_->targetFootPose(Foot::Left), 119 | ctl().footManager_->targetFootPose(Foot::Right), 0.5)); 120 | pose = ctl().manipManager_->config().objToFootMidTrans.inv() * footMidpose; 121 | } 122 | else 123 | { 124 | mc_rtc::log::error("[GuiManipState] Invalid target in \"UpdateObj\": {}", 125 | config(updateObjConfigKeys_.at("target"))); 126 | return; 127 | } 128 | ctl().manipManager_->appendWaypoint(Waypoint(startTime, endTime, pose)); 129 | }, 130 | mc_rtc::gui::FormComboInput(updateObjConfigKeys_.at("target"), true, {"real", "nominal"}, false, 0), 131 | mc_rtc::gui::FormNumberInput(updateObjConfigKeys_.at("interpDuration"), true, 1.0))); 132 | ctl().gui()->addElement( 133 | {ctl().name(), "GuiManip", "PoseOffset"}, 134 | mc_rtc::gui::Form( 135 | "PoseOffset", 136 | [this](const mc_rtc::Configuration & config) { 137 | Eigen::Vector3d rpy = config(poseOffsetConfigKeys_.at("rpy")); 138 | ctl().manipManager_->setObjPoseOffset( 139 | sva::PTransformd(mc_rbdyn::rpyToMat(rpy.unaryExpr(&mc_rtc::constants::toRad)), 140 | config(poseOffsetConfigKeys_.at("xyz"))), 141 | config(poseOffsetConfigKeys_.at("interpDuration"))); 142 | }, 143 | mc_rtc::gui::FormArrayInput(poseOffsetConfigKeys_.at("xyz"), true, Eigen::Vector3d::Zero()), 144 | mc_rtc::gui::FormArrayInput(poseOffsetConfigKeys_.at("rpy"), true, Eigen::Vector3d::Zero()), 145 | mc_rtc::gui::FormNumberInput(poseOffsetConfigKeys_.at("interpDuration"), true, 1.0))); 146 | 147 | output("OK"); 148 | } 149 | 150 | bool GuiManipState::run(mc_control::fsm::Controller &) 151 | { 152 | return false; 153 | } 154 | 155 | void GuiManipState::teardown(mc_control::fsm::Controller &) 156 | { 157 | // Clean up GUI 158 | ctl().gui()->removeCategory({ctl().name(), "GuiManip"}); 159 | } 160 | 161 | EXPORT_SINGLE_STATE("LMC::GuiManip", GuiManipState) 162 | -------------------------------------------------------------------------------- /src/states/InitialState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace LMC; 13 | 14 | void InitialState::start(mc_control::fsm::Controller & _ctl) 15 | { 16 | State::start(_ctl); 17 | 18 | phase_ = 0; 19 | 20 | // Setup GUI 21 | ctl().gui()->addElement({ctl().name()}, mc_rtc::gui::Button("Start", [this]() { phase_ = 1; })); 22 | 23 | output("OK"); 24 | } 25 | 26 | bool InitialState::run(mc_control::fsm::Controller &) 27 | { 28 | if(phase_ == 0) 29 | { 30 | // Auto start 31 | if(config_.has("configs") && config_("configs").has("autoStartTime") 32 | && ctl().t() > static_cast(config_("configs")("autoStartTime"))) 33 | { 34 | phase_ = 1; 35 | } 36 | } 37 | if(phase_ == 1) 38 | { 39 | phase_ = 2; 40 | 41 | // Clean up GUI 42 | ctl().gui()->removeElement({ctl().name()}, "Start"); 43 | 44 | // Reset and add tasks 45 | ctl().comTask_->reset(); 46 | ctl().solver().addTask(ctl().comTask_); 47 | ctl().baseOriTask_->reset(); 48 | ctl().solver().addTask(ctl().baseOriTask_); 49 | for(const auto & foot : Feet::Both) 50 | { 51 | ctl().footTasks_.at(foot)->reset(); 52 | ctl().solver().addTask(ctl().footTasks_.at(foot)); 53 | } 54 | 55 | // Setup task stiffness interpolation 56 | comTaskStiffness_ = ctl().comTask_->dimStiffness(); 57 | baseOriTaskStiffness_ = ctl().baseOriTask_->dimStiffness(); 58 | for(const auto & foot : Feet::Both) 59 | { 60 | footTasksStiffness_.emplace(foot, ctl().footTasks_.at(foot)->dimStiffness()); 61 | } 62 | constexpr double stiffnessInterpDuration = 1.0; // [sec] 63 | stiffnessRatioFunc_ = std::make_shared>( 64 | std::map{{ctl().t(), 0.0}, {ctl().t() + stiffnessInterpDuration, 1.0}}); 65 | 66 | // Reset managers 67 | ctl().manipManager_->reset(); 68 | ctl().footManager_->reset(); 69 | ctl().centroidalManager_->reset(); 70 | ctl().enableManagerUpdate_ = true; 71 | 72 | // Setup anchor frame 73 | ctl().centroidalManager_->setAnchorFrame(); 74 | 75 | // Send commands to gripper 76 | if(config_.has("configs") && config_("configs").has("gripperCommands")) 77 | { 78 | for(const auto & gripperCommandConfig : config_("configs")("gripperCommands")) 79 | { 80 | ctl().robot().gripper(gripperCommandConfig("name")).configure(gripperCommandConfig); 81 | } 82 | } 83 | 84 | // Add GUI of managers 85 | ctl().manipManager_->addToGUI(*ctl().gui()); 86 | ctl().footManager_->addToGUI(*ctl().gui()); 87 | ctl().centroidalManager_->addToGUI(*ctl().gui()); 88 | } 89 | else if(phase_ == 2) 90 | { 91 | phase_ = 3; 92 | 93 | // Add logger of managers 94 | // Considering the possibility that logger entries assume that variables are set in the manager's update method, 95 | // it is safe to call the update method once and then add the logger 96 | ctl().manipManager_->addToLogger(ctl().logger()); 97 | ctl().footManager_->addToLogger(ctl().logger()); 98 | ctl().centroidalManager_->addToLogger(ctl().logger()); 99 | } 100 | 101 | // Interpolate task stiffness 102 | if(stiffnessRatioFunc_) 103 | { 104 | if(ctl().t() <= stiffnessRatioFunc_->endTime()) 105 | { 106 | double stiffnessRatio = (*stiffnessRatioFunc_)(ctl().t()); 107 | ctl().comTask_->stiffness(stiffnessRatio * comTaskStiffness_); 108 | ctl().baseOriTask_->stiffness(stiffnessRatio * baseOriTaskStiffness_); 109 | for(const auto & foot : Feet::Both) 110 | { 111 | ctl().footTasks_.at(foot)->stiffness(stiffnessRatio * footTasksStiffness_.at(foot)); 112 | } 113 | } 114 | else 115 | { 116 | stiffnessRatioFunc_.reset(); 117 | } 118 | } 119 | 120 | return complete(); 121 | } 122 | 123 | void InitialState::teardown(mc_control::fsm::Controller &) {} 124 | 125 | bool InitialState::complete() const 126 | { 127 | if(phase_ != 3) 128 | { 129 | return false; 130 | } 131 | 132 | if(stiffnessRatioFunc_) 133 | { 134 | return false; 135 | } 136 | 137 | if(config_.has("configs") && config_("configs").has("gripperCommands")) 138 | { 139 | for(const auto & gripperCommandConfig : config_("configs")("gripperCommands")) 140 | { 141 | if(!ctl().robot().gripper(gripperCommandConfig("name")).complete()) 142 | { 143 | return false; 144 | } 145 | } 146 | } 147 | 148 | return true; 149 | } 150 | 151 | EXPORT_SINGLE_STATE("LMC::Initial", InitialState) 152 | -------------------------------------------------------------------------------- /src/states/TeleopState.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace LMC; 9 | 10 | void TeleopState::start(mc_control::fsm::Controller & _ctl) 11 | { 12 | State::start(_ctl); 13 | 14 | // Skip if ROS is not initialized 15 | if(!mc_rtc::ROSBridge::get_node_handle()) 16 | { 17 | mc_rtc::log::warning("[TeleopState] ROS is not initialized."); 18 | output("OK"); 19 | return; 20 | } 21 | 22 | // Load configuration 23 | std::string twistTopicName = "/cmd_vel"; 24 | if(config_.has("configs")) 25 | { 26 | if(config_("configs").has("velScale")) 27 | { 28 | velScale_ = config_("configs")("velScale"); 29 | velScale_[2] = mc_rtc::constants::toRad(velScale_[2]); 30 | } 31 | config_("configs")("twistTopicName", twistTopicName); 32 | } 33 | 34 | // Setup ROS 35 | nh_ = std::make_unique(); 36 | // Use a dedicated queue so as not to call callbacks of other modules 37 | nh_->setCallbackQueue(&callbackQueue_); 38 | twistSub_ = nh_->subscribe(twistTopicName, 1, &TeleopState::twistCallback, this); 39 | 40 | // Setup GUI 41 | ctl().gui()->addElement({ctl().name(), "Teleop"}, 42 | mc_rtc::gui::Button("StartTeleop", [this]() { ctl().manipManager_->startVelMode(); })); 43 | ctl().gui()->addElement({ctl().name(), "Teleop", "State"}, 44 | mc_rtc::gui::ArrayInput( 45 | "targetVel", {"x", "y", "theta"}, 46 | [this]() -> Eigen::Vector3d { 47 | return Eigen::Vector3d(targetVel_[0], targetVel_[1], 48 | mc_rtc::constants::toDeg(targetVel_[2])); 49 | }, 50 | [this](const Eigen::Vector3d & v) { 51 | targetVel_ = Eigen::Vector3d(v[0], v[1], mc_rtc::constants::toRad(v[2])); 52 | })); 53 | 54 | output("OK"); 55 | } 56 | 57 | bool TeleopState::run(mc_control::fsm::Controller &) 58 | { 59 | // Finish if ROS is not initialized 60 | if(!mc_rtc::ROSBridge::get_node_handle()) 61 | { 62 | return true; 63 | } 64 | 65 | // Call ROS callback 66 | callbackQueue_.callAvailable(ros::WallDuration()); 67 | 68 | // Update GUI 69 | bool velMode = ctl().manipManager_->velModeEnabled(); 70 | if(velMode && ctl().gui()->hasElement({ctl().name(), "Teleop"}, "StartTeleop")) 71 | { 72 | ctl().gui()->addElement({ctl().name(), "Teleop"}, 73 | mc_rtc::gui::Button("EndTeleop", [this]() { ctl().manipManager_->endVelMode(); })); 74 | ctl().gui()->removeElement({ctl().name(), "Teleop"}, "StartTeleop"); 75 | } 76 | else if(!velMode && ctl().gui()->hasElement({ctl().name(), "Teleop"}, "EndTeleop")) 77 | { 78 | ctl().gui()->addElement({ctl().name(), "Teleop"}, 79 | mc_rtc::gui::Button("StartTeleop", [this]() { ctl().manipManager_->startVelMode(); })); 80 | ctl().gui()->removeElement({ctl().name(), "Teleop"}, "EndTeleop"); 81 | } 82 | 83 | // Set target velocity 84 | if(ctl().manipManager_->velModeEnabled()) 85 | { 86 | ctl().manipManager_->setRelativeVel(targetVel_); 87 | } 88 | 89 | return false; 90 | } 91 | 92 | void TeleopState::teardown(mc_control::fsm::Controller &) 93 | { 94 | // Clean up GUI 95 | ctl().gui()->removeCategory({ctl().name(), "Teleop"}); 96 | } 97 | 98 | void TeleopState::twistCallback(const geometry_msgs::Twist::ConstPtr & twistMsg) 99 | { 100 | targetVel_ = velScale_.cwiseProduct(Eigen::Vector3d(twistMsg->linear.x, twistMsg->linear.y, twistMsg->angular.z)); 101 | } 102 | 103 | EXPORT_SINGLE_STATE("LMC::Teleop", TeleopState) 104 | -------------------------------------------------------------------------------- /src/states/data/states.json: -------------------------------------------------------------------------------- 1 | { 2 | } -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/LocomanipController/ee25576904f3e225c6004afdd836914fc75c2ef8/tests/CMakeLists.txt --------------------------------------------------------------------------------