Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

26
wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp Normal file
View File

@@ -0,0 +1,26 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/DifferentialDriveKinematics.h"
using namespace frc;
DifferentialDriveKinematics::DifferentialDriveKinematics(
units::meter_t trackWidth)
: m_trackWidth(trackWidth) {}
ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
}
DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const {
return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}

35
wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/DifferentialDriveOdometry.h"
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
}
const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& angle,
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}

21
wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp Normal file
View File

@@ -0,0 +1,21 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
using namespace frc;
void DifferentialDriveWheelSpeeds::Normalize(
units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}

69
wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveKinematics.h"
using namespace frc;
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation.X() != m_previousCoR.X() ||
centerOfRotation.Y() != m_previousCoR.Y()) {
auto fl = m_frontLeftWheel - centerOfRotation;
auto fr = m_frontRightWheel - centerOfRotation;
auto rl = m_rearLeftWheel - centerOfRotation;
auto rr = m_rearRightWheel - centerOfRotation;
SetInverseKinematics(fl, fr, rl, rr);
m_previousCoR = centerOfRotation;
}
Eigen::Vector3d chassisSpeedsVector;
chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
Eigen::Matrix<double, 4, 1> wheelsMatrix =
m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
return wheelSpeeds;
}
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) {
Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
// clang-format off
wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
// clang-format on
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(wheelSpeedsMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) {
// clang-format off
m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
1, 1, (fr.X() - fr.Y()).template to<double>(),
1, 1, (rl.X() - rl.Y()).template to<double>(),
1, -1, (-(rr.X() + rr.Y())).template to<double>();
// clang-format on
m_inverseKinematics /= std::sqrt(2);
}

35
wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveOdometry.h"
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& angle,
MecanumDriveWheelSpeeds wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}

34
wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include <algorithm>
#include <array>
#include <cmath>
using namespace frc;
void MecanumDriveWheelSpeeds::Normalize(
units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = *std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
return units::math::abs(a) < units::math::abs(b);
});
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
frontLeft = wheelSpeeds[0];
frontRight = wheelSpeeds[1];
rearLeft = wheelSpeeds[2];
rearRight = wheelSpeeds[3];
}
}