<jupyter_start><jupyter_text>Where is the Robot???This is where the rubber meets the road in figuring out where our robot is on the field. In FTC it is typicalthat there are navigation images taped to the field walls (which can be recognized with the viewforiasoftware). In FRC it is typical to have retro-reflective tapetargets on the field that the limelight can detect. Additionally, there are often common markings on thefield elements that could be used as navigation aids with appropriate image recognition software (equivalentto the FTC viewrofia).In this session we will use our knowledge from the last session to answer the question: If we can see someknown field marker from tha camera, and we know the heading of the robot, can we determine the robot'sposition on the field? And I think we now have the tools to do that. The plan:* Pick a several known field positions and get the camera position for the centroid of the retro-reflective  tape as the limelight XY angles.* Start with those limelight XY angle and robot heading and work backwards to get the robot position on the field:  * use the robot heading and limelight XY angles to get the vector from the target to tha limelight lens;  * use the limelight field height to determine the field distance from the target to the limelight lens;  * compute the field position of the limelight lens;  * transform the field position of the limelight lens to the field position of the robot.* Verify that the robot position on the field we just computed matches the position that generated the limelight  XY angles. If not, adjust code until we get that right.  To move ahead here we need to connect to the new `a05annexUtil` used in the last session:<jupyter_code>%%loadFromPOM
<repository>
  <id>oss-sonatype-staging</id>
  <url>https://oss.sonatype.org/content/repositories/staging/</url>
</repository>

<dependency>
  <groupId>org.a05annex</groupId>
  <artifactId>a05annexUtil</artifactId>
  <version>0.8.9</version>
</dependency><jupyter_output><empty_output><jupyter_text>And we need the field geometry, the `MyRobot`and class, and the `Limelight` class from the last session:<jupyter_code>import org.a05annex.util.AngleD;
import org.a05annex.util.geo3d.Point3d;
import org.a05annex.util.geo3d.Vector3d;
import org.a05annex.util.geo3d.Xfm4x4d;

static final int FIELD_CENTER = 0;
static final int START_CENTER = 1;
static final int START_RIGHT = 2;
static final int START_LEFT = 3;
static final int TRENCH_CORNER = 4;
static final int HEX_GOAL = 5;
static final int CENTER_GOAL = 6;
static final String[] FIELD_NAMES = {"field center     : ",
                                     "start center     : ",
                                     "start right      : ",
                                     "start left       : ",
                                     "trench corner    : ",
                                     "hex goal center  : ",
                                     "round goal center: "};
static final Point3d[] FIELD_POINTS = {new Point3d(0.0, 0.0, 0.0),
                                       new Point3d(0.0, 5.0, 0.0),
                                       new Point3d(1.7085, 5.0, 0.0),
                                       new Point3d(-1.56, 5.0, 0.0),
                                       new Point3d(2.695, 2.743, 0.0),
                                       new Point3d(1.7085, 7.99, 2.5),
                                       new Point3d(1.7085, 8.73, 2.5)};

public void printFieldPts(Point3d[] fieldPts)
{
    for (int i = 0; i < fieldPts.length; i++) {
        System.out.println(String.format("%s (%9.3f,%9.3f,%9.3f)", FIELD_NAMES[i],
                           fieldPts[i].getX(), fieldPts[i].getY(), fieldPts[i].getZ()));
    }
}

class MyRobot {
    
    // The robot coordinates
    static final int CENTER = 0;
    static final int RF = 1;
    static final int LF = 2;
    static final int LR = 3;
    static final int RR = 4;
    static final int LIMELIGHT = 5;

    /* The Z position (relative to the robot) of the limelight lens in meters. */
    static final double LIMELIGHT_HEIGHT = .52;
    /* The elevation angle of the camera relative to the XY plane (the floor) in radians. */
    static final double LIMELIGHT_ELEV_ANGLE = 0.337;
    /* The X position (relative to the robot) of the limelight lens in meters. */
    static final double LIMELIGHT_X_OFFSET = -0.20;
    /* The Y position (relative to the robot) of the limelight lens in meters. */
    static final double LIMELIGHT_Y_OFFSET = 0.14;
    
