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)
