diff options
| -rw-r--r-- | examples3d/joints3.rs | 58 | ||||
| -rw-r--r-- | src/dynamics/joint/ball_joint.rs | 24 | ||||
| -rw-r--r-- | src/dynamics/joint/revolute_joint.rs | 3 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/ball_position_constraint.rs | 78 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs | 253 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs | 4 |
6 files changed, 366 insertions, 54 deletions
diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 902d810..11cd533 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -368,6 +368,58 @@ fn create_ball_joints( } } +fn create_ball_joints_with_limits( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point<f32>, +) { + let shift = vector![0.0, 0.0, 3.0]; + + let ground = bodies.insert( + RigidBodyBuilder::new_static() + .translation(origin.coords) + .build(), + ); + + let ball1 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + shift) + .linvel(vector![20.0, 20.0, 0.0]) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), + ball1, + bodies, + ); + + let ball2 = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(origin.coords + shift * 2.0) + .build(), + ); + colliders.insert_with_parent( + ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), + ball2, + bodies, + ); + + let mut joint1 = BallJoint::new(Point::origin(), Point::from(-shift)); + joint1.limits_enabled = true; + joint1.limits_local_axis1 = Vector::z_axis(); + joint1.limits_local_axis2 = Vector::z_axis(); + joint1.limits_angle = 0.2; + joints.insert(ground, ball1, joint1); + + let mut joint2 = BallJoint::new(Point::origin(), Point::from(-shift)); + joint2.limits_enabled = true; + joint2.limits_local_axis1 = Vector::z_axis(); + joint2.limits_local_axis2 = Vector::z_axis(); + joint2.limits_angle = 0.3; + joints.insert(ball1, ball2, joint2); +} + fn create_actuated_revolute_joints( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, @@ -549,6 +601,12 @@ pub fn init_world(testbed: &mut Testbed) { 3, ); create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15); + create_ball_joints_with_limits( + &mut bodies, + &mut colliders, + &mut joints, + point![-5.0, 0.0, 0.0], + ); /* * Set up the testbed. diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs index ee11489..0b626c0 100644 --- a/src/dynamics/joint/ball_joint.rs +++ b/src/dynamics/joint/ball_joint.rs @@ -1,5 +1,5 @@ use crate::dynamics::SpringModel; -use crate::math::{Point, Real, Rotation, Vector}; +use crate::math::{Point, Real, Rotation, UnitVector, Vector}; #[derive(Copy, Clone, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -38,6 +38,17 @@ pub struct BallJoint { pub motor_impulse: Vector<Real>, /// The spring-like model used by the motor to reach the target velocity and . pub motor_model: SpringModel, + + /// Are the limits enabled for this joint? + pub limits_enabled: bool, + /// The axis of the limit cone for this joint, if the local-space of the first body. + pub limits_local_axis1: UnitVector<Real>, + /// The axis of the limit cone for this joint, if the local-space of the first body. + pub limits_local_axis2: UnitVector<Real>, + /// The maximum angle allowed between the two limit axes in world-space. + pub limits_angle: Real, + /// The impulse applied to enforce joint limits. + pub limits_impulse: Real, } impl BallJoint { @@ -62,13 +73,20 @@ impl BallJoint { motor_impulse: na::zero(), motor_max_impulse: Real::MAX, motor_model: SpringModel::default(), + limits_enabled: false, + limits_local_axis1: Vector::x_axis(), + limits_local_axis2: Vector::x_axis(), + limits_angle: Real::MAX, + limits_impulse: 0.0, } } /// Can a SIMD constraint be used for resolving this joint? pub fn supports_simd_constraints(&self) -> bool { - // 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) + // SIMD ball constraints don't support motors and limits right now. + !self.limits_enabled + && (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. diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 53a82f5..6531a89 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -93,7 +93,7 @@ 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. + // SIMD revolute constraints don't support motors and limits right now. !self.limits_enabled && (self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)) @@ -142,6 +142,7 @@ impl RevoluteJoint { ) } + /// Estimates the current position of the motor angle given the joint parameters. pub fn estimate_motor_angle_from_params( axis1: &Unit<Vector<Real>>, tangent1: &Vector<Real>, diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 0227960..0169f30 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ }; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, UnitVector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -19,9 +19,15 @@ pub(crate) struct BallPositionConstraint { ii1: AngularInertia<Real>, ii2: AngularInertia<Real>, + inv_ii1_ii2: AngularInertia<Real>, local_anchor1: Point<Real>, local_anchor2: Point<Real>, + + limits_enabled: bool, + limits_angle: Real, + limits_local_axis1: UnitVector<Real>, + limits_local_axis2: UnitVector<Real>, } impl BallPositionConstraint { @@ -33,17 +39,26 @@ impl BallPositionConstraint { let (mprops1, ids1) = rb1; let (mprops2, ids2) = rb2; + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let inv_ii1_ii2 = (ii1 + ii2).inverse(); + Self { local_com1: mprops1.local_mprops.local_com, local_com2: mprops2.local_mprops.local_com, im1: mprops1.effective_inv_mass, im2: mprops2.effective_inv_mass, - ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), - ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), + ii1, + ii2, + inv_ii1_ii2, local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, position1: ids1.active_set_offset, position2: ids2.active_set_offset, + limits_enabled: cparams.limits_enabled, + limits_angle: cparams.limits_angle, + limits_local_axis1: cparams.limits_local_axis1, + limits_local_axis2: cparams.limits_local_axis2, } } @@ -96,6 +111,31 @@ impl BallPositionConstraint { position1.rotation = Rotation::new(angle1) * position1.rotation; position2.rotation = Rotation::new(angle2) * position2.rotation; + /* + * Limits part. + */ + if self.limits_enabled { + let axis1 = position1 * self.limits_local_axis1; + let axis2 = position2 * self.limits_local_axis2; + + // TODO: handle the case where dot(axis1, axis2) = -1.0 + if let Some((axis, angle)) = + Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()) + { + if angle >= self.limits_angle { + let ang_error = angle - self.limits_angle; + let ang_impulse = self + .inv_ii1_ii2 + .transform_vector(*axis * ang_error * params.joint_erp); + + 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; } @@ -109,6 +149,11 @@ pub(crate) struct BallPositionGroundConstraint { ii2: AngularInertia<Real>, local_anchor2: Point<Real>, local_com2: Point<Real>, + + limits_enabled: bool, + limits_angle: Real, + limits_axis1: UnitVector<Real>, + limits_local_axis2: UnitVector<Real>, } impl BallPositionGroundConstraint { @@ -132,6 +177,10 @@ impl BallPositionGroundConstraint { local_anchor2: cparams.local_anchor1, position2: ids2.active_set_offset, local_com2: mprops2.local_mprops.local_com, + limits_enabled: cparams.limits_enabled, + limits_angle: cparams.limits_angle, + limits_axis1: poss1.next_position * cparams.limits_local_axis2, + limits_local_axis2: cparams.limits_local_axis1, } } else { Self { @@ -141,6 +190,10 @@ impl BallPositionGroundConstraint { local_anchor2: cparams.local_anchor2, position2: ids2.active_set_offset, local_com2: mprops2.local_mprops.local_com, + limits_enabled: cparams.limits_enabled, + limits_angle: cparams.limits_angle, + limits_axis1: poss1.next_position * cparams.limits_local_axis1, + limits_local_axis2: cparams.limits_local_axis2, } } } @@ -172,6 +225,25 @@ impl BallPositionGroundConstraint { let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); position2.rotation = Rotation::new(angle2) * position2.rotation; + + /* + * Limits part. + */ + if self.limits_enabled { + let axis2 = position2 * self.limits_local_axis2; + + // TODO: handle the case where dot(axis1, axis2) = -1.0 + if let Some((axis, angle)) = Rotation::rotation_between_axis(&axis2, &self.limits_axis1) + .and_then(|r| r.axis_angle()) + { + if angle >= self.limits_angle { + let ang_error = angle - self.limits_angle; + let ang_correction = *axis * ang_error * params.joint_erp; + position2.rotation = Rotation::new(ang_correction) * position2.rotation; + } + } + } + positions[self.position2 as usize] = position2; } } diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 9e5227e..0689c2b 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; -use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector}; +use crate::math::{AngVector, AngularInertia, Real, Rotation, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -26,6 +26,12 @@ pub(crate) struct BallVelocityConstraint { motor_inv_lhs: Option<AngularInertia<Real>>, motor_max_impulse: Real, + limits_active: bool, + limits_rhs: Real, + limits_inv_lhs: Real, + limits_impulse: Real, + limits_axis: Vector<Real>, + im1: Real, im2: Real, @@ -51,18 +57,18 @@ impl BallVelocityConstraint { ), joint: &BallJoint, ) -> Self { - let (poss1, vels1, mprops1, ids1) = rb1; - let (poss2, vels2, mprops2, ids2) = rb2; + let (rb_pos1, rb_vels1, rb_mprops1, rb_ids1) = rb1; + let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2; - let anchor_world1 = poss1.position * joint.local_anchor1; - let anchor_world2 = poss2.position * joint.local_anchor2; - let anchor1 = anchor_world1 - mprops1.world_com; - let anchor2 = anchor_world2 - mprops2.world_com; + let anchor_world1 = rb_pos1.position * joint.local_anchor1; + let anchor_world2 = rb_pos2.position * joint.local_anchor2; + let anchor1 = anchor_world1 - rb_mprops1.world_com; + let anchor2 = anchor_world2 - rb_mprops2.world_com; - let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); - let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); - let im1 = mprops1.effective_inv_mass; - let im2 = mprops2.effective_inv_mass; + let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1); + let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2); + let im1 = rb_mprops1.effective_inv_mass; + let im2 = rb_mprops2.effective_inv_mass; let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -73,12 +79,12 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] { - lhs = mprops2 + lhs = rb_mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2) - + mprops1 + + rb_mprops1 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat1) @@ -89,8 +95,8 @@ impl BallVelocityConstraint { // it's just easier that way. #[cfg(feature = "dim2")] { - let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; @@ -114,8 +120,8 @@ impl BallVelocityConstraint { ); if stiffness != 0.0 { - let dpos = poss2.position.rotation - * (poss1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = rb_pos2.position.rotation + * (rb_pos1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -127,15 +133,15 @@ impl BallVelocityConstraint { } if damping != 0.0 { - let curr_vel = vels2.angvel - vels1.angvel; + let curr_vel = rb_vels2.angvel - rb_vels1.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 = mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / (ii1 + ii2)) } else { Some(gamma) @@ -146,8 +152,8 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); Some((ii1 + ii2).inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -163,10 +169,47 @@ impl BallVelocityConstraint { let motor_impulse = joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; + /* + * Setup the limits constraint. + */ + let mut limits_active = false; + let mut limits_rhs = 0.0; + let mut limits_inv_lhs = 0.0; + let mut limits_impulse = 0.0; + let mut limits_axis = Vector::zeros(); + + if joint.limits_enabled { + let axis1 = rb_pos1.position * joint.limits_local_axis1; + let axis2 = rb_pos2.position * joint.limits_local_axis2; + + if let Some((axis, angle)) = + Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()) + { + // TODO: we should allow predictive constraint activation. + if angle >= joint.limits_angle { + limits_active = true; + limits_rhs = (rb_vels2.angvel.dot(&axis) - rb_vels1.angvel.dot(&axis)) + * params.velocity_solve_fraction; + + limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt(); + + let ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); + limits_inv_lhs = crate::utils::inv( + axis.dot(&ii2.transform_vector(*axis)) + + axis.dot(&ii1.transform_vector(*axis)), + ); + + limits_impulse = joint.limits_impulse * params.warmstart_coeff; + limits_axis = *axis; + } + } + } + BallVelocityConstraint { joint_id, - mj_lambda1: ids1.active_set_offset, - mj_lambda2: ids2.active_set_offset, + mj_lambda1: rb_ids1.active_set_offset, + mj_lambda2: rb_ids2.active_set_offset, im1, im2, impulse: joint.impulse * params.warmstart_coeff, @@ -178,8 +221,13 @@ impl BallVelocityConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, - ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, + ii1_sqrt: rb_mprops1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt, + limits_active, + limits_axis, + limits_rhs, + limits_inv_lhs, + limits_impulse, } } @@ -196,6 +244,16 @@ impl BallVelocityConstraint { .ii2_sqrt .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); + /* + * Warmstart limits. + */ + if self.limits_active { + let limit_impulse1 = -self.limits_axis * self.limits_impulse; + let limit_impulse2 = self.limits_axis * self.limits_impulse; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(limit_impulse1); + mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2); + } + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -217,6 +275,29 @@ impl BallVelocityConstraint { mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); } + fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) { + if self.limits_active { + let limits_torquedir1 = -self.limits_axis; + let limits_torquedir2 = self.limits_axis; + + 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 = limits_torquedir1.dot(&ang_vel1) + + limits_torquedir2.dot(&ang_vel2) + + self.limits_rhs; + let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + let ang_impulse1 = limits_torquedir1 * dimpulse; + let ang_impulse2 = limits_torquedir2 * dimpulse; + + mj_lambda1.angular += self.ii1_sqrt.transform_vector(ang_impulse1); + mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2); + } + } + fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) { if let Some(motor_inv_lhs) = &self.motor_inv_lhs { let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -244,6 +325,7 @@ impl BallVelocityConstraint { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + self.solve_limits(&mut mj_lambda1, &mut mj_lambda2); self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); self.solve_motors(&mut mj_lambda1, &mut mj_lambda2); @@ -256,6 +338,7 @@ impl BallVelocityConstraint { if let JointParams::BallJoint(ball) = &mut joint.params { ball.impulse = self.impulse; ball.motor_impulse = self.motor_impulse; + ball.limits_impulse = self.limits_impulse; } } } @@ -275,6 +358,12 @@ pub(crate) struct BallVelocityGroundConstraint { motor_inv_lhs: Option<AngularInertia<Real>>, motor_max_impulse: Real, + limits_active: bool, + limits_rhs: Real, + limits_inv_lhs: Real, + limits_impulse: Real, + limits_axis: Vector<Real>, + im2: Real, ii2_sqrt: AngularInertia<Real>, } @@ -293,27 +382,27 @@ impl BallVelocityGroundConstraint { joint: &BallJoint, flipped: bool, ) -> Self { - let (poss1, vels1, mprops1) = rb1; - let (poss2, vels2, mprops2, ids2) = rb2; + let (rb_pos1, rb_vels1, rb_mprops1) = rb1; + let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2; let (anchor_world1, anchor_world2) = if flipped { ( - poss1.position * joint.local_anchor2, - poss2.position * joint.local_anchor1, + rb_pos1.position * joint.local_anchor2, + rb_pos2.position * joint.local_anchor1, ) } else { ( - poss1.position * joint.local_anchor1, - poss2.position * joint.local_anchor2, + rb_pos1.position * joint.local_anchor1, + rb_pos2.position * joint.local_anchor2, ) }; - let anchor1 = anchor_world1 - mprops1.world_com; - let anchor2 = anchor_world2 - mprops2.world_com; + let anchor1 = anchor_world1 - rb_mprops1.world_com; + let anchor2 = anchor_world2 - rb_mprops2.world_com; - let im2 = mprops2.effective_inv_mass; - let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); - let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); + let im2 = rb_mprops2.effective_inv_mass; + let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1); + let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2); let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -324,7 +413,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] { - lhs = mprops2 + lhs = rb_mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) @@ -333,7 +422,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim2")] { - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im2 + cmat2.x * cmat2.x * ii2; let m12 = cmat2.x * cmat2.y * ii2; let m22 = im2 + cmat2.y * cmat2.y * ii2; @@ -357,8 +446,8 @@ impl BallVelocityGroundConstraint { ); if stiffness != 0.0 { - let dpos = poss2.position.rotation - * (poss1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = rb_pos2.position.rotation + * (rb_pos1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -370,14 +459,14 @@ impl BallVelocityGroundConstraint { } if damping != 0.0 { - let curr_vel = vels2.angvel - vels1.angvel; + let curr_vel = rb_vels2.angvel - rb_vels1.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 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / ii2) } else { Some(gamma) @@ -388,7 +477,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); Some(ii2.inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -404,9 +493,50 @@ impl BallVelocityGroundConstraint { let motor_impulse = joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff; + /* + * Setup the limits constraint. + */ + let mut limits_active = false; + let mut limits_rhs = 0.0; + let mut limits_inv_lhs = 0.0; + let mut limits_impulse = 0.0; + let mut limits_axis = Vector::zeros(); + + if joint.limits_enabled { + let (axis1, axis2) = if flipped { + ( + rb_pos1.position * joint.limits_local_axis2, + rb_pos2.position * joint.limits_local_axis1, + ) + } else { + ( + rb_pos1.position * joint.limits_local_axis1, + rb_pos2.position * joint.limits_local_axis2, + ) + }; + + if let Some((axis, angle)) = + Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()) + { + // TODO: we should allow predictive constraint activation. + if angle >= joint.limits_angle { + limits_active = true; + limits_rhs = (rb_vels2.angvel.dot(&axis) - rb_vels1.angvel.dot(&axis)) + * params.velocity_solve_fraction; + + limits_rhs += (angle - joint.limits_angle) * params.velocity_based_erp_inv_dt(); + + let ii2 = rb_mprops2.effective_world_inv_inertia_sqrt.squared(); + limits_inv_lhs = crate::utils::inv(axis.dot(&ii2.transform_vector(*axis))); + limits_impulse = joint.limits_impulse * params.warmstart_coeff; + limits_axis = *axis; + } + } + } + BallVelocityGroundConstraint { joint_id, - mj_lambda2: ids2.active_set_offset, + mj_lambda2: rb_ids2.active_set_offset, im2, impulse: joint.impulse * params.warmstart_coeff, r2: anchor2, @@ -416,7 +546,12 @@ impl BallVelocityGroundConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb_mprops2.effective_world_inv_inertia_sqrt, + limits_active, + limits_axis, + limits_rhs, + limits_inv_lhs, + limits_impulse, } } @@ -426,6 +561,15 @@ impl BallVelocityGroundConstraint { mj_lambda2.angular -= self .ii2_sqrt .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse); + + /* + * Warmstart limits. + */ + if self.limits_active { + let limit_impulse2 = self.limits_axis * self.limits_impulse; + mj_lambda2.angular += self.ii2_sqrt.transform_vector(limit_impulse2); + } + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -441,6 +585,21 @@ impl BallVelocityGroundConstraint { mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); } + fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) { + if self.limits_active { + let limits_torquedir2 = self.limits_axis; + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let ang_dvel = limits_torquedir2.dot(&ang_vel2) + self.limits_rhs; + let new_impulse = (self.limits_impulse - ang_dvel * self.limits_inv_lhs).max(0.0); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + let ang_impulse2 = limits_torquedir2 * dimpulse; + mj_lambda2.angular += self.ii2_sqrt.transform_vector(ang_impulse2); + } + } + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) { if let Some(motor_inv_lhs) = &self.motor_inv_lhs { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); @@ -464,6 +623,7 @@ impl BallVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + self.solve_limits(&mut mj_lambda2); self.solve_dofs(&mut mj_lambda2); self.solve_motors(&mut mj_lambda2); @@ -476,6 +636,7 @@ impl BallVelocityGroundConstraint { if let JointParams::BallJoint(ball) = &mut joint.params { ball.impulse = self.impulse; ball.motor_impulse = self.motor_impulse; + ball.limits_impulse = self.limits_impulse; } } } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 46114c5..ca15cd9 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -294,7 +294,9 @@ impl RevoluteVelocityConstraint { .transform_vector(self.motor_axis2 * self.motor_impulse); } - // Warmstart limits. + /* + * Warmstart limits. + */ if self.limits_active { let limit_impulse1 = -self.motor_axis2 * self.limits_impulse; let limit_impulse2 = self.motor_axis2 * self.limits_impulse; |