Commit 893d3426 authored by Mugisha David's avatar Mugisha David

Update, solving odometry

parent 4fcf1e06
......@@ -70,7 +70,7 @@ class RoverKinematics:
def twist_to_motors(self, twist, drive_cfg, skidsteer=False):
motors = RoverMotors()
comm = self.speed_steering_distribution(twist, drive_cfg)
# comm = self.speed_steering_distribution(twist, drive_cfg)
if skidsteer:
for k in drive_cfg.keys():
# Insert here the steering and velocity of
......@@ -86,9 +86,19 @@ 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] #
if -pi/2 <= atan2(vy, vx) <= pi/2:
motors.steering[k] = atan2(vy, vx) # comm['steering'][k] #
motors.drive[k] = 10*hypot(vy, vx)
elif pi/2 < atan2(vy, vx) <= pi:
motors.steering[k] = atan2(vy, vx) + pi # comm['steering'][k] #
motors.drive[k] = -10*hypot(vy, vx)
else:
motors.steering[k] = atan2(vy, vx) - pi
motors.drive[k] = -10*hypot(vy, vx)
# motors.steering[k] = atan2(vy, vx) # comm['steering'][k] #
# motors.drive[k] = 10*hypot(vy, vx)
return motors
def integrate_odometry(self, motor_state, drive_cfg):
......@@ -101,16 +111,22 @@ class RoverKinematics:
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])])
s_k = 0
beta_k = 0
if abs(motor_state.drive[k] - self.motor_state.drive[k]) > 5e-5:
s_k = motor_state.drive[k] - self.motor_state.drive[k]
if abs(motor_state.steering[k] - self.motor_state.steering[k]) > 5e-3:
beta_k = motor_state.steering[k] - self.motor_state.steering[k]
A_.append([1, 0, -drive_cfg[k].y])
B_.append([s_k*cos(beta_k)])
A_.append([0, 1, drive_cfg[k].x])
B_.append([s_k*sin(beta_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.X[0,0] = 0.0
# self.X[1,0] = 0.0
# self.X[2,0] = 0.0
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