franka/gripper/
mod.rs

1//! Franka Hand gripper client.
2//!
3//! Port of libfranka 0.21.2 `include/franka/gripper.h`, `include/franka/gripper_state.h` and
4//! `src/gripper.cpp`. [`Gripper`] maintains a network connection to the gripper, provides the
5//! current gripper state via [`Gripper::read_once`], and executes the four gripper commands
6//! (`homing`, `grasp`, `move_gripper`, `stop`).
7//!
8//! The gripper protocol reuses the FCI's TCP/UDP framing (see [`crate::network`]) with a
9//! 10-byte command header and `u16` statuses (see [`crate::wire::gripper`]) instead of the
10//! robot's 12-byte header and `u8` statuses.
11
12use std::mem::size_of;
13
14use zerocopy::{FromBytes, IntoBytes};
15
16use crate::duration::Duration;
17use crate::error::{FrankaError, FrankaResult};
18use crate::network::{connect_gripper, Network};
19use crate::wire::gripper::{
20    GraspRequest, GripperCommand, GripperCommandResponse, GripperMoveRequest,
21    GripperState as WireGripperState, GripperStatus,
22};
23use crate::wire::{parse_response, HeaderLayout};
24
25/// Describes the gripper state (`franka::GripperState`).
26#[derive(Debug, Clone, Copy, PartialEq)]
27pub struct GripperState {
28    /// Current gripper opening width, in metres.
29    pub width: f64,
30    /// Maximum gripper opening width, in metres. Estimated by [`Gripper::homing`]; see
31    /// `franka::GripperState::max_width`.
32    pub max_width: f64,
33    /// Whether an object is currently grasped.
34    pub is_grasped: bool,
35    /// Current gripper temperature, in degrees Celsius.
36    pub temperature: u16,
37    /// Strictly monotonically increasing timestamp since the gripper server started.
38    pub time: Duration,
39}
40
41impl From<&WireGripperState> for GripperState {
42    /// Port of the anonymous `convertGripperState` in `src/gripper.cpp`.
43    fn from(wire: &WireGripperState) -> Self {
44        GripperState {
45            width: wire.width.get(),
46            max_width: wire.max_width.get(),
47            is_grasped: wire.is_grasped != 0,
48            temperature: wire.temperature.get(),
49            time: Duration::from_millis(u64::from(wire.message_id.get())),
50        }
51    }
52}
53
54/// Maintains a network connection to the gripper, provides the current gripper state, and
55/// allows the execution of commands (port of `franka::Gripper`).
56///
57/// The underlying [`Network`] is `Sync` (see [`crate::network::TcpSession`] and
58/// [`crate::network::UdpChannel`]), so the members of this struct are threadsafe, matching
59/// libfranka's documented guarantee for `franka::Gripper` -- except that only one
60/// [`Gripper::read_once`] should be in flight at a time, since concurrent callers would race to
61/// claim the same UDP datagram.
62#[derive(Debug)]
63pub struct Gripper {
64    network: Network,
65    server_version: u16,
66}
67
68impl Gripper {
69    /// Establishes a connection with a gripper connected to a robot.
70    ///
71    /// `franka_address` is the IP/hostname of the robot the gripper is connected to, as
72    /// `"host"` or `"host:port"`; the gripper command port (1338) is used unless an explicit
73    /// port is embedded in the address.
74    ///
75    /// # Errors
76    /// Returns [`FrankaError::Network`] if the connection is unsuccessful and
77    /// [`FrankaError::IncompatibleVersion`] if this crate's gripper protocol version is not
78    /// supported by the server.
79    pub fn new(franka_address: &str) -> FrankaResult<Gripper> {
80        let (network, server_version) = connect_gripper(franka_address)?;
81        Ok(Gripper {
82            network,
83            server_version,
84        })
85    }
86
87    /// Returns the software version reported by the connected server.
88    pub fn server_version(&self) -> u16 {
89        self.server_version
90    }
91
92    /// Performs homing of the gripper.
93    ///
94    /// After changing the gripper fingers, a homing needs to be done. This is needed to
95    /// estimate the maximum grasping width.
96    ///
97    /// # Errors
98    /// Returns [`FrankaError::Command`] if the command failed or was aborted, and
99    /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout.
100    ///
101    /// See [`GripperState::max_width`] for the maximum grasping width.
102    pub fn homing(&self) -> FrankaResult<bool> {
103        self.execute_command(GripperCommand::Homing, &[])
104    }
105
106    /// Grasps an object.
107    ///
108    /// An object is considered grasped if the distance `d` between the gripper fingers
109    /// satisfies `(width - epsilon_inner) < d < (width + epsilon_outer)`.
110    ///
111    /// `epsilon_inner` and `epsilon_outer` default to `0.005` in libfranka's C++ overload; this
112    /// port takes both explicitly (there is no default-argument mechanism in a `fn`).
113    ///
114    /// # Errors
115    /// Returns [`FrankaError::Command`] if the command failed or was aborted, and
116    /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout.
117    pub fn grasp(
118        &self,
119        width: f64,
120        speed: f64,
121        force: f64,
122        epsilon_inner: f64,
123        epsilon_outer: f64,
124    ) -> FrankaResult<bool> {
125        let request = GraspRequest::new(width, epsilon_inner, epsilon_outer, speed, force);
126        self.execute_command(GripperCommand::Grasp, request.as_bytes())
127    }
128
129    /// Moves the gripper fingers to a specified width.
130    ///
131    /// # Errors
132    /// Returns [`FrankaError::Command`] if the command failed or was aborted, and
133    /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout.
134    pub fn move_gripper(&self, width: f64, speed: f64) -> FrankaResult<bool> {
135        let request = GripperMoveRequest::new(width, speed);
136        self.execute_command(GripperCommand::Move, request.as_bytes())
137    }
138
139    /// Stops a currently running gripper move or grasp.
140    ///
141    /// # Errors
142    /// Returns [`FrankaError::Command`] if the command failed or was aborted, and
143    /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout.
144    pub fn stop(&self) -> FrankaResult<bool> {
145        self.execute_command(GripperCommand::Stop, &[])
146    }
147
148    /// Waits for a gripper state update and returns it.
149    ///
150    /// Port of `Gripper::readOnce`: first drains every datagram already queued on the UDP
151    /// socket (stale data from before this call), then blocks for one fresh state.
152    ///
153    /// # Errors
154    /// Returns [`FrankaError::Network`] if the connection is lost, e.g. after a timeout.
155    pub fn read_once(&self) -> FrankaResult<GripperState> {
156        let mut buf = [0u8; size_of::<WireGripperState>()];
157        // Delete old data from the UDP buffer.
158        while self.network.udp.try_receive(&mut buf)?.is_some() {}
159
160        let n = self.network.udp.blocking_receive(&mut buf)?;
161        if n != buf.len() {
162            return Err(FrankaError::Protocol(
163                "libfranka: incorrect object size".to_string(),
164            ));
165        }
166        let wire_state = WireGripperState::read_from_bytes(&buf)
167            .map_err(|_| FrankaError::Protocol("libfranka: incorrect object size".to_string()))?;
168        Ok(GripperState::from(&wire_state))
169    }
170
171    /// Sends a gripper command and maps the response status, exactly like the anonymous
172    /// `executeCommand<T>` in `src/gripper.cpp`.
173    fn execute_command(&self, command: GripperCommand, payload: &[u8]) -> FrankaResult<bool> {
174        let command_id = self
175            .network
176            .tcp
177            .send_request(command.to_u16() as u32, payload)?;
178        let message = self.network.tcp.blocking_receive_response(command_id)?;
179        let response: GripperCommandResponse = parse_response(HeaderLayout::Gripper, &message)?;
180
181        match GripperStatus::from_u16(response.status.get()) {
182            Some(GripperStatus::Success) => Ok(true),
183            Some(GripperStatus::Fail) => Err(FrankaError::Command(
184                "libfranka gripper: Command failed!".to_string(),
185            )),
186            Some(GripperStatus::Unsuccessful) => Ok(false),
187            Some(GripperStatus::Aborted) => Err(FrankaError::Command(
188                "libfranka gripper: Command aborted!".to_string(),
189            )),
190            None => Err(FrankaError::Protocol(
191                "libfranka gripper: Unexpected response while handling command!".to_string(),
192            )),
193        }
194    }
195}
196
197#[cfg(test)]
198mod tests {
199    use super::*;
200    use std::io::{Read, Write};
201    use std::net::{Ipv4Addr, SocketAddr, TcpListener, TcpStream, UdpSocket};
202    use std::time::Instant;
203
204    use zerocopy::little_endian::U16;
205
206    use crate::wire::gripper::{GripperConnectResponse, GripperConnectStatus, GRIPPER_VERSION};
207    use crate::wire::GRIPPER_HEADER_LEN;
208
209    /// Accepts a single connection on an ephemeral loopback port and runs `serve` on it,
210    /// returning whatever `serve` returns.
211    fn accept_and_serve<F, T>(serve: F) -> (SocketAddr, std::thread::JoinHandle<T>)
212    where
213        F: FnOnce(TcpStream) -> T + Send + 'static,
214        T: Send + 'static,
215    {
216        let listener = TcpListener::bind("127.0.0.1:0").unwrap();
217        let addr = listener.local_addr().unwrap();
218        let handle = std::thread::spawn(move || {
219            let (stream, _) = listener.accept().unwrap();
220            serve(stream)
221        });
222        (addr, handle)
223    }
224
225    /// Reads one gripper command message (header + payload) off `stream`.
226    fn read_message(stream: &mut TcpStream) -> (u16, u32, Vec<u8>) {
227        let mut header = [0u8; GRIPPER_HEADER_LEN];
228        stream.read_exact(&mut header).unwrap();
229        let (command, command_id, size) = HeaderLayout::Gripper.decode_header(&header).unwrap();
230        let mut payload = vec![0u8; size as usize - GRIPPER_HEADER_LEN];
231        stream.read_exact(&mut payload).unwrap();
232        (command as u16, command_id, payload)
233    }
234
235    /// Writes one gripper response message.
236    fn write_message(stream: &mut TcpStream, command: u16, command_id: u32, payload: &[u8]) {
237        let mut message =
238            HeaderLayout::Gripper.encode_header(command as u32, command_id, payload.len());
239        message.extend_from_slice(payload);
240        stream.write_all(&message).unwrap();
241        stream.flush().unwrap();
242    }
243
244    /// Reads the `Connect` request, replies with `status`/`version`, and hands the (still open)
245    /// stream to `then`.
246    fn serve_connect<F, T>(
247        mut stream: TcpStream,
248        status: GripperConnectStatus,
249        version: u16,
250        then: F,
251    ) -> T
252    where
253        F: FnOnce(TcpStream) -> T,
254    {
255        let (command, command_id, payload) = read_message(&mut stream);
256        assert_eq!(command, GripperCommand::Connect.to_u16());
257        assert_eq!(payload.len(), 4, "Connect::Request is 4 bytes");
258        let response = GripperConnectResponse {
259            status: U16::new(status.to_u16()),
260            version: U16::new(version),
261        };
262        write_message(&mut stream, command, command_id, response.as_bytes());
263        then(stream)
264    }
265
266    #[test]
267    fn wire_gripper_state_is_23_bytes() {
268        assert_eq!(size_of::<WireGripperState>(), 23);
269    }
270
271    #[test]
272    fn connect_reports_server_version() {
273        let (addr, server) = accept_and_serve(|stream| {
274            serve_connect(
275                stream,
276                GripperConnectStatus::Success,
277                GRIPPER_VERSION,
278                |stream| {
279                    std::thread::sleep(std::time::Duration::from_millis(50));
280                    drop(stream);
281                },
282            );
283        });
284
285        let gripper = Gripper::new(&addr.to_string()).expect("handshake failed");
286        assert_eq!(gripper.server_version(), GRIPPER_VERSION);
287
288        drop(gripper);
289        server.join().unwrap();
290    }
291
292    /// Port of `Gripper.ThrowsOnIncompatibleLibraryVersion` (`test/gripper_tests.cpp`).
293    #[test]
294    fn connect_reports_incompatible_version_like_libfranka() {
295        let (addr, server) = accept_and_serve(|stream| {
296            serve_connect(
297                stream,
298                GripperConnectStatus::IncompatibleLibraryVersion,
299                7,
300                |stream| {
301                    std::thread::sleep(std::time::Duration::from_millis(50));
302                    drop(stream);
303                },
304            );
305        });
306
307        let error = Gripper::new(&addr.to_string()).expect_err("expected a handshake failure");
308        match error {
309            FrankaError::IncompatibleVersion {
310                server_version,
311                library_version,
312            } => {
313                assert_eq!(server_version, 7);
314                assert_eq!(library_version, GRIPPER_VERSION);
315            }
316            other => panic!("expected IncompatibleVersion, got {other:?}"),
317        }
318        assert_eq!(
319            error.to_string(),
320            format!(
321                "libfranka: Incompatible library version (server version: 7, library version: {GRIPPER_VERSION})."
322            )
323        );
324
325        server.join().unwrap();
326    }
327
328    /// Connects a `Gripper`, then answers exactly one subsequent command message with
329    /// `raw_status`, returning the gripper and a handle that yields the command byte the mock
330    /// server actually received.
331    fn mock_command_server(raw_status: u16) -> (Gripper, std::thread::JoinHandle<u16>) {
332        let (addr, server) = accept_and_serve(move |stream| {
333            serve_connect(
334                stream,
335                GripperConnectStatus::Success,
336                GRIPPER_VERSION,
337                move |mut stream| {
338                    let (command, command_id, _payload) = read_message(&mut stream);
339                    let response = GripperCommandResponse {
340                        status: U16::new(raw_status),
341                    };
342                    write_message(&mut stream, command, command_id, response.as_bytes());
343                    std::thread::sleep(std::time::Duration::from_millis(50));
344                    command
345                },
346            )
347        });
348        let gripper = Gripper::new(&addr.to_string()).expect("handshake failed");
349        (gripper, server)
350    }
351
352    /// What a status-mapping subtest expects `execute_command`'s result to look like.
353    enum Expect {
354        Ok(bool),
355        Command(&'static str),
356        Protocol,
357    }
358
359    /// Port of the `GripperCommand<T>` typed tests in `test/gripper_command_tests.cpp`
360    /// (`CanSendAndReceiveSuccess/Fail/Unsucessful/Aborted`), plus an "unrecognized status"
361    /// case (not exercised by libfranka's own C++ tests, since every status its mock server
362    /// can send is a valid enumerator).
363    #[test]
364    fn command_status_mapping() {
365        type Run = fn(&Gripper) -> FrankaResult<bool>;
366        let commands: [(&str, u16, Run); 4] = [
367            ("homing", GripperCommand::Homing.to_u16(), |g| g.homing()),
368            ("move", GripperCommand::Move.to_u16(), |g| {
369                g.move_gripper(0.05, 0.1)
370            }),
371            ("grasp", GripperCommand::Grasp.to_u16(), |g| {
372                g.grasp(0.05, 0.1, 400.0, 0.004, 0.005)
373            }),
374            ("stop", GripperCommand::Stop.to_u16(), |g| g.stop()),
375        ];
376
377        let cases: [(u16, Expect); 5] = [
378            (GripperStatus::Success.to_u16(), Expect::Ok(true)),
379            (
380                GripperStatus::Fail.to_u16(),
381                Expect::Command("libfranka gripper: Command failed!"),
382            ),
383            (GripperStatus::Unsuccessful.to_u16(), Expect::Ok(false)),
384            (
385                GripperStatus::Aborted.to_u16(),
386                Expect::Command("libfranka gripper: Command aborted!"),
387            ),
388            (99, Expect::Protocol),
389        ];
390
391        for (name, expected_command, run) in commands {
392            for (raw_status, expect) in &cases {
393                let (gripper, server) = mock_command_server(*raw_status);
394                let result = run(&gripper);
395                let received_command = server.join().unwrap();
396                assert_eq!(
397                    received_command, expected_command,
398                    "{name}: wrong command byte on the wire"
399                );
400
401                match expect {
402                    Expect::Ok(expected) => {
403                        assert_eq!(result.unwrap(), *expected, "{name}, status {raw_status}")
404                    }
405                    Expect::Command(message) => {
406                        let error = result.unwrap_err();
407                        assert_eq!(&error.to_string(), message, "{name}, status {raw_status}");
408                        assert!(matches!(error, FrankaError::Command(_)));
409                    }
410                    Expect::Protocol => {
411                        let error = result.unwrap_err();
412                        assert_eq!(
413                            error.to_string(),
414                            "libfranka gripper: Unexpected response while handling command!",
415                            "{name}, status {raw_status}"
416                        );
417                        assert!(matches!(error, FrankaError::Protocol(_)));
418                    }
419                }
420
421                drop(gripper);
422            }
423        }
424    }
425
426    /// Builds a wire `GripperState` datagram for the mock UDP server below.
427    fn wire_state(message_id: u32, width: f64) -> WireGripperState {
428        WireGripperState {
429            message_id: zerocopy::little_endian::U32::new(message_id),
430            width: zerocopy::little_endian::F64::new(width),
431            max_width: zerocopy::little_endian::F64::new(0.08),
432            is_grasped: 0,
433            temperature: U16::new(25),
434        }
435    }
436
437    #[test]
438    fn read_once_drains_queued_datagrams_then_blocks_for_a_fresh_one() {
439        // Serves the Connect handshake by hand (rather than via `serve_connect`) so the mock
440        // server can read the client's UDP port out of the request payload.
441        let (addr, server) = accept_and_serve(|mut stream| {
442            let (command, command_id, payload) = read_message(&mut stream);
443            assert_eq!(command, GripperCommand::Connect.to_u16());
444            assert_eq!(payload.len(), 4, "Connect::Request is 4 bytes");
445            let udp_port = u16::from_le_bytes([payload[2], payload[3]]);
446
447            let response = GripperConnectResponse {
448                status: U16::new(GripperConnectStatus::Success.to_u16()),
449                version: U16::new(GRIPPER_VERSION),
450            };
451            write_message(&mut stream, command, command_id, response.as_bytes());
452            std::thread::sleep(std::time::Duration::from_millis(50));
453            drop(stream);
454            udp_port
455        });
456
457        let gripper = Gripper::new(&addr.to_string()).expect("handshake failed");
458        let udp_port = server.join().unwrap();
459
460        let udp_server = UdpSocket::bind((Ipv4Addr::LOCALHOST, 0)).unwrap();
461        let peer: SocketAddr = (Ipv4Addr::LOCALHOST, udp_port).into();
462
463        // Queue three stale datagrams before ever calling read_once.
464        for id in 1..=3u32 {
465            udp_server
466                .send_to(wire_state(id, 0.01).as_bytes(), peer)
467                .unwrap();
468        }
469        std::thread::sleep(std::time::Duration::from_millis(50));
470
471        // A fresh datagram, sent only after read_once has had time to drain the stale ones and
472        // start blocking.
473        let sender = std::thread::spawn(move || {
474            std::thread::sleep(std::time::Duration::from_millis(150));
475            udp_server
476                .send_to(wire_state(4, 0.02).as_bytes(), peer)
477                .unwrap();
478        });
479
480        let started = Instant::now();
481        let state = gripper.read_once().expect("read_once failed");
482        let elapsed = started.elapsed();
483
484        assert!((state.width - 0.02).abs() < 1e-9, "got {}", state.width);
485        assert_eq!(state.time.as_millis(), 4);
486        assert!(
487            elapsed >= std::time::Duration::from_millis(100),
488            "read_once returned after {elapsed:?}; it should have blocked for the fresh datagram \
489             instead of returning stale data"
490        );
491
492        sender.join().unwrap();
493        drop(gripper);
494    }
495}
franka/gripper/mod.rs

franka/gripper/
mod.rs
