42 lines
1.2 KiB
Python
42 lines
1.2 KiB
Python
import numpy as np
|
|
|
|
|
|
def generate_trajectory(start, end, steps=100, space="cartesian"):
|
|
"""
|
|
Generates a linear trajectory from start to end over `steps`.
|
|
Supported spaces: 'cartesian', 'joint'
|
|
"""
|
|
if not isinstance(start, dict) or not isinstance(end, dict):
|
|
raise ValueError("Start and end must be dictionaries.")
|
|
|
|
keys = sorted(start.keys())
|
|
if keys != sorted(end.keys()):
|
|
raise ValueError("Start and end must have matching keys.")
|
|
|
|
traj = []
|
|
for i in range(steps):
|
|
point = {}
|
|
for key in keys:
|
|
point[key] = float(np.linspace(start[key], end[key], steps)[i])
|
|
traj.append(point)
|
|
|
|
return traj
|
|
|
|
|
|
def execute_trajectory(api, trajectory, space="cartesian", rate=0.012):
|
|
"""
|
|
Sends the trajectory to the robot using the RSIPI API.
|
|
- space: 'cartesian' or 'joint'
|
|
- rate: time between points in seconds (e.g., 0.012 for 12 ms)
|
|
"""
|
|
import time
|
|
|
|
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)
|