### Docker Setup for URSim

Source: https://github.com/universalrobots/urscript_examples/blob/main/socket-server/README.md

These commands set up a Docker container for the URSim simulator, mapping the 'programs' directory for persistent storage and exposing necessary ports. This allows for easy testing of URScript programs.

```bash
# Windows
docker run --rm -dit -p 5900:5900 -p 6080:6080 -v "$(pwd)/programs:/ursim/programs.UR5" --name ursim universalrobots/ursim_e-series

# Linux
docker run --rm -dit -p 5900:5900 -p 6080:6080 -v ./programs:/ursim/programs.UR5 --name ursim universalrobots/ursim_e-series
```

--------------------------------

### URScript Trajectory Initialization

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This example demonstrates how to initialize a robot trajectory from the current position using URScript, including calculating target joint velocities and torques for PD control.

```APIDOC
## URScript Trajectory Initialization

### Description
Initializes a robot trajectory from the current position, calculates target joint velocities and accelerations via the Jacobian, and computes torque using PD control with dynamics compensation.

### Method
N/A (URScript program)

### Endpoint
N/A (Local script)

### Parameters
None

### Request Example
```urscript
# Initialize trajectory from current position
time = 0
start_pos = get_target_tcp_pose()
offset_pos = pose_trans(start_pos, pose_inv(get_circle_pos(time).p))
speed_scale = 0

# Main control loop
while True:
    # Get desired trajectory state
    circle_state = get_circle_pos(time)
    next_pos = pose_trans(offset_pos, circle_state.p)
    target_q = get_inverse_kin(next_pos)

    # Compute joint velocities and accelerations via Jacobian
    inv_jac = inv(get_jacobian(target_q))
    target_qd = inv_jac * (screw_trans(offset_pos, circle_state.v) * speed_scale)
    target_qdd = inv_jac * (screw_trans(offset_pos, circle_state.a) - get_jacobian_time_derivative(target_q) * target_qd)

    # Calculate position and velocity errors
    q_err = target_q - get_actual_joint_positions()
    q_err_d = target_qd - get_actual_joint_speeds()

    # Compute torque using PD control + dynamics compensation
    torque_target = get_mass_matrix() * target_qdd + Kp * q_err + Kd * q_err_d + get_coriolis_and_centrifugal_torques()

    # Clamp and send torque command
    torque_target = clampArray(torque_target, max_torque)
    direct_torque(torque_target, friction_comp = True)

    # Advance time and ramp up speed smoothly
    time = time + speed_scale * get_steptime()
    if speed_scale < 1:
        speed_scale = speed_scale + 0.005
    end
end
```

### Response
N/A (This is a control script, not a request/response API)

### Response Example
N/A
```

--------------------------------

### Start URSim with Local Directory Mapping

Source: https://github.com/universalrobots/urscript_examples/blob/main/modbus/README.md

Commands to launch a Dockerized Universal Robots simulator with the current directory mapped to the simulator's program folder. This allows for testing URScript files directly from the host machine.

```bash
# Linux
docker run -dit --rm -p 5900:5900 -v ./:/ursim/programs/modbus --name ursim universalrobots/ursim_e-series

# Windows
docker run -dit --rm -p 5900:5900 -v "$(PWD):/ursim/programs/modbus" --name ursim universalrobots/ursim_e-series
```

--------------------------------

### Python HMI Example

Source: https://github.com/universalrobots/urscript_examples/blob/main/socket-server/README.md

This Python script serves as the Human-Machine Interface (HMI) that communicates with the UR robot controller over a socket connection. It handles sending and receiving messages to control or monitor the robot.

```python
import socket
import time

HOST = '192.168.1.100'  # The robot's IP address
PORT = 30002           # The same port as used by the server

with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
    s.connect((HOST, PORT))
    s.sendall(b'Hello, world')
    data = s.recv(1024)

print('Received', repr(data))
```

--------------------------------

### Socket Server Communication with External HMI

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This example demonstrates bidirectional TCP socket communication between a Universal Robot and an external HMI application. It facilitates real-time control and status monitoring, enabling seamless integration with custom interfaces.

```python
# URScript: Socket client connecting to external HMI

```

--------------------------------

### DMG MORI Ethernet/IP Communication

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This section details the URScript library for establishing Ethernet/IP communication with DMG MORI CNC machines. It includes functions for registering connections, initializing communication with signal mapping, starting CNC programs, waiting for program completion, controlling machine doors, and managing fixtures.

