Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add kinematics suite (#1787)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Prateek Machiraju
2019-09-08 00:11:49 -04:00
committed by Peter Johnson
parent 561cbbd144
commit f405582f86
67 changed files with 5060 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java Normal file
View File

@@ -0,0 +1,85 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
* this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of
* reference.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
@SuppressWarnings("MemberName")
public class ChassisSpeeds {
/**
* Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
*/
public double vxMetersPerSecond;
/**
* Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
*/
public double vyMetersPerSecond;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
public double omegaRadiansPerSecond;
/**
* Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
*/
public ChassisSpeeds() {
}
/**
* Constructs a ChassisSpeeds object.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
*/
public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
double omegaRadiansPerSecond) {
this.vxMetersPerSecond = vxMetersPerSecond;
this.vyMetersPerSecond = vyMetersPerSecond;
this.omegaRadiansPerSecond = omegaRadiansPerSecond;
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vyMetersPerSecond The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param omegaRadiansPerSecond The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's
* angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should
* be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
*/
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond, double vyMetersPerSecond,
double omegaRadiansPerSecond, Rotation2d robotAngle) {
return new ChassisSpeeds(
vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
omegaRadiansPerSecond
);
}
}

64
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java Normal file
View File

@@ -0,0 +1,64 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
*
* <p>Inverse kinematics converts a desired chassis speed into left and right
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
*/
public class DifferentialDriveKinematics {
private final double m_trackWidthMeters;
/**
* Constructs a differential drive kinematics object.
*
* @param trackWidthMeters The track width of the drivetrain. Theoretically, this is
* the distance between the left wheels and right wheels.
* However, the empirical value may be larger than the physical
* measured value due to scrubbing effects.
*/
public DifferentialDriveKinematics(double trackWidthMeters) {
m_trackWidthMeters = trackWidthMeters;
}
/**
* Returns a chassis speed from left and right component velocities using
* forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
(wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
/ m_trackWidthMeters
);
}
/**
* Returns left and right component velocities from a chassis speed using
* inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that
* represent the chassis' speed.
* @return The left and right velocities.
*/
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vxMetersPerSecond - m_trackWidthMeters / 2
* chassisSpeeds.omegaRadiansPerSecond,
chassisSpeeds.vxMetersPerSecond + m_trackWidthMeters / 2
* chassisSpeeds.omegaRadiansPerSecond
);
}
}

114
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java Normal file
View File

@@ -0,0 +1,114 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Twist2d;
/**
* Class for differential drive odometry. Odometry allows you to track the
* robot's position on the field over the course of a match using readings from
* 2 encoders and a gyroscope.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
* <p>Note: It is important to reset both your encoders to zero before you start
* using this class. Only reset your encoders ONCE. You should not reset your
* encoders even if you want to reset your robot's pose.
*/
public class DifferentialDriveOdometry {
private final DifferentialDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_previousAngle;
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param kinematics The differential drive kinematics for your drivetrain.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_previousAngle = initialPoseMeters.getRotation();
}
/**
* Constructs a DifferentialDriveOdometry object with the default pose at the origin.
*
* @param kinematics The differential drive kinematics for your drivetrain.
*/
public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics) {
this(kinematics, new Pose2d());
}
/**
* Resets the robot's position on the field. Do NOT zero your encoders if you
* call this function at any other time except initialization.
*
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param angle The current robot angle.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
DifferentialDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates the
* current time to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param angle The angle of the robot.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d angle,
DifferentialDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(System.currentTimeMillis() / 1000.0,
angle, wheelSpeeds);
}
}

62
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java Normal file
View File

@@ -0,0 +1,62 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
@SuppressWarnings("MemberName")
public class DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
public double leftMetersPerSecond;
/**
* Speed of the right side of the robot.
*/
public double rightMetersPerSecond;
/**
* Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
*/
public DifferentialDriveWheelSpeeds() {
}
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param leftMetersPerSecond The left speed.
* @param rightMetersPerSecond The right speed.
*/
public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
this.leftMetersPerSecond = leftMetersPerSecond;
this.rightMetersPerSecond = rightMetersPerSecond;
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
}
}
}

169
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java Normal file
View File

