def __init__(self): # Task parameters self.line_begin = Vector(-2, -4) self.line_end = Vector(2, 5) self.line = Vector(self.line_end[0] - self.line_begin[0], self.line_end[1] - self.line_begin[1]) self.position = Vector(0, -6) self.heading = math.pi / 6 # Robot parameters self.linear_velocity = 0.0 self.angular_velocity = 0.0 self.max_linear_velocity = 2 #m/s self.max_angular_velocity = 2 #rad/s # General planner parameters self.target_radius = 0.1 self.max_angle_error = 0.1 self.retarder = 0.1 # Line planner parameters self.rabbit_factor = 0.2 # Position planner parameters self.linear_scale_factor = 1 self.angular_scale_factor = 2 # Simulation parameters self.max_steps = 1000 self.stepsize = 0.1 # sec/step # Init simulation self.steps = 0 self.lin_vel = [] self.ang_vel = [] # init plot self.plot = Vectorplot(-10, 10, -10, 10) # self.plot.addPoint(self.line_begin) # self.plot.addPoint(self.line_end) # self.plot.addLine(self.line_begin,self.line_end) # init position planner self.destination = Vector(self.line_end[0], self.line_end[1]) path = self.destination - Vector(self.position[0], self.position[1]) self.distance_error = path.length() self.angle_error = 0.0
def __init__(self): # Task parameters self.line_begin = Vector(-2,-4) self.line_end = Vector(2,5) self.line = Vector(self.line_end[0]-self.line_begin[0],self.line_end[1]-self.line_begin[1]) self.position = Vector(0,-6) self.heading = math.pi/6 # Robot parameters self.linear_velocity = 0.0 self.angular_velocity = 0.0 self.max_linear_velocity = 2 #m/s self.max_angular_velocity = 2 #rad/s # General planner parameters self.target_radius = 0.1 self.max_angle_error = 0.1 self.retarder = 0.1 # Line planner parameters self.rabbit_factor = 0.2 # Position planner parameters self.linear_scale_factor = 1 self.angular_scale_factor = 2 # Simulation parameters self.max_steps = 1000 self.stepsize = 0.1 # sec/step # Init simulation self.steps = 0 self.lin_vel = [] self.ang_vel = [] # init plot self.plot = Vectorplot(-10, 10, -10, 10) # self.plot.addPoint(self.line_begin) # self.plot.addPoint(self.line_end) # self.plot.addLine(self.line_begin,self.line_end) # init position planner self.destination = Vector(self.line_end[0],self.line_end[1]) path = self.destination - Vector(self.position[0], self.position[1]) self.distance_error = path.length() self.angle_error = 0.0
class Planner(): def __init__(self): # Task parameters self.line_begin = Vector(-2,-4) self.line_end = Vector(2,5) self.line = Vector(self.line_end[0]-self.line_begin[0],self.line_end[1]-self.line_begin[1]) self.position = Vector(0,-6) self.heading = math.pi/6 # Robot parameters self.linear_velocity = 0.0 self.angular_velocity = 0.0 self.max_linear_velocity = 2 #m/s self.max_angular_velocity = 2 #rad/s # General planner parameters self.target_radius = 0.1 self.max_angle_error = 0.1 self.retarder = 0.1 # Line planner parameters self.rabbit_factor = 0.2 # Position planner parameters self.linear_scale_factor = 1 self.angular_scale_factor = 2 # Simulation parameters self.max_steps = 1000 self.stepsize = 0.1 # sec/step # Init simulation self.steps = 0 self.lin_vel = [] self.ang_vel = [] # init plot self.plot = Vectorplot(-10, 10, -10, 10) # self.plot.addPoint(self.line_begin) # self.plot.addPoint(self.line_end) # self.plot.addLine(self.line_begin,self.line_end) # init position planner self.destination = Vector(self.line_end[0],self.line_end[1]) path = self.destination - Vector(self.position[0], self.position[1]) self.distance_error = path.length() self.angle_error = 0.0 def updatePos(self): self.heading = self.heading + (self.angular_velocity*self.stepsize); self.position[0] = self.position[0] + (self.linear_velocity*self.stepsize) * math.cos(self.heading) self.position[1] = self.position[1] + (self.linear_velocity*self.stepsize) * math.sin(self.heading) def step(self): # self.positionPlanner(self.line_end) self.linePlanner() self.steps = self.steps + 1 self.lin_vel.append(math.fabs(self.linear_velocity)) self.ang_vel.append(math.fabs(self.angular_velocity)) # if not self.steps%20 : # self.plot.addVector(self.position,Vector(math.cos(self.heading),math.sin(self.heading))) def targetReached(self): return math.sqrt(math.pow(self.line_end[0]-self.position[0],2) + math.pow(self.line_end[1]-self.position[1],2)) < self.target_radius def spin(self): self.plot.addVector(self.position,Vector(math.cos(self.heading),math.sin(self.heading))) while not self.targetReached() and self.steps < self.max_steps: self.plot.addPose(self.position) self.step() self.updatePos() self.plot.addPose(self.position) def show(self): # print(sum(self.lin_vel)/len(self.lin_vel)) # print(sum(self.ang_vel)/len(self.ang_vel)) self.plot.addVector(self.position,Vector(math.cos(self.heading),math.sin(self.heading))) self.plot.addPoint(self.position) self.plot.show() def positionPlanner(self,goal): self.destination[0] = goal[0] self.destination[1] = goal[1] # Construct desired path vector and calculate distance error path = self.destination - Vector(self.position[0], self.position[1]) # Calculate distance error self.distance_error = path.length() # Construct heading vector head = Vector(math.cos(self.heading), math.sin(self.heading)) # Calculate angle between heading vector and path vector self.angle_error = head.angle(path) # Rotate the heading vector according to the calculated angle and test correspondence # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap. t1 = head.rotate(self.angle_error) if path.angle(t1) != 0 : self.angle_error = -self.angle_error # Generate twist from distance and angle errors (For now simply 1:1) self.linear_velocity = self.distance_error * self.linear_scale_factor self.angular_velocity = self.angle_error * self.angular_scale_factor if math.fabs(self.angle_error) > self.max_angle_error : self.linear_velocity *= self.retarder # Implement maximum linear_velocity velocity and maximum angular_velocity velocity if self.linear_velocity > self.max_linear_velocity: self.linear_velocity = self.max_linear_velocity if self.linear_velocity < -self.max_linear_velocity: self.linear_velocity = -self.max_linear_velocity if self.angular_velocity > self.max_angular_velocity: self.angular_velocity = self.max_angular_velocity if self.angular_velocity < -self.max_angular_velocity: self.angular_velocity = -self.max_angular_velocity def linePlanner(self): # Project position vector on line vector proj = (self.position - self.line_begin).projectedOn(self.line) perp = proj - self.position if perp.length() : self.rabbit_factor = 1/perp.length() # Construct rabbit point rabbit = self.line - proj rabbit = rabbit.scale(self.rabbit_factor) rabbit += self.line_begin rabbit += proj # Call positionPlanner # self.plot.addPoint(rabbit) self.positionPlanner(rabbit) def test(self): self.position = Vector(-8,4) self.heading = Vector(0,1) self.plot.addPoint(self.position) for i in range(8) : (next,next_next) = self.addRightTurn(self.position,self.heading) self.plot.addLine(self.position,next) self.plot.addLine(next,next_next) self.plot.addPoint(next) self.plot.addPoint(next_next) self.heading = -self.heading self.position = next_next (next,next_next) = self.addLeftTurn(self.position,self.heading) self.plot.addLine(self.position,next) self.plot.addLine(next,next_next) self.plot.addPoint(next) self.plot.addPoint(next_next) self.heading = -self.heading self.position = next_next # self.plot.addVector(next_point,next_heading) self.plot.show() def addRightTurn(self,position,heading): next_point = position + heading.hat().unit().scale(1) next_heading = -heading next_next_point = next_point + next_heading.unit().scale(8) return(next_point,next_next_point) def addLeftTurn(self,position,heading): next_point = position - heading.hat().unit().scale(1) next_heading = -heading next_next_point = next_point + next_heading.unit().scale(8) return(next_point,next_next_point)
class Planner(): def __init__(self): # Task parameters self.line_begin = Vector(-2, -4) self.line_end = Vector(2, 5) self.line = Vector(self.line_end[0] - self.line_begin[0], self.line_end[1] - self.line_begin[1]) self.position = Vector(0, -6) self.heading = math.pi / 6 # Robot parameters self.linear_velocity = 0.0 self.angular_velocity = 0.0 self.max_linear_velocity = 2 #m/s self.max_angular_velocity = 2 #rad/s # General planner parameters self.target_radius = 0.1 self.max_angle_error = 0.1 self.retarder = 0.1 # Line planner parameters self.rabbit_factor = 0.2 # Position planner parameters self.linear_scale_factor = 1 self.angular_scale_factor = 2 # Simulation parameters self.max_steps = 1000 self.stepsize = 0.1 # sec/step # Init simulation self.steps = 0 self.lin_vel = [] self.ang_vel = [] # init plot self.plot = Vectorplot(-10, 10, -10, 10) # self.plot.addPoint(self.line_begin) # self.plot.addPoint(self.line_end) # self.plot.addLine(self.line_begin,self.line_end) # init position planner self.destination = Vector(self.line_end[0], self.line_end[1]) path = self.destination - Vector(self.position[0], self.position[1]) self.distance_error = path.length() self.angle_error = 0.0 def updatePos(self): self.heading = self.heading + (self.angular_velocity * self.stepsize) self.position[0] = self.position[0] + ( self.linear_velocity * self.stepsize) * math.cos(self.heading) self.position[1] = self.position[1] + ( self.linear_velocity * self.stepsize) * math.sin(self.heading) def step(self): # self.positionPlanner(self.line_end) self.linePlanner() self.steps = self.steps + 1 self.lin_vel.append(math.fabs(self.linear_velocity)) self.ang_vel.append(math.fabs(self.angular_velocity)) # if not self.steps%20 : # self.plot.addVector(self.position,Vector(math.cos(self.heading),math.sin(self.heading))) def targetReached(self): return math.sqrt( math.pow(self.line_end[0] - self.position[0], 2) + math.pow(self.line_end[1] - self.position[1], 2)) < self.target_radius def spin(self): self.plot.addVector( self.position, Vector(math.cos(self.heading), math.sin(self.heading))) while not self.targetReached() and self.steps < self.max_steps: self.plot.addPose(self.position) self.step() self.updatePos() self.plot.addPose(self.position) def show(self): # print(sum(self.lin_vel)/len(self.lin_vel)) # print(sum(self.ang_vel)/len(self.ang_vel)) self.plot.addVector( self.position, Vector(math.cos(self.heading), math.sin(self.heading))) self.plot.addPoint(self.position) self.plot.show() def positionPlanner(self, goal): self.destination[0] = goal[0] self.destination[1] = goal[1] # Construct desired path vector and calculate distance error path = self.destination - Vector(self.position[0], self.position[1]) # Calculate distance error self.distance_error = path.length() # Construct heading vector head = Vector(math.cos(self.heading), math.sin(self.heading)) # Calculate angle between heading vector and path vector self.angle_error = head.angle(path) # Rotate the heading vector according to the calculated angle and test correspondence # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap. t1 = head.rotate(self.angle_error) if path.angle(t1) != 0: self.angle_error = -self.angle_error # Generate twist from distance and angle errors (For now simply 1:1) self.linear_velocity = self.distance_error * self.linear_scale_factor self.angular_velocity = self.angle_error * self.angular_scale_factor if math.fabs(self.angle_error) > self.max_angle_error: self.linear_velocity *= self.retarder # Implement maximum linear_velocity velocity and maximum angular_velocity velocity if self.linear_velocity > self.max_linear_velocity: self.linear_velocity = self.max_linear_velocity if self.linear_velocity < -self.max_linear_velocity: self.linear_velocity = -self.max_linear_velocity if self.angular_velocity > self.max_angular_velocity: self.angular_velocity = self.max_angular_velocity if self.angular_velocity < -self.max_angular_velocity: self.angular_velocity = -self.max_angular_velocity def linePlanner(self): # Project position vector on line vector proj = (self.position - self.line_begin).projectedOn(self.line) perp = proj - self.position if perp.length(): self.rabbit_factor = 1 / perp.length() # Construct rabbit point rabbit = self.line - proj rabbit = rabbit.scale(self.rabbit_factor) rabbit += self.line_begin rabbit += proj # Call positionPlanner # self.plot.addPoint(rabbit) self.positionPlanner(rabbit) def test(self): self.position = Vector(-8, 4) self.heading = Vector(0, 1) self.plot.addPoint(self.position) for i in range(8): (next, next_next) = self.addRightTurn(self.position, self.heading) self.plot.addLine(self.position, next) self.plot.addLine(next, next_next) self.plot.addPoint(next) self.plot.addPoint(next_next) self.heading = -self.heading self.position = next_next (next, next_next) = self.addLeftTurn(self.position, self.heading) self.plot.addLine(self.position, next) self.plot.addLine(next, next_next) self.plot.addPoint(next) self.plot.addPoint(next_next) self.heading = -self.heading self.position = next_next # self.plot.addVector(next_point,next_heading) self.plot.show() def addRightTurn(self, position, heading): next_point = position + heading.hat().unit().scale(1) next_heading = -heading next_next_point = next_point + next_heading.unit().scale(8) return (next_point, next_next_point) def addLeftTurn(self, position, heading): next_point = position - heading.hat().unit().scale(1) next_heading = -heading next_next_point = next_point + next_heading.unit().scale(8) return (next_point, next_next_point)