```APIDOC
## Ethernet/IP Communication - DMG MORI CNC Integration

### Description
The Ethernet/IP module enables communication with DMG MORI CNC machines using flexible adapter configuration. Requires Polyscope 5.17.0+ or PolyscopeX 10.8.0+.

### Functions

#### `dmgmori_register_connection(robot_to_cnc, cnc_to_robot, size)`
Registers a custom Ethernet/IP configuration for communication between the robot and the CNC machine.

- **robot_to_cnc** (integer) - Optional - Instance ID for robot to CNC communication. Defaults to 111.
- **cnc_to_robot** (integer) - Optional - Instance ID for CNC to robot communication. Defaults to 101.
- **size** (integer) - Optional - Size of the data instance. Defaults to 8.

#### `dmgmori_init_eip_communication(robot_to_cnc, cnc_to_robot, size)`
Initializes Ethernet/IP communication with signal mapping for both robot-to-CNC and CNC-to-robot data.

- **robot_to_cnc** (string) - Optional - Instance ID for robot to CNC communication. Defaults to "111".
- **cnc_to_robot** (string) - Optional - Instance ID for CNC to robot communication. Defaults to "101".
- **size** (integer) - Optional - Size of the data instance. Defaults to 8.

### Signal Mapping (Examples)

#### Robot-to-CNC Output Bits:
- `RobotRequestComplete`
- `AutoStart`
- `ArmInsideMachine`
- `Chuck1Unclamp`
- `Chuck1Clamp`
- `AutoDoorOpen`
- `AutoDoorClose`
- `PermitDoorUnlocking`

#### CNC-to-Robot Input Bits:
- `SpindleRotating`
- `RobotEnabled`
- `MachineAtZeroPoint`
- `Chuck1UnclampCompleted`
- `Chuck1ClampCompleted`
- `AutoDoorOpenCompleted`
- `AutoDoorCloseCompleted`
- `MachineAlarm`
- `ProgramEnd`
- `AutoOperationInProgress`

### Control Functions

#### `dmgmori_cycle_start(programNumber)`
Starts a CNC program on the machine.

- **programNumber** (integer) - The number of the program to start.

#### `dmgmori_wait_for_program_end()`
Waits until the currently running CNC program has finished.

#### `dmgmori_robot_door_open()`
Opens the machine door and waits for the operation to complete.

#### `dmgmori_robot_door_close()`
Closes the machine door and waits for the operation to complete.

#### `dmgmori_fixture_clamp(fixture)`
Clamps a specified fixture (e.g., chuck).

- **fixture** (integer) - The fixture to clamp: 0 for Main spindle, 1 for Secondary spindle, 2 for Steady rest.

### Usage Example
```python
# Register connection
dmgmori_register_connection(111, 101, 8)

# Initialize communication
dmgmori_init_eip_communication("111", "101", 8)

dmgmori_begin() # Assuming dmgmori_begin is defined elsewhere

# Open door and unclamp fixture
dmgmori_robot_door_open()
dmgmori_fixture_unclamp(0)  # Unclamp main spindle

# ... robot picks/places part ...

# Clamp fixture and close door
dmgmori_fixture_clamp(0)    # Clamp main spindle
dmgmori_end() # Assuming dmgmori_end is defined elsewhere
dmgmori_robot_door_close()

# Start CNC program and wait for completion
dmgmori_cycle_start(1)      # Start CNC program 1
dmgmori_wait_for_program_end()
```
```

--------------------------------

### Ethernet/IP Communication with DMG MORI CNC

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This Python script implements Ethernet/IP communication for DMG MORI CNC integration. It requires Polyscope 5.17.0+ or PolyscopeX 10.8.0+. The script handles connection registration, signal mapping for bidirectional data transfer, and control of CNC operations like program start and door management.

