Open midstreeeam opened 1 year ago
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.
Huh. Is that what's going on for me with https://github.com/mattdm/bevy-rapier3d-motor-test/blob/master/src/main.rs ?
Was it fixed by https://github.com/dimforge/rapier/pull/422 ?
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
underJointVelocityConstraintBuilder
contain the following piece:the
s_limits
is halved buts_ang
is not. I think it might cause the input limit to become half. I am not sure why thes_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.
s_limits
to unify it withs_ang
so that the limit won't be resized.s_ang
is greater than PI.impulse_bounds
andrhs_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 foundselect
to changerhs_bias
but no proper function for changingimpulse_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.