Commit bc6974fc by Mugisha David

Part 1

parent d434dfba
 ... ... @@ -17,7 +17,14 @@ Here is a quick representation of the reduction, with `x` the origin of the robo | | | o---o o ``` The rover cannot turn onto itself with such configuration. The rover cannot turn effectively on itself with that configuration. As matter of fact, applying a full right command (or left) would result for the wheel to have 90 degrees position, which is valid for a three wheeled vehicles whose wheels are aligned with the frame origin. Illustrations ![Grouped wheels](url) ![Decoupled wheels](url) The second approach is more thorough and consider each wheel without any reduction step. ... ...

197 KB

 ... ... @@ -34,57 +34,59 @@ class RoverKinematics: self.motor_state = RoverMotors() self.first_run = True def speed_steering_distribution(self, twist, driver_cfg): def speed_steering_distribution(self, twist, drive_cfg): """ Computing steering and radial speed from velocity if we group wheels by pairs. Args: twist(Twist): Data twist(Twist): Twist command, converted for example from joystick Returns: Dictionnary for each wheel Dictionnary for each wheel steering and speed """ vx_r = twist.linear.x vy_r = twist.linear.y + driver_cfg['RL'].x*twist.angular.z vy_r = twist.linear.y + 0.5*(drive_cfg["RL"].x+drive_cfg["RR"].x)*twist.angular.z vx_c = vx_r vy_c = twist.linear.y + driver_cfg['CL'].x*twist.angular.z vy_c = twist.linear.y + 0.5*(drive_cfg["CL"].x+drive_cfg["CR"].x)*twist.angular.z vx_f = vx_r 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) data['steering']['CL'] = atan2(vy_c,vx_c) data['steering']['CR'] = atan2(vy_c,vx_c) data['steering']['FL'] = atan2(vy_f,vx_f) data['steering']['FR'] = atan2(vy_f,vx_f) data['speed']['RL'] = 10*hypot(vy_r,vx_r) data['speed']['RR'] = 10*hypot(vy_r,vx_r) data['speed']['CL'] = 10*hypot(vy_c,vx_c) data['speed']['CR'] = 10*hypot(vy_c,vx_c) data['speed']['FL'] = 10*hypot(vy_f,vx_f) data['speed']['FR'] = 10*hypot(vy_f,vx_f) return data vy_f = twist.linear.y + 0.5*(drive_cfg["FL"].x+drive_cfg["FR"].x)*twist.angular.z wheelcommand = {'steering':{}, 'speed':{}} wheelcommand['steering']['RL'] = atan2(vy_r,vx_r) wheelcommand['steering']['RR'] = atan2(vy_r,vx_r) wheelcommand['steering']['CL'] = atan2(vy_c,vx_c) wheelcommand['steering']['CR'] = atan2(vy_c,vx_c) wheelcommand['steering']['FL'] = atan2(vy_f,vx_f) wheelcommand['steering']['FR'] = atan2(vy_f,vx_f) wheelcommand['speed']['RL'] = 10*hypot(vy_r,vx_r) wheelcommand['speed']['RR'] = 10*hypot(vy_r,vx_r) wheelcommand['speed']['CL'] = 10*hypot(vy_c,vx_c) wheelcommand['speed']['CR'] = 10*hypot(vy_c,vx_c) wheelcommand['speed']['FL'] = 10*hypot(vy_f,vx_f) wheelcommand['speed']['FR'] = 10*hypot(vy_f,vx_f) return wheelcommand def twist_to_motors(self, twist, drive_cfg, skidsteer=False): motors = RoverMotors() # comm = self.speed_steering_distribution(twist, driver_cfg) comm = self.speed_steering_distribution(twist, drive_cfg) if skidsteer: for k in drive_cfg.keys(): # Insert here the steering and velocity of # each wheel in skid-steer mode motors.steering[k] = twist.angular.z motors.drive[k] = 2*twist.linear.x vx = twist.linear.x - twist.angular.z*drive_cfg[k].y vy = twist.linear.y + twist.angular.z*drive_cfg[k].x motors.steering[k] = atan2(vy, vx) # comm['steering'][k] motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k] else: for k in drive_cfg.keys(): # Insert here the steering and velocity of # each wheel in rolling-without-slipping mode 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) vx = twist.linear.x - twist.angular.z*drive_cfg[k].y vy = twist.linear.y + twist.angular.z*drive_cfg[k].x motors.steering[k] = atan2(vy, vx) # comm['steering'][k] # motors.drive[k] = 10*hypot(vy, vx) # comm['speed'][k] # return motors def integrate_odometry(self, motor_state, drive_cfg): ... ...