```python
# DMG MORI Ethernet/IP Communication Library

# Register custom Ethernet/IP configuration
def dmgmori_register_connection(robot_to_cnc = 111, cnc_to_robot = 101, size = 8):
  local eip_configurator = rpc_factory("xmlrpc", "http://127.0.0.1:40000/RPC2")
  if not eip_configurator.add_configuration(robot_to_cnc, size, cnc_to_robot, size):
    popup("Adding CNC instances failed")
    halt
  end
  eip_configurator.closeXMLRPCClientConnection()
  sleep(1.0)
end

global dmgmori_eip_configured = False

# Initialize Ethernet/IP communication with signal mapping
def dmgmori_init_eip_communication(robot_to_cnc = "111", cnc_to_robot = "101", size = 8):
  global hwr = eip_writer_factory(robot_to_cnc, size)
  global hrd = eip_reader_factory(cnc_to_robot, size)

  # Define robot-to-CNC output bits
  hwr.define_bit(0, 0, "RobotRequestComplete")
  hwr.define_bit(0, 1, "AutoStart")
  hwr.define_bit(0, 2, "ArmInsideMachine")
  hwr.define_bit(0, 3, "Chuck1Unclamp")
  hwr.define_bit(0, 4, "Chuck1Clamp")
  hwr.define_bit(1, 2, "AutoDoorOpen")
  hwr.define_bit(1, 3, "AutoDoorClose")
  hwr.define_bit(1, 7, "PermitDoorUnlocking")

  # Define CNC-to-robot input bits
  hrd.define_bit(0, 0, "SpindleRotating")
  hrd.define_bit(0, 1, "RobotEnabled")
  hrd.define_bit(0, 2, "MachineAtZeroPoint")
  hrd.define_bit(0, 3, "Chuck1UnclampCompleted")
  hrd.define_bit(0, 4, "Chuck1ClampCompleted")
  hrd.define_bit(1, 2, "AutoDoorOpenCompleted")
  hrd.define_bit(1, 3, "AutoDoorCloseCompleted")
  hrd.define_bit(1, 4, "MachineAlarm")
  hrd.define_bit(2, 2, "ProgramEnd")
  hrd.define_bit(2, 5, "AutoOperationInProgress")

  # Set watchdog - pause after 1s of no communication
  hrd.set_watchdog(1, 1)
  hwr.clear()
  hwr.write(struct(ArmInsideMachine = True, PermitDoorUnlocking = True))

  global dmgmori_eip_configured = True
end

# Start CNC program
def dmgmori_cycle_start(programNumber):
  hwr.write(struct(AutoStart = True))
  sleep(1)
  hwr.write(struct(AutoStart = False))

  # Wait for program to start (ProgramEnd deasserted)
  while hrd.read("ProgramEnd"):
    sleep(0.5)
  end
end

# Wait for CNC program completion
def dmgmori_wait_for_program_end():
  while not hrd.read("ProgramEnd"):
    sleep(0.5)
  end
end

# Open machine door and wait
def dmgmori_robot_door_open():
  while hrd.read("AutoOperationInProgress"):
    sleep(0.5)
  end

  hwr.write(struct(AutoDoorOpen = True, PermitDoorUnlocking = False))
  while not hrd.read("AutoDoorOpenCompleted"):
    sleep(0.5)
  end
  hwr.write(struct(AutoDoorOpen = False, ArmInsideMachine = False))
end

# Close machine door and wait
def dmgmori_robot_door_close():
  hwr.write(struct(AutoDoorClose = True))
  while not hrd.read("AutoDoorCloseCompleted"):
    sleep(0.5)
  end
  hwr.write(struct(AutoDoorClose = False, PermitDoorUnlocking = True))
end

# Clamp/unclamp fixture
# fixture: 0=Main spindle, 1=Secondary spindle, 2=Steady rest
def dmgmori_fixture_clamp(fixture):
  local clamp = struct(Chuck1Clamp = False, Chuck2Clamp = False, SteadyRestClose = False)
  clamp[fixture] = True
  hwr.write(clamp)

  while not hrd.read(["Chuck1ClampCompleted", "Chuck2ClampCompleted", "SteadyRestCloseComplete"])[fixture]:
    sleep(0.5)
  end
  clamp[fixture] = False
  hwr.write(clamp)
end

# Usage example - CNC machine tending cycle
dmgmori_register_connection(111, 101, 8)
dmgmori_init_eip_communication("111", "101", 8)

dmgmori_begin()
dmgmori_robot_door_open()
dmgmori_fixture_unclamp(0)  # Unclamp main spindle
# ... robot picks/places part ...
dmgmori_fixture_clamp(0)    # Clamp main spindle
dmgmori_end()
dmgmori_robot_door_close()
dmgmori_cycle_start(1)      # Start CNC program 1
dmgmori_wait_for_program_end()

```

--------------------------------

### Implement Admittance Control for UR e-Series

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This script provides helper functions to calculate tool flange wrenches, apply smoothing filters, and define mass-damping matrices for admittance control. It is designed for UR e-Series robots to enable force-based interaction.

```URScript
def adm_diag_6_6(values):
  return [[values[0], 0, 0, 0, 0, 0], [0, values[1], 0, 0, 0, 0], [0, 0, values[2], 0, 0, 0], [0, 0, 0, values[3], 0, 0], [0, 0, 0, 0, values[4], 0], [0, 0, 0, 0, 0, values[5]]]
end

def adm_get_flange_wrench():
  local ft = get_tcp_force()
  local t_end_base = pose_trans(get_target_tcp_pose(), pose_inv(get_tcp_offset()))
  local current_rot_in_tool = pose_inv(p[0, 0, 0, t_end_base[3], t_end_base[4], t_end_base[5]])
  local f = pose_trans(current_rot_in_tool, p[ft[0], ft[1], ft[2], 0, 0, 0])
  local t = pose_trans(current_rot_in_tool, p[ft[3], ft[4], ft[5], 0, 0, 0])
  return [f[0], f[1], f[2], t[0], t[1], t[2]]
end

def adm_apply_dead_band_smooth(wrench_in, bandwidth_force, bandwidth_torque, smooth_force, smooth_torque):
  local wrench_gain = [adm_smooth_dead_single_dim_scale(wrench_in[0], bandwidth_force, smooth_force),
                       adm_smooth_dead_single_dim_scale(wrench_in[1], bandwidth_force, smooth_force),
                       adm_smooth_dead_single_dim_scale(wrench_in[2], bandwidth_force, smooth_force),
                       adm_smooth_dead_single_dim_scale(wrench_in[3], bandwidth_torque, smooth_torque),
                       adm_smooth_dead_single_dim_scale(wrench_in[4], bandwidth_torque, smooth_torque),
                       adm_smooth_dead_single_dim_scale(wrench_in[5], bandwidth_torque, smooth_torque)]
  return wrench_gain * wrench_in
end
```

