🚧 Work in progress: this documentation is still being written and reviewed. Check details against the source code.

Shared motion stages

Provide shared MTC model loading, planners, pose conversion, constraints, previews, and execution. Import reads installed planning configuration; the first stage instance initializes the shared C++ MTC node.

beambot.stages.base_stages · Source

Motion planning · Guide · Symbol index

How it is used

Operation-specific stage classes inherit BaseStages to build, plan, and execute MoveIt Task Constructor tasks. The usual flow is create_task_template() → add operation stages → load_plan_execute(). Use init_and_plan() when planning must be separated from execution, as in the orchestrator’s batch/cache path.

Planner builders and pose/constraint helpers support that flow. Blocking ROS waits require another executor thread to process responses. Import still requires ROS/MTC libraries and reads installed planning YAML. _get_mtc_node() initializes rclcpp and creates the shared beambot_mtc node only when the first stage instance is constructed.

Start here

Callable	Purpose
BaseStages.create_task_template	Create a task with CurrentState, clear task-local planners and endpoint pinning, and reuse the latest RobotModel when its revision matches. Managed revisions load the dedicated robot-description pair.
BaseStages.load_plan_execute	Initialize and plan a task, then execute it unless planning failed or dry_run was requested.
BaseStages.init_and_plan	Initialize the task and request one solution, serialize its first solution when possible, adjust endpoints and optionally publish a preview. Serialization failure alone does not turn a successful plan into an error.
BaseStages.execute_solution_msg	Send a serialized solution through a node-shared execute_task_solution action client, wait for acceptance/result and decode MoveIt success.

BaseStages

Shared implementation for robot-model reuse, planner construction, task planning, previews, and execution. Subclasses provide their operation-specific run(goal) or task-composition methods; the base does not implement a generic operation. The first instance initializes the shared native node; later instances reuse it.

BaseStages.create_task_template()

Create a task with CurrentState, clear task-local planners and endpoint pinning, and reuse the latest RobotModel when its revision matches. Managed revisions load the dedicated robot-description pair.

Method · Source

create_task_template(self, name: str) -> core.Task

Parameter	Meaning
name	Human-readable MTC task name.

Returns: New MTC Task with its model and current-state stage.

Errors: Model-loading and native binding errors propagate. Tool-specific group/link checks are performed by offline model tests, not this method.

Side effects: Resets _pin_goal_joints and planner cache, may load and replace the process model cache, and creates an MTC task with introspection disabled.

BaseStages.load_plan_execute()

Initialize and plan a task, then execute it unless planning failed or dry_run was requested.

Method · Source

load_plan_execute(self, task: core.Task, dry_run: bool=False) -> str | None

Parameter	Meaning
task	MTC Task with composed stages.
dry_run	When true, publish a preview and skip execution.

Returns: None on successful planning/execution, or an explanatory error string.

Errors: Planning/execution helpers convert their handled exceptions to strings.

Side effects: Plans trajectories, stores a serialized solution, and either publishes a preview or executes robot motion.

BaseStages.init_and_plan()

Initialize the task and request one solution, serialize its first solution when possible, adjust endpoints and optionally publish a preview. Serialization failure alone does not turn a successful plan into an error.

Method · Source

init_and_plan(self, task: core.Task, dry_run: bool=False) -> str | None

Parameter	Meaning
task	MTC Task to initialize and plan.
dry_run	Publish the serialized preview when true.

Returns: None after a successful plan; error string for initialization/planning/no-solution/outer exceptions.

Errors: Catches initialization and outer planning exceptions; serialization failures are logged as warnings.

Side effects: Resets then updates last_sol_msg, plans, logs final joint positions, mutates serialized endpoints and may publish a preview.

Referenced in source (1)

• BaseStages.load_plan_execute — beambot.stages.base_stages

BaseStages.execute_solution_msg()

Send a serialized solution through a node-shared execute_task_solution action client, wait for acceptance/result and decode MoveIt success.

Method · Source

execute_solution_msg(self, sol_msg, is_replay: bool=False) -> str | None

Parameter	Meaning
sol_msg	Serialized MTC solution.
is_replay	Emit an explicit cache-replay success log when true.

Returns: None on success; descriptive error string on unavailable server, rejection, timeout, MoveIt failure or exception.

