AMBF CRTK Plugin

This project is a plugin for Asynchronous Multibody Framework (AMBF) developed by Munawar et al. This plugin will enable users to communicate with AMBF simulation using Collaborative Robotics Toolkit(CRTK).

Please visit here for the CRTK docmuentation.

1. Installation Instructions

This plugin requires AMBF 3.0 and CRTK messages. Make sure to clone AMBF using the ambf-3.0 branch inside your ROS workspace.

1.1 Create a workspace

For ROS 1:

mkdir -p ~/ros1_ws/src
cd ~/ros1_ws/src

For ROS 2:

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

1.2 Clone AMBF

Clone AMBF inside the src directory of your workspace.

git clone git@github.com:WPI-AIM/ambf.git
cd ambf
git checkout ambf-3.0
git submodule update --init --recursive

Clone this repo

cd ~/ros1_ws/src   # or ~/ros2_ws/src
git clone git@github.com:LCSR-CIIS/ambf_crtk_plugin.git

1.3 Clone crtk_msgs

Follow these instructions to get crtk_msgs and don't forget to source it.

cd ~/ros1_ws/src # or ~/ros2_ws/src
git clone https://github.com/collaborative-robotics/crtk_msgs.git
git checkout devel

Build and source

For ROS 1:

 cd ~/ros1_ws
catkin build
source devel/setup.bash

For ROS 2:

cd ~/ros2_ws
colcon build
source install/setup.bash

2. How to use your plugin

2.1 Simulator plugin

You are required to specify configuration file such as example/CRTK_config.yaml:

Assuming you are in <ros_ws>: <plugin_path> is where the plugins build to, e.g. ./build/ambf_crtk_plugin <config_path> is the parent folder of the _config.yaml file, e.g. ./src/ambf_crtk_plugin/example/plugin-config/simulator_plugin

ambf_simulator --plugins <plugin_path>/libambf_crtk_simulator_plugin.so --conf <config_path>/CRTK_config.yaml

You can also define plugin in your launch.yaml:

plugins: [
  {
    name: CRTK,
    filename: libambf_crtk_simulator_plugin.so,
    path: <plugin_path>/ambf_crtk_simulator_plugin/build
  }
]

2.2 Model plugin

You can specify plugin in your ADF file as follows:

plugins: [
  {
    name: CRTK,
    filename: libambf_crtk_model_plugin.so,
    path: <plugin_path>/ambf_crtk_model_plugin/build
  }
]
crtk_config: <path_to_your_configuration_file> # relative to your ambf_crtk_model_plugin/src/model_plugin

2.3 Object plugin

You can specify plugin in your ADF file as follows:

plugins: [
  {
    name: CRTK,
    filename: libambf_crtk_object_plugin.so,
    path: <plugin_path>/ambf_crtk_object_plugin/build
  }
]

3. Configuration file

You are required to specify your custom made configuration file to specify what kind of objects you want to monitor/control with which CRTK command.

# AMBF CRTK configuration file template for CRTK_model_plugin
# Author: Hisashi Ishida
# Date: 04.04.2025


# Interfaces used for this plugin
interface:
- PSM1

PSM1:
  measured_cp: [
    {
      rigidbody: psm1/BODY yaw link,
      namespace: /PSM1/, # [Optional] Override namespace "/PSM1/measured_cp"
      reference: BODY tool link
    },
    # {
    #   rigidbody: psm1/BODY base link,
    #   reference: CameraFrame
    # }
  ]

  setpoint_cp: [
    {
      rigidbody: psm1/BODY yaw link,
      namespace: /PSM1/, # [Optional] Override namespace "/PSM1/measured_cp"
      reference: BODY tool link
    }
  ]

  measured_js: [
    {
      namespace: /PSM1,
      joints: [
        psm1/JOINT base link-yaw link,
        psm1/JOINT pitch bottom link-pitch end link,
        psm1/JOINT pitch end link-main insertion link,
        psm1/JOINT pitch front link-pitch bottom link,
        psm1/JOINT pitch top link-pitch end link,

        psm1/JOINT tool roll link-tool pitch link,
        psm1/JOINT yaw link-pitch back link,
        psm1/JOINT yaw link-pitch end link,
        psm1/JOINT yaw link-pitch front link,
      ]
    },
    {
      joints: [
        psm1/JOINT tool pitch link-tool gripper1 link,
        psm1/JOINT tool pitch link-tool gripper2 link,
      ],
      namespace: /PSM1/gripper
    }
  ]
    
  servo_cp:
  [
    {
      rigidbody: psm1/BODY yaw link,
      namespace: /PSM1/, # [Optional] Override namespace "/PSM1/servo_cp"
      reference: BODY tool link
    }
  ]

  servo_jp: [
    {
      namespace: /PSM1,
      joints: [
        psm1/JOINT base link-yaw link,
        psm1/JOINT main insertion link-tool roll link,
        psm1/JOINT pitch back link-pitch bottom link,
        psm1/JOINT pitch back link-pitch top link,
        psm1/JOINT pitch bottom link-pitch end link,

        psm1/JOINT tool roll link-tool pitch link,
        psm1/JOINT yaw link-pitch back link,
        psm1/JOINT yaw link-pitch end link,
        psm1/JOINT yaw link-pitch front link,
      ]
    },
    {
      joints: [
        psm1/JOINT tool pitch link-tool gripper1 link,
        psm1/JOINT tool pitch link-tool gripper2 link,
      ],
      namespace: /PSM1/gripper
    }
  ]
    

In this example, there will be the following rostopics:

/PSM1/gripper/measured_js
/PSM1/gripper/servo_jp
/PSM1/local/measured_cp  # This measured_cp is w.r.t ambf origin
/PSM1/local/servo_cp     # This servo_cp is w.r.t ambf origin
/PSM1/local/setpoint_cp  # This setpoint_cp is w.r.t ambf origin
/PSM1/measured_cp        # This measured_cp is w.r.t reference specified in the configuration file
/PSM1/measured_js
/PSM1/operating_state
/PSM1/servo_cp           # This servo_cp is w.r.t reference specified in the configuration file
/PSM1/servo_jp
/PSM1/setpoint_cp        # This setpoint_cp is w.r.t reference specified in the configuration file
/PSM1/state_command

Example command

Please refer to Surgical Robotics Challenge and use the following command to use it for SRC:

ambf_simulator --launch_file ~/surgical_robotics_challenge/launch.yaml -l 0,1,2,3,4,5 --plugins <plugin_path>/libambf_crtk_simulator_plugin.so --conf <config_path>/SRC_config.yaml 

You can use the following example:

ambf_simulator -a ~/3D-Slicer_ROS_Module_with_AMBF/AMBF_Plugin_3DSlicer/ADF/galen.yaml --plugins <plugin_path>/libambf_crtk_simulator_plugin.so --conf <config_path>/CRTK_config.yaml 

You can use the following example to use model plugin for dvrk:

ambf_simulator --launch_file launch.yaml -l 0,1,2

You can use the following example to use model plugin for full dvrk blender model:

ambf_simulator --launch_file launch.yaml -l 3,4,5

Trouble Shooting

Please refer to AMBF helper for installation procedure and how to debug the plugins.