--------------------------------

### Admittance Control Implementation in URScript

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

Implements admittance control to regulate the robot's interaction forces based on measured wrench data. It transforms wrench measurements, applies a dead band, and uses an admittance model (M, D, K) for feedback control. Requires F/T sensor and specific robot controller versions.

```URScript
def admittance_control(mass_scaling = 0.5, damping_scaling = 0.5,
                       mass_list = [22.5, 22.5, 22.5, 1, 1, 1],
                       damping_list = [25, 25, 25, 2, 2, 2],
                       base_to_compliance_frame = p[0, 0, 0, 0, 0, 0],
                       tool_flange_to_compliance_center = p[0, 0, 0, 0, 0, 0],
                       dead_band = [2, 0.15, 2, 0.15],
                       compliance_vector = [1, 1, 1, 1, 1, 1],
                       stiffness_params = [0, 0, 0, 0, 0, 0],
                       target_wrench = [0, 0, 0, 0, 0, 0]):

  # Initialize mass, damping, and stiffness matrices
  local M = adm_diag_6_6(mass_list) * mass_scaling
  local D = adm_diag_6_6(damping_list) * damping_scaling
  local K = adm_diag_6_6(stiffness_params)

  # Initialize error terms for integration
  local x_e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]   # position error
  local dx_e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  # velocity error
  local ddx_e = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # acceleration error

  zero_ftsensor()  # Zero F/T sensor before starting

  while True:
    # Apply dead band filtering to measured wrench
    local wrench_at_tool = adm_apply_dead_band_smooth(adm_get_flange_wrench(),
                                                       dead_band[0], dead_band[1],
                                                       dead_band[2], dead_band[3])

    # Transform wrench to compliance center and rotate to compliance frame
    local wrench_at_compliance_center = wrench_trans(tool_flange_to_compliance_center, wrench_at_tool)
    local force_torque_error = adm_rotate_wrench_in_frame(T_compliance_frame_to_compliance_center,
                                                          wrench_at_compliance_center)

    # Apply compliance selection vector
    force_torque_error = force_torque_error * compliance_vector

    # Admittance integration: M*ddx + D*dx + K*x = F - F_target
    ddx_e = inv(M) * ((force_torque_error - target_wrench) - (K * x_e) - (D * dx_e))
    dx_e = (get_steptime() * 0.5) * (ddx_e + last_ddx_e) + last_dx_e
    x_e = (get_steptime() * 0.5) * (dx_e + last_dx_e) + last_x_e

    # Transform velocity back to TCP frame and execute motion
    local vel_target_base_tcp = adm_rotate_velocity_in_frame(get_target_tcp_pose(), vel_target_tcp)
    speedl(vel_target_base_tcp, a = 5, t = get_steptime(), aRot = 45)
  end
end

# Usage: Run admittance control on all axes
ALL_AXIS = [1, 1, 1, 1, 1, 1]
ALONG_AND_AROUND_Z_AXIS = [0, 0, 1, 0, 0, 1]
admittance_control(compliance_vector = ALL_AXIS)
```

--------------------------------

### Configure Ethernet/IP Adapter for DMG MORI Machines (URScript)

Source: https://github.com/universalrobots/urscript_examples/blob/main/ethernet-ip/README.md

This URScript function configures the Ethernet/IP adapter on the Universal Robot by creating new input and output assemblies with specified sizes. It's essential for compatibility with DMG MORI machines that require a specific configuration different from the default.

```URScript
def dmgmori_register_connection(robot_to_cnc = 111, cnc_to_robot = 101, size = 8):
  # connect to internal XMLRPC service (do not change address)
  local eip_configurator = rpc_factory("xmlrpc", "http://127.0.0.1:40000/RPC2")

  # create new input and output assembly
  if(not eip_configurator.add_configuration(robot_to_cnc, size, cnc_to_robot, size)):
    popup("Adding CNC instances failed")
    halt
  end
  eip_configurator.closeXMLRPCClientConnection()

  # wait for restart of the adapter
  sleep(1.0)
end
```

--------------------------------

### Admittance Control

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This URScript function enables admittance control, allowing the robot to react compliantly to external forces. It includes parameters for mass, damping, stiffness, and dead bands.