Errors: A 10 s acceptance timeout leaves acceptance unknown and does not retry. A 120 s result timeout requests cancellation and waits up to 5 s without confirming motion stopped. Exceptions become strings.

Side effects: May create/cache an action client; executes physical trajectories, requests cancellation on result timeout and logs.

Referenced in source (1)

• BaseStages.execute_solution — beambot.stages.base_stages

BaseStages.__init__()

Obtain the shared C++ MTC node, initializing it on first construction, and bind the Python logger. Choose default arm/IK frames, configured speed scales and STOMP when loaded, otherwise OMPL.

Lifecycle · Source

__init__(self, rclpy_node, arm_group: str='', ik_frame: str='')

Parameter	Meaning
rclpy_node	Owning ROS Python node.
arm_group	Planning group; empty uses ur_arm.
ik_frame	IK frame; empty uses flange.

Errors: Configuration/scale failures fall back to 0.2 velocity and acceleration fractions; native ROS initialization, node construction, and logger failures propagate.

Side effects: On first stage construction, initializes rclcpp and creates the process-shared beambot_mtc node under a lock. Initializes this instance’s planner and serialized-solution state and may load configuration.

BaseStages.make_pipeline_planner()

Reuse or create the current task’s general planner: OMPL RRTstar or the selected STOMP pipeline, with configured speed scales and 1e-4 joint-goal tolerance.

Method · Source

make_pipeline_planner(self) -> core.PipelinePlanner

Returns: Task-local PipelinePlanner.

Errors: MTC planner construction/configuration errors propagate.

Side effects: Populates the current task’s planner cache.

Referenced in source (1)

• BaseStages.make_move_to_named_stage — beambot.stages.base_stages

BaseStages.make_cartesian_planner()

Reuse or create a CartesianPath planner with configured speed scales, 0.5 mm step size and minimum path fraction 0.9.

Method · Source

make_cartesian_planner(self) -> core.CartesianPath

Returns: Task-local CartesianPath planner.

Errors: MTC binding errors propagate.

Side effects: Populates the current task’s planner cache; callers may mutate the returned shared planner.

Referenced in source (1)

• BaseStages.create_relative_move_stage — beambot.stages.base_stages

BaseStages.make_pilz_planner()

Reuse or create a Pilz planner for the requested mode with configured speed scales.

Method · Source

make_pilz_planner(self, mode: str='LIN') -> core.PipelinePlanner

Parameter	Meaning
mode	Pilz planner ID, normally LIN or PTP; default LIN.

Returns: Task-local PipelinePlanner keyed by mode.

Errors: Unsupported modes are not validated locally; MTC/plugin failures propagate.

Side effects: Populates the current task’s planner cache.

Referenced in source (2)

• BaseStages.create_relative_move_stage — beambot.stages.base_stages
• BaseStages.make_move_to_named_stage — beambot.stages.base_stages

BaseStages.make_joint_interpolation_planner()

Reuse or create a JointInterpolationPlanner with configured speed scales for simple gripper motions.

Method · Source

make_joint_interpolation_planner(self) -> core.JointInterpolationPlanner

Returns: Task-local JointInterpolationPlanner.

Errors: MTC binding errors propagate.

Side effects: Populates the current task’s planner cache.

BaseStages.create_relative_move_stage()

Build relative translation in the configured IK frame. With no explicit planner, try Pilz LIN, CartesianPath and Pilz PTP in that order. The aliases y/right mean negative local Y, while -y/left mean positive local Y.

Method · Source

create_relative_move_stage(
    self,
    label: str,
    direction: str,
    distance: float,
    planner = None,
    constraints: Constraints | None = None,
)

Parameter	Meaning
label	Display label for the stage/fallback container.
direction	Supported direction word or axis alias.
distance	Travel in metres; positivity is validated by callers, not here.
planner	Explicit planner, or None for the fallback sequence.
constraints	Optional MoveIt path constraints applied to each candidate.

Returns: MoveRelative stage for an explicit planner, otherwise a Fallbacks container.

Errors: Raises ValueError for an unknown direction; binding/planner errors propagate.

Side effects: Creates stages and may populate planner caches; does not plan or move the robot.

BaseStages.execute_solution()

Require an available task solution, then execute last_sol_msg when present or fall back to native execution of the first solution.

Method · Source

execute_solution(self, task: core.Task) -> str | None

Parameter	Meaning
task	Planned MTC Task.

