Pose Manager

Applications accumulate named poses: a home pose, a pick position, a safe waypoint. The PoseManager stores such poses under names, so that they can be looked up, iterated, and exchanged through YAML files. It stores PoseTarget values, the same type the motion methods accept, so a stored pose is directly usable as a motion target.

Creating a Pose Manager and Adding Poses

Create a manager and add poses under unique names. Both JointPose and CartesianPose values are accepted. A CartesianTarget is accepted only with the defaults of a plain pose, since a pose file stores only the pose: another coordinate system, arriving frame or a configuration vector is rejected with UnstorableTarget, and a NaN or infinite value with InvalidPose. Adding a name that already exists is an error, not a silent overwrite:

let mut manager = PoseManager::new();
manager.add("home", poses::home())?;
manager.add(
    "pickup",
    CartesianPose::new(
        Meters(0.4),
        Meters(0.1),
        Meters(0.3),
        Radians(PI),
        Radians(0.0),
        Radians(0.0),
    ),
)?;

// Read the current robot joint pose and store it.
let current = robot.joint_pose()?;
manager.add("current", current)?;
info!("Added 3 poses to the manager.");

To replace a stored pose, for example when a point is taught again, use insert(). It returns the pose it replaced, or None for a new name:

// Teach 'current' again. Unlike `add`, `insert` overwrites an existing
// name, like `BTreeMap::insert`. It returns the old pose, or `None` for a
// new name.
let previous = manager.insert("current", robot.joint_pose()?)?;
info!("Re-taught 'current', replacing {previous:?}");

Retrieving Poses

A pose is looked up by name with get(). A miss is None. Because a PoseTarget is Copy, the stored pose can be handed straight to a motion:

let home = manager
    .get("home")
    .ok_or(ExampleError::PoseMissing("home"))?;
info!("Retrieved 'home': {home:?}");

// Use a stored pose as a motion target.
robot.move_ptp(*home).start()?.join()?;

All poses are iterated as pairs of name and pose:

info!("All poses:");
for (name, pose) in &manager {
    info!("  {name}: {pose:?}");
}

Removing Poses

remove() deletes a pose and returns it. Like get(), it returns None for an unknown name:

let removed = manager.remove("current");
info!(
    "Removed 'current' ({removed:?}). Remaining: {} poses.",
    manager.len()
);

YAML Export and Import

All poses are written to a YAML file with export_yaml():

// A private directory keeps concurrent runs apart.
let temp_dir = tempdir()?;
let file_path = temp_dir.path().join("voraus_poses.yaml");
manager.export_yaml(&file_path)?;
info!("Exported poses to {}", file_path.display());

The file lists each pose with its name, its type, and its components:

poses:
  - name: home
    type: JointPose
    pose:
      - 0.0
      - -1.57
      - 1.57
      - -1.57
      - -1.57
      - 0.0

import_yaml() reads such a file into a manager. Its second argument says what happens to a name that already exists: OnConflict::Fail imports nothing and returns DuplicateName, like add(); OnConflict::KeepExisting keeps the stored pose; OnConflict::Overwrite lets the file win. A name that appears twice in the file, a NaN or infinite value, an unknown type, or an entry with the wrong number of values fails the whole import. Files with VorausPose entries import as well; these entries become a CartesianPose:

let mut imported = PoseManager::new();
imported.import_yaml(&file_path, OnConflict::Fail)?;
info!("Imported {} poses from YAML.", imported.len());

// Importing the same file again fails on the duplicate names with
// `OnConflict::Fail`; `Overwrite` lets the file win, `KeepExisting` the
// manager.
imported.import_yaml(&file_path, OnConflict::Overwrite)?;

to_yaml_string() and import_yaml_str() do the same with a string instead of a file, for poses embedded with include_str!, kept in a database or received over the network.

Full Example

Pose manager example

//! Pose Manager examples.
//!
//! Demonstrates how to use [`PoseManager`] to store, retrieve, export, and
//! import named robot poses.

mod common;

use common::poses;
use std::f64::consts::PI;
use std::io;
use tempfile::tempdir;
use thiserror::Error;
use tracing::info;
use voraus_robot_arm::pose_manager::{PoseManager, PoseManagerError};
use voraus_robot_arm::prelude::*;

