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