repose_controller/
lib.rs

1//! Robot-agnostic unified joint controller for `copper-rs`.
2//!
3//! [`CuControllerTask`] sits between a trajectory follower
4//! (`CuTrajectoryFollowOutput`) and a robot bridge. It consumes the follower
5//! setpoint plus the measured joint state and emits one unified
6//! [`CuJointCommandPayload`] — the only command type a bridge has to carry.
7//! Simulators and hardware bridges implement the same
8//! `joint_command`/`joint_state` channels, so swapping sim for a real robot
9//! is a RON graph change, not an application change.
10//!
11//! Behaviour per cycle:
12//! - No setpoint yet: `Idle`, holding the last measured state (zeros before
13//!   the first state arrives).
14//! - Setpoint + fresh state: command mirrors the setpoint; status maps the
15//!   follower status (`Running` → `Running`, `Finished` → `Holding`,
16//!   `ErrorEmptyTrajectory` → `Fault`).
17//! - Tracking error above `tracking_error_limit`: `Fault`, command
18//!   holds the measured state instead of chasing the setpoint.
19//! - Setpoint without any measured state: `Fault`, holding the last command.
20//!
21//! State that affects later cycles (last command, last status) round-trips
22//! through `Freezable`, so deterministic replay resumes correctly.
23
24#![cfg_attr(not(feature = "std"), no_std)]
25
26use bincode::Decode;
27use bincode::Encode;
28use bincode::de::Decoder;
29use bincode::enc::Encoder;
30use bincode::error::DecodeError;
31use bincode::error::EncodeError;
32use cu29::prelude::*;
33use cu29::units::si::f64::{Force, Time, Torque};
34use cu29::units::si::force::newton;
35use cu29::units::si::time::second;
36use cu29::units::si::torque::newton_meter;
37use repose_kinematics::CuJointPosition;
38use repose_kinematics::CuJointStatePayload;
39use repose_kinematics::CuJointVelocity;
40use repose_kinematics::CuRobotStatus;
41use repose_trajectory::CuFollowStatus;
42use repose_trajectory::CuTrajectoryFollowOutput;
43use serde::Deserialize;
44use serde::Serialize;
45
46mod serde_arrays;
47
48/// Command level a bridge is asked to apply.
49#[derive(
50    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
51)]
52#[serde(rename_all = "snake_case")]
53pub enum CuControllerMode {
54    /// Position setpoints; the bridge closes the servo loop (with velocity
55    /// feedforward from the command velocities).
56    #[default]
57    Position,
58    /// Velocity setpoints; the bridge servo drives the commanded joint
59    /// velocities (positions ride along as a drift reference).
60    Velocity,
61    /// Effort setpoints computed by the controller's own PD law; the bridge
62    /// applies them as raw torque/force (plus its own gravity compensation).
63    Effort,
64}
65
66/// A joint effort carries the physical unit of its joint (torque or force).
67#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
68#[serde(rename_all = "snake_case")]
69pub enum CuJointEffort {
70    Revolute(Torque),
71    Prismatic(Force),
72    Fixed,
73}
74
75impl Default for CuJointEffort {
76    fn default() -> Self {
77        Self::Revolute(Torque::default())
78    }
79}
80
81impl CuJointEffort {
82    /// Builds an effort matching the kind of the given joint coordinate.
83    pub fn from_position(position: CuJointPosition, value: f64) -> Self {
84        match position {
85            CuJointPosition::Revolute(_) => Self::Revolute(Torque::new::<newton_meter>(value)),
86            CuJointPosition::Prismatic(_) => Self::Prismatic(Force::new::<newton>(value)),
87            CuJointPosition::Fixed => Self::Fixed,
88        }
89    }
90    /// Returns the effort in its joint's SI unit (N m or N; `0.0` for fixed).
91    pub fn si_value(self) -> f64 {
92        match self {
93            Self::Revolute(v) => v.get::<newton_meter>(),
94            Self::Prismatic(v) => v.get::<newton>(),
95            Self::Fixed => 0.0,
96        }
97    }
98}
99
100/// Controller status carried by [`CuJointCommandPayload`].
101#[derive(
102    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
103)]
104#[serde(rename_all = "snake_case")]
105pub enum CuControllerStatus {
106    /// No setpoint has been received yet.
107    #[default]
108    Idle,
109    /// Tracking a running setpoint within the error limit.
110    Running,
111    /// Holding a finished setpoint (or the measured state while idle).
112    Holding,
113    /// Tracking error exceeded the limit, the follower reported an error,
114    /// or a setpoint arrived without measured state.
115    Fault,
116}
117
118/// Unified joint command: the only command payload a bridge has to carry.
119#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
120#[reflect(no_field_bounds, from_reflect = false)]
121pub struct CuJointCommandPayload<const N: usize> {
122    #[serde(with = "repose_kinematics::serde_arrays::position_array")]
123    pub positions: [CuJointPosition; N],
124    #[serde(with = "repose_kinematics::serde_arrays::velocity_array")]
125    pub velocities: [CuJointVelocity; N],
126    pub status: CuControllerStatus,
127    /// Largest absolute joint error (setpoint − measured) seen this cycle.
128    pub max_tracking_error: CuJointPosition,
129    /// Command level the bridge is asked to apply.
130    pub mode: CuControllerMode,
131    /// Effort setpoints (only meaningful when `mode` is `Effort`).
132    #[serde(with = "crate::serde_arrays::effort_array")]
133    pub efforts: [CuJointEffort; N],
134}
135
136impl<const N: usize> Default for CuJointCommandPayload<N> {
137    fn default() -> Self {
138        Self {
139            positions: [CuJointPosition::default(); N],
140            velocities: [CuJointVelocity::default(); N],
141            status: CuControllerStatus::default(),
142            max_tracking_error: CuJointPosition::default(),
143            mode: CuControllerMode::default(),
144            efforts: [CuJointEffort::default(); N],
145        }
146    }
147}
148
149/// Effort-mode PD gains.
150///
151/// Both gains are dimensionless SI multipliers applied per joint kind:
152/// `kp` is N m/rad (revolute) or N/m (prismatic), `kv` is N m s/rad
153/// (revolute) or N s/m (prismatic). Both must be finite and `>= 0`.
154#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
155#[serde(default, deny_unknown_fields)]
156pub struct CuEffortGains {
157    /// Proportional gain (dimensionless SI, see type docs).
158    pub kp: f64,
159    /// Derivative gain (dimensionless SI, see type docs).
160    pub kv: f64,
161}
162
163impl Default for CuEffortGains {
164    /// `kp = 20.0`, `kv = 0.8`.
165    fn default() -> Self {
166        Self { kp: 20.0, kv: 0.8 }
167    }
168}
169
170impl CuEffortGains {
171    /// Checks that both gains are finite and non-negative.
172    pub fn validate(&self) -> CuResult<()> {
173        if !self.kp.is_finite() || self.kp < 0.0 || !self.kv.is_finite() || self.kv < 0.0 {
174            return Err(CuError::from(
175                "repose-controller: effort gains 'kp'/'kv' must be finite and >= 0",
176            ));
177        }
178        Ok(())
179    }
180}
181
182/// Per-joint command rate limit (position mode only).
183///
184/// Both limits are scalars applied to every joint in that joint's SI unit
185/// (dimensionless SI magnitude): `max_speed` is rad/s for revolute and m/s
186/// for prismatic joints, `max_acceleration` is rad/s² resp. m/s². Both must
187/// be positive and finite.
188#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
189#[serde(deny_unknown_fields)]
190pub struct CuRateLimit {
191    /// Max joint speed (dimensionless SI magnitude, see type docs).
192    pub max_speed: f64,
193    /// Max joint acceleration (dimensionless SI magnitude, see type docs).
194    pub max_acceleration: f64,
195}
196
197impl CuRateLimit {
198    /// Checks that both limits are positive and finite.
199    pub fn validate(&self) -> CuResult<()> {
200        let ok = |v: f64| v.is_finite() && v > 0.0;
201        if !ok(self.max_speed) || !ok(self.max_acceleration) {
202            return Err(CuError::from(
203                "repose-controller: rate limit 'max_speed'/'max_acceleration' must be positive and finite",
204            ));
205        }
206        Ok(())
207    }
208}
209
210/// Parameters of [`CuControllerTask`]; the node `config:` block.
211///
212/// ```ron
213/// config: {
214///     "mode": "position",            // position | velocity | effort
215///     "tracking_error_limit": 0.5,   // SI per joint kind (rad / m)
216///     "state_timeout": 0.2,          // s; omit to disable the watchdog
217///     "rate_limit": {"max_speed": 1.5, "max_acceleration": 6.0},
218///     "effort_gains": {"kp": 20.0, "kv": 0.8},
219/// }
220/// ```
221///
222/// ```rust
223/// use repose_controller::{CuControllerParams, CuControllerTask, CuRateLimit};
224///
225/// let params = CuControllerParams {
226///     rate_limit: Some(CuRateLimit { max_speed: 1.5, max_acceleration: 6.0 }),
227///     ..CuControllerParams::default()
228/// };
229/// let task = CuControllerTask::<6>::from_params(params).unwrap();
230/// assert_eq!(task.params().tracking_error_limit, 0.5);
231/// ```
232#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
233#[serde(default, deny_unknown_fields)]
234pub struct CuControllerParams {
235    /// Command level: `position`, `velocity`, or `effort`.
236    pub mode: CuControllerMode,
237    /// Max allowed absolute joint error (setpoint − measured) before the
238    /// controller faults and holds the measured state. Dimensionless SI
239    /// magnitude compared per joint in that joint's unit: radians for
240    /// revolute, meters for prismatic joints. Positive and finite.
241    pub tracking_error_limit: f64,
242    /// State watchdog: a joint state whose `Tov` is older than this (or in
243    /// the future, or missing a time) faults the controller. `None`
244    /// disables the watchdog.
245    pub state_timeout: Option<Time>,
246    /// Command rate limit; requires `mode: position`. `None` disables it.
247    pub rate_limit: Option<CuRateLimit>,
248    /// Effort-mode PD gains. In effort mode, hold/fault paths apply a
249    /// damping brake (`-kv * measured velocity`); with no measured state
250    /// the effort is zero and the bridge's gravity compensation is all that
251    /// holds the arm.
252    pub effort_gains: CuEffortGains,
253}
254
255impl Default for CuControllerParams {
256    /// Position mode, 0.5 tracking-error limit, no watchdog, no rate limit,
257    /// effort gains `kp = 20.0`, `kv = 0.8`.
258    fn default() -> Self {
259        Self {
260            mode: CuControllerMode::Position,
261            tracking_error_limit: 0.5,
262            state_timeout: None,
263            rate_limit: None,
264            effort_gains: CuEffortGains::default(),
265        }
266    }
267}
268
269impl CuControllerParams {
270    /// Checks the limit, watchdog, rate limit (position mode only) and gains.
271    pub fn validate(&self) -> CuResult<()> {
272        if !self.tracking_error_limit.is_finite() || self.tracking_error_limit <= 0.0 {
273            return Err(CuError::from(
274                "repose-controller: 'tracking_error_limit' must be positive and finite",
275            ));
276        }
277        if let Some(timeout) = self.state_timeout {
278            let s = timeout.get::<second>();
279            if !s.is_finite() || s <= 0.0 {
280                return Err(CuError::from(
281                    "repose-controller: 'state_timeout' must be positive and finite",
282                ));
283            }
284        }
285        if let Some(rate_limit) = self.rate_limit {
286            rate_limit.validate()?;
287            if self.mode != CuControllerMode::Position {
288                return Err(CuError::from(
289                    "repose-controller: 'rate_limit' requires 'mode: position'",
290                ));
291            }
292        }
293        self.effort_gains.validate()
294    }
295}
296
297/// Converts a positive time to nanoseconds, rounded to nearest and at least
298/// 1 ns (`as` saturates beyond `u64::MAX`).
299fn to_nanos(t: Time) -> u64 {
300    ((t.get::<second>() * 1e9 + 0.5) as u64).max(1)
301}
302
303/// Unified robot-agnostic joint controller task.
304///
305/// Inputs are `(setpoint, state)`: a `CuTrajectoryFollowOutput<N>` and a
306/// `repose_kinematics::CuJointStatePayload<N>`. Output is a
307/// [`CuJointCommandPayload<N>`] addressed to any bridge exposing a
308/// `joint_command` channel. The node `config:` block is a
309/// [`CuControllerParams`].
310#[derive(Reflect)]
311#[reflect(no_field_bounds, from_reflect = false)]
312pub struct CuControllerTask<const N: usize> {
313    #[reflect(ignore)]
314    params: CuControllerParams,
315    #[reflect(ignore)]
316    state_timeout_ns: Option<u64>,
317    #[reflect(ignore)]
318    last_command: [CuJointPosition; N],
319    #[reflect(ignore)]
320    last_status: CuControllerStatus,
321    has_command: bool,
322    last_time: Option<CuTime>,
323    #[reflect(ignore)]
324    last_velocity: [CuJointVelocity; N],
325}
326
327impl<const N: usize> CuControllerTask<N> {
328    /// Creates a controller from validated params.
329    pub fn from_params(params: CuControllerParams) -> CuResult<Self> {
330        params.validate()?;
331        Ok(Self::with_valid_params(params))
332    }
333
334    fn with_valid_params(params: CuControllerParams) -> Self {
335        Self {
336            state_timeout_ns: params.state_timeout.map(to_nanos),
337            params,
338            last_command: [CuJointPosition::default(); N],
339            last_status: CuControllerStatus::Idle,
340            has_command: false,
341            last_time: None,
342            last_velocity: [CuJointVelocity::default(); N],
343        }
344    }
345
346    /// Params this task runs with (including `set_mode`/`set_effort_gains`
347    /// changes).
348    pub fn params(&self) -> &CuControllerParams {
349        &self.params
350    }
351
352    /// Sets the command level (takes effect on the next cycle).
353    pub fn set_mode(&mut self, mode: CuControllerMode) {
354        self.params.mode = mode;
355    }
356
357    /// Returns the active command level.
358    pub fn mode(&self) -> CuControllerMode {
359        self.params.mode
360    }
361
362    /// Sets the effort-mode PD gains. Invalid gains (non-finite or negative)
363    /// are rejected and leave the current gains unchanged.
364    pub fn set_effort_gains(&mut self, gains: CuEffortGains) -> CuResult<()> {
365        gains.validate()?;
366        self.params.effort_gains = gains;
367        Ok(())
368    }
369}
370
371impl<const N: usize> Default for CuControllerTask<N> {
372    fn default() -> Self {
373        Self::with_valid_params(CuControllerParams::default())
374    }
375}
376
377impl<const N: usize> Freezable for CuControllerTask<N> {
378    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
379        let p = &self.params;
380        Encode::encode(&p.tracking_error_limit, encoder)?;
381        Encode::encode(&p.mode, encoder)?;
382        Encode::encode(&p.effort_gains.kp, encoder)?;
383        Encode::encode(&p.effort_gains.kv, encoder)?;
384        Encode::encode(&self.last_command, encoder)?;
385        Encode::encode(&self.last_status, encoder)?;
386        Encode::encode(&self.has_command, encoder)?;
387        Encode::encode(&self.state_timeout_ns, encoder)?;
388        Encode::encode(&p.state_timeout.map(|t| t.get::<second>()), encoder)?;
389        Encode::encode(&p.rate_limit.map(|r| r.max_speed), encoder)?;
390        Encode::encode(&p.rate_limit.map(|r| r.max_acceleration), encoder)?;
391        Encode::encode(&self.last_time, encoder)?;
392        Encode::encode(&self.last_velocity, encoder)
393    }
394
395    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
396        self.params.tracking_error_limit = Decode::decode(decoder)?;
397        self.params.mode = Decode::decode(decoder)?;
398        self.params.effort_gains.kp = Decode::decode(decoder)?;
399        self.params.effort_gains.kv = Decode::decode(decoder)?;
400        self.last_command = Decode::decode(decoder)?;
401        self.last_status = Decode::decode(decoder)?;
402        self.has_command = Decode::decode(decoder)?;
403        self.state_timeout_ns = Decode::decode(decoder)?;
404        let timeout: Option<f64> = Decode::decode(decoder)?;
405        self.params.state_timeout = timeout.map(Time::new::<second>);
406        let speed: Option<f64> = Decode::decode(decoder)?;
407        let acceleration: Option<f64> = Decode::decode(decoder)?;
408        self.params.rate_limit = speed
409            .zip(acceleration)
410            .map(|(max_speed, max_acceleration)| CuRateLimit {
411                max_speed,
412                max_acceleration,
413            });
414        self.last_time = Decode::decode(decoder)?;
415        self.last_velocity = Decode::decode(decoder)?;
416        Ok(())
417    }
418}
419
420impl<const N: usize> CuTask for CuControllerTask<N> {
421    type Resources<'r> = ();
422    type Input<'m> = input_msg!('m, CuTrajectoryFollowOutput<N>, CuJointStatePayload<N>);
423    type Output<'m> = output_msg!(CuJointCommandPayload<N>);
424
425    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
426    where
427        Self: Sized,
428    {
429        let params = cfg
430            .map(ComponentConfig::deserialize_into::<CuControllerParams>)
431            .transpose()?
432            .unwrap_or_default();
433        Self::from_params(params)
434    }
435
436    fn process<'i, 'o>(
437        &mut self,
438        ctx: &CuContext,
439        input: &Self::Input<'i>,
440        output: &mut Self::Output<'o>,
441    ) -> CuResult<()> {
442        output.tov = input.1.tov;
443        let state = input.1.payload();
444        let setpoint = input.0.payload();
445
446        let mode = self.params.mode;
447        let CuEffortGains { kp, kv } = self.params.effort_gains;
448        let mut command = CuJointCommandPayload::<N> {
449            mode,
450            ..Default::default()
451        };
452        let now = ctx.now();
453        let stale = self
454            .state_timeout_ns
455            .is_some_and(|timeout| match input.1.tov {
456                Tov::Time(t) => {
457                    t.as_nanos() > now.as_nanos() || now.as_nanos() - t.as_nanos() > timeout
458                }
459                _ => true,
460            });
461        match (setpoint, state) {
462            (_, _) if stale => {
463                command.positions = self.last_command;
464                command.status = CuControllerStatus::Fault;
465            }
466            (_, Some(state)) if state.status != CuRobotStatus::Ready => {
467                command.positions = state.positions;
468                command.status = CuControllerStatus::Fault;
469            }
470            (None, None) => {
471                command.positions = self.last_command;
472                command.status = if self.has_command {
473                    CuControllerStatus::Holding
474                } else {
475                    CuControllerStatus::Idle
476                };
477            }
478            (None, Some(state)) => {
479                command.positions = state.positions;
480                command.status = CuControllerStatus::Idle;
481                if mode == CuControllerMode::Effort {
482                    for j in 0..N {
483                        command.efforts[j] = CuJointEffort::from_position(
484                            state.positions[j],
485                            -kv * state.velocities[j].si_value(),
486                        );
487                    }
488                }
489            }
490            (Some(_), None) => {
491                command.positions = self.last_command;
492                command.status = CuControllerStatus::Fault;
493            }
494            (Some(setpoint), Some(state)) => {
495                let mut max_err = 0.0f64;
496                let mut max_kind = CuJointPosition::default();
497                for j in 0..N {
498                    let err =
499                        (setpoint.positions[j].si_value() - state.positions[j].si_value()).abs();
500                    if err > max_err {
501                        max_err = err;
502                        max_kind = setpoint.positions[j];
503                    }
504                }
505                command.max_tracking_error = max_kind.with_si_value(max_err);
506                if setpoint.status == CuFollowStatus::ErrorEmptyTrajectory
507                    || max_err > self.params.tracking_error_limit
508                {
509                    command.positions = state.positions;
510                    command.status = CuControllerStatus::Fault;
511                    if mode == CuControllerMode::Effort {
512                        // Damping brake: no stiffness towards the setpoint,
513                        // but resist motion instead of going limp.
514                        for j in 0..N {
515                            command.efforts[j] = CuJointEffort::from_position(
516                                state.positions[j],
517                                -kv * state.velocities[j].si_value(),
518                            );
519                        }
520                    }
521                } else {
522                    command.positions = setpoint.positions;
523                    command.velocities = setpoint.velocities;
524                    if mode == CuControllerMode::Effort {
525                        for j in 0..N {
526                            let pos_err =
527                                setpoint.positions[j].si_value() - state.positions[j].si_value();
528                            let vel_err =
529                                setpoint.velocities[j].si_value() - state.velocities[j].si_value();
530                            let effort = kp * pos_err + kv * vel_err;
531                            command.efforts[j] =
532                                CuJointEffort::from_position(setpoint.positions[j], effort);
533                        }
534                    }
535                    command.status = match setpoint.status {
536                        CuFollowStatus::Running => CuControllerStatus::Running,
537                        CuFollowStatus::Finished => CuControllerStatus::Holding,
538                        CuFollowStatus::Idle => CuControllerStatus::Idle,
539                        CuFollowStatus::ErrorEmptyTrajectory => CuControllerStatus::Fault,
540                    };
541                }
542            }
543        }
544
545        // Rate limiting applies while a setpoint is followed (running or
546        // holding its end). It starts from the measured state after idle or
547        // fault, so no stale command or velocity carries over.
548        if let Some(CuRateLimit {
549            max_speed,
550            max_acceleration,
551        }) = self.params.rate_limit
552            && matches!(
553                command.status,
554                CuControllerStatus::Running | CuControllerStatus::Holding
555            )
556            && let Some(measured) = state
557        {
558            let following = matches!(
559                self.last_status,
560                CuControllerStatus::Running | CuControllerStatus::Holding
561            );
562            if !following {
563                self.last_command = measured.positions;
564                self.last_velocity = measured
565                    .positions
566                    .map(|q| CuJointVelocity::from_coordinate(q, 0.0));
567            }
568            let nanos = match self.last_time {
569                Some(previous) if following => now.as_nanos().saturating_sub(previous.as_nanos()),
570                _ => 0,
571            };
572            if nanos == 0 {
573                // No elapsed time (first cycle or a repeated tick): repeat the
574                // last limited command.
575                command.positions = self.last_command;
576                command.velocities = self.last_velocity;
577            } else {
578                let dt = nanos as f64 * 1e-9;
579                let mut settled = true;
580                for j in 0..N {
581                    let current = self.last_command[j].si_value();
582                    let old_v = self.last_velocity[j].si_value();
583                    let desired = ((command.positions[j].si_value() - current) / dt)
584                        .clamp(-max_speed, max_speed);
585                    let v = desired
586                        .clamp(old_v - max_acceleration * dt, old_v + max_acceleration * dt)
587                        .clamp(-max_speed, max_speed);
588                    let target = command.positions[j].si_value();
589                    command.positions[j] = self.last_command[j].with_si_value(current + v * dt);
590                    command.velocities[j] =
591                        CuJointVelocity::from_coordinate(command.positions[j], v);
592                    settled &=
593                        (command.positions[j].si_value() - target).abs() <= 1e-9 && v.abs() <= 1e-9;
594                }
595                // Holding is only reported once the limited command has
596                // actually reached the held setpoint.
597                if command.status == CuControllerStatus::Holding && !settled {
598                    command.status = CuControllerStatus::Running;
599                }
600            }
601        }
602        self.last_time = Some(now);
603        self.last_velocity = command.velocities;
604        self.last_command = command.positions;
605        self.last_status = command.status;
606        self.has_command = true;
607        output.set_payload(command);
608        Ok(())
609    }
610}
611
612#[cfg(test)]
613mod tests {
614    use super::*;
615    use cu29::units::si::angle::radian;
616    use cu29::units::si::angular_velocity::radian_per_second;
617    use cu29::units::si::f64::{Angle, AngularVelocity};
618
619    fn follow(pos: f64, status: CuFollowStatus) -> CuTrajectoryFollowOutput<2> {
620        CuTrajectoryFollowOutput {
621            positions: [CuJointPosition::Revolute(Angle::new::<radian>(pos)); 2],
622            velocities: [CuJointVelocity::Revolute(AngularVelocity::new::<radian_per_second>(0.1));
623                2],
624            status,
625        }
626    }
627
628    fn with_limit(limit: f64) -> CuControllerTask<2> {
629        CuControllerTask::from_params(CuControllerParams {
630            tracking_error_limit: limit,
631            ..CuControllerParams::default()
632        })
633        .unwrap()
634    }
635
636    fn state(pos: f64) -> CuJointStatePayload<2> {
637        CuJointStatePayload {
638            status: CuRobotStatus::Ready,
639            positions: [CuJointPosition::Revolute(Angle::new::<radian>(pos)); 2],
640            velocities: [CuJointVelocity::default(); 2],
641        }
642    }
643
644    fn run(
645        task: &mut CuControllerTask<2>,
646        setpoint: Option<CuTrajectoryFollowOutput<2>>,
647        state: Option<CuJointStatePayload<2>>,
648    ) -> CuJointCommandPayload<2> {
649        let setpoint_msg = CuMsg::new(setpoint);
650        let state_msg = CuMsg::new(state);
651        let input = (&setpoint_msg, &state_msg);
652        let mut output = CuMsg::<CuJointCommandPayload<2>>::default();
653        let (ctx, _clock) = CuContext::new_mock_clock();
654        task.process(&ctx, &input, &mut output).unwrap();
655        output.payload().unwrap().clone()
656    }
657
658    #[test]
659    fn test_idle_holds_measured_state() {
660        let mut task = CuControllerTask::<2>::default();
661        let out = run(&mut task, None, Some(state(0.3)));
662        assert_eq!(out.status, CuControllerStatus::Idle);
663        assert_eq!(out.positions[0].si_value(), 0.3);
664    }
665
666    #[test]
667    fn test_running_mirrors_setpoint() {
668        let mut task = CuControllerTask::<2>::default();
669        let out = run(
670            &mut task,
671            Some(follow(0.4, CuFollowStatus::Running)),
672            Some(state(0.35)),
673        );
674        assert_eq!(out.status, CuControllerStatus::Running);
675        assert_eq!(out.positions[0].si_value(), 0.4);
676        assert!((out.max_tracking_error.si_value() - 0.05).abs() < 1e-12);
677    }
678
679    #[test]
680    fn test_finished_holds() {
681        let mut task = CuControllerTask::<2>::default();
682        let out = run(
683            &mut task,
684            Some(follow(0.4, CuFollowStatus::Finished)),
685            Some(state(0.4)),
686        );
687        assert_eq!(out.status, CuControllerStatus::Holding);
688    }
689
690    #[test]
691    fn test_tracking_fault_holds_measured() {
692        let mut task = with_limit(0.1);
693        let out = run(
694            &mut task,
695            Some(follow(1.0, CuFollowStatus::Running)),
696            Some(state(0.0)),
697        );
698        assert_eq!(out.status, CuControllerStatus::Fault);
699        assert_eq!(out.positions[0].si_value(), 0.0);
700    }
701
702    #[test]
703    fn test_setpoint_without_state_faults() {
704        let mut task = CuControllerTask::<2>::default();
705        let out = run(&mut task, Some(follow(0.4, CuFollowStatus::Running)), None);
706        assert_eq!(out.status, CuControllerStatus::Fault);
707    }
708
709    #[test]
710    fn test_effort_mode_computes_pd_effort() {
711        let mut task = with_limit(0.5);
712        task.set_mode(CuControllerMode::Effort);
713        task.set_effort_gains(CuEffortGains { kp: 10.0, kv: 2.0 })
714            .unwrap();
715        let out = run(
716            &mut task,
717            Some(follow(0.5, CuFollowStatus::Running)),
718            Some(state(0.0)),
719        );
720        assert_eq!(out.mode, CuControllerMode::Effort);
721        assert_eq!(out.status, CuControllerStatus::Running);
722        // tau = 10 * (0.5 - 0) + 2 * (0.1 - 0) = 5.2 N m
723        assert!((out.efforts[0].si_value() - 5.2).abs() < 1e-9);
724    }
725
726    #[test]
727    fn test_effort_fault_outputs_zero_effort() {
728        let mut task = with_limit(0.1);
729        task.set_mode(CuControllerMode::Effort);
730        let out = run(
731            &mut task,
732            Some(follow(1.0, CuFollowStatus::Running)),
733            Some(state(0.0)),
734        );
735        assert_eq!(out.status, CuControllerStatus::Fault);
736        assert_eq!(out.efforts[0].si_value(), 0.0);
737    }
738
739    #[test]
740    fn test_velocity_mode_tags_command() {
741        let mut task = with_limit(0.5);
742        task.set_mode(CuControllerMode::Velocity);
743        let out = run(
744            &mut task,
745            Some(follow(0.4, CuFollowStatus::Running)),
746            Some(state(0.35)),
747        );
748        assert_eq!(out.mode, CuControllerMode::Velocity);
749        assert_eq!(out.status, CuControllerStatus::Running);
750        assert_eq!(out.efforts[0].si_value(), 0.0);
751    }
752
753    #[test]
754    fn test_freeze_thaw_round_trip() {
755        let mut task = with_limit(0.25);
756        let _ = run(
757            &mut task,
758            Some(follow(0.4, CuFollowStatus::Running)),
759            Some(state(0.35)),
760        );
761        let mut bytes = [0u8; 256];
762        let mut encoder = bincode::enc::EncoderImpl::new(
763            bincode::enc::write::SliceWriter::new(&mut bytes),
764            bincode::config::standard(),
765        );
766        task.freeze(&mut encoder).unwrap();
767        let len = encoder.into_writer().bytes_written();
768        let mut restored = CuControllerTask::<2>::default();
769        restored
770            .thaw(&mut bincode::de::DecoderImpl::new(
771                bincode::de::read::SliceReader::new(&bytes[..len]),
772                bincode::config::standard(),
773                (),
774            ))
775            .unwrap();
776        assert_eq!(restored.last_command[0].si_value(), 0.4);
777        assert_eq!(restored.last_status, CuControllerStatus::Running);
778    }
779
780    #[test]
781    fn test_invalid_params_rejected() {
782        let bad = |p: CuControllerParams| CuControllerTask::<2>::from_params(p).is_err();
783        let d = CuControllerParams::default();
784        assert!(bad(CuControllerParams {
785            tracking_error_limit: 0.0,
786            ..d
787        }));
788        assert!(bad(CuControllerParams {
789            tracking_error_limit: f64::NAN,
790            ..d
791        }));
792        assert!(bad(CuControllerParams {
793            state_timeout: Some(Time::new::<second>(0.0)),
794            ..d
795        }));
796        let rate = CuRateLimit {
797            max_speed: 1.0,
798            max_acceleration: 2.0,
799        };
800        assert!(bad(CuControllerParams {
801            rate_limit: Some(CuRateLimit {
802                max_speed: -1.0,
803                ..rate
804            }),
805            ..d
806        }));
807        assert!(bad(CuControllerParams {
808            rate_limit: Some(CuRateLimit {
809                max_acceleration: f64::INFINITY,
810                ..rate
811            }),
812            ..d
813        }));
814        assert!(bad(CuControllerParams {
815            mode: CuControllerMode::Velocity,
816            rate_limit: Some(rate),
817            ..d
818        }));
819        assert!(bad(CuControllerParams {
820            effort_gains: CuEffortGains { kp: -1.0, kv: 0.0 },
821            ..d
822        }));
823        assert!(!bad(CuControllerParams {
824            rate_limit: Some(rate),
825            ..d
826        }));
827    }
828
829    #[test]
830    fn test_set_effort_gains_rejects_invalid() {
831        let mut task = CuControllerTask::<2>::default();
832        assert!(
833            task.set_effort_gains(CuEffortGains {
834                kp: f64::NAN,
835                kv: 1.0
836            })
837            .is_err()
838        );
839        assert_eq!(task.params().effort_gains, CuEffortGains::default());
840    }
841
842    #[cfg(feature = "std")]
843    #[test]
844    fn test_reads_config() {
845        let cfg: ComponentConfig = ron::from_str(
846            r#"#![enable(unwrap_newtypes)] {"mode": "position", "tracking_error_limit": 0.25,
847            "state_timeout": 0.2, "rate_limit": {"max_speed": 1.5, "max_acceleration": 6.0},
848            "effort_gains": {"kp": 10.0, "kv": 2.0}}"#,
849        )
850        .unwrap();
851        let task = CuControllerTask::<2>::new(Some(&cfg), ()).unwrap();
852        let p = task.params();
853        assert_eq!(p.tracking_error_limit, 0.25);
854        assert_eq!(p.state_timeout, Some(Time::new::<second>(0.2)));
855        assert_eq!(task.state_timeout_ns, Some(200_000_000));
856        assert_eq!(
857            p.rate_limit,
858            Some(CuRateLimit {
859                max_speed: 1.5,
860                max_acceleration: 6.0
861            })
862        );
863        assert_eq!(p.effort_gains, CuEffortGains { kp: 10.0, kv: 2.0 });
864        let task = CuControllerTask::<2>::new(None, ()).unwrap();
865        assert_eq!(task.params(), &CuControllerParams::default());
866        for bad in [
867            r#"{"tracking_error_limit_rad": 0.5}"#,
868            r#"{"rate_limit": {"max_speed": 1.5}}"#,
869            r#"{"mode": "effort", "rate_limit": {"max_speed": 1.5, "max_acceleration": 6.0}}"#,
870        ] {
871            let cfg: ComponentConfig =
872                ron::from_str(&format!("#![enable(unwrap_newtypes)] {bad}")).unwrap();
873            assert!(CuControllerTask::<2>::new(Some(&cfg), ()).is_err(), "{bad}");
874        }
875    }
876}
repose_controller/lib.rs

repose_controller/
lib.rs
