Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Angular joint limit value halved and never goes beyond PI #499

Open
midstreeeam opened this issue Jul 4, 2023 · 3 comments
Open

Angular joint limit value halved and never goes beyond PI #499

midstreeeam opened this issue Jul 4, 2023 · 3 comments
Labels

Comments

@midstreeeam
Copy link

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
Copy link
Author

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
Copy link

mattdm commented Oct 16, 2023

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

@Vrixyz
Copy link
Contributor

Vrixyz commented Jun 3, 2024

Was it fixed by #422 ?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

3 participants