def calculate_drive_velocities(self, speed, current_radius):
"""
Calculate target velocities for the drive motors based on desired speed and current turning radius
:param speed: Drive speed command range from -max_vel to max_vel, with max vel depending on the turning radius
:param radius: Current turning radius in m
"""
# clip the value to the maximum allowed velocity
speed = max(-self.max_vel, min(self.max_vel, speed))
cmd_msg = CommandDrive()
if speed == 0:
return cmd_msg
elif abs(current_radius) >= self.max_radius: # Very large turning radius, all wheels same speed
angular_vel = speed / self.wheel_radius
cmd_msg.left_front_vel = angular_vel
cmd_msg.left_middle_vel = angular_vel
cmd_msg.left_back_vel = angular_vel
cmd_msg.right_back_vel = angular_vel
cmd_msg.right_middle_vel = angular_vel
cmd_msg.right_front_vel = angular_vel
return cmd_msg
else:
# for the calculations, we assume positive radius (turn left) and adjust later
radius = abs(current_radius)
# the entire vehicle moves with the same angular velocity dictated by the desired speed,
# around the radius of the turn. v = r * omega
angular_velocity_center = float(speed) / radius
# calculate desired velocities of all centers of wheels. Corner wheels on the same side
# move with the same velocity. v = r * omega again
vel_middle_closest = (radius - self.d4) * angular_velocity_center
vel_corner_closest = math.hypot(radius - self.d1, self.d3) * angular_velocity_center
vel_corner_farthest = math.hypot(radius + self.d1, self.d3) * angular_velocity_center
vel_middle_farthest = (radius + self.d4) * angular_velocity_center
# now from these desired velocities, calculate the desired angular velocity of each wheel
# v = r * omega again
ang_vel_middle_closest = vel_middle_closest / self.wheel_radius
ang_vel_corner_closest = vel_corner_closest / self.wheel_radius
ang_vel_corner_farthest = vel_corner_farthest / self.wheel_radius
ang_vel_middle_farthest = vel_middle_farthest / self.wheel_radius
if current_radius > 0: # turning left
cmd_msg.left_front_vel = ang_vel_corner_closest
cmd_msg.left_back_vel = ang_vel_corner_closest
cmd_msg.left_middle_vel = ang_vel_middle_closest
cmd_msg.right_back_vel = ang_vel_corner_farthest
cmd_msg.right_front_vel = ang_vel_corner_farthest
cmd_msg.right_middle_vel = ang_vel_middle_farthest
else: # turning right
cmd_msg.left_front_vel = ang_vel_corner_farthest
cmd_msg.left_back_vel = ang_vel_corner_farthest
cmd_msg.left_middle_vel = ang_vel_middle_farthest
cmd_msg.right_back_vel = ang_vel_corner_closest
cmd_msg.right_front_vel = ang_vel_corner_closest
cmd_msg.right_middle_vel = ang_vel_middle_closest
return cmd_msg
def calculate_drive_ratios(self, speed, current_radius):
"""
Calculate target speed ratios for the drive motors based on desired speed and current turning radius.
:param speed: Drive speed command range from -max_vel to max_vel, with max vel depending on the turning radius.
:param radius: Current turning radius in meters.
"""
# clip the value to the maximum allowed velocity
cmd_msg = CommandDrive()
if speed == 0:
return cmd_msg
elif abs(current_radius) >= self.max_radius: # Very large turning radius, all wheels same speed
cmd_msg.left_front_vel = speed
cmd_msg.left_middle_vel = speed
cmd_msg.left_back_vel = speed
cmd_msg.right_back_vel = speed
cmd_msg.right_middle_vel = speed
cmd_msg.right_front_vel = speed
return cmd_msg
else:
# for the calculations, we assume positive radius (turn left) and adjust later
radius = abs(current_radius)
# the entire vehicle moves with the same angular velocity dictated by the desired speed,
# around the radius of the turn. v = r * omega
angular_velocity_center = float(speed) / radius
# calculate desired velocities of all centers of wheels. Corner wheels on the same side
# move with the same velocity. v = r * omega again
r1 = (radius - self.d4)
r2 = ((self.d3**2 + (radius - self.d1)**2)**0.5)
r3 = ((self.d3**2 + (radius + self.d1)**2)**0.5)
r4 = (radius + self.d4)
vel_middle_closest = (r1/r4) * speed
vel_corner_closest = (r2/r4) * speed
vel_corner_farthest = (r3/r4) * speed
vel_middle_farthest = speed
if current_radius > 0: # turning left
cmd_msg.left_front_vel = vel_corner_closest
cmd_msg.left_back_vel = vel_corner_closest
cmd_msg.left_middle_vel = vel_middle_closest
cmd_msg.right_back_vel = vel_corner_farthest
cmd_msg.right_front_vel = vel_corner_farthest
cmd_msg.right_middle_vel = vel_middle_farthest
else: # turning right
cmd_msg.left_front_vel = vel_corner_farthest
cmd_msg.left_back_vel = vel_corner_farthest
cmd_msg.left_middle_vel = vel_middle_farthest
cmd_msg.right_back_vel = vel_corner_closest
cmd_msg.right_front_vel = vel_corner_closest
cmd_msg.right_middle_vel = vel_middle_closest
return cmd_msg