can_motor_control/
group.rs

1//! Group types: [`MotorGroup`] base + [`Arm`], [`Gripper`], [`Generic`] newtypes
2//! and the [`GroupKind`] dispatcher.
3
4use std::collections::HashMap;
5use std::sync::{Arc, Mutex};
6
7use motor_codec::{Command, CommandKind, Event, MotorRef};
8
9use crate::bus::Bus;
10use crate::error::Error;
11use crate::motor::Motor;
12use crate::spec::GripperOpeningSpec;
13
14const DEFAULT_OPENING_CURRENT: f64 = 0.15;
15const DEFAULT_OPENING_VELOCITY: f64 = 10.0;
16pub(crate) const OPENING_CALIBRATION_TRAVEL_RAD: f64 = 4.0;
17pub(crate) const OPENING_CALIBRATION_MIN_SPAN_RAD: f64 = 0.1;
18
19#[derive(Copy, Clone, Debug, PartialEq)]
20pub(crate) struct OpeningCalibration {
21    pub(crate) closed_position: f64,
22    pub(crate) open_position: f64,
23}
24
25#[derive(Copy, Clone, Debug, PartialEq)]
26pub(crate) struct OpeningControl {
27    pub(crate) spec: GripperOpeningSpec,
28    pub(crate) calibration: Option<OpeningCalibration>,
29}
30
31impl OpeningControl {
32    fn new(spec: GripperOpeningSpec) -> Self {
33        Self {
34            spec,
35            calibration: None,
36        }
37    }
38}
39
40/// MIT impedance command.
41#[derive(Copy, Clone, Debug, PartialEq)]
42pub struct MitCmd {
43    /// Position gain.
44    pub kp: f64,
45    /// Velocity gain.
46    pub kd: f64,
47    /// Position setpoint (radians).
48    pub q: f64,
49    /// Velocity setpoint (rad/s).
50    pub dq: f64,
51    /// Feed-forward torque (Nm).
52    pub tau: f64,
53}
54
55/// PosVel command.
56#[derive(Copy, Clone, Debug, PartialEq)]
57pub struct PosVelCmd {
58    /// Position setpoint (radians).
59    pub q: f64,
60    /// Velocity setpoint (rad/s).
61    pub dq: f64,
62}
63
64/// Pure-velocity command.
65#[derive(Copy, Clone, Debug, PartialEq)]
66pub struct VelCmd {
67    /// Velocity setpoint (rad/s).
68    pub dq: f64,
69}
70
71/// PosForce command (position + velocity + current per-unit).
72#[derive(Copy, Clone, Debug, PartialEq)]
73pub struct PosForceCmd {
74    /// Position setpoint (radians).
75    pub q: f64,
76    /// Velocity setpoint (rad/s).
77    pub dq: f64,
78    /// Current setpoint (per-unit).
79    pub i_pu: f64,
80}
81
82impl From<MitCmd> for Command {
83    fn from(c: MitCmd) -> Self {
84        Command::Mit {
85            kp: c.kp,
86            kd: c.kd,
87            q: c.q,
88            dq: c.dq,
89            tau: c.tau,
90        }
91    }
92}
93impl From<PosVelCmd> for Command {
94    fn from(c: PosVelCmd) -> Self {
95        Command::PosVel { q: c.q, dq: c.dq }
96    }
97}
98impl From<VelCmd> for Command {
99    fn from(c: VelCmd) -> Self {
100        Command::Vel { dq: c.dq }
101    }
102}
103impl From<PosForceCmd> for Command {
104    fn from(c: PosForceCmd) -> Self {
105        Command::PosForce {
106            q: c.q,
107            dq: c.dq,
108            i_pu: c.i_pu,
109        }
110    }
111}
112
113/// Collection of motors that share a CAN bus and a vendor codec.
114pub struct MotorGroup {
115    name: String,
116    bus_name: String,
117    motors: Vec<Motor>,
118    by_name: HashMap<String, usize>,
119    pub(crate) bus: Option<Arc<Mutex<Bus>>>,
120}
121
122impl MotorGroup {
123    pub(crate) fn new(name: String, bus_name: String, motors: Vec<Motor>) -> Self {
124        let mut by_name = HashMap::with_capacity(motors.len());
125        for (i, m) in motors.iter().enumerate() {
126            by_name.insert(m.name().to_string(), i);
127        }
128        Self {
129            name,
130            bus_name,
131            motors,
132            by_name,
133            bus: None,
134        }
135    }
136
137    /// Group name.
138    pub fn name(&self) -> &str {
139        &self.name
140    }
141
142    /// Name of the bus this group is attached to.
143    pub fn bus_name(&self) -> &str {
144        &self.bus_name
145    }
146
147    /// Number of motors in the group.
148    pub fn len(&self) -> usize {
149        self.motors.len()
150    }
151
152    /// True if there are no motors (shouldn't happen via the builder).
153    pub fn is_empty(&self) -> bool {
154        self.motors.is_empty()
155    }
156
157    /// Get a motor by name.
158    pub fn motor(&self, name: &str) -> Option<&Motor> {
159        self.by_name.get(name).map(|&i| &self.motors[i])
160    }
161
162    /// Get a motor mutably by name.
163    pub fn motor_mut(&mut self, name: &str) -> Option<&mut Motor> {
164        let i = *self.by_name.get(name)?;
165        Some(&mut self.motors[i])
166    }
167
168    /// Get a motor by insertion index.
169    pub fn motor_at(&self, idx: usize) -> Option<&Motor> {
170        self.motors.get(idx)
171    }
172
173    /// All motors in insertion order.
174    pub fn motors(&self) -> &[Motor] {
175        &self.motors
176    }
177
178    /// Crate-private: attach the bus reference at builder time.
179    pub(crate) fn attach_bus(&mut self, bus: Arc<Mutex<Bus>>) {
180        self.bus = Some(bus);
181    }
182
183    /// Update one motor from an inbound [`Event`].
184    pub(crate) fn apply_event(&mut self, motor_index: usize, event: &Event) {
185        debug_assert!(motor_index < self.motors.len(), "apply_event index OOB");
186        if let Some(m) = self.motors.get_mut(motor_index) {
187            m.apply_event(event);
188        }
189    }
190
191    fn bus_arc(&self) -> Result<Arc<Mutex<Bus>>, Error> {
192        Ok(self.bus.as_ref().ok_or(Error::NotConnected)?.clone())
193    }
194
195    pub(crate) fn send_command(&mut self, idx: usize, cmd: Command) -> Result<(), Error> {
196        let motor = self
197            .motors
198            .get(idx)
199            .ok_or(Error::Internal("motor index out of range"))?;
200        let m_ref = MotorRef {
201            motor_type: motor.motor_type(),
202            send_id: motor.send_id(),
203            recv_id: motor.recv_id(),
204            name: motor.name(),
205        };
206        let bus_arc = self.bus_arc()?;
207        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
208        let frame = bus
209            .codec
210            .encode_command(m_ref, &cmd)
211            .map_err(Error::Codec)?;
212        bus.transport.send(&frame).map_err(Error::Transport)
213    }
214
215    pub(crate) fn bus_vendor(&self) -> Result<&'static str, Error> {
216        let bus = self.bus_arc()?;
217        let bus = bus.lock().map_err(|_| Error::BusPoisoned)?;
218        Ok(bus.codec.vendor_name())
219    }
220
221    pub(crate) fn send_control_mode_readback(&mut self) -> Result<Option<(u32, u32)>, Error> {
222        let motor = self
223            .motors
224            .first()
225            .ok_or(Error::Internal("gripper has no motor"))?;
226        let m_ref = MotorRef {
227            motor_type: motor.motor_type(),
228            send_id: motor.send_id(),
229            recv_id: motor.recv_id(),
230            name: motor.name(),
231        };
232        let bus_arc = self.bus_arc()?;
233        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
234        let Some(frame) = bus
235            .codec
236            .encode_control_mode_readback(m_ref)
237            .map_err(Error::Codec)?
238        else {
239            return Ok(None);
240        };
241        bus.transport.send(&frame).map_err(Error::Transport)?;
242        Ok(Some((motor.send_id(), motor.recv_id())))
243    }
244
245    pub(crate) fn batch_send_commands(&mut self, cmds: &[Command]) -> Result<(), Error> {
246        if cmds.len() != self.motors.len() {
247            return Err(Error::CommandLengthMismatch {
248                expected: self.motors.len(),
249                got: cmds.len(),
250            });
251        }
252        let bus_arc = self.bus_arc()?;
253        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
254        for (i, cmd) in cmds.iter().enumerate() {
255            let motor = &self.motors[i];
256            let m_ref = MotorRef {
257                motor_type: motor.motor_type(),
258                send_id: motor.send_id(),
259                recv_id: motor.recv_id(),
260                name: motor.name(),
261            };
262            let frame = bus.codec.encode_command(m_ref, cmd).map_err(Error::Codec)?;
263            bus.transport.send(&frame).map_err(Error::Transport)?;
264        }
265        Ok(())
266    }
267
268    /// Send the codec's enable frame to every motor in insertion order.
269    pub fn enable_all(&mut self) -> Result<(), Error> {
270        let bus_arc = self.bus_arc()?;
271        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
272        for motor in &mut self.motors {
273            let m_ref = MotorRef {
274                motor_type: motor.motor_type(),
275                send_id: motor.send_id(),
276                recv_id: motor.recv_id(),
277                name: motor.name(),
278            };
279            let frame = bus.codec.encode_enable(m_ref).map_err(Error::Codec)?;
280            bus.transport.send(&frame).map_err(Error::Transport)?;
281            motor.set_enabled(true);
282        }
283        Ok(())
284    }
285
286    /// Send the codec's disable frame to every motor in insertion order.
287    /// (Robot::disable reverses group order; individual groups disable motors
288    /// in declaration order.)
289    pub fn disable_all(&mut self) -> Result<(), Error> {
290        let bus_arc = self.bus_arc()?;
291        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
292        for motor in self.motors.iter_mut() {
293            let m_ref = MotorRef {
294                motor_type: motor.motor_type(),
295                send_id: motor.send_id(),
296                recv_id: motor.recv_id(),
297                name: motor.name(),
298            };
299            let frame = bus.codec.encode_disable(m_ref).map_err(Error::Codec)?;
300            bus.transport.send(&frame).map_err(Error::Transport)?;
301            motor.set_enabled(false);
302        }
303        Ok(())
304    }
305
306    /// Send the codec's set-zero frame to every motor in insertion order.
307    pub fn set_zero_all(&mut self) -> Result<(), Error> {
308        let bus_arc = self.bus_arc()?;
309        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
310        for motor in &self.motors {
311            let m_ref = MotorRef {
312                motor_type: motor.motor_type(),
313                send_id: motor.send_id(),
314                recv_id: motor.recv_id(),
315                name: motor.name(),
316            };
317            let frame = bus.codec.encode_set_zero(m_ref).map_err(Error::Codec)?;
318            bus.transport.send(&frame).map_err(Error::Transport)?;
319        }
320        Ok(())
321    }
322
323    /// Send the codec's state-refresh query to every motor that supports one,
324    /// in insertion order. Motors whose codec returns `None` (no refresh) are
325    /// skipped. Send-only: this never drains inbound frames — call `tick` to
326    /// receive the replies. Commands no motion.
327    pub fn refresh_all(&mut self) -> Result<(), Error> {
328        let bus_arc = self.bus_arc()?;
329        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
330        for motor in &self.motors {
331            let m_ref = MotorRef {
332                motor_type: motor.motor_type(),
333                send_id: motor.send_id(),
334                recv_id: motor.recv_id(),
335                name: motor.name(),
336            };
337            if let Some(frame) = bus.codec.encode_refresh(m_ref).map_err(Error::Codec)? {
338                bus.transport.send(&frame).map_err(Error::Transport)?;
339            }
340        }
341        Ok(())
342    }
343
344    /// Set the persistent control mode (MIT / PosVel / Vel / PosForce) on every
345    /// motor whose codec supports it, in insertion order. Commands no motion;
346    /// send-only. Call once at startup, before the matching control commands.
347    pub fn set_mode(&mut self, mode: CommandKind) -> Result<(), Error> {
348        let bus_arc = self.bus_arc()?;
349        let mut bus = bus_arc.lock().map_err(|_| Error::BusPoisoned)?;
350        for motor in &self.motors {
351            let m_ref = MotorRef {
352                motor_type: motor.motor_type(),
353                send_id: motor.send_id(),
354                recv_id: motor.recv_id(),
355                name: motor.name(),
356            };
357            if let Some(frame) = bus
358                .codec
359                .encode_set_mode(m_ref, mode)
360                .map_err(Error::Codec)?
361            {
362                bus.transport.send(&frame).map_err(Error::Transport)?;
363            }
364        }
365        Ok(())
366    }
367}
368
369/// Newtype wrapping a [`MotorGroup`] that represents an articulated arm.
370pub struct Arm(pub(crate) MotorGroup);
371
372impl Arm {
373    /// Number of motors.
374    pub fn len(&self) -> usize {
375        self.0.len()
376    }
377    /// True if empty.
378    pub fn is_empty(&self) -> bool {
379        self.0.is_empty()
380    }
381    /// Get a motor by name.
382    pub fn motor(&self, name: &str) -> Option<&Motor> {
383        self.0.motor(name)
384    }
385    /// Get a motor mutably by name.
386    pub fn motor_mut(&mut self, name: &str) -> Option<&mut Motor> {
387        self.0.motor_mut(name)
388    }
389    /// Get a motor by insertion index.
390    pub fn motor_at(&self, idx: usize) -> Option<&Motor> {
391        self.0.motor_at(idx)
392    }
393
394    /// Slice of positions in insertion order.
395    pub fn positions(&self) -> Vec<f64> {
396        self.0.motors().iter().map(|m| m.position()).collect()
397    }
398    /// Slice of velocities in insertion order.
399    pub fn velocities(&self) -> Vec<f64> {
400        self.0.motors().iter().map(|m| m.velocity()).collect()
401    }
402    /// Slice of torques in insertion order.
403    pub fn torques(&self) -> Vec<f64> {
404        self.0.motors().iter().map(|m| m.torque()).collect()
405    }
406
407    /// Batch MIT control. `cmds.len()` must equal `self.len()`.
408    pub fn mit_control(&mut self, cmds: &[MitCmd]) -> Result<(), Error> {
409        let v: Vec<Command> = cmds.iter().copied().map(Into::into).collect();
410        self.0.batch_send_commands(&v)
411    }
412    /// Batch PosVel control.
413    pub fn pos_vel_control(&mut self, cmds: &[PosVelCmd]) -> Result<(), Error> {
414        let v: Vec<Command> = cmds.iter().copied().map(Into::into).collect();
415        self.0.batch_send_commands(&v)
416    }
417    /// Batch pure-velocity control.
418    pub fn vel_control(&mut self, cmds: &[VelCmd]) -> Result<(), Error> {
419        let v: Vec<Command> = cmds.iter().copied().map(Into::into).collect();
420        self.0.batch_send_commands(&v)
421    }
422    /// Batch PosForce control.
423    pub fn pos_force_control(&mut self, cmds: &[PosForceCmd]) -> Result<(), Error> {
424        let v: Vec<Command> = cmds.iter().copied().map(Into::into).collect();
425        self.0.batch_send_commands(&v)
426    }
427
428    /// Enable every motor in insertion order.
429    pub fn enable_all(&mut self) -> Result<(), Error> {
430        self.0.enable_all()
431    }
432    /// Disable every motor in reverse insertion order.
433    pub fn disable_all(&mut self) -> Result<(), Error> {
434        self.0.disable_all()
435    }
436    /// Set every motor's current position as zero.
437    pub fn set_zero_all(&mut self) -> Result<(), Error> {
438        self.0.set_zero_all()
439    }
440    /// Send a state-refresh query to every motor (no motion). Pair with `tick`
441    /// to receive the replies.
442    pub fn refresh(&mut self) -> Result<(), Error> {
443        self.0.refresh_all()
444    }
445    /// Set the persistent control mode on every motor (no motion). Call once at
446    /// startup, before the matching control commands.
447    pub fn set_mode(&mut self, mode: CommandKind) -> Result<(), Error> {
448        self.0.set_mode(mode)
449    }
450
451    /// Borrow the underlying [`MotorGroup`].
452    pub fn inner(&self) -> &MotorGroup {
453        &self.0
454    }
455    /// Borrow the underlying [`MotorGroup`] mutably.
456    pub fn inner_mut(&mut self) -> &mut MotorGroup {
457        &mut self.0
458    }
459}
460
461/// One-motor gripper newtype with raw motor commands and optional normalized
462/// opening support.
463pub struct Gripper {
464    pub(crate) group: MotorGroup,
465    pub(crate) opening: Option<OpeningControl>,
466}
467
468impl Gripper {
469    pub(crate) fn raw(group: MotorGroup) -> Self {
470        Self {
471            group,
472            opening: None,
473        }
474    }
475
476    pub(crate) fn with_opening(group: MotorGroup, spec: GripperOpeningSpec) -> Self {
477        Self {
478            group,
479            opening: Some(OpeningControl::new(spec)),
480        }
481    }
482
483    /// The one motor.
484    pub fn motor(&self) -> &Motor {
485        self.group.motor_at(0).expect("gripper invariant")
486    }
487    /// The one motor (mutably).
488    pub fn motor_mut(&mut self) -> &mut Motor {
489        let i = 0;
490        &mut self.group.motors[i]
491    }
492
493    /// Enable the motor.
494    pub fn enable(&mut self) -> Result<(), Error> {
495        self.group.enable_all()
496    }
497    /// Disable the motor.
498    pub fn disable(&mut self) -> Result<(), Error> {
499        self.group.disable_all()
500    }
501    /// Single MIT command to the gripper motor.
502    pub fn mit_control(&mut self, cmd: MitCmd) -> Result<(), Error> {
503        self.group.send_command(0, cmd.into())
504    }
505    /// Single PosVel command to the gripper motor.
506    pub fn pos_vel_control(&mut self, cmd: PosVelCmd) -> Result<(), Error> {
507        self.group.send_command(0, cmd.into())
508    }
509    /// Single PosForce command to the gripper motor.
510    pub fn pos_force_control(&mut self, cmd: PosForceCmd) -> Result<(), Error> {
511        self.group.send_command(0, cmd.into())
512    }
513    /// Send a state-refresh query to the motor (no motion). Pair with `tick`.
514    pub fn refresh(&mut self) -> Result<(), Error> {
515        self.group.refresh_all()
516    }
517    /// Set the gripper motor's persistent control mode (no motion).
518    pub fn set_mode(&mut self, mode: CommandKind) -> Result<(), Error> {
519        self.group.set_mode(mode)
520    }
521
522    /// True if this gripper is configured for normalized opening control.
523    pub fn has_opening_control(&self) -> bool {
524        self.opening.is_some()
525    }
526
527    pub(crate) fn opening_direction_sign(&self) -> Option<f64> {
528        self.opening.map(|opening| opening.spec.direction.sign())
529    }
530
531    pub(crate) fn clear_opening_calibration(&mut self) {
532        if let Some(opening) = self.opening.as_mut() {
533            opening.calibration = None;
534        }
535    }
536
537    pub(crate) fn set_opening_calibration(
538        &mut self,
539        closed_position: f64,
540        open_position: f64,
541    ) -> Result<(), Error> {
542        let span = (open_position - closed_position).abs();
543        if span < OPENING_CALIBRATION_MIN_SPAN_RAD {
544            return Err(Error::OpeningCalibrationFailed {
545                name: self.group.name().to_string(),
546                reason: format!(
547                    "calibrated opening span is too small: closed={closed_position:.5}, open={open_position:.5}, span={span:.5}, minimum={OPENING_CALIBRATION_MIN_SPAN_RAD:.5} rad"
548                ),
549            });
550        }
551        let Some(opening) = self.opening.as_mut() else {
552            return Ok(());
553        };
554        opening.calibration = Some(OpeningCalibration {
555            closed_position,
556            open_position,
557        });
558        Ok(())
559    }
560
561    pub(crate) fn opening_calibration_command(&mut self, q: f64) -> Result<(), Error> {
562        let current = self.opening_current(None)?;
563        self.pos_force_control(PosForceCmd {
564            q,
565            dq: DEFAULT_OPENING_VELOCITY,
566            i_pu: current,
567        })
568    }
569
570    /// Queue a normalized opening command where 0.0 is closed and 1.0 is open.
571    pub fn set_opening(&mut self, opening: f64, current: Option<f64>) -> Result<(), Error> {
572        let q = self.opening_position(opening)?;
573        let i_pu = self.opening_current(current)?;
574        self.pos_force_control(PosForceCmd {
575            q,
576            dq: DEFAULT_OPENING_VELOCITY,
577            i_pu,
578        })
579    }
580
581    /// Return the normalized opening from the most recently cached motor feedback.
582    ///
583    /// `0.0` is fully closed and `1.0` is fully open. Call [`Self::refresh`]
584    /// followed by [`crate::Robot::tick`] before reading when fresh feedback is
585    /// required. Opening control must be configured and the per-session
586    /// calibration performed by [`crate::Robot::enable`] must have completed.
587    /// This getter only reads cached state and does not send CAN frames.
588    pub fn opening(&self) -> Result<f64, Error> {
589        let Some(control) = self.opening else {
590            return Err(Error::OpeningCalibrationRequired);
591        };
592        let Some(calibration) = control.calibration else {
593            return Err(Error::OpeningCalibrationRequired);
594        };
595        let opening = (self.motor().position() - calibration.closed_position)
596            / (calibration.open_position - calibration.closed_position);
597        Ok(opening.clamp(0.0, 1.0))
598    }
599
600    pub(crate) fn opening_position(&self, opening: f64) -> Result<f64, Error> {
601        if !(0.0..=1.0).contains(&opening) {
602            return Err(Error::OpeningOutOfRange { got: opening });
603        }
604        let Some(control) = self.opening else {
605            return Err(Error::OpeningCalibrationRequired);
606        };
607        let Some(calibration) = control.calibration else {
608            return Err(Error::OpeningCalibrationRequired);
609        };
610        Ok(calibration.closed_position
611            + opening * (calibration.open_position - calibration.closed_position))
612    }
613
614    pub(crate) fn opening_current(&self, current: Option<f64>) -> Result<f64, Error> {
615        let configured = self
616            .opening
617            .and_then(|control| control.spec.default_current);
618        let i_pu = current.or(configured).unwrap_or(DEFAULT_OPENING_CURRENT);
619        validate_opening_current(i_pu)?;
620        Ok(i_pu)
621    }
622
623    /// Queue a fully-open command.
624    pub fn open(&mut self, current: Option<f64>) -> Result<(), Error> {
625        self.set_opening(1.0, current)
626    }
627
628    /// Queue a fully-closed command.
629    pub fn close(&mut self, current: Option<f64>) -> Result<(), Error> {
630        self.set_opening(0.0, current)
631    }
632
633    /// Borrow the underlying [`MotorGroup`].
634    pub fn inner(&self) -> &MotorGroup {
635        &self.group
636    }
637    /// Borrow the underlying [`MotorGroup`] mutably.
638    pub fn inner_mut(&mut self) -> &mut MotorGroup {
639        &mut self.group
640    }
641}
642
643fn validate_opening_current(current: f64) -> Result<(), Error> {
644    if current > 0.0 && current <= 1.0 {
645        Ok(())
646    } else {
647        Err(Error::OpeningCurrentOutOfRange { got: current })
648    }
649}
650
651/// Generic group for topologies that don't fit [`Arm`] or [`Gripper`] (future
652/// hand, leg, head, etc.).
653pub struct Generic(pub(crate) MotorGroup);
654
655impl Generic {
656    /// Number of motors.
657    pub fn len(&self) -> usize {
658        self.0.len()
659    }
660    /// True if empty.
661    pub fn is_empty(&self) -> bool {
662        self.0.is_empty()
663    }
664
665    /// Borrowed view of the inner group for read-only access.
666    pub fn as_motor_group(&self) -> &MotorGroup {
667        &self.0
668    }
669    /// Borrow mutably.
670    pub fn as_motor_group_mut(&mut self) -> &mut MotorGroup {
671        &mut self.0
672    }
673
674    /// Borrow the underlying [`MotorGroup`].
675    pub fn inner(&self) -> &MotorGroup {
676        &self.0
677    }
678    /// Borrow the underlying [`MotorGroup`] mutably.
679    pub fn inner_mut(&mut self) -> &mut MotorGroup {
680        &mut self.0
681    }
682}
683
684/// Sum type of every group variant the robot can hold.
685#[non_exhaustive]
686pub enum GroupKind {
687    /// Articulated arm.
688    Arm(Arm),
689    /// Single-motor gripper.
690    Gripper(Gripper),
691    /// Generic catch-all group.
692    Generic(Generic),
693}
694
695impl GroupKind {
696    /// Downcast to [`Arm`].
697    pub fn as_arm(&self) -> Option<&Arm> {
698        if let Self::Arm(a) = self {
699            Some(a)
700        } else {
701            None
702        }
703    }
704    /// Downcast to [`Arm`] mutably.
705    pub fn as_arm_mut(&mut self) -> Option<&mut Arm> {
706        if let Self::Arm(a) = self {
707            Some(a)
708        } else {
709            None
710        }
711    }
712    /// Downcast to [`Gripper`].
713    pub fn as_gripper(&self) -> Option<&Gripper> {
714        if let Self::Gripper(g) = self {
715            Some(g)
716        } else {
717            None
718        }
719    }
720    /// Downcast to [`Gripper`] mutably.
721    pub fn as_gripper_mut(&mut self) -> Option<&mut Gripper> {
722        if let Self::Gripper(g) = self {
723            Some(g)
724        } else {
725            None
726        }
727    }
728    /// Downcast to [`Generic`].
729    pub fn as_generic(&self) -> Option<&Generic> {
730        if let Self::Generic(g) = self {
731            Some(g)
732        } else {
733            None
734        }
735    }
736    /// Downcast to [`Generic`] mutably.
737    pub fn as_generic_mut(&mut self) -> Option<&mut Generic> {
738        if let Self::Generic(g) = self {
739            Some(g)
740        } else {
741            None
742        }
743    }
744
745    /// Borrow the underlying [`MotorGroup`] regardless of variant.
746    pub fn inner(&self) -> &MotorGroup {
747        match self {
748            Self::Arm(a) => a.inner(),
749            Self::Gripper(g) => g.inner(),
750            Self::Generic(g) => g.inner(),
751        }
752    }
753    /// Borrow mutably regardless of variant.
754    pub fn inner_mut(&mut self) -> &mut MotorGroup {
755        match self {
756            Self::Arm(a) => a.inner_mut(),
757            Self::Gripper(g) => g.inner_mut(),
758            Self::Generic(g) => g.inner_mut(),
759        }
760    }
761
762    /// Group name (delegated to the inner MotorGroup).
763    pub fn name(&self) -> &str {
764        self.inner().name()
765    }
766    /// Bus name (delegated).
767    pub fn bus_name(&self) -> &str {
768        self.inner().bus_name()
769    }
770
771    pub(crate) fn apply_event(&mut self, motor_index: usize, event: &Event) {
772        self.inner_mut().apply_event(motor_index, event);
773    }
774
775    /// Enable all motors in this group (delegated).
776    pub fn enable_all(&mut self) -> Result<(), Error> {
777        self.inner_mut().enable_all()
778    }
779    /// Disable all motors in this group (delegated).
780    pub fn disable_all(&mut self) -> Result<(), Error> {
781        self.inner_mut().disable_all()
782    }
783    /// Send a state-refresh query to every motor in this group (delegated).
784    pub fn refresh_all(&mut self) -> Result<(), Error> {
785        self.inner_mut().refresh_all()
786    }
787    /// Set the persistent control mode on every motor in this group (delegated).
788    pub fn set_mode(&mut self, mode: CommandKind) -> Result<(), Error> {
789        self.inner_mut().set_mode(mode)
790    }
791}
792
793#[cfg(test)]
794mod tests {
795    use motor_codec::MotorTypeId;
796
797    use super::*;
798    use crate::spec::{GripperOpeningSpec, OpeningDirection};
799
800    fn make_group(name: &str, bus: &str, motor_names: &[&str]) -> MotorGroup {
801        let motors: Vec<_> = motor_names
802            .iter()
803            .enumerate()
804            .map(|(i, n)| {
805                Motor::new(
806                    n.to_string(),
807                    MotorTypeId::Damiao(3),
808                    0x01 + i as u32,
809                    0x11 + i as u32,
810                )
811            })
812            .collect();
813        MotorGroup::new(name.into(), bus.into(), motors)
814    }
815
816    fn opening_gripper(direction: OpeningDirection, default_current: Option<f64>) -> Gripper {
817        let mut gripper = Gripper::with_opening(
818            make_group("grip", "main", &["g"]),
819            GripperOpeningSpec::new(direction, default_current),
820        );
821        gripper.opening.as_mut().unwrap().calibration = Some(OpeningCalibration {
822            closed_position: 2.0,
823            open_position: 4.0,
824        });
825        gripper
826    }
827
828    #[test]
829    fn name_and_index_access_agree() {
830        let g = make_group("arm", "main", &["j0", "j1", "j2"]);
831        assert_eq!(g.motor("j1").map(|m| m.name()), Some("j1"));
832        assert_eq!(g.motor_at(1).map(|m| m.name()), Some("j1"));
833        assert!(g.motor("ghost").is_none());
834    }
835
836    #[test]
837    fn group_records_bus_name() {
838        let g = make_group("arm", "left", &["j0"]);
839        assert_eq!(g.bus_name(), "left");
840        assert_eq!(g.name(), "arm");
841    }
842
843    #[test]
844    fn opening_position_maps_closed_mid_open() {
845        let gripper = opening_gripper(OpeningDirection::IncreasingPosition, Some(0.25));
846        assert_eq!(gripper.opening_position(0.0).unwrap(), 2.0);
847        assert_eq!(gripper.opening_position(0.5).unwrap(), 3.0);
848        assert_eq!(gripper.opening_position(1.0).unwrap(), 4.0);
849    }
850
851    #[test]
852    fn opening_position_supports_decreasing_direction_calibration() {
853        let mut gripper = opening_gripper(OpeningDirection::DecreasingPosition, Some(0.25));
854        gripper.opening.as_mut().unwrap().calibration = Some(OpeningCalibration {
855            closed_position: 4.0,
856            open_position: 2.0,
857        });
858        assert_eq!(gripper.opening_position(0.0).unwrap(), 4.0);
859        assert_eq!(gripper.opening_position(0.5).unwrap(), 3.0);
860        assert_eq!(gripper.opening_position(1.0).unwrap(), 2.0);
861    }
862
863    fn set_feedback_position(gripper: &mut Gripper, position: f64) {
864        gripper.group.apply_event(
865            0,
866            &Event::State {
867                motor_id: 0x11,
868                q: position,
869                dq: 0.0,
870                tau: 0.0,
871                t_mos: 30,
872                t_rotor: 35,
873            },
874        );
875    }
876
877    #[test]
878    fn opening_maps_cached_closed_mid_and_open_feedback() {
879        let mut gripper = opening_gripper(OpeningDirection::IncreasingPosition, None);
880        for (position, expected) in [(2.0, 0.0), (3.0, 0.5), (4.0, 1.0)] {
881            set_feedback_position(&mut gripper, position);
882            assert_eq!(gripper.opening().unwrap(), expected);
883        }
884    }
885
886    #[test]
887    fn opening_supports_decreasing_position_calibration() {
888        let mut gripper = opening_gripper(OpeningDirection::DecreasingPosition, None);
889        gripper.opening.as_mut().unwrap().calibration = Some(OpeningCalibration {
890            closed_position: 4.0,
891            open_position: 2.0,
892        });
893        for (position, expected) in [(4.0, 0.0), (3.0, 0.5), (2.0, 1.0)] {
894            set_feedback_position(&mut gripper, position);
895            assert_eq!(gripper.opening().unwrap(), expected);
896        }
897    }
898
899    #[test]
900    fn opening_clamps_feedback_beyond_calibrated_endpoints() {
901        let mut gripper = opening_gripper(OpeningDirection::IncreasingPosition, None);
902        set_feedback_position(&mut gripper, 1.99);
903        assert_eq!(gripper.opening().unwrap(), 0.0);
904        set_feedback_position(&mut gripper, 4.01);
905        assert_eq!(gripper.opening().unwrap(), 1.0);
906    }
907
908    #[test]
909    fn opening_requires_configured_and_completed_calibration() {
910        let raw = Gripper::raw(make_group("grip", "main", &["g"]));
911        assert!(matches!(
912            raw.opening(),
913            Err(Error::OpeningCalibrationRequired)
914        ));
915
916        let uncalibrated = Gripper::with_opening(
917            make_group("grip", "main", &["g"]),
918            GripperOpeningSpec::new(OpeningDirection::IncreasingPosition, None),
919        );
920        assert!(matches!(
921            uncalibrated.opening(),
922            Err(Error::OpeningCalibrationRequired)
923        ));
924    }
925
926    #[test]
927    fn opening_rejects_out_of_range_values() {
928        let gripper = opening_gripper(OpeningDirection::IncreasingPosition, None);
929        assert!(matches!(
930            gripper.opening_position(-0.1),
931            Err(Error::OpeningOutOfRange { .. })
932        ));
933        assert!(matches!(
934            gripper.opening_position(1.1),
935            Err(Error::OpeningOutOfRange { .. })
936        ));
937    }
938
939    #[test]
940    fn opening_current_precedence_is_per_call_then_config_then_library_default() {
941        let configured = opening_gripper(OpeningDirection::IncreasingPosition, Some(0.25));
942        assert_eq!(configured.opening_current(None).unwrap(), 0.25);
943        assert_eq!(configured.opening_current(Some(0.4)).unwrap(), 0.4);
944
945        let fallback = opening_gripper(OpeningDirection::IncreasingPosition, None);
946        assert_eq!(
947            fallback.opening_current(None).unwrap(),
948            DEFAULT_OPENING_CURRENT
949        );
950    }
951
952    #[test]
953    fn opening_current_rejects_invalid_values() {
954        let gripper = opening_gripper(OpeningDirection::IncreasingPosition, None);
955        assert!(matches!(
956            gripper.opening_current(Some(0.0)),
957            Err(Error::OpeningCurrentOutOfRange { .. })
958        ));
959        assert!(matches!(
960            gripper.opening_current(Some(1.1)),
961            Err(Error::OpeningCurrentOutOfRange { .. })
962        ));
963    }
964
965    #[test]
966    fn apply_event_updates_target_motor() {
967        let mut g = make_group("arm", "m", &["j0", "j1", "j2"]);
968        g.apply_event(
969            2,
970            &Event::State {
971                motor_id: 0x13,
972                q: 0.5,
973                dq: 0.1,
974                tau: 0.0,
975                t_mos: 30,
976                t_rotor: 35,
977            },
978        );
979        assert_eq!(g.motor_at(2).unwrap().position(), 0.5);
980        assert_eq!(g.motor_at(0).unwrap().position(), 0.0);
981    }
982
983    #[test]
984    fn group_kind_downcast() {
985        let arm = Arm(make_group("a", "m", &["j0"]));
986        let mut kind = GroupKind::Arm(arm);
987        assert!(kind.as_arm().is_some());
988        assert!(kind.as_gripper().is_none());
989        assert!(kind.as_arm_mut().is_some());
990        assert_eq!(kind.name(), "a");
991        assert_eq!(kind.bus_name(), "m");
992    }
993
994    /// Compile-time check: MotorGroup, Arm, Gripper, Generic do not have a
995    /// `codec` field of any kind. We do a structural assertion by listing the
996    /// expected non-codec field names that DO exist.
997    ///
998    /// This is a smoke test; the grep test in `source_invariants` below is
999    /// the authoritative enforcement.
1000    #[test]
1001    fn group_layout_has_no_codec_field() {
1002        let g = make_group("a", "m", &["j0"]);
1003        assert_eq!(g.bus_name(), "m"); // we have `bus_name`, not `codec`.
1004        assert_eq!(g.len(), 1);
1005    }
1006
1007    /// Grep test: this source file's Arm / Gripper / Generic impl blocks must
1008    /// not reference `is_fd`, `FD_FORMAT`, or branch on CAN-FD vs classical.
1009    /// Frame-type handling is confined to the codec and bus impls.
1010    #[test]
1011    fn source_invariants() {
1012        let src = include_str!("group.rs");
1013        // Strip out the unit-test module before scanning so test-only mentions
1014        // don't trip the invariant.
1015        let scan = if let Some(idx) = src.find("#[cfg(test)]\nmod tests") {
1016            &src[..idx]
1017        } else {
1018            src
1019        };
1020        for forbidden in ["is_fd(", "FD_FORMAT", "FrameFlags::FD_FORMAT"] {
1021            assert!(
1022                !scan.contains(forbidden),
1023                "group source contains forbidden FD-discrimination token: {forbidden}"
1024            );
1025        }
1026    }
1027}
1028
1029#[cfg(test)]
1030mod integration_tests {
1031    use std::sync::{Arc, Mutex};
1032
1033    use motor_codec::{
1034        BusCapabilities, CanFrame, CodecError, Event, Limits, MotorCodec, MotorRef, MotorTypeId,
1035    };
1036
1037    use super::*;
1038    use crate::transport::MockCanBus;
1039    use crate::CanBus;
1040
1041    /// Codec stub specialized for these tests so we don't need to bring
1042    /// can-motor-damiao-codec into the unit-test compile graph.
1043    struct StubCodec;
1044    impl MotorCodec for StubCodec {
1045        fn vendor_name(&self) -> &'static str {
1046            "stub"
1047        }
1048        fn supports(&self, _: MotorTypeId) -> bool {
1049            true
1050        }
1051        fn limits(&self, _: MotorTypeId) -> Result<Limits, CodecError> {
1052            Ok(Limits {
1053                p_max: 1.0,
1054                v_max: 1.0,
1055                t_max: 1.0,
1056            })
1057        }
1058        fn bind_to_bus(&mut self, _: BusCapabilities) {}
1059        fn encode_enable(&self, m: MotorRef<'_>) -> Result<CanFrame, CodecError> {
1060            CanFrame::classical(m.send_id, &[0xFC])
1061                .map_err(|_| CodecError::DecodeFailed { reason: "frame" })
1062        }
1063        fn encode_disable(&self, m: MotorRef<'_>) -> Result<CanFrame, CodecError> {
1064            CanFrame::classical(m.send_id, &[0xFD])
1065                .map_err(|_| CodecError::DecodeFailed { reason: "frame" })
1066        }
1067        fn encode_set_zero(&self, m: MotorRef<'_>) -> Result<CanFrame, CodecError> {
1068            CanFrame::classical(m.send_id, &[0xFE])
1069                .map_err(|_| CodecError::DecodeFailed { reason: "frame" })
1070        }
1071        fn encode_command(&self, m: MotorRef<'_>, _: &Command) -> Result<CanFrame, CodecError> {
1072            CanFrame::classical(m.send_id, &[0x55])
1073                .map_err(|_| CodecError::DecodeFailed { reason: "frame" })
1074        }
1075        fn encode_refresh(&self, m: MotorRef<'_>) -> Result<Option<CanFrame>, CodecError> {
1076            // Mimic Damiao's 0xCC-on-0x7FF query so refresh paths are exercised.
1077            let p = [m.send_id as u8, (m.send_id >> 8) as u8, 0xCC, 0, 0, 0, 0, 0];
1078            CanFrame::classical(0x7FF, &p)
1079                .map(Some)
1080                .map_err(|_| CodecError::DecodeFailed { reason: "frame" })
1081        }
1082        fn decode(&self, _: &CanFrame) -> Result<Option<Event>, CodecError> {
1083            Ok(None)
1084        }
1085    }
1086
1087    fn build_arm() -> (
1088        Arm,
1089        std::sync::Arc<std::sync::Mutex<Bus>>,
1090        Arc<Mutex<MockCanBus>>, // not actually accessible; we read via sent_frames at bus.transport
1091    ) {
1092        let mock = MockCanBus::new("vcan_main");
1093        let transport: Box<dyn CanBus> = Box::new(mock);
1094        let codec: Box<dyn MotorCodec> = Box::new(StubCodec);
1095        let bus = Arc::new(Mutex::new(Bus::new(transport, codec)));
1096        let motors = vec![
1097            Motor::new("j0".into(), MotorTypeId::Damiao(3), 0x01, 0x11),
1098            Motor::new("j1".into(), MotorTypeId::Damiao(3), 0x02, 0x12),
1099            Motor::new("j2".into(), MotorTypeId::Damiao(3), 0x03, 0x13),
1100        ];
1101        let mut group = MotorGroup::new("arm".into(), "main".into(), motors);
1102        group.attach_bus(bus.clone());
1103        let arm = Arm(group);
1104        // Mock placeholder — we read sent frames via re-downcast trick.
1105        let placeholder = Arc::new(Mutex::new(MockCanBus::new("ignored")));
1106        (arm, bus, placeholder)
1107    }
1108
1109    #[test]
1110    fn enable_all_marks_every_motor_enabled() {
1111        let (mut arm, _bus, _) = build_arm();
1112        arm.enable_all().unwrap();
1113        for n in &["j0", "j1", "j2"] {
1114            assert!(arm.motor(n).unwrap().is_enabled(), "{n} not enabled");
1115        }
1116    }
1117
1118    #[test]
1119    fn disable_after_enable_clears_flag() {
1120        let (mut arm, _bus, _) = build_arm();
1121        arm.enable_all().unwrap();
1122        arm.disable_all().unwrap();
1123        for n in &["j0", "j1", "j2"] {
1124            assert!(!arm.motor(n).unwrap().is_enabled(), "{n} still enabled");
1125        }
1126    }
1127
1128    #[test]
1129    fn mit_length_mismatch_returns_error() {
1130        let (mut arm, _bus, _) = build_arm();
1131        let cmds = vec![MitCmd {
1132            kp: 0.0,
1133            kd: 0.0,
1134            q: 0.0,
1135            dq: 0.0,
1136            tau: 0.0,
1137        }];
1138        let r = arm.mit_control(&cmds);
1139        assert!(matches!(
1140            r,
1141            Err(Error::CommandLengthMismatch {
1142                expected: 3,
1143                got: 1
1144            })
1145        ));
1146    }
1147
1148    #[test]
1149    fn mit_correct_length_succeeds() {
1150        let (mut arm, _bus, _) = build_arm();
1151        let cmds = vec![
1152            MitCmd {
1153                kp: 0.0,
1154                kd: 0.0,
1155                q: 0.0,
1156                dq: 0.0,
1157                tau: 0.0,
1158            };
1159            3
1160        ];
1161        arm.mit_control(&cmds).unwrap();
1162    }
1163
1164    #[test]
1165    fn not_connected_when_bus_not_attached() {
1166        let motors = vec![Motor::new("g".into(), MotorTypeId::Damiao(3), 0x05, 0x18)];
1167        let mut group = MotorGroup::new("g".into(), "main".into(), motors);
1168        let r = group.enable_all();
1169        assert!(matches!(r, Err(Error::NotConnected)));
1170    }
1171
1172    #[test]
1173    fn refresh_emits_one_query_per_motor() {
1174        // Pair the bus transport with a peer we keep, so we can observe exactly
1175        // what refresh() put on the wire (one frame per motor, no extra traffic).
1176        let (tx, mut peer) = MockCanBus::pair("vcan_main", "peer");
1177        let transport: Box<dyn CanBus> = Box::new(tx);
1178        let codec: Box<dyn MotorCodec> = Box::new(StubCodec);
1179        let bus = Arc::new(Mutex::new(Bus::new(transport, codec)));
1180        let motors = vec![
1181            Motor::new("j0".into(), MotorTypeId::Damiao(3), 0x01, 0x11),
1182            Motor::new("j1".into(), MotorTypeId::Damiao(3), 0x02, 0x12),
1183            Motor::new("j2".into(), MotorTypeId::Damiao(3), 0x03, 0x13),
1184        ];
1185        let mut group = MotorGroup::new("arm".into(), "main".into(), motors);
1186        group.attach_bus(bus);
1187        let mut arm = Arm(group);
1188
1189        arm.refresh().unwrap();
1190
1191        let got = peer.drain_inbound_nonblocking().unwrap();
1192        assert_eq!(got.len(), 3, "one refresh frame per motor");
1193        assert!(
1194            got.iter().all(|f| f.id == 0x7FF && f.payload()[2] == 0xCC),
1195            "every emitted frame is a 0xCC refresh query on 0x7FF"
1196        );
1197    }
1198
1199    #[test]
1200    fn refresh_not_connected_when_bus_not_attached() {
1201        let motors = vec![Motor::new("g".into(), MotorTypeId::Damiao(3), 0x05, 0x18)];
1202        let mut group = MotorGroup::new("g".into(), "main".into(), motors);
1203        assert!(matches!(group.refresh_all(), Err(Error::NotConnected)));
1204    }
1205}
can_motor_control/group.rs

can_motor_control/
group.rs
