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

fix: also apply joint limits in case of equality

Browse Source
This commit is contained in:
Sébastien Crozet
2023-12-10 21:51:57 +01:00
parent fef7f0f92a
commit 76557448d7
2 changed files with 19 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
View File

@@ -214,8 +214,8 @@ impl JointVelocityConstraintBuilder<Real> {
); );
let dist = self.lin_err.dot(&lin_jac); 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 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; let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
@@ -368,8 +368,8 @@ impl JointVelocityConstraintBuilder<Real> {
let s_ang = (self.ang_err.angle() / 2.0).sin(); let s_ang = (self.ang_err.angle() / 2.0).sin();
#[cfg(feature = "dim3")] #[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 = [ let impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX, min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX, max_enabled as u32 as Real * Real::MAX,
@@ -612,8 +612,8 @@ impl JointVelocityConstraintBuilder<Real> {
); );
let dist = self.lin_err.dot(&lin_jac); 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 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; let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
@@ -755,8 +755,8 @@ impl JointVelocityConstraintBuilder<Real> {
let s_ang = (self.ang_err.angle() / 2.0).sin(); let s_ang = (self.ang_err.angle() / 2.0).sin();
#[cfg(feature = "dim3")] #[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 = [ let impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX, min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX, max_enabled as u32 as Real * Real::MAX,

22
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -95,8 +95,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id); self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id);
let dist = self.lin_err.dot(&constraint.lin_jac); 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 erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff()); let cfm_coeff = N::splat(params.joint_cfm_coeff());
@@ -293,8 +293,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let s_ang = (self.ang_err.angle() * half).simd_sin(); let s_ang = (self.ang_err.angle() * half).simd_sin();
#[cfg(feature = "dim3")] #[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 = [ let impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero), N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -498,8 +498,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let lin_jac = self.basis.column(limited_axis).into_owned(); let lin_jac = self.basis.column(limited_axis).into_owned();
let dist = self.lin_err.dot(&lin_jac); 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 = [ let impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero), N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -835,7 +835,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
joint_id: [JointIndex; LANES], joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>, body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>, body2: &SolverBody<N, LANES>,
limited_axis: usize, _limited_axis: usize,
limits: [N; 2], limits: [N; 2],
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> { ) -> JointVelocityGroundConstraint<N, LANES> {
@@ -845,9 +845,9 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang = (self.ang_err.angle() * half).simd_sin(); let s_ang = (self.ang_err.angle() * half).simd_sin();
#[cfg(feature = "dim3")] #[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 = [ let impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero), N::splat(-Real::INFINITY).select(min_enabled, zero),
@@ -857,7 +857,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let ang_jac = N::one(); let ang_jac = N::one();
#[cfg(feature = "dim3")] #[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 dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
let rhs_wo_bias = dvel; let rhs_wo_bias = dvel;