four-bar 7.0.5

Four🍀bar library provides simulation and synthesis function for four-bar linkages.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128

use super::*;

/// Motion generation task of planar four-bar linkage.
pub type MFbSyn = MotionSyn<MNormFourBar, 6, 2>;
/// Motion generation of a mechanism `M`.
pub type MotionSyn<M, const N: usize, const D: usize> = Syn<efd::PosedEfd<D>, M, N, D>;

impl<M, const N: usize, const D: usize> MotionSyn<M, N, D>
where
    efd::U<D>: efd::EfdDim<D>,
{
    /// Create a new task from target (two series of points). The harmonic
    /// number is selected automatically.
    pub fn from_series<C1, C2>(curve1: C1, curve2: C2, mode: Mode) -> Self
    where
        C1: efd::Curve<D>,
        C2: efd::Curve<D>,
    {
        let efd = efd::PosedEfd::from_series(curve1, curve2, mode.is_target_open());
        Self::from_efd(efd, mode)
    }

    /// Create a new task from target (a curve and its unit vectors). The
    /// harmonic number is selected automatically.
    pub fn from_uvec<C, V>(curve: C, vectors: V, mode: Mode) -> Self
    where
        C: efd::Curve<D>,
        V: efd::Curve<D>,
    {
        let efd = efd::PosedEfd::from_uvec(curve, vectors, mode.is_target_open());
        Self::from_efd(efd, mode)
    }

    /// Create a new task from target EFD coefficients.
    pub fn from_efd(efd: efd::PosedEfd<D>, mode: Mode) -> Self {
        Self::new(efd, mode)
    }

    /// The harmonic used of target EFD.
    pub fn harmonic(&self) -> usize {
        self.tar.harmonic()
    }
}

impl<M, const N: usize, const D: usize> mh::Bounded for MotionSyn<M, N, D>
where
    Self: mh::ObjFunc,
    M: mech::FromVectorized<N>,
    efd::U<D>: efd::EfdDim<D>,
{
    #[inline]
    fn bound(&self) -> &[[f64; 2]] {
        if self.mode == Mode::Partial {
            M::BOUND_PARTIAL
        } else {
            &M::BOUND
        }
    }
}

#[doc(hidden)]
#[derive(Clone)]
pub struct MOFit {
    pub curve: f64,
    pub pose: f64,
    pub center: f64,
    pub unit: f64,
}

impl mh::Fitness for MOFit {
    type Best<T: mh::Fitness> = mh::pareto::Pareto<T>;
    type Eval = f64;
    fn is_dominated(&self, rhs: &Self) -> bool {
        self.curve <= rhs.curve
            && self.pose <= rhs.pose
            && self.center <= rhs.center
            && self.unit <= rhs.unit
    }
    fn eval(&self) -> Self::Eval {
        self.curve + self.pose + self.center + self.unit
    }
}

impl Infeasible for MOFit {
    fn infeasible() -> Self {
        Self {
            curve: infeasible(),
            pose: infeasible(),
            center: infeasible(),
            unit: infeasible(),
        }
    }
}

impl<M, const N: usize, const D: usize> mh::ObjFunc for MotionSyn<M, N, D>
where
    M: SynBound<N> + mech::Normalized<D> + mech::PoseGen<D>,
    M::De: Default + Clone + Sync + Send + 'static,
    efd::Efd<D>: Sync + Send,
    efd::U<D>: efd::EfdDim<D>,
{
    type Ys = mh::WithProduct<MOFit, M::De>;

    fn fitness(&self, xs: &[f64]) -> Self::Ys {
        let is_open = self.mode.is_target_open();
        let get_series = |fb: &M, start, end| {
            let (curve, pose) = fb.pose_in(start, end, self.res);
            (curve.len() > 2).then_some((curve, pose))
        };
        impl_fitness(self.mode, xs, get_series, |((c, v), fb)| {
            let efd = efd::PosedEfd::from_uvec_harmonic(c, v, is_open, self.harmonic());
            let geo = efd.as_curve().as_geo().to(self.tar.as_curve().as_geo());
            let fb = fb.clone().trans_denorm(&geo);
            let err = MOFit {
                curve: efd.as_curve().err(self.tar.as_curve()),
                pose: efd.as_pose().err(self.tar.as_pose()),
                center: {
                    use efd::Distance as _;
                    let me = efd.as_pose().as_geo().trans();
                    let tar = self.tar.as_pose().as_geo().trans();
                    me.l2_err(tar)
                },
                unit: self.unit_err(&geo),
            };
            mh::WithProduct::new(err, fb)
        })
    }
}