libhans 0.2.0

Rust bindings for the Hans robot
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

use pyo3::{PyResult, pyclass, pymethods};
use robot_behavior::{
    behavior::*, py_arm, py_flange_move, py_flange_traj, py_joint_motion, py_robot,
};

use crate::{HANS_DOF, HansS30};

#[pyclass(name = "HansS30")]
pub struct PyHansS30(HansS30);

#[pymethods]
impl PyHansS30 {
    #[new]
    fn new(ip: &str) -> Self {
        PyHansS30(HansS30::new(ip))
    }

    fn __repr__(&self) -> String {
        "HansS30".to_string()
    }

    fn connect(&mut self, ip: &str, port: u16) {
        self.0.connect(ip, port)
    }

    fn disconnect(&mut self) {
        self.0.disconnect()
    }

    fn read_joint(&mut self) -> PyResult<[f64; HANS_DOF]> {
        self.0
            .state()
            .map(|s| s.joint.meas.q.unwrap_or_default())
            .map_err(Into::into)
    }

    fn read_joint_vel(&mut self) -> PyResult<[f64; HANS_DOF]> {
        self.0
            .state()
            .map(|s| s.joint.meas.dq.unwrap_or_default())
            .map_err(Into::into)
    }

    fn read_cartesian_euler(&mut self) -> PyResult<[f64; 6]> {
        self.0
            .state()
            .map(|s| s.flange.meas.pose.unwrap_or_default().into())
            .map_err(Into::into)
    }

    fn read_cartesian_vel(&mut self) -> PyResult<[f64; 6]> {
        self.0
            .state()
            .map(|s| s.flange.meas.vel.unwrap_or_default())
            .map_err(Into::into)
    }
}

py_robot!(PyHansS30(HansS30));
py_arm!(PyHansS30<{6}>(HansS30));
py_joint_motion!(PyHansS30<{6}>(HansS30));
py_flange_move!(PyHansS30(HansS30));
py_flange_traj!(PyHansS30(HansS30));