problemreductions/models/misc/
minimum_discrete_planar_inverse_kinematics.rs

1//! Minimum Discrete Planar Inverse Kinematics problem implementation.
2//!
3//! Given positive link lengths, a target point in `R^2`, a finite set of
4//! candidate absolute orientations per link, and admissible-pair sets between
5//! consecutive links, choose one orientation index per link so that all
6//! consecutive-pair constraints are satisfied and the squared distance from
7//! the end-effector to the target point is minimized.
8
9use crate::registry::{FieldInfo, ProblemSchemaEntry, ProblemSizeFieldEntry};
10use crate::traits::Problem;
11use crate::types::Min;
12use serde::{Deserialize, Serialize};
13
14inventory::submit! {
15    ProblemSchemaEntry {
16        name: "MinimumDiscretePlanarInverseKinematics",
17        display_name: "Minimum Discrete Planar Inverse Kinematics",
18        aliases: &[],
19        dimensions: &[],
20        module_path: module_path!(),
21        description: "Pick one sampled absolute orientation per link, subject to consecutive-pair feasibility constraints, to minimize the squared distance from the end-effector to a target point",
22        fields: &[
23            FieldInfo {
24                name: "link_lengths",
25                type_name: "Vec<f64>",
26                description: "Positive link lengths l_1, ..., l_n",
27            },
28            FieldInfo {
29                name: "target_point",
30                type_name: "(f64, f64)",
31                description: "Target point g = (g_x, g_y) in R^2",
32            },
33            FieldInfo {
34                name: "orientation_samples",
35                type_name: "Vec<Vec<f64>>",
36                description: "Sampled absolute orientations Phi_j for each link j",
37            },
38            FieldInfo {
39                name: "allowed_pairs",
40                type_name: "Vec<Vec<(usize, usize)>>",
41                description: "Admissible (a_{j-1}, a_j) pair sets A_j for j = 2, ..., n",
42            },
43        ],
44    }
45}
46
47inventory::submit! {
48    ProblemSizeFieldEntry {
49        name: "MinimumDiscretePlanarInverseKinematics",
50        fields: &["num_links"],
51    }
52}
53
54/// The Minimum Discrete Planar Inverse Kinematics problem.
55///
56/// Given positive link lengths `l_1, ..., l_n`, a target point `g` in
57/// `R^2`, sampled absolute orientations `Phi_j = {phi_{j,0}, ..., phi_{j,m_j-1}}`
58/// for each link, and admissible-pair sets
59/// `A_j ⊆ {0, ..., m_{j-1}-1} x {0, ..., m_j-1}` for `j = 2, ..., n`,
60/// choose indices `a_j ∈ {0, ..., m_j-1}` such that `(a_{j-1}, a_j) ∈ A_j`
61/// for every `j = 2, ..., n`, minimizing
62///
63/// `|| Σ_{j=1}^n l_j (cos(phi_{j,a_j}), sin(phi_{j,a_j})) - g ||_2^2`.
64#[derive(Debug, Clone, Serialize, Deserialize)]
65pub struct MinimumDiscretePlanarInverseKinematics {
66    link_lengths: Vec<f64>,
67    target_point: (f64, f64),
68    orientation_samples: Vec<Vec<f64>>,
69    allowed_pairs: Vec<Vec<(usize, usize)>>,
70}
71
72impl MinimumDiscretePlanarInverseKinematics {
73    /// Construct a new instance.
74    ///
75    /// # Panics
76    /// Panics if the input fields are not mutually consistent (see the
77    /// validation rules in the source).
78    pub fn new(
79        link_lengths: Vec<f64>,
80        target_point: (f64, f64),
81        orientation_samples: Vec<Vec<f64>>,
82        allowed_pairs: Vec<Vec<(usize, usize)>>,
83    ) -> Self {
84        let n = link_lengths.len();
85        assert!(
86            n >= 1,
87            "MinimumDiscretePlanarInverseKinematics requires at least one link"
88        );
89        for &length in &link_lengths {
90            assert!(
91                length.is_finite() && length > 0.0,
92                "link lengths must be positive finite reals"
93            );
94        }
95        assert!(
96            target_point.0.is_finite() && target_point.1.is_finite(),
97            "target point coordinates must be finite reals"
98        );
99        assert_eq!(
100            orientation_samples.len(),
101            n,
102            "orientation_samples must have one entry per link"
103        );
104        for samples in &orientation_samples {
105            assert!(
106                !samples.is_empty(),
107                "each link must have at least one candidate orientation"
108            );
109            for &angle in samples {
110                assert!(
111                    angle.is_finite(),
112                    "orientation samples must be finite real numbers"
113                );
114            }
115        }
116        assert_eq!(
117            allowed_pairs.len(),
118            n.saturating_sub(1),
119            "allowed_pairs must have one entry per junction (n - 1 entries)"
120        );
121        for (j_minus_1, pairs) in allowed_pairs.iter().enumerate() {
122            let m_prev = orientation_samples[j_minus_1].len();
123            let m_curr = orientation_samples[j_minus_1 + 1].len();
124            for &(a_prev, a_curr) in pairs {
125                assert!(
126                    a_prev < m_prev,
127                    "allowed_pair index out of range for previous link"
128                );
129                assert!(
130                    a_curr < m_curr,
131                    "allowed_pair index out of range for current link"
132                );
133            }
134        }
135        Self {
136            link_lengths,
137            target_point,
138            orientation_samples,
139            allowed_pairs,
140        }
141    }
142
143    /// Get the link lengths.
144    pub fn link_lengths(&self) -> &[f64] {
145        &self.link_lengths
146    }
147
148    /// Get the target point.
149    pub fn target_point(&self) -> (f64, f64) {
150        self.target_point
151    }
152
153    /// Get the per-link orientation samples.
154    pub fn orientation_samples(&self) -> &[Vec<f64>] {
155        &self.orientation_samples
156    }
157
158    /// Get the admissible-pair sets for consecutive junctions.
159    pub fn allowed_pairs(&self) -> &[Vec<(usize, usize)>] {
160        &self.allowed_pairs
161    }
162
163    /// Number of links `n`.
164    pub fn num_links(&self) -> usize {
165        self.link_lengths.len()
166    }
167
168    /// Total number of configurations (product of per-link sample counts):
169    /// `prod_{j=1}^n m_j`. This is the size of the brute-force search space.
170    pub fn total_configurations(&self) -> usize {
171        self.orientation_samples.iter().map(|s| s.len()).product()
172    }
173
174    /// Total number of sampled orientations across all links:
175    /// `sum_{j=1}^n m_j`. This is the QUBO variable count for the one-hot
176    /// encoding used by the QUBO reduction.
177    pub fn num_orientation_samples(&self) -> usize {
178        self.orientation_samples.iter().map(Vec::len).sum()
179    }
180
181    /// Check if a configuration is feasible (one index per link, in range,
182    /// and every consecutive pair lies in the corresponding admissible set).
183    pub fn is_feasible(&self, config: &[usize]) -> bool {
184        let n = self.num_links();
185        if config.len() != n {
186            return false;
187        }
188        for (j, &a) in config.iter().enumerate() {
189            if a >= self.orientation_samples[j].len() {
190                return false;
191            }
192        }
193        for j in 1..n {
194            let pair = (config[j - 1], config[j]);
195            if !self.allowed_pairs[j - 1].contains(&pair) {
196                return false;
197            }
198        }
199        true
200    }
201
202    /// Compute the end-effector position for a configuration.
203    /// Returns `None` if the configuration is infeasible.
204    pub fn end_effector(&self, config: &[usize]) -> Option<(f64, f64)> {
205        if !self.is_feasible(config) {
206            return None;
207        }
208        let mut x = 0.0_f64;
209        let mut y = 0.0_f64;
210        for (j, &a) in config.iter().enumerate() {
211            let phi = self.orientation_samples[j][a];
212            x += self.link_lengths[j] * phi.cos();
213            y += self.link_lengths[j] * phi.sin();
214        }
215        Some((x, y))
216    }
217
218    /// Compute the squared end-effector distance to the target.
219    /// Returns `None` if the configuration is infeasible.
220    pub fn squared_distance(&self, config: &[usize]) -> Option<f64> {
221        let (x, y) = self.end_effector(config)?;
222        let dx = x - self.target_point.0;
223        let dy = y - self.target_point.1;
224        Some(dx * dx + dy * dy)
225    }
226
227    /// Whether the configuration represents a valid feasible solution.
228    pub fn is_valid_solution(&self, config: &[usize]) -> bool {
229        self.is_feasible(config)
230    }
231}
232
233impl Problem for MinimumDiscretePlanarInverseKinematics {
234    const NAME: &'static str = "MinimumDiscretePlanarInverseKinematics";
235    type Value = Min<f64>;
236
237    fn variant() -> Vec<(&'static str, &'static str)> {
238        crate::variant_params![]
239    }
240
241    fn dims(&self) -> Vec<usize> {
242        self.orientation_samples
243            .iter()
244            .map(|samples| samples.len())
245            .collect()
246    }
247
248    fn evaluate(&self, config: &[usize]) -> Min<f64> {
249        match self.squared_distance(config) {
250            Some(value) => Min(Some(value)),
251            None => Min(None),
252        }
253    }
254}
255
256crate::declare_variants! {
257    default MinimumDiscretePlanarInverseKinematics => "total_configurations",
258}
259
260#[cfg(feature = "example-db")]
261pub(crate) fn canonical_model_example_specs() -> Vec<crate::example_db::specs::ModelExampleSpec> {
262    use std::f64::consts::FRAC_PI_2;
263    vec![crate::example_db::specs::ModelExampleSpec {
264        id: "minimum_discrete_planar_inverse_kinematics",
265        instance: Box::new(MinimumDiscretePlanarInverseKinematics::new(
266            vec![2.0, 1.0],
267            (2.0, 1.0),
268            vec![vec![0.0, FRAC_PI_2], vec![0.0, FRAC_PI_2]],
269            vec![vec![(0, 0), (0, 1), (1, 1)]],
270        )),
271        optimal_config: vec![0, 1],
272        optimal_value: serde_json::json!(0.0),
273    }]
274}
275
276#[cfg(test)]
277#[path = "../../unit_tests/models/misc/minimum_discrete_planar_inverse_kinematics.rs"]
278mod tests;
problemreductions/models/misc/minimum_discrete_planar_inverse_kinematics.rs

problemreductions/models/misc/
minimum_discrete_planar_inverse_kinematics.rs
