Struct FPDispatch 

pub struct FPDispatch<const N: usize, FK32, FK64>
where
    FK32: FKChain<N, f32>,
    FK64: FKChain<N, f64>,
{ /* private fields */ }

FK chain wrapper that holds both an f32 and an f64 representation of the same kinematic chain and dispatches to the correct precision when an FKChain<N, F> method is invoked.

The two inner chains are expected to describe the same robot — typically the f64 is canonical and the f32 is derived from it (see FPDispatch::from_f64). Callers that already have both built can use FPDispatch::new.

Use this when the same robot needs to be consumed by stages with different precision requirements: e.g. an RRT planner runs on f32 for SIMD speed, while a TOPP retimer runs on f64 for solver stability. A single FPDispatch can be passed to both — each picks up the precision-correct FKChain impl via type inference.

use deke_types::{DHChain, DHJoint, FKChain, FPDispatch, SRobotQ};

// Author the robot once in f64 (e.g. from a const URDF table).
const ARM: DHChain<2, f64> = DHChain::<2, f64>::new_f64([
    DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
    DHJoint { a: 1.0, alpha: 0.0, d: 0.0, theta_offset: 0.0 },
]);

// Derive the f32 chain via the cheap From cast and bundle them.
let dispatch: FPDispatch<2, DHChain<2, f32>, DHChain<2, f64>> =
    FPDispatch::from_f64(ARM);

// f32 consumers get the SIMD path:
let q32 = SRobotQ::<2, f32>::from_array([0.5, -0.3]);
let _ = <FPDispatch<_, _, _> as FKChain<2, f32>>::fk_end(&dispatch, &q32).unwrap();

// f64 consumers get the precise path:
let q64 = SRobotQ::<2, f64>::from_array([0.5, -0.3]);
let _ = <FPDispatch<_, _, _> as FKChain<2, f64>>::fk_end(&dispatch, &q64).unwrap();

Implementations

impl<const N: usize, FK32, FK64> FPDispatch<N, FK32, FK64>

where FK32: FKChain<N, f32>, FK64: FKChain<N, f64>,

pub fn new(f32_chain: FK32, f64_chain: FK64) -> Self

Build from explicit f32 and f64 chains. The two are expected to encode the same robot; nothing checks that here.

pub fn f32_chain(&self) -> &FK32

pub fn f64_chain(&self) -> &FK64

impl<const N: usize, FK32, FK64> FPDispatch<N, FK32, FK64>

where FK32: FKChain<N, f32> + From<FK64>, FK64: FKChain<N, f64> + Clone,

pub fn from_f64(f64_chain: FK64) -> Self

Build from only the f64 chain by deriving the f32 chain via From. Available when the f32 chain implements From<FK64> (which it does for the leaf chain types DHChain, HPChain, and URDFChain).

Trait Implementations

impl<const N: usize, FK32, FK64> Clone for FPDispatch<N, FK32, FK64>

where FK32: FKChain<N, f32> + Clone, FK64: FKChain<N, f64> + Clone,

fn clone(&self) -> FPDispatch<N, FK32, FK64>

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl<const N: usize, FK32, FK64> Debug for FPDispatch<N, FK32, FK64>

where FK32: FKChain<N, f32> + Debug, FK64: FKChain<N, f64> + Debug,

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl<const N: usize, FK32, FK64> FKChain<N> for FPDispatch<N, FK32, FK64>

where FK32: FKChain<N, f32>, FK64: FKChain<N, f64>,

type Error = <FK32 as FKChain<N>>::Error

fn base_tf(&self) -> f32::AAffine3

Configuration-independent transform from the robot’s base frame to the world frame. Defaults to identity; wrappers that install a static prefix (e.g. TransformedFK with a prefix set, or URDFChain with fixed leading joints baked in) override this so downstream consumers (collision validators, visualizers) can place the static base body at the correct pose.

fn max_reach(&self) -> Result<f32, Self::Error>

Theoretical maximum reach: sum of link lengths (upper bound, ignores joint limits).

