Pose Conversion

A joint pose determines where the tool center point (TCP) of the robot is, and a Cartesian pose of the TCP is reached by one or more joint poses. Pose conversion computes one from the other with the robot’s kinematics, without moving the robot.

Joint Pose to Cartesian Pose

joint_pose_to_cartesian() converts a joint pose into the Cartesian pose of a tool center point. The tool center point is given as a ToolTransformation: ToolTransformation::FLANGE is the flange itself, and ToolTransformation::new() takes the pose of a tool endpoint in flange coordinates. with_offset() adds an offset from that endpoint, given in tool coordinates. Pass a ToolTransformation for a tool that is not configured on the robot; for the selected tool, use joint_pose_to_tcp() below. The last argument is the coordinate system of the result, the robot coordinate system or a user coordinate system. The tool and camera coordinate systems are not supported and fail with RobotArmError::PoseConversionFailed:

let (flange_pose, configuration) =
    robot.joint_pose_to_cartesian(home, ToolTransformation::FLANGE, Cs::Robot)?;
info!("Flange pose: {flange_pose}, configuration: {configuration}");

// A tool reaching 10 cm along the flange z axis.
let tool = ToolTransformation::new(z(Meters(0.1)));
let (tool_pose, _configuration) = robot.joint_pose_to_cartesian(home, tool, Cs::Robot)?;

Besides the pose, the conversion returns the configuration vector of the joint pose, a ConfigurationVector. Most Cartesian poses are reached by several joint poses, for example with the elbow above or below the line from shoulder to wrist. The robot’s kinematics tells these solutions apart at its singularities, and the configuration vector holds one ConfigurationFlag, Positive or Negative, per singularity. The flags do not correspond to axes, and how many a robot uses depends on its kinematics: three (shoulder, elbow and wrist) for a six-axis arm, two (shoulder and elbow) for a palletizer, one (elbow) for a SCARA and none for a gantry. A ConfigurationVector holds at most three flags, and a vector the robot returns holds three; the flags the robot does not use are Positive.

Which solution a flag value selects depends on the axis convention of the robot; the ConfigurationFlag API documentation and the Configuration Vector section list the flags of a six-axis arm with a spherical wrist. The simplest way to find the flags of a solution is to move the robot there, or to take a joint pose you know, and read the vector back from this conversion.

joint_pose_to_tcp() converts into the pose of the TCP of the selected tool, or of the flange when no tool is selected:

let (tcp_pose, _) = robot.joint_pose_to_tcp(home, Cs::Robot)?;

Cartesian Pose to Joint Pose

cartesian_pose_to_joint() converts a Cartesian pose of a tool center point into the joint pose that reaches it. The configuration vector selects which of the joint solutions is returned; the vector returned by joint_pose_to_cartesian() leads back to the joint pose it was computed from:

let joint_pose = robot.cartesian_pose_to_joint(tool_pose, configuration, tool, Cs::Robot)?;

ConfigurationVector::new() builds a vector from the flags the robot uses, in the order of its kinematics, and fills the rest with Positive. A solution is only usable if its joint positions lie within the axis limits, so a flipped flag can lead to a joint pose the robot cannot move to:

let wrist_flipped = ConfigurationVector::new(&[
    ConfigurationFlag::Positive,
    ConfigurationFlag::Positive,
    ConfigurationFlag::Negative,
])?;
info!("Hand-built configuration: {wrist_flipped}");

// The same tool pose with the hand-built configuration. The robot
// control can reject the configuration, so the conversion can fail.
match robot.cartesian_pose_to_joint(tool_pose, wrist_flipped, tool, Cs::Robot) {
    Ok(joint_pose) => info!("Joint pose, hand-built configuration: {joint_pose}"),
    Err(RobotArmError::PoseConversionFailed(reason)) => info!("Not converted: {reason}"),
    Err(error) => return Err(error),
}

tcp_pose_to_joint() does the same for a pose of the selected tool’s TCP.

