GR-Actuator

1. Overview

GR-Actuator is the DDS interface for Fourier robot actuators. It receives body joint commands over DDS, drives the leg, waist, head, and arm actuators, and publishes body joint state and actuator faults over DDS.

Using GR3 as the example, this module covers 31 degrees of freedom: 12 DOF for both legs, 3 DOF for the waist, 2 DOF for the head, and 14 DOF for both arms. The left and right dexterous hands are not part of GR-Actuator.

Item	Value
Domain ID	123
Cycle	Joint state published at 1000 Hz; body control cycle is about 1 ms
Hardware	GR3 body joint actuators, 31 DOF
Group field	left_leg, right_leg, waist, head, left_manipulator, right_manipulator

2. DDS Interface Services

2.1 Topics

Subscribed

Topic	Message Type	Description
robot_joint_cmd	JointGroupCmd	Body control-group joint command

Published

Topic	Message Type	Rate	Description
robot_joint_state	RobotJointState	1000 Hz	Current body joint state: position, velocity, effort
hal_actuator_error_code	ErrorCodes	Event driven; persistent faults up to about 10 Hz	Actuator fault codes

Services

Service	Request / Response	Description
set_actuator_params	SetActuatorParams_Request / SetActuatorParams_Response	Set PID parameters for a control group
set_actuator_behavior_enable	SetActuatorBehavior_Request / SetActuatorBehavior_Response	Enable or disable a control group

2.2 IDL Definitions

JointGroupCmd

Idl

struct PvatData7 {
    double position[7];      // Target position; first N DOF are valid
    double velocity[7];      // Target velocity
    double acceleration[7];  // Target acceleration
    double effort[7];        // Target torque or feed-forward torque
};

struct JointGroupCmd {
    HeaderPod header;
    TracePointList trace_points;
    ShortString group_names[6];  // For example "left_manipulator"
    PvatData7 commands[6];       // Aligned with group_names
};

Common GR3 group_names are left_leg, right_leg, waist, head, left_manipulator, and right_manipulator. Each control group has up to 7 DOF, and commands[i] only uses the values for the actual DOF count of that group.

RobotJointState

Idl

struct RobotJointState {
    HeaderPod header;
    TracePointList trace_points;
    double position[32];  // Current joint position
    double velocity[32];  // Current joint velocity
    double effort[32];    // Current joint effort
    uint8 state_size;
};

GR3 body state is written in whole-body joint order. Consumers normally read the first 31 entries; dexterous hand state is not included in this message.

SetActuatorParams

Idl

enum ControlMode {
    POSITION,
    VELOCITY,
    TORQUE,
    PD
};

struct ControlParameters {
    double pos_kp[7];
    double pos_ki[7];
    double pos_kd[7];
    double pd_kp[7];
    double pd_kd[7];
};

struct SetActuatorParams_Request {
    HeaderPod header;
    boolean force_reset;      // Reserved field
    ShortString group_name;   // Target control group
    ControlMode motor_mode;   // POSITION or PD
    ControlParameters params;
};

struct SetActuatorParams_Response {
    HeaderPod header;
    LongString message;
    boolean success;
};

The current service supports writing parameters for POSITION and PD. Use success as the result.

SetActuatorBehavior

Idl

struct ControlBehavior {
    boolean enable;  // true enables the group, false disables it
    uint8 motor_mode;
};

struct SetActuatorBehavior_Request {
    HeaderPod header;
    boolean force_reset;       // Reserved field
    ShortString group_name;    // Target control group
    ControlBehavior behavior;
};

struct SetActuatorBehavior_Response {
    HeaderPod header;
    LongString message;
    boolean success;
};

The public service is set_actuator_behavior_enable, which uses behavior.enable.

ErrorCodes

Idl

struct ErrorCode {
    uint32 high32;
    uint32 low32;
};

struct ErrorCodes {
    std_msgs::msg::Header header;
    uint8 source;                  // 1 = actuator
    sequence<ErrorCode> error_codes;
};

3. Basic Examples

3.1 — Body Control Command Publisher

Python

import time
from fourierdds_py import DDSInterface, PublisherQosProfile
import fourier_msgs_pod.msg.RobotJointCmd as RobotJointCmd

