libaubo 0.1.1

Rust bindings for the Hans robot
Documentation 
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
129
130
131
132
133
134

use std::marker::PhantomData;

use robot_behavior::{
    Arm, ArmDOF, ArmParam, ArmState, Coord, LoadState, OverrideOnce, RobotResult,
};

pub trait AuboType {
    const N: usize;
}

pub struct AuboRobot<T: AuboType, const N: usize> {
    marker: PhantomData<T>,
    pub(crate) coord: OverrideOnce<Coord>,
    pub(crate) max_vel: OverrideOnce<[f64; N]>,
    pub(crate) max_acc: OverrideOnce<[f64; N]>,
    pub(crate) max_cartesian_vel: OverrideOnce<f64>,
    pub(crate) max_cartesian_acc: OverrideOnce<f64>,
    pub(crate) max_rotation_vel: OverrideOnce<f64>,
    pub(crate) max_rotation_acc: OverrideOnce<f64>,
}

impl<T: AuboType> ArmDOF for AuboRobot<T, { T::N }> {
    const N: usize = { T::N };
}

impl<T: AuboType, const N: usize> AuboRobot<T, N>
where
    Self: ArmParam<N>,
{
    pub fn new(_ip: &str) -> Self {
        Self {
            marker: PhantomData,
            coord: OverrideOnce::new(Coord::OCS),
            max_vel: OverrideOnce::new(Self::JOINT_VEL_BOUND),
            max_acc: OverrideOnce::new(Self::JOINT_ACC_BOUND),
            max_cartesian_vel: OverrideOnce::new(Self::CARTESIAN_VEL_BOUND),
            max_cartesian_acc: OverrideOnce::new(Self::CARTESIAN_ACC_BOUND),
            max_rotation_vel: OverrideOnce::new(Self::ROTATION_VEL_BOUND),
            max_rotation_acc: OverrideOnce::new(Self::ROTATION_ACC_BOUND),
        }
    }
}

impl<T: AuboType, const N: usize> Arm<N> for AuboRobot<T, N>
where
    AuboRobot<T, N>: ArmParam<N>,
{
    fn state(&mut self) -> RobotResult<ArmState<N>> {
        unimplemented!()
    }
    fn set_load(&mut self, _load: LoadState) -> RobotResult<()> {
        unimplemented!()
    }
    fn set_coord(&mut self, coord: Coord) -> RobotResult<()> {
        self.coord.set(coord);
        Ok(())
    }
    fn set_speed(&mut self, speed: f64) -> RobotResult<()> {
        self.max_vel.set(Self::JOINT_VEL_BOUND.map(|v| v * speed));
        self.max_acc.set(Self::JOINT_ACC_BOUND.map(|v| v * speed));
        self.max_cartesian_vel
            .set(Self::CARTESIAN_VEL_BOUND * speed);
        self.max_cartesian_acc
            .set(Self::CARTESIAN_ACC_BOUND * speed);
        Ok(())
    }

    fn with_coord(&mut self, coord: Coord) -> &mut Self {
        self.coord.once(coord);
        self
    }
    fn with_speed(&mut self, speed: f64) -> &mut Self {
        self.max_vel.once(Self::JOINT_VEL_BOUND.map(|v| v * speed));
        self.max_acc.once(Self::JOINT_ACC_BOUND.map(|v| v * speed));
        self.max_cartesian_vel
            .once(Self::CARTESIAN_VEL_BOUND * speed);
        self.max_cartesian_acc
            .once(Self::CARTESIAN_ACC_BOUND * speed);
        self
    }
    fn with_velocity(&mut self, joint_vel: &[f64; N]) -> &mut Self {
        self.max_vel.once(*joint_vel);
        self
    }
    fn with_acceleration(&mut self, joint_acc: &[f64; N]) -> &mut Self {
        self.max_acc.once(*joint_acc);
        self
    }
    fn with_jerk(&mut self, _joint_jerk: &[f64; N]) -> &mut Self {
        self
    }
    fn with_cartesian_velocity(&mut self, cartesian_vel: f64) -> &mut Self {
        self.max_cartesian_vel.once(cartesian_vel);
        self
    }
    fn with_cartesian_acceleration(&mut self, cartesian_acc: f64) -> &mut Self {
        self.max_cartesian_acc.once(cartesian_acc);
        self
    }
    fn with_cartesian_jerk(&mut self, _cartesian_jerk: f64) -> &mut Self {
        self
    }
    fn with_rotation_velocity(&mut self, rotation_vel: f64) -> &mut Self {
        self.max_rotation_vel.once(rotation_vel);
        self
    }
    fn with_rotation_acceleration(&mut self, rotation_acc: f64) -> &mut Self {
        self.max_rotation_acc.once(rotation_acc);
        self
    }
    fn with_rotation_jerk(&mut self, _rotation_jerk: f64) -> &mut Self {
        self
    }
}

impl<T: AuboType, const N: usize> ArmParam<N> for AuboRobot<T, N>
where
    T: ArmParam<N>,
{
    const JOINT_DEFAULT: [f64; N] = T::JOINT_DEFAULT;
    const JOINT_MIN: [f64; N] = T::JOINT_MIN;
    const JOINT_MAX: [f64; N] = T::JOINT_MAX;
    const JOINT_VEL_BOUND: [f64; N] = T::JOINT_VEL_BOUND;
    const JOINT_ACC_BOUND: [f64; N] = T::JOINT_ACC_BOUND;
    const JOINT_JERK_BOUND: [f64; N] = T::JOINT_JERK_BOUND;
    const CARTESIAN_VEL_BOUND: f64 = T::CARTESIAN_VEL_BOUND;
    const CARTESIAN_ACC_BOUND: f64 = T::CARTESIAN_ACC_BOUND;
    const CARTESIAN_JERK_BOUND: f64 = T::CARTESIAN_JERK_BOUND;
    const ROTATION_VEL_BOUND: f64 = T::ROTATION_VEL_BOUND;
    const ROTATION_ACC_BOUND: f64 = T::ROTATION_ACC_BOUND;
    const ROTATION_JERK_BOUND: f64 = T::ROTATION_JERK_BOUND;
    const TORQUE_BOUND: [f64; N] = T::TORQUE_BOUND;
    const TORQUE_DOT_BOUND: [f64; N] = T::TORQUE_DOT_BOUND;
}