Servo Joint

SUMMARY

Servo Joint streams a target joint configuration to the robot controller at high frequency (typically 500 Hz). It is designed to be called repeatedly in a tight control loop for custom real-time trajectories or teleoperation.

UNITS

joint_positions in degrees. speed in deg/s, acceleration in deg/s². time and lookahead_time in seconds, gain is unitless.

The Skill

robot.servo_joint(
    q=target_joints,
    speed=30.0,
    acceleration=40.0,
    time=0.002,
    lookahead_time=0.1,
    gain=300,
)

The Code

Safety first!

A real robot will faithfully do whatever you ask of it - so please take a moment to clear the workspace, keep an E-Stop within reach, and be ready to disconnect.

Operating real hardware is at your own risk.

Example: Servo Joint in a High-Frequency Control Loop

Stream joint positions to the robot at 125 Hz for 2 full oscillations of the base joint (j0) with a ±2 deg amplitude, while holding all other joints at their initial values.

"""
Streams joint targets at 125 Hz to oscillate the base joint (j0) sinusoidally with servo_joint.

Supports Universal Robots (UR).

Usage:
    python servo_joint.py [--ip <ROBOT_IP>]
"""

import argparse
import math
import time

from loguru import logger

from telekinesis.synapse.robots.manipulators import universal_robots


def main(ip: str) -> None:
    """Oscillate the base joint (j0) sinusoidally with servo_joint."""
    #===================== Create Robot ==========================================
    robot = universal_robots.UniversalRobotsUR10E(name='UR10e')

    # Servo-loop and trajectory parameters
    dt = 0.008          # 125 Hz servo loop
    amplitude = 2.0     # ±2 deg base oscillation
    period = 4.0        # seconds per full oscillation
    n_cycles = 2

    # ==================== Visualization (Optional) =============================
    # Live: subscribes to the robot's state topic and redraws as it moves.
    robot.visualize_rerun(live=True)

    try:
        #===================== Connect Robot ==========================================
        if ip:
            robot.connect(ip=ip)

        # ==================== Run Skill ============================================
        # Hold all joints at their current values; only j0 will be modulated.
        # Sine starts at 0, so the first target equals ``center`` exactly —
        # no step at t=0.
        center = robot.get_joint_positions()
        logger.info(
            f"Oscillating j0 by ±{amplitude} deg around {center[0]:.3f} deg "
            f"({n_cycles} cycles, {period}s each)"
        )

        duration = period * n_cycles
        t0 = time.monotonic()
        while True:
            t = time.monotonic() - t0
            if t >= duration:
                break

            theta = 2.0 * math.pi * t / period
            target = list(center)
            target[0] = center[0] + amplitude * math.sin(theta)

            # speed/acceleration are not used by UR's servoJ when time > 0,
            # but pass sane non-zero values to avoid controller-side validation
            # rejecting the script.
            robot.servo_joint(
                q=target,
                speed=60.0,
                acceleration=80.0,
                time=dt,
                lookahead_time=0.1,
                gain=300,
            )

            # Pace the loop. Sleep the remainder of this dt window.
            next_tick = t0 + (math.floor(t / dt) + 1) * dt
            sleep_for = next_tick - time.monotonic()
            if sleep_for > 0:
                time.sleep(sleep_for)

        robot.servo_stop()
        logger.success("servo_joint loop complete.")
    except (ConnectionError, OSError) as e:
        logger.error(f"Error occurred: {e}")
    finally:
        robot.disconnect()
        robot.shutdown()


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="UR10e servo_joint example")
    parser.add_argument("--ip", type=str, default=None,
                         help="UR robot IP address for real hardware, e.g. 192.168.1.100")
    args = parser.parse_args()

    main(ip=args.ip)

Running the Example

Real

python servo_joint.py --ip 192.168.1.100

For all options:

python servo_joint.py --help

Parameter Configuration

Parameter	Type	Default	Description
q	list[float]	-	Target joint positions in degrees.
speed	float	-	Joint speed in deg/s.
acceleration	float	-	Joint acceleration in deg/s².
time	float	-	Command execution time in seconds. Overrides speed if non-zero.
lookahead_time	float	-	Smoothing horizon in seconds, range [0.03, 0.2].
gain	float	-	Proportional gain, range [100, 2000].

Returns

Value	Description
None	This skill returns nothing.

Raises

Exception	Condition
RuntimeError	The robot is not connected.

How to Tune the Parameters

lookahead_time trades smoothness for latency: larger values (toward 0.2 s) smooth out jitter between successive targets but delay the robot's response, while smaller values (toward 0.03 s) track the streamed trajectory more tightly at the cost of jerkier motion. gain sets how aggressively the controller corrects toward the streamed target - higher gains (toward 2000) track more closely but can amplify noise, while lower gains (toward 100) are smoother but lag behind fast-changing targets. A clean, high-frequency trajectory source (e.g. a pre-computed motion profile) tolerates a higher gain and shorter lookahead; a noisier source (e.g. live teleoperation input) benefits from more smoothing on both.

Where to Use the Skill

• Real-time trajectory streaming (e.g., teleoperation, model-predictive control).
• Custom high-frequency motion profiles.

When Not to Use the Skill

Use a different skill instead in these cases:

• Use Set Joint Positions for single-waypoint blocking moves; servo commands require a continuous high-frequency loop to function correctly.