GR-MotionBank

1. Overview

GR-MotionBank plays pre-programmed motions from the local motion bank. User processes send a motion ID over DDS, and the runtime loads the matching motion files and reports MotionBank running state.

Element	Value
Domain ID	113
Command topic	algorithm_cmd
State topic	algorithm_state
Default example motion ID	6008

2. DDS Interface

2.1 Topics

Subscribed

Topic	Message Type	Description
algorithm_cmd	fourier_msgs/msg/AlgorithmCmd	Motion play / control command

Published

Topic	Message Type	Description
algorithm_state	fourier_msgs/msg/AlgorithmState	MotionBank running state

2.2 Message Fields

AlgorithmCmd

MotionBank currently parses only input_args[0]:

Field	Purpose
name	Set to "MotionBank"
cmd_state	Set to 1 to play a motion
group_mask	Control-group mask, commonly 0xFF
input_args[0].arg_int	Motion ID queue, for example [6008]
input_args[0].arg_string[0]	Optional control word: stop, break, continue, reset

AlgorithmState

Field	Purpose
name	MotionBank state uses "motionBank"
running_state	1 while a motion is in its lifecycle, 0 when stopped

3. Supported Motions

The currently active motion bank supports these motion IDs:

Motion ID	Motion Name
6001	greeting
6002	raise_hand_wave
6005	thumbs_up
6007	please_right
6008	please_left
6013	left_hand_heart
6014	right_hand_heart
6015	peace_sign
6023	clap
6026	take_photo
6030	fist_bump
6031	cheer_on
6032	hug
6033	right_handshake
6034	skr
6036	heart_hands
6101	etiquette_pose
6103	palm_up_explanation
6104	left_hand_emphasis
6105	right_hand_emphasis
6106	aerobics
6143	random_explanation_motion_1
6144	random_explanation_motion_2
6145	random_display_motion
6146	random_motion_test
6147	cross_arms
6148	high_five
6149	salute
6150	raise_hand
6151	both_arms_horizontal_raise

4. Basic Examples

4.1 Python — Play Motion 6008

The script below publishes a play command, so the robot will move. Confirm that the area around the robot is safe and that motionbank is running before executing it.

Python

import time
from threading import Event

from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs.msg.AlgorithmCmd as AlgorithmCmd

MOTION_ID = 6008
DOMAIN_ID = 113

dds = DDSInterface(domain_id=DOMAIN_ID)

cmd_pub = dds.create_publisher(
    AlgorithmCmd.AlgorithmCmdPubSubType,
    "algorithm_cmd",
)

started = Event()
finished = Event()
published = False
last_state = None


def on_state(msg):
    global last_state

    if msg.name() != "motionBank":
        return

    running = int(msg.running_state())
    if running != last_state:
        print(f"[state] motionBank running_state={running}")
        last_state = running

    if not published:
        return
    if running == 1:
        started.set()
    elif started.is_set() and running == 0:
        finished.set()


state_sub = dds.create_subscription(
    AlgorithmCmd.AlgorithmStatePubSubType,
    "algorithm_state",
    on_state,
    qos_profile=SubscriberQosProfile.default(),
)

# Wait for DDS discovery.
time.sleep(3.0)

base = AlgorithmCmd.BaseDataType()
base.arg_int([MOTION_ID])

cmd = AlgorithmCmd.AlgorithmCmd()
cmd.name("MotionBank")
cmd.cmd_state(1)
cmd.group_mask(0xFF)
cmd.input_args([base])

published = True
print(f"[publish] motion_id={MOTION_ID}")
cmd_pub.publish(cmd)

if not started.wait(5.0):
    raise RuntimeError(f"motion {MOTION_ID} did not enter running")
print(f"motion {MOTION_ID} entered running")

if not finished.wait(60.0):
    raise RuntimeError(f"motion {MOTION_ID} did not finish")
print(f"motion {MOTION_ID} finished")

dds.close()

Run local examples with sudo while preserving the environment variables. Otherwise the play command may be delivered, but the example may not receive algorithm_state callbacks from the root-owned runtime and can time out incorrectly.

Bash

sudo env "PYTHONPATH=$PYTHONPATH" "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" python3 -u play_motionbank_6008.py

4.2 C++ — Play Motion 6008