A pose the robot cannot reach has no joint pose. The conversion then fails with RobotArmError::PoseConversionFailed, which carries the reason the robot control reports. Catch it to check a pose before moving to it:

let unreachable = tool_pose + x(Meters(10.0));
match robot.cartesian_pose_to_joint(unreachable, configuration, tool, Cs::Robot) {
    Err(RobotArmError::PoseConversionFailed(reason)) => info!("Not converted: {reason}"),
    other => {
        other?;
    }
}

Configuration Vector

A Cartesian pose fixes where the TCP is and how it is oriented, but not how the arm is posed to get there. A six-axis arm can usually reach the same TCP pose with the elbow above or below the line from shoulder to wrist, with the arm pointing towards the target or reaching backwards over its base, and with the wrist bent one way or the other. Between two such postures lies a singularity of the kinematics, a joint pose in which the two solutions meet. The configuration vector picks one side of each singularity: it holds one ConfigurationFlag per singularity the robot’s kinematics resolves, not one per axis.

For a six-axis arm with a spherical wrist the robot uses three flags:

Flag	Joint	Selects
0	shoulder (axis 1)	the arm points towards the wrist, or it reaches backwards over its base
1	elbow (axis 3)	the elbow above or below the line from shoulder to wrist
2	wrist (axis 5)	the sign of the angle of axis 5

Other kinematics use fewer flags: two (shoulder and elbow) for a palletizer, one (elbow) for a SCARA and none for a gantry. Which of the two postures Positive and Negative stand for depends on the axis convention of the robot, so the reliable way to get a vector is to read it back from a joint pose you know. The configuration vector example moves to such a pose and converts it:

robot.move_ptp(poses::home()).start()?.join()?;
let start = robot.joint_pose()?;
info!("Joint pose:         {start}");

// The TCP pose of this joint pose and the configuration vector that
// describes its posture. For a six-axis arm the robot uses three flags:
//   flag 0, shoulder (axis 1): the arm points towards the wrist, or it
//           reaches backwards over its base;
//   flag 1, elbow (axis 3): the elbow is above or below the line from
//           shoulder to wrist;
//   flag 2, wrist (axis 5): the sign of the angle of axis 5.
// The remaining flags have no effect.
let (tcp_pose, reported) = robot.joint_pose_to_tcp(start, Cs::Robot)?;
info!("TCP pose:           {tcp_pose}");
info!("Configuration:      {reported}");

Flipping the elbow flag and converting the same TCP pose back gives a second joint pose. The arm axes take the other elbow posture and the wrist compensates, so both joint poses put the TCP at the same pose:

let elbow_flipped = reported.toggled(1)?;
info!("Elbow flipped:      {elbow_flipped}");

// One TCP pose, two joint poses: the arm axes take the other elbow
// posture and the wrist compensates, so the TCP pose stays the same.
let joints_reported = robot.tcp_pose_to_joint(tcp_pose, reported, Cs::Robot)?;
let joints_flipped = robot.tcp_pose_to_joint(tcp_pose, elbow_flipped, Cs::Robot)?;
info!("Joints, reported:   {joints_reported}");
info!("Joints, flipped:    {joints_flipped}");
let (tcp_flipped, _) = robot.joint_pose_to_tcp(joints_flipped, Cs::Robot)?;
info!("TCP pose, flipped:  {tcp_flipped}");

A CartesianTarget carries the vector into a PTP motion with with_configuration_vector() (see Perform a PTP Movement Using a CartesianTarget). Moving to the same Cartesian target once per configuration ends in the two different postures, which reading the vector back from the reached joint pose confirms:

fn move_in_configuration(
    robot: &(impl MovePtp + GetJointPose),
    tcp_pose: CartesianPose,
    configuration: ConfigurationVector,
) -> Result<JointPose> {
    robot
        .move_ptp(CartesianTarget::new(tcp_pose).with_configuration_vector(configuration))
        .start()?
        .join()?;
    robot.joint_pose()
}

