From 85951da0245c16c2e7415c225c075c111bdaee1a Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 28 May 2024 17:16:10 -0400 Subject: [PATCH] initial work on arm trajectories --- .../robot/subsystems/arm/ArmTrajectory.java | 201 ++++++++++++++++++ .../lib/utils/ArmTrajectoryGenerator.java | 168 --------------- 2 files changed, 201 insertions(+), 168 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmTrajectory.java delete mode 100644 src/main/java/lib/utils/ArmTrajectoryGenerator.java diff --git a/src/main/java/frc/robot/subsystems/arm/ArmTrajectory.java b/src/main/java/frc/robot/subsystems/arm/ArmTrajectory.java new file mode 100644 index 00000000..ccaa7e84 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmTrajectory.java @@ -0,0 +1,201 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N4; +import org.ejml.simple.SimpleMatrix; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +// Cubic spline trajectories +public class ArmTrajectory { + private double[] m_times; + private ArmTrajectoryState[] m_states; + private double m_startTime; + private double m_finalTime; + + public record ArmTrajectoryState( + double wristPositionDegs, + double wristVelocityDegsPerSec, + double armPositionDegs, + double armVelocityDegsPerSec + ) {} + + public ArmTrajectory(double[] times, ArmTrajectoryState[] states) { + if (times.length != states.length) { + throw new IllegalStateException("Number of states must match number of times"); + } + + m_times = times; + m_states = states; + m_startTime = m_times[0]; + m_finalTime = m_times[m_times.length - 1]; + } + + public double clipTime(double time) { + /* + Limits a given time between the trajectories start and end time + */ + + return MathUtil.clamp(time, m_startTime, m_finalTime); + } + + public ArmTrajectoryState sample(double desiredTime) { + /* + Samples the trajectory at a given time. + + Linearly interpolates between trajectory samples. + */ + + // finding the indexes of the next and last closest times + double time = clipTime(desiredTime); + int nextIdx = nextTimeIdx(m_times, time); + int prevIdx = prevTimeIdx(m_times, time); + + if (prevIdx == nextIdx) { + return m_states[prevIdx]; + } + + // find the next and last closest states and times + ArmTrajectoryState prevState = m_states[prevIdx]; + ArmTrajectoryState nextState = m_states[nextIdx]; + double prevTime = m_times[prevIdx]; + double nextTime = m_times[nextIdx]; + + // lots of math to create lots of terms ;w; + double newArmPosition = + prevState.armPositionDegs + (nextState.armPositionDegs - prevState.armPositionDegs) + / (nextTime - prevTime) * (time - prevTime); + + double newArmVelocity = + prevState.armVelocityDegsPerSec + (nextState.armVelocityDegsPerSec - prevState.armVelocityDegsPerSec) + / (nextTime - prevTime) * (time - prevTime); + + double newWristPosition = + prevState.wristPositionDegs + (nextState.wristPositionDegs - prevState.wristPositionDegs) + / (nextTime - prevTime) * (time - prevTime); + + double newWristVelocity = + prevState.wristVelocityDegsPerSec + (nextState.wristVelocityDegsPerSec - prevState.wristVelocityDegsPerSec) + / (nextTime - prevTime) * (time - prevTime); + + return new ArmTrajectoryState(newWristPosition, newWristVelocity, newArmPosition, newArmVelocity); + } + + public static ArmTrajectory fromCoeffs(Matrix coeffs, double t_0, double t_f) { + /* + Generate a trajectory from a polynomial coefficients matrix. + + Keyword arguments: + coeffs -- Polynomial coefficients as columns in increasing order. + Can have arbitrarily many columns. + t_0 -- time to start the interpolation + t_f -- time to end the interpolation + + This will only create a quadratic function, it will not create a quintic one. + + Returns: + Trajectory following the interpolation. The states will be in the form: + [pos_1, ..., pos_n, vel_1, ..., vel_n] + Where n is the number of columns in coeffs. + */ + + // create an array of timestamps to sample from + ArrayList tList = new ArrayList<>(); + double x = t_0; + while (x < t_f) { + tList.add(x); + x += 100; + } + tList.add(t_f); + + double[] t = tList.stream().mapToDouble(Double::valueOf).toArray(); + } + + public static Matrix cubic_interpolation( + double t_0, + double t_f, + ArmTrajectoryState state_0, + ArmTrajectoryState state_f) { + /* + Perform cubic interpolation between state₀ at t = t₀ and state_f at t = t_f. + Solves using the matrix equation: + + [1 t_0 t_0² t_0³][c₀₁ c₀₂] [x_0₁ x_0₂] + [0 1 2t_0 3t_0²][c₁₁ c₁₂] = [v_0₁ v_0₂] + [1 t_f t_f² t_f³][c₂₁ c₂₂] [x_f₁ x_f₂] + [0 1 2t_f 3t_f²][c₃₁ c₃₂] [v_f₁ v_f₂] + + To find the cubic polynomials: + + x₁(t) = c₀₁ + c₁₁t + c₂₁t² + c₃₁t³ + x₂(t) = c₀₂ + c₁₂t + c₂₂t² + c₃₂t³ + + where x₁ is the first joint position and x₂ is the second joint position, + such that the arm is in state_0 [x_0₁, x_0₂, v_0₁, v_0₂]ᵀ at t_0 and state_f + [x_f₁, x_f₂, v_f₁, v_f₂]ᵀ at t_f. + + Make sure to only use the interpolated cubic for t between t_0 and t_f. + + Keyword arguments: + t_0 -- start time of interpolation + t_f -- end time of interpolation + state_0 -- start state [θ₁, θ₂, ω₁, ω₂]ᵀ + state_f -- end state [θ₁, θ₂, ω₁, ω₂]ᵀ + + Returns: + coeffs -- 4x2 matrix containing the interpolation coefficients for joint 1 + in column 1 and joint 2 in column 2 + */ + + // copy our states into the "output" matrix + Matrix rhs = new Matrix<>(new SimpleMatrix(new double[][]{ + {state_0.armPositionDegs, state_0.armVelocityDegsPerSec, state_f.armPositionDegs, state_0.armVelocityDegsPerSec}, + {state_0.wristPositionDegs, state_0.wristVelocityDegsPerSec, state_f.wristPositionDegs, state_0.wristVelocityDegsPerSec} + })); + + SimpleMatrix lhsSimple = SimpleMatrix.filled(4, 4, 0.0); + lhsSimple.setRow(0, 0, posRow(t_0)); + lhsSimple.setRow(0, 0, velRow(t_0)); + lhsSimple.setRow(0, 0, posRow(t_f)); + lhsSimple.setRow(0, 0, velRow(t_f)); + + Matrix lhs = new Matrix<>(lhsSimple); + + return lhs.inv().times(rhs); + } + + private static double[] posRow(double t) { + return new double[]{1, t, t * t, t * t * t}; + } + + private static double[] velRow(double t) { + return new double[]{0, 1, 2 * t, 3 * t * t}; + } + + private static int nextTimeIdx(double[] array, double time) { + int returnvalue = -1; + for (int i = 0; i < array.length; ++i) { + if (array[i] >= time) { + returnvalue = i; + break; + } + } + return returnvalue; + } + + private static int prevTimeIdx(double[] array, double time) { + int returnvalue = -1; + for (int i = 0; i < array.length; ++i) { + if (array[i] <= time) { + returnvalue = i; + } + } + return returnvalue; + } +} diff --git a/src/main/java/lib/utils/ArmTrajectoryGenerator.java b/src/main/java/lib/utils/ArmTrajectoryGenerator.java deleted file mode 100644 index f6d5d3d1..00000000 --- a/src/main/java/lib/utils/ArmTrajectoryGenerator.java +++ /dev/null @@ -1,168 +0,0 @@ -package lib.utils; - -import java.util.ArrayList; -import java.util.List; - -public class ArmTrajectoryGenerator { - - private static double lerp(double startValue, double endValue, double t) { - return startValue + (endValue - startValue) * t; - } - - public static ArmTrajectory generateTrajectory(List setpoints, ArmTrajectoryConfig config) { - List m_states = new ArrayList<>(); - // forwards pass for ramp up - double timestamp = 0.0; - for (int i = 0; i < setpoints.size() - 1; i++) { - ArmTrajectoryPose current = setpoints.get(i); - ArmTrajectoryPose next = setpoints.get(i + 1); - - // look ahead a consistent time step (20 ms) and calculate the state - ArmTrajectory.ArmTrajectoryState prevState = null; - while (true) { - if (prevState == null) { - // inital state - prevState = new ArmTrajectory.ArmTrajectoryState( - timestamp, - current.armPoseDegs, - current.armVelocityDegsPerSecond, - config.armMaxAccelerationDegsPerSecondSq, - current.wristPoseDegs, - current.wristVelocityDegsPerSecond, - config.wristMaxAccelerationDegsPerSecondSq); - - m_states.add(prevState); - } - - // check to see if we're past the setpoint trying to move to, if we are then continue to the next set of - // setpoints - if (((prevState.armPoseDegs > next.armPoseDegs && prevState.armVelocityDegsPerSecond > 0.0) - || (prevState.armPoseDegs < next.armPoseDegs && prevState.armVelocityDegsPerSecond < 0.0)) - && ((prevState.wristPoseDegs > next.wristPoseDegs && prevState.wristVelocityDegsPerSecond > 0.0) - || (prevState.wristPoseDegs < next.wristPoseDegs && prevState.wristVelocityDegsPerSecond < 0.0))) { - break; - } - - timestamp += 0.02; - - // calculate new joint positions - // dx = x + vt + 1/2at^2 - double newArmPose = prevState.armPoseDegs + - (1.0 / 2.0) * config.armMaxAccelerationDegsPerSecondSq * Math.pow(0.02, 2); - double newWristPose = prevState.wristPoseDegs + - (1.0 / 2.0) * config.wristMaxAccelerationDegsPerSecondSq * Math.pow(0.02, 2); - - // calculate new joint velocities - // vf = sqrt(vi^2 + 2at) - double newArmVel = Math.min(Math.sqrt(Math.pow(prevState.armVelocityDegsPerSecond, 2) - + 2 * config.armMaxAccelerationDegsPerSecondSq + 0.02), - config.armMaxVelocityDegsPerSecond); - double newWristVel = Math.min(Math.sqrt(Math.pow(prevState.wristVelocityDegsPerSecond, 2) - + 2 * config.wristMaxAccelerationDegsPerSecondSq + 0.02), - config.wristMaxVelocityDegsPerSecond); - - // create new point at time step - prevState = new ArmTrajectory.ArmTrajectoryState( - timestamp, - newArmPose, - newArmVel, - config.armMaxAccelerationDegsPerSecondSq, - newWristPose, - newWristVel, - config.wristMaxAccelerationDegsPerSecondSq); - - m_states.add(prevState); - } - } - - return new ArmTrajectory(m_states); - } - - public record ArmTrajectoryPose(double armPoseDegs, double armVelocityDegsPerSecond, - double wristPoseDegs, double wristVelocityDegsPerSecond) {} - - public record ArmTrajectoryConfig(double armMaxVelocityDegsPerSecond, double armMaxAccelerationDegsPerSecondSq, - double wristMaxVelocityDegsPerSecond, double wristMaxAccelerationDegsPerSecondSq) {} - - public static class ArmTrajectory { - private final List states; - - public ArmTrajectory(List states) { - this.states = states; - } - - public record ArmTrajectoryState(double timeSeconds, - double armPoseDegs, - double armVelocityDegsPerSecond, - double armAccelerationDegsPerSecondSq, - double wristPoseDegs, - double wristVelocityDegsPerSecond, - double wristAccelerationDegsPerSecondSq) { - ArmTrajectoryState interpolate(ArmTrajectoryState endValue, double i) { - // Find the new t value. - final double newT = lerp(timeSeconds, endValue.timeSeconds, i); - - // Find the delta time between the current state and the interpolated state. - final double deltaT = newT - timeSeconds; - - // If delta time is negative, flip the order of interpolation. - if (deltaT < 0) { - return endValue.interpolate(this, 1 - i); - } - - // run interpolation on arm positions and velocities - // Check whether the robot is reversing at this stage. - final boolean armReversing = armVelocityDegsPerSecond < 0; - - // Calculate the new velocity - // v_f = v_0 + at - final double newArmV = armVelocityDegsPerSecond + (armAccelerationDegsPerSecondSq * deltaT); - - // Calculate the change in position. - // delta_s = v_0 t + 0.5at² - final double newArmS = - (armVelocityDegsPerSecond * deltaT - + 0.5 * armAccelerationDegsPerSecondSq * Math.pow(deltaT, 2)) - * (armReversing ? -1.0 : 1.0); - - // To find the new position for the new state, we need - // to interpolate between the two endpoint poses. The fraction for - // interpolation is the change in position (delta s) divided by the total - // distance between the two endpoints. - final double armInterpolationFrac = - newArmS / endValue.armPoseDegs() - armPoseDegs; - - // run interpolation on wrist positions and velocities - // Check whether the robot is reversing at this stage. - final boolean wristReversing = wristVelocityDegsPerSecond < 0; - - // Calculate the new velocity - // v_f = v_0 + at - final double newWristV = wristVelocityDegsPerSecond + (wristAccelerationDegsPerSecondSq * deltaT); - - // Calculate the change in position. - // delta_s = v_0 t + 0.5at² - final double newWristS = - (wristVelocityDegsPerSecond * deltaT - + 0.5 * wristAccelerationDegsPerSecondSq * Math.pow(deltaT, 2)) - * (wristReversing ? -1.0 : 1.0); - - // Return the new state. To find the new position for the new state, we need - // to interpolate between the two endpoint poses. The fraction for - // interpolation is the change in position (delta s) divided by the total - // distance between the two endpoints. - final double wristInterpolationFrac = - newWristS / endValue.armPoseDegs() - wristPoseDegs; - - return new ArmTrajectoryState( - newT, - lerp(armPoseDegs, endValue.armPoseDegs, armInterpolationFrac), - newArmV, - armAccelerationDegsPerSecondSq, - lerp(wristPoseDegs, endValue.wristPoseDegs, wristInterpolationFrac), - newWristV, - wristAccelerationDegsPerSecondSq); - } - } - } -}