diff options
Diffstat (limited to 'src/dynamics/solver')
6 files changed, 258 insertions, 309 deletions
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<Real>, r2: Vector<Real>, - basis: Rotation<Real>, + basis1: Rotation<Real>, + basis2: Rotation<Real>, dependant_set_mask: SpacialVector<Real>, vel: GenericConstraintPart, @@ -44,22 +45,22 @@ impl GenericVelocityConstraint { max_velocity: &SpatialVector<Real>, r1: &Vector<Real>, r2: &Vector<Real>, - basis: &Rotation<Real>, + basis1: &Rotation<Real>, + basis2: &Rotation<Real>, rb1: &RigidBody, rb2: &RigidBody, ) -> SpatialVector<Real> { - 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::<AngDim>(DIM).into_owned(); let max_linvel = max_velocity.xyz(); let max_angvel = max_velocity.fixed_rows::<AngDim>(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<Real>, + max_impulse: &Vector<Real>, + dependant_set_mask: &mut Vector<Real>, + mut delassus: na::Matrix3<Real>, + ) -> na::Matrix3<Real> { + // 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<Real>, @@ -169,32 +196,6 @@ impl GenericVelocityConstraint { } } - pub fn invert_partial_delassus_matrix( - min_impulse: &Vector<Real>, - max_impulse: &Vector<Real>, - dependant_set_mask: &mut Vector<Real>, - mut delassus: na::Matrix3<Real>, - ) -> na::Matrix3<Real> { - // 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>(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::<na::U3, na::U3>(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::<na::U2, na::U3>(3, 0) - .copy_from(&lhs10); - lhs.fixed_slice_mut::<na::U2, na::U2>(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::<na::U3>(0).into_owned(); - let ang_impulse1 = basis1 * impulse.fixed_rows::<na::U2>(3).into_owned(); - let ang_impulse2 = basis2 * impulse.fixed_rows::<na::U2>(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<Real>]) { 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::<na::U3, na::U3>(0, 0) - .copy_from(&lhs00.into_matrix()); - lhs.fixed_slice_mut::<na::U2, na::U3>(3, 0) - .copy_from(&lhs10); - lhs.fixed_slice_mut::<na::U2, na::U2>(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::<na::U3>(0).into_owned(); - let ang_impulse2 = basis2 * impulse.fixed_rows::<na::U2>(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<Real>, motor_axis2: Vector<Real>, @@ -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; - - 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 mut motor_max_impulse = joint.motor_max_impulse; + let mut motor_angle = 0.0; + + let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_co |