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 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)
