From fb20d72ee29de9311a81aec6eb9f02fd2aa35fc4 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 20 Feb 2022 12:55:00 +0100 Subject: Joint API and joint motors improvements --- src/dynamics/joint/fixed_joint.rs | 93 +++- src/dynamics/joint/generic_joint.rs | 501 +++++++++++++++++++++ src/dynamics/joint/impulse_joint/impulse_joint.rs | 4 +- .../joint/impulse_joint/impulse_joint_set.rs | 4 +- src/dynamics/joint/joint_data.rs | 275 ----------- src/dynamics/joint/mod.rs | 12 +- src/dynamics/joint/motor_model.rs | 49 +- .../joint/multibody_joint/multibody_joint.rs | 40 +- .../joint/multibody_joint/multibody_joint_set.rs | 4 +- .../joint/multibody_joint/unit_multibody_joint.rs | 27 +- src/dynamics/joint/prismatic_joint.rs | 188 ++++++-- src/dynamics/joint/revolute_joint.rs | 171 +++++-- src/dynamics/joint/spherical_joint.rs | 140 +++++- 13 files changed, 1055 insertions(+), 453 deletions(-) create mode 100644 src/dynamics/joint/generic_joint.rs delete mode 100644 src/dynamics/joint/joint_data.rs (limited to 'src/dynamics/joint') diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index c7ca904..192b7d9 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,10 +1,11 @@ -use crate::dynamics::{JointAxesMask, JointData}; +use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::math::{Isometry, Point, Real}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct FixedJoint { - data: JointData, + data: GenericJoint, } impl Default for FixedJoint { @@ -14,48 +15,100 @@ impl Default for FixedJoint { } impl FixedJoint { + #[must_use] pub fn new() -> Self { - #[cfg(feature = "dim2")] - let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X; - #[cfg(feature = "dim3")] - let mask = JointAxesMask::X - | JointAxesMask::Y - | JointAxesMask::Z - | JointAxesMask::ANG_X - | JointAxesMask::ANG_Y - | JointAxesMask::ANG_Z; - - let data = JointData::default().lock_axes(mask); + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build(); Self { data } } + #[must_use] + pub fn local_frame1(&self) -> &Isometry { + &self.data.local_frame1 + } + + pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { + self.data.set_local_frame1(local_frame); + self + } + + #[must_use] + pub fn local_frame2(&self) -> &Isometry { + &self.data.local_frame2 + } + + pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { + self.data.set_local_frame2(local_frame); + self + } + + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } +} + +impl Into for FixedJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq, Default)] +pub struct FixedJointBuilder(FixedJoint); + +impl FixedJointBuilder { + pub fn new() -> Self { + Self(FixedJoint::new()) + } + #[must_use] pub fn local_frame1(mut self, local_frame: Isometry) -> Self { - self.data = self.data.local_frame1(local_frame); + self.0.set_local_frame1(local_frame); self } #[must_use] pub fn local_frame2(mut self, local_frame: Isometry) -> Self { - self.data = self.data.local_frame2(local_frame); + self.0.set_local_frame2(local_frame); self } #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); self } + + #[must_use] + pub fn build(self) -> FixedJoint { + self.0 + } } -impl Into for FixedJoint { - fn into(self) -> JointData { - self.data +impl Into for FixedJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs new file mode 100644 index 0000000..7455b0d --- /dev/null +++ b/src/dynamics/joint/generic_joint.rs @@ -0,0 +1,501 @@ +use na::SimdRealField; + +use crate::dynamics::solver::MotorParameters; +use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint}; +use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; +use crate::utils::WBasis; + +#[cfg(feature = "dim3")] +use crate::dynamics::SphericalJoint; + +#[cfg(feature = "dim3")] +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + pub struct JointAxesMask: u8 { + const X = 1 << 0; + const Y = 1 << 1; + const Z = 1 << 2; + const ANG_X = 1 << 3; + const ANG_Y = 1 << 4; + const ANG_Z = 1 << 5; + const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + const LOCKED_SPHERICAL_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits; + const FREE_REVOLUTE_AXES = Self::ANG_X.bits; + const FREE_PRISMATIC_AXES = Self::X.bits; + const FREE_FIXED_AXES = 0; + const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + } +} + +#[cfg(feature = "dim2")] +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + pub struct JointAxesMask: u8 { + const X = 1 << 0; + const Y = 1 << 1; + const ANG_X = 1 << 2; + const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits; + const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::ANG_X.bits; + const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::ANG_X.bits; + const FREE_REVOLUTE_AXES = Self::ANG_X.bits; + const FREE_PRISMATIC_AXES = Self::X.bits; + const FREE_FIXED_AXES = 0; + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub enum JointAxis { + X = 0, + Y, + #[cfg(feature = "dim3")] + Z, + AngX, + #[cfg(feature = "dim3")] + AngY, + #[cfg(feature = "dim3")] + AngZ, +} + +impl From for JointAxesMask { + fn from(axis: JointAxis) -> Self { + JointAxesMask::from_bits(1 << axis as usize).unwrap() + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointLimits { + pub min: N, + pub max: N, + pub impulse: N, +} + +impl> Default for JointLimits { + fn default() -> Self { + Self { + min: -N::splat(Real::MAX), + max: N::splat(Real::MAX), + impulse: N::splat(0.0), + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointMotor { + pub target_vel: Real, + pub target_pos: Real, + pub stiffness: Real, + pub damping: Real, + pub max_force: Real, + pub impulse: Real, + pub model: MotorModel, +} + +impl Default for JointMotor { + fn default() -> Self { + Self { + target_pos: 0.0, + target_vel: 0.0, + stiffness: 0.0, + damping: 0.0, + max_force: Real::MAX, + impulse: 0.0, + model: MotorModel::AccelerationBased, // VelocityBased, + } + } +} + +impl JointMotor { + pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters { + let (erp_inv_dt, cfm_coeff, cfm_gain) = + self.model + .combine_coefficients(dt, self.stiffness, self.damping); + MotorParameters { + erp_inv_dt, + cfm_coeff, + cfm_gain, + // keep_lhs, + target_pos: self.target_pos, + target_vel: self.target_vel, + max_impulse: self.max_force * dt, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct GenericJoint { + pub local_frame1: Isometry, + pub local_frame2: Isometry, + pub locked_axes: JointAxesMask, + pub limit_axes: JointAxesMask, + pub motor_axes: JointAxesMask, + pub limits: [JointLimits; SPATIAL_DIM], + pub motors: [JointMotor; SPATIAL_DIM], +} + +impl Default for GenericJoint { + fn default() -> Self { + Self { + local_frame1: Isometry::identity(), + local_frame2: Isometry::identity(), + locked_axes: JointAxesMask::empty(), + limit_axes: JointAxesMask::empty(), + motor_axes: JointAxesMask::empty(), + limits: [JointLimits::default(); SPATIAL_DIM], + motors: [JointMotor::default(); SPATIAL_DIM], + } + } +} + +impl GenericJoint { + #[must_use] + pub fn new(locked_axes: JointAxesMask) -> Self { + *Self::default().lock_axes(locked_axes) + } + + /// Can this joint use SIMD-accelerated constraint formulations? + pub(crate) fn supports_simd_constraints(&self) -> bool { + self.limit_axes.is_empty() && self.motor_axes.is_empty() + } + + fn complete_ang_frame(axis: UnitVector) -> Rotation { + let basis = axis.orthonormal_basis(); + + #[cfg(feature = "dim2")] + { + use na::{Matrix2, Rotation2, UnitComplex}; + let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + UnitComplex::from_rotation_matrix(&rotmat) + } + + #[cfg(feature = "dim3")] + { + use na::{Matrix3, Rotation3, UnitQuaternion}; + let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + UnitQuaternion::from_rotation_matrix(&rotmat) + } + } + + pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self { + self.locked_axes |= axes; + self + } + + pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { + self.local_frame1 = local_frame; + self + } + + pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { + self.local_frame2 = local_frame; + self + } + + #[must_use] + pub fn local_axis1(&self) -> UnitVector { + self.local_frame1 * Vector::x_axis() + } + + pub fn set_local_axis1(&mut self, local_axis: UnitVector) -> &mut Self { + self.local_frame1.rotation = Self::complete_ang_frame(local_axis); + self + } + + #[must_use] + pub fn local_axis2(&self) -> UnitVector { + self.local_frame2 * Vector::x_axis() + } + + pub fn set_local_axis2(&mut self, local_axis: UnitVector) -> &mut Self { + self.local_frame2.rotation = Self::complete_ang_frame(local_axis); + self + } + + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.local_frame1.translation.vector.into() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.local_frame1.translation.vector = anchor1.coords; + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.local_frame2.translation.vector.into() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.local_frame2.translation.vector = anchor2.coords; + self + } + + #[must_use] + pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { + let i = axis as usize; + if self.limit_axes.contains(axis.into()) { + Some(&self.limits[i]) + } else { + None + } + } + + pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self { + let i = axis as usize; + self.limit_axes |= axis.into(); + self.limits[i].min = limits[0]; + self.limits[i].max = limits[1]; + self + } + + #[must_use] + pub fn motor_model(&self, axis: JointAxis) -> Option { + let i = axis as usize; + if self.motor_axes.contains(axis.into()) { + Some(self.motors[i].model) + } else { + None + } + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self { + self.motors[axis as usize].model = model; + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity( + &mut self, + axis: JointAxis, + target_vel: Real, + factor: Real, + ) -> &mut Self { + self.set_motor( + axis, + self.motors[axis as usize].target_pos, + target_vel, + 0.0, + factor, + ) + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.set_motor(axis, target_pos, 0.0, stiffness, damping) + } + + pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self { + self.motors[axis as usize].max_force = max_force; + self + } + + #[must_use] + pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> { + let i = axis as usize; + if self.motor_axes.contains(axis.into()) { + Some(&self.motors[i]) + } else { + None + } + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.motor_axes |= axis.into(); + let i = axis as usize; + self.motors[i].target_vel = target_vel; + self.motors[i].target_pos = target_pos; + self.motors[i].stiffness = stiffness; + self.motors[i].damping = damping; + self + } +} + +macro_rules! joint_conversion_methods( + ($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => { + #[must_use] + pub fn $as_joint(&self) -> Option<&$Joint> { + if self.locked_axes == $axes { + // SAFETY: this is OK because the target joint type is + // a `repr(transparent)` newtype of `Joint`. + Some(unsafe { std::mem::transmute(self) }) + } else { + None + } + } + + #[must_use] + pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> { + if self.locked_axes == $axes { + // SAFETY: this is OK because the target joint type is + // a `repr(transparent)` newtype of `Joint`. + Some(unsafe { std::mem::transmute(self) }) + } else { + None + } + } + } +); + +impl GenericJoint { + joint_conversion_methods!( + as_revolute, + as_revolute_mut, + RevoluteJoint, + JointAxesMask::LOCKED_REVOLUTE_AXES + ); + joint_conversion_methods!( + as_fixed, + as_fixed_mut, + FixedJoint, + JointAxesMask::LOCKED_FIXED_AXES + ); + joint_conversion_methods!( + as_prismatic, + as_prismatic_mut, + PrismaticJoint, + JointAxesMask::LOCKED_PRISMATIC_AXES + ); + + #[cfg(feature = "dim3")] + joint_conversion_methods!( + as_spherical, + as_spherical_mut, + SphericalJoint, + JointAxesMask::LOCKED_SPHERICAL_AXES + ); +} + +#[derive(Copy, Clone, Debug)] +pub struct GenericJointBuilder(GenericJoint); + +impl GenericJointBuilder { + #[must_use] + pub fn new(locked_axes: JointAxesMask) -> Self { + Self(GenericJoint::new(locked_axes)) + } + + #[must_use] + pub fn lock_axes(mut self, axes: JointAxesMask) -> Self { + self.0.lock_axes(axes); + self + } + + #[must_use] + pub fn local_frame1(mut self, local_frame: Isometry) -> Self { + self.0.set_local_frame1(local_frame); + self + } + + #[must_use] + pub fn local_frame2(mut self, local_frame: Isometry) -> Self { + self.0.set_local_frame2(local_frame); + self + } + + #[must_use] + pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { + self.0.set_local_axis1(local_axis); + self + } + + #[must_use] + pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { + self.0.set_local_axis2(local_axis); + self + } + + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.0.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.0.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { + self.0.set_limits(axis, limits); + self + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] + pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { + self.0.set_motor_model(axis, model); + self + } + + /// Sets the target velocity this motor needs to reach. + #[must_use] + pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { + self.0.set_motor_velocity(axis, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + #[must_use] + pub fn motor_position( + mut self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.0 + .set_motor_position(axis, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + #[must_use] + pub fn set_motor( + mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.0 + .set_motor(axis, target_pos, target_vel, stiffness, damping); + self + } + + #[must_use] + pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self { + self.0.set_motor_max_force(axis, max_force); + self + } + + #[must_use] + pub fn build(self) -> GenericJoint { + self.0 + } +} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs index f3f4f7c..993542a 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{ImpulseJointHandle, JointData, RigidBodyHandle}; +use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle}; use crate::math::{Real, SpacialVector}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -10,7 +10,7 @@ pub struct ImpulseJoint { /// Handle to the second body attached to this joint. pub body2: RigidBodyHandle, - pub data: JointData, + pub data: GenericJoint, pub impulses: SpacialVector, // A joint needs to know its handle to simplify its removal. diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 51d5989..8677772 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -3,8 +3,8 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio use crate::data::arena::Arena; use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut}; +use crate::dynamics::{GenericJoint, RigidBodyHandle}; use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; -use crate::dynamics::{JointData, RigidBodyHandle}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -177,7 +177,7 @@ impl ImpulseJointSet { &mut self, body1: RigidBodyHandle, body2: RigidBodyHandle, - data: impl Into, + data: impl Into, ) -> ImpulseJointHandle { let data = data.into(); let handle = self.joint_ids.insert(0.into()); diff --git a/src/dynamics/joint/joint_data.rs b/src/dynamics/joint/joint_data.rs deleted file mode 100644 index b1ad6c6..0000000 --- a/src/dynamics/joint/joint_data.rs +++ /dev/null @@ -1,275 +0,0 @@ -use crate::dynamics::solver::MotorParameters; -use crate::dynamics::MotorModel; -use crate::math::{Isometry, Point, Real, Rotation, UnitVector, SPATIAL_DIM}; -use crate::utils::WBasis; - -#[cfg(feature = "dim3")] -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - pub struct JointAxesMask: u8 { - const FREE = 0; - const X = 1 << 0; - const Y = 1 << 1; - const Z = 1 << 2; - const ANG_X = 1 << 3; - const ANG_Y = 1 << 4; - const ANG_Z = 1 << 5; - } -} - -#[cfg(feature = "dim2")] -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - pub struct JointAxesMask: u8 { - const FREE = 0; - const X = 1 << 0; - const Y = 1 << 1; - const ANG_X = 1 << 2; - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub enum JointAxis { - X = 0, - Y, - #[cfg(feature = "dim3")] - Z, - AngX, - #[cfg(feature = "dim3")] - AngY, - #[cfg(feature = "dim3")] - AngZ, -} - -impl From for JointAxesMask { - fn from(axis: JointAxis) -> Self { - JointAxesMask::from_bits(1 << axis as usize).unwrap() - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct JointLimits { - pub min: Real, - pub max: Real, - pub impulse: Real, -} - -impl Default for JointLimits { - fn default() -> Self { - Self { - min: -Real::MAX, - max: Real::MAX, - impulse: 0.0, - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct JointMotor { - pub target_vel: Real, - pub target_pos: Real, - pub stiffness: Real, - pub damping: Real, - pub max_impulse: Real, - pub impulse: Real, - pub model: MotorModel, -} - -impl Default for JointMotor { - fn default() -> Self { - Self { - target_pos: 0.0, - target_vel: 0.0, - stiffness: 0.0, - damping: 0.0, - max_impulse: Real::MAX, - impulse: 0.0, - model: MotorModel::VelocityBased, - } - } -} - -impl JointMotor { - pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters { - let (stiffness, damping, gamma, _keep_lhs) = - self.model - .combine_coefficients(dt, self.stiffness, self.damping); - MotorParameters { - stiffness, - damping, - gamma, - // keep_lhs, - target_pos: self.target_pos, - target_vel: self.target_vel, - max_impulse: self.max_impulse, - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct JointData { - pub local_frame1: Isometry, - pub local_frame2: Isometry, - pub locked_axes: JointAxesMask, - pub limit_axes: JointAxesMask, - pub motor_axes: JointAxesMask, - pub limits: [JointLimits; SPATIAL_DIM], - pub motors: [JointMotor; SPATIAL_DIM], -} - -impl Default for JointData { - fn default() -> Self { - Self { - local_frame1: Isometry::identity(), - local_frame2: Isometry::identity(), - locked_axes: JointAxesMask::FREE, - limit_axes: JointAxesMask::FREE, - motor_axes: JointAxesMask::FREE, - limits: [JointLimits::default(); SPATIAL_DIM], - motors: [JointMotor::default(); SPATIAL_DIM], - } - } -} - -impl JointData { - #[must_use] - pub fn new(locked_axes: JointAxesMask) -> Self { - Self::default().lock_axes(locked_axes) - } - - /// Can this joint use SIMD-accelerated constraint formulations? - pub fn supports_simd_constraints(&self) -> bool { - self.limit_axes.is_empty() && self.motor_axes.is_empty() - } - - #[must_use] - pub fn lock_axes(mut self, axes: JointAxesMask) -> Self { - self.locked_axes |= axes; - self - } - - fn complete_ang_frame(axis: UnitVector) -> Rotation { - let basis = axis.orthonormal_basis(); - - #[cfg(feature = "dim2")] - { - use na::{Matrix2, Rotation2, UnitComplex}; - let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]); - let rotmat = Rotation2::from_matrix_unchecked(mat); - UnitComplex::from_rotation_matrix(&rotmat) - } - - #[cfg(feature = "dim3")] - { - use na::{Matrix3, Rotation3, UnitQuaternion}; - let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]); - let rotmat = Rotation3::from_matrix_unchecked(mat); - UnitQuaternion::from_rotation_matrix(&rotmat) - } - } - - #[must_use] - pub fn local_frame1(mut self, local_frame: Isometry) -> Self { - self.local_frame1 = local_frame; - self - } - - #[must_use] - pub fn local_frame2(mut self, local_frame: Isometry) -> Self { - self.local_frame2 = local_frame; - self - } - - #[must_use] - pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { - self.local_frame1.rotation = Self::complete_ang_frame(local_axis); - self - } - - #[must_use] - pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { - self.local_frame2.rotation = Self::complete_ang_frame(local_axis); - self - } - - #[must_use] - pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.local_frame1.translation.vector = anchor1.coords; - self - } - - #[must_use] - pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.local_frame2.translation.vector = anchor2.coords; - self - } - - #[must_use] - pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { - let i = axis as usize; - self.limit_axes |= axis.into(); - self.limits[i].min = limits[0]; - self.limits[i].max = limits[1]; - self - } - - /// Set the spring-like model used by the motor to reach the desired target velocity and position. - #[must_use] - pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { - self.motors[axis as usize].model = model; - self - } - - /// Sets the target velocity this motor needs to reach. - #[must_use] - pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { - self.motor_axis( - axis, - self.motors[axis as usize].target_pos, - target_vel, - 0.0, - factor, - ) - } - - /// Sets the target angle this motor needs to reach. - #[must_use] - pub fn motor_position( - self, - axis: JointAxis, - target_pos: Real, - stiffness: Real, - damping: Real, - ) -> Self { - self.motor_axis(axis, target_pos, 0.0, stiffness, damping) - } - - /// Configure both the target angle and target velocity of the motor. - #[must_use] - pub fn motor_axis( - mut self, - axis: JointAxis, - target_pos: Real, - target_vel: Real, - stiffness: Real, - damping: Real, - ) -> Self { - self.motor_axes |= axis.into(); - let i = axis as usize; - self.motors[i].target_vel = target_vel; - self.motors[i].target_pos = target_pos; - self.motors[i].stiffness = stiffness; - self.motors[i].damping = damping; - self - } - - #[must_use] - pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self { - self.motors[axis as usize].max_impulse = max_impulse; - self - } -} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 6594b83..11b5c0f 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,17 +1,17 @@ -pub use self::fixed_joint::FixedJoint; +pub use self::fixed_joint::*; pub use self::impulse_joint::*; -pub use self::joint_data::*; +pub use self::generic_joint::*; pub use self::motor_model::MotorModel; pub use self::multibody_joint::*; -pub use self::prismatic_joint::PrismaticJoint; -pub use self::revolute_joint::RevoluteJoint; +pub use self::prismatic_joint::*; +pub use self::revolute_joint::*; #[cfg(feature = "dim3")] -pub use self::spherical_joint::SphericalJoint; +pub use self::spherical_joint::*; mod fixed_joint; mod impulse_joint; -mod joint_data; +mod generic_joint; mod motor_model; mod multibody_joint; mod prismatic_joint; diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs index e5a6549..e8ffffa 100644 --- a/src/dynamics/joint/motor_model.rs +++ b/src/dynamics/joint/motor_model.rs @@ -5,57 +5,40 @@ use crate::math::Real; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub enum MotorModel { /// The solved spring-like equation is: - /// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))` - /// - /// Here the `stiffness` is the ratio of position error to be solved at each timestep (like - /// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at - /// each timestep. - VelocityBased, - /// The solved spring-like equation is: - /// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))` + /// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)` AccelerationBased, - // /// The solved spring-like equation is: - // /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))` - // ForceBased, + /// The solved spring-like equation is: + /// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)` + ForceBased, } impl Default for MotorModel { fn default() -> Self { - MotorModel::VelocityBased + MotorModel::AccelerationBased } } impl MotorModel { /// Combines the coefficients used for solving the spring equation. /// - /// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs) - /// coefficients for the equivalent impulse-based equation. These new - /// coefficients must be used in the following way: - /// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`. - /// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`. - /// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero. + /// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain). pub fn combine_coefficients( self, dt: Real, stiffness: Real, damping: Real, - ) -> (Real, Real, Real, bool) { + ) -> (Real, Real, Real) { match self { - MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true), MotorModel::AccelerationBased => { - let effective_stiffness = stiffness * dt; - let effective_damping = damping * dt; - // TODO: Using gamma behaves very badly for some reasons. - // Maybe I got the formulation wrong, so let's keep it to 1.0 for now, - // and get back to this later. - // let gamma = effective_stiffness * dt + effective_damping; - (effective_stiffness, effective_damping, 1.0, true) - } // MotorModel::ForceBased => { - // let effective_stiffness = stiffness * dt; - // let effective_damping = damping * dt; - // let gamma = effective_stiffness * dt + effective_damping; - // (effective_stiffness, effective_damping, gamma, false) - // } + let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping); + let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping); + (erp_inv_dt, cfm_coeff, 0.0) + } + MotorModel::ForceBased => { + let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping); + let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping); + (erp_inv_dt, 0.0, cfm_gain) + } } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 1d14680..2f7a71e 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,6 +1,6 @@ use crate::dynamics::solver::AnyJointVelocityConstraint; use crate::dynamics::{ - joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink, + joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, RigidBodyVelocity, }; use crate::math::{ @@ -14,13 +14,13 @@ use na::{UnitQuaternion, Vector3}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug)] pub struct MultibodyJoint { - pub data: JointData, + pub data: GenericJoint, pub(crate) coords: SpacialVector, pub(crate) joint_rot: Rotation, } impl MultibodyJoint { - pub fn new(data: JointData) -> Self { + pub fn new(data: GenericJoint) -> Self { Self { data, coords: na::zero(), @@ -29,13 +29,13 @@ impl MultibodyJoint { } pub(crate) fn free(pos: Isometry) -> Self { - let mut result = Self::new(JointData::default()); + let mut result = Self::new(GenericJoint::default()); result.set_free_pos(pos); result } pub(crate) fn fixed(pos: Isometry) -> Self { - Self::new(FixedJoint::new().local_frame1(pos).into()) + Self::new(FixedJointBuilder::new().local_frame1(pos).build().into()) } pub(crate) fn set_free_pos(&mut self, pos: Isometry) { @@ -263,13 +263,20 @@ impl MultibodyJoint { for i in 0..DIM { if (locked_bits & (1 << i)) == 0 { - if (limit_bits & (1 << i)) != 0 { - joint::unit_joint_limit_constraint( + let limits = if (limit_bits & (1 << i)) != 0 { + Some([self.data.limits[i].min, self.data.limits[i].max]) + } else { + None + }; + + if (motor_bits & (1 << i)) != 0 { + joint::unit_joint_motor_constraint( params, multibody, link, - [self.data.limits[i].min, self.data.limits[i].max], + &self.data.motors[i], self.coords[i], + limits, dof_id + curr_free_dof, j_id, jacobians, @@ -277,12 +284,12 @@ impl MultibodyJoint { ); } - if (motor_bits & (1 << i)) != 0 { - joint::unit_joint_motor_constraint( + if (limit_bits & (1 << i)) != 0 { + joint::unit_joint_limit_constraint( params, multibody, link, - &self.data.motors[i], + [self.data.limits[i].min, self.data.limits[i].max], self.coords[i], dof_id + curr_free_dof, j_id, @@ -310,19 +317,23 @@ impl MultibodyJoint { // TODO: we should make special cases for multi-angular-dofs limits/motors for i in DIM..SPATIAL_DIM { if (locked_bits & (1 << i)) == 0 { - if (limit_bits & (1 << i)) != 0 { + let limits = if (limit_bits & (1 << i)) != 0 { + let limits = [self.data.limits[i].min, self.data.limits[i].max]; joint::unit_joint_limit_constraint( params, multibody, link, - [self.data.limits[i].min, self.data.limits[i].max], + limits, self.coords[i], dof_id + curr_free_dof, j_id, jacobians, constraints, ); - } + Some(limits) + } else { + None + }; if (motor_bits & (1 << i)) != 0 { joint::unit_joint_motor_constraint( @@ -331,6 +342,7 @@ impl MultibodyJoint { link, &self.data.motors[i], self.coords[i], + limits, dof_id + curr_free_dof, j_id, jacobians, diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 3e786e2..7e512a8 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,7 +1,7 @@ use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index}; use crate::dynamics::joint::MultibodyLink; use crate::dynamics::{ - IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, + GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodyType, }; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; @@ -112,7 +112,7 @@ impl MultibodyJointSet { &mut self, body1: RigidBodyHandle, body2: RigidBodyHandle, - data: impl Into, + data: impl Into, ) -> Option { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 3367108..42212be 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -26,7 +26,8 @@ pub fn unit_joint_limit_constraint( let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); + let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; let rhs_wo_bias = joint_velocity[dof_id]; @@ -54,6 +55,8 @@ pub fn unit_joint_limit_constraint( inv_lhs: crate::utils::inv(lhs), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, + cfm_coeff, + cfm_gain: 0.0, writeback_id: WritebackId::Limit(dof_id), }; @@ -71,11 +74,13 @@ pub fn unit_joint_motor_constraint( link: &MultibodyLink, motor: &JointMotor, curr_pos: Real, + limits: Option<[Real; 2]>, dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, constraints: &mut Vec, ) { + let inv_dt = params.inv_dt(); let ndofs = multibody.ndofs(); let joint_velocity = multibody.joint_velocity(link); @@ -93,14 +98,20 @@ pub fn unit_joint_motor_constraint( let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; let mut rhs_wo_bias = 0.0; - if motor_params.stiffness != 0.0 { - rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness; + if motor_params.erp_inv_dt != 0.0 { + rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt; } - if motor_params.damping != 0.0 { - let dvel = joint_velocity[dof_id]; - rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; - } + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + target_vel = target_vel.clamp( + (limits[0] - curr_pos) * inv_dt, + (limits[1] - curr_pos) * inv_dt, + ); + }; + + let dvel = joint_velocity[dof_id]; + rhs_wo_bias += dvel - target_vel; let constraint = JointGenericVelocityGroundConstraint { mj_lambda2: multibody.solver_id, @@ -109,6 +120,8 @@ pub fn unit_joint_motor_constraint( joint_id: usize::MAX, impulse: 0.0, impulse_bounds, + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, inv_lhs: crate::utils::inv(lhs), rhs: rhs_wo_bias, rhs_wo_bias, diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 92fabde..e3f7527 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,91 +1,215 @@ -use crate::dynamics::joint::{JointAxesMask, JointData}; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real, UnitVector}; +use super::{JointLimits, JointMotor}; + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct PrismaticJoint { - data: JointData, + data: GenericJoint, } impl PrismaticJoint { pub fn new(axis: UnitVector) -> Self { - #[cfg(feature = "dim2")] - let mask = JointAxesMask::Y | JointAxesMask::ANG_X; - #[cfg(feature = "dim3")] - let mask = JointAxesMask::Y - | JointAxesMask::Z - | JointAxesMask::ANG_X - | JointAxesMask::ANG_Y - | JointAxesMask::ANG_Z; - - let data = JointData::default() - .lock_axes(mask) + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES) .local_axis1(axis) - .local_axis2(axis); + .local_axis2(axis) + .build(); Self { data } } + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn local_axis1(&self) -> UnitVector { + self.data.local_axis1() + } + + pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { + self.data.set_local_axis1(axis1); + self + } + + #[must_use] + pub fn local_axis2(&self) -> UnitVector { + self.data.local_axis2() + } + + pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { + self.data.set_local_axis2(axis2); + self + } + + #[must_use] + pub fn motor(&self) -> Option<&JointMotor> { + self.data.motor(JointAxis::X) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::X, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { + self.data + .set_motor_velocity(JointAxis::X, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(JointAxis::X, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping); + self + } + + pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(JointAxis::X, max_force); + self + } + + #[must_use] + pub fn limits(&self) -> Option<&JointLimits> { + self.data.limits(JointAxis::X) + } + + pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(JointAxis::X, limits); + self + } +} + +impl Into for PrismaticJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +pub struct PrismaticJointBuilder(PrismaticJoint); + +impl PrismaticJointBuilder { + pub fn new(axis: UnitVector) -> Self { + Self(PrismaticJoint::new(axis)) + } + #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn local_axis1(mut self, axis1: UnitVector) -> Self { + self.0.set_local_axis1(axis1); + self + } + + #[must_use] + pub fn local_axis2(mut self, axis2: UnitVector) -> Self { + self.0.set_local_axis2(axis2); self } /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] pub fn motor_model(mut self, model: MotorModel) -> Self { - self.data = self.data.motor_model(JointAxis::X, model); + self.0.set_motor_model(model); self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { - self.data = self.data.motor_velocity(JointAxis::X, target_vel, factor); + self.0.set_motor_velocity(target_vel, factor); self } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self { - self.data = self - .data - .motor_position(JointAxis::X, target_pos, stiffness, damping); + self.0.set_motor_position(target_pos, stiffness, damping); self } /// Configure both the target angle and target velocity of the motor. - pub fn motor_axis( + #[must_use] + pub fn set_motor( mut self, target_pos: Real, target_vel: Real, stiffness: Real, damping: Real, ) -> Self { - self.data = self - .data - .motor_axis(JointAxis::X, target_pos, target_vel, stiffness, damping); + self.0.set_motor(target_pos, target_vel, stiffness, damping); self } - pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self { - self.data = self.data.motor_max_impulse(JointAxis::X, max_impulse); + #[must_use] + pub fn motor_max_force(mut self, max_force: Real) -> Self { + self.0.set_motor_max_force(max_force); self } #[must_use] - pub fn limit_axis(mut self, limits: [Real; 2]) -> Self { - self.data = self.data.limit_axis(JointAxis::X, limits); + pub fn limits(mut self, limits: [Real; 2]) -> Self { + self.0.set_limits(limits); self } + + #[must_use] + pub fn build(self) -> PrismaticJoint { + self.0 + } } -impl Into for PrismaticJoint { - fn into(self) -> JointData { - self.data +impl Into for PrismaticJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 9084c4d..a9d74f5 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -1,5 +1,5 @@ -use crate::dynamics::joint::{JointAxesMask, JointData}; -use crate::dynamics::{JointAxis, MotorModel}; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; +use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel}; use crate::math::{Point, Real}; #[cfg(feature = "dim3")] @@ -7,100 +7,197 @@ use crate::math::UnitVector; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct RevoluteJoint { - data: JointData, + data: GenericJoint, } impl RevoluteJoint { #[cfg(feature = "dim2")] pub fn new() -> Self { - let mask = JointAxesMask::X | JointAxesMask::Y; - - let data = JointData::default().lock_axes(mask); - Self { data } + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES); + Self { data: data.build() } } #[cfg(feature = "dim3")] pub fn new(axis: UnitVector) -> Self { - let mask = JointAxesMask::X - | JointAxesMask::Y - | JointAxesMask::Z - | JointAxesMask::ANG_Y - | JointAxesMask::ANG_Z; - - let data = JointData::default() - .lock_axes(mask) + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES) .local_axis1(axis) - .local_axis2(axis); + .local_axis2(axis) + .build(); Self { data } } - pub fn data(&self) -> &JointData { + pub fn data(&self) -> &GenericJoint { &self.data } + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn motor(&self) -> Option<&JointMotor> { + self.data.motor(JointAxis::AngX) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::AngX, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { + self.data + .set_motor_velocity(JointAxis::AngX, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(JointAxis::AngX, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(JointAxis::AngX, target_pos, target_vel, stiffness, damping); + self + } + + pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(JointAxis::AngX, max_force); + self + } + + #[must_use] + pub fn limits(&self) -> Option<&JointLimits> { + self.data.limits(JointAxis::AngX) + } + + pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(JointAxis::AngX, limits); + self + } +} + +impl Into for RevoluteJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct RevoluteJointBuilder(RevoluteJoint); + +impl RevoluteJointBuilder { + #[cfg(feature = "dim2")] + pub fn new() -> Self { + Self(RevoluteJoint::new()) + } + + #[cfg(feature = "dim3")] + pub fn new(axis: UnitVector) -> Self { + Self(RevoluteJoint::new(axis)) + } + #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); self } /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] pub fn motor_model(mut self, model: MotorModel) -> Self { - self.data = self.data.motor_model(JointAxis::AngX, model); + self.0.set_motor_model(model); self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { - self.data = self - .data - .motor_velocity(JointAxis::AngX, target_vel, factor); + self.0.set_motor_velocity(target_vel, factor); self } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self { - self.data = self - .data - .motor_position(JointAxis::AngX, target_pos, stiffness, damping); + self.0.set_motor_position(target_pos, stiffness, damping); self } /// Configure both the target angle and target velocity of the motor. - pub fn motor_axis( + #[must_use] + pub fn motor( mut self, target_pos: Real, target_vel: Real, stiffness: Real, damping: Real, ) -> Self { - self.data = - self.data - .motor_axis(JointAxis::AngX, target_pos, target_vel, stiffness, damping); + self.0.set_motor(target_pos, target_vel, stiffness, damping); self } - pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self { - self.data = self.data.motor_max_impulse(JointAxis::AngX, max_impulse); + #[must_use] + pub fn motor_max_force(mut self, max_force: Real) -> Self { + self.0.set_motor_max_force(max_force); self } #[must_use] - pub fn limit_axis(mut self, limits: [Real; 2]) -> Self { - self.data = self.data.limit_axis(JointAxis::AngX, limits); + pub fn limits(mut self, limits: [Real; 2]) -> Self { + self.0.set_limits(limits); self } + + #[must_use] + pub fn build(self) -> RevoluteJoint { + self.0 + } } -impl Into for RevoluteJoint { - fn into(self) -> JointData { - self.data +impl Into for RevoluteJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs index 194682d..3ff029e 100644 --- a/src/dynamics/joint/spherical_joint.rs +++ b/src/dynamics/joint/spherical_joint.rs @@ -1,11 +1,12 @@ -use crate::dynamics::joint::{JointAxesMask, JointData}; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct SphericalJoint { - data: JointData, + data: GenericJoint, } impl Default for SphericalJoint { @@ -16,40 +17,128 @@ impl Default for SphericalJoint { impl SphericalJoint { pub fn new() -> Self { - let data = - JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z); + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build(); Self { data } } - pub fn data(&self) -> &JointData { + pub fn data(&self) -> &GenericJoint { &self.data } + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self { + self.data.set_motor_model(axis, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity( + &mut self, + axis: JointAxis, + target_vel: Real, + factor: Real, + ) -> &mut Self { + self.data.set_motor_velocity(axis, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(axis, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(axis, target_pos, target_vel, stiffness, damping); + self + } + + pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(axis, max_force); + self + } + + pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(axis, limits); + self + } +} + +impl Into for SphericalJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct SphericalJointBuilder(SphericalJoint); + +impl Default for SphericalJointBuilder { + fn default() -> Self { + Self(SphericalJoint::new()) + } +} + +impl SphericalJointBuilder { + pub fn new() -> Self { + Self(SphericalJoint::new()) + } + #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); self } /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { - self.data = self.data.motor_model(axis, model); + self.0.set_motor_model(axis, model); self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { - self.data = self.data.motor_velocity(axis, target_vel, factor); + self.0.set_motor_velocity(axis, target_vel, factor); self } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position( mut self, axis: JointAxis, @@ -57,14 +146,14 @@ impl SphericalJoint { stiffness: Real, damping: Real, ) -> Self { - self.data = self - .data - .motor_position(axis, target_pos, stiffness, damping); + self.0 + .set_motor_position(axis, target_pos, stiffness, damping); self } /// Configure both the target angle and target velocity of the motor. - pub fn motor_axis( + #[must_use] + pub fn motor( mut self, axis: JointAxis, target_pos: Real, @@ -72,26 +161,31 @@ impl SphericalJoint { stiffness: Real, damping: Real, ) -> Self { - self.data = self - .data - .motor_axis(axis, target_pos, target_vel, stiffness, damping); + self.0 + .set_motor(axis, target_pos, target_vel, stiffness, damping); self } - pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self { - self.data = self.data.motor_max_impulse(axis, max_impulse); + #[must_use] + pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self { + self.0.set_motor_max_force(axis, max_force); self } #[must_use] - pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { - self.data = self.data.limit_axis(axis, limits); + pub fn limits(mut self, axis: JointAxis