let reached_reported = move_in_configuration(robot, tcp_pose, reported)?;
info!("Reached, reported:  {reached_reported}");
let reached_flipped = move_in_configuration(robot, tcp_pose, elbow_flipped)?;
info!("Reached, flipped:   {reached_flipped}");

// Reading the vector back from the reached joint pose confirms the
// posture.
let (_, configuration) = robot.joint_pose_to_tcp(reached_flipped, Cs::Robot)?;
if configuration != elbow_flipped {
    return Err(ExampleError::ConfigurationNotReached {
        expected: elbow_flipped,
        reached: configuration,
    });
}

Without a configuration vector the robot control chooses the joint solution. When the posture matters, set the vector or read the joint pose back after the motion:

// Without a configuration vector the robot control chooses the joint
// solution; here the robot starts in the flipped posture. Read the
// joint pose back where the posture matters.
robot.move_ptp(tcp_pose).start()?.join()?;
let reached = robot.joint_pose()?;
let (_, configuration) = robot.joint_pose_to_tcp(reached, Cs::Robot)?;
info!("Reached, no vector: {reached}, configuration: {configuration}");

Linear and circular motions ignore the vector and interpolate the path instead.

Full Example

Pose conversion example

//! Pose conversion examples.
//!
//! Demonstrates how to convert joint poses into Cartesian poses and back
//! with the robot's kinematics, for the flange, a given tool and the
//! selected tool.

mod common;

use common::poses;
use tracing::info;
use voraus_robot_arm::prelude::*;

/// Example application converting poses.
fn run_pose_conversion_example(robot: &impl PoseConversion) -> Result<()> {
    info!("--- Pose Conversion ---");

    let home = poses::home();

    // The flange pose at the home joint pose, in robot coordinates.
    let (flange_pose, configuration) =
        robot.joint_pose_to_cartesian(home, ToolTransformation::FLANGE, Cs::Robot)?;
    info!("Flange pose: {flange_pose}, configuration: {configuration}");

    // A tool reaching 10 cm along the flange z axis.
    let tool = ToolTransformation::new(z(Meters(0.1)));
    let (tool_pose, _configuration) = robot.joint_pose_to_cartesian(home, tool, Cs::Robot)?;
    info!("Tool pose: {tool_pose}");

    // The pose of the selected tool's TCP.
    let (tcp_pose, _) = robot.joint_pose_to_tcp(home, Cs::Robot)?;
    info!("TCP pose: {tcp_pose}");

    // Back to the joint pose that reaches the tool pose with the same
    // configuration.
    let joint_pose = robot.cartesian_pose_to_joint(tool_pose, configuration, tool, Cs::Robot)?;
    info!("Joint pose: {joint_pose}");

    // A configuration vector built by hand, with the flags a six-axis arm
    // uses: shoulder, elbow and wrist.
    let wrist_flipped = ConfigurationVector::new(&[
        ConfigurationFlag::Positive,
        ConfigurationFlag::Positive,
        ConfigurationFlag::Negative,
    ])?;
    info!("Hand-built configuration: {wrist_flipped}");

    // The same tool pose with the hand-built configuration. The robot
    // control can reject the configuration, so the conversion can fail.
    match robot.cartesian_pose_to_joint(tool_pose, wrist_flipped, tool, Cs::Robot) {
        Ok(joint_pose) => info!("Joint pose, hand-built configuration: {joint_pose}"),
        Err(RobotArmError::PoseConversionFailed(reason)) => info!("Not converted: {reason}"),
        Err(error) => return Err(error),
    }

    // A pose out of reach has no joint pose.
    let unreachable = tool_pose + x(Meters(10.0));
    match robot.cartesian_pose_to_joint(unreachable, configuration, tool, Cs::Robot) {
        Err(RobotArmError::PoseConversionFailed(reason)) => info!("Not converted: {reason}"),
        other => {
            other?;
        }
    }
    Ok(())
}

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

    run_pose_conversion_example(&robot)?;

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

Configuration vector example

