Skip to content
Browse files

updating location to master

  • Loading branch information...
2 parents ed080c7 + edc6dfb commit bb836f58861095302c7c524955fb10b60b7dc020 Andrew Lawrence committed Apr 16, 2010
Showing with 3,154 additions and 2,613 deletions.
  1. +8 −1 Makefile
  2. +41 −10 TODO.org
  3. +2 −0 cmake/base_definitions.cmake
  4. +0 −1 include/Common.h
  5. +0 −1 include/debug.h
  6. +0 −66 include/ifdefs.h
  7. +1 −1 noggin/Brain.py
  8. +5 −6 noggin/EKF.h
  9. +0 −1 noggin/FallStates.py
  10. +65 −28 noggin/LocEKF.cpp
  11. +4 −0 noggin/LocEKF.h
  12. +0 −424 noggin/NavStates.py
  13. +0 −213 noggin/Navigator.py
  14. +7 −7 noggin/Noggin.cpp
  15. +1 −1 noggin/NogginConstants.py
  16. +1 −1 noggin/NogginStructs.h
  17. +1 −2 noggin/Observation.cpp
  18. +1 −3 noggin/cmake.noggin/CMakeLists.txt
  19. +41 −0 noggin/headTracking/BasicStates.py
  20. +7 −1 noggin/headTracking/HeadTracking.py
  21. +1 −1 noggin/headTracking/HeadTrackingHelper.py
  22. +12 −4 noggin/headTracking/PanningStates.py
  23. +1 −28 noggin/headTracking/TrackingStates.py
  24. +24 −0 noggin/navigator/ChaseStates.py
  25. +1 −1 noggin/{ → navigator}/NavConstants.py
  26. +140 −0 noggin/navigator/NavHelper.py
  27. +1 −0 noggin/navigator/NavMath.py
  28. +268 −0 noggin/navigator/NavStates.py
  29. +130 −0 noggin/navigator/Navigator.py
  30. +1 −0 noggin/navigator/__init__.py
  31. +4 −4 noggin/offline/fakerIterators.cpp
  32. +16 −11 noggin/players/BrunswickStates.py
  33. +13 −17 noggin/players/ChaseBallStates.py
  34. +4 −7 noggin/players/ChaseBallTransitions.py
  35. +0 −1 noggin/players/DataStates.py
  36. +6 −11 noggin/players/FindBallStates.py
  37. +15 −14 noggin/players/GoaliePositionStates.py
  38. +2 −2 noggin/players/GoalieTransitions.py
  39. +8 −6 noggin/players/KickingConstants.py
  40. +9 −8 noggin/players/KickingHelpers.py
  41. +13 −32 noggin/players/KickingStates.py
  42. +13 −13 noggin/players/MotionStates.py
  43. +24 −22 noggin/players/PositionStates.py
  44. +53 −75 noggin/players/SoccerFSA.py
  45. +2 −2 noggin/players/SquatPositionStates.py
  46. +1 −1 noggin/players/WalkTestStates.py
  47. +15 −25 noggin/players/pBrunswick.py
  48. +3 −7 noggin/typeDefs/Ball.py
  49. +0 −2 noggin/typeDefs/Landmarks.py
  50. +121 −0 noggin/typeDefs/Location.py
  51. +3 −3 noggin/typeDefs/MyInfo.py
  52. +7 −7 noggin/typeDefs/TeamMember.py
  53. +3 −1 noggin/typeDefs/VisualObject.py
  54. +0 −48 noggin/util/MyMath.py
  55. +1 −1 vision/Ball.cpp
  56. +0 −2 vision/Ball.h
  57. +0 −1 vision/Blob.h
  58. +0 −2 vision/Blobs.h
  59. +311 −184 vision/ConcreteCorner.cpp
  60. +65 −59 vision/ConcreteCorner.h
  61. +246 −113 vision/ConcreteLine.cpp
  62. +44 −27 vision/ConcreteLine.h
  63. +3 −3 vision/Cross.cpp
  64. +6 −7 vision/Field.cpp
  65. +890 −836 vision/FieldLines.cpp
  66. +106 −72 vision/FieldLines.h
  67. +0 −2 vision/ObjectFragments.h
  68. +21 −21 vision/PyVision.cpp
  69. +2 −2 vision/PyVision.h
  70. +0 −1 vision/Threshold.cpp
  71. +14 −27 vision/Utility.cpp
  72. +2 −6 vision/Utility.h
  73. +6 −6 vision/Vision.cpp
  74. +1 −1 vision/Vision.h
  75. +0 −1 vision/VisualBall.h
  76. +174 −24 vision/VisualCorner.cpp
  77. +41 −22 vision/VisualCorner.h
  78. +6 −1 vision/VisualCross.cpp
  79. +1 −2 vision/VisualCross.h
  80. +5 −0 vision/VisualFieldObject.cpp
  81. +1 −0 vision/VisualFieldObject.h
  82. +2 −1 vision/VisualLandmark.h
  83. +90 −56 vision/VisualLine.cpp
  84. +12 −13 vision/VisualLine.h
  85. +5 −0 vision/cmake.vision/buildconfig.cmake
