Commit d434dfba authored by Mugisha David's avatar Mugisha David

instruction to complete tomorrow

parent 2dc170be
......@@ -4,7 +4,22 @@ This is part of my work to homework 4. The main goal is to control a 6 wheel rov
## Speed and Steering distribution
TODO
We compute each speed and steering wheel using a standard velocity tensor i.e `Vw = Vb + Ωb ∧ bw`.
Since each wheel sensor is accessible thanks to `driver_cfg`, we can deduce the steering and speed by applying classic formulas.
Our first approach was reducing the number of wheels, by grouping them by pair, resulting in an artificial rover of three wheels. However, this only allows the robots to move forward.
Here is a quick representation of the reduction, with `x` the origin of the robot frame and `o` a wheel.
```
----- -
o o o
| x | x
o o => o
| | |
o---o o
```
The rover cannot turn onto itself with such configuration.
The second approach is more thorough and consider each wheel without any reduction step.
## 2D Odometry
......
......@@ -34,13 +34,23 @@ class RoverKinematics:
self.motor_state = RoverMotors()
self.first_run = True
def speed_steering_distribution(self, twist):
def speed_steering_distribution(self, twist, driver_cfg):
"""
Computing steering and radial speed from velocity if we
group wheels by pairs.
Args:
twist(Twist): Data
Returns:
Dictionnary for each wheel
"""
vx_r = twist.linear.x
vy_r = twist.linear.y - 2*twist.angular.z
vy_r = twist.linear.y + driver_cfg['RL'].x*twist.angular.z
vx_c = vx_r
vy_c = twist.linear.y - 0.2*twist.angular.z
vy_c = twist.linear.y + driver_cfg['CL'].x*twist.angular.z
vx_f = vx_r
vy_f = twist.linear.y + 0.2*twist.angular.z
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)
......@@ -60,7 +70,7 @@ class RoverKinematics:
def twist_to_motors(self, twist, drive_cfg, skidsteer=False):
motors = RoverMotors()
comm = self.speed_steering_distribution(twist)
# comm = self.speed_steering_distribution(twist, driver_cfg)
if skidsteer:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
......@@ -71,8 +81,10 @@ class RoverKinematics:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
# each wheel in rolling-without-slipping mode
motors.steering[k] = comm['steering'][k]
motors.drive[k] = comm['speed'][k]
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)
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