From 28c1a302dd4faef6c5ab4a35d684a151ed5d4c76 Mon Sep 17 00:00:00 2001 From: Shubham Date: Sun, 3 Mar 2024 15:01:10 +0100 Subject: [PATCH] simpler calculation for lateral motion --- webots_spot/spot_driver.py | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/webots_spot/spot_driver.py b/webots_spot/spot_driver.py index b3d1b4c..4c6df0e 100644 --- a/webots_spot/spot_driver.py +++ b/webots_spot/spot_driver.py @@ -329,28 +329,18 @@ def __cmd_vel(self, msg): self.yawd = 0.0 self.StepLength = StepLength * msg.linear.x + if self.StepLength == 0.0: + self.StepLength = StepLength * abs(msg.linear.y) # Rotation along vertical axis self.YawRate = msg.angular.z if self.YawRate != 0 and self.StepLength == 0: self.StepLength = StepLength * 0.1 - # Holonomic motions - self.LateralFraction = np.arctan2(msg.linear.y, msg.linear.x) - # forcefully set 0, as output is never 0 if second term in arctan2 is -ve - if msg.linear.y == 0: - self.LateralFraction = 0 - if self.LateralFraction != 0: - if msg.linear.x != 0: - # change lateral fraction for 2nd and 4th quadrants - if msg.linear.x > 0 and self.LateralFraction < 0: - self.LateralFraction += np.pi - elif msg.linear.x < 0 and self.LateralFraction > 0: - self.LateralFraction -= np.pi - self.StepLength = StepLength * msg.linear.y * np.sign(msg.linear.x) - else: - # for sideway motion - self.StepLength = StepLength * abs(msg.linear.y) + # Lateral motion + self.LateralFraction = np.arctan2(msg.linear.y, abs(msg.linear.x)) + if msg.linear.x < 0: + self.LateralFraction *= -1 if 0.001 < self.StepLength < 0.05: self.StepLength = max(0.015, self.StepLength)