/// Errors of this example: the robot, the pose manager and the temporary
/// directory have separate error types.
#[derive(Debug, Error)]
enum ExampleError {
    /// A robot operation failed.
    #[error(transparent)]
    Robot(#[from] RobotArmError),
    /// A pose manager operation failed.
    #[error(transparent)]
    PoseManager(#[from] PoseManagerError),
    /// A pose that the example added is not stored.
    #[error("the pose '{0}' is not stored")]
    PoseMissing(&'static str),
    /// Creating or removing the temporary directory failed.
    #[error(transparent)]
    Io(#[from] io::Error),
}

/// Demonstrate basic `PoseManager` operations.
///
/// The manager stores `PoseTarget`s, so a stored pose is directly usable as
/// a motion target. Its own error type covers file and YAML failures; robot
/// errors stay `RobotArmError`.
fn run_pose_manager_example(robot: &(impl GetJointPose + MovePtp)) -> Result<(), ExampleError> {
    info!("--- Pose Manager ---");

    // Create a new manager and add some poses. Adding an existing name is
    // an error rather than a silent overwrite.
    let mut manager = PoseManager::new();
    manager.add("home", poses::home())?;
    manager.add(
        "pickup",
        CartesianPose::new(
            Meters(0.4),
            Meters(0.1),
            Meters(0.3),
            Radians(PI),
            Radians(0.0),
            Radians(0.0),
        ),
    )?;

    // Read the current robot joint pose and store it.
    let current = robot.joint_pose()?;
    manager.add("current", current)?;
    info!("Added 3 poses to the manager.");

    // Teach 'current' again. Unlike `add`, `insert` overwrites an existing
    // name, like `BTreeMap::insert`. It returns the old pose, or `None` for a
    // new name.
    let previous = manager.insert("current", robot.joint_pose()?)?;
    info!("Re-taught 'current', replacing {previous:?}");

    // Look up a pose by name. A miss is `None`, like any Rust lookup.
    let home = manager
        .get("home")
        .ok_or(ExampleError::PoseMissing("home"))?;
    info!("Retrieved 'home': {home:?}");

    // Use a stored pose as a motion target.
    robot.move_ptp(*home).start()?.join()?;

    // Iterate over all poses.
    info!("All poses:");
    for (name, pose) in &manager {
        info!("  {name}: {pose:?}");
    }

    // Export to a temporary YAML file.
    // A private directory keeps concurrent runs apart.
    let temp_dir = tempdir()?;
    let file_path = temp_dir.path().join("voraus_poses.yaml");
    manager.export_yaml(&file_path)?;
    info!("Exported poses to {}", file_path.display());

    // Import from the YAML file into a fresh manager.
    let mut imported = PoseManager::new();
    imported.import_yaml(&file_path, OnConflict::Fail)?;
    info!("Imported {} poses from YAML.", imported.len());

    // Importing the same file again fails on the duplicate names with
    // `OnConflict::Fail`; `Overwrite` lets the file win, `KeepExisting` the
    // manager.
    imported.import_yaml(&file_path, OnConflict::Overwrite)?;

    // Remove a pose. A miss is `None`, like `get`.
    let removed = manager.remove("current");
    info!(
        "Removed 'current' ({removed:?}). Remaining: {} poses.",
        manager.len()
    );

    // Clean up the temp directory.
    temp_dir.close()?;
    info!("Pose manager demo complete.");
    Ok(())
}

fn main() -> Result<(), ExampleError> {
    common::init_logging();
    let robot = common::create_connected_robot()?;

    run_pose_manager_example(&robot)?;

    info!("Done.");
    Ok(())
}

Error Type

PoseManagerError

pub enum PoseManagerError {
    /// A pose with this name already exists.
    #[error("A pose named '{0}' already exists.")]
    DuplicateName(String),
    /// Reading or writing the file failed.
    #[error("I/O error on '{}': {source}", path.display())]
    Io {
        /// The file that could not be read or written.
        path: PathBuf,
        /// The underlying I/O error.
        #[source]
        source: Arc<io::Error>,
    },
    /// The file or string is not valid pose YAML.
    #[error(
        "YAML error{}: {reason}",
        .path.as_ref().map_or_else(String::new, |p| format!(" in '{}'", p.display()))
    )]
    Yaml {
        /// The file that could not be parsed or written, or `None` for
        /// YAML passed as a string.
        path: Option<PathBuf>,
        /// The underlying YAML error, as reported by the YAML library. It is
        /// text so the YAML library stays an implementation detail.
        reason: String,
    },
    /// A pose entry has the right type but unusable values.
    #[error("Pose '{name}' is invalid: {reason}")]
    InvalidPose {
        /// The name of the pose entry.
        name: String,
        /// What is wrong with it.
        reason: String,
    },
    /// A Cartesian target names a coordinate system other than the robot's,
    /// an arriving frame other than the tool flange, or a configuration
    /// vector. A pose file stores only the pose, so these would be lost.
    #[error(
        "Pose '{0}' has a coordinate system, arriving frame or configuration vector, \
         which a pose file cannot store."
    )]
    UnstorableTarget(String),
}

The methods of PoseManager are documented in the API reference, cargo doc --open -p voraus-robot-arm.