Package org.chsrobotics.lib.models

Class DoubleJointedArmModel

java.lang.Object
org.chsrobotics.lib.models.DoubleJointedArmModel
public class DoubleJointedArmModel extends Object
Combined feedforward and simulation model of a double-jointed arm.

Derived from https://www.chiefdelphi.com/t/whitepaper-two-jointed-arm-dynamics/423060 and https://github.com/Mechanical-Advantage/RobotCode2023/blob/main/src/main/java/org/littletonrobotics/frc2023/subsystems/arm/ArmDynamics.java

  • Constructor Summary

    Constructors
    Constructor
    Description
    DoubleJointedArmModel(double localMass, double localCGRadius, double localMoment, double localLength, edu.wpi.first.math.system.plant.DCMotor localDrive, double distalMass, double distalCGRadius, double distalMoment, edu.wpi.first.math.system.plant.DCMotor distalDrive, double kG)
    Constructs a new DoubleJointedArmModel.

  • Method Summary

    Modifier and Type
    Method
    Description
    edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2>
    feedforward(edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> position)
    Computes feedforward motor voltages for a given arm state with no acceleration or velocity.
    edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2>
    feedforward(edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> position, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> velocity)
    Computes feedforward motor voltages for a given arm state and no acceleration.
    edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2>
    feedforward(edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> position, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> velocity, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> acceleration)
    Computes feedforward motor voltages for a given arm state.
    edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N2>
    simulate(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N2> state, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> voltages, double dtSeconds)
    Simulates the arm system by numerical integration.

    Methods inherited from class java.lang.Object

    equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Constructor Details

    • DoubleJointedArmModel

      public DoubleJointedArmModel(double localMass, double localCGRadius, double localMoment, double localLength, edu.wpi.first.math.system.plant.DCMotor localDrive, double distalMass, double distalCGRadius, double distalMoment, edu.wpi.first.math.system.plant.DCMotor distalDrive, double kG)
      Constructs a new DoubleJointedArmModel.
      Parameters:
      localMass - Mass of the local arm segment, in kg.
      localCGRadius - Length from the root of the local arm to its center of gravity, in meters.
      localMoment - Moment of inertia of the local arm about its center of gravity, in kg m^2.
      localLength - Length of the local arm segment, in meters.
      localDrive - A DCMotor to power the local arm segment. Should include any reductions.
      distalMass - Mass of the distal arm segment, in kg.
      distalCGRadius - Length from the root of the distal arm to its center of gravity, in meters.
      distalMoment - Moment of inertia of the distal arm about its center of gravity, in kg m^2.
      distalDrive - A DCMotor to power the distal arm segment. Should include any reductions.
      kG - A scalar scaling factor to apply to gravitational torques.

  • Method Details

    • feedforward

      public edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> feedforward(edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> position, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> velocity, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> acceleration)
      Computes feedforward motor voltages for a given arm state.
      Parameters:
      position - A Vector representing arm joint angles in radians, as [local, distal].
      velocity - A Vector representing arm angular velocities in rad/s, as [local, distal].
      acceleration - A Vector representing arm angular accelerations in rad/s^2, as [local, distal].
      Returns:
      A Vector of feedforward motor voltages, as [local, distal].

    • feedforward

      public edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> feedforward(edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> position, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> velocity)
      Computes feedforward motor voltages for a given arm state and no acceleration.
      Parameters:
      position - A Vector representing arm joint angles in radians, as [local, distal].
      velocity - A Vector representing arm angular velocities in rad/s, as [local, distal].
      Returns:
      A Vector of feedforward motor voltages, as [local, distal].

    • feedforward

      public edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> feedforward(edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> position)
      Computes feedforward motor voltages for a given arm state with no acceleration or velocity.
      Parameters:
      position - A Vector representing arm joint angles in radians, as [local, distal].
      Returns:
      A Vector of feedforward motor voltages, as [local, distal].

    • simulate

      public edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N2> simulate(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N2,edu.wpi.first.math.numbers.N2> state, edu.wpi.first.math.Vector<edu.wpi.first.math.numbers.N2> voltages, double dtSeconds)
      Simulates the arm system by numerical integration.
      Parameters:
      state - A Matrix of arm state, as

      [localAngle, localVelocity]

      [distalAngle, distalVelocity]

      voltages - A Vector of input voltages, as [local, distal].
      dtSeconds - The timestep of the simulated system, in seconds.
      Returns:
      A Matrix of arm state after the simulated timestep, as

      [localAngle, localVelocity]

      [distalAngle, distalVelocity]