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};
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        category: crate::registry::ProblemCategory::Misc,
21        module_path: module_path!(),
22        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",
23        fields: &[
24            FieldInfo {
25                name: "link_lengths",
26                type_name: "Vec<f64>",
27                description: "Positive link lengths l_1, ..., l_n",
28            },
29            FieldInfo {
30                name: "target_point",
31                type_name: "(f64, f64)",
32                description: "Target point g = (g_x, g_y) in R^2",
33            },
34            FieldInfo {
35                name: "orientation_samples",
36                type_name: "Vec<Vec<f64>>",
37                description: "Sampled absolute orientations Phi_j for each link j",
38            },
39            FieldInfo {
40                name: "allowed_pairs",
41                type_name: "Vec<Vec<(usize, usize)>>",
42                description: "Admissible (a_{j-1}, a_j) pair sets A_j for j = 2, ..., n",
43            },
44        ],
45    }
46}
47
48/// The Minimum Discrete Planar Inverse Kinematics problem.
49///
50/// Given positive link lengths `l_1, ..., l_n`, a target point `g` in
51/// `R^2`, sampled absolute orientations `Phi_j = {phi_{j,0}, ..., phi_{j,m_j-1}}`
52/// for each link, and admissible-pair sets
53/// `A_j ⊆ {0, ..., m_{j-1}-1} x {0, ..., m_j-1}` for `j = 2, ..., n`,
54/// choose indices `a_j ∈ {0, ..., m_j-1}` such that `(a_{j-1}, a_j) ∈ A_j`
55/// for every `j = 2, ..., n`, minimizing
56///
57/// `|| Σ_{j=1}^n l_j (cos(phi_{j,a_j}), sin(phi_{j,a_j})) - g ||_2^2`.
58#[derive(Debug, Clone, Serialize)]
59pub struct MinimumDiscretePlanarInverseKinematics {
60    link_lengths: Vec<f64>,
61    target_point: (f64, f64),
62    orientation_samples: Vec<Vec<f64>>,
63    allowed_pairs: Vec<Vec<(usize, usize)>>,
64}
65
66#[derive(Deserialize)]
67struct MinimumDiscretePlanarInverseKinematicsData {
68    link_lengths: Vec<f64>,
69    target_point: (f64, f64),
70    orientation_samples: Vec<Vec<f64>>,
71    allowed_pairs: Vec<Vec<(usize, usize)>>,
72}
73
74impl<'de> Deserialize<'de> for MinimumDiscretePlanarInverseKinematics {
75    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
76    where
77        D: serde::Deserializer<'de>,
78    {
79        let data = MinimumDiscretePlanarInverseKinematicsData::deserialize(deserializer)?;
80        Self::new(
81            data.link_lengths,
82            data.target_point,
83            data.orientation_samples,
84            data.allowed_pairs,
85        )
86        .map_err(serde::de::Error::custom)
87    }
88}
89
90impl MinimumDiscretePlanarInverseKinematics {
91    /// Construct a new instance.
92    ///
93    pub fn new(
94        link_lengths: Vec<f64>,
95        target_point: (f64, f64),
96        orientation_samples: Vec<Vec<f64>>,
97        allowed_pairs: Vec<Vec<(usize, usize)>>,
98    ) -> Result<Self, crate::registry::ConstructionError> {
99        let n = link_lengths.len();
100        if n == 0 {
101            return Err("MinimumDiscretePlanarInverseKinematics requires at least one link".into());
102        }
103        for (index, &length) in link_lengths.iter().enumerate() {
104            if !length.is_finite() || length <= 0.0 {
105                return Err(
106                    format!("link length at index {index} must be positive and finite").into(),
107                );
108            }
109        }
110        if !target_point.0.is_finite() || !target_point.1.is_finite() {
111            return Err("target point coordinates must be finite".into());
112        }
113        if orientation_samples.len() != n {
114            return Err("orientation_samples must have one entry per link".into());
115        }
116        let mut total_configurations = 1_usize;
117        for (link, samples) in orientation_samples.iter().enumerate() {
118            if samples.is_empty() {
119                return Err(
120                    format!("link {link} must have at least one candidate orientation").into(),
121                );
122            }
123            total_configurations = total_configurations
124                .checked_mul(samples.len())
125                .ok_or("orientation configuration count exceeds usize")?;
126            for (sample, &angle) in samples.iter().enumerate() {
127                if !angle.is_finite() {
128                    return Err(format!(
129                        "orientation sample {sample} for link {link} must be finite"
130                    )
131                    .into());
132                }
133            }
134        }
135        if allowed_pairs.len() != n - 1 {
136            return Err("allowed_pairs must have one entry per junction".into());
137        }
138        for (j_minus_1, pairs) in allowed_pairs.iter().enumerate() {
139            let m_prev = orientation_samples[j_minus_1].len();
140            let m_curr = orientation_samples[j_minus_1 + 1].len();
141            for &(a_prev, a_curr) in pairs {
142                if a_prev >= m_prev || a_curr >= m_curr {
143                    return Err(format!(
144                        "allowed pair ({a_prev}, {a_curr}) at junction {j_minus_1} is out of range"
145                    )
146                    .into());
147                }
148            }
149        }
150        Ok(Self {
151            link_lengths,
152            target_point,
153            orientation_samples,
154            allowed_pairs,
155        })
156    }
157
158    /// Get the link lengths.
159    pub fn link_lengths(&self) -> &[f64] {
160        &self.link_lengths
161    }
162
163    /// Get the target point.
164    pub fn target_point(&self) -> (f64, f64) {
165        self.target_point
166    }
167
168    /// Get the per-link orientation samples.
169    pub fn orientation_samples(&self) -> &[Vec<f64>] {
170        &self.orientation_samples
171    }
172
173    /// Get the admissible-pair sets for consecutive junctions.
174    pub fn allowed_pairs(&self) -> &[Vec<(usize, usize)>] {
175        &self.allowed_pairs
176    }
177
178    /// Number of links `n`.
179    pub fn num_links(&self) -> usize {
180        self.link_lengths.len()
181    }
182
183    /// Total number of configurations (product of per-link sample counts):
184    /// `prod_{j=1}^n m_j`. This is the size of the brute-force search space.
185    pub fn total_configurations(&self) -> usize {
186        self.orientation_samples
187            .iter()
188            .map(|samples| samples.len())
189            .try_fold(1_usize, usize::checked_mul)
190            .expect("validated orientation configuration count must fit usize")
191    }
192
193    /// Total number of sampled orientations across all links:
194    /// `sum_{j=1}^n m_j`. This is the QUBO variable count for the one-hot
195    /// encoding used by the QUBO reduction.
196    pub fn num_orientation_samples(&self) -> usize {
197        self.orientation_samples.iter().map(Vec::len).sum()
198    }
199
200    /// Check if a configuration is feasible (one index per link, in range,
201    /// and every consecutive pair lies in the corresponding admissible set).
202    pub fn is_feasible(&self, config: &[usize]) -> bool {
203        let n = self.num_links();
204        if config.len() != n {
205            return false;
206        }
207        for (j, &a) in config.iter().enumerate() {
208            if a >= self.orientation_samples[j].len() {
209                return false;
210            }
211        }
212        for j in 1..n {
213            let pair = (config[j - 1], config[j]);
214            if !self.allowed_pairs[j - 1].contains(&pair) {
215                return false;
216            }
217        }
218        true
219    }
220
221    /// Compute the end-effector position for a configuration.
222    /// Returns `None` if the configuration is infeasible.
223    pub fn end_effector(
224        &self,
225        config: &[usize],
226    ) -> Result<Option<(f64, f64)>, crate::traits::EvaluationError> {
227        if !self.is_feasible(config) {
228            return Ok(None);
229        }
230        let mut x = 0.0_f64;
231        let mut y = 0.0_f64;
232        for (j, &a) in config.iter().enumerate() {
233            let phi = self.orientation_samples[j][a];
234            let next_x = x + self.link_lengths[j] * phi.cos();
235            let next_y = y + self.link_lengths[j] * phi.sin();
236            if !next_x.is_finite() || !next_y.is_finite() {
237                return Err(crate::traits::EvaluationError::NonFiniteResult(
238                    "computing the inverse-kinematics end-effector position".into(),
239                ));
240            }
241            x = next_x;
242            y = next_y;
243        }
244        Ok(Some((x, y)))
245    }
246
247    /// Compute the squared end-effector distance to the target.
248    /// Returns `None` if the configuration is infeasible.
249    pub fn squared_distance(
250        &self,
251        config: &[usize],
252    ) -> Result<Option<f64>, crate::traits::EvaluationError> {
253        let Some((x, y)) = self.end_effector(config)? else {
254            return Ok(None);
255        };
256        let dx = x - self.target_point.0;
257        let dy = y - self.target_point.1;
258        let squared_distance = dx * dx + dy * dy;
259        if !squared_distance.is_finite() {
260            return Err(crate::traits::EvaluationError::NonFiniteResult(
261                "computing the inverse-kinematics squared distance".into(),
262            ));
263        }
264        Ok(Some(squared_distance))
265    }
266
267    /// Whether the configuration represents a valid feasible solution.
268    pub fn is_valid_solution(&self, config: &[usize]) -> bool {
269        self.is_feasible(config)
270    }
271}
272
273impl Problem for MinimumDiscretePlanarInverseKinematics {
274    const NAME: &'static str = "MinimumDiscretePlanarInverseKinematics";
275    type Solution = Vec<usize>;
276    type Value = Min<f64>;
277
278    crate::problem_parameters![
279        ("total_configurations", total_configurations),
280        ("num_links", num_links),
281        ("num_orientation_samples", num_orientation_samples),
282    ];
283
284    fn variant() -> Vec<(&'static str, &'static str)> {
285        crate::variant_params![]
286    }
287
288    fn evaluate(
289        &self,
290        config: &Self::Solution,
291    ) -> Result<Min<f64>, crate::traits::EvaluationError> {
292        if config.len() != self.num_links() {
293            return Err(crate::traits::EvaluationError::InvalidConfiguration(
294                "orientation assignment length does not match the links".into(),
295            ));
296        }
297        if config
298            .iter()
299            .enumerate()
300            .any(|(link, &orientation)| orientation >= self.orientation_samples[link].len())
301        {
302            return Err(crate::traits::EvaluationError::InvalidConfiguration(
303                "orientation assignment contains an out-of-range sample".into(),
304            ));
305        }
306        Ok({
307            match self.squared_distance(config)? {
308                Some(value) => Min(Some(value)),
309                None => Min(None),
310            }
311        })
312    }
313}
314
315impl crate::solvers::BruteForceProblem for MinimumDiscretePlanarInverseKinematics {
316    fn dimensions(&self) -> Vec<usize> {
317        self.orientation_samples
318            .iter()
319            .map(|samples| samples.len())
320            .collect()
321    }
322}
323
324crate::declare_variants! {
325    default MinimumDiscretePlanarInverseKinematics => "total_configurations",
326}
327
328crate::register_brute_force! {
329    MinimumDiscretePlanarInverseKinematics,
330}
331
332#[cfg(feature = "example-db")]
333pub(crate) fn canonical_model_example_specs() -> Vec<crate::example_db::specs::ModelExampleSpec> {
334    use std::f64::consts::FRAC_PI_2;
335    vec![crate::example_db::specs::ModelExampleSpec {
336        id: "minimum_discrete_planar_inverse_kinematics",
337        instance: Box::new(
338            MinimumDiscretePlanarInverseKinematics::new(
339                vec![2.0, 1.0],
340                (2.0, 1.0),
341                vec![vec![0.0, FRAC_PI_2], vec![0.0, FRAC_PI_2]],
342                vec![vec![(0, 0), (0, 1), (1, 1)]],
343            )
344            .unwrap(),
345        ),
346        optimal_config: serde_json::json!(vec![0, 1]),
347        optimal_value: serde_json::json!(0.0),
348    }]
349}
350
351#[cfg(test)]
352#[path = "../../unit_tests/models/misc/minimum_discrete_planar_inverse_kinematics.rs"]
353mod tests;
problemreductions/models/misc/minimum_discrete_planar_inverse_kinematics.rs

problemreductions/models/misc/
minimum_discrete_planar_inverse_kinematics.rs
