Trajectory Optimization in FRC

Asa Paparo, FRC 1155

An introduction

Motivation

2

1D Motion Planning

3

Trapezoid Profiles

4

A Problem

5

A Problem

6

How can we solve this?

7

A simple approach

Move each segment individually

Example for a pivoting and extending double jointed arm:

1. Retract to default state
2. Elbow to θ₁
3. Extend to 𝓍₁
4. Wrist to θ₂

8

The “Correct” Approach

Move all mechanisms at once

Identify a trajectory for the system to follow given a set of constraints

• Position
• Velocity
• Acceleration
• Voltage
• Jerk

While minimizing

• Time

9

The “Correct” Approach

Requires a lot of math, so let’s get started

10

Numerical Optimization

11

CasADi

​

Symbolic math library

• Supports Python
• Has numerical optimization library

Use Opti to optimize arbitrary functions

• Same concept as relative/absolute min/max in calculus
• Multiple optimizer “back ends”
• IPOPT is a very powerful and general solver

import casadi

​

opti = casadi.Opti()

​

x = opti.variable()

y = opti.variable()

​

opti.minimize((y - x**2) ** 2)

opti.subject_to(x**2 + y**2 == 1)

opti.subject_to(x + y >= 1)

​

opti.solver("ipopt")

solution = opti.solve()

​

print(f"x={solution.value(x)}", f"y={solution.value(y)}")

12

Dynamics

13

Derivation

• Voltage is a constraint
• A feedforward equation allows us to constrain our system
• Lagrange method can be used for arbitrary (n dimensional) systems

14

Derivation

• For our robot, we used a double jointed arm on elevator
• This required adapting the derivation
• We used mathematica
• Would not recommend

15

Visualization

• A fluid motion
• Notice the vertical movement of the elevator
• IPOPT is taking advantage of the inertia of our system to accelerate the arm with the elevator

16

Putting it together

17

Infrastructure

public record ArmState(double elevatorHeight, Rotation2d elbowAngle, Rotation2d wristAngle) {

​

public static ArmState fromArray(double[] state) {

return fromAbsolute(state[0], state[1], state[2]);

}

​

public ArmState {

elevatorHeight = round(elevatorHeight);

elbowAngle = Rotation2d.fromRadians(round(elbowAngle.getRadians()));

wristAngle = Rotation2d.fromRadians(round(wristAngle.getRadians()));

}

​

public double[] toArray() {

return new double[] {

elevatorHeight, elbowAngle.getRadians(), wristAngle.getRadians() + elbowAngle.getRadians()

};

}

}

​

18

More Infrastructure

/** A generalized trajectory based off [position velocity] states. */

public class Trajectory {

​

private final List<Double> states;

private final double totalTime;

​

public State sample(double time) { /* interpolates a sample state */ }

​

/** Returns a command to follow the trajectory using {@link TrajectoryCommand} */

public CommandBase follow(Consumer<State> output, Subsystem... requirements) {

return new TrajectoryCommand(this, output, requirements);

}

}

​

​

​

19

MORE RECORDS

public static record ArmTrajectory(

Trajectory elevator, Trajectory elbow, Trajectory wrist, Parameters params) {}

​

public static record Parameters(ArmState start, ArmState end) {}

​

public static record CachedTrajectory(

double[] initialPos,

double[] finalPos,

double totalTime,

double[] points) {}

​

public static record StoredTrajectory(int id, List<CachedTrajectory> trajectories) {}

20

Storing and deploying

• Store trajectories by hashed start + end states
• Make sure to avoid floating point roundoff
• Use current + new setpoint to search the hashmap
• Have CasADi script write to json file in deploy/ directory
• Load the json in Java program

Map<Integer, ArmTrajectory> trajectories = new HashMap<Integer, ArmTrajectory>();

​

for (var cachedTrajectory : cache.trajectories) {

List<Double> elevatorStates = new ArrayList<Double>();

List<Double> elbowStates = new ArrayList<Double>();

List<Double> wristStates = new ArrayList<Double>();

for (int i = 0; i <= cachedTrajectory.points().length - 3; i += 3) {

elevatorStates.add(cachedTrajectory.points()[i]);

elbowStates.add(cachedTrajectory.points()[i + 1]);

wristStates.add(cachedTrajectory.points()[i + 2] - cachedTrajectory.points()[i + 1]);

}

Parameters params =

new Parameters(

ArmState.fromArray(cachedTrajectory.initialPos),

ArmState.fromArray(cachedTrajectory.finalPos));

​

trajectories.put(

params.hashCode(),

new ArmTrajectory(

new Trajectory(elevatorStates, cachedTrajectory.totalTime),

new Trajectory(elbowStates, cachedTrajectory.totalTime),

new Trajectory(wristStates, cachedTrajectory.totalTime),

params));

}

​

21

Following

/** Follows a {@link Trajectory} for each joint's relative position */

private Command followTrajectory(ArmTrajectory trajectory) {

return Commands.parallel(

trajectory.elevator().follow(elevator::updateSetpoint),

trajectory.elbow().follow(elbow::updateSetpoint),

trajectory.wrist().follow(wrist::updateSetpoint, this))

.withName("following trajectory");

}

​

public Command goTo(Supplier<ArmState> goal) {

return new DeferredCommand(

() -> {

var trajectory = findTrajectory(goal.get());

return trajectory

.map(this::followTrajectory)

.orElse(safeFollowProfile(goal))

.alongWith(

Commands.print(

String.format(

"Arm goTo Command:\nStart: %s\nGoal: %s\nFound trajectory: %b\n",

getSetpoint(), goal.get(), trajectory.isPresent())));

},

this);

}

​

22

Simulation

• Remember to simulate!
• Without this, it would have been literally impossible for us to test any of this

​

​

​

​

Please excuse the gif quality

23

In real life

(IT ACTUALLY WORKS)

24

Other Applications: Auto

Choreo, an app that will complement path planner, uses CasADi and TrajoptLib to generate holonomic trajectories

​

You should go to Warren’s presentation to learn more about tools like this :)

25

Resources

Controls Engineering in FRC - Tyler Veness

Essence of Calculus and Linear Algebra - 3Blue1Brown

Underactuated Robotics - Russ Tedrake

Two Jointed Arm Dynamics - 449

Kairos - 6328

Choreo & TrajoptLib - 2363, 6657, 6995, 8033

26

IMPORTANT NOTICE #1

IMPORTANT NOTICE #2

You should not depend on trajectory optimization to make your bad robot design work.

While it is tempting to treat FRC as a science fair, you must implement the basics first.

(my team did not regard either notice)

27

Thank you so much!

Any questions?

28