Angular joint limit value halved and never goes beyond PI

[midstreeeam]

Hi! I was running into a problem with the joint limit.

Problem description

The problem has been mentioned in #378 , but later I found the reason for the limit and motor are not the same. After setting the joint limit of a joint (for example: [-90f32.to_radians(), 90f32.to_radians()]), the actual joint limit is halved. (It will become [-45,45] degrees). Also, if the input value is larger than PI, the joint limit will work abnormally. example: [-180f32.to_radians(),180f32.to_radians()] will not work, and [-200f32.to_radians(),200f32.to_radians()] will set a very small limit.

Possible reason

After spending some time looking into the code, I found all functions relate to limit_angular under JointVelocityConstraintBuilder contain the following piece:

\\ using limit_angular<const LANES: usize> as example
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;
#[cfg(feature = "dim3")]
let s_ang = self.ang_err.imag()[limited_axis];
let min_enabled = s_ang.simd_lt(s_limits[0]);
let max_enabled = s_limits[1].simd_lt(s_ang);

the s_limits is halved but s_ang is not. I think it might cause the input limit to become half. I am not sure why the s_limits is halved here, but once I remove the *half, the limit range comes back to normal.

Possible solution

I am new to rust and rapier, it is just a suggestion. I tried it on my own and it works.

1. Change s_limits to unify it with s_ang so that the limit won't be resized.
2. Set up a flag to determine whether s_ang is greater than PI.
3. Change the direction of impulse_bounds and rhs_bias according to the flag, so that the impulse direction will always remain correct.

The joint limit was extended from [-90,90] degrees to [-180,180] degrees in my test. However, I am not very familiar with Simba and its SimdValue, I found select to change rhs_bias but no proper function for changing impulse_bounds. So my test implementation might be slow.

I hope I can get some suggestions on it, and I will be happy to solve this problem with a quick pull request. I hope people give some suggestions, I REALLY need to fix this problem for my project.

[midstreeeam]

Just found I could use bitwise operation for branch operation. Here is the modified version of the function limit_angular. I didn't find any bugs after several rounds of tests.

    pub fn limit_angular<const LANES: usize>(
        &self,
        params: &IntegrationParameters,
        joint_id: [JointIndex; LANES],
        body1: &SolverBody<N, LANES>,
        body2: &SolverBody<N, LANES>,
        limited_axis: usize,
        limits: [N; 2],
        writeback_id: WritebackId,
    ) -> JointVelocityConstraint<N, LANES> {
        let zero = N::zero();
        let half = N::splat(0.5);
        let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()];

        let s_ang_dist = self.ang_err.angle();
        let over_half_pi = s_ang_dist.simd_abs().simd_gt(N::simd_frac_pi_2());

        #[cfg(feature = "dim3")]
        let s_ang = self.ang_err.imag()[limited_axis];

        #[cfg(feature = "dim2")]
        let min_triggered = s_ang_dist.simd_lt(limits[0]);
        #[cfg(feature = "dim3")]
        let min_triggered = s_ang.simd_lt(s_limits[0]);
    
        #[cfg(feature = "dim2")]
        let max_triggered = limits[1].simd_lt(s_ang_dist);
        #[cfg(feature = "dim3")]
        let max_triggered = s_limits[1].simd_lt(s_ang);

        let i0_flag = (over_half_pi & max_triggered) | (!over_half_pi & min_triggered);
        let i1_flag = (over_half_pi & min_triggered) | (!over_half_pi & max_triggered);
    
        let impulse_bounds = [
            N::splat(-Real::INFINITY).select(i0_flag, zero),
            N::splat(Real::INFINITY).select(i1_flag, zero),
        ];
    
        #[cfg(feature = "dim2")]
        let ang_jac = self.ang_basis[limited_axis];
        #[cfg(feature = "dim3")]
        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;

        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
        let cfm_coeff = N::splat(params.joint_cfm_coeff());

        let ni_rhs_bias = ((limits[1] - s_ang_dist).simd_max(zero)
            - (s_ang_dist - limits[0]).simd_max(zero))
            * erp_inv_dt;
        let i_rhs_bias = -ni_rhs_bias;
        let rhs_bias = i_rhs_bias.select(over_half_pi, ni_rhs_bias);

        let ang_jac1 = body1.sqrt_ii * ang_jac;
        let ang_jac2 = body2.sqrt_ii * ang_jac;

        JointVelocityConstraint {
            joint_id,
            mj_lambda1: body1.mj_lambda,
            mj_lambda2: body2.mj_lambda,
            im1: body1.im,
            im2: body2.im,
            impulse: N::zero(),
            impulse_bounds,
            lin_jac: na::zero(),
            ang_jac1,
            ang_jac2,
            inv_lhs: N::zero(), // Will be set during ortogonalization.
            cfm_coeff,
            cfm_gain: N::zero(),
            rhs: rhs_wo_bias + rhs_bias,
            rhs_wo_bias,
            writeback_id,
        }
    }

I only updated the 2d version and change nothing about 3d one. It somehow solves the problem.

[mattdm]

Huh. Is that what's going on for me with https://github.com/mattdm/bevy-rapier3d-motor-test/blob/master/src/main.rs ?

brokenclock1.webm