Commit bc6974fc authored by Mugisha David's avatar Mugisha David

Part 1

parent d434dfba
......@@ -17,7 +17,14 @@ Here is a quick representation of the reduction, with `x` the origin of the robo
| | |
o---o o
```
The rover cannot turn onto itself with such configuration.
The rover cannot turn effectively on itself with that configuration. As matter of fact, applying a full right command (or left) would result for the wheel to have 90 degrees position, which is valid for a three wheeled vehicles whose wheels are aligned with the frame origin.
Illustrations
![Grouped wheels](url)
![Decoupled wheels](url)
The second approach is more thorough and consider each wheel without any reduction step.
......
......@@ -34,57 +34,59 @@ class RoverKinematics:
self.motor_state = RoverMotors()
self.first_run = True
def speed_steering_distribution(self, twist, driver_cfg):
def speed_steering_distribution(self, twist, drive_cfg):
"""
Computing steering and radial speed from velocity if we
group wheels by pairs.
Args:
twist(Twist): Data
twist(Twist): Twist command, converted for example from joystick
Returns:
Dictionnary for each wheel
Dictionnary for each wheel steering and speed
"""
vx_r = twist.linear.x
vy_r = twist.linear.y + driver_cfg['RL'].x*twist.angular.z
vy_r = twist.linear.y + 0.5*(drive_cfg["RL"].x+drive_cfg["RR"].x)*twist.angular.z
vx_c = vx_r
vy_c = twist.linear.y + driver_cfg['CL'].x*twist.angular.z
vy_c = twist.linear.y + 0.5*(drive_cfg["CL"].x+drive_cfg["CR"].x)*twist.angular.z
vx_f = vx_r
vy_f = twist.linear.y + driver_cfg['FL']*twist.angular.z
data = {'steering':{}, 'speed':{}}
data['steering']['RL'] = atan2(vy_r,vx_r)
data['steering']['RR'] = atan2(vy_r,vx_r)
data['steering']['CL'] = atan2(vy_c,vx_c)
data['steering']['CR'] = atan2(vy_c,vx_c)
data['steering']['FL'] = atan2(vy_f,vx_f)
data['steering']['FR'] = atan2(vy_f,vx_f)
data['speed']['RL'] = 10*hypot(vy_r,vx_r)
data['speed']['RR'] = 10*hypot(vy_r,vx_r)
data['speed']['CL'] = 10*hypot(vy_c,vx_c)
data['speed']['CR'] = 10*hypot(vy_c,vx_c)
data['speed']['FL'] = 10*hypot(vy_f,vx_f)
data['speed']['FR'] = 10*hypot(vy_f,vx_f)
return data
vy_f = twist.linear.y + 0.5*(drive_cfg["FL"].x+drive_cfg["FR"].x)*twist.angular.z
wheelcommand = {'steering':{}, 'speed':{}}
wheelcommand['steering']['RL'] = atan2(vy_r,vx_r)
wheelcommand['steering']['RR'] = atan2(vy_r,vx_r)
wheelcommand['steering']['CL'] = atan2(vy_c,vx_c)
wheelcommand['steering']['CR'] = atan2(vy_c,vx_c)
wheelcommand['steering']['FL'] = atan2(vy_f,vx_f)
wheelcommand['steering']['FR'] = atan2(vy_f,vx_f)
wheelcommand['speed']['RL'] = 10*hypot(vy_r,vx_r)
wheelcommand['speed']['RR'] = 10*hypot(vy_r,vx_r)
wheelcommand['speed']['CL'] = 10*hypot(vy_c,vx_c)
wheelcommand['speed']['CR'] = 10*hypot(vy_c,vx_c)
wheelcommand['speed']['FL'] = 10*hypot(vy_f,vx_f)
wheelcommand['speed']['FR'] = 10*hypot(vy_f,vx_f)
return wheelcommand
def twist_to_motors(self, twist, drive_cfg, skidsteer=False):
motors = RoverMotors()
# comm = self.speed_steering_distribution(twist, driver_cfg)
comm = self.speed_steering_distribution(twist, drive_cfg)
if skidsteer:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
# each wheel in skid-steer mode
motors.steering[k] = twist.angular.z
motors.drive[k] = 2*twist.linear.x
vx = twist.linear.x - twist.angular.z*drive_cfg[k].y
vy = twist.linear.y + twist.angular.z*drive_cfg[k].x
motors.steering[k] = atan2(vy, vx) # comm['steering'][k]
motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k]
else:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
# each wheel in rolling-without-slipping mode
vx = twist.linear.x - twist.angular.z*driver_cfg[k].y
vy = twist.linear.y + twist.angular.z*driver_cfg[k].x
motors.steering[k] = atan2(vy, vx)
motors.drive[k] = 10*hypot(vy, vx)
vx = twist.linear.x - twist.angular.z*drive_cfg[k].y
vy = twist.linear.y + twist.angular.z*drive_cfg[k].x
motors.steering[k] = atan2(vy, vx) # comm['steering'][k] #
motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k] #
return motors
def integrate_odometry(self, motor_state, drive_cfg):
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment