def __init__(self,x=0, y=0, h=0): self.x = x self.y = y self.h = within_pi(h) # In case 'h' is confusing self.heading = self.h
def ALPHA(robot, goal): ''' Angle from the robot's heading to the vector from (robot location) to (goal location). Robot and goal poses should be given as cp.CartesianPose objects. ''' difference = goal - robot return within_pi(difference.theta())
def rotate(cp, angle): ''' 2D rotate <angle> radians around the origin. ''' c = cos(angle) s = sin(angle) x = cp.x*c - cp.y*s y = cp.x*s + cp.y*c h = within_pi(cp.h + angle) return CartesianPose(x,y,h)
def calculate_next_pose(old_pose, v, gamma, dt=1): ''' Calculate the next CartesianPose state of an object with a certain speed and steering angle. ''' # Calculate change in world state dx = dX(v=v, h=old_pose.h) dy = dY(v=v, h=old_pose.h) dh = dH(v=v, gamma=gamma, L=L) # Update world state new_pose = old_pose + CartesianPose(xyh=[dx, dy, dh]) * dt # Keep h in [-pi, pi] new_pose.h = within_pi(new_pose.h) return new_pose
def calculate_steering(self, speed, dt=1): ''' Use PID control to calculate PICAR_ANGLE and bound by picar limitations. ''' picar_angle = GAMMA(v=speed, alpha=self.picar.get_alpha(), beta=self.picar.get_beta(), alpha_controller=self.alpha_controller, beta_controller=self.beta_controller, L=self.picar.get_L(), dt=dt) # Check if no turn is required if picar_angle < 3: return 0 picar_angle *= self.picar.get_drive_direction() # Clip to max bound = self.picar._steering_picar_to_world(self.picar.MAX_PICAR_TURN) picar_angle = clip(picar_angle, -bound, bound) # Bound between [-pi, pi] return within_pi(picar_angle)
def __iadd__(self, cp): self.x = self.x+cp.x self.y = self.y+cp.y self.h = within_pi(self.h+cp.h) return self
def __radd__(self, cp): return CartesianPose (self.x+cp.x, self.y+cp.y, within_pi(self.h+cp.h))
def __add__(self, cp): return BicyclePose(self.rho + cp.rho, within_pi(self.alpha + cp.alpha), within_pi(self.beta + cp.beta))
def __mul__(self, k): return CartesianPose(k*self.x, k*self.y, within_pi(k*self.h))
def dALPHAdt(rho, alpha, speed, steer, direction=1): v = sign(direction) * abs(speed) return within_pi(v * sin(alpha) / rho - steer)
def __pow__(self, p): return BicyclePose(self.rho**p, within_pi(self.alpha**p), within_pi(self.beta**p))
def __mul__(self, k): return BicyclePose(k * self.rho, within_pi(k * self.alpha), within_pi(k * self.beta))
def __isub__(self, cp): self.rho = self.rho - cp.rho self.alpha = within_pi(self.alpha - cp.alpha) self.beta = within_pi(self.beta - cp.beta) return self
def __sub__(self, cp): return BicyclePose(self.rho - cp.rho, within_pi(self.alpha - cp.alpha), within_pi(self.beta - cp.beta))
def __iadd__(self, cp): self.rho = self.rho + cp.rho self.alpha = within_pi(self.alpha + cp.alpha) self.beta = within_pi(self.beta + cp.beta) return self
def __sub__(self, cp): return CartesianPose (self.x-cp.x, self.y-cp.y, within_pi(self.h-cp.h))
def __isub__(self, cp): self.x = self.x-cp.x self.y = self.y-cp.y self.h = within_pi(self.h-cp.h) return self
def dBETAdt(rho, alpha, speed, direction=1): v = sign(direction) * abs(speed) return within_pi(-v * cos(alpha) / rho)
def __pow__(self, p): return CartesianPose(self.x**p, self.y**p, within_pi(self.h**p))
def dHdt(speed, steer, L, direction=1): v = sign(direction) * abs(speed) return within_pi(v * tan(steer) / L)