Skip to content

Instantly share code, notes, and snippets.

@mmurooka

mmurooka/HVAC2023_Lecture_Murooka_20230924.md

Last active September 25, 2023 05:59
Show Gist options
  • Star 8 You must be signed in to star a gist
  • Fork 2 You must be signed in to fork a gist
  • Save mmurooka/32324ade4ce07d49ae4810c74b235f02 to your computer and use it in GitHub Desktop.
Save mmurooka/32324ade4ce07d49ae4810c74b235f02 to your computer and use it in GitHub Desktop.
Download ZIP
Humanoid Virtual Athletics Challenge 2023 技術講習会
Raw
HVAC2023_Lecture_Murooka_20230924.md

mc_rtcを用いた人型ロボットのコントローラ作成

本ドキュメントは，Humanoid Virtual Athletics Challenge 2023 技術講習会 (2023/09/24)で利用する説明資料です．

０．HVACにおけるmc_rtcの利用例

2021年出場時のコントローラはmc_rtcを利用しています． 不整地歩行，階段登り，ジャンプなどの動作を実行できました．

１．ロボットの歩行コントローラ BaselineWalkingControllerを試す

やり方１：作成済みのDockerイメージを実行してみる

参考：https://github.com/isri-aist/BaselineWalkingController#quick-trial-on-docker

docker pull ghcr.io/isri-aist/baseline_walking_controller:latest

xhost +local:

docker run --gpus all --rm -it \
  --env="DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
  ghcr.io/isri-aist/baseline_walking_controller:latest ./walk_on_plane.bash

docker run --gpus all --rm -it \
  --env="DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
  ghcr.io/isri-aist/baseline_walking_controller:latest ./walk_on_stairs.bash

やり方２：ローカルにインストールする

既存環境を壊さないように，Docker上でやることをおすすめします． 実行環境はUbuntu 20.04を想定します．

Dockerコンテナ起動

xhost +local:
docker run --gpus all -it --env="DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" ubuntu:20.04

参考：途中でコンテナを終了させた場合の再起動手順．

docker start <CONTAINER ID>
docker exec -it <CONTAINER ID> /bin/bash

フレームワークのインストール

# 基本パッケージをインストール
apt-get update
apt-get install -y sudo aptitude build-essential lsb-release wget gnupg2 curl emacs tmux
aptitude update

# ROSをインストール
export ROS_DISTRO=noetic
sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
wget http://packages.ros.org/ros.key -O - | apt-key add -
apt-get update
apt-get install -y ros-${ROS_DISTRO}-ros-base python3-catkin-tools python3-rosdep python3-wstool python3-rosinstall python3-rosinstall-generator doxygen graphviz

# mc_rtcをインストール
curl -1sLf 'https://dl.cloudsmith.io/public/mc-rtc/stable/setup.deb.sh' | bash
apt-get install -y libmc-rtc-dev mc-rtc-utils ros-${ROS_DISTRO}-mc-rtc-plugin ros-${ROS_DISTRO}-mc-rtc-rviz-panel libeigen-qld-dev mc-state-observation

コントローラのインストール

# catkinワークスペースのセットアップ
mkdir -p ${HOME}/catkin_ws/src
cd ${HOME}/catkin_ws
wstool init src
wstool set -t src isri-aist/BaselineWalkingController https://github.com/isri-aist/BaselineWalkingController --git -y
wstool update -t src isri-aist/BaselineWalkingController
wstool merge -t src src/isri-aist/BaselineWalkingController/depends.rosinstall
wstool update -t src

# ROS依存パッケージのインストール
source /opt/ros/${ROS_DISTRO}/setup.bash
rosdep init
rosdep update
rosdep install -y -r --from-paths src --ignore-src

# catkinワークスペースのビルド
source /opt/ros/${ROS_DISTRO}/setup.bash
catkin build baseline_walking_controller -DCMAKE_BUILD_TYPE=RelWithDebInfo -DENABLE_QLD=ON

シミュレーション

# シミュレーションをインストール
# 注意：既にインストールされているChoreonoidと干渉する可能性あり．Docker等の仮想環境での実行を奨励．
apt-get install -y jvrc-choreonoid

# シミュレーションのセットアップ
mkdir -p ${HOME}/.config/mc_rtc/controllers
cp ${HOME}/catkin_ws/src/isri-aist/BaselineWalkingController/etc/mc_rtc.yaml ${HOME}/.config/mc_rtc/mc_rtc.yaml

# シミュレーションを実行
# 複数のプロセスを起動するため，`tmux`上で実行する．

# ターミナル１：
source ${HOME}/catkin_ws/devel/setup.bash
roscore

# ターミナル２：
source ${HOME}/catkin_ws/devel/setup.bash
/usr/share/hrpsys/samples/JVRC1/clear-omninames.sh
cd /usr/share/hrpsys/samples/JVRC1
choreonoid --start-simulation sim_mc.cnoid

