Commit dda7221f by 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 ... ...
File deleted
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!