copp\robot/robot_core.rs
1//! Robot abstractions and constraint-ingestion utilities for TOPP/COPP.
2//!
3//! # Method identity
4//! This module defines:
5//! - model-side traits ([`RobotBasic`](crate::robot::RobotBasic), [`RobotTorque`](crate::robot::RobotTorque)),
6//! - generic wrapper [`Robot`](crate::robot::Robot) that owns a constraint buffer,
7//! - helper trait [`UpperBound`](crate::robot::robot_core::UpperBound) for broadcasting bound inputs,
8//! - conversion methods from robot kinematics/dynamics constraints to
9//! first/second/third-order inequalities consumed by solvers.
10//!
11//! # Layering
12//! - Model traits provide pure robot semantics (dimension, inverse dynamics).
13//! - [`Robot`](crate::robot::Robot) maps user constraints (`velocity/acceleration/jerk/torque`) into
14//! [`Constraints`](crate::constraints::Constraints).
15//! - Solvers read only normalized constraints, independent of concrete robot
16//! model type.
17//!
18//! # When to use this module
19//! - For most users, prefer [`Robot`](crate::robot::Robot) instead of operating on
20//! [`Constraints`](crate::constraints::Constraints) directly. This enables
21//! physically meaningful high-level APIs such as [`with_axial_velocity`](Robot::with_axial_velocity),
22//! [`with_axial_acceleration`](Robot::with_axial_acceleration), [`with_axial_jerk`](Robot::with_axial_jerk),
23//! and [`with_axial_torque`](Robot::with_axial_torque) constraints.
24//! - `Topp*Problem` workflows only require [`RobotBasic`](crate::robot::RobotBasic).
25//! - `Copp*Problem` workflows require [`RobotTorque`](crate::robot::RobotTorque).
26//! - If no real dynamics are involved but API integration expects
27//! [`RobotTorque`](crate::robot::RobotTorque), you can use `usize` as a trivial placeholder
28//! (`tau = ddq`).
29//! - For physical robots, implement [`RobotTorque`](crate::robot::RobotTorque) with your own inverse
30//! dynamics.
31//!
32//! # Feasibility contract
33//! Bound pairs must satisfy strict signed limits per station:
34//! - upper bound `> 0`, lower bound `< 0`.
35//! This guarantees the zero-state neighborhood remains strictly feasible after
36//! normalization.
37
38use crate::copp::constraints::{AsInputMatrix1D, Constraints, InputMatrix};
39use crate::diag::{ConstraintError, CoppError, RobotDynamicsError};
40use crate::path::Path;
41use nalgebra::{Const, DMatrix, Dyn, Matrix, ViewStorage};
42use std::f64::consts::SQRT_2;
43
44/// Borrowable upper-bound input accepted by robot constraint APIs.
45///
46/// This trait abstracts two common user inputs:
47/// - broadcast vectors `(&[f64], ncols)`;
48/// - explicit matrix views `&InputMatrix`.
49///
50/// Implementations must expose a matrix view of shape `(dim, ncols)`.
51pub trait UpperBound {
52 /// Validate that input row count is compatible with robot dimension `dim`.
53 fn check_valid(&self, dim: usize) -> bool;
54
55 /// Number of station columns represented by this bound input.
56 fn ncols(&self) -> usize;
57
58 /// Borrow input as a matrix view (`dim x ncols`).
59 fn as_matrix(&self) -> InputMatrix<'_>;
60}
61
62impl UpperBound for (&[f64], usize) {
63 #[inline(always)]
64 fn check_valid(&self, dim: usize) -> bool {
65 self.0.len() == dim
66 }
67
68 #[inline(always)]
69 fn ncols(&self) -> usize {
70 self.1
71 }
72
73 #[inline(always)]
74 fn as_matrix(&self) -> InputMatrix<'_> {
75 let dim = self.0.len();
76 let ncols = self.1;
77 // Zero-copy broadcast.
78 unsafe {
79 // Construct the matrix view directly using ViewStorage::from_raw_parts.
80 // Parameters:
81 // - data: Pointer to the original slice.
82 // - shape: (Rows: dim, Columns: ncols).
83 // - stride: (Row stride: 1, Column stride: 0).
84 // Setting the column stride to 0 achieves horizontal broadcasting
85 // without copying data, as every column starts at the same memory address.
86 let storage = ViewStorage::from_raw_parts(
87 self.0.as_ptr(),
88 (Dyn(dim), Dyn(ncols)),
89 (Const::<1>, Dyn(0)),
90 );
91 Matrix::from_data(storage)
92 }
93 }
94}
95
96impl UpperBound for &InputMatrix<'_> {
97 #[inline(always)]
98 fn check_valid(&self, dim: usize) -> bool {
99 self.nrows() == dim
100 }
101
102 #[inline(always)]
103 fn ncols(&self) -> usize {
104 (*self).ncols()
105 }
106
107 #[inline(always)]
108 fn as_matrix(&self) -> InputMatrix<'_> {
109 // Already a view; no conversion/allocation needed.
110 self.as_view()
111 }
112}
113
114/// Minimal robot metadata required by the planner.
115///
116/// A `usize` variable can serve as a trivial [`RobotBasic`](crate::robot::RobotBasic) implementation representing the robot dimension, but users can also implement this trait for their own robot models.
117pub trait RobotBasic {
118 /// Return robot dimension / DoF.
119 fn dim(&self) -> usize;
120}
121
122impl RobotBasic for usize {
123 #[inline(always)]
124 fn dim(&self) -> usize {
125 *self
126 }
127}
128
129/// User-facing robot wrapper that owns constraint storage and conversion logic.
130///
131/// # Design role
132/// [`Robot<M>`](crate::robot::Robot) bridges robot-side physical constraints and solver-side normalized
133/// inequalities. Internally it owns [`Constraints`](crate::constraints::Constraints),
134/// but exposes higher-level APIs with physical semantics.
135///
136/// # Why prefer this over direct [`Constraints`](crate::constraints::Constraints)
137/// For most applications, [`Robot`](crate::robot::Robot) is the recommended entry because it provides
138/// domain-meaningful methods ([`with_axial_velocity`](Robot::with_axial_velocity),
139/// [`with_axial_acceleration`](Robot::with_axial_acceleration), [`with_axial_jerk`](Robot::with_axial_jerk),
140/// [`with_axial_torque`](Robot::with_axial_torque)) and enforces common contracts.
141///
142/// # Trait requirements by solver family
143/// - `Topp*Problem`: model type `M` only needs [`RobotBasic`](crate::robot::RobotBasic).
144/// - `Copp*Problem`: model type `M` must implement [`RobotTorque`](crate::robot::RobotTorque).
145///
146/// If you do not have a real inverse-dynamics model yet, use `usize`
147/// as a placeholder implementing [`RobotTorque`](crate::robot::RobotTorque) (`tau = ddq`).
148pub struct Robot<M: RobotBasic> {
149 /// Concrete robot model implementation.
150 model: M,
151
152 /// Shared station-indexed constraint buffer used by TOPP/COPP solvers.
153 pub constraints: Constraints,
154}
155
156impl<M: RobotBasic> Robot<M> {
157 /// Access the robot model `M: RobotBasic` as a reference.
158 #[inline(always)]
159 pub fn model(&self) -> &M {
160 &self.model
161 }
162
163 /// Mutably access the robot model `M: RobotBasic`.
164 #[inline(always)]
165 pub fn model_mut(&mut self) -> &mut M {
166 &mut self.model
167 }
168
169 /// Enforce strict signed contract for upper/lower bounds.
170 ///
171 /// # Contract
172 /// For every element in the provided matrices:
173 /// - `upper > 0`
174 /// - `lower < 0`
175 ///
176 /// # Errors
177 /// Returns [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds) when contract is violated.
178 #[inline(always)]
179 fn check_strict_signed_limits(
180 upper: &InputMatrix,
181 lower: &InputMatrix,
182 bound_name: &'static str,
183 ) -> Result<(), ConstraintError> {
184 let upper_valid = upper.iter().all(|&u| u > 0.0);
185 let lower_valid = lower.iter().all(|&l| l < 0.0);
186 if upper_valid && lower_valid {
187 Ok(())
188 } else {
189 Err(ConstraintError::InvalidSignedBounds { bound_name })
190 }
191 }
192
193 /// Construct a robot wrapper with default constraint-buffer capacity.
194 ///
195 /// # Parameters
196 /// - `model`: concrete robot model implementing [`RobotBasic`](crate::robot::RobotBasic).
197 pub fn new(model: M) -> Self {
198 let dim = model.dim();
199 Self {
200 model,
201 constraints: Constraints::new(dim),
202 }
203 }
204
205 /// Construct a robot wrapper with explicit initial constraint capacity.
206 ///
207 /// # Parameters
208 /// - `model`: concrete robot model.
209 /// - `capacity`: initial circular-buffer column capacity.
210 pub fn with_capacity(model: M, capacity: usize) -> Self {
211 let dim = model.dim();
212 Self {
213 model,
214 constraints: Constraints::with_capacity(dim, capacity),
215 }
216 }
217
218 /// Get robot dimension / DoF.
219 #[inline(always)]
220 pub fn dim(&self) -> usize {
221 self.constraints.dim()
222 }
223
224 /// Append a new station segment into the internal constraint buffer.
225 ///
226 /// # Parameters
227 /// - `s_new`: station samples accepted as 1D slice or matrix view.
228 ///
229 /// # Errors
230 /// - [`ConstraintError::NonIncreasingS`](crate::diag::ConstraintError::NonIncreasingS) if `s_new` is not strictly
231 /// increasing, or if its first sample does not come after the current
232 /// last stored station.
233 ///
234 /// # Returns
235 /// Returns `&mut Self` for chaining on success.
236 #[inline(always)]
237 pub fn with_s<T: AsInputMatrix1D + ?Sized>(
238 &mut self,
239 s_new: &T,
240 ) -> Result<&mut Self, ConstraintError> {
241 self.constraints.with_s(s_new)?;
242 Ok(self)
243 }
244
245 /// Write path derivatives over interval starting at `idx_s`.
246 ///
247 /// # Parameters
248 /// - `q_new`, `dq_new`, `ddq_new`: required derivative matrices.
249 /// - `dddq_new`: optional third derivative matrix.
250 /// - `idx_s`: global start station id.
251 ///
252 /// # Behavior
253 /// - If `dddq_new` is provided, third-order derivative data is written for
254 /// the target interval.
255 /// - If `dddq_new` is `None`, existing third-order derivative data is
256 /// cleared over the target interval.
257 ///
258 /// # Errors
259 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions) if derivative matrix shapes are
260 /// inconsistent with each other or with the robot dimension.
261 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds) if the target station interval is
262 /// outside the stored station range.
263 ///
264 /// # Returns
265 /// Returns `&mut Self` for chaining on success.
266 #[inline(always)]
267 pub fn with_q(
268 &mut self,
269 q_new: &InputMatrix,
270 dq_new: &InputMatrix,
271 ddq_new: &InputMatrix,
272 dddq_new: Option<&InputMatrix>,
273 idx_s: usize,
274 ) -> Result<&mut Self, ConstraintError> {
275 self.constraints
276 .with_q(q_new, dq_new, ddq_new, dddq_new, idx_s)?;
277 Ok(self)
278 }
279
280 /// Sample a path over an existing station interval and store derivatives up to second order.
281 ///
282 /// # Parameters
283 /// - `path`: geometric path to evaluate at the stored station samples.
284 /// - `idx_s_from`: global start station id (inclusive).
285 /// - `idx_s_to`: global end station id (exclusive).
286 ///
287 /// # Behavior
288 /// - Reads the station samples already stored in `[idx_s_from, idx_s_to)`.
289 /// - Evaluates `q`, `dq`, and `ddq` from `path` at those samples.
290 /// - Copies the evaluated derivatives into the robot's constraint storage.
291 /// - Clears third-order derivative data over the sampled interval.
292 /// - Does not store a borrow of `path`; the path may be dropped after this call.
293 ///
294 /// # Errors
295 /// - [`CoppError::ConstraintError`](crate::diag::CoppError::ConstraintError) if the station interval is empty, out of
296 /// bounds, or the evaluated path dimension does not match the robot dimension.
297 /// - [`CoppError::PathError`](crate::diag::CoppError::PathError) if path evaluation fails, for example because a
298 /// stored station is outside the path range.
299 ///
300 /// # Returns
301 /// Returns `&mut Self` for chaining on success.
302 #[inline(always)]
303 pub fn with_q_from_path_2nd(
304 &mut self,
305 path: &Path,
306 idx_s_from: usize,
307 idx_s_to: usize,
308 ) -> Result<&mut Self, CoppError> {
309 self.constraints
310 .with_q_from_path_2nd(path, idx_s_from, idx_s_to)?;
311 Ok(self)
312 }
313
314 /// Sample a path over an existing station interval and store derivatives up to third order.
315 ///
316 /// # Parameters
317 /// - `path`: geometric path to evaluate at the stored station samples.
318 /// - `idx_s_from`: global start station id (inclusive).
319 /// - `idx_s_to`: global end station id (exclusive).
320 ///
321 /// # Behavior
322 /// - Reads the station samples already stored in `[idx_s_from, idx_s_to)`.
323 /// - Evaluates `q`, `dq`, `ddq`, and `dddq` from `path` at those samples.
324 /// - Copies the evaluated derivatives into the robot's constraint storage.
325 /// - Does not store a borrow of `path`; the path may be dropped after this call.
326 ///
327 /// # Errors
328 /// - [`CoppError::ConstraintError`](crate::diag::CoppError::ConstraintError) if the station interval is empty, out of
329 /// bounds, or the evaluated path dimension does not match the robot dimension.
330 /// - [`CoppError::PathError`](crate::diag::CoppError::PathError) if path evaluation fails, for example because a
331 /// stored station is outside the path range.
332 ///
333 /// # Returns
334 /// Returns `&mut Self` for chaining on success.
335 #[inline(always)]
336 pub fn with_q_from_path_3rd(
337 &mut self,
338 path: &Path,
339 idx_s_from: usize,
340 idx_s_to: usize,
341 ) -> Result<&mut Self, CoppError> {
342 self.constraints
343 .with_q_from_path_3rd(path, idx_s_from, idx_s_to)?;
344 Ok(self)
345 }
346
347 /// Add axial velocity limits on interval starting at `start_idx_s`.
348 ///
349 /// # Input semantics
350 /// Enforces per-axis bounds:
351 /// `axial_velocity_min < \dot{q} < axial_velocity_max`.
352 ///
353 /// # Mapping
354 /// Converts velocity bounds into first-order path-speed limits on
355 /// `a = \dot{s}^2`, then fuses into `amax`.
356 ///
357 /// # Errors
358 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
359 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
360 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo)
361 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds) when max/min signs are invalid
362 ///
363 /// # Returns
364 /// Returns `&mut Self` for chaining on success.
365 pub fn with_axial_velocity<T1, T2>(
366 &mut self,
367 axial_velocity_max: T1,
368 axial_velocity_min: T2,
369 start_idx_s: usize,
370 ) -> Result<&mut Self, ConstraintError>
371 where
372 T1: UpperBound,
373 T2: UpperBound,
374 {
375 // Check dimensions
376 if !axial_velocity_max.check_valid(self.dim())
377 || !axial_velocity_min.check_valid(self.dim())
378 || axial_velocity_max.ncols() != axial_velocity_min.ncols()
379 {
380 return Err(ConstraintError::NoMatchDimensions);
381 }
382 // Check bounds
383 self.constraints
384 .check_s_in_bounds(start_idx_s, axial_velocity_max.ncols())?;
385 // Check given dq
386 if !self
387 .constraints
388 .check_given_q(start_idx_s, start_idx_s + axial_velocity_max.ncols())
389 {
390 return Err(ConstraintError::NoGivenQInfo);
391 }
392 if axial_velocity_max.ncols() == 0 {
393 return Ok(self);
394 }
395 let axial_velocity_max = axial_velocity_max.as_matrix();
396 let axial_velocity_min = axial_velocity_min.as_matrix();
397 Self::check_strict_signed_limits(
398 &axial_velocity_max,
399 &axial_velocity_min,
400 "axial_velocity",
401 )?;
402 // Add new axial velocity constraints
403 let mut amax_new =
404 DMatrix::<f64>::from_element(self.dim(), axial_velocity_max.ncols(), f64::INFINITY);
405 let func = |start_idx: usize, ncols: usize, offset: usize| {
406 let amax_ = self.constraints.dq.columns(start_idx, ncols).zip_zip_map(
407 &axial_velocity_max.columns(offset, ncols),
408 &axial_velocity_min.columns(offset, ncols),
409 |dq, vmax, vmin| {
410 if dq > 0.0 {
411 (vmax / dq).powi(2)
412 } else if dq < 0.0 {
413 (vmin / dq).powi(2)
414 } else {
415 f64::INFINITY
416 }
417 },
418 );
419 amax_new.columns_mut(offset, ncols).copy_from(&amax_);
420 };
421 let ncols_mat = self.constraints.capacity();
422 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
423 Constraints::circular_process(ncols_mat, start_idx, axial_velocity_max.ncols(), func);
424
425 self.constraints
426 .with_constraint_1order(&amax_new.as_view(), start_idx_s)?;
427
428 Ok(self)
429 }
430
431 /// Add axial acceleration limits on interval starting at `start_idx_s`.
432 ///
433 /// # Input semantics
434 /// Enforces per-axis bounds:
435 /// `axial_acceleration_min < \ddot{q} < axial_acceleration_max`.
436 ///
437 /// # Mapping
438 /// Generates second-order rows:
439 /// `acc_a * a + acc_b * b <= acc_max`,
440 /// where `(a,b)` are path-speed variables.
441 ///
442 /// # Errors
443 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
444 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
445 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo)
446 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds)
447 ///
448 /// # Returns
449 /// Returns `&mut Self` for chaining on success.
450 pub fn with_axial_acceleration<T1, T2>(
451 &mut self,
452 axial_acceleration_max: T1,
453 axial_acceleration_min: T2,
454 start_idx_s: usize,
455 ) -> Result<&mut Self, ConstraintError>
456 where
457 T1: UpperBound,
458 T2: UpperBound,
459 {
460 // Check dimensions
461 if !axial_acceleration_max.check_valid(self.dim())
462 || !axial_acceleration_min.check_valid(self.dim())
463 || axial_acceleration_max.ncols() != axial_acceleration_min.ncols()
464 {
465 return Err(ConstraintError::NoMatchDimensions);
466 }
467 // Check bounds
468 self.constraints
469 .check_s_in_bounds(start_idx_s, axial_acceleration_max.ncols())?;
470 // Check given dq, ddq
471 if !self
472 .constraints
473 .check_given_q(start_idx_s, start_idx_s + axial_acceleration_max.ncols())
474 {
475 return Err(ConstraintError::NoGivenQInfo);
476 }
477 if axial_acceleration_max.ncols() == 0 {
478 return Ok(self);
479 }
480 let axial_acceleration_max = axial_acceleration_max.as_matrix();
481 let axial_acceleration_min = axial_acceleration_min.as_matrix();
482 Self::check_strict_signed_limits(
483 &axial_acceleration_max,
484 &axial_acceleration_min,
485 "axial_acceleration",
486 )?;
487 // Add new axial acceleration constraints
488 let mut acc_a_new = DMatrix::<f64>::zeros(self.dim(), axial_acceleration_max.ncols());
489 let mut acc_b_new = DMatrix::<f64>::zeros(self.dim(), axial_acceleration_max.ncols());
490 let func = |start_idx: usize, ncols: usize, offset: usize| {
491 acc_a_new
492 .columns_mut(offset, ncols)
493 .copy_from(&self.constraints.ddq.columns(start_idx, ncols));
494 acc_b_new
495 .columns_mut(offset, ncols)
496 .copy_from(&self.constraints.dq.columns(start_idx, ncols));
497 };
498 let ncols_mat = self.constraints.capacity();
499 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
500 Constraints::circular_process(ncols_mat, start_idx, axial_acceleration_max.ncols(), func);
501 self.constraints
502 .with_constraint_2order(
503 &acc_a_new.as_view(),
504 &acc_b_new.as_view(),
505 &axial_acceleration_max.as_view(),
506 start_idx_s,
507 false,
508 )?
509 .with_constraint_2order(
510 &acc_a_new.as_view(),
511 &acc_b_new.as_view(),
512 &axial_acceleration_min.as_view(),
513 start_idx_s,
514 true,
515 )?;
516
517 Ok(self)
518 }
519
520 /// Add axial jerk limits on interval starting at `start_idx_s`.
521 ///
522 /// # Input semantics
523 /// Enforces per-axis bounds:
524 /// `axial_jerk_min < \dddot{q} < axial_jerk_max`.
525 ///
526 /// # Mapping
527 /// Generates third-order rows used by TOPP3/COPP3:
528 /// `sqrt(a) * (jerk_a*a + jerk_b*b + jerk_c*c + jerk_d) <= jerk_max`.
529 ///
530 /// # Errors
531 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
532 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
533 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo) (needs `q/dq/ddq/dddq`)
534 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds)
535 ///
536 /// # Returns
537 /// Returns `&mut Self` for chaining on success.
538 pub fn with_axial_jerk<T1, T2>(
539 &mut self,
540 axial_jerk_max: T1,
541 axial_jerk_min: T2,
542 start_idx_s: usize,
543 ) -> Result<&mut Self, ConstraintError>
544 where
545 T1: UpperBound,
546 T2: UpperBound,
547 {
548 // Check dimensions
549 if !axial_jerk_max.check_valid(self.dim())
550 || !axial_jerk_min.check_valid(self.dim())
551 || axial_jerk_max.ncols() != axial_jerk_min.ncols()
552 {
553 return Err(ConstraintError::NoMatchDimensions);
554 }
555 // Check bounds
556 self.constraints
557 .check_s_in_bounds(start_idx_s, axial_jerk_max.ncols())?;
558 // Check given dq, ddq, dddq
559 if !self
560 .constraints
561 .check_given_q(start_idx_s, start_idx_s + axial_jerk_max.ncols())
562 || !self
563 .constraints
564 .check_given_dddq(start_idx_s, start_idx_s + axial_jerk_max.ncols())
565 {
566 return Err(ConstraintError::NoGivenQInfo);
567 }
568 if axial_jerk_max.ncols() == 0 {
569 return Ok(self);
570 }
571 let axial_jerk_max = axial_jerk_max.as_matrix();
572 let axial_jerk_min = axial_jerk_min.as_matrix();
573 Self::check_strict_signed_limits(&axial_jerk_max, &axial_jerk_min, "axial_jerk")?;
574 // Add new axial jerk constraints
575 let mut jerk_a_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
576 let mut jerk_b_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
577 let mut jerk_c_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
578 let jerk_d_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
579 let func = |start_idx: usize, ncols: usize, offset: usize| {
580 jerk_a_new
581 .columns_mut(offset, ncols)
582 .copy_from(&self.constraints.dddq.columns(start_idx, ncols));
583 jerk_b_new
584 .columns_mut(offset, ncols)
585 .copy_from(&self.constraints.ddq.columns(start_idx, ncols));
586 jerk_c_new
587 .columns_mut(offset, ncols)
588 .copy_from(&self.constraints.dq.columns(start_idx, ncols));
589 };
590 let ncols_mat = self.constraints.capacity();
591 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
592 Constraints::circular_process(ncols_mat, start_idx, axial_jerk_max.ncols(), func);
593 jerk_b_new.scale_mut(3.0);
594 self.constraints
595 .with_constraint_3order(
596 &jerk_a_new.as_view(),
597 &jerk_b_new.as_view(),
598 &jerk_c_new.as_view(),
599 &jerk_d_new.as_view(),
600 &axial_jerk_max,
601 start_idx_s,
602 false,
603 )?
604 .with_constraint_3order(
605 &jerk_a_new.as_view(),
606 &jerk_b_new.as_view(),
607 &jerk_c_new.as_view(),
608 &jerk_d_new.as_view(),
609 &axial_jerk_min,
610 start_idx_s,
611 true,
612 )?;
613
614 Ok(self)
615 }
616}
617
618/// Robot trait with inverse-dynamics capability.
619///
620/// This trait is mainly required when building COPP2/COPP3 problems with
621/// torque/dynamics constraints. For TOPP-only use cases, a direct
622/// [`Constraints`](crate::constraints::Constraints) workflow is usually
623/// enough.
624///
625/// A `usize` variable can serve as a trivial [`RobotTorque`](crate::robot::RobotTorque) implementation representing a point-mass model, where `tau = ddq`. For physical robots, users should implement this trait with their own inverse dynamics.
626pub trait RobotTorque: RobotBasic {
627 /// Evaluate inverse dynamics.
628 ///
629 /// `tau = M(q) * ddq + C(q, dq) * dq + g(q) + f(q, sgn(dq))`
630 ///
631 /// For a robot with dimension `dim`:
632 /// - `q` joint positions (`dim`).
633 /// - `dq`: joint velocities (`dim`).
634 /// - `ddq`: joint accelerations (`dim`).
635 /// - `tau`: output required torques/forces (`dim`).
636 /// - `dq`, `ddq`, and `tau` are vectors in `R^dim`.
637 /// - `M(q)` is the inertia/mass matrix in `R^(dim x dim)`.
638 /// - `C(q, dq)` is the Coriolis/centrifugal matrix in `R^(dim x dim)`.
639 /// It must satisfy `C(q, lambda * dq) = lambda * C(q, dq)` for any
640 /// scalar `lambda`.
641 /// - `g(q)` is the gravity torque/force vector in `R^dim`.
642 /// - `f(q, sgn(dq))` is the dry-friction torque/force vector in `R^dim`,
643 /// where `sgn(dq)` is interpreted element-wise. It is required that
644 /// `f(q, sgn(dq))` is zero if `dq` is zero. An common dry-friction model
645 /// is biased Coulomb friction.
646 ///
647 /// **Important:** when implementing the sign function `sgn(dq)`,
648 /// zero and near-zero velocities must map to `0`, not to `+1` or `-1`.
649 /// A recommended convention is to treat `abs(dq[i]) <= 1e-16` as zero.
650 ///
651 /// On success, write every entry of `tau` and return `Ok(())`.
652 /// The `tau` buffer may contain old values on entry; implementations
653 /// should overwrite it directly rather than reading or accumulating into
654 /// existing contents. If the dynamics backend cannot evaluate this state,
655 /// return `Err(RobotDynamicsError::new(message))` or `Err(message.into())`
656 /// with a user-facing reason.
657 fn inverse_dynamics(
658 &self,
659 q: &[f64],
660 dq: &[f64],
661 ddq: &[f64],
662 tau: &mut [f64],
663 ) -> Result<(), RobotDynamicsError>;
664}
665
666impl<M: RobotTorque> Robot<M> {
667 #[inline]
668 fn inverse_dynamics_with_context(
669 &self,
670 idx_s: usize,
671 call: &str,
672 q: &[f64],
673 dq: &[f64],
674 ddq: &[f64],
675 tau: &mut [f64],
676 ) -> Result<(), RobotDynamicsError> {
677 self.model
678 .inverse_dynamics(q, dq, ddq, tau)
679 .map_err(|error| {
680 RobotDynamicsError::new(format!(
681 "inverse_dynamics failed at idx_s={idx_s} during {call}: {error}"
682 ))
683 })?;
684 if let Some((index, value)) = tau
685 .iter()
686 .copied()
687 .enumerate()
688 .find(|(_, value)| !value.is_finite())
689 {
690 return Err(RobotDynamicsError::new(format!(
691 "inverse_dynamics returned non-finite tau[{index}] = {value} at idx_s={idx_s} during {call}"
692 )));
693 }
694 Ok(())
695 }
696
697 /// Compute torque profile from path-domain `(a,b)` samples.
698 ///
699 /// # Notes
700 /// This is a test helper used to evaluate dynamic feasibility of a profile.
701 ///
702 /// # Errors
703 /// Returns shape/range/data-availability errors when prerequisites are not met, and
704 /// propagates inverse-dynamics failures from the robot model.
705 pub(crate) fn get_torque_with_ab(
706 &self,
707 a_profile: &[f64],
708 b_profile: &[f64],
709 start_idx_s: usize,
710 ) -> Result<DMatrix<f64>, CoppError> {
711 if a_profile.len() != b_profile.len() {
712 return Err(ConstraintError::NoMatchDimensions.into());
713 }
714 if a_profile.is_empty() {
715 return Ok(DMatrix::zeros(self.dim(), 0));
716 }
717 self.constraints
718 .check_s_in_bounds(start_idx_s, a_profile.len())?;
719 if !self
720 .constraints
721 .check_given_q(start_idx_s, start_idx_s + a_profile.len())
722 {
723 return Err(ConstraintError::NoGivenQInfo.into());
724 }
725 let (mut coeff_a, mut coeff_b, mut coeff_g) =
726 self.torque_coeff(start_idx_s, a_profile.len())?;
727 for (mut coeff_a_col, &a_curr) in coeff_a.column_iter_mut().zip(a_profile.iter()) {
728 coeff_a_col.scale_mut(a_curr);
729 }
730 for (mut coeff_b_col, &b_curr) in coeff_b.column_iter_mut().zip(b_profile.iter()) {
731 coeff_b_col.scale_mut(b_curr);
732 }
733 coeff_g += coeff_a;
734 coeff_g += coeff_b;
735 Ok(coeff_g)
736 }
737
738 /// Build affine torque coefficients in path variables `(a,b)`.
739 ///
740 /// # Output
741 /// Returns `(coeff_a, coeff_b, coeff_g)` such that
742 /// `tau = coeff_a * a + coeff_b * b + coeff_g` column-wise.
743 ///
744 /// # Shape
745 /// Each returned matrix has shape `(dim, ncols)`.
746 ///
747 /// # Preconditions
748 /// Caller ensures target station interval is available.
749 #[allow(clippy::type_complexity)]
750 pub(crate) fn torque_coeff(
751 &self,
752 start_idx_s: usize,
753 ncols: usize,
754 ) -> Result<(DMatrix<f64>, DMatrix<f64>, DMatrix<f64>), RobotDynamicsError> {
755 let mut coeff_a = DMatrix::<f64>::zeros(self.dim(), ncols);
756 let mut coeff_b = DMatrix::<f64>::zeros(self.dim(), ncols);
757 let mut coeff_g = DMatrix::<f64>::zeros(self.dim(), ncols);
758 if ncols == 0 {
759 return Ok((coeff_a, coeff_b, coeff_g));
760 }
761 let mut dq_sqrt2 = vec![0.0; self.dim()];
762 let mut ddq_2 = vec![0.0; self.dim()];
763 let vec_zero_dim = vec![0.0; self.dim()];
764
765 let mut process_segment =
766 |start_idx: usize, ncols: usize, offset: usize| -> Result<(), RobotDynamicsError> {
767 for (local_col, (((((mut a, mut b), mut g), q), dq), ddq)) in coeff_a
768 .columns_mut(offset, ncols)
769 .column_iter_mut()
770 .zip(coeff_b.columns_mut(offset, ncols).column_iter_mut())
771 .zip(coeff_g.columns_mut(offset, ncols).column_iter_mut())
772 .zip(self.constraints.q.columns(start_idx, ncols).column_iter())
773 .zip(self.constraints.dq.columns(start_idx, ncols).column_iter())
774 .zip(self.constraints.ddq.columns(start_idx, ncols).column_iter())
775 .enumerate()
776 {
777 let idx_s = start_idx_s + offset + local_col;
778 let q_slice = q.as_slice();
779 let dq_slice = dq.as_slice();
780 let ddq_slice = ddq.as_slice();
781 // tau(q, dq/dt, ddq/ddt) = M(q) * ddq/ddt + C(q, dq/dt) * dq/dt + g(q) + f(g, sgn(q)).
782 // Then, we have the following solutions:
783 // (1) tau(q, 0, 0) = g
784 // (2) tau(q, 0, dq) = M * dq + g
785 // (3) tau(q, dq, ddq) = M * ddq + C * dq + g + f
786 // (4) tau(q, sqrt(2) * dq, 2 * ddq) = 2 * M * ddq + 2 * C * dq + g + f
787
788 // tau = M(q) * (ddq/dds * a + dq/ds * b) + C(q, dq/ds * sqrt(a)) * dq/ds * sqrt(a) + g(q) + f(g, sgn(q))
789 // tau = (M * ddq/dds + C * dq/ds) * a + M * dq/ds * b + g(q) + f(g, sgn(q))
790 // Therefore, the answer is:
791 // - coeff_b = M * dq = tau(q, 0, dq) - tau(q, 0, 0)
792 // - coeff_a = M * ddq + C * dq = tau(q, sqrt(2) * dq, 2 * ddq) - tau(q, dq, ddq)
793 // - coeff_g = g + f = tau(q, dq, ddq) - coeff_a
794
795 // Step 1. Compute coeff_b = tau(q, 0, dq) - tau(q, 0, 0)
796 // now g = tau(q, 0, 0)
797 self.inverse_dynamics_with_context(
798 idx_s,
799 "tau(q, 0, 0)",
800 q_slice,
801 &vec_zero_dim,
802 &vec_zero_dim,
803 g.as_mut_slice(),
804 )?;
805 // now b = tau(q, 0, dq)
806 self.inverse_dynamics_with_context(
807 idx_s,
808 "tau(q, 0, dq)",
809 q_slice,
810 &vec_zero_dim,
811 dq_slice,
812 b.as_mut_slice(),
813 )?;
814 // now b = tau(q, 0, dq) - tau(q, 0, 0)
815 b.iter_mut()
816 .zip(g.iter())
817 .for_each(|(b_i, &g_i)| *b_i -= g_i);
818
819 // Step 2. coeff_a = tau(q, sqrt(2) * dq, 2 * ddq) - tau(q, dq, ddq)
820 dq_sqrt2
821 .iter_mut()
822 .zip(dq.iter())
823 .for_each(|(dq_sqrt2_i, &dq_i)| *dq_sqrt2_i = SQRT_2 * dq_i);
824 ddq_2
825 .iter_mut()
826 .zip(ddq.iter())
827 .for_each(|(ddq_2_i, &ddq_i)| *ddq_2_i = 2.0 * ddq_i);
828 // now a = tau(q, sqrt(2) * dq, 2 * ddq)
829 self.inverse_dynamics_with_context(
830 idx_s,
831 "tau(q, sqrt(2) * dq, 2 * ddq)",
832 q_slice,
833 dq_sqrt2.as_slice(),
834 ddq_2.as_slice(),
835 a.as_mut_slice(),
836 )?;
837 // now g = tau(q, dq, ddq)
838 self.inverse_dynamics_with_context(
839 idx_s,
840 "tau(q, dq, ddq)",
841 q_slice,
842 dq_slice,
843 ddq_slice,
844 g.as_mut_slice(),
845 )?;
846 // now a = tau(q, sqrt(2) * dq, 2 * ddq) - tau(q, dq, ddq)
847 a.iter_mut()
848 .zip(g.iter())
849 .for_each(|(a_i, &g_i)| *a_i -= g_i);
850
851 // Step 3. coeff_g = tau(q, dq, ddq) - coeff_a
852 g.iter_mut()
853 .zip(a.iter())
854 .for_each(|(g_i, &a_i)| *g_i -= a_i);
855 }
856 Ok(())
857 };
858 let ncols_mat = self.constraints.capacity();
859 if ncols_mat - start_idx_s >= ncols {
860 process_segment(start_idx_s, ncols, 0)?;
861 } else {
862 let len_first = ncols_mat - start_idx_s;
863 process_segment(start_idx_s, len_first, 0)?;
864 let len_second = ncols - len_first;
865 process_segment(0, len_second, len_first)?;
866 }
867 Ok((coeff_a, coeff_b, coeff_g))
868 }
869
870 /// Build edge-coupled affine torque coefficients over `a[k], a[k+1]`.
871 ///
872 /// # Output
873 /// Returns `(coeff_a_curr, coeff_a_next, coeff_g)` such that
874 /// `tau[k] = coeff_a_curr * a[k] + coeff_a_next * a[k+1] + coeff_g`.
875 ///
876 /// # Shape
877 /// Each returned matrix has shape `(dim, ncols)`.
878 ///
879 /// # Preconditions
880 /// Requires station window `[start_idx_s, start_idx_s + ncols]` to be valid.
881 #[allow(clippy::type_complexity)]
882 pub(crate) fn torque2_coeff_a(
883 &self,
884 start_idx_s: usize,
885 ncols: usize,
886 ) -> Result<(DMatrix<f64>, DMatrix<f64>, DMatrix<f64>), RobotDynamicsError> {
887 let s = self
888 .constraints
889 .s_vec(start_idx_s, start_idx_s + ncols + 1)
890 .expect("torque2_coeff_a: s interval must be in bounds");
891 let ds_double_down = s
892 .windows(2)
893 .map(|s_pair| 0.5 / (s_pair[1] - s_pair[0]))
894 .collect::<Vec<f64>>();
895
896 // tau[k] = coeff_a * a[k] + coeff_b * b[k] + coeff_g
897 let (mut coeff_a, mut coeff_b, coeff_g) = self.torque_coeff(start_idx_s, ncols)?;
898 // tau[k] = coeff_a * a[k] + coeff_b * (a[k+1] - a[k]) * ds_double_down + coeff_g
899
900 // tau[k] = coeff_a * a[k] + coeff_b * (a[k+1] - a[k]) + coeff_g
901 for (mut v_b, &ds_double_down) in coeff_b.column_iter_mut().zip(ds_double_down.iter()) {
902 v_b.scale_mut(ds_double_down);
903 }
904 // tau[k] = (coeff_a - coeff_b) * a[k] + coeff_b * a[k+1] + coeff_g
905
906 // tau[k] = coeff_a * a[k] + coeff_b * a[k+1] + coeff_g
907 coeff_a -= &coeff_b;
908
909 Ok((coeff_a, coeff_b, coeff_g))
910 }
911
912 /// Add axial torque limits on interval starting at `start_idx_s`.
913 ///
914 /// # Input semantics
915 /// Enforces per-axis bounds:
916 /// `axial_torque_min < tau < axial_torque_max`.
917 ///
918 /// # Mapping
919 /// Using inverse dynamics, torque limits are transformed into second-order
920 /// rows on `(a,b)` and appended to the constraint buffer.
921 ///
922 /// # Errors
923 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
924 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
925 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo)
926 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds)
927 /// - [`RobotDynamicsError`](crate::diag::RobotDynamicsError) if inverse dynamics fails
928 ///
929 /// # Returns
930 /// Returns `&mut Self` for chaining on success.
931 pub fn with_axial_torque<T1, T2>(
932 &mut self,
933 axial_torque_max: T1,
934 axial_torque_min: T2,
935 start_idx_s: usize,
936 ) -> Result<&mut Self, CoppError>
937 where
938 T1: UpperBound,
939 T2: UpperBound,
940 {
941 // Check dimensions
942 if !axial_torque_max.check_valid(self.dim())
943 || !axial_torque_min.check_valid(self.dim())
944 || axial_torque_max.ncols() != axial_torque_min.ncols()
945 {
946 return Err(ConstraintError::NoMatchDimensions.into());
947 }
948 // Check bounds
949 self.constraints
950 .check_s_in_bounds(start_idx_s, axial_torque_max.ncols())?;
951 // Check given dq
952 if !self
953 .constraints
954 .check_given_q(start_idx_s, start_idx_s + axial_torque_max.ncols())
955 {
956 return Err(ConstraintError::NoGivenQInfo.into());
957 }
958 if axial_torque_max.ncols() == 0 {
959 return Ok(self);
960 }
961 let axial_torque_max = axial_torque_max.as_matrix();
962 let axial_torque_min = axial_torque_min.as_matrix();
963 Self::check_strict_signed_limits(&axial_torque_max, &axial_torque_min, "axial_torque")?;
964
965 // torque_min <= tau = coeff_a * a + coeff_b * b + coeff_g <= torque_max
966 let (coeff_a, coeff_b, coeff_g) =
967 self.torque_coeff(start_idx_s, axial_torque_max.ncols())?;
968 // coeff_a * a + coeff_b * b <= torque_max - coeff_g
969 // torque_min - coeff_g <= coeff_a * a + coeff_b * b
970 self.constraints
971 .with_constraint_2order(
972 &coeff_a.as_view(),
973 &coeff_b.as_view(),
974 &(axial_torque_max - &coeff_g).as_view(),
975 start_idx_s,
976 false,
977 )?
978 .with_constraint_2order(
979 &coeff_a.as_view(),
980 &coeff_b.as_view(),
981 &(axial_torque_min - coeff_g).as_view(),
982 start_idx_s,
983 true,
984 )?;
985
986 Ok(self)
987 }
988}
989
990impl RobotTorque for usize {
991 /// Evaluate inverse dynamics for point-mass model.
992 ///
993 /// Since `tau = ddq`, this function copies `ddq` directly into `tau`.
994 #[inline(always)]
995 fn inverse_dynamics(
996 &self,
997 _q: &[f64],
998 _dq: &[f64],
999 ddq: &[f64],
1000 tau: &mut [f64],
1001 ) -> Result<(), RobotDynamicsError> {
1002 tau.copy_from_slice(ddq);
1003 Ok(())
1004 }
1005}