Fix joint limits not being flipped in one body constrains. (#549)

This commit is contained in:
Fun Maker
2024-04-21 20:48:35 +02:00
committed by Sébastien Crozet
parent a2fdeab7e1
commit 4332818e02
3 changed files with 28 additions and 16 deletions

View File

@@ -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(