From 6432909110c4b96e59d4ee2ebdae1d79abe8d4b3 Mon Sep 17 00:00:00 2001 From: pellico Date: Sun, 27 Nov 2022 18:47:35 +0100 Subject: Fix #378 Added one example join_motor_position --- .../joint_generic_velocity_constraint_builder.rs | 30 ++++++++++++++++------ .../joint_velocity_constraint_builder.rs | 29 +++++++++++++++------ 2 files changed, 43 insertions(+), 16 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 1e7df89..4156680 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -12,6 +12,8 @@ use na::{DVector, SVector}; #[cfg(feature = "dim3")] use crate::utils::WAngularInertia; +#[cfg(feature = "dim2")] +const PI: Real = std::f64::consts::PI as Real; impl SolverBody { pub fn fill_jacobians( @@ -413,11 +415,17 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = 0.0; if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] - let s_ang_dist = self.ang_err.im; + { + let s_ang_dist = self.ang_err.angle(); + let s_target_ang = motor_params.target_pos; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0*PI))/(2.0*PI) * motor_params.erp_inv_dt; + } #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + { + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = motor_params.target_pos.sin(); + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + } } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -791,11 +799,17 @@ impl JointVelocityConstraintBuilder { let mut rhs = 0.0; if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] - let s_ang_dist = self.ang_err.im; + { + let s_ang_dist = self.ang_err.angle(); + let s_target_ang = motor_params.target_pos; + rhs += ((s_ang_dist - s_target_ang) % (2.0*PI))/(2.0*PI) * motor_params.erp_inv_dt; + } #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.sin(); - rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + { + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = motor_params.target_pos.sin(); + rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + } } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index 439030e..e6cb872 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -354,11 +354,18 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { #[cfg(feature = "dim2")] - let s_ang_dist = self.ang_err.im; + { + let s_ang_dist = self.ang_err.angle(); + let s_target_ang = motor_params.target_pos; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi())/N::simd_two_pi() * motor_params.erp_inv_dt; + } #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + { + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = motor_params.target_pos.simd_sin(); + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + } + } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -795,11 +802,17 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { #[cfg(feature = "dim2")] - let s_ang_dist = self.ang_err.im; + { + let s_ang_dist = self.ang_err.angle(); + let s_target_ang = motor_params.target_pos; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi())/N::simd_two_pi() * motor_params.erp_inv_dt; + } #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + { + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = motor_params.target_pos.simd_sin(); + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; + } } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); -- cgit From 9f3b5c86421e957e94f14e23b7e9912ada88d7e2 Mon Sep 17 00:00:00 2001 From: pellico Date: Mon, 28 Nov 2022 16:47:15 +0100 Subject: Fixed formatting of modified files. --- .../joint_constraint/joint_generic_velocity_constraint_builder.rs | 6 ++++-- .../solver/joint_constraint/joint_velocity_constraint_builder.rs | 7 ++++--- 2 files changed, 8 insertions(+), 5 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 4156680..50a37e2 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -418,7 +418,8 @@ impl JointVelocityConstraintBuilder { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0*PI))/(2.0*PI) * motor_params.erp_inv_dt; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { @@ -802,7 +803,8 @@ impl JointVelocityConstraintBuilder { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs += ((s_ang_dist - s_target_ang) % (2.0*PI))/(2.0*PI) * motor_params.erp_inv_dt; + rhs += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index e6cb872..2fae33a 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -357,7 +357,8 @@ impl JointVelocityConstraintBuilder { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi())/N::simd_two_pi() * motor_params.erp_inv_dt; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { @@ -365,7 +366,6 @@ impl JointVelocityConstraintBuilder { let s_target_ang = motor_params.target_pos.simd_sin(); rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; } - } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -805,7 +805,8 @@ impl JointVelocityConstraintBuilder { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi())/N::simd_two_pi() * motor_params.erp_inv_dt; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { -- cgit From a05622cfe995cb681f01bc22956addf99c208cc6 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 10 Jul 2023 09:14:16 +0200 Subject: fix: avoid perpetual movement when the target ang motor position is overshot --- .../joint_generic_velocity_constraint_builder.rs | 41 ++++++++-------------- .../joint_velocity_constraint_builder.rs | 41 +++++++++------------- 2 files changed, 31 insertions(+), 51 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 50a37e2..a38f9d3 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -6,14 +6,13 @@ use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, use crate::dynamics::solver::MotorParameters; use crate::dynamics::{IntegrationParameters, JointIndex, Multibody}; use crate::math::{Real, Vector, ANG_DIM, DIM, SPATIAL_DIM}; +use crate::utils; use crate::utils::IndexMut2; use crate::utils::WDot; use na::{DVector, SVector}; #[cfg(feature = "dim3")] use crate::utils::WAngularInertia; -#[cfg(feature = "dim2")] -const PI: Real = std::f64::consts::PI as Real; impl SolverBody { pub fn fill_jacobians( @@ -358,7 +357,7 @@ impl JointVelocityConstraintBuilder { let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; #[cfg(feature = "dim2")] - let s_ang = self.ang_err.im; + let s_ang = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang < s_limits[0]; @@ -393,7 +392,7 @@ impl JointVelocityConstraintBuilder { ) -> JointGenericVelocityConstraint { // let mut ang_jac = self.ang_basis.column(motor_axis).into_owned(); #[cfg(feature = "dim2")] - let ang_jac = na::Vector1::new(1.0); + let ang_jac = na::Vector1::new(1.0); // self.ang_basis[(0, 0)]); #[cfg(feature = "dim3")] let ang_jac = self.basis.column(_motor_axis).into_owned(); @@ -415,18 +414,12 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = 0.0; if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos / 2.0).sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -746,7 +739,7 @@ impl JointVelocityConstraintBuilder { let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; #[cfg(feature = "dim2")] - let s_ang = self.ang_err.im; + let s_ang = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang < s_limits[0]; @@ -800,18 +793,12 @@ impl JointVelocityConstraintBuilder { let mut rhs = 0.0; if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.sin(); - rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos / 2.0).sin(); + rhs += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index 2fae33a..b8b0fe4 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -5,6 +5,7 @@ use crate::dynamics::solver::joint_constraint::SolverBody; use crate::dynamics::solver::MotorParameters; use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits}; use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; +use crate::utils; use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; use na::SMatrix; @@ -289,7 +290,7 @@ impl JointVelocityConstraintBuilder { 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; + let s_ang = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang.simd_lt(s_limits[0]); @@ -353,19 +354,15 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { + let half = N::splat(0.5); + #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos * half).simd_sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -801,19 +798,15 @@ impl JointVelocityConstraintBuilder { let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { + let half = N::splat(0.5); + #[cfg(feature = "dim2")] - { - let s_ang_dist = self.ang_err.angle(); - let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() - * motor_params.erp_inv_dt; - } + let s_ang_dist = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] - { - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; - } + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos * half).simd_sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -852,7 +845,7 @@ impl JointVelocityConstraintBuilder { 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; + let s_ang = (self.ang_err.angle() * half).simd_sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[limited_axis]; let min_enabled = s_ang.simd_lt(s_limits[0]); -- cgit From fef7f0f92a281dcbcccac50d1019e72ffb1ec3ee Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 10 Dec 2023 21:39:11 +0100 Subject: fix more 2D angular motor/limits jacobians --- .../joint_generic_velocity_constraint_builder.rs | 42 ++++++++++++++-------- .../joint_velocity_constraint_builder.rs | 29 ++++++++------- 2 files changed, 41 insertions(+), 30 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index a38f9d3..b42ab9f 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -13,6 +13,8 @@ use na::{DVector, SVector}; #[cfg(feature = "dim3")] use crate::utils::WAngularInertia; +#[cfg(feature = "dim2")] +use na::Vector1; impl SolverBody { pub fn fill_jacobians( @@ -295,10 +297,13 @@ impl JointVelocityConstraintBuilder { body2: &SolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, - locked_axis: usize, + _locked_axis: usize, writeback_id: WritebackId, ) -> JointGenericVelocityConstraint { - let ang_jac = self.ang_basis.column(locked_axis).into_owned(); + #[cfg(feature = "dim2")] + let ang_jac = Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); let mut constraint = self.lock_jacobians_generic( params, @@ -319,7 +324,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; + let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; constraint.rhs += rhs_bias; constraint } @@ -334,11 +339,14 @@ impl JointVelocityConstraintBuilder { body2: &SolverBody, mb1: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>, - limited_axis: usize, + _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, ) -> JointGenericVelocityConstraint { - let ang_jac = self.ang_basis.column(limited_axis).into_owned(); + #[cfg(feature = "dim2")] + let ang_jac = Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); let mut constraint = self.lock_jacobians_generic( params, @@ -359,7 +367,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let s_ang = (self.ang_err.angle() / 2.0).sin(); #[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 max_enabled = s_limits[1] < s_ang; let impulse_bounds = [ @@ -390,9 +398,8 @@ impl JointVelocityConstraintBuilder { motor_params: &MotorParameters, writeback_id: WritebackId, ) -> JointGenericVelocityConstraint { - // let mut ang_jac = self.ang_basis.column(motor_axis).into_owned(); #[cfg(feature = "dim2")] - let ang_jac = na::Vector1::new(1.0); // self.ang_basis[(0, 0)]); + let ang_jac = na::Vector1::new(1.0); #[cfg(feature = "dim3")] let ang_jac = self.basis.column(_motor_axis).into_owned(); @@ -683,10 +690,13 @@ impl JointVelocityConstraintBuilder { joint_id: JointIndex, body1: &SolverBody, mb2: (&Multibody, usize), - locked_axis: usize, + _locked_axis: usize, writeback_id: WritebackId, ) -> JointGenericVelocityGroundConstraint { - let ang_jac = self.ang_basis.column(locked_axis).into_owned(); + #[cfg(feature = "dim2")] + let ang_jac = Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); let mut constraint = self.lock_jacobians_generic_ground( params, @@ -705,7 +715,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; + let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; constraint.rhs += rhs_bias; constraint } @@ -718,11 +728,14 @@ impl JointVelocityConstraintBuilder { joint_id: JointIndex, body1: &SolverBody, mb2: (&Multibody, usize), - limited_axis: usize, + _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, ) -> JointGenericVelocityGroundConstraint { - let ang_jac = self.ang_basis.column(limited_axis).into_owned(); + #[cfg(feature = "dim2")] + let ang_jac = Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); let mut constraint = self.lock_jacobians_generic_ground( params, @@ -741,7 +754,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let s_ang = (self.ang_err.angle() / 2.0).sin(); #[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 max_enabled = s_limits[1] < s_ang; let impulse_bounds = [ @@ -771,7 +784,6 @@ impl JointVelocityConstraintBuilder { motor_params: &MotorParameters, writeback_id: WritebackId, ) -> JointGenericVelocityGroundConstraint { - // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned(); #[cfg(feature = "dim2")] let ang_jac = na::Vector1::new(1.0); #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index b8b0fe4..7a4ccc7 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -282,7 +282,7 @@ impl JointVelocityConstraintBuilder { joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, - limited_axis: usize, + _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, ) -> JointVelocityConstraint { @@ -292,7 +292,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let s_ang = (self.ang_err.angle() * half).simd_sin(); #[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 max_enabled = s_limits[1].simd_lt(s_ang); @@ -302,9 +302,9 @@ impl JointVelocityConstraintBuilder { ]; #[cfg(feature = "dim2")] - let ang_jac = self.ang_basis[limited_axis]; + let ang_jac = N::one(); #[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 rhs_wo_bias = dvel; @@ -346,7 +346,6 @@ impl JointVelocityConstraintBuilder { motor_params: &MotorParameters, writeback_id: WritebackId, ) -> JointVelocityConstraint { - // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned(); #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] @@ -397,13 +396,13 @@ impl JointVelocityConstraintBuilder { joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, - locked_axis: usize, + _locked_axis: usize, writeback_id: WritebackId, ) -> JointVelocityConstraint { #[cfg(feature = "dim2")] - let ang_jac = self.ang_basis[locked_axis]; + let ang_jac = N::one(); #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(locked_axis).into_owned(); + let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); let rhs_wo_bias = dvel; @@ -413,7 +412,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; + let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -790,7 +789,6 @@ impl JointVelocityConstraintBuilder { motor_params: &MotorParameters, writeback_id: WritebackId, ) -> JointVelocityGroundConstraint { - // let mut ang_jac = self.ang_basis.column(_motor_axis).into_owned(); #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] @@ -857,7 +855,7 @@ impl JointVelocityConstraintBuilder { ]; #[cfg(feature = "dim2")] - let ang_jac = self.ang_basis[limited_axis]; + let ang_jac = N::one(); #[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); @@ -894,13 +892,14 @@ impl JointVelocityConstraintBuilder { joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, - locked_axis: usize, + _locked_axis: usize, writeback_id: WritebackId, ) -> JointVelocityGroundConstraint { #[cfg(feature = "dim2")] - let ang_jac = self.ang_basis[locked_axis]; + let ang_jac = N::one(); #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(locked_axis).into_owned(); + let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); + let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); let rhs_wo_bias = dvel; @@ -909,7 +908,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; + let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; let ang_jac2 = body2.sqrt_ii * ang_jac; -- cgit From 76557448d7bff7d1c057c982aa4f55f7ee6a612f Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 10 Dec 2023 21:51:57 +0100 Subject: fix: also apply joint limits in case of equality --- .../joint_generic_velocity_constraint_builder.rs | 16 ++++++++-------- .../joint_velocity_constraint_builder.rs | 22 +++++++++++----------- 2 files changed, 19 insertions(+), 19 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index b42ab9f..c16151d 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -214,8 +214,8 @@ impl JointVelocityConstraintBuilder { ); let dist = self.lin_err.dot(&lin_jac); - let min_enabled = dist < limits[0]; - let max_enabled = limits[1] < dist; + let min_enabled = dist <= limits[0]; + let max_enabled = limits[1] <= dist; 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; @@ -368,8 +368,8 @@ impl JointVelocityConstraintBuilder { let s_ang = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[_limited_axis]; - let min_enabled = s_ang < s_limits[0]; - let max_enabled = s_limits[1] < s_ang; + let min_enabled = s_ang <= s_limits[0]; + let max_enabled = s_limits[1] <= s_ang; let impulse_bounds = [ min_enabled as u32 as Real * -Real::MAX, max_enabled as u32 as Real * Real::MAX, @@ -612,8 +612,8 @@ impl JointVelocityConstraintBuilder { ); let dist = self.lin_err.dot(&lin_jac); - let min_enabled = dist < limits[0]; - let max_enabled = limits[1] < dist; + let min_enabled = dist <= limits[0]; + let max_enabled = limits[1] <= dist; 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; @@ -755,8 +755,8 @@ impl JointVelocityConstraintBuilder { let s_ang = (self.ang_err.angle() / 2.0).sin(); #[cfg(feature = "dim3")] let s_ang = self.ang_err.imag()[_limited_axis]; - let min_enabled = s_ang < s_limits[0]; - let max_enabled = s_limits[1] < s_ang; + let min_enabled = s_ang <= s_limits[0]; + let max_enabled = s_limits[1] <= s_ang; let impulse_bounds = [ min_enabled as u32 as Real * -Real::MAX, max_enabled as u32 as Real * Real::MAX, diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index 7a4ccc7..f7101a1 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -95,8 +95,8 @@ impl JointVelocityConstraintBuilder { self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id); let dist = self.lin_err.dot(&constraint.lin_jac); - let min_enabled = dist.simd_lt(limits[0]); - let max_enabled = limits[1].simd_lt(dist); + let min_enabled = dist.simd_le(limits[0]); + let max_enabled = limits[1].simd_le(dist); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); @@ -293,8 +293,8 @@ impl JointVelocityConstraintBuilder { let s_ang = (self.ang_err.angle() * half).simd_sin(); #[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); + let min_enabled = s_ang.simd_le(s_limits[0]); + let max_enabled = s_limits[1].simd_le(s_ang); let impulse_bounds = [ N::splat(-Real::INFINITY).select(min_enabled, zero), @@ -498,8 +498,8 @@ impl JointVelocityConstraintBuilder { let lin_jac = self.basis.column(limited_axis).into_owned(); let dist = self.lin_err.dot(&lin_jac); - let min_enabled = dist.simd_lt(limits[0]); - let max_enabled = limits[1].simd_lt(dist); + let min_enabled = dist.simd_le(limits[0]); + let max_enabled = limits[1].simd_le(dist); let impulse_bounds = [ N::splat(-Real::INFINITY).select(min_enabled, zero), @@ -835,7 +835,7 @@ impl JointVelocityConstraintBuilder { joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, - limited_axis: usize, + _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, ) -> JointVelocityGroundConstraint { @@ -845,9 +845,9 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let s_ang = (self.ang_err.angle() * half).simd_sin(); #[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); + let s_ang = self.ang_err.imag()[_limited_axis]; + let min_enabled = s_ang.simd_le(s_limits[0]); + let max_enabled = s_limits[1].simd_le(s_ang); let impulse_bounds = [ N::splat(-Real::INFINITY).select(min_enabled, zero), @@ -857,7 +857,7 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[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 rhs_wo_bias = dvel; -- cgit