    static final String[] GEOMETRY_NAMES = {"center     : ", "right front: ", "left front : ",
                                            "left rear  : ", "right rear : ", "limelight  : " };
    static final Point3d[] ROBOT_GEOMETRY = {new Point3d(0.0, 0.0, 0.0), new Point3d(0.1, 0.3, 0.0),
        new Point3d(-0.1, 0.3, 0.0), new Point3d(-0.1, -0.3, 0.0), new Point3d(0.1, -0.3, 0.0),
        new Point3d(LIMELIGHT_X_OFFSET, LIMELIGHT_Y_OFFSET, LIMELIGHT_HEIGHT)};
    
    double m_fieldX = 0.0;
    double m_fieldY = 0.0;
    final AngleD m_heading = new AngleD(AngleD.RADIANS, 0.0);
    
    final Xfm4x4d m_robotToFieldXfm = new Xfm4x4d();
    final Xfm4x4d m_fieldToRobotXfm = new Xfm4x4d();
    
    public MyRobot()
    {
    }
    
    public MyRobot(double fieldX, double fieldY, AngleD heading)
    {
        setFieldPosition(fieldX, fieldY, heading);
    }
    
    /**
     * Set the position of the robot on the field.
     */
    public void setFieldPosition(double fieldX, double fieldY, AngleD heading)
    {
        m_fieldX = fieldX;
        m_fieldY = fieldY;
        m_heading.setValue(heading);
        // build the transform that positions the robot relative to the field
        // by initializing the transform to an identify, applying the heading
        // rotation, and then translating the robot to some position on the field.
        m_robotToFieldXfm.identity();
        m_robotToFieldXfm.rotate(Xfm4x4d.AXIS_Z, heading.cloneAngleD().mult(-1.0));
        m_robotToFieldXfm.translate(fieldX, fieldY, 0.0);
        // invert that to buld the transform that describes where the field
        // elements are relative to the robot
        m_robotToFieldXfm.invert(m_fieldToRobotXfm);
    }
    
    /**
     * Transform an array of field-relative coordinates to robot-relative
     * coordinates. This takes an array of field-relative coordinates, and
     * creates an array of robot-relative coordinates. The field-relative
     * coordinates are unchanged.
     */
    public Point3d[] xfmFieldPtsToRobot(Point3d[] fieldPts)
    {
        Point3d[] robotPts = new Point3d[fieldPts.length];
        for (int i = 0; i < fieldPts.length; i++) {
            robotPts[i] = fieldPts[i].clonePoint3d();
        }
        m_fieldToRobotXfm.transform(robotPts);
        return robotPts;
    }
    
    /**
     * Transform the robot-relative geometry to field-relative coordinates and
     * return those field-relative coordinates. This is useful if you want to
     * display the robot on a diagram of the field.
     */
    public Point3d[] xfmRobotPtsToField()
    {
        Point3d[] fieldPts = new Point3d[ROBOT_GEOMETRY.length];
        for (int i = 0; i < ROBOT_GEOMETRY.length; i++) {
            fieldPts[i] = ROBOT_GEOMETRY[i].clonePoint3d();
        }
        m_robotToFieldXfm.transform(fieldPts);
        return fieldPts;
    }
    
    /**
     * Print an annotatted list of the robot geometry transformed to field-relative
     * coordinates for the current field position. This is primarily a debugging method.
     */
    public void printRobotPtsToField()
    {
        Point3d[] pts = xfmRobotPtsToField();
        for (int i = 0; i < pts.length; i++) {
            System.out.println(String.format("%s (%9.3f,%9.3f,%9.3f)", GEOMETRY_NAMES[i],
                                             pts[i].getX(), pts[i].getY(), pts[i].getZ()));
        }
    }
}

class Limelight {
    
    // The limelight coordinates
    static final int CENTER = 0;
    static final int UNIT_X = 1;
    static final int UNIT_Y = 2;
    static final int UNIT_Z = 3;

    static final String[] LIMELIGHT_NAMES = {"center : ",
                                             "unit X : ",
                                             "unit Y : ",
                                             "unit Z : "};
    static final Point3d[] LIMELIGHT_GEOMETRY = {new Point3d(0.0, 0.0, 0.0),
        new Point3d(1.0, 0.0, 0.0), new Point3d(0.0, 1.0, 0.0), new Point3d(0.0, 0.0, 1.0)};
    
    double m_robotX = 0.0;
    double m_robotY = 0.0;
    double m_robotZ = 0.0;
    final AngleD m_elevation = new AngleD(AngleD.RADIANS, 0.0);
    