# ターミナル３：
source ${HOME}/catkin_ws/devel/setup.bash
roslaunch baseline_walking_controller display.launch

# Rvizを操作して歩行動作を実行する．
# 1. Rvizの"BWC"タブで，"Start"ボタンをクリック.
# 2．Rvizの"BWC"->"GuiWalk"タブで，ゴール位置(x, y, theta)をセットし”Walk”ボタンをクリック．

事前に与えたフットステップ列の通りにロボットを歩かせるには，以下のように設定ファイルを作成する．

cp ${HOME}/catkin_ws/src/isri-aist/BaselineWalkingController/.github/workflows/config/WalkingOnPlane.yaml ${HOME}/.config/mc_rtc/controllers/BaselineWalkingController.yaml

フットステップ列を変えるには，${HOME}/.config/mc_rtc/controllers/BaselineWalkingController.yamlのfootstepListを編集する．

参考：GitHub上で変更が加わるたびに，CIで同様のプロセスが実行され，Dockerイメージがリリースされています．

参考：制御手法の概要は，Humanoids 2022 Workshopのスライドを参照．

２．mc_rtcの概説

参考：https://jrl-umi3218.github.io/mc_rtc/jp/tutorials.html

  • mc_rtcは以下のライブラリを統合してロボットコントローラを構築するためのフレームワーク．

    • 三次元空間での運動を扱うための数学・幾何学ライブラリ：SpaceVecAlg
    • ロボットモデリング・運動学・動力学計算ライブラリ：RBDyn
    • 逆運動学計算ライブラリ：Tasks
    • SpaceVecAlgとRBDynの入門資料としては，sva_rbdyn_presentationとsva_rbdyn_tutorialsが参考になる．
    • その他に，インターフェース，可視化，ログなどの機能を含む．

  • インストール手順：aptによるインストールが推奨だが，mc-rtc-superbuildを利用してソースからのインストールも可能．

  • インターフェース：Chorenoid，MuJoCo，V-REPなどのシミュレータで実行可能．ChoreonoidとはOpenRTMプラグインを経由して接続．ROS環境がある場合は可視化インターフェースとしてRvizを利用．

  • ロボット：HRPヒューマノイド，Nao，Pepper，Fetch，UR5e，Sawyer，Pandaの実機で動作実績あり．

  • オンラインデモ：ブラウザ上で動かしてみることができる．

３．自分でmc_rtcのコントローラを作って動かしてみる

参考：https://jrl-umi3218.github.io/mc_rtc/jp/tutorials/introduction/first-controller.html

https://github.com/mmurooka/HVACTutorialController に，同じコントローラのソースコードを公開．

実行環境はUbuntu 20.04を想定します．

コントローラのテンプレートを作成し実行

事前にフレームワークのインストールに記載の手順でROSとmc_rtcをインストールしておく．

# コントローラのテンプレートを作成
mkdir -p ${HOME}/src
cd ${HOME}/src
# mc_rtc_new_controller <ディレクトリ> <コントローラ名>
mc_rtc_new_controller HVACTutorialController HVACTutorialController

# コントローラをビルド・インストール
cd HVACTutorialController
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
make
make install

# コントローラのセットアップ
mkdir -p ${HOME}/.config/mc_rtc

${HOME}/.config/mc_rtc/mc_rtc.yamlに以下を記載．

MainRobot: JVRC1
Enabled: HVACTutorialController
# コントローラの実行
# ターミナル１：
roscore

# ターミナル２：
rosrun mc_rtc_ticker mc_rtc_ticker

# ターミナル３：
roslaunch mc_rtc_ticker display.launch

コントローラの機能を追加

首を振る（関節角度を直接指令する）

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/6832ff71e3978efaf9bceed267606c4adccd2425

// Update the target angle of the neck joint
postureTask->target({{"NECK_Y", {mc_rtc::constants::toRad(45) * std::sin(0.5 * t)}}});

手を軌道に沿って動かす（タスク空間の位置を指令する）

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/9081b7e03a6331ac1b0fc922a3babeac323329bd

// Construct hand task
handTask = std::make_shared<mc_tasks::TransformTask>("LeftGripper", robots(), 0, 100.0, 500.0);

// <略>

// Update the target pose of the hand
handTask->target(
    sva::PTransformd(sva::RotX(mc_rtc::constants::PI),
                     Eigen::Vector3d(0.4 + 0.2 * std::cos(t), 0.2 + 0.2 * std::sin(t), 0.8)));

足を固定する

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/db2956adf707281b288e8700b5eaeb2b8a320c10

  // Construct foot tasks
  footTasks[0] = std::make_shared<mc_tasks::TransformTask>("LeftFoot", robots(), 0, 1000.0, 500.0);
  footTasks[1] = std::make_shared<mc_tasks::TransformTask>("RightFoot", robots(), 0, 1000.0, 500.0);

