feat: adding mc_mujoco tutorials

.gitmodules

@@ -24,3 +24,9 @@
 [submodule "robots_interface/mc_rtde"]
 	path = robots_interface/mc_rtde
 	url = https://github.com/isri-aist/mc_rtde
+[submodule "mujoco_description/kinova_mj_description"]
+	path = mujoco_description/kinova_mj_description
+	url = https://github.com/mathieu-celerier/kinova_mj_description.git
+[submodule "mujoco_description/ur5e_mj_desription"]
+	path = mujoco_description/ur5e_mj_desription
+	url = https://github.com/isri-aist/ur5e_mj_description.git

README.md

@@ -5,6 +5,8 @@ This repository contains various tutorials for [mc_rtc](https://jrl.cnrs.fr/mc_r
 - **dual_arm_controller**: This tutorial demonstrates how to implement a dual-arm controller using C++ and Python. It showcases the control of two robotic arms working together to perform tasks.
 - **mobile_arm_controller**: This tutorial focuses on controlling a mobile manipulator robot using C++ and Python. It illustrates how to manage both the mobility and manipulation capabilities of the robot.
 
+Mujoco description for UR5e and Kinova robots are also provided to run controller with [mc_mujoco](https://github.com/rohanpsingh/mc_mujoco), a MuJoCo interface for mc_rtc.
+
 ## Installation
 Clone this repository to your local machine:
 ```bash
@@ -41,7 +43,8 @@ It is not recommended to run the tutorials without Docker since it is mixed betw
     make && make install
     ```
 
-## Running preinstalled controller
+## mc_rtc controller
+
 In one terminal, run RVIZ2 for visualization:
 ```bash
 ros2 launch mc_rtc_ticker display.launch
@@ -70,7 +73,7 @@ PYTHONPATH=<path_to_mc_rtc_tutorials>/controllers/mobile_arm_controller/python m
 ```
 If you are using docker: `PYTHONPATH=${HOME}/mc_rtc_ws/mobile_arm_controller/python mc_rtc_ticker -f ~/config/mobile_arm_controller_python.yaml`
 
-## Adding controller
+### Adding controller
 You can add controllers of your own and build it easily.
 
 Put your new controller inside `controllers` directory. If you have never work with `mc_rtc` before, you can find several tutorials to get started [here](https://jrl.cnrs.fr/mc_rtc/tutorials.html).
@@ -92,3 +95,20 @@ cmake ..
 make
 sudo make install
 ```
+
+## mc_mujoco
+
+With the provided mc_mujoco descriptions, you can run `dual_arm_controller` with mc_mujoco
+```sh
+mc_mujoco -sf ~/config/dual_arm_controller.yaml
+```
+
+You can add other controllers for UR5e and Kinova and simulate them with mc_mujoco using the same command.
+
+```sh
+mc_mujoco -sf <path_to_config_file>
+```
+
+## Note
+
+For both `mc_rtc_ticker` and `mc_mujoco`, you can run them with flag `-h` or `--help` to see all available options.

controllers/dual_arm_controller/src/DualArmController.cpp

@@ -3,13 +3,15 @@
 #include <mc_rbdyn/RobotLoader.h>
 
 DualArmController::DualArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
-: mc_control::MCController({rm, mc_rbdyn::RobotLoader::get_robot_module("KinovaDefault")}, dt)
+: mc_control::MCController({rm, mc_rbdyn::RobotLoader::get_robot_module("Kinova")}, dt)
 {
   solver().addConstraintSet(contactConstraint);
   solver().addConstraintSet(kinematicsConstraint);
   solver().addConstraintSet(selfCollisionConstraint);
 
-  addCollisions("ur5e", "kinova_default", {{"*", "*", iDist, sDist, 0}});
+  robots().robot("kinova").posW(sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(0.7, 0.5, 0)));
+
+  addCollisions("ur5e", "kinova", {{"*", "*", iDist, sDist, 0}});
 
   postureTask->stiffness(1);
   postureTask->weight(1);
@@ -83,8 +85,6 @@ void DualArmController::reset(const mc_control::ControllerResetData & reset_data
 
   kinovaKinematics_ = std::make_unique<mc_solver::KinematicsConstraint>(robots(), 1, solver().dt());
   solver().addConstraintSet(kinovaKinematics_);
-
-  robots().robot(1).posW(sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(0.7, 0.5, 0)));
 }
 
 CONTROLLER_CONSTRUCTOR("DualArmController", DualArmController)

dockerfiles/.zshrc

@@ -0,0 +1,116 @@
+# If you come from bash you might have to change your $PATH.
+# export PATH=$HOME/bin:$HOME/.local/bin:/usr/local/bin:$PATH
+
+# Path to your Oh My Zsh installation.
+export ZSH="$HOME/.oh-my-zsh"
+
+# Set name of the theme to load --- if set to "random", it will
+# load a random theme each time Oh My Zsh is loaded, in which case,
+# to know which specific one was loaded, run: echo $RANDOM_THEME
+# See https://github.com/ohmyzsh/ohmyzsh/wiki/Themes
+ZSH_THEME="robbyrussell"
+
+# Set list of themes to pick from when loading at random
+# Setting this variable when ZSH_THEME=random will cause zsh to load
+# a theme from this variable instead of looking in $ZSH/themes/
+# If set to an empty array, this variable will have no effect.
+# ZSH_THEME_RANDOM_CANDIDATES=( "robbyrussell" "agnoster" )
+
+# Uncomment the following line to use case-sensitive completion.
+# CASE_SENSITIVE="true"
+
+# Uncomment the following line to use hyphen-insensitive completion.
+# Case-sensitive completion must be off. _ and - will be interchangeable.
+# HYPHEN_INSENSITIVE="true"
+
+# Uncomment one of the following lines to change the auto-update behavior
+# zstyle ':omz:update' mode disabled  # disable automatic updates
+# zstyle ':omz:update' mode auto      # update automatically without asking
+# zstyle ':omz:update' mode reminder  # just remind me to update when it's time
+
+# Uncomment the following line to change how often to auto-update (in days).
+# zstyle ':omz:update' frequency 13
+
+# Uncomment the following line if pasting URLs and other text is messed up.
+# DISABLE_MAGIC_FUNCTIONS="true"
+
+# Uncomment the following line to disable colors in ls.
+# DISABLE_LS_COLORS="true"
+
+# Uncomment the following line to disable auto-setting terminal title.
+# DISABLE_AUTO_TITLE="true"
+
+# Uncomment the following line to enable command auto-correction.
+# ENABLE_CORRECTION="true"
+
+# Uncomment the following line to display red dots whilst waiting for completion.
+# You can also set it to another string to have that shown instead of the default red dots.
+# e.g. COMPLETION_WAITING_DOTS="%F{yellow}waiting...%f"
+# Caution: this setting can cause issues with multiline prompts in zsh < 5.7.1 (see #5765)
+# COMPLETION_WAITING_DOTS="true"
+
+# Uncomment the following line if you want to disable marking untracked files
+# under VCS as dirty. This makes repository status check for large repositories
+# much, much faster.
+# DISABLE_UNTRACKED_FILES_DIRTY="true"
+
+# Uncomment the following line if you want to change the command execution time
+# stamp shown in the history command output.
+# You can set one of the optional three formats:
+# "mm/dd/yyyy"|"dd.mm.yyyy"|"yyyy-mm-dd"
+# or set a custom format using the strftime function format specifications,
+# see 'man strftime' for details.
+# HIST_STAMPS="mm/dd/yyyy"
+
+# Would you like to use another custom folder than $ZSH/custom?
+# ZSH_CUSTOM=/path/to/new-custom-folder
+
+# Which plugins would you like to load?
+# Standard plugins can be found in $ZSH/plugins/
+# Custom plugins may be added to $ZSH_CUSTOM/plugins/
+# Example format: plugins=(rails git textmate ruby lighthouse)
+# Add wisely, as too many plugins slow down shell startup.
+plugins=(
+	git
+	nvm
+	pip
+	pre-commit
+	ssh
+	zsh-interactive-cd
+	zsh-autosuggestions
+)
+
+# for zsh-completions
+fpath+=${ZSH_CUSTOM:-${ZSH:-~/.oh-my-zsh}/custom}/plugins/zsh-completions/src
+autoload -U compinit && compinit
+
+source $ZSH/oh-my-zsh.sh
+
+# User configuration
+
+# export MANPATH="/usr/local/man:$MANPATH"
+
+# You may need to manually set your language environment
+# export LANG=en_US.UTF-8
+
+# Preferred editor for local and remote sessions
+# if [[ -n $SSH_CONNECTION ]]; then
+#   export EDITOR='vim'
+# else
+#   export EDITOR='nvim'
+# fi
+
+# Compilation flags
+# export ARCHFLAGS="-arch $(uname -m)"
+
+# Set personal aliases, overriding those provided by Oh My Zsh libs,
+# plugins, and themes. Aliases can be placed here, though Oh My Zsh
+# users are encouraged to define aliases within a top-level file in
+# the $ZSH_CUSTOM folder, with .zsh extension. Examples:
+# - $ZSH_CUSTOM/aliases.zsh
+# - $ZSH_CUSTOM/macos.zsh
+# For a full list of active aliases, run `alias`.
+#
+# Example aliases
+# alias zshconfig="mate ~/.zshrc"
+# alias ohmyzsh="mate ~/.oh-my-zsh"

dockerfiles/Dockerfile

@@ -6,45 +6,54 @@ LABEL org.opencontainers.image.description="mc_rtc_tutorial image"
 LABEL org.opencontainers.image.licenses=BSD
 
 # Configure environment
-ENV DISPLAY host.docker.internal:0.0
-ENV LD_LIBRARY_PATH "/usr/local/lib"
+ENV LD_LIBRARY_PATH="/usr/local/lib"
 
-# Set default bash shell
 SHELL ["/bin/bash", "-c"]
 
-RUN apt-get update && apt install -y curl ca-certificates gpg wget
+RUN apt-get update && apt install -y curl ca-certificates gpg wget zsh fzf
 
-RUN curl -1sLf 'https://dl.cloudsmith.io/public/mc-rtc/head/setup.deb.sh' | bash
+RUN sh -c "$(curl -fsSL https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh)" "" --unattended && \
+    git clone https://github.com/zsh-users/zsh-autosuggestions ${ZSH_CUSTOM:-~/.oh-my-zsh/custom}/plugins/zsh-autosuggestions && \
+    git clone https://github.com/zsh-users/zsh-completions ${ZSH_CUSTOM:-~/.oh-my-zsh/custom}/plugins/zsh-completions
+COPY ./dockerfiles/.zshrc /root/.zshrc
 
-RUN apt install -y libmc-rtc-dev mc-rtc-utils
-
-RUN apt install -y ros-${ROS_DISTRO}-mc-rtc-plugin ros-${ROS_DISTRO}-mc-rtc-tools
-
-RUN wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null
-RUN echo 'deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ jammy main' | sudo tee /etc/apt/sources.list.d/kitware.list >/dev/null
-RUN apt-get update && apt-get install -y \
-    kitware-archive-keyring \
-    cmake \
-    ros-humble-gazebo-ros-pkgs \
-    ros-humble-hardware-interface \
+RUN curl -1sLf 'https://dl.cloudsmith.io/public/mc-rtc/head/setup.deb.sh' | bash && \
+    apt install -y libmc-rtc-dev mc-rtc-utils && \
+    apt install -y ros-${ROS_DISTRO}-mc-rtc-plugin ros-${ROS_DISTRO}-mc-rtc-tools && \
+    wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null && \
+    echo 'deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ jammy main' | sudo tee /etc/apt/sources.list.d/kitware.list >/dev/null && \
+    apt-get update && apt-get install -y kitware-archive-keyring cmake ros-humble-gazebo-ros-pkgs ros-humble-hardware-interface \
     && rm -rf /var/lib/apt/lists/*
 
+# Install dependencies for mc_mujoco
+WORKDIR /opt
+RUN apt-get update && apt-get install -y libxrandr-dev libxinerama-dev libxcursor-dev libxi-dev libglew-dev && \
+    source /opt/ros/${ROS_DISTRO}/setup.bash && \
+    git clone --recursive https://github.com/rohanpsingh/mc_mujoco.git && \
+    cd mc_mujoco && \
+    mkdir build && cd build && \
+    cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo && \
+    make && \
+    make install
+
 # Install dependencies for robots_interface
-RUN apt-get update \
-    && apt-get install -y software-properties-common \
-    && add-apt-repository ppa:sdurobotics/ur-rtde \
-    && apt-get update \
-    && apt-get install librtde librtde-dev
+WORKDIR /opt
+RUN apt-get update && apt-get install -y software-properties-common \
+    && rm -rf /var/lib/apt/lists/* && \
+    git clone https://gitlab.com/sdurobotics/ur_rtde.git && \
+    cd ur_rtde && \
+    mkdir -p build && cd build && \
+    cmake -DPYTHON_BINDINGS:BOOL=OFF .. && \
+    make && make install
 
 WORKDIR /opt
 RUN git clone https://github.com/jrl-umi3218/ur_modern_driver && \
     cd ur_modern_driver && \
     mkdir build && cd build && \
-    cmake .. && \
-    make install
+    cmake .. && make install
 
 COPY ./dockerfiles/entrypoint.sh /entrypoint.sh
 RUN chmod +x /entrypoint.sh
 WORKDIR /root
 ENTRYPOINT ["/entrypoint.sh"]
-CMD ["/bin/bash"]
+CMD ["/bin/zsh"]

dockerfiles/docker-compose.yml

@@ -1,17 +1,16 @@
-version: '3.3'
-
 services:
   mc_rtc_tutorials:
-    image: ghcr.io/isri-aist/mc_rtc_tutorials:latest
+    # image: ghcr.io/isri-aist/mc_rtc_tutorials:latest
     user: root
     container_name: mc_rtc_tutorials
     environment:
       DISPLAY: $DISPLAY
     # Uncomment if you can to build local image
-    # build:
-    #   context: ..
-    #   dockerfile: ./dockerfiles/Dockerfile
+    build:
+      context: ..
+      dockerfile: ./dockerfiles/Dockerfile
     volumes:
+      - ./entrypoint.sh:/entrypoint.sh:ro
       - /tmp/.X11-unix:/tmp/.X11-unix
       # Use the command 'echo $XAUTHORITY' to find exact path
       - /run/user/1000/gdm/Xauthority:/root/.Xauthority
@@ -22,6 +21,8 @@ services:
       - "./colcon_defaults.yaml:/root/colcon_ws/colcon_defaults.yaml"
       - "../robots/:/root/mc_robots/"
       - "../robots_interface/:/root/mc_interface/"
-    privileged: True
-    stdin_open: True
-    command: bash
+      - "../mujoco_description/:/root/mc_mujoco_description/"
+    privileged: true
+    stdin_open: true
+    tty: true
+    command: zsh

dockerfiles/entrypoint.sh

@@ -3,35 +3,52 @@ set -e
 
 source /opt/ros/humble/setup.bash
 
+echo "--- Building colcon workspace ---"
 cd /root/colcon_ws
 colcon build
+if [[ -f "/root/colcon_ws/install/setup.bash" ]]; then
+    source "/root/colcon_ws/install/setup.bash"
+fi
+echo "--- Colcon workspace build complete ---"
+
+build_project() {
+    local name="$1"
+    local src="$2"
+    if [[ ! -d "$src" ]]; then
+        echo "--- Skipping $name (directory not found) ---"
+        return 0
+    fi
+    echo "--- Compiling $name ---"
+    mkdir -p "$src/build"
+    cd "$src/build"
+    cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
+    make -j$(nproc)
+    make install
+    echo "--- $name compilation complete ---"
+}
+
+build_project "mc_robots"    /root/mc_robots
+build_project "controllers"  /root/mc_rtc_ws
+build_project "mc_mujoco"    /root/mc_mujoco_description
+build_project "mc_interface"  /root/mc_interface
+
+# Source ROS in shell configs
+if ! grep -q "^ros/humble/setup.bash" ~/.bashrc 2>/dev/null; then
+    echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
+    echo "source /root/colcon_ws/install/setup.bash" >> ~/.bashrc
+fi
+if ! grep -q "^ros/humble/setup.zsh" ~/.zshrc 2>/dev/null; then
+    echo "source /opt/ros/humble/setup.zsh" >> ~/.zshrc
+    echo "source /root/colcon_ws/install/setup.zsh" >> ~/.zshrc
+fi
 
-if [ -f "/root/colcon_ws/install/setup.bash" ]; then
-  source "/root/colcon_ws/install/setup.bash"
+if ! grep -q "^RobotModulePaths" /usr/etc/mc_rtc.yaml; then
+    echo 'RobotModulePaths: [/usr/lib/mc_robots]' >> /usr/etc/mc_rtc.yaml
+fi
+if ! grep -q "^ControllerModulePaths" /usr/etc/mc_rtc.yaml; then
+    echo 'ControllerModulePaths: [/usr/lib/mc_controller]' >> /usr/etc/mc_rtc.yaml
 fi
 
-echo "--- Compiling mc_robots ---"
-mkdir -p /root/mc_robots/build
-cd /root/mc_robots/build
-cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
-make install
-echo "--- Compilation Complete ---"
-
-echo "--- Compiling controllers ---"
-mkdir -p /root/mc_rtc_ws/build
-cd /root/mc_rtc_ws/build
-cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
-make install
-echo "--- Compilation Complete ---"
-
-echo "--- Compiling mc_interface ---"
-mkdir -p /root/mc_interface/build
-cd /root/mc_interface/build
-cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
-make install
-echo "--- Compilation Complete ---"
-
-echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
-echo "source /root/colcon_ws/install/setup.bash" >> ~/.bashrc
+echo "--- COMPILATION COMPLETED ---"
 
 exec "$@"