Publishing Control Commands

SDK Practice

Overview

Configure MIT impedance control parameters, enable the motors, then interpolate smoothly from each joint’s measured position to zero and hold there.

This is the basic pattern for controlling a Wuji Hand 2. The example homes the hand, but the same publisher.send() path carries teleoperation, trajectory playback, and output from a higher-level controller.

Matching examples: examples/python/wuji_hand_2/2.publish.py and examples/c/wuji_hand_2/1_control_motion.c.

For API and field definitions, see JointCommand and MitParam.

Interpolation

Sending pos=0 directly and interpolating to pos=0 are entirely different motions under MIT impedance.

MIT control produces torque proportional to position error. With the hand starting curled, the first frame at pos=0 carries an error of “current angle − 0”, and with kp=3 that becomes a large torque that snaps the hand back to zero.

Interpolating means reading the current position first, then pulling the target linearly from there to 0. Each frame’s error stays small, torque stays small with it, and the whole motion is gentle. Gentleness comes from the target trajectory, not from lowering the gains, so kp and kd keep their normal values.

Prerequisites

Hardware

This example connects to the first Wuji Hand 2 it finds (hand_devices[0]). On a shared network, change it to name a serial number.

Software

Control Parameters

These three control values match the factory defaults, and reading them back returns kp=3, kd=0.05, effort_limit=1.5.

Data Flow

Parameter Setters Broadcast

set() on effort_limit and mit_params is polymorphic:

hand.effort_limit().set(1.5)          # scalar, broadcast to every joint
hand.effort_limit().set([1.5] * 20)   # 20-element list, applied per joint
hand.mit_params().set((3.0, 0.05))    # (kp, kd) tuple, broadcast

Integration Steps

1. Connect and Configure Control Parameters

After discovering and connecting to the target Wuji Hand 2, set the effort limit and MIT parameters. Python uses parameter accessors, and C uses the corresponding Hand 2 functions. Firmware defaults to MIT mode, so no separate mode setting is needed.

• Python

• C

hand.effort_limit().set(1.5)
hand.mit_params().set((3.0, 0.05))

wuji_hand_2_set_all_effort_limit(dev, 1.5f);
float kp[20], kd[20];
for (int i = 0; i < 20; i++) { kp[i] = 3.0f; kd[i] = 0.05f; }
wuji_hand_2_set_all_mit_params(dev, kp, kd);

2. Enable and Wait for Motor Readiness

Enable is an action request. Before sending motion commands, subscribe to joint_diagnostics and confirm that all online joints reach Enabled. The Python example waits up to five seconds, exits on timeout, and closes the diagnostic subscription in finally.

hand.enable()
diag = hand.joint_diagnostics().subscribe()
try:
    deadline = time.monotonic() + 5.0
    while time.monotonic() < deadline:
        time.sleep(0.2)
        frame = diag.recv()
        if frame and frame.joints and all(e.status_word.ext_state == 2 for e in frame.joints):
            break
    else:
        raise TimeoutError("Enable timeout")
finally:
    diag.close()

C calls wuji_hand_2_enable(dev, NULL) and registers on_diag through wuji_hand_2_subscribe_joint_diagnostics(). The callback updates atomic state. The main thread waits for readiness or a timeout, then closes the diagnostic subscription. Disable and exit if the wait times out or subscription fails.

3. Create a Publisher and Send a Trajectory

Python example and C example demonstrate different motions. Python homes smoothly from the current position over 1.5 seconds, then holds zero for five seconds. C runs several sine trajectories and sends zero-position commands between them. Both send 20-joint command frames at 200 Hz.

• Python

• C

publisher = hand.joint_command().publish()
smooth_home(hand, publisher, duration=1.5)

struct WujiJointCommandPublisher *pub = NULL;
WujiStatus st = wuji_hand_2_joint_command_publish(dev, &pub);
/* After creation succeeds, send 20-joint frames in the trajectory loop. */
wuji_joint_command_publisher_send(pub, cmds);

smooth_home() is a helper in Python example. It waits for a valid state frame, interpolates from the current position, and calls publisher.send() each cycle. The C trajectory loop calculates positions and velocities over time and skips offline joints using the online bitmap. Retain the read-timeout, send-failure, and stop-request handling from Python example and C example.

4. Stop Sending and Release Resources

Python example and C example close the publisher after the send loop, then disable and disconnect. Python performs cleanup in finally. C send failures also enter the cleanup path.

• Python

• C

if publisher is not None:
    publisher.close()
hand.disable()
manager.disconnect_all()

wuji_joint_command_publisher_close(pub);
wuji_hand_2_disable(dev, NULL);
wuji_dev_disconnect(dev);
wuji_dev_release(dev);
wuji_shutdown();

Running It

• Python

• C

python examples/python/wuji_hand_2/2.publish.py

./build/hand2/1_control_motion

Measurements

On a right-hand Wuji Hand 2 with all 20 joints online:

There’s a 26x gap between enable() returning and the motors being ready. Commands sent inside that window are discarded or behave unexpectedly, which is why the wait is required.

After homing, joints don’t land exactly on 0 but within 0.02 rad. MIT impedance is compliant control, and static friction plus gravity leave a steady-state offset between actual position and target. That’s normal.

Notes

ext_state values: 0 is Init, 1 is Ready, 2 is Enabled, 3 is Stopped. Before enable every joint reads 1, and within 52 ms after enable every joint reads 2.

Adapting It

Tune How Gentle Homing Is

Change HOME_SECONDS. A longer duration makes each frame’s position step smaller and the motion gentler. Don’t chase gentleness by lowering kp, which sacrifices tracking accuracy overall.

Feed Your Own Trajectory

Replace the interpolation in step 5 with your own trajectory source, leaving the publisher.send() call unchanged. Each send() carries a complete 20-element frame, which the SDK serializes and transmits without waiting for a response, so it sustains a high-rate loop.

Choosing a Publish Rate

The example uses 200 Hz. Higher rates make trajectories smoother, provided generating one frame stays reliably faster than the period. The measurement above shows an actual rate of exactly 200.0 Hz, so there’s headroom at this scale.

Controlling Only Some Joints

Every frame still carries the full 20-element array. For joints you don’t want to move, fill in their current measured positions rather than 0. Filling 0 commands them home, which is the trap the MIT joint sweep ran into.

Related

• Running an MIT Joint Sweep: the minimal command-path check, and a warning that it pulls other joints toward zero
• Replaying a Thumb Opposition: high-rate playback of a complete recorded trajectory
• Subscribing With Callbacks: how to read joint state
• Common Data Types: JointCommand and diagnostic structures