Le serveur gitlab sera inaccessible le mercredi 19 février 2020 de 13h à 14h pour une intervention de maintenance programmée.

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 ...@@ -17,7 +17,14 @@ Here is a quick representation of the reduction, with `x` the origin of the robo
| | | | | |
o---o o 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. The second approach is more thorough and consider each wheel without any reduction step.
......
...@@ -34,57 +34,59 @@ class RoverKinematics: ...@@ -34,57 +34,59 @@ class RoverKinematics:
self.motor_state = RoverMotors() self.motor_state = RoverMotors()
self.first_run = True 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 Computing steering and radial speed from velocity if we
group wheels by pairs. group wheels by pairs.
Args: Args:
twist(Twist): Data twist(Twist): Twist command, converted for example from joystick
Returns: Returns:
Dictionnary for each wheel Dictionnary for each wheel steering and speed
""" """
vx_r = twist.linear.x 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 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 vx_f = vx_r
vy_f = twist.linear.y + driver_cfg['FL']*twist.angular.z vy_f = twist.linear.y + 0.5*(drive_cfg["FL"].x+drive_cfg["FR"].x)*twist.angular.z
data = {'steering':{}, 'speed':{}} wheelcommand = {'steering':{}, 'speed':{}}
data['steering']['RL'] = atan2(vy_r,vx_r) wheelcommand['steering']['RL'] = atan2(vy_r,vx_r)
data['steering']['RR'] = atan2(vy_r,vx_r) wheelcommand['steering']['RR'] = atan2(vy_r,vx_r)
data['steering']['CL'] = atan2(vy_c,vx_c) wheelcommand['steering']['CL'] = atan2(vy_c,vx_c)
data['steering']['CR'] = atan2(vy_c,vx_c) wheelcommand['steering']['CR'] = atan2(vy_c,vx_c)
data['steering']['FL'] = atan2(vy_f,vx_f) wheelcommand['steering']['FL'] = atan2(vy_f,vx_f)
data['steering']['FR'] = atan2(vy_f,vx_f) wheelcommand['steering']['FR'] = atan2(vy_f,vx_f)
data['speed']['RL'] = 10*hypot(vy_r,vx_r) wheelcommand['speed']['RL'] = 10*hypot(vy_r,vx_r)
data['speed']['RR'] = 10*hypot(vy_r,vx_r) wheelcommand['speed']['RR'] = 10*hypot(vy_r,vx_r)
data['speed']['CL'] = 10*hypot(vy_c,vx_c) wheelcommand['speed']['CL'] = 10*hypot(vy_c,vx_c)
data['speed']['CR'] = 10*hypot(vy_c,vx_c) wheelcommand['speed']['CR'] = 10*hypot(vy_c,vx_c)
data['speed']['FL'] = 10*hypot(vy_f,vx_f) wheelcommand['speed']['FL'] = 10*hypot(vy_f,vx_f)
data['speed']['FR'] = 10*hypot(vy_f,vx_f) wheelcommand['speed']['FR'] = 10*hypot(vy_f,vx_f)
return data return wheelcommand
def twist_to_motors(self, twist, drive_cfg, skidsteer=False): def twist_to_motors(self, twist, drive_cfg, skidsteer=False):
motors = RoverMotors() motors = RoverMotors()
# comm = self.speed_steering_distribution(twist, driver_cfg) comm = self.speed_steering_distribution(twist, drive_cfg)
if skidsteer: if skidsteer:
for k in drive_cfg.keys(): for k in drive_cfg.keys():
# Insert here the steering and velocity of # Insert here the steering and velocity of
# each wheel in skid-steer mode # each wheel in skid-steer mode
motors.steering[k] = twist.angular.z vx = twist.linear.x - twist.angular.z*drive_cfg[k].y
motors.drive[k] = 2*twist.linear.x 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: else:
for k in drive_cfg.keys(): for k in drive_cfg.keys():
# Insert here the steering and velocity of # Insert here the steering and velocity of
# each wheel in rolling-without-slipping mode # each wheel in rolling-without-slipping mode
vx = twist.linear.x - twist.angular.z*driver_cfg[k].y vx = twist.linear.x - twist.angular.z*drive_cfg[k].y
vy = twist.linear.y + twist.angular.z*driver_cfg[k].x vy = twist.linear.y + twist.angular.z*drive_cfg[k].x
motors.steering[k] = atan2(vy, vx) motors.steering[k] = atan2(vy, vx) # comm['steering'][k] #
motors.drive[k] = 10*hypot(vy, vx) motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k] #
return motors return motors
def integrate_odometry(self, motor_state, drive_cfg): 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