"""Kinematics service types mirroring olo.kinematics.v1 messages."""
from dataclasses import dataclass
from datetime import datetime
from enum import Enum
from olo.spatial import Pose
[docs]
class EndEffectorType(str, Enum):
"""SDK-only end-effector kind derived from proto EndEffectorInfo.kind."""
GRIPPER = "gripper"
[docs]
class JointType(str, Enum):
"""Joint kinematic type, mirroring olo.kinematics.v1.JointType."""
UNSPECIFIED = "unspecified"
REVOLUTE = "revolute"
CONTINUOUS = "continuous"
PRISMATIC = "prismatic"
FIXED = "fixed"
FLOATING = "floating"
PLANAR = "planar"
class Activity(str, Enum):
"""Whether this executor currently owns an active trajectory execution."""
UNSPECIFIED = "unspecified"
IDLE = "idle"
EXECUTING = "executing"
@dataclass(frozen=True, slots=True)
class KinematicsState:
"""Executor-owned kinematics snapshot for a robot namespace."""
activity: Activity = Activity.UNSPECIFIED
@property
def executing(self) -> bool:
"""Whether a trajectory is currently executing."""
return self.activity is Activity.EXECUTING
[docs]
@dataclass(frozen=True, slots=True)
class JointStates:
"""Sampled joint states for a robot."""
names: tuple[str, ...] = ()
positions: tuple[float, ...] = ()
velocities: tuple[float, ...] = ()
efforts: tuple[float, ...] = ()
sample_time: datetime | None = None
@property
def position_by_name(self) -> dict[str, float]:
"""Map joint name to position."""
return dict(zip(self.names, self.positions))
[docs]
@dataclass(frozen=True, slots=True)
class JointTarget:
"""Target joint positions (parallel names and positions)."""
names: tuple[str, ...]
positions: tuple[float, ...]
def __post_init__(self) -> None:
if len(self.names) != len(self.positions):
raise ValueError("names and positions must have the same length")
[docs]
@dataclass(frozen=True, slots=True)
class TrajectoryPoint:
"""Single trajectory sample; time_from_start is in seconds."""
positions: tuple[float, ...] = ()
velocities: tuple[float, ...] = ()
accelerations: tuple[float, ...] = ()
efforts: tuple[float, ...] = ()
time_from_start: float = 0.0
[docs]
@dataclass(frozen=True, slots=True)
class Trajectory:
"""Joint-space trajectory."""
joint_names: tuple[str, ...] = ()
points: tuple[TrajectoryPoint, ...] = ()
planning_group: str = ""
@property
def duration(self) -> float:
"""Trajectory duration in seconds (final point's time_from_start)."""
if not self.points:
return 0.0
return self.points[-1].time_from_start
[docs]
@dataclass(frozen=True, slots=True)
class GroupState:
"""Named joint target declared by an SRDF planning group."""
name: str
joints: JointTarget
[docs]
@dataclass(frozen=True, slots=True)
class PlanningGroup:
"""MoveIt planning group metadata."""
name: str
joints: tuple[str, ...] = ()
end_effector_link: str = ""
planning_frame: str = ""
group_states: tuple[GroupState, ...] = ()
[docs]
def group_state(self, name: str) -> GroupState:
"""Resolve a named SRDF group state for this planning group."""
for state in self.group_states:
if state.name == name:
return state
raise ValueError(f"unknown group state {name!r} for group {self.name!r}")
[docs]
@dataclass(frozen=True, slots=True)
class JointInfo:
"""Joint limits and type from the robot model."""
name: str
type: JointType = JointType.UNSPECIFIED
lower_limit: float = 0.0
upper_limit: float = 0.0
max_velocity: float = 0.0
max_effort: float = 0.0
[docs]
@dataclass(frozen=True, slots=True)
class GripperInfo:
"""Gripper metadata from the robot model.
``open_position`` and ``closed_position`` are the semantic targets declared
by the robot description (SRDF group states). They are not assumed to equal
``max_position``/``min_position`` because some grippers open at the lower
joint limit.
"""
min_position: float
max_position: float
max_effort: float
joints: tuple[str, ...] = ()
open_position: float = 0.0
closed_position: float = 0.0
[docs]
@dataclass(frozen=True, slots=True)
class GripperResult:
"""Outcome of a gripper command."""
reached_goal: bool
stalled: bool
position: float
[docs]
@dataclass(frozen=True, slots=True)
class EndEffector:
"""End-effector metadata from the robot model."""
parent_group: str
parent_link: str
planning_group: str
gripper: GripperInfo
@property
def type(self) -> EndEffectorType:
"""Classify the end-effector kind."""
return EndEffectorType.GRIPPER
[docs]
@dataclass(frozen=True, slots=True)
class RobotModel:
"""Kinematics model metadata for a robot."""
robot_namespace: str = ""
default_group: str = ""
groups: tuple[PlanningGroup, ...] = ()
joints: tuple[JointInfo, ...] = ()
end_effectors: tuple[EndEffector, ...] = ()
[docs]
def group(self, name: str | None = None) -> PlanningGroup:
"""Resolve a planning group by name or the model default."""
if not self.groups:
raise ValueError("robot model has no planning groups")
if name:
for group in self.groups:
if group.name == name:
return group
raise ValueError(f"unknown planning group: {name!r}")
if self.default_group:
for group in self.groups:
if group.name == self.default_group:
return group
return self.groups[0]
[docs]
def end_effector_for(self, group: str) -> EndEffector | None:
"""Return the end effector attached to a planning group, if any."""
for ee in self.end_effectors:
if group in (ee.parent_group, ee.planning_group):
return ee
return None
[docs]
@dataclass(frozen=True, slots=True)
class PlanResult:
"""Outcome of a plan or move call; planning_time is in seconds."""
trajectory: Trajectory
planning_time: float
@property
def duration(self) -> float:
"""Duration of the planned trajectory in seconds."""
return self.trajectory.duration
MoveResult = PlanResult
__all__ = [
"Activity",
"EndEffector",
"EndEffectorType",
"GripperInfo",
"GripperResult",
"GroupState",
"JointInfo",
"JointStates",
"JointTarget",
"JointType",
"KinematicsState",
"MoveResult",
"PlanningGroup",
"PlanResult",
"Pose",
"RobotModel",
"Trajectory",
"TrajectoryPoint",
]