franka/robot/target_control/
cartesian.rs

1//! The Cartesian interface of [target control](super): a pose target in the base frame. The
2//! slot holds the position and a unit quaternion `[x, y, z, w]`; one synchronised six-axis
3//! generator runs on the position and on the base-frame rotation vector of the orientation
4//! error (see [`chart`]). [`Backend::Impedance`] follows the generator's pose with the
5//! differential IK and sends the impedance law's torques; [`Backend::RobotController`]
6//! streams the pose through [`Robot::control_cartesian_pose`].
7
8use std::sync::mpsc::SyncSender;
9use std::sync::Arc;
10
11use nalgebra::Matrix3;
12
13use super::rotation::{
14    angle_between, checked_pose, distance, exp, from_quaternion, log, pose_from, rotation_of,
15    to_quaternion, translation_of, unit_quaternion,
16};
17use super::runner::Step;
18use super::torque::{PoseTracker, TorqueLoop};
19use super::{
20    check_posture, joint_position_limits, spawn, Backend, Handle, ImpedanceOptions, Runner, Shared,
21    TargetControlOptions,
22};
23use crate::control_types::CartesianPose;
24use crate::error::FrankaResult;
25use crate::lowpass_filter::MAX_CUTOFF_FREQUENCY;
26use crate::math_utils::orthonormalized_rotation;
27use crate::model::Model;
28use crate::otg::OtgLimits;
29use crate::rate_limiting::{
30    limit_rate_cartesian_pose, DELTA_T, FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE,
31};
32use crate::robot::Robot;
33use crate::robot_state::RobotState;
34use crate::wire::robot::codec::FciVersion;
35
36/// What one cycle sent, for the observer.
37#[derive(Debug, Clone, Copy, PartialEq)]
38pub struct CartesianSent {
39    /// The pose that went to the robot (after the backstop), or with [`Backend::Impedance`]
40    /// the desired pose the IK follows; column-major.
41    pub pose: [f64; 16],
42    /// The orientation of `pose` as a unit quaternion, `[x, y, z, w]`.
43    pub orientation: [f64; 4],
44    /// The target position the generator was planning towards, base frame, m.
45    pub target: [f64; 3],
46    /// The target orientation the generator was planning towards, `[x, y, z, w]`.
47    pub target_orientation: [f64; 4],
48    /// The generator's velocity at the end of the cycle, m/s.
49    pub velocity: [f64; 3],
50    /// The generator's acceleration at the end of the cycle, m/s^2.
51    pub acceleration: [f64; 3],
52    /// The generator's angular velocity at the end of the cycle, base frame, rad/s.
53    pub angular_velocity: [f64; 3],
54    /// The generator's angular acceleration at the end of the cycle, base frame, rad/s^2.
55    pub angular_acceleration: [f64; 3],
56    /// How far the backstop moved the generator's position, m: zero unless the generator
57    /// outran the budget (or without `limit_rate`, or with [`Backend::Impedance`]).
58    pub backstop_alteration: f64,
59    /// How far the backstop turned the generator's orientation, rad; as above.
60    pub backstop_angular_alteration: f64,
61    /// The joint target of the impedance law, rad; zeros with [`Backend::RobotController`].
62    pub q_goal: [f64; 7],
63    /// The torques sent, Nm, clamped to the torque limits; zeros with
64    /// [`Backend::RobotController`].
65    pub tau: [f64; 7],
66    /// The IK's residual toward `pose` after this cycle's iterations, m plus rad in one norm;
67    /// 0 with [`Backend::RobotController`].
68    pub ik_error: f64,
69    /// How far, m, the leash pulled the generator's anchor back from the previous desired
70    /// position toward the measured one: zero while the arm follows, positive while it is
71    /// held back ([`Leash`](super::Leash)); 0 with [`Backend::RobotController`].
72    pub leash_alteration: f64,
73    /// The same for the orientation, rad.
74    pub leash_angular_alteration: f64,
75}
76
77/// The observer's type: called every cycle on the realtime thread, so it must not allocate
78/// or block.
79pub type CartesianObserver = Box<dyn FnMut(&RobotState, &CartesianSent) + Send>;
80
81/// The handle of a running Cartesian target control; see the [module documentation](super).
82/// The target is a pose; orientations are unit quaternions in **`[x, y, z, w]` order** (the
83/// scalar part last) or the rotation block of a column-major pose as in `O_T_EE`.
84pub struct CartesianTargetControl {
85    inner: Handle<7>,
86}
87
88impl CartesianTargetControl {
89    /// Sets the target position, absolute, in the base frame, m; the target orientation
90    /// stays what it is. Callable from any thread at any rate; only the latest target counts.
91    ///
92    /// # Errors
93    /// [`crate::error::FrankaError::InvalidArgument`] if a coordinate is not finite,
94    /// [`crate::error::FrankaError::InvalidOperation`] with [`super::ENDED_MESSAGE`] once the
95    /// loop has ended for any reason.
96    pub fn set_position(&self, position_in_base: [f64; 3]) -> FrankaResult<()> {
97        self.inner
98            .modify_target(|target| target[..3].copy_from_slice(&position_in_base))
99    }
100
101    /// Sets the target orientation, absolute, in the base frame, as a unit quaternion in
102    /// **`[x, y, z, w]` order**; the target position stays what it is.
103    ///
104    /// # Errors
105    /// [`crate::error::FrankaError::InvalidArgument`] if a component is not finite or the norm
106    /// is further than [`super::UNIT_QUATERNION_TOLERANCE`] from one (within it, the
107    /// quaternion is normalised), [`crate::error::FrankaError::InvalidOperation`] with
108    /// [`super::ENDED_MESSAGE`] once the loop has ended.
109    pub fn set_orientation(&self, orientation_xyzw: [f64; 4]) -> FrankaResult<()> {
110        let orientation = unit_quaternion(orientation_xyzw)?;
111        self.inner
112            .modify_target(|target| target[3..].copy_from_slice(&orientation))
113    }
114
115    /// Sets the target position, m, and orientation, a unit quaternion in **`[x, y, z, w]`
116    /// order** (the scalar part `w` last), both absolute in the base frame.
117    ///
118    /// # Errors
119    /// As [`set_position`](Self::set_position) and [`set_orientation`](Self::set_orientation).
120    pub fn set_target(
121        &self,
122        position_in_base: [f64; 3],
123        orientation_xyzw: [f64; 4],
124    ) -> FrankaResult<()> {
125        let orientation = unit_quaternion(orientation_xyzw)?;
126        self.inner.set_target(join(&position_in_base, &orientation))
127    }
128
129    /// Sets the target pose, a column-major 4x4 transform in the base frame as `O_T_EE`.
130    ///
131    /// # Errors
132    /// [`crate::error::FrankaError::InvalidArgument`] if an entry is not finite, the last row
133    /// is not `[0, 0, 0, 1]` or the rotation block is further than
134    /// [`super::ORTHONORMAL_TOLERANCE`] from orthonormal (within it, it is
135    /// re-orthonormalised), [`crate::error::FrankaError::InvalidOperation`] with
136    /// [`super::ENDED_MESSAGE`] once the loop has ended.
137    pub fn set_pose(&self, pose: &[f64; 16]) -> FrankaResult<()> {
138        let (position, rotation) = checked_pose(pose)?;
139        self.set_target(position, to_quaternion(&rotation))
140    }
141
142    /// The latest target position (the start until something sets it).
143    pub fn target(&self) -> [f64; 3] {
144        split(&self.inner.target()).0
145    }
146
147    /// The latest target orientation, `[x, y, z, w]`.
148    pub fn target_orientation(&self) -> [f64; 4] {
149        split(&self.inner.target()).1
150    }
151
152    /// The latest target pose, column-major as `O_T_EE`.
153    pub fn target_pose(&self) -> [f64; 16] {
154        let (position, orientation) = split(&self.inner.target());
155        pose_from(&from_quaternion(&orientation), &position)
156    }
157
158    /// The latest robot state the loop received, copied out.
159    pub fn state(&self) -> RobotState {
160        self.inner.state()
161    }
162
163    /// Whether the loop is still running; `false` after it ended for any reason.
164    pub fn is_running(&self) -> bool {
165        self.inner.is_running()
166    }
167
168    /// Settles at the current target, finishes the motion, joins the thread and returns the
169    /// loop's result: `Ok` for a regular end, [`crate::error::FrankaError::Control`] if the
170    /// robot aborted the motion or a deviation guard fired.
171    pub fn stop(self) -> FrankaResult<()> {
172        self.inner.stop()
173    }
174}
175
176/// A position and a rotation: an echo, a measured pose or a desired one.
177pub(super) type Placement = ([f64; 3], Matrix3<f64>);
178
179/// The Cartesian [`Chart`](super::runner::Chart): the position, re-anchored on the echo's,
180/// and the base-frame rotation vector `log(R_target * R_echo^T)` from zero; the runner's
181/// rotational step is composed back as `exp(step) * R_echo`.
182pub(super) fn chart(target: &[f64; 7], commanded: &[f64; 7]) -> ([f64; 6], [f64; 6]) {
183    let ([tx, ty, tz], target_orientation) = split(target);
184    let ([x, y, z], orientation) = split(commanded);
185    let r_target = from_quaternion(&target_orientation);
186    let r_commanded = from_quaternion(&orientation);
187    let [a, b, c] = log(&(r_target * r_commanded.transpose()));
188    ([x, y, z, 0.0, 0.0, 0.0], [tx, ty, tz, a, b, c])
189}
190
191/// The translation and the orthonormalised rotation of a pose (float32 on FCI v10:
192/// orthonormal to 1e-7 only, and composed into the next command).
193pub(super) fn placement(pose: &[f64; 16]) -> Placement {
194    (
195        translation_of(pose),
196        orthonormalized_rotation(&rotation_of(pose)),
197    )
198}
199
200/// The column-major pose of a placement.
201pub(super) fn pose_of(placement: &Placement) -> [f64; 16] {
202    pose_from(&placement.1, &placement.0)
203}
204
205/// The pose a step lands on: its position, and its rotational step composed onto `rotation`,
206/// the one the runner was anchored on.
207pub(super) fn compose(step: &Step<6, 7>, rotation: &Matrix3<f64>) -> Placement {
208    let [x, y, z, a, b, c] = step.position;
209    ([x, y, z], exp(&[a, b, c]) * rotation)
210}
211
212/// Whether the measured pose is further from `start` than either guard allows.
213pub(super) fn strayed(
214    state: &RobotState,
215    start: &Placement,
216    max_deviation: f64,
217    max_angular: f64,
218) -> bool {
219    distance(&translation_of(&state.O_T_EE), &start.0) > max_deviation
220        || angle_between(&start.1, &rotation_of(&state.O_T_EE)) > max_angular
221}
222
223/// The observer's record of a step with `pose` as sent; the backend's own fields stay zero.
224pub(super) fn sent(step: &Step<6, 7>, pose: [f64; 16]) -> CartesianSent {
225    let [vx, vy, vz, wx, wy, wz] = step.velocity;
226    let [ax, ay, az, bx, by, bz] = step.acceleration;
227    let (target, target_orientation) = split(&step.target);
228    CartesianSent {
229        pose,
230        orientation: to_quaternion(&rotation_of(&pose)),
231        target,
232        target_orientation,
233        velocity: [vx, vy, vz],
234        acceleration: [ax, ay, az],
235        angular_velocity: [wx, wy, wz],
236        angular_acceleration: [bx, by, bz],
237        backstop_alteration: 0.0,
238        backstop_angular_alteration: 0.0,
239        q_goal: [0.0; 7],
240        tau: [0.0; 7],
241        ik_error: 0.0,
242        leash_alteration: 0.0,
243        leash_angular_alteration: 0.0,
244    }
245}
246
247/// The translation and the base-frame rotation vector from the echo to `pose`.
248fn increment(pose: &[f64; 16], echo: &Placement) -> [f64; 6] {
249    let [x, y, z] = translation_of(pose);
250    let [a, b, c] = log(&(rotation_of(pose) * echo.1.transpose()));
251    [x - echo.0[0], y - echo.0[1], z - echo.0[2], a, b, c]
252}
253
254/// The seven slot values of a pose: its translation and the quaternion of its rotation.
255pub(super) fn slot_values(translation: &[f64; 3], rotation: &Matrix3<f64>) -> [f64; 7] {
256    join(translation, &to_quaternion(rotation))
257}
258
259fn join(position: &[f64; 3], orientation: &[f64; 4]) -> [f64; 7] {
260    let mut slot = [0.0; 7];
261    slot[..3].copy_from_slice(position);
262    slot[3..].copy_from_slice(orientation);
263    slot
264}
265
266fn split(slot: &[f64; 7]) -> ([f64; 3], [f64; 4]) {
267    let [x, y, z, a, b, c, d] = *slot;
268    ([x, y, z], [a, b, c, d])
269}
270
271/// The per-axis limits of the six generator axes; the rotational ones carry the backstop's
272/// 0.99 factor, or it binds on every synchronised jerk and the two orbit instead of landing.
273pub(super) fn axis_limits(limits: OtgLimits, rotation_limits: OtgLimits) -> [OtgLimits; 6] {
274    let translational = limits.per_axis_for_norm(3);
275    let rotational = rotation_limits
276        .scaled(FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE)
277        .per_axis_for_norm(3);
278    [
279        translational,
280        translational,
281        translational,
282        rotational,
283        rotational,
284        rotational,
285    ]
286}
287
288const THREAD: &str = "franka-cartesian-target";
289
290pub(super) fn start(
291    robot: &Arc<Robot>,
292    options: TargetControlOptions,
293) -> FrankaResult<CartesianTargetControl> {
294    options.validate()?;
295    check_posture(
296        &options.backend,
297        &joint_position_limits(robot.fci_version()),
298    )?;
299    let shared = Arc::new(Shared::<7>::default());
300    let loop_shared = Arc::clone(&shared);
301    let priority = options.realtime_priority;
302    let inner = match options.backend {
303        Backend::RobotController => spawn(
304            THREAD,
305            robot,
306            shared,
307            priority,
308            move |robot: &Robot, started| pose_loop(robot, started, options, loop_shared),
309        )?,
310        Backend::Impedance(impedance) => {
311            let model = Arc::new(robot.load_model()?);
312            spawn(
313                THREAD,
314                robot,
315                shared,
316                priority,
317                move |robot: &Robot, started| {
318                    let limit_rate = options.limit_rate;
319                    let version = robot.fci_version();
320                    torque_loop(options, impedance, model, version, loop_shared, started)?
321                        .run(robot, limit_rate)
322                },
323            )?
324        }
325    };
326    Ok(CartesianTargetControl { inner })
327}
328
329/// The [`Backend::Impedance`] loop of this interface: a [`PoseTracker`] on the six-axis
330/// runner, over the negotiated version's joint limits.
331pub(super) fn torque_loop(
332    options: TargetControlOptions,
333    impedance: ImpedanceOptions,
334    model: Arc<Model>,
335    version: FciVersion,
336    shared: Arc<Shared<7>>,
337    started: SyncSender<()>,
338) -> FrankaResult<TorqueLoop<6, 7, PoseTracker>> {
339    let runner = Runner::new(
340        shared,
341        started,
342        axis_limits(options.limits, options.rotation_limits),
343        options.settle,
344        chart,
345    )?;
346    let tracker = PoseTracker::new(
347        &options,
348        &impedance,
349        Arc::clone(&model),
350        joint_position_limits(version),
351    );
352    Ok(TorqueLoop::new(
353        runner,
354        model,
355        impedance,
356        tracker,
357        options.observer,
358    ))
359}
360
361/// [`Backend::RobotController`]: the pose stream, re-anchored on the robot's echo every cycle
362/// with the rate limiter as the backstop; a stop holds the echo bit for bit.
363fn pose_loop(
364    robot: &Robot,
365    started: SyncSender<()>,
366    options: TargetControlOptions,
367    shared: Arc<Shared<7>>,
368) -> FrankaResult<()> {
369    let TargetControlOptions {
370        limits,
371        rotation_limits,
372        controller_mode,
373        max_deviation,
374        max_angular_deviation,
375        settle,
376        limit_rate,
377        mut observer,
378        ..
379    } = options;
380    let mut runner = Runner::new(
381        shared,
382        started,
383        axis_limits(limits, rotation_limits),
384        settle,
385        chart,
386    )?;
387    let mut start: Option<Placement> = None;
388    let mut held: Option<[f64; 16]> = None;
389    // The backstop's reference: the twist and acceleration of what it sent, not the echoed
390    // ones, whose float32 rounding is worth 200 rad/s^3 of jerk on FCI v10.
391    let (mut last_twist, mut last_acceleration) = ([0.0; 6], [0.0; 6]);
392    let result = robot.control_cartesian_pose(
393        |state: &RobotState, _period| {
394            let echo = placement(&state.O_T_EE_c);
395            let start = *start.get_or_insert(echo);
396            let strayed = strayed(state, &start, max_deviation, max_angular_deviation);
397            let step = runner.cycle(state, slot_values(&echo.0, &echo.1), strayed);
398
399            let (mut backstop_alteration, mut backstop_angular_alteration) = (0.0, 0.0);
400            let pose = if step.hold {
401                // The robot's echo of the last command, bit for bit, for the whole hold.
402                *held.get_or_insert(state.O_T_EE_c)
403            } else {
404                let (position, rotation) = compose(&step, &echo.1);
405                let mut pose = pose_from(&rotation, &position);
406                if limit_rate {
407                    // It only fails on non-finite input; the echo is then the safe command.
408                    #[rustfmt::skip]
409                    let limited = limit_rate_cartesian_pose(
410                        limits.max_velocity, limits.max_acceleration, limits.max_jerk,
411                        rotation_limits.max_velocity, rotation_limits.max_acceleration,
412                        rotation_limits.max_jerk,
413                        &pose, &state.O_T_EE_c, &last_twist, &last_acceleration,
414                    ).unwrap_or(state.O_T_EE_c);
415                    backstop_alteration = distance(&translation_of(&limited), &position);
416                    backstop_angular_alteration = angle_between(&rotation, &rotation_of(&limited));
417                    pose = limited;
418                }
419                pose
420            };
421            let twist = increment(&pose, &echo).map(|d| d / DELTA_T);
422            last_acceleration = std::array::from_fn(|i| (twist[i] - last_twist[i]) / DELTA_T);
423            last_twist = twist;
424            if let Some(observe) = observer.as_mut() {
425                observe(
426                    state,
427                    &CartesianSent {
428                        backstop_alteration,
429                        backstop_angular_alteration,
430                        ..sent(&step, pose)
431                    },
432                );
433            }
434            let mut output = CartesianPose::new(pose);
435            output.motion_finished = step.finished;
436            output
437        },
438        controller_mode,
439        limit_rate,
440        MAX_CUTOFF_FREQUENCY,
441    );
442    runner.finish(result)
443}
franka/robot/target_control/cartesian.rs

franka/robot/target_control/
cartesian.rs