    Xfm4x4d m_limelightToRobotXfm = new Xfm4x4d();
    Xfm4x4d m_robotToLimelightXfm = new Xfm4x4d();
    
    public Limelight()
    {
    }
    
    public Limelight(double robotX, double robotY, double robotZ, AngleD elevation)
    {
        setLimelightPosition(robotX, robotY, robotZ, elevation);
    }
    
    /**
     * Set the position of the limelight on the robot.
     */
    public void setLimelightPosition(double robotX, double robotY, double robotZ, AngleD elevation)
    {
        m_robotX = robotX;
        m_robotY = robotY;
        m_robotZ = robotZ;
        m_elevation.setValue(elevation);
        // build the transform that positions the limelight relative to the robot
        // by initializing the transform to an identify, applying a scaling to map
        // from a left handed coordinate system to a right handed, a rotation to
        // set the elevation angle, and a translation to set position of the limelight
        // on the robot.
        m_limelightToRobotXfm.identity();
        // rescale the axis system from a left handed to right handed axis system
        m_limelightToRobotXfm.scale(1.0, 1.0, -1.0);
        // rotate around X until the Y is straight up (90deg) plus the tilt angle for the
        // camera facing forward
        AngleD xRotation = new AngleD(AngleD.DEGREES, 90.0).add(m_elevation);
        m_limelightToRobotXfm.rotate(Xfm4x4d.AXIS_X, xRotation);
        m_limelightToRobotXfm.translate(m_robotX, m_robotY, m_robotZ);
        // invert that to buld the transform that describes where the robot
        // elements are relative to the limelight
        m_limelightToRobotXfm.invert(m_robotToLimelightXfm);
    }
    
    /**
     * Transform an array of robot-relative coordinates to limelight-relative
     * coordinates. This takes an array of robot-relative coordinates, and
     * creates an array of limelight-relative coordinates. The robot-relative
     * coordinates are unchanged.
     */
    public Point3d[] xfmRobotPtsToLimelight(Point3d[] robotPts)
    {
        Point3d[] limelightPts = new Point3d[robotPts.length];
        for (int i = 0; i < robotPts.length; i++) {
            limelightPts[i] = robotPts[i].clonePoint3d();
        }
        return m_robotToLimelightXfm.transform(limelightPts);
    }
    
    /**
     * Transform the limelight-relative geometry to robot-relative coordinates and
     * return those robot-relative coordinates. This is useful if you want to
     * display the limelight on a diagram of the robot.
     */
    public Point3d[] xfmLimelightPtsToRobot()
    {
        Point3d[] robotPts = new Point3d[LIMELIGHT_GEOMETRY.length];
        for (int i = 0; i < LIMELIGHT_GEOMETRY.length; i++) {
            robotPts[i] = LIMELIGHT_GEOMETRY[i].clonePoint3d();
        }
        return m_limelightToRobotXfm.transform(robotPts);
    }
    
    /**
     * Print an annotated list of the limelight geometry transformed to robot-relative
     * coordinates for the current camera position. This is primarily a debugging method.
     */
    public void printLimelightPtsToRobot()
    {
        Point3d[] pts = xfmLimelightPtsToRobot();
        for (int i = 0; i < pts.length; i++) {
            System.out.println(String.format("%s (%7.3f,%7.3f,%7.3f)", LIMELIGHT_NAMES[i],
                                             pts[i].getX(), pts[i].getY(), pts[i].getZ()));
        }
    }
    
    /**
     *
     */
    public Vector3d limelightXYtoRobotDirection(AngleD limelightX, AngleD limelightY) {
        return m_limelightToRobotXfm.transform(
            new Vector3d(limelightX.tan(), limelightY.tan(), 1.0).normalize());
    }
    
    public AngleD[] limelightXYtoRobotRotationElevation(AngleD limelightX, AngleD limelightY) {
        Vector3d vRobot = limelightXYtoRobotDirection(limelightX, limelightY);
        AngleD[] rotationElevation = {new AngleD().atan2(vRobot.getI(),vRobot.getJ()),
               new AngleD().asin(vRobot.getK())};
        return rotationElevation;
    }
}

MyRobot myRobot = new MyRobot();