@@ -0,0 +1,169 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.geometry.Translation2d;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds.
*
* <p>The inverse kinematics (converting from a desired chassis velocity to
* individual wheel speeds) uses the relative locations of the wheels with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion manuevers.
*
* <p>Forward kinematics (converting an array of wheel speeds into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
* multiply by [wheelSpeeds] to get our chassis speeds.
*
* <p>Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
public class MecanumDriveKinematics {
private SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
private final Translation2d m_frontLeftWheelMeters;
private final Translation2d m_frontRightWheelMeters;
private final Translation2d m_rearLeftWheelMeters;
private final Translation2d m_rearRightWheelMeters;
private Translation2d m_prevCoR = new Translation2d();
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheelMeters The location of the front-left wheel relative to the
* physical center of the robot.
* @param frontRightWheelMeters The location of the front-right wheel relative to
* the physical center of the robot.
* @param rearLeftWheelMeters The location of the rear-left wheel relative to the
* physical center of the robot.
* @param rearRightWheelMeters The location of the rear-right wheel relative to the
* physical center of the robot.
*/
public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
Translation2d frontRightWheelMeters,
Translation2d rearLeftWheelMeters,
Translation2d rearRightWheelMeters) {
m_frontLeftWheelMeters = frontLeftWheelMeters;
m_frontRightWheelMeters = frontRightWheelMeters;
m_rearLeftWheelMeters = rearLeftWheelMeters;
m_rearRightWheelMeters = rearRightWheelMeters;
m_inverseKinematics = new SimpleMatrix(4, 3);
setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
rearLeftWheelMeters, rearRightWheelMeters);
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
}
/**
* Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
* method is often used to convert joystick values into wheel speeds.
*
* <p>This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* manuevers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide
* a chassis speed that only has a dtheta component, the robot
* will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user
* input may cause one of the wheel speeds to go above the attainable max velocity. Use
* the {@link MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
Translation2d centerOfRotationMeters) {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotationMeters.getX() != m_prevCoR.getX()
|| centerOfRotationMeters.getY() != m_prevCoR.getY()) {
var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
setInverseKinematics(fl, fr, rl, rr);
m_prevCoR = centerOfRotationMeters;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(0, 0,
chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
return new MecanumDriveWheelSpeeds(
wheelsMatrix.get(0, 0),
wheelsMatrix.get(1, 0),
wheelsMatrix.get(2, 0),
wheelsMatrix.get(3, 0)
);
}
/**
* Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
* information.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toWheelSpeeds(chassisSpeeds, new Translation2d());
}
/**
* Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world speed of each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @return The resulting chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
wheelSpeedsMatrix.setColumn(0, 0,
wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
);
var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
}
/**
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical center of the robot.
* @param fr The location of the front-right wheel relative to the physical center of the robot.
* @param rl The location of the rear-left wheel relative to the physical center of the robot.
* @param rr The location of the rear-right wheel relative to the physical center of the robot.
*/
private void setInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr) {
m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
}
}

107
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java Normal file
View File

@@ -0,0 +1,107 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Twist2d;
/**
* Class for mecnaum drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* mecanum wheel encoders.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_previousAngle;
/**
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_previousAngle = initialPoseMeters.getRotation();
}
/**
* Constructs a MecanumDriveOdometry object with the default pose at the origin.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics) {
this(kinematics, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param angle The angle of the robot.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
MecanumDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates the
* current time to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param angle THe angle of the robot.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d angle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(System.currentTimeMillis() / 1000.0, angle,
wheelSpeeds);
}
}

84
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java Normal file
View File

@@ -0,0 +1,84 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import java.util.stream.DoubleStream;
@SuppressWarnings("MemberName")
public class MecanumDriveWheelSpeeds {
/**
* Speed of the front left wheel.
*/
public double frontLeftMetersPerSecond;
/**
* Speed of the front right wheel.
*/
public double frontRightMetersPerSecond;
/**
* Speed of the rear left wheel.
*/
public double rearLeftMetersPerSecond;
/**
* Speed of the rear right wheel.
*/
public double rearRightMetersPerSecond;
/**
* Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
*/
public MecanumDriveWheelSpeeds() {
}
/**
* Constructs a MecanumDriveWheelSpeeds.
*
* @param frontLeftMetersPerSecond Speed of the front left wheel.
* @param frontRightMetersPerSecond Speed of the front right wheel.
* @param rearLeftMetersPerSecond Speed of the rear left wheel.
* @param rearRightMetersPerSecond Speed of the rear right wheel.
*/
public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
double frontRightMetersPerSecond,
double rearLeftMetersPerSecond,
double rearRightMetersPerSecond) {
this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
this.frontRightMetersPerSecond = frontRightMetersPerSecond;
this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
this.rearRightMetersPerSecond = rearRightMetersPerSecond;
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
.max().getAsDouble();
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
}
}
}

196
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java Normal file
View File

