diff --git a/README.md b/README.md index 0e85226..5e816ab 100644 --- a/README.md +++ b/README.md @@ -101,10 +101,22 @@ The crate provides checked variants for input-sensitive operations: - `MultiBody::from_config` - `try_minimal_to_homogeneous_configuration` - `try_compute_regressor_matrix` +- `try_step_dynamics` The unchecked compatibility methods panic with clear messages when the checked variant would return an error. +## Integration Helpers + +`MultiBody::step_dynamics` and `MultiBody::try_step_dynamics` provide +convenience stepping around `forward_dynamics_ab`. They support +semi-implicit Euler and RK4 through `IntegrationOptions`. + +For `SixDOF` joints, generalized velocities are interpreted as body-frame +twists ordered `[linear; angular]`, matching the crate's spatial-vector +conventions. The RK4 mode uses a fourth-order generalized-velocity update and +ordered SE(3) stage composition for `SixDOF` poses. + ## Development Run the main verification commands before publishing changes: diff --git a/src/math_functions.rs b/src/math_functions.rs index 2ced9fd..af4a0c1 100644 --- a/src/math_functions.rs +++ b/src/math_functions.rs @@ -2,7 +2,10 @@ #![allow(dead_code)] extern crate nalgebra as na; -use na::{Dyn, Isometry3, Matrix3, Matrix6, OMatrix, OVector, SMatrix, Vector3, Vector6}; +use na::{ + Dyn, Isometry3, Matrix3, Matrix6, OMatrix, OVector, SMatrix, Translation3, UnitQuaternion, + Vector3, Vector6, +}; #[inline(always)] pub fn Ad_inv(h: &Isometry3) -> Matrix6 { @@ -62,6 +65,30 @@ pub fn ad_se3(v: &Vector6) -> SMatrix { ad } +/// Computes the SE(3) exponential for a body-frame twist ordered as `[linear; angular]`. +pub fn exp_se3(v: &Vector6) -> Isometry3 { + let linear = v.fixed_rows::<3>(0).into_owned(); + let angular = v.fixed_rows::<3>(3).into_owned(); + let theta_squared = angular.norm_squared(); + let angular_skew = skew(&angular); + let angular_skew_squared = angular_skew * angular_skew; + + let V = if theta_squared < 1e-12 { + Matrix3::identity() + 0.5 * angular_skew + (1.0 / 6.0) * angular_skew_squared + } else { + let theta = theta_squared.sqrt(); + Matrix3::identity() + + ((1.0 - theta.cos()) / theta_squared) * angular_skew + + ((theta - theta.sin()) / (theta_squared * theta)) * angular_skew_squared + }; + let translation = V * linear; + + Isometry3::from_parts( + Translation3::new(translation[0], translation[1], translation[2]), + UnitQuaternion::from_scaled_axis(angular), + ) +} + pub fn ad_se3_dyn(v: &OVector) -> OMatrix { let mut ad = OMatrix::::zeros(6, 6); // let mut ad = OMatrix::::zeros(6, 6); diff --git a/src/multibody.rs b/src/multibody.rs index 99600c0..95d03bb 100644 --- a/src/multibody.rs +++ b/src/multibody.rs @@ -129,6 +129,21 @@ pub type JointRegressorFn<'a, const NUM_PARAMS: usize> = dyn Fn( ) -> JointRegressorOut + 'a; +/// Callback type for spatial forces applied to each body during forward dynamics. +/// +/// The first slice contains the relative body transforms used by the articulated-body +/// recursion: `h[i] = offset_matrices[i] * conf[i]`. Each `h[i]` transforms body `i` +/// coordinates into its parent coordinates, or into the inertial root coordinates for +/// root bodies. Equivalently, `Ad_inv(&h[i])` maps parent-frame spatial vectors into +/// body `i`'s frame. These are not accumulated world poses. +/// +/// The second slice contains `nu[i]`, the spatial velocity of body `i` expressed in +/// body `i`'s frame and ordered `[linear; angular]`. The returned matrix column `i` +/// is the external spatial force applied to body `i`, expressed in body `i`'s frame +/// and ordered `[force; torque]`. +pub type RigidBodyForcesFn<'a, const NUM_BODIES: usize> = + dyn Fn(&[Isometry3], &[Vector6]) -> SMatrix + 'a; + /// Allows overloading of functions for both a single 6DOF configuration and for a vector of 6DOF configurations, which is required when there are more than one 6DOF joint in the multibody system. pub trait IntoHomogeneousConfigurationVec { fn into(&self) -> Vec>; @@ -164,6 +179,47 @@ pub struct MultiBody { rho: Option, } +#[derive(Clone, Debug)] +pub struct DynamicsState { + /// Per-joint homogeneous configurations in topology order. + pub conf: Vec>, + /// Generalized velocity vector. + pub mu: SVector, +} + +#[derive(Clone, Copy)] +pub struct DynamicsStepInput<'a, const NUM_BODIES: usize, const NUM_DOFS: usize> { + /// Callback returning body-frame spatial forces for each body. + pub rigid_body_forces: &'a RigidBodyForcesFn<'a, NUM_BODIES>, + /// Per-body spatial thruster forces. + pub thruster_forces: &'a [Vector6], + /// Generalized effort input. + pub eta: &'a SVector, + /// Ambient/current linear velocity used by the hydrodynamic forward-dynamics terms. + pub lin_vel_current: &'a Vector3, + /// Ambient/current linear acceleration used by the hydrodynamic forward-dynamics terms. + pub lin_accel_current: &'a Vector3, +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum IntegrationMethod { + /// Update velocity first, then advance configuration with the new velocity. + SemiImplicitEuler, + /// Fourth-order Runge-Kutta velocity update. + /// + /// Scalar joint configurations use the RK4 weighted velocity. `SixDOF` joint poses use ordered + /// stage exponential composition for their body-frame twists. + Rk4, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct IntegrationOptions { + /// Integration timestep in seconds. + pub dt: f64, + /// Integration scheme to use for this step. + pub method: IntegrationMethod, +} + #[derive(Clone, Debug)] pub struct ForwardDynamicsWorkspace { h: Vec>, @@ -563,6 +619,214 @@ impl MultiBody, + input: DynamicsStepInput<'_, NUM_BODIES, NUM_DOFS>, + options: IntegrationOptions, + ) -> DynamicsState { + match self.try_step_dynamics(state, input, options) { + Ok(state) => state, + Err(err) => panic!("{}", err), + } + } + + /// Checked variant of [`step_dynamics`]. + pub fn try_step_dynamics( + &self, + state: &DynamicsState, + input: DynamicsStepInput<'_, NUM_BODIES, NUM_DOFS>, + options: IntegrationOptions, + ) -> Result, &'static str> { + self.validate_dynamics_step(state, input, options)?; + let mut workspace = ForwardDynamicsWorkspace::::new(); + + let next_state = match options.method { + IntegrationMethod::SemiImplicitEuler => { + self.step_dynamics_euler(state, input, options.dt, &mut workspace) + } + IntegrationMethod::Rk4 => { + self.step_dynamics_rk4(state, input, options.dt, &mut workspace) + } + }; + + Ok(next_state) + } + + fn validate_dynamics_step( + &self, + state: &DynamicsState, + input: DynamicsStepInput<'_, NUM_BODIES, NUM_DOFS>, + options: IntegrationOptions, + ) -> Result<(), &'static str> { + if !options.dt.is_finite() || options.dt < 0.0 { + return Err("dt must be finite and non-negative"); + } + if state.conf.len() != NUM_BODIES { + return Err("conf length mismatch"); + } + if input.thruster_forces.len() != NUM_BODIES { + return Err("thruster_forces length mismatch"); + } + Ok(()) + } + + fn step_dynamics_euler( + &self, + state: &DynamicsState, + input: DynamicsStepInput<'_, NUM_BODIES, NUM_DOFS>, + dt: f64, + workspace: &mut ForwardDynamicsWorkspace, + ) -> DynamicsState { + let acceleration = self.dynamics_acceleration(state, input, workspace); + let mu = state.mu + dt * acceleration; + let conf = self.advance_configuration(&state.conf, &mu, dt); + + DynamicsState { conf, mu } + } + + fn step_dynamics_rk4( + &self, + state: &DynamicsState, + input: DynamicsStepInput<'_, NUM_BODIES, NUM_DOFS>, + dt: f64, + workspace: &mut ForwardDynamicsWorkspace, + ) -> DynamicsState { + let k1_mu = self.dynamics_acceleration(state, input, workspace); + let k1_conf_velocity = state.mu; + + let state2 = DynamicsState { + conf: self.advance_configuration(&state.conf, &k1_conf_velocity, 0.5 * dt), + mu: state.mu + 0.5 * dt * k1_mu, + }; + let k2_mu = self.dynamics_acceleration(&state2, input, workspace); + let k2_conf_velocity = state2.mu; + + let state3 = DynamicsState { + conf: self.advance_configuration(&state.conf, &k2_conf_velocity, 0.5 * dt), + mu: state.mu + 0.5 * dt * k2_mu, + }; + let k3_mu = self.dynamics_acceleration(&state3, input, workspace); + let k3_conf_velocity = state3.mu; + + let state4 = DynamicsState { + conf: self.advance_configuration(&state.conf, &k3_conf_velocity, dt), + mu: state.mu + dt * k3_mu, + }; + let k4_mu = self.dynamics_acceleration(&state4, input, workspace); + let k4_conf_velocity = state4.mu; + + let mu = state.mu + (dt / 6.0) * (k1_mu + 2.0 * k2_mu + 2.0 * k3_mu + k4_mu); + let conf = self.advance_configuration_rk4( + &state.conf, + &k1_conf_velocity, + &k2_conf_velocity, + &k3_conf_velocity, + &k4_conf_velocity, + dt, + ); + + DynamicsState { conf, mu } + } + + fn dynamics_acceleration( + &self, + state: &DynamicsState, + input: DynamicsStepInput<'_, NUM_BODIES, NUM_DOFS>, + workspace: &mut ForwardDynamicsWorkspace, + ) -> SVector { + self.forward_dynamics_ab_with_workspace( + &state.conf, + &state.mu, + input.rigid_body_forces, + input.thruster_forces, + input.eta, + input.lin_vel_current, + input.lin_accel_current, + workspace, + ) + } + + fn advance_configuration( + &self, + conf: &[Isometry3], + mu: &SVector, + dt: f64, + ) -> Vec> { + let mut next_conf = Vec::with_capacity(NUM_BODIES); + + for (i, conf_i) in conf.iter().enumerate().take(NUM_BODIES) { + next_conf.push(*conf_i * self.joint_delta(i, mu, dt)); + } + + next_conf + } + + fn advance_configuration_rk4( + &self, + conf: &[Isometry3], + k1: &SVector, + k2: &SVector, + k3: &SVector, + k4: &SVector, + dt: f64, + ) -> Vec> { + let scalar_velocity = (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0; + let mut next_conf = Vec::with_capacity(NUM_BODIES); + + for (i, conf_i) in conf.iter().enumerate().take(NUM_BODIES) { + match &self.joint_types[i] { + JointType::Revolute(_) | JointType::Prismatic(_) => { + next_conf.push(*conf_i * self.joint_delta(i, &scalar_velocity, dt)); + } + JointType::SixDOF => { + let idx = i + self.joint_size_offsets[i]; + // Body-frame SE(3) twists from different RK stages live in their staged + // frames, so compose ordered stage exponentials instead of averaging twists. + let delta = exp_se3(&(Self::six_dof_twist(k1, idx) * (dt / 6.0))) + * exp_se3(&(Self::six_dof_twist(k2, idx) * (dt / 3.0))) + * exp_se3(&(Self::six_dof_twist(k3, idx) * (dt / 3.0))) + * exp_se3(&(Self::six_dof_twist(k4, idx) * (dt / 6.0))); + next_conf.push(*conf_i * delta); + } + } + } + + next_conf + } + + fn joint_delta(&self, body_id: usize, mu: &SVector, dt: f64) -> Isometry3 { + let idx = body_id + self.joint_size_offsets[body_id]; + match &self.joint_types[body_id] { + JointType::Revolute(axis) => { + let angle = mu[idx] * dt; + let rotation = match axis { + Axis::X => UnitQuaternion::from_axis_angle(&Vector3::x_axis(), angle), + Axis::Y => UnitQuaternion::from_axis_angle(&Vector3::y_axis(), angle), + Axis::Z => UnitQuaternion::from_axis_angle(&Vector3::z_axis(), angle), + }; + Isometry3::from_parts(Translation3::identity(), rotation) + } + JointType::Prismatic(axis) => { + let distance = mu[idx] * dt; + let translation = match axis { + Axis::X => Translation3::new(distance, 0.0, 0.0), + Axis::Y => Translation3::new(0.0, distance, 0.0), + Axis::Z => Translation3::new(0.0, 0.0, distance), + }; + Isometry3::from_parts(translation, UnitQuaternion::identity()) + } + JointType::SixDOF => exp_se3(&(Self::six_dof_twist(mu, idx) * dt)), + } + } + + fn six_dof_twist(mu: &SVector, idx: usize) -> Vector6 { + Vector6::from_column_slice(mu.rows(idx, 6).as_slice()) + } + pub fn generalized_newton_euler( &self, conf: &[Isometry3], diff --git a/tests/integration_helpers.rs b/tests/integration_helpers.rs new file mode 100644 index 0000000..f5b0a89 --- /dev/null +++ b/tests/integration_helpers.rs @@ -0,0 +1,496 @@ +use approx::assert_relative_eq; +use multibody_dynamics::math_functions::exp_se3; +use multibody_dynamics::multibody::*; +use nalgebra as na; + +type Vector3 = na::Vector3; +type Vector6 = na::SVector; + +fn scalar_link() -> LinkProperties { + LinkProperties { + mass: Some(1.0), + r_com: Some(Vector3::zeros()), + inertia3: Some(na::Matrix3::identity()), + ..LinkProperties::default() + } +} + +fn one_body_model(joint_type: JointType) -> MultiBody<1, NUM_DOFS> { + MultiBody::from_config(MultiBodyConfig { + topology: Topology { + offset_matrices: vec![na::Isometry3::identity()], + joint_types: vec![joint_type], + parent: vec![0], + }, + link_props: Some(vec![scalar_link()]), + env: Environment::default(), + }) + .unwrap() +} + +#[test] +fn semi_implicit_euler_advances_revolute_joint() { + let model = one_body_model::<1>(JointType::Revolute(Axis::Z)); + let state = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::from_element(1.25), + }; + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let eta = na::SVector::::zeros(); + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + + let stationary = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::zeros(), + }; + let stationary_next = model.step_dynamics( + &stationary, + input, + IntegrationOptions { + dt: 0.2, + method: IntegrationMethod::SemiImplicitEuler, + }, + ); + assert_relative_eq!( + stationary_next.conf[0] + .rotation + .angle_to(&na::UnitQuaternion::identity()), + 0.0, + epsilon = 1e-12 + ); + + let next = model.step_dynamics( + &state, + input, + IntegrationOptions { + dt: 0.2, + method: IntegrationMethod::SemiImplicitEuler, + }, + ); + let expected_rotation = na::UnitQuaternion::from_axis_angle(&Vector3::z_axis(), 1.25 * 0.2); + + assert_relative_eq!( + next.conf[0].rotation.angle_to(&expected_rotation), + 0.0, + epsilon = 1e-12 + ); + assert_relative_eq!(next.mu, state.mu, epsilon = 1e-12); +} + +#[test] +fn semi_implicit_euler_advances_prismatic_joint() { + let model = one_body_model::<1>(JointType::Prismatic(Axis::X)); + let state = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::from_element(2.0), + }; + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let eta = na::SVector::::zeros(); + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + + let next = model.step_dynamics( + &state, + input, + IntegrationOptions { + dt: 0.25, + method: IntegrationMethod::SemiImplicitEuler, + }, + ); + + assert_relative_eq!(next.conf[0].translation.vector, Vector3::new(0.5, 0.0, 0.0)); + assert_relative_eq!(next.mu, state.mu, epsilon = 1e-12); +} + +#[test] +fn six_dof_step_advances_body_frame_linear_and_angular_velocity() { + let model = one_body_model::<6>(JointType::SixDOF); + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let eta = na::SVector::::zeros(); + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + + let mut linear_mu = na::SVector::::zeros(); + linear_mu[0] = 1.0; + let linear_state = DynamicsState::<1, 6> { + conf: vec![na::Isometry3::identity()], + mu: linear_mu, + }; + let linear_next = model.step_dynamics( + &linear_state, + input, + IntegrationOptions { + dt: 0.25, + method: IntegrationMethod::Rk4, + }, + ); + assert_relative_eq!( + linear_next.conf[0].translation.vector, + Vector3::new(0.25, 0.0, 0.0), + epsilon = 1e-12 + ); + assert_relative_eq!( + linear_next.conf[0] + .rotation + .angle_to(&na::UnitQuaternion::identity()), + 0.0, + epsilon = 1e-12 + ); + + let mut angular_mu = na::SVector::::zeros(); + angular_mu[5] = 2.0; + let angular_state = DynamicsState::<1, 6> { + conf: vec![na::Isometry3::identity()], + mu: angular_mu, + }; + let angular_next = model.step_dynamics( + &angular_state, + input, + IntegrationOptions { + dt: 0.25, + method: IntegrationMethod::Rk4, + }, + ); + let expected_rotation = na::UnitQuaternion::from_axis_angle(&Vector3::z_axis(), 2.0 * 0.25); + assert_relative_eq!( + angular_next.conf[0].rotation.angle_to(&expected_rotation), + 0.0, + epsilon = 1e-12 + ); + assert_relative_eq!( + angular_next.conf[0].rotation.quaternion().norm(), + 1.0, + epsilon = 1e-12 + ); +} + +#[test] +fn euler_velocity_update_matches_forward_dynamics() { + let model = one_body_model::<1>(JointType::Revolute(Axis::Z)); + let state = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::zeros(), + }; + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let eta = na::SVector::::from_element(2.0); + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + let acceleration = model.forward_dynamics_ab( + &state.conf, + &state.mu, + rigid_body_forces, + &thruster_forces, + &eta, + &zero3, + &zero3, + ); + let dt = 0.125; + + let next = model.step_dynamics( + &state, + input, + IntegrationOptions { + dt, + method: IntegrationMethod::SemiImplicitEuler, + }, + ); + + assert_relative_eq!(next.mu, state.mu + dt * acceleration, epsilon = 1e-12); +} + +#[test] +fn rk4_matches_constant_velocity_scalar_motion() { + let model = one_body_model::<1>(JointType::Revolute(Axis::Z)); + let state = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::from_element(0.75), + }; + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let eta = na::SVector::::zeros(); + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + + let next = model.step_dynamics( + &state, + input, + IntegrationOptions { + dt: 0.4, + method: IntegrationMethod::Rk4, + }, + ); + let expected_rotation = na::UnitQuaternion::from_axis_angle(&Vector3::z_axis(), 0.75 * 0.4); + + assert_relative_eq!( + next.conf[0].rotation.angle_to(&expected_rotation), + 0.0, + epsilon = 1e-12 + ); + assert_relative_eq!(next.mu, state.mu, epsilon = 1e-12); +} + +#[test] +fn rk4_six_dof_composes_noncommuting_stage_twists() { + let model = one_body_model::<6>(JointType::SixDOF); + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let mut eta = na::SVector::::zeros(); + eta[4] = 3.0; + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + let mut initial_mu = na::SVector::::zeros(); + initial_mu[3] = 1.5; + initial_mu[5] = 0.7; + let state = DynamicsState::<1, 6> { + conf: vec![na::Isometry3::identity()], + mu: initial_mu, + }; + let dt = 0.4; + + let k1_mu = model.forward_dynamics_ab( + &state.conf, + &state.mu, + rigid_body_forces, + &thruster_forces, + &eta, + &zero3, + &zero3, + ); + let k1_conf_velocity = state.mu; + let state2 = DynamicsState::<1, 6> { + conf: vec![state.conf[0] * exp_se3(&(k1_conf_velocity * (0.5 * dt)))], + mu: state.mu + 0.5 * dt * k1_mu, + }; + let k2_mu = model.forward_dynamics_ab( + &state2.conf, + &state2.mu, + rigid_body_forces, + &thruster_forces, + &eta, + &zero3, + &zero3, + ); + let k2_conf_velocity = state2.mu; + let state3 = DynamicsState::<1, 6> { + conf: vec![state.conf[0] * exp_se3(&(k2_conf_velocity * (0.5 * dt)))], + mu: state.mu + 0.5 * dt * k2_mu, + }; + let k3_mu = model.forward_dynamics_ab( + &state3.conf, + &state3.mu, + rigid_body_forces, + &thruster_forces, + &eta, + &zero3, + &zero3, + ); + let k3_conf_velocity = state3.mu; + let state4 = DynamicsState::<1, 6> { + conf: vec![state.conf[0] * exp_se3(&(k3_conf_velocity * dt))], + mu: state.mu + dt * k3_mu, + }; + let _k4_mu = model.forward_dynamics_ab( + &state4.conf, + &state4.mu, + rigid_body_forces, + &thruster_forces, + &eta, + &zero3, + &zero3, + ); + let k4_conf_velocity = state4.mu; + + let expected_delta = exp_se3(&(k1_conf_velocity * (dt / 6.0))) + * exp_se3(&(k2_conf_velocity * (dt / 3.0))) + * exp_se3(&(k3_conf_velocity * (dt / 3.0))) + * exp_se3(&(k4_conf_velocity * (dt / 6.0))); + let averaged_velocity = + (k1_conf_velocity + 2.0 * k2_conf_velocity + 2.0 * k3_conf_velocity + k4_conf_velocity) + / 6.0; + let averaged_delta = exp_se3(&(averaged_velocity * dt)); + + let next = model.step_dynamics( + &state, + input, + IntegrationOptions { + dt, + method: IntegrationMethod::Rk4, + }, + ); + + assert!( + expected_delta.rotation.angle_to(&averaged_delta.rotation) > 1e-4, + "test setup must exercise non-commuting stage rotations" + ); + assert_relative_eq!( + next.conf[0].rotation.angle_to(&expected_delta.rotation), + 0.0, + epsilon = 1e-12 + ); + assert_relative_eq!( + next.conf[0].translation.vector, + expected_delta.translation.vector, + epsilon = 1e-12 + ); +} + +#[test] +fn try_step_dynamics_validates_inputs() { + let model = one_body_model::<1>(JointType::Revolute(Axis::Z)); + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let thruster_forces = vec![Vector6::zeros()]; + let eta = na::SVector::::zeros(); + let zero3 = Vector3::zeros(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &thruster_forces, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + let valid_state = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::zeros(), + }; + + assert_eq!( + model + .try_step_dynamics( + &valid_state, + input, + IntegrationOptions { + dt: f64::NAN, + method: IntegrationMethod::SemiImplicitEuler, + }, + ) + .unwrap_err(), + "dt must be finite and non-negative" + ); + + let bad_conf_state = DynamicsState::<1, 1> { + conf: Vec::new(), + mu: na::SVector::::zeros(), + }; + assert_eq!( + model + .try_step_dynamics( + &bad_conf_state, + input, + IntegrationOptions { + dt: 0.1, + method: IntegrationMethod::SemiImplicitEuler, + }, + ) + .unwrap_err(), + "conf length mismatch" + ); + + let empty_thrusters: Vec = Vec::new(); + let bad_input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &empty_thrusters, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + assert_eq!( + model + .try_step_dynamics( + &valid_state, + bad_input, + IntegrationOptions { + dt: 0.1, + method: IntegrationMethod::SemiImplicitEuler, + }, + ) + .unwrap_err(), + "thruster_forces length mismatch" + ); +} + +#[test] +#[should_panic(expected = "thruster_forces length mismatch")] +fn step_dynamics_panics_with_validation_reason() { + let model = one_body_model::<1>(JointType::Revolute(Axis::Z)); + let rigid_body_forces = |_: &[na::Isometry3], _: &[Vector6]| -> na::SMatrix { + na::SMatrix::::zeros() + }; + let eta = na::SVector::::zeros(); + let zero3 = Vector3::zeros(); + let empty_thrusters: Vec = Vec::new(); + let input = DynamicsStepInput { + rigid_body_forces: &rigid_body_forces, + thruster_forces: &empty_thrusters, + eta: &eta, + lin_vel_current: &zero3, + lin_accel_current: &zero3, + }; + let state = DynamicsState::<1, 1> { + conf: vec![na::Isometry3::identity()], + mu: na::SVector::::zeros(), + }; + + model.step_dynamics( + &state, + input, + IntegrationOptions { + dt: 0.1, + method: IntegrationMethod::SemiImplicitEuler, + }, + ); +}