franka/robot/control_loop/
mod.rs

1//! The 1 kHz control loop.
2//!
3//! Port of `franka::ControlLoop` (libfranka 0.21.2 `src/control_loop.{h,cpp}`): start a motion,
4//! call the user callbacks once per cycle, low-pass filter and rate limit what they return,
5//! send it, and finish or cancel the motion at the end.
6//!
7//! This module holds the pieces every control loop shares — the per-version rate-limiting
8//! envelope, the joint-velocity-limit source and the `ConvertContext` the conversions run
9//! in. The conversions themselves are in the `motion` submodule (the four motion generator
10//! command types) and `torque`; the loop that drives them is in `runner`.
11
12mod motion;
13mod runner;
14mod torque;
15
16pub(crate) use runner::{control_torques, ControlLoop};
17pub(crate) use torque::convert_torques;
18#[cfg(test)]
19pub(crate) use torque::torque_rate_margin;
20
21use crate::control_types::{MotionGenerator, MotionGeneratorKind, Torques};
22use crate::duration::Duration;
23use crate::error::FrankaResult;
24use crate::lowpass_filter::MAX_CUTOFF_FREQUENCY;
25use crate::rate_limiting;
26use crate::robot::robot_impl::RobotImpl;
27use crate::robot_state::RobotState;
28use crate::wire::robot::codec::FciVersion;
29use crate::wire::robot::{MotionGeneratorCommand, MoveMotionGeneratorMode};
30
31/// The rate-limiting envelope of one FCI version.
32///
33/// FCI v10 uses `franka::` constants of libfranka 0.21.2 (`include/franka/rate_limiting.h`),
34/// FCI v5 the FER constants of libfranka 0.9.2 ([`crate::rate_limiting::fer`]). The joint
35/// *velocity* limits are not here: they come from the robot through
36/// [`JointVelocityLimitsSource`], which is what makes the FR3's position-dependent envelope and
37/// the FER's flat one interchangeable.
38#[derive(Debug, Clone, Copy)]
39pub(crate) struct RateLimits {
40    pub max_joint_acceleration: [f64; 7],
41    pub max_joint_jerk: [f64; 7],
42    pub max_translational_velocity: f64,
43    pub max_translational_acceleration: f64,
44    pub max_translational_jerk: f64,
45    pub max_rotational_velocity: f64,
46    pub max_rotational_acceleration: f64,
47    pub max_rotational_jerk: f64,
48    pub max_elbow_velocity: f64,
49    pub min_elbow_velocity: f64,
50    pub max_elbow_acceleration: f64,
51    pub max_elbow_jerk: f64,
52    pub max_torque_rate: [f64; 7],
53    /// Whether the `float32` `tau_J_d` margin of [`torque_rate_margin`] applies.
54    ///
55    /// FCI v10 publishes `tau_J_d` as a `float32` (`rbk_types.h:104`), so the margin is needed
56    /// there. The FCI v5 `RobotState` carries `tau_J_d` as a `double` (`rbk_types.h` of
57    /// libfranka 0.9.2), so an FER's rate limiting is exact and the margin — which would
58    /// silently shrink the user's torque budget — is switched off.
59    pub torque_f32_margin: bool,
60}
61
62/// The constants `version` rate limits against.
63pub(crate) const fn rate_limits(version: FciVersion) -> RateLimits {
64    match version {
65        FciVersion::V5 => RateLimits {
66            max_joint_acceleration: rate_limiting::fer::MAX_JOINT_ACCELERATION,
67            max_joint_jerk: rate_limiting::fer::MAX_JOINT_JERK,
68            max_translational_velocity: rate_limiting::fer::MAX_TRANSLATIONAL_VELOCITY,
69            max_translational_acceleration: rate_limiting::fer::MAX_TRANSLATIONAL_ACCELERATION,
70            max_translational_jerk: rate_limiting::fer::MAX_TRANSLATIONAL_JERK,
71            max_rotational_velocity: rate_limiting::fer::MAX_ROTATIONAL_VELOCITY,
72            max_rotational_acceleration: rate_limiting::fer::MAX_ROTATIONAL_ACCELERATION,
73            max_rotational_jerk: rate_limiting::fer::MAX_ROTATIONAL_JERK,
74            max_elbow_velocity: rate_limiting::fer::MAX_ELBOW_VELOCITY,
75            min_elbow_velocity: rate_limiting::fer::MIN_ELBOW_VELOCITY,
76            max_elbow_acceleration: rate_limiting::fer::MAX_ELBOW_ACCELERATION,
77            max_elbow_jerk: rate_limiting::fer::MAX_ELBOW_JERK,
78            max_torque_rate: rate_limiting::fer::MAX_TORQUE_RATE,
79            torque_f32_margin: false,
80        },
81        FciVersion::V10 => RateLimits {
82            max_joint_acceleration: rate_limiting::MAX_JOINT_ACCELERATION,
83            max_joint_jerk: rate_limiting::MAX_JOINT_JERK,
84            max_translational_velocity: rate_limiting::MAX_TRANSLATIONAL_VELOCITY,
85            max_translational_acceleration: rate_limiting::MAX_TRANSLATIONAL_ACCELERATION,
86            max_translational_jerk: rate_limiting::MAX_TRANSLATIONAL_JERK,
87            max_rotational_velocity: rate_limiting::MAX_ROTATIONAL_VELOCITY,
88            max_rotational_acceleration: rate_limiting::MAX_ROTATIONAL_ACCELERATION,
89            max_rotational_jerk: rate_limiting::MAX_ROTATIONAL_JERK,
90            max_elbow_velocity: rate_limiting::MAX_ELBOW_VELOCITY,
91            min_elbow_velocity: rate_limiting::MIN_ELBOW_VELOCITY,
92            max_elbow_acceleration: rate_limiting::MAX_ELBOW_ACCELERATION,
93            max_elbow_jerk: rate_limiting::MAX_ELBOW_JERK,
94            max_torque_rate: rate_limiting::MAX_TORQUE_RATE,
95            torque_f32_margin: true,
96        },
97    }
98}
99
100/// Source of the position-dependent joint velocity limits used by the rate limiter.
101///
102/// libfranka 0.21 reads them from the robot through `RobotControl::getUpper/LowerJointVelocity
103/// Limits`; the trait exists so the conversion functions can be unit tested with the fixed
104/// limits the C++ tests inject through `MockRobotControl`.
105pub(crate) trait JointVelocityLimitsSource {
106    /// Upper joint velocity limits at `q`.
107    fn upper_joint_velocity_limits(&self, q: &[f64; 7]) -> [f64; 7];
108    /// Lower joint velocity limits at `q`.
109    fn lower_joint_velocity_limits(&self, q: &[f64; 7]) -> [f64; 7];
110}
111
112impl JointVelocityLimitsSource for RobotImpl {
113    fn upper_joint_velocity_limits(&self, q: &[f64; 7]) -> [f64; 7] {
114        RobotImpl::upper_joint_velocity_limits(self, q)
115    }
116
117    fn lower_joint_velocity_limits(&self, q: &[f64; 7]) -> [f64; 7] {
118        RobotImpl::lower_joint_velocity_limits(self, q)
119    }
120}
121
122/// Everything `ControlLoop::convertMotion` needs besides the state and the command.
123pub(crate) struct ConvertContext<'a> {
124    /// Where the joint velocity limits come from.
125    pub limits: &'a dyn JointVelocityLimitsSource,
126    /// The version's acceleration/jerk/Cartesian/elbow envelope.
127    pub rate: RateLimits,
128    /// Whether the rate limiter is active.
129    pub limit_rate: bool,
130    /// Low-pass cutoff; filtering is skipped at [`MAX_CUTOFF_FREQUENCY`] and above.
131    pub cutoff_frequency: f64,
132    /// False until the first command of the motion has been converted
133    /// (`ControlLoop::initialized_filter_`).
134    pub initialized_filter: bool,
135    /// Whether the *first* converted command of a motion is its own filter and rate-limiter
136    /// reference.
137    ///
138    /// libfranka 0.21.2 does that through `ControlLoop::initialized_filter_`
139    /// (`src/control_loop.h:120`, used at `src/control_loop.cpp:194-200` and `:245-252`), so on
140    /// an FR3 the first joint-position / Cartesian-pose setpoint of a motion passes the rate
141    /// limiter unchanged. libfranka 0.9.2 has no such field: its `convertMotion`
142    /// (`src/control_loop.cpp:188-205`, `:225-262`) always references `robot_state.q_d` /
143    /// `O_T_EE_c`, so on an FER the very first setpoint is clamped to one integration step
144    /// from the robot's own state like every other one. `true` on FCI v10, `false` on FCI v5.
145    pub first_command_is_its_own_reference: bool,
146}
147
148impl ConvertContext<'_> {
149    fn filtering(&self) -> bool {
150        self.cutoff_frequency < MAX_CUTOFF_FREQUENCY
151    }
152}
153
154/// A command type that a [`ControlLoop`] can turn into a `MotionGeneratorCommand`.
155///
156/// Port of the `ControlLoop<T>::convertMotion` specialisations.
157pub(crate) trait ControlLoopMotion: MotionGenerator {
158    /// The `Move` motion generator mode this command type requests.
159    const MOVE_MODE: MoveMotionGeneratorMode;
160
161    /// Filters, rate limits and validates the callback's output, then writes it into `command`.
162    fn convert(
163        &self,
164        context: &mut ConvertContext<'_>,
165        state: &RobotState,
166        command: &mut MotionGeneratorCommand,
167    ) -> FrankaResult<()>;
168}
169
170/// Maps a [`MotionGeneratorKind`] onto the wire enum (libfranka's `MotionGeneratorTraits`).
171pub(crate) const fn move_mode(kind: MotionGeneratorKind) -> MoveMotionGeneratorMode {
172    match kind {
173        MotionGeneratorKind::JointPosition => MoveMotionGeneratorMode::JointPosition,
174        MotionGeneratorKind::JointVelocity => MoveMotionGeneratorMode::JointVelocity,
175        MotionGeneratorKind::CartesianPosition => MoveMotionGeneratorMode::CartesianPosition,
176        MotionGeneratorKind::CartesianVelocity => MoveMotionGeneratorMode::CartesianVelocity,
177    }
178}
179
180/// The user's motion generator callback (`ControlLoop::MotionGeneratorCallback`).
181pub(crate) type MotionCallback<'a, M> = &'a mut dyn FnMut(&RobotState, Duration) -> M;
182
183/// The user's controller callback (`ControlLoop::ControlCallback`).
184pub(crate) type ControlCallback<'a> = &'a mut dyn FnMut(&RobotState, Duration) -> Torques;
185
186#[cfg(test)]
187mod tests;
franka/robot/control_loop/mod.rs

franka/robot/control_loop/
mod.rs
