dynpick_force_torque_sensor/
lib.rs

1//! Unofficial device driver for [Dyn Pick, Wacoh-tech force-torque sensor](https://wacoh-tech.com/en/products/dynpick/).
2//! # Examples
3//! ```no_run
4//! use dynpick_force_torque_sensor::DynpickSensorBuilder;
5//!
6//! let mut sensor = DynpickSensorBuilder::open("/dev/ttyUSB0")
7//!     .and_then(|b| b.set_sensitivity_by_builtin_data())
8//!     .and_then(|b| b.build())
9//!     .unwrap();
10//!
11//! sensor.zeroed_next().unwrap(); // Calibration
12//!
13//! let wrench = sensor.update().unwrap();
14//! println!("Force: {}, Torque: {}", wrench.force, wrench.torque);
15//! ```
16//!
17//! # Dependency under Linux environment
18//! `libudev-dev` is required under Linux environment. Please install it by  
19//! `sudo apt install libudev-dev`
20//!
21//! # Setup
22//! It may be required to customize udev rules.
23//!
24//! [This shell script](https://github.com/Amelia10007/dynpick-force-torque-sensor-rs/blob/master/examples/setup_udev_rule.sh) can be useful for customize (see the file in detail).
25//!
26//! # Note
27//! I tested this crate only by WDF-6M200-3 sensor because I have no other dynpick sensor.
28#![warn(missing_docs)]
29
30use easy_ext::ext;
31use itertools::Itertools;
32pub use pair_macro::Triplet;
33pub use serialport;
34use serialport::{DataBits, FlowControl, Parity, SerialPort, StopBits};
35use std::borrow::Cow;
36use std::fmt::{self, Display, Formatter};
37use std::marker::PhantomData;
38use std::str::FromStr;
39use std::time::Duration;
40
41/// Marker type for builder.
42pub struct Ready;
43
44/// Marker type for builder.
45pub struct SensitivityNotSetYet;
46
47/// Builder of a connection to a dynpick sensor.
48/// # Examples
49/// ```no_run
50/// use dynpick_force_torque_sensor::DynpickSensorBuilder;
51///
52/// let sensor = DynpickSensorBuilder::open("/dev/ttyUSB0")
53///     .and_then(|b| b.set_sensitivity_by_builtin_data())
54///     .and_then(|b| b.build())
55///     .unwrap();
56/// ```
57pub struct DynpickSensorBuilder<C> {
58    /// Serial port device.
59    port: Box<dyn SerialPort>,
60    /// The sensitivity of the connected sensor.
61    sensitivity: Sensitivity,
62    /// 👻
63    _calibrated: PhantomData<fn() -> C>,
64}
65
66impl DynpickSensorBuilder<SensitivityNotSetYet> {
67    /// Connects to the dynpick force torque sensor.
68    /// # Params
69    /// 1. `path` The sensor's path.
70    ///
71    /// # Returns
72    /// `Ok(builder)` if successfully connected, `Err(reason)` if failed.  
73    /// Before you use the sensor, you need to calibrate the sensor by calling [`Self::set_sensitivity_by_builtin_data`] or [`Self::set_sensitivity_manually`].
74    ///
75    /// # Examples
76    /// See the example [here](`DynpickSensorBuilder`).
77    pub fn open<'a>(
78        path: impl Into<Cow<'a, str>>,
79    ) -> Result<DynpickSensorBuilder<SensitivityNotSetYet>, Error> {
80        // These settings were determined according to the hardware configuration.
81        let port = serialport::new(path, 921600)
82            .data_bits(DataBits::Eight)
83            .flow_control(FlowControl::None)
84            .parity(Parity::None)
85            .stop_bits(StopBits::One)
86            .timeout(Duration::from_millis(1))
87            .open()
88            .map_err(Error::SerialPort)?;
89
90        let builder = Self {
91            port,
92            sensitivity: Sensitivity {
93                digital_per_newton: Triplet::default(),
94                digital_per_newtonmeter: Triplet::default(),
95            },
96            _calibrated: PhantomData,
97        };
98
99        Ok(builder)
100    }
101
102    ///  Set the [`Sensitivity`] of the connected sensor by using the specified sensitivity.
103    /// # Examples
104    /// ```no_run
105    /// use dynpick_force_torque_sensor::{DynpickSensorBuilder, Sensitivity, Triplet};
106    ///
107    /// let sensitivity = {
108    ///     let force = Triplet::new(24.9, 24.6, 24.5);
109    ///     let torque = Triplet::new(1664.7, 1639.7, 1638.0);
110    ///     Sensitivity::new(force, torque)
111    /// };
112    ///
113    /// let sensor = DynpickSensorBuilder::open("/dev/ttyUSB0")
114    ///     .map(|b| b.set_sensitivity_manually(sensitivity))
115    ///     .and_then(|b| b.build())
116    ///     .unwrap();
117    /// ```
118    pub fn set_sensitivity_manually(self, sensitivity: Sensitivity) -> DynpickSensorBuilder<Ready> {
119        DynpickSensorBuilder {
120            port: self.port,
121            sensitivity,
122            _calibrated: PhantomData,
123        }
124    }
125
126    /// Set the [`Sensitivity`] of the connected sensor, reading its sensitivity from it.  
127    /// Some sensors may not support this functionality (`Err(_)` will be returned under this situation).
128    /// # Examples
129    /// See the example [here](`DynpickSensorBuilder`)
130    ///
131    /// # Note
132    /// This method has not been tested yet because my sensor (WDF-6M200-3) does not support this functionality.
133    pub fn set_sensitivity_by_builtin_data(mut self) -> Result<DynpickSensorBuilder<Ready>, Error> {
134        const SENSITIVITY_RESPONSE_LENGTH: usize = 46;
135
136        // Send and wait.
137        self.port.write_all(&['p' as u8]).map_err(Error::IO)?;
138        std::thread::sleep(self.port.timeout());
139
140        let mut res = [0; SENSITIVITY_RESPONSE_LENGTH];
141        self.port.read_exact(&mut res).map_err(Error::IO)?;
142
143        let res = std::str::from_utf8(&res).or(Err(Error::Utf8(res.to_vec())))?;
144        let (fx, fy, fz, mx, my, mz) = res
145            .split(',')
146            .map(f64::from_str)
147            .filter_map(Result::ok)
148            .next_tuple()
149            .ok_or(Error::ParseResponse(res.to_owned()))?;
150
151        let force = Triplet::new(fx, fy, fz);
152        let torque = Triplet::new(mx, my, mz);
153
154        let sensitivity = Sensitivity::new(force, torque);
155
156        Ok(self.set_sensitivity_manually(sensitivity))
157    }
158}
159
160impl DynpickSensorBuilder<Ready> {
161    /// Consuming this builder, attempts to construct a sensor instance.
162    pub fn build(self) -> Result<DynpickSensor, Error> {
163        let mut sensor = DynpickSensor {
164            port: self.port,
165            last_wrench: Wrench::zeroed(),
166            sensitivity: self.sensitivity,
167        };
168
169        // First single data request.
170        sensor.request_next_wrench()?;
171
172        Ok(sensor)
173    }
174}
175
176/// Dynpick 6-axis force-torque sensor.
177pub struct DynpickSensor {
178    /// Serial port device.
179    port: Box<dyn SerialPort>,
180    /// The latest wrench acquired by `update`.
181    last_wrench: Wrench,
182    /// The sensitivity of the connected sensor.
183    sensitivity: Sensitivity,
184}
185
186impl DynpickSensor {
187    /// Returns the latest wrench that is stored in this instance without communicaing the sensor.
188    ///
189    /// Use [`Self::update`] instead to obtain a new wrench from the sensor.
190    /// # Returns
191    /// `Ok(sensor)` if successfully connected, `Err(reason)` if failed.
192    pub fn last_wrench(&self) -> Wrench {
193        self.last_wrench
194    }
195
196    /// Returns the sensitivity of this sensor.
197    pub fn sensitivity(&self) -> Sensitivity {
198        self.sensitivity
199    }
200
201    /// Communicating to the sensor, updates the latest wrench.
202    /// # Returns
203    /// `Ok(wrench)` if succeeds, `Err(reason)` if failed.
204    pub fn update(&mut self) -> Result<Wrench, Error> {
205        const WRENCH_RESPONSE_LENGTH: usize = 27;
206
207        // Regardless of success or failure of receive and parse the messsage, request the next single data.
208        // If we do not so, after updating failed once, updating will fail everytime due to no reception from the sensor.
209        let mut res = [0; WRENCH_RESPONSE_LENGTH];
210        self.port
211            .read_exact(&mut res)
212            .map_err(Error::IO)
213            .finalize(|| self.request_next_wrench())?;
214
215        let res = std::str::from_utf8(&res)
216            .or(Err(Error::Utf8(res.to_vec())))
217            .finalize(|| self.request_next_wrench())?;
218
219        let (fx, fy, fz, mx, my, mz) = (0..6)
220            .map(|i| 1 + i * 4)
221            .map(|start| &res[start..start + 4])
222            .map(|src| i32::from_str_radix(src, 16))
223            .filter_map(Result::ok)
224            .next_tuple()
225            .ok_or(Error::ParseResponse(res.to_owned()))
226            .finalize(|| self.request_next_wrench())?;
227
228        let digital_force = Triplet::new(fx, fy, fz);
229        let digital_torque = Triplet::new(mx, my, mz);
230
231        let force = digital_force
232            .map(|d| d - 8192)
233            .map(|d| d as f64)
234            .map_entrywise(self.sensitivity.digital_per_newton, |d, s| d / s);
235
236        let torque = digital_torque
237            .map(|d| d - 8192)
238            .map(|d| d as f64)
239            .map_entrywise(self.sensitivity.digital_per_newtonmeter, |d, s| d / s);
240
241        self.last_wrench = Wrench::new(force, torque);
242
243        // Send a request to obtain a new wrench.
244        self.request_next_wrench()?;
245
246        Ok(self.last_wrench)
247    }
248
249    /// If this method succeeds, the next wrench acquired by [`Self::update`] will be zeroed.  
250    /// This methos is useful for zero-point calibration.
251    /// # Examples
252    /// ```no_run
253    /// use dynpick_force_torque_sensor::{DynpickSensorBuilder, Triplet};
254    ///
255    /// let mut sensor = DynpickSensorBuilder::open("/dev/ttyUSB0")
256    ///     .and_then(|b| b.set_sensitivity_by_builtin_data())
257    ///     .and_then(|b| b.build())
258    ///     .unwrap();
259    ///
260    /// sensor.zeroed_next().unwrap();
261    ///
262    /// let wrench = sensor.update().unwrap();
263    ///
264    /// assert_eq!(wrench.force, Triplet::new(0.0, 0.0, 0.0));
265    /// assert_eq!(wrench.torque, Triplet::new(0.0, 0.0, 0.0));
266    /// ```
267    pub fn zeroed_next(&mut self) -> Result<(), Error> {
268        self.port.write_all(&['O' as u8]).map_err(Error::IO)
269    }
270
271    /// Reads the product info from the sensor.
272    /// # Returns
273    /// `Ok(product_info)` if succeeds, `Err(reason)` if failed.
274    pub fn receive_product_info(&mut self) -> Result<String, Error> {
275        // Buffer may not be empty due to request_next_wrench() or its response.
276        self.port
277            .clear(serialport::ClearBuffer::All)
278            .map_err(Error::SerialPort)?;
279
280        // Send and wait.
281        self.port.write_all(&['V' as u8]).map_err(Error::IO)?;
282        std::thread::sleep(self.port.timeout());
283
284        // The response may be non-fixed size.
285        let bytes = self.port.bytes_to_read().map_err(Error::SerialPort)?;
286        let mut res = vec![0; bytes as usize];
287
288        let info = match self.port.read(&mut res) {
289            Ok(_) => match std::str::from_utf8(&res) {
290                Ok(str) => Ok(str.to_owned()),
291                Err(_) => Err(Error::Utf8(res)),
292            },
293            Err(e) => Err(Error::IO(e)),
294        };
295
296        // Restart sensing wrenches.
297        self.request_next_wrench()?;
298
299        info
300    }
301
302    /// Returns the reference to the serial port for the sensor.
303    pub fn inner_port(&self) -> &Box<dyn SerialPort> {
304        &self.port
305    }
306
307    /// Request single wrench.
308    fn request_next_wrench(&mut self) -> Result<(), Error> {
309        self.port.write_all(&['R' as u8]).map_err(Error::IO)
310    }
311}
312
313/// Represents an error occurred while communicating sensors.
314#[derive(Debug)]
315pub enum Error {
316    /// Failed to manipulate the port for the sensor.
317    SerialPort(serialport::Error),
318    /// Failed to read or write data during communication.
319    IO(std::io::Error),
320    /// The received data cannot be interpleted with UTF-8.
321    /// Inner value is the raw reception.
322    Utf8(Vec<u8>),
323    /// Received an unexpected format string.
324    /// Inner value is the raw string.
325    ParseResponse(String),
326}
327
328impl Display for Error {
329    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
330        match self {
331            Error::SerialPort(e) => write!(f, "SerialPort: {}", e),
332            Error::IO(e) => write!(f, "IO: {}", e),
333            Error::Utf8(v) => write!(
334                f,
335                "The response from the sensor is invalid for utf8. Raw response: {:X?}",
336                v
337            ),
338            Error::ParseResponse(res) => write!(
339                f,
340                "Failed to parse the response from the sensor. The response: {}",
341                res
342            ),
343        }
344    }
345}
346
347impl std::error::Error for Error {
348    fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
349        match self {
350            Error::SerialPort(e) => Some(e),
351            Error::IO(e) => Some(e),
352            _ => None,
353        }
354    }
355}
356
357/// A pair of force and torque.
358#[derive(Debug, Copy, Clone, PartialEq)]
359pub struct Wrench {
360    /// 3-dimensional force in Newton.
361    pub force: Triplet<f64>,
362    /// 3-dimensional torque in NewtonMeter.
363    pub torque: Triplet<f64>,
364}
365
366impl Wrench {
367    /// Returns a new wrench.
368    pub fn new(force: Triplet<f64>, torque: Triplet<f64>) -> Wrench {
369        Self { force, torque }
370    }
371
372    /// Returns a new wrench, initializing it to 0 Newton and 0 NewtonMeter.
373    pub fn zeroed() -> Wrench {
374        Wrench::new(Triplet::default(), Triplet::default())
375    }
376}
377
378/// How much the digital value from the sensor increses per 1 Newton (for force) and per 1 NewtonMeter (for torque).
379#[derive(Debug, Clone, Copy, PartialEq)]
380pub struct Sensitivity {
381    /// How much the digital value from the sensor increses per 1 Newton.
382    digital_per_newton: Triplet<f64>,
383    /// How much the digital value from the sensor increses per 1 NewtonMeter.
384    digital_per_newtonmeter: Triplet<f64>,
385}
386
387impl Sensitivity {
388    /// Initialize a new sensitivity of a sensor.
389    /// # Params
390    /// 1. `digital_per_newton` How much the digital value from the sensor increses per 1 Newton.
391    /// 1. `digital_per_newtonmeter` How much the digital value from the sensor increses per 1 NewtonMeter.
392    pub fn new(
393        digital_per_newton: Triplet<f64>,
394        digital_per_newtonmeter: Triplet<f64>,
395    ) -> Sensitivity {
396        Self {
397            digital_per_newton,
398            digital_per_newtonmeter,
399        }
400    }
401}
402
403// Helper trait
404#[ext]
405impl<T, E>  Result<T, E> {
406    /// # Returns
407    /// `Ok(v)` without calling `f` if itself is `Ok(v)`.  
408    /// `Err(e1)` if itself if `Err(e1)` and `f` returns `Ok()`.  
409    /// `Err(e2)` if itself if `Err(e1)` and `f` returns `Err(e2)`.
410    fn finalize<U, F>(self, f: F) -> Self
411    where
412        F: FnOnce() -> Result<U, E>,
413    {
414        match self {
415            Ok(value) => Ok(value),
416            Err(e1) => match f() {
417                Ok(_) => Err(e1),
418                Err(e2) => Err(e2),
419            },
420        }
421    }
422}
dynpick_force_torque_sensor/lib.rs

dynpick_force_torque_sensor/
lib.rs