重心を制御してバランスをとる

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/69a7dff49cdbed512f894d6fd48290aed2ceb2d5

// Construct CoM task
comTask = std::make_shared<mc_tasks::CoMTask>(robots(), 0, 1000.0, 500.0);

// <略>

// Update the target position of the CoM
comTask->com(Eigen::Vector3d(0.0, 0.0, 0.8));

ベースリンクが傾かないようにする

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/e7166504ffb47426edb03710b97d616f230b9b42

// Construct base task
baseTask = std::make_shared<mc_tasks::OrientationTask>("WAIST_R_S", robots(), 0, 50.0, 30.0);

自己干渉を回避する

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/cd640e960db591bbc2616e7ea5e0391a2d585db9

solver().addConstraintSet(selfCollisionConstraint);

ボタンで指示を送る

参考：https://jrl-umi3218.github.io/mc_rtc/jp/tutorials/usage/gui.html
変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/81e928138138faea12c3cdd3fc39d5df21313be9

// Add buttons to move the CoM
gui()->addElement(
    {"HVACTutorialController"},
    mc_rtc::gui::Button("Move CoM to the left", [this]() { comFlag = 1; }),
    mc_rtc::gui::Button("Move CoM to the center", [this]() { comFlag = 0; }),
    mc_rtc::gui::Button("Move CoM to the right", [this]() { comFlag = -1; }));

// <略>

// Update the target position of the CoM
comTask->com(Eigen::Vector3d(0.0, static_cast<double>(comFlag) * 0.1, 0.8));

ログを確認する

参考：https://jrl-umi3218.github.io/mc_rtc/jp/tutorials/usage/logging.html
　　　https://jrl-umi3218.github.io/mc_rtc/jp/tutorials/tools/mc_log_ui.html
変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/45ba367fcbd1b70731dbe2d837b9d8770b62c330

// Add the sample log entry
logger().addLogEntry("sampleEntry", this, [this]() { return std::sin(t) + std::cos(0.4 * t); });
mc_log_ui /tmp/mc-control-HVACTutorialController-latest.bin

Choreonoid上で実行する

変更箇所：https://github.com/mmurooka/HVACTutorialController/commit/4f46b13c5da2c9c2c333ba1b44a52ae590c01f52

重心の目標位置が両足の真ん中になるようにする．

// Update the target position of the CoM
Eigen::Vector3d footCenter = 0.5 * (footTasks[0]->target().translation() + footTasks[1]->target().translation());
comTask->com(footCenter + Eigen::Vector3d(0.0, static_cast<double>(comFlag) * 0.1, 0.8));

mc_rtc_tickerの代わりにChoreonoidを実行する．RvizはChoreonoidでもユーザーインターフェースとして利用する．

/usr/share/hrpsys/samples/JVRC1/clear-omninames.sh
cd /usr/share/hrpsys/samples/JVRC1
choreonoid --start-simulation sim_mc.cnoid

動画：https://drive.google.com/file/d/19uUrKQNyQ9wc-badsQ0Qi0iThq49ypha/view?usp=sharing

参考：順番に動作を実行する

ここでは詳細は省略します．詳しくは https://jrl-umi3218.github.io/mc_rtc/jp/tutorials/recipes/fsm.html から始まる一連のチュートリアルをご覧ください．

  • mc_rtc_new_controllerの代わりにmc_rtc_new_fsm_controllerでコントローラのテンプレートを作成する．
  • YAMLファイルに個別の動作の間の状態遷移を記述する．
    • YAMLファイルにはその他の設定を記述してコントローラでロードすることができ，コンパイルせずにパラメータを変更することが可能．

４．mc_rtcについてもう少し詳しく

（本節の内容は，mc_rtcの公式チュートリアルでも内容が不足している部分があります．mc_rtcのGitHubリポジトリでのIssueやPull Requestの作成を歓迎します．）

どのようなソフトウェア構成でChoreonoid上で動作しているか

新しいロボットでmc_rtcを使う

参考：https://jrl-umi3218.github.io/mc_rtc/jp/tutorials/advanced/new-robot.html

個別のロボットごとに必要なファイル群

  • Robot description：ロボットモデル（URDF形式）やそれに付随する制御部位や干渉形状に関する情報．
  • Robot module：mc_rtcのRobotModuleクラスを継承したクラス．初期姿勢・関節角度やセンサの種類・位置を指定する．

JVRC1の例

Fetchの例

実行の手順

  • ${HOME}/.config/mc_rtc/mc_rtc.yaml内のMainRobotを該当するロボットに書き換える．

５．mc_rtcに関連するオープンソース・ソフトウェア

歩行よりも複雑な運動のためのコントローラ

BaselineWalkingControllerと連携可能なプランナ

コントローラの内部で利用されている機能

Choreonoid拡張機能

  • ROS機能を使うためのChoreonoidプラグイン：CnoidRosUtils

MuJoCo拡張機能

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment