Adam
7bfe5cccf1
- Fix socket lifecycle: create in child process, add cleanup with try/finally - Add ClientState enum with validated state transitions to prevent invalid operations - Decouple CSV logging from network loop using queue-based CSVLogger process - Fix broken imports: change absolute (src.RSIPI.x) to relative (.x) across 7 files - Add missing @staticmethod decorator to generate_report() - Add command queue for inter-process communication (logging control) - Add 34 unit tests for XMLGenerator, SafetyManager, and trajectory_planner - Add pytest configuration to pyproject.toml - Add CLAUDE.md with architecture documentation
66 lines
2.4 KiB
Python
66 lines
2.4 KiB
Python
from .safety_manager import SafetyManager
|
|
import time
|
|
|
|
def generate_trajectory(start, end, steps=100, space="cartesian", mode="absolute", include_resets=False):
|
|
"""
|
|
Generates a trajectory from start to end across N steps.
|
|
|
|
- Absolute mode (default): full poses, no resets
|
|
- Relative mode: incremental steps, optional resets after each step
|
|
"""
|
|
if mode not in ["relative", "absolute"]:
|
|
raise ValueError("mode must be 'relative' or 'absolute'")
|
|
if space not in ["cartesian", "joint"]:
|
|
raise ValueError("space must be 'cartesian' or 'joint'")
|
|
|
|
if mode == "absolute":
|
|
include_resets = False # Smart safeguard
|
|
|
|
axes = start.keys()
|
|
trajectory = []
|
|
|
|
# Optional safety check hook — assumes SafetyManager has static validation methods
|
|
safety_fn = SafetyManager.check_cartesian_limits if space == "cartesian" else SafetyManager.check_joint_limits
|
|
global enforce_safety
|
|
enforce_safety = hasattr(SafetyManager, "check_cartesian_limits") # Enable only if those methods exist
|
|
|
|
|
|
for i in range(1, steps + 1):
|
|
point = {}
|
|
for axis in axes:
|
|
delta = end[axis] - start[axis]
|
|
value = start[axis] + (delta * i / steps)
|
|
|
|
point[axis] = delta / steps if mode == "relative" else value
|
|
|
|
# Optional safety enforcement
|
|
if enforce_safety and not safety_fn(point):
|
|
raise ValueError(f"⚠️ Safety check failed at step {i}: {point}")
|
|
|
|
trajectory.append(point)
|
|
|
|
if mode == "relative" and include_resets:
|
|
# Insert a zero-correction step to prevent drift
|
|
trajectory.append({axis: 0.0 for axis in axes})
|
|
|
|
return trajectory
|
|
|
|
def execute_trajectory(api, trajectory, space="cartesian", rate=0.004):
|
|
"""
|
|
Sends a list of corrections to the RSI API at fixed intervals.
|
|
|
|
Args:
|
|
api: An RSI-compatible API object with update_cartesian / update_joints methods.
|
|
trajectory (list[dict]): Movement steps generated by generate_trajectory().
|
|
space (str): "cartesian" or "joint".
|
|
rate (float): Time between steps in seconds (default = 4ms).
|
|
"""
|
|
for point in trajectory:
|
|
if space == "cartesian":
|
|
api.update_cartesian(**point)
|
|
elif space == "joint":
|
|
api.update_joints(**point)
|
|
else:
|
|
raise ValueError("space must be 'cartesian' or 'joint'")
|
|
time.sleep(rate)
|