"""Kinematics wrapper over olo.kinematics.v1.Kinematics."""
import math
import time
from typing import Optional, Sequence, Union, overload
import grpc
from olo_protos.kinematics.v1 import kinematics_pb2
from olo._convert import (
duration_to_seconds,
seconds_to_duration,
timestamp_to_datetime,
)
from olo.errors import OloAmbiguousPlanningGroup, OloInvalidArgument, wrap_rpc_error
from olo.kinematics._timeouts import (
_DEFAULT_SHORT_TIMEOUT_SEC,
_DEFAULT_TIMEOUT_SEC,
_EXECUTION_SLACK_SEC,
_MIN_REMAINING_TIMEOUT_SEC,
_RPC_OVERHEAD_SEC,
)
from olo.kinematics.ee import Gripper
from olo.kinematics.types import (
Activity,
EndEffector,
EndEffectorType,
GripperInfo,
GripperResult,
GroupState,
JointInfo,
JointStates,
JointTarget,
JointType,
KinematicsState,
MoveResult,
PlanningGroup,
PlanResult,
RobotModel,
Trajectory,
TrajectoryPoint,
)
from olo.spatial import Pose
from olo.spatial._convert import pose_from_proto, pose_to_proto
from olo.utils.common import normalize_robot_namespace
# Default fraction of max velocity/acceleration serialized on every PlanRequest.
_DEFAULT_SPEED_SCALING = 0.5
_JOINT_TYPES = {
kinematics_pb2.JOINT_TYPE_UNSPECIFIED: JointType.UNSPECIFIED,
kinematics_pb2.JOINT_TYPE_REVOLUTE: JointType.REVOLUTE,
kinematics_pb2.JOINT_TYPE_CONTINUOUS: JointType.CONTINUOUS,
kinematics_pb2.JOINT_TYPE_PRISMATIC: JointType.PRISMATIC,
kinematics_pb2.JOINT_TYPE_FIXED: JointType.FIXED,
kinematics_pb2.JOINT_TYPE_FLOATING: JointType.FLOATING,
kinematics_pb2.JOINT_TYPE_PLANAR: JointType.PLANAR,
}
_ACTIVITY = {
kinematics_pb2.ACTIVITY_UNSPECIFIED: Activity.UNSPECIFIED,
kinematics_pb2.ACTIVITY_IDLE: Activity.IDLE,
kinematics_pb2.ACTIVITY_EXECUTING: Activity.EXECUTING,
}
def _joint_states_proto(
states: Optional[JointStates],
) -> Optional[kinematics_pb2.JointStates]:
"""Build a JointStates protobuf, or None if empty."""
if states is None or not states.names:
return None
return kinematics_pb2.JointStates(
names=list(states.names),
positions=list(states.positions),
velocities=list(states.velocities),
efforts=list(states.efforts),
)
def _joint_target_proto(
target: Optional[JointTarget],
) -> Optional[kinematics_pb2.JointTarget]:
"""Build a JointTarget protobuf, or None if empty."""
if target is None or not target.names:
return None
return kinematics_pb2.JointTarget(
names=list(target.names),
positions=list(target.positions),
)
def _trajectory_proto(
trajectory: Union[Trajectory, PlanResult, kinematics_pb2.Trajectory],
) -> kinematics_pb2.Trajectory:
"""Convert a Trajectory, PlanResult, or protobuf to a Trajectory protobuf."""
if isinstance(trajectory, kinematics_pb2.Trajectory):
return trajectory
if isinstance(trajectory, PlanResult):
trajectory = trajectory.trajectory
if not isinstance(trajectory, Trajectory):
raise ValueError("trajectory must be a Trajectory, PlanResult, or Trajectory protobuf")
proto = kinematics_pb2.Trajectory(
joint_names=list(trajectory.joint_names),
planning_group=trajectory.planning_group,
)
for point in trajectory.points:
point_proto = proto.points.add()
point_proto.positions.extend(point.positions)
point_proto.velocities.extend(point.velocities)
point_proto.accelerations.extend(point.accelerations)
point_proto.efforts.extend(point.efforts)
point_proto.time_from_start.CopyFrom(seconds_to_duration(point.time_from_start))
return proto
def _trajectory_from_proto(trajectory: kinematics_pb2.Trajectory) -> Trajectory:
"""Convert a Trajectory protobuf to a Trajectory."""
return Trajectory(
joint_names=tuple(trajectory.joint_names),
planning_group=trajectory.planning_group,
points=tuple(
TrajectoryPoint(
positions=tuple(point.positions),
velocities=tuple(point.velocities),
accelerations=tuple(point.accelerations),
efforts=tuple(point.efforts),
time_from_start=duration_to_seconds(point.time_from_start),
)
for point in trajectory.points
),
)
def _end_effector_from_proto(ee: kinematics_pb2.EndEffectorInfo) -> EndEffector:
"""Convert an EndEffectorInfo protobuf to an EndEffector."""
if ee.WhichOneof("kind") != "gripper":
raise ValueError("EndEffectorInfo missing gripper kind")
info = ee.gripper
return EndEffector(
parent_group=ee.parent_group,
parent_link=ee.parent_link,
planning_group=ee.planning_group,
gripper=GripperInfo(
min_position=info.min_position,
max_position=info.max_position,
max_effort=info.max_effort,
joints=tuple(info.joints),
open_position=info.open_position,
closed_position=info.closed_position,
),
)
def _group_state_from_proto(state: kinematics_pb2.GroupState) -> GroupState:
"""Convert a GroupState protobuf to a GroupState."""
return GroupState(
name=state.name,
joints=JointTarget(
names=tuple(state.joints.names),
positions=tuple(state.joints.positions),
),
)
def _model_from_proto(model: kinematics_pb2.RobotModel) -> RobotModel:
"""Convert a RobotModel protobuf to a RobotModel."""
return RobotModel(
robot_namespace=model.robot_namespace,
default_group=model.default_group,
groups=tuple(
PlanningGroup(
name=group.name,
joints=tuple(group.joints),
end_effector_link=group.end_effector_link,
planning_frame=group.planning_frame,
group_states=tuple(
_group_state_from_proto(state) for state in group.group_states
),
)
for group in model.groups
),
joints=tuple(
JointInfo(
name=joint.name,
type=_JOINT_TYPES.get(joint.type, JointType.UNSPECIFIED),
lower_limit=joint.lower_limit,
upper_limit=joint.upper_limit,
max_velocity=joint.max_velocity,
max_effort=joint.max_effort,
)
for joint in model.joints
),
end_effectors=tuple(
_end_effector_from_proto(ee) for ee in model.end_effectors
),
)
def _plan_result(response: kinematics_pb2.PlanResponse) -> PlanResult:
"""Convert a Plan response to a PlanResult."""
return PlanResult(
trajectory=_trajectory_from_proto(response.trajectory),
planning_time=duration_to_seconds(response.planning_time),
)
def _gripper_result(response: kinematics_pb2.CommandEndEffectorResponse) -> GripperResult:
"""Convert a CommandEndEffector response to a GripperResult."""
return GripperResult(
reached_goal=response.reached_goal,
stalled=response.stalled,
position=response.position,
)
[docs]
class Kinematics:
"""Sync wrapper for the kinematics gRPC service."""
[docs]
def __init__(self, session):
"""Bind to a channel session."""
self._session = session
def _call(self, stub_method, request, timeout: float):
"""Invoke a stub method and map gRPC errors."""
try:
return stub_method(request, timeout=timeout)
except grpc.RpcError as exc:
raise wrap_rpc_error(exc) from exc
def _request(self, message_cls, robot_namespace: str = "", **fields):
"""Build a protobuf request with a normalized robot_namespace."""
return message_cls(robot_namespace=normalize_robot_namespace(robot_namespace), **fields)
def _plan_request(
self,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
avoid_obstacles: bool = False,
planning_timeout: Optional[float] = None,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
) -> kinematics_pb2.PlanRequest:
"""Build a PlanRequest shared across planning methods."""
if not (math.isfinite(speed_scaling) and 0.0 < speed_scaling <= 1.0):
raise ValueError("speed_scaling must be a finite number in (0, 1]")
request = self._request(
kinematics_pb2.PlanRequest,
robot_namespace,
avoid_obstacles=avoid_obstacles,
speed_scaling=float(speed_scaling),
)
if planning_group is not None:
request.planning_group = planning_group
if ee_link is not None:
request.ee_link = ee_link
if planning_timeout is not None:
request.planning_timeout.CopyFrom(seconds_to_duration(planning_timeout))
start = _joint_states_proto(start_state)
if start is not None:
request.start_state.CopyFrom(start)
if gripper_position is not None:
request.gripper_position = float(gripper_position)
return request
[docs]
def get_model(
self,
robot_namespace: str = "",
*,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> RobotModel:
"""Fetch kinematics model metadata for a robot."""
request = self._request(kinematics_pb2.GetModelRequest, robot_namespace)
response = self._call(
self._session.kinematics_stub.GetModel,
request,
timeout=timeout,
)
return _model_from_proto(response.model)
[docs]
def get_joints(
self,
robot_namespace: str = "",
*,
timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
) -> JointStates:
"""Fetch current joint states for a robot."""
request = self._request(kinematics_pb2.GetJointsRequest, robot_namespace)
response = self._call(
self._session.kinematics_stub.GetJoints,
request,
timeout=timeout,
)
states = response.joint_states
return JointStates(
names=tuple(states.names),
positions=tuple(states.positions),
velocities=tuple(states.velocities),
efforts=tuple(states.efforts),
sample_time=timestamp_to_datetime(response.sample_time) if response.HasField("sample_time") else None,
)
[docs]
def get_pose(
self,
robot_namespace: str = "",
*,
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> Pose:
"""Fetch the end-effector pose for a robot.
The pose is expressed in the group's planning frame (see
:attr:`PlanningGroup.planning_frame` on the robot model).
"""
request = self._request(kinematics_pb2.GetPoseRequest, robot_namespace)
if planning_group is not None:
request.planning_group = planning_group
if ee_link is not None:
request.ee_link = ee_link
response = self._call(
self._session.kinematics_stub.GetPose,
request,
timeout=timeout,
)
return pose_from_proto(response.pose)
[docs]
def get_state(
self,
robot_namespace: str = "",
*,
timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
) -> KinematicsState:
"""Fetch the executor-owned kinematics snapshot for a robot.
Activity reflects motion started by any client, not just this one.
"""
request = self._request(kinematics_pb2.GetStateRequest, robot_namespace)
response = self._call(
self._session.kinematics_stub.GetState,
request,
timeout=timeout,
)
return KinematicsState(
activity=_ACTIVITY.get(response.state.activity, Activity.UNSPECIFIED),
)
def _stop(
self,
robot_namespace: str = "",
*,
timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
) -> None:
"""Stop motion for a robot.
Underscore-prefixed for now: every SDK call is blocking, so this cannot
interrupt an in-flight move from the same thread, and there is nothing
to stop once a blocking call returns. It remains available for advanced
multi-threaded / signal-handler use until a dedicated protective-stop
channel exists.
"""
request = self._request(kinematics_pb2.StopRequest, robot_namespace)
self._call(
self._session.kinematics_stub.Stop,
request,
timeout=timeout,
)
[docs]
def plan_pose(
self,
target_pose: Pose,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
avoid_obstacles: bool = False,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a trajectory to a target pose.
``target_pose`` must be expressed in the group's planning frame (see
:attr:`PlanningGroup.planning_frame` on the robot model).
"""
request = self._plan_request(
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
avoid_obstacles=avoid_obstacles,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
)
request.pose.CopyFrom(pose_to_proto(target_pose))
response = self._call(
self._session.kinematics_stub.Plan,
request,
timeout=float(timeout if timeout is not None else planning_timeout + _RPC_OVERHEAD_SEC),
)
return _plan_result(response)
[docs]
def plan_joints(
self,
joint_positions: JointTarget,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a trajectory to target joint positions."""
request = self._plan_request(
robot_namespace=robot_namespace,
planning_group=planning_group,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
)
target = _joint_target_proto(joint_positions)
if target is None:
raise ValueError("joint_positions must include at least one joint")
request.joints.CopyFrom(target)
response = self._call(
self._session.kinematics_stub.Plan,
request,
timeout=float(timeout if timeout is not None else planning_timeout + _RPC_OVERHEAD_SEC),
)
return _plan_result(response)
[docs]
def plan_pose_sequence(
self,
poses: Sequence[Pose],
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a point-to-point sequence through poses.
Requires at least two poses. ``blend_radius`` is applied to every
non-final item; the final item always uses radius 0.
"""
if len(poses) < 2:
raise ValueError("poses must include at least two poses")
if not (math.isfinite(blend_radius) and blend_radius >= 0.0):
raise ValueError("blend_radius must be a non-negative finite number")
request = self._plan_request(
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
planning_timeout=planning_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
)
request.pose_sequence.CopyFrom(
kinematics_pb2.PoseSequenceTarget(
poses=[pose_to_proto(pose) for pose in poses],
blend_radius=float(blend_radius),
)
)
response = self._call(
self._session.kinematics_stub.Plan,
request,
timeout=float(timeout if timeout is not None else planning_timeout + _RPC_OVERHEAD_SEC),
)
return _plan_result(response)
[docs]
def plan_linear(
self,
target_pose: Pose,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a linear trajectory to a target pose.
``target_pose`` must be expressed in the group's planning frame.
"""
request = self._plan_request(
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
)
request.linear.CopyFrom(
kinematics_pb2.LinearTarget(pose=pose_to_proto(target_pose))
)
response = self._call(
self._session.kinematics_stub.Plan,
request,
timeout=float(timeout if timeout is not None else planning_timeout + _RPC_OVERHEAD_SEC),
)
return _plan_result(response)
[docs]
def plan_linear_sequence(
self,
poses: Sequence[Pose],
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a linear sequence through poses.
Requires at least two poses. ``blend_radius`` is applied to every
non-final item; the final item always uses radius 0.
"""
if len(poses) < 2:
raise ValueError("poses must include at least two poses")
if not (math.isfinite(blend_radius) and blend_radius >= 0.0):
raise ValueError("blend_radius must be a non-negative finite number")
request = self._plan_request(
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
planning_timeout=planning_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
)
request.linear_sequence.CopyFrom(
kinematics_pb2.LinearSequenceTarget(
poses=[pose_to_proto(pose) for pose in poses],
blend_radius=float(blend_radius),
)
)
response = self._call(
self._session.kinematics_stub.Plan,
request,
timeout=float(timeout if timeout is not None else planning_timeout + _RPC_OVERHEAD_SEC),
)
return _plan_result(response)
[docs]
def move_pose(
self,
target_pose: Pose,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
avoid_obstacles: bool = False,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Plan and execute a move to a target pose (in the planning frame)."""
started = time.monotonic()
result = self.plan_pose(
target_pose,
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
avoid_obstacles=avoid_obstacles,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
return self._execute_move(
result,
robot_namespace=robot_namespace,
execution_timeout=execution_timeout,
timeout=timeout,
started=started,
)
[docs]
def move_joints(
self,
joint_positions: JointTarget,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Plan and execute a move to target joint positions."""
started = time.monotonic()
result = self.plan_joints(
joint_positions,
robot_namespace=robot_namespace,
planning_group=planning_group,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
return self._execute_move(
result,
robot_namespace=robot_namespace,
execution_timeout=execution_timeout,
timeout=timeout,
started=started,
)
[docs]
def move_pose_sequence(
self,
poses: Sequence[Pose],
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Plan and execute a point-to-point sequence through poses."""
started = time.monotonic()
result = self.plan_pose_sequence(
poses,
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
blend_radius=blend_radius,
planning_timeout=planning_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
timeout=timeout,
)
return self._execute_move(
result,
robot_namespace=robot_namespace,
execution_timeout=execution_timeout,
timeout=timeout,
started=started,
)
[docs]
def move_linear(
self,
target_pose: Pose,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Plan and execute a linear move to a target pose."""
started = time.monotonic()
result = self.plan_linear(
target_pose,
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
return self._execute_move(
result,
robot_namespace=robot_namespace,
execution_timeout=execution_timeout,
timeout=timeout,
started=started,
)
[docs]
def move_linear_sequence(
self,
poses: Sequence[Pose],
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Plan and execute a linear sequence through poses."""
started = time.monotonic()
result = self.plan_linear_sequence(
poses,
robot_namespace=robot_namespace,
planning_group=planning_group,
ee_link=ee_link,
blend_radius=blend_radius,
planning_timeout=planning_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
timeout=timeout,
)
return self._execute_move(
result,
robot_namespace=robot_namespace,
execution_timeout=execution_timeout,
timeout=timeout,
started=started,
)
def _execute_move(
self,
result: PlanResult,
*,
robot_namespace: str,
execution_timeout: float,
timeout: Optional[float],
started: float,
) -> MoveResult:
"""Execute a plan result; shared tail of the move_* methods.
When the caller supplied an overall timeout it is treated as a budget
across both phases: the execute deadline is what remains after planning.
"""
remaining: Optional[float] = None
if timeout is not None:
remaining = max(_MIN_REMAINING_TIMEOUT_SEC, timeout - (time.monotonic() - started))
self.execute_trajectory(
result,
robot_namespace=robot_namespace,
execution_timeout=execution_timeout,
timeout=remaining,
)
return result
@overload
def execute_trajectory(
self,
trajectory: Trajectory,
*,
robot_namespace: str = "",
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
timeout: Optional[float] = None,
) -> None: ...
@overload
def execute_trajectory(
self,
trajectory: PlanResult,
*,
robot_namespace: str = "",
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
timeout: Optional[float] = None,
) -> None: ...
[docs]
def execute_trajectory(
self,
trajectory: Union[Trajectory, PlanResult],
*,
robot_namespace: str = "",
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
timeout: Optional[float] = None,
) -> None:
"""Execute a pre-planned trajectory (accepts a PlanResult directly).
By default the RPC deadline is derived from the trajectory's own
duration (plus slack) so long trajectories are never cancelled
mid-motion; an explicit ``timeout`` overrides it.
"""
proto = _trajectory_proto(trajectory)
if timeout is None:
duration = (
duration_to_seconds(proto.points[-1].time_from_start) if proto.points else 0.0
)
timeout = (
max(execution_timeout, duration + _EXECUTION_SLACK_SEC) + _RPC_OVERHEAD_SEC
)
request = self._request(
kinematics_pb2.ExecuteTrajectoryRequest,
robot_namespace,
trajectory=proto,
execution_timeout=seconds_to_duration(execution_timeout),
)
self._call(
self._session.kinematics_stub.ExecuteTrajectory,
request,
timeout=float(timeout),
)
[docs]
def command_end_effector(
self,
position: float,
*,
robot_namespace: str = "",
planning_group: Optional[str] = None,
max_effort: Optional[float] = None,
accept_stall: bool = False,
timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
) -> GripperResult:
"""Command the gripper end-effector to a position."""
request = self._request(
kinematics_pb2.CommandEndEffectorRequest,
robot_namespace,
)
request.gripper.position = float(position)
request.gripper.accept_stall = bool(accept_stall)
if planning_group is not None:
request.planning_group = planning_group
if max_effort is not None:
request.gripper.max_effort = float(max_effort)
response = self._call(
self._session.kinematics_stub.CommandEndEffector,
request,
timeout=timeout,
)
return _gripper_result(response)
[docs]
def handle(self, namespace: Optional[str] = None) -> "KinematicsHandle":
"""Return a namespace-scoped kinematics handle."""
return KinematicsHandle(self, self._session.resolve_namespace(namespace))
class _ModelHolder:
"""Shared mutable cache for a robot model's lazy fetch."""
__slots__ = ("model",)
def __init__(self, model: RobotModel | None = None) -> None:
self.model = model
[docs]
class KinematicsHandle:
"""Namespace-scoped robot handle for common kinematics operations.
Optionally bound to a planning group. Unbound handles resolve the sole
manipulator group lazily; multi-arm robots must call :meth:`group` first.
Model-dependent properties fetch and cache the model on first access and
may raise typed availability errors. Failed fetches are not cached.
"""
[docs]
def __init__(
self,
client: Kinematics,
namespace: str,
*,
group: str | None = None,
model: RobotModel | None = None,
_holder: _ModelHolder | None = None,
):
"""Bind to a client and normalized namespace.
The kinematics model is fetched on first model-dependent use and cached
for the handle's lifetime — including across :meth:`group` rebinding.
Failed fetches are not cached, so a later call retries. Use
:meth:`Kinematics.get_model` for a fresh fetch outside this cache.
Accessing :attr:`model`, :attr:`ee`, :attr:`planning_frame`, or
:attr:`end_effector_link` may perform an RPC and raise typed errors
such as :class:`~olo.errors.OloFailedPrecondition`.
"""
self._client = client
self.namespace = namespace
self._holder = _holder if _holder is not None else _ModelHolder(model)
if model is not None:
self._holder.model = model
self._group = group
self._group_validated = False
self._ee: Gripper | None | object = _EE_UNRESOLVED
[docs]
def group(self, name: str) -> "KinematicsHandle":
"""Return a handle bound to ``name``, sharing this handle's model cache."""
return KinematicsHandle(
self._client,
self.namespace,
group=name,
_holder=self._holder,
)
def _ensure_model(self) -> RobotModel:
"""Return the cached model, fetching it once on first use."""
if self._holder.model is None:
self._holder.model = self._client.get_model(robot_namespace=self.namespace)
model = self._holder.model
if self._group is not None and not self._group_validated:
group_names = {g.name for g in model.groups}
if self._group not in group_names:
available = ", ".join(sorted(group_names)) or "(none)"
raise OloInvalidArgument(
f"unknown planning group {self._group!r}; available: {available}"
)
self._group_validated = True
return model
def _manipulator_candidates(self) -> tuple[str, ...]:
"""Planning groups that act as manipulators for group resolution."""
model = self._ensure_model()
seen: set[str] = set()
candidates: list[str] = []
for ee in model.end_effectors:
parent = ee.parent_group
if parent and parent not in seen:
seen.add(parent)
candidates.append(parent)
if candidates:
return tuple(candidates)
return tuple(g.name for g in model.groups)
def _resolved_group(self) -> str:
"""Resolve the bound planning group, or the sole unambiguous candidate."""
if self._group is not None:
self._ensure_model()
return self._group
candidates = self._manipulator_candidates()
if len(candidates) == 1:
return candidates[0]
if not candidates:
raise OloAmbiguousPlanningGroup(
"Robot model has no planning groups to bind"
)
raise OloAmbiguousPlanningGroup(
"Multiple planning groups available "
f"({', '.join(candidates)}); call .group(name) explicitly"
)
@property
def model(self) -> RobotModel:
"""Return the cached kinematics model for this robot.
Fetches the model on first access. Failed fetches are not cached.
"""
return self._ensure_model()
@property
def ee(self) -> Gripper | None:
"""Return the gripper, fetching the model on first access."""
if self._ee is _EE_UNRESOLVED:
ee = self._ensure_model().end_effector_for(self._resolved_group())
self._ee = (
Gripper(self._client, self.namespace, ee, ee.gripper)
if ee is not None
else None
)
return self._ee # type: ignore[return-value]
@property
def planning_frame(self) -> str:
"""Return the planning frame, fetching the model on first access."""
return self.model.group(self._resolved_group()).planning_frame
@property
def end_effector_link(self) -> str:
"""Return the end-effector link, fetching the model on first access."""
return self.model.group(self._resolved_group()).end_effector_link
[docs]
def joints(self, *, timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC) -> JointStates:
"""Fetch current joint states for this robot."""
return self._client.get_joints(robot_namespace=self.namespace, timeout=timeout)
[docs]
def pose(
self,
*,
ee_link: Optional[str] = None,
timeout: float = _DEFAULT_TIMEOUT_SEC,
) -> Pose:
"""Fetch the end-effector pose for this robot."""
return self._client.get_pose(
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
timeout=timeout,
)
[docs]
def get_state(self, *, timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC) -> KinematicsState:
"""Fetch the executor-owned kinematics snapshot for this robot."""
return self._client.get_state(robot_namespace=self.namespace, timeout=timeout)
def _stop(self, *, timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC) -> None:
"""Stop motion for this robot."""
return self._client._stop(robot_namespace=self.namespace, timeout=timeout)
[docs]
def plan_pose(
self,
target_pose: Pose,
*,
ee_link: Optional[str] = None,
avoid_obstacles: bool = False,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan to a target pose for this robot."""
return self._client.plan_pose(
target_pose,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
avoid_obstacles=avoid_obstacles,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def plan_joints(
self,
joint_positions: JointTarget,
*,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan to target joint positions for this robot."""
return self._client.plan_joints(
joint_positions,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def plan_pose_sequence(
self,
poses: Sequence[Pose],
*,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a point-to-point sequence for this robot."""
return self._client.plan_pose_sequence(
poses,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
blend_radius=blend_radius,
planning_timeout=planning_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def plan_linear(
self,
target_pose: Pose,
*,
ee_link: Optional[str] = None,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a linear move for this robot."""
return self._client.plan_linear(
target_pose,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
planning_timeout=planning_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def plan_linear_sequence(
self,
poses: Sequence[Pose],
*,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> PlanResult:
"""Plan a linear sequence for this robot."""
return self._client.plan_linear_sequence(
poses,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
blend_radius=blend_radius,
planning_timeout=planning_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def move_pose(
self,
target_pose: Pose,
*,
ee_link: Optional[str] = None,
avoid_obstacles: bool = False,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Move to a target pose for this robot."""
return self._client.move_pose(
target_pose,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
avoid_obstacles=avoid_obstacles,
planning_timeout=planning_timeout,
execution_timeout=execution_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def move_joints(
self,
joint_positions: JointTarget,
*,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Move to target joint positions for this robot."""
return self._client.move_joints(
joint_positions,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
planning_timeout=planning_timeout,
execution_timeout=execution_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def move_pose_sequence(
self,
poses: Sequence[Pose],
*,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Move through a point-to-point sequence for this robot."""
return self._client.move_pose_sequence(
poses,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
blend_radius=blend_radius,
planning_timeout=planning_timeout,
execution_timeout=execution_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def move_linear(
self,
target_pose: Pose,
*,
ee_link: Optional[str] = None,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
gripper_position: Optional[float] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Move linearly to a target pose for this robot."""
return self._client.move_linear(
target_pose,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
planning_timeout=planning_timeout,
execution_timeout=execution_timeout,
start_state=start_state,
gripper_position=gripper_position,
speed_scaling=speed_scaling,
timeout=timeout,
)
[docs]
def move_linear_sequence(
self,
poses: Sequence[Pose],
*,
ee_link: Optional[str] = None,
blend_radius: float = 0.0,
planning_timeout: float = _DEFAULT_SHORT_TIMEOUT_SEC,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
start_state: Optional[JointStates] = None,
speed_scaling: float = _DEFAULT_SPEED_SCALING,
timeout: Optional[float] = None,
) -> MoveResult:
"""Move through a linear sequence for this robot."""
return self._client.move_linear_sequence(
poses,
robot_namespace=self.namespace,
planning_group=self._resolved_group(),
ee_link=ee_link,
blend_radius=blend_radius,
planning_timeout=planning_timeout,
execution_timeout=execution_timeout,
start_state=start_state,
speed_scaling=speed_scaling,
timeout=timeout,
)
@overload
def execute_trajectory(
self,
trajectory: Trajectory,
*,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
timeout: Optional[float] = None,
) -> None: ...
@overload
def execute_trajectory(
self,
trajectory: PlanResult,
*,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
timeout: Optional[float] = None,
) -> None: ...
[docs]
def execute_trajectory(
self,
trajectory: Union[Trajectory, PlanResult],
*,
execution_timeout: float = _DEFAULT_TIMEOUT_SEC,
timeout: Optional[float] = None,
) -> None:
"""Execute a trajectory for this robot."""
return self._client.execute_trajectory(
trajectory,
robot_namespace=self.namespace,
execution_timeout=execution_timeout,
timeout=timeout,
)
_EE_UNRESOLVED = object()
__all__ = [
"Activity",
"EndEffector",
"EndEffectorType",
"Gripper",
"GripperInfo",
"GroupState",
"JointInfo",
"JointStates",
"JointTarget",
"JointType",
"Kinematics",
"KinematicsHandle",
"KinematicsState",
"MoveResult",
"PlanningGroup",
"PlanResult",
"Pose",
"RobotModel",
"Trajectory",
"TrajectoryPoint",
]