ik_geo/inverse_kinematics/
setups.rs

1use {
2    super::{
3        auxiliary::{Kinematics, Matrix3x7},
4        gen_six_dof, spherical, spherical_two_intersecting, spherical_two_parallel, three_parallel,
5        three_parallel_two_intersecting, two_intersecting, two_parallel,
6    },
7    crate::subproblems::{
8        auxiliary::{random_angle, random_norm_vector3, random_vector3},
9        setups::SetupStatic,
10    },
11    nalgebra::{Matrix3, Matrix3x6, Vector3, Vector6},
12    std::f64::NAN,
13};
14
15pub trait SetupIk {
16    fn setup(&mut self);
17    fn setup_from_str(&mut self, raw: &str);
18    fn write_output(&self) -> String;
19    fn run(&mut self);
20    fn error(&self) -> f64;
21    fn ls_count(&self) -> usize;
22    fn solution_count(&self) -> usize;
23    fn name(&self) -> &'static str;
24    fn debug(&self, i: usize);
25}
26
27macro_rules! define_struct {
28    ($name:ident) => {
29        pub struct $name {
30            kin: Kinematics<6, 7>,
31            r: Matrix3<f64>,
32            t: Vector3<f64>,
33
34            q: Vec<Vector6<f64>>,
35            is_ls: Vec<bool>,
36        }
37    };
38}
39
40// Define all the structs
41define_struct!(SphericalTwoParallelSetup);
42define_struct!(SphericalTwoIntersectingSetup);
43define_struct!(SphericalSetup);
44define_struct!(ThreeParallelTwoIntersectingSetup);
45define_struct!(ThreeParallelSetup);
46define_struct!(TwoParallelSetup);
47define_struct!(TwoIntersectingSetup);
48define_struct!(GenSixDofSetup);
49
50pub fn calculate_ik_error(
51    kin: &Kinematics<6, 7>,
52    r: &Matrix3<f64>,
53    t: &Vector3<f64>,
54    q: &Vector6<f64>,
55) -> f64 {
56    let (r_t, t_t) = kin.forward_kinematics(q);
57    (r_t - r).norm() + (t_t - t).norm()
58}
59
60fn ik_setup_from_string(
61    raw: &str,
62    kin: &mut Kinematics<6, 7>,
63    r: &mut Matrix3<f64>,
64    t: &mut Vector3<f64>,
65) {
66    let data: Vec<f64> = raw.split(',').map(|s| s.parse().unwrap()).collect();
67
68    kin.h = Matrix3x6::from_columns(&[
69        Vector3::new(data[0], data[1], data[2]),
70        Vector3::new(data[3], data[4], data[5]),
71        Vector3::new(data[6], data[7], data[8]),
72        Vector3::new(data[9], data[10], data[11]),
73        Vector3::new(data[12], data[13], data[14]),
74        Vector3::new(data[15], data[16], data[17]),
75    ]);
76
77    kin.p = Matrix3x7::from_columns(&[
78        Vector3::new(data[18], data[19], data[20]),
79        Vector3::new(data[21], data[22], data[23]),
80        Vector3::new(data[24], data[25], data[26]),
81        Vector3::new(data[27], data[28], data[29]),
82        Vector3::new(data[30], data[31], data[32]),
83        Vector3::new(data[33], data[34], data[35]),
84        Vector3::new(data[36], data[37], data[38]),
85    ]);
86
87    *r = Matrix3::new(
88        data[39], data[40], data[41], data[42], data[43], data[44], data[45], data[46], data[47],
89    );
90
91    *t = Vector3::new(data[48], data[49], data[50]);
92}
93
94pub fn ik_write_output(q: &Vec<Vector6<f64>>) -> String {
95    q.iter()
96        .map(|q| {
97            q.iter()
98                .map(|x| x.to_string())
99                .collect::<Vec<String>>()
100                .join(",")
101        })
102        .collect::<Vec<String>>()
103        .join(",")
104}
105
106// Do setup static as a macro to avoid repition
107macro_rules! impl_setup_static {
108    ($name:ident, $long_name:expr) => {
109        impl SetupStatic for $name {
110            fn new() -> Self {
111                Self {
112                    kin: Kinematics::new(),
113                    r: Matrix3::zeros(),
114                    t: Vector3::zeros(),
115
116                    q: Vec::new(),
117                    is_ls: Vec::new(),
118                }
119            }
120
121            fn name() -> &'static str {
122                $long_name
123            }
124        }
125    };
126}
127
128impl_setup_static!(SphericalTwoParallelSetup, "Spherical two Parallel");
129impl_setup_static!(SphericalTwoIntersectingSetup, "Spherical Two Intersecting");
130impl_setup_static!(SphericalSetup, "Spherical");
131impl_setup_static!(
132    ThreeParallelTwoIntersectingSetup,
133    "Three Parallel Two Intersecting"
134);
135impl_setup_static!(ThreeParallelSetup, "Three Parallel");
136impl_setup_static!(TwoParallelSetup, "Two Parallel");
137impl_setup_static!(TwoIntersectingSetup, "Two Intersecting");
138impl_setup_static!(GenSixDofSetup, "Gen Six DOF");
139
140// Most of the implementations in SetupIk are the same, so we can use a macro to generate them.
141macro_rules! impl_setup_ik {
142    // Generate the function setup_from_str, write_output,ls_count, solution_count, name, and debug
143    ($name:ident) => {
144        fn setup_from_str(&mut self, raw: &str) {
145            ik_setup_from_string(raw, &mut self.kin, &mut self.r, &mut self.t);
146        }
147
148        fn write_output(&self) -> String {
149            ik_write_output(&self.q)
150        }
151
152        fn error(&self) -> f64 {
153            self.q
154                .iter()
155                .map(|q| calculate_ik_error(&self.kin, &self.r, &self.t, q))
156                .reduce(f64::min)
157                .unwrap_or(NAN)
158        }
159
160        fn ls_count(&self) -> usize {
161            self.is_ls.iter().filter(|b| **b).count()
162        }
163
164        fn solution_count(&self) -> usize {
165            self.is_ls.len()
166        }
167
168        fn name(&self) -> &'static str {
169            <Self as SetupStatic>::name()
170        }
171
172        fn debug(&self, i: usize) {
173            println!("{i}\nr{}t{}h{}p{}", self.r, self.t, self.kin.h, self.kin.p);
174        }
175    };
176}
177
178impl SetupIk for SphericalTwoParallelSetup {
179    fn setup(&mut self) {
180        for i in 0..self.kin.h.ncols() {
181            self.kin.h.set_column(i, &random_norm_vector3());
182        }
183
184        let q = Vector6::zeros().map(|_: f64| random_angle());
185
186        let h_column_1: Vector3<f64> = self.kin.h.column(1).into();
187        self.kin.h.set_column(2, &h_column_1);
188
189        self.kin.p = Matrix3x7::from_columns(&[
190            random_vector3(),
191            random_vector3(),
192            random_vector3(),
193            random_vector3(),
194            Vector3::zeros(),
195            Vector3::zeros(),
196            random_vector3(),
197        ]);
198
199        (self.r, self.t) = self.kin.forward_kinematics(&q);
200    }
201
202    fn run(&mut self) {
203        (self.q, self.is_ls) = spherical_two_parallel(&self.r, &self.t, &self.kin);
204    }
205
206    impl_setup_ik!(SphericalTwoParallelSetup);
207}
208
209impl SetupIk for SphericalTwoIntersectingSetup {
210    fn setup(&mut self) {
211        for i in 0..self.kin.h.ncols() {
212            self.kin.h.set_column(i, &random_norm_vector3());
213        }
214
215        let q = Vector6::zeros().map(|_: f64| random_angle());
216
217        self.kin.p = Matrix3x7::from_columns(&[
218            random_vector3(),
219            Vector3::zeros(),
220            random_vector3(),
221            random_vector3(),
222            Vector3::zeros(),
223            Vector3::zeros(),
224            random_vector3(),
225        ]);
226
227        (self.r, self.t) = self.kin.forward_kinematics(&q);
228    }
229
230    fn run(&mut self) {
231        (self.q, self.is_ls) = spherical_two_intersecting(&self.r, &self.t, &self.kin);
232    }
233
234    impl_setup_ik!(SphericalTwoParallelSetup);
235}
236
237impl SetupIk for SphericalSetup {
238    fn setup(&mut self) {
239        for i in 0..self.kin.h.ncols() {
240            self.kin.h.set_column(i, &random_norm_vector3());
241        }
242
243        let q = Vector6::zeros().map(|_: f64| random_angle());
244
245        self.kin.p = Matrix3x7::from_columns(&[
246            random_vector3(),
247            random_vector3(),
248            random_vector3(),
249            random_vector3(),
250            Vector3::zeros(),
251            Vector3::zeros(),
252            random_vector3(),
253        ]);
254
255        (self.r, self.t) = self.kin.forward_kinematics(&q);
256    }
257
258    fn run(&mut self) {
259        (self.q, self.is_ls) = spherical(&self.r, &self.t, &self.kin);
260    }
261
262    impl_setup_ik!(SphericalTwoParallelSetup);
263}
264
265impl SetupIk for ThreeParallelTwoIntersectingSetup {
266    fn setup(&mut self) {
267        for i in 0..self.kin.h.ncols() {
268            self.kin.h.set_column(i, &random_norm_vector3());
269        }
270
271        let q = Vector6::zeros().map(|_: f64| random_angle());
272
273        let h_1: Vector3<f64> = self.kin.h.column(1).into();
274        self.kin.h.set_column(2, &h_1);
275        self.kin.h.set_column(3, &h_1);
276
277        self.kin.p = Matrix3x7::from_columns(&[
278            random_vector3(),
279            random_vector3(),
280            random_vector3(),
281            random_vector3(),
282            random_vector3(),
283            Vector3::zeros(),
284            random_vector3(),
285        ]);
286
287        (self.r, self.t) = self.kin.forward_kinematics(&q);
288    }
289
290    fn run(&mut self) {
291        (self.q, self.is_ls) = three_parallel_two_intersecting(&self.r, &self.t, &self.kin);
292    }
293
294    impl_setup_ik!(SphericalTwoParallelSetup);
295}
296
297impl SetupIk for ThreeParallelSetup {
298    fn setup(&mut self) {
299        for i in 0..self.kin.h.ncols() {
300            self.kin.h.set_column(i, &random_norm_vector3());
301        }
302
303        let q = Vector6::zeros().map(|_: f64| random_angle());
304
305        let h_1: Vector3<f64> = self.kin.h.column(1).into();
306        self.kin.h.set_column(2, &h_1);
307        self.kin.h.set_column(3, &h_1);
308
309        self.kin.p = Matrix3x7::from_columns(&[
310            random_vector3(),
311            random_vector3(),
312            random_vector3(),
313            random_vector3(),
314            random_vector3(),
315            random_vector3(),
316            random_vector3(),
317        ]);
318
319        (self.r, self.t) = self.kin.forward_kinematics(&q);
320    }
321
322    fn run(&mut self) {
323        (self.q, self.is_ls) = three_parallel(&self.r, &self.t, &self.kin);
324    }
325
326    impl_setup_ik!(SphericalTwoParallelSetup);
327}
328
329impl SetupIk for TwoParallelSetup {
330    fn setup(&mut self) {
331        for i in 0..self.kin.h.ncols() {
332            self.kin.h.set_column(i, &random_norm_vector3());
333        }
334
335        let q = Vector6::zeros().map(|_: f64| random_angle());
336
337        let h_1: Vector3<f64> = self.kin.h.column(1).into();
338        self.kin.h.set_column(2, &h_1);
339
340        self.kin.p = Matrix3x7::from_columns(&[
341            random_vector3(),
342            random_vector3(),
343            random_vector3(),
344            random_vector3(),
345            random_vector3(),
346            random_vector3(),
347            random_vector3(),
348        ]);
349
350        (self.r, self.t) = self.kin.forward_kinematics(&q);
351    }
352
353    fn run(&mut self) {
354        (self.q, self.is_ls) = two_parallel(&self.r, &self.t, &self.kin);
355    }
356
357    impl_setup_ik!(SphericalTwoParallelSetup);
358}
359
360impl SetupIk for TwoIntersectingSetup {
361    fn setup(&mut self) {
362        for i in 0..self.kin.h.ncols() {
363            self.kin.h.set_column(i, &random_norm_vector3());
364        }
365
366        let q = Vector6::zeros().map(|_: f64| random_angle());
367
368        self.kin.p = Matrix3x7::from_columns(&[
369            random_vector3(),
370            random_vector3(),
371            random_vector3(),
372            random_vector3(),
373            random_vector3(),
374            Vector3::zeros(),
375            random_vector3(),
376        ]);
377
378        (self.r, self.t) = self.kin.forward_kinematics(&q);
379    }
380
381    fn run(&mut self) {
382        (self.q, self.is_ls) = two_intersecting(&self.r, &self.t, &self.kin);
383    }
384
385    impl_setup_ik!(SphericalTwoParallelSetup);
386}
387
388impl SetupIk for GenSixDofSetup {
389    fn setup(&mut self) {
390        let q = Vector6::zeros().map(|_: f64| random_angle());
391
392        for i in 0..self.kin.h.ncols() {
393            self.kin.h.set_column(i, &random_norm_vector3());
394        }
395
396        for i in 0..self.kin.p.ncols() {
397            self.kin.p.set_column(i, &random_vector3());
398        }
399
400        (self.r, self.t) = self.kin.forward_kinematics(&q);
401    }
402
403    fn run(&mut self) {
404        (self.q, self.is_ls) = gen_six_dof(&self.r, &self.t, &self.kin);
405    }
406
407    impl_setup_ik!(SphericalTwoParallelSetup);
408}
ik_geo/inverse_kinematics/setups.rs

ik_geo/inverse_kinematics/
setups.rs