Limelight myLimelight = new Limelight();
myLimelight.setLimelightPosition(-0.200, 0.140, 0.520, new AngleD(AngleD.RADIANS,0.337));<jupyter_output><empty_output><jupyter_text>In session 12, we ended with a question about how the angle to the target changes when the X angle on thelimelight changes. And we didn't have a good way to compute an answer for that:![alt text](./resources/ElevatedLimelight.jpg "simple distance with limelight")Let's go back to the simple robot camera geometry drawing:![alt text](./resources/limelight_side_geom.jpg "limelight side view")We can use our newly acquired vector math skills to answer that question. Start by imagining the camera centered on the robot with the correct height and elevation. The height is .52m, and the angle is .337rad. The target height 2.5m so if we put the target `(2.5m. - .52m)/tan(.337) = 5.6517m` in front of the robot, the limelight should be centered on the target.<jupyter_code>void printPoint(String title, Point3d pt) {
    System.out.println(String.format("%s (%7.3f,%7.3f,%7.3f)", title, pt.getX(), pt.getY(), pt.getZ()));
}

void printVector(String title, Vector3d v) {
    System.out.println(String.format("%s (%7.3f,%7.3f,%7.3f)", title, v.getI(), v.getJ(), v.getK()));
}

Point3d ptTarget = new Point3d(0.0, 5.6517, 2.5);
MyRobot myRobot = new MyRobot();
Limelight myLimelight = new Limelight();
myLimelight.setLimelightPosition(0.0, 0.0, 0.52, new AngleD(AngleD.RADIANS,0.337));

myRobot.setFieldPosition(0.0, 0.0, new AngleD(AngleD.DEGREES,0.0));
Point3d ptTargetRelativeToRobot = myRobot.m_fieldToRobotXfm.transform(ptTarget, new Point3d());
printPoint("  target center relative to robot:   ", ptTargetRelativeToRobot);
Point3d ptTargetRelativeToCam = myLimelight.m_robotToLimelightXfm.transform(ptTargetRelativeToRobot, new Point3d());
printPoint("  target center relative to camera:  ", ptTargetRelativeToCam);<jupyter_output>target center relative to robot:    (  0.000,  5.652,  2.500)
  target center relative to camera:   (  0.000, -0.000,  5.988)<jupyter_text>OK, so that works exactly as expected. So now we can rotate the robot through -&pi;/2 (-90&deg)to &pi;/2 (90&deg) and see what happens.<jupyter_code>for (double degrees = -90.0; degrees <= 95; degrees += 10.0) {
    myRobot.setFieldPosition(0.0, 0.0, new AngleD(AngleD.DEGREES,degrees));
    Point3d ptTargetRelativeToRobot = myRobot.m_fieldToRobotXfm.transform(ptTarget, new Point3d());
    Point3d ptTargetRelativeToCam = myLimelight.m_robotToLimelightXfm.transform(ptTargetRelativeToRobot, new Point3d());
    System.out.println(String.format(
        "rotation: %7.3f  target location: (%7.3f,%7.3f,%7.3f)  X angle= %7.3f Y angle %7.3f", degrees,
        ptTargetRelativeToCam.getX(), ptTargetRelativeToCam.getY(), ptTargetRelativeToCam.getZ(),
        new AngleD().atan2(ptTargetRelativeToCam.getX(),ptTargetRelativeToCam.getZ()).getDegrees(),
        new AngleD().atan2(ptTargetRelativeToCam.getY(),ptTargetRelativeToCam.getZ()).getDegrees()));
}

for (double degrees = -90.0; degrees <= 95; degrees += 10.0) {
    myRobot.setFieldPosition(0.0, 0.0, new AngleD(AngleD.DEGREES,degrees));
    Point3d ptTargetRelativeToRobot = myRobot.m_fieldToRobotXfm.transform(ptTarget, new Point3d());
    Point3d ptTargetRelativeToCam = myLimelight.m_robotToLimelightXfm.transform(ptTargetRelativeToRobot, new Point3d());
    AngleD limelightX = new AngleD().atan2(ptTargetRelativeToCam.getX(),ptTargetRelativeToCam.getZ());
    AngleD limelightY = new AngleD().atan2(ptTargetRelativeToCam.getY(),ptTargetRelativeToCam.getZ());
    Vector3d robotTargetDir = myLimelight.limelightXYtoRobotDirection(limelightX,limelightY);
    AngleD[] rotElev = myLimelight.limelightXYtoRobotRotationElevation(limelightX,limelightY);
    System.out.println(String.format(
        "rotation: %7.3f, limelightX= %7.3f, limelightY %7.3f, heading %7.3f, elevation %7.3f, heading %7.3f, elevation %7.3f",
        degrees, limelightX.getDegrees(), limelightY.getDegrees(),
        new AngleD().atan2(robotTargetDir.getI(),robotTargetDir.getJ()).getDegrees(),
        new AngleD().asin(robotTargetDir.getK()).getDegrees(),
        rotElev[0].getDegrees(), rotElev[1].getDegrees()));
}