```APIDOC
## Admittance Control Function

### Description
Implements admittance control for compliant robot motion. The robot's behavior is defined by mass, damping, and stiffness parameters, allowing it to react to external forces.

### Method
URScript Function Definition

### Endpoint
N/A (Script Function)

### Parameters
- **mass_scaling** (float) - Optional - Scaling factor for the mass matrix.
- **damping_scaling** (float) - Optional - Scaling factor for the damping matrix.
- **mass_list** (list of floats) - Optional - Diagonal elements of the mass matrix.
- **damping_list** (list of floats) - Optional - Diagonal elements of the damping matrix.
- **base_to_compliance_frame** (pose) - Optional - Transformation from base to compliance frame.
- **tool_flange_to_compliance_center** (pose) - Optional - Transformation from tool flange to compliance center.
- **dead_band** (list of floats) - Optional - Dead band values for wrench filtering.
- **compliance_vector** (list of floats) - Optional - Vector to select compliance axes.
- **stiffness_params** (list of floats) - Optional - Diagonal elements of the stiffness matrix.
- **target_wrench** (list of floats) - Optional - Target wrench to counteract.

### Request Example
```urscript
admittance_control(compliance_vector = [1, 1, 1, 1, 1, 1])
```

### Response
N/A (This is a function definition, not an endpoint call)

#### Success Response (200)
N/A

#### Response Example
N/A
```

--------------------------------

### Wago 750 I/O Module Digital Input Reading

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

Demonstrates how to configure a Modbus signal to read 24 digital inputs from a Wago 750-series module and extract specific sensor states.

```URScript
modbus_add_signal("192.168.1.100", 255, 0, 0, "D_IN", True, 24)
sleep(1)
global inputs = modbus_get_signal_status("D_IN", False)
global optical_sensor_1 = inputs[0]
global optical_sensor_2 = inputs[3]
global magnetic_sensor_in = inputs[4]
global magnetic_sensor_out = inputs[23]
modbus_delete_signal("D_IN")
```

--------------------------------

### Python: HMI Socket Server with Tkinter GUI

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This Python script implements a socket server that communicates with a robot controller. It uses Tkinter to create a graphical user interface with a slider and buttons for sending control values to the robot. The server listens for incoming connections, receives robot status, and sends control data back.

```python
#!/usr/bin/env python3
# Python HMI Server Application with Tkinter GUI

import socket
import struct
import threading
import tkinter as tk

class SocketServerThread(threading.Thread):
    def __init__(self, port, gui):
        super().__init__(daemon=True)
        self.port = port
        self.gui = gui

    def _process(self, received):
        # Display received status from robot
        self.gui.status.config(text=received.decode('utf-8'), fg='green')

        # Return 3 INT32 values in network byte order
        return struct.pack("!iii",
            self.gui.analog_var.get(),
            self.gui.button1_var.get(),
            self.gui.button2_var.get())

    def run(self):
        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.bind(("0.0.0.0", self.port))
            s.listen()
            while True:
                conn, remote = s.accept()
                with conn:
                    while True:
                        data = conn.recv(1024)
                        if not data:
                            break
                        response = self._process(data)
                        conn.sendall(response)

# Create GUI with slider and buttons
window = tk.Tk()
window.analog_var = tk.IntVar()
window.button1_var = tk.IntVar()
window.button2_var = tk.IntVar()

tk.Scale(window, orient=tk.HORIZONTAL, label="Analog",
         variable=window.analog_var, length=300).pack()
tk.Checkbutton(window, text="Button 1", variable=window.button1_var).pack()
tk.Checkbutton(window, text="Button 2", variable=window.button2_var).pack()
window.status = tk.Label(window, text='No connection', fg='red')
window.status.pack()

# Start server and GUI
server = SocketServerThread(port=60000, gui=window)
server.start()
window.mainloop()
```

--------------------------------

### Impedance Control (Joint-Space PD Torque Controller) in Python

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

Implements a joint-space PD torque controller with dynamics compensation for e-Series robots. It automatically detects robot size to adjust parameters and enables compliant circular motion. Requires Polyscope 5.23+ or PolyscopeX 10.10+.