Returns: None on MoveIt SUCCESS; otherwise an execution error string with decoded code where available.

Errors: Catches execution exceptions and converts them to strings; no solutions returns an error.

Side effects: Commands robot motion and logs failures; native fallback bypasses serialized endpoint adjustments.

Referenced in source (1)

• BaseStages.load_plan_execute — beambot.stages.base_stages

BaseStages.parse_poses()

Decode poses JSON without validating its root type.

Method · Source

parse_poses(self, poses_json: str) -> dict[str, Any] | None

Parameter	Meaning
poses_json	Serialized pose registry; empty input denotes no poses.

Returns: Decoded JSON value, {} for empty input, or None for invalid JSON.

Errors: Catches JSONDecodeError and logs; wrong input types can raise TypeError.

Side effects: Logs malformed JSON.

BaseStages.get_joint_pose()

Look up a named pose and require a list of exactly six elements; numeric contents are left for later conversion.

Method · Source

get_joint_pose(self, poses: dict[str, Any], pose_key: str) -> list[float] | None

Parameter	Meaning
poses	Decoded mapping of pose names to angle lists.
pose_key	Pose name to retrieve.

Returns: Original six-element degree list, or None if absent or wrong shape.

Errors: Logs absent/invalid poses; malformed container types can still raise.

Side effects: Logs failures without mutating poses.

Referenced in source (1)

• BaseStages.make_move_to_named_stage — beambot.stages.base_stages

BaseStages.make_move_to_named_stage()

Convert a six-element named degree pose into joint targets, record it for endpoint pinning and build an explicit-planner stage or Pilz PTP/general-pipeline fallbacks.

Method · Source

make_move_to_named_stage(
    self,
    label: str,
    pose_key: str,
    poses: dict[str, Any],
    planner = None,
    constraints: Constraints | None = None,
)

Parameter	Meaning
label	Display name.
pose_key	Key in the pose mapping.
poses	Named six-joint degree lists.
planner	Explicit planner; None enables Pilz PTP then general pipeline with a 5 s timeout.
constraints	Optional path constraints.

Returns: MoveTo stage or Fallbacks container; None for a missing/wrong-shape pose.

Errors: Numeric/model/planner errors propagate; lookup failures are logged and return None.

Side effects: Sets _pin_goal_joints, creates stages and may populate planner caches.

BaseStages.make_gripper_stage()

Build a MoveTo stage targeting an SRDF state in a gripper group; omit the stage when either name is empty.

Method · Source

make_gripper_stage(
    self,
    label: str,
    planner,
    gripper_group: str,
    state_name: str,
) -> stages.MoveTo | None

Parameter	Meaning
label	Stage label.
planner	Planner used for gripper motion.
gripper_group	SRDF gripper group.
state_name	SRDF group-state name.

Returns: MoveTo stage, or None for empty group/state.

Errors: Binding errors propagate; group/state existence is not validated locally.

Side effects: Creates/configures a stage or logs the skipped action.

BaseStages.compute_deterministic_ik()

Capture a live JointState, round seed positions to 0.01 degrees, request /compute_ik, and retain every configured arm joint from a successful solution.

Method · Source

compute_deterministic_ik(
    self,
    approach: PoseStamped,
    ik_frame: str,
) -> dict[str, float] | None

Parameter	Meaning
approach	Target PoseStamped, including its reference frame.
ik_frame	IK link name for the requested target.

Returns: Configured arm-joint name to radian-position mapping on success, otherwise None.

Errors: Missing joint state (1 s), service (2 s), response (5 s), unsuccessful code or incomplete solution return None with warnings. Service-phase exceptions are caught; initial subscription errors propagate.

Side effects: Creates/destroys a temporary joint-state subscription and IK service client. The blocking call allows 5 s for a response and requires another executor thread to process it.

Internal callbacks and helpers

BaseStages._set_ik_frame()

Set the stage IK frame via a PoseStamped whose header uses this stage object’s configured frame.

Internal helper · Source

_set_ik_frame(self, stage) -> None

Parameter	Meaning
stage	MTC stage to configure.

Returns: None.

Errors: Stage binding errors propagate.

Side effects: Mutates stage.ik_frame.

Referenced in source (2)

• BaseStages.create_relative_move_stage — beambot.stages.base_stages
• BaseStages.make_move_to_named_stage — beambot.stages.base_stages

