aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics/joint/multibody_joint/multibody.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics/joint/multibody_joint/multibody.rs')
-rw-r--r--src/dynamics/joint/multibody_joint/multibody.rs80
1 files changed, 36 insertions, 44 deletions
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index 617d447..753fa8f 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -1,11 +1,7 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
-use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
-#[cfg(feature = "dim3")]
-use crate::math::Matrix;
-use crate::math::{
- AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
-};
+use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, Velocity};
+use crate::math::*;
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU};
@@ -13,12 +9,12 @@ use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMat
#[repr(C)]
#[derive(Copy, Clone, Debug, Default)]
struct Force {
- linear: Vector<Real>,
- angular: AngVector<Real>,
+ linear: Vector,
+ angular: AngVector,
}
impl Force {
- fn new(linear: Vector<Real>, angular: AngVector<Real>) -> Self {
+ fn new(linear: Vector, angular: AngVector) -> Self {
Self { linear, angular }
}
@@ -28,10 +24,7 @@ impl Force {
}
#[cfg(feature = "dim2")]
-fn concat_rb_mass_matrix(
- mass: Vector<Real>,
- inertia: Real,
-) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
+fn concat_rb_mass_matrix(mass: Vector, inertia: Real) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
result[(0, 0)] = mass.x;
result[(1, 1)] = mass.y;
@@ -40,17 +33,14 @@ fn concat_rb_mass_matrix(
}
#[cfg(feature = "dim3")]
-fn concat_rb_mass_matrix(
- mass: Vector<Real>,
- inertia: Matrix<Real>,
-) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
+fn concat_rb_mass_matrix(mass: Vector, inertia: Matrix) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
result[(0, 0)] = mass.x;
result[(1, 1)] = mass.y;
result[(2, 2)] = mass.z;
result
.fixed_view_mut::<ANG_DIM, ANG_DIM>(DIM, DIM)
- .copy_from(&inertia);
+ .copy_from(&inertia.into());
result
}
@@ -375,7 +365,7 @@ impl Multibody {
let link = &self.links[i];
let rb = &bodies[link.rigid_body];
- let mut acc = RigidBodyVelocity::zero();
+ let mut acc = Velocity::zero();
if i != 0 {
let parent_id = link.parent_internal_id;
@@ -388,7 +378,7 @@ impl Multibody {
acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel);
#[cfg(feature = "dim3")]
{
- acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel);
+ acc.angvel += parent_rb.vels.angvel.cross(link.joint_velocity.angvel);
}
acc.linvel += parent_rb
@@ -411,7 +401,7 @@ impl Multibody {
#[cfg(feature = "dim3")]
{
- gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel));
+ gyroscopic = rb.vels.angvel.cross(rb_inertia * rb.vels.angvel);
}
#[cfg(feature = "dim2")]
{
@@ -500,7 +490,7 @@ impl Multibody {
let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
let shift_tr = (link.shift02).gcross_matrix_tr();
- link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
+ link_j_v.gemm(1.0, &shift_tr.into(), &parent_j_w, 1.0);
}
} else {
self.body_jacobians[i].fill(0.0);
@@ -522,7 +512,7 @@ impl Multibody {
let (mut link_j_v, link_j_w) =
link_j.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
let shift_tr = link.shift23.gcross_matrix_tr();
- link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
+ link_j_v.gemm(1.0, &shift_tr.into(), &link_j_w, 1.0);
}
}
}
@@ -608,27 +598,27 @@ impl Multibody {
// [c1 - c0].gcross() * (JDot + JDot/u * qdot)"
let shift_cross_tr = link.shift02.gcross_matrix_tr();
- coriolis_v.gemm(1.0, &shift_cross_tr, parent_coriolis_w, 1.0);
+ coriolis_v.gemm(1.0, &shift_cross_tr.into(), parent_coriolis_w, 1.0);
// JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
let dvel_cross = (rb.vels.angvel.gcross(link.shift02)
+ 2.0 * link.joint_velocity.linvel)
.gcross_matrix_tr();
- coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0);
+ coriolis_v.gemm(1.0, &dvel_cross.into(), &parent_j_w, 1.0);
// JDot/u * qdot
coriolis_v.gemm(
1.0,
- &link.joint_velocity.linvel.gcross_matrix_tr(),
+ &link.joint_velocity.linvel.gcross_matrix_tr().into(),
&parent_j_w,
1.0,
);
- coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr), &parent_j_w, 1.0);
+ coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr).into(), &parent_j_w, 1.0);
#[cfg(feature = "dim3")]
{
let vel_wrt_joint_w = link.joint_velocity.angvel.gcross_matrix();
- coriolis_w.gemm(-1.0, &vel_wrt_joint_w, &parent_j_w, 1.0);
+ coriolis_w.gemm(-1.0, &vel_wrt_joint_w.into(), &parent_j_w, 1.0);
}
// JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
@@ -644,13 +634,13 @@ impl Multibody {
);
let rb_joint_j_v = rb_joint_j.fixed_rows::<DIM>(0);
- coriolis_v_part.gemm(2.0, &parent_w, &rb_joint_j_v, 1.0);
+ coriolis_v_part.gemm(2.0, &parent_w.into(), &rb_joint_j_v, 1.0);
#[cfg(feature = "dim3")]
{
let rb_joint_j_w = rb_joint_j.fixed_rows::<ANG_DIM>(DIM);
let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs);
- coriolis_w_part.gemm(1.0, &parent_w, &rb_joint_j_w, 1.0);
+ coriolis_w_part.gemm(1.0, &parent_w.into(), &rb_joint_j_w, 1.0);
}
}
} else {
@@ -664,16 +654,16 @@ impl Multibody {
{
// [c3 - c2].gcross() * (JDot + JDot/u * qdot)
let shift_cross_tr = link.shift23.gcross_matrix_tr();
- coriolis_v.gemm(1.0, &shift_cross_tr, coriolis_w, 1.0);
+ coriolis_v.gemm(1.0, &shift_cross_tr.into(), coriolis_w, 1.0);
// JDot
let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr();
- coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0);
+ coriolis_v.gemm(1.0, &dvel_cross.into(), &rb_j_w, 1.0);
// JDot/u * qdot
coriolis_v.gemm(
1.0,
- &(rb.vels.angvel.gcross_matrix() * shift_cross_tr),
+ &(rb.vels.angvel.gcross_matrix() * shift_cross_tr).into(),
&rb_j_w,
1.0,
);
@@ -690,7 +680,7 @@ impl Multibody {
i_coriolis_dt_v.copy_from(coriolis_v);
i_coriolis_dt_v
.column_iter_mut()
- .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt)));
+ .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt).into()));
}
#[cfg(feature = "dim2")]
@@ -702,7 +692,7 @@ impl Multibody {
#[cfg(feature = "dim3")]
{
let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::<ANG_DIM>(DIM);
- i_coriolis_dt_w.gemm(dt, &rb_inertia, coriolis_w, 0.0);
+ i_coriolis_dt_w.gemm(dt, &rb_inertia.into(), coriolis_w, 0.0);
}
self.acc_augmented_mass
@@ -856,10 +846,15 @@ impl Multibody {
{
let parent_rb = &bodies[parent_link.rigid_body];
let link_rb = &bodies[link.rigid_body];
- let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
- let c2 = link.local_to_world
- * Point::from(link.joint.data.local_frame2.translation.vector);
- let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
+ let c0 = parent_link
+ .local_to_world
+ .transform_point(&parent_rb.mprops.local_mprops.local_com);
+ let c2 = link.local_to_world.transform_point(&Point::from(
+ link.joint.data.local_frame2.translation.into_vector(),
+ ));
+ let c3 = link
+ .local_to_world
+ .transform_point(&link_rb.mprops.local_mprops.local_com);
link.shift02 = c2 - c0;
link.shift23 = c3 - c2;
@@ -896,7 +891,7 @@ impl Multibody {
pub(crate) fn fill_jacobians(
&self,
link_id: usize,
- unit_force: Vector<Real>,
+ unit_force: Vector,
unit_torque: SVector<Real, ANG_DIM>,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
@@ -908,10 +903,7 @@ impl Multibody {
let wj_id = *j_id + self.ndofs;
let force = Force {
linear: unit_force,
- #[cfg(feature = "dim2")]
- angular: unit_torque[0],
- #[cfg(feature = "dim3")]
- angular: unit_torque,
+ angular: unit_torque.into(),
};
let link = &self.links[link_id];
generated by cgit (git 2.34.1) at 2026-01-16 09:19:17 +0000