```Python
# Joint-Space PD Torque Controller with Dynamics Compensation
# Requires Polyscope 5.23+ or PolyscopeX 10.10+

# Maximum joint torques for UR3 (Nm) - used as reference
max_torque_ur3_spec = [56.0, 56.0, 28.0, 9.0, 9.0, 9.0]

# Auto-detect robot size by reach and configure parameters
robot_reach = get_forward_kin([0,0,0,0,0,0])
reach = norm(robot_reach[0])

if reach < 0.5:
    # UR3e configuration
    max_torque = max_torque_ur3_spec / 4
    Kp = [1000, 1000, 500, 100, 100, 100] * 0.7      # Proportional gains
    Kd = [2*sqrt(Kp[0]), 2*sqrt(Kp[1]), 2*sqrt(Kp[2]), 2*sqrt(Kp[3]), 2*sqrt(Kp[4]), 2*sqrt(Kp[5])]
    radius = 0.15
    rad_per_sec = 1.5
elif reach < 1.0:
    # UR5e configuration
    max_torque = max_torque_ur3_spec / 2
    Kp = [1000, 1000, 500, 100, 100, 100]
    Kd = [2*sqrt(Kp[0]), 2*sqrt(Kp[1]), 2*sqrt(Kp[2]), 2*sqrt(Kp[3]), 2*sqrt(Kp[4]), 2*sqrt(Kp[5])]
    radius = 0.25
    rad_per_sec = 1.5
else:
    # UR10e or larger
    max_torque = max_torque_ur3_spec
    Kp = [1000, 1000, 500, 100, 100, 100] * 1.3
    Kd = [2*sqrt(Kp[0]), 2*sqrt(Kp[1]), 2*sqrt(Kp[2]), 2*sqrt(Kp[3]), 2*sqrt(Kp[4]), 2*sqrt(Kp[5])] * 1.3
    radius = 0.35
    rad_per_sec = 1.5
end

# Clamp array elements to +/- limits
def clampArray(value, clampValue):
    ret = value
    j = 0
    while j < length(value):
        if value[j] - clampValue[j] > 0:
            ret[j] = clampValue[j]
        elif value[j] + clampValue[j] < 0:
            ret[j] = -clampValue[j]
        end
        j = j + 1
    end
    return ret
end

# Generate circular trajectory at given time
def get_circle_pos(time):
    circ_pose = p[radius * cos(rad_per_sec * time), radius * sin(rad_per_sec * time), 0, 0, 0, 0]
    circ_vel = [radius * -sin(rad_per_sec * time), radius * cos(rad_per_sec * time), 0, 0, 0, 0]
    circ_acc = [radius * -cos(rad_per_sec * time), radius * -sin(rad_per_sec * time), 0, 0, 0, 0]
    return struct(p = circ_pose, v = circ_vel, a = circ_acc)
end

```

--------------------------------

### URScript: Initialize and Control Trajectory from Current Position

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This URScript code initializes a trajectory based on the robot's current position and enters a main control loop. It calculates target joint positions, velocities, and accelerations using inverse kinematics and Jacobian transformations, then computes torque commands via PD control and dynamics compensation. The script continuously updates the robot's motion and smoothly ramps up speed.

```URScript
# Initialize trajectory from current position
time = 0
start_pos = get_target_tcp_pose()
offset_pos = pose_trans(start_pos, pose_inv(get_circle_pos(time).p))
speed_scale = 0

# Main control loop
while True:
    # Get desired trajectory state
    circle_state = get_circle_pos(time)
    next_pos = pose_trans(offset_pos, circle_state.p)
    target_q = get_inverse_kin(next_pos)

    # Compute joint velocities and accelerations via Jacobian
    inv_jac = inv(get_jacobian(target_q))
    target_qd = inv_jac * (screw_trans(offset_pos, circle_state.v) * speed_scale)
    target_qdd = inv_jac * (screw_trans(offset_pos, circle_state.a) - get_jacobian_time_derivative(target_q) * target_qd)

    # Calculate position and velocity errors
    q_err = target_q - get_actual_joint_positions()
    q_err_d = target_qd - get_actual_joint_speeds()

    # Compute torque using PD control + dynamics compensation
    torque_target = get_mass_matrix() * target_qdd + Kp * q_err + Kd * q_err_d + get_coriolis_and_centrifugal_torques()

    # Clamp and send torque command
    torque_target = clampArray(torque_target, max_torque)
    direct_torque(torque_target, friction_comp = True)

    # Advance time and ramp up speed smoothly
    time = time + speed_scale * get_steptime()
    if speed_scale < 1:
        speed_scale = speed_scale + 0.005
    end
end
```

--------------------------------

### Festo CMMT-AS Servo Drive Modbus Control Library

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This library provides functions for controlling Festo CMMT-AS servo drives via Modbus. It utilizes telegram 111 protocol for communication, enabling operations such as position control, homing, and status monitoring. Global buffers are defined for command and status exchange.

```Python
# Festo CMMT-AS Servo Drive Modbus Control Library

# Global buffers for telegram 111 communication
global cmd_buffer = [1086, 0, 0, 0, 16384, 0, 0, 0, 0, 16384, 16384]
global stat_buffer = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

```

--------------------------------

### Festo CPX Modbus Library for I/O Communication

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This Python library facilitates Modbus communication with Festo CPX-AP modbus couplers. It supports connecting to digital output coils, digital inputs, and analog input channels. Functions are provided to set output states, read input states, retrieve analog values, and check module counts. Requires Polyscope 5.14.0 or later.

