Implement limits for ball joints.

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.

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.
+        // SIMD ball constraints don't support motors and limits right now.
-        self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)
+        !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.

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>,

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(),
+            ii1,
-            ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
+            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;
     }
 }

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 (rb_pos1, rb_vels1, rb_mprops1, rb_ids1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
+        let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
 
-        let anchor_world1 = poss1.position * joint.local_anchor1;
+        let anchor_world1 = rb_pos1.position * joint.local_anchor1;
-        let anchor_world2 = poss2.position * joint.local_anchor2;
+        let anchor_world2 = rb_pos2.position * joint.local_anchor2;
-        let anchor1 = anchor_world1 - mprops1.world_com;
+        let anchor1 = anchor_world1 - rb_mprops1.world_com;
-        let anchor2 = anchor_world2 - mprops2.world_com;
+        let anchor2 = anchor_world2 - rb_mprops2.world_com;
 
-        let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
+        let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
-        let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
+        let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(anchor2);
-        let im1 = mprops1.effective_inv_mass;
+        let im1 = rb_mprops1.effective_inv_mass;
-        let im2 = mprops2.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 ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-            let ii2 = mprops2.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
+                let dpos = rb_pos2.position.rotation
-                    * (poss1.position.rotation * joint.motor_target_pos).inverse();
+                    * (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 ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = mprops2.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 ii1 = rb_mprops1.effective_world_inv_inertia_sqrt.squared();
-                    let ii2 = mprops2.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_lambda1: rb_ids1.active_set_offset,
-            mj_lambda2: ids2.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,
+            ii1_sqrt: rb_mprops1.effective_world_inv_inertia_sqrt,
-            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,
         }
     }
 
@@ -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 (rb_pos1, rb_vels1, rb_mprops1) = rb1;
-        let (poss2, vels2, mprops2, ids2) = rb2;
+        let (rb_pos2, rb_vels2, rb_mprops2, rb_ids2) = rb2;
 
         let (anchor_world1, anchor_world2) = if flipped {
             (
-                poss1.position * joint.local_anchor2,
+                rb_pos1.position * joint.local_anchor2,
-                poss2.position * joint.local_anchor1,
+                rb_pos2.position * joint.local_anchor1,
             )
         } else {
             (
-                poss1.position * joint.local_anchor1,
+                rb_pos1.position * joint.local_anchor1,
-                poss2.position * joint.local_anchor2,
+                rb_pos2.position * joint.local_anchor2,
             )
         };
 
-        let anchor1 = anchor_world1 - mprops1.world_com;
+        let anchor1 = anchor_world1 - rb_mprops1.world_com;
-        let anchor2 = anchor_world2 - mprops2.world_com;
+        let anchor2 = anchor_world2 - rb_mprops2.world_com;
 
-        let im2 = mprops2.effective_inv_mass;
+        let im2 = rb_mprops2.effective_inv_mass;
-        let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
+        let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(anchor1);
-        let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
+        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
+                let dpos = rb_pos2.position.rotation
-                    * (poss1.position.rotation * joint.motor_target_pos).inverse();
+                    * (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;
         }
     }
 }

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;