Merge pull request #422 from pellico/Fix_Revolute-joint-motor-target-position-is-broken-at-and-beyond-90-degree-angles

Fix #378 Added one example join_motor_position

CHANGELOG.md

@@ -1,4 +1,7 @@
 ## Unlereased
+### Fix
+- Fix bug causing angular joint limits and motor to sometimes only take into account half of the angles specified by the user.
+
 ### Added
 - Add `SphericalJoint::local_frame1/2`, `::set_local_frame1/2`, and `SphericalJointBuilder::local_frame1/2` to set both
   the joint’s anchor and reference orientation.

examples2d/all_examples2.rs

@@ -17,6 +17,7 @@ mod damping2;
 mod debug_box_ball2;
 mod drum2;
 mod heightfield2;
+mod joint_motor_position2;
 mod joints2;
 mod locked_rotations2;
 mod one_way_platforms2;
@@ -79,6 +80,7 @@ pub fn main() {
         ("Rope Joints", rope_joints2::init_world),
         ("Sensor", sensor2::init_world),
         ("Trimesh", trimesh2::init_world),
+        ("Joint motor position", joint_motor_position2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
     ];
 

examples2d/joint_motor_position2.rs

@@ -0,0 +1,79 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Fixed ground to attach one end of the joints.
+     */
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+
+    /*
+     * A rectangle on a motor with target position.
+     */
+    for num in 0..9 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 2.0])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(point![x_pos, 1.5])
+            .local_anchor2(point![0.0, -0.5])
+            .motor_position(
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+                1000.0,
+                150.0,
+            );
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * A rectangle on a motor with limits.
+     */
+    for num in 0..8 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 4.5])
+            .rotation(std::f32::consts::PI)
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(point![x_pos, 5.0])
+            .local_anchor2(point![0.0, -0.5])
+            .motor_velocity(1.5, 30.0)
+            .motor_max_force(100.0)
+            .limits([
+                -std::f32::consts::PI,
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+            ]);
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        vector![0.0, 0.0],
+        (),
+    );
+    testbed.look_at(point![0.0, 0.0], 40.0);
+}

examples3d/all_examples3.rs

@@ -35,6 +35,7 @@ mod joints3;
 // mod joints3;
 mod character_controller3;
 mod debug_internal_edges3;
+mod joint_motor_position3;
 mod keva3;
 mod locked_rotations3;
 mod newton_cradle3;
@@ -97,6 +98,7 @@ pub fn main() {
         ("Domino", domino3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Impulse Joints", joints3::init_world_with_joints),
+        ("Joint Motor Position", joint_motor_position3::init_world),
         ("Locked rotations", locked_rotations3::init_world),
         ("One-way platforms", one_way_platforms3::init_world),
         ("Platform", platform3::init_world),

examples3d/joint_motor_position3.rs

@@ -0,0 +1,79 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Fixed ground to attach one end of the joints.
+     */
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+
+    /*
+     * A rectangle on a motor with target position.
+     */
+    for num in 0..9 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 2.0, 0.0])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5, 0.1);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new(Vector::z_axis())
+            .local_anchor1(point![x_pos, 1.5, 0.0])
+            .local_anchor2(point![0.0, -0.5, 0.0])
+            .motor_position(
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+                1000.0,
+                150.0,
+            );
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * A rectangle on a motor with limits.
+     */
+    for num in 0..8 {
+        let x_pos = -6.0 + 1.5 * num as f32;
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![x_pos, 4.5, 0.0])
+            .rotation(vector![0.0, 0.0, std::f32::consts::PI])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.1, 0.5, 0.1);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let joint = RevoluteJointBuilder::new(Vector::z_axis())
+            .local_anchor1(point![x_pos, 5.0, 0.0])
+            .local_anchor2(point![0.0, -0.5, 0.0])
+            .motor_velocity(1.5, 30.0)
+            .motor_max_force(100.0)
+            .limits([
+                -std::f32::consts::PI,
+                -std::f32::consts::PI + std::f32::consts::PI / 4.0 * num as f32,
+            ]);
+        impulse_joints.insert(ground_handle, handle, joint, true);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        vector![0.0, 0.0, 0.0],
+        (),
+    );
+    testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
+}

src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs

@@ -6,12 +6,15 @@ use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder,
 use crate::dynamics::solver::MotorParameters;
 use crate::dynamics::{IntegrationParameters, JointIndex, Multibody};
 use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM};
+use crate::utils;
 use crate::utils::IndexMut2;
 use crate::utils::WDot;
 use na::{DVector, SVector};
 
 #[cfg(feature = "dim3")]
 use crate::utils::WAngularInertia;
