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`, `RobotTorque`),
6//! - generic wrapper [`Robot`] that owns a constraint buffer,
7//! - helper trait [`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`] 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`] instead of operating on
20//! [`Constraints`](crate::constraints::Constraints) directly. This enables
21//! physically meaningful high-level APIs such as [`Robot::with_axial_velocity`],
22//! [`Robot::with_axial_acceleration`], [`Robot::with_axial_jerk`], and torque constraints.
23//! - `Topp*Problem` workflows only require [`RobotBasic`].
24//! - `Copp*Problem` workflows require [`RobotTorque`].
25//! - If no real dynamics are involved but API integration expects
26//! [`RobotTorque`], you can use `usize` as a trivial placeholder
27//! (`tau = ddq`).
28//! - For physical robots, implement [`RobotTorque`] with your own inverse
29//! dynamics.
30//!
31//! # Feasibility contract
32//! Bound pairs must satisfy strict signed limits per station:
33//! - upper bound `> 0`, lower bound `< 0`.
34//! This guarantees the zero-state neighborhood remains strictly feasible after
35//! normalization.
36
37use crate::copp::constraints::{AsInputMatrix1D, Constraints, InputMatrix};
38use crate::diag::ConstraintError;
39use nalgebra::{Const, DMatrix, Dyn, Matrix, ViewStorage};
40
41/// Borrowable upper-bound input accepted by robot constraint APIs.
42///
43/// This trait abstracts two common user inputs:
44/// - broadcast vectors `(&[f64], ncols)`;
45/// - explicit matrix views `&InputMatrix`.
46///
47/// Implementations must expose a matrix view of shape `(dim, ncols)`.
48pub trait UpperBound {
49 /// Validate that input row count is compatible with robot dimension `dim`.
50 fn check_valid(&self, dim: usize) -> bool;
51
52 /// Number of station columns represented by this bound input.
53 fn ncols(&self) -> usize;
54
55 /// Borrow input as a matrix view (`dim x ncols`).
56 fn as_matrix(&self) -> InputMatrix<'_>;
57}
58
59impl UpperBound for (&[f64], usize) {
60 #[inline(always)]
61 fn check_valid(&self, dim: usize) -> bool {
62 self.0.len() == dim
63 }
64
65 #[inline(always)]
66 fn ncols(&self) -> usize {
67 self.1
68 }
69
70 #[inline(always)]
71 fn as_matrix(&self) -> InputMatrix<'_> {
72 let dim = self.0.len();
73 let ncols = self.1;
74 // Zero-copy broadcast.
75 unsafe {
76 // Construct the matrix view directly using ViewStorage::from_raw_parts.
77 // Parameters:
78 // - data: Pointer to the original slice.
79 // - shape: (Rows: dim, Columns: ncols).
80 // - stride: (Row stride: 1, Column stride: 0).
81 // Setting the column stride to 0 achieves horizontal broadcasting
82 // without copying data, as every column starts at the same memory address.
83 let storage = ViewStorage::from_raw_parts(
84 self.0.as_ptr(),
85 (Dyn(dim), Dyn(ncols)),
86 (Const::<1>, Dyn(0)),
87 );
88 Matrix::from_data(storage)
89 }
90 }
91}
92
93impl UpperBound for &InputMatrix<'_> {
94 #[inline(always)]
95 fn check_valid(&self, dim: usize) -> bool {
96 self.nrows() == dim
97 }
98
99 #[inline(always)]
100 fn ncols(&self) -> usize {
101 (*self).ncols()
102 }
103
104 #[inline(always)]
105 fn as_matrix(&self) -> InputMatrix<'_> {
106 // Already a view; no conversion/allocation needed.
107 self.as_view()
108 }
109}
110
111/// Minimal robot metadata required by the planner.
112///
113/// A `usize` variable can serve as a trivial `RobotBasic` implementation representing the robot dimension, but users can also implement this trait for their own robot models.
114pub trait RobotBasic {
115 /// Return robot dimension / DoF.
116 fn dim(&self) -> usize;
117}
118
119impl RobotBasic for usize {
120 #[inline(always)]
121 fn dim(&self) -> usize {
122 *self
123 }
124}
125
126/// User-facing robot wrapper that owns constraint storage and conversion logic.
127///
128/// # Design role
129/// `Robot<M>` bridges robot-side physical constraints and solver-side normalized
130/// inequalities. Internally it owns [`Constraints`](crate::constraints::Constraints),
131/// but exposes higher-level APIs with physical semantics.
132///
133/// # Why prefer this over direct `Constraints`
134/// For most applications, `Robot` is the recommended entry because it provides
135/// domain-meaningful methods ([`Robot::with_axial_velocity`], [`Robot::with_axial_acceleration`],
136/// [`Robot::with_axial_jerk`], torque constraints) and enforces common contracts.
137///
138/// # Trait requirements by solver family
139/// - `Topp*Problem`: model type `M` only needs [`RobotBasic`].
140/// - `Copp*Problem`: model type `M` must implement [`RobotTorque`].
141///
142/// If you do not have a real inverse-dynamics model yet, use `usize`
143/// as a placeholder implementing [`RobotTorque`] (`tau = ddq`).
144pub struct Robot<M: RobotBasic> {
145 /// Concrete robot model implementation.
146 model: M,
147
148 /// Shared station-indexed constraint buffer used by TOPP/COPP solvers.
149 pub constraints: Constraints,
150}
151
152impl<M: RobotBasic> Robot<M> {
153 /// Enforce strict signed contract for upper/lower bounds.
154 ///
155 /// # Contract
156 /// For every element in the provided matrices:
157 /// - `upper > 0`
158 /// - `lower < 0`
159 ///
160 /// # Errors
161 /// Returns `ConstraintError::InvalidSignedBounds` when contract is violated.
162 #[inline(always)]
163 fn check_strict_signed_limits(
164 upper: &InputMatrix,
165 lower: &InputMatrix,
166 bound_name: &'static str,
167 ) -> Result<(), ConstraintError> {
168 let upper_valid = upper.iter().all(|&u| u > 0.0);
169 let lower_valid = lower.iter().all(|&l| l < 0.0);
170 if upper_valid && lower_valid {
171 Ok(())
172 } else {
173 Err(ConstraintError::InvalidSignedBounds { bound_name })
174 }
175 }
176
177 /// Construct a robot wrapper with default constraint-buffer capacity.
178 ///
179 /// # Parameters
180 /// - `model`: concrete robot model implementing [`RobotBasic`].
181 pub fn new(model: M) -> Self {
182 let dim = model.dim();
183 Self {
184 model,
185 constraints: Constraints::new(dim),
186 }
187 }
188
189 /// Construct a robot wrapper with explicit initial constraint capacity.
190 ///
191 /// # Parameters
192 /// - `model`: concrete robot model implementing [`RobotBasic`].
193 /// Pass a `usize` value for a dimension-only placeholder that also satisfies
194 /// [`RobotTorque`] with a trivial identity dynamics (`tau = ddq`), which is
195 /// convenient for testing or applications without real inverse dynamics.
196 /// - `capacity`: pre-allocated number of station columns in the internal circular
197 /// constraint buffer. Setting this to the expected number of path samples (e.g.
198 /// `n`) avoids re-allocations during constraint ingestion. Use [`Robot::new`]
199 /// when the size is unknown up-front.
200 pub fn with_capacity(model: M, capacity: usize) -> Self {
201 let dim = model.dim();
202 Self {
203 model,
204 constraints: Constraints::with_capacity(dim, capacity),
205 }
206 }
207
208 /// Get robot dimension / DoF.
209 #[inline(always)]
210 pub fn dim(&self) -> usize {
211 self.constraints.dim()
212 }
213
214 /// Append a new station segment into the internal constraint buffer.
215 ///
216 /// # Parameters
217 /// - `s_new`: station samples accepted as 1D slice or matrix view.
218 ///
219 /// # Errors
220 /// Propagates monotonicity/range errors from
221 /// `Constraints::with_s`.
222 #[inline(always)]
223 pub fn with_s<T: AsInputMatrix1D + ?Sized>(
224 &mut self,
225 s_new: &T,
226 ) -> Result<(), ConstraintError> {
227 self.constraints.with_s(s_new)
228 }
229
230 /// Write path derivatives over interval starting at `idx_s`.
231 ///
232 /// # Parameters
233 /// - `q_new`, `dq_new`, `ddq_new`: required derivative matrices.
234 /// - `dddq_new`: optional third derivative matrix.
235 /// - `idx_s`: global start station id.
236 ///
237 /// # Errors
238 /// Forwards shape/range errors from
239 /// `Constraints::with_q`.
240 #[inline(always)]
241 pub fn with_q(
242 &mut self,
243 q_new: &InputMatrix,
244 dq_new: &InputMatrix,
245 ddq_new: &InputMatrix,
246 dddq_new: Option<&InputMatrix>,
247 idx_s: usize,
248 ) -> Result<(), ConstraintError> {
249 self.constraints
250 .with_q(q_new, dq_new, ddq_new, dddq_new, idx_s)
251 }
252
253 /// Add axial velocity limits on interval starting at `start_idx_s`.
254 ///
255 /// # Input semantics
256 /// Enforces per-axis bounds:
257 /// `axial_velocity_min < \dot{q} < axial_velocity_max`.
258 ///
259 /// # Mapping
260 /// Converts velocity bounds into first-order path-speed limits on
261 /// `a = \dot{s}^2`, then fuses into `amax`.
262 ///
263 /// # Errors
264 /// - `ConstraintError::NoMatchDimensions`
265 /// - `ConstraintError::OutOfSBounds`
266 /// - `ConstraintError::NoGivenQInfo`
267 /// - `ConstraintError::InvalidSignedBounds` when max/min signs are invalid
268 pub fn with_axial_velocity<T1, T2>(
269 &mut self,
270 axial_velocity_max: T1,
271 axial_velocity_min: T2,
272 start_idx_s: usize,
273 ) -> Result<(), ConstraintError>
274 where
275 T1: UpperBound,
276 T2: UpperBound,
277 {
278 // Check dimensions
279 if !axial_velocity_max.check_valid(self.dim())
280 || !axial_velocity_min.check_valid(self.dim())
281 || axial_velocity_max.ncols() != axial_velocity_min.ncols()
282 {
283 return Err(ConstraintError::NoMatchDimensions);
284 }
285 // Check bounds
286 self.constraints
287 .check_s_in_bounds(start_idx_s, axial_velocity_max.ncols())?;
288 // Check given dq
289 if !self
290 .constraints
291 .check_given_q(start_idx_s, start_idx_s + axial_velocity_max.ncols())
292 {
293 return Err(ConstraintError::NoGivenQInfo);
294 }
295 if axial_velocity_max.ncols() == 0 {
296 return Ok(());
297 }
298 let axial_velocity_max = axial_velocity_max.as_matrix();
299 let axial_velocity_min = axial_velocity_min.as_matrix();
300 Self::check_strict_signed_limits(
301 &axial_velocity_max,
302 &axial_velocity_min,
303 "axial_velocity",
304 )?;
305 // Add new axial velocity constraints
306 let mut amax_new =
307 DMatrix::<f64>::from_element(self.dim(), axial_velocity_max.ncols(), f64::INFINITY);
308 let func = |start_idx: usize, ncols: usize, offset: usize| {
309 let amax_ = self.constraints.dq.columns(start_idx, ncols).zip_zip_map(
310 &axial_velocity_max.columns(offset, ncols),
311 &axial_velocity_min.columns(offset, ncols),
312 |dq, vmax, vmin| {
313 if dq > 0.0 {
314 (vmax / dq).powi(2)
315 } else if dq < 0.0 {
316 (vmin / dq).powi(2)
317 } else {
318 f64::INFINITY
319 }
320 },
321 );
322 amax_new.columns_mut(offset, ncols).copy_from(&amax_);
323 };
324 let ncols_mat = self.constraints.capacity();
325 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
326 Constraints::circular_process(ncols_mat, start_idx, axial_velocity_max.ncols(), func);
327
328 self.constraints
329 .with_constraint_1order(&amax_new.as_view(), start_idx_s)?;
330
331 Ok(())
332 }
333
334 /// Add axial acceleration limits on interval starting at `start_idx_s`.
335 ///
336 /// # Input semantics
337 /// Enforces per-axis bounds:
338 /// `axial_acceleration_min < \ddot{q} < axial_acceleration_max`.
339 ///
340 /// # Mapping
341 /// Generates second-order rows:
342 /// `acc_a * a + acc_b * b <= acc_max`,
343 /// where `(a,b)` are path-speed variables.
344 ///
345 /// # Errors
346 /// - `ConstraintError::NoMatchDimensions`
347 /// - `ConstraintError::OutOfSBounds`
348 /// - `ConstraintError::NoGivenQInfo`
349 /// - `ConstraintError::InvalidSignedBounds`
350 pub fn with_axial_acceleration<T1, T2>(
351 &mut self,
352 axial_acceleration_max: T1,
353 axial_acceleration_min: T2,
354 start_idx_s: usize,
355 ) -> Result<(), ConstraintError>
356 where
357 T1: UpperBound,
358 T2: UpperBound,
359 {
360 // Check dimensions
361 if !axial_acceleration_max.check_valid(self.dim())
362 || !axial_acceleration_min.check_valid(self.dim())
363 || axial_acceleration_max.ncols() != axial_acceleration_min.ncols()
364 {
365 return Err(ConstraintError::NoMatchDimensions);
366 }
367 // Check bounds
368 self.constraints
369 .check_s_in_bounds(start_idx_s, axial_acceleration_max.ncols())?;
370 // Check given dq, ddq
371 if !self
372 .constraints
373 .check_given_q(start_idx_s, start_idx_s + axial_acceleration_max.ncols())
374 {
375 return Err(ConstraintError::NoGivenQInfo);
376 }
377 if axial_acceleration_max.ncols() == 0 {
378 return Ok(());
379 }
380 let axial_acceleration_max = axial_acceleration_max.as_matrix();
381 let axial_acceleration_min = axial_acceleration_min.as_matrix();
382 Self::check_strict_signed_limits(
383 &axial_acceleration_max,
384 &axial_acceleration_min,
385 "axial_acceleration",
386 )?;
387 // Add new axial acceleration constraints
388 let mut acc_a_new = DMatrix::<f64>::zeros(self.dim(), axial_acceleration_max.ncols());
389 let mut acc_b_new = DMatrix::<f64>::zeros(self.dim(), axial_acceleration_max.ncols());
390 let func = |start_idx: usize, ncols: usize, offset: usize| {
391 acc_a_new
392 .columns_mut(offset, ncols)
393 .copy_from(&self.constraints.ddq.columns(start_idx, ncols));
394 acc_b_new
395 .columns_mut(offset, ncols)
396 .copy_from(&self.constraints.dq.columns(start_idx, ncols));
397 };
398 let ncols_mat = self.constraints.capacity();
399 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
400 Constraints::circular_process(ncols_mat, start_idx, axial_acceleration_max.ncols(), func);
401 self.constraints.with_constraint_2order(
402 &acc_a_new.as_view(),
403 &acc_b_new.as_view(),
404 &axial_acceleration_max.as_view(),
405 start_idx_s,
406 false,
407 )?;
408 self.constraints.with_constraint_2order(
409 &acc_a_new.as_view(),
410 &acc_b_new.as_view(),
411 &axial_acceleration_min.as_view(),
412 start_idx_s,
413 true,
414 )?;
415
416 Ok(())
417 }
418
419 /// Add axial jerk limits on interval starting at `start_idx_s`.
420 ///
421 /// # Input semantics
422 /// Enforces per-axis bounds:
423 /// `axial_jerk_min < \dddot{q} < axial_jerk_max`.
424 ///
425 /// # Mapping
426 /// Generates third-order rows used by TOPP3/COPP3:
427 /// `sqrt(a) * (jerk_a*a + jerk_b*b + jerk_c*c + jerk_d) <= jerk_max`.
428 ///
429 /// # Errors
430 /// - `ConstraintError::NoMatchDimensions`
431 /// - `ConstraintError::OutOfSBounds`
432 /// - `ConstraintError::NoGivenQInfo` (needs `q/dq/ddq/dddq`)
433 /// - `ConstraintError::InvalidSignedBounds`
434 pub fn with_axial_jerk<T1, T2>(
435 &mut self,
436 axial_jerk_max: T1,
437 axial_jerk_min: T2,
438 start_idx_s: usize,
439 ) -> Result<(), ConstraintError>
440 where
441 T1: UpperBound,
442 T2: UpperBound,
443 {
444 // Check dimensions
445 if !axial_jerk_max.check_valid(self.dim())
446 || !axial_jerk_min.check_valid(self.dim())
447 || axial_jerk_max.ncols() != axial_jerk_min.ncols()
448 {
449 return Err(ConstraintError::NoMatchDimensions);
450 }
451 // Check bounds
452 self.constraints
453 .check_s_in_bounds(start_idx_s, axial_jerk_max.ncols())?;
454 // Check given dq, ddq, dddq
455 if !self
456 .constraints
457 .check_given_q(start_idx_s, start_idx_s + axial_jerk_max.ncols())
458 || !self
459 .constraints
460 .check_given_dddq(start_idx_s, start_idx_s + axial_jerk_max.ncols())
461 {
462 return Err(ConstraintError::NoGivenQInfo);
463 }
464 if axial_jerk_max.ncols() == 0 {
465 return Ok(());
466 }
467 let axial_jerk_max = axial_jerk_max.as_matrix();
468 let axial_jerk_min = axial_jerk_min.as_matrix();
469 Self::check_strict_signed_limits(&axial_jerk_max, &axial_jerk_min, "axial_jerk")?;
470 // Add new axial jerk constraints
471 let mut jerk_a_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
472 let mut jerk_b_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
473 let mut jerk_c_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
474 let jerk_d_new = DMatrix::<f64>::zeros(self.dim(), axial_jerk_max.ncols());
475 let func = |start_idx: usize, ncols: usize, offset: usize| {
476 jerk_a_new
477 .columns_mut(offset, ncols)
478 .copy_from(&self.constraints.dddq.columns(start_idx, ncols));
479 jerk_b_new
480 .columns_mut(offset, ncols)
481 .copy_from(&self.constraints.ddq.columns(start_idx, ncols));
482 jerk_c_new
483 .columns_mut(offset, ncols)
484 .copy_from(&self.constraints.dq.columns(start_idx, ncols));
485 };
486 let ncols_mat = self.constraints.capacity();
487 let start_idx = self.constraints.col_at_idx_s_unchecked(start_idx_s);
488 Constraints::circular_process(ncols_mat, start_idx, axial_jerk_max.ncols(), func);
489 jerk_b_new.scale_mut(3.0);
490 self.constraints.with_constraint_3order(
491 &jerk_a_new.as_view(),
492 &jerk_b_new.as_view(),
493 &jerk_c_new.as_view(),
494 &jerk_d_new.as_view(),
495 &axial_jerk_max,
496 start_idx_s,
497 false,
498 )?;
499 self.constraints.with_constraint_3order(
500 &jerk_a_new.as_view(),
501 &jerk_b_new.as_view(),
502 &jerk_c_new.as_view(),
503 &jerk_d_new.as_view(),
504 &axial_jerk_min,
505 start_idx_s,
506 true,
507 )?;
508
509 Ok(())
510 }
511}
512
513/// Robot trait with inverse-dynamics capability.
514///
515/// This trait is mainly required when building COPP2/COPP3 problems with
516/// torque/dynamics constraints. For TOPP-only use cases, a direct
517/// [`Constraints`](crate::copp::constraints::Constraints) workflow is usually
518/// enough.
519///
520/// A `usize` variable can serve as a trivial `RobotTorque` implementation representing a point-mass model, where `tau = ddq`. For physical robots, users should implement this trait with their own inverse dynamics.
521pub trait RobotTorque: RobotBasic {
522 /// Evaluate inverse dynamics.
523 ///
524 /// # Model
525 /// `tau = M(q) * ddq + C(q, dq) * dq + g(q)`
526 ///
527 /// # Parameters
528 /// - `q`: joint positions (`dim`).
529 /// - `dq`: joint velocities (`dim`).
530 /// - `ddq`: joint accelerations (`dim`).
531 /// - `tau`: output required torques/forces (`dim`).
532 fn inverse_dynamics(&self, q: &[f64], dq: &[f64], ddq: &[f64], tau: &mut [f64]);
533}
534
535impl<M: RobotTorque> Robot<M> {
536 /// Compute torque profile from path-domain `(a,b)` samples.
537 ///
538 /// # Notes
539 /// This is a test helper used to evaluate dynamic feasibility of a profile.
540 ///
541 /// # Errors
542 /// Returns shape/range/data-availability errors when prerequisites are not met.
543 #[cfg(test)]
544 pub(crate) fn get_torque_with_ab(
545 &self,
546 a_profile: &[f64],
547 b_profile: &[f64],
548 start_idx_s: usize,
549 ) -> Result<DMatrix<f64>, ConstraintError> {
550 if a_profile.len() != b_profile.len() {
551 return Err(ConstraintError::NoMatchDimensions);
552 }
553 if a_profile.is_empty() {
554 return Ok(DMatrix::zeros(self.dim(), 0));
555 }
556 self.constraints
557 .check_s_in_bounds(start_idx_s, a_profile.len())?;
558 if !self
559 .constraints
560 .check_given_q(start_idx_s, start_idx_s + a_profile.len())
561 {
562 return Err(ConstraintError::NoGivenQInfo);
563 }
564 let (mut coeff_a, mut coeff_b, mut coeff_g) =
565 self.torque_coeff(start_idx_s, a_profile.len());
566 for (mut coeff_a_col, &a_curr) in coeff_a.column_iter_mut().zip(a_profile.iter()) {
567 coeff_a_col.scale_mut(a_curr);
568 }
569 for (mut coeff_b_col, &b_curr) in coeff_b.column_iter_mut().zip(b_profile.iter()) {
570 coeff_b_col.scale_mut(b_curr);
571 }
572 coeff_g += coeff_a;
573 coeff_g += coeff_b;
574 Ok(coeff_g)
575 }
576
577 /// Build affine torque coefficients in path variables `(a,b)`.
578 ///
579 /// # Output
580 /// Returns `(coeff_a, coeff_b, coeff_g)` such that
581 /// `tau = coeff_a * a + coeff_b * b + coeff_g` column-wise.
582 ///
583 /// # Shape
584 /// Each returned matrix has shape `(dim, ncols)`.
585 ///
586 /// # Preconditions
587 /// Caller ensures target station interval is available.
588 #[allow(clippy::type_complexity)]
589 pub(crate) fn torque_coeff(
590 &self,
591 start_idx_s: usize,
592 ncols: usize,
593 ) -> (DMatrix<f64>, DMatrix<f64>, DMatrix<f64>) {
594 let mut coeff_a = DMatrix::<f64>::zeros(self.dim(), ncols);
595 let mut coeff_b = DMatrix::<f64>::zeros(self.dim(), ncols);
596 let mut coeff_g = DMatrix::<f64>::zeros(self.dim(), ncols);
597 let vec_zero_dim = vec![0.0; self.dim()];
598
599 let func = |start_idx: usize, ncols: usize, offset: usize| {
600 for (((((mut a, mut b), mut g), q), dq), ddq) in coeff_a
601 .columns_mut(offset, ncols)
602 .column_iter_mut()
603 .zip(coeff_b.columns_mut(offset, ncols).column_iter_mut())
604 .zip(coeff_g.columns_mut(offset, ncols).column_iter_mut())
605 .zip(self.constraints.q.columns(start_idx, ncols).column_iter())
606 .zip(self.constraints.dq.columns(start_idx, ncols).column_iter())
607 .zip(self.constraints.ddq.columns(start_idx, ncols).column_iter())
608 {
609 let q_slice = q.as_slice();
610 let dq_slice = dq.as_slice();
611 let ddq_slice = ddq.as_slice();
612 // tau(q, dq/dt, ddq/ddt) = M(q) * ddq/ddt + C(q, dq/dt) * dq/dt + g(q).
613 // tau = M(q) * (ddq/dds * a + dq/ds * b) + C(q, dq/ds * sqrt(a)) * dq/ds * sqrt(a) + g(q)
614 // tau = (M * ddq/dds + C * dq/ds) * a + M * dq/ds * b + g(q)
615 // Step 1. coeff_g = g(q) = tau(q, 0, 0)
616 self.model.inverse_dynamics(
617 q_slice,
618 &vec_zero_dim,
619 &vec_zero_dim,
620 g.as_mut_slice(),
621 );
622 // Step 2. coeff_b = M(q) * dq = tau(q, 0, dq) - g(q)
623 self.model
624 .inverse_dynamics(q_slice, &vec_zero_dim, dq_slice, b.as_mut_slice());
625 b.iter_mut()
626 .zip(g.iter())
627 .for_each(|(b_i, g_i)| *b_i -= *g_i);
628 // Step 3. coeff_a = M(q) * ddq + C(q, dq) * dq = tau(q, dq, ddq) - g(q)
629 self.model
630 .inverse_dynamics(q_slice, dq_slice, ddq_slice, a.as_mut_slice());
631 a.iter_mut()
632 .zip(g.iter())
633 .for_each(|(a_i, g_i)| *a_i -= *g_i);
634 }
635 };
636 let ncols_mat = self.constraints.capacity();
637 Constraints::circular_process(ncols_mat, start_idx_s, ncols, func);
638 (coeff_a, coeff_b, coeff_g)
639 }
640
641 /// Build edge-coupled affine torque coefficients over `a[k], a[k+1]`.
642 ///
643 /// # Output
644 /// Returns `(coeff_a_curr, coeff_a_next, coeff_g)` such that
645 /// `tau[k] = coeff_a_curr * a[k] + coeff_a_next * a[k+1] + coeff_g`.
646 ///
647 /// # Shape
648 /// Each returned matrix has shape `(dim, ncols)`.
649 ///
650 /// # Preconditions
651 /// Requires station window `[start_idx_s, start_idx_s + ncols]` to be valid.
652 #[allow(clippy::type_complexity)]
653 pub(crate) fn torque2_coeff_a(
654 &self,
655 start_idx_s: usize,
656 ncols: usize,
657 ) -> (DMatrix<f64>, DMatrix<f64>, DMatrix<f64>) {
658 let s = self
659 .constraints
660 .s_vec(start_idx_s, start_idx_s + ncols + 1)
661 .expect("torque2_coeff_a: s interval must be in bounds");
662 let ds_double_down = s
663 .windows(2)
664 .map(|s_pair| 0.5 / (s_pair[1] - s_pair[0]))
665 .collect::<Vec<f64>>();
666
667 // tau[k] = coeff_a * a[k] + coeff_b * b[k] + coeff_g
668 let (mut coeff_a, mut coeff_b, coeff_g) = self.torque_coeff(start_idx_s, ncols);
669 // tau[k] = coeff_a * a[k] + coeff_b * (a[k+1] - a[k]) * ds_double_down + coeff_g
670
671 // tau[k] = coeff_a * a[k] + coeff_b * (a[k+1] - a[k]) + coeff_g
672 for (mut v_b, &ds_double_down) in coeff_b.column_iter_mut().zip(ds_double_down.iter()) {
673 v_b.scale_mut(ds_double_down);
674 }
675 // tau[k] = (coeff_a - coeff_b) * a[k] + coeff_b * a[k+1] + coeff_g
676
677 // tau[k] = coeff_a * a[k] + coeff_b * a[k+1] + coeff_g
678 coeff_a -= &coeff_b;
679
680 (coeff_a, coeff_b, coeff_g)
681 }
682
683 /// Add axial torque limits on interval starting at `start_idx_s`.
684 ///
685 /// # Input semantics
686 /// Enforces per-axis bounds:
687 /// `axial_torque_min < tau < axial_torque_max`.
688 ///
689 /// # Mapping
690 /// Using inverse dynamics, torque limits are transformed into second-order
691 /// rows on `(a,b)` and appended to the constraint buffer.
692 ///
693 /// # Errors
694 /// - `ConstraintError::NoMatchDimensions`
695 /// - `ConstraintError::OutOfSBounds`
696 /// - `ConstraintError::NoGivenQInfo`
697 /// - `ConstraintError::InvalidSignedBounds`
698 pub fn with_axial_torque<T1, T2>(
699 &mut self,
700 axial_torque_max: T1,
701 axial_torque_min: T2,
702 start_idx_s: usize,
703 ) -> Result<(), ConstraintError>
704 where
705 T1: UpperBound,
706 T2: UpperBound,
707 {
708 // Check dimensions
709 if !axial_torque_max.check_valid(self.dim())
710 || !axial_torque_min.check_valid(self.dim())
711 || axial_torque_max.ncols() != axial_torque_min.ncols()
712 {
713 return Err(ConstraintError::NoMatchDimensions);
714 }
715 // Check bounds
716 self.constraints
717 .check_s_in_bounds(start_idx_s, axial_torque_max.ncols())?;
718 // Check given dq
719 if !self
720 .constraints
721 .check_given_q(start_idx_s, start_idx_s + axial_torque_max.ncols())
722 {
723 return Err(ConstraintError::NoGivenQInfo);
724 }
725 if axial_torque_max.ncols() == 0 {
726 return Ok(());
727 }
728 let axial_torque_max = axial_torque_max.as_matrix();
729 let axial_torque_min = axial_torque_min.as_matrix();
730 Self::check_strict_signed_limits(&axial_torque_max, &axial_torque_min, "axial_torque")?;
731
732 // torque_min <= tau = coeff_a * a + coeff_b * b + coeff_g <= torque_max
733 let (coeff_a, coeff_b, coeff_g) = self.torque_coeff(start_idx_s, axial_torque_max.ncols());
734 // coeff_a * a + coeff_b * b <= torque_max - coeff_g
735 self.constraints.with_constraint_2order(
736 &coeff_a.as_view(),
737 &coeff_b.as_view(),
738 &(axial_torque_max - &coeff_g).as_view(),
739 start_idx_s,
740 false,
741 )?;
742 // torque_min - coeff_g <= coeff_a * a + coeff_b * b
743 self.constraints.with_constraint_2order(
744 &coeff_a.as_view(),
745 &coeff_b.as_view(),
746 &(axial_torque_min - coeff_g).as_view(),
747 start_idx_s,
748 true,
749 )?;
750
751 Ok(())
752 }
753}
754
755impl RobotTorque for usize {
756 /// Evaluate inverse dynamics for point-mass model.
757 ///
758 /// Since `tau = ddq`, this function copies `ddq` directly into `tau`.
759 #[inline(always)]
760 fn inverse_dynamics(&self, _q: &[f64], _dq: &[f64], ddq: &[f64], tau: &mut [f64]) {
761 tau.copy_from_slice(ddq);
762 }
763}