From 4332818e021644aa716e7ef0cca774cab09f4860 Mon Sep 17 00:00:00 2001 From: Fun Maker Date: Sun, 21 Apr 2024 20:48:35 +0200 Subject: Fix joint limits not being flipped in one body constrains. (#549) --- src/dynamics/joint/generic_joint.rs | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) (limited to 'src/dynamics/joint/generic_joint.rs') diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 76a7fe1..98f9311 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -496,6 +496,26 @@ impl GenericJoint { self.motors[i].damping = damping; self } + + /// Flips the orientation of the joint, including limits and motors. + pub fn flip(&mut self) -> &mut Self { + std::mem::swap(&mut self.local_frame1, &mut self.local_frame2); + + let coupled_bits = self.coupled_axes.bits(); + + for dim in 0..SPATIAL_DIM { + if coupled_bits & (1 << dim) == 0 { + let limit = self.limits[dim]; + self.limits[dim].min = -limit.max; + self.limits[dim].max = -limit.min; + } + + self.motors[dim].target_vel = -self.motors[dim].target_vel; + self.motors[dim].target_pos = -self.motors[dim].target_pos; + } + + self + } } macro_rules! joint_conversion_methods( -- cgit From 0d76a55d80dcee517012d1c3a5d6f7aae67dad2e Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 5 May 2024 16:15:00 +0200 Subject: chore: don’t return &mut Self with GenericJoint::flip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/dynamics/joint/generic_joint.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'src/dynamics/joint/generic_joint.rs') diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 98f9311..8c8a4aa 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -498,7 +498,7 @@ impl GenericJoint { } /// Flips the orientation of the joint, including limits and motors. - pub fn flip(&mut self) -> &mut Self { + pub fn flip(&mut self) { std::mem::swap(&mut self.local_frame1, &mut self.local_frame2); let coupled_bits = self.coupled_axes.bits(); @@ -513,8 +513,6 @@ impl GenericJoint { self.motors[dim].target_vel = -self.motors[dim].target_vel; self.motors[dim].target_pos = -self.motors[dim].target_pos; } - - self } } -- cgit