System.out.println("\n-- Robot field coordinates when centered on the center of the start line, heading 30 degrees --");
System.out.println("-- Field elements --");
printFieldPts(FIELD_POINTS);
System.out.println("-- Field elements relative to the robot --");
myRobot.setFieldPosition(FIELD_POINTS[START_CENTER].getX(), FIELD_POINTS[START_CENTER].getY(), new AngleD(AngleD.DEGREES,30.0));
printFieldPts(myRobot.xfmFieldPtsToRobot(FIELD_POINTS));
System.out.println("-- Field elements relative to the limelight --");
printFieldPts(myLimelight.xfmRobotPtsToLimelight(myRobot.xfmFieldPtsToRobot(FIELD_POINTS)));<jupyter_output>-- Robot field coordinates when centered on the center of the start line, heading 30 degrees --
-- Field elements --
field center     :  (    0.000,    0.000,    0.000)
start center     :  (    0.000,    5.000,    0.000)
start right      :  (    1.709,    5.000,    0.000)
start left       :  (   -1.560,    5.000,    0.000)
trench corner    :  (    2.695,    2.743,    0.000)
hex goal center  :  (    1.709,    7.990,    2.500)
round goal center:  (    1.709,    8.730,    2.500)
-- Field elements relative to the robot --
field center     :  (    2.500,   -4.330,    0.000)
start center     :  (    0.000,    0.000,    0.000)
start right      :  (    1.480,    0.854,    0.000)
start left       :  (   -1.351,   -0.780,    0.000)
trench corner    :  (    3.462,   -0.607,    0.000)
hex goal center  :  (   -0.015,    3.444,    2.500)
round goal center:  (   -0.385,    4.085,    2.500)
-- Field elements relative to the limelight --
field center     :  (    2.500,    0.941,   -4.259)
start center[...]<jupyter_text>Lets start by defining a set of test positions on the field. Four easy positions to think of are center field atheading 0.0&deg;, center start at heading 30&deg;, target start at heading 0&deg;, and end of trench start at heading -15&deg;. We will use the limelight positions from the last session (the limelight position on the 2021 robot:<jupyter_code>void printPoint(String title, Point3d pt) {
    System.out.println(String.format("%s (%7.3f,%7.3f,%7.3f)", title, pt.getX(), pt.getY(), pt.getZ()));
}

void printVector(String title, Vector3d v) {
    System.out.println(String.format("%s (%7.3f,%7.3f,%7.3f)", title, v.getI(), v.getJ(), v.getK()));
}

MyRobot myRobot = new MyRobot();
Limelight myLimelight = new Limelight();
myLimelight.setLimelightPosition(-0.200, 0.140, 0.520, new AngleD(AngleD.RADIANS,0.337));

myRobot.setFieldPosition(0.0, 0.0, new AngleD(AngleD.DEGREES,30.0));
Point3d ptTargetRelativeToRobot = myRobot.m_fieldToRobotXfm.transform(FIELD_POINTS[HEX_GOAL], new Point3d());
printPoint("  target center relative to robot:   ", ptTargetRelativeToRobot);
Point3d ptTargetRelativeToCam = myLimelight.m_robotToLimelightXfm.transform(ptTargetRelativeToRobot, new Point3d());
printPoint("  target center relative to camera:  ", ptTargetRelativeToCam);
Vector3d vCameraToTarget = new Vector3d(new Point3d(0.0,0.0,0.0),ptTargetRelativeToCam).normalize();
printVector("  direction vector camera to target: ", vCameraToTarget);<jupyter_output><empty_output>
StarcoderdataJupyterStructuredCleanDedup