def actionToPoint(goalPoint, robotPose, forwardGain, rotationGain, maxVel, angleEps): """ Internal procedure that returns an action to take to drive toward a specified goal point. """ rvel = 0 fvel = 0 robotPoint = robotPose.point() # Compute the angle between the robot and the goal point headingTheta = robotPoint.angleTo(goalPoint) # Difference between the way the robot is currently heading and # the angle to the goal. This is an angular error relative to the # robot's current heading, in the range +pi to -pi. headingError = util.fixAnglePlusMinusPi(headingTheta - robotPose.theta) if util.nearAngle(robotPose.theta, headingTheta, angleEps): # The robot is pointing toward the goal, so it's okay to move # forward. Note that we are actually doing two proportional # controllers simultaneously: one to reduce angular error # and one to reduce distance error. distToGoal = robotPoint.distance(goalPoint) fvel = distToGoal * forwardGain rvel = headingError * rotationGain else: # The robot is not pointing close enough to the goal, so don't # start moving foward yet. This is a proportional controller # to reduce angular error. rvel = headingError * rotationGain return io.Action(fvel = util.clip(fvel, -maxVel, maxVel), rvel = util.clip(rvel, -maxVel, maxVel))
def actionToPoint(goalPoint, robotPose, forwardGain, rotationGain, maxVel, angleEps): """ Internal procedure that returns an action to take to drive toward a specified goal point. """ rvel = 0 fvel = 0 robotPoint = robotPose.point() # Compute the angle between the robot and the goal point headingTheta = robotPoint.angleTo(goalPoint) # Difference between the way the robot is currently heading and # the angle to the goal. This is an angular error relative to the # robot's current heading, in the range +pi to -pi. headingError = util.fixAnglePlusMinusPi(headingTheta - robotPose.theta) if util.nearAngle(robotPose.theta, headingTheta, angleEps): # The robot is pointing toward the goal, so it's okay to move # forward. Note that we are actually doing two proportional # controllers simultaneously: one to reduce angular error # and one to reduce distance error. distToGoal = robotPoint.distance(goalPoint) fvel = distToGoal * forwardGain rvel = headingError * rotationGain else: # The robot is not pointing close enough to the goal, so don't # start moving foward yet. This is a proportional controller # to reduce angular error. rvel = headingError * rotationGain return io.Action(fvel=util.clip(fvel, -maxVel, maxVel), rvel=util.clip(rvel, -maxVel, maxVel))
def getNextValues(self, state, inp): (goalPoint, sensors) = inp robotPose = sensors.odometry robotPoint = robotPose.point() robotTheta = robotPose.theta nearGoal = robotPoint.isNear(goalPoint, self.distEps) headingTheta = robotPoint.angleTo(goalPoint) r = robotPoint.distance(goalPoint) if nearGoal: # At the right place, so do nothing a = io.Action() elif util.nearAngle(robotTheta, headingTheta, self.angleEps): # Pointing in the right direction, so move forward a = io.Action( fvel=util.clip(r * self.forwardGain, -self.maxFVel, self.maxFVel)) else: # Rotate to point toward goal headingError = util.fixAnglePlusMinusPi(headingTheta - robotTheta) a = io.Action( rvel=util.clip(headingError * self.rotationGain, -self.maxRVel, self.maxRVel)) return (nearGoal, a)
def done(self, state): if state == 'start': return False else: (thetaDesired, thetaLast) = state # We're done if the desired heading is within epsilon of our # current heading return util.nearAngle(thetaDesired, thetaLast, self.angleEpsilon)
def getNextValues(self, state, inp): (goalPoint, sensors) = inp robotPose = sensors.odometry robotPoint = robotPose.point() robotTheta = robotPose.theta nearGoal = robotPoint.isNear(goalPoint, self.distEps) headingTheta = robotPoint.angleTo(goalPoint) r = robotPoint.distance(goalPoint) if nearGoal: # At the right place, so do nothing a = io.Action() elif util.nearAngle(robotTheta, headingTheta, self.angleEps): # Pointing in the right direction, so move forward a = io.Action(fvel = util.clip(r * self.forwardGain, -self.maxFVel, self.maxFVel)) else: # Rotate to point toward goal headingError = util.fixAnglePlusMinusPi(headingTheta - robotTheta) a = io.Action(rvel = util.clip(headingError * self.rotationGain, -self.maxRVel, self.maxRVel)) return (nearGoal, a)