//! Configuration vector example.
//!
//! A Cartesian pose is usually reached by several joint poses. The
//! configuration vector selects one of them. This example reads the vector
//! of a known joint pose, flips its elbow flag, and shows that the same TCP
//! pose then converts into another joint pose and that a PTP motion to the
//! same Cartesian target ends in another posture of the arm.

mod common;

use common::poses;
use thiserror::Error;
use tracing::info;
use voraus_robot_arm::prelude::*;

/// The errors of this example.
#[derive(Debug, Error)]
enum ExampleError {
    /// A call to the robot failed.
    #[error(transparent)]
    Robot(#[from] RobotArmError),
    /// The configuration vector has no flag at the given index.
    #[error(transparent)]
    InvalidFlag(#[from] ArgumentError),
    /// The robot did not end in the requested posture.
    #[error("the robot reached configuration {reached}, not {expected}")]
    ConfigurationNotReached {
        /// The configuration vector of the motion target.
        expected: ConfigurationVector,
        /// The configuration vector of the reached joint pose.
        reached: ConfigurationVector,
    },
}

/// Move to a Cartesian target in the given configuration and return the
/// joint pose the robot ends in.
fn move_in_configuration(
    robot: &(impl MovePtp + GetJointPose),
    tcp_pose: CartesianPose,
    configuration: ConfigurationVector,
) -> Result<JointPose> {
    robot
        .move_ptp(CartesianTarget::new(tcp_pose).with_configuration_vector(configuration))
        .start()?
        .join()?;
    robot.joint_pose()
}

/// Example application showing the effect of the configuration vector.
fn run_configuration_vector_example(
    robot: &(impl MovePtp + GetJointPose + PoseConversion),
) -> Result<(), ExampleError> {
    info!("--- Configuration Vector ---");

    // Start from a known joint pose, away from singularities: the elbow is
    // bent and the wrist is not stretched.
    robot.move_ptp(poses::home()).start()?.join()?;
    let start = robot.joint_pose()?;
    info!("Joint pose:         {start}");

    // The TCP pose of this joint pose and the configuration vector that
    // describes its posture. For a six-axis arm the robot uses three flags:
    //   flag 0, shoulder (axis 1): the arm points towards the wrist, or it
    //           reaches backwards over its base;
    //   flag 1, elbow (axis 3): the elbow is above or below the line from
    //           shoulder to wrist;
    //   flag 2, wrist (axis 5): the sign of the angle of axis 5.
    // The remaining flags have no effect.
    let (tcp_pose, reported) = robot.joint_pose_to_tcp(start, Cs::Robot)?;
    info!("TCP pose:           {tcp_pose}");
    info!("Configuration:      {reported}");

    // The same posture with the elbow on the other side of the line from
    // shoulder to wrist.
    let elbow_flipped = reported.toggled(1)?;
    info!("Elbow flipped:      {elbow_flipped}");

    // One TCP pose, two joint poses: the arm axes take the other elbow
    // posture and the wrist compensates, so the TCP pose stays the same.
    let joints_reported = robot.tcp_pose_to_joint(tcp_pose, reported, Cs::Robot)?;
    let joints_flipped = robot.tcp_pose_to_joint(tcp_pose, elbow_flipped, Cs::Robot)?;
    info!("Joints, reported:   {joints_reported}");
    info!("Joints, flipped:    {joints_flipped}");
    let (tcp_flipped, _) = robot.joint_pose_to_tcp(joints_flipped, Cs::Robot)?;
    info!("TCP pose, flipped:  {tcp_flipped}");

    // The configuration vector on a Cartesian target selects the posture a
    // PTP motion ends in. The same target, once per configuration; the
    // robot already stands in the reported one:
    let reached_reported = move_in_configuration(robot, tcp_pose, reported)?;
    info!("Reached, reported:  {reached_reported}");
    let reached_flipped = move_in_configuration(robot, tcp_pose, elbow_flipped)?;
    info!("Reached, flipped:   {reached_flipped}");

    // Reading the vector back from the reached joint pose confirms the
    // posture.
    let (_, configuration) = robot.joint_pose_to_tcp(reached_flipped, Cs::Robot)?;
    if configuration != elbow_flipped {
        return Err(ExampleError::ConfigurationNotReached {
            expected: elbow_flipped,
            reached: configuration,
        });
    }

    // Without a configuration vector the robot control chooses the joint
    // solution; here the robot starts in the flipped posture. Read the
    // joint pose back where the posture matters.
    robot.move_ptp(tcp_pose).start()?.join()?;
    let reached = robot.joint_pose()?;
    let (_, configuration) = robot.joint_pose_to_tcp(reached, Cs::Robot)?;
    info!("Reached, no vector: {reached}, configuration: {configuration}");
    Ok(())
}

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

    run_configuration_vector_example(&robot)?;

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

Trait Definition

PoseConversion

pub trait PoseConversion {
    /// Convert a joint pose into the Cartesian pose of a tool center point.
    ///
    /// The robot does not move. The returned configuration vector leads back
    /// to `joint_pose` with
    /// [`cartesian_pose_to_joint`](Self::cartesian_pose_to_joint).
    ///
    /// # Example
    ///
    /// ```rust
    /// # use voraus_robot_arm::prelude::*;
    /// # fn demo(robot: &impl PoseConversion, joints: JointPose) -> Result<()> {
    /// let (pose, configuration) =
    ///     robot.joint_pose_to_cartesian(joints, ToolTransformation::FLANGE, Cs::Robot)?;
    /// # let _ = (pose, configuration);
    /// # Ok(())
    /// # }
    /// ```
    ///
    /// # Arguments
    ///
    /// * `joint_pose` - The joint pose to convert.
    /// * `tool` - Where the tool center point sits relative to the flange.
    /// * `cs` - The coordinate system of the resulting pose.
    ///
    /// # Returns
    ///
    /// The Cartesian pose in `cs` and the configuration vector of
    /// `joint_pose`.
    ///
    /// # Errors
    ///
    /// Returns [`RobotArmError::UnitLimit`](crate::error::RobotArmError::UnitLimit)
    /// for a non-finite component,
    /// [`RobotArmError::PoseConversionFailed`](crate::error::RobotArmError::PoseConversionFailed)
    /// if `cs` is the tool or camera coordinate system
    /// ([`PoseConversionError::UnsupportedCs`](crate::error::PoseConversionError::UnsupportedCs)) or the
    /// control cannot compute the pose ([`PoseConversionError::Rejected`](crate::error::PoseConversionError::Rejected)
    /// or [`PoseConversionError::EmptyResult`](crate::error::PoseConversionError::EmptyResult)),
    /// [`RobotArmError::InvalidValue`](crate::error::RobotArmError::InvalidValue)
    /// if the control reports a configuration flag other than `1` or `-1`,
    /// or [`RobotArmError::Backend`](crate::error::RobotArmError::Backend) if
    /// the call fails.
    fn joint_pose_to_cartesian(
        &self,
        joint_pose: JointPose,
        tool: ToolTransformation,
        cs: Cs,
    ) -> Result<(CartesianPose, ConfigurationVector)>;

    /// Convert a joint pose into the pose of the selected tool's TCP.
    ///
    /// The conversion works as for
    /// [`joint_pose_to_cartesian`](Self::joint_pose_to_cartesian).
    /// Without a selected tool the TCP is the flange.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use voraus_robot_arm::prelude::*;
    /// # fn demo(robot: &impl PoseConversion, joints: JointPose) -> Result<()> {
    /// let (tcp_pose, configuration) = robot.joint_pose_to_tcp(joints, Cs::Robot)?;
    /// # let _ = (tcp_pose, configuration);
    /// # Ok(())
    /// # }
    /// ```
    ///
    /// # Arguments
    ///
    /// * `joint_pose` - The joint pose to convert.
    /// * `cs` - The coordinate system of the resulting pose.
    ///
    /// # Returns
    ///
    /// The TCP pose in `cs` and the configuration vector of `joint_pose`.
    ///
    /// # Errors
    ///
    /// As [`joint_pose_to_cartesian`](Self::joint_pose_to_cartesian).
    fn joint_pose_to_tcp(
        &self,
        joint_pose: JointPose,
        cs: Cs,
    ) -> Result<(CartesianPose, ConfigurationVector)>;

    /// Convert the Cartesian pose of a tool center point into a joint pose.
    ///
    /// The robot does not move. The configuration vector selects one of the
    /// joint poses that reach `pose`. The configuration vector from
    /// [`joint_pose_to_cartesian`](Self::joint_pose_to_cartesian)
    /// leads back to the joint pose it was computed for.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use voraus_robot_arm::prelude::*;
    /// # fn demo(robot: &impl PoseConversion, joints: JointPose) -> Result<()> {
    /// let (pose, configuration) =
    ///     robot.joint_pose_to_cartesian(joints, ToolTransformation::FLANGE, Cs::Robot)?;
    /// let joint_pose = robot.cartesian_pose_to_joint(
    ///     pose,
    ///     configuration,
    ///     ToolTransformation::FLANGE,
    ///     Cs::Robot,
    /// )?;
    /// # let _ = joint_pose;
    /// # Ok(())
    /// # }
    /// ```
    ///
    /// # Arguments
    ///
    /// * `pose` - The Cartesian pose the tool center point reaches.
    /// * `configuration` - Selects the joint solution; see
    ///   [`ConfigurationVector`].
    /// * `tool` - Where the tool center point sits relative to the flange.
    /// * `cs` - The coordinate system of `pose`.
    ///
    /// # Returns
    ///
    /// The joint pose at which the tool center point reaches `pose`.
    ///
    /// # Errors
    ///
    /// Returns [`RobotArmError::UnitLimit`](crate::error::RobotArmError::UnitLimit)
    /// for a non-finite component,
    /// [`RobotArmError::PoseConversionFailed`](crate::error::RobotArmError::PoseConversionFailed)
    /// if `cs` is the tool or camera coordinate system
    /// ([`PoseConversionError::UnsupportedCs`](crate::error::PoseConversionError::UnsupportedCs)) or the
    /// control cannot compute the joint pose ([`PoseConversionError::Rejected`](crate::error::PoseConversionError::Rejected)
    /// or [`PoseConversionError::EmptyResult`](crate::error::PoseConversionError::EmptyResult)), for example because
    /// `pose` is out of reach, or
    /// [`RobotArmError::Backend`](crate::error::RobotArmError::Backend) if
    /// the call fails.
    fn cartesian_pose_to_joint(
        &self,
        pose: CartesianPose,
        configuration: ConfigurationVector,
        tool: ToolTransformation,
        cs: Cs,
    ) -> Result<JointPose>;

    /// Convert a pose of the selected tool's TCP into a joint pose.
    ///
    /// The conversion works as for
    /// [`cartesian_pose_to_joint`](Self::cartesian_pose_to_joint).
    /// Without a selected tool the TCP is the flange.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use voraus_robot_arm::prelude::*;
    /// # fn demo(robot: &impl PoseConversion, joints: JointPose) -> Result<()> {
    /// let (tcp_pose, configuration) = robot.joint_pose_to_tcp(joints, Cs::Robot)?;
    /// let joint_pose = robot.tcp_pose_to_joint(tcp_pose, configuration, Cs::Robot)?;
    /// # let _ = joint_pose;
    /// # Ok(())
    /// # }
    /// ```
    ///
    /// # Arguments
    ///
    /// * `pose` - The pose the TCP reaches.
    /// * `configuration` - Selects the joint solution; see
    ///   [`ConfigurationVector`].
    /// * `cs` - The coordinate system of `pose`.
    ///
    /// # Returns
    ///
    /// The joint pose at which the TCP reaches `pose`.
    ///
    /// # Errors
    ///
    /// As [`cartesian_pose_to_joint`](Self::cartesian_pose_to_joint).
    fn tcp_pose_to_joint(
        &self,
        pose: CartesianPose,
        configuration: ConfigurationVector,
        cs: Cs,
    ) -> Result<JointPose>;
}