BaseStages._pin_endpoints()

Overwrite the first populated trajectory point with known live joints and the last populated point with a saved named goal, without retiming or revalidating the trajectory.

Internal helper · Source

_pin_endpoints(self, sol_msg) -> None

Parameter	Meaning
sol_msg	Serialized MTC solution to modify in place.

Returns: None.

Errors: Catches all adjustment errors and logs a warning; earlier partial edits are not undone.

Side effects: Mutates first/last joint positions and zeros existing velocity/acceleration arrays.

Referenced in source (1)

• BaseStages.init_and_plan — beambot.stages.base_stages

BaseStages._pin_endpoints._set()

Replace positions for names found in a target mapping and zero any existing velocity/acceleration arrays.

Internal helper · Source

_set(pt, names, targets)

Parameter	Meaning
pt	Trajectory point to mutate.
names	Joint names aligned with the point positions.
targets	Mapping from joint name to desired position in radians.

Returns: None.

Errors: Mismatched lengths may raise IndexError, handled by the enclosing endpoint adjustment.

Side effects: Mutates the point’s position, velocity and acceleration fields.

Referenced in source (1)

• BaseStages._pin_endpoints — beambot.stages.base_stages

BaseStages._publish_preview_trajectory()

Build DisplayTrajectory from serialized subtrajectories, deriving the start state from the first populated joint trajectory and preserving original timing.

Internal helper · Source

_publish_preview_trajectory(self, sol_msg, task_name: str) -> None

Parameter	Meaning
sol_msg	Serialized solution with sub_trajectory entries.
task_name	Name used in the publication log.

Returns: None.

Errors: Catches publication/message errors and logs a warning.

Side effects: Creates/reuses a reliable transient-local publisher and publishes /beambot/preview_trajectory.

Referenced in source (1)

• BaseStages.init_and_plan — beambot.stages.base_stages

BaseStages.compute_deterministic_ik._on_js()

Store the received JointState for the enclosing IK request and signal its waiter.

Callback · Source

_on_js(msg)

Parameter	Meaning
msg	Latest JointState message.

Returns: None.

Errors: No explicit failure handling.

Side effects: Updates the closure-held message and sets its threading event.

Referenced in source (1)

• BaseStages.compute_deterministic_ik — beambot.stages.base_stages

Functions

wait_for_future()

Poll future.done without spinning an executor, using a monotonic deadline; another executor must process responses concurrently.

Function · Source

wait_for_future(future, timeout: float, poll_interval: float=0.01) -> bool

Parameter	Meaning
future	Future supporting done().
timeout	Maximum wait in seconds.
poll_interval	Sleep interval in seconds, default 0.01.

Returns: True once done, including a future completed with an exception; False on timeout.

Errors: Does not inspect future.result or cancel it. Errors from done or invalid sleep intervals propagate.

Side effects: Blocks the calling thread in short sleeps.

Referenced in source (6)

• MTCOrchestratorServer._send_and_wait — beambot.action_servers.orchestrator
• BaseStages.execute_solution_msg — beambot.stages.base_stages
• PipettorStages._execute_pipettor_action — beambot.stages.pipettor_stages
• VisionEngine._apply_collision_object — beambot.vision.vision_engine
• ROS2BridgeNode.cancel_all_goals — src.beambot.mcp_server.beambot_mcp_server
• ROS2BridgeNode.trigger_capture — src.beambot.mcp_server.beambot_mcp_server

_get_mtc_node()

Return the shared C++ MTC node, initializing it under a lock on first use.

Lifecycle · Source

_get_mtc_node()

Returns: The process-shared rclcpp.Node named beambot_mtc; later calls return the same object.

Errors: Native ROS initialization or node-construction errors propagate. If construction fails, the cached node remains unset.

Side effects: Acquires the module lock. On first successful use, calls rclcpp.init() and constructs the node with planning options prepared at module import. Does not create or execute an MTC task.

Referenced in source (1)

• BaseStages.__init__ — beambot.stages.base_stages

joints_from_degrees()

Pair each configured default joint name with the corresponding angle converted from degrees to radians.

Function · Source

joints_from_degrees(degrees: list[float]) -> dict[str, float]

Parameter	Meaning
degrees	Ordered degree values, exactly matching DEFAULT_JOINT_NAMES length.

