Examples with Twist2d edu.wpi.first.math.geometry.Twist2d used on opensource projects

Example 1 with Twist2d

use of edu.wpi.first.math.geometry.Twist2d in project 2022-RapidReact by Spartronics4915.

the class RobotStateEstimator method run.

public void run() {
    if (DriverStation.getInstance().isDisabled())
        return;
    double ts = Timer.getFPGATimestamp();
    final RobotStateMap.State last = mEncoderStateMap.getLatestState();
    /*
         * There are two ways to measure current velocity:
         * Method 1, integrationVelocity
         * Look at the distance traveled since last measurement, consider
         * current gyro heading rather than our stored state
         * Divide by delta time to produce a velocity. Note that
         * 254's implementation doesn't include time computations explicitly.
         * In method 1, the implicit time is the time between samples which relates
         * to the looper time interval. Thus: leftDelta is measured in
         * meters/loopinterval. To the degree that the loop interval isn't a
         * constant the result will be noisy. OTH: we can interpret this
         * velocity as also a distance traveled since last loop.
         */
    final Twist2d iVal;
    synchronized (this) {
        final double leftDist = mDrive.getLeftMotor().getEncoder().getPosition();
        final double rightDist = mDrive.getRightMotor().getEncoder().getPosition();
        final double leftDelta = leftDist - mLeftPrevDist;
        final double rightDelta = rightDist - mRightPrevDist;
        final Rotation2d heading = mDrive.getIMUHeading();
        mLeftPrevDist = leftDist;
        mRightPrevDist = rightDist;
        iVal = mKinematics.forwardKinematics(last.pose.getRotation(), leftDelta, rightDelta, heading);
        if (mBestEstimatorSource == EstimatorSource.Fused) {
            var ekfPose = mEKF.update(mCameraStateMap.getLatestFieldToVehicle(), leftDist, rightDist, heading.getRadians() - mPrevHeading, ts);
            mPrevHeading = heading.getRadians();
            mFusedStateMap.addObservations(ts, ekfPose);
        }
    }
    /*
         * integrateForward: given a last state and a current velocity,
         * estimate a new state (P2 = P1 + dPdt * dt)
         */
    Pose2d nextP = mKinematics.integrateForwardKinematics(last.pose, iVal);
    /* record the new state estimate */
    mEncoderStateMap.addObservations(ts, nextP);
    if (mBestEstimatorSource == EstimatorSource.VisionResetEncoderOdometry) {
        nextP = mKinematics.integrateForwardKinematics(mVisionResetEncoderStateMap.getLatestFieldToVehicle(), iVal);
        mVisionResetEncoderStateMap.addObservations(ts, nextP);
    }
    // We convert meters/loopinterval and radians/loopinterval to meters/sec and
    // radians/sec
    final double loopintervalToSeconds = 1 / (ts - last.timestamp);
    final Twist2d normalizedIVal = new Twist2d(iVal.dx * loopintervalToSeconds, iVal.dy * loopintervalToSeconds, iVal.dtheta * loopintervalToSeconds);
    if (mSLAMCamera != null) {
        try {
            // Sometimes (for unknown reasons) the native code can't send odometry info.
            // We throw a Java exception when this happens, but we'd like to ignore that
            // exception in this situation.
            mSLAMCamera.sendOdometry(normalizedIVal);
        } catch (CameraJNIException e) {
            Logger.exception(e);
        }
    }
}

Example 2 with Twist2d

use of edu.wpi.first.math.geometry.Twist2d in project RobotCode2022 by Mechanical-Advantage.

the class GeomUtil method interpolate.

/**
 * Interpolates between two poses based on the scale factor t. For example, t=0 would result in
 * the first pose, t=1 would result in the last pose, and t=0.5 would result in a pose which is
 * exactly halfway between the two poses. Values of t less than zero return the first pose, and
 * values of t greater than 1 return the last pose.
 *
 * @param lhs The left hand side, or first pose to use for interpolation
 * @param rhs The right hand side, or last pose to use for interpolation
 * @param t The scale factor, 0 <= t <= 1
 * @return The pose which represents the interpolation. For t <= 0, the "lhs" parameter is
 *         returned directly. For t >= 1, the "rhs" parameter is returned directly.
 */
public static Pose2d interpolate(Pose2d lhs, Pose2d rhs, double t) {
    if (t <= 0) {
        return lhs;
    } else if (t >= 1) {
        return rhs;
    }
    Twist2d twist = lhs.log(rhs);
    Twist2d scaled = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
    return lhs.exp(scaled);
}

Example 3 with Twist2d

use of edu.wpi.first.math.geometry.Twist2d in project BearSwerve by 6391-Ursuline-Bearbotics.

the class QuadSwerveSim method update.

/**
 * Steps the module simulation forward by one discrete step.
 * @param dtSeconds size of the discrete step to take
 */
