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

Example 11 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d in project FRC2022-Rapid-React by BitBucketsFRC4183.

the class ForceAtPose2d method getTorque.

/**
 * Returns the torque associated with this force at distance.
 * positive is counter-clockwise, negative is clockwise
 */
public double getTorque(Pose2d centerOfRotation) {
    Transform2d transCORtoF = new Transform2d(centerOfRotation, m_pos);
    // Align the force to the reference frame of the center of rotation
    Force2d alignedForce = getForceInRefFrame(centerOfRotation);
    // Calculate the lever arm the force acts at
    Vector2d leverArm = new Vector2d(transCORtoF.getX(), transCORtoF.getY());
    return leverArm.cross(alignedForce.getVector2d());
}

Example 12 with Transform2d

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

the class ForceAtPose2d method getTorque.

/**
 * @param centerOfRotation Pose at the center of rotation that the
 * torque should be calculated from
 * @return Returns the torque associated with this force at distance.
 * positive is counter-clockwise, negative is clockwise
 */
public double getTorque(Pose2d centerOfRotation) {
    Transform2d transCORtoF = new Transform2d(centerOfRotation, m_pos);
    // Align the force to the reference frame of the center of rotation
    Force2d alignedForce = getForceInRefFrame(centerOfRotation);
    // Calculate the lever arm the force acts at
    Vector2d leverArm = new Vector2d(transCORtoF.getX(), transCORtoF.getY());
    return leverArm.cross(alignedForce.getVector2d());
}

Example 13 with Transform2d

use of edu.wpi.first.math.geometry.Transform2d 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 14 with Transform2d

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

the class PoseTelemetry method update.

public void update(double time) {
    // Check if the user moved the robot with the Field2D
    // widget, and reset the model if so.
    Pose2d startPose = field.getRobotPose();
    Transform2d deltaPose = startPose.minus(endPose);
    if (deltaPose.getRotation().getDegrees() > 0.01 || deltaPose.getTranslation().getNorm() > 0.01) {
        swerveDt.modelReset(startPose);
    }
    if (RobotBase.isSimulation()) {
    // field.getObject("Robot").setPose(actualPose);
    }
    field.setRobotPose(m_poseEstimator.getEstimatedPosition());
    endPose = field.getRobotPose();
}

Example 15 with Transform2d

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

the class SwerveDrivetrainModel method update.

/**
 * Advance the simulation forward by one step
 * @param isDisabled Boolean that indicates if the robot is in the disabled mode
 * @param batteryVoltage Amount of voltage available to the drivetrain at the current step.
 */
public void update(boolean isDisabled, double batteryVoltage) {
    // Calculate and update input voltages to each motor.
    if (isDisabled) {
        for (int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++) {
            modules.get(idx).setInputVoltages(0.0, 0.0);
        }
    } else {
        for (int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++) {
            double steerVolts = realModules.get(idx).getSteerController().getOutputVoltage();
            double wheelVolts = realModules.get(idx).getDriveController().getOutputVoltage();
            modules.get(idx).setInputVoltages(wheelVolts, steerVolts);
        }
    }
    // Update the main drivetrain plant model
    swerveDt.update(SimConstants.SIM_SAMPLE_RATE_SEC);
    // Update each encoder
    for (int idx = 0; idx < QuadSwerveSim.NUM_MODULES; idx++) {
        double azmthShaftPos = modules.get(idx).getAzimuthEncoderPositionRev();
        double steerMotorPos = modules.get(idx).getAzimuthMotorPositionRev();
        double wheelPos = modules.get(idx).getWheelEncoderPositionRev();
        double azmthShaftVel = modules.get(idx).getAzimuthEncoderVelocityRPM();
        double steerVelocity = modules.get(idx).getAzimuthMotorVelocityRPM();
        double wheelVelocity = modules.get(idx).getWheelEncoderVelocityRPM();
        realModules.get(idx).getAbsoluteEncoder().setAbsoluteEncoder(azmthShaftPos, azmthShaftVel);
        realModules.get(idx).getSteerController().setSteerEncoder(steerMotorPos, steerVelocity);
        realModules.get(idx).getDriveController().setDriveEncoder(wheelPos, wheelVelocity);
    }
    // Update associated devices based on drivetrain motion
    gyro.setAngle(-swerveDt.getCurPose().getRotation().getDegrees());
    // Based on gyro and measured module speeds and positions, estimate where our
    // robot should have moved to.
    Pose2d prevEstPose = curEstPose;
    if (states != null) {
        curEstPose = m_poseEstimator.update(getGyroscopeRotation(), states[0], states[1], states[2], states[3]);
        // Calculate a "speedometer" velocity in ft/sec
        Transform2d chngPose = new Transform2d(prevEstPose, curEstPose);
        curSpeed = Units.metersToFeet(chngPose.getTranslation().getNorm()) / SimConstants.CTRLS_SAMPLE_RATE_SEC;
        updateDownfieldFlag();
    }
}