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`).
148///
149/// # Example
150/// The example below builds a two-dimensional point-mass robot, writes a station
151/// grid and path derivatives, then adds velocity, acceleration, and jerk
152/// constraints in one chain.
153///
154/// ```rust
155/// # fn main() -> Result<(), copp::diag::CoppError> {
156/// use copp::robot::Robot;
157/// use nalgebra::DMatrix;
158///
159/// let mut robot = Robot::with_capacity(2usize, 3);
160/// let s = [0.0, 0.5, 1.0];
161///
162/// let q = DMatrix::from_row_slice(
163/// 2,
164/// 3,
165/// &[
166/// 0.0, 0.5, 1.0,
167/// 1.0, 0.5, 0.0,
168/// ],
169/// );
170/// let dq = DMatrix::from_row_slice(
171/// 2,
172/// 3,
173/// &[
174/// 1.0, 1.0, 1.0,
175/// -1.0, -1.0, -1.0,
176/// ],
177/// );
178/// let ddq = DMatrix::zeros(2, 3);
179/// let dddq = DMatrix::zeros(2, 3);
180/// let dddq_view = dddq.as_view();
181///
182/// let vel_max = [2.0, 2.0];
183/// let vel_min = [-2.0, -2.0];
184/// let acc_max = [3.0, 3.0];
185/// let acc_min = [-3.0, -3.0];
186/// let jerk_max = [10.0, 10.0];
187/// let jerk_min = [-10.0, -10.0];
188///
189/// robot
190/// .with_s(s.as_slice())?
191/// .with_q(&q.as_view(), &dq.as_view(), &ddq.as_view(), Some(&dddq_view), 0)?
192/// .with_axial_velocity((vel_max.as_slice(), s.len()), (vel_min.as_slice(), s.len()), 0)?
193/// .with_axial_acceleration((acc_max.as_slice(), s.len()), (acc_min.as_slice(), s.len()), 0)?
194/// .with_axial_jerk((jerk_max.as_slice(), s.len()), (jerk_min.as_slice(), s.len()), 0)?;
195///
196/// assert_eq!(robot.constraints.len(), 3);
197/// # Ok(())
198/// # }
199/// ```
200pub struct Robot<M: RobotBasic> {
201 /// Concrete robot model implementation.
202 model: M,
203
204 /// Shared station-indexed constraint buffer used by TOPP/COPP solvers.
205 pub constraints: Constraints,
206}
207
208impl<M: RobotBasic> Robot<M> {
209 /// Access the robot model `M: RobotBasic` as a reference.
210 #[inline(always)]
211 pub fn model(&self) -> &M {
212 &self.model
213 }
214
215 /// Mutably access the robot model `M: RobotBasic`.
216 #[inline(always)]
217 pub fn model_mut(&mut self) -> &mut M {
218 &mut self.model
219 }
220
221 /// Enforce strict signed contract for upper/lower bounds.
222 ///
223 /// # Contract
224 /// For every element in the provided matrices:
225 /// - `upper > 0`
226 /// - `lower < 0`
227 ///
228 /// # Errors
229 /// Returns [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds) when contract is violated.
230 #[inline(always)]
231 fn check_strict_signed_limits(
232 upper: &InputMatrix,
233 lower: &InputMatrix,
234 bound_name: &'static str,
235 ) -> Result<(), ConstraintError> {
236 let upper_valid = upper.iter().all(|&u| u > 0.0);
237 let lower_valid = lower.iter().all(|&l| l < 0.0);
238 if upper_valid && lower_valid {
239 Ok(())
240 } else {
241 Err(ConstraintError::InvalidSignedBounds { bound_name })
242 }
243 }
244
245 /// Construct a robot wrapper with default constraint-buffer capacity.
246 ///
247 /// # Parameters
248 /// - `model`: concrete robot model implementing [`RobotBasic`](crate::robot::RobotBasic).
249 pub fn new(model: M) -> Self {
250 let dim = model.dim();
251 Self {
252 model,
253 constraints: Constraints::new(dim),
254 }
255 }
256
257 /// Construct a robot wrapper with explicit initial constraint capacity.
258 ///
259 /// # Parameters
260 /// - `model`: concrete robot model.
261 /// - `capacity`: initial circular-buffer column capacity.
262 pub fn with_capacity(model: M, capacity: usize) -> Self {
263 let dim = model.dim();
264 Self {
265 model,
266 constraints: Constraints::with_capacity(dim, capacity),
267 }
268 }
269
270 /// Get robot dimension / DoF.
271 #[inline(always)]
272 pub fn dim(&self) -> usize {
273 self.constraints.dim()
274 }
275
276 /// Append a new station segment into the internal constraint buffer.
277 ///
278 /// This is the robot-level convenience wrapper for
279 /// [`Constraints::with_s`](crate::constraints::Constraints::with_s). Use
280 /// the lower-level method directly when constructing
281 /// [`Constraints`](crate::constraints::Constraints) without a robot model.
282 ///
283 /// # Parameters
284 /// - `s_new`: station samples accepted as 1D slice or matrix view.
285 ///
286 /// # Errors
287 /// - [`ConstraintError::NonIncreasingS`](crate::diag::ConstraintError::NonIncreasingS) if `s_new` is not strictly
288 /// increasing, or if its first sample does not come after the current
289 /// last stored station.
290 ///
291 /// # Returns
292 /// Returns `&mut Self` for chaining on success.
293 #[inline(always)]
294 pub fn with_s<T: AsInputMatrix1D + ?Sized>(
295 &mut self,
296 s_new: &T,
297 ) -> Result<&mut Self, ConstraintError> {
298 self.constraints.with_s(s_new)?;
299 Ok(self)
300 }
301
302 /// Write path derivatives over interval starting at `idx_s`.
303 ///
304 /// # Parameters
305 /// - `q_new`, `dq_new`, `ddq_new`: required derivative matrices.
306 /// - `dddq_new`: optional third derivative matrix.
307 /// - `idx_s`: global start station id.
308 ///
309 /// # Behavior
310 /// - If `dddq_new` is provided, third-order derivative data is written for
311 /// the target interval.
312 /// - If `dddq_new` is `None`, existing third-order derivative data is
313 /// cleared over the target interval.
314 ///
315 /// # Errors
316 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions) if derivative matrix shapes are
317 /// inconsistent with each other or with the robot dimension.
318 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds) if the target station interval is
319 /// outside the stored station range.
320 ///
321 /// # Returns
322 /// Returns `&mut Self` for chaining on success.
323 #[inline(always)]
324 pub fn with_q(
325 &mut self,
326 q_new: &InputMatrix,
327 dq_new: &InputMatrix,
328 ddq_new: &InputMatrix,
329 dddq_new: Option<&InputMatrix>,
330 idx_s: usize,
331 ) -> Result<&mut Self, ConstraintError> {
332 self.constraints
333 .with_q(q_new, dq_new, ddq_new, dddq_new, idx_s)?;
334 Ok(self)
335 }
336
337 /// Sample a path over an existing station interval and store derivatives up to second order.
338 ///
339 /// # Parameters
340 /// - `path`: geometric path to evaluate at the stored station samples.
341 /// - `idx_s_from`: global start station id (inclusive).
342 /// - `idx_s_to`: global end station id (exclusive).
343 ///
344 /// # Behavior
345 /// - Reads the station samples already stored in `[idx_s_from, idx_s_to)`.
346 /// - Evaluates `q`, `dq`, and `ddq` from `path` at those samples.
347 /// - Copies the evaluated derivatives into the robot's constraint storage.
348 /// - Clears third-order derivative data over the sampled interval.
349 /// - Does not store a borrow of `path`; the path may be dropped after this call.
350 ///
351 /// # Errors
352 /// - [`CoppError::ConstraintError`](crate::diag::CoppError::ConstraintError) if the station interval is empty, out of
353 /// bounds, or the evaluated path dimension does not match the robot dimension.
354 /// - [`CoppError::PathError`](crate::diag::CoppError::PathError) if path evaluation fails, for example because a
355 /// stored station is outside the path range.
356 ///
357 /// # Returns
358 /// Returns `&mut Self` for chaining on success.
359 #[inline(always)]
360 pub fn with_q_from_path_2nd(
361 &mut self,
362 path: &Path,
363 idx_s_from: usize,
364 idx_s_to: usize,
365 ) -> Result<&mut Self, CoppError> {
366 self.constraints
367 .with_q_from_path_2nd(path, idx_s_from, idx_s_to)?;
368 Ok(self)
369 }
370
371 /// Sample a path over an existing station interval and store derivatives up to third order.
372 ///
373 /// # Parameters
374 /// - `path`: geometric path to evaluate at the stored station samples.
375 /// - `idx_s_from`: global start station id (inclusive).
376 /// - `idx_s_to`: global end station id (exclusive).
377 ///
378 /// # Behavior
379 /// - Reads the station samples already stored in `[idx_s_from, idx_s_to)`.
380 /// - Evaluates `q`, `dq`, `ddq`, and `dddq` from `path` at those samples.
381 /// - Copies the evaluated derivatives into the robot's constraint storage.
382 /// - Does not store a borrow of `path`; the path may be dropped after this call.
383 ///
384 /// # Errors
385 /// - [`CoppError::ConstraintError`](crate::diag::CoppError::ConstraintError) if the station interval is empty, out of
386 /// bounds, or the evaluated path dimension does not match the robot dimension.
387 /// - [`CoppError::PathError`](crate::diag::CoppError::PathError) if path evaluation fails, for example because a
388 /// stored station is outside the path range.
389 ///
390 /// # Returns
391 /// Returns `&mut Self` for chaining on success.
392 #[inline(always)]
393 pub fn with_q_from_path_3rd(
394 &mut self,
395 path: &Path,
396 idx_s_from: usize,
397 idx_s_to: usize,
398 ) -> Result<&mut Self, CoppError> {
399 self.constraints
400 .with_q_from_path_3rd(path, idx_s_from, idx_s_to)?;
401 Ok(self)
402 }
403
404 /// Add axial velocity limits on interval starting at `start_idx_s`.
405 ///
406 /// # Input semantics
407 /// Enforces per-axis bounds:
408 /// `axial_velocity_min < \dot{q} < axial_velocity_max`.
409 ///
410 /// # Mapping
411 /// Converts velocity bounds into first-order path-speed limits on
412 /// `a = \dot{s}^2`, then fuses into `amax`.
413 ///
414 /// # Errors
415 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
416 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
417 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo)
418 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds) when max/min signs are invalid
419 ///
420 /// # Returns
421 /// Returns `&mut Self` for chaining on success.
422 pub fn with_axial_velocity<T1, T2>(
423 &mut self,
424 axial_velocity_max: T1,
425 axial_velocity_min: T2,
426 start_idx_s: usize,
427 ) -> Result<&mut Self, ConstraintError>
428 where
429 T1: UpperBound,
430 T2: UpperBound,
431 {
432 // Check dimensions
433 if !axial_velocity_max.check_valid(self.dim())
434 || !axial_velocity_min.check_valid(self.dim())
435 || axial_velocity_max.ncols() != axial_velocity_min.ncols()
436 {
437 return Err(ConstraintError::NoMatchDimensions);
438 }
439 // Check bounds
440 self.constraints
441 .check_s_in_bounds(start_idx_s, axial_velocity_max.ncols())?;
442 // Check given dq
443 if !self
444 .constraints
445 .check_given_q(start_idx_s, start_idx_s + axial_velocity_max.ncols())
446 {
447 return Err(ConstraintError::NoGivenQInfo);
448 }
449 if axial_velocity_max.ncols() == 0 {
450 return Ok(self);
451 }
452 let axial_velocity_max = axial_velocity_max.as_matrix();
453 let axial_velocity_min = axial_velocity_min.as_matrix();
454 Self::check_strict_signed_limits(
455 &axial_velocity_max,
456 &axial_velocity_min,
457 "axial_velocity",
458 )?;
459 // Add new axial velocity constraints
460 let mut amax_new =
461 DMatrix::<f64>::from_element(self.dim(), axial_velocity_max.ncols(), f64::INFINITY);
462 let func = |start_idx: usize, ncols: usize, offset: usize| {
463 let amax_ = self.constraints.dq.columns(start_idx, ncols).zip_zip_map(
464 &axial_velocity_max.columns(offset, ncols),
465 &axial_velocity_min.columns(offset, ncols),
466 |dq, vmax, vmin| {
467 if dq > 0.0 {
468 (vmax / dq).powi(2)
469 } else if dq < 0.0 {
470 (vmin / dq).powi(2)
471 } else {
472 f64::INFINITY
473 }
474 },
475 );
476 amax_new.columns_mut(offset, ncols).copy_from(&amax_);
477 };
478 let ncols_mat = self.constraints.capacity();
479 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
480 Constraints::circular_process(ncols_mat, start_idx, axial_velocity_max.ncols(), func);
481
482 self.constraints
483 .with_constraint_1order(&amax_new.as_view(), start_idx_s)?;
484
485 Ok(self)
486 }
487
488 /// Add axial acceleration limits on interval starting at `start_idx_s`.
489 ///
490 /// # Input semantics
491 /// Enforces per-axis bounds:
492 /// `axial_acceleration_min < \ddot{q} < axial_acceleration_max`.
493 ///
494 /// # Mapping
495 /// Generates second-order rows:
496 /// `acc_a * a + acc_b * b <= acc_max`,
497 /// where `(a,b)` are path-speed variables.
498 ///
499 /// # Errors
500 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
501 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
502 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo)
503 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds)
504 ///
505 /// # Returns
506 /// Returns `&mut Self` for chaining on success.
507 pub fn with_axial_acceleration<T1, T2>(
508 &mut self,
509 axial_acceleration_max: T1,
510 axial_acceleration_min: T2,
511 start_idx_s: usize,
512 ) -> Result<&mut Self, ConstraintError>
513 where
514 T1: UpperBound,
515 T2: UpperBound,
516 {
517 // Check dimensions
518 if !axial_acceleration_max.check_valid(self.dim())
519 || !axial_acceleration_min.check_valid(self.dim())
520 || axial_acceleration_max.ncols() != axial_acceleration_min.ncols()
521 {
522 return Err(ConstraintError::NoMatchDimensions);
523 }
524 // Check bounds
525 self.constraints
526 .check_s_in_bounds(start_idx_s, axial_acceleration_max.ncols())?;
527 // Check given dq, ddq
528 if !self
529 .constraints
530 .check_given_q(start_idx_s, start_idx_s + axial_acceleration_max.ncols())
531 {
532 return Err(ConstraintError::NoGivenQInfo);
533 }
534 if axial_acceleration_max.ncols() == 0 {
535 return Ok(self);
536 }
537 let axial_acceleration_max = axial_acceleration_max.as_matrix();
538 let axial_acceleration_min = axial_acceleration_min.as_matrix();
539 Self::check_strict_signed_limits(
540 &axial_acceleration_max,
541 &axial_acceleration_min,
542 "axial_acceleration",
543 )?;
544 // Add new axial acceleration constraints
545 let mut acc_a_new = DMatrix::<f64>::zeros(self.dim(), axial_acceleration_max.ncols());
546 let mut acc_b_new = DMatrix::<f64>::zeros(self.dim(), axial_acceleration_max.ncols());
547 let func = |start_idx: usize, ncols: usize, offset: usize| {
548 acc_a_new
549 .columns_mut(offset, ncols)
550 .copy_from(&self.constraints.ddq.columns(start_idx, ncols));
551 acc_b_new
552 .columns_mut(offset, ncols)
553 .copy_from(&self.constraints.dq.columns(start_idx, ncols));
554 };
555 let ncols_mat = self.constraints.capacity();
556 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
557 Constraints::circular_process(ncols_mat, start_idx, axial_acceleration_max.ncols(), func);
558 self.constraints
559 .with_constraint_2order(
560 &acc_a_new.as_view(),
561 &acc_b_new.as_view(),
562 &axial_acceleration_max.as_view(),
563 start_idx_s,
564 false,
565 )?
566 .with_constraint_2order(
567 &acc_a_new.as_view(),
568 &acc_b_new.as_view(),
569 &axial_acceleration_min.as_view(),
570 start_idx_s,
571 true,
572 )?;
573
574 Ok(self)
575 }
576
577 /// Add axial jerk limits on interval starting at `start_idx_s`.
578 ///
579 /// # Input semantics
580 /// Enforces per-axis bounds:
581 /// `axial_jerk_min < \dddot{q} < axial_jerk_max`.
582 ///
583 /// # Mapping
584 /// Generates third-order rows used by TOPP3/COPP3:
585 /// `sqrt(a) * (jerk_a*a + jerk_b*b + jerk_c*c + jerk_d) <= jerk_max`.
586 ///
587 /// # Errors
588 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
589 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
590 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo) (needs `q/dq/ddq/dddq`)
591 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds)
592 ///
593 /// # Returns
594 /// Returns `&mut Self` for chaining on success.
595 pub fn with_axial_jerk<T1, T2>(
596 &mut self,
597 axial_jerk_max: T1,
598 axial_jerk_min: T2,
599 start_idx_s: usize,
600 ) -> Result<&mut Self, ConstraintError>
601 where
602 T1: UpperBound,
603 T2: UpperBound,
604 {
605 // Check dimensions
606 if !axial_jerk_max.check_valid(self.dim())
607 || !axial_jerk_min.check_valid(self.dim())
608 || axial_jerk_max.ncols() != axial_jerk_min.ncols()
609 {
610 return Err(ConstraintError::NoMatchDimensions);
611 }
612 // Check bounds
613 self.constraints
614 .check_s_in_bounds(start_idx_s, axial_jerk_max.ncols())?;
615 // Check given dq, ddq, dddq
616 if !self
617 .constraints
618 .check_given_q(start_idx_s, start_idx_s + axial_jerk_max.ncols())
619 || !self
620 .constraints
621 .check_given_dddq(start_idx_s, start_idx_s + axial_jerk_max.ncols())
622 {
623 return Err(ConstraintError::NoGivenQInfo);
624 }
625 if axial_jerk_max.ncols() == 0 {
626 return Ok(self);
627 }
628 let axial_jerk_max = axial_jerk_max.as_matrix();
629 let axial_jerk_min = axial_jerk_min.as_matrix();
630 Self::check_strict_signed_limits(&axial_jerk_max, &axial_jerk_min, "axial_jerk")?;
631 // Add new axial jerk constraints
632 let mut jerk_a_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
633 let mut jerk_b_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
634 let mut jerk_c_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
635 let jerk_d_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
636 let func = |start_idx: usize, ncols: usize, offset: usize| {
637 jerk_a_new
638 .columns_mut(offset, ncols)
639 .copy_from(&self.constraints.dddq.columns(start_idx, ncols));
640 jerk_b_new
641 .columns_mut(offset, ncols)
642 .copy_from(&self.constraints.ddq.columns(start_idx, ncols));
643 jerk_c_new
644 .columns_mut(offset, ncols)
645 .copy_from(&self.constraints.dq.columns(start_idx, ncols));
646 };
647 let ncols_mat = self.constraints.capacity();
648 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
649 Constraints::circular_process(ncols_mat, start_idx, axial_jerk_max.ncols(), func);
650 jerk_b_new.scale_mut(3.0);
651 self.constraints
652 .with_constraint_3order(
653 &jerk_a_new.as_view(),
654 &jerk_b_new.as_view(),
655 &jerk_c_new.as_view(),
656 &jerk_d_new.as_view(),
657 &axial_jerk_max,
658 start_idx_s,
659 false,
660 )?
661 .with_constraint_3order(
662 &jerk_a_new.as_view(),
663 &jerk_b_new.as_view(),
664 &jerk_c_new.as_view(),
665 &jerk_d_new.as_view(),
666 &axial_jerk_min,
667 start_idx_s,
668 true,
669 )?;
670
671 Ok(self)
672 }
673}
674
675/// Robot trait with inverse-dynamics capability.
676///
677/// This trait is mainly required when building COPP2/COPP3 problems with
678/// torque/dynamics constraints. For TOPP-only use cases, a direct
679/// [`Constraints`](crate::constraints::Constraints) workflow is usually
680/// enough.
681///
682/// 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.
683///
684/// For a fuller closed-form robot implementation, see the test reference model
685/// in [robot_2dof.rs](demo/robot_2dof.rs).
686///
687/// # Example
688/// The example below uses `usize` as the built-in point-mass model for a quick
689/// inverse-dynamics smoke test.
690///
691/// ```rust
692/// # fn main() -> Result<(), copp::diag::CoppError> {
693/// use copp::robot::RobotTorque;
694///
695/// let model = 2usize;
696/// let mut tau = [0.0; 2];
697///
698/// model.inverse_dynamics(
699/// &[0.0, 0.0],
700/// &[0.0, 0.0],
701/// &[1.0, -2.0],
702/// &mut tau,
703/// )?;
704///
705/// assert_eq!(tau, [1.0, -2.0]);
706/// # Ok(())
707/// # }
708/// ```
709pub trait RobotTorque: RobotBasic {
710 /// Evaluate inverse dynamics.
711 ///
712 /// `tau = M(q) * ddq + C(q, dq) * dq + g(q) + f(q, sgn(dq))`
713 ///
714 /// For a robot with dimension `dim`:
715 /// - `q` joint positions (`dim`).
716 /// - `dq`: joint velocities (`dim`).
717 /// - `ddq`: joint accelerations (`dim`).
718 /// - `tau`: output required torques/forces (`dim`).
719 /// - `dq`, `ddq`, and `tau` are vectors in `R^dim`.
720 /// - `M(q)` is the inertia/mass matrix in `R^(dim x dim)`.
721 /// - `C(q, dq)` is the Coriolis/centrifugal matrix in `R^(dim x dim)`.
722 /// It must satisfy `C(q, lambda * dq) = lambda * C(q, dq)` for any
723 /// scalar `lambda`.
724 /// - `g(q)` is the gravity torque/force vector in `R^dim`.
725 /// - `f(q, sgn(dq))` is the dry-friction torque/force vector in `R^dim`,
726 /// where `sgn(dq)` is interpreted element-wise. It is required that
727 /// `f(q, sgn(dq))` is zero if `dq` is zero. An common dry-friction model
728 /// is biased Coulomb friction.
729 ///
730 /// **Important:** when implementing the sign function `sgn(dq)`,
731 /// zero and near-zero velocities must map to `0`, not to `+1` or `-1`.
732 /// A recommended convention is to treat `abs(dq[i]) <= 1e-16` as zero.
733 ///
734 /// On success, write every entry of `tau` and return `Ok(())`.
735 /// The `tau` buffer may contain old values on entry; implementations
736 /// should overwrite it directly rather than reading or accumulating into
737 /// existing contents. If the dynamics backend cannot evaluate this state,
738 /// return `Err(RobotDynamicsError::new(message))` or `Err(message.into())`
739 /// with a user-facing reason.
740 fn inverse_dynamics(
741 &self,
742 q: &[f64],
743 dq: &[f64],
744 ddq: &[f64],
745 tau: &mut [f64],
746 ) -> Result<(), RobotDynamicsError>;
747}
748
749impl<M: RobotTorque> Robot<M> {
750 #[inline]
751 fn inverse_dynamics_with_context(
752 &self,
753 idx_s: usize,
754 call: &str,
755 q: &[f64],
756 dq: &[f64],
757 ddq: &[f64],
758 tau: &mut [f64],
759 ) -> Result<(), RobotDynamicsError> {
760 self.model
761 .inverse_dynamics(q, dq, ddq, tau)
762 .map_err(|error| {
763 RobotDynamicsError::new(format!(
764 "inverse_dynamics failed at idx_s={idx_s} during {call}: {error}"
765 ))
766 })?;
767 if let Some((index, value)) = tau
768 .iter()
769 .copied()
770 .enumerate()
771 .find(|(_, value)| !value.is_finite())
772 {
773 return Err(RobotDynamicsError::new(format!(
774 "inverse_dynamics returned non-finite tau[{index}] = {value} at idx_s={idx_s} during {call}"
775 )));
776 }
777 Ok(())
778 }
779
780 /// Compute torque profile from path-domain `(a,b)` samples.
781 ///
782 /// # Notes
783 /// This is a test helper used to evaluate dynamic feasibility of a profile.
784 ///
785 /// # Errors
786 /// Returns shape/range/data-availability errors when prerequisites are not met, and
787 /// propagates inverse-dynamics failures from the robot model.
788 #[cfg(any(feature = "c", feature = "python", test))]
789 pub(crate) fn get_torque_with_ab(
790 &self,
791 a_profile: &[f64],
792 b_profile: &[f64],
793 start_idx_s: usize,
794 ) -> Result<DMatrix<f64>, CoppError> {
795 if a_profile.len() != b_profile.len() {
796 return Err(ConstraintError::NoMatchDimensions.into());
797 }
798 if a_profile.is_empty() {
799 return Ok(DMatrix::zeros(self.dim(), 0));
800 }
801 self.constraints
802 .check_s_in_bounds(start_idx_s, a_profile.len())?;
803 if !self
804 .constraints
805 .check_given_q(start_idx_s, start_idx_s + a_profile.len())
806 {
807 return Err(ConstraintError::NoGivenQInfo.into());
808 }
809 let (mut coeff_a, mut coeff_b, mut coeff_g) =
810 self.torque_coeff(start_idx_s, a_profile.len())?;
811 for (mut coeff_a_col, &a_curr) in coeff_a.column_iter_mut().zip(a_profile.iter()) {
812 coeff_a_col.scale_mut(a_curr);
813 }
814 for (mut coeff_b_col, &b_curr) in coeff_b.column_iter_mut().zip(b_profile.iter()) {
815 coeff_b_col.scale_mut(b_curr);
816 }
817 coeff_g += coeff_a;
818 coeff_g += coeff_b;
819 Ok(coeff_g)
820 }
821
822 /// Build affine torque coefficients in path variables `(a,b)`.
823 ///
824 /// # Output
825 /// Returns `(coeff_a, coeff_b, coeff_g)` such that
826 /// `tau = coeff_a * a + coeff_b * b + coeff_g` column-wise.
827 ///
828 /// # Shape
829 /// Each returned matrix has shape `(dim, ncols)`.
830 ///
831 /// # Preconditions
832 /// Caller ensures target station interval is available.
833 #[allow(clippy::type_complexity)]
834 pub(crate) fn torque_coeff(
835 &self,
836 start_idx_s: usize,
837 ncols: usize,
838 ) -> Result<(DMatrix<f64>, DMatrix<f64>, DMatrix<f64>), RobotDynamicsError> {
839 let mut coeff_a = DMatrix::<f64>::zeros(self.dim(), ncols);
840 let mut coeff_b = DMatrix::<f64>::zeros(self.dim(), ncols);
841 let mut coeff_g = DMatrix::<f64>::zeros(self.dim(), ncols);
842 if ncols == 0 {
843 return Ok((coeff_a, coeff_b, coeff_g));
844 }
845 let mut dq_sqrt2 = vec![0.0; self.dim()];
846 let mut ddq_2 = vec![0.0; self.dim()];
847 let vec_zero_dim = vec![0.0; self.dim()];
848
849 let mut process_segment =
850 |start_idx: usize, ncols: usize, offset: usize| -> Result<(), RobotDynamicsError> {
851 for (local_col, (((((mut a, mut b), mut g), q), dq), ddq)) in coeff_a
852 .columns_mut(offset, ncols)
853 .column_iter_mut()
854 .zip(coeff_b.columns_mut(offset, ncols).column_iter_mut())
855 .zip(coeff_g.columns_mut(offset, ncols).column_iter_mut())
856 .zip(self.constraints.q.columns(start_idx, ncols).column_iter())
857 .zip(self.constraints.dq.columns(start_idx, ncols).column_iter())
858 .zip(self.constraints.ddq.columns(start_idx, ncols).column_iter())
859 .enumerate()
860 {
861 let idx_s = start_idx_s + offset + local_col;
862 let q_slice = q.as_slice();
863 let dq_slice = dq.as_slice();
864 let ddq_slice = ddq.as_slice();
865 // tau(q, dq/dt, ddq/ddt) = M(q) * ddq/ddt + C(q, dq/dt) * dq/dt + g(q) + f(g, sgn(q)).
866 // Then, we have the following solutions:
867 // (1) tau(q, 0, 0) = g
868 // (2) tau(q, 0, dq) = M * dq + g
869 // (3) tau(q, dq, ddq) = M * ddq + C * dq + g + f
870 // (4) tau(q, sqrt(2) * dq, 2 * ddq) = 2 * M * ddq + 2 * C * dq + g + f
871
872 // 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))
873 // tau = (M * ddq/dds + C * dq/ds) * a + M * dq/ds * b + g(q) + f(g, sgn(q))
874 // Therefore, the answer is:
875 // - coeff_b = M * dq = tau(q, 0, dq) - tau(q, 0, 0)
876 // - coeff_a = M * ddq + C * dq = tau(q, sqrt(2) * dq, 2 * ddq) - tau(q, dq, ddq)
877 // - coeff_g = g + f = tau(q, dq, ddq) - coeff_a
878
879 // Step 1. Compute coeff_b = tau(q, 0, dq) - tau(q, 0, 0)
880 // now g = tau(q, 0, 0)
881 self.inverse_dynamics_with_context(
882 idx_s,
883 "tau(q, 0, 0)",
884 q_slice,
885 &vec_zero_dim,
886 &vec_zero_dim,
887 g.as_mut_slice(),
888 )?;
889 // now b = tau(q, 0, dq)
890 self.inverse_dynamics_with_context(
891 idx_s,
892 "tau(q, 0, dq)",
893 q_slice,
894 &vec_zero_dim,
895 dq_slice,
896 b.as_mut_slice(),
897 )?;
898 // now b = tau(q, 0, dq) - tau(q, 0, 0)
899 b.iter_mut()
900 .zip(g.iter())
901 .for_each(|(b_i, &g_i)| *b_i -= g_i);
902
903 // Step 2. coeff_a = tau(q, sqrt(2) * dq, 2 * ddq) - tau(q, dq, ddq)
904 dq_sqrt2
905 .iter_mut()
906 .zip(dq.iter())
907 .for_each(|(dq_sqrt2_i, &dq_i)| *dq_sqrt2_i = SQRT_2 * dq_i);
908 ddq_2
909 .iter_mut()
910 .zip(ddq.iter())
911 .for_each(|(ddq_2_i, &ddq_i)| *ddq_2_i = 2.0 * ddq_i);
912 // now a = tau(q, sqrt(2) * dq, 2 * ddq)
913 self.inverse_dynamics_with_context(
914 idx_s,
915 "tau(q, sqrt(2) * dq, 2 * ddq)",
916 q_slice,
917 dq_sqrt2.as_slice(),
918 ddq_2.as_slice(),
919 a.as_mut_slice(),
920 )?;
921 // now g = tau(q, dq, ddq)
922 self.inverse_dynamics_with_context(
923 idx_s,
924 "tau(q, dq, ddq)",
925 q_slice,
926 dq_slice,
927 ddq_slice,
928 g.as_mut_slice(),
929 )?;
930 // now a = tau(q, sqrt(2) * dq, 2 * ddq) - tau(q, dq, ddq)
931 a.iter_mut()
932 .zip(g.iter())
933 .for_each(|(a_i, &g_i)| *a_i -= g_i);
934
935 // Step 3. coeff_g = tau(q, dq, ddq) - coeff_a
936 g.iter_mut()
937 .zip(a.iter())
938 .for_each(|(g_i, &a_i)| *g_i -= a_i);
939 }
940 Ok(())
941 };
942 let ncols_mat = self.constraints.capacity();
943 if ncols_mat - start_idx_s >= ncols {
944 process_segment(start_idx_s, ncols, 0)?;
945 } else {
946 let len_first = ncols_mat - start_idx_s;
947 process_segment(start_idx_s, len_first, 0)?;
948 let len_second = ncols - len_first;
949 process_segment(0, len_second, len_first)?;
950 }
951 Ok((coeff_a, coeff_b, coeff_g))
952 }
953
954 /// Build edge-coupled affine torque coefficients over `a[k], a[k+1]`.
955 ///
956 /// # Output
957 /// Returns `(coeff_a_curr, coeff_a_next, coeff_g)` such that
958 /// `tau[k] = coeff_a_curr * a[k] + coeff_a_next * a[k+1] + coeff_g`.
959 ///
960 /// # Shape
961 /// Each returned matrix has shape `(dim, ncols)`.
962 ///
963 /// # Preconditions
964 /// Requires station window `[start_idx_s, start_idx_s + ncols]` to be valid.
965 #[allow(clippy::type_complexity)]
966 pub(crate) fn torque2_coeff_a(
967 &self,
968 start_idx_s: usize,
969 ncols: usize,
970 ) -> Result<(DMatrix<f64>, DMatrix<f64>, DMatrix<f64>), RobotDynamicsError> {
971 let s = self
972 .constraints
973 .s_vec(start_idx_s, start_idx_s + ncols + 1)
974 .expect("torque2_coeff_a: s interval must be in bounds");
975 let ds_double_down = s
976 .windows(2)
977 .map(|s_pair| 0.5 / (s_pair[1] - s_pair[0]))
978 .collect::<Vec<f64>>();
979
980 // tau[k] = coeff_a * a[k] + coeff_b * b[k] + coeff_g
981 let (mut coeff_a, mut coeff_b, coeff_g) = self.torque_coeff(start_idx_s, ncols)?;
982 // tau[k] = coeff_a * a[k] + coeff_b * (a[k+1] - a[k]) * ds_double_down + coeff_g
983
984 // tau[k] = coeff_a * a[k] + coeff_b * (a[k+1] - a[k]) + coeff_g
985 for (mut v_b, &ds_double_down) in coeff_b.column_iter_mut().zip(ds_double_down.iter()) {
986 v_b.scale_mut(ds_double_down);
987 }
988 // tau[k] = (coeff_a - coeff_b) * a[k] + coeff_b * a[k+1] + coeff_g
989
990 // tau[k] = coeff_a * a[k] + coeff_b * a[k+1] + coeff_g
991 coeff_a -= &coeff_b;
992
993 Ok((coeff_a, coeff_b, coeff_g))
994 }
995
996 /// Add axial torque limits on interval starting at `start_idx_s`.
997 ///
998 /// # Input semantics
999 /// Enforces per-axis bounds:
1000 /// `axial_torque_min < tau < axial_torque_max`.
1001 ///
1002 /// # Mapping
1003 /// Using inverse dynamics, torque limits are transformed into second-order
1004 /// rows on `(a,b)` and appended to the constraint buffer.
1005 ///
1006 /// For a fuller inverse-dynamics implementation used with this method, see
1007 /// the test reference model in [robot_2dof.rs](demo/robot_2dof.rs).
1008 ///
1009 /// # Errors
1010 /// - [`ConstraintError::NoMatchDimensions`](crate::diag::ConstraintError::NoMatchDimensions)
1011 /// - [`ConstraintError::OutOfSBounds`](crate::diag::ConstraintError::OutOfSBounds)
1012 /// - [`ConstraintError::NoGivenQInfo`](crate::diag::ConstraintError::NoGivenQInfo)
1013 /// - [`ConstraintError::InvalidSignedBounds`](crate::diag::ConstraintError::InvalidSignedBounds)
1014 /// - [`RobotDynamicsError`](crate::diag::RobotDynamicsError) if inverse dynamics fails
1015 ///
1016 /// # Returns
1017 /// Returns `&mut Self` for chaining on success.
1018 pub fn with_axial_torque<T1, T2>(
1019 &mut self,
1020 axial_torque_max: T1,
1021 axial_torque_min: T2,
1022 start_idx_s: usize,
1023 ) -> Result<&mut Self, CoppError>
1024 where
1025 T1: UpperBound,
1026 T2: UpperBound,
1027 {
1028 // Check dimensions
1029 if !axial_torque_max.check_valid(self.dim())
1030 || !axial_torque_min.check_valid(self.dim())
1031 || axial_torque_max.ncols() != axial_torque_min.ncols()
1032 {
1033 return Err(ConstraintError::NoMatchDimensions.into());
1034 }
1035 // Check bounds
1036 self.constraints
1037 .check_s_in_bounds(start_idx_s, axial_torque_max.ncols())?;
1038 // Check given dq
1039 if !self
1040 .constraints
1041 .check_given_q(start_idx_s, start_idx_s + axial_torque_max.ncols())
1042 {
1043 return Err(ConstraintError::NoGivenQInfo.into());
1044 }
1045 if axial_torque_max.ncols() == 0 {
1046 return Ok(self);
1047 }
1048 let axial_torque_max = axial_torque_max.as_matrix();
1049 let axial_torque_min = axial_torque_min.as_matrix();
1050 Self::check_strict_signed_limits(&axial_torque_max, &axial_torque_min, "axial_torque")?;
1051
1052 // torque_min <= tau = coeff_a * a + coeff_b * b + coeff_g <= torque_max
1053 let (coeff_a, coeff_b, coeff_g) =
1054 self.torque_coeff(start_idx_s, axial_torque_max.ncols())?;
1055 // coeff_a * a + coeff_b * b <= torque_max - coeff_g
1056 // torque_min - coeff_g <= coeff_a * a + coeff_b * b
1057 self.constraints
1058 .with_constraint_2order(
1059 &coeff_a.as_view(),
1060 &coeff_b.as_view(),
1061 &(axial_torque_max - &coeff_g).as_view(),
1062 start_idx_s,
1063 false,
1064 )?
1065 .with_constraint_2order(
1066 &coeff_a.as_view(),
1067 &coeff_b.as_view(),
1068 &(axial_torque_min - coeff_g).as_view(),
1069 start_idx_s,
1070 true,
1071 )?;
1072
1073 Ok(self)
1074 }
1075}
1076
1077impl RobotTorque for usize {
1078 /// Evaluate inverse dynamics for point-mass model.
1079 ///
1080 /// Since `tau = ddq`, this function copies `ddq` directly into `tau`.
1081 #[inline(always)]
1082 fn inverse_dynamics(
1083 &self,
1084 _q: &[f64],
1085 _dq: &[f64],
1086 ddq: &[f64],
1087 tau: &mut [f64],
1088 ) -> Result<(), RobotDynamicsError> {
1089 tau.copy_from_slice(ddq);
1090 Ok(())
1091 }
1092}