diff options
Diffstat (limited to 'src/dynamics/joint')
| -rw-r--r-- | src/dynamics/joint/multibody_joint/multibody.rs | 30 | ||||
| -rw-r--r-- | src/dynamics/joint/multibody_joint/multibody_joint.rs | 14 |
2 files changed, 21 insertions, 23 deletions
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 2eb90eb..63e87e2 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -11,9 +11,7 @@ use crate::math::{ }; use crate::prelude::MultibodyJoint; use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix}; -use na::{ - self, DMatrix, DVector, DVectorSlice, DVectorSliceMut, Dynamic, OMatrix, SMatrix, SVector, LU, -}; +use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU}; #[repr(C)] #[derive(Copy, Clone, Debug, Default)] @@ -54,7 +52,7 @@ fn concat_rb_mass_matrix( result[(1, 1)] = mass.y; result[(2, 2)] = mass.z; result - .fixed_slice_mut::<ANG_DIM, ANG_DIM>(DIM, DIM) + .fixed_view_mut::<ANG_DIM, ANG_DIM>(DIM, DIM) .copy_from(&inertia); result } @@ -72,10 +70,10 @@ pub struct Multibody { body_jacobians: Vec<Jacobian<Real>>, // TODO: use sparse matrices? augmented_mass: DMatrix<Real>, - inv_augmented_mass: LU<Real, Dynamic, Dynamic>, + inv_augmented_mass: LU<Real, Dyn, Dyn>, acc_augmented_mass: DMatrix<Real>, - acc_inv_augmented_mass: LU<Real, Dynamic, Dynamic>, + acc_inv_augmented_mass: LU<Real, Dyn, Dyn>, ndofs: usize, pub(crate) root_is_dynamic: bool, @@ -85,8 +83,8 @@ pub struct Multibody { * Workspaces. */ workspace: MultibodyWorkspace, - coriolis_v: Vec<OMatrix<Real, Dim, Dynamic>>, - coriolis_w: Vec<OMatrix<Real, AngDim, Dynamic>>, + coriolis_v: Vec<OMatrix<Real, Dim, Dyn>>, + coriolis_w: Vec<OMatrix<Real, AngDim, Dyn>>, i_coriolis_dt: Jacobian<Real>, } impl Default for Multibody { @@ -234,7 +232,7 @@ impl Multibody { } /// The inverse augmented mass matrix of this multibody. - pub fn inv_augmented_mass(&self) -> &LU<Real, Dynamic, Dynamic> { + pub fn inv_augmented_mass(&self) -> &LU<Real, Dyn, Dyn> { &self.inv_augmented_mass } @@ -547,11 +545,11 @@ impl Multibody { if self.coriolis_v.len() != self.links.len() { self.coriolis_v.resize( self.links.len(), - OMatrix::<Real, Dim, Dynamic>::zeros(self.ndofs), + OMatrix::<Real, Dim, Dyn>::zeros(self.ndofs), ); self.coriolis_w.resize( self.links.len(), - OMatrix::<Real, AngDim, Dynamic>::zeros(self.ndofs), + OMatrix::<Real, AngDim, Dyn>::zeros(self.ndofs), ); self.i_coriolis_dt = Jacobian::zeros(self.ndofs); } @@ -730,9 +728,9 @@ impl Multibody { /// The generalized velocity at the multibody_joint of the given link. #[inline] - pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorSlice<Real> { + pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> { let ndofs = link.joint().ndofs(); - DVectorSlice::from_slice( + DVectorView::from_slice( &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs], ndofs, ) @@ -740,19 +738,19 @@ impl Multibody { /// The generalized accelerations of this multibodies. #[inline] - pub fn generalized_acceleration(&self) -> DVectorSlice<Real> { + pub fn generalized_acceleration(&self) -> DVectorView<Real> { self.accelerations.rows(0, self.ndofs) } /// The generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity(&self) -> DVectorSlice<Real> { + pub fn generalized_velocity(&self) -> DVectorView<Real> { self.velocities.rows(0, self.ndofs) } /// The mutable generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity_mut(&mut self) -> DVectorSliceMut<Real> { + pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> { self.velocities.rows_mut(0, self.ndofs) } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index c4d9adb..da650e6 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -4,10 +4,10 @@ use crate::dynamics::{ RigidBodyVelocity, }; use crate::math::{ - Isometry, JacobianSliceMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM, + Isometry, JacobianViewMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM, SPATIAL_DIM, }; -use na::{DVector, DVectorSliceMut}; +use na::{DVector, DVectorViewMut}; #[cfg(feature = "dim3")] use na::{UnitQuaternion, Vector3}; @@ -126,14 +126,14 @@ impl MultibodyJoint { } /// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`. - pub fn jacobian(&self, transform: &Rotation<Real>, out: &mut JacobianSliceMut<Real>) { + pub fn jacobian(&self, transform: &Rotation<Real>, out: &mut JacobianViewMut<Real>) { let locked_bits = self.data.locked_axes.bits(); let mut curr_free_dof = 0; for i in 0..DIM { if (locked_bits & (1 << i)) == 0 { let transformed_axis = transform * Vector::ith(i, 1.0); - out.fixed_slice_mut::<DIM, 1>(0, curr_free_dof) + out.fixed_view_mut::<DIM, 1>(0, curr_free_dof) .copy_from(&transformed_axis); curr_free_dof += 1; } @@ -153,7 +153,7 @@ impl MultibodyJoint { { let dof_id = (!locked_ang_bits).trailing_zeros() as usize; let rotmat = transform.to_rotation_matrix().into_inner(); - out.fixed_slice_mut::<ANG_DIM, 1>(DIM, curr_free_dof) + out.fixed_view_mut::<ANG_DIM, 1>(DIM, curr_free_dof) .copy_from(&rotmat.column(dof_id)); } } @@ -163,7 +163,7 @@ impl MultibodyJoint { #[cfg(feature = "dim3")] 3 => { let rotmat = transform.to_rotation_matrix(); - out.fixed_slice_mut::<3, 3>(3, curr_free_dof) + out.fixed_view_mut::<3, 3>(3, curr_free_dof) .copy_from(rotmat.matrix()); } _ => unreachable!(), @@ -213,7 +213,7 @@ impl MultibodyJoint { } /// Fill `out` with the non-zero entries of a damping that can be applied by default to ensure a good stability of the multibody_joint. - pub fn default_damping(&self, out: &mut DVectorSliceMut<Real>) { + pub fn default_damping(&self, out: &mut DVectorViewMut<Real>) { let locked_bits = self.data.locked_axes.bits(); let mut curr_free_dof = self.num_free_lin_dofs(); |