```Python
# Festo CPX Modbus Library for I/O Communication

# Connect to output coils on Festo CPX module
# Creates signals for 32 output coils and 4 input coils
def festo_connect_coil_io(cpx_io_ip, timeout):
  # Add 32 output coils (4 DO, 4 gap, 24 valves)
  modbus_add_signal(cpx_io_ip, 255, 0, 1, "CPX_DO", True, 32)
  # Add 4 digital inputs
  modbus_add_signal(cpx_io_ip, 255, 0, 0, "CPX_DI", True, 4)

  local i = 0
  while i < timeout * 10:
    if modbus_get_error("CPX_DO") == 0:
      global coil_outputs = modbus_get_signal_status("CPX_DO", False)
      textmsg("FESTO: connect t=", i)
      return True
    end
    sleep(0.1)
    i = i + 1
  end
  return False
end

# Connect to 4 analog input channels (CPX-AP-I-4AI-U-I-RTD module)
def festo_connect_analog_in(cpx_io_ip, timeout):
  # Read holding registers 5000-5003 for analog values
  modbus_add_signal(cpx_io_ip, 255, 5000, 3, "CPX_RTDI", True, 4)
  modbus_set_signal_watchdog("CPX_RTDI", 30)  # 30s watchdog for less critical signals

  local i = 0
  while i < timeout * 10:
    if modbus_get_error("CPX_RTDI") == 0:
      return True
    end
    sleep(0.1)
    i = i + 1
  end
  return False
end

# Read number of connected I/O modules
def festo_get_module_count(cpx_io_ip, timeout):
  modbus_add_signal(cpx_io_ip, 255, 12000, 3, "CPX_NMOD", True, 1)
  modbus_set_signal_update_frequency("CPX_NMOD", -1)  # Async - read only once

  local i = 0
  while i < timeout * 10:
    if modbus_get_error("CPX_NMOD") == 0:
      return modbus_get_signal_status("CPX_NMOD")
    end
    sleep(0.1)
    i = i + 1
  end
  return 0
end

# Set digital output coil
def festo_set_output_coil(number, state):
  coil_outputs[number] = state
  modbus_set_output_signal("CPX_DO", coil_outputs, False)
end

# Read digital input coil
def festo_get_input_coil(number):
  global coil_inputs = modbus_get_signal_status("CPX_DI", False)
  return coil_inputs[number]
end

# Read analog input with error handling
def festo_get_analog_in(number):
  if modbus_get_time_since_signal_invalid("CPX_RTDI") > 30:
    return -1  # Error condition
  end
  local analog_in = modbus_get_signal_status("CPX_RTDI", False)
  return analog_in[number]
end

# Clean up all created signals
def festo_delete_all():
  modbus_delete_signal("CPX_DO")
  modbus_delete_signal("CPX_DI")
  modbus_delete_signal("CPX_RTDI")
  modbus_delete_signal("CPX_NMOD")
  modbus_delete_signal("CPX_DIAG")
end

# Usage example
festo_connect_coil_io("192.168.1.100", 5.0)
festo_connect_analog_in("192.168.1.100", 5.0)

# Set output 0 to True
festo_set_output_coil(0, True)

# Read input 0 state
input_state = festo_get_input_coil(0)

# Read analog channel 0
analog_value = festo_get_analog_in(0)

```

--------------------------------

### URScript: Robot Follower via Modbus

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This URScript code configures a follower robot to replicate the joint positions of a leader robot using Modbus communication. It reads joint angles from the leader robot via Modbus signals and uses the `servoj` function for smooth, real-time motion replication. Requires Modbus service to be enabled on the leader robot and proper signal mapping configured.

```urscript
# Follower Robot Configuration
# Requires Polyscope 5.19+ and Modbus service enabled on leader

# Configure Modbus to read leader robot's joint positions
# Leader robot's Modbus registers contain joint angles
# Register addresses for actual_q (joint positions) vary by robot model

# In Polyscope Installation -> Fieldbus -> Modbus:
# - Add leader robot IP address
# - Configure signal mapping for joint position registers

# Main follower control loop
def follower_control():
  # Read leader joint positions via Modbus
  # Actual register addresses depend on leader robot configuration
  leader_q = modbus_get_signal_status("leader_joints", False)

  # Optional: Apply mirror transformation for facing robots
  def mirror_joints(q):
    return [q[0], -q[1], -q[2], q[3], -q[4], q[5]]
  end

  # Enable mirror mode if robots face each other
  mirror_mode = False

  while True:
    # Read current leader position
    leader_q = modbus_get_signal_status("leader_joints", False)

    if mirror_mode:
      target_q = mirror_joints(leader_q)
    else:
      target_q = leader_q
    end

    # Use servoj for smooth real-time following
    # t: time to reach position (typically 0.008s = 125Hz)
    # lookahead_time: smoothing parameter (0.03-0.2s)
    # gain: position gain (100-2000)
    servoj(target_q, t=0.008, lookahead_time=0.1, gain=300)
  end
end

follower_control()
```