DOMAIN_ID = 123
GROUP_NAME = "left_manipulator"
DOF = 7

def short_string(value):
    data = value.encode("utf-8")
    if len(data) > 64:
        raise ValueError("group name exceeds ShortString capacity")
    msg = RobotJointCmd.ShortString()
    msg.content(data + b"\0" * (64 - len(data)))
    return msg

dds = DDSInterface(domain_id=DOMAIN_ID)
pub = dds.create_publisher(
    pub_sub_type=RobotJointCmd.JointGroupCmdPubSubType(),
    topic_name="robot_joint_cmd",
    qos_profile=PublisherQosProfile.best_effort(),
)
time.sleep(3)

msg = RobotJointCmd.JointGroupCmd()
msg.group_names([short_string(GROUP_NAME)] + [RobotJointCmd.ShortString() for _ in range(5)])

cmd = RobotJointCmd.PvatData__7()
for i in range(DOF):
    cmd.position()[i] = 0.0
    cmd.velocity()[i] = 0.0
    cmd.acceleration()[i] = 0.0
    cmd.effort()[i] = 0.0
msg.commands([cmd] + [RobotJointCmd.PvatData__7() for _ in range(5)])

pub.publish(msg)

3.2 — Body Joint State Subscriber

Python

import time
from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs_pod.msg.RobotJointState as RobotJointState

dds = DDSInterface(domain_id=123)

def on_state(msg):
    pos = msg.position()
    vel = msg.velocity()
    eff = msg.effort()
    print(f"joint[0]: pos={pos[0]:.4f}, vel={vel[0]:.4f}, effort={eff[0]:.4f}")

