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.

SUPPORTED ROBOTS

Currently supported only on Universal Robots.

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.

"""
Servo Joint example for the Synapse SDK.

Streams joint targets at 125 Hz to oscillate the base joint (j0) around
the current joint configuration using ``servo_joint`` — a smooth sinusoidal
motion of small amplitude. Only the base moves; all other joints are held
at their starting values.

Currently supported only for Universal Robots (UR10e).

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(robot_ip: str):
    """Oscillate the base joint (j0) sinusoidally with servo_joint."""

    # 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

    # Create robot instance
    robot = universal_robots.UniversalRobotsUR10E()

    # Connect to the robot
    robot.connect(ip=robot_ip)

    try:
        # 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.")
    finally:
        robot.disconnect()


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="UR10e servo_joint example")
    parser.add_argument("--ip", type=str, required=True, help="IP address of the UR robot")
    args = parser.parse_args()

    main(args.ip)

The Explanation of the Code

servo_joint streams a joint target to the controller at high frequency. q is the target joint configuration in degrees. speed and acceleration are in deg/s and deg/s². time is the command execution time in seconds - if non-zero, it overrides speed. lookahead_time (range [0.03, 0.2] s) controls trajectory smoothing: larger values smooth the path more but increase latency. gain (range [100, 2000]) is the proportional gain. Always call servo_stop() after exiting the loop.

How to Tune the Parameters

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].

Return Value

Value	Description
None	This skill returns nothing.

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.