--------------------------------

### Festo CMMT-AS Servo Drive Modbus Control

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This library facilitates control of Festo CMMT-AS servo drives via Modbus, supporting position control, homing, and status monitoring using the telegram 111 protocol.

```APIDOC
## Modbus Communication - Festo CMMT-AS Servo Drive

### Description
The Festo servo library enables control of CMMT-AS servo drives via Modbus, supporting position control, homing, and status monitoring using telegram 111 protocol.

### Method
N/A (URScript library)

### Endpoint
N/A (Modbus communication)

### Parameters
None

### Request Example
```python
# Festo CMMT-AS Servo Drive Modbus Control Library

# Global buffers for telegram 111 communication
global cmd_buffer = [1086, 0, 0, 0, 16384, 0, 0, 0, 0, 16384, 16384]
global stat_buffer = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
```

### Response
N/A (This is a library of functions, not a request/response API)
```

--------------------------------

### Socket Server Communication - External HMI

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This section outlines the URScript code for establishing a TCP socket client connection to an external HMI application. This enables bidirectional communication for real-time control and status monitoring.

```APIDOC
## Socket Server Communication - External HMI

### Description
The socket server example demonstrates bidirectional TCP socket communication between a Universal Robot and an external HMI application for real-time control and status monitoring.

### Language
URScript

### Endpoint
(Not applicable for client-side script, connection details are configured within the script)

### Parameters
(Configuration parameters are typically defined within the script, e.g., IP address, port)

### Request Body
(Not applicable for this client-side script)

### Response
(Not applicable for this client-side script)

### Code Snippet Example
```python
# URScript: Socket client connecting to external HMI
# Note: Actual implementation details like IP address, port, and data format
# would be specific to the HMI application and are not fully detailed here.

# Example of initializing a socket connection (conceptual)
# socket_client = socket_factory("192.168.1.100", 5000)

# Example of sending data (conceptual)
# socket_client.send_data(struct(robot_status = "moving", position = get_actual_tcp_pose()))

# Example of receiving data (conceptual)
# received_data = socket_client.receive_data()
# if received_data:
#   process_hmi_command(received_data)

# Example of closing the connection (conceptual)
# socket_client.close_connection()
```
```

--------------------------------

### Impedance Control (Joint-Space PD Torque Controller)

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This URScript code implements an impedance control strategy using a joint-space PD torque controller with dynamics compensation. It automatically adjusts parameters based on the robot's reach.

```APIDOC
## Impedance Control (Joint-Space PD Torque Controller)

### Description
Implements a joint-space PD torque controller with dynamics compensation for e-Series robots. Automatically adjusts parameters based on robot reach for compliant circular motion.

### Method
URScript Script

### Endpoint
N/A (Script Execution)

### Parameters
- **max_torque_ur3_spec** (list of floats) - Reference maximum joint torques for UR3.
- **robot_reach** (float) - Calculated robot reach.
- **Kp** (list of floats) - Proportional gains for the PD controller.
- **Kd** (list of floats) - Derivative gains for the PD controller.
- **radius** (float) - Radius of the circular trajectory.
- **rad_per_sec** (float) - Angular velocity for the circular trajectory.

### Request Example
```urscript
# Example usage within a script:
# ... (robot reach detection and parameter setup)
# ... (call to impedance control logic)
```

### Response
N/A (This is a script that executes robot control)

#### Success Response (200)
N/A

#### Response Example
N/A
```

--------------------------------

### URScript: Real-time HMI Socket Communication

Source: https://context7.com/universalrobots/urscript_examples/llms.txt

This URScript function establishes a socket connection to an HMI server, continuously sending robot status and receiving control values. It handles reading binary integer data and uses received values to control robot movements. Requires a running HMI server on the specified IP and port.

```urscript
def socket_communication():
  # Open connection to HMI server
  socket_open("192.168.1.10", 60000, "hmi_socket")

  while True:
    # Send current robot status to HMI
    socket_send_string("Robot Ready", "hmi_socket")

    # Receive 3 INT32 values from HMI (analog slider, button1, button2)
    # Each INT32 is 4 bytes in network byte order
    local response = socket_read_binary_integer(3, "hmi_socket", 1.0)

    if response[0] == 3:  # Successfully received 3 values
      local analog_value = response[1]
      local button1 = response[2]
      local button2 = response[3]

      # Use values to control robot behavior
      if button1 == 1:
        # Perform action for button 1
        movel(waypoint_1)
      end
      if button2 == 1:
        # Perform action for button 2
        movel(waypoint_2)
      end
    end

    sleep(0.1)  # Communication cycle rate
  end

  socket_close("hmi_socket")
end
```