dds.create_subscription(
    pub_sub_type=RobotJointState.RobotJointStatePubSubType(),
    topic_name="robot_joint_state",
    callback=on_state,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

4. Advanced — Demos and Tools

4.1 Demo: Enable a Control Group and Set PD Parameters

Python

import time
from fourierdds_py import DDSInterface
import fourier_msgs_pod.srv.SetActuatorBehavior as SetActuatorBehavior
import fourier_msgs_pod.srv.SetActuatorParams as SetActuatorParams

DOMAIN_ID = 123
GROUP_NAME = "left_manipulator"
DOF = 7

def short_string(module, value):
    data = value.encode("utf-8")
    msg = module.ShortString()
    msg.content(data + b"\0" * (64 - len(data)))
    return msg

dds = DDSInterface(domain_id=DOMAIN_ID)
enable_client = dds.create_client(
    req_type=SetActuatorBehavior.SetActuatorBehavior_RequestPubSubType(),
    res_type=SetActuatorBehavior.SetActuatorBehavior_ResponsePubSubType(),
    service_name="set_actuator_behavior_enable",
)
params_client = dds.create_client(
    req_type=SetActuatorParams.SetActuatorParams_RequestPubSubType(),
    res_type=SetActuatorParams.SetActuatorParams_ResponsePubSubType(),
    service_name="set_actuator_params",
)
time.sleep(3)

enable_req = SetActuatorBehavior.SetActuatorBehavior_Request()
behavior = SetActuatorBehavior.ControlBehavior()
behavior.enable(True)
enable_req.behavior(behavior)
enable_req.group_name(short_string(SetActuatorBehavior, GROUP_NAME))
enable_res = enable_client.send_request(enable_req, wait_ms=1000)
if enable_res is None or not enable_res.success():
    raise RuntimeError("set_actuator_behavior_enable failed")

params_req = SetActuatorParams.SetActuatorParams_Request()
params_req.group_name(short_string(SetActuatorParams, GROUP_NAME))
params_req.motor_mode(SetActuatorParams.ControlMode_PD)
params = SetActuatorParams.ControlParameters()
params.pd_kp([80.0] * DOF)
params.pd_kd([4.0] * DOF)
params_req.params(params)
params_res = params_client.send_request(params_req, wait_ms=1000)
if params_res is None or not params_res.success():
    raise RuntimeError("set_actuator_params failed")

4.2 Demo: Complete Joint Control Demo

Python

import time
from fourierdds_py import DDSInterface, PublisherQosProfile, SubscriberQosProfile
import fourier_msgs_pod.msg.RobotJointCmd as RobotJointCmd
import fourier_msgs_pod.msg.RobotJointState as RobotJointState
import fourier_msgs_pod.srv.SetActuatorBehavior as SetActuatorBehavior
import fourier_msgs_pod.srv.SetActuatorParams as SetActuatorParams

DOMAIN_ID = 123
GROUP_NAME = "left_manipulator"
DOF = 7

# KP and KD values for the left manipulator.
left_manipulator_kp = [400.0, 200.0, 200.0, 200.0, 50.0, 50.0, 50.0]
left_manipulator_kd = [20.0, 10.0, 10.0, 10.0, 2.5, 2.5, 2.5]

# Global storage for the latest subscribed joint positions.
current_joint_positions = None

def short_string(module, value):
    data = value.encode("utf-8")
    if len(data) > 64:
        raise ValueError("control group name exceeds 64 bytes")
    msg = module.ShortString()
    msg.content(data + b"\0" * (64 - len(data)))
    return msg

# Subscriber callback: keep updating the real manipulator position.
def on_state_callback(msg):
    global current_joint_positions
    current_joint_positions = list(msg.position())


# ==========================================
# 0. Initialize the DDS node and communication interfaces
# ==========================================
print("Initializing DDS node...")
dds = DDSInterface(domain_id=DOMAIN_ID)

# Create service clients.
enable_client = dds.create_client(
    req_type=SetActuatorBehavior.SetActuatorBehavior_RequestPubSubType(),
    res_type=SetActuatorBehavior.SetActuatorBehavior_ResponsePubSubType(),
    service_name="set_actuator_behavior_enable",
)
params_client = dds.create_client(
    req_type=SetActuatorParams.SetActuatorParams_RequestPubSubType(),
    res_type=SetActuatorParams.SetActuatorParams_ResponsePubSubType(),
    service_name="set_actuator_params",
)

# Create the topic subscriber and publisher.
dds.create_subscription(
    pub_sub_type=RobotJointState.RobotJointStatePubSubType(),
    topic_name="robot_joint_state",
    callback=on_state_callback,
    qos_profile=SubscriberQosProfile.default(),
)
pub = dds.create_publisher(
    pub_sub_type=RobotJointCmd.JointGroupCmdPubSubType(),
    topic_name="robot_joint_cmd",
    qos_profile=PublisherQosProfile.best_effort(),
)

time.sleep(3) # Wait for DDS discovery.
print("DDS initialization complete.\n")


# ==========================================
# Stage 1: Enable motors and configure PD parameters
# ==========================================
input(">>> Press [Enter] to start: enable motors and set PD parameters...")

# 1.1 Send the enable request.
print("Enabling control group...")
enable_req = SetActuatorBehavior.SetActuatorBehavior_Request()
behavior = SetActuatorBehavior.ControlBehavior()
behavior.enable(True)
enable_req.behavior(behavior)
enable_req.group_name(short_string(SetActuatorBehavior, GROUP_NAME))
enable_res = enable_client.send_request(enable_req, wait_ms=1000)
if enable_res is None or not enable_res.success():
    raise RuntimeError("failed to enable the control group; aborting")
print("-> Control group enabled.")

# 1.2 Send the PD parameter request.
print("Setting custom PD parameters...")
params_req = SetActuatorParams.SetActuatorParams_Request()
params_req.group_name(short_string(SetActuatorParams, GROUP_NAME))
params_req.motor_mode(SetActuatorParams.ControlMode_PD)

params = SetActuatorParams.ControlParameters()
params.pd_kp(left_manipulator_kp)
params.pd_kd(left_manipulator_kd)
params_req.params(params)

params_res = params_client.send_request(params_req, wait_ms=1000)
if params_res is None or not params_res.success():
    raise RuntimeError("failed to set control group parameters; aborting")
print("-> PD parameters set.\n")


# ==========================================
# Stage 2: Get the current real position
# ==========================================
print("Reading the current manipulator position...")
for _ in range(30):
    if current_joint_positions is not None:
        break
    time.sleep(0.1)

if current_joint_positions is None:
    raise RuntimeError("failed to read manipulator state; check controller communication or topic name")

start_pos = current_joint_positions[20]
target_pos = -1.4  # Target position.

print(f"-> Current state locked.")
print(f"   Current position of motor 21 (left_elbow_pitch_joint): {start_pos:.4f}")
print(f"   Interpolation target position: {target_pos:.4f}\n")


# ==========================================
# Stage 3: Safe step-by-step control
# ==========================================
input(f">>> Confirm the manipulator area is safe, then press [Enter] to interpolate from {start_pos:.4f} to {target_pos:.4f}...")

# Build the base command message.
msg = RobotJointCmd.JointGroupCmd()
msg.group_names([short_string(RobotJointCmd, GROUP_NAME)] + [RobotJointCmd.ShortString() for _ in range(5)])

cmd = RobotJointCmd.PvatData__7()
# Initialize all axes to the latest real positions to avoid jumps on other axes.
for i in range(DOF):
    cmd.position()[i] = current_joint_positions[i]
    cmd.velocity()[i] = 0.0
    cmd.acceleration()[i] = 0.0
    cmd.effort()[i] = 0.0
msg.commands([cmd] + [RobotJointCmd.PvatData__7() for _ in range(5)])

# Interpolation parameters.
DURATION = 2.0       # 2 seconds.
STEPS = 100          # 100 steps.
INTERVAL = DURATION / STEPS

print("Executing smooth interpolation trajectory...")
for step in range(STEPS + 1):
    alpha = step / STEPS
    current_target = start_pos + (target_pos - start_pos) * alpha

    # Only modify the 4th motor target position (index 3).
    msg.commands()[0].position()[3] = current_target

    # Publish the DDS command.
    pub.publish(msg)
    time.sleep(INTERVAL)

print(f"\n[Done] Interpolation finished. Motor 21 (left_elbow_pitch_joint) reached target position: {target_pos}")

4.3 Tool: Joint State Watcher

Python

import time
from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs_pod.msg.RobotJointState as RobotJointState

dds = DDSInterface(domain_id=123)

def on_state(msg):
    pos = list(msg.position())
    vel = list(msg.velocity())
    eff = list(msg.effort())
    print("pos[0..6] =", " ".join(f"{value:.4f}" for value in pos[:7]))
    print("vel[0..6] =", " ".join(f"{value:.4f}" for value in vel[:7]))
    print("eff[0..6] =", " ".join(f"{value:.4f}" for value in eff[:7]))

dds.create_subscription(
    pub_sub_type=RobotJointState.RobotJointStatePubSubType(),
    topic_name="robot_joint_state",
    callback=on_state,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

4.4 Tool: Actuator Fault Subscriber

Python

import time
from fourierdds_py import DDSInterface, SubscriberQosProfile
import fourier_msgs.msg.ErrorCodes as ErrorCodes

dds = DDSInterface(domain_id=123)

def on_error(msg):
    print(f"source={msg.source()} count={msg.error_codes().size()}")
    for i in range(msg.error_codes().size()):
        code = msg.error_codes()[i]
        print(f"high32=0x{code.high32():08X}, low32=0x{code.low32():08X}")

dds.create_subscription(
    pub_sub_type=ErrorCodes.ErrorCodesPubSubType(),
    topic_name="hal_actuator_error_code",
    callback=on_error,
    qos_profile=SubscriberQosProfile.default(),
)

while True:
    time.sleep(0.1)

5. Troubleshooting and Notes

5.1 Group Names Must Match Exactly

Commands are routed by group_names. Common GR3 control groups are left_leg, right_leg, waist, head, left_manipulator, and right_manipulator. If the name does not match, that group command will not take effect.

5.2 Leave 3 Seconds for DDS Discovery

After creating a publisher or client, wait about 3 seconds. If the first command is sent immediately, the subscriber or service server may not have completed discovery yet.

5.3 Send Commands After Actuator Power Is On

If the runtime is still waiting for hal_fscb_status, the actuator power path has not met the activation condition. Commands will not be sent to the physical actuators in that state.

5.4 Joint Target Units

position is in rad, velocity is in rad/s, and effort is torque or feed-forward torque. Target positions should stay within the body joint limits.