Commit 893d3426 by 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!