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):
print 'DynamicMoveToPoint', 'state=', state, 'inp=', inp
assert isinstance(inp,tuple), 'inp should be a tuple'
assert len(inp) == 2, 'inp should be of length 2'
assert isinstance(inp[0],util.Point), 'inp[0] should be a Point'
goal_point, sensors = inp
pose = sensors.odometry
goal_theta = util.fixAngle02Pi(pose.point().angleTo(goal_point))
if not util.nearAngle(goal_theta, pose.theta, 0.04):
print "Goal theta is", goal_theta
print "Pose theta is", pose.theta
if is_between(goal_theta, pose.theta, pose.theta + math.pi):
return (state, io.Action(rvel=0.03, fvel=0.0))
else:
return (state, io.Action(rvel=-0.03, fvel=0.0))
return (state, io.Action(rvel=0.00, fvel=pose.point().distance(goal_point)))
def step():
global i
x, y, theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
currentPoint = util.Point(x,y).add(robot.initialLocation)
currentAngle = theta
destinationPoint = robot.path[i]
thetad = currentPoint.angleTo(destinationPoint)
if util.nearAngle(currentAngle,thetad,math.pi/180.0):
io.setForward(0.1)
io.setRotational(0)
if currentPoint.distance(destinationPoint) < 0.02:
i += 1
print i
else:
theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle)
io.setRotational(theta_constant)
io.setForward(0)
def step():
global i
x, y, theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
currentPoint = util.Point(x, y).add(robot.initialLocation)
currentAngle = theta
destinationPoint = robot.path[i]
thetad = currentPoint.angleTo(destinationPoint)
if util.nearAngle(currentAngle, thetad, math.pi / 180.0):
io.setForward(0.1)
io.setRotational(0)
if currentPoint.distance(destinationPoint) < 0.02:
i += 1
print i
else:
theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle)
io.setRotational(theta_constant)
io.setForward(0)
def step():
if len(robot.path) == 0:
io.setForward(0)
io.setRotational(0)
return
x, y, theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
currentPoint = util.Point(x,y).add(robot.initialLocation)
currentAngle = theta
destinationPoint = robot.maze.indicesToPoint(robot.path[0])
desiredTheta = math.atan2(float(destinationPoint.y - currentPoint.y), float(destinationPoint.x - currentPoint.x))
if desiredTheta < 0:
desiredTheta += math.pi * 2
if util.nearAngle(desiredTheta, theta, 0.1):
io.setForward(0.3)
io.setRotational(0)
else:
io.setForward(0)
if desiredTheta > theta:
if desiredTheta - theta < math.pi:
io.setRotational(.5)
else:
io.setRotational(-.5)
else:
if desiredTheta - theta < math.pi:
io.setRotational(-.5)
else:
io.setRotational(.5)
if closeToPoint(currentPoint, destinationPoint, 0.1):
if len(robot.path) > 0:
robot.path.pop(0)