fn fk(&self, q: &SRobotQ<N, f32>) -> Result<[f32::AAffine3; N], Self::Error>

fn fk_end(&self, q: &SRobotQ<N, f32>) -> Result<f32::AAffine3, Self::Error>

fn all_fk( &self, q: &SRobotQ<N, f32>, ) -> Result<(f32::AAffine3, [f32::AAffine3; N], f32::AAffine3), Self::Error>

Compute base transform, per-link frames, and the end-effector frame in one call. Consumers that need all three (collision validators, visualizers) should prefer this over invoking base_tf, fk, and fk_end separately.

fn joint_axes_positions( &self, q: &SRobotQ<N, f32>, ) -> Result<([f32::AVec3; N], [f32::AVec3; N], f32::AVec3), Self::Error>

Returns joint rotation axes and axis-origin positions in world frame at configuration q, plus the end-effector position.

fn jacobian(&self, q: &SRobotQ<N, f32>) -> Result<[[f32; N]; 6], Self::Error>

Geometric Jacobian (6×N) at configuration q. Rows 0–2: linear velocity, rows 3–5: angular velocity.

fn jacobian_dot( &self, q: &SRobotQ<N, f32>, qdot: &SRobotQ<N, f32>, ) -> Result<[[f32; N]; 6], Self::Error>

First time-derivative of the geometric Jacobian.

fn jacobian_ddot( &self, q: &SRobotQ<N, f32>, qdot: &SRobotQ<N, f32>, qddot: &SRobotQ<N, f32>, ) -> Result<[[f32; N]; 6], Self::Error>

Second time-derivative of the geometric Jacobian.

fn dof(&self) -> usize

impl<const N: usize, FK32, FK64> FKChain<N, f64> for FPDispatch<N, FK32, FK64>

where FK32: FKChain<N, f32>, FK64: FKChain<N, f64>,

type Error = <FK64 as FKChain<N, f64>>::Error

fn base_tf(&self) -> f64::AAffine3

Configuration-independent transform from the robot’s base frame to the world frame. Defaults to identity; wrappers that install a static prefix (e.g. TransformedFK with a prefix set, or URDFChain with fixed leading joints baked in) override this so downstream consumers (collision validators, visualizers) can place the static base body at the correct pose.

fn max_reach(&self) -> Result<f64, Self::Error>

Theoretical maximum reach: sum of link lengths (upper bound, ignores joint limits).

fn fk(&self, q: &SRobotQ<N, f64>) -> Result<[f64::AAffine3; N], Self::Error>

fn fk_end(&self, q: &SRobotQ<N, f64>) -> Result<f64::AAffine3, Self::Error>

fn all_fk( &self, q: &SRobotQ<N, f64>, ) -> Result<(f64::AAffine3, [f64::AAffine3; N], f64::AAffine3), Self::Error>

Compute base transform, per-link frames, and the end-effector frame in one call. Consumers that need all three (collision validators, visualizers) should prefer this over invoking base_tf, fk, and fk_end separately.

fn joint_axes_positions( &self, q: &SRobotQ<N, f64>, ) -> Result<([f64::AVec3; N], [f64::AVec3; N], f64::AVec3), Self::Error>

Returns joint rotation axes and axis-origin positions in world frame at configuration q, plus the end-effector position.

fn jacobian(&self, q: &SRobotQ<N, f64>) -> Result<[[f64; N]; 6], Self::Error>

Geometric Jacobian (6×N) at configuration q. Rows 0–2: linear velocity, rows 3–5: angular velocity.

fn jacobian_dot( &self, q: &SRobotQ<N, f64>, qdot: &SRobotQ<N, f64>, ) -> Result<[[f64; N]; 6], Self::Error>

First time-derivative of the geometric Jacobian.

fn jacobian_ddot( &self, q: &SRobotQ<N, f64>, qdot: &SRobotQ<N, f64>, qddot: &SRobotQ<N, f64>, ) -> Result<[[f64; N]; 6], Self::Error>

Second time-derivative of the geometric Jacobian.

fn dof(&self) -> usize