@@ -0,0 +1,196 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
*
* <p>The inverse kinematics (converting from a desired chassis velocity to
* individual module states) uses the relative locations of the modules with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion manuevers.
*
* <p>Forward kinematics (converting an array of module states into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
* multiply by [moduleStates] to get our chassis speeds.
*
* <p>Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
public class SwerveDriveKinematics {
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
private final int m_numModules;
private final Translation2d[] m_modules;
private Translation2d m_prevCoR = new Translation2d();
/**
* Constructs a swerve drive kinematics object. This takes in a variable
* number of wheel locations as Translation2ds. The order in which you pass in
* the wheel locations is the same order that you will recieve the module
* states when performing inverse kinematics. It is also expected that you
* pass in the module states in the same order when calling the forward
* kinematics methods.
*
* @param wheelsMetersPerSecond The locations of the wheels relative to the physical center
* of the robot.
*/
public SwerveDriveKinematics(Translation2d... wheelsMetersPerSecond) {
if (wheelsMetersPerSecond.length < 2) {
throw new IllegalArgumentException("A swerve drive requires at least two modules");
}
m_numModules = wheelsMetersPerSecond.length;
m_modules = Arrays.copyOf(wheelsMetersPerSecond, m_numModules);
m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
}
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
}
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
*
* <p>This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* manuevers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide
* a chassis speed that only has a dtheta component, the robot
* will rotate around that corner.
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* {@link #normalizeWheelSpeeds(SwerveModuleState[], double) normalizeWheelSpeeds}
* function to rectify this issue.
*/
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
Translation2d centerOfRotationMeters) {
if (centerOfRotationMeters.getX() != m_prevCoR.getX()
|| centerOfRotationMeters.getY() != m_prevCoR.getY()) {
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
-m_modules[i].getY() + centerOfRotationMeters.getY());
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
+m_modules[i].getX() - centerOfRotationMeters.getX());
}
m_prevCoR = centerOfRotationMeters;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(0, 0,
chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
for (int i = 0; i < m_numModules; i++) {
double x = moduleStatesMatrix.get(i * 2, 0);
double y = moduleStatesMatrix.get(i * 2 + 1, 0);
double speed = Math.hypot(x, y);
Rotation2d angle = new Rotation2d(x, y);
moduleStates[i] = new SwerveModuleState(speed, angle);
}
return moduleStates;
}
/**
* Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
* toSwerveModuleStates for more information.
*
* @param chassisSpeeds The desired chassis speed.
* @return An array containing the module states.
*/
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
return toSwerveModuleStates(chassisSpeeds, new Translation2d());
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
*
* @param wheelStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
* @return The resulting chassis speed.
*/
ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
if (wheelStates.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor"
);
}
var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
for (int i = 0; i < m_numModules; i++) {
var module = wheelStates[i];
moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
}
var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
*/
public static void normalizeWheelSpeeds(SwerveModuleState[] moduleStates,
double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
}
}
}
}

110
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Twist2d;
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* swerve drive encoders and swerve azimuth encoders.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*/
public class SwerveDriveOdometry {
private final SwerveDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_previousAngle;
/**
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param initialPose The starting position of the robot on the field.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPose;
m_previousAngle = initialPose.getRotation();
}
/**
* Constructs a SwerveDriveOdometry object with the default pose at the origin.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics) {
this(kinematics, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(Pose2d pose) {
m_poseMeters = pose;
m_previousAngle = pose.getRotation();
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param angle The angle of the robot.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated your
* SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
SwerveModuleState... moduleStates) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates the
* current time to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param angle The angle of the robot.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated your
* SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d angle, SwerveModuleState... moduleStates) {
return updateWithTime(System.currentTimeMillis() / 1000.0, angle, moduleStates);
}
}

57
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java Normal file
View File

@@ -0,0 +1,57 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
/**
* Represents the state of one swerve module.
*/
@SuppressWarnings("MemberName")
public class SwerveModuleState implements Comparable<SwerveModuleState> {
/**
* Speed of the wheel of the module.
*/
public double speedMetersPerSecond;
/**
* Angle of the module.
*/
public Rotation2d angle = Rotation2d.fromDegrees(0);
/**
* Constructs a SwerveModuleState with zeros for speed and angle.
*/
public SwerveModuleState() {
}
/**
* Constructs a SwerveModuleState.
*
* @param speedMetersPerSecond The speed of the wheel of the module.
* @param angle The angle of the module.
*/
public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
this.speedMetersPerSecond = speedMetersPerSecond;
this.angle = angle;
}
/**
* Compares two swerve module states. One swerve module is "greater" than the other if its speed
* is higher than the other.
*
* @param o The other swerve module.
* @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
*/
@Override
@SuppressWarnings("ParameterName")
public int compareTo(SwerveModuleState o) {
return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
}
}