Eskimue/ExoMy_Cuno
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
cb7d6c6c92edd6340e515dc642803dc37a58e38e
ExoMy_Cuno/ExoMy_Software-master/src/rover.py
T
Eskimue 32843cc1ae Ackermann gefixt
2026-05-22 09:56:41 +02:00

323 lines
13 KiB
Python
Raw Blame History

#!/usr/bin/env python
import rospy
import math
from locomotion_modes import LocomotionMode
import numpy as np
class Rover():
'''
Rover class contains all the math and motor control algorithms to move the rover
'''
# Defining wheel names
FL, FR, CL, CR, RL, RR = range(0, 6)
# Defining locomotion modes
FAKE_ACKERMANN, ACKERMANN, POINT_TURN, CRABBING = range(0, 4)
def __init__(self):
self.locomotion_mode = LocomotionMode.FAKE_ACKERMANN
self.ackermann_straight_tolerance_deg = 5
# x = Achsabstand, y = Spurbreite
self.wheel_rx = 14.0
self.wheel_ry = 20.3
self.wheel_fx = 16.0
self.wheel_fy = 20.3
self.point_turn_max_angle = 45
max_steering_angle = 45
self.ackermann_r_max = 250
self.ackermann_rr_min = abs(
self.wheel_rx) / math.tan(max_steering_angle * math.pi / 180.0) + (self.wheel_ry / 2)
self.ackermann_fr_min = abs(
self.wheel_fx) / math.tan(max_steering_angle * math.pi / 180.0) + (self.wheel_fy / 2)
self.ackermann_r_min = max(self.ackermann_fr_min, self.ackermann_rr_min)
def is_ackermann_straight(self, steering_command):
return abs(abs(steering_command) - 90) <= self.ackermann_straight_tolerance_deg
def setLocomotionMode(self, locomotion_mode_command):
'''
Sets the locomotion mode
'''
if(self.locomotion_mode != locomotion_mode_command):
self.locomotion_mode = locomotion_mode_command
rospy.loginfo('Set locomotion mode to: %s',
LocomotionMode(locomotion_mode_command).name)
def joystickToSteeringAngle(self, driving_command, steering_command):
'''
Converts the steering command [angle of joystick] to angles for the different motors
:param int driving_command: Drive speed command range from -100 to 100
:param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right)
'''
steering_angles = [0]*6
deg = steering_command
if(self.locomotion_mode == LocomotionMode.FAKE_ACKERMANN.value):
if (driving_command == 0):
# Stop
steering_angles[self.FL] = 0
steering_angles[self.FR] = 0
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = 0
steering_angles[self.RR] = 0
return steering_angles
if(80 < deg < 100):
# Drive straight forward
steering_angles[self.FL] = 0
steering_angles[self.FR] = 0
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = 0
steering_angles[self.RR] = 0
elif(-80 < deg < -100):
# Drive straight backwards
steering_angles[self.FL] = 0
steering_angles[self.FR] = 0
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = 0
steering_angles[self.RR] = 0
elif(100 < deg <= 180):
# Drive right forwards
steering_angles[self.FL] = 45
steering_angles[self.FR] = 45
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = -45
steering_angles[self.RR] = -45
elif(-100 > deg >= -180):
# Drive right backwards
steering_angles[self.FL] = 45
steering_angles[self.FR] = 45
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = -45
steering_angles[self.RR] = -45
elif(80 > deg >= 0):
# Drive left forwards
steering_angles[self.FL] = -45
steering_angles[self.FR] = -45
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = 45
steering_angles[self.RR] = 45
elif(0 > deg > -80):
# Drive left backwards
steering_angles[self.FL] = -45
steering_angles[self.FR] = -45
steering_angles[self.CR] = 0
steering_angles[self.CL] = 0
steering_angles[self.RL] = 45
steering_angles[self.RR] = 45
return steering_angles
if(self.locomotion_mode == LocomotionMode.ACKERMANN.value):
# No steering if robot is not driving
if(driving_command == 0):
return steering_angles
if self.is_ackermann_straight(steering_command):
return steering_angles
radius = self.ackermann_r_max - \
abs(math.cos(math.radians(steering_command))) * \
((self.ackermann_r_max-self.ackermann_r_min))
rear_inner_angle = int(math.degrees(
math.atan(self.wheel_rx / (abs(radius) - (self.wheel_ry / 2)))))
rear_outer_angle = int(math.degrees(
math.atan(self.wheel_rx / (abs(radius) + (self.wheel_ry / 2)))))
front_inner_angle = int(math.degrees(
math.atan(self.wheel_fx / (abs(radius) - (self.wheel_fy / 2)))))
front_outer_angle = int(math.degrees(
math.atan(self.wheel_fx / (abs(radius) + (self.wheel_fy / 2)))))
if steering_command > 90 or steering_command < -90:
# Steering to the right
steering_angles[self.FL] = front_outer_angle
steering_angles[self.FR] = front_inner_angle
steering_angles[self.RL] = -rear_outer_angle
steering_angles[self.RR] = -rear_inner_angle
else:
# Steering to the left
steering_angles[self.FL] = -front_inner_angle
steering_angles[self.FR] = -front_outer_angle
steering_angles[self.RL] = rear_inner_angle
steering_angles[self.RR] = rear_outer_angle
return steering_angles
if(self.locomotion_mode == LocomotionMode.POINT_TURN.value):
raw_point_turn_angle = math.degrees(
math.atan((self.wheel_rx + self.wheel_fx) / self.wheel_ry))
raw_point_turn_angle_center = math.degrees(
math.atan((((self.wheel_rx + self.wheel_fx) / 2) - self.wheel_fx) / (self.wheel_ry / 2)))
point_turn_angle = int(min(self.point_turn_max_angle, raw_point_turn_angle))
center_scale = 0.0 if raw_point_turn_angle == 0 else abs(raw_point_turn_angle_center / raw_point_turn_angle)
point_turn_angle_center = int(point_turn_angle * center_scale)
steering_angles[self.FL] = point_turn_angle
steering_angles[self.FR] = -point_turn_angle
steering_angles[self.CL] = point_turn_angle_center
steering_angles[self.CR] = -point_turn_angle_center
steering_angles[self.RL] = -point_turn_angle
steering_angles[self.RR] = point_turn_angle
return steering_angles
if(self.locomotion_mode == LocomotionMode.CRABBING.value):
if(driving_command != 0):
wheel_direction = 0
if(steering_command >= 0):
wheel_direction = steering_command - 90
elif(steering_command < 0):
wheel_direction = steering_command + 90
wheel_direction = np.clip(wheel_direction, -75, 75)
steering_angles[self.FL] = wheel_direction
steering_angles[self.FR] = wheel_direction
steering_angles[self.CL] = wheel_direction
steering_angles[self.CR] = wheel_direction
steering_angles[self.RL] = wheel_direction
steering_angles[self.RR] = wheel_direction
return steering_angles
def joystickToVelocity(self, driving_command, steering_command):
'''
Converts the steering and drive command to the speeds of the individual motors
:param int driving_command: Drive speed command range from -100 to 100
:param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right)
'''
motor_speeds = [0]*6
if (self.locomotion_mode == LocomotionMode.FAKE_ACKERMANN.value):
if(driving_command > 0 and steering_command >= 0):
motor_speeds[self.FL] = 50
motor_speeds[self.FR] = 50
motor_speeds[self.CR] = 50
motor_speeds[self.CL] = 50
motor_speeds[self.RL] = 50
motor_speeds[self.RR] = 50
elif(driving_command > 0 and steering_command <= 0):
motor_speeds[self.FL] = -50
motor_speeds[self.FR] = -50
motor_speeds[self.CR] = -50
motor_speeds[self.CL] = -50
motor_speeds[self.RL] = -50
motor_speeds[self.RR] = -50
return motor_speeds
if (self.locomotion_mode == LocomotionMode.ACKERMANN.value):
v = driving_command
if(steering_command < 0):
v *= -1
# Scale between min and max Ackermann radius
radius = self.ackermann_r_max - \
abs(math.cos(math.radians(steering_command))) * \
((self.ackermann_r_max-self.ackermann_r_min))
if (v == 0):
return motor_speeds
if self.is_ackermann_straight(steering_command):
return [v] * 6
if (radius == self.ackermann_r_max):
return [v] * 6
else:
r1 = (radius - (self.wheel_fy / 2)) / math.cos(
math.atan(self.wheel_fx / (abs(radius) - (self.wheel_fy / 2))))
r2 = (radius + (self.wheel_fy / 2)) / math.cos(
math.atan(self.wheel_fx / (abs(radius) + (self.wheel_fy / 2))))
r3 = radius - (self.wheel_fy / 2)
r4 = radius + (self.wheel_fy / 2)
r5 = (radius - (self.wheel_ry / 2)) / math.cos(
math.atan(self.wheel_rx / (abs(radius) - (self.wheel_ry / 2))))
r6 = (radius + (self.wheel_ry / 2)) / math.cos(
math.atan(self.wheel_rx / (abs(radius) + (self.wheel_ry / 2))))
reference_radius = max(r1, r2, r3, r4, r5, r6)
v1 = int(v * r1 / reference_radius)
v2 = int(v * r2 / reference_radius)
v3 = int(v * r3 / reference_radius)
v4 = int(v * r4 / reference_radius)
v5 = int(v * r5 / reference_radius)
v6 = int(v * r6 / reference_radius)
if (steering_command > 90 or steering_command < -90):
motor_speeds = [v2, v1, v4, v3, v6, v5]
else:
motor_speeds = [v1, v2, v3, v4, v5, v6]
return motor_speeds
if (self.locomotion_mode == LocomotionMode.POINT_TURN.value):
outer_turning_radius = math.sqrt(
math.pow(self.wheel_rx + self.wheel_fx, 2) + math.pow(self.wheel_ry, 2)) / 2
inner_turning_radius = math.sqrt(
math.pow(((self.wheel_rx + self.wheel_fx) / 2) - self.wheel_rx, 2) +
math.pow((self.wheel_ry / 2), 2))
deg = steering_command
if(driving_command != 0):
v = int(driving_command)
v_outer = v
v_inner = int(v * inner_turning_radius / outer_turning_radius)
# Left turn
if(deg < 85 and deg > -85):
motor_speeds[self.FL] = -v_outer
motor_speeds[self.FR] = v_outer
motor_speeds[self.CL] = -v_inner
motor_speeds[self.CR] = v_inner
motor_speeds[self.RL] = -v_outer
motor_speeds[self.RR] = v_outer
# Right turn
elif(deg > 95 or deg < -95):
motor_speeds[self.FL] = v_outer
motor_speeds[self.FR] = -v_outer
motor_speeds[self.CL] = v_inner
motor_speeds[self.CR] = -v_inner
motor_speeds[self.RL] = v_outer
motor_speeds[self.RR] = -v_outer
else:
# Stop
motor_speeds[self.FL] = 0
motor_speeds[self.FR] = 0
motor_speeds[self.CL] = 0
motor_speeds[self.CR] = 0
motor_speeds[self.RL] = 0
motor_speeds[self.RR] = 0
return motor_speeds
if(self.locomotion_mode == LocomotionMode.CRABBING.value):
v = driving_command
if(steering_command < 0):
v *= -1
motor_speeds[self.FL] = v
motor_speeds[self.FR] = v
motor_speeds[self.CL] = v
motor_speeds[self.CR] = v
motor_speeds[self.RL] = v
motor_speeds[self.RR] = v
return motor_speeds
Reference in New Issue View Git Blame Copy Permalink