Returns: Dictionary of joint-name to radian angle.

Errors: Raises ValueError for length mismatch; invalid numeric values raise during conversion.

Referenced in source (2)

• BaseStages.make_move_to_named_stage — beambot.stages.base_stages
• ToolExchangeStages.run — beambot.stages.tool_exchange_stages

parse_constraints()

Translate joint and orientation constraint dictionaries into MoveIt messages, converting degree angles and tolerances to radians. Orientation triples become quaternions; rotation-vector parameterization is the default.

Function · Source

parse_constraints(constraints_dict: dict[str, Any] | None) -> Constraints | None

Parameter	Meaning
constraints_dict	Optional mapping with joint_constraints and orientation_constraints lists. Joint tolerances default to 1 degree; orientation tolerances to 5 degrees per axis; weights to 1.0.

Returns: Constraints message, or None for absent input or no recognized constraints.

Errors: Missing required keys, undersized triples and invalid numeric values propagate as Python errors; no range validation is performed.

Side effects: Allocates ROS messages.

Referenced in source (6)

• MoveToStages.add_to_task — beambot.stages.move_to_stages
• PickSampleStages.run — beambot.stages.pick_sample_stages
• PlaceSampleStages.run — beambot.stages.place_sample_stages
• VisionTaskStages._execute_cartesian — beambot.stages.vision_task_stages
• VisionTaskStages._execute_joints — beambot.stages.vision_task_stages
• VisionTaskStages._move_to_scan — beambot.stages.vision_task_stages

apply_constraints()

Assign a stage’s path_constraints only when a non-None message is supplied.

Function · Source

apply_constraints(stage, constraints: Constraints | None) -> None

Parameter	Meaning
stage	MTC stage accepting path_constraints.
constraints	Constraints message or None.

Returns: None.

Errors: Attribute/type errors from the stage binding propagate.

Side effects: May mutate the stage.

Referenced in source (7)

• BaseStages.create_relative_move_stage — beambot.stages.base_stages
• BaseStages.make_move_to_named_stage — beambot.stages.base_stages
• MoveToStages.add_to_task — beambot.stages.move_to_stages
• PickSampleStages._run_vision — beambot.stages.pick_sample_stages
• PlaceSampleStages._run_vision — beambot.stages.place_sample_stages
• VisionTaskStages._execute_cartesian — beambot.stages.vision_task_stages
• VisionEngine._move_to_approach — beambot.vision.vision_engine

Internal callbacks and helpers

_load_joint_accel_limits()

Read joint_limits.yaml from sorted MoveIt config subdirectories, collecting enabled max_acceleration values and keeping the first value on conflicts.

Internal helper · Source

_load_joint_accel_limits() -> dict[str, float]

Returns: Mapping from joint name to float acceleration limit.

Errors: Config-helper failure selects cms_moveit_config; package, directory, YAML and float-conversion errors otherwise propagate.

Side effects: Reads installed YAML files and writes conflict warnings to stderr; invoked at module import.

_build_joint_limit_args()

Render acceleration enable/value overrides under a selected robot-description planning namespace.

Internal helper · Source

_build_joint_limit_args(
    limits: dict[str, float],
    robot_description: str = 'robot_description',
) -> list[str]

Parameter	Meaning
limits	Joint-name to acceleration-limit mapping.
robot_description	Parameter namespace root, robot_description by default.

Returns: Flat ROS -p argument list with two parameters per joint.

Errors: No explicit validation; values are string-interpolated.

_build_kinematics_args()

Read installed config/kinematics.yaml and emit each group setting under the requested description’s kinematics namespace.

Internal helper · Source

_build_kinematics_args(robot_description: str) -> list[str]

Parameter	Meaning
robot_description	Robot-description parameter root, including the verified-model root for managed revisions.

Returns: Flat ROS -p argument list.

Errors: Package lookup, file, YAML shape and serialization errors propagate; config-helper failure selects the default package.

Side effects: Reads kinematics YAML.

_build_cartesian_limit_args()

Read the required cartesian_limits block from installed Pilz limits and emit robot_description_planning overrides.

Internal helper · Source

_build_cartesian_limit_args() -> list[str]

Returns: Flat ROS -p argument list.

Errors: Missing file/block, malformed YAML or package lookup errors propagate.

Side effects: Reads pilz_cartesian_limits.yaml.