+#[cfg(feature = "dim2")]
+use na::Vector1;
 
 impl SolverBody<Real, 1> {
     pub fn fill_jacobians(
@@ -211,8 +214,8 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let dist = self.lin_err.dot(&lin_jac);
-        let min_enabled = dist < limits[0];
+        let min_enabled = dist <= limits[0];
-        let max_enabled = limits[1] < dist;
+        let max_enabled = limits[1] <= dist;
 
         let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
@@ -294,10 +297,13 @@ impl JointVelocityConstraintBuilder<Real> {
         body2: &SolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
-        locked_axis: usize,
+        _locked_axis: usize,
         writeback_id: WritebackId,
     ) -> JointGenericVelocityConstraint {
-        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_jac = Vector1::new(1.0);
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic(
             params,
@@ -318,7 +324,7 @@ impl JointVelocityConstraintBuilder<Real> {
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+        let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
         constraint.rhs += rhs_bias;
         constraint
     }
@@ -333,11 +339,14 @@ impl JointVelocityConstraintBuilder<Real> {
         body2: &SolverBody<Real, 1>,
         mb1: Option<(&Multibody, usize)>,
         mb2: Option<(&Multibody, usize)>,
-        limited_axis: usize,
+        _limited_axis: usize,
         limits: [Real; 2],
         writeback_id: WritebackId,
     ) -> JointGenericVelocityConstraint {
-        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_jac = Vector1::new(1.0);
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic(
             params,
@@ -356,11 +365,11 @@ impl JointVelocityConstraintBuilder<Real> {
 
         let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
         #[cfg(feature = "dim2")]
-        let s_ang = self.ang_err.im;
+        let s_ang = (self.ang_err.angle() / 2.0).sin();
         #[cfg(feature = "dim3")]
-        let s_ang = self.ang_err.imag()[limited_axis];
+        let s_ang = self.ang_err.imag()[_limited_axis];
-        let min_enabled = s_ang < s_limits[0];
+        let min_enabled = s_ang <= s_limits[0];
-        let max_enabled = s_limits[1] < s_ang;
+        let max_enabled = s_limits[1] <= s_ang;
         let impulse_bounds = [
             min_enabled as u32 as Real * -Real::MAX,
             max_enabled as u32 as Real * Real::MAX,
@@ -389,7 +398,6 @@ impl JointVelocityConstraintBuilder<Real> {
         motor_params: &MotorParameters<Real>,
         writeback_id: WritebackId,
     ) -> JointGenericVelocityConstraint {
-        // let mut ang_jac = self.ang_basis.column(motor_axis).into_owned();
         #[cfg(feature = "dim2")]
         let ang_jac = na::Vector1::new(1.0);
         #[cfg(feature = "dim3")]
@@ -413,11 +421,12 @@ impl JointVelocityConstraintBuilder<Real> {
         let mut rhs_wo_bias = 0.0;
         if motor_params.erp_inv_dt != 0.0 {
             #[cfg(feature = "dim2")]
-            let s_ang_dist = self.ang_err.im;
+            let s_ang_dist = (self.ang_err.angle() / 2.0).sin();
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
-            let s_target_ang = motor_params.target_pos.sin();
+            let s_target_ang = (motor_params.target_pos / 2.0).sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
+            rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang)
+                * motor_params.erp_inv_dt;
         }
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
@@ -603,8 +612,8 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let dist = self.lin_err.dot(&lin_jac);
-        let min_enabled = dist < limits[0];
+        let min_enabled = dist <= limits[0];
-        let max_enabled = limits[1] < dist;
+        let max_enabled = limits[1] <= dist;
 
         let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
@@ -681,10 +690,13 @@ impl JointVelocityConstraintBuilder<Real> {
         joint_id: JointIndex,
         body1: &SolverBody<Real, 1>,
         mb2: (&Multibody, usize),
-        locked_axis: usize,
+        _locked_axis: usize,
         writeback_id: WritebackId,
     ) -> JointGenericVelocityGroundConstraint {
-        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_jac = Vector1::new(1.0);
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic_ground(
             params,
@@ -703,7 +715,7 @@ impl JointVelocityConstraintBuilder<Real> {
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+        let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
         constraint.rhs += rhs_bias;
         constraint
     }
@@ -716,11 +728,14 @@ impl JointVelocityConstraintBuilder<Real> {
         joint_id: JointIndex,
         body1: &SolverBody<Real, 1>,
         mb2: (&Multibody, usize),
-        limited_axis: usize,
+        _limited_axis: usize,
         limits: [Real; 2],
         writeback_id: WritebackId,
     ) -> JointGenericVelocityGroundConstraint {
-        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_jac = Vector1::new(1.0);
+        #[cfg(feature = "dim3")]
+        let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
 
         let mut constraint = self.lock_jacobians_generic_ground(
             params,
@@ -737,11 +752,11 @@ impl JointVelocityConstraintBuilder<Real> {
 
         let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
         #[cfg(feature = "dim2")]
-        let s_ang = self.ang_err.im;
+        let s_ang = (self.ang_err.angle() / 2.0).sin();
         #[cfg(feature = "dim3")]
-        let s_ang = self.ang_err.imag()[limited_axis];
+        let s_ang = self.ang_err.imag()[_limited_axis];
-        let min_enabled = s_ang < s_limits[0];
+        let min_enabled = s_ang <= s_limits[0];
-        let max_enabled = s_limits[1] < s_ang;
+        let max_enabled = s_limits[1] <= s_ang;
         let impulse_bounds = [
             min_enabled as u32 as Real * -Real::MAX,
             max_enabled as u32 as Real * Real::MAX,
@@ -769,7 +784,6 @@ impl JointVelocityConstraintBuilder<Real> {
         motor_params: &MotorParameters<Real>,
         writeback_id: WritebackId,
     ) -> JointGenericVelocityGroundConstraint {
-        // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
         #[cfg(feature = "dim2")]
         let ang_jac = na::Vector1::new(1.0);
         #[cfg(feature = "dim3")]
@@ -791,11 +805,12 @@ impl JointVelocityConstraintBuilder<Real> {
         let mut rhs = 0.0;
         if motor_params.erp_inv_dt != 0.0 {
             #[cfg(feature = "dim2")]
-            let s_ang_dist = self.ang_err.im;
+            let s_ang_dist = (self.ang_err.angle() / 2.0).sin();
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
-            let s_target_ang = motor_params.target_pos.sin();
+            let s_target_ang = (motor_params.target_pos / 2.0).sin();
-            rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
+            rhs += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang)
+                * motor_params.erp_inv_dt;
         }
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);

src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs

@@ -5,6 +5,7 @@ use crate::dynamics::solver::joint_constraint::SolverBody;
 use crate::dynamics::solver::MotorParameters;
 use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits};
 use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
+use crate::utils;
 use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
 use na::SMatrix;
 
@@ -94,8 +95,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id);
 
         let dist = self.lin_err.dot(&constraint.lin_jac);
-        let min_enabled = dist.simd_lt(limits[0]);
+        let min_enabled = dist.simd_le(limits[0]);
-        let max_enabled = limits[1].simd_lt(dist);
+        let max_enabled = limits[1].simd_le(dist);
 
         let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
         let cfm_coeff = N::splat(params.joint_cfm_coeff());
@@ -281,7 +282,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
-        limited_axis: usize,
+        _limited_axis: usize,
         limits: [N; 2],
         writeback_id: WritebackId,
     ) -> JointVelocityConstraint<N, LANES> {
@@ -289,11 +290,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let half = N::splat(0.5);
         let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
         #[cfg(feature = "dim2")]
-        let s_ang = self.ang_err.im;
+        let s_ang = (self.ang_err.angle() * half).simd_sin();
         #[cfg(feature = "dim3")]
-        let s_ang = self.ang_err.imag()[limited_axis];
+        let s_ang = self.ang_err.imag()[_limited_axis];
-        let min_enabled = s_ang.simd_lt(s_limits[0]);
+        let min_enabled = s_ang.simd_le(s_limits[0]);
-        let max_enabled = s_limits[1].simd_lt(s_ang);
+        let max_enabled = s_limits[1].simd_le(s_ang);
 
         let impulse_bounds = [
             N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -301,9 +302,9 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         ];
 
         #[cfg(feature = "dim2")]
-        let ang_jac = self.ang_basis[limited_axis];
+        let ang_jac = N::one();
         #[cfg(feature = "dim3")]
-        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+        let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
         let rhs_wo_bias = dvel;
 
@@ -345,7 +346,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         motor_params: &MotorParameters<N>,
         writeback_id: WritebackId,
     ) -> JointVelocityConstraint<N, LANES> {
-        // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
         #[cfg(feature = "dim2")]
         let ang_jac = N::one();
         #[cfg(feature = "dim3")]
@@ -353,12 +353,15 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let mut rhs_wo_bias = N::zero();
         if motor_params.erp_inv_dt != N::zero() {
+            let half = N::splat(0.5);
+
             #[cfg(feature = "dim2")]
-            let s_ang_dist = self.ang_err.im;
+            let s_ang_dist = (self.ang_err.angle() * half).simd_sin();
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
-            let s_target_ang = motor_params.target_pos.simd_sin();
+            let s_target_ang = (motor_params.target_pos * half).simd_sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
+            rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang)
+                * motor_params.erp_inv_dt;
         }
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
@@ -393,13 +396,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
-        locked_axis: usize,
+        _locked_axis: usize,
         writeback_id: WritebackId,
     ) -> JointVelocityConstraint<N, LANES> {
         #[cfg(feature = "dim2")]
-        let ang_jac = self.ang_basis[locked_axis];
+        let ang_jac = N::one();
         #[cfg(feature = "dim3")]
-        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+        let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
         let rhs_wo_bias = dvel;
@@ -409,7 +412,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+        let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -495,8 +498,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let lin_jac = self.basis.column(limited_axis).into_owned();
         let dist = self.lin_err.dot(&lin_jac);
 
-        let min_enabled = dist.simd_lt(limits[0]);
+        let min_enabled = dist.simd_le(limits[0]);
-        let max_enabled = limits[1].simd_lt(dist);
+        let max_enabled = limits[1].simd_le(dist);
 
         let impulse_bounds = [
             N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -786,7 +789,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         motor_params: &MotorParameters<N>,
         writeback_id: WritebackId,
     ) -> JointVelocityGroundConstraint<N, LANES> {
-        // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned();
         #[cfg(feature = "dim2")]
         let ang_jac = N::one();
         #[cfg(feature = "dim3")]
@@ -794,12 +796,15 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let mut rhs_wo_bias = N::zero();
         if motor_params.erp_inv_dt != N::zero() {
+            let half = N::splat(0.5);
+
             #[cfg(feature = "dim2")]
-            let s_ang_dist = self.ang_err.im;
+            let s_ang_dist = (self.ang_err.angle() * half).simd_sin();
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
-            let s_target_ang = motor_params.target_pos.simd_sin();
+            let s_target_ang = (motor_params.target_pos * half).simd_sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
+            rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang)
+                * motor_params.erp_inv_dt;
         }
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
@@ -830,7 +835,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
-        limited_axis: usize,
+        _limited_axis: usize,
         limits: [N; 2],
         writeback_id: WritebackId,
     ) -> JointVelocityGroundConstraint<N, LANES> {
@@ -838,11 +843,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let half = N::splat(0.5);
         let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];
         #[cfg(feature = "dim2")]
-        let s_ang = self.ang_err.im;
+        let s_ang = (self.ang_err.angle() * half).simd_sin();
         #[cfg(feature = "dim3")]
-        let s_ang = self.ang_err.imag()[limited_axis];
+        let s_ang = self.ang_err.imag()[_limited_axis];
-        let min_enabled = s_ang.simd_lt(s_limits[0]);
+        let min_enabled = s_ang.simd_le(s_limits[0]);
-        let max_enabled = s_limits[1].simd_lt(s_ang);
+        let max_enabled = s_limits[1].simd_le(s_ang);
 
         let impulse_bounds = [
             N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -850,9 +855,9 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         ];
 
         #[cfg(feature = "dim2")]
-        let ang_jac = self.ang_basis[limited_axis];
+        let ang_jac = N::one();
         #[cfg(feature = "dim3")]
-        let ang_jac = self.ang_basis.column(limited_axis).into_owned();
+        let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
         let rhs_wo_bias = dvel;
 
@@ -887,13 +892,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
-        locked_axis: usize,
+        _locked_axis: usize,
         writeback_id: WritebackId,
     ) -> JointVelocityGroundConstraint<N, LANES> {
         #[cfg(feature = "dim2")]
-        let ang_jac = self.ang_basis[locked_axis];
+        let ang_jac = N::one();
         #[cfg(feature = "dim3")]
-        let ang_jac = self.ang_basis.column(locked_axis).into_owned();
+        let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
+
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
         let rhs_wo_bias = dvel;
 
@@ -902,7 +908,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
+        let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 

src/utils.rs

@@ -804,3 +804,13 @@ impl<T> IndexMut2<usize> for [T] {
         }
     }
 }
+
+/// Calculate the difference with smallest absolute value between the two given values.
+pub fn smallest_abs_diff_between_sin_angles<N: WReal>(a: N, b: N) -> N {
+    // Select the smallest path among the two angles to reach the target.
+    let s_err = a - b;
+    let sgn = s_err.simd_signum();
+    let s_err_complement = s_err - sgn * N::splat(2.0);
+    let s_err_is_smallest = s_err.simd_abs().simd_lt(s_err_complement.simd_abs());
+    s_err.select(s_err_is_smallest, s_err_complement)
+}