From 5b80c4efbf93ad1294c9d3d390d8c8f090681b0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Wed, 10 Feb 2021 11:56:51 +0100 Subject: [PATCH 01/20] Start experimenting with a generic joint implementation for joint drives. --- src/dynamics/joint/generic_joint.rs | 46 ++ src/dynamics/joint/joint.rs | 23 +- src/dynamics/joint/mod.rs | 2 + src/dynamics/mod.rs | 2 +- .../generic_position_constraint.rs | 171 +++++++ .../generic_position_constraint_wide.rs | 51 ++ .../generic_velocity_constraint.rs | 460 +++++++++++++++++ .../generic_velocity_constraint_wide.rs | 472 ++++++++++++++++++ .../joint_constraint/joint_constraint.rs | 56 ++- .../joint_position_constraint.rs | 37 +- src/dynamics/solver/joint_constraint/mod.rs | 21 + src/lib.rs | 6 + src_testbed/nphysics_backend.rs | 5 + src_testbed/physx_backend.rs | 5 + 14 files changed, 1350 insertions(+), 7 deletions(-) create mode 100644 src/dynamics/joint/generic_joint.rs create mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs new file mode 100644 index 0000000..cfea537 --- /dev/null +++ b/src/dynamics/joint/generic_joint.rs @@ -0,0 +1,46 @@ +use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM}; + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that prevents all relative movement between two bodies. +/// +/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space. +pub struct GenericJoint { + /// The frame of reference for the first body affected by this joint, expressed in the local frame + /// of the first body. + pub local_anchor1: Isometry, + /// The frame of reference for the second body affected by this joint, expressed in the local frame + /// of the first body. + pub local_anchor2: Isometry, + /// The impulse applied to the first body affected by this joint. + /// + /// The impulse applied to the second body affected by this joint is given by `-impulse`. + /// This combines both linear and angular impulses: + /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. + /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. + pub impulse: SpacialVector, + + pub min_position: SpacialVector, + pub max_position: SpacialVector, + pub target_velocity: SpacialVector, + /// The maximum negative impulse the joint can apply on each DoF. Must be <= 0.0 + pub max_negative_impulse: SpacialVector, + /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0 + pub max_positive_impulse: SpacialVector, +} + +impl GenericJoint { + /// Creates a new fixed joint from the frames of reference of both bodies. + pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { + Self { + local_anchor1, + local_anchor2, + impulse: SpacialVector::zeros(), + min_position: SpacialVector::zeros(), + max_position: SpacialVector::zeros(), + target_velocity: SpacialVector::zeros(), + max_negative_impulse: SpacialVector::repeat(-Real::MAX), + max_positive_impulse: SpacialVector::repeat(Real::MAX), + } + } +} diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index 9fe6488..31c5e0a 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -1,6 +1,8 @@ #[cfg(feature = "dim3")] use crate::dynamics::RevoluteJoint; -use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle}; +use crate::dynamics::{ + BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle, +}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -17,6 +19,7 @@ pub enum JointParams { /// A revolute joint that removes all degrees of degrees of freedom between the affected /// bodies except for the translation along one axis. RevoluteJoint(RevoluteJoint), + GenericJoint(GenericJoint), } impl JointParams { @@ -26,8 +29,9 @@ impl JointParams { JointParams::BallJoint(_) => 0, JointParams::FixedJoint(_) => 1, JointParams::PrismaticJoint(_) => 2, + JointParams::GenericJoint(_) => 3, #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(_) => 3, + JointParams::RevoluteJoint(_) => 4, } } @@ -49,6 +53,15 @@ impl JointParams { } } + /// Gets a reference to the underlying generic joint, if `self` is one. + pub fn as_generic_joint(&self) -> Option<&GenericJoint> { + if let JointParams::GenericJoint(j) = self { + Some(j) + } else { + None + } + } + /// Gets a reference to the underlying prismatic joint, if `self` is one. pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> { if let JointParams::PrismaticJoint(j) = self { @@ -81,6 +94,12 @@ impl From for JointParams { } } +impl From for JointParams { + fn from(j: GenericJoint) -> Self { + JointParams::GenericJoint(j) + } +} + #[cfg(feature = "dim3")] impl From for JointParams { fn from(j: RevoluteJoint) -> Self { diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index b4dd60e..92dd715 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,5 +1,6 @@ pub use self::ball_joint::BallJoint; pub use self::fixed_joint::FixedJoint; +pub use self::generic_joint::GenericJoint; pub use self::joint::{Joint, JointParams}; pub(crate) use self::joint_set::{JointGraphEdge, JointIndex}; pub use self::joint_set::{JointHandle, JointSet}; @@ -9,6 +10,7 @@ pub use self::revolute_joint::RevoluteJoint; mod ball_joint; mod fixed_joint; +mod generic_joint; mod joint; mod joint_set; mod prismatic_joint; diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 8c38dc2..3b6a977 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -5,7 +5,7 @@ pub(crate) use self::joint::JointIndex; #[cfg(feature = "dim3")] pub use self::joint::RevoluteJoint; pub use self::joint::{ - BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, + BallJoint, FixedJoint, GenericJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, }; pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs new file mode 100644 index 0000000..f5138ea --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -0,0 +1,171 @@ +use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; +use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::math::{ + AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, + DIM, +}; +use crate::utils::{WAngularInertia, WCross}; +use na::{Vector3, Vector6}; + +// FIXME: review this code for the case where the center of masses are not the origin. +#[derive(Debug)] +pub(crate) struct GenericPositionConstraint { + position1: usize, + position2: usize, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, + + joint: GenericJoint, + + lin_impulse: Cell>, + ang_impulse: Cell>, +} + +impl GenericPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, joint: &GenericJoint) -> Self { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; + + Self { + local_anchor1: joint.local_anchor1, + local_anchor2: joint.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + joint: *joint, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = positions[self.position1 as usize]; + let mut position2 = positions[self.position2 as usize]; + + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + + let delta_pos = anchor1.inverse() * anchor2; + let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( + &self.joint, + self.im1, + self.im2, + self.ii1, + self.ii2, + r1, + r2, + false, + ); + + let lin_err = delta_pos.translation.vector * params.joint_erp; + let ang_err = delta_pos.rotation.scaled_axis() * params.joint_erp; + let err = Vector6::new( + lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + ); + let impulse = mass_matrix * err; + let lin_impulse = impulse.xyz(); + let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); + + position1.rotation = Rotation::new( + self.ii1 + .transform_vector(ang_impulse + r1.gcross(lin_impulse)), + ) * position1.rotation; + position2.rotation = Rotation::new( + self.ii2 + .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), + ) * position2.rotation; + + position1.translation.vector += self.im1 * lin_impulse; + position2.translation.vector -= self.im2 * lin_impulse; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct GenericPositionGroundConstraint { + position2: usize, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: Real, + ii2: AngularInertia, + joint: GenericJoint, +} + +impl GenericPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + flipped: bool, + ) -> Self { + let anchor1; + let local_anchor2; + + if flipped { + anchor1 = rb1.predicted_position * joint.local_anchor2; + local_anchor2 = joint.local_anchor1; + } else { + anchor1 = rb1.predicted_position * joint.local_anchor1; + local_anchor2 = joint.local_anchor2; + }; + + Self { + anchor1, + local_anchor2, + position2: rb2.active_set_offset, + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, + joint: *joint, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2 as usize]; + + let anchor2 = position2 * self.local_anchor2; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + + let delta_pos = self.anchor1.inverse() * anchor2; + let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( + &self.joint, + self.im2, + self.ii2, + r2, + false, + ); + + let lin_err = delta_pos.translation.vector * params.joint_erp; + let ang_err = Vector3::zeros(); // delta_pos.rotation.scaled_axis() * params.joint_erp; + let err = Vector6::new( + lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + ); + let impulse = mass_matrix * err; + let lin_impulse = impulse.xyz(); + let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); + + position2.rotation = Rotation::new( + self.ii2 + .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), + ) * position2.rotation; + position2.translation.vector -= self.im2 * lin_impulse; + + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs new file mode 100644 index 0000000..9ceea67 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs @@ -0,0 +1,51 @@ +use super::{GenericPositionConstraint, GenericPositionGroundConstraint}; +use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::math::{Isometry, Real, SIMD_WIDTH}; + +// TODO: this does not uses SIMD optimizations yet. +#[derive(Debug)] +pub(crate) struct WGenericPositionConstraint { + constraints: [GenericPositionConstraint; SIMD_WIDTH], +} + +impl WGenericPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} + +#[derive(Debug)] +pub(crate) struct WGenericPositionGroundConstraint { + constraints: [GenericPositionGroundConstraint; SIMD_WIDTH], +} + +impl WGenericPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs new file mode 100644 index 0000000..6f501d2 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -0,0 +1,460 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector}; +use crate::parry::math::SpatialVector; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim2")] +use na::{Matrix3, Vector3}; +#[cfg(feature = "dim3")] +use na::{Matrix6, Vector6, U3}; + +#[derive(Debug)] +pub(crate) struct GenericVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector, + max_positive_impulse: SpatialVector, + max_negative_impulse: SpatialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: Real, + im2: Real, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl GenericVelocityConstraint { + #[inline(always)] + pub fn compute_mass_matrix( + joint: &GenericJoint, + im1: Real, + im2: Real, + ii1: AngularInertia, + ii2: AngularInertia, + r1: Vector, + r2: Vector, + velocity_solver: bool, + ) -> Matrix6 { + let rmat1 = r1.gcross_matrix(); + let rmat2 = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + + // Adjust the mass matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + if velocity_solver { + for i in 0..6 { + if joint.max_negative_impulse[i] > -Real::MAX + || joint.max_positive_impulse[i] < Real::MAX + { + let diag = lhs[(i, i)]; + lhs.row_mut(i).fill(0.0); + lhs[(i, i)] = diag; + } + } + } else { + for i in 0..6 { + let diag = lhs[(i, i)]; + lhs.row_mut(i).fill(0.0); + lhs[(i, i)] = diag; + } + } + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + } + + // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + return lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + return lhs.cholesky().expect("Singular system.").inverse(); + } + + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + ) -> Self { + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r2 = anchor2.translation.vector - rb2.world_com.coords; + + let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); + let ang_dvel = -rb1.angvel + rb2.angvel; + + let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true); + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + let impulse = (joint.impulse * params.warmstart_coeff) + .inf(&joint.max_positive_impulse) + .sup(&joint.max_negative_impulse); + + GenericVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + impulse, + max_positive_impulse: joint.max_positive_impulse, + max_negative_impulse: joint.max_negative_impulse, + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + self.inv_lhs * dvel) + .sup(&self.max_negative_impulse) + .inf(&self.max_positive_impulse); + let effective_impulse = new_impulse - self.impulse; + self.impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} + +#[derive(Debug)] +pub(crate) struct GenericVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector, + max_positive_impulse: SpatialVector, + max_negative_impulse: SpatialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3, + + im2: Real, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, +} + +impl GenericVelocityGroundConstraint { + #[inline(always)] + pub fn compute_mass_matrix( + joint: &GenericJoint, + im2: Real, + ii2: AngularInertia, + r2: Vector, + velocity_solver: bool, + ) -> Matrix6 { + let rmat2 = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii2.transform_matrix(&rmat2); + let lhs11 = ii2.into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + + // Adjust the mass matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + if velocity_solver { + for i in 0..6 { + if joint.max_negative_impulse[i] > -Real::MAX + || joint.max_positive_impulse[i] < Real::MAX + { + let diag = lhs[(i, i)]; + lhs.row_mut(i).fill(0.0); + lhs[(i, i)] = diag; + } + } + } + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im2 + rmat2.x * rmat2.x * ii2; + let m12 = rmat2.x * rmat2.y * ii2; + let m22 = im2 + rmat2.y * rmat2.y * ii2; + let m13 = rmat2.x * ii2; + let m23 = rmat2.y * ii2; + let m33 = ii2; + lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + } + + #[cfg(feature = "dim2")] + return lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + return lhs.cholesky().expect("Singular system.").inverse(); + } + + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + joint: &GenericJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * joint.local_anchor2, + rb2.position * joint.local_anchor1, + ) + } else { + ( + rb1.position * joint.local_anchor1, + rb2.position * joint.local_anchor2, + ) + }; + + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r2 = anchor2.translation.vector - rb2.world_com.coords; + + let inv_lhs = Self::compute_mass_matrix(joint, im2, ii2, r2, true); + + let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); + let ang_dvel = rb2.angvel - rb1.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + let impulse = (joint.impulse * params.warmstart_coeff) + .inf(&joint.max_positive_impulse) + .sup(&joint.max_negative_impulse); + + GenericVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + impulse, + max_positive_impulse: joint.max_positive_impulse, + max_negative_impulse: joint.max_negative_impulse, + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + self.inv_lhs * dvel) + .sup(&self.max_negative_impulse) + .inf(&self.max_positive_impulse); + let effective_impulse = new_impulse - self.impulse; + self.impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs new file mode 100644 index 0000000..8a6be8c --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs @@ -0,0 +1,472 @@ +use simba::simd::SimdValue; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector, + Vector, SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix6, Vector6, U3}; +#[cfg(feature = "dim2")] +use { + na::{Matrix3, Vector3}, + parry::utils::SdpMatrix3, +}; + +#[derive(Debug)] +pub(crate) struct WGenericVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: SimdReal, + im2: SimdReal, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl WGenericVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii1 = ii1_sqrt.squared(); + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat1: CrossMatrix<_> = r1.gcross_matrix(); + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2); + let ang_dvel = -angvel1 + angvel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WGenericVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + ii1, + ii2, + ii1_sqrt, + ii2_sqrt, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse.extract(ii) + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WGenericVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im2: SimdReal, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, +} + +impl WGenericVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&GenericJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + let local_anchor2 = Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii2.transform_matrix(&rmat2); + let lhs11 = ii2.into_matrix(); + + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im2 + rmat2.x * rmat2.x * ii2; + let m12 = rmat2.x * rmat2.y * ii2; + let m22 = im2 + rmat2.y * rmat2.y * ii2; + let m13 = rmat2.x * ii2; + let m23 = rmat2.y * ii2; + let m33 = ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); + let ang_dvel = angvel2 - angvel1; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WGenericVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + ii2, + ii2_sqrt, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse.extract(ii) + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index ed6d758..12b8f77 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -7,12 +7,16 @@ use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{ WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint, - WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint, + WFixedVelocityGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint, + WGenericVelocityConstraint, WGenericVelocityGroundConstraint, WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint, }; #[cfg(feature = "dim3")] #[cfg(feature = "simd-is-enabled")] use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint}; +use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{ + GenericVelocityConstraint, GenericVelocityGroundConstraint, +}; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, @@ -34,6 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint { WFixedConstraint(WFixedVelocityConstraint), #[cfg(feature = "simd-is-enabled")] WFixedGroundConstraint(WFixedVelocityGroundConstraint), + GenericConstraint(GenericVelocityConstraint), + GenericGroundConstraint(GenericVelocityGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WGenericConstraint(WGenericVelocityConstraint), + #[cfg(feature = "simd-is-enabled")] + WGenericGroundConstraint(WGenericVelocityGroundConstraint), PrismaticConstraint(PrismaticVelocityConstraint), PrismaticGroundConstraint(PrismaticVelocityGroundConstraint), #[cfg(feature = "simd-is-enabled")] @@ -79,6 +89,9 @@ impl AnyJointVelocityConstraint { JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint( PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), ), + JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint( + GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), + ), #[cfg(feature = "dim3")] JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint( RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), @@ -109,6 +122,12 @@ impl AnyJointVelocityConstraint { params, joint_id, rbs1, rbs2, joints, )) } + JointParams::GenericJoint(_) => { + let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WGenericConstraint( + WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), + ) + } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -148,6 +167,11 @@ impl AnyJointVelocityConstraint { JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint( FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped), ), + JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint( + GenericVelocityGroundConstraint::from_params( + params, joint_id, rb1, rb2, p, flipped, + ), + ), JointParams::PrismaticJoint(p) => { AnyJointVelocityConstraint::PrismaticGroundConstraint( PrismaticVelocityGroundConstraint::from_params( @@ -199,6 +223,14 @@ impl AnyJointVelocityConstraint { ), ) } + JointParams::GenericJoint(_) => { + let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WGenericGroundConstraint( + WGenericVelocityGroundConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, flipped, + ), + ) + } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -235,6 +267,12 @@ impl AnyJointVelocityConstraint { AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] @@ -269,6 +307,12 @@ impl AnyJointVelocityConstraint { AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] @@ -311,6 +355,16 @@ impl AnyJointVelocityConstraint { AnyJointVelocityConstraint::WFixedGroundConstraint(c) => { c.writeback_impulses(joints_all) } + AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all), + AnyJointVelocityConstraint::GenericGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WGenericGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => { c.writeback_impulses(joints_all) diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 97a81ba..29acac6 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -1,6 +1,7 @@ use super::{ BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint, - FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint, + FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint, + PrismaticPositionConstraint, PrismaticPositionGroundConstraint, }; #[cfg(feature = "dim3")] use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; @@ -10,8 +11,8 @@ use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{ WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint, - WFixedPositionGroundConstraint, WPrismaticPositionConstraint, - WPrismaticPositionGroundConstraint, + WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint, + WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint, }; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; #[cfg(feature = "simd-is-enabled")] @@ -31,6 +32,12 @@ pub(crate) enum AnyJointPositionConstraint { WFixedJoint(WFixedPositionConstraint), #[cfg(feature = "simd-is-enabled")] WFixedGroundConstraint(WFixedPositionGroundConstraint), + GenericJoint(GenericPositionConstraint), + GenericGroundConstraint(GenericPositionGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WGenericJoint(WGenericPositionConstraint), + #[cfg(feature = "simd-is-enabled")] + WGenericGroundConstraint(WGenericPositionGroundConstraint), PrismaticJoint(PrismaticPositionConstraint), PrismaticGroundConstraint(PrismaticPositionGroundConstraint), #[cfg(feature = "simd-is-enabled")] @@ -61,6 +68,9 @@ impl AnyJointPositionConstraint { JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint( FixedPositionConstraint::from_params(rb1, rb2, p), ), + JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint( + GenericPositionConstraint::from_params(rb1, rb2, p), + ), JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint( PrismaticPositionConstraint::from_params(rb1, rb2, p), ), @@ -89,6 +99,12 @@ impl AnyJointPositionConstraint { rbs1, rbs2, joints, )) } + JointParams::GenericJoint(_) => { + let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params( + rbs1, rbs2, joints, + )) + } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -123,6 +139,9 @@ impl AnyJointPositionConstraint { JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint( FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped), ), + JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint( + GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped), + ), JointParams::PrismaticJoint(p) => { AnyJointPositionConstraint::PrismaticGroundConstraint( PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped), @@ -161,6 +180,12 @@ impl AnyJointPositionConstraint { WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) } + JointParams::GenericJoint(_) => { + let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WGenericGroundConstraint( + WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + ) + } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -193,6 +218,12 @@ impl AnyJointPositionConstraint { AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions), #[cfg(feature = "simd-is-enabled")] AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions), + AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions), + AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions), AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions), AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions), #[cfg(feature = "simd-is-enabled")] diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index 154ff83..b8e833e 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -18,6 +18,21 @@ pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocity pub(self) use fixed_velocity_constraint_wide::{ WFixedVelocityConstraint, WFixedVelocityGroundConstraint, }; +pub(self) use generic_position_constraint::{ + GenericPositionConstraint, GenericPositionGroundConstraint, +}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use generic_position_constraint_wide::{ + WGenericPositionConstraint, WGenericPositionGroundConstraint, +}; +pub(self) use generic_velocity_constraint::{ + GenericVelocityConstraint, GenericVelocityGroundConstraint, +}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use generic_velocity_constraint_wide::{ + WGenericVelocityConstraint, WGenericVelocityGroundConstraint, +}; + pub(crate) use joint_constraint::AnyJointVelocityConstraint; pub(crate) use joint_position_constraint::AnyJointPositionConstraint; pub(self) use prismatic_position_constraint::{ @@ -63,6 +78,12 @@ mod fixed_position_constraint_wide; mod fixed_velocity_constraint; #[cfg(feature = "simd-is-enabled")] mod fixed_velocity_constraint_wide; +mod generic_position_constraint; +#[cfg(feature = "simd-is-enabled")] +mod generic_position_constraint_wide; +mod generic_velocity_constraint; +#[cfg(feature = "simd-is-enabled")] +mod generic_velocity_constraint_wide; mod joint_constraint; mod joint_position_constraint; mod prismatic_position_constraint; diff --git a/src/lib.rs b/src/lib.rs index 564a758..b3ca3d4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -148,4 +148,10 @@ pub mod math { /// single contact constraint. #[cfg(feature = "dim3")] pub const MAX_MANIFOLD_POINTS: usize = 4; + + #[cfg(feature = "dim2")] + pub const SPATIAL_DIM: usize = 3; + + #[cfg(feature = "dim3")] + pub const SPATIAL_DIM: usize = 6; } diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index 8b88e28..cce56e8 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -122,6 +122,11 @@ impl NPhysicsWorld { nphysics_joints.insert(c); } + JointParams::GenericJoint(_) => { + eprintln!( + "Joint type currently unsupported by the nphysics backend: GenericJoint." + ) + } } } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 319736c..011a450 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -421,6 +421,11 @@ impl PhysxWorld { &frame2 as *const _, ); } + JointParams::GenericJoint(_) => { + eprintln!( + "Joint type currently unsupported by the nphysics backend: GenericJoint." + ) + } } } } From cc80e40067d100d0f519c9a20abb020726dd8514 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Thu, 11 Feb 2021 18:52:07 +0100 Subject: [PATCH 02/20] More experiments with the way the generic joint is stabilized. --- src/dynamics/joint/generic_joint.rs | 18 ++ .../generic_position_constraint.rs | 57 +++- .../generic_velocity_constraint.rs | 248 +++++++++++++++++- .../joint_constraint/joint_constraint.rs | 17 ++ .../joint_position_constraint.rs | 17 ++ 5 files changed, 332 insertions(+), 25 deletions(-) diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index cfea537..2aa9a51 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,4 +1,6 @@ +use crate::dynamics::RevoluteJoint; use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM}; +use crate::na::{Rotation3, UnitQuaternion}; #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -44,3 +46,19 @@ impl GenericJoint { } } } + +impl From for GenericJoint { + fn from(joint: RevoluteJoint) -> Self { + let basis1 = [joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1[..]); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2[..]); + let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); + let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); + + let mut result = Self::new(local_anchor1, local_anchor2); + result.min_position[3] = -Real::MAX; + result.max_position[3] = Real::MAX; + result + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index f5138ea..1e80311 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -1,4 +1,5 @@ use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; +use crate::dynamics::solver::DeltaVel; use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; use crate::math::{ AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, @@ -22,9 +23,6 @@ pub(crate) struct GenericPositionConstraint { ii2: AngularInertia, joint: GenericJoint, - - lin_impulse: Cell>, - ang_impulse: Cell>, } impl GenericPositionConstraint { @@ -58,7 +56,11 @@ impl GenericPositionConstraint { let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - let delta_pos = anchor1.inverse() * anchor2; + let delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( &self.joint, self.im1, @@ -70,11 +72,15 @@ impl GenericPositionConstraint { false, ); - let lin_err = delta_pos.translation.vector * params.joint_erp; - let ang_err = delta_pos.rotation.scaled_axis() * params.joint_erp; - let err = Vector6::new( - lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + let lin_dpos = delta_pos.translation.vector; + let ang_dpos = delta_pos.rotation.scaled_axis(); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, ); + let err = dpos + - dpos + .sup(&self.joint.min_position) + .inf(&self.joint.max_position); let impulse = mass_matrix * err; let lin_impulse = impulse.xyz(); let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); @@ -94,6 +100,15 @@ impl GenericPositionConstraint { positions[self.position1 as usize] = position1; positions[self.position2 as usize] = position2; } + + pub fn solve2( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + dpos: &mut [DeltaVel], + ) { + return; + } } #[derive(Debug)] @@ -142,7 +157,10 @@ impl GenericPositionGroundConstraint { let anchor2 = position2 * self.local_anchor2; let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - let delta_pos = self.anchor1.inverse() * anchor2; + let delta_pos = Isometry::from_parts( + anchor2.translation * self.anchor1.translation.inverse(), + anchor2.rotation * self.anchor1.rotation.inverse(), + ); let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( &self.joint, self.im2, @@ -151,11 +169,15 @@ impl GenericPositionGroundConstraint { false, ); - let lin_err = delta_pos.translation.vector * params.joint_erp; - let ang_err = Vector3::zeros(); // delta_pos.rotation.scaled_axis() * params.joint_erp; - let err = Vector6::new( - lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, + let lin_dpos = delta_pos.translation.vector; + let ang_dpos = delta_pos.rotation.scaled_axis(); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, ); + let err = dpos + - dpos + .sup(&self.joint.min_position) + .inf(&self.joint.max_position); let impulse = mass_matrix * err; let lin_impulse = impulse.xyz(); let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); @@ -168,4 +190,13 @@ impl GenericPositionGroundConstraint { positions[self.position2 as usize] = position2; } + + pub fn solve2( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + dpos: &mut [DeltaVel], + ) { + return; + } } diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index 6f501d2..db8010b 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -2,8 +2,8 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector}; -use crate::parry::math::SpatialVector; +use crate::math::{AngularInertia, Dim, Isometry, Real, SpacialVector, Vector, DIM}; +use crate::parry::math::{AngDim, SpatialVector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim2")] use na::{Matrix3, Vector3}; @@ -18,6 +18,8 @@ pub(crate) struct GenericVelocityConstraint { joint_id: JointIndex, impulse: SpacialVector, + pos_impulse: SpacialVector, + max_positive_impulse: SpatialVector, max_negative_impulse: SpatialVector, @@ -31,6 +33,8 @@ pub(crate) struct GenericVelocityConstraint { #[cfg(feature = "dim2")] rhs: Vector3, + pos_rhs: Vector6, + im1: Real, im2: Real, @@ -88,16 +92,11 @@ impl GenericVelocityConstraint { || joint.max_positive_impulse[i] < Real::MAX { let diag = lhs[(i, i)]; + lhs.column_mut(i).fill(0.0); lhs.row_mut(i).fill(0.0); lhs[(i, i)] = diag; } } - } else { - for i in 0..6 { - let diag = lhs[(i, i)]; - lhs.row_mut(i).fill(0.0); - lhs[(i, i)] = diag; - } } } @@ -144,13 +143,38 @@ impl GenericVelocityConstraint { let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true); #[cfg(feature = "dim2")] - let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); #[cfg(feature = "dim3")] - let rhs = Vector6::new( + let dvel = Vector6::new( lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, ); + let target_linvel = anchor2 * joint.target_velocity.xyz(); + let target_angvel = anchor2 * joint.target_velocity.fixed_rows::(DIM).into_owned(); + let target_vel = Vector6::new( + target_linvel.x, + target_linvel.y, + target_linvel.z, + target_angvel.x, + target_angvel.y, + target_angvel.z, + ); + + let rhs = dvel - dvel.sup(&target_vel).inf(&target_vel); + + let delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let lin_dpos = delta_pos.translation.vector; + let ang_dpos = delta_pos.rotation.scaled_axis(); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); + let pos_rhs = err * params.inv_dt() * params.joint_erp; + let impulse = (joint.impulse * params.warmstart_coeff) .inf(&joint.max_positive_impulse) .sup(&joint.max_negative_impulse); @@ -166,12 +190,14 @@ impl GenericVelocityConstraint { ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse, + pos_impulse: na::zero(), max_positive_impulse: joint.max_positive_impulse, max_negative_impulse: joint.max_negative_impulse, inv_lhs, r1, r2, rhs, + pos_rhs, } } @@ -200,6 +226,7 @@ impl GenericVelocityConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + return; let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; @@ -244,6 +271,99 @@ impl GenericVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } + pub fn solve2( + &mut self, + mj_lambdas: &mut [DeltaVel], + mj_lambdas_pos: &mut [DeltaVel], + ) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + let mut mj_lambda_pos1 = mj_lambdas_pos[self.mj_lambda1 as usize]; + let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; + + /* + * Solve velocity. + */ + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + self.inv_lhs * dvel) + .sup(&self.max_negative_impulse) + .inf(&self.max_positive_impulse); + let effective_impulse = new_impulse - self.impulse; + self.impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + /* + * Solve positions. + */ + + let ang_pos1 = self.ii1_sqrt.transform_vector(mj_lambda_pos1.angular); + let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular); + + let dlinpos = -mj_lambda_pos1.linear - ang_pos1.gcross(self.r1) + + mj_lambda_pos2.linear + + ang_pos2.gcross(self.r2); + let dangpos = -ang_pos1 + ang_pos2; + + #[cfg(feature = "dim3")] + let dpos = Vector6::new( + dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z, + ) + self.pos_rhs; + + let new_impulse = self.pos_impulse + self.inv_lhs * dpos; + let effective_impulse = new_impulse - self.pos_impulse; + self.pos_impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda_pos1.linear += self.im1 * lin_impulse; + mj_lambda_pos1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda_pos2.linear -= self.im2 * lin_impulse; + mj_lambda_pos2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + mj_lambdas_pos[self.mj_lambda1 as usize] = mj_lambda_pos1; + mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2; + } + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::GenericJoint(fixed) = &mut joint.params { @@ -259,6 +379,8 @@ pub(crate) struct GenericVelocityGroundConstraint { joint_id: JointIndex, impulse: SpacialVector, + pos_impulse: SpacialVector, + max_positive_impulse: SpatialVector, max_negative_impulse: SpatialVector, @@ -272,6 +394,8 @@ pub(crate) struct GenericVelocityGroundConstraint { #[cfg(feature = "dim2")] rhs: Vector3, + pos_rhs: Vector6, + im2: Real, ii2: AngularInertia, ii2_sqrt: AngularInertia, @@ -317,6 +441,7 @@ impl GenericVelocityGroundConstraint { || joint.max_positive_impulse[i] < Real::MAX { let diag = lhs[(i, i)]; + lhs.column_mut(i).fill(0.0); lhs.row_mut(i).fill(0.0); lhs[(i, i)] = diag; } @@ -374,11 +499,35 @@ impl GenericVelocityGroundConstraint { let ang_dvel = rb2.angvel - rb1.angvel; #[cfg(feature = "dim2")] - let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); #[cfg(feature = "dim3")] - let rhs = Vector6::new( + let dvel = Vector6::new( lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, ); + let target_linvel = anchor2 * joint.target_velocity.xyz(); + let target_angvel = anchor2 * joint.target_velocity.fixed_rows::(DIM).into_owned(); + let target_vel = Vector6::new( + target_linvel.x, + target_linvel.y, + target_linvel.z, + target_angvel.x, + target_angvel.y, + target_angvel.z, + ); + + let mut rhs = dvel - dvel.sup(&target_vel).inf(&target_vel); + + let delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let lin_dpos = delta_pos.translation.vector; + let ang_dpos = delta_pos.rotation.scaled_axis(); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); + let pos_rhs = err * params.inv_dt() * params.joint_erp; let impulse = (joint.impulse * params.warmstart_coeff) .inf(&joint.max_positive_impulse) @@ -391,11 +540,13 @@ impl GenericVelocityGroundConstraint { ii2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse, + pos_impulse: na::zero(), max_positive_impulse: joint.max_positive_impulse, max_negative_impulse: joint.max_negative_impulse, inv_lhs, r2, rhs, + pos_rhs, } } @@ -417,6 +568,7 @@ impl GenericVelocityGroundConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + return; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -450,6 +602,78 @@ impl GenericVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } + pub fn solve2( + &mut self, + mj_lambdas: &mut [DeltaVel], + mj_lambdas_pos: &mut [DeltaVel], + ) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; + + /* + * Solve velocities. + */ + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + self.inv_lhs * dvel) + .sup(&self.max_negative_impulse) + .inf(&self.max_positive_impulse); + let effective_impulse = new_impulse - self.impulse; + self.impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + /* + * Solve positions. + */ + let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular); + + let dlinpos = mj_lambda_pos2.linear + ang_pos2.gcross(self.r2); + let dangpos = ang_pos2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinpos.x, dlinpos.y, dangpos) + self.rhs; + #[cfg(feature = "dim3")] + let dpos = Vector6::new( + dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z, + ) + self.pos_rhs; + + let new_impulse = self.pos_impulse + self.inv_lhs * dpos; + let effective_impulse = new_impulse - self.pos_impulse; + self.pos_impulse = new_impulse; + + let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda_pos2.linear -= self.im2 * lin_impulse; + mj_lambda_pos2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2; + } + // FIXME: duplicated code with the non-ground constraint. pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 12b8f77..78332e8 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -333,6 +333,23 @@ impl AnyJointVelocityConstraint { } } + pub fn solve2( + &mut self, + mj_lambdas: &mut [DeltaVel], + mj_lambdas_pos: &mut [DeltaVel], + ) { + match self { + AnyJointVelocityConstraint::GenericConstraint(c) => { + c.solve2(mj_lambdas, mj_lambdas_pos) + } + AnyJointVelocityConstraint::GenericGroundConstraint(c) => { + c.solve2(mj_lambdas, mj_lambdas_pos) + } + AnyJointVelocityConstraint::Empty => unreachable!(), + _ => {} + } + } + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all), diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 29acac6..0c35883 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -14,6 +14,7 @@ use super::{ WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint, WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint, }; +use crate::dynamics::solver::DeltaVel; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; @@ -241,4 +242,20 @@ impl AnyJointPositionConstraint { AnyJointPositionConstraint::Empty => unreachable!(), } } + + pub fn solve2( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + dpos: &mut [DeltaVel], + ) { + match self { + AnyJointPositionConstraint::GenericJoint(c) => c.solve2(params, positions, dpos), + AnyJointPositionConstraint::GenericGroundConstraint(c) => { + c.solve2(params, positions, dpos) + } + _ => {} + AnyJointPositionConstraint::Empty => unreachable!(), + } + } } From d9b6198fa0c7d933960030b7cff15cdaecb504e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Fri, 12 Feb 2021 16:00:57 +0100 Subject: [PATCH 03/20] Various generic joint fixes. --- src/dynamics/joint/generic_joint.rs | 86 +- .../generic_position_constraint.rs | 90 +- .../generic_velocity_constraint.rs | 775 +++++++++--------- 3 files changed, 449 insertions(+), 502 deletions(-) diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 2aa9a51..fa35520 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::RevoluteJoint; +use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint}; use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM}; use crate::na::{Rotation3, UnitQuaternion}; @@ -24,11 +24,16 @@ pub struct GenericJoint { pub min_position: SpacialVector, pub max_position: SpacialVector, - pub target_velocity: SpacialVector, - /// The maximum negative impulse the joint can apply on each DoF. Must be <= 0.0 - pub max_negative_impulse: SpacialVector, + pub min_velocity: SpacialVector, + pub max_velocity: SpacialVector, + /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0 + pub min_impulse: SpacialVector, /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0 - pub max_positive_impulse: SpacialVector, + pub max_impulse: SpacialVector, + /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0 + pub min_pos_impulse: SpacialVector, + /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0 + pub max_pos_impulse: SpacialVector, } impl GenericJoint { @@ -40,25 +45,78 @@ impl GenericJoint { impulse: SpacialVector::zeros(), min_position: SpacialVector::zeros(), max_position: SpacialVector::zeros(), - target_velocity: SpacialVector::zeros(), - max_negative_impulse: SpacialVector::repeat(-Real::MAX), - max_positive_impulse: SpacialVector::repeat(Real::MAX), + min_velocity: SpacialVector::zeros(), + max_velocity: SpacialVector::zeros(), + min_impulse: SpacialVector::repeat(-Real::MAX), + max_impulse: SpacialVector::repeat(Real::MAX), + min_pos_impulse: SpacialVector::repeat(-Real::MAX), + max_pos_impulse: SpacialVector::repeat(Real::MAX), } } + + pub fn free_dof(&mut self, dof: u8) { + self.min_position[dof as usize] = -Real::MAX; + self.max_position[dof as usize] = Real::MAX; + self.min_velocity[dof as usize] = -Real::MAX; + self.max_velocity[dof as usize] = Real::MAX; + self.min_impulse[dof as usize] = 0.0; + self.max_impulse[dof as usize] = 0.0; + self.min_pos_impulse[dof as usize] = 0.0; + self.max_pos_impulse[dof as usize] = 0.0; + } + + pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) { + self.min_position[dof as usize] = min; + self.max_position[dof as usize] = max; + } } impl From for GenericJoint { fn from(joint: RevoluteJoint) -> Self { - let basis1 = [joint.local_axis1, joint.basis1[0], joint.basis1[1]]; - let basis2 = [joint.local_axis2, joint.basis2[0], joint.basis2[1]]; - let quat1 = UnitQuaternion::from_basis_unchecked(&basis1[..]); - let quat2 = UnitQuaternion::from_basis_unchecked(&basis2[..]); + let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); let mut result = Self::new(local_anchor1, local_anchor2); - result.min_position[3] = -Real::MAX; - result.max_position[3] = Real::MAX; + result.free_dof(3); result } } + +impl From for GenericJoint { + fn from(joint: BallJoint) -> Self { + let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero()); + let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero()); + + let mut result = Self::new(local_anchor1, local_anchor2); + result.free_dof(3); + result.free_dof(4); + result.free_dof(5); + result + } +} + +impl From for GenericJoint { + fn from(joint: PrismaticJoint) -> Self { + let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]]; + let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]]; + let quat1 = UnitQuaternion::from_basis_unchecked(&basis1); + let quat2 = UnitQuaternion::from_basis_unchecked(&basis2); + let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1); + let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2); + + let mut result = Self::new(local_anchor1, local_anchor2); + result.free_dof(0); + result.set_dof_limits(0, joint.limits[0], joint.limits[1]); + result + } +} + +impl From for GenericJoint { + fn from(joint: FixedJoint) -> Self { + Self::new(joint.local_anchor1, joint.local_anchor2) + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 1e80311..9d74bf3 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -48,57 +48,7 @@ impl GenericPositionConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position1 = positions[self.position1 as usize]; - let mut position2 = positions[self.position2 as usize]; - - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; - let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; - let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - - let delta_pos = Isometry::from_parts( - anchor2.translation * anchor1.translation.inverse(), - anchor2.rotation * anchor1.rotation.inverse(), - ); - - let mass_matrix = GenericVelocityConstraint::compute_mass_matrix( - &self.joint, - self.im1, - self.im2, - self.ii1, - self.ii2, - r1, - r2, - false, - ); - - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - - dpos - .sup(&self.joint.min_position) - .inf(&self.joint.max_position); - let impulse = mass_matrix * err; - let lin_impulse = impulse.xyz(); - let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); - - position1.rotation = Rotation::new( - self.ii1 - .transform_vector(ang_impulse + r1.gcross(lin_impulse)), - ) * position1.rotation; - position2.rotation = Rotation::new( - self.ii2 - .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), - ) * position2.rotation; - - position1.translation.vector += self.im1 * lin_impulse; - position2.translation.vector -= self.im2 * lin_impulse; - - positions[self.position1 as usize] = position1; - positions[self.position2 as usize] = position2; + return; } pub fn solve2( @@ -152,43 +102,7 @@ impl GenericPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = positions[self.position2 as usize]; - - let anchor2 = position2 * self.local_anchor2; - let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; - - let delta_pos = Isometry::from_parts( - anchor2.translation * self.anchor1.translation.inverse(), - anchor2.rotation * self.anchor1.rotation.inverse(), - ); - let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix( - &self.joint, - self.im2, - self.ii2, - r2, - false, - ); - - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - - dpos - .sup(&self.joint.min_position) - .inf(&self.joint.max_position); - let impulse = mass_matrix * err; - let lin_impulse = impulse.xyz(); - let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]); - - position2.rotation = Rotation::new( - self.ii2 - .transform_vector(-ang_impulse - r2.gcross(lin_impulse)), - ) * position2.rotation; - position2.translation.vector -= self.im2 * lin_impulse; - - positions[self.position2 as usize] = position2; + return; } pub fn solve2( diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index db8010b..fe20e36 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -2,7 +2,8 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Dim, Isometry, Real, SpacialVector, Vector, DIM}; +use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM}; +use crate::na::UnitQuaternion; use crate::parry::math::{AngDim, SpatialVector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[cfg(feature = "dim2")] @@ -17,23 +18,10 @@ pub(crate) struct GenericVelocityConstraint { joint_id: JointIndex, - impulse: SpacialVector, - pos_impulse: SpacialVector, - - max_positive_impulse: SpatialVector, - max_negative_impulse: SpatialVector, - #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. - #[cfg(feature = "dim3")] - rhs: Vector6, - + inv_lhs: Matrix6, // TODO: replace by Cholesky? #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. - #[cfg(feature = "dim2")] - rhs: Vector3, - - pos_rhs: Vector6, + inv_lhs: Matrix3, im1: Real, im2: Real, @@ -46,79 +34,147 @@ pub(crate) struct GenericVelocityConstraint { r1: Vector, r2: Vector, + rot2: Rotation, + + vel: GenericConstraintPart, + pos: GenericConstraintPart, } impl GenericVelocityConstraint { #[inline(always)] - pub fn compute_mass_matrix( - joint: &GenericJoint, + pub fn compute_delassus_matrix( im1: Real, im2: Real, ii1: AngularInertia, ii2: AngularInertia, r1: Vector, r2: Vector, - velocity_solver: bool, + rot2: Rotation, ) -> Matrix6 { - let rmat1 = r1.gcross_matrix(); - let rmat2 = r2.gcross_matrix(); - - #[allow(unused_mut)] // For 2D - let mut lhs; + let rotmat = rot2.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; #[cfg(feature = "dim3")] { - let lhs00 = + let del00 = ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); - let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); - let lhs11 = (ii1 + ii2).into_matrix(); + let del10 = + rotmat.transpose() * (ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2)); + let del11 = (ii1 + ii2).quadform(&rotmat).into_matrix(); // Note that Cholesky only reads the lower-triangular part of the matrix - // so we don't need to fill lhs01. - lhs = Matrix6::zeros(); - lhs.fixed_slice_mut::(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); - lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); - - // Adjust the mass matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - if velocity_solver { - for i in 0..6 { - if joint.max_negative_impulse[i] > -Real::MAX - || joint.max_positive_impulse[i] < Real::MAX - { - let diag = lhs[(i, i)]; - lhs.column_mut(i).fill(0.0); - lhs.row_mut(i).fill(0.0); - lhs[(i, i)] = diag; - } - } - } + // so we don't need to fill del01. + let mut del = Matrix6::zeros(); + del.fixed_slice_mut::(0, 0) + .copy_from(&del00.into_matrix()); + del.fixed_slice_mut::(3, 0).copy_from(&del10); + del.fixed_slice_mut::(3, 3).copy_from(&del11); + del } // In 2D we just unroll the computation because // it's just easier that way. #[cfg(feature = "dim2")] { + panic!("Take the rotmat into account."); let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; let m13 = rmat1.x * ii1 + rmat2.x * ii2; let m23 = rmat1.y * ii1 + rmat2.y * ii2; let m33 = ii1 + ii2; - lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) } + } + + pub fn compute_velocity_error( + min_velocity: &SpatialVector, + max_velocity: &SpatialVector, + r1: &Vector, + r2: &Vector, + _anchor1: &Isometry, + anchor2: &Isometry, + rb1: &RigidBody, + rb2: &RigidBody, + ) -> SpatialVector { + let lin_dvel = -rb1.linvel - rb1.angvel.gcross(*r1) + rb2.linvel + rb2.angvel.gcross(*r2); + let ang_dvel = -rb1.angvel + rb2.angvel; + + let lin_dvel2 = anchor2.inverse_transform_vector(&lin_dvel); + let ang_dvel2 = anchor2.inverse_transform_vector(&ang_dvel); + + dbg!(lin_dvel); + dbg!(lin_dvel2); + + let min_linvel = min_velocity.xyz(); + let min_angvel = min_velocity.fixed_rows::(DIM).into_owned(); + let max_linvel = max_velocity.xyz(); + let max_angvel = max_velocity.fixed_rows::(DIM).into_owned(); + let lin_rhs = lin_dvel2 - lin_dvel2.sup(&min_linvel).inf(&max_linvel); + let ang_rhs = ang_dvel2 - ang_dvel2.sup(&min_angvel).inf(&max_angvel); - // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix - // for which a textbook inverse is still efficient. #[cfg(feature = "dim2")] - return lhs.try_inverse().expect("Singular system."); + return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs); + #[cfg(feature = "dim3")] - return lhs.cholesky().expect("Singular system.").inverse(); + return Vector6::new( + lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z, + ); + } + + pub fn compute_position_error( + joint: &GenericJoint, + anchor1: &Isometry, + anchor2: &Isometry, + ) -> SpatialVector { + let delta_pos = Isometry::from_parts( + anchor2.translation * anchor1.translation.inverse(), + anchor2.rotation * anchor1.rotation.inverse(), + ); + let lin_dpos = anchor2.inverse_transform_vector(&delta_pos.translation.vector); + let ang_dpos = anchor2.inverse_transform_vector(&delta_pos.rotation.scaled_axis()); + + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + + let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); + let error_code = + (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize; + + match error_code { + 0 => error, + 1 => { + let constrained_axis = error.rows(3, 3).iamin(); + let axis1 = anchor1 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let axis2 = anchor2 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2) + .unwrap_or(UnitQuaternion::identity()); + let ang_dpos = anchor2.inverse_transform_vector(&rot_cross.scaled_axis()); + let dpos = Vector6::new( + lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, + ); + + dpos - dpos.sup(&joint.min_position).inf(&joint.max_position) + } + 2 => { + // TODO + error + } + 3 => error, + _ => unreachable!(), + } } pub fn from_params( @@ -136,48 +192,67 @@ impl GenericVelocityConstraint { let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1.translation.vector - rb1.world_com.coords; let r2 = anchor2.translation.vector - rb2.world_com.coords; + let mut min_impulse = joint.min_impulse; + let mut max_impulse = joint.max_impulse; + let mut min_pos_impulse = joint.min_pos_impulse; + let mut max_pos_impulse = joint.max_pos_impulse; + let mut min_velocity = joint.min_velocity; + let mut max_velocity = joint.max_velocity; - let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); - let ang_dvel = -rb1.angvel + rb2.angvel; + let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2) + * params.inv_dt() + * params.joint_erp; - let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true); + for i in 0..6 { + if pos_rhs[i] < 0.0 { + min_impulse[i] = -Real::MAX; + min_pos_impulse[i] = -Real::MAX; + min_velocity[i] = 0.0; + } + if pos_rhs[i] > 0.0 { + max_impulse[i] = Real::MAX; + max_pos_impulse[i] = Real::MAX; + max_velocity[i] = 0.0; + } + } + let rhs = Self::compute_velocity_error( + &min_velocity, + &max_velocity, + &r1, + &r2, + &anchor1, + &anchor2, + rb1, + rb2, + ); + + let mut delassus = + Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, anchor2.rotation); + + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + for i in 0..6 { + if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + } + } + + // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. #[cfg(feature = "dim2")] - let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); - + let inv_lhs = delassus.try_inverse().expect("Singular system."); #[cfg(feature = "dim3")] - let dvel = Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); - - let target_linvel = anchor2 * joint.target_velocity.xyz(); - let target_angvel = anchor2 * joint.target_velocity.fixed_rows::(DIM).into_owned(); - let target_vel = Vector6::new( - target_linvel.x, - target_linvel.y, - target_linvel.z, - target_angvel.x, - target_angvel.y, - target_angvel.z, - ); - - let rhs = dvel - dvel.sup(&target_vel).inf(&target_vel); - - let delta_pos = Isometry::from_parts( - anchor2.translation * anchor1.translation.inverse(), - anchor2.rotation * anchor1.rotation.inverse(), - ); - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); - let pos_rhs = err * params.inv_dt() * params.joint_erp; + let inv_lhs = delassus.cholesky().expect("Singular system.").inverse(); let impulse = (joint.impulse * params.warmstart_coeff) - .inf(&joint.max_positive_impulse) - .sup(&joint.max_negative_impulse); + .inf(&max_impulse) + .sup(&min_impulse); GenericVelocityConstraint { joint_id, @@ -189,15 +264,22 @@ impl GenericVelocityConstraint { ii2, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse, - pos_impulse: na::zero(), - max_positive_impulse: joint.max_positive_impulse, - max_negative_impulse: joint.max_negative_impulse, inv_lhs, r1, r2, - rhs, - pos_rhs, + rot2: anchor2.rotation, + vel: GenericConstraintPart { + impulse, + min_impulse, + max_impulse, + rhs, + }, + pos: GenericConstraintPart { + impulse: na::zero(), + min_impulse: min_pos_impulse, + max_impulse: max_pos_impulse, + rhs: pos_rhs, + }, } } @@ -205,11 +287,11 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = self.impulse[2]; + let ang_impulse = self.rot2 * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::(3).into_owned(); mj_lambda1.linear += self.im1 * lin_impulse; mj_lambda1.angular += self @@ -227,48 +309,6 @@ impl GenericVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { return; - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) - + mj_lambda2.linear - + ang_vel2.gcross(self.r2); - let dangvel = -ang_vel1 + ang_vel2; - - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; - - let new_impulse = (self.impulse + self.inv_lhs * dvel) - .sup(&self.max_negative_impulse) - .inf(&self.max_positive_impulse); - let effective_impulse = new_impulse - self.impulse; - self.impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda1.linear += self.im1 * lin_impulse; - mj_lambda1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } pub fn solve2( @@ -281,82 +321,10 @@ impl GenericVelocityConstraint { let mut mj_lambda_pos1 = mj_lambdas_pos[self.mj_lambda1 as usize]; let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; - /* - * Solve velocity. - */ - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) - + mj_lambda2.linear - + ang_vel2.gcross(self.r2); - let dangvel = -ang_vel1 + ang_vel2; - - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; - - let new_impulse = (self.impulse + self.inv_lhs * dvel) - .sup(&self.max_negative_impulse) - .inf(&self.max_positive_impulse); - let effective_impulse = new_impulse - self.impulse; - self.impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda1.linear += self.im1 * lin_impulse; - mj_lambda1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - /* - * Solve positions. - */ - - let ang_pos1 = self.ii1_sqrt.transform_vector(mj_lambda_pos1.angular); - let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular); - - let dlinpos = -mj_lambda_pos1.linear - ang_pos1.gcross(self.r1) - + mj_lambda_pos2.linear - + ang_pos2.gcross(self.r2); - let dangpos = -ang_pos1 + ang_pos2; - - #[cfg(feature = "dim3")] - let dpos = Vector6::new( - dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z, - ) + self.pos_rhs; - - let new_impulse = self.pos_impulse + self.inv_lhs * dpos; - let effective_impulse = new_impulse - self.pos_impulse; - self.pos_impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda_pos1.linear += self.im1 * lin_impulse; - mj_lambda_pos1.angular += self - .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); - - mj_lambda_pos2.linear -= self.im2 * lin_impulse; - mj_lambda_pos2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + self.vel.impulse = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + self.pos.impulse = self + .pos + .solve(self, &mut mj_lambda_pos1, &mut mj_lambda_pos2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -367,7 +335,7 @@ impl GenericVelocityConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::GenericJoint(fixed) = &mut joint.params { - fixed.impulse = self.impulse; + fixed.impulse = self.vel.impulse; } } } @@ -378,94 +346,61 @@ pub(crate) struct GenericVelocityGroundConstraint { joint_id: JointIndex, - impulse: SpacialVector, - pos_impulse: SpacialVector, - - max_positive_impulse: SpatialVector, - max_negative_impulse: SpatialVector, - #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // FIXME: replace by Cholesky. - #[cfg(feature = "dim3")] - rhs: Vector6, - + inv_lhs: Matrix6, // TODO: replace by Cholesky? #[cfg(feature = "dim2")] - inv_lhs: Matrix3, // FIXME: replace by Cholesky. - #[cfg(feature = "dim2")] - rhs: Vector3, - - pos_rhs: Vector6, + inv_lhs: Matrix3, im2: Real, ii2: AngularInertia, ii2_sqrt: AngularInertia, r2: Vector, + rot2: Rotation, + + vel: GenericConstraintPart, + pos: GenericConstraintPart, } impl GenericVelocityGroundConstraint { #[inline(always)] - pub fn compute_mass_matrix( - joint: &GenericJoint, + pub fn compute_delassus_matrix( im2: Real, ii2: AngularInertia, r2: Vector, - velocity_solver: bool, + rot2: Rotation, ) -> Matrix6 { - let rmat2 = r2.gcross_matrix(); - - #[allow(unused_mut)] // For 2D. - let mut lhs; + let rotmat2 = rot2.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat2; #[cfg(feature = "dim3")] { - let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); - let lhs10 = ii2.transform_matrix(&rmat2); - let lhs11 = ii2.into_matrix(); + let del00 = ii2.quadform(&rmat2).add_diagonal(im2); + let del10 = rotmat2.transpose() * ii2.transform_matrix(&rmat2); + let del11 = ii2.quadform(&rotmat2).into_matrix(); // Note that Cholesky only reads the lower-triangular part of the matrix // so we don't need to fill lhs01. - lhs = Matrix6::zeros(); - lhs.fixed_slice_mut::(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); - lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); - - // Adjust the mass matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - if velocity_solver { - for i in 0..6 { - if joint.max_negative_impulse[i] > -Real::MAX - || joint.max_positive_impulse[i] < Real::MAX - { - let diag = lhs[(i, i)]; - lhs.column_mut(i).fill(0.0); - lhs.row_mut(i).fill(0.0); - lhs[(i, i)] = diag; - } - } - } + let mut del = Matrix6::zeros(); + del.fixed_slice_mut::(0, 0) + .copy_from(&del00.into_matrix()); + del.fixed_slice_mut::(3, 0).copy_from(&del10); + del.fixed_slice_mut::(3, 3).copy_from(&del11); + del } // In 2D we just unroll the computation because // it's just easier that way. #[cfg(feature = "dim2")] { + panic!("Properly handle the rotmat2"); let m11 = im2 + rmat2.x * rmat2.x * ii2; let m12 = rmat2.x * rmat2.y * ii2; let m22 = im2 + rmat2.y * rmat2.y * ii2; let m13 = rmat2.x * ii2; let m23 = rmat2.y * ii2; let m33 = ii2; - lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) } - - #[cfg(feature = "dim2")] - return lhs.try_inverse().expect("Singular system."); - #[cfg(feature = "dim3")] - return lhs.cholesky().expect("Singular system.").inverse(); } pub fn from_params( @@ -488,50 +423,70 @@ impl GenericVelocityGroundConstraint { ) }; - let r1 = anchor1.translation.vector - rb1.world_com.coords; let im2 = rb2.effective_inv_mass; let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; let r2 = anchor2.translation.vector - rb2.world_com.coords; + let mut min_impulse = joint.min_impulse; + let mut max_impulse = joint.max_impulse; + let mut min_pos_impulse = joint.min_pos_impulse; + let mut max_pos_impulse = joint.max_pos_impulse; + let mut min_velocity = joint.min_velocity; + let mut max_velocity = joint.max_velocity; - let inv_lhs = Self::compute_mass_matrix(joint, im2, ii2, r2, true); + let pos_rhs = GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2) + * params.inv_dt() + * params.joint_erp; - let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_dvel = rb2.angvel - rb1.angvel; + for i in 0..6 { + if pos_rhs[i] < 0.0 { + min_impulse[i] = -Real::MAX; + min_pos_impulse[i] = -Real::MAX; + min_velocity[i] = 0.0; + } + if pos_rhs[i] > 0.0 { + max_impulse[i] = Real::MAX; + max_pos_impulse[i] = Real::MAX; + max_velocity[i] = 0.0; + } + } + let rhs = GenericVelocityConstraint::compute_velocity_error( + &min_velocity, + &max_velocity, + &r1, + &r2, + &anchor1, + &anchor2, + rb1, + rb2, + ); + + let mut delassus = Self::compute_delassus_matrix(im2, ii2, r2, anchor2.rotation); + + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + for i in 0..6 { + if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + } + } + + // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. #[cfg(feature = "dim2")] - let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + let inv_lhs = delassus.try_inverse().expect("Singular system."); #[cfg(feature = "dim3")] - let dvel = Vector6::new( - lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, - ); - let target_linvel = anchor2 * joint.target_velocity.xyz(); - let target_angvel = anchor2 * joint.target_velocity.fixed_rows::(DIM).into_owned(); - let target_vel = Vector6::new( - target_linvel.x, - target_linvel.y, - target_linvel.z, - target_angvel.x, - target_angvel.y, - target_angvel.z, - ); - - let mut rhs = dvel - dvel.sup(&target_vel).inf(&target_vel); - - let delta_pos = Isometry::from_parts( - anchor2.translation * anchor1.translation.inverse(), - anchor2.rotation * anchor1.rotation.inverse(), - ); - let lin_dpos = delta_pos.translation.vector; - let ang_dpos = delta_pos.rotation.scaled_axis(); - let dpos = Vector6::new( - lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, - ); - let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position); - let pos_rhs = err * params.inv_dt() * params.joint_erp; + let inv_lhs = delassus.cholesky().expect("Singular system.").inverse(); let impulse = (joint.impulse * params.warmstart_coeff) - .inf(&joint.max_positive_impulse) - .sup(&joint.max_negative_impulse); + .inf(&max_impulse) + .sup(&min_impulse); GenericVelocityGroundConstraint { joint_id, @@ -539,25 +494,32 @@ impl GenericVelocityGroundConstraint { im2, ii2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse, - pos_impulse: na::zero(), - max_positive_impulse: joint.max_positive_impulse, - max_negative_impulse: joint.max_negative_impulse, inv_lhs, r2, - rhs, - pos_rhs, + rot2: anchor2.rotation, + vel: GenericConstraintPart { + impulse, + min_impulse, + max_impulse, + rhs, + }, + pos: GenericConstraintPart { + impulse: na::zero(), + min_impulse: min_pos_impulse, + max_impulse: max_pos_impulse, + rhs: pos_rhs, + }, } } pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = self.impulse[2]; + let ang_impulse = self.rot2 * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self @@ -569,37 +531,6 @@ impl GenericVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { return; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let dangvel = ang_vel2; - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; - - let new_impulse = (self.impulse + self.inv_lhs * dvel) - .sup(&self.max_negative_impulse) - .inf(&self.max_positive_impulse); - let effective_impulse = new_impulse - self.impulse; - self.impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } pub fn solve2( @@ -610,13 +541,47 @@ impl GenericVelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; - /* - * Solve velocities. - */ - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + self.vel.impulse = self.vel.solve_ground(self, &mut mj_lambda2); + self.pos.impulse = self.pos.solve_ground(self, &mut mj_lambda_pos2); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2; + } + + // TODO: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::GenericJoint(fixed) = &mut joint.params { + fixed.impulse = self.vel.impulse; + } + } +} + +#[derive(Debug)] +struct GenericConstraintPart { + impulse: SpacialVector, + max_impulse: SpatialVector, + min_impulse: SpatialVector, + + #[cfg(feature = "dim3")] + rhs: Vector6, + #[cfg(feature = "dim2")] + rhs: Vector3, +} + +impl GenericConstraintPart { + fn solve_ground( + &self, + parent: &GenericVelocityGroundConstraint, + mj_lambda2: &mut DeltaVel, + ) -> SpatialVector { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = parent + .rot2 + .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); + let dangvel = parent.rot2.inverse_transform_vector(&ang_vel2); - let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let dangvel = ang_vel2; #[cfg(feature = "dim2")] let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; #[cfg(feature = "dim3")] @@ -624,61 +589,71 @@ impl GenericVelocityGroundConstraint { dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, ) + self.rhs; - let new_impulse = (self.impulse + self.inv_lhs * dvel) - .sup(&self.max_negative_impulse) - .inf(&self.max_positive_impulse); + let new_impulse = (self.impulse + parent.inv_lhs * dvel) + .sup(&self.min_impulse) + .inf(&self.max_impulse); let effective_impulse = new_impulse - self.impulse; - self.impulse = new_impulse; - let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); + let lin_impulse = parent.rot2 * effective_impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; + let ang_impulse = parent.rot2 * effective_impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); + let ang_impulse = parent.rot2 * effective_impulse.fixed_rows::(3).into_owned(); - mj_lambda2.linear -= self.im2 * lin_impulse; - mj_lambda2.angular -= self + mj_lambda2.linear -= parent.im2 * lin_impulse; + mj_lambda2.angular -= parent .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); - /* - * Solve positions. - */ - let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular); - - let dlinpos = mj_lambda_pos2.linear + ang_pos2.gcross(self.r2); - let dangpos = ang_pos2; - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinpos.x, dlinpos.y, dangpos) + self.rhs; - #[cfg(feature = "dim3")] - let dpos = Vector6::new( - dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z, - ) + self.pos_rhs; - - let new_impulse = self.pos_impulse + self.inv_lhs * dpos; - let effective_impulse = new_impulse - self.pos_impulse; - self.pos_impulse = new_impulse; - - let lin_impulse = effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda_pos2.linear -= self.im2 * lin_impulse; - mj_lambda_pos2.angular -= self - .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2; + new_impulse } - // FIXME: duplicated code with the non-ground constraint. - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - let joint = &mut joints_all[self.joint_id].weight; - if let JointParams::GenericJoint(fixed) = &mut joint.params { - fixed.impulse = self.impulse; - } + fn solve( + &self, + parent: &GenericVelocityConstraint, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) -> SpatialVector { + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = parent.rot2.inverse_transform_vector( + &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) + + mj_lambda2.linear + + ang_vel2.gcross(parent.r2)), + ); + let dangvel = parent + .rot2 + .inverse_transform_vector(&(-ang_vel1 + ang_vel2)); + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let dvel = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let new_impulse = (self.impulse + parent.inv_lhs * dvel) + .sup(&self.min_impulse) + .inf(&self.max_impulse); + let effective_impulse = new_impulse - self.impulse; + + let lin_impulse = parent.rot2 * effective_impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = parent.rot2 * effective_impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = parent.rot2 * effective_impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += parent.im1 * lin_impulse; + mj_lambda1.angular += parent + .ii1_sqrt + .transform_vector(ang_impulse + parent.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= parent.im2 * lin_impulse; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); + + new_impulse } } From de39a41faa2ea722042231f91b5d579efdf1a02d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 15 Feb 2021 11:20:09 +0100 Subject: [PATCH 04/20] Implement non-linear position stabilization for the generic constraint. --- src/dynamics/integration_parameters.rs | 2 +- src/dynamics/joint/prismatic_joint.rs | 4 +- .../generic_position_constraint.rs | 270 +++++++++- .../generic_velocity_constraint.rs | 487 ++++++++++++------ .../joint_constraint/joint_constraint.rs | 17 - .../joint_position_constraint.rs | 16 - 6 files changed, 591 insertions(+), 205 deletions(-) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 628e07a..dd67442 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,7 +1,7 @@ use crate::math::Real; /// Parameters for a time-step of the physics engine. -#[derive(Clone)] +#[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { /// The timestep length (default: `1.0 / 60.0`) diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 174ce79..0edc939 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -89,8 +89,8 @@ impl PrismaticJoint { Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3) { [ - local_bitangent1.into_inner(), local_bitangent1.cross(&local_axis1), + local_bitangent1.into_inner(), ] } else { local_axis1.orthonormal_basis() @@ -100,8 +100,8 @@ impl PrismaticJoint { Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3) { [ - local_bitangent2.into_inner(), local_bitangent2.cross(&local_axis2), + local_bitangent2.into_inner(), ] } else { local_axis2.orthonormal_basis() diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 9d74bf3..1502403 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -5,7 +5,7 @@ use crate::math::{ AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, DIM, }; -use crate::utils::{WAngularInertia, WCross}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Vector3, Vector6}; // FIXME: review this code for the case where the center of masses are not the origin. @@ -48,16 +48,136 @@ impl GenericPositionConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - return; - } + let mut position1 = positions[self.position1]; + let mut position2 = positions[self.position2]; + let mut params = *params; + params.joint_erp = 0.8; - pub fn solve2( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - dpos: &mut [DeltaVel], - ) { - return; + /* + * + * Translation part. + * + */ + { + let anchor1 = position1 * self.joint.local_anchor1; + let anchor2 = position2 * self.joint.local_anchor2; + let basis = anchor1.rotation; + let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + let mut min_pos_impulse = self.joint.min_pos_impulse.xyz(); + let mut max_pos_impulse = self.joint.max_pos_impulse.xyz(); + + let pos_rhs = GenericVelocityConstraint::compute_lin_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.xyz(), + &self.joint.max_position.xyz(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1) + + self.ii2.quadform(&rmat2).add_diagonal(self.im2)) + .into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot1 = self.ii1.transform_vector(r1.gcross(impulse)); + let rot2 = self.ii2.transform_vector(r2.gcross(impulse)); + + position1.translation.vector += self.im1 * impulse; + position1.rotation = position1.rotation.append_axisangle_linearized(&rot1); + position2.translation.vector -= self.im2 * impulse; + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + /* + * + * Rotation part + * + */ + { + let anchor1 = position1 * self.joint.local_anchor1; + let anchor2 = position2 * self.joint.local_anchor2; + let basis = anchor1.rotation; + let mut min_pos_impulse = self + .joint + .min_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + let mut max_pos_impulse = self + .joint + .max_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + + let pos_rhs = GenericVelocityConstraint::compute_ang_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.fixed_rows::(DIM).into_owned(), + &self.joint.max_position.fixed_rows::(DIM).into_owned(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot1 = self.ii1.transform_vector(impulse); + let rot2 = self.ii2.transform_vector(impulse); + + position1.rotation = position1.rotation.append_axisangle_linearized(&rot1); + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + positions[self.position1] = position1; + positions[self.position2] = position2; } } @@ -102,15 +222,127 @@ impl GenericPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - return; - } + let mut position2 = positions[self.position2]; + let mut params = *params; + params.joint_erp = 0.8; - pub fn solve2( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - dpos: &mut [DeltaVel], - ) { - return; + /* + * + * Translation part. + * + */ + { + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let basis = anchor1.rotation; + let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2; + let mut min_pos_impulse = self.joint.min_pos_impulse.xyz(); + let mut max_pos_impulse = self.joint.max_pos_impulse.xyz(); + + let pos_rhs = GenericVelocityConstraint::compute_lin_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.xyz(), + &self.joint.max_position.xyz(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat; + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let delassus = self + .ii2 + .quadform(&rmat2) + .add_diagonal(self.im2) + .into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + + let rot2 = self.ii2.transform_vector(r2.gcross(impulse)); + + position2.translation.vector -= self.im2 * impulse; + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + /* + * + * Rotation part + * + */ + { + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let basis = anchor1.rotation; + let mut min_pos_impulse = self + .joint + .min_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + let mut max_pos_impulse = self + .joint + .max_pos_impulse + .fixed_rows::(DIM) + .into_owned(); + + let pos_rhs = GenericVelocityConstraint::compute_ang_position_error( + &anchor1, + &anchor2, + &basis, + &self.joint.min_position.fixed_rows::(DIM).into_owned(), + &self.joint.max_position.fixed_rows::(DIM).into_owned(), + ) * params.joint_erp; + + for i in 0..3 { + if pos_rhs[i] < 0.0 { + min_pos_impulse[i] = -Real::MAX; + } + if pos_rhs[i] > 0.0 { + max_pos_impulse[i] = Real::MAX; + } + } + + // Will be actually inverted right after. + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let delassus = self.ii2.quadform(&rotmat).into_matrix(); + + let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse, + &max_pos_impulse, + &mut Vector3::zeros(), + delassus, + ); + + let local_impulse = (inv_delassus * pos_rhs) + .inf(&max_pos_impulse) + .sup(&min_pos_impulse); + let impulse = basis * local_impulse; + let rot2 = self.ii2.transform_vector(impulse); + + position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2); + } + + positions[self.position2] = position2; } } diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index fe20e36..ba3efbe 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -34,10 +34,10 @@ pub(crate) struct GenericVelocityConstraint { r1: Vector, r2: Vector, - rot2: Rotation, + basis: Rotation, + dependant_set_mask: SpacialVector, vel: GenericConstraintPart, - pos: GenericConstraintPart, } impl GenericVelocityConstraint { @@ -49,9 +49,9 @@ impl GenericVelocityConstraint { ii2: AngularInertia, r1: Vector, r2: Vector, - rot2: Rotation, + basis: Rotation, ) -> Matrix6 { - let rotmat = rot2.to_rotation_matrix().into_inner(); + let rotmat = basis.to_rotation_matrix().into_inner(); let rmat1 = r1.gcross_matrix() * rotmat; let rmat2 = r2.gcross_matrix() * rotmat; @@ -93,19 +93,15 @@ impl GenericVelocityConstraint { max_velocity: &SpatialVector, r1: &Vector, r2: &Vector, - _anchor1: &Isometry, - anchor2: &Isometry, + basis: &Rotation, rb1: &RigidBody, rb2: &RigidBody, ) -> SpatialVector { let lin_dvel = -rb1.linvel - rb1.angvel.gcross(*r1) + rb2.linvel + rb2.angvel.gcross(*r2); let ang_dvel = -rb1.angvel + rb2.angvel; - let lin_dvel2 = anchor2.inverse_transform_vector(&lin_dvel); - let ang_dvel2 = anchor2.inverse_transform_vector(&ang_dvel); - - dbg!(lin_dvel); - dbg!(lin_dvel2); + let lin_dvel2 = basis.inverse_transform_vector(&lin_dvel); + let ang_dvel2 = basis.inverse_transform_vector(&ang_dvel); let min_linvel = min_velocity.xyz(); let min_angvel = min_velocity.fixed_rows::(DIM).into_owned(); @@ -123,17 +119,68 @@ impl GenericVelocityConstraint { ); } + pub fn compute_lin_position_error( + anchor1: &Isometry, + anchor2: &Isometry, + basis: &Rotation, + min_position: &Vector, + max_position: &Vector, + ) -> Vector { + let dpos = anchor2.translation.vector - anchor1.translation.vector; + let local_dpos = basis.inverse_transform_vector(&dpos); + local_dpos - local_dpos.sup(min_position).inf(max_position) + } + + pub fn compute_ang_position_error( + anchor1: &Isometry, + anchor2: &Isometry, + basis: &Rotation, + min_position: &Vector, + max_position: &Vector, + ) -> Vector { + let drot = anchor2.rotation * anchor1.rotation.inverse(); + let local_drot_diff = basis.inverse_transform_vector(&drot.scaled_axis()); + + let error = local_drot_diff - local_drot_diff.sup(min_position).inf(max_position); + let error_code = + (error[0] == 0.0) as usize + (error[1] == 0.0) as usize + (error[2] == 0.0) as usize; + + if error_code == 1 { + // Align two axes. + let constrained_axis = error.iamin(); + let axis1 = anchor1 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let axis2 = anchor2 + .rotation + .to_rotation_matrix() + .into_inner() + .column(constrained_axis) + .into_owned(); + let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2) + .unwrap_or(UnitQuaternion::identity()); + let local_drot_diff = basis.inverse_transform_vector(&rot_cross.scaled_axis()); + local_drot_diff - local_drot_diff.sup(min_position).inf(max_position) + } else { + error + } + } + pub fn compute_position_error( joint: &GenericJoint, anchor1: &Isometry, anchor2: &Isometry, + basis: &Rotation, ) -> SpatialVector { let delta_pos = Isometry::from_parts( anchor2.translation * anchor1.translation.inverse(), anchor2.rotation * anchor1.rotation.inverse(), ); - let lin_dpos = anchor2.inverse_transform_vector(&delta_pos.translation.vector); - let ang_dpos = anchor2.inverse_transform_vector(&delta_pos.rotation.scaled_axis()); + let lin_dpos = basis.inverse_transform_vector(&delta_pos.translation.vector); + let ang_dpos = basis.inverse_transform_vector(&delta_pos.rotation.scaled_axis()); let dpos = Vector6::new( lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, @@ -144,7 +191,6 @@ impl GenericVelocityConstraint { (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize; match error_code { - 0 => error, 1 => { let constrained_axis = error.rows(3, 3).iamin(); let axis1 = anchor1 @@ -161,22 +207,73 @@ impl GenericVelocityConstraint { .into_owned(); let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2) .unwrap_or(UnitQuaternion::identity()); - let ang_dpos = anchor2.inverse_transform_vector(&rot_cross.scaled_axis()); + let ang_dpos = basis.inverse_transform_vector(&rot_cross.scaled_axis()); let dpos = Vector6::new( lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z, ); dpos - dpos.sup(&joint.min_position).inf(&joint.max_position) } - 2 => { - // TODO - error - } - 3 => error, - _ => unreachable!(), + _ => error, } } + pub fn invert_partial_delassus_matrix( + min_impulse: &Vector, + max_impulse: &Vector, + dependant_set_mask: &mut Vector, + mut delassus: na::Matrix3, + ) -> na::Matrix3 { + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + for i in 0..3 { + if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + dependant_set_mask[i] = 0.0; + } else { + dependant_set_mask[i] = 1.0; + } + } + + delassus.try_inverse().unwrap() + } + + pub fn invert_delassus_matrix( + min_impulse: &Vector6, + max_impulse: &Vector6, + dependant_set_mask: &mut Vector6, + mut delassus: Matrix6, + ) -> Matrix6 { + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + dependant_set_mask.fill(1.0); + + for i in 0..6 { + if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + dependant_set_mask[i] = 0.0; + } + } + + // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. + #[cfg(feature = "dim2")] + return delassus.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + return delassus.cholesky().expect("Singular system.").inverse(); + } + pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, @@ -186,6 +283,7 @@ impl GenericVelocityConstraint { ) -> Self { let anchor1 = rb1.position * joint.local_anchor1; let anchor2 = rb2.position * joint.local_anchor2; + let basis = anchor1.rotation; let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); @@ -198,8 +296,9 @@ impl GenericVelocityConstraint { let mut max_pos_impulse = joint.max_pos_impulse; let mut min_velocity = joint.min_velocity; let mut max_velocity = joint.max_velocity; + let mut dependant_set_mask = SpacialVector::repeat(1.0); - let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2) + let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis) * params.inv_dt() * params.joint_erp; @@ -216,40 +315,17 @@ impl GenericVelocityConstraint { } } - let rhs = Self::compute_velocity_error( - &min_velocity, - &max_velocity, - &r1, - &r2, - &anchor1, - &anchor2, - rb1, - rb2, + let rhs = + Self::compute_velocity_error(&min_velocity, &max_velocity, &r1, &r2, &basis, rb1, rb2); + + let mut delassus = Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, basis); + let inv_lhs = Self::invert_delassus_matrix( + &min_impulse, + &max_impulse, + &mut dependant_set_mask, + delassus, ); - let mut delassus = - Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, anchor2.rotation); - - // Adjust the Delassus matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - for i in 0..6 { - if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX { - let diag = delassus[(i, i)]; - delassus.column_mut(i).fill(0.0); - delassus.row_mut(i).fill(0.0); - delassus[(i, i)] = diag; - } - } - - // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. - #[cfg(feature = "dim2")] - let inv_lhs = delassus.try_inverse().expect("Singular system."); - #[cfg(feature = "dim3")] - let inv_lhs = delassus.cholesky().expect("Singular system.").inverse(); - let impulse = (joint.impulse * params.warmstart_coeff) .inf(&max_impulse) .sup(&min_impulse); @@ -267,19 +343,14 @@ impl GenericVelocityConstraint { inv_lhs, r1, r2, - rot2: anchor2.rotation, + basis, + dependant_set_mask, vel: GenericConstraintPart { impulse, min_impulse, max_impulse, rhs, }, - pos: GenericConstraintPart { - impulse: na::zero(), - min_impulse: min_pos_impulse, - max_impulse: max_pos_impulse, - rhs: pos_rhs, - }, } } @@ -287,11 +358,11 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::(0).into_owned(); + let lin_impulse = self.basis * self.vel.impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = self.rot2 * self.vel.impulse[2]; + let ang_impulse = self.basis * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis * self.vel.impulse.fixed_rows::(3).into_owned(); mj_lambda1.linear += self.im1 * lin_impulse; mj_lambda1.angular += self @@ -308,28 +379,13 @@ impl GenericVelocityConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - return; - } - - pub fn solve2( - &mut self, - mj_lambdas: &mut [DeltaVel], - mj_lambdas_pos: &mut [DeltaVel], - ) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let mut mj_lambda_pos1 = mj_lambdas_pos[self.mj_lambda1 as usize]; - let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; self.vel.impulse = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); - self.pos.impulse = self - .pos - .solve(self, &mut mj_lambda_pos1, &mut mj_lambda_pos2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - mj_lambdas_pos[self.mj_lambda1 as usize] = mj_lambda_pos1; - mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2; } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -355,10 +411,11 @@ pub(crate) struct GenericVelocityGroundConstraint { ii2: AngularInertia, ii2_sqrt: AngularInertia, r2: Vector, - rot2: Rotation, + basis: Rotation, + + dependant_set_mask: SpacialVector, vel: GenericConstraintPart, - pos: GenericConstraintPart, } impl GenericVelocityGroundConstraint { @@ -367,9 +424,9 @@ impl GenericVelocityGroundConstraint { im2: Real, ii2: AngularInertia, r2: Vector, - rot2: Rotation, + basis: Rotation, ) -> Matrix6 { - let rotmat2 = rot2.to_rotation_matrix().into_inner(); + let rotmat2 = basis.to_rotation_matrix().into_inner(); let rmat2 = r2.gcross_matrix() * rotmat2; #[cfg(feature = "dim3")] @@ -423,6 +480,7 @@ impl GenericVelocityGroundConstraint { ) }; + let basis = anchor2.rotation; let im2 = rb2.effective_inv_mass; let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1.translation.vector - rb1.world_com.coords; @@ -433,10 +491,12 @@ impl GenericVelocityGroundConstraint { let mut max_pos_impulse = joint.max_pos_impulse; let mut min_velocity = joint.min_velocity; let mut max_velocity = joint.max_velocity; + let mut dependant_set_mask = SpacialVector::repeat(1.0); - let pos_rhs = GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2) - * params.inv_dt() - * params.joint_erp; + let pos_rhs = + GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2, &basis) + * params.inv_dt() + * params.joint_erp; for i in 0..6 { if pos_rhs[i] < 0.0 { @@ -456,33 +516,18 @@ impl GenericVelocityGroundConstraint { &max_velocity, &r1, &r2, - &anchor1, - &anchor2, + &basis, rb1, rb2, ); - let mut delassus = Self::compute_delassus_matrix(im2, ii2, r2, anchor2.rotation); - - // Adjust the Delassus matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - for i in 0..6 { - if min_impulse[i] > -Real::MAX && max_impulse[i] < Real::MAX { - let diag = delassus[(i, i)]; - delassus.column_mut(i).fill(0.0); - delassus.row_mut(i).fill(0.0); - delassus[(i, i)] = diag; - } - } - - // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. - #[cfg(feature = "dim2")] - let inv_lhs = delassus.try_inverse().expect("Singular system."); - #[cfg(feature = "dim3")] - let inv_lhs = delassus.cholesky().expect("Singular system.").inverse(); + let delassus = Self::compute_delassus_matrix(im2, ii2, r2, basis); + let inv_lhs = GenericVelocityConstraint::invert_delassus_matrix( + &min_impulse, + &max_impulse, + &mut dependant_set_mask, + delassus, + ); let impulse = (joint.impulse * params.warmstart_coeff) .inf(&max_impulse) @@ -496,30 +541,25 @@ impl GenericVelocityGroundConstraint { ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, inv_lhs, r2, - rot2: anchor2.rotation, + basis, vel: GenericConstraintPart { impulse, min_impulse, max_impulse, rhs, }, - pos: GenericConstraintPart { - impulse: na::zero(), - min_impulse: min_pos_impulse, - max_impulse: max_pos_impulse, - rhs: pos_rhs, - }, + dependant_set_mask, } } pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.rot2 * self.vel.impulse.fixed_rows::(0).into_owned(); + let lin_impulse = self.basis * self.vel.impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = self.rot2 * self.vel.impulse[2]; + let ang_impulse = self.basis * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.rot2 * self.vel.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis * self.vel.impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self @@ -530,22 +570,9 @@ impl GenericVelocityGroundConstraint { } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - return; - } - - pub fn solve2( - &mut self, - mj_lambdas: &mut [DeltaVel], - mj_lambdas_pos: &mut [DeltaVel], - ) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize]; - self.vel.impulse = self.vel.solve_ground(self, &mut mj_lambda2); - self.pos.impulse = self.pos.solve_ground(self, &mut mj_lambda_pos2); - mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; - mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2; } // TODO: duplicated code with the non-ground constraint. @@ -575,12 +602,48 @@ impl GenericConstraintPart { parent: &GenericVelocityGroundConstraint, mj_lambda2: &mut DeltaVel, ) -> SpatialVector { + let mut new_impulse = SpacialVector::zeros(); + + for i in 0..3 { + // Solve the independent 1D constraints. + if parent.dependant_set_mask[i] == 0.0 + && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) + { + let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = if i < 3 { + (mj_lambda2.linear + ang_vel2.gcross(parent.r2)).dot(&constraint_axis) + + self.rhs[i] + } else { + ang_vel2.dot(&constraint_axis) + self.rhs[i] + }; + + new_impulse[i] = na::clamp( + self.impulse[i] + parent.inv_lhs[(i, i)] * dvel, + self.min_impulse[i], + self.max_impulse[i], + ); + + let effective_impulse = new_impulse[i] - self.impulse[i]; + let impulse = constraint_axis * effective_impulse; + + if i < 3 { + mj_lambda2.linear -= parent.im2 * impulse; + mj_lambda2.angular -= + parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); + } else { + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); + } + } + } + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); let dlinvel = parent - .rot2 + .basis .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); - let dangvel = parent.rot2.inverse_transform_vector(&ang_vel2); + let dangvel = parent.basis.inverse_transform_vector(&ang_vel2); #[cfg(feature = "dim2")] let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; @@ -589,22 +652,58 @@ impl GenericConstraintPart { dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, ) + self.rhs; - let new_impulse = (self.impulse + parent.inv_lhs * dvel) - .sup(&self.min_impulse) - .inf(&self.max_impulse); - let effective_impulse = new_impulse - self.impulse; + new_impulse += + (self.impulse + parent.inv_lhs * dvel).component_mul(&parent.dependant_set_mask); + let effective_impulse = + (new_impulse - self.impulse).component_mul(&parent.dependant_set_mask); - let lin_impulse = parent.rot2 * effective_impulse.fixed_rows::(0).into_owned(); + let lin_impulse = parent.basis * effective_impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = parent.rot2 * effective_impulse[2]; + let ang_impulse = parent.basis * effective_impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = parent.rot2 * effective_impulse.fixed_rows::(3).into_owned(); + let ang_impulse = parent.basis * effective_impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= parent.im2 * lin_impulse; mj_lambda2.angular -= parent .ii2_sqrt .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); + for i in 3..6 { + // Solve the independent 1D constraints. + if parent.dependant_set_mask[i] == 0.0 + && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) + { + let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = if i < 3 { + (mj_lambda2.linear + ang_vel2.gcross(parent.r2)).dot(&constraint_axis) + + self.rhs[i] + } else { + ang_vel2.dot(&constraint_axis) + self.rhs[i] + }; + + let inv_lhs = parent.inv_lhs[(i, i)]; + + new_impulse[i] = na::clamp( + self.impulse[i] + inv_lhs * dvel, + self.min_impulse[i], + self.max_impulse[i], + ); + + let effective_impulse = new_impulse[i] - self.impulse[i]; + let impulse = constraint_axis * effective_impulse; + + if i < 3 { + mj_lambda2.linear -= parent.im2 * impulse; + mj_lambda2.angular -= + parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); + } else { + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); + } + } + } + new_impulse } @@ -614,16 +713,60 @@ impl GenericConstraintPart { mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, ) -> SpatialVector { + let mut new_impulse = SpacialVector::zeros(); + + for i in 0..3 { + // Solve the independent 1D constraints. + if parent.dependant_set_mask[i] == 0.0 + && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) + { + let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + + let dvel = if i < 3 { + (mj_lambda2.linear + ang_vel2.gcross(parent.r2) + - mj_lambda1.linear + - ang_vel1.gcross(parent.r1)) + .dot(&constraint_axis) + + self.rhs[i] + } else { + (ang_vel2 - ang_vel1).dot(&constraint_axis) + self.rhs[i] + }; + + new_impulse[i] = na::clamp( + self.impulse[i] + parent.inv_lhs[(i, i)] * dvel, + self.min_impulse[i], + self.max_impulse[i], + ); + + let effective_impulse = new_impulse[i] - self.impulse[i]; + let impulse = constraint_axis * effective_impulse; + + if i < 3 { + mj_lambda1.linear += parent.im1 * impulse; + mj_lambda1.angular += + parent.ii1_sqrt.transform_vector(parent.r1.gcross(impulse)); + mj_lambda2.linear -= parent.im2 * impulse; + mj_lambda2.angular -= + parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); + } else { + mj_lambda1.angular += parent.ii1_sqrt.transform_vector(impulse); + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); + } + } + } + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let dlinvel = parent.rot2.inverse_transform_vector( + let dlinvel = parent.basis.inverse_transform_vector( &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) + mj_lambda2.linear + ang_vel2.gcross(parent.r2)), ); let dangvel = parent - .rot2 + .basis .inverse_transform_vector(&(-ang_vel1 + ang_vel2)); #[cfg(feature = "dim2")] @@ -633,16 +776,16 @@ impl GenericConstraintPart { dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, ) + self.rhs; - let new_impulse = (self.impulse + parent.inv_lhs * dvel) - .sup(&self.min_impulse) - .inf(&self.max_impulse); - let effective_impulse = new_impulse - self.impulse; + new_impulse += + (self.impulse + parent.inv_lhs * dvel).component_mul(&parent.dependant_set_mask); + let effective_impulse = + (new_impulse - self.impulse).component_mul(&parent.dependant_set_mask); - let lin_impulse = parent.rot2 * effective_impulse.fixed_rows::(0).into_owned(); + let lin_impulse = parent.basis * effective_impulse.fixed_rows::(0).into_owned(); #[cfg(feature = "dim2")] - let ang_impulse = parent.rot2 * effective_impulse[2]; + let ang_impulse = parent.basis * effective_impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = parent.rot2 * effective_impulse.fixed_rows::(3).into_owned(); + let ang_impulse = parent.basis * effective_impulse.fixed_rows::(3).into_owned(); mj_lambda1.linear += parent.im1 * lin_impulse; mj_lambda1.angular += parent @@ -654,6 +797,50 @@ impl GenericConstraintPart { .ii2_sqrt .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); + for i in 3..6 { + // Solve the independent 1D constraints. + if parent.dependant_set_mask[i] == 0.0 + && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) + { + let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + + let dvel = if i < 3 { + (mj_lambda2.linear + ang_vel2.gcross(parent.r2) + - mj_lambda1.linear + - ang_vel1.gcross(parent.r1)) + .dot(&constraint_axis) + + self.rhs[i] + } else { + (ang_vel2 - ang_vel1).dot(&constraint_axis) + self.rhs[i] + }; + + let inv_lhs = parent.inv_lhs[(i, i)]; + + new_impulse[i] = na::clamp( + self.impulse[i] + inv_lhs * dvel, + self.min_impulse[i], + self.max_impulse[i], + ); + + let effective_impulse = new_impulse[i] - self.impulse[i]; + let impulse = constraint_axis * effective_impulse; + + if i < 3 { + mj_lambda1.linear += parent.im1 * impulse; + mj_lambda1.angular += + parent.ii1_sqrt.transform_vector(parent.r1.gcross(impulse)); + mj_lambda2.linear -= parent.im2 * impulse; + mj_lambda2.angular -= + parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); + } else { + mj_lambda1.angular += parent.ii1_sqrt.transform_vector(impulse); + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); + } + } + } + new_impulse } } diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 78332e8..12b8f77 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -333,23 +333,6 @@ impl AnyJointVelocityConstraint { } } - pub fn solve2( - &mut self, - mj_lambdas: &mut [DeltaVel], - mj_lambdas_pos: &mut [DeltaVel], - ) { - match self { - AnyJointVelocityConstraint::GenericConstraint(c) => { - c.solve2(mj_lambdas, mj_lambdas_pos) - } - AnyJointVelocityConstraint::GenericGroundConstraint(c) => { - c.solve2(mj_lambdas, mj_lambdas_pos) - } - AnyJointVelocityConstraint::Empty => unreachable!(), - _ => {} - } - } - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { match self { AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all), diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 0c35883..a13cc27 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -242,20 +242,4 @@ impl AnyJointPositionConstraint { AnyJointPositionConstraint::Empty => unreachable!(), } } - - pub fn solve2( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - dpos: &mut [DeltaVel], - ) { - match self { - AnyJointPositionConstraint::GenericJoint(c) => c.solve2(params, positions, dpos), - AnyJointPositionConstraint::GenericGroundConstraint(c) => { - c.solve2(params, positions, dpos) - } - _ => {} - AnyJointPositionConstraint::Empty => unreachable!(), - } - } } From ebd5562af3e3b2a6108dd695931c85da2da99d22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 15 Feb 2021 12:08:18 +0100 Subject: [PATCH 05/20] Generic velocity constraint: split the translation and rotation terms. --- .../generic_velocity_constraint.rs | 575 +++++++----------- 1 file changed, 223 insertions(+), 352 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index ba3efbe..63ffdfd 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -6,10 +6,10 @@ use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, use crate::na::UnitQuaternion; use crate::parry::math::{AngDim, SpatialVector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Matrix3, Matrix6, Vector3, Vector6, U3}; #[cfg(feature = "dim2")] use na::{Matrix3, Vector3}; -#[cfg(feature = "dim3")] -use na::{Matrix6, Vector6, U3}; #[derive(Debug)] pub(crate) struct GenericVelocityConstraint { @@ -18,10 +18,8 @@ pub(crate) struct GenericVelocityConstraint { joint_id: JointIndex, - #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // TODO: replace by Cholesky? - #[cfg(feature = "dim2")] - inv_lhs: Matrix3, + inv_lhs_lin: Matrix3, + inv_lhs_ang: Matrix3, im1: Real, im2: Real, @@ -41,53 +39,6 @@ pub(crate) struct GenericVelocityConstraint { } impl GenericVelocityConstraint { - #[inline(always)] - pub fn compute_delassus_matrix( - im1: Real, - im2: Real, - ii1: AngularInertia, - ii2: AngularInertia, - r1: Vector, - r2: Vector, - basis: Rotation, - ) -> Matrix6 { - let rotmat = basis.to_rotation_matrix().into_inner(); - let rmat1 = r1.gcross_matrix() * rotmat; - let rmat2 = r2.gcross_matrix() * rotmat; - - #[cfg(feature = "dim3")] - { - let del00 = - ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); - let del10 = - rotmat.transpose() * (ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2)); - let del11 = (ii1 + ii2).quadform(&rotmat).into_matrix(); - - // Note that Cholesky only reads the lower-triangular part of the matrix - // so we don't need to fill del01. - let mut del = Matrix6::zeros(); - del.fixed_slice_mut::(0, 0) - .copy_from(&del00.into_matrix()); - del.fixed_slice_mut::(3, 0).copy_from(&del10); - del.fixed_slice_mut::(3, 3).copy_from(&del11); - del - } - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - { - panic!("Take the rotmat into account."); - let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; - let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; - let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; - let m13 = rmat1.x * ii1 + rmat2.x * ii2; - let m23 = rmat1.y * ii1 + rmat2.y * ii2; - let m33 = ii1 + ii2; - Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) - } - } - pub fn compute_velocity_error( min_velocity: &SpatialVector, max_velocity: &SpatialVector, @@ -244,36 +195,6 @@ impl GenericVelocityConstraint { delassus.try_inverse().unwrap() } - pub fn invert_delassus_matrix( - min_impulse: &Vector6, - max_impulse: &Vector6, - dependant_set_mask: &mut Vector6, - mut delassus: Matrix6, - ) -> Matrix6 { - // Adjust the Delassus matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - dependant_set_mask.fill(1.0); - - for i in 0..6 { - if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX { - let diag = delassus[(i, i)]; - delassus.column_mut(i).fill(0.0); - delassus.row_mut(i).fill(0.0); - delassus[(i, i)] = diag; - dependant_set_mask[i] = 0.0; - } - } - - // NOTE: we don't use Cholesky in 2D because we only have a 3x3 matrix. - #[cfg(feature = "dim2")] - return delassus.try_inverse().expect("Singular system."); - #[cfg(feature = "dim3")] - return delassus.cholesky().expect("Singular system.").inverse(); - } - pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, @@ -317,19 +238,42 @@ impl GenericVelocityConstraint { let rhs = Self::compute_velocity_error(&min_velocity, &max_velocity, &r1, &r2, &basis, rb1, rb2); + let rhs_lin = rhs.xyz(); + let rhs_ang = rhs.fixed_rows::(DIM).into(); - let mut delassus = Self::compute_delassus_matrix(im1, im2, ii1, ii2, r1, r2, basis); - let inv_lhs = Self::invert_delassus_matrix( - &min_impulse, - &max_impulse, - &mut dependant_set_mask, - delassus, + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat; + let rmat2 = r2.gcross_matrix() * rotmat; + let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1) + + ii2.quadform(&rmat2).add_diagonal(im2)) + .into_matrix(); + let delassus11 = (ii1.quadform(&rotmat) + ii2.quadform(&rotmat)).into_matrix(); + + let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.xyz(), + &max_pos_impulse.xyz(), + &mut Vector3::zeros(), + delassus00, + ); + let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.fixed_rows::(DIM).into_owned(), + &max_pos_impulse.fixed_rows::(DIM).into_owned(), + &mut Vector3::zeros(), + delassus11, ); let impulse = (joint.impulse * params.warmstart_coeff) .inf(&max_impulse) .sup(&min_impulse); + let lin_impulse = impulse.xyz(); + let ang_impulse = impulse.fixed_rows::(DIM).into_owned(); + let min_lin_impulse = min_impulse.xyz(); + let min_ang_impulse = min_impulse.fixed_rows::(DIM).into_owned(); + let max_lin_impulse = max_impulse.xyz(); + let max_ang_impulse = max_impulse.fixed_rows::(DIM).into_owned(); + GenericVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, @@ -340,16 +284,21 @@ impl GenericVelocityConstraint { ii2, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - inv_lhs, + inv_lhs_lin, + inv_lhs_ang, r1, r2, basis, dependant_set_mask, vel: GenericConstraintPart { - impulse, - min_impulse, - max_impulse, - rhs, + lin_impulse, + ang_impulse, + min_lin_impulse, + min_ang_impulse, + max_lin_impulse, + max_ang_impulse, + rhs_lin, + rhs_ang, }, } } @@ -358,11 +307,11 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.basis * self.vel.impulse.fixed_rows::(0).into_owned(); + let lin_impulse = self.basis * self.vel.lin_impulse; #[cfg(feature = "dim2")] let ang_impulse = self.basis * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.basis * self.vel.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis * self.vel.ang_impulse; mj_lambda1.linear += self.im1 * lin_impulse; mj_lambda1.angular += self @@ -382,7 +331,9 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - self.vel.impulse = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + let (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2); + self.vel.lin_impulse = lin_imp; + self.vel.ang_impulse = ang_imp; mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -391,7 +342,15 @@ impl GenericVelocityConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::GenericJoint(fixed) = &mut joint.params { - fixed.impulse = self.vel.impulse; + let impulse = Vector6::new( + self.vel.lin_impulse.x, + self.vel.lin_impulse.y, + self.vel.lin_impulse.z, + self.vel.ang_impulse.x, + self.vel.ang_impulse.y, + self.vel.ang_impulse.z, + ); + fixed.impulse = impulse; } } } @@ -402,10 +361,8 @@ pub(crate) struct GenericVelocityGroundConstraint { joint_id: JointIndex, - #[cfg(feature = "dim3")] - inv_lhs: Matrix6, // TODO: replace by Cholesky? - #[cfg(feature = "dim2")] - inv_lhs: Matrix3, + inv_lhs_lin: Matrix3, + inv_lhs_ang: Matrix3, im2: Real, ii2: AngularInertia, @@ -520,33 +477,58 @@ impl GenericVelocityGroundConstraint { rb1, rb2, ); + let rhs_lin = rhs.xyz(); + let rhs_ang = rhs.fixed_rows::(DIM).into_owned(); - let delassus = Self::compute_delassus_matrix(im2, ii2, r2, basis); - let inv_lhs = GenericVelocityConstraint::invert_delassus_matrix( - &min_impulse, - &max_impulse, - &mut dependant_set_mask, - delassus, + // TODO: we should keep the SdpMatrix3 type. + let rotmat = basis.to_rotation_matrix().into_inner(); + let rmat2 = r2.gcross_matrix() * rotmat; + let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix(); + let delassus11 = ii2.quadform(&rotmat).into_matrix(); + + let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.xyz(), + &max_pos_impulse.xyz(), + &mut Vector3::zeros(), + delassus00, + ); + let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix( + &min_pos_impulse.fixed_rows::(DIM).into_owned(), + &max_pos_impulse.fixed_rows::(DIM).into_owned(), + &mut Vector3::zeros(), + delassus11, ); let impulse = (joint.impulse * params.warmstart_coeff) .inf(&max_impulse) .sup(&min_impulse); + let lin_impulse = impulse.xyz(); + let ang_impulse = impulse.fixed_rows::(DIM).into_owned(); + let min_lin_impulse = min_impulse.xyz(); + let min_ang_impulse = min_impulse.fixed_rows::(DIM).into_owned(); + let max_lin_impulse = max_impulse.xyz(); + let max_ang_impulse = max_impulse.fixed_rows::(DIM).into_owned(); + GenericVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, ii2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - inv_lhs, + inv_lhs_lin, + inv_lhs_ang, r2, basis, vel: GenericConstraintPart { - impulse, - min_impulse, - max_impulse, - rhs, + lin_impulse, + ang_impulse, + min_lin_impulse, + min_ang_impulse, + max_lin_impulse, + max_ang_impulse, + rhs_lin, + rhs_ang, }, dependant_set_mask, } @@ -555,11 +537,11 @@ impl GenericVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.basis * self.vel.impulse.fixed_rows::(0).into_owned(); + let lin_impulse = self.basis * self.vel.lin_impulse; #[cfg(feature = "dim2")] let ang_impulse = self.basis * self.vel.impulse[2]; #[cfg(feature = "dim3")] - let ang_impulse = self.basis * self.vel.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis * self.vel.ang_impulse; mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self @@ -571,7 +553,11 @@ impl GenericVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - self.vel.impulse = self.vel.solve_ground(self, &mut mj_lambda2); + + let (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2); + self.vel.lin_impulse = lin_imp; + self.vel.ang_impulse = ang_imp; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -579,268 +565,153 @@ impl GenericVelocityGroundConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::GenericJoint(fixed) = &mut joint.params { - fixed.impulse = self.vel.impulse; + let impulse = Vector6::new( + self.vel.lin_impulse.x, + self.vel.lin_impulse.y, + self.vel.lin_impulse.z, + self.vel.ang_impulse.x, + self.vel.ang_impulse.y, + self.vel.ang_impulse.z, + ); + fixed.impulse = impulse; } } } #[derive(Debug)] struct GenericConstraintPart { - impulse: SpacialVector, - max_impulse: SpatialVector, - min_impulse: SpatialVector, + lin_impulse: Vector3, + max_lin_impulse: Vector3, + min_lin_impulse: Vector3, + rhs_lin: Vector3, - #[cfg(feature = "dim3")] - rhs: Vector6, - #[cfg(feature = "dim2")] - rhs: Vector3, + ang_impulse: Vector3, + max_ang_impulse: Vector3, + min_ang_impulse: Vector3, + rhs_ang: Vector3, } impl GenericConstraintPart { - fn solve_ground( - &self, - parent: &GenericVelocityGroundConstraint, - mj_lambda2: &mut DeltaVel, - ) -> SpatialVector { - let mut new_impulse = SpacialVector::zeros(); - - for i in 0..3 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2)).dot(&constraint_axis) - + self.rhs[i] - } else { - ang_vel2.dot(&constraint_axis) + self.rhs[i] - }; - - new_impulse[i] = na::clamp( - self.impulse[i] + parent.inv_lhs[(i, i)] * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); - - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; - - if i < 3 { - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } - } - - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dlinvel = parent - .basis - .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); - let dangvel = parent.basis.inverse_transform_vector(&ang_vel2); - - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; - - new_impulse += - (self.impulse + parent.inv_lhs * dvel).component_mul(&parent.dependant_set_mask); - let effective_impulse = - (new_impulse - self.impulse).component_mul(&parent.dependant_set_mask); - - let lin_impulse = parent.basis * effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = parent.basis * effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = parent.basis * effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda2.linear -= parent.im2 * lin_impulse; - mj_lambda2.angular -= parent - .ii2_sqrt - .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); - - for i in 3..6 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2)).dot(&constraint_axis) - + self.rhs[i] - } else { - ang_vel2.dot(&constraint_axis) + self.rhs[i] - }; - - let inv_lhs = parent.inv_lhs[(i, i)]; - - new_impulse[i] = na::clamp( - self.impulse[i] + inv_lhs * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); - - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; - - if i < 3 { - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } - } - - new_impulse - } - fn solve( &self, parent: &GenericVelocityConstraint, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, - ) -> SpatialVector { - let mut new_impulse = SpacialVector::zeros(); + ) -> (Vector3, Vector3) { + let new_lin_impulse; + let new_ang_impulse; - for i in 0..3 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + /* + * + * Solve translations. + * + */ + { + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2) - - mj_lambda1.linear - - ang_vel1.gcross(parent.r1)) - .dot(&constraint_axis) - + self.rhs[i] - } else { - (ang_vel2 - ang_vel1).dot(&constraint_axis) + self.rhs[i] - }; + let dvel = parent.basis.inverse_transform_vector( + &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) + + mj_lambda2.linear + + ang_vel2.gcross(parent.r2)), + ); - new_impulse[i] = na::clamp( - self.impulse[i] + parent.inv_lhs[(i, i)] * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); + let err = dvel + self.rhs_lin; - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; + new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err) + .sup(&self.min_lin_impulse) + .inf(&self.max_lin_impulse); + let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse); - if i < 3 { - mj_lambda1.linear += parent.im1 * impulse; - mj_lambda1.angular += - parent.ii1_sqrt.transform_vector(parent.r1.gcross(impulse)); - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda1.angular += parent.ii1_sqrt.transform_vector(impulse); - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } + mj_lambda1.linear += parent.im1 * effective_impulse; + mj_lambda1.angular += parent + .ii1_sqrt + .transform_vector(parent.r1.gcross(effective_impulse)); + + mj_lambda2.linear -= parent.im2 * effective_impulse; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(parent.r2.gcross(effective_impulse)); } - let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + /* + * + * Solve rotations. + * + */ + { + let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let dlinvel = parent.basis.inverse_transform_vector( - &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) - + mj_lambda2.linear - + ang_vel2.gcross(parent.r2)), - ); - let dangvel = parent - .basis - .inverse_transform_vector(&(-ang_vel1 + ang_vel2)); + let dvel = parent + .basis + .inverse_transform_vector(&(ang_vel2 - ang_vel1)); + let err = dvel + self.rhs_ang; - #[cfg(feature = "dim2")] - let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; - #[cfg(feature = "dim3")] - let dvel = Vector6::new( - dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, - ) + self.rhs; + new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err) + .sup(&self.min_ang_impulse) + .inf(&self.max_ang_impulse); + let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse); - new_impulse += - (self.impulse + parent.inv_lhs * dvel).component_mul(&parent.dependant_set_mask); - let effective_impulse = - (new_impulse - self.impulse).component_mul(&parent.dependant_set_mask); - - let lin_impulse = parent.basis * effective_impulse.fixed_rows::(0).into_owned(); - #[cfg(feature = "dim2")] - let ang_impulse = parent.basis * effective_impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = parent.basis * effective_impulse.fixed_rows::(3).into_owned(); - - mj_lambda1.linear += parent.im1 * lin_impulse; - mj_lambda1.angular += parent - .ii1_sqrt - .transform_vector(ang_impulse + parent.r1.gcross(lin_impulse)); - - mj_lambda2.linear -= parent.im2 * lin_impulse; - mj_lambda2.angular -= parent - .ii2_sqrt - .transform_vector(ang_impulse + parent.r2.gcross(lin_impulse)); - - for i in 3..6 { - // Solve the independent 1D constraints. - if parent.dependant_set_mask[i] == 0.0 - && (self.min_impulse[i] != 0.0 || self.max_impulse[i] != 0.0) - { - let constraint_axis = parent.basis * Vector::ith(i % 3, 1.0); - let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); - - let dvel = if i < 3 { - (mj_lambda2.linear + ang_vel2.gcross(parent.r2) - - mj_lambda1.linear - - ang_vel1.gcross(parent.r1)) - .dot(&constraint_axis) - + self.rhs[i] - } else { - (ang_vel2 - ang_vel1).dot(&constraint_axis) + self.rhs[i] - }; - - let inv_lhs = parent.inv_lhs[(i, i)]; - - new_impulse[i] = na::clamp( - self.impulse[i] + inv_lhs * dvel, - self.min_impulse[i], - self.max_impulse[i], - ); - - let effective_impulse = new_impulse[i] - self.impulse[i]; - let impulse = constraint_axis * effective_impulse; - - if i < 3 { - mj_lambda1.linear += parent.im1 * impulse; - mj_lambda1.angular += - parent.ii1_sqrt.transform_vector(parent.r1.gcross(impulse)); - mj_lambda2.linear -= parent.im2 * impulse; - mj_lambda2.angular -= - parent.ii2_sqrt.transform_vector(parent.r2.gcross(impulse)); - } else { - mj_lambda1.angular += parent.ii1_sqrt.transform_vector(impulse); - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(impulse); - } - } + mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse); + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse); } - new_impulse + (new_lin_impulse, new_ang_impulse) + } + + fn solve_ground( + &self, + parent: &GenericVelocityGroundConstraint, + mj_lambda2: &mut DeltaVel, + ) -> (Vector3, Vector3) { + let new_lin_impulse; + let new_ang_impulse; + + /* + * + * Solve translations. + * + */ + { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = parent + .basis + .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); + + let err = dvel + self.rhs_lin; + + new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err) + .sup(&self.min_lin_impulse) + .inf(&self.max_lin_impulse); + let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse); + + mj_lambda2.linear -= parent.im2 * effective_impulse; + mj_lambda2.angular -= parent + .ii2_sqrt + .transform_vector(parent.r2.gcross(effective_impulse)); + } + + /* + * + * Solve rotations. + * + */ + { + let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = parent.basis.inverse_transform_vector(&ang_vel2); + let err = dvel + self.rhs_ang; + + new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err) + .sup(&self.min_ang_impulse) + .inf(&self.max_ang_impulse); + let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse); + + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse); + } + + (new_lin_impulse, new_ang_impulse) } } From 4f8f8017f447fa5137fa5ed5fc3820faebb5c1bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 15 Feb 2021 16:44:55 +0100 Subject: [PATCH 06/20] Properly writeback the generic constrainst impulse when it comes from a revolute joint. --- .../generic_position_constraint.rs | 2 - .../generic_velocity_constraint.rs | 97 +++++++------------ 2 files changed, 36 insertions(+), 63 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 1502403..be12258 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -152,7 +152,6 @@ impl GenericPositionConstraint { } } - // Will be actually inverted right after. // TODO: we should keep the SdpMatrix3 type. let rotmat = basis.to_rotation_matrix().into_inner(); let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix(); @@ -259,7 +258,6 @@ impl GenericPositionGroundConstraint { let rotmat = basis.to_rotation_matrix().into_inner(); let rmat2 = r2.gcross_matrix() * rotmat; - // Will be actually inverted right after. // TODO: we should keep the SdpMatrix3 type. let delassus = self .ii2 diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index 63ffdfd..0208dda 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -341,16 +341,24 @@ impl GenericVelocityConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; - if let JointParams::GenericJoint(fixed) = &mut joint.params { - let impulse = Vector6::new( - self.vel.lin_impulse.x, - self.vel.lin_impulse.y, - self.vel.lin_impulse.z, - self.vel.ang_impulse.x, - self.vel.ang_impulse.y, - self.vel.ang_impulse.z, - ); - fixed.impulse = impulse; + match &mut joint.params { + JointParams::GenericJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.impulse[3] = self.vel.ang_impulse.x; + out.impulse[4] = self.vel.ang_impulse.y; + out.impulse[5] = self.vel.ang_impulse.z; + } + JointParams::RevoluteJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.motor_impulse = self.vel.ang_impulse.x; + out.impulse[3] = self.vel.ang_impulse.y; + out.impulse[4] = self.vel.ang_impulse.z; + } + _ => unimplemented!(), } } } @@ -376,47 +384,6 @@ pub(crate) struct GenericVelocityGroundConstraint { } impl GenericVelocityGroundConstraint { - #[inline(always)] - pub fn compute_delassus_matrix( - im2: Real, - ii2: AngularInertia, - r2: Vector, - basis: Rotation, - ) -> Matrix6 { - let rotmat2 = basis.to_rotation_matrix().into_inner(); - let rmat2 = r2.gcross_matrix() * rotmat2; - - #[cfg(feature = "dim3")] - { - let del00 = ii2.quadform(&rmat2).add_diagonal(im2); - let del10 = rotmat2.transpose() * ii2.transform_matrix(&rmat2); - let del11 = ii2.quadform(&rotmat2).into_matrix(); - - // Note that Cholesky only reads the lower-triangular part of the matrix - // so we don't need to fill lhs01. - let mut del = Matrix6::zeros(); - del.fixed_slice_mut::(0, 0) - .copy_from(&del00.into_matrix()); - del.fixed_slice_mut::(3, 0).copy_from(&del10); - del.fixed_slice_mut::(3, 3).copy_from(&del11); - del - } - - // In 2D we just unroll the computation because - // it's just easier that way. - #[cfg(feature = "dim2")] - { - panic!("Properly handle the rotmat2"); - let m11 = im2 + rmat2.x * rmat2.x * ii2; - let m12 = rmat2.x * rmat2.y * ii2; - let m22 = im2 + rmat2.y * rmat2.y * ii2; - let m13 = rmat2.x * ii2; - let m23 = rmat2.y * ii2; - let m33 = ii2; - Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) - } - } - pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, @@ -564,16 +531,24 @@ impl GenericVelocityGroundConstraint { // TODO: duplicated code with the non-ground constraint. pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; - if let JointParams::GenericJoint(fixed) = &mut joint.params { - let impulse = Vector6::new( - self.vel.lin_impulse.x, - self.vel.lin_impulse.y, - self.vel.lin_impulse.z, - self.vel.ang_impulse.x, - self.vel.ang_impulse.y, - self.vel.ang_impulse.z, - ); - fixed.impulse = impulse; + match &mut joint.params { + JointParams::GenericJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.impulse[3] = self.vel.ang_impulse.x; + out.impulse[4] = self.vel.ang_impulse.y; + out.impulse[5] = self.vel.ang_impulse.z; + } + JointParams::RevoluteJoint(out) => { + out.impulse[0] = self.vel.lin_impulse.x; + out.impulse[1] = self.vel.lin_impulse.y; + out.impulse[2] = self.vel.lin_impulse.z; + out.motor_impulse = self.vel.ang_impulse.x; + out.impulse[3] = self.vel.ang_impulse.y; + out.impulse[4] = self.vel.ang_impulse.z; + } + _ => unimplemented!(), } } } From a1ddda5077811e07b1f6d067969d692eafa32577 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Wed, 17 Feb 2021 15:57:58 +0100 Subject: [PATCH 07/20] Revolute joint constraints: properly adjust the angular impulse and torque projection. --- .../revolute_position_constraint.rs | 253 +++++++++++++-- .../revolute_velocity_constraint.rs | 290 +++++++++++++----- 2 files changed, 441 insertions(+), 102 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index afc23f3..e468508 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -1,20 +1,22 @@ use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; -use crate::utils::WAngularInertia; -use na::Unit; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use na::{Matrix3x2, Matrix5, Unit}; #[derive(Debug)] pub(crate) struct RevolutePositionConstraint { position1: usize, position2: usize, + local_com1: Point, + local_com2: Point, + im1: Real, im2: Real, ii1: AngularInertia, ii2: AngularInertia, - lin_inv_lhs: Real, ang_inv_lhs: AngularInertia, local_anchor1: Point, @@ -22,6 +24,8 @@ pub(crate) struct RevolutePositionConstraint { local_axis1: Unit>, local_axis2: Unit>, + local_basis1: [Vector; 2], + local_basis2: [Vector; 2], } impl RevolutePositionConstraint { @@ -30,7 +34,6 @@ impl RevolutePositionConstraint { let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; - let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); Self { @@ -38,14 +41,17 @@ impl RevolutePositionConstraint { im2, ii1, ii2, - lin_inv_lhs, ang_inv_lhs, + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, position1: rb1.active_set_offset, position2: rb2.active_set_offset, + local_basis1: cparams.basis1, + local_basis2: cparams.basis2, } } @@ -53,28 +59,123 @@ impl RevolutePositionConstraint { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; - let axis1 = position1 * self.local_axis1; - let axis2 = position2 * self.local_axis2; - let delta_rot = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let ang_error = delta_rot.scaled_axis() * params.joint_erp; - let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error); - - position1.rotation = - Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; - position2.rotation = - Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; - let anchor1 = position1 * self.local_anchor1; let anchor2 = position2 * self.local_anchor2; + let axis1 = position1 * self.local_axis1; + let axis2 = position2 * self.local_axis2; + + let basis1 = Matrix3x2::from_columns(&[ + position1 * self.local_basis1[0], + position1 * self.local_basis1[1], + ]); + let basis2 = Matrix3x2::from_columns(&[ + position2 * self.local_basis2[0], + position2 * self.local_basis2[1], + ]); + + let basis_filter1 = basis1 * basis1.transpose(); + let basis_filter2 = basis2 * basis2.transpose(); + let basis2 = basis_filter2 * basis1; + + let r1 = anchor1 - position1 * self.local_com1; + let r2 = anchor2 - position2 * self.local_com2; + let r1_mat = basis_filter1 * r1.gcross_matrix(); + let r2_mat = basis_filter2 * r2.gcross_matrix(); + + let mut lhs = Matrix5::zeros(); + let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2) + + self.ii1.quadform(&r1_mat).add_diagonal(self.im1); + let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat)) + basis1.tr_mul(&(self.ii1 * r1_mat)); + let lhs11 = (self.ii1.quadform3x2(&basis1) + self.ii2.quadform3x2(&basis2)).into_matrix(); + + // Note that cholesky won't read the upper-right part + // of lhs so we don't have to fill it. + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0) + .copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3) + .copy_from(&lhs11); + + let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse(); let delta_tra = anchor2 - anchor1; let lin_error = delta_tra * params.joint_erp; - let lin_impulse = self.lin_inv_lhs * lin_error; + let delta_rot = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp; + let error = na::Vector5::new( + lin_error.x, + lin_error.y, + lin_error.z, + ang_error.x, + ang_error.y, + ); + let impulse = inv_lhs * error; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = basis2 * impulse.fixed_rows::(3).into_owned(); + + let rot1 = self.ii1 * (r1_mat * lin_impulse + ang_impulse1); + let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2); + position1.rotation = Rotation::new(rot1) * position1.rotation; + position2.rotation = Rotation::new(-rot2) * position2.rotation; position1.translation.vector += self.im1 * lin_impulse; position2.translation.vector -= self.im2 * lin_impulse; + /* + /* + * Linear part. + */ + { + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + + let r1 = anchor1 - position1 * self.local_com1; + let r2 = anchor2 - position2 * self.local_com2; + // TODO: don't the the "to_matrix". + let lhs = (self + .ii2 + .quadform(&r2.gcross_matrix()) + .add_diagonal(self.im2) + + self + .ii1 + .quadform(&r1.gcross_matrix()) + .add_diagonal(self.im1)) + .into_matrix(); + let inv_lhs = lhs.try_inverse().unwrap(); + + let delta_tra = anchor2 - anchor1; + let lin_error = delta_tra * params.joint_erp; + let lin_impulse = inv_lhs * lin_error; + + let rot1 = self.ii1 * r1.gcross(lin_impulse); + let rot2 = self.ii2 * r2.gcross(lin_impulse); + position1.rotation = Rotation::new(rot1) * position1.rotation; + position2.rotation = Rotation::new(-rot2) * position2.rotation; + position1.translation.vector += self.im1 * lin_impulse; + position2.translation.vector -= self.im2 * lin_impulse; + } + + /* + * Angular part. + */ + { + let axis1 = position1 * self.local_axis1; + let axis2 = position2 * self.local_axis2; + let delta_rot = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + let ang_error = delta_rot.scaled_axis() * params.joint_erp; + let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error); + + position1.rotation = + Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; + position2.rotation = + Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; + } + */ + positions[self.position1 as usize] = position1; positions[self.position2 as usize] = position2; } @@ -83,10 +184,16 @@ impl RevolutePositionConstraint { #[derive(Debug)] pub(crate) struct RevolutePositionGroundConstraint { position2: usize, + local_com2: Point, + im2: Real, + ii2: AngularInertia, anchor1: Point, local_anchor2: Point, axis1: Unit>, local_axis2: Unit>, + + basis1: [Vector; 2], + local_basis2: [Vector; 2], } impl RevolutePositionGroundConstraint { @@ -100,42 +207,138 @@ impl RevolutePositionGroundConstraint { let local_anchor2; let axis1; let local_axis2; + let basis1; + let local_basis2; if flipped { anchor1 = rb1.predicted_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; axis1 = rb1.predicted_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; + basis1 = [ + rb1.predicted_position * cparams.basis2[0], + rb1.predicted_position * cparams.basis2[1], + ]; + local_basis2 = cparams.basis1; } else { anchor1 = rb1.predicted_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; axis1 = rb1.predicted_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; + basis1 = [ + rb1.predicted_position * cparams.basis1[0], + rb1.predicted_position * cparams.basis1[1], + ]; + local_basis2 = cparams.basis2; }; Self { anchor1, local_anchor2, + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, axis1, local_axis2, position2: rb2.active_set_offset, + basis1, + local_basis2, } } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; + let anchor1 = self.anchor1; + let anchor2 = position2 * self.local_anchor2; + let axis1 = self.axis1; let axis2 = position2 * self.local_axis2; - let delta_rot = - Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp) - .unwrap_or_else(Rotation::identity); - position2.rotation = delta_rot * position2.rotation; + let basis1 = Matrix3x2::from_columns(&self.basis1[..]); + let basis2 = Matrix3x2::from_columns(&[ + position2 * self.local_basis2[0], + position2 * self.local_basis2[1], + ]); - let anchor2 = position2 * self.local_anchor2; - let delta_tra = anchor2 - self.anchor1; + let basis_filter2 = basis2 * basis2.transpose(); + let basis2 = basis_filter2 * basis1; + + let r2 = anchor2 - position2 * self.local_com2; + let r2_mat = basis_filter2 * r2.gcross_matrix(); + + let mut lhs = Matrix5::zeros(); + let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2); + let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat)); + let lhs11 = self.ii2.quadform3x2(&basis2).into_matrix(); + + // Note that cholesky won't read the upper-right part + // of lhs so we don't have to fill it. + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0) + .copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3) + .copy_from(&lhs11); + + let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse(); + + let delta_tra = anchor2 - anchor1; let lin_error = delta_tra * params.joint_erp; - position2.translation.vector -= lin_error; + let delta_rot = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); + + let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp; + let error = na::Vector5::new( + lin_error.x, + lin_error.y, + lin_error.z, + ang_error.x, + ang_error.y, + ); + let impulse = inv_lhs * error; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + let ang_impulse2 = basis2 * impulse.fixed_rows::(3).into_owned(); + + let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2); + position2.rotation = Rotation::new(-rot2) * position2.rotation; + position2.translation.vector -= self.im2 * lin_impulse; + + /* + /* + * Linear part. + */ + { + let anchor2 = position2 * self.local_anchor2; + + let r2 = anchor2 - position2 * self.local_com2; + // TODO: don't the the "to_matrix". + let lhs = self + .ii2 + .quadform(&r2.gcross_matrix()) + .add_diagonal(self.im2) + .into_matrix(); + let inv_lhs = lhs.try_inverse().unwrap(); + + let delta_tra = anchor2 - self.anchor1; + let lin_error = delta_tra * params.joint_erp; + let lin_impulse = inv_lhs * lin_error; + + let rot2 = self.ii2 * r2.gcross(lin_impulse); + position2.rotation = Rotation::new(-rot2) * position2.rotation; + position2.translation.vector -= self.im2 * lin_impulse; + } + + /* + * Angular part. + */ + { + let axis2 = position2 * self.local_axis2; + let delta_rot = Rotation::rotation_between_axis(&self.axis1, &axis2) + .unwrap_or_else(Rotation::identity); + let ang_error = delta_rot.scaled_axis() * params.joint_erp; + position2.rotation = Rotation::new(-ang_error) * position2.rotation; + } + */ positions[self.position2 as usize] = position2; } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 6270a8e..650a9e5 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -1,10 +1,12 @@ -use crate::dynamics::solver::DeltaVel; +use crate::dynamics::solver::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, + GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, + RigidBody, }; -use crate::math::{AngularInertia, Real, Vector}; +use crate::math::{AngularInertia, Real, Rotation, Vector}; +use crate::na::UnitQuaternion; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, RealField, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct RevoluteVelocityConstraint { @@ -13,14 +15,23 @@ pub(crate) struct RevoluteVelocityConstraint { joint_id: JointIndex, - r1: Vector, - r2: Vector, + r1_mat: Matrix3, + r2_mat: Matrix3, inv_lhs: Matrix5, rhs: Vector5, impulse: Vector5, + motor_inv_lhs: Real, + motor_rhs: Real, + motor_impulse: Real, + motor_max_impulse: Real, + + motor_axis1: Vector, + motor_axis2: Vector, + basis1: Matrix3x2, + basis2: Matrix3x2, im1: Real, im2: Real, @@ -35,41 +46,41 @@ impl RevoluteVelocityConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &RevoluteJoint, - ) -> Self { + joint: &RevoluteJoint, + ) -> AnyJointVelocityConstraint { // Linear part. - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; let basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], ]); + let basis_filter1 = basis1 * basis1.transpose(); + let basis2 = Matrix3x2::from_columns(&[ + rb2.position * joint.basis2[0], + rb2.position * joint.basis2[1], + ]); + let basis_filter2 = basis2 * basis2.transpose(); + let basis2 = basis_filter2 * basis1; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: to simplify, we use basis2 = basis1. - // Though we may want to test if that does not introduce any instability. let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; - let r1_mat = r1.gcross_matrix(); + let r1_mat = basis_filter1 * r1.gcross_matrix(); let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r2 = anchor2 - rb2.world_com; - let r2_mat = r2.gcross_matrix(); + let r2_mat = basis_filter2 * r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat)); - let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix(); + let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat)); + let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix(); - // Note that cholesky won't read the upper-right part + // Note that Cholesky won't read the upper-right part // of lhs so we don't have to fill it. lhs.fixed_slice_mut::(0, 0) .copy_from(&lhs00.into_matrix()); @@ -78,43 +89,99 @@ impl RevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel)); - let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let lin_rhs = (rb2.linvel - r2_mat * rb2.angvel) - (rb1.linvel - r1_mat * rb1.angvel); + let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + let mut rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); - RevoluteVelocityConstraint { + /* + * Motor. + */ + let motor_axis1 = rb1.position * *joint.local_axis1; + let motor_axis2 = rb2.position * *joint.local_axis2; + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 0.0; + let mut motor_max_impulse = 0.0; + + if let Some(motor_target_vel) = joint.motor_target_vel { + motor_rhs = + rb2.angvel.dot(&motor_axis1) - rb1.angvel.dot(&motor_axis1) - motor_target_vel; + motor_inv_lhs = crate::utils::inv( + motor_axis2.dot(&ii2.transform_vector(motor_axis2)) + + motor_axis1.dot(&ii1.transform_vector(motor_axis1)), + ); + motor_max_impulse = joint.motor_max_torque; + } + + /* + * Adjust the warmstart impulse. + * If the velocity along the free axis is somewhat high, + * we need to adjust the angular warmstart impulse because it + * may have a direction that is too different than last frame, + * making it counter-productive. + */ + let mut impulse = joint.impulse * params.warmstart_coeff; + let axis_rot = Rotation::rotation_between(&joint.prev_axis1, &motor_axis1) + .unwrap_or_else(UnitQuaternion::identity); + let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse)); + impulse[3] = rotated_impulse.x * params.warmstart_coeff; + impulse[4] = rotated_impulse.y * params.warmstart_coeff; + + let result = RevoluteVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, mj_lambda2: rb2.active_set_offset, im1, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, basis1, + basis2, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, + impulse, inv_lhs, rhs, - r1, - r2, - } + r1_mat, + r2_mat, + motor_rhs, + motor_inv_lhs, + motor_max_impulse, + motor_axis1, + motor_axis2, + motor_impulse: joint.motor_impulse * params.warmstart_coeff, + }; + + AnyJointVelocityConstraint::RevoluteConstraint(result) } pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = self.impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); - mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.linear += self.im1 * lin_impulse1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); - mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.linear -= self.im2 * lin_impulse2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); + + /* + * Motor + */ + { + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.motor_axis1 * self.motor_impulse); + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.motor_axis2 * self.motor_impulse); + } mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -126,26 +193,43 @@ impl RevoluteVelocityConstraint { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2) - - mj_lambda1.linear - - ang_vel1.gcross(self.r1); - let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1)); + + let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2) + - (mj_lambda1.linear - self.r1_mat * ang_vel1); + let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; - let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * impulse.fixed_rows::(3).into_owned(); - mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.linear += self.im1 * lin_impulse1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); - mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.linear -= self.im2 * lin_impulse2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); + + /* + * Motor. + */ + if self.motor_inv_lhs != 0.0 { + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1); + let rhs = ang_dvel + self.motor_rhs; + let impulse = self.motor_inv_lhs * rhs; + self.motor_impulse += impulse; + + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse); + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse); + } mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -155,6 +239,10 @@ impl RevoluteVelocityConstraint { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::RevoluteJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + let rot_part = self.impulse.fixed_rows::(3).into_owned(); + revolute.world_ang_impulse = self.basis1 * rot_part; + revolute.prev_axis1 = self.motor_axis1; + revolute.motor_impulse = self.motor_impulse; } } } @@ -171,7 +259,13 @@ pub(crate) struct RevoluteVelocityGroundConstraint { rhs: Vector5, impulse: Vector5, - basis1: Matrix3x2, + motor_axis2: Vector, + motor_inv_lhs: Real, + motor_rhs: Real, + motor_impulse: Real, + motor_max_impulse: Real, + + basis2: Matrix3x2, im2: Real, @@ -184,34 +278,29 @@ impl RevoluteVelocityGroundConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &RevoluteJoint, + joint: &RevoluteJoint, flipped: bool, - ) -> Self { + ) -> AnyJointVelocityConstraint { let anchor2; let anchor1; - let basis1; + let basis2; if flipped { - anchor1 = rb1.position * cparams.local_anchor2; - anchor2 = rb2.position * cparams.local_anchor1; - basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis2[0], - rb1.position * cparams.basis2[1], + anchor1 = rb1.position * joint.local_anchor2; + anchor2 = rb2.position * joint.local_anchor1; + basis2 = Matrix3x2::from_columns(&[ + rb2.position * joint.basis2[0], + rb2.position * joint.basis2[1], ]); } else { - anchor1 = rb1.position * cparams.local_anchor1; - anchor2 = rb2.position * cparams.local_anchor2; - basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + anchor1 = rb1.position * joint.local_anchor1; + anchor2 = rb2.position * joint.local_anchor2; + basis2 = Matrix3x2::from_columns(&[ + rb2.position * joint.basis1[0], + rb2.position * joint.basis1[1], ]); }; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = /*r21 * */ basis1; let im2 = rb2.effective_inv_mass; let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; @@ -220,8 +309,8 @@ impl RevoluteVelocityGroundConstraint { let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)); - let lhs11 = ii2.quadform3x2(&basis1).into_matrix(); + let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)); + let lhs11 = ii2.quadform3x2(&basis2).into_matrix(); // Note that cholesky won't read the upper-right part // of lhs so we don't have to fill it. @@ -233,33 +322,64 @@ impl RevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel)); + let ang_rhs = basis2.tr_mul(&(rb2.angvel - rb1.angvel)); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); - RevoluteVelocityGroundConstraint { + /* + * Motor part. + */ + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 0.0; + let mut motor_max_impulse = 0.0; + let mut motor_axis2 = Vector::zeros(); + + if let Some(motor_target_vel) = joint.motor_target_vel { + motor_axis2 = rb2.position * *joint.local_axis2; + motor_rhs = rb2.angvel.dot(&motor_axis2) - motor_target_vel; + motor_inv_lhs = crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2))); + motor_max_impulse = joint.motor_max_torque; + } + + let result = RevoluteVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, - basis1, + impulse: joint.impulse * params.warmstart_coeff, + basis2, inv_lhs, rhs, r2, - } + motor_inv_lhs, + motor_impulse: joint.motor_impulse, + motor_axis2, + motor_max_impulse, + motor_rhs, + }; + + AnyJointVelocityConstraint::RevoluteGroundConstraint(result) } pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + /* + * Motor + */ + { + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.motor_axis2 * self.motor_impulse); + } + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -267,20 +387,35 @@ impl RevoluteVelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let ang_vel2 = ang_vel2 - self.motor_axis2 * ang_vel2.dot(&self.motor_axis2); + let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let ang_dvel = self.basis1.tr_mul(&ang_vel2); + let ang_dvel = self.basis2.tr_mul(&ang_vel2); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= self.im2 * lin_impulse; mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + /* + * Motor. + */ + if self.motor_inv_lhs != 0.0 { + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let ang_dvel = ang_vel2.dot(&self.motor_axis2); + let rhs = ang_dvel + self.motor_rhs; + let impulse = self.motor_inv_lhs * rhs; + self.motor_impulse += impulse; + + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse); + } + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -289,6 +424,7 @@ impl RevoluteVelocityGroundConstraint { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::RevoluteJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + revolute.motor_impulse = self.motor_impulse; } } } From e9f17f32e8dda4b97d2eb7b2118b7373d0c554d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Fri, 19 Feb 2021 15:21:25 +0100 Subject: [PATCH 08/20] Complete the implementation of non-simd joint motor for the revolute joint. --- src/dynamics/joint/generic_joint.rs | 22 +++ src/dynamics/joint/joint.rs | 34 ++-- src/dynamics/joint/mod.rs | 6 +- src/dynamics/joint/revolute_joint.rs | 95 ++++++++++- src/dynamics/joint/spring_model.rs | 59 +++++++ src/dynamics/mod.rs | 9 +- .../generic_velocity_constraint.rs | 152 ++++++++++-------- .../joint_constraint/joint_constraint.rs | 84 +++++----- .../joint_position_constraint.rs | 39 +++-- src/dynamics/solver/joint_constraint/mod.rs | 40 ++--- .../revolute_position_constraint.rs | 126 +-------------- .../revolute_velocity_constraint.rs | 122 ++++++++++---- src_testbed/nphysics_backend.rs | 11 +- src_testbed/physx_backend.rs | 27 +++- 14 files changed, 483 insertions(+), 343 deletions(-) create mode 100644 src/dynamics/joint/spring_model.rs diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index fa35520..5d776b0 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -54,6 +54,13 @@ impl GenericJoint { } } + pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) { + self.min_velocity[dof as usize] = target_vel; + self.max_velocity[dof as usize] = target_vel; + self.min_impulse[dof as usize] = -max_force; + self.max_impulse[dof as usize] = max_force; + } + pub fn free_dof(&mut self, dof: u8) { self.min_position[dof as usize] = -Real::MAX; self.max_position[dof as usize] = Real::MAX; @@ -82,6 +89,18 @@ impl From for GenericJoint { let mut result = Self::new(local_anchor1, local_anchor2); result.free_dof(3); + + if joint.motor_damping != 0.0 { + result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse); + } + + result.impulse[0] = joint.impulse[0]; + result.impulse[1] = joint.impulse[1]; + result.impulse[2] = joint.impulse[2]; + result.impulse[3] = joint.motor_impulse; + result.impulse[4] = joint.impulse[3]; + result.impulse[5] = joint.impulse[4]; + result } } @@ -92,6 +111,9 @@ impl From for GenericJoint { let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero()); let mut result = Self::new(local_anchor1, local_anchor2); + result.impulse[0] = joint.impulse[0]; + result.impulse[1] = joint.impulse[1]; + result.impulse[2] = joint.impulse[2]; result.free_dof(3); result.free_dof(4); result.free_dof(5); diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index 31c5e0a..fe77b4b 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -1,8 +1,6 @@ #[cfg(feature = "dim3")] use crate::dynamics::RevoluteJoint; -use crate::dynamics::{ - BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle, -}; +use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -19,7 +17,7 @@ pub enum JointParams { /// A revolute joint that removes all degrees of degrees of freedom between the affected /// bodies except for the translation along one axis. RevoluteJoint(RevoluteJoint), - GenericJoint(GenericJoint), + // GenericJoint(GenericJoint), } impl JointParams { @@ -29,7 +27,7 @@ impl JointParams { JointParams::BallJoint(_) => 0, JointParams::FixedJoint(_) => 1, JointParams::PrismaticJoint(_) => 2, - JointParams::GenericJoint(_) => 3, + // JointParams::GenericJoint(_) => 3, #[cfg(feature = "dim3")] JointParams::RevoluteJoint(_) => 4, } @@ -53,14 +51,14 @@ impl JointParams { } } - /// Gets a reference to the underlying generic joint, if `self` is one. - pub fn as_generic_joint(&self) -> Option<&GenericJoint> { - if let JointParams::GenericJoint(j) = self { - Some(j) - } else { - None - } - } + // /// Gets a reference to the underlying generic joint, if `self` is one. + // pub fn as_generic_joint(&self) -> Option<&GenericJoint> { + // if let JointParams::GenericJoint(j) = self { + // Some(j) + // } else { + // None + // } + // } /// Gets a reference to the underlying prismatic joint, if `self` is one. pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> { @@ -94,11 +92,11 @@ impl From for JointParams { } } -impl From for JointParams { - fn from(j: GenericJoint) -> Self { - JointParams::GenericJoint(j) - } -} +// impl From for JointParams { +// fn from(j: GenericJoint) -> Self { +// JointParams::GenericJoint(j) +// } +// } #[cfg(feature = "dim3")] impl From for JointParams { diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 92dd715..72a7483 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,18 +1,20 @@ pub use self::ball_joint::BallJoint; pub use self::fixed_joint::FixedJoint; -pub use self::generic_joint::GenericJoint; +// pub use self::generic_joint::GenericJoint; pub use self::joint::{Joint, JointParams}; pub(crate) use self::joint_set::{JointGraphEdge, JointIndex}; pub use self::joint_set::{JointHandle, JointSet}; pub use self::prismatic_joint::PrismaticJoint; #[cfg(feature = "dim3")] pub use self::revolute_joint::RevoluteJoint; +pub use self::spring_model::SpringModel; mod ball_joint; mod fixed_joint; -mod generic_joint; +// mod generic_joint; mod joint; mod joint_set; mod prismatic_joint; #[cfg(feature = "dim3")] mod revolute_joint; +mod spring_model; diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index ad7db0d..9af0ae6 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -1,6 +1,7 @@ -use crate::math::{Point, Real, Vector}; +use crate::dynamics::SpringModel; +use crate::math::{Isometry, Point, Real, Vector}; use crate::utils::WBasis; -use na::{Unit, Vector5}; +use na::{RealField, Unit, Vector5}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -22,6 +23,28 @@ pub struct RevoluteJoint { /// /// The impulse applied to the second body is given by `-impulse`. pub impulse: Vector5, + /// The target relative angular velocity the motor will attempt to reach. + pub motor_target_vel: Real, + /// The target relative angle along the joint axis the motor will attempt to reach. + pub motor_target_pos: Real, + /// The motor's stiffness. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_stiffness: Real, + /// The motor's damping. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_damping: Real, + /// The maximal impulse the motor is able to deliver. + pub motor_max_impulse: Real, + /// The angular impulse applied by the motor. + pub motor_impulse: Real, + /// The spring-like model used by the motor to reach the target velocity and . + pub motor_model: SpringModel, + // Used to handle cases where the position target ends up being more than pi radians away. + pub(crate) motor_last_angle: Real, + // The angular impulse expressed in world-space. + pub(crate) world_ang_impulse: Vector, + // The world-space orientation of the free axis of the first attached body. + pub(crate) prev_axis1: Vector, } impl RevoluteJoint { @@ -41,6 +64,74 @@ impl RevoluteJoint { basis1: local_axis1.orthonormal_basis(), basis2: local_axis2.orthonormal_basis(), impulse: na::zero(), + world_ang_impulse: na::zero(), + motor_target_vel: 0.0, + motor_target_pos: 0.0, + motor_stiffness: 0.0, + motor_damping: 0.0, + motor_max_impulse: Real::MAX, + motor_impulse: 0.0, + prev_axis1: *local_axis1, + motor_model: SpringModel::VelocityBased, + motor_last_angle: 0.0, } } + + pub fn configure_motor_model(&mut self, model: SpringModel) { + self.motor_model = model; + } + + pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) { + self.configure_motor(target_pos, 0.0, stiffness, damping) + } + + pub fn configure_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) { + self.motor_target_vel = target_vel; + self.motor_target_pos = target_pos; + self.motor_stiffness = stiffness; + self.motor_damping = damping; + } + + /// Estimates the current position of the motor angle. + pub fn estimate_motor_angle( + &self, + body_pos1: &Isometry, + body_pos2: &Isometry, + ) -> Real { + let motor_axis1 = body_pos1 * self.local_axis1; + let ref1 = body_pos1 * self.basis1[0]; + let ref2 = body_pos2 * self.basis2[0]; + + let last_angle_cycles = (self.motor_last_angle / Real::two_pi()).trunc() * Real::two_pi(); + + // Measure the position between 0 and 2-pi + let new_angle = if ref1.cross(&ref2).dot(&motor_axis1) < 0.0 { + Real::two_pi() - ref1.angle(&ref2) + } else { + ref1.angle(&ref2) + }; + + // The last angle between 0 and 2-pi + let last_angle_zero_two_pi = self.motor_last_angle - last_angle_cycles; + + // Figure out the smallest angle differance. + let mut angle_diff = new_angle - last_angle_zero_two_pi; + if angle_diff > Real::pi() { + angle_diff -= Real::two_pi() + } else if angle_diff < -Real::pi() { + angle_diff += Real::two_pi() + } + + self.motor_last_angle + angle_diff + } } diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs new file mode 100644 index 0000000..dd6035d --- /dev/null +++ b/src/dynamics/joint/spring_model.rs @@ -0,0 +1,59 @@ +use crate::math::Real; + +/// The spring-like model used for constraints resolution. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SpringModel { + /// No equation is solved. + Disabled, + /// 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))` + AccelerationBased, + /// The solved spring-like equation is: + /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))` + ForceBased, +} + +impl SpringModel { + /// Combines the coefficients used for solving the spring equation. + /// + /// Returns the new coefficients (stiffness, damping, inv_lhs_scale, 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. + pub fn combine_coefficients( + self, + dt: Real, + stiffness: Real, + damping: Real, + ) -> (Real, Real, Real, bool) { + match self { + SpringModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true), + SpringModel::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) + } + SpringModel::ForceBased => { + let effective_stiffness = stiffness * dt; + let effective_damping = damping * dt; + let gamma = effective_stiffness * dt + effective_damping; + (effective_stiffness, effective_damping, gamma, false) + } + SpringModel::Disabled => return (0.0, 0.0, 0.0, false), + } + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 3b6a977..eab6916 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -5,7 +5,14 @@ pub(crate) use self::joint::JointIndex; #[cfg(feature = "dim3")] pub use self::joint::RevoluteJoint; pub use self::joint::{ - BallJoint, FixedJoint, GenericJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, + BallJoint, + FixedJoint, + Joint, + JointHandle, + JointParams, + JointSet, + PrismaticJoint, + SpringModel, // GenericJoint }; pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index 0208dda..f391873 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -32,7 +32,8 @@ pub(crate) struct GenericVelocityConstraint { r1: Vector, r2: Vector, - basis: Rotation, + basis1: Rotation, + basis2: Rotation, dependant_set_mask: SpacialVector, vel: GenericConstraintPart, @@ -44,22 +45,22 @@ impl GenericVelocityConstraint { max_velocity: &SpatialVector, r1: &Vector, r2: &Vector, - basis: &Rotation, + basis1: &Rotation, + basis2: &Rotation, rb1: &RigidBody, rb2: &RigidBody, ) -> SpatialVector { - let lin_dvel = -rb1.linvel - rb1.angvel.gcross(*r1) + rb2.linvel + rb2.angvel.gcross(*r2); - let ang_dvel = -rb1.angvel + rb2.angvel; - - let lin_dvel2 = basis.inverse_transform_vector(&lin_dvel); - let ang_dvel2 = basis.inverse_transform_vector(&ang_dvel); + let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1))) + + basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2))); + let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel) + + basis2.inverse_transform_vector(&rb2.angvel); let min_linvel = min_velocity.xyz(); let min_angvel = min_velocity.fixed_rows::(DIM).into_owned(); let max_linvel = max_velocity.xyz(); let max_angvel = max_velocity.fixed_rows::(DIM).into_owned(); - let lin_rhs = lin_dvel2 - lin_dvel2.sup(&min_linvel).inf(&max_linvel); - let ang_rhs = ang_dvel2 - ang_dvel2.sup(&min_angvel).inf(&max_angvel); + let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel); + let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel); #[cfg(feature = "dim2")] return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs); @@ -120,6 +121,32 @@ impl GenericVelocityConstraint { } } + pub fn invert_partial_delassus_matrix( + min_impulse: &Vector, + max_impulse: &Vector, + dependant_set_mask: &mut Vector, + mut delassus: na::Matrix3, + ) -> na::Matrix3 { + // Adjust the Delassus matrix to take force limits into account. + // If a DoF has a force limit, then we need to make its + // constraint independent from the others because otherwise + // the force clamping will cause errors to propagate in the + // other constraints. + for i in 0..3 { + if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX { + let diag = delassus[(i, i)]; + delassus.column_mut(i).fill(0.0); + delassus.row_mut(i).fill(0.0); + delassus[(i, i)] = diag; + dependant_set_mask[i] = 0.0; + } else { + dependant_set_mask[i] = 1.0; + } + } + + delassus.try_inverse().unwrap() + } + pub fn compute_position_error( joint: &GenericJoint, anchor1: &Isometry, @@ -169,32 +196,6 @@ impl GenericVelocityConstraint { } } - pub fn invert_partial_delassus_matrix( - min_impulse: &Vector, - max_impulse: &Vector, - dependant_set_mask: &mut Vector, - mut delassus: na::Matrix3, - ) -> na::Matrix3 { - // Adjust the Delassus matrix to take force limits into account. - // If a DoF has a force limit, then we need to make its - // constraint independent from the others because otherwise - // the force clamping will cause errors to propagate in the - // other constraints. - for i in 0..3 { - if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX { - let diag = delassus[(i, i)]; - delassus.column_mut(i).fill(0.0); - delassus.row_mut(i).fill(0.0); - delassus[(i, i)] = diag; - dependant_set_mask[i] = 0.0; - } else { - dependant_set_mask[i] = 1.0; - } - } - - delassus.try_inverse().unwrap() - } - pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, @@ -204,7 +205,8 @@ impl GenericVelocityConstraint { ) -> Self { let anchor1 = rb1.position * joint.local_anchor1; let anchor2 = rb2.position * joint.local_anchor2; - let basis = anchor1.rotation; + let basis1 = anchor1.rotation; + let basis2 = anchor2.rotation; let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); @@ -219,7 +221,7 @@ impl GenericVelocityConstraint { let mut max_velocity = joint.max_velocity; let mut dependant_set_mask = SpacialVector::repeat(1.0); - let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis) + let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1) * params.inv_dt() * params.joint_erp; @@ -236,19 +238,28 @@ impl GenericVelocityConstraint { } } - let rhs = - Self::compute_velocity_error(&min_velocity, &max_velocity, &r1, &r2, &basis, rb1, rb2); + let rhs = Self::compute_velocity_error( + &min_velocity, + &max_velocity, + &r1, + &r2, + &basis1, + &basis2, + rb1, + rb2, + ); let rhs_lin = rhs.xyz(); let rhs_ang = rhs.fixed_rows::(DIM).into(); // TODO: we should keep the SdpMatrix3 type. - let rotmat = basis.to_rotation_matrix().into_inner(); - let rmat1 = r1.gcross_matrix() * rotmat; - let rmat2 = r2.gcross_matrix() * rotmat; + let rotmat1 = basis1.to_rotation_matrix().into_inner(); + let rotmat2 = basis2.to_rotation_matrix().into_inner(); + let rmat1 = r1.gcross_matrix() * rotmat1; + let rmat2 = r2.gcross_matrix() * rotmat2; let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2)) .into_matrix(); - let delassus11 = (ii1.quadform(&rotmat) + ii2.quadform(&rotmat)).into_matrix(); + let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix(); let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix( &min_pos_impulse.xyz(), @@ -288,7 +299,8 @@ impl GenericVelocityConstraint { inv_lhs_ang, r1, r2, - basis, + basis1, + basis2, dependant_set_mask, vel: GenericConstraintPart { lin_impulse, @@ -307,21 +319,20 @@ impl GenericVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_impulse = self.basis * self.vel.lin_impulse; - #[cfg(feature = "dim2")] - let ang_impulse = self.basis * self.vel.impulse[2]; - #[cfg(feature = "dim3")] - let ang_impulse = self.basis * self.vel.ang_impulse; + let lin_impulse1 = self.basis1 * self.vel.lin_impulse; + let ang_impulse1 = self.basis1 * self.vel.ang_impulse; + let lin_impulse2 = self.basis2 * self.vel.lin_impulse; + let ang_impulse2 = self.basis2 * self.vel.ang_impulse; - mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.linear += self.im1 * lin_impulse1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); - mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.linear -= self.im2 * lin_impulse2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -441,6 +452,7 @@ impl GenericVelocityGroundConstraint { &r1, &r2, &basis, + &basis, rb1, rb2, ); @@ -585,28 +597,30 @@ impl GenericConstraintPart { let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let dvel = parent.basis.inverse_transform_vector( - &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1) - + mj_lambda2.linear - + ang_vel2.gcross(parent.r2)), - ); + let dvel = parent + .basis1 + .inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1))) + + parent + .basis2 + .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2))); let err = dvel + self.rhs_lin; new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err) .sup(&self.min_lin_impulse) .inf(&self.max_lin_impulse); - let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse); + let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse); + let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse); - mj_lambda1.linear += parent.im1 * effective_impulse; + mj_lambda1.linear += parent.im1 * effective_impulse1; mj_lambda1.angular += parent .ii1_sqrt - .transform_vector(parent.r1.gcross(effective_impulse)); + .transform_vector(parent.r1.gcross(effective_impulse1)); - mj_lambda2.linear -= parent.im2 * effective_impulse; + mj_lambda2.linear -= parent.im2 * effective_impulse2; mj_lambda2.angular -= parent .ii2_sqrt - .transform_vector(parent.r2.gcross(effective_impulse)); + .transform_vector(parent.r2.gcross(effective_impulse2)); } /* @@ -618,18 +632,18 @@ impl GenericConstraintPart { let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular); - let dvel = parent - .basis - .inverse_transform_vector(&(ang_vel2 - ang_vel1)); + let dvel = parent.basis2.inverse_transform_vector(&ang_vel2) + - parent.basis1.inverse_transform_vector(&ang_vel1); let err = dvel + self.rhs_ang; new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err) .sup(&self.min_ang_impulse) .inf(&self.max_ang_impulse); - let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse); + let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse); + let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse); - mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse); - mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse); + mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1); + mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2); } (new_lin_impulse, new_ang_impulse) diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index 12b8f77..ebc3c4a 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -14,9 +14,9 @@ use super::{ #[cfg(feature = "dim3")] #[cfg(feature = "simd-is-enabled")] use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint}; -use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{ - GenericVelocityConstraint, GenericVelocityGroundConstraint, -}; +// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{ +// GenericVelocityConstraint, GenericVelocityGroundConstraint, +// }; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, @@ -38,12 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint { WFixedConstraint(WFixedVelocityConstraint), #[cfg(feature = "simd-is-enabled")] WFixedGroundConstraint(WFixedVelocityGroundConstraint), - GenericConstraint(GenericVelocityConstraint), - GenericGroundConstraint(GenericVelocityGroundConstraint), - #[cfg(feature = "simd-is-enabled")] - WGenericConstraint(WGenericVelocityConstraint), - #[cfg(feature = "simd-is-enabled")] - WGenericGroundConstraint(WGenericVelocityGroundConstraint), + // GenericConstraint(GenericVelocityConstraint), + // GenericGroundConstraint(GenericVelocityGroundConstraint), + // #[cfg(feature = "simd-is-enabled")] + // WGenericConstraint(WGenericVelocityConstraint), + // #[cfg(feature = "simd-is-enabled")] + // WGenericGroundConstraint(WGenericVelocityGroundConstraint), PrismaticConstraint(PrismaticVelocityConstraint), PrismaticGroundConstraint(PrismaticVelocityGroundConstraint), #[cfg(feature = "simd-is-enabled")] @@ -89,9 +89,9 @@ impl AnyJointVelocityConstraint { JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint( PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), ), - JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint( - GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), - ), + // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint( + // GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), + // ), #[cfg(feature = "dim3")] JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint( RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), @@ -167,11 +167,11 @@ impl AnyJointVelocityConstraint { JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint( FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped), ), - JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint( - GenericVelocityGroundConstraint::from_params( - params, joint_id, rb1, rb2, p, flipped, - ), - ), + // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint( + // GenericVelocityGroundConstraint::from_params( + // params, joint_id, rb1, rb2, p, flipped, + // ), + // ), JointParams::PrismaticJoint(p) => { AnyJointVelocityConstraint::PrismaticGroundConstraint( PrismaticVelocityGroundConstraint::from_params( @@ -180,10 +180,8 @@ impl AnyJointVelocityConstraint { ) } #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint( - RevoluteVelocityGroundConstraint::from_params( - params, joint_id, rb1, rb2, p, flipped, - ), + JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params( + params, joint_id, rb1, rb2, p, flipped, ), } } @@ -267,12 +265,12 @@ impl AnyJointVelocityConstraint { AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas), - AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas), - AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas), + // AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas), + // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] @@ -307,12 +305,12 @@ impl AnyJointVelocityConstraint { AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas), - AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas), - AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas), + // AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas), + // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] @@ -355,16 +353,16 @@ impl AnyJointVelocityConstraint { AnyJointVelocityConstraint::WFixedGroundConstraint(c) => { c.writeback_impulses(joints_all) } - AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all), - AnyJointVelocityConstraint::GenericGroundConstraint(c) => { - c.writeback_impulses(joints_all) - } - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all), - #[cfg(feature = "simd-is-enabled")] - AnyJointVelocityConstraint::WGenericGroundConstraint(c) => { - c.writeback_impulses(joints_all) - } + // AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all), + // AnyJointVelocityConstraint::GenericGroundConstraint(c) => { + // c.writeback_impulses(joints_all) + // } + // #[cfg(feature = "simd-is-enabled")] + // AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => { + // c.writeback_impulses(joints_all) + // } AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all), AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => { c.writeback_impulses(joints_all) diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index a13cc27..40f5d5a 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -1,7 +1,6 @@ use super::{ BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint, - FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint, - PrismaticPositionConstraint, PrismaticPositionGroundConstraint, + FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint, }; #[cfg(feature = "dim3")] use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; @@ -33,12 +32,12 @@ pub(crate) enum AnyJointPositionConstraint { WFixedJoint(WFixedPositionConstraint), #[cfg(feature = "simd-is-enabled")] WFixedGroundConstraint(WFixedPositionGroundConstraint), - GenericJoint(GenericPositionConstraint), - GenericGroundConstraint(GenericPositionGroundConstraint), - #[cfg(feature = "simd-is-enabled")] - WGenericJoint(WGenericPositionConstraint), - #[cfg(feature = "simd-is-enabled")] - WGenericGroundConstraint(WGenericPositionGroundConstraint), + // GenericJoint(GenericPositionConstraint), + // GenericGroundConstraint(GenericPositionGroundConstraint), + // #[cfg(feature = "simd-is-enabled")] + // WGenericJoint(WGenericPositionConstraint), + // #[cfg(feature = "simd-is-enabled")] + // WGenericGroundConstraint(WGenericPositionGroundConstraint), PrismaticJoint(PrismaticPositionConstraint), PrismaticGroundConstraint(PrismaticPositionGroundConstraint), #[cfg(feature = "simd-is-enabled")] @@ -69,9 +68,9 @@ impl AnyJointPositionConstraint { JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint( FixedPositionConstraint::from_params(rb1, rb2, p), ), - JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint( - GenericPositionConstraint::from_params(rb1, rb2, p), - ), + // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint( + // GenericPositionConstraint::from_params(rb1, rb2, p), + // ), JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint( PrismaticPositionConstraint::from_params(rb1, rb2, p), ), @@ -140,9 +139,9 @@ impl AnyJointPositionConstraint { JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint( FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped), ), - JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint( - GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped), - ), + // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint( + // GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped), + // ), JointParams::PrismaticJoint(p) => { AnyJointPositionConstraint::PrismaticGroundConstraint( PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped), @@ -219,12 +218,12 @@ impl AnyJointPositionConstraint { AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions), #[cfg(feature = "simd-is-enabled")] AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions), - AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions), - AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions), - #[cfg(feature = "simd-is-enabled")] - AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions), - #[cfg(feature = "simd-is-enabled")] - AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions), + // AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions), + // AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions), + // #[cfg(feature = "simd-is-enabled")] + // AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions), AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions), AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions), #[cfg(feature = "simd-is-enabled")] diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index b8e833e..9196e69 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -18,20 +18,20 @@ pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocity pub(self) use fixed_velocity_constraint_wide::{ WFixedVelocityConstraint, WFixedVelocityGroundConstraint, }; -pub(self) use generic_position_constraint::{ - GenericPositionConstraint, GenericPositionGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -pub(self) use generic_position_constraint_wide::{ - WGenericPositionConstraint, WGenericPositionGroundConstraint, -}; -pub(self) use generic_velocity_constraint::{ - GenericVelocityConstraint, GenericVelocityGroundConstraint, -}; -#[cfg(feature = "simd-is-enabled")] -pub(self) use generic_velocity_constraint_wide::{ - WGenericVelocityConstraint, WGenericVelocityGroundConstraint, -}; +// pub(self) use generic_position_constraint::{ +// GenericPositionConstraint, GenericPositionGroundConstraint, +// }; +// #[cfg(feature = "simd-is-enabled")] +// pub(self) use generic_position_constraint_wide::{ +// WGenericPositionConstraint, WGenericPositionGroundConstraint, +// }; +// pub(self) use generic_velocity_constraint::{ +// GenericVelocityConstraint, GenericVelocityGroundConstraint, +// }; +// #[cfg(feature = "simd-is-enabled")] +// pub(self) use generic_velocity_constraint_wide::{ +// WGenericVelocityConstraint, WGenericVelocityGroundConstraint, +// }; pub(crate) use joint_constraint::AnyJointVelocityConstraint; pub(crate) use joint_position_constraint::AnyJointPositionConstraint; @@ -78,12 +78,12 @@ mod fixed_position_constraint_wide; mod fixed_velocity_constraint; #[cfg(feature = "simd-is-enabled")] mod fixed_velocity_constraint_wide; -mod generic_position_constraint; -#[cfg(feature = "simd-is-enabled")] -mod generic_position_constraint_wide; -mod generic_velocity_constraint; -#[cfg(feature = "simd-is-enabled")] -mod generic_velocity_constraint_wide; +// mod generic_position_constraint; +// #[cfg(feature = "simd-is-enabled")] +// mod generic_position_constraint_wide; +// mod generic_velocity_constraint; +// #[cfg(feature = "simd-is-enabled")] +// mod generic_velocity_constraint_wide; mod joint_constraint; mod joint_position_constraint; mod prismatic_position_constraint; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index e468508..9075ed7 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -59,72 +59,6 @@ impl RevolutePositionConstraint { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; - let axis1 = position1 * self.local_axis1; - let axis2 = position2 * self.local_axis2; - - let basis1 = Matrix3x2::from_columns(&[ - position1 * self.local_basis1[0], - position1 * self.local_basis1[1], - ]); - let basis2 = Matrix3x2::from_columns(&[ - position2 * self.local_basis2[0], - position2 * self.local_basis2[1], - ]); - - let basis_filter1 = basis1 * basis1.transpose(); - let basis_filter2 = basis2 * basis2.transpose(); - let basis2 = basis_filter2 * basis1; - - let r1 = anchor1 - position1 * self.local_com1; - let r2 = anchor2 - position2 * self.local_com2; - let r1_mat = basis_filter1 * r1.gcross_matrix(); - let r2_mat = basis_filter2 * r2.gcross_matrix(); - - let mut lhs = Matrix5::zeros(); - let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2) - + self.ii1.quadform(&r1_mat).add_diagonal(self.im1); - let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat)) + basis1.tr_mul(&(self.ii1 * r1_mat)); - let lhs11 = (self.ii1.quadform3x2(&basis1) + self.ii2.quadform3x2(&basis2)).into_matrix(); - - // Note that cholesky won't read the upper-right part - // of lhs so we don't have to fill it. - lhs.fixed_slice_mut::(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::(3, 0) - .copy_from(&lhs10); - lhs.fixed_slice_mut::(3, 3) - .copy_from(&lhs11); - - let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse(); - - let delta_tra = anchor2 - anchor1; - let lin_error = delta_tra * params.joint_erp; - let delta_rot = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - - let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp; - let error = na::Vector5::new( - lin_error.x, - lin_error.y, - lin_error.z, - ang_error.x, - ang_error.y, - ); - let impulse = inv_lhs * error; - let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse1 = basis1 * impulse.fixed_rows::(3).into_owned(); - let ang_impulse2 = basis2 * impulse.fixed_rows::(3).into_owned(); - - let rot1 = self.ii1 * (r1_mat * lin_impulse + ang_impulse1); - let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2); - position1.rotation = Rotation::new(rot1) * position1.rotation; - position2.rotation = Rotation::new(-rot2) * position2.rotation; - position1.translation.vector += self.im1 * lin_impulse; - position2.translation.vector -= self.im2 * lin_impulse; - - /* /* * Linear part. */ @@ -134,7 +68,8 @@ impl RevolutePositionConstraint { let r1 = anchor1 - position1 * self.local_com1; let r2 = anchor2 - position2 * self.local_com2; - // TODO: don't the the "to_matrix". + + // TODO: don't do the "to_matrix". let lhs = (self .ii2 .quadform(&r2.gcross_matrix()) @@ -174,7 +109,6 @@ impl RevolutePositionConstraint { position2.rotation = Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; } - */ positions[self.position1 as usize] = position1; positions[self.position2 as usize] = position2; @@ -249,61 +183,6 @@ impl RevolutePositionGroundConstraint { pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { let mut position2 = positions[self.position2 as usize]; - let anchor1 = self.anchor1; - let anchor2 = position2 * self.local_anchor2; - let axis1 = self.axis1; - let axis2 = position2 * self.local_axis2; - - let basis1 = Matrix3x2::from_columns(&self.basis1[..]); - let basis2 = Matrix3x2::from_columns(&[ - position2 * self.local_basis2[0], - position2 * self.local_basis2[1], - ]); - - let basis_filter2 = basis2 * basis2.transpose(); - let basis2 = basis_filter2 * basis1; - - let r2 = anchor2 - position2 * self.local_com2; - let r2_mat = basis_filter2 * r2.gcross_matrix(); - - let mut lhs = Matrix5::zeros(); - let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2); - let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat)); - let lhs11 = self.ii2.quadform3x2(&basis2).into_matrix(); - - // Note that cholesky won't read the upper-right part - // of lhs so we don't have to fill it. - lhs.fixed_slice_mut::(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::(3, 0) - .copy_from(&lhs10); - lhs.fixed_slice_mut::(3, 3) - .copy_from(&lhs11); - - let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse(); - - let delta_tra = anchor2 - anchor1; - let lin_error = delta_tra * params.joint_erp; - let delta_rot = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - - let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp; - let error = na::Vector5::new( - lin_error.x, - lin_error.y, - lin_error.z, - ang_error.x, - ang_error.y, - ); - let impulse = inv_lhs * error; - let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse2 = basis2 * impulse.fixed_rows::(3).into_owned(); - - let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2); - position2.rotation = Rotation::new(-rot2) * position2.rotation; - position2.translation.vector -= self.im2 * lin_impulse; - - /* /* * Linear part. */ @@ -338,7 +217,6 @@ impl RevolutePositionGroundConstraint { let ang_error = delta_rot.scaled_axis() * params.joint_erp; position2.rotation = Rotation::new(-ang_error) * position2.rotation; } - */ positions[self.position2 as usize] = position2; } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 650a9e5..1219c39 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -1,11 +1,11 @@ use crate::dynamics::solver::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{ - GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, - RigidBody, + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; use crate::math::{AngularInertia, Real, Rotation, Vector}; use crate::na::UnitQuaternion; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use downcast_rs::Downcast; use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, RealField, Vector5, U2, U3}; #[derive(Debug)] @@ -26,6 +26,7 @@ pub(crate) struct RevoluteVelocityConstraint { motor_rhs: Real, motor_impulse: Real, motor_max_impulse: Real, + motor_angle: Real, // Exists only to write it back into the joint. motor_axis1: Vector, motor_axis2: Vector, @@ -47,7 +48,7 @@ impl RevoluteVelocityConstraint { rb1: &RigidBody, rb2: &RigidBody, joint: &RevoluteJoint, - ) -> AnyJointVelocityConstraint { + ) -> Self { // Linear part. let anchor1 = rb1.position * joint.local_anchor1; let anchor2 = rb2.position * joint.local_anchor2; @@ -100,16 +101,35 @@ impl RevoluteVelocityConstraint { let motor_axis2 = rb2.position * *joint.local_axis2; let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; - let mut motor_max_impulse = 0.0; + let mut motor_max_impulse = joint.motor_max_impulse; + let mut motor_angle = 0.0; - if let Some(motor_target_vel) = joint.motor_target_vel { - motor_rhs = - rb2.angvel.dot(&motor_axis1) - rb1.angvel.dot(&motor_axis1) - motor_target_vel; - motor_inv_lhs = crate::utils::inv( - motor_axis2.dot(&ii2.transform_vector(motor_axis2)) - + motor_axis1.dot(&ii1.transform_vector(motor_axis1)), - ); - motor_max_impulse = joint.motor_max_torque; + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position); + motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness; + } + + if damping != 0.0 { + let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1); + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + crate::utils::inv( + motor_axis2.dot(&ii2.transform_vector(motor_axis2)) + + motor_axis1.dot(&ii1.transform_vector(motor_axis1)), + ) * gamma + } else { + gamma + }; + motor_rhs /= gamma; } /* @@ -125,8 +145,10 @@ impl RevoluteVelocityConstraint { let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse)); impulse[3] = rotated_impulse.x * params.warmstart_coeff; impulse[4] = rotated_impulse.y * params.warmstart_coeff; + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) + * params.warmstart_coeff; - let result = RevoluteVelocityConstraint { + RevoluteVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, mj_lambda2: rb2.active_set_offset, @@ -146,10 +168,9 @@ impl RevoluteVelocityConstraint { motor_max_impulse, motor_axis1, motor_axis2, - motor_impulse: joint.motor_impulse * params.warmstart_coeff, - }; - - AnyJointVelocityConstraint::RevoluteConstraint(result) + motor_impulse, + motor_angle, + } } pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { @@ -174,7 +195,7 @@ impl RevoluteVelocityConstraint { /* * Motor */ - { + if self.motor_inv_lhs != 0.0 { mj_lambda1.angular += self .ii1_sqrt .transform_vector(self.motor_axis1 * self.motor_impulse); @@ -224,8 +245,14 @@ impl RevoluteVelocityConstraint { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1); let rhs = ang_dvel + self.motor_rhs; - let impulse = self.motor_inv_lhs * rhs; - self.motor_impulse += impulse; + + let new_motor_impulse = na::clamp( + self.motor_impulse + self.motor_inv_lhs * rhs, + -self.motor_max_impulse, + self.motor_max_impulse, + ); + let impulse = new_motor_impulse - self.motor_impulse; + self.motor_impulse = new_motor_impulse; mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse); @@ -242,6 +269,7 @@ impl RevoluteVelocityConstraint { let rot_part = self.impulse.fixed_rows::(3).into_owned(); revolute.world_ang_impulse = self.basis1 * rot_part; revolute.prev_axis1 = self.motor_axis1; + revolute.motor_last_angle = self.motor_angle; revolute.motor_impulse = self.motor_impulse; } } @@ -264,6 +292,7 @@ pub(crate) struct RevoluteVelocityGroundConstraint { motor_rhs: Real, motor_impulse: Real, motor_max_impulse: Real, + motor_angle: Real, // Exists just for writing it into the joint. basis2: Matrix3x2, @@ -328,18 +357,41 @@ impl RevoluteVelocityGroundConstraint { /* * Motor part. */ + let motor_axis1 = rb1.position * *joint.local_axis1; + let motor_axis2 = rb2.position * *joint.local_axis2; let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; - let mut motor_max_impulse = 0.0; - let mut motor_axis2 = Vector::zeros(); + let mut motor_max_impulse = joint.motor_max_impulse; + let mut motor_angle = 0.0; - if let Some(motor_target_vel) = joint.motor_target_vel { - motor_axis2 = rb2.position * *joint.local_axis2; - motor_rhs = rb2.angvel.dot(&motor_axis2) - motor_target_vel; - motor_inv_lhs = crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2))); - motor_max_impulse = joint.motor_max_torque; + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position); + motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness; } + if damping != 0.0 { + let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1); + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2))) * gamma + } else { + gamma + }; + motor_rhs /= gamma; + } + + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) + * params.warmstart_coeff; + let result = RevoluteVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, @@ -351,10 +403,11 @@ impl RevoluteVelocityGroundConstraint { rhs, r2, motor_inv_lhs, - motor_impulse: joint.motor_impulse, + motor_impulse, motor_axis2, motor_max_impulse, motor_rhs, + motor_angle, }; AnyJointVelocityConstraint::RevoluteGroundConstraint(result) @@ -374,7 +427,7 @@ impl RevoluteVelocityGroundConstraint { /* * Motor */ - { + if self.motor_inv_lhs != 0.0 { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(self.motor_axis2 * self.motor_impulse); @@ -410,8 +463,14 @@ impl RevoluteVelocityGroundConstraint { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let ang_dvel = ang_vel2.dot(&self.motor_axis2); let rhs = ang_dvel + self.motor_rhs; - let impulse = self.motor_inv_lhs * rhs; - self.motor_impulse += impulse; + + let new_motor_impulse = na::clamp( + self.motor_impulse + self.motor_inv_lhs * rhs, + -self.motor_max_impulse, + self.motor_max_impulse, + ); + let impulse = new_motor_impulse - self.motor_impulse; + self.motor_impulse = new_motor_impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse); } @@ -425,6 +484,7 @@ impl RevoluteVelocityGroundConstraint { if let JointParams::RevoluteJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; revolute.motor_impulse = self.motor_impulse; + revolute.motor_last_angle = self.motor_angle; } } } diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index cce56e8..698e255 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -121,12 +121,11 @@ impl NPhysicsWorld { } nphysics_joints.insert(c); - } - JointParams::GenericJoint(_) => { - eprintln!( - "Joint type currently unsupported by the nphysics backend: GenericJoint." - ) - } + } // JointParams::GenericJoint(_) => { + // eprintln!( + // "Joint type currently unsupported by the nphysics backend: GenericJoint." + // ) + // } } } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 011a450..f2a4f8a 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -344,13 +344,27 @@ impl PhysxWorld { .into_physx() .into(); - physx_sys::phys_PxRevoluteJointCreate( + let revolute_joint = physx_sys::phys_PxRevoluteJointCreate( physics.as_mut_ptr(), actor1, &frame1 as *const _, actor2, &frame2 as *const _, ); + + physx_sys::PxRevoluteJoint_setDriveVelocity_mut( + revolute_joint, + params.motor_target_vel, + true, + ); + + if params.motor_damping != 0.0 { + physx_sys::PxRevoluteJoint_setRevoluteJointFlag_mut( + revolute_joint, + physx_sys::PxRevoluteJointFlag::eDRIVE_ENABLED, + true, + ); + } } JointParams::PrismaticJoint(params) => { @@ -420,12 +434,11 @@ impl PhysxWorld { actor2, &frame2 as *const _, ); - } - JointParams::GenericJoint(_) => { - eprintln!( - "Joint type currently unsupported by the nphysics backend: GenericJoint." - ) - } + } // JointParams::GenericJoint(_) => { + // eprintln!( + // "Joint type currently unsupported by the PhysX backend: GenericJoint." + // ) + // } } } } From dc8ccc0c30e75aea8f144dbfad16be088d4d4ea2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Fri, 19 Feb 2021 17:32:09 +0100 Subject: [PATCH 09/20] Make revolute joint actuation work properly even when SIMD is enabled. --- src/dynamics/joint/ball_joint.rs | 5 + src/dynamics/joint/fixed_joint.rs | 5 + src/dynamics/joint/joint.rs | 11 ++ src/dynamics/joint/prismatic_joint.rs | 5 + src/dynamics/joint/revolute_joint.rs | 6 + src/dynamics/solver/interaction_groups.rs | 6 + .../joint_constraint/joint_constraint.rs | 31 ++-- .../joint_position_constraint.rs | 28 ++-- .../revolute_velocity_constraint.rs | 21 +-- .../revolute_velocity_constraint_wide.rs | 151 ++++++++++++------ 10 files changed, 176 insertions(+), 93 deletions(-) diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index 82e2a10..f1e8fdf 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -31,4 +31,9 @@ impl BallJoint { impulse, } } + + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + true + } } diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 359e14a..2917757 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -30,4 +30,9 @@ impl FixedJoint { impulse: SpacialVector::zeros(), } } + + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + true + } } diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index fe77b4b..b4089ae 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -128,3 +128,14 @@ pub struct Joint { /// The joint geometric parameters and impulse. pub params: JointParams, } + +impl Joint { + pub fn supports_simd_constraints(&self) -> bool { + match &self.params { + JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(), + JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(), + JointParams::FixedJoint(joint) => joint.supports_simd_constraints(), + JointParams::BallJoint(joint) => joint.supports_simd_constraints(), + } + } +} diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 0edc939..6e32a01 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -135,6 +135,11 @@ impl PrismaticJoint { self.local_axis2 } + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + true + } + // FIXME: precompute this? #[cfg(feature = "dim2")] pub(crate) fn local_frame1(&self) -> Isometry { diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 9af0ae6..022779d 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -77,6 +77,12 @@ impl RevoluteJoint { } } + /// Can a SIMD constraint be used for resolving this joint? + pub fn supports_simd_constraints(&self) -> bool { + // SIMD revolute constraints don't support motors right now. + self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) + } + pub fn configure_motor_model(&mut self, model: SpringModel) { self.motor_model = model; } diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 21cc642..0f01798 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -214,6 +214,12 @@ impl InteractionGroups { continue; } + if !interaction.supports_simd_constraints() { + // This joint does not support simd constraints yet. + self.nongrouped_interactions.push(*interaction_i); + continue; + } + let ijoint = interaction.params.type_id(); let i1 = body1.active_set_offset; let i2 = body2.active_set_offset; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index ebc3c4a..ed27e51 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -7,8 +7,7 @@ use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{ WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint, - WFixedVelocityGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint, - WGenericVelocityConstraint, WGenericVelocityGroundConstraint, WPrismaticVelocityConstraint, + WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint, }; #[cfg(feature = "dim3")] @@ -122,12 +121,12 @@ impl AnyJointVelocityConstraint { params, joint_id, rbs1, rbs2, joints, )) } - JointParams::GenericJoint(_) => { - let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; - AnyJointVelocityConstraint::WGenericConstraint( - WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), - ) - } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointVelocityConstraint::WGenericConstraint( + // WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), + // ) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -221,14 +220,14 @@ impl AnyJointVelocityConstraint { ), ) } - JointParams::GenericJoint(_) => { - let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; - AnyJointVelocityConstraint::WGenericGroundConstraint( - WGenericVelocityGroundConstraint::from_params( - params, joint_id, rbs1, rbs2, joints, flipped, - ), - ) - } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointVelocityConstraint::WGenericGroundConstraint( + // WGenericVelocityGroundConstraint::from_params( + // params, joint_id, rbs1, rbs2, joints, flipped, + // ), + // ) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 40f5d5a..f0c374e 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -10,8 +10,8 @@ use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint}; #[cfg(feature = "simd-is-enabled")] use super::{ WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint, - WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint, - WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint, + WFixedPositionGroundConstraint, WPrismaticPositionConstraint, + WPrismaticPositionGroundConstraint, }; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; @@ -99,12 +99,12 @@ impl AnyJointPositionConstraint { rbs1, rbs2, joints, )) } - JointParams::GenericJoint(_) => { - let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; - AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params( - rbs1, rbs2, joints, - )) - } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params( + // rbs1, rbs2, joints, + // )) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; @@ -180,12 +180,12 @@ impl AnyJointPositionConstraint { WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) } - JointParams::GenericJoint(_) => { - let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; - AnyJointPositionConstraint::WGenericGroundConstraint( - WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), - ) - } + // JointParams::GenericJoint(_) => { + // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // AnyJointPositionConstraint::WGenericGroundConstraint( + // WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + // ) + // } JointParams::PrismaticJoint(_) => { let joints = array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 1219c39..10a042c 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -56,24 +56,25 @@ impl RevoluteVelocityConstraint { rb1.position * joint.basis1[0], rb1.position * joint.basis1[1], ]); - let basis_filter1 = basis1 * basis1.transpose(); - let basis2 = Matrix3x2::from_columns(&[ + let basis_projection1 = basis1 * basis1.transpose(); + + let basis_projection_half2 = Matrix3x2::from_columns(&[ rb2.position * joint.basis2[0], rb2.position * joint.basis2[1], ]); - let basis_filter2 = basis2 * basis2.transpose(); - let basis2 = basis_filter2 * basis1; + let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose(); + let basis2 = basis_projection2 * basis1; let im1 = rb1.effective_inv_mass; let im2 = rb2.effective_inv_mass; let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; - let r1_mat = basis_filter1 * r1.gcross_matrix(); + let r1_mat = basis_projection1 * r1.gcross_matrix(); let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r2 = anchor2 - rb2.world_com; - let r2_mat = basis_filter2 * r2.gcross_matrix(); + let r2_mat = basis_projection2 * r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); let lhs00 = @@ -318,15 +319,15 @@ impl RevoluteVelocityGroundConstraint { anchor1 = rb1.position * joint.local_anchor2; anchor2 = rb2.position * joint.local_anchor1; basis2 = Matrix3x2::from_columns(&[ - rb2.position * joint.basis2[0], - rb2.position * joint.basis2[1], + rb2.position * joint.basis1[0], + rb2.position * joint.basis1[1], ]); } else { anchor1 = rb1.position * joint.local_anchor1; anchor2 = rb2.position * joint.local_anchor2; basis2 = Matrix3x2::from_columns(&[ - rb2.position * joint.basis1[0], - rb2.position * joint.basis1[1], + rb2.position * joint.basis2[0], + rb2.position * joint.basis2[1], ]); }; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 047763d..5be1def 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -4,9 +4,11 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; -use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH, +}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct WRevoluteVelocityConstraint { @@ -15,14 +17,16 @@ pub(crate) struct WRevoluteVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r1: Vector, - r2: Vector, + r1_mat: Matrix3, + r2_mat: Matrix3, inv_lhs: Matrix5, rhs: Vector5, impulse: Vector5, + axis1: [Vector; SIMD_WIDTH], basis1: Matrix3x2, + basis2: Matrix3x2, im1: SimdReal, im2: SimdReal, @@ -37,7 +41,7 @@ impl WRevoluteVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], rbs1: [&RigidBody; SIMD_WIDTH], rbs2: [&RigidBody; SIMD_WIDTH], - cparams: [&RevoluteJoint; SIMD_WIDTH], + joints: [&RevoluteJoint; SIMD_WIDTH], ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); @@ -59,18 +63,27 @@ impl WRevoluteVelocityConstraint { ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]); let local_basis1 = [ - Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]), - Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]), + Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]), + Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]), ]; - let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let local_basis2 = [ + Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]), + Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]), + ]; + let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; let basis1 = Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]); + let basis_projection1 = basis1 * basis1.transpose(); + let basis_projection_half2 = + Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]); + let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose(); + let basis2 = basis_projection2 * basis1; // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) // .unwrap_or_else(Rotation::identity) @@ -81,19 +94,19 @@ impl WRevoluteVelocityConstraint { // Though we may want to test if that does not introduce any instability. let ii1 = ii1_sqrt.squared(); let r1 = anchor1 - world_com1; - let r1_mat = r1.gcross_matrix(); + let r1_mat = basis_projection1 * r1.gcross_matrix(); let ii2 = ii2_sqrt.squared(); let r2 = anchor2 - world_com2; - let r2_mat = r2.gcross_matrix(); + let r2_mat = basis_projection2 * r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat)); - let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix(); + let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat)); + let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix(); - // Note that cholesky won't read the upper-right part + // Note that Cholesky won't read the upper-right part // of lhs so we don't have to fill it. lhs.fixed_slice_mut::(0, 0) .copy_from(&lhs00.into_matrix()); @@ -103,23 +116,46 @@ impl WRevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); - let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1)); + let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + /* + * Adjust the warmstart impulse. + * If the velocity along the free axis is somewhat high, + * we need to adjust the angular warmstart impulse because it + * may have a direction that is too different than last frame, + * making it counter-productive. + */ + let warmstart_coeff = SimdReal::splat(params.warmstart_coeff); + let mut impulse = impulse * warmstart_coeff; + + let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH]; + let rotated_impulse = Vector::from(array![|ii| { + let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii]) + .unwrap_or_else(Rotation::identity); + axis_rot * joints[ii].world_ang_impulse + }; SIMD_WIDTH]); + + let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse); + impulse[3] = rotated_basis_impulse.x * warmstart_coeff; + impulse[4] = rotated_basis_impulse.y * warmstart_coeff; + WRevoluteVelocityConstraint { joint_id, mj_lambda1, mj_lambda2, im1, ii1_sqrt, + axis1, basis1, + basis2, im2, ii2_sqrt, - impulse: impulse * SimdReal::splat(params.warmstart_coeff), + impulse, inv_lhs, rhs, - r1, - r2, + r1_mat, + r2_mat, } } @@ -141,18 +177,20 @@ impl WRevoluteVelocityConstraint { ), }; - let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = self.impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); - mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.linear += lin_impulse1 * self.im1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); - mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.linear -= lin_impulse2 * self.im2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -184,26 +222,28 @@ impl WRevoluteVelocityConstraint { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2) - - mj_lambda1.linear - - ang_vel1.gcross(self.r1); - let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1)); + + let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2) + - (mj_lambda1.linear - self.r1_mat * ang_vel1); + let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; - let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let lin_impulse1 = impulse.fixed_rows::(0).into_owned(); + let lin_impulse2 = impulse.fixed_rows::(0).into_owned(); + let ang_impulse1 = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse2 = self.basis2 * impulse.fixed_rows::(3).into_owned(); - mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.linear += lin_impulse1 * self.im1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); - mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.linear -= lin_impulse2 * self.im2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -216,10 +256,15 @@ impl WRevoluteVelocityConstraint { } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let rot_part = self.impulse.fixed_rows::(3).into_owned(); + let world_ang_impulse = self.basis1 * rot_part; + for ii in 0..SIMD_WIDTH { let joint = &mut joints_all[self.joint_id[ii]].weight; if let JointParams::RevoluteJoint(rev) = &mut joint.params { - rev.impulse = self.impulse.extract(ii) + rev.impulse = self.impulse.extract(ii); + rev.world_ang_impulse = world_ang_impulse.extract(ii); + rev.prev_axis1 = self.axis1[ii]; } } } @@ -237,7 +282,7 @@ pub(crate) struct WRevoluteVelocityGroundConstraint { rhs: Vector5, impulse: Vector5, - basis1: Matrix3x2, + basis2: Matrix3x2, im2: SimdReal, @@ -250,7 +295,7 @@ impl WRevoluteVelocityGroundConstraint { joint_id: [JointIndex; SIMD_WIDTH], rbs1: [&RigidBody; SIMD_WIDTH], rbs2: [&RigidBody; SIMD_WIDTH], - cparams: [&RevoluteJoint; SIMD_WIDTH], + joints: [&RevoluteJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); @@ -267,22 +312,22 @@ impl WRevoluteVelocityGroundConstraint { array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]); let local_anchor1 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH], ); let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH], ); - let basis1 = Matrix3x2::from_columns(&[ - position1 + let basis2 = Matrix3x2::from_columns(&[ + position2 * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH], ), - position1 + position2 * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH], + array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH], ), ]); @@ -301,8 +346,8 @@ impl WRevoluteVelocityGroundConstraint { let mut lhs = Matrix5::zeros(); let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2); - let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)); - let lhs11 = ii2.quadform3x2(&basis1).into_matrix(); + let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)); + let lhs11 = ii2.quadform3x2(&basis2).into_matrix(); // Note that cholesky won't read the upper-right part // of lhs so we don't have to fill it. @@ -314,7 +359,7 @@ impl WRevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); - let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1)); + let ang_rhs = basis2.tr_mul(&(angvel2 - angvel1)); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); WRevoluteVelocityGroundConstraint { @@ -323,7 +368,7 @@ impl WRevoluteVelocityGroundConstraint { im2, ii2_sqrt, impulse: impulse * SimdReal::splat(params.warmstart_coeff), - basis1, + basis2, inv_lhs, rhs, r2, @@ -341,7 +386,7 @@ impl WRevoluteVelocityGroundConstraint { }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * self.impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= lin_impulse * self.im2; mj_lambda2.angular -= self @@ -366,13 +411,13 @@ impl WRevoluteVelocityGroundConstraint { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); - let ang_dvel = self.basis1.tr_mul(&ang_vel2); + let ang_dvel = self.basis2.tr_mul(&ang_vel2); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; let impulse = self.inv_lhs * rhs; self.impulse += impulse; let lin_impulse = impulse.fixed_rows::(0).into_owned(); - let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + let ang_impulse = self.basis2 * impulse.fixed_rows::(3).into_owned(); mj_lambda2.linear -= lin_impulse * self.im2; mj_lambda2.angular -= self From f5515c39736aced54f65879a42b2a74a68609ee7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Sun, 21 Feb 2021 17:14:34 +0100 Subject: [PATCH 10/20] Fix lever-arm handling in the revolute joint. --- src/dynamics/joint/revolute_joint.rs | 2 +- src/dynamics/joint/spring_model.rs | 6 ++ .../revolute_velocity_constraint.rs | 62 ++++++++++++------- .../revolute_velocity_constraint_wide.rs | 57 +++++++++-------- 4 files changed, 73 insertions(+), 54 deletions(-) diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 022779d..e1e1441 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -72,7 +72,7 @@ impl RevoluteJoint { motor_max_impulse: Real::MAX, motor_impulse: 0.0, prev_axis1: *local_axis1, - motor_model: SpringModel::VelocityBased, + motor_model: SpringModel::default(), motor_last_angle: 0.0, } } diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs index dd6035d..c2c9ebd 100644 --- a/src/dynamics/joint/spring_model.rs +++ b/src/dynamics/joint/spring_model.rs @@ -21,6 +21,12 @@ pub enum SpringModel { ForceBased, } +impl Default for SpringModel { + fn default() -> Self { + SpringModel::VelocityBased + } +} + impl SpringModel { /// Combines the coefficients used for solving the spring equation. /// diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 10a042c..ef3a1ba 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -15,8 +15,8 @@ pub(crate) struct RevoluteVelocityConstraint { joint_id: JointIndex, - r1_mat: Matrix3, - r2_mat: Matrix3, + r1: Vector, + r2: Vector, inv_lhs: Matrix5, rhs: Vector5, @@ -56,13 +56,12 @@ impl RevoluteVelocityConstraint { rb1.position * joint.basis1[0], rb1.position * joint.basis1[1], ]); - let basis_projection1 = basis1 * basis1.transpose(); - let basis_projection_half2 = Matrix3x2::from_columns(&[ + let basis2 = Matrix3x2::from_columns(&[ rb2.position * joint.basis2[0], rb2.position * joint.basis2[1], ]); - let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose(); + let basis_projection2 = basis2 * basis2.transpose(); let basis2 = basis_projection2 * basis1; let im1 = rb1.effective_inv_mass; @@ -70,13 +69,14 @@ impl RevoluteVelocityConstraint { let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; - let r1_mat = basis_projection1 * r1.gcross_matrix(); + let r1_mat = r1.gcross_matrix(); let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r2 = anchor2 - rb2.world_com; - let r2_mat = basis_projection2 * r2.gcross_matrix(); + let r2_mat = r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); + let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat)); @@ -91,7 +91,7 @@ impl RevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = (rb2.linvel - r2_mat * rb2.angvel) - (rb1.linvel - r1_mat * rb1.angvel); + let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); let mut rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); @@ -162,8 +162,8 @@ impl RevoluteVelocityConstraint { impulse, inv_lhs, rhs, - r1_mat, - r2_mat, + r1, + r2, motor_rhs, motor_inv_lhs, motor_max_impulse, @@ -186,12 +186,12 @@ impl RevoluteVelocityConstraint { mj_lambda1.linear += self.im1 * lin_impulse1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); mj_lambda2.linear -= self.im2 * lin_impulse2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); /* * Motor @@ -216,8 +216,8 @@ impl RevoluteVelocityConstraint { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2) - - (mj_lambda1.linear - self.r1_mat * ang_vel1); + let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2)) + - (mj_lambda1.linear + ang_vel1.gcross(self.r1)); let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; @@ -231,12 +231,12 @@ impl RevoluteVelocityConstraint { mj_lambda1.linear += self.im1 * lin_impulse1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); mj_lambda2.linear -= self.im2 * lin_impulse2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); /* * Motor. @@ -313,24 +313,41 @@ impl RevoluteVelocityGroundConstraint { ) -> AnyJointVelocityConstraint { let anchor2; let anchor1; + let axis1; + let axis2; + let basis1; let basis2; if flipped { + axis1 = rb1.position * *joint.local_axis2; + axis2 = rb2.position * *joint.local_axis1; anchor1 = rb1.position * joint.local_anchor2; anchor2 = rb2.position * joint.local_anchor1; + basis1 = Matrix3x2::from_columns(&[ + rb1.position * joint.basis2[0], + rb1.position * joint.basis2[1], + ]); basis2 = Matrix3x2::from_columns(&[ rb2.position * joint.basis1[0], rb2.position * joint.basis1[1], ]); } else { + axis1 = rb1.position * *joint.local_axis1; + axis2 = rb2.position * *joint.local_axis2; anchor1 = rb1.position * joint.local_anchor1; anchor2 = rb2.position * joint.local_anchor2; + basis1 = Matrix3x2::from_columns(&[ + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], + ]); basis2 = Matrix3x2::from_columns(&[ rb2.position * joint.basis2[0], rb2.position * joint.basis2[1], ]); }; + let basis_projection2 = basis2 * basis2.transpose(); + let basis2 = basis_projection2 * basis1; let im2 = rb2.effective_inv_mass; let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; @@ -351,15 +368,13 @@ impl RevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_rhs = basis2.tr_mul(&(rb2.angvel - rb1.angvel)); + let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); + let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); /* * Motor part. */ - let motor_axis1 = rb1.position * *joint.local_axis1; - let motor_axis2 = rb2.position * *joint.local_axis2; let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; let mut motor_max_impulse = joint.motor_max_impulse; @@ -377,13 +392,13 @@ impl RevoluteVelocityGroundConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1); + let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1); motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2))) * gamma + crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2))) * gamma } else { gamma }; @@ -405,7 +420,7 @@ impl RevoluteVelocityGroundConstraint { r2, motor_inv_lhs, motor_impulse, - motor_axis2, + motor_axis2: axis2, motor_max_impulse, motor_rhs, motor_angle, @@ -441,7 +456,6 @@ impl RevoluteVelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let ang_vel2 = ang_vel2 - self.motor_axis2 * ang_vel2.dot(&self.motor_axis2); let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); let ang_dvel = self.basis2.tr_mul(&ang_vel2); diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 5be1def..d229457 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -17,8 +17,8 @@ pub(crate) struct WRevoluteVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - r1_mat: Matrix3, - r2_mat: Matrix3, + r1: Vector, + r2: Vector, inv_lhs: Matrix5, rhs: Vector5, @@ -79,26 +79,18 @@ impl WRevoluteVelocityConstraint { let anchor2 = position2 * local_anchor2; let basis1 = Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]); - let basis_projection1 = basis1 * basis1.transpose(); - let basis_projection_half2 = + let basis2 = Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]); - let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose(); + let basis_projection2 = basis2 * basis2.transpose(); let basis2 = basis_projection2 * basis1; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: to simplify, we use basis2 = basis1. - // Though we may want to test if that does not introduce any instability. let ii1 = ii1_sqrt.squared(); let r1 = anchor1 - world_com1; - let r1_mat = basis_projection1 * r1.gcross_matrix(); + let r1_mat = r1.gcross_matrix(); let ii2 = ii2_sqrt.squared(); let r2 = anchor2 - world_com2; - let r2_mat = basis_projection2 * r2.gcross_matrix(); + let r2_mat = r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); let lhs00 = @@ -154,8 +146,8 @@ impl WRevoluteVelocityConstraint { impulse, inv_lhs, rhs, - r1_mat, - r2_mat, + r1, + r2, } } @@ -185,12 +177,12 @@ impl WRevoluteVelocityConstraint { mj_lambda1.linear += lin_impulse1 * self.im1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); mj_lambda2.linear -= lin_impulse2 * self.im2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -223,8 +215,8 @@ impl WRevoluteVelocityConstraint { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); - let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2) - - (mj_lambda1.linear - self.r1_mat * ang_vel1); + let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2)) + - (mj_lambda1.linear + ang_vel1.gcross(self.r1)); let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1); let rhs = Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; @@ -238,12 +230,12 @@ impl WRevoluteVelocityConstraint { mj_lambda1.linear += lin_impulse1 * self.im1; mj_lambda1.angular += self .ii1_sqrt - .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1); + .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1)); mj_lambda2.linear -= lin_impulse2 * self.im2; mj_lambda2.angular -= self .ii2_sqrt - .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2); + .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -320,6 +312,16 @@ impl WRevoluteVelocityGroundConstraint { let local_anchor2 = Point::from( array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH], ); + let basis1 = Matrix3x2::from_columns(&[ + position1 + * Vector::from( + array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH], + ), + position1 + * Vector::from( + array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH], + ), + ]); let basis2 = Matrix3x2::from_columns(&[ position2 * Vector::from( @@ -330,15 +332,12 @@ impl WRevoluteVelocityGroundConstraint { array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH], ), ]); + let basis_projection2 = basis2 * basis2.transpose(); + let basis2 = basis_projection2 * basis1; let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = /*r21 * */ basis1; let ii2 = ii2_sqrt.squared(); let r1 = anchor1 - world_com1; let r2 = anchor2 - world_com2; @@ -358,8 +357,8 @@ impl WRevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); - let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); - let ang_rhs = basis2.tr_mul(&(angvel2 - angvel1)); + let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1)); + let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1); let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); WRevoluteVelocityGroundConstraint { From 01496d43e5a39a7348578faa4e6c2fcf1793785b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Sun, 21 Feb 2021 17:15:00 +0100 Subject: [PATCH 11/20] Add motors to ball joints. --- src/dynamics/joint/ball_joint.rs | 92 +++++++- src/dynamics/joint/joint.rs | 3 +- .../ball_velocity_constraint.rs | 212 ++++++++++++++++-- 3 files changed, 288 insertions(+), 19 deletions(-) diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index f1e8fdf..47c90ab 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -1,4 +1,5 @@ -use crate::math::{Point, Real, Vector}; +use crate::dynamics::SpringModel; +use crate::math::{Point, Real, Rotation, Vector}; #[derive(Copy, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -12,6 +13,33 @@ pub struct BallJoint { /// /// The impulse applied to the second body is given by `-impulse`. pub impulse: Vector, + + /// The target relative angular velocity the motor will attempt to reach. + #[cfg(feature = "dim2")] + pub motor_target_vel: Real, + /// The target relative angular velocity the motor will attempt to reach. + #[cfg(feature = "dim3")] + pub motor_target_vel: Vector, + /// The target angular position of this joint, expressed as an axis-angle. + pub motor_target_pos: Rotation, + /// The motor's stiffness. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_stiffness: Real, + /// The motor's damping. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_damping: Real, + /// The maximal impulse the motor is able to deliver. + pub motor_max_impulse: Real, + /// The angular impulse applied by the motor. + #[cfg(feature = "dim2")] + pub motor_impulse: Real, + /// The angular impulse applied by the motor. + #[cfg(feature = "dim3")] + pub motor_impulse: Vector, + /// The spring-like model used by the motor to reach the target velocity and . + pub motor_model: SpringModel, + // Used to handle cases where the position target ends up being more than pi radians away. + pub(crate) motor_last_angle: Real, } impl BallJoint { @@ -29,11 +57,71 @@ impl BallJoint { local_anchor1, local_anchor2, impulse, + motor_target_vel: na::zero(), + motor_target_pos: Rotation::identity(), + motor_stiffness: 0.0, + motor_damping: 0.0, + motor_impulse: na::zero(), + motor_max_impulse: Real::MAX, + motor_model: SpringModel::default(), + motor_last_angle: 0.0, } } /// Can a SIMD constraint be used for resolving this joint? pub fn supports_simd_constraints(&self) -> bool { - true + // SIMD ball constraints don't support motors right now. + self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) + } + + pub fn configure_motor_model(&mut self, model: SpringModel) { + self.motor_model = model; + } + + #[cfg(feature = "dim2")] + pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + #[cfg(feature = "dim3")] + pub fn configure_motor_velocity(&mut self, target_vel: Vector, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + pub fn configure_motor_position( + &mut self, + target_pos: Rotation, + stiffness: Real, + damping: Real, + ) { + self.configure_motor(target_pos, na::zero(), stiffness, damping) + } + + #[cfg(feature = "dim2")] + pub fn configure_motor( + &mut self, + target_pos: Rotation, + target_vel: Real, + stiffness: Real, + damping: Real, + ) { + self.motor_target_vel = target_vel; + self.motor_target_pos = target_pos; + self.motor_stiffness = stiffness; + self.motor_damping = damping; + } + + #[cfg(feature = "dim3")] + pub fn configure_motor( + &mut self, + target_pos: Rotation, + target_vel: Vector, + stiffness: Real, + damping: Real, + ) { + self.motor_target_vel = target_vel; + self.motor_target_pos = target_pos; + self.motor_stiffness = stiffness; + self.motor_damping = damping; } } diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index b4089ae..290c34c 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -132,10 +132,11 @@ pub struct Joint { impl Joint { pub fn supports_simd_constraints(&self) -> bool { match &self.params { - JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(), JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(), JointParams::FixedJoint(joint) => joint.supports_simd_constraints(), JointParams::BallJoint(joint) => joint.supports_simd_constraints(), + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(), } } } diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index e75f978..8c19ee6 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{AngularInertia, Real, SdpMatrix, Vector}; +use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -13,13 +13,17 @@ pub(crate) struct BallVelocityConstraint { joint_id: JointIndex, rhs: Vector, - pub(crate) impulse: Vector, + impulse: Vector, r1: Vector, r2: Vector, inv_lhs: SdpMatrix, + motor_rhs: AngVector, + motor_impulse: AngVector, + motor_inv_lhs: Option>, + im1: Real, im2: Real, @@ -33,10 +37,10 @@ impl BallVelocityConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &BallJoint, + joint: &BallJoint, ) -> Self { - let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com; - let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com; + let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com; + let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); @@ -77,17 +81,86 @@ impl BallVelocityConstraint { let inv_lhs = lhs.inverse_unchecked(); + /* + * Motor part. + */ + let mut motor_rhs = na::zero(); + let mut motor_inv_lhs = None; + let motor_max_impulse = joint.motor_max_impulse; + + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + let dpos = + rb2.position.rotation * (rb1.position.rotation * joint.motor_target_pos).inverse(); + #[cfg(feature = "dim2")] + { + motor_rhs += dpos.angle() * stiffness; + } + #[cfg(feature = "dim3")] + { + motor_rhs += dpos.scaled_axis() * stiffness; + } + } + + if damping != 0.0 { + let curr_vel = rb2.angvel - rb1.angvel; + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + #[cfg(feature = "dim2")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(gamma / (ii1 + ii2)) + } else { + Some(gamma) + }; + motor_rhs /= gamma; + } + + #[cfg(feature = "dim3")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some((ii1 + ii2).inverse_unchecked() * gamma) + } else { + Some(SdpMatrix::diagonal(gamma)) + }; + motor_rhs /= gamma; + } + + #[cfg(feature = "dim2")] + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) + * params.warmstart_coeff; + + #[cfg(feature = "dim3")] + let motor_impulse = joint + .motor_impulse + .try_clamp_magnitude(-motor_max_impulse, motor_max_impulse, 1.0e-6) + .unwrap_or_else(na::zero) + * params.warmstart_coeff; + BallVelocityConstraint { joint_id, mj_lambda1: rb1.active_set_offset, mj_lambda2: rb2.active_set_offset, im1, im2, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, r1: anchor1, r2: anchor2, rhs, inv_lhs, + motor_rhs, + motor_impulse, + motor_inv_lhs, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } @@ -98,9 +171,13 @@ impl BallVelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda1.linear += self.im1 * self.impulse; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse)); + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse); mj_lambda2.linear -= self.im2 * self.impulse; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse)); + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -125,6 +202,21 @@ impl BallVelocityConstraint { mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); + /* + * Motor part. + */ + if let Some(motor_inv_lhs) = &self.motor_inv_lhs { + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs; + let impulse = motor_inv_lhs.transform_vector(dangvel); + self.motor_impulse += impulse; + + mj_lambda1.angular += self.ii1_sqrt.transform_vector(impulse); + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(impulse); + } + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -141,10 +233,16 @@ impl BallVelocityConstraint { pub(crate) struct BallVelocityGroundConstraint { mj_lambda2: usize, joint_id: JointIndex, + r2: Vector, + rhs: Vector, impulse: Vector, - r2: Vector, inv_lhs: SdpMatrix, + + motor_rhs: AngVector, + motor_impulse: AngVector, + motor_inv_lhs: Option>, + im2: Real, ii2_sqrt: AngularInertia, } @@ -155,18 +253,18 @@ impl BallVelocityGroundConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &BallJoint, + joint: &BallJoint, flipped: bool, ) -> Self { let (anchor1, anchor2) = if flipped { ( - rb1.position * cparams.local_anchor2 - rb1.world_com, - rb2.position * cparams.local_anchor1 - rb2.world_com, + rb1.position * joint.local_anchor2 - rb1.world_com, + rb2.position * joint.local_anchor1 - rb2.world_com, ) } else { ( - rb1.position * cparams.local_anchor1 - rb1.world_com, - rb2.position * cparams.local_anchor2 - rb2.world_com, + rb1.position * joint.local_anchor1 - rb1.world_com, + rb2.position * joint.local_anchor2 - rb2.world_com, ) }; @@ -199,14 +297,81 @@ impl BallVelocityGroundConstraint { let inv_lhs = lhs.inverse_unchecked(); + /* + * Motor part. + */ + let mut motor_rhs = na::zero(); + let mut motor_inv_lhs = None; + let motor_max_impulse = joint.motor_max_impulse; + + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + let dpos = + rb2.position.rotation * (rb1.position.rotation * joint.motor_target_pos).inverse(); + #[cfg(feature = "dim2")] + { + motor_rhs += dpos.angle() * stiffness; + } + #[cfg(feature = "dim3")] + { + motor_rhs += dpos.scaled_axis() * stiffness; + } + } + + if damping != 0.0 { + let curr_vel = rb2.angvel - rb1.angvel; + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + #[cfg(feature = "dim2")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(gamma / ii2) + } else { + Some(gamma) + }; + motor_rhs /= gamma; + } + + #[cfg(feature = "dim3")] + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(ii2.inverse_unchecked() * gamma) + } else { + Some(SdpMatrix::diagonal(gamma)) + }; + motor_rhs /= gamma; + } + + #[cfg(feature = "dim2")] + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) + * params.warmstart_coeff; + + #[cfg(feature = "dim3")] + let motor_impulse = joint + .motor_impulse + .try_clamp_magnitude(-motor_max_impulse, motor_max_impulse, 1.0e-6) + .unwrap_or_else(na::zero) + * params.warmstart_coeff; + BallVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, r2: anchor2, rhs, inv_lhs, + motor_rhs, + motor_impulse, + motor_inv_lhs, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } @@ -214,7 +379,9 @@ impl BallVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; mj_lambda2.linear -= self.im2 * self.impulse; - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse)); + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -231,6 +398,19 @@ impl BallVelocityGroundConstraint { mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); + /* + * Motor part. + */ + if let Some(motor_inv_lhs) = &self.motor_inv_lhs { + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dangvel = ang_vel2 + self.motor_rhs; + let impulse = motor_inv_lhs.transform_vector(dangvel); + self.motor_impulse += impulse; + + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(impulse); + } + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } From cc8dc13fc03eb6862b7de2fb52132ef89b8731c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Sun, 21 Feb 2021 17:23:24 +0100 Subject: [PATCH 12/20] Add actuated ball and revolute joint to the 3D joint demo. --- examples3d/joints3.rs | 153 ++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 146 insertions(+), 7 deletions(-) diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index d777655..ec83c9f 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -1,7 +1,7 @@ -use na::{Isometry3, Point3, Unit, Vector3}; +use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3}; use rapier3d::dynamics::{ BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, - RigidBodySet, + RigidBodyHandle, RigidBodySet, }; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -14,7 +14,7 @@ fn create_prismatic_joints( num: usize, ) { let rad = 0.4; - let shift = 1.0; + let shift = 2.0; let ground = RigidBodyBuilder::new_static() .translation(origin.x, origin.y, origin.z) @@ -40,7 +40,7 @@ fn create_prismatic_joints( let z = Vector3::z(); let mut prism = PrismaticJoint::new( - Point3::origin(), + Point3::new(0.0, 0.0, 0.0), axis, z, Point3::new(0.0, 0.0, -shift), @@ -50,6 +50,7 @@ fn create_prismatic_joints( prism.limits_enabled = true; prism.limits[0] = -2.0; prism.limits[1] = 2.0; + joints.insert(bodies, curr_parent, curr_child, prism); curr_parent = curr_child; @@ -222,6 +223,130 @@ fn create_ball_joints( } } +fn create_actuated_revolute_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + // We will reuse this base configuration for all the joints here. + let joint_template = RevoluteJoint::new( + Point3::origin(), + Vector3::z_axis(), + Point3::new(0.0, 0.0, -shift), + Vector3::z_axis(), + ); + + let mut parent_handle = RigidBodyHandle::invalid(); + + for i in 0..num { + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let shifty = (i >= 1) as u32 as f32 * -2.0; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(origin.x, origin.y + shifty, origin.z + fi * shift) + // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) + .build(); + + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build(); + colliders.insert(collider, child_handle, bodies); + + if i > 0 { + let mut joint = joint_template.clone(); + + if i % 3 == 1 { + joint.configure_motor_velocity(-20.0, 0.1); + } else if i == num - 1 { + let stiffness = 0.2; + let damping = 1.0; + joint.configure_motor_position(3.14 / 2.0, stiffness, damping); + } + + if i == 1 { + joint.local_anchor2.y = 2.0; + joint.configure_motor_velocity(-2.0, 0.1); + } + + joints.insert(bodies, parent_handle, child_handle, joint); + } + + parent_handle = child_handle; + } +} + +fn create_actuated_ball_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + // We will reuse this base configuration for all the joints here. + let joint_template = BallJoint::new(Point3::new(0.0, 0.0, shift), Point3::origin()); + + let mut parent_handle = RigidBodyHandle::invalid(); + + for i in 0..num { + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(origin.x, origin.y, origin.z + fi * shift) + // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) + .build(); + + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build(); + colliders.insert(collider, child_handle, bodies); + + if i > 0 { + let mut joint = joint_template.clone(); + + if i == 1 { + joint.configure_motor_velocity(Vector3::new(0.0, 0.5, -2.0), 0.1); + } else if i == num - 1 { + let stiffness = 0.2; + let damping = 1.0; + joint.configure_motor_position( + UnitQuaternion::new(Vector3::new(0.0, 1.0, 3.14 / 2.0)), + stiffness, + damping, + ); + } + + joints.insert(bodies, parent_handle, child_handle, joint); + } + + parent_handle = child_handle; + } +} + pub fn init_world(testbed: &mut Testbed) { /* * World @@ -234,8 +359,8 @@ pub fn init_world(testbed: &mut Testbed) { &mut bodies, &mut colliders, &mut joints, - Point3::new(20.0, 10.0, 0.0), - 5, + Point3::new(20.0, 5.0, 0.0), + 4, ); create_revolute_joints( &mut bodies, @@ -249,7 +374,21 @@ pub fn init_world(testbed: &mut Testbed) { &mut colliders, &mut joints, Point3::new(0.0, 10.0, 0.0), - 5, + 10, + ); + create_actuated_revolute_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(20.0, 10.0, 0.0), + 6, + ); + create_actuated_ball_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(13.0, 10.0, 0.0), + 3, ); create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15); From f204a5f7361fea92ff0f795246a667afb738d56d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 10:15:13 +0100 Subject: [PATCH 13/20] Take max motor impulse into account for the ball joint. --- .../ball_velocity_constraint.rs | 213 ++++++++++-------- 1 file changed, 114 insertions(+), 99 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 8c19ee6..88e7c83 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -23,6 +23,7 @@ pub(crate) struct BallVelocityConstraint { motor_rhs: AngVector, motor_impulse: AngVector, motor_inv_lhs: Option>, + motor_max_impulse: Real, im1: Real, im2: Real, @@ -88,64 +89,62 @@ impl BallVelocityConstraint { let mut motor_inv_lhs = None; let motor_max_impulse = joint.motor_max_impulse; - let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( - params.dt, - joint.motor_stiffness, - joint.motor_damping, - ); + if motor_max_impulse > 0.0 { + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + let dpos = rb2.position.rotation + * (rb1.position.rotation * joint.motor_target_pos).inverse(); + #[cfg(feature = "dim2")] + { + motor_rhs += dpos.angle() * stiffness; + } + #[cfg(feature = "dim3")] + { + motor_rhs += dpos.scaled_axis() * stiffness; + } + } + + if damping != 0.0 { + let curr_vel = rb2.angvel - rb1.angvel; + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } - if stiffness != 0.0 { - let dpos = - rb2.position.rotation * (rb1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] - { - motor_rhs += dpos.angle() * stiffness; + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(gamma / (ii1 + ii2)) + } else { + Some(gamma) + }; + motor_rhs /= gamma; } + #[cfg(feature = "dim3")] - { - motor_rhs += dpos.scaled_axis() * stiffness; + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some((ii1 + ii2).inverse_unchecked() * gamma) + } else { + Some(SdpMatrix::diagonal(gamma)) + }; + motor_rhs /= gamma; } } - if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; - motor_rhs += (curr_vel - joint.motor_target_vel) * damping; - } - - #[cfg(feature = "dim2")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - Some(gamma / (ii1 + ii2)) - } else { - Some(gamma) - }; - motor_rhs /= gamma; - } - - #[cfg(feature = "dim3")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - Some((ii1 + ii2).inverse_unchecked() * gamma) - } else { - Some(SdpMatrix::diagonal(gamma)) - }; - motor_rhs /= gamma; - } - #[cfg(feature = "dim2")] let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) * params.warmstart_coeff; - #[cfg(feature = "dim3")] - let motor_impulse = joint - .motor_impulse - .try_clamp_magnitude(-motor_max_impulse, motor_max_impulse, 1.0e-6) - .unwrap_or_else(na::zero) - * params.warmstart_coeff; + let motor_impulse = + joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; BallVelocityConstraint { joint_id, @@ -161,6 +160,7 @@ impl BallVelocityConstraint { motor_rhs, motor_impulse, motor_inv_lhs, + motor_max_impulse: joint.motor_max_impulse, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } @@ -210,11 +210,19 @@ impl BallVelocityConstraint { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs; - let impulse = motor_inv_lhs.transform_vector(dangvel); - self.motor_impulse += impulse; - mj_lambda1.angular += self.ii1_sqrt.transform_vector(impulse); - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(impulse); + let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); + + #[cfg(feature = "dim2")] + let clamped_impulse = na::clamp(new_impulse, -motor_max_impulse, motor_max_impulse); + #[cfg(feature = "dim3")] + let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); + + let effective_impulse = clamped_impulse - self.motor_impulse; + self.motor_impulse = clamped_impulse; + + mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse); + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); } mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; @@ -242,6 +250,7 @@ pub(crate) struct BallVelocityGroundConstraint { motor_rhs: AngVector, motor_impulse: AngVector, motor_inv_lhs: Option>, + motor_max_impulse: Real, im2: Real, ii2_sqrt: AngularInertia, @@ -304,62 +313,60 @@ impl BallVelocityGroundConstraint { let mut motor_inv_lhs = None; let motor_max_impulse = joint.motor_max_impulse; - let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( - params.dt, - joint.motor_stiffness, - joint.motor_damping, - ); + if motor_max_impulse > 0.0 { + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + let dpos = rb2.position.rotation + * (rb1.position.rotation * joint.motor_target_pos).inverse(); + #[cfg(feature = "dim2")] + { + motor_rhs += dpos.angle() * stiffness; + } + #[cfg(feature = "dim3")] + { + motor_rhs += dpos.scaled_axis() * stiffness; + } + } + + if damping != 0.0 { + let curr_vel = rb2.angvel - rb1.angvel; + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } - if stiffness != 0.0 { - let dpos = - rb2.position.rotation * (rb1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] - { - motor_rhs += dpos.angle() * stiffness; + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(gamma / ii2) + } else { + Some(gamma) + }; + motor_rhs /= gamma; } + #[cfg(feature = "dim3")] - { - motor_rhs += dpos.scaled_axis() * stiffness; + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + Some(ii2.inverse_unchecked() * gamma) + } else { + Some(SdpMatrix::diagonal(gamma)) + }; + motor_rhs /= gamma; } } - if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; - motor_rhs += (curr_vel - joint.motor_target_vel) * damping; - } - - #[cfg(feature = "dim2")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - Some(gamma / ii2) - } else { - Some(gamma) - }; - motor_rhs /= gamma; - } - - #[cfg(feature = "dim3")] - if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - Some(ii2.inverse_unchecked() * gamma) - } else { - Some(SdpMatrix::diagonal(gamma)) - }; - motor_rhs /= gamma; - } - #[cfg(feature = "dim2")] let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse) * params.warmstart_coeff; - #[cfg(feature = "dim3")] - let motor_impulse = joint - .motor_impulse - .try_clamp_magnitude(-motor_max_impulse, motor_max_impulse, 1.0e-6) - .unwrap_or_else(na::zero) - * params.warmstart_coeff; + let motor_impulse = + joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; BallVelocityGroundConstraint { joint_id, @@ -372,6 +379,7 @@ impl BallVelocityGroundConstraint { motor_rhs, motor_impulse, motor_inv_lhs, + motor_max_impulse: joint.motor_max_impulse, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } @@ -405,10 +413,17 @@ impl BallVelocityGroundConstraint { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let dangvel = ang_vel2 + self.motor_rhs; - let impulse = motor_inv_lhs.transform_vector(dangvel); - self.motor_impulse += impulse; + let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); - mj_lambda2.angular -= self.ii2_sqrt.transform_vector(impulse); + #[cfg(feature = "dim2")] + let clamped_impulse = na::clamp(new_impulse, -motor_max_impulse, motor_max_impulse); + #[cfg(feature = "dim3")] + let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); + + let effective_impulse = clamped_impulse - self.motor_impulse; + self.motor_impulse = clamped_impulse; + + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); } mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; From aaba6c8927b0cdd0de7739b37f2bdfe267074dbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 12:12:24 +0100 Subject: [PATCH 14/20] Add motors to prismatic joints. --- src/dynamics/joint/prismatic_joint.rs | 72 +++++- src/dynamics/joint/revolute_joint.rs | 2 + .../prismatic_velocity_constraint.rs | 240 +++++++++++++----- 3 files changed, 240 insertions(+), 74 deletions(-) diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 6e32a01..4f5c984 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::SpringModel; use crate::math::{Isometry, Point, Real, Vector, DIM}; use crate::utils::WBasis; use na::Unit; @@ -36,10 +37,23 @@ pub struct PrismaticJoint { /// /// The impulse applied to the second body is given by `-impulse`. pub limits_impulse: Real, - // pub motor_enabled: bool, - // pub target_motor_vel: Real, - // pub max_motor_impulse: Real, - // pub motor_impulse: Real, + + /// The target relative angular velocity the motor will attempt to reach. + pub motor_target_vel: Real, + /// The target relative angle along the joint axis the motor will attempt to reach. + pub motor_target_pos: Real, + /// The motor's stiffness. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_stiffness: Real, + /// The motor's damping. + /// See the documentation of `SpringModel` for more information on this parameter. + pub motor_damping: Real, + /// The maximal impulse the motor is able to deliver. + pub motor_max_impulse: Real, + /// The angular impulse applied by the motor. + pub motor_impulse: Real, + /// The spring-like model used by the motor to reach the target velocity and . + pub motor_model: SpringModel, } impl PrismaticJoint { @@ -63,10 +77,13 @@ impl PrismaticJoint { limits_enabled: false, limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, - // motor_enabled: false, - // target_motor_vel: 0.0, - // max_motor_impulse: Real::MAX, - // motor_impulse: 0.0, + motor_target_vel: 0.0, + motor_target_pos: 0.0, + motor_stiffness: 0.0, + motor_damping: 0.0, + motor_max_impulse: Real::MAX, + motor_impulse: 0.0, + motor_model: SpringModel::VelocityBased, } } @@ -118,10 +135,13 @@ impl PrismaticJoint { limits_enabled: false, limits: [-Real::MAX, Real::MAX], limits_impulse: 0.0, - // motor_enabled: false, - // target_motor_vel: 0.0, - // max_motor_impulse: Real::MAX, - // motor_impulse: 0.0, + motor_target_vel: 0.0, + motor_target_pos: 0.0, + motor_stiffness: 0.0, + motor_damping: 0.0, + motor_max_impulse: Real::MAX, + motor_impulse: 0.0, + motor_model: SpringModel::VelocityBased, } } @@ -137,7 +157,8 @@ impl PrismaticJoint { /// Can a SIMD constraint be used for resolving this joint? pub fn supports_simd_constraints(&self) -> bool { - true + // SIMD revolute constraints don't support motors right now. + self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) } // FIXME: precompute this? @@ -195,4 +216,29 @@ impl PrismaticJoint { let translation = self.local_anchor2.coords.into(); Isometry::from_parts(translation, rotation) } + + pub fn configure_motor_model(&mut self, model: SpringModel) { + self.motor_model = model; + } + + pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { + self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) + } + + pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) { + self.configure_motor(target_pos, 0.0, stiffness, damping) + } + + pub fn configure_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) { + self.motor_target_vel = target_vel; + self.motor_target_pos = target_pos; + self.motor_stiffness = stiffness; + self.motor_damping = damping; + } } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index e1e1441..6895d3d 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -23,6 +23,7 @@ pub struct RevoluteJoint { /// /// The impulse applied to the second body is given by `-impulse`. pub impulse: Vector5, + /// The target relative angular velocity the motor will attempt to reach. pub motor_target_vel: Real, /// The target relative angle along the joint axis the motor will attempt to reach. @@ -39,6 +40,7 @@ pub struct RevoluteJoint { pub motor_impulse: Real, /// The spring-like model used by the motor to reach the target velocity and . pub motor_model: SpringModel, + // Used to handle cases where the position target ends up being more than pi radians away. pub(crate) motor_last_angle: Real, // The angular impulse expressed in world-space. diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index a361a37..9aaf172 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -41,6 +41,13 @@ pub(crate) struct PrismaticVelocityConstraint { #[cfg(feature = "dim2")] impulse: Vector2, + motor_axis1: Vector, + motor_axis2: Vector, + motor_impulse: Real, + motor_rhs: Real, + motor_inv_lhs: Real, + motor_max_impulse: Real, + limits_impulse: Real, limits_forcedirs: Option<(Vector, Vector)>, limits_rhs: Real, @@ -63,35 +70,21 @@ impl PrismaticVelocityConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &PrismaticJoint, + joint: &PrismaticJoint, ) -> Self { // Linear part. - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; - let axis1 = rb1.position * cparams.local_axis1; - let axis2 = rb2.position * cparams.local_axis2; + let anchor1 = rb1.position * joint.local_anchor1; + let anchor2 = rb2.position * joint.local_anchor2; + let axis1 = rb1.position * joint.local_axis1; + let axis2 = rb2.position * joint.local_axis2; #[cfg(feature = "dim2")] - let basis1 = rb1.position * cparams.basis1[0]; + let basis1 = rb1.position * joint.basis1[0]; #[cfg(feature = "dim3")] let basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], ]); - // #[cfg(feature = "dim2")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .to_rotation_matrix() - // .into_inner(); - // #[cfg(feature = "dim3")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: we use basis2 := basis1 for now is that allows - // simplifications of the computation without introducing - // much instabilities. - let im1 = rb1.effective_inv_mass; let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; @@ -149,25 +142,57 @@ impl PrismaticVelocityConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - // Setup limit constraint. + /* + * Setup motor. + */ + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 0.0; + let mut motor_max_impulse = joint.motor_max_impulse; + + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1); + motor_rhs += (dist - joint.motor_target_pos) * stiffness; + } + + if damping != 0.0 { + let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1); + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { gamma / (im1 + im2) } else { gamma }; + motor_rhs /= gamma; + } + + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse); + + /* + * Setup limit constraint. + */ let mut limits_forcedirs = None; let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; - if cparams.limits_enabled { + if joint.limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // FIXME: we should allow both limits to be active at + // TODO: we should allow both limits to be active at // the same time, and allow predictive constraint activation. - if dist < cparams.limits[0] { + if dist < joint.limits[0] { limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner())); limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; - } else if dist > cparams.limits[1] { + limits_impulse = joint.limits_impulse; + } else if dist > joint.limits[1] { limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner())); limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; + limits_impulse = joint.limits_impulse; } } @@ -179,10 +204,16 @@ impl PrismaticVelocityConstraint { ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, limits_forcedirs, limits_rhs, + motor_rhs, + motor_inv_lhs, + motor_impulse, + motor_axis1: *axis1, + motor_axis2: *axis2, + motor_max_impulse, basis1, inv_lhs, rhs, @@ -211,6 +242,11 @@ impl PrismaticVelocityConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + // Warmstart motors. + mj_lambda1.linear += self.motor_axis1 * (self.im1 * self.motor_impulse); + mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse); + + // Warmstart limits. if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); @@ -225,7 +261,7 @@ impl PrismaticVelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; /* - * Joint consraint. + * Joint constraint. */ let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -256,12 +292,29 @@ impl PrismaticVelocityConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + /* + * Motors. + */ + if self.motor_inv_lhs != 0.0 { + let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear) + - self.motor_axis1.dot(&mj_lambda1.linear) + + self.motor_rhs; + let new_impulse = na::clamp( + self.motor_impulse + lin_dvel * self.motor_inv_lhs, + -self.motor_max_impulse, + self.motor_max_impulse, + ); + let dimpulse = new_impulse - self.motor_impulse; + self.motor_impulse = new_impulse; + + mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse); + mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse); + } + /* * Joint limits. */ if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - // FIXME: the transformation by ii2_sqrt could be avoided by - // reusing some computations above. let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -284,6 +337,7 @@ impl PrismaticVelocityConstraint { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::PrismaticJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + revolute.motor_impulse = self.motor_impulse; revolute.limits_impulse = self.limits_impulse; } } @@ -315,6 +369,11 @@ pub(crate) struct PrismaticVelocityGroundConstraint { limits_rhs: Real, axis2: Vector, + motor_impulse: Real, + motor_rhs: Real, + motor_inv_lhs: Real, + motor_max_impulse: Real, + #[cfg(feature = "dim2")] basis1: Vector2, #[cfg(feature = "dim3")] @@ -331,7 +390,7 @@ impl PrismaticVelocityGroundConstraint { joint_id: JointIndex, rb1: &RigidBody, rb2: &RigidBody, - cparams: &PrismaticJoint, + joint: &PrismaticJoint, flipped: bool, ) -> Self { let anchor2; @@ -341,35 +400,35 @@ impl PrismaticVelocityGroundConstraint { let basis1; if flipped { - anchor2 = rb2.position * cparams.local_anchor1; - anchor1 = rb1.position * cparams.local_anchor2; - axis2 = rb2.position * cparams.local_axis1; - axis1 = rb1.position * cparams.local_axis2; + anchor2 = rb2.position * joint.local_anchor1; + anchor1 = rb1.position * joint.local_anchor2; + axis2 = rb2.position * joint.local_axis1; + axis1 = rb1.position * joint.local_axis2; #[cfg(feature = "dim2")] { - basis1 = rb1.position * cparams.basis2[0]; + basis1 = rb1.position * joint.basis2[0]; } #[cfg(feature = "dim3")] { basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis2[0], - rb1.position * cparams.basis2[1], + rb1.position * joint.basis2[0], + rb1.position * joint.basis2[1], ]); } } else { - anchor2 = rb2.position * cparams.local_anchor2; - anchor1 = rb1.position * cparams.local_anchor1; - axis2 = rb2.position * cparams.local_axis2; - axis1 = rb1.position * cparams.local_axis1; + anchor2 = rb2.position * joint.local_anchor2; + anchor1 = rb1.position * joint.local_anchor1; + axis2 = rb2.position * joint.local_axis2; + axis1 = rb1.position * joint.local_axis1; #[cfg(feature = "dim2")] { - basis1 = rb1.position * cparams.basis1[0]; + basis1 = rb1.position * joint.basis1[0]; } #[cfg(feature = "dim3")] { basis1 = Matrix3x2::from_columns(&[ - rb1.position * cparams.basis1[0], - rb1.position * cparams.basis1[1], + rb1.position * joint.basis1[0], + rb1.position * joint.basis1[1], ]); } }; @@ -438,26 +497,62 @@ impl PrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - // Setup limit constraint. + /* + * Setup motor. + */ + + /* + * Setup motor. + */ + let mut motor_rhs = 0.0; + let mut motor_inv_lhs = 0.0; + let mut motor_max_impulse = joint.motor_max_impulse; + + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( + params.dt, + joint.motor_stiffness, + joint.motor_damping, + ); + + if stiffness != 0.0 { + let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1); + motor_rhs += (dist - joint.motor_target_pos) * stiffness; + } + + if damping != 0.0 { + let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1); + motor_rhs += (curr_vel - joint.motor_target_vel) * damping; + } + + if stiffness != 0.0 || damping != 0.0 { + motor_inv_lhs = if keep_lhs { gamma / im2 } else { gamma }; + motor_rhs /= gamma; + } + + let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse); + + /* + * Setup limit constraint. + */ let mut limits_forcedir2 = None; let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; - if cparams.limits_enabled { + if joint.limits_enabled { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); - // FIXME: we should allow both limits to be active at + // TODO: we should allow both limits to be active at // the same time. - // FIXME: allow predictive constraint activation. - if dist < cparams.limits[0] { + // TODO: allow predictive constraint activation. + if dist < joint.limits[0] { limits_forcedir2 = Some(axis2.into_inner()); limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; - } else if dist > cparams.limits[1] { + limits_impulse = joint.limits_impulse; + } else if dist > joint.limits[1] { limits_forcedir2 = Some(-axis2.into_inner()); limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); - limits_impulse = cparams.limits_impulse; + limits_impulse = joint.limits_impulse; } } @@ -466,8 +561,12 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2: rb2.active_set_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, - impulse: cparams.impulse * params.warmstart_coeff, + impulse: joint.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, + motor_rhs, + motor_inv_lhs, + motor_impulse, + motor_max_impulse, basis1, inv_lhs, rhs, @@ -492,6 +591,10 @@ impl PrismaticVelocityGroundConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + // Warmstart motors. + mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse); + + // Warmstart limits. if let Some(limits_forcedir2) = self.limits_forcedir2 { mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); } @@ -503,7 +606,7 @@ impl PrismaticVelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; /* - * Joint consraint. + * Joint constraint. */ let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); @@ -527,12 +630,26 @@ impl PrismaticVelocityGroundConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + /* + * Motors. + */ + if self.motor_inv_lhs != 0.0 { + let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs; + let new_impulse = na::clamp( + self.motor_impulse + lin_dvel * self.motor_inv_lhs, + -self.motor_max_impulse, + self.motor_max_impulse, + ); + let dimpulse = new_impulse - self.motor_impulse; + self.motor_impulse = new_impulse; + + mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse); + } + /* * Joint limits. */ if let Some(limits_forcedir2) = self.limits_forcedir2 { - // FIXME: the transformation by ii2_sqrt could be avoided by - // reusing some computations above. let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) @@ -547,11 +664,12 @@ impl PrismaticVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - // FIXME: duplicated code with the non-ground constraint. + // TODO: duplicated code with the non-ground constraint. pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::PrismaticJoint(revolute) = &mut joint.params { revolute.impulse = self.impulse; + revolute.motor_impulse = self.motor_impulse; revolute.limits_impulse = self.limits_impulse; } } From 73192d41c2e1dd32f1fead6e29e18c89fabe0835 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 13:22:15 +0100 Subject: [PATCH 15/20] Make prismatic joint limit transmit torque. --- .../prismatic_velocity_constraint.rs | 133 +++++++++++------- 1 file changed, 79 insertions(+), 54 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 9aaf172..227338a 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -51,6 +51,7 @@ pub(crate) struct PrismaticVelocityConstraint { limits_impulse: Real, limits_forcedirs: Option<(Vector, Vector)>, limits_rhs: Real, + limits_inv_lhs: Real, #[cfg(feature = "dim2")] basis1: Vector2, @@ -178,6 +179,7 @@ impl PrismaticVelocityConstraint { let mut limits_forcedirs = None; let mut limits_rhs = 0.0; let mut limits_impulse = 0.0; + let mut limits_inv_lhs = 0.0; if joint.limits_enabled { let danchor = anchor2 - anchor1; @@ -194,6 +196,16 @@ impl PrismaticVelocityConstraint { limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); limits_impulse = joint.limits_impulse; } + + if dist < joint.limits[0] || dist > joint.limits[1] { + let gcross1 = r1.gcross(*axis1); + let gcross2 = r2.gcross(*axis2); + limits_inv_lhs = crate::utils::inv( + im1 + im2 + + gcross1.dot(&ii1.transform_vector(gcross1)) + + gcross2.dot(&ii2.transform_vector(gcross2)), + ); + } } PrismaticVelocityConstraint { @@ -208,6 +220,7 @@ impl PrismaticVelocityConstraint { limits_impulse: limits_impulse * params.warmstart_coeff, limits_forcedirs, limits_rhs, + limits_inv_lhs, motor_rhs, motor_inv_lhs, motor_impulse, @@ -248,21 +261,24 @@ impl PrismaticVelocityConstraint { // Warmstart limits. if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + let limit_impulse1 = limits_forcedir1 * self.limits_impulse; + let limit_impulse2 = limits_forcedir2 * self.limits_impulse; + + mj_lambda1.linear += self.im1 * limit_impulse1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.r1.gcross(limit_impulse1)); + mj_lambda2.linear += self.im2 * limit_impulse2; + mj_lambda2.angular += self + .ii2_sqrt + .transform_vector(self.r2.gcross(limit_impulse2)); } mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - /* - * Joint constraint. - */ + fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); @@ -291,10 +307,31 @@ impl PrismaticVelocityConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Motors. - */ + fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { + if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + + self.limits_rhs; + let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs).max(0.0); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + let lin_impulse1 = limits_forcedir1 * dimpulse; + let lin_impulse2 = limits_forcedir2 * dimpulse; + + mj_lambda1.linear += self.im1 * lin_impulse1; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1)); + mj_lambda2.linear += self.im2 * lin_impulse2; + mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); + } + } + + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { if self.motor_inv_lhs != 0.0 { let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear) - self.motor_axis1.dot(&mj_lambda1.linear) @@ -310,24 +347,15 @@ impl PrismaticVelocityConstraint { mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse); mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse); } + } - /* - * Joint limits. - */ - if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) - + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) - + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0); - let dimpulse = new_impulse - self.limits_impulse; - self.limits_impulse = new_impulse; - - mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); - } + self.solve_limits(&mut mj_lambda1, &mut mj_lambda2); + self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -497,10 +525,6 @@ impl PrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - /* - * Setup motor. - */ - /* * Setup motor. */ @@ -602,12 +626,7 @@ impl PrismaticVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - - /* - * Joint constraint. - */ + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); let lin_dvel = self.basis1.tr_mul(&lin_vel2); @@ -629,10 +648,23 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Motors. - */ + fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { + if let Some(limits_forcedir2) = self.limits_forcedir2 { + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + + self.limits_rhs; + let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + } + } + + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { if self.motor_inv_lhs != 0.0 { let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs; let new_impulse = na::clamp( @@ -645,21 +677,14 @@ impl PrismaticVelocityGroundConstraint { mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse); } + } - /* - * Joint limits. - */ - if let Some(limits_forcedir2) = self.limits_forcedir2 { - let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) - + self.limits_rhs; - let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0); - let dimpulse = new_impulse - self.limits_impulse; - self.limits_impulse = new_impulse; - - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); - } + self.solve_limits(&mut mj_lambda2); + self.solve_motors(&mut mj_lambda2); + self.solve_dofs(&mut mj_lambda2); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } From 052a5a5fc04e36f7aac7a2fa50af352e2ac2bf0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 13:44:38 +0100 Subject: [PATCH 16/20] Make Simd prismatic joint limit code transmit torque. --- .../prismatic_velocity_constraint_wide.rs | 42 +++++++++++++++---- 1 file changed, 34 insertions(+), 8 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index 77a6fe7..bba9cb6 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -48,6 +48,7 @@ pub(crate) struct WPrismaticVelocityConstraint { limits_impulse: SimdReal, limits_forcedirs: Option<(Vector, Vector)>, limits_rhs: SimdReal, + limits_inv_lhs: SimdReal, #[cfg(feature = "dim2")] basis1: Vector2, @@ -187,10 +188,13 @@ impl WPrismaticVelocityConstraint { #[cfg(feature = "dim3")] let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); - // Setup limit constraint. + /* + * Setup limit constraint. + */ let mut limits_forcedirs = None; let mut limits_rhs = na::zero(); let mut limits_impulse = na::zero(); + let mut limits_inv_lhs = na::zero(); let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); if limits_enabled.any() { @@ -210,9 +214,17 @@ impl WPrismaticVelocityConstraint { let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0)); if sign != _0 { + let gcross1 = r1.gcross(axis1); + let gcross2 = r2.gcross(axis2); + limits_forcedirs = Some((axis1 * -sign, axis2 * sign)); limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign; limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0); + limits_inv_lhs = SimdReal::splat(1.0) + / (im1 + + im2 + + gcross1.dot(&ii1.transform_vector(gcross1)) + + gcross2.dot(&ii2.transform_vector(gcross2))); } } @@ -228,6 +240,7 @@ impl WPrismaticVelocityConstraint { limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff), limits_forcedirs, limits_rhs, + limits_inv_lhs, basis1, inv_lhs, rhs, @@ -270,9 +283,19 @@ impl WPrismaticVelocityConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + // Warmstart limits. if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + let limit_impulse1 = limits_forcedir1 * self.limits_impulse; + let limit_impulse2 = limits_forcedir2 * self.limits_impulse; + + mj_lambda1.linear += limit_impulse1 * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(self.r1.gcross(limit_impulse1)); + mj_lambda2.linear += limit_impulse2 * self.im2; + mj_lambda2.angular += self + .ii2_sqrt + .transform_vector(self.r2.gcross(limit_impulse2)); } for ii in 0..SIMD_WIDTH { @@ -339,8 +362,6 @@ impl WPrismaticVelocityConstraint { * Joint limits. */ if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { - // FIXME: the transformation by ii2_sqrt could be avoided by - // reusing some computations above. let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -348,12 +369,17 @@ impl WPrismaticVelocityConstraint { + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.limits_rhs; let new_impulse = - (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero()); + (self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero()); let dimpulse = new_impulse - self.limits_impulse; self.limits_impulse = new_impulse; - mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse); - mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + let lin_impulse1 = limits_forcedir1 * dimpulse; + let lin_impulse2 = limits_forcedir2 * dimpulse; + + mj_lambda1.linear += lin_impulse1 * self.im1; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1)); + mj_lambda2.linear += lin_impulse2 * self.im2; + mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); } for ii in 0..SIMD_WIDTH { From 4c9138fd2b8413a44ea665a2db5d245370ff54fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 13:58:43 +0100 Subject: [PATCH 17/20] Some minor cleanup and joint constraint refactoring. --- examples3d/joints3.rs | 74 +++++++++++++ .../ball_velocity_constraint.rs | 40 ++++--- .../prismatic_velocity_constraint_wide.rs | 104 +++++++++--------- .../revolute_velocity_constraint.rs | 35 +++--- 4 files changed, 170 insertions(+), 83 deletions(-) diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index ec83c9f..1246bc4 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -57,6 +57,73 @@ fn create_prismatic_joints( } } +fn create_actuated_prismatic_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + let ground = RigidBodyBuilder::new_static() + .translation(origin.x, origin.y, origin.z) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_parent, bodies); + + for i in 0..num { + let z = origin.z + (i + 1) as f32 * shift; + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(origin.x, origin.y, z) + .build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_child, bodies); + + let axis = if i % 2 == 0 { + Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)) + } else { + Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0)) + }; + + let z = Vector3::z(); + let mut prism = PrismaticJoint::new( + Point3::new(0.0, 0.0, 0.0), + axis, + z, + Point3::new(0.0, 0.0, -shift), + axis, + z, + ); + + if i == 1 { + prism.configure_motor_velocity(1.0, 1.0); + prism.limits_enabled = true; + prism.limits[1] = 5.0; + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + prism.motor_max_impulse = 1.0; + } else if i > 1 { + prism.configure_motor_position(2.0, 0.2, 1.0); + } else { + prism.configure_motor_velocity(1.0, 1.0); + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + prism.motor_max_impulse = 1.0; + prism.limits_enabled = true; + prism.limits[0] = -2.0; + prism.limits[1] = 5.0; + } + + joints.insert(bodies, curr_parent, curr_child, prism); + + curr_parent = curr_child; + } +} + fn create_revolute_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, @@ -362,6 +429,13 @@ pub fn init_world(testbed: &mut Testbed) { Point3::new(20.0, 5.0, 0.0), 4, ); + create_actuated_prismatic_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(25.0, 5.0, 0.0), + 4, + ); create_revolute_joints( &mut bodies, &mut colliders, diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 88e7c83..cb95532 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -183,10 +183,7 @@ impl BallVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); @@ -201,10 +198,9 @@ impl BallVelocityConstraint { mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); + } - /* - * Motor part. - */ + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { if let Some(motor_inv_lhs) = &self.motor_inv_lhs { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -224,6 +220,14 @@ impl BallVelocityConstraint { mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); + self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -232,7 +236,8 @@ impl BallVelocityConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse + ball.impulse = self.impulse; + ball.motor_impulse = self.motor_impulse; } } } @@ -393,9 +398,7 @@ impl BallVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let vel2 = mj_lambda2.linear + angvel.gcross(self.r2); let dvel = vel2 + self.rhs; @@ -405,10 +408,9 @@ impl BallVelocityGroundConstraint { mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); + } - /* - * Motor part. - */ + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { if let Some(motor_inv_lhs) = &self.motor_inv_lhs { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -425,6 +427,13 @@ impl BallVelocityGroundConstraint { mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda2); + self.solve_motors(&mut mj_lambda2); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -433,7 +442,8 @@ impl BallVelocityGroundConstraint { pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let joint = &mut joints_all[self.joint_id].weight; if let JointParams::BallJoint(ball) = &mut joint.params { - ball.impulse = self.impulse + ball.impulse = self.impulse; + ball.motor_impulse = self.motor_impulse; } } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index bba9cb6..be85024 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -308,27 +308,11 @@ impl WPrismaticVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), - }; - let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), - }; - - /* - * Joint consraint. - */ + fn solve_dofs( + &mut self, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); @@ -357,10 +341,13 @@ impl WPrismaticVelocityConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Joint limits. - */ + fn solve_limits( + &mut self, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) { if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -381,6 +368,28 @@ impl WPrismaticVelocityConstraint { mj_lambda2.linear += lin_impulse2 * self.im2; mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); + self.solve_limits(&mut mj_lambda1, &mut mj_lambda2); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -503,19 +512,6 @@ impl WPrismaticVelocityGroundConstraint { let axis1 = position1 * local_axis1; let axis2 = position2 * local_axis2; - // #[cfg(feature = "dim2")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .to_rotation_matrix() - // .into_inner(); - // #[cfg(feature = "dim3")] - // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) - // .unwrap_or_else(Rotation::identity) - // .to_rotation_matrix() - // .into_inner(); - // let basis2 = r21 * basis1; - // NOTE: we use basis2 := basis1 for now is that allows - // simplifications of the computation without introducing - // much instabilities. let ii2 = ii2_sqrt.squared(); let r1 = anchor1 - world_com1; let r2 = anchor2 - world_com2; @@ -642,19 +638,7 @@ impl WPrismaticVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), - }; - - /* - * Joint consraint. - */ + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); let lin_dvel = self.basis1.tr_mul(&lin_vel2); @@ -676,10 +660,9 @@ impl WPrismaticVelocityGroundConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + } - /* - * Joint limits. - */ + fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel) { if let Some(limits_forcedir2) = self.limits_forcedir2 { // FIXME: the transformation by ii2_sqrt could be avoided by // reusing some computations above. @@ -693,6 +676,20 @@ impl WPrismaticVelocityGroundConstraint { mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + self.solve_dofs(&mut mj_lambda2); + self.solve_limits(&mut mj_lambda2); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); @@ -700,7 +697,6 @@ impl WPrismaticVelocityGroundConstraint { } } - // FIXME: duplicated code with the non-ground constraint. pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { for ii in 0..SIMD_WIDTH { let joint = &mut joints_all[self.joint_id[ii]].weight; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index ef3a1ba..c1975da 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -209,10 +209,7 @@ impl RevoluteVelocityConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -237,10 +234,9 @@ impl RevoluteVelocityConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2)); + } - /* - * Motor. - */ + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel) { if self.motor_inv_lhs != 0.0 { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -258,6 +254,14 @@ impl RevoluteVelocityConstraint { mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); + self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; @@ -452,9 +456,7 @@ impl RevoluteVelocityGroundConstraint { mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { - let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - + fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel) { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); @@ -470,10 +472,8 @@ impl RevoluteVelocityGroundConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); - - /* - * Motor. - */ + } + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel) { if self.motor_inv_lhs != 0.0 { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let ang_dvel = ang_vel2.dot(&self.motor_axis2); @@ -489,6 +489,13 @@ impl RevoluteVelocityGroundConstraint { mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse); } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + self.solve_dofs(&mut mj_lambda2); + self.solve_motors(&mut mj_lambda2); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } From 0eec28325ecf192f386a6894526e97a462e68802 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 14:20:06 +0100 Subject: [PATCH 18/20] Fix warnings. --- src/dynamics/joint/ball_joint.rs | 6 +++++ src/dynamics/joint/generic_joint.rs | 2 +- src/dynamics/joint/joint.rs | 1 + src/dynamics/joint/prismatic_joint.rs | 4 ++++ src/dynamics/joint/revolute_joint.rs | 4 ++++ .../ball_velocity_constraint.rs | 6 +++-- .../joint_position_constraint.rs | 1 - .../prismatic_velocity_constraint.rs | 24 ++++++++++++------- .../prismatic_velocity_constraint_wide.rs | 6 ++--- .../revolute_position_constraint.rs | 2 +- .../revolute_velocity_constraint.rs | 11 ++++----- .../revolute_velocity_constraint_wide.rs | 2 +- src/lib.rs | 6 ----- 13 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index 47c90ab..8481961 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -74,20 +74,24 @@ impl BallJoint { self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) } + /// Set the spring-like model used by the motor to reach the desired target velocity and position. pub fn configure_motor_model(&mut self, model: SpringModel) { self.motor_model = model; } + /// Sets the target velocity and velocity correction factor this motor. #[cfg(feature = "dim2")] pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) } + /// Sets the target velocity and velocity correction factor this motor. #[cfg(feature = "dim3")] pub fn configure_motor_velocity(&mut self, target_vel: Vector, factor: Real) { self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) } + /// Sets the target orientation this motor needs to reach. pub fn configure_motor_position( &mut self, target_pos: Rotation, @@ -97,6 +101,7 @@ impl BallJoint { self.configure_motor(target_pos, na::zero(), stiffness, damping) } + /// Sets the target orientation this motor needs to reach. #[cfg(feature = "dim2")] pub fn configure_motor( &mut self, @@ -111,6 +116,7 @@ impl BallJoint { self.motor_damping = damping; } + /// Configure both the target orientation and target velocity of the motor. #[cfg(feature = "dim3")] pub fn configure_motor( &mut self, diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 5d776b0..c1549ff 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,5 +1,5 @@ use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint}; -use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM}; +use crate::math::{Isometry, Real, SpacialVector}; use crate::na::{Rotation3, UnitQuaternion}; #[derive(Copy, Clone, Debug)] diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs index 290c34c..e0a9d38 100644 --- a/src/dynamics/joint/joint.rs +++ b/src/dynamics/joint/joint.rs @@ -130,6 +130,7 @@ pub struct Joint { } impl Joint { + /// Can this joint use SIMD-accelerated constraint formulations? pub fn supports_simd_constraints(&self) -> bool { match &self.params { JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(), diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 4f5c984..3736b7f 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -217,18 +217,22 @@ impl PrismaticJoint { Isometry::from_parts(translation, rotation) } + /// Set the spring-like model used by the motor to reach the desired target velocity and position. pub fn configure_motor_model(&mut self, model: SpringModel) { self.motor_model = model; } + /// Sets the target velocity this motor needs to reach. pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) } + /// Sets the target position this motor needs to reach. pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) { self.configure_motor(target_pos, 0.0, stiffness, damping) } + /// Configure both the target position and target velocity of the motor. pub fn configure_motor( &mut self, target_pos: Real, diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 6895d3d..d1181e9 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -85,18 +85,22 @@ impl RevoluteJoint { self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0) } + /// Set the spring-like model used by the motor to reach the desired target velocity and position. pub fn configure_motor_model(&mut self, model: SpringModel) { self.motor_model = model; } + /// Sets the target velocity this motor needs to reach. pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) { self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor) } + /// Sets the target angle this motor needs to reach. pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) { self.configure_motor(target_pos, 0.0, stiffness, damping) } + /// Configure both the target angle and target velocity of the motor. pub fn configure_motor( &mut self, target_pos: Real, diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index cb95532..a110fbb 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -210,7 +210,8 @@ impl BallVelocityConstraint { let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); #[cfg(feature = "dim2")] - let clamped_impulse = na::clamp(new_impulse, -motor_max_impulse, motor_max_impulse); + let clamped_impulse = + na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse); #[cfg(feature = "dim3")] let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); @@ -418,7 +419,8 @@ impl BallVelocityGroundConstraint { let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel); #[cfg(feature = "dim2")] - let clamped_impulse = na::clamp(new_impulse, -motor_max_impulse, motor_max_impulse); + let clamped_impulse = + na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse); #[cfg(feature = "dim3")] let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse); diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index f0c374e..635e0b1 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -13,7 +13,6 @@ use super::{ WFixedPositionGroundConstraint, WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint, }; -use crate::dynamics::solver::DeltaVel; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 227338a..ae3f3e2 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, }; use crate::math::{AngularInertia, Real, Vector}; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; #[cfg(feature = "dim3")] use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] @@ -148,7 +148,6 @@ impl PrismaticVelocityConstraint { */ let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; - let mut motor_max_impulse = joint.motor_max_impulse; let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( params.dt, @@ -171,7 +170,11 @@ impl PrismaticVelocityConstraint { motor_rhs /= gamma; } - let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse); + let motor_impulse = na::clamp( + joint.motor_impulse, + -joint.motor_max_impulse, + joint.motor_max_impulse, + ); /* * Setup limit constraint. @@ -202,8 +205,8 @@ impl PrismaticVelocityConstraint { let gcross2 = r2.gcross(*axis2); limits_inv_lhs = crate::utils::inv( im1 + im2 - + gcross1.dot(&ii1.transform_vector(gcross1)) - + gcross2.dot(&ii2.transform_vector(gcross2)), + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2)), ); } } @@ -226,7 +229,7 @@ impl PrismaticVelocityConstraint { motor_impulse, motor_axis1: *axis1, motor_axis2: *axis2, - motor_max_impulse, + motor_max_impulse: joint.motor_max_impulse, basis1, inv_lhs, rhs, @@ -530,7 +533,6 @@ impl PrismaticVelocityGroundConstraint { */ let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; - let mut motor_max_impulse = joint.motor_max_impulse; let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( params.dt, @@ -553,7 +555,11 @@ impl PrismaticVelocityGroundConstraint { motor_rhs /= gamma; } - let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse); + let motor_impulse = na::clamp( + joint.motor_impulse, + -joint.motor_max_impulse, + joint.motor_max_impulse, + ); /* * Setup limit constraint. @@ -590,7 +596,7 @@ impl PrismaticVelocityGroundConstraint { motor_rhs, motor_inv_lhs, motor_impulse, - motor_max_impulse, + motor_max_impulse: joint.motor_max_impulse, basis1, inv_lhs, rhs, diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index be85024..442393d 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -7,7 +7,7 @@ use crate::dynamics::{ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH, }; -use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; #[cfg(feature = "dim3")] use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] @@ -223,8 +223,8 @@ impl WPrismaticVelocityConstraint { limits_inv_lhs = SimdReal::splat(1.0) / (im1 + im2 - + gcross1.dot(&ii1.transform_vector(gcross1)) - + gcross2.dot(&ii2.transform_vector(gcross2))); + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2))); } } diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 9075ed7..2da49e6 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -1,7 +1,7 @@ use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Matrix3x2, Matrix5, Unit}; +use na::Unit; #[derive(Debug)] pub(crate) struct RevolutePositionConstraint { diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index c1975da..61cb720 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -1,12 +1,11 @@ -use crate::dynamics::solver::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel}; +use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel}; use crate::dynamics::{ IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, }; use crate::math::{AngularInertia, Real, Rotation, Vector}; use crate::na::UnitQuaternion; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use downcast_rs::Downcast; -use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, RealField, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct RevoluteVelocityConstraint { @@ -93,7 +92,7 @@ impl RevoluteVelocityConstraint { let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); - let mut rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); /* * Motor. @@ -102,8 +101,8 @@ impl RevoluteVelocityConstraint { let motor_axis2 = rb2.position * *joint.local_axis2; let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; - let mut motor_max_impulse = joint.motor_max_impulse; let mut motor_angle = 0.0; + let motor_max_impulse = joint.motor_max_impulse; let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( params.dt, @@ -381,8 +380,8 @@ impl RevoluteVelocityGroundConstraint { */ let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; - let mut motor_max_impulse = joint.motor_max_impulse; let mut motor_angle = 0.0; + let motor_max_impulse = joint.motor_max_impulse; let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( params.dt, diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index d229457..7750d98 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -8,7 +8,7 @@ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; -use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, Vector5, U2, U3}; +use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[derive(Debug)] pub(crate) struct WRevoluteVelocityConstraint { diff --git a/src/lib.rs b/src/lib.rs index b3ca3d4..564a758 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -148,10 +148,4 @@ pub mod math { /// single contact constraint. #[cfg(feature = "dim3")] pub const MAX_MANIFOLD_POINTS: usize = 4; - - #[cfg(feature = "dim2")] - pub const SPATIAL_DIM: usize = 3; - - #[cfg(feature = "dim3")] - pub const SPATIAL_DIM: usize = 6; } From 0910e3235b348396c7fddd8043043c22aa60673e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 14:53:47 +0100 Subject: [PATCH 19/20] Use the github versions of nalgebra and parry until they are released. --- Cargo.toml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index db1ac16..0b0cdd2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,10 +18,11 @@ members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "e #nalgebra = { path = "../nalgebra" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } -#parry2d = { git = "https://github.com/sebcrozet/parry" } -#parry3d = { git = "https://github.com/sebcrozet/parry" } -#parry2d-f64 = { git = "https://github.com/sebcrozet/parry" } -#parry3d-f64 = { git = "https://github.com/sebcrozet/parry" } +nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } +parry2d = { git = "https://github.com/dimforge/parry" } +parry3d = { git = "https://github.com/dimforge/parry" } +parry2d-f64 = { git = "https://github.com/dimforge/parry" } +parry3d-f64 = { git = "https://github.com/dimforge/parry" } #ncollide2d = { git = "https://github.com/dimforge/ncollide" } #ncollide3d = { git = "https://github.com/dimforge/ncollide" } #nphysics2d = { git = "https://github.com/dimforge/nphysics" } From e5c4c2e8ffccf81aa5436c166b426a01b8b8831e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 22 Feb 2021 15:06:33 +0100 Subject: [PATCH 20/20] Ball joint: remove unused field. --- src/dynamics/joint/ball_joint.rs | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index 8481961..01b0f7f 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -38,8 +38,6 @@ pub struct BallJoint { pub motor_impulse: Vector, /// The spring-like model used by the motor to reach the target velocity and . pub motor_model: SpringModel, - // Used to handle cases where the position target ends up being more than pi radians away. - pub(crate) motor_last_angle: Real, } impl BallJoint { @@ -64,7 +62,6 @@ impl BallJoint { motor_impulse: na::zero(), motor_max_impulse: Real::MAX, motor_model: SpringModel::default(), - motor_last_angle: 0.0, } }