Commit dda7221f authored by Mugisha David's avatar Mugisha David

Yeah

parent 2ba456b2
......@@ -32,7 +32,25 @@ The second approach is more thorough and consider each wheel without any reducti
## 2D Odometry
TODO
We used a least square method to compute the position and steering of the robot in the global frame.
We had 12 equations each one resulting from relations between wheels data and position in the global frame.
For each wheel we have the following :
`Vw = V - Ω*W`
`Sw = V*dt - Ω∧W*dt`
resulting in
`Sw*cos(beta_w) = delta_x - delta_theta*Wy`
and
`Sw*sin(beta_w) = delta_y + delta_theta*Wx`
after projection
where
* Sw, delta_x stands for the position of each wheel respectively in the robot and global frame,
* Ω rotation speed in the global frame and position of the wheel in the global frame
* `beta_w` bearing of the wheel
* `Wy` and `Wx` are actually the radius of each wheel
We tried to visualize our odomoetry using rviz, however it was marked among unvisualible topics...
## Task Manager
......
......@@ -46,11 +46,11 @@ class RoverKinematics:
Dictionnary for each wheel steering and speed
"""
vx_r = twist.linear.x
vy_r = twist.linear.y + 0.5*(drive_cfg["RL"].x+drive_cfg["RR"].x)*twist.angular.z
vy_r = twist.linear.y + drive_cfg["RL"].x*twist.angular.z
vx_c = vx_r
vy_c = twist.linear.y + 0.5*(drive_cfg["CL"].x+drive_cfg["CR"].x)*twist.angular.z
vy_c = twist.linear.y + drive_cfg["CL"].x*twist.angular.z
vx_f = vx_r
vy_f = twist.linear.y + 0.5*(drive_cfg["FL"].x+drive_cfg["FR"].x)*twist.angular.z
vy_f = twist.linear.y + drive_cfg["FL"].x*twist.angular.z
wheelcommand = {'steering':{}, 'speed':{}}
wheelcommand['steering']['RL'] = atan2(vy_r,vx_r)
wheelcommand['steering']['RR'] = atan2(vy_r,vx_r)
......@@ -77,6 +77,7 @@ class RoverKinematics:
# each wheel in skid-steer mode
vx = twist.linear.x - twist.angular.z*drive_cfg[k].y
vy = twist.linear.y + twist.angular.z*drive_cfg[k].x
# if -pi/2 <= atan2(vy, vx) <= pi/2:
motors.steering[k] = atan2(vy, vx) # comm['steering'][k]
motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k]
else:
......@@ -85,6 +86,7 @@ class RoverKinematics:
# each wheel in rolling-without-slipping mode
vx = twist.linear.x - twist.angular.z*drive_cfg[k].y
vy = twist.linear.y + twist.angular.z*drive_cfg[k].x
# if -pi/2 <= atan2(vy, vx) <= pi/2:
motors.steering[k] = atan2(vy, vx) # comm['steering'][k] #
motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k] #
return motors
......@@ -96,10 +98,19 @@ class RoverKinematics:
self.first_run = False
return self.X
# Insert here your odometry code
self.X[0,0] += (drive_cfg['FR'].x + drive_cfg['FL'].x + drive_cfg['RL'].x + drive_cfg['RR'].x)/4
self.X[1,0] += (drive_cfg['FR'].y + drive_cfg['FL'].y + drive_cfg['RL'].y + drive_cfg['RR'].y)/4
self.X[2,0] += (drive_cfg['FR'].radius + drive_cfg['FL'].radius + drive_cfg['RL'].radius + drive_cfg['RR'].radius)/4
A_ = []
B_ = []
for k in drive_cfg.keys():
A_.append([1, 0, -2*drive_cfg[k].radius])
B_.append([motor_state.drive[k]*cos(motor_state.steering[k])])
A_.append([0, 1, 2*drive_cfg[k].radius])
B_.append([motor_state.drive[k]*sin(motor_state.steering[k])])
A = numpy.array(A_)
B = numpy.array(B_)
self.X += pinv(A).dot(B)
# self.X[0,0] += X_[0]
# self.X[1,0] += X_[1]
# self.X[2,0] += X_[2]
self.motor_state.copy(motor_state)
return self.X
......
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