Servo Cartesian

SUMMARY

Servo Cartesian streams a target TCP pose to the robot controller at high frequency. It is designed to be called repeatedly in a tight loop for real-time Cartesian teleoperation or custom trajectory streaming.

UNITS

cartesian_pose is [x, y, z, rx, ry, rz] in meters and Euler XYZ degrees. speed in m/s, acceleration in m/s². time and lookahead_time in seconds, gain is unitless.

The Skill

robot.servo_cartesian(
    pose=target_pose,
    speed=0.1,
    acceleration=0.5,
    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 Cartesian in a High-Frequency Control Loop

Hold the current TCP pose and stream it at 500 Hz for 1 second.

"""
Streams TCP poses at 500 Hz to trace a small circle in the YZ plane around the current TCP pose.

Supports Universal Robots (UR).

Usage:
    python servo_cartesian.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:
    """Trace a YZ circle around the current TCP pose with servo_cartesian."""
    #===================== Create Robot ==========================================
    robot = universal_robots.UniversalRobotsUR10E(name='UR10e')

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

    # Motion parameters
    dt = 0.002              # 500 Hz servo loop
    radius = 0.02           # 2 cm circle
    period = 4.0            # seconds per revolution
    n_revolutions = 2

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

        # ==================== Run Skill ============================================
        # Read the current TCP pose as the centre of the circle.
        # The circle is offset so it "kisses" the start pose at t=0.
        center = robot.get_cartesian_pose()
        logger.info(f"Tracing YZ circle (r={radius} m) around {center}")

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

            theta = 2.0 * math.pi * t / period
            target = list(center)  # copy so writes below don't mutate `center`
            target[1] = center[1] + radius * math.cos(theta) - radius
            target[2] = center[2] + radius * math.sin(theta)

            robot.servo_cartesian(
                pose=target,
                speed=0.1,
                acceleration=0.1,
                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_cartesian 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_cartesian 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_cartesian.py --ip 192.168.1.100

For all options:

python servo_cartesian.py --help

Parameter Configuration

Parameter	Type	Default	Description
pose	list[float]	-	Target TCP pose [x, y, z, rx, ry, rz]. Position in meters, orientation in degrees.
speed	float	-	TCP linear speed in m/s.
acceleration	float	-	TCP linear acceleration in m/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 in the streamed pose but delay the robot's response to each new target, while smaller values (toward 0.03 s) track the input more tightly at the cost of a jerkier motion. gain controls how aggressively the controller corrects toward the streamed target - higher gains (toward 2000) track more closely but can amplify noise or overshoot, while lower gains (toward 100) are smoother but lag behind fast-moving targets. Tune both together: a high-frequency, low-noise input (e.g. a motion planner) tolerates a higher gain and shorter lookahead, while a noisier input (e.g. raw teleoperation) benefits from more smoothing.

Where to Use the Skill

• Real-time Cartesian teleoperation.
• Streaming poses from a motion planner or human input device at high frequency.

When Not to Use the Skill

Use a different skill instead in these cases:

• Use Set Cartesian Pose for single-waypoint blocking moves.