The example below also publishes a play command, so the robot will move. Confirm that the area around the robot is safe and that motionbank is running before executing it.

C++

#include <AlgorithmCmd/AlgorithmCmdPubSubTypes.hpp>
#include <AlgorithmState/AlgorithmStatePubSubTypes.hpp>
#include "fourier_dds/dds_node.hpp"
#include "fourier_dds/dds_publisher.hpp"
#include "fourier_dds/dds_subscription.hpp"

#include <atomic>
#include <chrono>
#include <iostream>
#include <memory>
#include <thread>
#include <vector>

int main() {
    constexpr int32_t kMotionId = 6008;

    std::atomic<bool> published{false};
    std::atomic<bool> started{false};
    std::atomic<bool> finished{false};
    std::atomic<int> last_state{-1};

    auto node = std::make_shared<fourier_dds::DdsNode>(113);
    auto cmd_pub = std::make_shared<
        fourier_dds::DdsPublisher<fourier_msgs::msg::AlgorithmCmdPubSubType>>(
        node,
        "algorithm_cmd");

    auto state_sub = std::make_shared<
        fourier_dds::DdsSubscription<fourier_msgs::msg::AlgorithmStatePubSubType>>(
        node,
        "algorithm_state",
        [&](const fourier_msgs::msg::AlgorithmState& msg) {
            if (msg.name() != "motionBank") {
                return;
            }

            const int running = static_cast<int>(msg.running_state());
            const int previous = last_state.exchange(running);
            if (running != previous) {
                std::cout << "[state] motionBank running_state=" << running << std::endl;
            }

            if (!published.load()) {
                return;
            }
            if (running == 1) {
                started = true;
            } else if (started.load() && running == 0) {
                finished = true;
            }
        });

    std::this_thread::sleep_for(std::chrono::seconds(3));

    std_msgs::msg::BaseDataType base;
    base.arg_int(std::vector<int32_t>{kMotionId});

    fourier_msgs::msg::AlgorithmCmd cmd;
    cmd.name("MotionBank");
    cmd.cmd_state(1);
    cmd.group_mask(0xFF);
    cmd.input_args(std::vector<std_msgs::msg::BaseDataType>{base});

    published = true;
    std::cout << "[publish] motion_id=" << kMotionId << std::endl;
    if (!cmd_pub->publish(cmd)) {
        std::cerr << "failed to publish motion command" << std::endl;
        return 1;
    }

    const auto enter_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
    while (!started.load() && std::chrono::steady_clock::now() < enter_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    if (!started.load()) {
        std::cerr << "motion " << kMotionId << " did not enter running" << std::endl;
        return 2;
    }
    std::cout << "motion " << kMotionId << " entered running" << std::endl;

    const auto finish_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(60);
    while (!finished.load() && std::chrono::steady_clock::now() < finish_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    if (!finished.load()) {
        std::cerr << "motion " << kMotionId << " did not finish" << std::endl;
        return 3;
    }
    std::cout << "motion " << kMotionId << " finished" << std::endl;
    return 0;
}

Build example:

Bash

g++ -std=c++17 play_motionbank_6008.cpp -o play_motionbank_6008 \
  -I/opt/fftai/include/fourier_dds \
  -I/opt/fftai/fourier_dds_msgs/include/fourier_dds_msgs \
  -L/opt/fftai/fourier_dds_msgs/lib \
  -Wl,-rpath,/opt/fftai/fourier_dds_msgs/lib \
  -lAlgorithmCmd \
  -lAlgorithmState \
  -lfastdds \
  -lfastcdr \
  -pthread

For local C++ runs, also use sudo and preserve the environment variables.

Bash

sudo env "PYTHONPATH=$PYTHONPATH" "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" ./play_motionbank_6008

5. Troubleshooting & Reminders

Motion does not enter running

Confirm that the motionbank process is running, both sides use domain 113, and the topics are algorithm_cmd / algorithm_state.

Message types must match

algorithm_cmd uses fourier_msgs/msg/AlgorithmCmd, and algorithm_state uses fourier_msgs/msg/AlgorithmState. In the Python bindings, import both PubSubTypes from fourier_msgs.msg.AlgorithmCmd.

Motion ID

The motion ID maps to MotionBank files under motion_<id>/config_motion.json and arms/arm<id>/left_arm.txt / right_arm.txt. This example uses 6008.