franka_rust 0.1.14

Rust bindings for the Franka Emika Panda 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

use robot_behavior::{RobotException, RobotResult};
use std::sync::{Arc, RwLock};

use crate::{
    network::Network,
    types::{gripper_command::GripperCommand, gripper_state::*, gripper_types::*},
    FRANKA_GRIPPER_VERSION, PORT_GRIPPER_COMMAND, PORT_GRIPPER_UDP,
};

/// # FrankaGripper
///
pub struct FrankaGripper {
    network: Network,
    gripper_state: Arc<RwLock<GripperStateInter>>,
}

macro_rules! cmd_fn {
    ($fn_name:ident, $command:expr; $arg_name:ident: $arg_type:ty ; $ret_type:ty) => {
        fn $fn_name(&mut self, $arg_name: $arg_type) -> RobotResult<$ret_type> {
            let response: Response<$command, $ret_type> = self
                .network
                .tcp_send_and_recv(&mut Request::<$command, $arg_type>::from($arg_name))?;
            Ok(response.status)
        }
    };
}

impl FrankaGripper {
    pub fn new(ip: &str) -> Self {
        let (_, gripper_state) = Network::spawn_udp_thread::<GripperCommand, _>(PORT_GRIPPER_UDP);
        let mut gripper = FrankaGripper {
            network: Network::new(ip, PORT_GRIPPER_COMMAND),
            gripper_state,
        };
        gripper.connect().unwrap();
        gripper
    }

    cmd_fn!(_connect, { Command::Connect}; data: ConnectData; ConnectStatus);
    cmd_fn!(_homing, { Command::Homing}; data: (); HomingStatus);
    cmd_fn!(_grasp, { Command::Grasp}; data: GraspData; GraspStatus);
    cmd_fn!(_move, { Command::Move}; data: MoveData; MoveStatus);
    cmd_fn!(_stop, { Command::Stop}; data: (); StopStatus);

    pub fn connect(&mut self) -> RobotResult<()> {
        let result = self._connect(ConnectData {
            version: FRANKA_GRIPPER_VERSION,
            udp_port: PORT_GRIPPER_UDP,
        })?;
        if let Status::Success = result.status {
            Ok(())
        } else {
            Err(RobotException::IncompatibleVersionException {
                server_version: result.version as u64,
                client_version: FRANKA_GRIPPER_VERSION as u64,
            })
        }
    }

    pub fn homing(&mut self) -> RobotResult<bool> {
        self._homing(())?.into()
    }

    pub fn grasp(&mut self, width: f64, speed: f64, force: f64) -> RobotResult<bool> {
        let data = GraspData { width, epsilon: (0.01, 0.01), speed, force };
        self._grasp(data)?.into()
    }

    pub fn move_gripper(&mut self, width: f64, speed: f64) -> RobotResult<bool> {
        let data = MoveData { width, speed };
        self._move(data)?.into()
    }

    pub fn stop(&mut self) -> RobotResult<bool> {
        self._stop(())?.into()
    }

    pub fn read_state(&mut self) -> RobotResult<GripperState> {
        let state = self.gripper_state.read().unwrap();
        Ok((*state).into())
    }
}