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 getNextValues(self, state, inp):
currentTheta = inp.odometry.theta
if state == 'start':
print "Starting to rotate", self.headingDelta
# Compute a desired absolute heading by adding the desired
# delta to our current heading
thetaDesired = \
util.fixAnglePlusMinusPi(currentTheta + self.headingDelta)
else:
(thetaDesired, thetaLast) = state
newState = (thetaDesired, currentTheta)
# Rotate at a velocity proportional to angular error
# This sets the 'rvel' field in the action specification, and
# leaves the other fields at their default values
action = io.Action(rvel = util.clip(self.rotationalGain * \
util.fixAnglePlusMinusPi(thetaDesired - currentTheta),
-self.maxVel, self.maxVel))
return (newState, action)
def getNextValues(self, state, inp):
currentTheta = inp.odometry.theta
if state == 'start':
print "Starting to rotate", self.headingDelta
# Compute a desired absolute heading by adding the desired
# delta to our current heading
thetaDesired = \
util.fixAnglePlusMinusPi(currentTheta + self.headingDelta)
else:
(thetaDesired, thetaLast) = state
newState = (thetaDesired, currentTheta)
# Rotate at a velocity proportional to angular error
# This sets the 'rvel' field in the action specification, and
# leaves the other fields at their default values
action = io.Action(rvel = util.clip(self.rotationalGain * \
util.fixAnglePlusMinusPi(thetaDesired - currentTheta),
-self.maxVel, self.maxVel))
return (newState, action)
def actionToPose(goalPose, robotPose, forwardGain, rotationGain, maxVel, angleEps, distEps):
"""
Internal procedure that returns an action to take to drive
toward a specified goal pose.
"""
if robotPose.distance(goalPose) < distEps:
finalRotError = util.fixAnglePlusMinusPi(goalPose.theta - robotPose.theta)
return io.Action(rvel=finalRotError * rotationGain)
else:
return actionToPoint(goalPose.point(), robotPose, forwardGain, rotationGain, maxVel, angleEps)
def actionToPose(goalPose, robotPose, forwardGain, rotationGain,
maxVel, angleEps, distEps):
"""
Internal procedure that returns an action to take to drive
toward a specified goal pose.
"""
if robotPose.distance(goalPose) < distEps:
finalRotError = util.fixAnglePlusMinusPi(goalPose.theta -robotPose.theta)
return io.Action(rvel = finalRotError * rotationGain)
else:
return actionToPoint(goalPose.point(), robotPose, forwardGain,
rotationGain, maxVel, angleEps)
def getNextValues(self, state, inp):
"""
@param state: tuple of indices C{(ix, iy)} representing
robot's location in grid map
@param inp: an action, which is one of the legal inputs
@returns: C{(nextState, cost)}
"""
(ix, iy, angle) = state
(dx, dy) = inp
(newX, newY) = (ix + dx, iy + dy)
if not self.legal(ix, iy, newX, newY):
# Stay here; but every step has to have positive cost
return (state, 10)
else:
# compute the distance (sq) in meters
delta = math.sqrt((dx * self.theMap.xStep) ** 2 + (dy * self.theMap.yStep) ** 2)
target = math.atan2(dy, dx)
turn = abs(util.fixAnglePlusMinusPi(target - angle))
return ((newX, newY, target), delta + self.rotationCost * turn)
def getNextValues(self, state, inp):
"""
@param state: tuple of indices C{(ix, iy)} representing
robot's location in grid map
@param inp: an action, which is one of the legal inputs
@returns: C{(nextState, cost)}
"""
(ix, iy, angle) = state
(dx, dy) = inp
(newX, newY) = (ix + dx, iy + dy)
if not self.legal(ix, iy, newX, newY):
# Stay here; but every step has to have positive cost
return (state, 10)
else:
# compute the distance (sq) in meters
delta = math.sqrt((dx*self.theMap.xStep)**2 + \
(dy*self.theMap.yStep)**2)
target = math.atan2(dy, dx)
turn = abs(util.fixAnglePlusMinusPi(target - angle))
return ((newX, newY, target), delta + self.rotationCost * turn)
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)
