nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix NaN resulting from non-clamped input to simd_asin in angular motor solver (#840)

Browse Source 
* Fix non-clamped input to simd_asin in motor_angular

* Fix implementation & cleanup

* Cleanup

* Fix formatting
This commit is contained in:
Ryan H. Code
2025-07-11 09:32:59 -04:00
committed by GitHub
parent 9c9ba4570e
commit 86a257d4f1
1 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

@@ -753,10 +753,20 @@ impl<N: SimdRealCopy> JointTwoBodyConstraintHelper<N> {
let mut rhs_wo_bias = N::zero(); let mut rhs_wo_bias = N::zero();
if motor_params.erp_inv_dt != N::zero() { if motor_params.erp_inv_dt != N::zero() {
let ang_dist;
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_dist = self.ang_err.angle(); {
ang_dist = self.ang_err.angle();
}
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_dist = self.ang_err.imag()[_motor_axis].simd_asin() * N::splat(2.0); {
// Clamp the component from -1.0 to 1.0 to account for slight imprecision
let clamped_err = self.ang_err.imag()[_motor_axis].simd_clamp(-N::one(), N::one());
ang_dist = clamped_err.simd_asin() * N::splat(2.0);
}
let target_ang = motor_params.target_pos; let target_ang = motor_params.target_pos;
rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang) rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang)
* motor_params.erp_inv_dt; * motor_params.erp_inv_dt;