public void update(double dtSeconds) {
    // global origin
    Pose2d fieldRF = new Pose2d();
    Transform2d fieldToRobot = new Transform2d(fieldRF, m_curPose);
    // Calculate each module's new position, and step it through simulation.
    for (int idx = 0; idx < NUM_MODULES; idx++) {
        Pose2d tmp = fieldRF.transformBy(fieldToRobot);
        Pose2d modPose = tmp.transformBy(m_robotToModule.get(idx));
        m_modules.get(idx).setModulePose(modPose);
        m_modules.get(idx).update(dtSeconds);
    }
    // Force on frame from wheel motive forces (along-tread)
    ArrayList<ForceAtPose2d> wheelMotiveForces = new ArrayList<ForceAtPose2d>(NUM_MODULES);
    for (int idx = 0; idx < NUM_MODULES; idx++) {
        wheelMotiveForces.add(m_modules.get(idx).getWheelMotiveForce());
    }
    // Friction Model
    Force2d preFricNetForce = new Force2d();
    wheelMotiveForces.forEach((ForceAtPose2d mf) -> {
        // Add up all the forces that friction gets a chance to fight against
        preFricNetForce.accum(mf.getForceInRefFrame(m_curPose));
    });
    // TODO - make a generic "external force" input?
    Force2d sidekickForce = new Force2d(0, 0);
    preFricNetForce.accum(sidekickForce);
    // calculate force on the robot, "pre friction" (pf)
    ForceAtPose2d pfRobotForce = new ForceAtPose2d(preFricNetForce, m_curPose);
    // Calculate the forces from cross-tread friction at each module
    ArrayList<ForceAtPose2d> xtreadFricFrc = new ArrayList<ForceAtPose2d>(NUM_MODULES);
    for (int idx = 0; idx < NUM_MODULES; idx++) {
        SwerveModuleSim mod = m_modules.get(idx);
        // Assume force evenly applied to all modules.
        double ffrac = 1.0 / NUM_MODULES;
        Force2d pfModForce = pfRobotForce.getForceInRefFrame(mod.getPose()).times(ffrac);
        xtreadFricFrc.add(mod.getCrossTreadFricForce(pfModForce, dtSeconds));
    }
    // //////////////////////////////////////////////////////////////
    // Combine forces in free-body diagram
    // Using all the above force components, do Sum of Forces
    Force2d forceOnRobotCenter = preFricNetForce;
    xtreadFricFrc.forEach((ForceAtPose2d f) -> {
        forceOnRobotCenter.accum(f.getForceInRefFrame(m_curPose));
    });
    ForceAtPose2d netForce = new ForceAtPose2d(forceOnRobotCenter, m_curPose);
    Force2d robotForceInFieldRefFrame = netForce.getForceInRefFrame(fieldRF);
    // Sum of Torques
    double netTorque = 0;
    for (int idx = 0; idx < NUM_MODULES; idx++) {
        netTorque += wheelMotiveForces.get(idx).getTorque(m_curPose);
        netTorque += xtreadFricFrc.get(idx).getTorque(m_curPose);
    }
    // //////////////////////////////////////////////////////////////
    // Apply Newton's 2nd law to get motion from forces
    // a = F/m in field frame
    Vector2d accel = robotForceInFieldRefFrame.times(1 / m_robotMasskg).getVector2d();
    // Trapezoidal integration
    Vector2d velocity = new Vector2d(m_velPrev.x + (accel.x + m_accelPrev.x) / 2 * dtSeconds, m_velPrev.y + (accel.y + m_accelPrev.y) / 2 * dtSeconds);
    // Trapezoidal integration
    Translation2d posChange = new Translation2d((velocity.x + m_velPrev.x) / 2 * dtSeconds, (velocity.y + m_velPrev.y) / 2 * dtSeconds);
    // Twist needs to be relative to robot reference frame
    posChange = posChange.rotateBy(m_curPose.getRotation().unaryMinus());
    m_velPrev = velocity;
    m_accelPrev = accel;
    // alpha = T/I in field frame
    double rotAccel = netTorque / m_robotMOI;
    double rotVel = m_rotVelPrev + (rotAccel + m_rotAccelPrev) / 2 * dtSeconds;
    double rotPosChange = (rotVel + m_rotVelPrev) / 2 * dtSeconds;
    m_rotVelPrev = rotVel;
    m_rotAccelPrev = rotAccel;
    Twist2d motionThisLoop = new Twist2d(posChange.getX(), posChange.getY(), rotPosChange);
    m_curPose = m_curPose.exp(motionThisLoop);
}

Example 4 with Twist2d

use of edu.wpi.first.math.geometry.Twist2d in project rapid-react-robot by wando-advanced-robotics.

the class Odometry method update.

public Pose2d update(Rotation2d gyroAngle, double leftDist, double rightDist) {
    double deltaLeft = leftDist - prevLeftDistance;
    double deltaRight = rightDist - prevRightDistance;
    prevLeftDistance = leftDist;
    prevRightDistance = rightDist;
    double averageDeltaDist = (deltaLeft + deltaRight) / 2.0;
    var angle = gyroAngle.plus(gyroOffset);
    var newPosition = position.exp(new Twist2d(averageDeltaDist, 0.0, angle.minus(previousAngle).getRadians()));
    previousAngle = angle;
    position = new Pose2d(newPosition.getTranslation(), angle);
    return position;
}

Example 5 with Twist2d

use of edu.wpi.first.math.geometry.Twist2d in project rapid-react-robot by wando-advanced-robotics.

the class Odometry method getTransformation.

public Translation2d getTransformation(Rotation2d updateAngle, double leftDist, double rightDist) {
    double deltaLeft = leftDist - prevLeftDistance;
    double deltaRight = rightDist - prevRightDistance;
    prevLeftDistance = leftDist;
    prevRightDistance = rightDist;
    double averageDeltaDist = (deltaLeft + deltaRight) / 2.0;
    var angle = updateAngle.plus(gyroOffset);
    var newPosition = position.exp(new Twist2d(averageDeltaDist, 0.0, angle.minus(previousAngle).getRadians()));
    previousAngle = angle;
    return newPosition.getTranslation();
}