View
9 Makefile
@@ -116,4 +116,11 @@ clean:
@$(MAKE) -C comm clean
$(RM) -r install/* $(PYC_FILES)
-
+check:
+ @if [ -e $(CROSS_FILE) ]; then \
+ echo "Making for CROSS"; \
+ elif [ -e $(STRAIGHT_FILE) ]; then \
+ echo "Making for STRAIGHT"; \
+ elif [ -e $(WEBOTS_FILE) ]; then \
+ echo "Making for WEBOTS"; \
+ fi
View
51 TODO.org
@@ -113,25 +113,56 @@
** Vision
*** TODO Opponent recognition (also with sonar)
*** TODO Open goal detection
-*** TODO Improve lines
-
+*** TODO Improve FieldLines
+**** TODO Implement TOOL-side regression testing in learning modul
+**** Corners
+***** DONE Check edge detection
+***** TODO [#A] Improve green between line endpoints and corner sanity check
+***** TODO Improve CC detection
+****** TODO Improve intersection between endpoints detection
+ Possibly: augment simple between(intersection,endpoint1,endpoint2) with info about where the visible line ends, how many points on each side of it.
+****** TODO When detecting a CC, label the line as CC-Lines
+***** TODO Improve T detection
+ Some of CC detection could be applied here
+***** TODO Improve T classification
+****** Use length of bar and stem
+ Bottom/top T corners will have very long stem and bar. Either means it's a bottom/top.
+***** DONE Use some ambiguous landmarks to ID corners
+ A goal post can at least narrow down the choices.
+***** TODO Use other VisualCorners to identify corners
+ If there is an L corner near a T corner, then the T-corner is a goal T corner
+***** TODO Fix CC-unused point identification
+ Sees far too many CC corners because of stray points. Maybe check on opposite side of T-intersection for lots of white/try and create line points on opposite side (maybe in tighter configuration).
+***** TODO Fix/remove field angle sanity check
+ Possibly just broaden it significantly
+***** TODO Set shape when setting possible corners
+***** TODO Loop through and remark all CC-intersections given other corners
+**** Lines
+***** TODO Better use corner information to identify lines
+***** TODO Identify lines without corners
+***** TODO Extend lines?
+ Maybe scan off the end of a line for more white, then try to make more line points, tightly and add them to the line. Debug current system.
** Localization
-*** TODO Improve Kalman filter
-**** Investigate using unscented or multi-modal filter
-**** TODO Improve handling of unexpected observations
+*** TODO [#B] Improve Kalman filter
+**** TODO [#B] Investigate using unscented or multi-modal filter
+**** TODO [#B] Improve handling of unexpected observations
Could turn it off or use a counter for when to use observations again.
-**** TODO Move from boost/UBLAS to the Eigen matrix library.
-*** TODO Do ball covariance differently / landmark cartesian coordinates
+**** TODO [#C] Move from boost/UBLAS to the Eigen matrix library.
+*** DONE Do ball covariance differently / landmark cartesian coordinates
There will be comments in the LocEKF and BallEKF describing this
*** TODO Investigate sensor based odometry
+*** TODO [#B] Test/improve odometry
+ Take a robot with a blank color table, have it walk forward for a set distance. See what the EKF gives and compare it to the truth value.
*** TODO Topological localization
-*** TODO Teammate localization
-*** TODO Opponent localization
+*** TODO [#C] Teammate localization
+*** TODO [#C] Opponent localization
Each opponent can be treated the same way as the ball. The code from the BallEKF should work fine, the one difference is that we need to do data association between the observations and the different robot estimates. Said otherwise, we have to track four opponent robots and we need to have some way of matching the observation to the correct of the four robots. First try is nearest neighbor. The literature has many other solutions. If people need help send Tucker an email and he can give you some jumping off papers.
-*** TODO Better ambiguous landmark usage
+*** TODO [#A] Better ambiguous landmark usage
**** Use a joint probability for all landmarks, instead of a NN association for each landmark independently
**** Use a multi-modal kalman filter
+**** TODO Use L-Corner shape to identify ambiguous corners.
+ If we're not in the goalbox, field corners should look very different than other ones.
** Behaviors
*** TODO [#A] Replace hand coded kick decision with computational best kick choice.
View
2 cmake/base_definitions.cmake
@@ -34,6 +34,8 @@ IF( "x$ENV{AL_DIR}x" STREQUAL "xx")
SET( AL_DIR "/usr/local/nao-1.6" )
ENDIF (WEBOTS_BACKEND)
SET( ENV{AL_DIR} ${AL_DIR} )
+ MESSAGE( STATUS
+ "Environment variable AL_DIR was not set, reseting to default ${AL_DIR}!" )
ELSE( "x$ENV{AL_DIR}x" STREQUAL "xx")
SET( AL_DIR $ENV{AL_DIR} )
ENDIF( "x$ENV{AL_DIR}x" STREQUAL "xx")
View
1 include/Common.h
@@ -6,7 +6,6 @@
#define Common_h_DEFINED
#include <math.h> // for PI
-#include "ifdefs.h"
#include "NBMath.h"
// ROBOT TYPES
View
1 include/debug.h
@@ -1,7 +1,6 @@
#ifndef DEBUG_H_DEFINED
#define DEBUG_H_DEFINED
-#include "ifdefs.h"
#include "Common.h"
#if !defined(OFFLINE) && ROBOT(AIBO)
View
66 include/ifdefs.h
@@ -1,66 +0,0 @@
-
-//
-// ifdefs.h
-// All macros defined in this file are/should be compile-time switches, for
-// ease of use and change on the robot (ex. Compiling for different robots, or
-// online versus offline). As such, all macros are also required to check for
-// redefinition (i.e. via ifndef) so as to allow command-line specified
-// definitions to take priority.
-//
-
-
-// Offline switch
-#ifndef OFFLINE
-//# define OFFLINE
-#endif
-
-// Cortex 2.0 -- not used
-#if defined(OFFLINE) && !defined(CORTEX__2_0)
-//# define CORTEX__2_0
-#endif
-
-
-//-----System Switches (uncomment the define to activate the system)----//
-// Turn on/off the entire Vision-processing system
-#ifndef USE_VISION
-//# define USE_VISION //js - should be set with cmake now
-#endif
-// it's baaack. due to battery life, here's an easy ifdef to enable/disable
-// Python
-//#ifndef USE_PYTHON
-//# define USE_PYTHON
-//#endif
-// Use the old, small calibration tables
-#ifndef SMALL_TABLES
-//# define SMALL_TABLES
-#endif
-
-#ifndef USE_TOOL_CONNECT
-//# define USE_TOOL_CONNECT
-#endif
-
-// Switches for debugging purposes
-
-
-#ifndef USE_TIME_PROFILING
-//# define USE_TIME_PROFILING
-//# define USE_PROFILER_AUTO_PRINT
-#endif
-#ifndef FRAME_BY_FRAME_PROFILING
-//# define FRAME_BY_FRAME_PROFILING
-#endif
-#ifndef USE_JPEG
-//# define USE_JPEG
-#endif
-#ifndef USE_AES
-//# define USE_AES
-#endif
-#ifndef NEW_LOGGING
-//# define NEW_LOGGING
-#endif
-
-// Other switches
-
-#ifndef USE_FAST_WALK
-# define USE_FAST_WALK
-#endif
View
2 noggin/Brain.py
@@ -15,7 +15,7 @@
from . import GameController
from . import FallController
from .headTracking import HeadTracking
-from . import Navigator
+from .navigator import Navigator
from .util import NaoOutput
from . import NogginConstants as Constants
from .typeDefs import (MyInfo, Ball, Landmarks, Sonar, Packet, Play, TeamMember)
View
11 noggin/EKF.h
@@ -187,8 +187,8 @@ class EKF
// Kalman gain matrix
StateMeasurementMatrix K_k =
boost::numeric::ublas::scalar_matrix<float>(numStates,
- measurementSize,
- 0.0f);
+ measurementSize,
+ 0.0f);
// Observation jacobian
StateMeasurementMatrix H_k =
boost::numeric::ublas::scalar_matrix<float>(measurementSize,
@@ -207,15 +207,14 @@ class EKF
continue;
}
// Calculate the Kalman gain matrix
- StateMeasurementMatrix pTimesHTrans = prod(P_k_bar, trans(H_k));
+ const StateMeasurementMatrix pTimesHTrans = prod(P_k_bar, trans(H_k));
if(measurementSize == 2){
K_k = prod(pTimesHTrans,
NBMath::invert2by2(prod(H_k, pTimesHTrans) + R_k));
}else{
- MeasurementMatrix temp = prod(H_k, pTimesHTrans) + R_k;
- MeasurementMatrix inv =
- NBMath::solve(temp,
+ const MeasurementMatrix inv =
+ NBMath::solve(prod(H_k, pTimesHTrans) + R_k,
boost::numeric::ublas::identity_matrix<float>(
measurementSize));
K_k = prod(pTimesHTrans, inv);
View
1 noggin/FallStates.py
@@ -37,7 +37,6 @@ def standup(guard):
#guard.printf("standup angleY is "+str(inertial.angleY))
if guard.firstFrame():
- guard.brain.tracker.stopHeadMoves()
guard.brain.tracker.setNeutralHead()
# If on back, perform back stand up
View
93 noggin/LocEKF.cpp
@@ -239,24 +239,32 @@ void LocEKF::incorporateMeasurement(Observation z,
cout << "\t\t\tIncorporating measurement " << z << endl;
#endif
int obsIndex;
+
+ // Hack in here for if the vision system cannot identify this observation
+ // (no possible identities)
+ // @todo this should never happen, so fix vision system to stop it from happening.
+ if (z.getNumPossibilities() == 0){
+ R_k(0,0) = DONT_PROCESS_KEY;
+ return;
+ }
+
// Get the best fit for ambigious data
// NOTE: this is only done, if useAmbiguous is turned to true
- if (z.getNumPossibilities() > 1) {
+ else if (z.getNumPossibilities() > 1) {
obsIndex = findBestLandmark(&z);
- // No landmark is close enough, don't attempt to use one
- if (obsIndex < 0) {
- R_k(0,0) = DONT_PROCESS_KEY;
- return;
- }
} else {
obsIndex = 0;
}
+ // No landmark is close enough, don't attempt to use one
+ if (obsIndex < 0) {
+ R_k(0,0) = DONT_PROCESS_KEY;
+ return;
+ }
+
if ( z.isLine() ){
incorporatePolarMeasurement( obsIndex, z, H_k, R_k, V_k);
- }
-
- else if (z.getVisDistance() < USE_CARTESIAN_DIST) {
+ } else if (z.getVisDistance() < USE_CARTESIAN_DIST) {
incorporateCartesianMeasurement( obsIndex, z, H_k, R_k, V_k);
} else {
incorporatePolarMeasurement( obsIndex, z, H_k, R_k, V_k);
@@ -374,23 +382,28 @@ void LocEKF::incorporatePolarMeasurement(int obsIndex,
const float h_r = xhat_k_bar(2);
pair<float, float> closestLandmarkXY =
- Utility::findClosestLinePointCartesian(l, x_r, y_r, h_r);
+ findClosestLinePointCartesian(l, x_r, y_r, h_r);
// Relativize the closest point
const float relX_p = closestLandmarkXY.first;
const float relY_p = closestLandmarkXY.second;
- if (relX_p == 0.0 && relY_p == 0.0)
+ // If we're standing on top of the point (can happen in rare cases
+ // such as when loc is initialized), don't process it (leads to nan)
+ if (z.getVisDistance() < 0.001 ||
+ (relX_p < 0.001 && relY_p < 0.001)) {
+ R_k(0,0) = DONT_PROCESS_KEY;
return;
+ }
// Jacobians for line updates
- H_k(0,0) = (-2*y_b*x_l*y_l + x_b*(-1 + pow(x_l,2) + pow(y_l,2)) - (-1 + pow(x_l,2) + pow(y_l,2))*x_r)/ (pow(x_b - x_r + x_l*(x_l*(-x_b + x_r) + y_l*(y_b + y_r)),2) + pow(y_b - y_r + y_l*(x_l*(-x_b + x_r) + y_l*(y_b + y_r)),2));
- H_k(0,1) = (2*x_l*y_l*(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)) + 2*(-1 + pow(y_l,2))* (y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)))/ (2.*sqrt(pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2)));
- H_k(0,2) = 0;
+ H_k(0,0) = (2.0f*(-1.0f + pow(x_l,2.0f))*(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)) + 2.0f*x_l*y_l*(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)))/ (2.0f*sqrt(pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f)));
+ H_k(0,1) = (2.0f*x_l*y_l*(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)) + 2.0f*(-1.0f + pow(y_l,2.0f))* (y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)))/ (2.0f*sqrt(pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f)));
+ H_k(0,2) = 0.0f;
- H_k(1,0) = -(((-1 + pow(x_l,2) + pow(y_l,2))* (y_b - y_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r), 2) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2)));
- H_k(1,1) = ((-1 + pow(x_l,2) + pow(y_l,2))*(x_b - x_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r), 2) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2));
- H_k(1,2) = -1;
+ H_k(1,0) = -(((-1.0f + pow(x_l,2.0f) + pow(y_l,2.0f))*(y_b - y_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r), 2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f)));
+ H_k(1,1) = ((-1.0f + pow(x_l,2.0f) + pow(y_l,2.0f))*(x_b - x_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f));
+ H_k(1,2) = -1.0f;
MeasurementVector d_x(2);
d_x(0) = static_cast<float>(hypot(relX_p, relY_p));
@@ -411,8 +424,9 @@ void LocEKF::incorporatePolarMeasurement(int obsIndex,
z_x(1) = z.getVisBearing();
// Get expected values of the post
- const float x_b = z.getPointPossibilities()[obsIndex].x;
- const float y_b = z.getPointPossibilities()[obsIndex].y;
+ PointLandmark bestPossibility = z.getPointPossibilities()[obsIndex];
+ const float x_b = bestPossibility.x;
+ const float y_b = bestPossibility.y;
MeasurementVector d_x(2);
const float x = xhat_k_bar(0);
@@ -478,7 +492,7 @@ int LocEKF::findMostLikelyLine(Observation *z)
vector<LineLandmark> possibleLines = z->getLinePossibilities();
int minIndex = -1;
- float minDivergence = 1000.0f;
+ float minDivergence = 800000.0f;
for (unsigned int i = 0; i < possibleLines.size(); ++i) {
float distance = getMahalanobisDistance(z, possibleLines[i]);
@@ -500,25 +514,26 @@ int LocEKF::findMostLikelyLine(Observation *z)
*/
float LocEKF::getMahalanobisDistance(Observation *z, LineLandmark l)
{
- // x,y coordinates of observed line
- MeasurementVector z_x(2);
- z_x(0) = z->getVisDistance() * cos(z->getVisBearing());
- z_x(1) = z->getVisDistance() * sin(z->getVisBearing());
-
+ // Robot's current estimated position
const float x_r = xhat_k_bar(0);
const float y_r = xhat_k_bar(1);
const float h_r = xhat_k_bar(2);
+
+ // x,y coordinates of observed line in the field frame of reference
+ MeasurementVector z_x(2);
+ z_x(0) = x_r + z->getVisDistance() * cos(z->getVisBearing() + h_r);
+ z_x(1) = y_r + z->getVisDistance() * sin(z->getVisBearing() + h_r);
+
pair<float, float> line_xy =
- Utility::findClosestLinePointCartesian(l, x_r, y_r, h_r);
+ findClosestLinePointCartesian(l, x_r, y_r, h_r);
MeasurementVector u(2);
u(0) = line_xy.first;
u(1) = line_xy.second;
const float dist_sd_2 = pow(z->getDistanceSD(), 2);
- const float v = dist_sd_2 * sin(z->getBearingSD()) * cos(z->getBearingSD());
- const float bsd = z->getBearingSD();
+ const float bsd = z->getVisBearing();
float cosb, sinb;
sincosf(bsd, &sinb, &cosb);
@@ -737,3 +752,25 @@ void LocEKF::deadzone(float &R, float &innovation,
}
}
+// Finds the closest point on a line to the robot's position.
+// Returns relative coordinates of the point, in the frame of
+// reference of the field's coordinate system.
+std::pair<float,float>
+LocEKF::findClosestLinePointCartesian(LineLandmark l, float x_r,
+ float y_r, float h_r)
+{
+ const float x_l = l.dx;
+ const float y_l = l.dy;
+
+ const float x_b = l.x1;
+ const float y_b = l.y1;
+
+ // Find closest point on the line to the robot (global frame)
+ const float x_p = ((x_r - x_b)*x_l + (y_r - y_b)*y_l)*x_l + x_b;
+ const float y_p = ((x_r - x_b)*x_l + (y_r - y_b)*y_l)*y_l + y_b;
+
+ // Relativize the closest point
+ const float relX_p = x_p - x_r;
+ const float relY_p = y_p - y_r;
+ return std::pair<float,float>(relX_p, relY_p);
+}
View
4 noggin/LocEKF.h
@@ -170,6 +170,10 @@ class LocEKF : public EKF<Observation,
void limitPosteriorUncert();
void clipRobotPose();
void deadzone(float &R, float &innovation, float CPC, float EPS);
+ std::pair<float, float> findClosestLinePointCartesian(LineLandmark l,
+ float x_r,
+ float y_r,
+ float h_r);
// Last odometry update
MotionModel lastOdo;
View
424 noggin/NavStates.py
@@ -1,424 +0,0 @@
-""" States for finding our way on the field """
-
-from .util import MyMath
-import NavConstants as constants
-from .players import ChaseBallConstants
-from .playbook.PBConstants import GOALIE
-from math import fabs, cos, sin, radians
-
-DEBUG = False
-# States for the standard spin - walk - spin go to
-def spinToWalkHeading(nav):
- """
- Spin to the heading needed to walk to a specific point
- """
- targetH = MyMath.getTargetHeading(nav.brain.my, nav.destX, nav.destY)
- headingDiff = fabs(nav.brain.my.h - targetH)
- newSpinDir = MyMath.getSpinDir(nav.brain.my.h, targetH)
-
- if nav.firstFrame():
- nav.changeSpinDirCounter = 0
- nav.stopSpinToWalkCount = 0
- nav.curSpinDir = newSpinDir
-
- if newSpinDir != nav.curSpinDir:
- nav.changeSpinDirCounter += 1
- else:
- nav.changeSpinDirCounter = 0
-
- if nav.changeSpinDirCounter > constants.CHANGE_SPIN_DIR_THRESH:
- nav.curSpinDir = newSpinDir
- nav.changeSpinDirCounter = 0
- if DEBUG: nav.printf("Switching spin directions my.h " +
- str(nav.brain.my.h)+
- " and my target h: " + str(targetH))
-
- if nav.atHeadingGoTo(targetH):
- nav.stopSpinToWalkCount += 1
- else :
- nav.stopSpinToWalkCount -= 1
- nav.stopSpinToWalkCount = max(0, nav.stopSpinToWalkCount)
-
- if nav.stopSpinToWalkCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('walkStraightToPoint')
- if nav.atDestinationCloser():
- return nav.goLater('spinToFinalHeading')
-
- sTheta = nav.curSpinDir * constants.GOTO_SPIN_SPEED * \
- nav.getRotScale(headingDiff)
-
- if sTheta != nav.walkTheta:
- nav.setSpeed(0, 0, sTheta)
-
- return nav.stay()
-
-def walkStraightToPoint(nav):
- """
- State to walk forward w/some turning
- until localization thinks we are close to the point
- Stops if we get there
- If we no longer are heading towards it change to the spin state.
- """
- if nav.firstFrame():
- nav.walkToPointCount = 0
- nav.walkToPointSpinCount = 0
-
- if nav.atDestinationCloser():
- nav.walkToPointCount += 1
- else :
- nav.walkToPointCount = 0
-
- if nav.walkToPointCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('spinToFinalHeading')
-
- my = nav.brain.my
- targetH = MyMath.getTargetHeading(my, nav.destX, nav.destY)
-
- if nav.notAtHeading(targetH):
- nav.walkToPointSpinCount += 1
- else :
- nav.walkToPointSpinCount = 0
-
- if nav.walkToPointSpinCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('spinToWalkHeading')
-
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX,nav.destY)
- distToDest = MyMath.dist(my.x, my.y, nav.destX, nav.destY)
- if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST:
- gain = constants.GOTO_FORWARD_GAIN * MyMath.dist(my.x, my.y,
- nav.destX, nav.destY)
- else :
- gain = 1.0
-
- sTheta = MyMath.clip(MyMath.sign(bearing) *
- constants.GOTO_STRAIGHT_SPIN_SPEED *
- nav.getRotScale(bearing),
- -constants.GOTO_STRAIGHT_SPIN_SPEED,
- constants.GOTO_STRAIGHT_SPIN_SPEED )
-
- if fabs(sTheta) < constants.MIN_SPIN_MAGNITUDE_WALK:
- sTheta = 0
-
- sX = MyMath.clip(constants.GOTO_FORWARD_SPEED*gain,
- constants.WALK_TO_MIN_X_SPEED,
- constants.WALK_TO_MAX_X_SPEED)
-
- nav.setSpeed(sX, 0, sTheta)
- return nav.stay()
-
-def spinToFinalHeading(nav):
- """
- Spins until we are facing the final desired heading
- Stops when at heading
- """
- if nav.firstFrame():
- nav.stopSpinToWalkCount = 0
-
- targetH = nav.destH
-
- headingDiff = nav.brain.my.h - targetH
- if DEBUG:
- nav.printf("Need to spin to %g, heading diff is %g,heading uncert is %g"
- % (targetH, headingDiff, nav.brain.my.uncertH))
- spinDir = MyMath.getSpinDir(nav.brain.my.h, targetH)
-
- spin = spinDir*constants.GOTO_SPIN_SPEED*nav.getRotScale(headingDiff)
-
- if nav.atHeading(targetH):
- nav.stopSpinToWalkCount += 1
- else:
- nav.stopSpinToWalkCount = 0
-
- if nav.stopSpinToWalkCount > constants.CHANGE_SPIN_DIR_THRESH:
- if nav.movingOrtho:
- return nav.goLater('orthoWalkToPoint')
- return nav.goLater('stop')
-
- nav.setSpeed(0, 0, spin)
- return nav.stay()
-
-def orthoWalkToPoint(nav):
- """
- State to walk forward until localization thinks we are close to the point
- Stops if we get there
- If we no longer are heading towards it change to the spin state
- """
- my = nav.brain.my
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX,nav.destY)
- absBearing = fabs(bearing)
- if DEBUG:
- nav.printf('bearing: %g absBearing: %g my.x: %g my.y: %g my.h: %g'%
- (bearing, absBearing, my.x, my.y, my.h))
- if DEBUG: nav.printf((' nav.destX: ', nav.destX,
- ' nav.destY: ', nav.destY, ' nav.destH: ', nav.destH))
-
- if absBearing <= 45:
- return nav.goNow('orthoForward')
- elif absBearing <= 135:
- if bearing < 0:
- return nav.goNow('orthoRightStrafe')
- else:
- return nav.goNow('orthoLeftStrafe')
- elif absBearing <= 180:
- return nav.goNow('orthoBackward')
-
- return nav.stay()
-
-def orthoForward(nav):
-
- if nav.firstFrame():
- nav.walkToPointCount = 0
- nav.walkToPointSpinCount = 0
- nav.switchOrthoCount = 0
-
- if nav.notAtHeading(nav.destH):
- nav.walkToPointSpinCount += 1
- if nav.walkToPointSpinCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('spinToFinalHeading')
-
- if nav.atDestinationCloser():
- nav.walkToPointCount += 1
- if nav.walkToPointCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('stop')
-
- nav.setSpeed(constants.GOTO_FORWARD_SPEED,0,0)
- my = nav.brain.my
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX,nav.destY)
-
- if -45 <= bearing <= 45:
- return nav.stay()
-
- nav.switchOrthoCount += 1
- if nav.switchOrthoCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('orthoWalkToPoint')
- return nav.stay()
-
-def orthoBackward(nav):
- if nav.firstFrame():
- nav.walkToPointCount = 0
- nav.walkToPointSpinCount = 0
- nav.switchOrthoCount = 0
-
- if nav.notAtHeading(nav.destH):
- nav.walkToPointSpinCount += 1
- if nav.walkToPointSpinCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('spinToFinalHeading')
-
- if nav.atDestinationCloser():
- nav.walkToPointCount += 1
- if nav.walkToPointCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('stop')
-
- nav.setSpeed(constants.GOTO_BACKWARD_SPEED,0,0)
- my = nav.brain.my
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX,nav.destY)
-
- if 135 <= bearing or -135 >= bearing:
- return nav.stay()
-
- nav.switchOrthoCount += 1
- if nav.switchOrthoCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('orthoWalkToPoint')
- return nav.stay()
-
-def orthoRightStrafe(nav):
- if nav.firstFrame():
- nav.walkToPointCount = 0
- nav.walkToPointSpinCount = 0
- nav.switchOrthoCount = 0
-
- if nav.notAtHeading(nav.destH):
- nav.walkToPointSpinCount += 1
- if nav.walkToPointSpinCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('spinToFinalHeading')
-
- if nav.atDestinationCloser():
- nav.walkToPointCount += 1
- if nav.walkToPointCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('stop')
-
- nav.setSpeed(0, -constants.GOTO_STRAFE_SPEED,0)
- my = nav.brain.my
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX,nav.destY)
-
- if -135 <= bearing <= -45:
- return nav.stay()
-
- nav.switchOrthoCount += 1
- if nav.switchOrthoCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('orthoWalkToPoint')
- return nav.stay()
-
-def orthoLeftStrafe(nav):
- if nav.firstFrame():
- nav.walkToPointCount = 0
- nav.walkToPointSpinCount = 0
- nav.switchOrthoCount = 0
-
- if nav.notAtHeading(nav.destH):
- nav.walkToPointSpinCount += 1
- if nav.walkToPointSpinCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('spinToFinalHeading')
-
- if nav.atDestinationCloser():
- nav.walkToPointCount += 1
- if nav.walkToPointCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('stop')
-
- nav.setSpeed(0, constants.GOTO_STRAFE_SPEED,0)
- my = nav.brain.my
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX,nav.destY)
- absBearing = abs(bearing)
-
- if 45 <= bearing <= 135:
- return nav.stay()
-
- nav.switchOrthoCount += 1
- if nav.switchOrthoCount > constants.GOTO_SURE_THRESH:
- return nav.goLater('orthoWalkToPoint')
- return nav.stay()
-
-def omniWalkToPoint(nav):
- if nav.firstFrame():
- nav.walkToPointCount = 0
- if nav.brain.play.isRole(GOALIE):
- if nav.atDestinationGoalie() and nav.atHeading():
- return nav.goNow('stop')
- else:
- if nav.atDestinationCloser() and nav.atHeading():
- return nav.goNow('stop')
-
- my = nav.brain.my
- bearing = MyMath.getRelativeBearing(my.x, my.y, my.h, nav.destX, nav.destY)
- absBearing = abs(bearing)
- sX, sY, sTheta = 0.0, 0.0, 0.0
-
- distToDest = MyMath.dist(my.x, my.y, nav.destX, nav.destY)
-
- forwardGain = constants.OMNI_GOTO_X_GAIN * distToDest* \
- cos(radians(bearing))
- strafeGain = constants.OMNI_GOTO_Y_GAIN * distToDest* \
- sin(radians(bearing))
- spinGain = constants.GOTO_SPIN_GAIN
-
- sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain
- sY = constants.OMNI_GOTO_STRAFE_SPEED * strafeGain
-
- sX = MyMath.clip(sX,
- constants.OMNI_MIN_X_SPEED,
- constants.OMNI_MAX_X_SPEED)
- sY = MyMath.clip(sY,
- constants.OMNI_MIN_Y_SPEED,
- constants.OMNI_MAX_Y_SPEED,)
-
- if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
- sY = 0
- if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
- sX = 0
-
-
- spinDir = MyMath.getSpinDir(my.h, nav.destH)
- sTheta = spinDir * fabs(my.h - nav.destH) * spinGain
-
- sTheta = MyMath.clip(sTheta,
- constants.OMNI_MIN_SPIN_SPEED,
- constants.OMNI_MAX_SPIN_SPEED)
-
- if fabs(sTheta) < constants.OMNI_MIN_SPIN_MAGNITUDE:
- sTheta = 0.0
-
- if nav.atDestinationCloser():
- sX = sY = 0.0
- if nav.atHeading():
- sTheta = 0.0
-
- if DEBUG: nav.printf("sX: %g sY: %g sTheta: %g" %
- (sX, sY, sTheta))
- nav.setSpeed(sX, sY, sTheta)
- return nav.stay()
-
-# State to be used with standard setSpeed movement
-def walking(nav):
- """
- State to be used when setSpeed is called
- """
- if nav.firstFrame() or nav.updatedTrajectory:
- nav.setSpeed(nav.walkX, nav.walkY, nav.walkTheta)
- nav.updatedTrajectory = False
-
- return nav.stay()
-# State to use with the setSteps method
-def stepping(nav):
- """
- We use this to go a specified number of steps.
- This is different from walking.
- """
- if nav.firstFrame():
- nav.step(nav.stepX, nav.stepY, nav.stepTheta, nav.numSteps)
- elif not nav.brain.motion.isWalkActive():
- return nav.goNow("stopped")
- return nav.stay()
-
-### Stopping States ###
-def stop(nav):
- """
- Wait until the walk is finished.
- """
- if nav.firstFrame():
- if nav.brain.motion.isWalkActive():
- nav.setSpeed(0,0,0)
- nav.walkX = nav.walkY = nav.walkTheta = 0
-
-
- if not nav.brain.motion.isWalkActive():
- return nav.goNow('stopped')
-
- return nav.stay()
-
-def stopped(nav):
- nav.walkX = nav.walkY = nav.walkTheta = 0
- return nav.stay()
-
-def orbitPoint(nav):
- if nav.firstFrame():
- nav.setSpeed(0,
- nav.orbitDir*constants.ORBIT_STRAFE_SPEED,
- nav.orbitDir*constants.ORBIT_SPIN_SPEED )
- return nav.stay()
-
-
-def orbitPointThruAngle(nav):
- """
- Circles around a point in front of robot, for a certain angle
- """
- if fabs(nav.angleToOrbit) < constants.MIN_ORBIT_ANGLE:
- return nav.goNow('stop')
- if nav.firstFrame():
- if nav.angleToOrbit < 0:
- nav.orbitDir = constants.ORBIT_LEFT
- else:
- nav.orbitDir = constants.ORBIT_RIGHT
-
- if fabs(nav.angleToOrbit) <= constants.ORBIT_SMALL_ANGLE:
- sY = constants.ORBIT_STRAFE_SPEED * constants.ORBIT_SMALL_GAIN
- sT = constants.ORBIT_SPIN_SPEED * constants.ORBIT_SMALL_GAIN
-
- elif fabs(nav.angleToOrbit) <= constants.ORBIT_LARGE_ANGLE:
- sY = constants.ORBIT_STRAFE_SPEED * \
- constants.ORBIT_MID_GAIN
- sT = constants.ORBIT_SPIN_SPEED * \
- constants.ORBIT_MID_GAIN
- else :
- sY = constants.ORBIT_STRAFE_SPEED * \
- constants.ORBIT_LARGE_GAIN
- sT = constants.ORBIT_SPIN_SPEED * \
- constants.ORBIT_LARGE_GAIN
-
- nav.setSpeed(-0.5, nav.orbitDir*sY, nav.orbitDir*sT)
-
- # (frames/second) / (degrees/second) * degrees
- framesToOrbit = fabs((constants.FRAME_RATE / nav.walkTheta) *
- nav.angleToOrbit)
- if nav.counter >= framesToOrbit:
- return nav.goLater('stop')
- return nav.stay()
View
213 noggin/Navigator.py
@@ -1,213 +0,0 @@
-
-from . import NavStates
-from .util import FSA
-from .util import MyMath
-import NavConstants as constants
-import man.motion as motion
-
-from math import fabs
-
-class Navigator(FSA.FSA):
- def __init__(self,brain):
- FSA.FSA.__init__(self,brain)
- self.brain = brain
- self.addStates(NavStates)
- self.currentState = 'stopped'
- self.setName('Navigator')
- self.setPrintStateChanges(True)
- self.setPrintFunction(self.brain.out.printf)
- self.stateChangeColor = 'cyan'
-
- # Goto controls
- self.lastDestX = 0
- self.lastDestY = 0
- self.lastDestH = 0
- self.destX= 0
- self.destY= 0
- self.destH = 0
- self.oScale = -1
- self.hScale = -1
-
- # Walk controls
- self.walkX = 0
- self.walkY = 0
- self.walkTheta = 0
- self.currentGait = None
- self.movingOrtho = False
- self.movingOmni = False
-
- # Step controls
- self.stepX = 0
- self.stepY = 0
- self.stepTheta = 0
-
- self.orbitDir = None
-
- def omniGoTo(self, dest):
- if len(dest) == 2:
- self.destX, self.destY = dest
- self.destH = 0.0
- elif len(dest) == 3:
- self.destX,self.destY, self.destH = dest
- self.movingOrtho = False
- self.movingOmni = True
- self.switchTo('omniWalkToPoint')
-
- def orthoGoTo(self, dest, oScale = -1.0, hScale = -1.0):
- '''
- takes in a relative bearing [-180...0...180],
- takes in a heading to keep your heading constant (relatively)
- '''
- if len(dest) == 2:
- self.destX, self.destY = dest
- self.destH = 0.0
- elif len(dest) == 3:
- self.destX,self.destY, self.destH = dest
- self.oScale = oScale
- self.hScale = hScale
- self.movingOrtho = True
- self.movingOmni = False
-
- self.switchTo('orthoWalkToPoint')
-
- def goTo(self,dest):
- self.movingOrtho = False
- self.movingOmni = False
- if len(dest) == 2:
- self.destX, self.destY = dest
- self.destH = 0.0
- elif len(dest) == 3:
- self.destX,self.destY, self.destH = dest
-
- if not self.currentState == 'spinToWalkHeading' and \
- not self.currentState == 'walkStraightToPoint' and \
- not self.currentState == 'spinToFinalHeading':
- if not self.atHeadingGoTo(self.destH):
- self.switchTo('spinToWalkHeading')
- elif self.atHeadingGoTo(self.destH):
- self.switchTo('walkStraightToPoint')
-
- def setWalk(self, x, y, theta):
- """
- Sets a new walk command
- Returns False if it is the same as the current walk
- True otherwise
- """
- # Make sure we stop
- if (x == 0 and y == 0 and theta == 0):
- if self.walkX == 0 and self.walkY == 0 and self.walkTheta == 0:
- return False
- # If the walk changes are really small, then ignore them
- elif (fabs(self.walkX - x) < constants.FORWARD_EPSILON and
- fabs(self.walkY - y) < constants.STRAFE_EPSILON and
- fabs(self.walkTheta - theta) < constants.SPIN_EPSILON):
- return False
-
- self.walkX = x
- self.walkY = y
- self.walkTheta = theta
-
- self.updatedTrajectory = True
- return True
-
- def setSpeed(self,x,y,theta):
- """
- Wrapper method to easily change the walk vector of the robot
- """
- self.walkX, self.walkY, self.walkTheta = x, y, theta
- walk = motion.WalkCommand(x=x,y=y,theta=theta)
- self.brain.motion.setNextWalkCommand(walk)
-
- def setSteps(self, x, y, theta, numSteps):
- """
- Set the step commands
- """
- self.stepX = x
- self.stepY = y
- self.stepTheta = theta
- self.numSteps = numSteps
-
- def step(self,x,y,theta,numSteps):
- """
- Wrapper method to easily change the walk vector of the robot
- """
- steps = motion.StepCommand(x=x,y=y,theta=theta,numSteps=numSteps)
- self.brain.motion.sendStepCommand(steps)
-
-
- def atDestination(self):
- """
- Returns true if we are at an (x, y) close enough to the one we want
- """
-# self.printf("X diff is " + str(self.brain.my.x - self.destX))
-# self.printf("Y diff is " + str(self.brain.my.y - self.destY))
- return (abs(self.brain.my.x - self.destX) < constants.CLOSE_ENOUGH_XY
- and abs(self.brain.my.y - self.destY) < constants.CLOSE_ENOUGH_XY)
-
- def atDestinationCloser(self):
- """
- Returns true if we are at an (x, y) close enough to the one we want
- """
-# self.printf("X diff is " + str(self.brain.my.x - self.destX))
-# self.printf("Y diff is " + str(self.brain.my.y - self.destY))
- return (abs(self.brain.my.x - self.destX) < constants.CLOSER_XY
- and abs(self.brain.my.y - self.destY) < constants.CLOSER_XY)
-
- def atDestinationGoalie(self):
- return (abs(self.brain.my.x - self.destX) < constants.GOALIE_CLOSE_X
- and abs(self.brain.my.y - self.destY) < constants.GOALIE_CLOSE_Y)
-
- def atHeadingGoTo(self,targetHeading):
- hDiff = abs(MyMath.sub180Angle(self.brain.my.h - targetHeading))
- #self.printf("H diff is " + str(hDiff))
- return hDiff < constants.AT_HEADING_GOTO_DEG
-
-
- def atHeading(self, targetHeading = None):
- """
- Returns true if we are at a heading close enough to what we want
- """
- if targetHeading is None:
- targetHeading = self.destH
- hDiff = abs(MyMath.sub180Angle(self.brain.my.h - targetHeading))
- #self.printf("H diff is " + str(hDiff))
- return hDiff < constants.CLOSE_ENOUGH_H and \
- self.brain.my.uncertH < constants.LOC_IS_ACTIVE_H
-
- def notAtHeading(self, targetHeading= None):
- if targetHeading is None:
- targetHeading = self.destH
- hDiff = abs(MyMath.sub180Angle(self.brain.my.h - targetHeading))
- #self.printf("H diff is " + str(hDiff))
- return hDiff > constants.ALMOST_CLOSE_ENOUGH_H and \
- self.brain.my.uncertH < constants.LOC_IS_ACTIVE_H
-
- def getRotScale(self, headingDiff):
- absHDiff = abs(headingDiff)
- if absHDiff < constants.HEADING_NEAR_THRESH:
- return constants.HEADING_NEAR_SCALE
- elif absHDiff < constants.HEADING_MEDIUM_THRESH:
- return constants.HEADING_MEDIUM_SCALE
- else:
- return constants.HEADING_FAR_SCALE
-
- def getOScale(self):
- dist = MyMath.dist(self.brain.my.x, self.brain.my.y,
- self.destX, self.destY)
- if dist < constants.POSITION_NEAR_THRESH:
- return constants.POSITION_NEAR_SCALE
- elif dist < constants.HEADING_MEDIUM_THRESH:
- return constants.POSITION_MEDIUM_THRESH
- else:
- return constants.POSITION_FAR_SCALE
-
- def isStopped(self):
- return self.currentState == 'stopped'
-
- def orbit(self, orbitDir):
- self.orbitDir = orbitDir
- self.switchTo('orbitPoint')
-
- def orbitAngle(self, angleToOrbit):
- self.angleToOrbit = angleToOrbit
- self.switchTo('orbitPointThruAngle')
View
14 noggin/Noggin.cpp
@@ -383,15 +383,15 @@ void Noggin::updateLocalization()
# endif
}
- // We currently have lines turned off for localization, since we have no
- // model for using them in the EKF.
-
// Lines
- // const vector<VisualLine> * lines = vision->fieldLines->getLines();
- // vector <VisualLine>::const_iterator j;
+ // const vector< shared_ptr<VisualLine> > * lines = vision->fieldLines->getLines();
+ // vector <shared_ptr<VisualLine> >::const_iterator j;
// for ( j = lines->begin(); j != lines->end(); ++j) {
- // Observation seen(*j);
- // observations.push_back(seen);
+ // if ( !(*j)->getCCLine() &&
+ // (*j)->getPossibleLines().size() < ConcreteLine::NUM_LINES) {
+ // Observation seen(**j);
+ // observations.push_back(seen);
+ // }
// }
// Process the information
View
2 noggin/NogginConstants.py
@@ -49,7 +49,7 @@
IMAGE_ANGLE_X = IMAGE_WIDTH / FOV_X_DEG
IMAGE_ANGLE_Y = IMAGE_HEIGHT / FOV_Y_DEG
-NUM_TOTAL_BALL_VALUES = 32
+NUM_TOTAL_BALL_VALUES = 30
NUM_VISION_BALL_VALUES = 9 #unused? 1/20/10
NUM_VISION_FIELD_OBJECT_VALUES = 9
View
2 noggin/NogginStructs.h
@@ -140,7 +140,7 @@ class LineLandmark
// We want to make dx, dy components of line's _unit_ vector,
// so we normalize them
if (dx !=0 || dy != 0) {
- const float length = hypot(dx,dy);
+ const float length = hypotf(dx,dy);
dx = dx / length;
dy = dy / length;
}
View
3 noggin/Observation.cpp
@@ -75,8 +75,7 @@ Observation::Observation(VisualCross &_cross) :
Observation::Observation(const VisualLine &_line) :
visDist(_line.getDistance()), visBearing(_line.getBearing()),
sigma_d(_line.getDistanceSD()), sigma_b(_line.getBearingSD()),
- id(_line.getID()),
- line_truth(true), numPossibilities(0)
+ id(_line.getID()), line_truth(true), numPossibilities(0)
{
// Build our possibilitiy list
View
4 noggin/cmake.noggin/CMakeLists.txt
@@ -147,16 +147,14 @@ SET( NOGGIN_PYTHON_SRCS ${NOGGIN_INCLUDE_DIR}/__init__.py
${NOGGIN_INCLUDE_DIR}/GameStates.py
${NOGGIN_INCLUDE_DIR}/FallController.py
${NOGGIN_INCLUDE_DIR}/FallStates.py
- ${NOGGIN_INCLUDE_DIR}/NavStates.py
- ${NOGGIN_INCLUDE_DIR}/NavConstants.py
- ${NOGGIN_INCLUDE_DIR}/Navigator.py
${NOGGIN_INCLUDE_DIR}/NogginConstants.py
${NOGGIN_INCLUDE_DIR}/Leds.py
${NOGGIN_INCLUDE_DIR}/TeamConfig.py
${NOGGIN_INCLUDE_DIR}/WebotsConfig.py
)
SET( NOGGIN_PYTHON_SUBDIRS ${NOGGIN_INCLUDE_DIR}/headTracking
+ ${NOGGIN_INCLUDE_DIR}/navigator
${NOGGIN_INCLUDE_DIR}/playbook
${NOGGIN_INCLUDE_DIR}/players
${NOGGIN_INCLUDE_DIR}/robots
View
41 noggin/headTracking/BasicStates.py
@@ -0,0 +1,41 @@
+from man.motion import HeadMoves
+
+def stopped(tracker):
+ '''default state where the tracker does nothing'''
+ tracker.activeLocOn = False
+ return tracker.stay()
+
+def stop(tracker):
+ ''' stop all head moves '''
+ if tracker.firstFrame():
+ tracker.activeLocOn = False
+ tracker.brain.motion.stopHeadMoves()
+
+ if not tracker.brain.motion.isHeadActive():
+ return tracker.goLater('stopped')
+
+ return tracker.stay()
+
+def neutralHead(tracker):
+ '''move head to neutral position'''
+ if tracker.firstFrame():
+ tracker.activeLocOn = False
+ tracker.brain.motion.stopHeadMoves()
+ tracker.helper.executeHeadMove(HeadMoves.NEUT_HEADS)
+
+ if not tracker.brain.motion.isHeadActive():
+ return tracker.goLater('stopped')
+
+ return tracker.stay()
+
+def doHeadMove(tracker):
+ '''executes the currently set headMove'''
+ if tracker.firstFrame():
+ tracker.activeLocOn = False
+ tracker.brain.motion.stopHeadMoves()
+ tracker.helper.executeHeadMove(tracker.headMove)
+
+ if not tracker.brain.motion.isHeadActive():
+ return tracker.goLater('stopped')
+
+ return tracker.stay()
View
8 noggin/headTracking/HeadTracking.py
@@ -1,6 +1,7 @@
from . import TrackingStates
from . import PanningStates
from . import ActiveLookStates
+from . import BasicStates
from . import HeadTrackingHelper as helper
from ..util import FSA
@@ -12,6 +13,7 @@ def __init__(self, brain):
self.addStates(TrackingStates)
self.addStates(PanningStates)
self.addStates(ActiveLookStates)
+ self.addStates(BasicStates)
self.currentState = 'stopped'
self.setPrintFunction(self.brain.out.printf)
@@ -20,6 +22,7 @@ def __init__(self, brain):
self.setName('headTracking')
self.currentHeadScan = None
+ self.headMove = None
self.activePanDir = False
self.activeLocOn = False
@@ -40,6 +43,10 @@ def setNeutralHead(self):
Does not call stop head moves. In TrackingStates.py"""
self.switchTo('neutralHead')
+ def performHeadMove(self, headMove):
+ self.headMove = headMove
+ self.switchTo('doHeadMove')
+
def trackBall(self):
"""automatically tracks the ball. scans for the ball if not in view"""
self.target = self.brain.ball
@@ -51,7 +58,6 @@ def trackBall(self):
def locPans(self):
"""repeatedly performs quick pan"""
self.activeLocOn = False
- self.stopHeadMoves()
self.switchTo('locPans')
def activeLoc(self):
View
2 noggin/headTracking/HeadTrackingHelper.py
@@ -5,7 +5,7 @@
from man.motion import StiffnessModes
from math import (fabs, atan, pi, hypot)
-class HeadTrackingHelper():
+class HeadTrackingHelper(object):
def __init__(self, tracker):
self.tracker = tracker
View
16 noggin/headTracking/PanningStates.py
@@ -24,17 +24,25 @@ def scanBall(tracker):
return tracker.stay()
def scanning(tracker):
- if (tracker.firstFrame() or not
- tracker.brain.motion.isHeadActive()):
+ if tracker.firstFrame():
+ tracker.brain.motion.stopHeadMoves()
+ tracker.helper.executeHeadMove(tracker.currentHeadScan)
+
+ if not tracker.brain.motion.isHeadActive():
tracker.activeLocOn = False
tracker.helper.executeHeadMove(tracker.currentHeadScan)
+
return tracker.stay()
def locPans(tracker):
- if tracker.firstFrame() \
- or not tracker.brain.motion.isHeadActive():
+ if tracker.firstFrame():
+ tracker.brain.motion.stopHeadMoves()
+ tracker.helper.executeHeadMove(HeadMoves.QUICK_PANS)
+
+ if not tracker.brain.motion.isHeadActive():
tracker.activeLocOn = False
tracker.helper.executeHeadMove(HeadMoves.QUICK_PANS)
+
return tracker.stay()
def panLeftOnce(tracker):
View
29 noggin/headTracking/TrackingStates.py
@@ -1,34 +1,7 @@
from man.motion import MotionConstants
from . import TrackingConstants as constants
-from man.motion import HeadMoves
-DEBUG = False
-
-def stopped(tracker):
- '''default state where the tracker does nothing'''
- tracker.activeLocOn = False
- return tracker.stay()
-
-def stop(tracker):
- ''' stop all head moves '''
- if tracker.firstFrame():
- tracker.activeLocOn = False
- tracker.brain.motion.stopHeadMoves()
- if not tracker.brain.motion.isHeadActive():
- return tracker.goLater('stopped')
-
- return tracker.stay()
-
-def neutralHead(tracker):
- '''move head to neutral position'''
- if tracker.firstFrame():
- tracker.activeLocOn = False
- tracker.helper.executeHeadMove(HeadMoves.NEUT_HEADS)
-
- if not tracker.brain.motion.isHeadActive():
- return tracker.goLater('stopped')
-
- return tracker.stay()
+DEBUG = False
def ballTracking(tracker):
'''Super state which handles following/refinding the ball'''
View
24 noggin/navigator/ChaseStates.py
@@ -0,0 +1,24 @@
+
+DEBUG = False
+
+def chaseToPoint(nav):
+ pass
+
+def approachTargetWide(self, target):
+ '''
+ will act similarily to current approachBallWithLoc
+ '''
+ pass
+
+def approachTargetClose(self, target):
+ '''
+ will act similarily to approachBallWalk
+ '''
+ pass
+
+def positionForKick(self):
+ '''
+ will position us to kick the ball
+ '''
+ pass
+
View
2 noggin/NavConstants.py → noggin/navigator/NavConstants.py
@@ -1,4 +1,4 @@
-from .players import BrunswickSpeeds as speeds
+from ..players import BrunswickSpeeds as speeds
#navstates.py
GOTO_FORWARD_SPEED = speeds.MAX_X_SPEED
View
140 noggin/navigator/NavHelper.py
@@ -0,0 +1,140 @@
+from . import NavConstants as constants
+from math import fabs, cos, sin, radians
+from man.noggin.util import MyMath
+import man.motion as motion
+
+def setSpeed(motionInst, x, y, theta):
+ """
+ Wrapper method to easily change the walk vector of the robot
+ """
+ walk = motion.WalkCommand(x=x,y=y,theta=theta)
+ motionInst.setNextWalkCommand(walk)
+
+def step(motionInst, x, y, theta, numSteps):
+ """
+ Wrapper method to easily change the walk vector of the robot
+ """
+ steps = motion.StepCommand(x=x,y=y,theta=theta,numSteps=numSteps)
+ motionInst.sendStepCommand(steps)
+
+def executeMove(motionInst, sweetMove):
+ """
+ Method to enqueue a SweetMove
+ Can either take in a head move or a body command
+ (see SweetMove files for descriptions of command tuples)
+ """
+
+ for position in sweetMove:
+ if len(position) == 7:
+ move = motion.BodyJointCommand(position[4], #time
+ position[0], #larm
+ position[1], #lleg
+ position[2], #rleg
+ position[3], #rarm
+ position[6], # Chain Stiffnesses
+ position[5], #interpolation type
+ )
+
+ elif len(position) == 5:
+ move = motion.BodyJointCommand(position[2], # time
+ position[0], # chainID
+ position[1], # chain angles
+ position[4], # chain stiffnesses
+ position[3], # interpolation type
+ )
+
+ else:
+ print("What kind of sweet ass-Move is this?")
+
+ motionInst.enqueue(move)
+
+def getOmniWalkParam(my, dest):
+
+ bearing = radians(my.getRelativeBearing(dest))
+
+ distToDest = my.dist(dest)
+
+ # calculate forward speed
+ forwardGain = constants.OMNI_GOTO_X_GAIN * distToDest* \
+ cos(bearing)
+ sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain
+ sX = MyMath.clip(sX,
+ constants.OMNI_MIN_X_SPEED,
+ constants.OMNI_MAX_X_SPEED)
+ if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
+ sX = 0
+
+ # calculate sideways speed
+ strafeGain = constants.OMNI_GOTO_Y_GAIN * distToDest* \
+ sin(bearing)
+ sY = constants.OMNI_GOTO_STRAFE_SPEED * strafeGain
+ sY = MyMath.clip(sY,
+ constants.OMNI_MIN_Y_SPEED,
+ constants.OMNI_MAX_Y_SPEED,)
+ if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
+ sY = 0
+
+ if atDestinationCloser(my, dest):
+ sX = sY = 0.0
+
+ # calculate spin speed
+ spinGain = constants.GOTO_SPIN_GAIN
+ spinDir = my.spinDirToHeading(dest.h)
+ sTheta = spinDir * fabs(my.h - dest.h) * spinGain
+ sTheta = MyMath.clip(sTheta,
+ constants.OMNI_MIN_SPIN_SPEED,
+ constants.OMNI_MAX_SPIN_SPEED)
+ if fabs(sTheta) < constants.OMNI_MIN_SPIN_MAGNITUDE:
+ sTheta = 0.0
+
+ if atHeading(my, dest.h):
+ sTheta = 0.0
+
+ return (sX, sY, sTheta)
+
+def atDestination(my, dest):
+ """
+ Returns true if we are at an (x, y) close enough to the one we want
+ """
+ return ( fabs(my.x - dest.x) < constants.CLOSE_ENOUGH_XY and
+ fabs(my.y - dest.y) < constants.CLOSE_ENOUGH_XY)
+
+def atDestinationCloser(my, dest):
+ """
+ Returns true if we are at an (x, y) close enough to the one we want
+ """
+ return (fabs(my.x - dest.x) < constants.CLOSER_XY and
+ fabs(my.y - dest.y) < constants.CLOSER_XY)
+
+def atDestinationGoalie(my, dest):
+ return (fabs(my.x - dest.x) < constants.GOALIE_CLOSE_X and
+ fabs(my.y - dest.y) < constants.GOALIE_CLOSE_Y)
+
+def atHeadingGoTo(my, targetHeading):
+ hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
+ return hDiff < constants.AT_HEADING_GOTO_DEG
+
+def atHeading(my, targetHeading):
+ """
+ Returns true if we are at a heading close enough to what we want
+ """
+ hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
+
+ return hDiff < constants.CLOSE_ENOUGH_H and \
+ my.uncertH < constants.LOC_IS_ACTIVE_H
+
+def notAtHeading(my, targetHeading):
+ hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
+
+ return hDiff > constants.ALMOST_CLOSE_ENOUGH_H and \
+ my.uncertH < constants.LOC_IS_ACTIVE_H
+
+def getRotScale(headingDiff):
+ absHDiff = fabs(headingDiff)
+ if absHDiff < constants.HEADING_NEAR_THRESH:
+ return constants.HEADING_NEAR_SCALE
+ elif absHDiff < constants.HEADING_MEDIUM_THRESH:
+ return constants.HEADING_MEDIUM_SCALE
+ else:
+ return constants.HEADING_FAR_SCALE
+
View
1 noggin/navigator/NavMath.py
@@ -0,0 +1 @@
+
View
268 noggin/navigator/NavStates.py
@@ -0,0 +1,268 @@
+""" States for finding our way on the field """
+
+from ..util import MyMath
+from . import NavConstants as constants
+from . import NavHelper as helper
+from ..players import ChaseBallConstants
+from ..playbook.PBConstants import GOALIE
+from math import fabs
+
+DEBUG = False
+
+def doingSweetMove(nav):
+ '''executes the currently set sweetmove'''
+ motion = nav.brain.motion
+ if nav.firstFrame():
+ nav.walkX, nav.walkY, nav.walkTheta = 0, 0, 0
+ helper.setSpeed(motion, 0, 0, 0)
+ helper.executeMove(motion, nav.sweetMove)
+
+ if not nav.brain.motion.isBodyActive():
+ del nav.sweetMove
+ return nav.goNow('stopped')
+
+ return nav.stay()
+
+
+# States for the standard spin - walk - spin go to
+def spinToWalkHeading(nav):
+ """
+ Spin to the heading needed to walk to a specific point
+ """
+ my = nav.brain.my
+ targetH = my.getTargetHeading(nav.dest)
+ newSpinDir = my.spinDirToHeading(targetH)
+
+ if nav.firstFrame():
+ nav.changeSpinDirCounter = 0
+ nav.stopSpinToWalkCount = 0
+ nav.curSpinDir = newSpinDir
+
+ if newSpinDir != nav.curSpinDir:
+ nav.changeSpinDirCounter += 1
+ else:
+ nav.changeSpinDirCounter = 0
+
+ if nav.changeSpinDirCounter > constants.CHANGE_SPIN_DIR_THRESH:
+ nav.curSpinDir = newSpinDir
+ nav.changeSpinDirCounter = 0
+ if DEBUG: nav.printf("Switching spin directions my.h " +
+ str(nav.brain.my.h)+
+ " and my target h: " + str(targetH))
+
+ if helper.atHeadingGoTo(my, targetH):
+ nav.stopSpinToWalkCount += 1
+ else :
+ nav.stopSpinToWalkCount -= 1
+ nav.stopSpinToWalkCount = max(0, nav.stopSpinToWalkCount)
+
+ if nav.stopSpinToWalkCount > constants.GOTO_SURE_THRESH:
+ return nav.goLater('walkStraightToPoint')
+ if helper.atDestinationCloser(my, nav.dest):
+ return nav.goLater('spinToFinalHeading')
+
+ headingDiff = fabs(nav.brain.my.h - targetH)
+ sTheta = nav.curSpinDir * constants.GOTO_SPIN_SPEED * \
+ helper.getRotScale(headingDiff)
+
+ if sTheta != nav.walkTheta:
+ nav.walkX, nav.walkY = 0, 0
+ nav.walkTheta = sTheta
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta)
+
+ return nav.stay()
+
+def walkStraightToPoint(nav):
+ """
+ State to walk forward w/some turning
+ until localization thinks we are close to the point
+ Stops if we get there
+ If we no longer are heading towards it change to the spin state.
+ """
+ if nav.firstFrame():
+ nav.walkToPointCount = 0
+ nav.walkToPointSpinCount = 0
+
+ my = nav.brain.my
+ if helper.atDestinationCloser(my, nav.dest):
+ nav.walkToPointCount += 1
+ else :
+ nav.walkToPointCount = 0
+
+ if nav.walkToPointCount > constants.GOTO_SURE_THRESH:
+ return nav.goLater('spinToFinalHeading')
+
+
+ targetH = my.getTargetHeading(nav.dest)
+
+ if helper.notAtHeading(my, targetH):
+ nav.walkToPointSpinCount += 1
+ else :
+ nav.walkToPointSpinCount = 0
+
+ if nav.walkToPointSpinCount > constants.GOTO_SURE_THRESH:
+ return nav.goLater('spinToWalkHeading')
+
+ bearing = MyMath.sub180Angle(nav.brain.my.h - targetH)
+ distToDest = my.dist(nav.dest)
+ if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST:
+ gain = constants.GOTO_FORWARD_GAIN * distToDest
+ else :
+ gain = 1.0
+
+ sTheta = MyMath.clip(MyMath.sign(bearing) *
+ constants.GOTO_STRAIGHT_SPIN_SPEED *
+ helper.getRotScale(bearing),
+ -constants.GOTO_STRAIGHT_SPIN_SPEED,
+ constants.GOTO_STRAIGHT_SPIN_SPEED )
+
+ if fabs(sTheta) < constants.MIN_SPIN_MAGNITUDE_WALK:
+ sTheta = 0
+
+ sX = MyMath.clip(constants.GOTO_FORWARD_SPEED*gain,
+ constants.WALK_TO_MIN_X_SPEED,
+ constants.WALK_TO_MAX_X_SPEED)
+
+ nav.walkY = 0
+ nav.walkX = sX
+ nav.walkTheta = sTheta
+
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta)
+ return nav.stay()
+
+def spinToFinalHeading(nav):
+ """
+ Spins until we are facing the final desired heading
+ Stops when at heading
+ """
+ if nav.firstFrame():
+ nav.stopSpinToWalkCount = 0
+
+ targetH = nav.dest.h
+
+ #may be able to keep sign of this and eliminate spinDir
+ headingDiff = fabs(MyMath.sub180Angle(nav.brain.my.h - targetH))
+ if DEBUG:
+ nav.printf("Need to spin to %g, heading diff is %g,heading uncert is %g"
+ % (targetH, headingDiff, nav.brain.my.uncertH))
+ spinDir = nav.brain.my.spinDirToHeading(targetH)
+
+ spin = spinDir*constants.GOTO_SPIN_SPEED*helper.getRotScale(headingDiff)
+
+ if helper.atHeading(nav.brain.my, targetH):
+ nav.stopSpinToWalkCount += 1
+ else:
+ nav.stopSpinToWalkCount = 0
+
+ if nav.stopSpinToWalkCount > constants.CHANGE_SPIN_DIR_THRESH:
+ return nav.goLater('stop')
+
+ nav.walkX, nav.walkY = 0, 0
+ nav.walkTheta = spin
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta)
+ return nav.stay()
+
+def omniWalkToPoint(nav):
+ my = nav.brain.my
+ dest = nav.dest
+ if nav.firstFrame():
+ nav.walkToPointCount = 0
+ if nav.brain.play.isRole(GOALIE):
+ if helper.atDestinationGoalie(my, dest) and helper.atHeading(my, dest.h):
+ return nav.goNow('stop')
+ else:
+ if helper.atDestinationCloser(my, dest) and helper.atHeading(my, dest.h):
+ return nav.goNow('stop')
+
+ nav.walkX, nav.walkY, nav.walkTheta = helper.getOmniWalkParam(my, dest)
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta)
+
+ return nav.stay()
+
+# State to be used with standard setSpeed movement
+def walking(nav):
+ """
+ State to be used when setSpeed is called
+ """
+ if nav.updatedTrajectory:
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta)
+ nav.updatedTrajectory = False
+
+ return nav.stay()
+# State to use with the setSteps method
+def stepping(nav):
+ """
+ We use this to go a specified number of steps.
+ This is different from walking.
+ """
+ if nav.firstFrame():
+ helper.step(nav.brain.motion, nav.stepX, nav.stepY, nav.stepTheta, nav.numSteps)
+ elif not nav.brain.motion.isWalkActive():
+ return nav.goNow("stopped")
+ return nav.stay()
+
+### Stopping States ###
+def stop(nav):
+ """
+ Wait until the walk is finished.
+ """
+ if nav.firstFrame():
+ helper.setSpeed(nav.brain.motion, 0, 0, 0)
+ nav.walkX = nav.walkY = nav.walkTheta = nav.stepX = nav.stepY \
+ = nav.stepTheta = nav.numSteps = 0
+
+ if not nav.brain.motion.isWalkActive():
+ return nav.goNow('stopped')
+
+ return nav.stay()
+
+def stopped(nav):
+
+ return nav.stay()
+
+def orbitPoint(nav):
+ if nav.updatedTrajectory:
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta)
+ nav.updatedTrajectory = False
+
+ return nav.stay()
+
+
+def orbitPointThruAngle(nav):
+ """
+ Circles around a point in front of robot, for a certain angle
+ """
+ if fabs(nav.angleToOrbit) < constants.MIN_ORBIT_ANGLE:
+ return nav.goNow('stop')
+ if nav.updatedTrajectory:
+ if nav.angleToOrbit < 0:
+ orbitDir = constants.ORBIT_LEFT
+ else:
+ orbitDir = constants.ORBIT_RIGHT
+
+ if fabs(nav.angleToOrbit) <= constants.ORBIT_SMALL_ANGLE:
+ sY = constants.ORBIT_STRAFE_SPEED * constants.ORBIT_SMALL_GAIN
+ sT = constants.ORBIT_SPIN_SPEED * constants.ORBIT_SMALL_GAIN
+
+ elif fabs(nav.angleToOrbit) <= constants.ORBIT_LARGE_ANGLE:
+ sY = constants.ORBIT_STRAFE_SPEED * \
+ constants.ORBIT_MID_GAIN
+ sT = constants.ORBIT_SPIN_SPEED * \
+ constants.ORBIT_MID_GAIN
+ else :
+ sY = constants.ORBIT_STRAFE_SPEED * \
+ constants.ORBIT_LARGE_GAIN
+ sT = constants.ORBIT_SPIN_SPEED * \
+ constants.ORBIT_LARGE_GAIN
+
+ nav.walkX = -0.5
+ nav.walkY = orbitDir*sY
+ nav.walkTheta = orbitDir*sT
+ helper.setSpeed(nav.brain.motion, nav.walkX, nav.walkY, nav.walkTheta )
+
+ # (frames/second) / (degrees/second) * degrees
+ framesToOrbit = fabs((constants.FRAME_RATE / nav.walkTheta) *
+ nav.angleToOrbit)
+ if nav.counter >= framesToOrbit:
+ return nav.goLater('stop')
+ return nav.stay()
View
130 noggin/navigator/Navigator.py
@@ -0,0 +1,130 @@
+from math import fabs
+from ..util import FSA
+from . import NavStates
+from . import NavConstants as constants
+from . import NavHelper as helper
+from man.noggin.typeDefs.Location import Location
+
+class Navigator(FSA.FSA):
+ def __init__(self,brain):
+ FSA.FSA.__init__(self,brain)
+ self.brain = brain
+ self.addStates(NavStates)
+ self.currentState = 'stopped'
+ self.setName('Navigator')
+ self.setPrintStateChanges(True)
+ self.setPrintFunction(self.brain.out.printf)
+ self.stateChangeColor = 'cyan'
+
+ # Goto controls
+ self.dest = Location(0, 0)
+
+ # Walk controls
+ self.currentGait = None
+ self.walkX = 0
+ self.walkY = 0
+ self.walkTheta = 0
+ self.orbitDir = 0
+ self.angleToOrbit = 0
+
+ def performSweetMove(self, move):
+ self.sweetMove = move
+ self.switchTo('doingSweetMove')
+
+ def positionReady(self, dest):
+ self.dest = dest
+ self.switchTo('positioningReady')
+
+ def positionPlaybook(self, dest):
+ self.dest = dest
+ self.switchTo('positioningPlaybook')
+
+ def omniGoTo(self, dest):
+ self.dest = dest
+ self.switchTo('omniWalkToPoint')
+
+ def goTo(self,dest):
+ self.dest = dest
+
+ if not self.currentState == 'spinToWalkHeading' and \
+ not self.currentState == 'walkStraightToPoint' and \
+ not self.currentState == 'spinToFinalHeading':
+ if not helper.atHeadingGoTo(self.brain.my, self.dest.h):
+ self.switchTo('spinToWalkHeading')
+ elif helper.atHeadingGoTo(self.brain.my, self.dest.h):
+ self.switchTo('walkStraightToPoint')
+
+ def stop(self):
+ if self.currentState =='stop':
+ pass
+ else:
+ self.switchTo('stop')
+
+ def isStopped(self):
+ return self.currentState == 'stopped'
+
+ def movingOmni(self):
+ return self.currentState == 'omniWalkToPoint'
+
+ def orbit(self, orbitDir):
+
+ # If the orbit direction is the same ignore the command
+ if (self.orbitDir == orbitDir):
+ self.updatedTrajectory = False
+ return
+
+ self.orbitDir = orbitDir
+ self.walkX = 0
+ self.walkY = self.orbitDir*constants.ORBIT_STRAFE_SPEED
+ self.walkTheta = self.orbitDir*constants.ORBIT_SPIN_SPEED
+ self.updatedTrajectory = True
+
+ self.switchTo('orbitPoint')
+
+ def orbitAngle(self, angleToOrbit):
+
+ if (self.angleToOrbit == angleToOrbit):
+ self.updatedTrajectory = False
+ return
+
+ self.angleToOrbit = angleToOrbit
+ self.updatedTrajectory = True
+
+ self.switchTo('orbitPointThruAngle')
+
+ def walk(self, x, y, theta):
+ """
+ Starts a new walk command
+ Does nothing if it is the same as the current walk
+ Switches to it otherwise
+ """
+ # Make sure we stop
+ if (x == 0 and y == 0 and theta == 0):
+ self.printf("!!!!!! USE player.stopWalking() NOT walk(0,0,0)!!!!!")
+ return
+ # If the walk changes are really small, then ignore them
+ if (fabs(self.walkX - x) < constants.FORWARD_EPSILON and
+ fabs(self.walkY - y) < constants.STRAFE_EPSILON and
+ fabs(self.walkTheta - theta) < constants.SPIN_EPSILON):
+ self.updatedTrajectory = False
+ return
+
+ self.walkX = x
+ self.walkY = y
+ self.walkTheta = theta
+
+ self.updatedTrajectory = True
+ self.switchTo('walking')
+
+ def takeSteps(self, x, y, theta, numSteps):
+ """
+ Set the step commands
+ """
+ self.walkX, self.walkY, self.walkTheta = 0, 0, 0
+ self.stepX = x
+ self.stepY = y
+ self.stepTheta = theta
+ self.numSteps = numSteps
+ self.switchTo('stepping')
+
+#######SHOULD NOT BE CALLED BY ANYTHING OUTSIDE NAVIGATOR FOLDER#######
View
1 noggin/navigator/__init__.py
@@ -0,0 +1 @@
+
View
8 noggin/offline/fakerIterators.cpp
@@ -466,11 +466,11 @@ void checkLines(vector<Observation> &Z_t, PoseEst myPos)
case YELLOW_GOAL_ENDLINE:
line = &ConcreteLine::yellow_goal_endline;
break;
- case BLUE_YELLOW_SIDELINE:
- line = &ConcreteLine::blue_yellow_sideline;
+ case TOP_SIDELINE:
+ line = &ConcreteLine::top_sideline;
break;
- case YELLOW_BLUE_SIDELINE:
- line = &ConcreteLine::yellow_blue_sideline;
+ case BOTTOM_SIDELINE:
+ line = &ConcreteLine::bottom_sideline;
break;
case CENTER_FIELD_LINE:
line = &ConcreteLine::center_field_line;
View
27 noggin/players/BrunswickStates.py
@@ -14,14 +14,15 @@ def gameInitial(player):
player.isChasing = False
player.inKickingState = False
player.justKicked = False
+ player.gainsOn()
+ player.zeroHeads()
+ player.GAME_INITIAL_satDown = False
+
if player.squatting:
player.executeMove(SweetMoves.GOALIE_SQUAT_STAND_UP)
player.squatting = False
else:
player.stopWalking()
- player.gainsOn()
- player.zeroHeads()
- player.GAME_INITIAL_satDown = False
elif (player.brain.nav.isStopped() and not player.GAME_INITIAL_satDown
and not player.motion.isBodyActive()):
@@ -35,14 +36,16 @@ def gamePenalized(player):
player.isChasing = False
player.inKickingState = False
player.justKicked = False
+ player.penalizeHeads()
+
if player.squatting:
player.executeMove(SweetMoves.GOALIE_SQUAT_STAND_UP)
player.squatting = False
else:
player.stopWalking()
- player.penalizeHeads()
- if (player.stateTime >=
+ if (player.brain.play.isRole(GOALIE) and
+ player.stateTime >=
SweetMoves.getMoveTime(SweetMoves.GOALIE_SQUAT_STAND_UP)):
player.stopWalking()
@@ -57,17 +60,18 @@ def gameReady(player):
player.inKickingState = False
player.justKicked = False
player.brain.CoA.setRobotGait(player.brain.motion)
+
+ if player.squatting:
+ player.executeMove(SweetMoves.GOALIE_SQUAT_STAND_UP)
+ player.squatting = False
+ else:
+ player.standup()
+
if player.brain.gameController.ownKickOff:
player.hasKickedOffKick = False
else:
player.hasKickedOffKick = True
- if player.squatting:
- player.executeMove(SweetMoves.GOALIE_SQUAT_STAND_UP)
- player.squatting = False
- else:
- player.standup()
-
player.brain.tracker.locPans()
if player.lastDiffState == 'gameInitial':
return player.goLater('relocalize')
@@ -86,6 +90,7 @@ def gameSet(player):
player.inKickingState = False
player.justKicked = False
player.brain.CoA.setRobotGait(player.brain.motion)
+
if player.firstFrame() and player.lastDiffState == 'gamePenalized':
player.brain.resetLocalization()
View
30 noggin/players/ChaseBallStates.py
@@ -62,7 +62,7 @@ def chaseAfterKick(player):
elif player.chosenKick == SweetMoves.RIGHT_SIDE_KICK:
turnDir = constants.TURN_LEFT
- player.setSpeed(0, 0, turnDir * constants.BALL_SPIN_SPEED)
+ player.setWalk(0, 0, turnDir * constants.BALL_SPIN_SPEED)
return player.stay()
if player.brain.ball.framesOn > constants.BALL_ON_THRESH:
@@ -92,7 +92,7 @@ def turnToBall(player):
turnRate = MyMath.sign(turnRate)*constants.MIN_BALL_SPIN_MAGNITUDE
if ball.on:
- player.setSpeed(x=0,y=0,theta=turnRate)
+ player.setWalk(x=0,y=0,theta=turnRate)
if transitions.shouldKick(player):
return player.goNow('waitBeforeKick')
@@ -161,22 +161,20 @@ def approachBallWithLoc(player):
player.brain.tracker.trackBall()
dest = player.getApproachPosition()
- useOmni = MyMath.dist(my.x, my.y, dest[0], dest[1]) <= \
+ useOmni = my.dist(dest) <= \
constants.APPROACH_OMNI_DIST
changedOmni = False
- if useOmni != nav.movingOmni:
+ if useOmni != nav.movingOmni():
player.changeOmniGoToCounter += 1
else :
player.changeOmniGoToCounter = 0
if player.changeOmniGoToCounter > PositionConstants.CHANGE_OMNI_THRESH:
changedOmni = True
if player.firstFrame() or \
- nav.destX != dest[0] or \
- nav.destY != dest[1] or \
- nav.destH != dest[2] or \
- changedOmni:
+ nav.dest != dest or \
+ changedOmni:
if not useOmni:
player.brain.CoA.setRobotGait(player.brain.motion)
nav.goTo(dest)
@@ -274,7 +272,7 @@ def approachBallWalk(player):
# Set our walk towards the ball
if ball.on:
- player.setSpeed(sX,0,sTheta)
+ player.setWalk(sX,0,sTheta)
return player.stay()
@@ -325,7 +323,7 @@ def positionForKick(player):
sX = 0.0
if ball.on:
- player.setSpeed(sX,sY,0)
+ player.setWalk(sX,sY,0)
return player.stay()
def dribble(